/*
 * W2ASX Antenna Rotator Control with Rotary Encoder
 * 
 * This Arduino sketch provides:
 * - Rotary encoder position feedback
 * - Serial communication with HTML interface
 * - Motor control commands
 * - Real-time position updates
 * 
 * Hardware Connections:
 * - Rotary Encoder CLK -> Pin 2 (interrupt)
 * - Rotary Encoder DT -> Pin 3 (interrupt)
 * - Rotary Encoder SW -> Pin 4 (optional button)
 * - Rotary Encoder VCC -> 5V
 * - Rotary Encoder GND -> GND
 * - Serial communication via USB
 * 
 * Commands from HTML interface:
 * - ENABLE: Enable motor
 * - DISABLE: Disable motor
 * - GOTO <degrees>: Move to specific position
 * - CW: Start clockwise rotation
 * - CCW: Start counter-clockwise rotation
 * - STOP: Stop rotation
 * - SPEED <value>: Set rotation speed
 * - STATUS: Get current status
 * - SET_HOME: Set current position as home
 */

// Rotary encoder pins
#define CLK_PIN 2
#define DT_PIN 3
#define SW_PIN 4

// 28BYJ-48 Motor control pins (ULN2003 driver)
#define IN1 8
#define IN2 9
#define IN3 10
#define IN4 11

// Encoder variables
volatile int encoderPos = 0;
volatile int lastCLK = 0;
int lastPosition = 0;
int currentPosition = 0;
int targetPosition = 0;
bool motorEnabled = false;
int motorSpeed = 500; // steps per second
bool isRotating = false;
bool rotationDirection = true; // true = CW, false = CCW

// Position tracking
unsigned long lastPositionUpdate = 0;
const unsigned long POSITION_UPDATE_INTERVAL = 500; // Update every 500ms (2 times per second)
int updateCount = 0;
unsigned long lastUpdateTime = 0;
unsigned long lastMovementTime = 0; // Track when encoder last moved
const unsigned long SLEEP_DELAY = 10000; // Sleep 10 seconds after last movement
bool isSleeping = false;

// Motor control
unsigned long lastStepTime = 0;
unsigned long stepInterval;

void setup() {
  Serial.begin(9600);
  
  // Setup encoder pins
  pinMode(CLK_PIN, INPUT_PULLUP);
  pinMode(DT_PIN, INPUT_PULLUP);
  pinMode(SW_PIN, INPUT_PULLUP);
  
  // Setup 28BYJ-48 motor pins
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  
  // Initialize motor as stopped
  stopMotor();
  
  // Attach interrupts for encoder
  attachInterrupt(digitalPinToInterrupt(CLK_PIN), readEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(DT_PIN), readEncoder, CHANGE);
  
  // Initialize position
  lastCLK = digitalRead(CLK_PIN);
  
  // Calculate step interval based on speed
  updateStepInterval();
  
  Serial.println("W2ASX Rotator Control with Encoder Ready");
  Serial.println("28BYJ-48 Motor pins: IN1=8, IN2=9, IN3=10, IN4=11");
  Serial.println("Encoder pins: CLK=2, DT=3, SW=4");
  Serial.println("Commands: ENABLE, DISABLE, GOTO <deg>, CW, CCW, STOP, SPEED <val>, STATUS, SET_HOME");
}

void loop() {
  // Handle serial commands
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    command.trim();
    command.toUpperCase();
    
    Serial.println("Received: " + command);
    handleCommand(command);
  }
  
  // Update position display - only when not sleeping
  if (!isSleeping && millis() - lastPositionUpdate >= POSITION_UPDATE_INTERVAL) {
    updatePosition();
    lastPositionUpdate = millis();
  }
  
  // Check if we should go to sleep (10 seconds after last movement)
  if (!isSleeping && millis() - lastMovementTime >= SLEEP_DELAY) {
    isSleeping = true;
    Serial.println("Position updates sleeping - move encoder to wake up");
  }
  
  // Handle motor stepping - 28BYJ-48 half-step sequence
  if (isRotating && motorEnabled) {
    if (micros() - lastStepTime >= stepInterval) {
      stepMotor28BYJ48();
      lastStepTime = micros();
      
      // Update encoder position
      if (rotationDirection) {
        encoderPos++;
      } else {
        encoderPos--;
      }
    }
  } else if (isRotating && !motorEnabled) {
    // Stop rotation if motor gets disabled
    isRotating = false;
    stopMotor();
    Serial.println("Rotation stopped - motor disabled");
  }
  
  // Debug: Print status every 5 seconds
  static unsigned long lastDebugTime = 0;
  if (millis() - lastDebugTime >= 5000) {
    lastDebugTime = millis();
    Serial.print("Debug - Rotating: ");
    Serial.print(isRotating);
    Serial.print(", Enabled: ");
    Serial.print(motorEnabled);
    Serial.print(", Position: ");
    Serial.println(currentPosition);
  }
}

