// Put your name and CWID here

#include <Servo.h>

// Pin definitions
#define GREY_BUTTON_PIN 3
#define BLUE_BUTTON_PIN A1
#define WHITE_BUTTON_PIN A4
#define BLACK_BUTTON_PIN A2
#define YELLOW_BUTTON_PIN A3
#define BLUE_LED_PIN 2
#define RED_LED_PIN 10
#define SWITCH_PIN 12
#define RIGHTSERVO_PIN 9
#define LEFTSERVO_PIN 6

// Servo objects
Servo rightServo;
Servo leftServo;

// Variables to store servo positions
int rightServoPos = 90; // Start at center position (0 to 180)
int leftServoPos = 90;  // Start at center position (0 to 180)

unsigned long lastButtonPress = 0; // For debounce handling

void setup() {
  // Initialize buttons and LEDs
  pinMode(GREY_BUTTON_PIN, INPUT_PULLUP);
  pinMode(BLUE_BUTTON_PIN, INPUT_PULLUP);
  pinMode(WHITE_BUTTON_PIN, INPUT_PULLUP);
  pinMode(BLACK_BUTTON_PIN, INPUT_PULLUP);
  pinMode(YELLOW_BUTTON_PIN, INPUT_PULLUP);
  pinMode(SWITCH_PIN, INPUT_PULLUP);
  pinMode(BLUE_LED_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);

  // Initialize servos
  rightServo.attach(RIGHTSERVO_PIN);
  leftServo.attach(LEFTSERVO_PIN);

  // Start both servos at the center position
  rightServo.write(rightServoPos);
  leftServo.write(leftServoPos);

  // Initially turn off blue LED and turn on red LED
  digitalWrite(BLUE_LED_PIN, LOW);
  digitalWrite(RED_LED_PIN, HIGH);
}

void loop() {
  // Read the slider switch to determine the mode
  int switchState = digitalRead(SWITCH_PIN);

  // Check debounce and button presses
  if (millis() - lastButtonPress > 10) {
    lastButtonPress = millis();

    // If slider switch is to the right (HIGH)
    if (switchState == HIGH) {
      digitalWrite(RED_LED_PIN, HIGH);
      digitalWrite(BLUE_LED_PIN, LOW);

      // Grey button: Center both servos
      if (digitalRead(GREY_BUTTON_PIN) == LOW) {
        rightServoPos = 90;
        leftServoPos = 90;
        rightServo.write(rightServoPos);
        leftServo.write(leftServoPos);
        delay(10); // Debounce delay
      }
    } 
    // If slider switch is to the left (LOW)
    else {
      digitalWrite(RED_LED_PIN, LOW);
      digitalWrite(BLUE_LED_PIN, HIGH);

      // Handle the other buttons
      if (digitalRead(BLUE_BUTTON_PIN) == LOW) {
        // Blue button: Rotate right servo negative and left servo positive
        rightServoPos = constrain(rightServoPos - 1, 0, 180);
        leftServoPos = constrain(leftServoPos + 1, 0, 180);
        rightServo.write(rightServoPos);
        leftServo.write(leftServoPos);
        delay(10); // Debounce delay
      }

      if (digitalRead(YELLOW_BUTTON_PIN) == LOW) {
        // Yellow button: Rotate right servo positive and left servo negative
        rightServoPos = constrain(rightServoPos + 1, 0, 180);
        leftServoPos = constrain(leftServoPos - 1, 0, 180);
        rightServo.write(rightServoPos);
        leftServo.write(leftServoPos);
        delay(10); // Debounce delay
      }

      if (digitalRead(BLACK_BUTTON_PIN) == LOW) {
        // Black button: Rotate both servos positive
        rightServoPos = constrain(rightServoPos + 1, 0, 180);
        leftServoPos = constrain(leftServoPos + 1, 0, 180);
        rightServo.write(rightServoPos);
        leftServo.write(leftServoPos);
        delay(10); // Debounce delay
      }

      if (digitalRead(WHITE_BUTTON_PIN) == LOW) {
        // White button: Rotate both servos negative
        rightServoPos = constrain(rightServoPos - 1, 0, 180);
        leftServoPos = constrain(leftServoPos - 1, 0, 180);
        rightServo.write(rightServoPos);
        leftServo.write(leftServoPos);
        delay(10); // Debounce delay
      }
    }
  }
}