// ============================================================
// CNC PRO — FIRMWARE FINAL
// Pico W 2 + DRV8825 x4 + Servo herramienta
// ISR + Bresenham + Smoothstep + Buffer + Jog directo + G-code
//
// Jog directo: mueve continuamente mientras el botón
// está presionado, sin pasar por el buffer ni el ISR
// Servo solo rota durante G1 (grabado real)
// ============================================================

#include <Arduino.h>
#include <Servo.h>

// ============================
// PINES MOTORES
// ============================

#define X1_STEP 16
#define X1_DIR  17
#define X1_EN   18

#define X2_STEP 7
#define X2_DIR  8
#define X2_EN   6

#define Y_STEP  1
#define Y_DIR   2
#define Y_EN    0

#define Z_STEP  4
#define Z_DIR   5
#define Z_EN    3

// ============================
// BOTONES
// ============================

#define PIN_ORIGEN_XY 19
#define PIN_ORIGEN_Z  13
#define PIN_X_PLUS    28
#define PIN_X_MINUS   27
#define PIN_Y_PLUS    26
#define PIN_Y_MINUS   22
#define PIN_Z_PLUS    21
#define PIN_Z_MINUS   20

// ============================
// SERVO
// ============================

#define SERVO_PIN      12
#define SERVO_MIN_ANG  45
#define SERVO_MAX_ANG  135
#define SERVO_CENTRO   90

Servo servoTool;
float servoTarget  = SERVO_CENTRO;
float servoCurrent = SERVO_CENTRO;

// ============================
// PARÁMETROS
// ============================

#define PASOS_MM      800.0
#define VEL_MIN_US    2000
#define VEL_MAX_US    400
#define JOG_DELAY_US  600    // velocidad del jog — menor = más rápido

// Soft limits (mm)
#define X_MIN   0.0
#define X_MAX   220.0
#define Y_MIN   0.0
#define Y_MAX   200.0
#define Z_MIN  -10.0
#define Z_MAX    5.0

// ============================
// BUFFER
// ============================

#define BUFFER_SIZE 30

struct Movimiento {
  float x;
  float y;
  float z;
  float servoAng;
};

Movimiento bufferMov[BUFFER_SIZE];
int bufHead = 0;
int bufTail = 0;

// ============================
// ESTADO GLOBAL
// ============================

volatile long posX = 0, posY = 0, posZ = 0;
volatile long dx, dy, dz;
volatile long stepsTotal;
volatile long errX, errY, errZ;
volatile long stepsDone    = 0;
volatile bool running      = false;
volatile bool softLimitHit = false;

// true  = viene de G1 (grabado) → servo rota
// false = jog o G0             → servo quieto
bool modoGrabado = false;

repeating_timer_t timer;

// ============================
// PULSOS STEP
// ============================

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

// Eje X dual — pulsa los dos motores en el mismo ciclo
inline void stepPulseX() {
  digitalWrite(X1_STEP, HIGH);
  digitalWrite(X2_STEP, HIGH);
  delayMicroseconds(2);
  digitalWrite(X1_STEP, LOW);
  digitalWrite(X2_STEP, LOW);
}

// ============================
// SOFT LIMITS
// ============================

bool dentroLimites(float x, float y, float z) {
  if (x < X_MIN || x > X_MAX) return false;
  if (y < Y_MIN || y > Y_MAX) return false;
  if (z < Z_MIN || z > Z_MAX) return false;
  return true;
}

// ============================
// SERVO
// ============================

float calcularAnguloServo(float dx_mm, float dy_mm) {
  if (dx_mm == 0.0 && dy_mm == 0.0) return servoCurrent;

  float angMov   = atan2(dy_mm, dx_mm) * 180.0 / PI;
  float angPunta = angMov + 90.0;

  if (angPunta < 0)   angPunta += 360.0;
  if (angPunta > 360) angPunta -= 360.0;
  if (angPunta > 180) angPunta -= 180.0;

  return SERVO_MIN_ANG + (angPunta / 180.0) * (SERVO_MAX_ANG - SERVO_MIN_ANG);
}

