/*
  OCTv2 (Oreo Cookie Thrower v2) - ESP32 Motor Controller

  Controls two stepper motors for rotation and pitch
  Receives commands via Serial at 115200 baud

  Hardware:
  - Rotation Stepper: A4988 driver
  - Pitch Stepper: A4988 driver
  - Fire mechanism: Servo or solenoid
  - Limit switches for homing
*/

#include <ESP32Servo.h>
#include <AccelStepper.h>

// Pin definitions
#define ROTATION_STEP_PIN     2
#define ROTATION_DIR_PIN      3
#define ROTATION_ENABLE_PIN   4
#define ROTATION_LIMIT_PIN    5   // Home limit switch

#define PITCH_STEP_PIN    6
#define PITCH_DIR_PIN     7
#define PITCH_ENABLE_PIN  8
#define PITCH_LIMIT_PIN   9   // Home limit switch

#define FIRE_SERVO_PIN        10
#define FIRE_SOLENOID_PIN     11
#define STATUS_LED_PIN        13

// Motor configuration
#define STEPS_PER_REVOLUTION  200   // Standard stepper motor
#define MICROSTEPS           16     // A4988 microstepping
#define TOTAL_STEPS          (STEPS_PER_REVOLUTION * MICROSTEPS)

// Mechanical configuration
#define ROTATION_GEAR_RATIO   5.0   // 5:1 gear reduction
#define PITCH_GEAR_RATIO  3.0   // 3:1 gear reduction
#define ROTATION_STEPS_PER_DEGREE  (TOTAL_STEPS * ROTATION_GEAR_RATIO / 360.0)
#define PITCH_STEPS_PER_DEGREE (TOTAL_STEPS * PITCH_GEAR_RATIO / 360.0)

// Movement limits
#define ROTATION_MIN_DEGREES  -90.0
#define ROTATION_MAX_DEGREES   90.0
#define PITCH_MIN_DEGREES   0.0
#define PITCH_MAX_DEGREES  60.0

// Speed settings
#define MAX_SPEED_ROTATION    2000   // Steps per second
#define MAX_SPEED_PITCH   1500   // Steps per second
#define ACCELERATION         1000    // Steps per second squared

// Create stepper objects
AccelStepper rotationStepper(AccelStepper::DRIVER, ROTATION_STEP_PIN, ROTATION_DIR_PIN);
AccelStepper pitchStepper(AccelStepper::DRIVER, PITCH_STEP_PIN, PITCH_DIR_PIN);

// Fire mechanism
Servo fireServo;
bool useServoForFire = true;  // Set to false to use solenoid instead

// Current positions in degrees
float currentRotation = 0.0;
float currentPitch = 0.0;
bool isHomed = false;

// Safety and error handling
bool emergencyStop = false;
unsigned long lastCommandTime = 0;
const unsigned long WATCHDOG_TIMEOUT = 30000; // 30 seconds

// Command parsing
String inputCommand = "";
bool commandReady = false;

void setup() {
  Serial.begin(115200);
  Serial.println("🍪 OCTv2 ESP32 Motor Controller Starting...");

  // Setup pins
  pinMode(ROTATION_ENABLE_PIN, OUTPUT);
  pinMode(PITCH_ENABLE_PIN, OUTPUT);
  pinMode(ROTATION_LIMIT_PIN, INPUT_PULLUP);
  pinMode(PITCH_LIMIT_PIN, INPUT_PULLUP);
  pinMode(FIRE_SOLENOID_PIN, OUTPUT);
  pinMode(STATUS_LED_PIN, OUTPUT);

  // Configure steppers
  rotationStepper.setMaxSpeed(MAX_SPEED_ROTATION);
  rotationStepper.setAcceleration(ACCELERATION);
  pitchStepper.setMaxSpeed(MAX_SPEED_ELEVATION);
  pitchStepper.setAcceleration(ACCELERATION);

  // Enable steppers
  digitalWrite(ROTATION_ENABLE_PIN, LOW);  // LOW = enabled for A4988
  digitalWrite(PITCH_ENABLE_PIN, LOW);

  // Setup fire mechanism
  if (useServoForFire) {
    fireServo.attach(FIRE_SERVO_PIN);
    fireServo.write(0);  // Home position
  } else {
    digitalWrite(FIRE_SOLENOID_PIN, LOW);
  }

  // Status LED
  digitalWrite(STATUS_LED_PIN, HIGH);

  Serial.println("✅ OCTv2 Motor Controller Ready");
  Serial.println("Commands: HOME, MOVE <rot> <pitch>, REL <rot> <pitch>, FIRE, POS");
}

