#define version "2024-04-30"

#define GANTRY_SERIAL_ENABLED true
#define GANTRY_SIMPLE_ENABLED false

#include <AccelStepper.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
//#include <EEPROM.h>
#include <Bounce2.h> // Library for debouncing buttons
#include <avr/wdt.h>

// Motor interface type must be defined as 1 when using a driver
#define motorInterfaceType 1

// Define the pins (CNC Shield V3 pinout: https://forum.arduino.cc/t/4-stepper-and-2-servo-with-cnc-shield/984330)
const uint8_t turntableStepPin = 4; // (CNC Shield Z Axis)
const uint8_t turntableDirPin = 7; 
const uint8_t enablePin = 8; // You can choose an appropriate pin
const uint8_t ejectPistonPin = 12; // Pin for the eject piston (SpinEn Pin)
const uint8_t heaterPin = A3;   // Pin for the heater (CoolEn Pin)
const uint8_t turntableHomeSensorPin = 9; // Pin for the home sensor
const uint8_t heaterLimitSwitchPin = 10;  // Upper limit switch
const uint8_t heaterLimitSwitchPin2 = 2;  // Upper limit switch
const uint8_t cycleStartButtonPin = A1; // Pin for cycle start button
const uint8_t panicButtonPin = 2; // Pin for panic button
const uint8_t heartbeatLEDPin = 13; // The built-in LED pin on most Arduino boards
const uint8_t actuatorUpPin = A0; // "Abort" on CNC Shield
const uint8_t actuatorDownPin = A2; // "Resume" on CNC Shield
const uint8_t loadRequestPin = A4; // Output
const uint8_t loadAcknowledgePin = 6; // Input
const uint8_t loadCompletePin = A5; // Input
const uint8_t loadErrorPin = 3; // Input

// Setup parameters
const bool panicFunctionEnabled = false; // Set to false to disable the panic button
const bool confirmHeaterRaisedEnabled = true;
const bool confirmHeaterLoweredEnabled = true;
const bool reverseTurntable = false; // Set true to reverse turntable direction
bool gantryEnabled = true;

// User-adjustable values
const unsigned long ejectPistonDuration = 500; // Piston activation duration in milliseconds (0.5 seconds)
const unsigned long heatDuration = 90000; // Heat duration in milliseconds (55 seconds)
const long turntableMaxSpeed = 1000;
const long turntableAcceleration = 8000;
const long stepsPerIndex = 2040; // 408 teeth / 20 teeth * 200 steps per rev * 2 half-steps / 4 tuntable positions
const long heaterAdditionalSteps = 0;
const long tippingAdditionalSteps = 0;
const long ejectAdditionalSteps = 2040;
const long loadingAdditionalSteps = 0;
const unsigned long heaterRaiseFullyDuration = 3000; // 3 seconds to fully raise heater
const unsigned long heaterRaiseMinimumDuration = 2600; // 2.6 seconds minimum time to fully raise heater
const unsigned long heaterLowerFullyDuration = 3000; // 3 seconds to fully lower heater
const unsigned long cooldownDuration = 4 * 60 * 1000; // 4 minutes cooldown to give machine a break to prevent overcurrent error
const long cooldownAfterCyclesCount = 12;

const unsigned long heartbeatInterval = 500; // Heartbeat interval in milliseconds (1 second)
const unsigned long debounceInterval = 500; // Debounce interval in milliseconds, adjust as needed

// Various global variables
unsigned long lastHeartbeatTime = 0; // Last time the LED was toggled
bool heartbeatLEDState = LOW; // Current state of the LED
long currentPartIndex = 0; // Track the current position
volatile bool panicButtonPressedFlag = false;
bool isPanicking = false; // Flag to track if panic mode is active
volatile unsigned long lastPanicPressTime = 0;

AccelStepper turntableMotor(motorInterfaceType, turntableStepPin, turntableDirPin);
Bounce cycleStartButton = Bounce(); // Create a button object for cycle start button

