//https://forum.arduino.cc/t/stop-blocking-on-button-press/1099294/13

#include <ezButton.h>
#include <Servo.h>

#define buttonLeftPin   2
#define buttonRightPin  3
#define servoLeftPin    11
#define servoRightPin   10

ezButton buttonLeft(buttonLeftPin);
ezButton buttonRight(buttonRightPin);  // create ezButton object that attach to pin 7;

Servo servoLeft;
Servo servoRight;

int dt = 50;
int pos = 50;

uint8_t numberOfData = 2;

uint8_t leftIndex = 0;
uint8_t rightIndex = 0;

int leftDegreeTables[] = {0, 180};
int rightDegreeTables[] = {180, 0};

int leftDegree = 0;
int rightDegree = 0;

void setup() {
  Serial.begin(115200);
  servoLeft.attach(servoLeftPin);
  servoRight.attach(servoRightPin);

  leftDegree = leftDegreeTables[leftIndex];
  rightDegree = rightDegreeTables[leftIndex];
  servoLeft.write(leftDegree);
  servoRight.write(rightDegree);
}



void loop() {
  // Delta Time for each scan
  static unsigned long prevTime = micros();
  unsigned long currTime = micros();
  unsigned long delatTime = currTime - prevTime ;
  prevTime = currTime;

  buttonLeft.loop();
  buttonRight.loop(); // MUST call the loop() function first


  if (buttonLeft.isPressed()) {
    leftIndex = (leftIndex + 1) % numberOfData;
  }

  if (buttonRight.isPressed()) {
    rightIndex = (rightIndex + 1 ) % numberOfData;
  }

  leftDegree = RampRate(leftDegree, leftDegreeTables[leftIndex]);
  rightDegree = RampRate(rightDegree, rightDegreeTables[rightIndex]);

  servoLeft.write(leftDegree);
  servoRight.write(rightDegree);

}

int  RampRate(int degree, int taget) {
  if (degree < taget) {
    degree++;
    if (degree > taget ) {
      degree = taget ;
    }
  } else if (degree > taget) {
    degree--;
    if (degree < taget ) {
      degree = taget ;
    }
  }
  return degree;
}