#define arduino.h

////////////////////// Stepper Teensy V21 (closed loop) WIP /////////////////////////

// ////////////////////// DOUBLE TO STRING CONVERSION /////////////////////////
// // Convert double to string macro with specified precision
// String dToStr(double val, byte precision = 6) {
//   String s = String(int(val));
//   if (precision > 0) {
//     s += ".";
//   }
//   unsigned long frac;
//   unsigned long mult = 1;
//   byte padding = precision - 1;
//   while (precision--) {
//     mult *= 10;
//   }
//   if (val >= 0) {
//     frac = (val - int(val)) * mult;
//   } else {
//     frac = (int(val) - val) * mult;
//   }
//   unsigned long frac1 = frac;
//   while (frac1 /= 10) {
//     padding--;
//   }
//   while (padding--) {
//     s += "0";
//   }
//   s += String(frac, DEC);
//   return s;
// }

////////////////////// MICROCONTRLOLER SELECTION - PIN ASSIGNMENTS /////////////////////////

// Uncomment one of these lines to select your microcontroller
// #define USE_TEENSY
// #define USE_MEGA
#define USE_NANO_2040

// Define pin assignments based on the selected microcontroller
#ifdef USE_TEENSY

// Global system pins
#define   OPEN_PIN                 2  // Open pin
#define   OPT_DIS_PIN              3  // Optical disable pin
#define   CRUSH_PREV_PIN           4  // Crush prevention pin
#define   AUTO_CALIBRATION_PIN    20  // Auto Calibration start (two pushes)
#define   AUTO_CALIBRATION_LED    19  // LED pin
#define   SETUP_LED               13  // LED pin

// M1 pins
#define   STEP_PIN_M1              6  // Step pin for M1
#define   DIR_PIN_M1               7  // Direction pin for M1
#define   LATCH_PIN_M1            18  // Latch pin for M1 A1 or 10
#define   UNLATCH1_EXT_PIN_M1     14  // Unlatch pin for M1 A2 or 11
#define   UNLATCH1_RET_PIN_M1     15  // Unlatch pin for M1 A2 or 11
#define   OPEN_LIMIT_PIN_M1       12  // Open limit pin for M1
#define   CLOSE_LIMIT_PIN_M1      11  // Close limit pin for M1

#elif defined(USE_MEGA)

// Global system pins
#define   OPEN_PIN                 2  // Open pin
#define   OPT_DIS_PIN              3  // Optical disable pin
#define   CRUSH_PREV_PIN           4  // Crush prevention pin
#define   AUTO_CALIBRATION_PIN    20  // Auto Calibration start (two pushes)
#define   AUTO_CALIBRATION_LED    19  // LED pin
#define   SETUP_LED               13  // LED pin            

// M1 pins
#define   STEP_PIN_M1              6  // Step pin for M1
#define   DIR_PIN_M1               7  // Direction pin for M1
#define   LATCH_PIN_M1            14  // Latch pin for M1 A1 or 10
#define   UNLATCH1_EXT_PIN_M1      9  // Unlatch pin for M1 A2 or 11
#define   UNLATCH1_RET_PIN_M1     10  // Unlatch pin for M1 A2 or 11
#define   OPEN_LIMIT_PIN_M1       15  // Open limit pin for M1
#define   CLOSE_LIMIT_PIN_M1      16  // Close limit pin for M1

#elif defined(USE_NANO_2040)

// Global system pins
#define   OPEN_PIN                 2  // Open pin
#define   OPT_DIS_PIN              3  // Optical disable pin
#define   CRUSH_PREV_PIN           4  // Crush prevention pin
#define   AUTO_CALIBRATION_PIN    20  // Auto Calibration start (two pushes)
#define   AUTO_CALIBRATION_LED    19  // LED pin
#define   SETUP_LED               13  // LED pin            

// M1 pins
#define   STEP_PIN_M1              6  // Step pin for M1
#define   DIR_PIN_M1               7  // Direction pin for M1
#define   LATCH_PIN_M1            14  // Latch pin for M1
#define   UNLATCH1_EXT_PIN_M1      9  // Unlatch pin for M1
#define   UNLATCH1_RET_PIN_M1     10  // Unlatch pin for M1
#define   OPEN_LIMIT_PIN_M1       15  // Open limit pin for M1
#define   CLOSE_LIMIT_PIN_M1      16  // Close limit pin for M1
#endif

////////////////////// VARIBLES, FUNCTIONS AND ARRAY DECLARATIONS /////////////////////////

// Enumeration for system states
enum SystemState {
  OPENING,
  CLOSING,
  OPENED,
  CLOSED,
  PARTIAL,
  RAPID_STOP,
};

// Enumeration for auto-calibration phases
enum AutoCalibrationPhase {
  SYSTEM_NEEDS_CALIBRATION,
  CALIBRATION_OPENING,
  CALIBRATION_CLOSING,
  SYSTEM_CALIBRATED,
  CALIBRATION_ABORTED
};

// Enumeration for curve types
enum CurveType {
  LINEAR,
  QUADRATIC,
  CUBIC,
  QUARTIC,
  QUINTIC
};

// Signal states and validation arrays
#define       NUM_SIGNALS          11
boolean       signalState[NUM_SIGNALS],
              lastSignalState[NUM_SIGNALS];
