// Define Motor 1 (Servo Motor) pins
#define EN1 33   // Enable pin for Motor 1
#define IN1 34   // Input pin 1 for Motor 1
#define IN2 35   // Input pin 2 for Motor 1

// Define Motor 2 (Linear Actuator) pins
#define EN2 38   // Enable pin for Motor 2
#define IN3 36   // Input pin 1 for Motor 2
#define IN4 37   // Input pin 2 for Motor 2

// Define LEDs
#define YELLOW_LED 12   // LED for Inductive Sensor 1
#define GREEN_LED 13    // LED for Inductive Sensor 2
#define WHITE_LED 46    // LED for Limit Switch

#define CW_LED1 47    // Violet LED for Motor 1 (Clockwise)
#define CCW_LED1 48   // LightBlue LED for Motor 1 (Counterclockwise)

// Define buttons
#define YELLOW_BUTTON 31  // Button for Inductive Sensor 1
#define GREEN_BUTTON 32   // Button for Inductive Sensor 2
#define WHITE_BUTTON 45   // Button for Limit Switch

// Blinking interval (milliseconds)
const unsigned long BLINK_INTERVAL = 500;

// Variables to store the last blink time for each LED
unsigned long previousYellowTime = 0;
unsigned long previousGreenTime = 0;
unsigned long previousWhiteTime = 0;

// Variables to track the current LED states
bool yellowState = false;
bool greenState = false;
bool whiteState = false;

// State variables
bool yellowActive = false;
bool greenActive = false;
bool whiteActive = false;

void setup() {
  // Set motor control pins as outputs
  pinMode(EN1, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(EN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // Set LED pins as outputs
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(WHITE_LED, OUTPUT);
  pinMode(CW_LED1, OUTPUT);
  pinMode(CCW_LED1, OUTPUT);


  // Set button pins as inputs with pull-up resistors
  pinMode(YELLOW_BUTTON, INPUT_PULLUP);
  pinMode(GREEN_BUTTON, INPUT_PULLUP);
  pinMode(WHITE_BUTTON, INPUT_PULLUP);

  // Initialize all outputs to LOW
  resetOutputs();
}

void loop() {
  // Read button states
  bool yellowButton = digitalRead(YELLOW_BUTTON) == LOW;
  bool greenButton = digitalRead(GREEN_BUTTON) == LOW;
  bool whiteButton = digitalRead(WHITE_BUTTON) == LOW;

  unsigned long currentTime = millis(); // Get the current time

  // Sequence logic
  if (yellowButton && !yellowActive) {
    resetOutputs();         // Turn off all outputs before activating yellow
    yellowActive = true;
    greenActive = false;
    whiteActive = false;
  } else if (greenButton && !greenActive) {
    resetOutputs();         // Turn off all outputs before activating green
    yellowActive = false;
    greenActive = true;
    whiteActive = false;
  } else if (whiteButton && !whiteActive) {
    resetOutputs();         // Turn off all outputs before activating white
    yellowActive = false;
    greenActive = false;
    whiteActive = true;
  }

  // Execute the active function
  if (yellowActive) {
    controlYellow(currentTime);
  } else if (greenActive) {
    controlGreen(currentTime);
  } else if (whiteActive) {
    controlWhite(currentTime);
  }
}

void resetOutputs() {
  // Turn off all LEDs
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(GREEN_LED, LOW);
  digitalWrite(WHITE_LED, LOW);
  digitalWrite(CW_LED1, LOW);
  digitalWrite(CCW_LED1, LOW);


  // Disable both motors
  digitalWrite(EN1, LOW);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(EN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

void controlYellow(unsigned long currentTime) {
  // Blink the Yellow LED at the specified interval
  if (currentTime - previousYellowTime >= BLINK_INTERVAL) {
    previousYellowTime = currentTime;
    yellowState = !yellowState; // Toggle LED state
    digitalWrite(YELLOW_LED, yellowState);
  }
  digitalWrite(EN1, HIGH);        // Enable Motor 1
  digitalWrite(IN1, HIGH);        // Rotate Motor 1 Clockwise
  digitalWrite(IN2, LOW);
  digitalWrite(CW_LED1, HIGH);    // Turn on CW LED for Motor 1
  digitalWrite(CCW_LED1, LOW);    // Turn off CCW LED for Motor 1
}

void controlGreen(unsigned long currentTime) {
  // Blink the Green LED at the specified interval
  if (currentTime - previousGreenTime >= BLINK_INTERVAL) {
    previousGreenTime = currentTime;
    greenState = !greenState; // Toggle LED state
    digitalWrite(GREEN_LED, greenState);
  }
  digitalWrite(EN2, HIGH);        // Enable Motor 2
  digitalWrite(IN3, HIGH);        // Rotate Motor 2 Clockwise
  digitalWrite(IN4, LOW);
  digitalWrite(CW_LED1, HIGH);    // Turn on CW LED for Motor 2
  digitalWrite(CCW_LED1, LOW);    // Turn off CCW LED for Motor 2
}

void controlWhite(unsigned long currentTime) {
  // Blink the White LED at the specified interval
  if (currentTime - previousWhiteTime >= BLINK_INTERVAL) {
    previousWhiteTime = currentTime;
    whiteState = !whiteState; // Toggle LED state
    digitalWrite(WHITE_LED, whiteState);
  }
  digitalWrite(EN2, HIGH);        // Enable Motor 2
  digitalWrite(IN3, LOW);         // Rotate Motor 2 Counterclockwise
  digitalWrite(IN4, HIGH);
  digitalWrite(CW_LED1, LOW);     // Turn off CW LED for Motor 2
  digitalWrite(CCW_LED1, HIGH);   // Turn on CCW LED for Motor 2
}