#include <Servo.h>
#include <Wire.h>
#include <Toggle.h>
#include <U8g2lib.h>
#include <AceRoutine.h>
using namespace ace_routine;

Servo servoA;
Servo servoB;
Servo esc;

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

#define AnalogRes 1023 // AnalogRead resolution (0-1023 for UNO, NANO)
#define MinPPM 1000 // 1000-2000us is default
#define MaxPPM 2000

#define MENUDEBUG
//=================PINS=====================================
#define PotPin1 A0
#define PotPin2 A1

#define shunt1 A2
#define shunt2 A3

#define servoPinA 12
#define servoPinB 11
#define escPin 11

#define btnA1 3
#define btnA2 2
#define btnB1 10
#define btnB2 9

//===========================================================

Toggle modeA(btnA1);
Toggle selectA(btnA2);
Toggle modeB(btnB1);
Toggle SelectB(btnB2);
uint8_t tabA = 1;
uint8_t optionA = 1;
uint8_t tabB = 1;
uint8_t optionB = 1;

//=================VALUES=====================================
uint16_t potA = 0;
uint16_t potB = 0;

void setup() {
  u8g2.begin();
  servoA.attach(servoPinA, MinPPM, MaxPPM);
  servoB.attach(servoPinB, MinPPM, MaxPPM);
  esc.attach(escPin, MinPPM, MaxPPM);

#ifdef MENUDEBUG
  Serial.begin(115200);
  Serial.println("RC Tester");
#endif

  CoroutineScheduler::setup();
}

void loop() {

  CoroutineScheduler::loop();
  potA = map(potA, 0, 1023, MinPPM, MaxPPM);
  potB = map(potB, 0, 1023, MinPPM, MaxPPM);
  servoA.writeMicroseconds(potA);
  servoB.writeMicroseconds(potB);
  SerialDebug();
}

COROUTINE(InputScheduler) {
  COROUTINE_LOOP() {
    ReadInputs();
    ModeSelect();
    COROUTINE_DELAY(1);
  }
}