////////////////////// Stepper Mega V5 debugging /////////////////////////

////////////////////// 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

// Include necessary libraries based on the selected microcontroller
#ifdef USE_TEENSY
#include                          <ILI9341_t3.h>  // Optimized library for Teensy
#include                          <XPT2046_Touchscreen.h>
#elif defined(USE_MEGA)
#include                          <Adafruit_ILI9341.h>  // Standard library for Arduino Mega
#include                          <XPT2046_Touchscreen.h>
#endif

// Define pin assignments based on the selected microcontroller
#ifdef USE_TEENSY
// TFT Display pins
#define   TFT_MOSI                11
#define   TFT_MISO                12
#define   TFT_SCLK                13
#define   TFT_CS                  10
#define   TFT_DC                   9
#define   TFT_RST                  8
#define   TOUCH_CS                 6
#define   TOUCH_IRQ                5
#define   SPI_SPEED         60000000  // 60 MHz for Teensy 4.0

// 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               18  // 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            10  // Latch pin for M1 A1 or 10
#define   UNLATCH_PIN_M1          11  // Unlatch pin for M1 A2 or 11
#define   OPEN_LIMIT_PIN_M1       12  // Open limit pin for M1
#define   CLOSE_LIMIT_PIN_M1      13  // Close limit pin for M1

#elif defined(USE_MEGA)
// TFT Display pins
#define   TFT_MOSI                51
#define   TFT_MISO                50
#define   TFT_SCLK                52
#define   TFT_CS                  53
#define   TFT_DC                   9
#define   TFT_RST                  8
#define   TOUCH_CS                 6
#define   TOUCH_IRQ                5
#define   SPI_SPEED         16000000   // 16 MHz for Arduino 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               18  // 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            10  // Latch pin for M1 A1 or 10
#define   UNLATCH_PIN_M1          11  // Unlatch pin for M1 A2 or 11
#define   OPEN_LIMIT_PIN_M1       12  // Open limit pin for M1
#define   CLOSE_LIMIT_PIN_M1      13  // Close limit pin for M1
#endif

// Initialize the display and touchscreen
#ifdef USE_TEENSY
ILI9341_t3 tft = ILI9341_t3(TFT_CS, TFT_DC, TFT_RST, TFT_MISO, TFT_MOSI, TFT_SCLK);
XPT2046_Touchscreen touch(TOUCH_CS, TOUCH_IRQ);
#elif defined(USE_MEGA)
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);
XPT2046_Touchscreen touch(TOUCH_CS, TOUCH_IRQ);
#endif

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

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

// Enumeration for auto-calibration phases
enum AutoCalibrationPhase {
  SYSTEM_NEEDS_CALIBRATION,
  CALIBRATION_INITIATED,
  CALIBRATION_OPENING,
  CALIBRATION_OPENED,
  CALIBRATION_CLOSING,
  CALIBRATION_CLOSED,
  CALIBRATION_COMPLETED,
  SYSTEM_CALIBRATED
};

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

// Signal states and validation arrays
#define       NUM_SIGNALS          8
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,
  UNLATCH_PIN_M1,
  AUTO_CALIBRATION_PIN,
};

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

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

// Global input variables
#define       EPSILON               0.00000024
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  latchDurationM1      = 2,
              unLatchDurationM1    = 2,
              distanceM1           = 340;

// Global system variables
boolean       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,
         autoCalibrationMicros;

// M1 system variables
int           latchStatusM1,
              unLatchStatusM1,
              stepCountM1,
              delayStepCountM1,
              rampStepsM1,
              pRampStepsM1,
              stepTargetM1,
              openStepTargetM1,
              oldOpenStepTargetM1,
              closeStepTargetM1,
              openStepsLeftM1,
              closeStepsLeftM1,
              vMaxStepsM1,
              rpmM1;
unsigned long latchMicrosM1,
         unLatchMicrosM1;
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),
              latchControlM1(boolean action),
              validateAndDebounceSignals(),
              manageOpClCounter(),
              evaluateSystemState(),
              handleOpeningState(),
              handleClosingState(),
              handleOpenedState(),
              handleClosedState(),
              handlePartialState(),
              handleLatchingState(),
              handleUnlatchingState(),
              handleRapidStop(),
              debug(int active, int period),
              precomputeDelays(),
              autoCalibration(),
              systemFailsafe(),
              displaySystemState(),
              handleTouch ();
double        calculateDelayForStep(int step);

////////////////////// SETUP /////////////////////////

