#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_I2CDevice.h>
#include "models.h"

// #define MODIFY_WITH_SERIAL
#define MICROSECONDS 0.000001
#define MILLISECONDS 0.001

// Tasks
TaskHandle_t calculationTask;
TaskHandle_t renderingTask;
bool renderTaskReady = true;

// Display
Adafruit_SSD1306 display(128, 64, &Wire, -1, 3000000); // Define Display

const int centerX = 64; // SSD1306 display width on all models is always 128
const int centerY = display.height() / 2;

// Model
using namespace crtModel;

float angleZ = 0;
float angleY = 0;
float angleX = PI;
float posX = 0;
float posY = 0;
float posZ = 0;
float scale = 15;

float globalRotatedVertices[4096] = {};

// Final render
bool showSP = false;
int start = 0;
int deltaTime = 1;
double camFOV = 90;

void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);        // Begin I2C Communication With Display

  display.clearDisplay();                           // Clear Display Buffer
  display.setTextSize(1);                           // Test size should be 1px
  display.setTextColor(SSD1306_INVERSE);            // Toggle pixels while drawing istead of setting to one specific color
  display.display();                                // Display On Screen
  
  #ifdef MODIFY_WITH_SERIAL
    Serial.begin(115200);
    Serial.println("\n");
  #endif
  
  xTaskCreatePinnedToCore(
                    RenderingTaskFunction,    // Task function
                    "RenderingTask",          // Task name
                    10000,                    // Task stack size
                    NULL,                     // Task parameters
                    1,                        // Task priority
                    &renderingTask,           // Task handle to keep track of created task
                    0);                       // Pin task to core 0
  
  xTaskCreatePinnedToCore(
                    CalculationTaskFunction,  // Task function
                    "CalculationTaskTask",    // Task name
                    10000,                    // Task stack size
                    NULL,                     // Task parameters
                    1,                        // Task priority
                    &calculationTask,         // Task handle to keep track of created task
                    1);                       // Pin task to core 1
}

void CalculationTaskFunction(void* pvParameters) {
  float rotatedVertices[sizeof(vertices) / sizeof(vertices[0])];
  float sinX, cosX;
  float sinY, cosY;
  float sinZ, cosZ;

  for (;;) { // Infinite loop
    start = millis();
    
    #ifdef MODIFY_WITH_SERIAL
      if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n');
        Serial.println(input);
        String cmd = input.substring(0, input.indexOf(":"));
        String val = input.substring(input.indexOf(" ") + 1);

        if (cmd == "a" || cmd == "angle") {
          val.toUpperCase();
          float arg2 = val.substring(val.indexOf(" ") + 1).toFloat();
          if (val.charAt(0) == 'X') angleX = arg2;
          else if (val.charAt(0) == 'Y') angleY = arg2;
          else if (val.charAt(0) == 'Z') angleZ = arg2;

          Serial.println("angle" + String(val.charAt(0)) + " set to " + String(arg2));
        }
        
        if (cmd == "pos" || cmd == "position") {
          val.toUpperCase();
          float arg2 = val.substring(val.indexOf(" ") + 1).toFloat();
          if (val.charAt(0) == 'X') posX = arg2;
          else if (val.charAt(0) == 'Y') posY = arg2;
          else if (val.charAt(0) == 'Z') posZ = arg2;

          Serial.println("pos" + String(val.charAt(0)) + " set to " + String(arg2));
        }
        
        if (cmd == "sc" || cmd == "sz") {
          scale = val.toFloat();
          Serial.println("scale set to " + String(scale));
        }

        if (cmd == "fov" || cmd == "FOV") {
          camFOV = val.toDouble();
          Serial.println("camFOV set to " + String(camFOV));
        }
        
        if (cmd == "sp" || cmd == "SP") {
          showSP = (val == "true" || val == "True");
          Serial.println("showSP set to " + String(showSP));
        }

        Serial.println();
      }
    #endif

    angleY -= (1.6*MILLISECONDS) * deltaTime; // Rotate 1.6 radians (≈91.5 degrees) per second
    
    sinX = sinf(angleX); cosX = cosf(angleX);
    sinY = sinf(angleY); cosY = cosf(angleY);
    sinZ = sinf(angleZ); cosZ = cosf(angleZ);
    for (int i = 0; i < sizeof(vertices) / sizeof(vertices[0]); i+=3) {
      float x = pgm_read_float(&vertices[i]) * scale;
      float y = pgm_read_float(&vertices[i + 1]) * scale;
      float z = pgm_read_float(&vertices[i + 2]) * scale;

      //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  + posZ;

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

      // Project onto 2D
      float scaleFactor = camFOV / (camFOV + rotatedZ2);
      rotatedVertices[i] = rotatedX2 * scaleFactor;
      rotatedVertices[i + 1] = rotatedY2 * scaleFactor;
    }
    
    while (!renderTaskReady) vTaskDelay(0);

    renderTaskReady = false;
    for (int i = 0; i < sizeof(rotatedVertices) / sizeof(rotatedVertices[0]); i++) // Copy rotatedVertices to globalRotatedVertices
      globalRotatedVertices[i] = rotatedVertices[i];
  }
}

void RenderingTaskFunction(void* pvParameters) {
  int indices[3];

  for (;;) { // Infinite loop
    display.clearDisplay();
    while (renderTaskReady) vTaskDelay(0);

    // Draw the rotated triangles
    for (int i = 0; i < sizeof(tris) / sizeof(tris[0]); i += 3) {
      indices[0] = pgm_read_word(&tris[i]);
      indices[1] = pgm_read_word(&tris[i + 1]);
      indices[2] = pgm_read_word(&tris[i + 2]);
      
      display.drawTriangle(
        globalRotatedVertices[indices[0] * 3] + centerX,
        globalRotatedVertices[indices[0] * 3 + 1] + centerY,
        globalRotatedVertices[indices[1] * 3] + centerX,
        globalRotatedVertices[indices[1] * 3 + 1] + centerY,
        globalRotatedVertices[indices[2] * 3] + centerX,
        globalRotatedVertices[indices[2] * 3 + 1] + centerY,
        SSD1306_WHITE
      );
    }
    
    display.setCursor(0, 0);
    display.println(1000 / deltaTime);
    if (showSP) display.println("0x" + String(heap_caps_get_free_size(0), HEX));
    
    display.display();
    deltaTime = millis() - start;
    renderTaskReady = true;
  }
}

void loop() { Serial.println("D:"); } // This will never run if the program is working properly, so it stays empty