void loop() {
  // Check for emergency stop or communication timeout
  if (emergencyStop || (millis() - lastCommandTime > WATCHDOG_TIMEOUT && isHomed)) {
    if (!emergencyStop) {
      Serial.println("WARNING: Communication timeout - emergency stop");
      emergencyStop = true;
    }
    // Emergency stop: disable motors and fast blink LED
    rotationStepper.stop();
    pitchStepper.stop();
    digitalWrite(ROTATION_ENABLE_PIN, HIGH);
    digitalWrite(PITCH_ENABLE_PIN, HIGH);

    static unsigned long emergencyBlink = 0;
    if (millis() - emergencyBlink > 200) {
      digitalWrite(STATUS_LED_PIN, !digitalRead(STATUS_LED_PIN));
      emergencyBlink = millis();
    }
    return;
  }

  // Handle serial commands
  handleSerialInput();

  if (commandReady) {
    processCommand();
    commandReady = false;
    inputCommand = "";
    lastCommandTime = millis(); // Reset watchdog timer
  }

  // Run steppers (only if not in emergency stop)
  if (!emergencyStop) {
    rotationStepper.run();
    pitchStepper.run();
  }

  // Status LED: blink when moving, solid when idle
  static unsigned long lastBlink = 0;
  if (rotationStepper.isRunning() || pitchStepper.isRunning()) {
    if (millis() - lastBlink > 100) {
      digitalWrite(STATUS_LED_PIN, !digitalRead(STATUS_LED_PIN));
      lastBlink = millis();
    }
  } else {
    digitalWrite(STATUS_LED_PIN, HIGH);
  }
}

void handleSerialInput() {
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') {
      if (inputCommand.length() > 0) {
        commandReady = true;
      }
    } else {
      inputCommand += c;
    }
  }
}

void processCommand() {
  inputCommand.trim();
  inputCommand.toUpperCase();

  if (inputCommand == "HOME") {
    homeMotors();
  }
  else if (inputCommand.startsWith("MOVE ")) {
    handleMoveCommand();
  }
  else if (inputCommand.startsWith("REL ")) {
    handleRelativeCommand();
  }
  else if (inputCommand == "FIRE") {
    fireOreo();
  }
  else if (inputCommand == "POS") {
    reportPosition();
  }
  else if (inputCommand == "STOP") {
    emergencyStop = true;
    Serial.println("EMERGENCY STOP ACTIVATED");
  }
  else if (inputCommand == "RESET") {
    emergencyStop = false;
    digitalWrite(ROTATION_ENABLE_PIN, LOW);
    digitalWrite(PITCH_ENABLE_PIN, LOW);
    lastCommandTime = millis();
    Serial.println("Emergency stop reset - system ready");
  }
  else if (inputCommand == "STATUS") {
    reportStatus();
  }
  else {
    Serial.println("ERROR: Unknown command");
  }
}

void homeMotors() {
  Serial.println("🏠 Homing motors...");

  // Disable acceleration for homing
  rotationStepper.setAcceleration(500);
  pitchStepper.setAcceleration(500);

  // Home rotation motor
  Serial.println("Homing rotation...");
  rotationStepper.setSpeed(-500);  // Move slowly towards limit
  while (digitalRead(ROTATION_LIMIT_PIN) == HIGH) {
    rotationStepper.runSpeed();
    delay(1);
  }
  rotationStepper.stop();
  rotationStepper.setCurrentPosition(0);

  // Back off from limit
  rotationStepper.move(100);  // Move away from limit
  while (rotationStepper.run()) { delay(1); }

  // Home pitch motor
  Serial.println("Homing pitch...");
  pitchStepper.setSpeed(-300);  // Move slowly towards limit
  while (digitalRead(PITCH_LIMIT_PIN) == HIGH) {
    pitchStepper.runSpeed();
    delay(1);
  }
  pitchStepper.stop();
  pitchStepper.setCurrentPosition(0);

  // Back off from limit
  pitchStepper.move(50);
  while (pitchStepper.run()) { delay(1); }

  // Restore normal acceleration
  rotationStepper.setAcceleration(ACCELERATION);
  pitchStepper.setAcceleration(ACCELERATION);

  // Set home position
  currentRotation = 0.0;
  currentPitch = 0.0;
  isHomed = true;

  Serial.println("OK");
}

void handleMoveCommand() {
  // Parse "MOVE <rotation> <pitch>"
  int firstSpace = inputCommand.indexOf(' ', 5);
  if (firstSpace == -1) {
    Serial.println("ERROR: Invalid MOVE syntax");
    return;
  }

  float targetRotation = inputCommand.substring(5, firstSpace).toFloat();
  float targetPitch = inputCommand.substring(firstSpace + 1).toFloat();

  moveToPosition(targetRotation, targetPitch);
}

