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

#define SDA_PIN 6
#define SCL_PIN 7
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

float angleX = 0;
float angleY = 0;

struct Point3D { float x, y, z; };

Point3D cube[8] = {
  {-1, -1, -1}, { 1, -1, -1}, { 1,  1, -1}, {-1,  1, -1},
  {-1, -1,  1}, { 1, -1,  1}, { 1,  1,  1}, {-1,  1,  1}
};

int edges[12][2] = {
  {0,1},{1,2},{2,3},{3,0},
  {4,5},{5,6},{6,7},{7,4},
  {0,4},{1,5},{2,6},{3,7}
};

void project(Point3D p, int &x2d, int &y2d) {
  float scale = 50;
  float z = p.z + 4;
  x2d = (int)(p.x * scale / z) + 64;
  y2d = (int)(p.y * scale / z) + 32;
}

void setup() {
  Wire.begin(SDA_PIN, SCL_PIN);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
}

void loop() {
  display.clearDisplay();

  Point3D r[8];

  for (int i = 0; i < 8; i++) {
    float x = cube[i].x;
    float y = cube[i].y;
    float z = cube[i].z;

    float cosX = cos(angleX);
    float sinX = sin(angleX);
    float y1 = y * cosX - z * sinX;
    float z1 = y * sinX + z * cosX;

    float cosY = cos(angleY);
    float sinY = sin(angleY);
    float x2 = x * cosY + z1 * sinY;
    float z2 = -x * sinY + z1 * cosY;

    r[i] = {x2, y1, z2};
  }

  for (int i = 0; i < 12; i++) {
    int x0, y0, x1, y1;
    project(r[edges[i][0]], x0, y0);
    project(r[edges[i][1]], x1, y1);
    display.drawLine(x0, y0, x1, y1, SSD1306_WHITE);
  }

  display.display();

  angleX += 0.05;
  angleY += 0.04;

  delay(15);
}