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

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

const int centerX = display.width() / 2;
const int centerY = display.height() / 2;

const float deg2rad = 0.01745329251;

/*
namespace cubeModel {
  float vertices[] = {
    // Bottom square
    -25, -25, 25,
    25, -25, 25,
    -25, -25, -25,
    25, -25, -25,
    // Top square
    -25, 25, 25,
    25, 25, 25,
    -25, 25, -25,
    25, 25, -25
  };

  uint16_t tris[36] = {
    0, 1, 2, // Roof1
    3, 1, 2, // Roof2

    4, 5, 6, // Floor1
    7, 5, 6, // Floor2

    2, 3, 6, // Front1
    7, 3, 6, // Front2

    0, 1, 4, // Back1
    5, 1, 4, // Back2

    3, 1, 7, // Right1
    5, 1, 7, // Right2

    0, 2, 4, // Left1
    6, 2, 4  // Left2
  };
};*/

using namespace crtModel;

float angleZ = 0;
float angleY = -150;//-21.5;
float angleX = 180;
float xPos = centerX;
float yPos = centerY;
float scale = 15;

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

  display.clearDisplay();                           // Clear Display Buffer
  display.setTextSize(1);
  display.setTextColor(SSD1306_INVERSE);
  display.setCursor(0, 0);
  display.display();                                // Display On Screen
}

int start = 0;
int end = 0;
int deltaTime = 0;
double camFOV = 60;
double idx = 0; // Just kinda stops at 2097152.00 lol

void loop() {
  start = millis();

  //idx += 0.002 * deltaTime;
  //scale = (sin(idx) / 8) + 0.65;

  if (!digitalRead(10)) angleY -= 0.12 * deltaTime;
  //if (!digitalRead(10)) camFOV += 0.01 * deltaTime;
  //if (!digitalRead(8)) camFOV -= 0.01 * deltaTime;
  if (!digitalRead(8)) angleY += 0.12 * deltaTime;
  angleY = (angleY % 360) - 180;

  //Find length of the vertex array
  float rotatedVertices[sizeof(vertices) / sizeof(vertices[0])];

  //Calculate sin and cos for each angle
  float cosX = cosf(angleX * deg2rad);
  float sinX = sinf(angleX * deg2rad);
  float cosY = cosf(angleY * deg2rad);
  float sinY = sinf(angleY * deg2rad);
  float cosZ = cosf(angleZ * deg2rad);
  float sinZ = sinf(angleZ * deg2rad);

  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;

    // 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);
    rotatedVertices[i] = max(-127, min(rotatedX2 * scaleFactor, 127));
    rotatedVertices[i + 1] = max(-127, min(rotatedY2 * scaleFactor, 127));
  }

  display.clearDisplay(); //Clear display buffer

  //Draw the rotated and projected triangles
  for (int i = 0; i < sizeof(tris) / sizeof(tris[0]); i += 3) {
    int indices[3] = {
      pgm_read_word(&tris[i]),
      pgm_read_word(&tris[i + 1]),
      pgm_read_word(&tris[i + 2])
    };

    display.drawTriangle(
      rotatedVertices[indices[0] * 3] + xPos,
      rotatedVertices[indices[0] * 3 + 1] + yPos,
      rotatedVertices[indices[1] * 3] + xPos,
      rotatedVertices[indices[1] * 3 + 1] + yPos,
      rotatedVertices[indices[2] * 3] + xPos,
      rotatedVertices[indices[2] * 3 + 1] + yPos,
      SSD1306_WHITE
    );
  }
  
  /*for (int i = 0; i < sizeof(rotatedVertices) / sizeof(rotatedVertices[0]); i+=3) {
    display.setCursor(rotatedVertices[i] + xPos - 2, rotatedVertices[i + 1] + yPos - 3);
    display.print(i / 3);
  }*/

  
  display.setCursor(0, 0);

  display.println(angleY);
  display.print("0x");
  display.println(SP, HEX);
  display.print(1000 / deltaTime);
  
  display.display();

  end = millis();
  deltaTime = end - start;
}