// Stepper Complete Basic Functions (array) v4 Teensy LIMPA

// Convert double to string macro with specified precision
String dToStr(double val, byte precision = 6) {
  String s = String(int(val));  // Convert the integer part to string

  if (precision > 0) {
    s += ".";  // Add decimal point if precision is greater than 0
  }

  unsigned long frac;  // Fractional part
  unsigned long mult = 1;  // Multiplier for precision
  byte padding = precision - 1;  // Padding zeros

  // Calculate multiplier based on precision
  while (precision--) {
    mult *= 10;
  }

  // Calculate fractional part
  if (val >= 0) {
    frac = (val - int(val)) * mult;
  } else {
    frac = (int(val) - val) * mult;
  }

  unsigned long frac1 = frac;  // Temporary variable for fractional part

  // Calculate number of padding zeros
  while (frac1 /= 10) {
    padding--;
  }
  while (padding--) {
    s += "0";  // Add padding zeros
  }

  s += String(frac, DEC);  // Append fractional part

  return s;  // Return formatted string
}

// Enumeration for system states
enum SystemState {
  OPENING = 0,  // Opening state
  CLOSING = 1,  // Closing state
  OPENED = 2,  // Opened state
  CLOSED = 3,  // Closed state
  PARTIAL = 4  // Partial state
};

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

// 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 - 12 for Teensy, A1 for Arduino Nano
          UNLATCH_PIN_M1 = 14,  // Unlatch pin for M1 - 14 for Teensy, A2 for Arduino Nano
          OPEN_LIMIT_PIN_M1 = 6,  // Open limit pin for M1
          CLOSE_LIMIT_PIN_M1 = 7;  // Close limit pin for M1

// Array for signal pins
#define NUM_SIGNALS 5
const int SIGNAL_PINS[NUM_SIGNALS] = {
  OPEN_PIN,           // 0
  OPT_DIS_PIN,        // 1
  CRUSH_PREV_PIN,     // 2
  OPEN_LIMIT_PIN_M1,  // 3
  CLOSE_LIMIT_PIN_M1  // 4
};

// Signal states and validation arrays
boolean signalState[NUM_SIGNALS],               // Current state of signals
        lastSignalState[NUM_SIGNALS];           // Last state of signals
int highSignalValidationCounter[NUM_SIGNALS],  // High signal validation counter
    lowSignalValidationCounter[NUM_SIGNALS];   // Low signal validation counter

// Global input variables
#define EPSILON 0.00000024
const CurveType curve = QUARTIC;  // Curve type
const int pulleyPace = 2,  // (mm) Pulley pace
          numberTeethPulley = 20,  // (teeth) Number of teeth on pulley
          stepPerRevolution = 400;  // (stewps) Steps per revolution
const double tTime = 5,  // (s) Total time
             accRampTime = 14,  // (%) Acceleration ramp time
             t0 = 0;  // (s) Initial time

// M1 input variables
const double latchDurationM1 = 2,  // (s) Latch duration for M1
             unLatchDurationM1 = 2,  // (s) Unlatch duration for M1
             distanceM1 = 340;  // (mm)Distance for M1

// Global system variables
boolean systemStateChange,  // Flag for system state change
        failsafeActivated = false; // Global flag to indicate if failsafe was activated
SystemState currentSystemState,  // Current system state
            systemState,  // System state
            previousSystemState,  // Previous system state
            oldSystemState;  // Old system state
int opClCounter,  // Open/Close counter
    previousOpClCounter,
    pendingOpClCounter,
    key;  // Key for debugging
double rampTime,  // Ramp time
       distancePerRev,  // Distance per revolution
       stepLength,  // Step length
       sumDelay,  // Sum of delays
       previousStepTime;  // Previous step time
unsigned long currentMicros,  // Current microseconds
         currentMillis,  // Current milliseconds
         lastDebounceTime,  // Last debounce time
         debounceDelay,  // Debounce delay
         lastDebugTime;  // Last debug time