int           highSignalValidationCounter[NUM_SIGNALS],
              lowSignalValidationCounter[NUM_SIGNALS];
const int     SIGNAL_PINS[NUM_SIGNALS] = {
  OPEN_PIN,
  OPT_DIS_PIN,
  CRUSH_PREV_PIN,
  OPEN_LIMIT_PIN_M1,
  CLOSE_LIMIT_PIN_M1,
  LATCH_PIN_M1,
  UNLATCH1_EXT_PIN_M1,
  UNLATCH1_RET_PIN_M1,
  AUTO_CALIBRATION_PIN,
};

// Precalculated delays array
#define       MAX_RAMP_STEPS 300
double        delays[MAX_RAMP_STEPS];

//
#define       CALIBRATION_CYCLES    3
int calibrationStepcountsM1[CALIBRATION_CYCLES];

// Global input variables
#define       EPSILON                   0.00000024
#define       microsTolerance           100
const         CurveType curve           = QUARTIC;
const int     pulleyPace                = 2,
              numberTeethPulley         = 20,
              stepPerRevolution         = 400;
int           stopMovementSteps         = 20;
const double  tTime                     = 5,
              accRampTime               = 14,
              t0                        = 0,
              autoCalibrationDelays     = 2;

// M1 input variables
const double  distanceM1                = 340;
unsigned long latchDurationM1           = 2000000,
              unLatchDurationM1         = 2000000,
              motorToUnLatch1DelayM1    = 200000;

// Global system variables
boolean       stepperMovement,
              systemStateChange,
              systemFailsafeActivated,
              autoCalibrationState;
SystemState   currentSystemState,
              systemState,
              previousSystemState,
              oldSystemState;
AutoCalibrationPhase
autoCalibrationPhase;
int           opClCounter,
              previousOpClCounter,
              previousAutoCalibCounter,
              autoCalibCounter,
              stopMovementIndex,
              currentCalibrationCycle,
              sumCalibrationStepCountsM1,
              calibratedOpenStepTargetM1,
              key;
double        rampTime,
              distancePerRev,
              stepLength,
              sumDelay;
unsigned long currentMicros,
              currentMillis,
              lastDebounceTime,
              debounceDelay,
              lastDebugTime;

// M1 system variables
boolean       stepperTriggerMicrosM1,
              unLatchCalledM1,
              unLatchClearedM1,
              unLatchClearedMicrosTriggerM1,
              latchCalledM1;
int           latchStatusM1,
              unLatchStatusM1,
              stepCountM1,
              delayStepCountM1,
              rampStepsM1,
              pRampStepsM1,
              stepTargetM1,
              openStepTargetM1,
              oldOpenStepTargetM1,
              closeStepTargetM1,
              openStepsLeftM1,
              closeStepsLeftM1,
              vMaxStepsM1,
              rpmM1;
unsigned long unLatchMicrosM1,
              unLatchPin2StartMicrosM1,
              motorToUnLatch1StartMicrosM1,
              unLatchSafetyMicrosM1,
              unLatchClearedMicrosM1;
double        stepDelayM1,
              vMaxDelayM1,
              vMaxM1,
              vMinM1;

// Function prototypes
double        steps(const double x, const double vMinM1, const double vMaxM1),
              findStepTimes(int rampStepsM1),
              calculateRampSteps(),
              calculateDelayForStep(int step),
              delayM1();
void          stepControlM1(boolean direction),
              unLatchControlM1(),
              validateAndDebounceSignals(),
              manageOpClCounter(),
              manageUnLatchSafety(),
              manageAutoCalibCounter(),
              evaluateSystemState(),
              handleOpeningState(),
              handleClosingState(),
              handleOpenedState(),
              handleClosedState(),
              handlePartialState(),
              handleLatchingState(),
              handleUnlatchingState(),
              handleRapidStop(),
              debug(int active, int period),
              precomputeDelays(),
              autoCalibration(),
              systemFailsafe(),
              handleTouch ();
double        calculateDelayForStep(int step);

////////////////////// LIBRARIES /////////////////////////

#include <ArduinoBearSSL.h>
#include <ArduinoECCX08.h>
#include <ArduinoMqttClient.h>
#include <WiFiNINA.h> // change to #include <WiFi101.h> for MKR1000


////////////////////// WIFI AND MQTT CONNECTION /////////////////////////
#include "arduino_secrets.h"

const char ssid[]        = SECRET_SSID;
const char pass[]        = SECRET_PASS;
const char broker[]      = "your-aws-iot-endpoint"; // Replace with your AWS IoT endpoint
const char* certificate  = "path/to/your/certificate.pem.crt"; // Path to your certificate
const char* privateKey = "path/to/your/private.pem.key"; // Path to your private key
const char* rootCA = "path/to/your/AmazonRootCA1.pem"; // Path to the root CA

WiFiClient    wifiClient;            // Used for the TCP socket connection
BearSSLClient sslClient(wifiClient); // Used for SSL/TLS connection, integrates with ECC508
MqttClient    mqttClient(sslClient);

unsigned long lastMillis = 0;

unsigned long getTime() {
  return WiFi.getTime();
}

