#include <ESP32Servo.h>

Servo servo;
int servo_pin;

int curve[] = {0,60,120,180,120,60,0}; // make default curve sinusoidal
int curve_times[] = {1000,1000,1000,1000,1000,1000,1000}; 
int index_in_curve=0;

int position;
int inc;
int delayTime;
unsigned long prev_time = 0;
int curve_length; // should be set whenever the curve is changed
int curve_times_length;

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

  // handle pins
  servo_pin = 12;
  servo.attach(servo_pin);
  zeroServo();

  // set initial curve
  // setCurve(curve,curve_times);
  curve_length = sizeof(curve);
  curve_times_length = sizeof(curve_times);

}

void loop() {

  // incriment servo, (distance/time)*time step
  if (curve_times[index_in_curve] != 0) {
    inc = int(round( ((curve[index_in_curve+1]-curve[index_in_curve])/curve_times[index_in_curve])*delayTime ));
  }
  else {
    inc = 100;
  }

  incriment_servo();
  delay(delayTime); // this speeds up the simulation
}

void incriment_servo() {
  // int targetPosition = curve[index_in_curve];

  if( int(round(millis()-prev_time)) > curve_times[index_in_curve] ) {
    Serial.println("Here");

    if(index_in_curve==curve_length-1) {
      index_in_curve=0;
      prev_time = millis();
      return;
    }
    else {
      position = position+inc; // inc can be negative
      servo.write(position);
      prev_time = millis();
      index_in_curve++;
    }
  }
  else {
    prev_time = millis();
    return;
  }
}

void zeroServo() {
  index_in_curve = 0;
  position = curve[index_in_curve];
  servo.write(position);
}

// void setCurve(int _curve[6], int _curve_times) {
//   curve = _curve;
//   curve_times = _curve_times;

//   curve_length = sizeof(curve);
//   curve_times_length = sizeof(curve_times);
// }