#include <Servo.h>

// Define pins
#define relay1Pin 4
#define relay2Pin 5
#define relay3Pin 6
#define relay4Pin 7
#define motorPin 8
#define led1Pin 9
#define led2Pin 10
#define button1Pin 2 //Relay 1
#define button2Pin 3 //Relay 2
#define button3Pin 11 //Relay 3
#define button4Pin 12 //Relay 4
#define button5Pin 13 //Relay 5 (Motor)
#define button6Pin A0 //Reset

Servo servo1, servo2, servo3;

unsigned long relayActivationDelay = 2000; //Relay will start 2 seconds after the button press
unsigned long relayDeactivationDelay = 2000; //relay will stay on for 2 seconds

unsigned long relayActivationStartTime[] = {0, 0, 0, 0, 0}; 
unsigned long relayDeactivationStartTime[] = {0, 0, 0, 0, 0}; 

bool buttonState[] = {LOW, LOW, LOW, LOW, LOW}; 
bool lastButtonState[] = {LOW, LOW, LOW, LOW, LOW}; 
unsigned long lastDebounceTime[] = {0, 0, 0, 0, 0}; 
unsigned long debounceDelay = 50;

int servoPin1 = A1;
int servoPin2 = A2;
int servoPin3 = A3;

void setup() 
{
  Serial.begin(9600);
  pinMode(relay1Pin, OUTPUT);
  pinMode(relay2Pin, OUTPUT);
  pinMode(relay3Pin, OUTPUT);
  pinMode(relay4Pin, OUTPUT);
  pinMode(motorPin, OUTPUT);
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  pinMode(button1Pin, INPUT_PULLUP);
  pinMode(button2Pin, INPUT_PULLUP);
  pinMode(button3Pin, INPUT_PULLUP);
  pinMode(button4Pin, INPUT_PULLUP);
  pinMode(button5Pin, INPUT_PULLUP);
  pinMode(button6Pin, INPUT_PULLUP);
  
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
}

void loop() 
{
  handleButtonPress(button1Pin, relay1Pin, 0);
  handleButtonPress(button2Pin, relay2Pin, 1);
  handleButtonPress(button3Pin, relay3Pin, 2);
  handleButtonPress(button4Pin, relay4Pin, 3);
  handleButtonPress(button5Pin, motorPin, 4);

  if (digitalRead(button6Pin) == HIGH) 
  {
    Serial.println("Emergency Shut Down.");
    digitalWrite(relay1Pin, LOW);
    digitalWrite(relay2Pin, LOW);
    digitalWrite(relay3Pin, LOW);
    digitalWrite(relay4Pin, LOW);
    digitalWrite(motorPin, LOW);
  }
}

void handleButtonPress(int buttonPin, int relayPin, int relayIndex) {
  static bool relayActive[] = {false, false, false, false, false};
  bool buttonState = digitalRead(buttonPin);

  int servoIndex = -1;

  // Assign servo index based on relay index
  if (relayIndex == 0) {
    servoIndex = 0; 
  } else if (relayIndex == 1) {
    servoIndex = 1; 
  } else if (relayIndex == 2) {
    servoIndex = 2; 
  }
  
  if ((millis() - lastDebounceTime[relayIndex]) > debounceDelay) {
    if ((buttonState == LOW && !relayActive[relayIndex])) {
      digitalWrite(led2Pin, HIGH); // Turn on LED when the corresponding relay is activated
      relayActive[relayIndex] = true;
      delay(relayActivationDelay);
      Serial.print("Relay ");
      Serial.print(relayPin);
      Serial.println(" ON");
      digitalWrite(relayPin, HIGH);

      // Move associated servo to 90 degrees when the corresponding relay is activated
      if (servoIndex != -1) {
        switch (servoIndex) {
          case 0:
            servo1.write(90);
            break;
          case 1:
            servo2.write(90);
            break;
          case 2:
            servo3.write(90);
            break;
        }
      }

      digitalWrite(led2Pin, HIGH); // Turn on LED when the corresponding relay is activated

      delay(relayDeactivationDelay);
      Serial.print("Relay ");
      Serial.print(relayPin);
      Serial.println(" OFF");
      digitalWrite(relayPin, LOW);

      // Move associated servo back to 0 degrees when the corresponding relay is turned off
      if (servoIndex != -1) {
        switch (servoIndex) {
          case 0:
            servo1.write(0);
            break;
          case 1:
            servo2.write(0);
            break;
          case 2:
            servo3.write(0);
            break;
        }
      }

      digitalWrite(led2Pin, LOW); // Turn off LED when the corresponding relay is turned off

      relayActive[relayIndex] = false; 
    }
    lastDebounceTime[relayIndex] = millis();
  }
}