void connectWiFi() {
  Serial.print("Attempting to connect to SSID: ");
  Serial.print(ssid);
  Serial.print(" ");

  while (WiFi.begin(ssid, pass) != WL_CONNECTED) {
    Serial.print(".");
    delay(5000);
  }
  Serial.println();

  Serial.println("You're connected to the network");
  Serial.println();
}

void onMessageReceived(int messageSize) {
  Serial.print("Received a message with topic '");
  Serial.print(mqttClient.messageTopic());
  Serial.print("', length ");
  Serial.print(messageSize);
  Serial.println(" bytes:");

  String message;
  while (mqttClient.available()) {
    message += (char)mqttClient.read();
  }
  Serial.println(message);

  if (message == "open") {
    highSignalValidationCounter[0] = 1; // Simulate open button press
  } else if (message == "auto_calibrate") {
    highSignalValidationCounter[10] = 2; // Simulate auto calibration button press
  }
}

void connectMQTT() {
  mqttClient.setId("ATLNANOIoT01");

  Serial.print("Attempting to MQTT broker: ");
  Serial.println(broker);

  while (!mqttClient.connected()) {
    if (mqttClient.connect(broker, 8883)) {
      Serial.println("You're connected to the MQTT broker!");
      break;
    } else {
      Serial.print("MQTT connection failed, error: ");
      Serial.println(mqttClient.connectError());
      delay(5000);
    }
  }

  mqttClient.subscribe("arduino/incoming");
}

String getStateName(SystemState state) {
  switch (state) {
    case OPENING: return "OPENING";
    case CLOSING: return "CLOSING";
    case OPENED: return "OPENED";
    case CLOSED: return "CLOSED";
    case PARTIAL: return "PARTIAL";
    case RAPID_STOP: return "RAPID_STOP";
    default: return "UNKNOWN";
  }
}

// Modify the publishMessage function to use getStateName
void publishMessage() {
  Serial.println("Publishing message");

  mqttClient.beginMessage("arduino/outgoing");

  // Create a JSON object to hold the state and step count information
  String message = "{";
  message += "\"state\":\"" + getStateName(currentSystemState) + "\",";
  message += "\"stepCount\":" + String(stepCountM1);
  message += "}";

  mqttClient.print(message);
  mqttClient.endMessage();
}

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

  if (!ECCX08.begin()) {
    Serial.println("No ECCX08 present!");
    while (1);
  }

  ArduinoBearSSL.onGetTime(getTime);
  sslClient.setEccSlot(0, certificate);
  sslClient.setPrivateKey(privateKey);
  sslClient.setRootCA(rootCA);

  mqttClient.onMessage(onMessageReceived);

  pinMode     (OPEN_PIN, INPUT);
  pinMode     (OPT_DIS_PIN, INPUT);
  pinMode     (CRUSH_PREV_PIN, INPUT);
  pinMode     (AUTO_CALIBRATION_PIN, INPUT);
  pinMode     (AUTO_CALIBRATION_LED, OUTPUT);

  pinMode     (OPEN_LIMIT_PIN_M1, INPUT);
  pinMode     (CLOSE_LIMIT_PIN_M1, INPUT);
  pinMode     (STEP_PIN_M1, OUTPUT);
  pinMode     (DIR_PIN_M1, OUTPUT);
  pinMode     (LATCH_PIN_M1, OUTPUT);
  pinMode     (UNLATCH1_EXT_PIN_M1, OUTPUT);
  pinMode     (UNLATCH1_RET_PIN_M1, OUTPUT);

  lastDebounceTime                = 0;
  lastDebugTime                   = 0;
  debounceDelay                   = 10;
  systemState                     = CLOSED;  // Set the initial system state to CLOSED
  currentSystemState              = CLOSED;  // Ensure currentSystemState matches the initial state
  previousSystemState             = CLOSED;  // Set previousSystemState to CLOSED for initial setup
  opClCounter                     = 0;
  previousOpClCounter             = 0;
  previousAutoCalibCounter        = 0;
  autoCalibCounter                = 0;
  stopMovementIndex               = 0;
  currentCalibrationCycle         = 0;
  sumCalibrationStepCountsM1      = 0;
  calibratedOpenStepTargetM1      = openStepTargetM1;
  systemStateChange               = false;
  systemFailsafeActivated         = false;
  autoCalibrationState            = false;
  stepperMovement                 = false;
  stepperTriggerMicrosM1          = false;
  unLatchCalledM1                 = false;
  unLatchClearedM1                = false;
  unLatchClearedMicrosTriggerM1   = false;
  latchCalledM1                   = false;
  distancePerRev                  = numberTeethPulley * pulleyPace;
  rampTime                        = tTime * accRampTime / 100;
  stepLength                      = distancePerRev / stepPerRevolution;
  stepCountM1                     = 0;
  delayStepCountM1                = 0;
  closeStepTargetM1               = 0;
  vMinM1                          = 3.5;
  vMaxM1                          = (distanceM1 - (rampTime * vMinM1)) / (tTime - rampTime);
  stepTargetM1                    = distanceM1 / (numberTeethPulley * pulleyPace) * stepPerRevolution;
  openStepTargetM1                = stepTargetM1;
  oldOpenStepTargetM1             = openStepTargetM1;
  calibratedOpenStepTargetM1      = 0;
  rampStepsM1                     = calculateRampSteps();
  vMaxDelayM1                     = findStepTimes(rampStepsM1) - findStepTimes(rampStepsM1 - 1);
  unLatchStatusM1                 = 0;
  latchStatusM1                   = 0;
  unLatchMicrosM1                 = 0;
  unLatchPin2StartMicrosM1        = 0;
  motorToUnLatch1StartMicrosM1    = 0;
  unLatchSafetyMicrosM1           = 1.25 * unLatchDurationM1;

  digitalWrite(LATCH_PIN_M1, LOW);
  digitalWrite(UNLATCH1_EXT_PIN_M1, LOW);
  digitalWrite(UNLATCH1_RET_PIN_M1, LOW);

  for (int i = 0; i < NUM_SIGNALS; i++) {
    signalState[i]                 = 0;
    lastSignalState[i]             = 0;
    highSignalValidationCounter[i] = 0;
    lowSignalValidationCounter[i]  = 0;
  }

  precomputeDelays();

  // Setup completed
  digitalWrite(SETUP_LED, HIGH);
}