//enum TurntablePosition { HOME, TO_HEATER, TO_TIPPING, TO_EJECT, LOADING };
enum TurntableDirection { FORWARD, REVERSE };
enum HeaterDirection { LOWER, RAISE };

void processCommand(String command);
//void moveTurntable(TurntablePosition targetPosition);
void heaterFindHomePosition();
void turntableFindHomePosition();
void heatPart(long duration);
void fireEjectPiston(long duration);
void advancePartIndex();
void printHelp();
//void panicButtonPressed();

void setup() {
  // Initialize Serial communication
  Serial.begin(9600);
  wdt_enable(WDTO_2S); // Enable the watchdog with a 2-second timeout
  #if GANTRY_SERIAL_ENABLED
    Serial1.begin(9600);
  #endif
  #if GANTRY_SIMPLE_ENABLED
    pinMode(loadRequestPin, OUTPUT);
    pinMode(loadAcknowledgePin, INPUT);
    pinMode(loadCompletePin, INPUT);
    pinMode(loadErrorPin, INPUT);
    digitalWrite(loadRequestPin, LOW);
  #endif
  Serial.print(F("Firmware version: "));
  Serial.println(version);
  Serial.println(F("i=0"));
  pinMode(heartbeatLEDPin, OUTPUT); // Set the built-in LED as an output
  digitalWrite(heartbeatLEDPin, heartbeatLEDState); // Initialize the LED state
/*
  if (panicFunctionEnabled) {
    pinMode(panicButtonPin, INPUT_PULLUP); // Set the panic button pin as input
    attachInterrupt(digitalPinToInterrupt(panicButtonPin), panicButtonPressed, FALLING); // Attach interrupt
  }
  */
  // Set the maximum speed and acceleration for the stepper motors
  turntableMotor.setMaxSpeed(turntableMaxSpeed); // Adjust as needed
  turntableMotor.setAcceleration(turntableAcceleration); // Adjust as needed
  // Initialize actuator control pins
  pinMode(actuatorUpPin, OUTPUT);
  pinMode(actuatorDownPin, OUTPUT);
  // Set the enable pin as an output
  pinMode(enablePin, OUTPUT);
  // Enable the stepper motors (LOW to enable on most CNC shields, but verify with your specific shield)
  digitalWrite(enablePin, LOW);
  // Initialize the limit switch pins as inputs
  pinMode(heaterLimitSwitchPin, INPUT_PULLUP);
  pinMode(heaterLimitSwitchPin2, INPUT_PULLUP);
  // Initialize the home sensor pin
  pinMode(turntableHomeSensorPin, INPUT_PULLUP);
  // Initialize the heater pin
  pinMode(heaterPin, OUTPUT);
  digitalWrite(heaterPin, LOW);
  // Initialize the eject piston pin
  pinMode(ejectPistonPin, OUTPUT);
  digitalWrite(ejectPistonPin , LOW);
  pinMode(cycleStartButtonPin, INPUT_PULLUP); // Set cycle start button pin as input with internal pull-up resistor
  cycleStartButton.attach(cycleStartButtonPin);
  cycleStartButton.interval(5); // Debounce interval in milliseconds  
}

void loop() {
  wdt_reset(); // Reset the watchdog timer
  unsigned long currentMillis = millis();
  // Check if it's time to toggle the heartbeat LED
  if (currentMillis - lastHeartbeatTime >= heartbeatInterval) {
      // Save the last time you toggled the LED
      lastHeartbeatTime = currentMillis;
      
      // Toggle the state of the LED
      heartbeatLEDState = !heartbeatLEDState;
      digitalWrite(heartbeatLEDPin, heartbeatLEDState);
  }
  // Check for button presses
  cycleStartButton.update();
  if (cycleStartButton.fell()) {
    // Cycle start button is pressed, start an auto cycle
    startAutomaticProcess(1); // Start 1 auto cycle
  }
  // Check for panic button flag
  if (panicButtonPressedFlag) {
    // Panic button is pressed, take action
    panicButtonPressedFlag = false; // Reset the flag
    // Stop any blocking code, such as stepper moves or delays
    // Execute any necessary actions for panic
    Serial.println(F("Source 20"));
    handlePanicEvent();
  }
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    int spaceIndex = input.indexOf(' ');
    while (spaceIndex != -1) {
      String command = input.substring(0, spaceIndex);
      command.trim();  // Trim the command string
      processCommand(command);
      input = input.substring(spaceIndex + 1);
      spaceIndex = input.indexOf(' ');
    }
    if (input.length() > 0) {
      input.trim();  // Trim the remaining input
      processCommand(input);
    }
  }
}

