#include <Servo.h>

// Define servo objects
Servo servo1;
Servo servo2;

// Define pin assignments for LEDs and RGB LED
const int ledPins[3] = {22, 23, 24};
const int rgbPins[3] = {10, 11, 12}; // order: R, G, B

// Define servo control pins
const int servo1Pin = 2;
const int servo2Pin = 3;

// Variables to keep track of the LED and RGB color state
int currentLED = 0;  
int currentRGB = 0;  // 0: red, 1: green, 2: blue

// Function to update rgb LED color using PWM brightness values (0-255)
void setRGBColor(int colorIndex) {
  // Turn off all channels first
  analogWrite(rgbPins[0], 0);
  analogWrite(rgbPins[1], 0);
  analogWrite(rgbPins[2], 0);
  
  // Set the desired color (full brightness)
  switch (colorIndex) {
    case 0: // red
      analogWrite(rgbPins[0], 255);
      break;
    case 1: // green
      analogWrite(rgbPins[1], 255);
      break;
    case 2: // blue
      analogWrite(rgbPins[2], 255);
      break;
  }
}

// Function to blink the current LED and update the RGB LED color
void blinkAndUpdate() {
  // Blink the current single-color LED for 200 ms
  digitalWrite(ledPins[currentLED], HIGH);
  delay(200);
  digitalWrite(ledPins[currentLED], LOW);
  
  // Advance to next LED for the next cycle
  currentLED = (currentLED + 1) % 3;
  
  // Cycle RGB LED color in the order red -> green -> blue
  currentRGB = (currentRGB + 1) % 3;
  setRGBColor(currentRGB);
}

void setup() {
  // Initialize LED pins as OUTPUT
  for (int i = 0; i < 3; i++) {
    pinMode(ledPins[i], OUTPUT);
    digitalWrite(ledPins[i], LOW);
  }
  
  // Initialize RGB LED pins also as OUTPUT
  for (int i = 0; i < 3; i++) {
    pinMode(rgbPins[i], OUTPUT);
  }
  // Set initial RGB LED color to red (as required)
  currentRGB = 0;
  setRGBColor(currentRGB);
  
  // Attach the servo motors
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  
  // Initially set both servos to 0 degrees
  servo1.write(0);
  servo2.write(0);
  delay(500);  // small delay to allow servos to move
}

void loop() {
  const int sweepDelay = 11; // delay per degree step, approx. 11ms => ~2 sec per sweep across 180 degrees

  // At 0° endpoint: blink LED and update RGB
  blinkAndUpdate();
  delay(2000);  // 2 seconds pause at endpoint
  
  // Sweep from 0 to 180 degrees
  for (int angle = 0; angle <= 180; angle++) {
    servo1.write(angle);
    servo2.write(angle);
    delay(sweepDelay);
  }
  
  // At 180° endpoint: blink LED and update RGB
  blinkAndUpdate();
  delay(2000);  // 2 seconds pause at endpoint
  
  // Sweep from 180 back to 0 degrees
  for (int angle = 180; angle >= 0; angle--) {
    servo1.write(angle);
    servo2.write(angle);
    delay(sweepDelay);
  }
  
  // When the servos have reached 0° again, the next iteration of loop
  // will blink the next LED and update the RGB LED
}