////////////////////// MAIN LOOP /////////////////////////

void loop() {
  if (WiFi.status() != WL_CONNECTED) {
    connectWiFi();
  }

  if (!mqttClient.connected()) {
    connectMQTT();
  }

  mqttClient.poll(); // Ensure frequent polling of the MQTT client

  currentMicros = micros();
  currentMillis = millis();

  // Publish message more frequently to update the state and step count in real-time
  if (millis() - lastMillis > 1000) { // Adjust the interval as needed
    lastMillis = millis();
    publishMessage();
  }

  validateAndDebounceSignals();
  manageOpClCounter();
  manageAutoCalibCounter();
  manageUnLatchSafety();
  systemFailsafe();
  evaluateSystemState();
  autoCalibration();
  // debug(1, 100);
}

////////////////////// DATA PROCESSING /////////////////////////

double steps(const double x, const double vMinM1, const double vMaxM1) {
  switch (curve) {
    case LINEAR:
      return (vMinM1 + 0.5 * (vMaxM1 - vMinM1) * x) * x;
    case QUADRATIC:
      return (vMinM1 + 0.5 * (vMaxM1 - vMinM1) * x * x * (2 - x)) * x;
    case CUBIC:
      return (vMinM1 + 0.5 * (vMaxM1 - vMinM1) * x * x * x * (5 - x * (6 - x * 2))) * x;
    case QUARTIC:
      return (vMinM1 + 0.5 * (vMaxM1 - vMinM1) * x * x * x * x * (14 - x * (28 - x * (20 - x * 5)))) * x;
    case QUINTIC:
      return (vMinM1 + 0.5 * (vMaxM1 - vMinM1) * x * x * x * x * x * (42 - x * (120 - x * (135 - x * (70 - x * 14))))) * x;
  }
}

double findStepTimes(int rampStepsM1) {
  const double target_s = rampStepsM1 * stepLength / (rampTime - t0);
  double x0 = 0.0, x1 = 1.0, xeps = EPSILON * stepLength / (rampTime - t0);
  while (true) {
    const double x = 0.5 * (x0 + x1);
    if (x1 - x0 <= xeps || x == x0 || x == x1) {
      return (1 - x) * t0 + x * rampTime;
    }
    const double s = steps(x, vMinM1, vMaxM1);
    if (s < target_s) {
      x0 = x;
    }
    else if (s > target_s) {
      x1 = x;
    }
    else {
      return (1 - x) * t0 + x * rampTime;
    }
  }
}

double calculateRampSteps() {
  return (rampTime - t0) * (vMaxM1 + vMinM1) / (2 * stepLength);
}

double calculateDelayForStep(int step) {
  double stepTime1 = findStepTimes(step);
  double stepTime2 = findStepTimes(step + 1);
  return stepTime2 - stepTime1;
}

void precomputeDelays() {
  for (int i = 0; i < 299; i++) {
    delays[i] = calculateDelayForStep(i);
  }
  delays[299] = 0;
}