void processCommand(String command) {
  if (command == "h") {
    homeAxes();
  } else if (command.startsWith("a")) {
    int repeatCount = command.length() > 1 ? command.substring(1).toInt() : 1; // Default to 1 if no number specified
    startAutomaticProcess(repeatCount);
  } else if (command.startsWith("l")) {
    // Load parts using gantry.
    gantryRequestLoad();
    gantryConfirmLoaded();
  } else if (command.startsWith("x")) {
    if (command.length() > 1) {
      // Handle relative movements if needed
      if (command[1] == '+') {
        //moveActuator(RAISE, dur, dur);
        moveTurntable(FORWARD);
      } else if (command[1] == '-') {
        //moveActuator(LOWER, dur, dur);
        moveTurntable(REVERSE);
      } else {
        int position = command.substring(1).toInt(); // Convert the position part of the command to an integer
        int directionMultiplier = reverseTurntable ? -1 : 1;
        // Move the turntable motor by the calculated total steps
        turntableMotor.moveTo(position*directionMultiplier);
        // Run the motor until it reaches the target position
        while (turntableMotor.distanceToGo() != 0) {
          wdt_reset(); // Reset the watchdog timer
          turntableMotor.run();
          if (panicButtonPressedFlag) {
            // Panic button is pressed during motion, handle panic event
            Serial.println(F("Source 1"));
            handlePanicEvent();
            turntableMotor.stop();
            return; // Exit the function
          }
        }
        Serial.print(F("x="));
        Serial.println(position*directionMultiplier);
      }
    } else {
      long currentPosition = turntableMotor.currentPosition();
      Serial.print(F("x="));
      Serial.println(currentPosition);
    }
  } else if (command.startsWith("y")) {
    if (command == "y0") {
      moveActuator(LOWER, heaterLowerFullyDuration, heaterLowerFullyDuration);
    } else if (command == "y1") {
      moveActuator(RAISE, heaterRaiseFullyDuration, heaterRaiseFullyDuration);
    } else {
      // Handle incremental actuator movements if needed
      unsigned long dur = command.substring(2).toInt(); // Extract duration from command
      if (command[1] == '+') {
        moveActuator(RAISE, dur, dur);
      } else if (command[1] == '-') {
        moveActuator(LOWER, dur, dur);
      }
    }
  } else if (command.startsWith("i")) {
    Serial.print(F("i="));
    Serial.println(currentPartIndex);
  } else if (command.startsWith("q")) {
    unsigned long duration = command.substring(1).toInt();
    heatPart(duration);
  } else if (command.startsWith("e")) {
    unsigned long duration = command.substring(1).toInt();
    fireEjectPiston(duration);
  } else if (command.startsWith("g")) {
    unsigned long gantryEn = command.substring(1).toInt();
    if (gantryEn > 0) {
      gantryEnabled = true;
    } else {
      gantryEnabled = false;
    }
    Serial.print(F("g"));
    Serial.println(gantryEnabled);
  } else if (command.startsWith("z")) {
    printHelp();
  } else if (command.startsWith("p")) {
    // Do nothing...
  }
  // ... more else if conditions for other commands
}