void setup() {
  Serial.begin(115200);
  // Setup touchscreen
  // Initialize the SPI and TFT display
  tft.begin(SPI_SPEED);
  // Initialize the touchscreen
  touch.begin();
  touch.setRotation(1);

  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     (UNLATCH_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;
  autoCalibrationPhase            = SYSTEM_CALIBRATED;
  systemStateChange               = false;
  systemFailsafeActivated         = false;
  autoCalibrationState            = 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;
  autoCalibrationMicros           = 0;
  autoCalibrationState            = false;

  digitalWrite(LATCH_PIN_M1, LOW);
  digitalWrite(UNLATCH_PIN_M1, LOW);

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

  precomputeDelays();

  // Ensure all signals and states are correctly initialized
  validateAndDebounceSignals();
  manageOpClCounter();

  // Re-check the system state after initialization is complete
  evaluateSystemState();
  // Initial screen setup
  tft.fillScreen(ILI9341_BLACK);
  tft.setCursor(0, 0);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.println("System Monitor:");
  // Display the final initial state
  displaySystemState();

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

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

void loop() {
  currentMicros = micros();
  currentMillis = millis();
  validateAndDebounceSignals();
  manageOpClCounter();
  manageAutoCalibCounter();
  systemFailsafe();
  evaluateSystemState();
  autoCalibration();
  debug(1, 100);
}

////////////////////// TFT DISPLAY /////////////////////////

void handleTouch () {
  // Example of touch handling
  if (touch.touched()) {
    TS_Point p = touch.getPoint();
    // Re-map the coordinates based on screen rotation if needed
    // e.g., p.x, p.y
    tft.fillCircle(p.x, p.y, 3, ILI9341_WHITE);
  }
}

void displaySystemState() {
  tft.fillRect(0, 40, 240, 60, ILI9341_BLACK);  // Clear previous status area
  tft.setCursor(0, 40);

  switch (systemState) {
    case OPENING:
      tft.setTextColor(ILI9341_GREEN);
      tft.println("System State: OPENING");
      break;
    case CLOSING:
      tft.setTextColor(ILI9341_GREEN);
      tft.println("System State: CLOSING");
      break;
    case OPENED:
      tft.setTextColor(ILI9341_GREEN);
      tft.println("System State: OPENED");
      break;
    case CLOSED:
      tft.setTextColor(ILI9341_GREEN);
      tft.println("System State: CLOSED");
      break;
    case PARTIAL:
      tft.setTextColor(ILI9341_YELLOW);  // Yellow for PARTIAL
      tft.println("System State: PARTIAL");
      break;
    case RAPID_STOP:
      tft.setTextColor(ILI9341_RED);
      tft.println("System State: RAPID STOP");
      break;
    case LATCHING:
      tft.setTextColor(ILI9341_PINK);  // Pink for LATCHING
      tft.println("System State: LATCHING");
      break;
    case UNLATCHING:
      tft.setTextColor(ILI9341_PINK);  // Pink for UNLATCHING
      tft.println("System State: UNLATCHING");
      break;
    default:
      tft.setTextColor(ILI9341_WHITE);
      tft.println("System State: UNKNOWN");
      break;
  }
  tft.setCursor(0, 80);
  tft.setTextColor(ILI9341_WHITE);
  tft.print("Failsafe: ");
  tft.println(systemFailsafeActivated ? "ACTIVATED" : "NOT ACTIVATED");
}

void displayCalibrationStatus(int phase, int oldTarget, int newTarget, int cycle, int status = 0) {
  tft.fillRect(0, 100, 240, 60, ILI9341_BLACK);  // Clear previous message area
  tft.setCursor(0, 100);

  // Display status messages based on the phase and status
  switch (phase) {
    case CALIBRATION_INITIATED:
      tft.setTextColor(ILI9341_CYAN);
      tft.println("Calibration Initiated");
      break;
    case CALIBRATION_OPENING:
      tft.setTextColor(ILI9341_GREEN);
      tft.println("Calibration: Opening");
      break;
    case CALIBRATION_OPENED:
      tft.setTextColor(ILI9341_GREEN);
      tft.println("Calibration: Opened");
      break;
    case CALIBRATION_CLOSING:
      tft.setTextColor(ILI9341_GREEN);
      tft.println("Calibration: Closing");
      break;
    case CALIBRATION_CLOSED:
      tft.setTextColor(ILI9341_GREEN);
      tft.println("Calibration: Closed");
      break;
    case CALIBRATION_COMPLETED:
      tft.setTextColor(ILI9341_GREEN);
      tft.println("Calibration Completed");
      break;
    case SYSTEM_CALIBRATED:
      tft.setTextColor(ILI9341_GREEN);
      tft.println("System Calibrated");
      tft.print("Old Target: ");
      tft.println(oldTarget);
      tft.print("New Target: ");
      tft.println(newTarget);
      tft.print("Cycles: ");
      tft.println(cycle);
      break;
    default:
      tft.setTextColor(ILI9341_RED);
      tft.println("Calibration Failed!");
      tft.print("Old Target: ");
      tft.println(oldTarget);
      tft.print("New Target: ");
      tft.println(newTarget);
      tft.print("Cycles Completed: ");
      tft.println(cycle);
      break;
  }

  // Display error message if the status indicates a failure
  if (status == 2) {
    tft.setTextColor(ILI9341_RED);
    tft.println("Calibration Failed!");
  }
}

////////////////////// GETTING STRINGS /////////////////////////

// String getStateName(SystemState systemState) {
//   switch (systemState) {
//     case CLOSED: return "CLOSED";
//     case OPENING: return "OPENING";
//     case OPENED: return "OPENED";
//     case CLOSING: return "CLOSING";
//     case PARTIAL: return "PARTIAL";
//     case RAPID_STOP: return "RAPID_STOP";
//     case LATCHING: return "LATCHING";
//     case UNLATCHING: return "UNLATCHING";
//     default: return "UNKNOWN_STATE";
//   }
// }

// String getCalibrationPhaseName (AutoCalibrationPhase autoCalibrationPhase){
//   switch (autoCalibrationPhase) {
//     case SYSTEM_NEEDS_CALIBRATION: return "SYSTEM NEEDS CALIBRATION";
//     case CALIBRATION_INITIATED: return "CALIBRATION INITIATED";
//     case CALIBRATION_OPENING: return "CALIBRATION OPENING";
//     case CALIBRATION_OPENED: return "CALIBRATION OPENED";
//     case CALIBRATION_CLOSING: return "CALIBRATION CLOSING";
//     case CALIBRATION_CLOSED: return "CALIBRATION CLOSED";
//     case CALIBRATION_COMPLETED: return "CALIBRATION COMPLETED";
//     case SYSTEM_CALIBRATED: return "SYSTEM CALIBRATED";
//     default: return "UNKNOWN_PHASE";
//   }

////////////////////// 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 AND MANAGEMENT /////////////////////////

void validateAndDebounceSignals() {
  if ((currentMillis - lastDebounceTime) > debounceDelay) {
    for (int i = 0; i < NUM_SIGNALS; i++) {
      if (SIGNAL_PINS[i] == OPEN_PIN && (latchStatusM1 == 1 || 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];
    }
  }
}

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[7] >= 2) {
    newAutoCalibCounter = 1;
  }
  else {
    newAutoCalibCounter = 0;
  }

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

////////////////////// MOTORS' MOTION CONTROL /////////////////////////

void latchControlM1(boolean action) {
  unsigned long *microsM1 = action ? &latchMicrosM1 : &unLatchMicrosM1;
  int *statusM1           = action ? &latchStatusM1 : &unLatchStatusM1;
  int pin                 = action ? LATCH_PIN_M1 : UNLATCH_PIN_M1;
  double duration         = action ? latchDurationM1 : unLatchDurationM1;
  if (*statusM1 == 0) {
    *microsM1 = currentMicros;
    *statusM1 = 1;
  }
  if (currentMicros - *microsM1 <= duration * 1000000) {
    key = action ? 1000 : 2000;
    digitalWrite(pin, HIGH);
    *statusM1 = 1;
  }
  if (*statusM1 != 2 && currentMicros - *microsM1 > duration * 1000000) {
    key = action ? 3000 : 4000;
    digitalWrite(pin, LOW);
    *statusM1 = 2;
  }
}

void stepControlM1(boolean direction) {
  static unsigned long lastMicros = 0;
  static boolean stepState = LOW;
  unsigned long stepDelayMicros = 1000000L * stepDelayM1;
  digitalWrite(DIR_PIN_M1, direction);
  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;
      }
    }
  }
}