double delayM1() {
  if (systemState == RAPID_STOP)  {
    if (stopMovementIndex > 0 && stopMovementIndex < stopMovementSteps) {
      key = 7000 + stopMovementIndex;
      double stopMovementDelays = delays[stopMovementIndex];
      stopMovementIndex--;
      return stopMovementDelays;
    } else if (stopMovementIndex == 0) {
      key = 8000;
      double stopMovementDelays = delays[stopMovementIndex];
      stopMovementIndex = stopMovementIndex - 1;
      return stopMovementDelays;
    }
  }

  if (systemState == OPENING) {
    key = 1;
    if (stepCountM1 <= pRampStepsM1) {
      key = 2;
      return delays[delayStepCountM1];
    }
    if (stepCountM1 > pRampStepsM1 && stepCountM1 < vMaxStepsM1 + pRampStepsM1) {
      key = 4;
      if (previousSystemState == PARTIAL && systemState == OPENING) {
        key = 5;
        if (delayStepCountM1 <= pRampStepsM1) {
          key = 6;
          return delays[delayStepCountM1];
        } else {
          key = 7;
          return vMaxDelayM1;
        }
      } else {
        key = 8;
        return vMaxDelayM1;
      }
    } else if (stepCountM1 >= vMaxStepsM1 + pRampStepsM1) {
      key = 9;
      if (previousSystemState == PARTIAL && systemState == OPENING) {
        key = 10;
        if (delayStepCountM1 <= pRampStepsM1) {
          key = 11;
          return delays[delayStepCountM1];
        } else {
          key = 12;
          int index = openStepsLeftM1 - delayStepCountM1;

          if (index >= 0 && index < MAX_RAMP_STEPS) {
            return delays[index];
          } else {
            key = 106;
            return vMaxDelayM1;
          }
        }
      } else {
        key = 13;
        int index = openStepTargetM1 - delayStepCountM1;
        if (index >= 0 && index < MAX_RAMP_STEPS) {
          return delays[index];
        } else {
          key = 109;
          return vMaxDelayM1;
        }
      }
    }
  }

  if (systemState == CLOSING) {
    key = 14;
    if (stepCountM1 >= closeStepsLeftM1 - pRampStepsM1) {
      key = 15;
      return delays[delayStepCountM1];
    }
    if (stepCountM1 > pRampStepsM1 && stepCountM1 < vMaxStepsM1 + pRampStepsM1) {
      key = 16;
      if (previousSystemState == PARTIAL && systemState == CLOSING) {
        key = 17;
        if (delayStepCountM1 <= pRampStepsM1) {
          key = 18;
          return delays[delayStepCountM1];
        } else {
          key = 19;
          return vMaxDelayM1;
        }
      } else {
        key = 20;
        return vMaxDelayM1;
      }
    } else if (stepCountM1 <= pRampStepsM1) {
      key = 21;
      if (previousSystemState == PARTIAL && systemState == CLOSING) {
        key = 22;
        if (delayStepCountM1 <= pRampStepsM1) {
          key = 23;
          return delays[delayStepCountM1];
        } else {
          key = 24;
          int index = closeStepsLeftM1 - delayStepCountM1;
          if (index >= 0 && index < MAX_RAMP_STEPS) {
            return delays[index];
          } else {
            key = 107;
            return vMaxDelayM1;
          }
        }
      } else {
        key = 25;
        int index = closeStepsLeftM1 - delayStepCountM1;

        if (index >= 0 && index < MAX_RAMP_STEPS) {
          return delays[index];
        } else {
          key = 108;
          return vMaxDelayM1;
        }
      }
    }
  }
  return 0;
}

void setupRampSteps(int& stepsLeft, int& pRampSteps, int& vMaxSteps, bool isOpening) {
  stepsLeft                         = isOpening ? openStepTargetM1 - stepCountM1 : stepCountM1;
  pRampSteps                        = (stepsLeft >= 2 * rampStepsM1) ? rampStepsM1 : stepsLeft / 2;
  vMaxSteps                         = stepsLeft - 2 * pRampSteps;
  if (vMaxSteps == 1) vMaxSteps     = 0;
}

////////////////////// SIGNALS VALIDATION /////////////////////////

void validateAndDebounceSignals() {
  if ((currentMillis - lastDebounceTime) > debounceDelay) {
    for (int i = 0; i < NUM_SIGNALS; i++) {
      if (SIGNAL_PINS[i] == OPEN_PIN && unLatchStatusM1 == 1) {
        continue;
      }
      signalState[i] = digitalRead(SIGNAL_PINS[i]);
      if (signalState[i] != lastSignalState[i]) {
        lastDebounceTime = currentMillis;
        if (signalState[i] == HIGH) {
          highSignalValidationCounter[i]++;
          highSignalValidationCounter[0] = highSignalValidationCounter[0] % 4;
        }
        else {
          lowSignalValidationCounter[i]++;
        }
      }
      lastSignalState[i] = signalState[i];
    }
  }
}

////////////////////// TRIGGER MANAGEMENT /////////////////////////

void manageOpClCounter() {
  int newOpClCounter = 0;
  if (highSignalValidationCounter[0] % 4 == 1) {
    newOpClCounter = 1;
  }
  else if (highSignalValidationCounter[0] % 4 == 3) {
    newOpClCounter = 2;
  }
  if (newOpClCounter != opClCounter) {
    previousOpClCounter = opClCounter;
    opClCounter = newOpClCounter;
  }
}

void manageAutoCalibCounter() {
  int newAutoCalibCounter = 0;
  if (highSignalValidationCounter[10] >= 2) {
    newAutoCalibCounter = 1;
  }
  else {
    newAutoCalibCounter = 0;
  }

  if (newAutoCalibCounter != autoCalibCounter) {
    previousAutoCalibCounter = autoCalibCounter;
    autoCalibCounter = newAutoCalibCounter;
  }
}

void manageUnLatchSafety() {
  if (unLatchClearedMicrosTriggerM1 && currentMicros - unLatchClearedMicrosM1 >= unLatchSafetyMicrosM1) {
    unLatchClearedM1 = true;
    unLatchClearedMicrosTriggerM1 = false;
    Serial.println("Entrei");
  }
  else if (opClCounter == 2 && unLatchClearedMicrosTriggerM1) {
    highSignalValidationCounter[0] = 2;
  }
}


////////////////////// STEPPER MOTORS MOTION CONTROL /////////////////////////