void moveActuator(HeaterDirection direction, unsigned long durationMax, unsigned long durationMin) {
  // Added a bunch of safety checks to this function to make sure that the move operation doesn't get skipped or happens too fast. Some checks may be redundant.
  unsigned long startTime = millis();
  unsigned long endTime = startTime;
  wdt_reset(); // Reset the watchdog timer
  if (direction == RAISE) {
    Serial.println(F("Moving up"));
    digitalWrite(actuatorDownPin, LOW);
    digitalWrite(actuatorUpPin, HIGH);
  } else if (direction == LOWER) {
    Serial.println(F("Moving down"));
    digitalWrite(actuatorUpPin, LOW);
    digitalWrite(actuatorDownPin, HIGH);
  }
  Serial.print(F("Move start millis: "));
  Serial.println(startTime);
  while (endTime - startTime < durationMax) {
    wdt_reset(); // Reset the watchdog timer
    endTime = millis();
    if (direction == RAISE) {
      // Check that in upward motion, the move stops when the travel reaches the limit switch
      if (digitalRead(heaterLimitSwitchPin) == HIGH && endTime - startTime > durationMin) {
        // Reached top of travel
        Serial.println(F("Reached top of travel."));
        delay(50);
        break;
      }
      // Check for panic button press
      if (panicButtonPressedFlag) {
        Serial.println(F("Source 3a"));
        handlePanicEvent();
        break; // Exit if panic is detected
      }
    } else if (direction == LOWER) {
      // Check in downward motion
      if (endTime - startTime > durationMin) {
        // Passed min duration of travel, but continue until durationMax (no sensor to detect lowered).
        Serial.println(F("Passed minimum duration for downward travel."));
      }
      // Check for panic button press
      if (panicButtonPressedFlag) {
        Serial.println(F("Source 3b"));
        handlePanicEvent();
        break; // Exit if panic is detected
      }
    }
    // Check for panic button press
    if (panicButtonPressedFlag) {
      Serial.println(F("Source 4a"));
      handlePanicEvent();
      digitalWrite(actuatorUpPin, LOW); // Stop movement of actuator
      digitalWrite(actuatorDownPin, LOW); // Stop movement of actuator
      break; // Exit if panic is detected
    }
    if(endTime - startTime < 0) {
      Serial.println(F("Source 4b"));
      handlePanicEvent();
      digitalWrite(actuatorUpPin, LOW); // Stop movement of actuator
      digitalWrite(actuatorDownPin, LOW); // Stop movement of actuator
      break; // Exit if panic is detected
    }
  }
  Serial.print(F("Move stop millis: "));
  Serial.println(endTime);
  // Stop the actuator movement
  digitalWrite(actuatorUpPin, LOW);
  digitalWrite(actuatorDownPin, LOW);
  Serial.println(F("Done move."));
  if (startTime > endTime) {
    // Detected a logical fault.
    Serial.println(F("Error: A timing fault was detected during heater move. System will now panic."));
    panicButtonPressedFlag = true;
    Serial.println(F("Source 4c"));
    handlePanicEvent();
  }
}

void confirmHeaterRaised() {
  // Check for confirmation of heater fully raised with limit switch:
  if (confirmHeaterRaisedEnabled) {
    if (digitalRead(heaterLimitSwitchPin) == LOW) {
      // Heater was not fully raised.
      Serial.println(F("Error: Heater may not have fully raised. System will now panic to prevent possible collision with heater coil."));
      panicButtonPressedFlag = true;
      Serial.println(F("Source 5"));
      handlePanicEvent();
    }
  }
}

void confirmHeaterNotRaised() {
  // Check for confirmation of heater fully raised with limit switch:
  if (confirmHeaterRaisedEnabled) {
    if (digitalRead(heaterLimitSwitchPin) == HIGH) {
      // Heater is still raised.
      Serial.println(F("Error: Heater may not have lowered. System will now panic to prevent running dry."));
      panicButtonPressedFlag = true;
      Serial.println(F("Source 5a"));
      handlePanicEvent();
    }
  }
}

void confirmHeaterLowered() {
  // Check for confirmation of heater fully raised with limit switch:
  if (confirmHeaterLoweredEnabled) {
    if (digitalRead(heaterLimitSwitchPin2) == LOW) {
      // Heater was not lowered raised.
      Serial.println(F("Error: Heater may not have fully lowered. System will now panic to prevent running coil in upward position."));
      panicButtonPressedFlag = true;
      Serial.println(F("Source 5.1"));
      handlePanicEvent();
    }
  }
}