// M1 system variables
int latchStatusM1,  // Latch status of M1
    unLatchStatusM1,  // Unlatch status of M1
    stepCountM1,  // Step count of M1
    delayStepCountM1,  // Delay step count of M1
    rampStepsM1,  // Ramp steps of M1
    pRampStepsM1,  // Partial ramp steps of M1
    stepTargetM1,  // Step target of M1
    openStepTargetM1,  // Open step target of M1
    closeStepTargetM1,  // Close step target of M1
    openStepsLeftM1,  // Open steps left of M1
    closeStepsLeftM1,  // Close steps left of M1
    vMaxStepsM1,  // Maximum steps of M1
    maxRPMM1,  // Maximum RPM of M1
    rpmM1;  // RPM of M1
unsigned long latchMicrosM1,  // Latch microseconds of M1
         unLatchMicrosM1;  // Unlatch microseconds of M1
double stepDelayM1,  // Step delay of M1
       vMaxDelayM1,  // Maximum delay of M1
       vMaxM1,  // Maximum velocity of M1
       vMinM1;  // Minimum velocity of M1

// Precomputed delays array
const int maxElements = 300;  // Reduced number of elements
double delays[maxElements];  // Array to store precomputed delays

// Function prototypes
boolean checkSignalStates();              // Check specific signal states
double calculateAccDelay(),              // Calculate acceleration delay
       steps(const double x, const double vMinM1, const double vMaxM1), // Calculate steps based on curve
       findStepTimes(int rampStepsM1),   // Find step times for given ramp steps
       calculateRampSteps(),             // Calculate ramp steps based on ramp time
       delayCalcM1();                    // Calculate delay for M1 based on current state
void precomputeDelays(),               // Precompute delays
     stepControlM1(boolean direction), // Control step for M1 with direction parameter
     latchControlM1(boolean action),   // Combined latch and unlatch function for M1 with an action parameter
     signalVal(),                      // Validate signals with debounce
     stateEval(),                      // Evaluate state and execute move commands
     handleOpeningState(),             // Handle opening state
     handleClosingState(),             // Handle closing state
     handleOpenedState(),              // Handle opened state
     handleClosedState(),              // Handle closed state
     handlePartialState(),             // Handle partial state
     printArray(double arr[], int size), // Print elements of the array
     debug(int active, int period);    // Debug function for outputting system status