void stepControlM1(boolean direction) {
  static unsigned long lastMicros = 0;
  static boolean stepState = LOW;
  unsigned long stepDelayMicros = 1000000L * stepDelayM1;
  digitalWrite(DIR_PIN_M1, direction);
  if (stepperMovement == true); {
    if (currentMicros - lastMicros >= stepDelayMicros) {
      stepState = !stepState;
      digitalWrite(STEP_PIN_M1, stepState);
      lastMicros = currentMicros;
      sumDelay += stepDelayM1;
      if (stepState == LOW) {
        stepDelayM1 = delayM1();
        stepCountM1 += (direction == HIGH) ? 1 : -1;
        if (systemState == CLOSING || systemState == OPENING) {
          delayStepCountM1++;
        }
        if (fabs(stepDelayM1) < EPSILON) {
          rpmM1 = 0.0;
        } else {
          rpmM1 = ((stepLength / stepDelayM1) / distancePerRev) * 60.0;
        }
      }
    }
    else {
      return;
    }
  }
}

////////////////////// FAILSAFE MANAGEMENT /////////////////////////

void systemFailsafe() {
  if ((highSignalValidationCounter[0] == 1 ||
       highSignalValidationCounter[0] == 3 ||
       systemState == OPENING              ||
       systemState == CLOSING)
      &&
      (signalState[1] == HIGH              ||
       signalState[2] == HIGH))
  {
    systemFailsafeActivated = true;
    key = 5555;
    highSignalValidationCounter[0] = (opClCounter == 1) ? 2 : 0;
  }
  if (systemFailsafeActivated) {
    if ((signalState[1] == LOW &&
         signalState[2] == LOW)
        &&
        (opClCounter == 1                  ||
         opClCounter == 2))
    {
      systemFailsafeActivated = false;
    }
  }
}

////////////////////// AUTO CALIBRATION /////////////////////////

void autoCalibration() {
  static unsigned long autoCalibrationMicros = 0;
  static int lastCalibrationHalfCycle = 0;
  static int calibrationHalfCycle = 0;

  if (autoCalibCounter == 1 && !autoCalibrationState) {
    Serial.println("ENTREI ERRADO");
    autoCalibrationState = true;
    digitalWrite(AUTO_CALIBRATION_LED, HIGH);
    highSignalValidationCounter[10] = 0;
    autoCalibCounter = 0;
    autoCalibrationMicros = currentMicros;
    oldOpenStepTargetM1 = openStepTargetM1;
  }

  if (autoCalibrationState && (systemFailsafeActivated || signalState[0] == HIGH)) {
    autoCalibrationState = false;
    systemState = RAPID_STOP;
    return;
  }

  if (autoCalibrationState && currentCalibrationCycle < CALIBRATION_CYCLES) {

    if (systemState == CLOSED) {
      if (lastCalibrationHalfCycle != calibrationHalfCycle) {
        autoCalibrationMicros = currentMicros; // Reset timing for the new cycle
        lastCalibrationHalfCycle = calibrationHalfCycle; // Update to current cycle
      }

      else if (currentMicros - autoCalibrationMicros > 2000000) { // 2-second delay
        highSignalValidationCounter[0] = 1; // Start open cycle
        validateAndDebounceSignals();
        manageOpClCounter();
        evaluateSystemState();
        autoCalibrationPhase = CALIBRATION_OPENING;
        calibrationHalfCycle++;// Increment the calibration half cycle
      }
    }

    else if (systemState == OPENED) {
      if (lastCalibrationHalfCycle != calibrationHalfCycle) {
        calibrationStepcountsM1[currentCalibrationCycle] = stepCountM1;
        sumCalibrationStepCountsM1 += calibrationStepcountsM1[currentCalibrationCycle];
        autoCalibrationMicros = currentMicros; // Reset timing for the new cycle
        lastCalibrationHalfCycle = calibrationHalfCycle; // Update to current cycle
      }

      else if (currentMicros - autoCalibrationMicros > 2000000) { // 2-second delay
        highSignalValidationCounter[0] = 3; // Start close cycle
        validateAndDebounceSignals();
        manageOpClCounter();
        evaluateSystemState();
        autoCalibrationPhase = CALIBRATION_CLOSING;
        calibrationHalfCycle++;
        currentCalibrationCycle++;
        if (currentCalibrationCycle == CALIBRATION_CYCLES) {
          autoCalibrationState = false;
          calibratedOpenStepTargetM1 = sumCalibrationStepCountsM1 / CALIBRATION_CYCLES;
          openStepTargetM1 = calibratedOpenStepTargetM1;

          autoCalibrationPhase = SYSTEM_CALIBRATED;
          digitalWrite(AUTO_CALIBRATION_LED, LOW);
        }
      }
    }
  }
  return;
}

////////////////////// STATE MACHINE LOGIC MANAGEMENT /////////////////////////

