// === Pin Assignments ===
#define X_DIR_PIN 16
#define X_STEP_PIN 17
#define Z_DIR_PIN 18
#define Z_STEP_PIN 19
#define C_DIR_PIN 21
#define C_STEP_PIN 22

// === UART Settings ===
#define UART_RX 4 // to review
#define UART_TX 2 // to reivew
#define BAUD_RATE 115200

// === Motion Settings ===
#define STEPS_PER_MM 10
#define STEP_MIN_DELAY 200       // µs - fastest step rate
#define STEP_MAX_DELAY 1500      // µs - slowest (start/end)
#define ACCEL_STEPS     100       // Steps to ramp up/down

//HardwareSerial CNCSerial(2);

// === Position Tracking (Absolute) ===
float xPos = 0;
float zPos = 0;
float cPos = 0;

// --- Function Declarations ---
void moveLinearTo(float targetX, float targetZ);
void jogWithAccel(float dx, float dz);
void moveAxisTo(int dirPin, int stepPin, float targetPos, float currentPos);
float extractValue(String cmd, char axis, float fallback);
int calculateAccelDelay(int stepNum, int totalSteps);
void stepPulse(int pin);

void setup() {
  pinMode(X_DIR_PIN, OUTPUT);
  pinMode(X_STEP_PIN, OUTPUT);
  pinMode(Z_DIR_PIN, OUTPUT);
  pinMode(Z_STEP_PIN, OUTPUT);
  pinMode(C_DIR_PIN, OUTPUT);
  pinMode(C_STEP_PIN, OUTPUT);

  //CNCSerial.begin(BAUD_RATE, SERIAL_8N1, UART_RX, UART_TX);
  Serial.begin(115200);
  Serial.println("ESP32 CNC Lathe Controller Ready (with Jog + Accel)");
}

void loop() {
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    Serial.println("Received: " + cmd);
    parseCommand(cmd);
  }
  if (Serial.read() == 0x5A && Serial.read() == 0xA5) {
    Serial.println("Received: " "Yes");
  }
}

void parseCommand(String cmd) {
  if (cmd.startsWith("G0") || cmd.startsWith("G1")) {
    float targetX = extractValue(cmd, 'X', xPos);
    float targetZ = extractValue(cmd, 'Z', zPos);
    moveLinearTo(targetX, targetZ);
  } 
  else if (cmd.startsWith("C")) {
    float targetC = extractValue(cmd, 'C', cPos);
    moveAxisTo(C_DIR_PIN, C_STEP_PIN, targetC, cPos);
    cPos = targetC;
  }
  else if (cmd.startsWith("JOG")) {
    float dx = extractValue(cmd, 'X', 0);
    float dz = extractValue(cmd, 'Z', 0);
    float dc = extractValue(cmd, 'C', 0);

    if (dx != 0 || dz != 0)
      jogWithAccel(dx, dz);

    if (dc != 0) {
      moveAxisTo(C_DIR_PIN, C_STEP_PIN, cPos + dc, cPos);
      cPos += dc;
    }
  }
}

float extractValue(String cmd, char axis, float fallback) {
  int idx = cmd.indexOf(axis);
  if (idx == -1) return fallback;
  int endIdx = cmd.indexOf(' ', idx);
  if (endIdx == -1) endIdx = cmd.length();
  return cmd.substring(idx + 1, endIdx).toFloat();
}

void moveLinearTo(float targetX, float targetZ) {
  float deltaX = targetX - xPos;
  float deltaZ = targetZ - zPos;
  int x_steps = abs(deltaX * STEPS_PER_MM);
  int z_steps = abs(deltaZ * STEPS_PER_MM);
  int x_dir = (deltaX >= 0) ? HIGH : LOW;
  int z_dir = (deltaZ >= 0) ? HIGH : LOW;

  digitalWrite(X_DIR_PIN, x_dir);
  digitalWrite(Z_DIR_PIN, z_dir);

  int max_steps = max(x_steps, z_steps);
  int x_counter = 0;
  int z_counter = 0;

  for (int i = 0; i < max_steps; i++) {
    x_counter += x_steps;
    z_counter += z_steps;

    if (x_counter >= max_steps) {
      stepPulse(X_STEP_PIN);
      x_counter -= max_steps;
    }

    if (z_counter >= max_steps) {
      stepPulse(Z_STEP_PIN);
      z_counter -= max_steps;
    }

    delayMicroseconds(STEP_MIN_DELAY); // Constant speed for G1
  }

  xPos = targetX;
  zPos = targetZ;
}

void jogWithAccel(float dx, float dz) {
  float targetX = xPos + dx;
  float targetZ = zPos + dz;

  float deltaX = targetX - xPos;
  float deltaZ = targetZ - zPos;
  int x_steps = abs(deltaX * STEPS_PER_MM);
  int z_steps = abs(deltaZ * STEPS_PER_MM);
  int x_dir = (deltaX >= 0) ? HIGH : LOW;
  int z_dir = (deltaZ >= 0) ? HIGH : LOW;

  digitalWrite(X_DIR_PIN, x_dir);
  digitalWrite(Z_DIR_PIN, z_dir);

  int max_steps = max(x_steps, z_steps);
  int x_counter = 0;
  int z_counter = 0;

  for (int i = 0; i < max_steps; i++) {
    int delayMicros = calculateAccelDelay(i, max_steps);

    x_counter += x_steps;
    z_counter += z_steps;

    if (x_counter >= max_steps) {
      stepPulse(X_STEP_PIN);
      x_counter -= max_steps;
    }

    if (z_counter >= max_steps) {
      stepPulse(Z_STEP_PIN);
      z_counter -= max_steps;
    }

    delayMicroseconds(delayMicros);
  }

  xPos = targetX;
  zPos = targetZ;
}

int calculateAccelDelay(int stepNum, int totalSteps) {
  if (stepNum < ACCEL_STEPS) {
    float factor = 1.0 - (float)stepNum / ACCEL_STEPS;
    return STEP_MAX_DELAY - factor * (STEP_MAX_DELAY - STEP_MIN_DELAY);
  } else if (stepNum > totalSteps - ACCEL_STEPS) {
    float factor = (float)(stepNum - (totalSteps - ACCEL_STEPS)) / ACCEL_STEPS;
    return STEP_MIN_DELAY + factor * (STEP_MAX_DELAY - STEP_MIN_DELAY);
  } else {
    return STEP_MIN_DELAY;
  }
}

void moveAxisTo(int dirPin, int stepPin, float targetPos, float currentPos) {
  float delta = targetPos - currentPos;
  int steps = abs(delta * STEPS_PER_MM);
  digitalWrite(dirPin, delta >= 0 ? HIGH : LOW);
  for (int i = 0; i < steps; i++) {
    stepPulse(stepPin);
    delayMicroseconds(STEP_MIN_DELAY);
  }
}

void stepPulse(int pin) {
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, LOW);
}