// Setup function
void setup() {
  Serial.begin(115200);  // Start serial communication

  // Global system pins modes
  pinMode(OPEN_PIN, INPUT);  // Set OPEN_PIN as input
  pinMode(OPT_DIS_PIN, INPUT);  // Set OPT_DIS_PIN as input
  pinMode(CRUSH_PREV_PIN, INPUT);  // Set CRUSH_PREV_PIN as input
  pinMode(LED_PIN, OUTPUT);  // Set LED_PIN as output

  // M1 pin modes
  pinMode(OPEN_LIMIT_PIN_M1, INPUT);  // Set OPEN_LIMIT_PIN_M1 as input
  pinMode(CLOSE_LIMIT_PIN_M1, INPUT);  // Set CLOSE_LIMIT_PIN_M1 as input
  pinMode(STEP_PIN_M1, OUTPUT);  // Set STEP_PIN_M1 as output
  pinMode(DIR_PIN_M1, OUTPUT);  // Set DIR_PIN_M1 as output
  pinMode(LATCH_PIN_M1, OUTPUT);  // Set LATCH_PIN_M1 as output
  pinMode(UNLATCH_PIN_M1, OUTPUT);  // Set UNLATCH_PIN_M1 as output

  // Initialize system variables
  lastDebounceTime = 0;  // Initialize last debounce time
  lastDebugTime = 0;  // Initialize last debug time
  debounceDelay = 10;  // Set debounce delay
  systemState = CLOSED;  // Set initial system state to CLOSED
  opClCounter = 0;  // Initialize open/close counter
  previousOpClCounter = 0;
  pendingOpClCounter = 0;
  systemStateChange = false;  // Initialize system state change flag
  currentSystemState = systemState;  // Set current system state
  distancePerRev = numberTeethPulley * pulleyPace;  // Calculate distance per revolution
  rampTime = tTime * accRampTime / 100;  // Calculate ramp time
  stepLength = distancePerRev / stepPerRevolution;  // Calculate step length

  // Initialize M1 variables
  stepCountM1 = 0;  // Initialize step count for M1
  delayStepCountM1 = 0;  // Initialize delay step count for M1
  closeStepTargetM1 = 0;  // Initialize close step target for M1
  vMinM1 = 3.5;  // (mm) Set minimum velocity for M1
  vMaxM1 = (distanceM1 - (rampTime * vMinM1)) / (tTime - rampTime);  // Calculate maximum velocity for M1
  maxRPMM1 = vMaxM1 / distancePerRev * 60;  // Calculate maximum RPM for M1
  stepTargetM1 = distanceM1 / (numberTeethPulley * pulleyPace) * stepPerRevolution;  // Calculate step target for M1
  openStepTargetM1 = stepTargetM1;  // Set open step target for M1
  rampStepsM1 = calculateRampSteps();  // Calculate ramp steps for M1
  vMaxDelayM1 = findStepTimes(rampStepsM1) - findStepTimes(rampStepsM1 - 1);  // Calculate maximum delay for M1
  digitalWrite(LATCH_PIN_M1, LOW);  // Initialize LATCH_PIN_M1 LOW
  digitalWrite(UNLATCH_PIN_M1, LOW);  // Initialize UNLATCH_PIN_M1 LOW

  // Initialize arrays
  for (int i = 0; i < NUM_SIGNALS; i++) {
    signalState[i] = 0;  // Initialize signal states
    lastSignalState[i] = 0;  // Initialize last signal states
    highSignalValidationCounter[i] = 0;  // Initialize high signal validation counters
    lowSignalValidationCounter[i] = 0;  // Initialize low signal validation counters
  }

  // Precompute delays
  precomputeDelays();  // Call function to precompute delays

  // Setup Complete
  digitalWrite(LED_PIN, HIGH);
}

// Main loop function
void loop() {
  currentMicros = micros();  // Get the current microseconds at the beginning of the loop
  currentMillis = millis();  // Get the current milliseconds at the beginning of the loop

  signalVal();  // Validate input signals and trigger failsafe if needed
  stateEval();  // Evaluate and command motion state

  // For debugging purposes
  debug(1, 100);

  // Additional functions (optional)
  // openMoveAdjustment();
  // closeMoveAdjustment();
}

// Function implementations
void precomputeDelays() {
  previousStepTime = t0;  // Initialize previous step time
  delayStepCountM1 = 0;  // Initialize delay step count for M1
  for (int i = 0; i < rampStepsM1 && i < maxElements; ++i) {
    delays[i] = calculateAccDelay();  // Precompute delays
  }
  vMaxDelayM1 = delays[rampStepsM1 - 1];  // Set maximum delay for M1
  // Fill remaining elements for constant speed delays
  for (int i = rampStepsM1; i < maxElements; ++i) {
    delays[i] = 0.0;  // Fill remaining elements with 0
  }
}

double calculateAccDelay() {
  double stepTime = findStepTimes(delayStepCountM1),  // Find step times
         rampDelay = stepTime - previousStepTime;    // Calculate ramp delays
  previousStepTime = stepTime;  // Update previous step time
  delayStepCountM1++;  // Increment delay step count
  return rampDelay;  // Return calculated delay
}

double steps(const double x, const double vMinM1, const double vMaxM1) {
  switch (curve) {
    case LINEAR:
      return (vMinM1 + 0.5 * (vMaxM1 - vMinM1) * x) * x;  // Calculate linear steps
    case QUADRATIC:
      return (vMinM1 + 0.5 * (vMaxM1 - vMinM1) * x * x * (2 - x)) * x;  // Calculate quadratic steps
    case CUBIC:
      return (vMinM1 + 0.5 * (vMaxM1 - vMinM1) * x * x * x * (5 - x * (6 - x * 2))) * x;  // Calculate cubic steps
    case QUARTIC:
      return (vMinM1 + 0.5 * (vMaxM1 - vMinM1) * x * x * x * x * (14 - x * (28 - x * (20 - x * 5)))) * x;  // Calculate quartic steps
    case QUINTIC:
      return (vMinM1 + 0.5 * (vMaxM1 - vMinM1) * x * x * x * x * x * (42 - x * (120 - x * (135 - x * (70 - x * 14))))) * x;  // Calculate quintic steps
  }
  return 0;  // Default return if curve type is not matched
}

