#include <Servo.h>
#include <Wire.h>
#include <Toggle.h>
#include <U8g2lib.h>

// Use Debugging only when necessary, slows down the OLED display
#define MENUDEBUG
//#define OUTPUTDEBUG
//#define BUTTONDEBUG
#define DISABLE_OLED
//#define OLED_DEBUG
//===========================================================
Servo servoA;
Servo servoB;
Servo escB;

#ifndef DISABLE_OLED
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
#endif

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

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

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

#define servoPinA 11
#define servoPinB 12
#define escPinB 7

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

//===========================================================
uint8_t tabA = 1;
uint8_t optionA = 1;
uint8_t tabB = 1;
uint8_t optionB = 1;

//=================VALUES=====================================
uint16_t servoOutA = 0;
uint8_t ledpwmA = 0;
int16_t mpwmA = 0;
uint16_t servoOutB = 0;
uint16_t escOutB = 0;

void setup() {
#ifndef DISABLE_OLED
  u8g2.begin();
  u8g2.setBusClock(400000);
  u8g2.enableUTF8Print();
#endif
  servoA.attach(servoPinA, MinPPM, MaxPPM);
  servoB.attach(servoPinB, MinPPM, MaxPPM);
  escB.attach(escPinB, MinPPM, MaxPPM);

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

void loop() {
  ModeSelect();
  //delayWithMillis(20, ReadInputs);
  ReadInputs();
  //delayWithMillis(20, ServoControl);
  ServoControl();
  delayWithMillis(10, OledUpdate);
  //OledUpdate();


  //delayWithMillis(100, SerialDebug);
  SerialDebug();
}