// Original v17 code as a base with critical changes from v16 applied

// Convert double to string macro with specified precision
String dToStr(double val, byte precision = 6) {
  // Convert the integer part to string
  String s = String(int(val));  
  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

  while (precision--) {
    mult *= 10;
  }

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

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

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

// 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;  // (steps) 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

// Define a single array to hold precomputed delays for acceleration and deceleration
double delays[300];  // Array for delays

// Function prototypes
boolean checkFailsafeSignalStates();              // Check specific signal states
double steps(const double x, const double vMinM1, const double vMaxM1); // Calculate steps based on curve
double findStepTimes(int rampStepsM1);   // Find step times for given ramp steps
double calculateRampSteps();             // Calculate ramp steps based on ramp time
double delayCalcM1();                    // Calculate delay for M1 based on current state
void stepControlM1(boolean direction); // Control step for M1 with direction parameter
void latchControlM1(boolean action);   // Combined latch and unlatch function for M1 with an action parameter
void validateAndDebounceSignals();     // Validate signals with debounce
void handleFailsafe();                 // Handle failsafe logic
void manageOpClCounter();              // Manage opClCounter based on signals
void evaluateSystemState();            // Evaluate state and execute move commands
void handleOpeningState();             // Handle opening state
void handleClosingState();             // Handle closing state
void handleOpenedState();              // Handle opened state
void handleClosedState();              // Handle closed state
void handlePartialState();             // Handle partial state
void debug(int active, int period);    // Debug function for outputting system status
void precomputeDelays();               // Precompute delays and store them in the array
double calculateDelayForStep(int step); // Calculate delay for a specific step

// 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
  unLatchStatusM1 = 0;
  latchStatusM1 = 0;
  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 for opening and closing
  precomputeDelays();

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

  validateAndDebounceSignals();  // Validate input signals and debounce
  handleFailsafe();  // Handle failsafe conditions
  manageOpClCounter();  // Manage the open/close counter based on signals
  evaluateSystemState();  // Evaluate and command motion state

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

// Function implementations

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
}

// Function to calculate delay for a specific step
double calculateDelayForStep(int step) {
  double stepTime1 = findStepTimes(step);       // Calculate the time for the current step
  double stepTime2 = findStepTimes(step + 1);   // Calculate the time for the next step
  return stepTime2 - stepTime1;                 // Delay is the difference between subsequent step times
}

// Precompute delays and store them in the array
void precomputeDelays() {
  for (int i = 0; i < 299; i++) {  // Calculate delays for each step (excluding the last step to avoid out-of-bounds access)
    delays[i] = calculateDelayForStep(i);  // Calculate delays for each step
    // Serial.print("Delays[");
    // Serial.print(i);
    // Serial.print("] = ");
    // Serial.println(delays[i], 6);  // Print each delay with six decimals
  }
  delays[299] = 0; // The last delay is set to 0 (or the constant speed delay) to handle the edge case
}

// Calculate delay for M1 based on current state by retrieving from the precomputed array
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 closing
        key = 17;
        if (delayStepCountM1 <= pRampStepsM1) {  // Continue acceleration after partial
          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 closing
        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
}

// Function to update the step count, delay step count, and RPM
void updateStepCountAndRPM() {
  stepCountM1 += (opClCounter == 1) ? 1 : -1;

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

  if (fabs(stepDelayM1) < EPSILON) {  // Using a small epsilon value
    rpmM1 = 0.0;
  } else {
    rpmM1 = ((stepLength / stepDelayM1) / distancePerRev) * 60.0;
  }
}

// Function to set the motor direction
void setDirectionM1(boolean direction) {
  digitalWrite(DIR_PIN_M1, direction);
}

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

}

// Other utility functions
void validateAndDebounceSignals() {
  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]);

      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[0] = highSignalValidationCounter[0] % 4;
        } else {
          lowSignalValidationCounter[i]++;
        }
      }
      lastSignalState[i] = signalState[i];
    }
  }
}


