#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.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)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


float cubeVertices[8][3] = {
  {-10, -10, -10},
  {10, -10, -10},
  {10, 10, -10},
  {-10, 10, -10},
  {-10, -10, 10},
  {10, -10, 10},
  {10, 10, 10},
  {-10, 10, 10},
};

int cubeIndices[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},
};

float thetaZ = 0;

void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
}

void drawLine(int x1, int y1, int x2, int y2) {
  display.drawLine(x1, y1, x2, y2, SSD1306_WHITE);
}

void drawCube() {
  for (int i = 0; i < 12; i++) {
    int index1 = cubeIndices[i][0];
    int index2 = cubeIndices[i][1];
    int x1 = cubeVertices[index1][0] + SCREEN_WIDTH / 2;
    int y1 = cubeVertices[index1][1] + SCREEN_HEIGHT / 2;
    int x2 = cubeVertices[index2][0] + SCREEN_WIDTH / 2;
    int y2 = cubeVertices[index2][1] + SCREEN_HEIGHT / 2;
    drawLine(x1, y1, x2, y2);
  }
}
void rotateCube_3D() {
  for (int i = 0; i < 8; i++) {
    float x = cubeVertices[i][0];
    float y = cubeVertices[i][1];
    float z = cubeVertices[i][2];

    cubeVertices[i][0] = x * cos(thetaZ) - y * sin(thetaZ);
    cubeVertices[i][1] = y * cos(thetaZ) + x * sin(thetaZ);
    cubeVertices[i][2] = z;
  }
}

void rotateCube_2D() {
  for (int i = 0; i < 8; i++) {
    float x = cubeVertices[i][0];
    float y = cubeVertices[i][1];

    cubeVertices[i][0] = x * cos(thetaZ) - y * sin(thetaZ);
    cubeVertices[i][1] = y * cos(thetaZ) + x * sin(thetaZ);
  }
}

void loop() {
  display.clearDisplay();
  //rotateCube_2D();
  rotateCube_3D();
  drawCube();
  display.display();
  
  delay(100);
  thetaZ += 0.1;
}

//---------------------------------------------------------------------------------

#if 0
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.h>

#define CAM_F 120

#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)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

/*
#define OLED_DC     6
#define OLED_CS     7
#define OLED_RESET  8
Adafruit_SSD1306 display(OLED_DC, OLED_RESET, OLED_CS);
*/

typedef struct {
  int x;
  int y;
  int z;
} point3D_type, *point3D;

double theta = 0.0;
double sinTheta;
double cosTheta;

void advanceRotation() {
  theta += 0.1;

  if (theta >= (2 * 3.1417)) {
    theta = 0.0;
  }

  // we re-use these a LOT
  sinTheta = sin(theta);
  cosTheta = cos(theta);
}

int xpos = 0;
int ypos = 0;
int zpos = 0;
int dx = 1;
int dy = 2;
int dz = 3;

void advancePosition() {

  xpos += dx;
  ypos += dy;
  zpos += dz;

  if (xpos >= 64 || xpos <= -64) {
    dx *= -1;
  }

  if (ypos >= 32 || ypos <= -32) {
    dy *= -1;
  }

  if (zpos >= 32 || zpos <= -32) {
    dz *= -1;
  }

}

void drawProjectedLine(point3D_type from, point3D_type to) {

  /* apply camera transform */

  from.z += CAM_F;
  to.z += CAM_F;

  int x1_2D = 64 + (int)(((double)from.x / (double)from.z) * CAM_F);
  int y1_2D = 32 + (int)(((double)from.y / (double)from.z) * CAM_F);

  int x2_2D = 64 + (int)(((double)to.x / (double)to.z) * CAM_F);
  int y2_2D = 32 + (int)(((double)to.y / (double)to.z) * CAM_F);

  /* take projected positions and draw on screen */
  display.drawLine(x1_2D, y1_2D, x2_2D, y2_2D, WHITE);
}

void rotateAndTranslate(point3D p, int x, int y, int z) {
  int new_x = (p->x * cosTheta) + (p->z *  sinTheta) + (x);
  int new_y = (p->y) + (y);
  int new_z = (-(p->x) * sinTheta) + (p->z * cosTheta) + z;

  p->x = new_x;
  p->y = new_y;
  p->z = new_z;
}

// draw a cube centered on the point (x,y,z) with sides of length w
void drawCube(int x, int y, int z, int w) {

  int r = w / 2; // not sure how to abreviate half-of-width;

  // variables we'll use to create our sides
  point3D_type p0;
  point3D_type p1;
  point3D_type p2;
  point3D_type p3;
  point3D_type p4;
  point3D_type p5;
  point3D_type p6;
  point3D_type p7;

  p0 = { -r,  r, -r };
  p1 = {  r,  r, -r };
  p2 = { -r, -r, -r };
  p3 = {  r, -r, -r };
  p4 = { -r,  r,  r };
  p5 = {  r,  r,  r };
  p6 = { -r, -r,  r };
  p7 = {  r, -r,  r };

  rotateAndTranslate(&p0, x, y, z);
  rotateAndTranslate(&p1, x, y, z);
  rotateAndTranslate(&p2, x, y, z);
  rotateAndTranslate(&p3, x, y, z);
  rotateAndTranslate(&p4, x, y, z);
  rotateAndTranslate(&p5, x, y, z);
  rotateAndTranslate(&p6, x, y, z);
  rotateAndTranslate(&p7, x, y, z);

  drawProjectedLine(p0, p1);
  drawProjectedLine(p0, p2);
  drawProjectedLine(p0, p4);
  drawProjectedLine(p3, p1);
  drawProjectedLine(p3, p2);
  drawProjectedLine(p3, p7);
  drawProjectedLine(p6, p2);
  drawProjectedLine(p6, p4);
  drawProjectedLine(p6, p7);
  drawProjectedLine(p5, p4);
  drawProjectedLine(p5, p7);
  drawProjectedLine(p5, p1);
}


void setup() {
  Serial.begin(9600);
  display.begin(SSD1306_SWITCHCAPVCC);

    // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
    // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  // display.display();
  // delay(2000); // Pause for 2 seconds
  
}

void loop() {
  display.clearDisplay();

  drawCube(xpos, ypos, zpos, 16);
  drawCube(32, 32, 32, 12);

  display.display();

  advancePosition();
  advanceRotation();

  // slow down animation slightly
  delay(10);
}
#endif