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

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

// Enumeration for system states
enum SystemState {
  OPENING                         = 0,
  CLOSING                         = 1,
  OPENED                          = 2,
  CLOSED                          = 3,
  PARTIAL                         = 4,
  RAPID_STOP                      = 5,
  LATCHING                        = 6,
  UNLATCHING                      = 7
};

// Enumeration for curve types
enum CurveType {
  LINEAR                          = 0,
  QUADRATIC                       = 1,
  CUBIC                           = 2,
  QUARTIC                         = 3,
  QUINTIC                         = 4
};

// Array for signal pins
#define NUM_SIGNALS 8
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,
};

// Signal states and validation arrays
boolean signalState[NUM_SIGNALS],
        lastSignalState[NUM_SIGNALS];
int highSignalValidationCounter[NUM_SIGNALS],
    lowSignalValidationCounter[NUM_SIGNALS];

// Array for delays
#define MAX_RAMP_STEPS 300
double delays[MAX_RAMP_STEPS];

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

// M1 input variables
const double latchDurationM1        = 2,
             unLatchDurationM1      = 2,
             distanceM1             = 340;

// Global system variables
boolean     systemStateChange,
            systemFailsafeActivated,
            stopMovementActivated;
SystemState currentSystemState,
            systemState,
            previousSystemState,
            oldSystemState;
int         opClCounter,
            previousOpClCounter,
            pendingOpClCounter,
            stopMovementIndex,
            key;
double      rampTime,
            distancePerRev,
            stepLength,
            sumDelay,
            previousStepTime;
unsigned long currentMicros,
         currentMillis,
         lastDebounceTime,
         debounceDelay,
         lastDebugTime;

// M1 system variables
boolean     ignore_OPEN_LIMIT_SWITCH,
            ignore_CLOSE_LIMIT_SWITCH;
int         latchStatusM1,
            unLatchStatusM1,
            stepCountM1,
            delayStepCountM1,
            rampStepsM1,
            pRampStepsM1,
            stepTargetM1,
            openStepTargetM1,
            closeStepTargetM1,
            openStepsLeftM1,
            closeStepsLeftM1,
            vMaxStepsM1,
            maxRPMM1,
            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(),
            stopMovement(),
            systemFailsafe(),
            displaySystemState(),
            handleTouch ();
double      calculateDelayForStep(int step);

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;
  // Initialize the system state
  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;
  pendingOpClCounter = 0;
  stopMovementIndex = 0;
  systemStateChange = false;
  systemFailsafeActivated = false;
  stopMovementActivated  = 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);
  maxRPMM1 = vMaxM1 / distancePerRev * 60;
  stepTargetM1 = distanceM1 / (numberTeethPulley * pulleyPace) * stepPerRevolution;
  openStepTargetM1 = stepTargetM1;
  rampStepsM1 = calculateRampSteps();
  vMaxDelayM1 = findStepTimes(rampStepsM1) - findStepTimes(rampStepsM1 - 1);
  unLatchStatusM1 = 0;
  latchStatusM1 = 0;
  ignore_OPEN_LIMIT_SWITCH = false;
  ignore_CLOSE_LIMIT_SWITCH = 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);
}


void loop() {
  currentMicros = micros();
  currentMillis = millis();
  validateAndDebounceSignals();
  manageOpClCounter();
  systemFailsafe();
  evaluateSystemState();
  if (systemStateChange) {
    displaySystemState();  // Update the display when state changes
  }

  debug(1, 100);
}

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");
}



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 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 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;
      }
    }
  }
}

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;
    }
  }
}

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(); // Call displaySystemState to update the display
  }
  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;
  }
}

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;
}

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

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

  if (stepCountM1 == openStepTargetM1 || (ignore_OPEN_LIMIT_SWITCH == false && signalState[3] == HIGH)) {
    systemState = OPENED;
    sumDelay = 0;
    highSignalValidationCounter[0] = 2;
    if (signalState[3] == HIGH) {
      key = 8500;
      ignore_OPEN_LIMIT_SWITCH = true;
    }
    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 || (ignore_CLOSE_LIMIT_SWITCH == false && signalState[4] == HIGH)) {
    systemState = CLOSED;
    stepCountM1 = 0;
    sumDelay = 0;
    highSignalValidationCounter[0] = 0;
    if (signalState[4] == HIGH) {
      key = 8500;
      ignore_CLOSE_LIMIT_SWITCH = true;
    }
    return;
  }

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

void handleLatchingState() {
  latchControlM1(true);

  if (latchStatusM1 == 2) {
    latchStatusM1 = 0;
    systemState = CLOSED;
  }
}

void handleUnlatchingState() {
  latchControlM1(false);

  if (unLatchStatusM1 == 2) {
    unLatchStatusM1 = 0;
    systemState = OPENING;
  }
}

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

void handleClosedState() {
  if (previousSystemState == CLOSING) {
    systemState = LATCHING;
    return;
  }

  if (opClCounter == 1) {
    systemState = OPENING;
    delayStepCountM1 = 0;
    sumDelay = 0;
    ignore_CLOSE_LIMIT_SWITCH = 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 handleRapidStop() {
  if (stopMovementIndex >= 0) {
    if (systemState == RAPID_STOP) {
      if (previousSystemState == OPENING) {
        stepControlM1(HIGH);
      } else if (previousSystemState == CLOSING) {
        stepControlM1(LOW);
      }
    }
  }

  if (stopMovementIndex < 0) {
    systemState = PARTIAL;
  }
}

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 += " OPPin: " + String(digitalRead(OPEN_PIN));
    s += " hivlco: " + String(highSignalValidationCounter[0]);
    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