#include "display.h"

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <math.h>
#define M_2PI (M_PI * 2)

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "driver/i2c_master.h"
#include "soc/clk_tree_defs.h"
#include "esp_task_wdt.h"
#include "esp_timer.h"

i2c_master_bus_handle_t master;
i2c_master_bus_config_t i2c_mst_config = {
  .clk_source = I2C_CLK_SRC_DEFAULT,
  .i2c_port   = -1,
  .scl_io_num = 22,
  .sda_io_num = 21,

  .glitch_ignore_cnt = 7,
  .flags.enable_internal_pullup = true,
};

display_handle_t display;
i2c_master_dev_handle_t display_dev_handle;
i2c_device_config_t display_cfg = {
  .dev_addr_length = I2C_ADDR_BIT_LEN_7,
  .device_address = 0x3C,
  .scl_speed_hz = 400000,
};

TaskHandle_t displayTask;
void displayTaskFunction(void* pvParameters) {
  printf("Initializing display...\n");
  display = display_create(master, &display_cfg, display_dev_handle, 128, 64, true);
  if (display == NULL) {
    printf("Init failed.\n");
    vTaskDelete(NULL);
  }
  display_clear(display);

  int64_t start, end;

  start = esp_timer_get_time();
  for(double ang = 0; ang <= M_2PI; ang += (M_PI / 8)) {
    display_line(display, 64, 32, sin(ang)*32 +64, cos(ang)*32 +32);
  }
  for (uint8_t i = 0; i < 64; i++) display_hline(display, i, i, i);
  end = esp_timer_get_time();
  printf("\nGeneration: %lldµs\n", end - start);
  printf("Theoretical FPS: %lld\n", 1000000 / (end - start));

  start = esp_timer_get_time();
  display_submit(display);
  end = esp_timer_get_time();
  printf("Submit: %lldµs\n", end - start);
  printf("Submits/s: %lld\n\n", 1000000 / (end - start));

  for(;;) taskYIELD();
  // vTaskDelete(NULL);  // Delete this task
}

void app_main() {
  printf("\nInitializing i2c...\n");
  i2c_new_master_bus(&i2c_mst_config, &master);

  printf("Launching displayTask...\n");
  xTaskCreatePinnedToCore(
                    displayTaskFunction,      // Task function
                    "displayTask",            // Task name
                    0x4000,                   // Task stack size
                    NULL,                     // Task parameters
                    configMAX_PRIORITIES - 1, // Task priority
                    &displayTask,             // Task handle to keep track of created task
                    1                         // Pin task to core 1
  );

  vTaskDelete(NULL);  // Delete this task
}