void homeAxes() {
  // Home both axes
  heaterFindHomePosition();
  turntableFindHomePosition(); 
}

void startAutomaticProcess(int repeatCount) {
  for (int i = 0; i < repeatCount; i++) {
    // Sequence of operations

    if ((i + 1) % cooldownAfterCyclesCount == 0) {
      Serial.print(F("Cooling down for (ms): "));
      Serial.println(cooldownDuration);
      Serial.flush(); // Ensure message is sent before potentially blocking
      checkPanicButtonDuringDelay(cooldownDuration);
    }

    gantryRequestLoad();
    moveActuator(LOWER, heaterLowerFullyDuration, heaterLowerFullyDuration);
    confirmHeaterNotRaised();
    confirmHeaterLowered();
    heatPart(heatDuration);
    moveActuator(RAISE, heaterRaiseFullyDuration, heaterRaiseMinimumDuration);
    confirmHeaterRaised();
    fireEjectPiston(ejectPistonDuration);
    gantryConfirmLoaded();
    advancePartIndex();
    moveTurntable(FORWARD);
  }
}

bool checkPanicButtonDuringDelay(int delayTime) {
  for (int i = 0; i < delayTime; i += 100) {
    wdt_reset(); // Reset the watchdog timer
    // Check the panicButtonPressedFlag during delay
    if (panicButtonPressedFlag) {
      // Panic button is pressed during delay, handle panic event
      Serial.println(F("Source 6"));
      handlePanicEvent();
      return true; // Exit the function
    }
    delay(100);
  }
  return false;
}

void moveTurntable(TurntableDirection direction) {
    wdt_reset();  // Reset the watchdog timer

    // Define the initial speed and ramping parameters
    int initialSpeed = 0;  // Start from a speed of 0
    const int rampStep = 100;  // Speed increase per step
    const int rampInterval = 10;  // Time interval in milliseconds between speed increases
    const int rampDuration = 500;  // Total ramping time in milliseconds

    int targetSpeed = (direction == FORWARD) ? turntableMaxSpeed : -turntableMaxSpeed;

    Serial.print(F("Moving turntable in direction: "));
    Serial.println(direction == FORWARD ? F("FORWARD") : F("REVERSE"));

    // Start moving
    unsigned long rampStartTime = millis();
    unsigned long lastUpdate = rampStartTime;

    while (millis() - rampStartTime < rampDuration) {
        wdt_reset();

        // Update speed at intervals of rampInterval
        if (millis() - lastUpdate >= rampInterval) {
            initialSpeed += (direction == FORWARD ? rampStep : -rampStep);  // Increase or decrease speed based on direction
            initialSpeed = (direction == FORWARD) ? min(initialSpeed, turntableMaxSpeed) : max(initialSpeed, -turntableMaxSpeed);  // Clamp speed to maxSpeed
            turntableMotor.setSpeed(initialSpeed);
            lastUpdate = millis();
        }

        turntableMotor.runSpeed();  // Run at the current speed
    }

    // Continue running at max speed until the switch goes HIGH again
    turntableMotor.setSpeed(targetSpeed);
    while (digitalRead(turntableHomeSensorPin) == LOW) {
        wdt_reset();
        turntableMotor.runSpeed();
    }

    // Stop the motor after the switch goes HIGH
    turntableMotor.stop();
    Serial.println(F("Turntable movement completed successfully."));
}

