// Define a struct to encapsulate servo information
struct ServoInfo {
  int pin; // Pin number of the servo
  int pos; // Current position of the servo in degrees
};

// Constants for servo control
const int numServos = 2; // Number of servos

// Define servo information for each servo
ServoInfo servo1 = {12, 90}; // Example servo 1
ServoInfo servo2 = {11, 90}; // Example servo 2

const int servoMinPulseWidth = 544; // Minimum pulse width in microseconds
const int servoMaxPulseWidth = 2400; // Maximum pulse width in microseconds

void setup() {
  // Initialize servo pins as outputs
  pinMode(servo1.pin, OUTPUT);
  pinMode(servo2.pin, OUTPUT);
}

void loop() {
  easeServo(servo1, 90, 5000); // Move qdServo to position 90 over 5000 ms
  //delay(1000); // Wait for 1 second between movements
  
  easeServo(servo2, 180, 10000); // Move qdHead to position 180 over 10000 ms
  delay(1000); // Wait for 1 second between movements


  
  easeServo(servo2, 0, 10000); // Move qdHead to position 0 over 10000 ms
  //delay(1000); // Wait for 1 second between movements
  
  easeServo(servo1, 180, 5000); // Move qdHead to position 180 over 5000 ms
  delay(1000); // Wait for 1 second between movements
  
   

}

float cubicEaseInOut(float t) {
  if (t < 0.5) return 4 * t * t * t;
  else return 1 + 4 * pow(t - 1, 3);
}

void easeServo(ServoInfo &servo, int targetPos, int duration) {
  int startPos = servo.pos;
  int delta = targetPos - startPos;
  unsigned long startTime = millis();

  while (millis() - startTime < duration) {
    float t = float(millis() - startTime) / duration;
    float easedT = cubicEaseInOut(t);
    int currentPos = startPos + easedT * delta;

    // Map current position to servo pulse width
    int pulseWidth = map(currentPos, 0, 180, servoMinPulseWidth, servoMaxPulseWidth);

    // Convert pulse width to microseconds for software PWM
    int pulseMicroseconds = map(pulseWidth, servoMinPulseWidth, servoMaxPulseWidth, 544, 2400);

    // Update servo position with software PWM
    digitalWrite(servo.pin, HIGH);
    delayMicroseconds(pulseMicroseconds);
    digitalWrite(servo.pin, LOW);
    delayMicroseconds(20000 - pulseMicroseconds); // 20ms period - pulse width

    delay(0.5); // Adjust delay for smoother operation
  }

  servo.pos = targetPos; // Update servo position after easing completes
}