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

int servoPos = 90; // Current position of the servo in degrees

void setup() {
  pinMode(servoPin, OUTPUT);
}

void loop() {
  easeServo(0, 4000); // Move servo to position 0 over 4000 ms
  delay(1000); // Wait for 1 second between movements
  easeServo(180, 4000); // Move servo back to position 90 over 4000 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(int targetPos, int duration) {
  int startPos = servoPos;
  int delta = targetPos - startPos;
  unsigned long startTime = millis();

  Serial.print("Starting easing from ");
  Serial.print(startPos);
  Serial.print(" to ");
  Serial.println(targetPos);

  while (millis() - startTime < duration) {
    float t = float(millis() - startTime) / duration;
    float easedT = cubicEaseInOut(t); // Cubic easing in and out
    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); // Adjust range as needed

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

    // Output debug information
    Serial.print("t = ");
    Serial.print(t);
    Serial.print(", easedT = ");
    Serial.print(easedT);
    Serial.print(", currentPos = ");
    Serial.print(currentPos);
    Serial.print(", pulseWidth = ");
    Serial.print(pulseWidth);
    Serial.print(", pulseMicroseconds = ");
    Serial.println(pulseMicroseconds);

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

  servoPos = targetPos; // Update servoPos to targetPos after easing completes
}