/*
void moveTurntable(TurntablePosition targetPosition) {
  wdt_reset(); // Reset the watchdog timer
  Serial.print(F("Moving turntable to position: "));
  Serial.println(targetPosition);
  int additionalSteps = 0; // Additional steps based on the target position
  if (targetPosition == TO_HEATER) {
    additionalSteps = heaterAdditionalSteps;
  } else if (targetPosition == TO_TIPPING) {
    additionalSteps = tippingAdditionalSteps;
  } else if (targetPosition == TO_EJECT) {
    additionalSteps = ejectAdditionalSteps;
  } else if (targetPosition == LOADING) {
    additionalSteps = loadingAdditionalSteps;
  }
  long totalSteps = currentPartIndex * stepsPerIndex + additionalSteps;
  int directionMultiplier = reverseTurntable ? -1 : 1;
  turntableMotor.moveTo(totalSteps * directionMultiplier); // Move the turntable motor by the calculated total steps
  // Run the motor until it reaches the target position or panic button is pressed
  while (turntableMotor.distanceToGo() != 0) {
    wdt_reset(); // Reset the watchdog timer
    // Check the panicButtonPressedFlag during motor movement
    if (panicButtonPressedFlag) {
      // Panic button is pressed during motion, handle panic event
      Serial.println(F("Source 7"));
      handlePanicEvent();
      turntableMotor.stop();
      return; // Exit the function
    }
    turntableMotor.run();
  }
  Serial.print(F("x="));
  Serial.println(turntableMotor.currentPosition());
}
*/

void turntableFindHomePosition() {
  wdt_reset(); // Reset the watchdog timer
  Serial.print(F("Moving turntable to position: "));
  Serial.println(F("Home"));
  // Rotate the turntable until the home sensor is triggered
  int directionMultiplier = reverseTurntable ? -1 : 1;
  int speed = 200 * directionMultiplier; // Adjust this value based on your requirements
  Serial.println(F("Homing turntable"));
  while (digitalRead(turntableHomeSensorPin) == HIGH) {
    wdt_reset(); // Reset the watchdog timer
    // Check the panicButtonPressedFlag during motion
    if (panicButtonPressedFlag) {
      // Panic button is pressed during motion, handle panic event
      Serial.println(F("Source 10"));
      handlePanicEvent();
      turntableMotor.stop();
      return; // Exit the function
    }
    turntableMotor.setSpeed(speed);
    turntableMotor.runSpeed();
  }
  turntableMotor.stop();
  turntableMotor.setMaxSpeed(200); // Adjust as needed
  turntableMotor.move(-200*directionMultiplier); // Move back a certain number of steps
  while (turntableMotor.distanceToGo() != 0) {
    wdt_reset(); // Reset the watchdog timer
    // Check the panicButtonPressedFlag during motor movement
    if (panicButtonPressedFlag) {
      // Panic button is pressed during motion, handle panic event
      Serial.println(F("Source 11"));
      handlePanicEvent();
      turntableMotor.stop();
      return; // Exit the function
    }
    turntableMotor.run();
  }
  speed = 100 * directionMultiplier; // Adjust this value based on your requirements
  while (digitalRead(turntableHomeSensorPin) == HIGH) {
    wdt_reset(); // Reset the watchdog timer
    // Check the panicButtonPressedFlag during motion
    if (panicButtonPressedFlag) {
      // Panic button is pressed during motion, handle panic event
      Serial.println(F("Source 12"));
      handlePanicEvent();
      turntableMotor.stop();
      return; // Exit the function
    }
    turntableMotor.setSpeed(speed);
    turntableMotor.runSpeed();
  }
  turntableMotor.stop();
  // Reset the position index to HOME
  turntableMotor.setMaxSpeed(turntableMaxSpeed);
  turntableMotor.setCurrentPosition(0);
  currentPartIndex = 0;
  Serial.println(F("Turntable homed"));
}

void heaterFindHomePosition() {
  moveActuator(RAISE, heaterRaiseFullyDuration, heaterRaiseFullyDuration); // Assume top position as home
  Serial.println(F("Heater homed"));
}

void heatPart(unsigned long duration) {
  wdt_reset(); // Reset the watchdog timer
  Serial.print(F("Heating for ms: "));
  Serial.println(duration);
  Serial.println(F("Heater on"));
  digitalWrite(heaterPin, HIGH); // Activate the heater
  unsigned long startTime = millis(); // Record the start time
  while (millis() - startTime < duration) {
    wdt_reset(); // Reset the watchdog timer
    // Check the panicButtonPressedFlag during the delay
    if (panicButtonPressedFlag) {
      // Panic button is pressed during the delay, handle panic event
      Serial.println(F("Source 14"));
      handlePanicEvent();
      digitalWrite(heaterPin, LOW); // Deactivate the heater
      Serial.println(F("Heater off"));
      return; // Exit the function
    }
  }
  digitalWrite(heaterPin, LOW); // Deactivate the heater
  Serial.println(F("Heater off"));
}