void manageOpClCounter() {
  if (!failsafeActivated) {  // Only manage opClCounter if failsafe is not active
    if (highSignalValidationCounter[0] % 4 == 1) {
      opClCounter = 1;
    } else if (highSignalValidationCounter[0] % 4 == 3) {
      opClCounter = 2;
    } else {
      opClCounter = 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;
}

void evaluateSystemState() {
  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 handleFailsafe() {
  // Activation
  if ((signalState[1] == HIGH || signalState[2] == HIGH) && (opClCounter == 1 || opClCounter == 2)) {
    key = 2222;
    previousOpClCounter = opClCounter;  // Store current opClCounter
    opClCounter = 0;  // Reset opClCounter to stop movement
    pendingOpClCounter = previousOpClCounter;
    systemState = PARTIAL;  // Set state to PARTIAL
    failsafeActivated = true;  // Activate failsafe
  }

  // Clear failsafe
  if (opClCounter == 0 && failsafeActivated) {
    key = 3333;
    if (pendingOpClCounter == 1 && highSignalValidationCounter[0] == 2) {
      highSignalValidationCounter[0]--;  // Prepare for the next command to clear failsafe and start closing
    } else if (pendingOpClCounter == 2 && highSignalValidationCounter[0] == 0) {
      highSignalValidationCounter[0]++;  // Prepare for the next command to clear failsafe and start opening
    }
    failsafeActivated = false;  // Clear failsafe
  }
}

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

  // Execute unlatch if starting from the CLOSED state
  if (previousSystemState == CLOSED && latchStatusM1 == 0) {
    latchControlM1(LOW);  // Unlatch
  }

  if (unLatchStatusM1 == 2 && latchStatusM1 == 0) {
    latchControlM1(HIGH);  // Latch when unlatched and not latched
  }

  if (latchStatusM1 == 2 && unLatchStatusM1 == 2) {
    if (stepCountM1 < openStepTargetM1) {
      key = 5000;
      stepControlM1(HIGH);  // Execute motor movement to open
    } else {
      key = 6000;
      systemState = OPENED;  // Set state to OPENED when target is reached
    }
  }
}

void handleClosingState() {
  if (systemStateChange) {
    setupRampSteps(closeStepsLeftM1, pRampStepsM1, vMaxStepsM1, false);
  }

  if (previousSystemState == OPENED && latchStatusM1 == 0) {
    latchControlM1(LOW);  // Unlatch
  }

  if (unLatchStatusM1 == 2 && latchStatusM1 == 0) {
    latchControlM1(HIGH);  // Latch when unlatched and not latched
  }

  if (latchStatusM1 == 2 && unLatchStatusM1 == 2) {
    if (stepCountM1 > closeStepTargetM1) {
      key = 7000;
      stepControlM1(LOW);  // Execute motor movement to close
    } else {
      key = 8000;
      systemState = CLOSED;  // Set state to CLOSED when target is reached
    }
  }
}

void handleOpenedState() {
  // Stay in OPENED state
  key = 9000;
}

void handleClosedState() {
  // Stay in CLOSED state
  key = 10000;
}

void handlePartialState() {
  // Stay in PARTIAL state
  key = 11000;
}

void debug(int active, int period) {
  if (active == 1) {
    if (millis() - lastDebugTime >= period) {
      lastDebugTime = millis();
      Serial.print("St: ");
      Serial.print(stepCountM1);
      Serial.print(" DlSt: ");
      Serial.print(delayStepCountM1);
      Serial.print(" key: ");
      Serial.print(key);
      Serial.print(" rpm: ");
      Serial.print(rpmM1);
      Serial.print(" OPPin: ");
      Serial.print(signalState[0]);
      Serial.print(" hivlco: ");
      Serial.print(highSignalValidationCounter[0]);
      Serial.print(" opCl: ");
      Serial.print(opClCounter);
      Serial.print(" fail: ");
      Serial.print(failsafeActivated);
      Serial.print(" latc: ");
      Serial.print(latchStatusM1);
      Serial.print(" unlatc: ");
      Serial.println(unLatchStatusM1);
    }
  }
}