#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 19
#define PIN_B 18
#define PIN_C 5
#define PIN_D 4

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

typedef struct {
  int priority;
  TickType_t period;
  TickType_t nextWakeTime;
  uint32_t busyTime_ms;

  int pin;
  int counter;
} task_param_t;

task_param_t my_tasks[] = {
  {3, pdMS_TO_TICKS(40),  0, 10, PIN_A, 0},
  {2, pdMS_TO_TICKS(80),  0, 20, PIN_B, 0},
  {1, pdMS_TO_TICKS(120), 0, 30, PIN_C, 0}
};

void taskGeneric(void *pvParameters) {
  task_param_t *self = (task_param_t *)(pvParameters);
  pinMode(self->pin, OUTPUT);
  digitalWrite(self->pin, LOW);
  self->nextWakeTime = 0; //pdMS_TO_TICKS(500);
  vTaskPrioritySet(NULL, tskIDLE_PRIORITY + 10 + self->priority);
  while (1) {
    vTaskDelayUntil(&self->nextWakeTime, pdMS_TO_TICKS(self->period));
    busyTimeToggle_ms(self->busyTime_ms, self->pin);
  }
}



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

  pinMode(LED_BUILTIN, OUTPUT);

  // 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(taskGeneric, "Task A", configMINIMAL_STACK_SIZE + 1024,
                          (void *)&my_tasks[0], tskIDLE_PRIORITY, NULL,
                          APP_CPU_NUM);

  xTaskCreatePinnedToCore(taskGeneric, "Task B", configMINIMAL_STACK_SIZE + 1024,
                          (void *)&my_tasks[1], tskIDLE_PRIORITY, NULL,
                          APP_CPU_NUM);
/*
  xTaskCreatePinnedToCore(taskGeneric, "Task C", configMINIMAL_STACK_SIZE + 1024,
                          (void *)&my_tasks[2], tskIDLE_PRIORITY, NULL,
                          APP_CPU_NUM);
*/

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

}

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



// Busy loop

#define REPEAT_X2(block) do {block; block; } while(0)
#define REPEAT_X4(block) REPEAT_X2(REPEAT_X2(block))

void busyTime(uint32_t n) {
  static volatile uint32_t dummy = 1234;
  for (uint32_t i = 0; i < n; i++) {
    REPEAT_X4(REPEAT_X4(
        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 */
        break;
      }
    }
  }
  busyTime(ms * calib);
}


void busyTimeToggle(uint32_t n, int pin) {
  static volatile uint32_t dummy = 1234;
  for (uint32_t i = 0; i < n; i++) {
    REPEAT_X4(REPEAT_X4(
        dummy = (dummy + 5678) / 27;
    ));
    digitalWrite(pin, i % 2);
  }
  // Leave pin low at the end
  digitalWrite(pin, LOW);
}

void busyTimeToggle_ms(uint32_t ms, int pin) {
  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();
      busyTimeToggle(n, pin);
      time_us += micros();
      if (time_us > 10*1000) { /* > 10ms */
        calib = n * 1000.0f / time_us; /* n = loops / ms */
        break;
      }
    }
  }
  busyTimeToggle(ms * calib, pin);
}