#include <Arduino.h>

// -------------------------------
// Pines Step/Dir/Enable
// -------------------------------
#define X_STEP 2
#define X_DIR  3
#define X_EN   4

#define Y_STEP 5
#define Y_DIR  6
#define Y_EN   7

#define Z_STEP 8
#define Z_DIR  9
#define Z_EN   10

// Endstops
#define X_MIN 19
#define Y_MIN 20
#define Z_MIN 21

// Dispensador
#define DISPENSER_PIN 22

// LED de estado (si aplica en tu placa)
#define LED_PIN 25

// -------------------------------
// Objeto CNC
// -------------------------------
CNCServo42 cnc(X_STEP, X_DIR, X_EN,
               Y_STEP, Y_DIR, Y_EN,
               Z_STEP, Z_DIR, Z_EN);

void printMenu(){
  Serial1.println(F("\n===== MENU CNC ====="));
  Serial1.println(F("1) Home simultaneo"));
  Serial1.println(F("2) Movimiento lineal demo"));
  Serial1.println(F("3) Circulo (dispensador ON durante todo el patron)"));
  Serial1.println(F("4) Espiral (dispensador ON durante todo el patron)"));
  Serial1.println(F("5) Puntos aleatorios (dispensador aleatorio por punto)"));
  Serial1.println(F("m) Mostrar menu"));
  Serial1.print(F("> "));
}

void setup() {
  Serial1.begin(115200);
  delay(300);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);

  cnc.setDispenserPin(DISPENSER_PIN);
  cnc.begin();
  cnc.setStepsPerMM(80, 80, 800);   // Ajusta a tu mecánica real
  cnc.setFeedrate(1800);            // mm/min
  cnc.setAcceleration(400);         // mm/s^2 (placeholder)
  cnc.setZero();

  cnc.setEndstops(X_MIN, Y_MIN, Z_MIN);
  cnc.setHomingParams(2000, 500, 0, 0, 0);

  // Semilla para aleatorios
  randomSeed(analogRead(A0));

  Serial.println(F("CNCServo42 listo."));
  printMenu();
}

void loop() {
  if (Serial1.available()){
    char c = Serial1.read();

    switch(c){
      case '1': {
        Serial1.println(F("\nHoming simultaneo..."));
        digitalWrite(LED_PIN, HIGH);
        cnc.homeAllSimultaneous();
        digitalWrite(LED_PIN, LOW);
        Serial1.println(F("Homing OK."));
        break;
      }

      case '2': {
        Serial1.println(F("\nMovimiento lineal demo..."));
        // Pequeño cuadrado
        cnc.moveLinearTrapezoid( 30,  0, 10, 2000);
        cnc.moveLinearTrapezoid( 30, 30, 10, 2000);
        cnc.moveLinearTrapezoid(  0, 30, 10, 2000);
        cnc.moveLinearTrapezoid(  0,  0, 10, 2000);
        cnc.moveLinearTrapezoid(  0,  0,  0, 2000);
        Serial1.println(F("Lineal demo terminado."));
        break;
      }

      case '3': {
        Serial1.println(F("\nPatron CIRCULO... (dispensador ON durante el patron)"));
        float cx=50, cy=50, cz=10, R=30;
        cnc.moveCircle(cx, cy, cz, R, 160, 2000);
        Serial1.println(F("Circulo terminado."));
        break;
      }

      case '4': {
        Serial1.println(F("\nPatron ESPIRAL... (dispensador ON durante el patron)"));
        float cx=50, cy=50, cz=10;
        cnc.moveSpiral(cx, cy, cz, /*maxRadius*/ 35, /*stepRadius*/ 5,
                       /*pointsPerTurn*/ 160, /*feedrate*/ 2000);
        Serial1.println(F("Espiral terminada."));
        break;
      }

      case '5': {
        Serial1.println(F("\nPuntos aleatorios dentro del circulo..."));
        float cx=50, cy=50, cz=10, R=30;
        cnc.moveRandomPointsInCircle(cx, cy, cz, R,
                                     /*numPoints*/ 20,
                                     /*minMs*/ 80, /*maxMs*/ 300);
        Serial1.println(F("Puntos aleatorios terminados."));
        break;
      }

      case 'm':
      case 'M':
        printMenu();
        break;

      case '\r':
      case '\n':
        // ignora saltos de línea
        break;

      default:
        Serial1.println(F("Opcion invalida."));
        printMenu();
        break;
    }

    Serial1.print(F("> "));
  }
}