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

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

  Hardware:
  - Rotation Stepper: A4988 driver
  - Elevation 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 ELEVATION_STEP_PIN    6
#define ELEVATION_DIR_PIN     7
#define ELEVATION_ENABLE_PIN  8
#define ELEVATION_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 ELEVATION_GEAR_RATIO  3.0   // 3:1 gear reduction
#define ROTATION_STEPS_PER_DEGREE  (TOTAL_STEPS * ROTATION_GEAR_RATIO / 360.0)
#define ELEVATION_STEPS_PER_DEGREE (TOTAL_STEPS * ELEVATION_GEAR_RATIO / 360.0)

// Movement limits
#define ROTATION_MIN_DEGREES  -90.0
#define ROTATION_MAX_DEGREES   90.0
#define ELEVATION_MIN_DEGREES   0.0
#define ELEVATION_MAX_DEGREES  60.0

// Speed settings
#define MAX_SPEED_ROTATION    2000   // Steps per second
#define MAX_SPEED_ELEVATION   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 elevationStepper(AccelStepper::DRIVER, ELEVATION_STEP_PIN, ELEVATION_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 currentElevation = 0.0;
bool isHomed = false;

// 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(ELEVATION_ENABLE_PIN, OUTPUT);
  pinMode(ROTATION_LIMIT_PIN, INPUT_PULLUP);
  pinMode(ELEVATION_LIMIT_PIN, INPUT_PULLUP);
  pinMode(FIRE_SOLENOID_PIN, OUTPUT);
  pinMode(STATUS_LED_PIN, OUTPUT);

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

  // Enable steppers
  digitalWrite(ROTATION_ENABLE_PIN, LOW);  // LOW = enabled for A4988
  digitalWrite(ELEVATION_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> <elev>, REL <rot> <elev>, FIRE, POS");
}

void loop() {
  // Handle serial commands
  handleSerialInput();

  if (commandReady) {
    processCommand();
    commandReady = false;
    inputCommand = "";
  }

  // Run steppers
  rotationStepper.run();
  elevationStepper.run();

  // Blink status LED when moving
  static unsigned long lastBlink = 0;
  if (rotationStepper.isRunning() || elevationStepper.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") {
    stopMotors();
  }
  else if (inputCommand == "STATUS") {
    reportStatus();
  }
  else {
    Serial.println("ERROR: Unknown command");
  }
}

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

  // Disable acceleration for homing
  rotationStepper.setAcceleration(500);
  elevationStepper.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 elevation motor
  Serial.println("Homing elevation...");
  elevationStepper.setSpeed(-300);  // Move slowly towards limit
  while (digitalRead(ELEVATION_LIMIT_PIN) == HIGH) {
    elevationStepper.runSpeed();
    delay(1);
  }
  elevationStepper.stop();
  elevationStepper.setCurrentPosition(0);

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

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

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

  Serial.println("OK");
}

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

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

  moveToPosition(targetRotation, targetElevation);
}

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

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

  float targetRotation = currentRotation + deltaRotation;
  float targetElevation = currentElevation + deltaElevation;

  moveToPosition(targetRotation, targetElevation);
}

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

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

  // Convert to steps
  long rotationSteps = (long)(targetRotation * ROTATION_STEPS_PER_DEGREE);
  long elevationSteps = (long)(targetElevation * ELEVATION_STEPS_PER_DEGREE);

  // Move steppers
  rotationStepper.moveTo(rotationSteps);
  elevationStepper.moveTo(elevationSteps);

  // Wait for completion
  while (rotationStepper.isRunning() || elevationStepper.isRunning()) {
    rotationStepper.run();
    elevationStepper.run();
    delay(1);
  }

  // Update current position
  currentRotation = targetRotation;
  currentElevation = targetElevation;

  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();
  elevationStepper.stop();
  Serial.println("OK");
}

void reportPosition() {
  // Report actual position in degrees
  Serial.print(currentRotation, 1);
  Serial.print(" ");
  Serial.println(currentElevation, 1);
}

void reportStatus() {
  Serial.print("HOMED:");
  Serial.print(isHomed ? "1" : "0");
  Serial.print(" ROT:");
  Serial.print(currentRotation, 1);
  Serial.print(" ELEV:");
  Serial.print(currentElevation, 1);
  Serial.print(" MOVING:");
  Serial.print((rotationStepper.isRunning() || elevationStepper.isRunning()) ? "1" : "0");
  Serial.print(" LIMITS:");
  Serial.print(digitalRead(ROTATION_LIMIT_PIN) ? "0" : "1");
  Serial.print(",");
  Serial.println(digitalRead(ELEVATION_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(" | Elev pos: ");
  Serial.print(elevationStepper.currentPosition());
  Serial.print(" target: ");
  Serial.println(elevationStepper.targetPosition());
}