void actualizarServo() {
  float error = servoTarget - servoCurrent;
  if (abs(error) > 0.5) {
    servoCurrent += error * 0.15;
    servoTool.write((int)round(servoCurrent));
  }
}

// ============================
// ISR — núcleo del firmware
// ============================

bool stepperISR(repeating_timer_t *t) {

  if (!running) return true;

  if (stepsDone >= stepsTotal) {
    running = false;
    return true;
  }

  float progress = (float)stepsDone / stepsTotal;
  float factor   = progress * progress * (3.0 - 2.0 * progress);
  int   vel      = VEL_MIN_US - (int)((VEL_MIN_US - VEL_MAX_US) * factor);

  errX += dx;
  errY += dy;
  errZ += dz;

  if (errX >= stepsTotal) {
    stepPulseX();
    posX += digitalRead(X1_DIR) ? 1 : -1;
    errX -= stepsTotal;
  }
  if (errY >= stepsTotal) {
    stepPulse(Y_STEP);
    posY += digitalRead(Y_DIR) ? 1 : -1;
    errY -= stepsTotal;
  }
  if (errZ >= stepsTotal) {
    stepPulse(Z_STEP);
    posZ += digitalRead(Z_DIR) ? 1 : -1;
    errZ -= stepsTotal;
  }

  if (!dentroLimites(posX / PASOS_MM, posY / PASOS_MM, posZ / PASOS_MM)) {
    running      = false;
    softLimitHit = true;
    return true;
  }

  stepsDone++;
  cancel_repeating_timer(&timer);
  add_repeating_timer_us(-vel, stepperISR, NULL, &timer);
  return true;
}

// ============================
// LANZAR MOVIMIENTO
// ============================

void lanzarMovimiento(float x, float y, float z, float angServo) {
  dx = (long)abs(x * PASOS_MM);
  dy = (long)abs(y * PASOS_MM);
  dz = (long)abs(z * PASOS_MM);

  digitalWrite(X1_DIR, (x > 0) ? HIGH : LOW);
  digitalWrite(X2_DIR, (x > 0) ? HIGH : LOW);
  digitalWrite(Y_DIR,  (y > 0) ? HIGH : LOW);
  digitalWrite(Z_DIR,  (z > 0) ? HIGH : LOW);
  delayMicroseconds(200);

  stepsTotal = max(dx, max(dy, dz));
  if (stepsTotal == 0) return;

  servoTarget = angServo;
  errX = errY = errZ = 0;
  stepsDone   = 0;
  running     = true;

  add_repeating_timer_us(-VEL_MIN_US, stepperISR, NULL, &timer);
}

// ============================
// BUFFER
// ============================

bool agregarAlBuffer(float x, float y, float z) {
  float ax = posX / PASOS_MM;
  float ay = posY / PASOS_MM;
  float az = posZ / PASOS_MM;

  if (!dentroLimites(ax + x, ay + y, az + z)) {
    Serial.println("ERROR: fuera de limites");
    return false;
  }

  int next = (bufHead + 1) % BUFFER_SIZE;
  if (next == bufTail) {
    Serial.println("ERROR: buffer lleno");
    return false;
  }

  float angServo = modoGrabado ? calcularAnguloServo(x, y) : servoCurrent;

  bufferMov[bufHead] = { x, y, z, angServo };
  bufHead = next;
  return true;
}

void ejecutarBuffer() {
  if (running)            return;
  if (bufTail == bufHead) return;

  Movimiento m = bufferMov[bufTail];
  bufTail = (bufTail + 1) % BUFFER_SIZE;
  lanzarMovimiento(m.x, m.y, m.z, m.servoAng);
}

// ============================
// JOG DIRECTO
// Genera pulsos directamente al pin — sin buffer ni ISR
// Se llama en cada iteración del loop mientras el botón esté presionado
// ============================

void jogDirecto(int stepPin, int dirPin, bool positivo,
                volatile long &pos, float limMin, float limMax) {

  float posEnMm = pos / PASOS_MM;

  if (positivo  && posEnMm >= limMax) return;
  if (!positivo && posEnMm <= limMin) return;

  digitalWrite(dirPin, positivo ? HIGH : LOW);
  delayMicroseconds(200);

  digitalWrite(stepPin, HIGH);
  delayMicroseconds(2);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(JOG_DELAY_US);

  pos += positivo ? 1 : -1;
}