////////////////////// 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() {
  // Check for auto-calibration trigger
  if (autoCalibCounter == 1 && !autoCalibrationState) {
    key = 000000;
    // Ensure calibration only starts if the previous state was CLOSED
    if (previousSystemState != CLOSED) {
      displayCalibrationStatus(autoCalibrationPhase, oldOpenStepTargetM1, openStepTargetM1, currentCalibrationCycle, 2);
      return;
    }
    else {
      autoCalibrationState = true;
      autoCalibrationPhase = CALIBRATION_INITIATED;
    }
  }
  else {
    return;
  }

  // Register auto calibration has effectively started
  if (autoCalibrationPhase == CALIBRATION_INITIATED) {
    displayCalibrationStatus(autoCalibrationPhase, oldOpenStepTargetM1, openStepTargetM1, currentCalibrationCycle);
  }

  for (currentCalibrationCycle = 0; currentCalibrationCycle < CALIBRATION_CYCLES; currentCalibrationCycle++) {
    currentMicros = micros();
    currentMillis = millis();
    validateAndDebounceSignals();
    manageOpClCounter();
    manageAutoCalibCounter();
    systemFailsafe();
    evaluateSystemState();

    autoCalibrationPhase = CALIBRATION_OPENING;
    // systemState = OPENING;
    highSignalValidationCounter[0] = 1;

    displayCalibrationStatus(autoCalibrationPhase, oldOpenStepTargetM1, openStepTargetM1, currentCalibrationCycle);


    while (systemState != OPENED) {
      if (systemFailsafeActivated || signalState[0] == HIGH) {
        highSignalValidationCounter[0] = (opClCounter == 1) ? 2 : 0;
        displayCalibrationStatus(autoCalibrationPhase, oldOpenStepTargetM1, openStepTargetM1, currentCalibrationCycle, 2);
        autoCalibrationState = false;
        systemState = RAPID_STOP;
        return;
      }
    }

    if (autoCalibrationPhase == CALIBRATION_OPENING && systemState == OPENED) {
      autoCalibrationPhase = CALIBRATION_OPENED;
      calibrationStepcountsM1[currentCalibrationCycle] = stepCountM1;
      sumCalibrationStepCountsM1 += calibrationStepcountsM1[currentCalibrationCycle];
      displayCalibrationStatus(autoCalibrationPhase, oldOpenStepTargetM1, openStepTargetM1, currentCalibrationCycle);
      autoCalibrationMicros = currentMicros;

      while (currentMicros - autoCalibrationMicros < autoCalibrationDelays * 1000000) {
        // Wait until 2 seconds have passed
      }

      autoCalibrationPhase = CALIBRATION_CLOSING;
    }

    if (autoCalibrationPhase == CALIBRATION_CLOSING && systemState == OPENED) {
      highSignalValidationCounter[0] = 3;
      displayCalibrationStatus(autoCalibrationPhase, oldOpenStepTargetM1, openStepTargetM1, currentCalibrationCycle);
    }

    while (systemState != CLOSED) {
      if (systemFailsafeActivated || signalState[0] == HIGH) {
        highSignalValidationCounter[0] = (opClCounter == 1) ? 2 : 0;
        displayCalibrationStatus(autoCalibrationPhase, oldOpenStepTargetM1, openStepTargetM1, currentCalibrationCycle, 2);
        autoCalibrationState = false;
        systemState = RAPID_STOP;
        return;
      }
    }

    if (autoCalibrationPhase == CALIBRATION_CLOSING && systemState == CLOSED) {
      autoCalibrationPhase = CALIBRATION_CLOSED;
      displayCalibrationStatus(autoCalibrationPhase, oldOpenStepTargetM1, openStepTargetM1, currentCalibrationCycle);
      autoCalibrationMicros = currentMicros;

      while (currentMicros - autoCalibrationMicros < autoCalibrationDelays * 1000000) {
        // Wait until 2 seconds have passed
      }
    }
  }

  if (autoCalibrationPhase == CALIBRATION_CLOSED && systemState == CLOSED) {
    calibratedOpenStepTargetM1 = sumCalibrationStepCountsM1 / CALIBRATION_CYCLES;
    openStepTargetM1 = calibratedOpenStepTargetM1;
    autoCalibrationState = false;
    highSignalValidationCounter[7] = 0; // Reset the counter after activation
    displayCalibrationStatus(CALIBRATION_COMPLETED, oldOpenStepTargetM1, openStepTargetM1, currentCalibrationCycle);
    digitalWrite(AUTO_CALIBRATION_LED, LOW);
  }
}


