// Define input pins
// Inputs
#define SYSTEM 3
#define CYCLE 6
#define ESTOP 2
#define RESET_PIN 4   // New Reset Button Pin

// Outputs
#define LIGHT 7
#define INTERLOCK 8
#define MCU 10
#define CLUTCH 11
#define STRIP_FEED 12

// System States
enum SystemState {
  NORMAL,
  EMERGENCY_STOP
};

volatile SystemState currentState = NORMAL; // Using volatile as it can be changed in ISR
bool is_on = false; // Flag to check system status for cycle press

// Function Prototypes
void startup();
void shutdownSystem();
void cycleOperation();
void emergencyStop();
void resetSystem();

void setup() {
  Serial.begin(9600);

  // Initialize input pins
  pinMode(SYSTEM, INPUT);
  pinMode(CYCLE, INPUT);
  pinMode(ESTOP, INPUT_PULLUP);   // Use internal pull-up resistor for safety
  pinMode(RESET_PIN, INPUT_PULLUP); // Use internal pull-up resistor

  // Initialize output pins
  pinMode(LIGHT, OUTPUT);
  pinMode(INTERLOCK, OUTPUT);
  pinMode(MCU, OUTPUT);
  pinMode(CLUTCH, OUTPUT);
  pinMode(STRIP_FEED, OUTPUT);

  // Ensure all outputs are OFF initially
  shutdownSystem();

  // Attach Interrupt for Emergency Stop (Triggered on FALLING edge due to INPUT_PULLUP)
  attachInterrupt(digitalPinToInterrupt(ESTOP), emergencyStop, FALLING);
}

void loop() {
  // If the system is in emergency stop, only allow reset
  if (currentState == EMERGENCY_STOP) {
    // Check if reset button is pressed
    if (digitalRead(RESET_PIN) == LOW) { // Assuming LOW when pressed
      resetSystem();
    }
    // Optional: Provide feedback that the system is in emergency stop
    Serial.println("System is in Emergency Stop. Press reset to continue.");
    delay(500); // Reduce serial spam
    return; // Skip the rest of the loop
  }

  // Normal operation
  int onoff = digitalRead(SYSTEM);       // Check if system should be ON
  int cyclepressed = digitalRead(CYCLE); // Check if cycle button is pressed

  if (onoff == HIGH) {
    // Execute the startup sequence
    if (!is_on) { // Prevent re-executing startup if already on
      startup();
      is_on = true; // Set the flag
    }
  } else {
    // Execute shutdown sequence
    if (is_on) { // Prevent re-executing shutdown if already off
      shutdownSystem();
      is_on = false; // Clear the flag
    }
  }

  // Handle cycle operation
  if (cyclepressed == HIGH) { // Cycle start button pressed
    // Simple debounce: wait until button is released
    while (digitalRead(CYCLE) == HIGH) {
      delay(100);
    }

    if (is_on) { // Check that power is already on
      cycleOperation();
    }

    // Note: No need to reset the CYCLE button here as it's an INPUT
  }

  // Small delay to avoid bouncing and reduce CPU usage
  delay(50);
}

// Function to start up the system
void startup() {
  Serial.println("System Starting Up...");
  digitalWrite(LIGHT, HIGH);      // Turn on the light
  delay(500);
  digitalWrite(INTERLOCK, HIGH);  // Activate the interlock
  digitalWrite(MCU, HIGH);        // Activate the MCU
  Serial.println("System is ON.");
}

// Function to shut down the system
void shutdownSystem() {
  Serial.println("System Shutting Down...");
  digitalWrite(LIGHT, LOW);       // Turn off the light
  delay(500);
  digitalWrite(INTERLOCK, LOW);   // Deactivate the interlock
  digitalWrite(MCU, LOW);         // Deactivate the MCU
  Serial.println("System is OFF.");
}

// Function to carry out a cycle operation
void cycleOperation() {
  Serial.println("Cycle Operation Started.");
  digitalWrite(CLUTCH, HIGH);     // Engage the clutch
  delay(500);
  digitalWrite(CLUTCH, LOW);      // Disengage the clutch
  delay(500);
  digitalWrite(STRIP_FEED, HIGH); // Activate the strip feeder
  delay(500);
  digitalWrite(STRIP_FEED, LOW);  // Deactivate the strip feeder
  delay(500);
  Serial.println("Cycle Operation Completed.");
}

// Emergency Stop Interrupt Service Routine (ISR)
void emergencyStop() {
  // Set system state to EMERGENCY_STOP
  currentState = EMERGENCY_STOP;

  // Perform emergency shutdown
  digitalWrite(LIGHT, LOW);      // Turn off the light
  digitalWrite(INTERLOCK, LOW);  // Deactivate the interlock
  digitalWrite(MCU, LOW);        // Deactivate the MCU
  digitalWrite(CLUTCH, LOW);     // Disengage the clutch
  digitalWrite(STRIP_FEED, LOW); // Deactivate the strip feeder

  // Optional: Send a message via Serial (Note: Avoid using Serial in ISR)
  // Serial.println("Emergency Stop Triggered!"); // Not recommended in ISR
}

// Function to reset the system from Emergency Stop
void resetSystem() {
  Serial.println("Resetting System from Emergency Stop...");
  currentState = NORMAL; // Reset the system state

  // Optionally, reinitialize outputs if necessary
  // Depending on your hardware, you might need to perform additional steps
  if (digitalRead(SYSTEM) == HIGH) {
    startup(); // Restart the system if SYSTEM is still ON
    is_on = true;
  }
  Serial.println("System Reset Completed. Normal Operation Resumed.");
}