void evaluateSystemState() {
  // Prevent actions if failsafe is active and OPEN_PIN is HIGH
  if (systemFailsafeActivated && signalState[0] == HIGH) {
    return;  // Do nothing but still evaluate conditions that may clear the failsafe
  }

  if (currentSystemState != systemState) {
    systemStateChange = true;
    oldSystemState = previousSystemState;
    previousSystemState = currentSystemState;
    currentSystemState = systemState;
  } else {
    systemStateChange = false;
  }

  switch (systemState) {
    case OPENING:
      handleOpeningState();
      break;
    case CLOSING:
      handleClosingState();
      break;
    case OPENED:
      handleOpenedState();
      break;
    case CLOSED:
      handleClosedState();
      break;
    case PARTIAL:
      handlePartialState();
      break;
    case RAPID_STOP:
      handleRapidStop();
      break;
    default:
      break;
  }
}

////////////////////// STATE MACHINE SUPPORTING FUNCTIONS /////////////////////////

void handleOpeningState() {
  
  if (latchStatusM1 == 1) {
    Serial.println("DESLATCH");                 // Indicate latching started
    digitalWrite(LATCH_PIN_M1, LOW);          // Set pin high (latching)
    latchStatusM1 = 0;                         // Update status to "latched"
  } 
  
// Initialize unlatch sequence
if (!unLatchCalledM1 && (previousSystemState == CLOSED || systemState == CLOSED) && 
    !stepperTriggerMicrosM1 && unLatchStatusM1 == 0) {
    unLatchMicrosM1 = currentMicros;  // Start timing
    unLatchStatusM1 = 1;             // Move to the first state
    unLatchCalledM1 = true;
    unLatchClearedM1 = false;
}

// Handle unlatching sequence
switch (unLatchStatusM1) {
    case 1:  // Start extension
        digitalWrite(UNLATCH1_EXT_PIN_M1, HIGH);
        key = 2000;                  // Optional debugging or signaling
        unLatchStatusM1 = 2;         // Move to the next state
        break;

    case 2:  // Transition from EXT to RET
        if ((currentMicros - unLatchMicrosM1 >= unLatchDurationM1) && 
            digitalRead(UNLATCH1_EXT_PIN_M1) == HIGH) {
            digitalWrite(UNLATCH1_EXT_PIN_M1, LOW);  // Stop EXT
            digitalWrite(UNLATCH1_RET_PIN_M1, HIGH); // Start RET
            unLatchStatusM1 = 3;                     // Move to the next state
        }
        break;

    case 3:  // Complete retraction
        if ((currentMicros - unLatchMicrosM1 >= 2.1 * unLatchDurationM1)) {
            digitalWrite(UNLATCH1_RET_PIN_M1, LOW); // Stop RET
            unLatchClearedMicrosTriggerM1 = true;  // Mark process complete
            unLatchClearedMicrosM1 = currentMicros;
            key = 4000;                            // Optional debugging or signaling
            unLatchStatusM1 = 4;                   // Move to the final state
        }
        break;

    case 4:  // Reset for next cycle
        unLatchStatusM1 = 0;
        break;
}

  // Additional logic for system state changes and movement control
  if (systemStateChange) {
    setupRampSteps(openStepsLeftM1, pRampStepsM1, vMaxStepsM1, true);
  }

  // Check for OPENED state or signal
  if ((stepCountM1 == openStepTargetM1 && !autoCalibrationState) || signalState[3] == HIGH) {
    systemState = OPENED;
    sumDelay = 0;
    highSignalValidationCounter[0] = 2;
  }

  // Handle failsafe and rapid stop conditions
  if (systemFailsafeActivated || opClCounter == 0) {
    stopMovementIndex = (stopMovementSteps - 1);
    systemState = RAPID_STOP;
  }
  // Handle stepper trigger conditions
  else if (!stepperTriggerMicrosM1 && currentMicros - unLatchMicrosM1 >= motorToUnLatch1DelayM1) {
    stepperTriggerMicrosM1 = true;  // Set the flag to prevent retriggering
    stepperMovement = true;
    stepControlM1(HIGH);
  }
  else if (stepperTriggerMicrosM1) {
    stepperMovement = true;
    stepControlM1(HIGH);
  }
}


void handleClosingState() {
  if (systemStateChange) {
    setupRampSteps(closeStepsLeftM1, pRampStepsM1, vMaxStepsM1, false);
    key = 0000;
  }
  if ((stepCountM1 == 0 && !autoCalibrationState) || signalState[4] == HIGH) {
    systemState = CLOSED;
    stepCountM1 = 0;
    sumDelay = 0;
    highSignalValidationCounter[0] = 0;
    return;
  }
  if (systemFailsafeActivated || opClCounter == 0) {
    stopMovementIndex = (stopMovementSteps - 1);
    systemState = RAPID_STOP;
  }

  else if (unLatchClearedM1) {
    stepperMovement = true;
    stepControlM1(LOW);
  }
}

void handleOpenedState() {
  stepperMovement = false;
  if (opClCounter == 2) {
    systemState = CLOSING;
    delayStepCountM1 = 0;
    sumDelay = 0;
  }
}

void handleClosedState() {
  stepperMovement = false;
  stepperTriggerMicrosM1 = false;
  unLatchCalledM1 = false;
  if (opClCounter == 1) {
    systemState = OPENING;
    delayStepCountM1 = 0;
    sumDelay = 0;
  }
  else if (latchStatusM1 == 0) {
    Serial.println("LATCH");                // Indicate latching started
    digitalWrite(LATCH_PIN_M1, HIGH);           // Set pin high (latching)
    latchStatusM1 = 1;                         // Reset status to "latched"
  }
}


