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

Servo servoA;
Servo servoB;
Servo escB;

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

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

// Use Debugging only when necessary, slows down the OLED display
//#define MENUDEBUG
//#define OUTPUTDEBUG
#define BUTTONDEBUG
//=================PINS=====================================
#define PotPin1 A1
#define PotPin2 A0

#define vsensePin A4
#define shunt1 A2
#define shunt2 A3

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

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

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

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 voltage = 0;
uint16_t potA = 0;
uint16_t potB = 0;

uint16_t ppmA = 0;
uint16_t ppmB = 0;

uint16_t servoOutA = 0;
uint16_t ledpwmA = 0;
int16_t mpwmA = 0;
uint16_t servoOutB = 0;
uint16_t escOutB = 0;

void setup() {
  CoroutineScheduler::setup();

  u8g2.begin();
  u8g2.setBusClock(100000);
  u8g2.enableUTF8Print();

  servoA.attach(servoPinA, MinPPM, MaxPPM);
  servoB.attach(servoPinB, MinPPM, MaxPPM);
  escB.attach(escOutB, MinPPM, MaxPPM);

#ifdef MENUDEBUG || OUTPUTDEBUG || BUTTONDEBUG
  Serial.begin(9600);
  Serial.println("RC Tester");
#endif
}

void loop() {
  CoroutineScheduler::loop();
  //ServoControl();
  //OledUpdate();

  SerialDebug();
}

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