#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Cube vertices coordinates (x, y, z)
const int numVertices = 8;
int vertices[numVertices][3] = {
  {-20, -20, -20},
  {20, -20, -20},
  {20, 20, -20},
  {-20, 20, -20},
  {-20, -20, 20},
  {20, -20, 20},
  {20, 20, 20},
  {-20, 20, 20}
};

// Cube edges represented by vertex indices
const int numEdges = 12;
int edges[numEdges][2] = {
  {0, 1}, {1, 2}, {2, 3}, {3, 0}, // bottom face
  {4, 5}, {5, 6}, {6, 7}, {7, 4}, // top face
  {0, 4}, {1, 5}, {2, 6}, {3, 7} // vertical edges
};

float angleX = 0;
float angleY = 0;
float angleZ = 0;

void setup() {
  // Initialize the display
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  display.clearDisplay();
  display.display();
}

void loop() {
  // Clear the display
  display.clearDisplay();
  
  // Rotate the cube
  angleX += 0.05;
  angleY += 0.1;
  angleZ += 0.15;

  // Project and draw each edge
  for (int i = 0; i < numEdges; i++) {
    int vertex1 = edges[i][0];
    int vertex2 = edges[i][1];

    int x1, y1, x2, y2;
    projectVertex(vertices[vertex1], angleX, angleY, angleZ, x1, y1);
    projectVertex(vertices[vertex2], angleX, angleY, angleZ, x2, y2);

    display.drawLine(x1 + SCREEN_WIDTH/2, y1 + SCREEN_HEIGHT/2, x2 + SCREEN_WIDTH/2, y2 + SCREEN_HEIGHT/2, WHITE);
  }
  
  // Update the display
  display.display();
  delay(10);
}

void projectVertex(int vertex[3], float angleX, float angleY, float angleZ, int &x, int &y) {
  // Apply rotation around X-axis
  float y1 = vertex[1] * cos(angleX) - vertex[2] * sin(angleX);
  float z1 = vertex[1] * sin(angleX) + vertex[2] * cos(angleX);

  // Apply rotation around Y-axis
  float x2 = vertex[0] * cos(angleY) - z1 * sin(angleY);
  float z2 = vertex[0] * sin(angleY) + z1 * cos(angleY);

  // Apply rotation around Z-axis
  float x3 = x2 * cos(angleZ) - y1 * sin(angleZ);
  float y3 = x2 * sin(angleZ) + y1 * cos(angleZ);

  // Perspective projection
  //x = (int)(200 * x3 / (z2 + 200));
  //y = (int)(200 * y3 / (z2 + 200));
  x = x3;
  y = y3;
}