void handlePartialState() {
  stepperMovement = false;
  if (stepCountM1 < openStepTargetM1 && opClCounter == 1) {
    systemState = OPENING;
    delayStepCountM1 = 0;
  }
  else if (unLatchClearedM1 && stepCountM1 > closeStepTargetM1 && opClCounter == 2) {
    systemState = CLOSING;
    delayStepCountM1 = 0;
  }
}

void handleRapidStop() {
  if (stopMovementIndex >= 0) {
    if (systemState == RAPID_STOP) {
      if (previousSystemState == OPENING) {
        stepperMovement = true;
        stepControlM1(HIGH);
      }
      else if (previousSystemState == CLOSING) {
        stepperMovement = true;
        stepControlM1(LOW);
      }
    }
  }
  if (stopMovementIndex < 0) {
    systemState = PARTIAL;
  }
}

// ////////////////////// DEBUGGING TOOLS /////////////////////////

// void debug(int active, int period) {
//   if (active && (currentMillis - lastDebugTime > period)) {
//     String s = "";
//     s += " St: " + String(stepCountM1);
//     // s += " ClSt: " + String(closeStepsLeftM1);
//     // s += " OpSt: " + String(openStepsLeftM1);
//     // s += " ClStTGT: " + String(closeStepTargetM1);
//     // s += " OpStTGT: " + String(openStepTargetM1);
//     // s += " DlSt: " + String(delayStepCountM1);
//     // s += " delay: " + dToStr(stepDelayM1);
//     s += " key: " + String(key);
//     s += " rpm: " + String(rpmM1);
//     // s += " sysStt: " + String(systemState);
//     // s += " sysStt: " + getStateName(systemState);
//     //s += " OPPin: " + String(digitalRead(OPEN_PIN));
//     s += " hivlco 10: " + String(highSignalValidationCounter[10]);
//     // s += " hivlco 0: " + String(highSignalValidationCounter[0]);
//     // s += " autCalCtr: " + String(autoCalibCounter);
//     // s += " autCalSt: " + String(autoCalibrationState);
//     //s += " autCalPH: " + String(autoCalibrationPhase);
//     // s += " autCalCyc: " + String(currentCalibrationCycle);
//     // s += " signalState[4]: " + String(signalState[4]);
//     // s += " sumCalStCt: " + String(sumCalibrationStepCountsM1);
//     // s += " oldOpStpTrg: " + String(oldOpenStepTargetM1);
//     // s += " calOpStpTrg: " + String(calibratedOpenStepTargetM1);
//     // s += " opStpTrg: " + String(openStepTargetM1);
//     //s += " autCal: " + getCalibrationPhaseName(autoCalibrationPhase);
//     s += " opCl: " + String(opClCounter);
//     //s += " preOpCl: " + String(previousOpClCounter);
//     // s += " sig[1]: " + String(signalState[1]);
//     // s += " sig[2]: " + String(signalState[2]);
//     //s += " fail: " + String(systemFailsafeActivated);
//     // s += " unlatcCal-0: " + String(unLatchCalledM1);
//     // s += " prevSysSt-3: " + String(previousSystemState);
//     // s += " SysSt-3: " + String(systemState);
//     // s += " stpTrigMic-0: " + String(stepperTriggerMicrosM1);
//     s += " unlatc-0: " + String(unLatchStatusM1);
//     s += " latc-0: " + String(latchStatusM1);
//     s += " autCa: " + String (autoCalibrationState);
//     s += " autoCaCt: " + String (autoCalibCounter);
//     s += " STUCK-1: " + String ((autoCalibCounter == 1 && !autoCalibrationState));
//     // s += " sig[1]: " + String(signalState[1]);
//     // s += " currMcr: " + String(currentMicros);
//     // s += " unlatCl: " + String(unLatchClearedM1);
//     // s += " unlatClMcrTrg: " + String(unLatchClearedMicrosTriggerM1);
//     // s += " unlatc2Mcr: " + String(unLatchPin2StartMicrosM1);
//     lastDebugTime = currentMillis;
//     Serial.println(s);
//   }
// }




//     // s += " sig[2]: " + String(signalState[2]);
//     //s += " fail: " + String(systemFailsafeActivated);
//     // s += " unlatcCal-0: " + String(unLatchCalledM1);
//     // s += " prevSysSt-3: " + String(previousSystemState);
//     // s += " SysSt-3: " + String(systemState);
//     // s += " stpTrigMic-0: " + String(stepperTriggerMicrosM1);
//     s += " unlatc-0: " + String(unLatchStatusM1);
//     s += " latc-0: " + String(latchStatusM1);
//     s += " autCa: " + String (autoCalibrationState);
//     s += " autoCaCt: " + String (autoCalibCounter);
//     s += " STUCK-1: " + String ((autoCalibCounter == 1 && !autoCalibrationState));
//     // s += " currMcr: " + String(currentMicros);
//     // s += " unlatCl: " + String(unLatchClearedM1);
//     // s += " unlatClMcrTrg: " + String(unLatchClearedMicrosTriggerM1);
//     // s += " unlatc2Mcr: " + String(unLatchPin2StartMicrosM1);
//     lastDebugTime = currentMillis;
//     Serial.println(s);
//   }
// }