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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const int PIN_SDA = 22;
const int PIN_SCL = 23;

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

struct V3 { float x,y,z; };
struct V2 { int16_t x,y; };

V3 verts[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}
};

uint8_t 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}
};

V3 rot(V3 p, float ax, float ay, float az){
  float cx=cosf(ax), sx=sinf(ax);
  float cy=cosf(ay), sy=sinf(ay);
  float cz=cosf(az), sz=sinf(az);
  float y=cx*p.y - sx*p.z, z=sx*p.y + cx*p.z; p.y=y; p.z=z;
  float x=cy*p.x + sy*p.z; z=-sy*p.x + cy*p.z; p.x=x; p.z=z;
  x=cz*p.x - sz*p.y; y=sz*p.x + cz*p.y; p.x=x; p.y=y;
  return p;
}

V2 proj(V3 p){
  float d=3.0, f=64.0, s=f/(d - p.z);
  int16_t x=(int16_t)(p.x*s + SCREEN_WIDTH/2);
  int16_t y=(int16_t)(p.y*s + SCREEN_HEIGHT/2);
  return {x,y};
}

void setup(){
  Wire.begin(PIN_SDA, PIN_SCL);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();
}

void loop(){
  display.clearDisplay();
  V2 pts[8];
  for(int i=0;i<8;i++){
    V3 p=verts[i];
    p.x*=20; p.y*=20; p.z*=20;
    p=rot(p, angleX, angleY, angleZ);
    pts[i]=proj(p);
  }
  for(int i=0;i<12;i++){
    uint8_t a=edges[i][0], b=edges[i][1];
    display.drawLine(pts[a].x, pts[a].y, pts[b].x, pts[b].y, SSD1306_WHITE);
  }
  display.display();
  angleX+=0.02; angleY+=0.03; angleZ+=0.015;
  delay(16);
}