// Jog eje X dual — pulsa X1 y X2 al mismo tiempo
void jogX(bool positivo) {
  float posEnMm = posX / PASOS_MM;

  if (positivo  && posEnMm >= X_MAX) return;
  if (!positivo && posEnMm <= X_MIN) return;

  digitalWrite(X1_DIR, positivo ? HIGH : LOW);
  digitalWrite(X2_DIR, positivo ? HIGH : LOW);
  delayMicroseconds(200);

  digitalWrite(X1_STEP, HIGH);
  digitalWrite(X2_STEP, HIGH);
  delayMicroseconds(2);
  digitalWrite(X1_STEP, LOW);
  digitalWrite(X2_STEP, LOW);
  delayMicroseconds(JOG_DELAY_US);

  posX += positivo ? 1 : -1;
}

// ============================
// LEER BOTONES
// ============================

void leerBotones() {

  if (running) return;

  // Jog continuo — un paso por iteración del loop
  // El motor se mueve mientras el botón esté presionado
  if      (digitalRead(PIN_X_PLUS))  jogX(true);
  else if (digitalRead(PIN_X_MINUS)) jogX(false);

  if      (digitalRead(PIN_Y_PLUS))  jogDirecto(Y_STEP, Y_DIR, true,  posY, Y_MIN, Y_MAX);
  else if (digitalRead(PIN_Y_MINUS)) jogDirecto(Y_STEP, Y_DIR, false, posY, Y_MIN, Y_MAX);

  if      (digitalRead(PIN_Z_PLUS))  jogDirecto(Z_STEP, Z_DIR, true,  posZ, Z_MIN, Z_MAX);
  else if (digitalRead(PIN_Z_MINUS)) jogDirecto(Z_STEP, Z_DIR, false, posZ, Z_MIN, Z_MAX);

  // Guardar origen XY — posición actual pasa a ser X=0 Y=0
  if (digitalRead(PIN_ORIGEN_XY) == HIGH) {
    posX = 0;
    posY = 0;
    Serial.println("Origen XY guardado — X:0 Y:0");
    delay(300);
  }

  // Guardar origen Z — altura actual pasa a ser Z=0
  if (digitalRead(PIN_ORIGEN_Z) == HIGH) {
    posZ = 0;
    Serial.println("Origen Z guardado — Z:0");
    delay(300);
  }
}

// ============================
// G-CODE
// ============================

String gcodeLinea = "";

float parsear(String linea, char letra, float porDefecto) {
  int idx = linea.indexOf(letra);
  if (idx == -1) return porDefecto;
  return linea.substring(idx + 1).toFloat();
}