////////////////////// 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;
    displaySystemState();
  } 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;
    case LATCHING:
      handleLatchingState();
      break;
    case UNLATCHING:
      handleUnlatchingState();
      break;
    default:
      break;
  }
}

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

void handleOpeningState() {
  if (previousSystemState == CLOSED) {
    latchControlM1(false);
    systemState = UNLATCHING;
    return;
  }

  if (systemStateChange) {
    setupRampSteps(openStepsLeftM1, pRampStepsM1, vMaxStepsM1, true);
  }

  if ((stepCountM1 == openStepTargetM1 && !autoCalibrationState) || signalState[3] == HIGH) {
    systemState = OPENED;
    sumDelay = 0;
    highSignalValidationCounter[0] = 2;
    return;
  }

  if (systemFailsafeActivated || opClCounter == 0) {
    stopMovementIndex = (stopMovementSteps - 1);
    systemState = RAPID_STOP;
  }
  else {
    stepControlM1(HIGH);
  }
}

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

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

void handleClosedState() {
  if (previousSystemState == CLOSING) {
    systemState = LATCHING;
    return;
  }
  if (opClCounter == 1) {
    systemState = OPENING;
    delayStepCountM1 = 0;
    sumDelay = 0;
    autoCalibrationState = false;
  }
}

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

