#include <Arduino.h>

#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>

#include <ESP32Servo.h>

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


#define PIN_SERVO 23
#define PIN_SDA 21
#define PIN_SCL 22

#define PIN_A 21
#define PIN_B 19
#define PIN_C 18
#define PIN_D 5

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


void busyTime(uint32_t n) {
  static volatile uint32_t dummy = 1234;
  for (uint32_t i = 0; i < n; i++) {
    dummy = (dummy + 5678) / 27;
  }
}

void busyTime_ms(uint32_t ms) {
  static float calib = 0.0;
  if (calib == 0.0) {
    // First time: calibrate the busy loop
    for (int n = 8; n < 0x04000; n *= 2) {
      int32_t time_us = -micros();
      busyTime(n);
      time_us += micros();
      if (time_us > 10*1000) { /* > 10ms */
        calib = n * 1000.0f / time_us; /* n = loops / ms */
        Serial.printf("%u loops in %d µs\n", n, time_us);
        Serial.printf("Calibration factor: %3.3f loops per ms\n", calib);
        break;
      }
    }
  }
  busyTime(ms * calib);
}


void taskA(void *pvParameters) {
  int counter = 0;
  TickType_t xNextWakeTime = xTaskGetTickCount();
  while (1) {
    digitalWrite(PIN_A, HIGH);
    busyTime_ms(10);
    digitalWrite(PIN_A, LOW);
    vTaskDelayUntil(&xNextWakeTime, pdMS_TO_TICKS(40));
  }
}

void taskB(void *pvParameters) {
  TickType_t xNextWakeTime = xTaskGetTickCount();
  while (1) {
    digitalWrite(PIN_B, HIGH);
    busyTime_ms(20);
    digitalWrite(PIN_B, LOW);
    vTaskDelayUntil(&xNextWakeTime, pdMS_TO_TICKS(80));
  }
}

void taskC(void *pvParameters) {
  TickType_t xNextWakeTime = xTaskGetTickCount();
  while (1) {
    digitalWrite(PIN_C, HIGH);
    busyTime_ms(30);
    digitalWrite(PIN_C, LOW);
    vTaskDelayUntil(&xNextWakeTime, pdMS_TO_TICKS(120));
  }
}



void setup() {
  Serial.begin(115200);
  Serial.printf("\n\n\n");
  Serial.printf("Application " __FILE__ " compiled " __DATE__ " at " __TIME__ " \n");

  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(PIN_A, OUTPUT);
  pinMode(PIN_B, OUTPUT);
  pinMode(PIN_C, OUTPUT);

  digitalWrite(PIN_A, LOW);
  digitalWrite(PIN_B, LOW);
  digitalWrite(PIN_C, LOW);

  // Calibrate the busy loop
  busyTime_ms(0);

  u8g2.begin();
  u8g2.clearBuffer();					// clear the internal memory
  u8g2.setFont(u8g2_font_ncenB08_tr);	// choose a suitable font
  u8g2.drawStr(15, 10, "LONELY BINARY");	// write something to the internal memory
  u8g2.sendBuffer();					// transfer internal memory to the display


  xTaskCreatePinnedToCore(taskA, "Task A", configMINIMAL_STACK_SIZE + 1024,
                          NULL, tskIDLE_PRIORITY + 3, NULL,
                          APP_CPU_NUM);

  xTaskCreatePinnedToCore(taskB, "Task B", configMINIMAL_STACK_SIZE + 1024,
                          NULL, tskIDLE_PRIORITY + 2, NULL,
                          APP_CPU_NUM);

  xTaskCreatePinnedToCore(taskC, "Task C", configMINIMAL_STACK_SIZE + 1024,
                          NULL, tskIDLE_PRIORITY + 1, NULL,
                          APP_CPU_NUM);

  Serial.printf("Ready to start!\n");

}

void loop() {
  digitalWrite(LED_BUILTIN, HIGH);
  delay(100);
  digitalWrite(LED_BUILTIN, LOW);
  delay(400);
}