void readEncoder() {
  int clkState = digitalRead(CLK_PIN);
  int dtState = digitalRead(DT_PIN);
  
  if (clkState != lastCLK) {
    if (dtState != clkState) {
      encoderPos++;
    } else {
      encoderPos--;
    }
    lastCLK = clkState;
  }
}

void updatePosition() {
  // Convert encoder position to degrees (corrected scaling)
  // Your encoder has 20 clicks per full turn
  // 1 click = 18 degrees actual movement
  // 5 clicks = 90 degrees, 10 clicks = 180 degrees, 20 clicks = 360 degrees
  // But encoder is giving 36 degrees per click, so divide by 2
  float degrees = (float)encoderPos * 9.0;  // Correct scaling: 1 click = 18 degrees (36/2 = 18)
  int newPosition = ((int)degrees) % 360;
  if (newPosition < 0) newPosition += 360;
  
  // Check if position actually changed (encoder moved)
  if (newPosition != currentPosition) {
    // Wake up if sleeping
    if (isSleeping) {
      isSleeping = false;
      Serial.println("Position updates awakened - encoder moved");
    }
    
    // Update movement time
    lastMovementTime = millis();
  }
  
  // Update position
  currentPosition = newPosition;
  updateCount++;
  
  // Send to HTML interface - always send 0-359 range
  Serial.print("POSITION:");
  Serial.println(currentPosition);
  
  // Also send in the format the HTML expects
  Serial.print("Position: ");
  Serial.println(currentPosition);
  
  lastUpdateTime = millis();
}

void handleCommand(String command) {
  if (command == "ENABLE") {
    motorEnabled = true;
    Serial.println("Motor ENABLED");
    
  } else if (command == "DISABLE") {
    motorEnabled = false;
    isRotating = false;
    stopMotor();
    Serial.println("Motor DISABLED");
    
  } else if (command.startsWith("GOTO ")) {
    int spaceIndex = command.indexOf(' ');
    if (spaceIndex > 0) {
      targetPosition = command.substring(spaceIndex + 1).toInt();
      if (targetPosition >= 0 && targetPosition < 360) {
        // Wake up encoder updates for motor movement
        isSleeping = false;
        lastMovementTime = millis();
        moveToPosition(targetPosition);
      } else {
        Serial.println("ERROR: Invalid position (0-359)");
      }
    }
    
  } else if (command == "CW") {
    if (motorEnabled) {
      isRotating = true;
      rotationDirection = true; // CW
      // Wake up encoder updates for motor movement
      isSleeping = false;
      lastMovementTime = millis();
      Serial.println("Rotating CLOCKWISE");
    } else {
      Serial.println("ERROR: Motor not enabled");
    }
    
  } else if (command == "CCW") {
    if (motorEnabled) {
      isRotating = true;
      rotationDirection = false; // CCW
      // Wake up encoder updates for motor movement
      isSleeping = false;
      lastMovementTime = millis();
      Serial.println("Rotating COUNTER-CLOCKWISE");
    } else {
      Serial.println("ERROR: Motor not enabled");
    }
    
  } else if (command == "STOP") {
    isRotating = false;
    stopMotor();
    Serial.println("Rotation STOPPED");
    
  } else if (command.startsWith("SPEED ")) {
    int spaceIndex = command.indexOf(' ');
    if (spaceIndex > 0) {
      motorSpeed = command.substring(spaceIndex + 1).toInt();
      if (motorSpeed > 0 && motorSpeed <= 1000) {
        updateStepInterval();
        Serial.print("Speed set to ");
        Serial.print(motorSpeed);
        Serial.println(" steps/sec");
      } else {
        Serial.println("ERROR: Invalid speed (1-1000)");
      }
    }
    
  } else if (command == "STATUS") {
    sendStatus();
    
  } else if (command == "SET_HOME") {
    encoderPos = 0;
    currentPosition = 0;
    lastPosition = 0;
    Serial.println("Home position set to current position");
    
  } else if (command == "TEST") {
    // Test 28BYJ-48 motor with slow steps
    if (motorEnabled) {
      Serial.println("Testing 28BYJ-48 motor - 8 steps (1 full rotation)");
      Serial.println("Watch the motor - it should move slowly");
      for (int i = 0; i < 8; i++) {
        Serial.print("Step ");
        Serial.print(i + 1);
        Serial.print(" - Phase ");
        Serial.println(i);
        stepMotor28BYJ48();
        delay(1000); // 1 second for easy observation
      }
      stopMotor();
      Serial.println("Test complete");
    } else {
      Serial.println("ERROR: Motor not enabled for test");
    }
    
  } else if (command == "PINS") {
    // Test individual 28BYJ-48 pins
    Serial.println("Testing 28BYJ-48 pins...");
    Serial.println("Setting IN1 pin HIGH for 2 seconds");
    digitalWrite(IN1, HIGH);
    delay(2000);
    digitalWrite(IN1, LOW);
    Serial.println("IN1 pin set LOW");
    
    Serial.println("Setting IN2 pin HIGH for 2 seconds");
    digitalWrite(IN2, HIGH);
    delay(2000);
    digitalWrite(IN2, LOW);
    Serial.println("IN2 pin set LOW");
    
    Serial.println("Setting IN3 pin HIGH for 2 seconds");
    digitalWrite(IN3, HIGH);
    delay(2000);
    digitalWrite(IN3, LOW);
    Serial.println("IN3 pin set LOW");
    
    Serial.println("Setting IN4 pin HIGH for 2 seconds");
    digitalWrite(IN4, HIGH);
    delay(2000);
    digitalWrite(IN4, LOW);
    Serial.println("IN4 pin set LOW");
    
  } else {
    Serial.println("ERROR: Unknown command");
  }
}