void handleRelativeCommand() {
  // Parse "REL <delta_rotation> <delta_pitch>"
  int firstSpace = inputCommand.indexOf(' ', 4);
  if (firstSpace == -1) {
    Serial.println("ERROR: Invalid REL syntax");
    return;
  }

  float deltaRotation = inputCommand.substring(4, firstSpace).toFloat();
  float deltaPitch = inputCommand.substring(firstSpace + 1).toFloat();

  float targetRotation = currentRotation + deltaRotation;
  float targetPitch = currentPitch + deltaPitch;

  moveToPosition(targetRotation, targetPitch);
}

void moveToPosition(float targetRotation, float targetPitch) {
  if (!isHomed) {
    Serial.println("ERROR: Not homed");
    return;
  }

  // Clamp to limits
  targetRotation = constrain(targetRotation, ROTATION_MIN_DEGREES, ROTATION_MAX_DEGREES);
  targetPitch = constrain(targetPitch, ELEVATION_MIN_DEGREES, ELEVATION_MAX_DEGREES);

  // Convert to steps
  long rotationSteps = (long)(targetRotation * ROTATION_STEPS_PER_DEGREE);
  long elevationSteps = (long)(targetPitch * PITCH_STEPS_PER_DEGREE);

  // Set new targets (non-blocking)
  rotationStepper.moveTo(rotationSteps);
  pitchStepper.moveTo(elevationSteps);

  // Update target positions
  currentRotation = targetRotation;
  currentPitch = targetPitch;

  // Immediate response - movement happens in background
  Serial.println("OK");
}

void fireOreo() {
  Serial.println("🔥 FIRING OREO!");

  if (useServoForFire) {
    // Servo fire mechanism
    fireServo.write(90);   // Fire position
    delay(200);            // Fire duration
    fireServo.write(0);    // Return to home
  } else {
    // Solenoid fire mechanism
    digitalWrite(FIRE_SOLENOID_PIN, HIGH);
    delay(100);            // Fire pulse
    digitalWrite(FIRE_SOLENOID_PIN, LOW);
  }

  Serial.println("OK");
}

void stopMotors() {
  rotationStepper.stop();
  pitchStepper.stop();
  Serial.println("OK");
}

void reportPosition() {
  // Report actual current position based on stepper positions
  float actualRotation = rotationStepper.currentPosition() / ROTATION_STEPS_PER_DEGREE;
  float actualPitch = pitchStepper.currentPosition() / PITCH_STEPS_PER_DEGREE;

  Serial.print(actualRotation, 1);
  Serial.print(" ");
  Serial.println(actualPitch, 1);
}

void reportStatus() {
  // Report actual positions and targets
  float actualRotation = rotationStepper.currentPosition() / ROTATION_STEPS_PER_DEGREE;
  float actualPitch = pitchStepper.currentPosition() / PITCH_STEPS_PER_DEGREE;
  float targetRotation = rotationStepper.targetPosition() / ROTATION_STEPS_PER_DEGREE;
  float targetPitch = pitchStepper.targetPosition() / PITCH_STEPS_PER_DEGREE;

  Serial.print("HOMED:");
  Serial.print(isHomed ? "1" : "0");
  Serial.print(" ROT:");
  Serial.print(actualRotation, 1);
  Serial.print(" PITCH:");
  Serial.print(actualPitch, 1);
  Serial.print(" TARGET_ROT:");
  Serial.print(targetRotation, 1);
  Serial.print(" TARGET_PITCH:");
  Serial.print(targetPitch, 1);
  Serial.print(" MOVING:");
  Serial.print((rotationStepper.isRunning() || pitchStepper.isRunning()) ? "1" : "0");
  Serial.print(" EMERGENCY:");
  Serial.print(emergencyStop ? "1" : "0");
  Serial.print(" LIMITS:");
  Serial.print(digitalRead(ROTATION_LIMIT_PIN) ? "0" : "1");
  Serial.print(",");
  Serial.println(digitalRead(PITCH_LIMIT_PIN) ? "0" : "1");
}

// Helper function for debugging
void printDebugInfo() {
  Serial.print("Debug - Rot pos: ");
  Serial.print(rotationStepper.currentPosition());
  Serial.print(" target: ");
  Serial.print(rotationStepper.targetPosition());
  Serial.print(" | Pitch pos: ");
  Serial.print(pitchStepper.currentPosition());
  Serial.print(" target: ");
  Serial.println(pitchStepper.targetPosition());
}