// Servo Sweep example for the ESP32
// https://wokwi.com/arduino/projects/323706614646309460

#include <ESP32Servo.h>
#include <stdio.h>
#include <time.h>

struct ServoTweener {
  Servo* servo;
  int start;
  int end;
  int duration;
  int value;
  int elapsed;
};

const int servo1Pin = 19;
const int servo2Pin = 18;

Servo servo1;
Servo servo2;

float easeIn(int elapsed, int start, int end, int duration) {
  float distance = end - start;
  float percent_complete = (float)elapsed / (float)duration;
  return distance * percent_complete + (float)start;
}
float easeOut(int elapsed, int start, int end, int duration) {
  float distance = end - start;
  float percent_complete = (float)elapsed / (float)duration;
  return -distance * percent_complete + (float)end;
}
float easeInOut(ServoTweener &tweener) {
  int distance = tweener.end - tweener.start;
  if (tweener.elapsed < tweener.duration/2) {
    return easeIn(tweener.elapsed*2, 0, distance, tweener.duration) * 0.5 * tweener.start;
  } else {
    return easeOut(tweener.elapsed*2 - tweener.duration, 0, distance, tweener.duration) * 0.5 + distance*0.5 + tweener.start;
  }
}

boolean tween(ServoTweener &tweener) {
  tweener.elapsed++;
  if (tweener.elapsed >= tweener.duration) {
    //completed tween
    return false;
  }
  else {
    float progress = (float)tweener.elapsed / (float)tweener.duration;
    tweener.value = tweener.start + (tweener.end - tweener.start) * progress;
    //tweener.value = easeInOut(tweener);
    Serial.printf("Tween:elapsed=%d value=%d end=%d\n", tweener.elapsed, tweener.value, tweener.end);
    //Servo s = *(tweener.servo);
    //s.write(45);//tweener.value);
    return true;
  }
}

void tween_old(Servo servo, int start_angle, int end_angle, int duration) {
  time_t start_time = time(NULL);
  while (time(NULL) < start_time + duration) {
      float t = (time(NULL) - start_time) / (float)duration;
      int angle = (1 - t) * start_angle + t * end_angle;
      servo.write(angle);
      Serial.print(angle);
      delay(1);
  }
}


void setup() {
  servo1.attach(servo1Pin, 500, 2400);
  servo2.attach(servo2Pin, 500, 2400);
  Serial.begin(115200);
}

int pos = 0;

ServoTweener create_servo_tweener(Servo servo, int start_angle, int end_angle, int duration) {
  ServoTweener servoTweener;
  servoTweener.servo = &servo;
  servoTweener.start = start_angle;
  servoTweener.end = end_angle;
  servoTweener.duration = duration;
  servoTweener.elapsed = 0;
  servo.write(start_angle);
  delay(150);
  return servoTweener; //new
}

void loop() {
   //servo1.write(180);
   //servo2.write(180);
  // delay(150);

  ServoTweener servo_tweener1 = create_servo_tweener(servo2, 180, 0, 100);
  for(int i = 0; i < 100; i++) {
    tween(servo_tweener1);
    servo1.write(servo_tweener1.value);
    delay(50);
  }

  //tween(servo1, 0, 180, 5);
  //tween(servo2, 180 , 0, 5);
  // for (pos = 0; pos <= 180; pos += 1) {
  //   servo1.write(pos);
  //   servo1.write(pos);
  //   delay(15);
  // }
  // for (pos = 180; pos >= 0; pos -= 1) {
  //   servo1.write(pos);
  //   servo2.write(pos);
  //   delay(15);
  // }
}