#include <AccelStepper.h>
#include <Servo.h>

// Global position
float currentX = 0;
float currentY = 0;
float currentR = 0;

// Calibration: steps required to move 1mm or 1 degree
const float STEPS_PER_MM_X = 17.95;  // adjust based on mechanical setup
const float STEPS_PER_MM_Y = 17.00;
const float STEPS_PER_DEG_R = 45.50; // for rotation

// Stepper motor pins
const int STEP_PIN_RF = 19;
const int DIR_PIN_RF  = 18;
const int STEP_PIN_LF = 12;
const int DIR_PIN_LF  = 13;
const int STEP_PIN_RB = 17;
const int DIR_PIN_RB  = 16;
const int STEP_PIN_LB = 14;
const int DIR_PIN_LB  = 15;

// Microstepping control
const int EN_PIN = 9;
const int MS1 = 6, MS2 = 7, MS3 = 8;

// Servo Z-axis
Servo servoZ;
const int SERVO_PIN_Z = 22;
int currentZ = 120; //

// Limit switches
const int LIMIT_X = 28;
const int LIMIT_Y = 27;

const int MAX_SPEED = 800;
const int ACCEL = 600;
const int HOMING_SPEED = 200;
const int BACK_OFF_STEPS = 500;

const unsigned long INACTIVITY_TIMEOUT = 5000; // ms
unsigned long lastCommandTime = 0;

String input = "";

// Stepper drivers
AccelStepper stepperRF(AccelStepper::DRIVER, STEP_PIN_RF, DIR_PIN_RF);
AccelStepper stepperLF(AccelStepper::DRIVER, STEP_PIN_LF, DIR_PIN_LF);
AccelStepper stepperRB(AccelStepper::DRIVER, STEP_PIN_RB, DIR_PIN_RB);
AccelStepper stepperLB(AccelStepper::DRIVER, STEP_PIN_LB, DIR_PIN_LB);

void setup() {
  Serial.begin(115200);

  pinMode(EN_PIN, OUTPUT);
  pinMode(MS1, OUTPUT); pinMode(MS2, OUTPUT); pinMode(MS3, OUTPUT);
  pinMode(LIMIT_X, INPUT_PULLUP);
  pinMode(LIMIT_Y, INPUT_PULLUP);

  digitalWrite(EN_PIN, LOW); // enable drivers
  digitalWrite(MS1, HIGH); digitalWrite(MS2, HIGH); digitalWrite(MS3, HIGH);

  stepperRF.setMaxSpeed(MAX_SPEED); stepperRF.setAcceleration(ACCEL);
  stepperLF.setMaxSpeed(MAX_SPEED); stepperLF.setAcceleration(ACCEL);
  stepperRB.setMaxSpeed(MAX_SPEED); stepperRB.setAcceleration(ACCEL);
  stepperLB.setMaxSpeed(MAX_SPEED); stepperLB.setAcceleration(ACCEL);

  servoZ.attach(SERVO_PIN_Z, 500, 2400);
  servoZ.write(currentZ);

  lastCommandTime = millis();

  Serial.println("Ready for G-code like G1 X100 Y0 Z90 or G28");
  Serial.print("Initial position: X="); Serial.print(currentX);
  Serial.print(" Y="); Serial.print(currentY);
  Serial.print(" R="); Serial.println(currentR);
}

void homeAxes() {
  Serial.println("Starting zeroing routine...");

  stepperRF.setMaxSpeed(HOMING_SPEED);
  stepperLF.setMaxSpeed(HOMING_SPEED);
  stepperRB.setMaxSpeed(HOMING_SPEED);
  stepperLB.setMaxSpeed(HOMING_SPEED);

  // --- Homing X ---
  stepperRF.setSpeed(100);  // Set speed directly (positive or negative)
  stepperLF.setSpeed(-100);
  stepperRB.setSpeed(-100);
  stepperLB.setSpeed(100);

  while (digitalRead(LIMIT_X) == HIGH) {
    stepperRF.runSpeed();
    stepperLF.runSpeed();
    stepperRB.runSpeed();
    stepperLB.runSpeed();
  }

  Serial.println("LIMIT_X triggered. X position zeroed.");
  delay(200);

  // --- Homing Y ---
  stepperRF.setSpeed(-100);  // Set speed directly (positive or negative)
  stepperLF.setSpeed(-100);
  stepperRB.setSpeed(-100);
  stepperLB.setSpeed(-100);

  while (digitalRead(LIMIT_Y) == HIGH) {
    stepperRF.runSpeed();
    stepperLF.runSpeed();
    stepperRB.runSpeed();
    stepperLB.runSpeed();
  }

  // --- Stop and zero
  stepperRF.stop(); stepperLF.stop(); stepperRB.stop(); stepperLB.stop();
  stepperRF.setCurrentPosition(0); stepperLF.setCurrentPosition(0);
  stepperRB.setCurrentPosition(0); stepperLB.setCurrentPosition(0);

  Serial.println("LIMIT_Y triggered. Y position zeroed.");
  delay(200);
  
  // Reset relative position 0
  currentX = 0;
  currentY = 0;
  currentR = 0;

  // Back off
  stepperLF.move(BACK_OFF_STEPS);
  stepperRB.move(BACK_OFF_STEPS);
  while (stepperRF.isRunning() || stepperLF.isRunning() || stepperRB.isRunning() || stepperLB.isRunning()) {
    stepperRF.run(); stepperLF.run(); stepperRB.run(); stepperLB.run();
  }

  Serial.println("Zeroing complete");
}