void fireEjectPiston(unsigned long duration) {
  wdt_reset(); // Reset the watchdog timer
  Serial.println(F("Eject Piston on"));
  digitalWrite(ejectPistonPin, HIGH);
  unsigned long startTime = millis(); // Record the start time
  while (millis() - startTime < duration) {
    wdt_reset(); // Reset the watchdog timer
    // Check the panicButtonPressedFlag during the delay
    if (panicButtonPressedFlag) {
      // Panic button is pressed during the delay, handle panic event
      Serial.println(F("Source 15"));
      handlePanicEvent();
      return; // Exit the function
    }
  }
  digitalWrite(ejectPistonPin, LOW);
  Serial.println(F("Eject Piston off"));
  if (checkPanicButtonDuringDelay(500)) return; // Check for panic button during delay
}

void advancePartIndex() {
  currentPartIndex++; // Increment the part index
  Serial.print("i=");
  Serial.println(currentPartIndex);
}

void panicButtonPressed() {
  unsigned long currentTime = millis();
  if (currentTime - lastPanicPressTime > debounceInterval) {
    panicButtonPressedFlag = true; // Enough time has passed since the last press - consider this a valid press
    lastPanicPressTime = currentTime; // Update the last press time
  }
  // If not enough time has passed, ignore this press (bounce)
}

void handlePanicEvent() {
  Serial.println(F("handlePanicEvent() called!")); // This is here for debugging
  panicButtonPressedFlag = false;
  digitalWrite(heaterPin, LOW); // Stop the heater
  digitalWrite(ejectPistonPin, LOW); // Shut off piston
  if (!isPanicking) {
    // Raise the heater
    isPanicking = true;
    digitalWrite(actuatorDownPin, LOW);
    digitalWrite(actuatorUpPin, LOW);
    moveActuator(RAISE, heaterRaiseFullyDuration, heaterRaiseFullyDuration);
  }
  digitalWrite(enablePin, HIGH); // Disable the steppers
  Serial.println(F("Panic Event! Reset Arduino to continue."));
  while (1) {
    wdt_reset(); // Reset the watchdog timer
    delay(1000); // Loop forever and wait for arduino to be reset.
  }
}

void gantryRequestLoad() {
  if (gantryEnabled) {
    #if GANTRY_SERIAL_ENABLED
      bool panic = false;
      wdt_reset(); // Reset the watchdog timer
      delay(5);
      while (Serial1.available() > 0) {
        delay(5);
        Serial1.read(); // Read and discard any stale data
        wdt_reset(); // Reset the watchdog timer
      }
      // Send command to load 2 widgets.
      Serial1.println("l");
      wdt_reset(); // Reset the watchdog timer
      delay(100);
      wdt_reset(); // Reset the watchdog timer
      if (Serial1.available() > 0) {
        wdt_reset(); // Reset the watchdog timer
        String buffer = Serial1.readStringUntil('\n');
        buffer.trim(); // Trim any newline or whitespace characters
        Serial.print("Received: '");
        Serial.print(buffer); // Show what was exactly received
        Serial.println("'");
        if (buffer == "Ack") {
          // Good result, proceed.
        } else {
          // Something went wrong, panic.
          panic = true;
          Serial.println("error1");
        }
      } else {
        // something went wrong, panic.
        panic = true;
        Serial.println("error2");
      }
      if (panic) {
        wdt_reset(); // Reset the watchdog timer
        Serial.println("Communication error with gantry.");
        panicButtonPressedFlag = true;
        Serial.println(F("Source 16"));
        handlePanicEvent();
      }
    #endif
    #if GANTRY_SIMPLE_ENABLED
      wdt_reset(); // Reset the watchdog timer
      bool panic = false;
      digitalWrite(loadRequestPin, HIGH); // Signal the gantry to load widgets
      unsigned long startTime = millis();
      // Wait for acknowledgment or timeout after 5 seconds
      while (digitalRead(loadAcknowledgePin) == LOW) {
        if (millis() - startTime > 5000) {
          panic = true;
          break;
        }
        wdt_reset(); // Reset the watchdog timer
        delay(10);
      }
      digitalWrite(loadRequestPin, LOW); // Clear the load request
      wdt_reset(); // Reset the watchdog timer
      delay(100);
      wdt_reset(); // Reset the watchdog timer
      if (panic) {
        Serial.println("Failed to receive load acknowledgment.");
        handlePanicEvent();
        return;
      }
      if (panic || digitalRead(loadErrorPin) == HIGH) {
        Serial.println("Loading error or timeout.");
        handlePanicEvent();
      } else {
        Serial.println("Load command acknowledged by gantry.");
      }
    #endif
  }
}

