#include <Servo.h>

// ----- Pin assignments -----
const int ledPin = 11;           // LED (led1) control pin

// RGB LED rgb1 pins
const int rgb1_rPin = 2;
const int rgb1_gPin = 3;
const int rgb1_bPin = 4;

// RGB LED rgb2 pins
const int rgb2_rPin = 5;
const int rgb2_gPin = 6;
const int rgb2_bPin = 7;

// Servo pins are defined below:
const int servo1Pin = 8;
const int servo2Pin = 9;
const int servo3Pin = 10;

// ----- Global Variables -----
Servo servo1;
Servo servo2;
Servo servo3;

int servo1Angle = 0;    // initial for servo1 = 0°
int servo2Angle = 180;  // initial for servo2 = 180°
int servo3Angle = 90;   // initial for servo3 = 90°

bool ledState = false;  // start off

// Define the rainbow colors for the RGB LEDs (red, green, blue, yellow, cyan, magenta, white)
const int numColors = 7;
int colors[numColors][3] = {
  {255, 0, 0},    // red
  {0, 255, 0},    // green
  {0, 0, 255},    // blue
  {255, 255, 0},  // yellow
  {0, 255, 255},  // cyan
  {255, 0, 255},  // magenta
  {255, 255, 255} // white
};
int currentColorIndex = 0;

void setup() {
  // Set the LED (led1) pin mode
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, LOW);  // Make sure led1 is off initially
  
  // Set rgb1 and rgb2 pins as outputs
  pinMode(rgb1_rPin, OUTPUT);
  pinMode(rgb1_gPin, OUTPUT);
  pinMode(rgb1_bPin, OUTPUT);
  pinMode(rgb2_rPin, OUTPUT);
  pinMode(rgb2_gPin, OUTPUT);
  pinMode(rgb2_bPin, OUTPUT);
  
  // Initialize rgb1 and rgb2 to red (first color in the array)
  setRGB(rgb1_rPin, rgb1_gPin, rgb1_bPin, colors[currentColorIndex]);
  setRGB(rgb2_rPin, rgb2_gPin, rgb2_bPin, colors[currentColorIndex]);
  
  // Attach the servo motors
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  
  // Set initial positions for the servo motors
  servo1.write(servo1Angle);
  servo2.write(servo2Angle);
  servo3.write(servo3Angle);
}

void loop() {
  // 1. Toggle LED state (blink every 2 seconds)
  ledState = !ledState;
  digitalWrite(ledPin, ledState ? HIGH : LOW);
  
  // 2. Cycle RGB LEDs through rainbow colors
  // Use the current color from our list and then update the index
  setRGB(rgb1_rPin, rgb1_gPin, rgb1_bPin, colors[currentColorIndex]);
  setRGB(rgb2_rPin, rgb2_gPin, rgb2_bPin, colors[currentColorIndex]);
  currentColorIndex = (currentColorIndex + 1) % numColors;
  
  // 3. Update servo positions in a synchronized manner
  
  // Servo1: if at 0, move to 180; if at 180, move to 0.
  if (servo1Angle == 0)
    servo1Angle = 180;
  else if (servo1Angle == 180)
    servo1Angle = 0;
  servo1.write(servo1Angle);
  
  // Servo2: if at 180, move to 0; if at 0, move to 180.
  if (servo2Angle == 180)
    servo2Angle = 0;
  else if (servo2Angle == 0)
    servo2Angle = 180;
  servo2.write(servo2Angle);
  
  // Servo3: if at 90, move to 135; if at 135, move to 90.
  if (servo3Angle == 90)
    servo3Angle = 135;
  else if (servo3Angle == 135)
    servo3Angle = 90;
  servo3.write(servo3Angle);
  
  // Wait 2 seconds before repeating the cycle
  delay(2000);
}

// Helper function to set the color of an RGB LED
void setRGB(int rPin, int gPin, int bPin, int color[3]) {
  analogWrite(rPin, color[0]);
  analogWrite(gPin, color[1]);
  analogWrite(bPin, color[2]);
}