double findStepTimes(int rampStepsM1) {
  const double target_s = rampStepsM1 * stepLength / (rampTime - t0);  // Calculate target step length
  double x0 = 0.0, x1 = 1.0, xeps = EPSILON * stepLength / (rampTime - t0);  // Initialize variables

  while (true) {
    const double x = 0.5 * (x0 + x1);  // Calculate midpoint
    if (x1 - x0 <= xeps || x == x0 || x == x1) {
      return (1 - x) * t0 + x * rampTime;  // Return calculated step time
    }
    const double s = steps(x, vMinM1, vMaxM1);  // Calculate steps
    if (s < target_s) {
      x0 = x;  // Update x0
    } else if (s > target_s) {
      x1 = x;  // Update x1
    } else {
      return (1 - x) * t0 + x * rampTime;  // Return calculated step time
    }
  }
}

double calculateRampSteps() {
  return (rampTime - t0) * (vMaxM1 + vMinM1) / (2 * stepLength);  // Calculate ramp steps based on ramp time
}

double delayCalcM1() {
  if (systemState == OPENING) {  // Opening
    key = 1;
    if (stepCountM1 <= pRampStepsM1) {  // Accelerate
      key = 2;
      return delays[delayStepCountM1];
    }
    if (stepCountM1 > pRampStepsM1 && stepCountM1 < vMaxStepsM1 + pRampStepsM1) {  // Constant speed
      key = 4;
      if (previousSystemState == PARTIAL && systemState == OPENING) {  // From partial to opening
        key = 5;
        if (delayStepCountM1 <= pRampStepsM1) {  // Accelerate
          key = 6;
          return delays[delayStepCountM1];
        } else {  // vMax if partial
          key = 7;
          return vMaxDelayM1;
        }
      } else {  // vMax if no partial
        key = 8;
        return vMaxDelayM1;
      }
    } else if (stepCountM1 >= vMaxStepsM1 + pRampStepsM1) {  // Decelerate
      key = 9;
      if (previousSystemState == PARTIAL && systemState == OPENING) {  // From partial to opening
        key = 10;
        if (delayStepCountM1 <= pRampStepsM1) {  // Accelerate if partial
          key = 11;
          return delays[delayStepCountM1];
        } else {  // Decelerate if partial
          key = 12;
          return delays[openStepTargetM1 - delayStepCountM1];
        }
      } else {  // Decelerate if no partial
        key = 13;
        return delays[openStepTargetM1 - delayStepCountM1];
      }
    }
  }

  if (systemState == CLOSING) {  // Closing
    key = 14;
    if (stepCountM1 >= closeStepsLeftM1 - pRampStepsM1) {  // Accelerate
      key = 15;
      return delays[delayStepCountM1];
    }
    if (stepCountM1 > pRampStepsM1 && stepCountM1 < vMaxStepsM1 + pRampStepsM1) {  // Constant speed
      key = 16;
      if (previousSystemState == PARTIAL && systemState == CLOSING) {  // From partial to opening
        key = 17;
        if (delayStepCountM1 <= pRampStepsM1) {  // Accelerate
          key = 18;
          return delays[delayStepCountM1];
        } else {  // vMax if partial
          key = 19;
          return vMaxDelayM1;
        }
      } else {  // vMax if no partial
        key = 20;
        return vMaxDelayM1;
      }
    } else if (stepCountM1 <= pRampStepsM1) {  // Decelerate
      key = 21;
      if (previousSystemState == PARTIAL && systemState == CLOSING) {  // From partial to opening
        key = 22;
        if (delayStepCountM1 <= pRampStepsM1) {  // Accelerate if partial
          key = 23;
          return delays[delayStepCountM1];
        } else {  // Decelerate if partial
          key = 24;
          return delays[closeStepsLeftM1 - delayStepCountM1];
        }
      } else {  // Decelerate if no partial
        key = 25;
        return delays[closeStepsLeftM1 - delayStepCountM1];
      }
    }
  }
  return 0;  // Default return if system state is not matched
}