void ejecutarGcode(String cmd) {
  cmd.trim();
  cmd.toUpperCase();
  if (cmd.length() == 0)   return;
  if (cmd.startsWith(";")) return;
  if (cmd.startsWith("(")) return;

  float ax = posX / PASOS_MM;
  float ay = posY / PASOS_MM;
  float az = posZ / PASOS_MM;

  int gNum = -1, mNum = -1;
  if (cmd.indexOf('G') != -1) gNum = (int)parsear(cmd, 'G', -1);
  if (cmd.indexOf('M') != -1) mNum = (int)parsear(cmd, 'M', -1);

  float x = parsear(cmd, 'X', ax);
  float y = parsear(cmd, 'Y', ay);
  float z = parsear(cmd, 'Z', az);

  // G0 — desplazamiento rápido sin cortar — servo quieto
  if (gNum == 0) {
    modoGrabado = false;
    agregarAlBuffer(x - ax, y - ay, z - az);
    Serial.println("ok");
    return;
  }

  // G1 — movimiento de corte — servo rota según dirección
  if (gNum == 1) {
    modoGrabado = true;
    agregarAlBuffer(x - ax, y - ay, z - az);
    Serial.println("ok");
    return;
  }

  // G21 — confirmar unidades en mm
  if (gNum == 21) {
    Serial.println("ok");
    return;
  }

  // G28 — homing (resetea posición a cero)
  if (gNum == 28) {
    while (running);
    posX = 0; posY = 0; posZ = 0;
    modoGrabado = false;
    Serial.println("ok");
    return;
  }

  // G92 — fijar posición actual como origen personalizado
  if (gNum == 92) {
    if (cmd.indexOf('X') != -1) posX = (long)(x * PASOS_MM);
    if (cmd.indexOf('Y') != -1) posY = (long)(y * PASOS_MM);
    if (cmd.indexOf('Z') != -1) posZ = (long)(z * PASOS_MM);
    Serial.println("ok");
    return;
  }

  // M114 — reportar posición actual
  if (mNum == 114) {
    Serial.print("X:"); Serial.print(posX / PASOS_MM);
    Serial.print(" Y:"); Serial.print(posY / PASOS_MM);
    Serial.print(" Z:"); Serial.println(posZ / PASOS_MM);
    Serial.println("ok");
    return;
  }

  // M280 — mover servo manualmente para calibración
  // Uso desde Python: M280 P90 → lleva el servo a 90°
  if (mNum == 280) {
    float ang = parsear(cmd, 'P', SERVO_CENTRO);
    servoTarget = constrain(ang, SERVO_MIN_ANG, SERVO_MAX_ANG);
    Serial.println("ok");
    return;
  }

  // Comando no reconocido — responde igual para no bloquear el Python
  Serial.print("ERROR: desconocido -> ");
  Serial.println(cmd);
}

void leerGcodeSerial() {
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n') {
      ejecutarGcode(gcodeLinea);
      gcodeLinea = "";
    } else if (c != '\r') {
      gcodeLinea += c;
    }
  }
}

// ============================
// SETUP
// ============================

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

  // Pines de motores
  pinMode(X1_STEP, OUTPUT); pinMode(X1_DIR, OUTPUT);
  pinMode(X2_STEP, OUTPUT); pinMode(X2_DIR, OUTPUT);
  pinMode(Y_STEP,  OUTPUT); pinMode(Y_DIR,  OUTPUT);
  pinMode(Z_STEP,  OUTPUT); pinMode(Z_DIR,  OUTPUT);

  // Activar los 4 drivers (EN=LOW en DRV8825)
  pinMode(X1_EN, OUTPUT); digitalWrite(X1_EN, LOW);
  pinMode(X2_EN, OUTPUT); digitalWrite(X2_EN, LOW);
  pinMode(Y_EN,  OUTPUT); digitalWrite(Y_EN,  LOW);
  pinMode(Z_EN,  OUTPUT); digitalWrite(Z_EN,  LOW);

  // Botones con pull-down — HIGH cuando se presionan
  pinMode(PIN_X_PLUS,    INPUT_PULLDOWN);
  pinMode(PIN_X_MINUS,   INPUT_PULLDOWN);
  pinMode(PIN_Y_PLUS,    INPUT_PULLDOWN);
  pinMode(PIN_Y_MINUS,   INPUT_PULLDOWN);
  pinMode(PIN_Z_PLUS,    INPUT_PULLDOWN);
  pinMode(PIN_Z_MINUS,   INPUT_PULLDOWN);
  pinMode(PIN_ORIGEN_XY, INPUT_PULLDOWN);
  pinMode(PIN_ORIGEN_Z,  INPUT_PULLDOWN);

  // Servo al centro
  servoTool.attach(SERVO_PIN);
  servoTool.write(SERVO_CENTRO);
  delay(500);

  // El Python lee este mensaje al conectarse
  Serial.println("CNC lista");
}

// ============================
// LOOP
// ============================

void loop() {
  leerBotones();       // jog directo + botones de origen
  leerGcodeSerial();   // comandos desde PC
  ejecutarBuffer();    // despachar siguiente movimiento G-code
  actualizarServo();   // mover servo suavemente

  if (softLimitHit) {
    Serial.println("ERROR: soft limit");
    softLimitHit = false;
  }
}