#include "pins.h"
#include <Toggle.h>
#include <U8g2lib.h>
#include <pwmWrite.h>

uint16_t servoX = 0;
uint16_t servoY = 0;
uint16_t servoZ = 0;

uint16_t vin = 1210;
uint16_t vbat = 740;

uint16_t vsens1 = 36;
uint16_t vsens2 = 72;
uint16_t vsens3 = 108;
uint16_t vsens4 = 144;

uint16_t current = 1050;

uint8_t mode = 1; // 1=Servo, 2=ESC, 3=PWM
uint8_t modeA = 1;
uint8_t modeB = 1;
uint8_t modeC = 1;
//==============================================================================================
//#define CORE_DEBUG
//#define OLED_DEBUG
//#define MENU_DEBUG
//#define SERVO_DEBUG

//==============================================================================================
Pwm pwm = Pwm();
TaskHandle_t Task1;
TaskHandle_t Task2;

void setup() {

#ifdef CORE_DEBUG || SERVO_DEBUG || OLED_DEBUG
  Serial.begin(115200);
  Serial.println("AIO RC Tester");
#endif

  xTaskCreatePinnedToCore(
    Task2code, /* Task function. */
    "Task2",   /* name of task. */
    10000,     /* Stack size of task */
    NULL,      /* parameter of the task */
    1,         /* priority of the task */
    &Task2,    /* Task handle to keep track of created task */
    1);        /* pin task to core 1 */
  delay(5);
  xTaskCreatePinnedToCore( Task1code, "Task1", 10000, NULL, 1, &Task1, 0);
  delay(5);
}
//==============================================================================================
//    OLED Display is running on Core 1
void Task2code(void* pvParameters) {
#if defined CORE_DEBUG
  Serial.print("OLED Display running on core ");
  Serial.println(xPortGetCoreID());
#endif

  OledSetup();
  for (;;) {
    OledUpdate();
    delay(1);
  }
}
//==============================================================================================

void Task1code(void* pvParameters) {
#if defined CORE_DEBUG
  Serial.print("Control Tasks running on core ");
  Serial.println(xPortGetCoreID());
#endif

  InitialSetup();
  for (;;) {
    ModeSelect();
    repeatFunction(10,ServoControl);
    //ServoControl();

    repeatFunction(200, ReadAnalog);
  }
}
//==============================================================================================
void loop() {
}