void stepControlM1(boolean direction) {
  static unsigned long lastMicros = 0;  // Time of the last state change
  static boolean stepState = LOW;  // Current state of the step pin

  // If failsafe is activated, stop movement
  if (failsafeActivated) {
    stepState = LOW;
    digitalWrite(STEP_PIN_M1, stepState);
    return;  // Exit the function without performing any movement
  }

  // Set movement direction
  digitalWrite(DIR_PIN_M1, direction);

  // Use the full step delay
  unsigned long fullStepDelay = 1000000L * stepDelayM1;

  // Check if it's time to change the state
  if (currentMicros - lastMicros >= fullStepDelay) {
    stepState = !stepState;  // Toggle step state
    digitalWrite(STEP_PIN_M1, stepState);  // Set the step pin to the new state
    lastMicros = currentMicros;  // Update the last state change time

    // Update step count and delayStepCountM1 only when the LOW phase is completed
    if (stepState == LOW) {
      stepCountM1 += (direction == HIGH) ? 1 : -1;

      // Additional logic for system state
      if (systemState == CLOSING || systemState == OPENING) {
        delayStepCountM1++;
      }

      // Calculate the delay and RPM after each completed step
      stepDelayM1 = delayCalcM1();
      sumDelay += stepDelayM1;
      if (fabs(stepDelayM1) < EPSILON) {  // Using a small epsilon value
        rpmM1 = 0.0;
      } else {
        rpmM1 = ((stepLength / stepDelayM1) / distancePerRev) * 60.0;
      }
    }
  }
}


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;  // Store the current time
    *statusM1 = 1;
  }
  if (currentMicros - *microsM1 <= duration * 1000000L) {
    key = action ? 1000 : 2000;
    digitalWrite(pin, HIGH);  // Set the pin HIGH
    *statusM1 = 1;
  }
  if (*statusM1 != 2 && currentMicros - *microsM1 > duration * 1000000L) {
    key = action ? 3000 : 4000;
    digitalWrite(pin, LOW);  // Set the pin LOW
    *statusM1 = 2;
  }
}


