#include <Servo.h>

// Declare servo objects
Servo myServo1;
Servo myServo2;

// Speed for servo movement (increment per cycle)
float speed = 2;  // Adjust for slower or faster movement

// Pin assignments
int S0 = 2;
int potPin = A0;  // Pin for potentiometer 1
int servo1Pin = 3; // Servo 1 on pin 13
int servo2Pin = 4; // Servo 2 on pin 14
int statusP1=7;   //servo movement status LED

// Potentiometer and servo angle variables
int potVal1, potVal2;
float servoAngle1 = 90; // Initial angle for Servo 1
float servoAngle2 = 90; // Initial angle for Servo 2

void setup() {
  Serial.begin(9600);

  // Attach the servos to their respective pins
  myServo1.attach(servo1Pin);  
  myServo2.attach(servo2Pin);   

  pinMode(S0, OUTPUT); // Set S0 pin as OUTPUT
  pinMode(statusP1, OUTPUT);

}

void loop() {

    // Read potentiometer 1 value (for Servo 1)
    digitalWrite(S0, LOW); 
    delay(10); 
    potVal1 = analogRead(potPin);
    
    // Read potentiometer 2 value (for Servo 2)
    digitalWrite(S0, HIGH); 
    delay(10);
    potVal2 = analogRead(potPin);

    // Map potentiometer values to servo angles (0 to 180 degrees)
    float targetAngle1 = constrain(map(potVal1, 0, 1023, 0, 180),75,105);
    float targetAngle2 = constrain(map(potVal2, 0, 1023, 0, 180),75,105);

    //using TERNARY operator
    digitalWrite(statusP1, abs(targetAngle1 - servoAngle1) || abs(targetAngle2 - servoAngle2) > 2);


    // Move Servo 1 smoothly
    if (servoAngle1 < targetAngle1) {
      servoAngle1 += speed;  // Increment angle for smooth movement
      if (servoAngle1 > targetAngle1){
        servoAngle1 = targetAngle1;  // Avoid overshooting
      }
    } else if (servoAngle1 > targetAngle1) {
      servoAngle1 -= speed;  // Decrement angle for smooth movement
        if (servoAngle1 < targetAngle1){
          servoAngle1 = targetAngle1;  // Avoid undershooting
        }      
    }

/*  using IF statement
  if (targetAngle1 - servoAngle1 > 2 || targetAngle1 - servoAngle1 < -2) {
      digitalWrite(statusP1, HIGH);
    }else{
      digitalWrite(statusP1, LOW);
  }
*/

    // Move Servo 2 smoothly
    if (servoAngle2 < targetAngle2) {
      servoAngle2 += speed;  // Increment angle for smooth movement
      if (servoAngle2 > targetAngle2){
        servoAngle2 = targetAngle2;  // Avoid overshooting
      } 
    } else if (servoAngle2 > targetAngle2) {
      servoAngle2 -= speed;  // Decrement angle for smooth movement
        if (servoAngle2 < targetAngle2){
          servoAngle2 = targetAngle2;  // Avoid undershooting
        } 
    }   

    // Update the servo positions
    myServo1.write(servoAngle1);
    myServo2.write(servoAngle2);

    // Print potentiometer values and servo angles for debugging
    Serial.print("Pot1: ");
    Serial.print(potVal1);
    Serial.print("\tServo1: ");
    Serial.print(servoAngle1);
    Serial.print("\tPot2: ");
    Serial.print(potVal2);
    Serial.print("\tServo2: ");
    Serial.println(servoAngle2);

}