void parseAndExecute(String cmd) {
  cmd.trim();
  cmd.toUpperCase();
  lastCommandTime = millis();
  digitalWrite(EN_PIN, LOW);

  if (cmd.startsWith("G28")) {
    homeAxes();
    return;
  }

  if (!cmd.startsWith("G1")) {
    Serial.println("Invalid or unsupported command.");
    return;
  }

  float targetX = currentX;
  float targetY = currentY;
  float targetR = currentR;
  int z = -1;

  int xIndex = cmd.indexOf('X');
  if (xIndex != -1) targetX = cmd.substring(xIndex + 1).toFloat();

  int yIndex = cmd.indexOf('Y');
  if (yIndex != -1) targetY = cmd.substring(yIndex + 1).toFloat();

  int rIndex = cmd.indexOf('R');
  if (rIndex != -1) targetR = cmd.substring(rIndex + 1).toFloat();

  int zIndex = cmd.indexOf('Z');
  if (zIndex != -1) z = cmd.substring(zIndex + 1).toInt();

  if (z != -1) {
    z = constrain(z, 0, 180);
    servoZ.write(z);
    currentZ = z;
    Serial.print("Servo moved to "); Serial.print(z); Serial.println(" degrees");
  }

  // Calculate deltas
  float deltaX = targetX - currentX;
  float deltaY = targetY - currentY;
  float deltaR = targetR - currentR;

  // Convert to steps
  float deltaXSteps = deltaX * STEPS_PER_MM_X;
  float deltaYSteps = deltaY * STEPS_PER_MM_Y;
  float deltaRSteps = deltaR * STEPS_PER_DEG_R;

  // Inverse kinematics: calculate wheel steps
  long rfSteps = round(-deltaXSteps + deltaYSteps - deltaRSteps);
  long lfSteps = round( deltaXSteps + deltaYSteps + deltaRSteps);
  long rbSteps = round( deltaXSteps + deltaYSteps - deltaRSteps);
  long lbSteps = round(-deltaXSteps + deltaYSteps + deltaRSteps);

  stepperRF.move(rfSteps);
  stepperLF.move(lfSteps);
  stepperRB.move(rbSteps);
  stepperLB.move(lbSteps);

  while (stepperRF.isRunning() || stepperLF.isRunning() || stepperRB.isRunning() || stepperLB.isRunning()) {
    stepperRF.run(); stepperLF.run(); stepperRB.run(); stepperLB.run();
  }

  // Accumulate actual movement
  currentX = targetX;
  currentY = targetY;
  currentR = targetR;

  Serial.println("Moved to:");
  Serial.print("X: "); Serial.print(currentX, 2); Serial.print(" mm");
  Serial.print(" | Y: "); Serial.print(currentY, 2); Serial.print(" mm");
  Serial.print(" | R: "); Serial.print(currentR, 2); Serial.println(" deg");

  Serial.print("Servo Z (degrees): ");
  Serial.println(currentZ);
}

void loop() {
  // Serial input
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') {
      if (input.length() > 0) {
        parseAndExecute(input);
        input = "";
        Serial.println("ok");
      }
    } else {
      input += c;
    }
  }

  // Sleep mode
  if (millis() - lastCommandTime > INACTIVITY_TIMEOUT) {
    digitalWrite(EN_PIN, HIGH); // disable drivers
  }
}