/*
Stepper Motor: Nema 17 + driver A4988
ATTENZIONE: nel pin VDD del A4988, dovree andare il collegamento con 3v3 dell'ESP32, tuttavia mettendo quello
non funziona. Funziona solo collegando il pin VDD con il pin 5V dell'ESP32.
Vedere i collegamenti dal progetto mio Wokwi:
https://wokwi.com/projects/447174952259917825

I miei componenti:
Motore Nema 17: https://it.aliexpress.com/item/1005005281739306.html?spm=a2g0o.order_list.order_list_main.48.6d6a36962IIKQB&gatewayAdapt=glo2ita
A4988: https://it.aliexpress.com/item/1005006476647469.html?spm=a2g0o.order_list.order_list_main.34.6d6a36962IIKQB&gatewayAdapt=glo2ita

Per la configurazione dell'A4988 vedere come settare la vite da qui:
https://www.makerslab.it/arduino-ed-i-motori-passo-passo/

Collegamenti A4988:
VMO: è l'alimentatore per il motore, bisogna usare almeno 12 Volt
GND (di fianco a VMO): GND per il motore. E' consigliabile usare un condensatore tra questo GND e VMO da almeno 47 uF
2B -> cavo Arancione Nema 17 (avvolgimento 1 output)
2A -> cavo Blu Nema 17 (avvolgimento 1 input)
1A -> cavo Rosso Nema 17 (avvolgimento 2 input)
1B -> cavo Giallo Nema 17 (avvolgimento 2 output)

Nema 11 con guida:
- A+: rosso --> 2A
- A-: blu --> 2B
- B+: verde --> 1A
- B-: nero --> 1B

VDD: alimentazione per il A4988. Si potrebbe usare da 3.3 a 5 V, ma con il pin 3V3 dell'ESP32 non va, però funziona usando il PIN 5V dell'esp32
GND: collegarlo a GND comune (anche dell'ESP32)
EN: pin di abilitazione, lasciarlo scollegato
MS1, MS2, MS3; servono per configurare il microstep (https://docs.wokwi.com/parts/wokwi-a4988)
RESET, SLEEP: collegarli insieme.
STEP: collegarlo al PIN digitale dell'ESP32
DIR: collegarlo al PIN digitale dell'ESP32

Cellula fotoelettrica per stop
Va alimentato a 5 V e quando c'è l'interruzione il seggnale è Alto
*/

#include <AccelStepper.h>
#include <ArduinoJson.h>

// ================= PIN =================
#define STEP_PIN    12
#define DIR_PIN     13
#define ENABLE_PIN  4
#define HOME_PIN    2

#define HOME_ACTIVE HIGH   // <<< sensore attivo a 5V

// ================= MOTORE =================
AccelStepper stepper(AccelStepper::DRIVER, 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 =================
const float defaultMaxSpeed = 1200;   // step/s
const float accel = 6000;              // step/s^2
const float homeSpeedFast = 600;      // step/s
const float homeBackoffSteps = 2 * steps_per_mm;

// ================= STATI =================
enum State {
  IDLE,
  HOME_FAST,
  HOME_BACKOFF,
  HOME_SLOW,
  MOVE_TO_POS,
  OSCILLATE
};

State state = IDLE;

// ================= OSCILLAZIONE =================
long centerPos = 0;
long deltaSteps = 0;
int totalCycles = 0;
int currentCycle = 0;
bool goingPositive = true;

// ================= FLAGS =================
bool homed = false;
bool stopRequested = false;

// ================= FEEDBACK =================
long lastPosReported = 0;

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

  pinMode(HOME_PIN, INPUT);     // sensore push-pull 0-5V
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, LOW);

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

  StaticJsonDocument<300> doc;
  doc["message"] = "Arduino UNO - Stepper FSM Ready";
  doc["commands"] = "HOME | MOVE pos_mm speed_mm_s | OSC delta_mm cicli speed_mm_s | STOP";
  serializeJson(doc, Serial);
  Serial.println();
}

// =================================================
void loop() {
  stepper.run();
  handleSerial();

  if (stopRequested) {
    emergencyStop();
    return;
  }

  switch (state) {
    case IDLE:           break;
    case HOME_FAST:      handleHomeFast(); break;
    case HOME_BACKOFF:   handleHomeBackoff(); break;
    case HOME_SLOW:      handleHomeSlow(); break;
    case MOVE_TO_POS:    handleMoveToPos(); break;
    case OSCILLATE:      handleOscillate(); break;
  }

  sendFeedback();
}

// ================= HANDLERS =================
void handleHomeFast() {
  if (digitalRead(HOME_PIN) == HOME_ACTIVE) {
    stepper.move(homeBackoffSteps);
    state = HOME_BACKOFF;
  }
}

void handleHomeBackoff() {
  if (stepper.distanceToGo() == 0) {
    stepper.setMaxSpeed(homeSpeedFast / 4);
    stepper.moveTo(-10000);
    state = HOME_SLOW;
  }
}

