#include "display.h"
#include "paperplane.obj.h"

#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <math.h>

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

#include "driver/i2c_master.h"
#include "soc/clk_tree_defs.h"
#include "esp_system.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,
};

int64_t last = 0;
float deltaTime = 1.0f;

bool handshake = false;
display_handle_t display = NULL;
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,
};

void submit_async() {
  handshake = true;
  while (handshake) vTaskDelay(1);

  deltaTime = (float)(esp_timer_get_time() - last) / 1000000;
  last = esp_timer_get_time();
}

TaskHandle_t displayManager;
void displayManagerFunction(void* pvParameters) {
  display = display_create(master, &display_cfg, display_dev_handle, 128, 64, true, DISPLAY_DOUBLE_BUFFER);
  last = esp_timer_get_time();
  if (display == NULL) {
    printf("Display initialization failed.\n");
    vTaskDelete(NULL);
  }

  for (;;) {
    while (!handshake) vTaskDelay(1);
    handshake = false;
    display_submit(display);
  }

  display_destroy(master, display);
  vTaskDelete(NULL);
}

typedef struct {
  float x, y;
} Vec2_t;

TaskHandle_t renderer;
void rendererFunction(void* pvParameters) {
  // Wait for display to be initialized
  while (display == NULL) vTaskDelay(1);

  uint16_t vertexCount = 6;
  uint16_t indexCount = 9;
  Vec2_t* projectedVertices = (Vec2_t*)malloc(sizeof(Vec2_t) * vertexCount);
  float camFOV = 90;

  float angleX = M_PI * 0.5;
  float angleY = 0.0f;
  float angleZ = 0.0f;

  float sinX = sinf(angleX), cosX = cosf(angleX);
  float sinY = sinf(angleY), cosY = cosf(angleY);
  float sinZ = sinf(angleZ), cosZ = cosf(angleZ);

  int64_t start;

  for (;;) {
    display_swap_buffer(display);
    display_clear(display);

    angleY -= (M_PI * 0.5) * deltaTime;
    sinY = sinf(angleY);
    cosY = cosf(angleY);
    for (uint16_t i = 0; i < vertexCount; i++) {
      float x = paperplane_v[i].x * 48.0f;
      float y = paperplane_v[i].y * 48.0f;
      float z = paperplane_v[i].z * 48.0f;

      // Rotate around the X-axis
      float rotatedY = y * cosX - z * sinX;
      float rotatedZ = y * sinX + z * cosX;

      // Rotate around the Y-axis
      float rotatedX = x * cosY + rotatedZ * sinY;
      float rotatedZ2 = -x * sinY + rotatedZ * cosY;

      // Rotate around the Z-axis
      float rotatedX2 = rotatedX * cosZ - rotatedY * sinZ;
      float rotatedY2 = rotatedX * sinZ + rotatedY * cosZ;

      // Project onto 2D
      float scaleFactor = camFOV / (camFOV + rotatedZ2);
      projectedVertices[i].x = rotatedX2 * scaleFactor;
      projectedVertices[i].y = rotatedY2 * scaleFactor;
    }

    start = esp_timer_get_time();

    for (uint16_t i = 0; i < indexCount; i++) {
      display_line(
        display,
        projectedVertices[paperplane_i[i].x].x + 64,
        projectedVertices[paperplane_i[i].x].y + 24,

        projectedVertices[paperplane_i[i].y].x + 64,
        projectedVertices[paperplane_i[i].y].y + 24
      );
    }

    // printf(/*"\x1B[1A"*/"%lld us\n", esp_timer_get_time() - start);

    submit_async();
  }

  free(projectedVertices);
  vTaskDelete(NULL);
}

void app_main() {
  printf("\napp_main()\n");
  if (esp_reset_reason() == ESP_RST_PANIC) {
    printf("ESP32 reset due to ESP_RST_PANIC, exit.\n");
    vTaskDelete(NULL);
  }

  i2c_new_master_bus(&i2c_mst_config, &master);

  xTaskCreatePinnedToCore(
                    displayManagerFunction,   // Task function
                    "displayManager",         // Task name
                    0x1000,                   // Task stack size
                    NULL,                     // Task parameters
                    configMAX_PRIORITIES - 1, // Task priority
                    &displayManager,          // Task handle to keep track of created task
                    1                         // Pin task to core 1
  );
  // Can asas.c fuck off please it's not part of my code and it's pissing me off.
  xTaskCreatePinnedToCore(
                    rendererFunction,         // Task function
                    "renderer",               // Task name
                    0x4000,                   // Task stack size
                    NULL,                     // Task parameters
                    configMAX_PRIORITIES - 1, // Task priority
                    &renderer,                // Task handle to keep track of created task
                    0                         // Pin task to core 0
  );

  vTaskDelete(NULL);  // Delete this task
}