#include <Servo.h>

// Pin definitions
const int led1Pin = 2;

// RGB LED 1 pins
const int rgb1_r = 3;
const int rgb1_g = 4;
const int rgb1_b = 5;

// RGB LED 2 pins
const int rgb2_r = 6;
const int rgb2_g = 7;
const int rgb2_b = 8;

// Servo pins
const int servo1Pin = 9;
const int servo2Pin = 10;
const int servo3Pin = 11;

// Servo objects
Servo servo1;
Servo servo2;
Servo servo3;

// State variables
bool led1State = false;
int rgbColorIndex = 0; // Starting with red

// Servo positions and directions
int servo1Current = 0;      // Initial position: 0 degrees
int servo1Target = 180;     // Target position for servo1

int servo2Current = 180;    // Initial position: 180 degrees
int servo2Target = 0;       // Target position for servo2

int servo3Current = 90;     // Initial position: 90 degrees
int servo3Target = 135;     // Target position for servo3

// Timing variables
unsigned long startMillis;
const long interval = 2000; // 2 seconds interval

// RGB color definitions (R, G, B)
const int colors[][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
};
const int numColors = 7;

void setup() {
  // Initialize LED
  pinMode(led1Pin, OUTPUT);
  digitalWrite(led1Pin, LOW); // LED off initially
  
  // Initialize RGB LEDs
  pinMode(rgb1_r, OUTPUT);
  pinMode(rgb1_g, OUTPUT);
  pinMode(rgb1_b, OUTPUT);
  pinMode(rgb2_r, OUTPUT);
  pinMode(rgb2_g, OUTPUT);
  pinMode(rgb2_b, OUTPUT);
  
  // Set initial RGB LED colors to red
  setRGBColor(rgb1_r, rgb1_g, rgb1_b, colors[0][0], colors[0][1], colors[0][2]);
  setRGBColor(rgb2_r, rgb2_g, rgb2_b, colors[0][0], colors[0][1], colors[0][2]);
  
  // Initialize servos
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  
  // Set initial servo positions
  servo1.write(servo1Current);
  servo2.write(servo2Current);
  servo3.write(servo3Current);
  
  startMillis = millis();
}

void loop() {
  unsigned long currentMillis = millis();
  unsigned long elapsedMillis = currentMillis - startMillis;
  
  if (elapsedMillis >= interval) {
    // 2 seconds have passed, start a new cycle
    startMillis = currentMillis;
    
    // Toggle LED state
    led1State = !led1State;
    digitalWrite(led1Pin, led1State ? HIGH : LOW);
    
    // Change RGB LED colors
    rgbColorIndex = (rgbColorIndex + 1) % numColors;
    setRGBColor(rgb1_r, rgb1_g, rgb1_b, colors[rgbColorIndex][0], colors[rgbColorIndex][1], colors[rgbColorIndex][2]);
    setRGBColor(rgb2_r, rgb2_g, rgb2_b, colors[rgbColorIndex][0], colors[rgbColorIndex][1], colors[rgbColorIndex][2]);
    
    // Swap current and target positions for servos
    int temp;
    
    temp = servo1Current;
    servo1Current = servo1Target;
    servo1Target = temp;
    
    temp = servo2Current;
    servo2Current = servo2Target;
    servo2Target = temp;
    
    temp = servo3Current;
    servo3Current = servo3Target;
    servo3Target = temp;
  } else {
    // Interpolate servo positions during the interval
    float progress = (float)elapsedMillis / interval;
    
    int interpolatedPos1 = servo1Current + (servo1Target - servo1Current) * progress;
    int interpolatedPos2 = servo2Current + (servo2Target - servo2Current) * progress;
    int interpolatedPos3 = servo3Current + (servo3Target - servo3Current) * progress;
    
    servo1.write(interpolatedPos1);
    servo2.write(interpolatedPos2);
    servo3.write(interpolatedPos3);
  }
}

// Function to set RGB LED color
void setRGBColor(int r_pin, int g_pin, int b_pin, int r_val, int g_val, int b_val) {
  analogWrite(r_pin, r_val);
  analogWrite(g_pin, g_val);
  analogWrite(b_pin, b_val);
}