#include <CodeCell.h>
#include <Adafruit_SSD1306.h>

#include "3D_Object.hpp"

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define SCREEN_I2C_ADDR 0x3C

#define FOCAL 100

CodeCell codeCell;
Adafruit_SSD1306 screen(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

_3D::Object cube {{
  {{20, 20, 20}, {20, 20, -20}, {20, -20, -20}},
  {{20, 20, 20}, {20, -20, 20}, {20, -20, -20}},
  {{-20, 20, 20}, {-20, 20, -20}, {-20, -20, -20}},
  {{-20, 20, 20}, {-20, -20, 20}, {-20, -20, -20}},
  {{20, 20, 20}, {-20, 20, 20}, {-20, -20, 20}},
  {{20, 20, 20}, {-20, -20, 20}, {20, -20, 20}},
  {{20, 20, -20}, {-20, 20, -20}, {-20, -20, -20}},
  {{20, 20, -20}, {20, -20, -20}, {-20, -20, -20}},
}};

const size_t NB_TRIANGLES {cube.triangles.size()};

void setup()
{
  // Init CodeCell
  codeCell.Init(LIGHT);

  // Init screen
  screen.begin(SSD1306_SWITCHCAPVCC, SCREEN_I2C_ADDR);

  // Set text parameters
  screen.setTextSize(1);
  screen.setTextColor(WHITE);
}

void loop()
{
  if(codeCell.Run())
  {
    // Get tima at frame start
    unsigned long frame_start = millis();

    // Clear screen
    screen.clearDisplay();

    array<float, 3> calculatedXPoints;
    array<float, 3> calculatedYPoints;

    // Rendering triangles
    for(const _3D::Triangle &triangle : cube.triangles)
    {
      for(uint8_t pointIndex {0}; pointIndex < 3; pointIndex++)
      {
        const _3D::Vector3D<float> &point = triangle.points.at(pointIndex);

        calculatedXPoints.at(pointIndex) = (point.x * FOCAL) / (point.z + FOCAL) + SCREEN_WIDTH / 2;
        calculatedYPoints.at(pointIndex) = (point.y * FOCAL) / (point.z + FOCAL) + SCREEN_HEIGHT / 2;

        screen.fillCircle(calculatedXPoints.at(pointIndex), calculatedYPoints.at(pointIndex), 3, SSD1306_WHITE);
      }

      screen.drawTriangle(calculatedXPoints.at(0), calculatedYPoints.at(0),
                          calculatedXPoints.at(1), calculatedYPoints.at(1), 
                          calculatedXPoints.at(2), calculatedYPoints.at(2), WHITE);
    }

    // Get time at frame end
    unsigned long frame_end = millis();
    // Get frame time
    unsigned long frame_time = frame_end - frame_start;

    // Print on screen frame time
    screen.setCursor(0, 0);
    screen.print(frame_time);

    // Upcate screen
    screen.display();
  }
}