void signalVal() {
  if ((currentMillis - lastDebounceTime) > debounceDelay) { // Debounce delay
    // Check all signal pins
    for (int i = 0; i < NUM_SIGNALS; i++) {
      // Skip processing for OPEN_PIN if latch or unlatch is active
      if (SIGNAL_PINS[i] == OPEN_PIN && (latchStatusM1 == 1 || unLatchStatusM1 == 1)) {
        continue;  // Skip OPEN_PIN processing during latch/unlatch
      }

      // Read and process the signal pin state
      signalState[i] = digitalRead(SIGNAL_PINS[i]);

      // Failsafe Trigger Logic
      if (signalState[i] != lastSignalState[i]) {
        lastDebounceTime = currentMillis;  // Reset debounce timer
        if (signalState[i] == HIGH) {
          highSignalValidationCounter[i]++;
          
          // Constrain highSignalValidationCounter[0] to cycle between 0 and 3
          highSignalValidationCounter[i] = highSignalValidationCounter[i] % 4;

          // If OPT_DIS_PIN or CRUSH_PREV_PIN goes HIGH, trigger failsafe
          if ((SIGNAL_PINS[i] == OPT_DIS_PIN || SIGNAL_PINS[i] == CRUSH_PREV_PIN) && (opClCounter == 1 || opClCounter == 2)) {
            key = 2222;
            previousOpClCounter = opClCounter;  // Store current opClCounter before setting to 0
            opClCounter = 0;  // Ensure the system stops movement immediately
            failsafeActivated = true;  // Indicate failsafe was triggered

            // Adjust highSignalValidationCounter[0] to prepare for correct opClCounter update
            if (previousOpClCounter == 1) {
              key = 3333;
              highSignalValidationCounter[0] = 2;  // Prepare for next pulse to set opClCounter to 2
            } else if (previousOpClCounter == 2) {
              key = 4444;
              highSignalValidationCounter[0] = 3;  // Prepare for next pulse to set opClCounter to 1
            }

            Serial.println("Failsafe triggered: movement stopped.");
            Serial.println("highSignalValidationCounter[0]: " + String(highSignalValidationCounter[0]));
            Serial.println("previousOpClCounter: " + String(previousOpClCounter));
            Serial.println("opClCounter: " + String(opClCounter));
          }
        } else {
          lowSignalValidationCounter[i]++;
        }
      }
      lastSignalState[i] = signalState[i];

      // Handle the OPEN_PIN based on validated state changes
      if (highSignalValidationCounter[0] % 2 == 1) {  // Odd pulses
        key = 5555;
        if (failsafeActivated) {
          // If failsafe is active, determine what the opClCounter should be
          pendingOpClCounter = (highSignalValidationCounter[0] % 4 == 1) ? 1 : 2;

          // Check if the failsafe can be cleared
          if ((pendingOpClCounter == 1 && previousSystemState == CLOSING) ||
              (pendingOpClCounter == 2 && previousSystemState == OPENING)) {
            opClCounter = pendingOpClCounter;  // Now update opClCounter
            failsafeActivated = false;  // Reset failsafe flag
            systemState = (opClCounter == 1) ? OPENING : CLOSING;  // Set the new state
            Serial.println("Failsafe reset: system resuming operation in opposite direction.");
          }
        } else {
          opClCounter = (highSignalValidationCounter[0] % 4 == 1) ? 1 : 2;
        }
      } else {  // Even pulses
        key = 6666;
        opClCounter = 0;
      }

      // Debugging output
      Serial.println("highSignalValidationCounter[0]: " + String(highSignalValidationCounter[0]));
      Serial.println("previousOpClCounter: " + String(previousOpClCounter));
      Serial.println("opClCounter: " + String(opClCounter));
    }
  }
}


  boolean checkSignalStates() {
    return (signalState[1] == LOW || signalState[2] == LOW);
  }

  void stateEval() {
    if (currentSystemState != systemState) {
      systemStateChange = true;
      oldSystemState = previousSystemState;  // Store the state before the last change
      previousSystemState = currentSystemState;  // Update previousSystemState
      currentSystemState = systemState;  // Update currentSystemState
    } else {
      systemStateChange = false;
    }

    if (systemState == OPENING) {
      handleOpeningState();
    } else if (systemState == CLOSING) {
      handleClosingState();
    } else if (systemState == OPENED) {
      handleOpenedState();
    } else if (systemState == CLOSED) {
      handleClosedState();
    } else if (systemState == PARTIAL) {
      handlePartialState();
    }
  }

  void handleOpeningState() {
    if (systemStateChange) {
      if (previousSystemState == PARTIAL || previousSystemState == CLOSED) {
        openStepsLeftM1 = openStepTargetM1 - stepCountM1;
        pRampStepsM1 = (openStepsLeftM1 >= 2 * rampStepsM1) ? rampStepsM1 : openStepsLeftM1 / 2;
        vMaxStepsM1 = openStepsLeftM1 - 2 * pRampStepsM1;
        if (vMaxStepsM1 == 1) vMaxStepsM1 = 0;
      }
    }

    if (previousSystemState == CLOSED) {
      latchControlM1(false);  // Unlatch
      if (unLatchStatusM1 == 2) {
        stepControlM1(HIGH);  // Move open
      }
    } else {
      stepControlM1(HIGH);  // Move open
    }

    if (signalState[1] == HIGH || signalState[2] == HIGH || opClCounter == 0 || stepCountM1 > openStepTargetM1) {
      systemState = PARTIAL;  // Stop opening motion, set state to partial
    } else if (signalState[3] == HIGH || stepCountM1 == openStepTargetM1) {
      systemState = OPENED;  // Stop movement and change state to open
      openStepTargetM1 = stepCountM1;  // Reset StepCounter
      sumDelay = 0;
      highSignalValidationCounter[0] = 2;
    }
  }

  void handleClosingState() {
    if (systemStateChange) {
      if (previousSystemState == PARTIAL || previousSystemState == OPENED) {
        closeStepsLeftM1 = stepCountM1;
        pRampStepsM1 = (closeStepsLeftM1 >= 2 * rampStepsM1) ? rampStepsM1 : closeStepsLeftM1 / 2;
        vMaxStepsM1 = closeStepsLeftM1 - 2 * pRampStepsM1;
        if (vMaxStepsM1 == 1) vMaxStepsM1 = 0;
      }
    }
    stepControlM1(LOW);  // Move close
    if (signalState[1] == HIGH || signalState[2] == HIGH || opClCounter == 0 || stepCountM1 < closeStepTargetM1) {
      systemState = PARTIAL;  // Stop closing motion, set state to partial
    } else if (signalState[4] == HIGH || stepCountM1 == closeStepTargetM1) {
      systemState = CLOSED;  // Stop movement and change state to closed
      unLatchStatusM1 = 0;
      latchStatusM1 = 0;
      stepCountM1 = 0;  // Reset step count (Index)
      sumDelay = 0;
      highSignalValidationCounter[0] = 0;
    }
  }

  void handleOpenedState() {
    if (checkSignalStates()) {
      if (opClCounter == 2) {  // Check close command signal
        delayStepCountM1 = 0;
        systemState = CLOSING;  // Set close motion change status to closing
        sumDelay = 0;
      }
    }
  }

  void handleClosedState() {
    if (previousSystemState == CLOSING) {
      latchControlM1(true);  // Latch
      key = 400;
    }
    if (checkSignalStates()) {
      if (opClCounter == 1) {  // Check open command signal
        delayStepCountM1 = 0;
        systemState = OPENING;  // Set open motion change status to opening
        sumDelay = 0;
      }
    }
  }

  void handlePartialState() {
    if (signalState[1] == LOW && signalState[2] == LOW) {
      if (stepCountM1 < openStepTargetM1 && opClCounter == 1) {  // Check open command signal
        delayStepCountM1 = 0;
        systemState = OPENING;  // Set open motion change status to opening
      } else if (stepCountM1 > closeStepTargetM1 && opClCounter == 2) {  // Check close command signal
        delayStepCountM1 = 0;
        systemState = CLOSING;  // Set close motion change status to closing
      }
    }
  }

  void printArray(double arr[], int size) {
    for (int i = 0; i < size; i++) {
      Serial.println(arr[i], 6); // 6 is the number of decimal places for floating point numbers
    }
  }

  void debug(int active, int period) {
    if (active && (currentMillis - lastDebugTime > period)) {
      String s = "";
      s += " St: " + String(stepCountM1);
      // s += " rmpSt: " + String(rampStepsM1);
      // s += " St: " + String(delayStepCountM1);
      s += " time: " + dToStr(sumDelay);
      // s += " DIR: " + String(digitalRead(DIR_PIN_M1));
      // s += " STP: " + String(digitalRead(STEP_PIN_M1));
      // s += " stDelay: " + dToStr(stepDelayM1);
      s += " key: " + String(key);
      s += " rpm: " + String(rpmM1);
      s += " fail: " + String(failsafeActivated);
      s += " opCl: " + String(opClCounter);
      // s += " sysCh: " + String(systemStateChange);
      // s += " curSys: " + String(currentSystemState);
      // s += " sys: " + String(systemState);
      // s += " preSys: " + String(previousSystemState);
      // s += " ltchSt: " + String(latchStatusM1);
      // s += " unLtchSt: " + String(unLatchStatusM1);
      // s += " ltch: " + String(digitalRead(LATCH_PIN_M1));
      // s += " unltch: " + String(digitalRead(UNLATCH_PIN_M1));
      // s += " currentMicros: " + dToStr(currentMicros);
      // s += " ltchMcrs: " + dToStr(latchMicrosM1);
      // s += " unltchMcrs: " + dToStr(unLatchMicrosM1);
      lastDebugTime = currentMillis;
      Serial.println(s);
    }
  }