#include <ESP32Servo.h>
// #include <algorithm>
#define SERVO_PIN_1 12
#define SERVO_PIN_2 27
#define POT_PIN   14

/*
KNOWN ISSUES:
- inc in Oscillator::oscillate() cannot be below 3, or else the servo is unresponsive
- current servo should operate at higher voltages, i am using 3.3
- potentially add feedback by soldering to internal potentiometer
- this only works with one servo for now, will make a container "Flight Controller" class that handles inputs and serves instructions to two oscillators
*/

class Oscillator {
  public:
    int min;
    int max;
    bool dir; // the direction that the servo is oscillating in. true means increasing angle, false means decreasing angle;
    Servo servo;

    Oscillator(int servo_pin, int minimum_amplitude, int maximum_amplitude) {
      // these amplitudes need not be met, but they are enforced
      pinMode(servo_pin, OUTPUT);
      min = minimum_amplitude;
      max = maximum_amplitude;
      servo.attach(servo_pin);
    };
    void zeroServo() {
      servo.write(min);
      delay(1000);
    };

    void oscillate_dynamic(int period, int delayTime) {
      // does not do the delaying here
      int targetPosition = 0; // defaults, ik that this will bite me in the future
      if (dir) {
        targetPosition = min;
      }
      else if (!dir) {
        targetPosition = max;
      }
      // Serial.println("Min: " + String(min));
      // Serial.println("Max: " + String(max));
      // Serial.println("Period: " + String(period));
      // Serial.println("Delay: " + String(delayTime));

      int inc = int(round(  ((max-min)*delayTime*1/period)  )); // ∆theta = (theta)*(1/period)*(∆time), inc must be int

      // GH-S37D - Speed: 0.13sec/60°(at 4.8V) 0.10sec/60°(at 6V) 

      Serial.println("Incriment: " + String(inc));
      // boundaries to prevent freezing in operation. for some reason, it breaks if inc < 3
      if(inc < 3) {  
        inc = 3;
      } 
      else if (inc > 200) {
        inc = 200;
      }

      
      int buff = round((inc/2) + 1); // so that inc does not overshoot/undershoot
      int currentPosition = servo.read();

      int _max = targetPosition+buff;
      int _min = targetPosition-buff;

      // Serial.println("Buffer: " + String(buff));
      // Serial.println("Incriment: " + String(inc));

      if( currentPosition >= _min && currentPosition <= _max ) { // if position within acceptable range. 
        dir = !dir; 
      }
      else if (currentPosition > _max) {
        servo.write(int(currentPosition-inc));
      }
      else if (currentPosition < _min){
        servo.write(int(currentPosition+inc));
      }
    };
    void oscillate_basic(int period, int delayTime) {
      int inc = int(round(  ((max-min)*delayTime*1/period)  )); // ∆theta = (theta)*(1/period)*(∆time), inc must be int
      float velo = inc/delayTime;
      Serial.println(inc);
      
      for(int i=min; i<max; i= i+inc) {
        servo.write(i);
        delay(delayTime);
      }
      for(int i=max; i<min; i= i-inc) {
        servo.write(i);
        delay(delayTime);
      }
    }
    
};

Oscillator osc1(SERVO_PIN_1,0,180); // (pin, min, max)
Oscillator osc2(SERVO_PIN_2, 0, 180);
int delayTime = 10; // the time between each step, set globally so that one delay affects all servos

void setup() {
  Serial.begin(9600);
  pinMode(POT_PIN,INPUT);
  osc1.zeroServo(); 
  osc2.zeroServo(); 
}

void loop() {
  int reading = analogRead(POT_PIN);
  int period1 = map(reading,0,4095,200,800);
  int period2 = map(reading,0,4095,800,200);
  osc1.oscillate_dynamic(period1,delayTime);
  osc2.oscillate_dynamic(period2,delayTime);
  // Serial.println(String(period));
  // osc.oscillate_basic(period, delayTime); // input is period in ms, range(0,1000)
  // delay(delayTime); // delay is done outside of oscillate() ao multiple servos can shart the same delay
}