void moveToPosition(int target) {
  if (!motorEnabled) {
    Serial.println("ERROR: Motor not enabled");
    return;
  }
  
  int current = currentPosition;
  int difference = target - current;
  
  // Handle wraparound (shortest path)
  if (difference > 180) {
    difference -= 360;
  } else if (difference < -180) {
    difference += 360;
  }
  
  Serial.print("Moving from ");
  Serial.print(current);
  Serial.print("° to ");
  Serial.print(target);
  Serial.print("° (");
  Serial.print(difference);
  Serial.println("° difference)");
  
  // Set rotation direction
  if (difference > 0) {
    rotationDirection = true; // CW
  } else {
    rotationDirection = false; // CCW
    difference = -difference; // Make positive
  }
  
  // Start rotation
  isRotating = true;
  
  // Calculate steps needed (28BYJ-48: 4096 steps per 360 degrees)
  int stepsNeeded = difference * (4096 / 360);
  
  // Step the motor
  for (int i = 0; i < stepsNeeded && isRotating; i++) {
    stepMotor28BYJ48();
    delayMicroseconds(stepInterval);
  }
  
  isRotating = false;
  targetPosition = currentPosition;
  
  Serial.print("Movement completed at position ");
  Serial.print(currentPosition);
  Serial.print("° (Azimuth: ");
  Serial.print(currentPosition);
  Serial.println("°)");
}

void stepMotor28BYJ48() {
  static int stepPhase = 0;
  
  // Half-step sequence for 28BYJ-48
  switch(stepPhase) {
    case 0:
      digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); digitalWrite(IN3, LOW); digitalWrite(IN4, LOW);
      break;
    case 1:
      digitalWrite(IN1, HIGH); digitalWrite(IN2, HIGH); digitalWrite(IN3, LOW); digitalWrite(IN4, LOW);
      break;
    case 2:
      digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH); digitalWrite(IN3, LOW); digitalWrite(IN4, LOW);
      break;
    case 3:
      digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
      break;
    case 4:
      digitalWrite(IN1, LOW); digitalWrite(IN2, LOW); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
      break;
    case 5:
      digitalWrite(IN1, LOW); digitalWrite(IN2, LOW); digitalWrite(IN3, HIGH); digitalWrite(IN4, HIGH);
      break;
    case 6:
      digitalWrite(IN1, LOW); digitalWrite(IN2, LOW); digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
      break;
    case 7:
      digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
      break;
  }
  
  // Move to next phase
  if (rotationDirection) {
    stepPhase = (stepPhase + 1) % 8; // Clockwise
  } else {
    stepPhase = (stepPhase + 7) % 8; // Counter-clockwise
  }
}

void stopMotor() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

void updateStepInterval() {
  // Calculate microseconds between steps - back to original approach
  stepInterval = 1000000 / motorSpeed; // Convert steps/sec to microseconds/step
}

void sendStatus() {
  Serial.print("Position: ");
  Serial.println(currentPosition);
  Serial.print("Azimuth: ");
  Serial.println(currentPosition);
  Serial.print("Enabled: ");
  Serial.println(motorEnabled ? "YES" : "NO");
  Serial.print("Speed: ");
  Serial.println(motorSpeed);
  Serial.print("Rotating: ");
  Serial.println(isRotating ? "YES" : "NO");
  Serial.print("Encoder Raw: ");
  Serial.println(encoderPos);
  Serial.print("Updates: ");
  Serial.println(updateCount);
}