#include <Arduino.h>
#include <Servo.h>
#include "EButton.h"

Servo ESC_R;     // create servo object to control the ESC
Servo ESC_L;
EButton button(4);  // Button on pin 4.
int speed = 1500;
bool init_done = false;


void singleClick(EButton &btn){
  speed = speed + 100;
  if (speed > 2000){
    speed = 1100;
  }
}

void doubleClick(EButton &btn){
  if (!init_done) { // Initialization ARMING
    ESC_L.writeMicroseconds(2000);
    ESC_R.writeMicroseconds(2000);
    digitalWrite(LED_BUILTIN, HIGH);
    delay(2000);
    ESC_L.writeMicroseconds(1000);
    ESC_R.writeMicroseconds(1000);
    delay(2000);
    init_done = true;  
  }
}

void longPress(EButton &btn){
  if (init_done) {
    ESC_R.writeMicroseconds(speed);    // Send the signal to the ESC (0-180 min.max)
    ESC_L.writeMicroseconds(speed);
    digitalWrite(LED_BUILTIN, HIGH);
    }
}

void longPressEnd(EButton &btn){
  ESC_R.write(0);
  ESC_L.write(0);
  digitalWrite(LED_BUILTIN, LOW);
}

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  ESC_L.attach(3, 1000, 2000); // (pin, min pulse width, max pulse width in microseconds)
  ESC_R.attach(5, 1000, 2000); // (pin, min pulse width, max pulse width in microseconds)
  button.attachSingleClick(singleClick);
  button.attachDoubleClick(doubleClick);
  button.attachDuringLongPress(longPress);
  button.attachLongPressEnd(longPressEnd);
}

void loop() {
	button.tick();
}