#include <Servo.h>

// Define servo objects
Servo servo1;
Servo servo2;

// Define pin numbers for LEDs and bar graph segments
const int led1Pin = 22;
const int led2Pin = 23;
const int bargraphPins[10] = {30, 31, 32, 33, 34, 35, 36, 37, 38, 39};

// Global delay between steps (in milliseconds)
const int stepDelay = 2000;

void setup() {
  // Initialize Serial Monitor if needed for debugging
  Serial.begin(9600);
  
  // Set pin modes for LED outputs
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
    
  // Set pin modes for each bar graph LED segment.
  for (int i = 0; i < 10; i++) {
    pinMode(bargraphPins[i], OUTPUT);
    // Initially off
    digitalWrite(bargraphPins[i], LOW);
  }
  
  // Attach servos to PWM pins.
  servo1.attach(10); // servo1 control signal on pin 10
  servo2.attach(11); // servo2 control signal on pin 11
  
  // Initialize servos to starting positions.
  servo1.write(0);
  servo2.write(180);
  
  // Initialize LEDs: both off.
  digitalWrite(led1Pin, LOW);
  digitalWrite(led2Pin, LOW);
}

void loop() {
  // Sweep servo1 from 0 to 180 in 10° increments
  // And servo2 from 180 to 0 synchronously.
  for (int angle = 0; angle <= 180; angle += 10) {

    int servo1Angle = angle;
    int servo2Angle = 180 - angle;

    // Write positions to servos
    servo1.write(servo1Angle);
    servo2.write(servo2Angle);

    // Update LED1 status based on servo1 position:
    // ON if servo1 is between 0° and 90° (inclusive), OFF if between 91° and 180°.
    if (servo1Angle <= 90) {
      digitalWrite(led1Pin, HIGH);
    } else {
      digitalWrite(led1Pin, LOW);
    }

    // Update LED2 status based on servo2 position:
    // ON if servo2 is between 90° and 180° (inclusive), OFF if between 0° and 89°.
    if (servo2Angle >= 90) {
      digitalWrite(led2Pin, HIGH);
    } else {
      digitalWrite(led2Pin, LOW);
    }

    // Update bar graph based on servo1 position.
    // Calculate how many segments to light up. (10 segments corresponds to 180°, so each segment ~18°)
    int segmentsToLight = servo1Angle / 18; // integer division; e.g., 0->0, 90->5, 180->10

    // Light up first "segmentsToLight" segments and turn off the rest.
    for (int i = 0; i < 10; i++) {
      if (i < segmentsToLight) {
        digitalWrite(bargraphPins[i], HIGH);
      } else {
        digitalWrite(bargraphPins[i], LOW);
      }
    }
    
    // Debug print (optional)
    Serial.print("Servo1: ");
    Serial.print(servo1Angle);
    Serial.print(" | Servo2: ");
    Serial.print(servo2Angle);
    Serial.print(" | LED1: ");
    Serial.print((servo1Angle <= 90) ? "ON" : "OFF");
    Serial.print(" | LED2: ");
    Serial.println((servo2Angle >= 90) ? "ON" : "OFF");
    
    delay(stepDelay);
  }
}