void gantryConfirmLoaded() {
  if (gantryEnabled) {
    #if GANTRY_SERIAL_ENABLED
      bool panic = false;
      wdt_reset(); // Reset the watchdog timer
      while (Serial1.available() == 0) {
        Serial.println("waiting for gantry...");
        wdt_reset(); // Reset the watchdog timer
        delay(1000);
      }
      Serial.println("rec...");
      if (Serial1.available() > 0) {
        wdt_reset(); // Reset the watchdog timer
        String buffer = Serial1.readStringUntil('\n');
        buffer.trim(); // Trim any newline or whitespace characters
        Serial.print("Received: '");
        Serial.print(buffer); // Show what was exactly received
        Serial.println("'");
        if (buffer == "Loaded") {
          // Good result, proceed.
          Serial.println("Load completed.");
        } else {
          // Something went wrong, panic.
          panic = true;
        }
      } else {
        // something went wrong, panic.
        panic = true;
      }
      if (panic) {
        Serial.println("Communication error with gantry.");
        panicButtonPressedFlag = true;
        Serial.println(F("Source 17"));
        handlePanicEvent();
      }

    #endif
    #if GANTRY_SIMPLE_ENABLED
      // Now wait for the load to complete
      wdt_reset(); // Reset the watchdog timer
      bool panic = false;
      unsigned long startTime = millis();
      while (digitalRead(loadCompletePin) == LOW) {
        if (millis() - startTime > 120000) { // Wait longer for the actual loading (2 mins max)
          panic = true;
          break;
        }
        wdt_reset(); // Reset the watchdog timer
        delay(1000);
        Serial.println("Waiting for gantry to finish...");
      }
      if (panic) {
        Serial.println("Communication error with gantry.");
        panicButtonPressedFlag = true;
        Serial.println(F("Source 17.5"));
        handlePanicEvent();
      }
      digitalWrite(loadRequestPin, HIGH);
      wdt_reset(); // Reset the watchdog timer
      while (digitalRead(loadCompletePin) == HIGH) {
        // Waiting for load complete to be acknowledged
        wdt_reset(); // Reset the watchdog timer
        delay(1000);
      }
      digitalWrite(loadRequestPin, LOW);
      Serial.println("Load completed.");
      wdt_reset(); // Reset the watchdog timer
      delay(10);
      // Completed
    #endif
  }
}

void printHelp() {
  Serial.println(F("Command Guide:"));
  Serial.println(F("h - Home both axes"));
  Serial.println(F("a[#] - Start auto mode, # times (e.g., a5 for 5 times)"));
  Serial.println(F("x[pos] - Move turntable to position [pos]"));
  Serial.println(F("y[pos] - Move heater to position [pos]"));
  Serial.println(F("q[time] - Turn heater on for [time] miliseconds"));
  Serial.println(F("e[time] - Fire eject piston for [time] miliseconds"));
  Serial.println(F("g[0 or 1] - Enable or disable gantry robot"));
  Serial.println(F("p - Adjust EEPROM Program Values"));
  Serial.println(F("z - Print this helpful command guide"));
  // Add more command descriptions as needed
}