void handleLatchingState() {
  latchControlM1(true);
  if (latchStatusM1 == 2) {
    latchStatusM1 = 0;
    systemState = CLOSED;
  }
}

void handleUnlatchingState() {
  latchControlM1(false);
  if (unLatchStatusM1 == 2) {
    unLatchStatusM1 = 0;
    systemState = OPENING;
  }
}

void handleRapidStop() {
  if (stopMovementIndex >= 0) {
    if (systemState == RAPID_STOP) {
      if (previousSystemState == OPENING) {
        stepControlM1(HIGH);
      }
      else if (previousSystemState == CLOSING) {
        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 7: " + String(highSignalValidationCounter[7]);
    s += " autCalCtr: " + String(autoCalibCounter);
    s += " autCalSt: " + String(autoCalibrationState);
    s += " autCalPH: " + String(autoCalibrationPhase);
    //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 += " latc: " + String(latchStatusM1);
    //s += " unlatc: " + String(unLatchStatusM1);
    lastDebugTime = currentMillis;
    Serial.println(s);
  }
}



//Arduino Nano
// // Global system pins
// const int OPEN_PIN = 2,  // Open pin
//           OPT_DIS_PIN = 10,  // Optical disable pin
//           CRUSH_PREV_PIN = 11,  // Crush prevention pin
//           LED_PIN = 13;  // LED pin

// // M1 pins
// const int STEP_PIN_M1 = 4,  // Step pin for M1
//           DIR_PIN_M1 = 5,  // Direction pin for M1
//           LATCH_PIN_M1 = A1,  // Latch pin for M1 A1 or 10
//           UNLATCH_PIN_M1 = A2,  // Unlatch pin for M1 A2 or 11
//           OPEN_LIMIT_PIN_M1 = 6,  // Open limit pin for M1
//           CLOSE_LIMIT_PIN_M1 = 7;  // Close limit pin for M1

//Teensy 4.0
// // Global system pins
// const int OPEN_PIN = 2,  // Open pin
//           OPT_DIS_PIN = 10,  // Optical disable pin
//           CRUSH_PREV_PIN = 11,  // Crush prevention pin
//           LED_PIN = 13;  // LED pin

// // M1 pins
// const int STEP_PIN_M1 = 4,  // Step pin for M1
//           DIR_PIN_M1 = 5,  // Direction pin for M1
//           LATCH_PIN_M1 = 12,  // Latch pin for M1
//           UNLATCH_PIN_M1 = 14,  // Unlatch pin for M1
//           OPEN_LIMIT_PIN_M1 = 6,  // Open limit pin for M1
//           CLOSE_LIMIT_PIN_M1 = 7;  // Close limit pin for M1



// TOUCH_IRQ	  2
// TOUCH_DO	    12
// TOUCH_DIN	  11
// TOUCH_CS	    8	 //Alternate: any digital pin
// TOUCH_CLK	  13

// TFT_MISO    12
// LED 	      VIN	//Use 100 ohm resistor
// TFT_SCK     13
// TFT_MOSI    11
// TFT_DC      20  //Alternate Pins: 10, 15, 20, 21
// RESET	      +3.3V
// TFT_CS      21  //Alternate Pins: 9, 15, 20, 21
// GND         GND
// VCC         5V  //3.6 to 5.5 volts