void handleHomeSlow() {
  if (digitalRead(HOME_PIN) == HOME_ACTIVE) {
    stepper.setCurrentPosition(0);
    stepper.setMaxSpeed(defaultMaxSpeed);
    homed = true;
    state = IDLE;

    StaticJsonDocument<200> doc;
    doc["event"] = "HOME_COMPLETED";
    serializeJson(doc, Serial);
    Serial.println();
  }
}

void handleMoveToPos() {
  if (stepper.distanceToGo() == 0) {
    stepper.setMaxSpeed(defaultMaxSpeed);
    state = IDLE;

    StaticJsonDocument<200> doc;
    doc["event"] = "MOVE_DONE";
    serializeJson(doc, Serial);
    Serial.println();
  }
}

void handleOscillate() {
  if (stepper.distanceToGo() == 0) {
    if (goingPositive) {
      stepper.moveTo(centerPos - deltaSteps);
      goingPositive = false;
    } else {
      currentCycle++;
      if (currentCycle >= totalCycles) {
        stepper.moveTo(centerPos);
        stepper.setMaxSpeed(defaultMaxSpeed);
        state = IDLE;

        StaticJsonDocument<200> doc;
        doc["event"] = "OSC_COMPLETED";
        serializeJson(doc, Serial);
        Serial.println();
      } else {
        stepper.moveTo(centerPos + deltaSteps);
        goingPositive = true;
      }
    }
  }
}

// ================= ERROR HELPER =================
void sendNotHomedError(const char* cmdName) {
  StaticJsonDocument<200> doc;
  doc["error"] = "NOT_HOMED";
  doc["command"] = cmdName;
  serializeJson(doc, Serial);
  Serial.println();
}

// ================= COMANDI SERIALI =================
void handleSerial() {
  if (!Serial.available()) return;

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

  if (cmd == "STOP") {
    stopRequested = true;
    return;
  }

  if (cmd == "HOME") {
    startHoming();
    return;
  }

  if (cmd.startsWith("MOVE")) {
    if (!homed) {
      sendNotHomedError("MOVE");
      return;
    }

    float pos, speed;
    if (sscanf(cmd.c_str(), "MOVE %f %f", &pos, &speed) == 2) {
      startMove(pos, speed);
    }
    return;
  }

  if (cmd.startsWith("OSC")) {
    if (!homed) {
      sendNotHomedError("OSC");
      return;
    }

    float delta, speed;
    int cicli;
    if (sscanf(cmd.c_str(), "OSC %f %d %f", &delta, &cicli, &speed) == 3) {
      startOscillation(delta, cicli, speed);
    }
    return;
  }
}

// ================= AZIONI =================
void startHoming() {
  if (state != IDLE) return;

  homed = false;
  stepper.setMaxSpeed(homeSpeedFast);
  stepper.moveTo(-100000);
  state = HOME_FAST;

  StaticJsonDocument<200> doc;
  doc["event"] = "HOME_STARTED";
  serializeJson(doc, Serial);
  Serial.println();
}

void startMove(float pos_mm, float speed_mm_s) {
  stepper.setMaxSpeed(speed_mm_s * steps_per_mm);
  centerPos = pos_mm * steps_per_mm;
  stepper.moveTo(centerPos);
  state = MOVE_TO_POS;
}

void startOscillation(float delta_mm, int cycles, float speed_mm_s) {
  stepper.setMaxSpeed(speed_mm_s * steps_per_mm);

  centerPos = stepper.currentPosition();
  deltaSteps = delta_mm * steps_per_mm;
  totalCycles = cycles;
  currentCycle = 0;
  goingPositive = true;

  stepper.moveTo(centerPos + deltaSteps);
  state = OSCILLATE;
}

// ================= EMERGENCY =================
void emergencyStop() {
  stepper.stop();
  stepper.runToPosition();
  digitalWrite(ENABLE_PIN, HIGH);

  stepper.setMaxSpeed(defaultMaxSpeed);
  state = IDLE;
  stopRequested = false;

  StaticJsonDocument<200> doc;
  doc["event"] = "EMERGENCY_STOP";
  serializeJson(doc, Serial);
  Serial.println();
}

// ================= FEEDBACK =================
void sendFeedback() {
  long pos = stepper.currentPosition();
  if (abs(pos - lastPosReported) >= steps_per_mm / 10) {
    StaticJsonDocument<200> doc;

    const char* stateName;
    switch (state) {
      case IDLE: stateName = "IDLE"; break;
      case HOME_FAST: stateName = "HOME_FAST"; break;
      case HOME_BACKOFF: stateName = "HOME_BACKOFF"; break;
      case HOME_SLOW: stateName = "HOME_SLOW"; break;
      case MOVE_TO_POS: stateName = "MOVE"; break;
      case OSCILLATE: stateName = "OSCILLATE"; break;
    }

    doc["state"] = stateName;
    doc["cycle"] = currentCycle;
    doc["pos_mm"] = pos / steps_per_mm;

    serializeJson(doc, Serial);
    Serial.println();
    lastPosReported = pos;
  }
}