#include <AccelStepper.h>

// ====== PIN ======
#define STEP_PIN    2
#define DIR_PIN     4
#define ENABLE_PIN  8     // opzionale
#define HOME_PIN    34

// ====== MOTORE ======
#define MOTOR_INTERFACE AccelStepper::DRIVER
AccelStepper stepper(MOTOR_INTERFACE, STEP_PIN, DIR_PIN);

// ====== MECCANICA ======
const float passo_vite_mm = 12.0;
const int steps_per_rev = 200;
const int microstep = 16;

const float steps_per_mm = (steps_per_rev * microstep) / passo_vite_mm;

// ====== PARAMETRI ======
float maxSpeed = 1500;     // step/s (UNO più prudente)
float accel    = 800;      // step/s^2
int homingSpeed = -600;    // negativo = verso home

// ====== STATO ======
bool homed = false;

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

  pinMode(HOME_PIN, INPUT_PULLUP);
  pinMode(ENABLE_PIN, OUTPUT);

  digitalWrite(ENABLE_PIN, LOW); // abilita driver (A4988 = LOW)

  stepper.setMaxSpeed(maxSpeed);
  stepper.setAcceleration(accel);

  Serial.println("Arduino UNO Stepper pronto");
  Serial.println("Comandi:");
  Serial.println("HOME");
  Serial.println("MOVE offset_mm delta_mm cicli");
}

// --------------------------------------------------
void loop() {
  stepper.run();

  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();

    if (cmd == "HOME") {
      homeStepper();
    }
    else if (cmd.startsWith("MOVE")) {
      parseMoveCommand(cmd);
    }
  }
}

// --------------------------------------------------
void homeStepper() {
  Serial.println("Homing...");

  stepper.setMaxSpeed(abs(homingSpeed));
  stepper.setSpeed(homingSpeed);

  // Avanza finché non trova il sensore
  while (digitalRead(HOME_PIN) == HIGH) {
    stepper.runSpeed();
  }

  // Back-off
  stepper.move(steps_per_mm * 2);
  while (stepper.isRunning()) stepper.run();

  // Riavvicinamento lento
  stepper.setSpeed(homingSpeed / 3);
  while (digitalRead(HOME_PIN) == HIGH) {
    stepper.runSpeed();
  }

  stepper.setCurrentPosition(0);
  homed = true;

  stepper.setMaxSpeed(maxSpeed);

  Serial.println("Home completata");
}

// --------------------------------------------------
void parseMoveCommand(String cmd) {
  if (!homed) {
    Serial.println("Errore: eseguire HOME prima");
    return;
  }

  float offset, delta;
  int cicli;

  int n = sscanf(cmd.c_str(), "MOVE %f %f %d", &offset, &delta, &cicli);

  if (n != 3) {
    Serial.println("Formato errato. Usa: MOVE offset delta cicli");
    return;
  }

  executeOscillation(offset, delta, cicli);
}

// --------------------------------------------------
void executeOscillation(float offset_mm, float delta_mm, int cycles) {
  long offsetSteps = offset_mm * steps_per_mm;
  long deltaSteps  = delta_mm  * steps_per_mm;

  Serial.println("Movimento a offset...");
  stepper.moveTo(offsetSteps);
  while (stepper.isRunning()) stepper.run();

  for (int i = 0; i < cycles; i++) {
    Serial.print("Ciclo ");
    Serial.println(i + 1);

    stepper.moveTo(offsetSteps + deltaSteps);
    while (stepper.isRunning()) stepper.run();

    stepper.moveTo(offsetSteps - deltaSteps);
    while (stepper.isRunning()) stepper.run();
  }

  // Ritorno all'offset
  stepper.moveTo(offsetSteps);
  while (stepper.isRunning()) stepper.run();

  Serial.println("Sequenza completata");
}