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

// Joystick connection
#define VERT_PIN 14
#define HORZ_PIN 26
#define SEL_PIN  33

#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);

int points[8][2]; // eight 2D points for the cube, values will be calculated in the code

int orig_points [8][3] = {  // eight 3D points - set values for 3D cube
{-1,-1, 1},
{1,-1,1},
{1,1,1},
{-1,1,1},
{-1,-1,-1},
{1,-1,-1},
{1,1,-1},
{-1,1,-1}
};

float rotated_3d_points [8][3];   // eight 3D points - rotated around Y axis
float Zrotated_3d_points [8][3];  // eight 3D points - rotated around Z axis
float XZrotated_3d_points [8][3];
float angle_degY = 0;          // rotation around the Y axis
float angle_degX = 0; 
float angle_degZ = 0; 
float z_offset = - 4.0;            // offset on Z axis, offset so it goes further away from the screen
float cube_size = 70.0;           // cube size (multiplier)

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

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  display.clearDisplay();

  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);
  
}


void loop() {

  int horz = analogRead(HORZ_PIN);                   // HORZ_PIN goes from 0 (right) to 1023 (left)
  int vert = analogRead(VERT_PIN);                   // VERT_PIN goes from 0 (bottom) to 1023 (top)
  bool selPressed = digitalRead(SEL_PIN) == LOW;     // selPressed is true is the joystick is pressed
  
  horz = map(horz, 0, 4096, 0, 200);     // horz goes from 0 (right) to 200 (left)
  vert = map(vert, 0, 4096, 0, 200);     // vert goes from 0 (bottom) to 200 (top)

  float angle_userX = 0;
  float angle_userY = 0;
  float angle_userZ = 0;

  //Serial.print("horz = ");
  //Serial.println(horz);

///////////////////////////////////////// X
  if (horz > 120) {
    angle_userX = -5;
  }
  else if (horz < 80){
    angle_userX = 5;
  }
  else {
    angle_userX = 0;
  }
  
  if (angle_degX < 360) {
    angle_degX = angle_degX + angle_userX;
  } else {
    angle_degX = 0;
  }

///////////////////////////////////////// Y
  if (vert > 120) {
    angle_userY = -5;
  }
  else if (vert < 80){
    angle_userY = 5;
  }
  else {
    angle_userY = 0;
  }
  
  if (angle_degY < 360) {
    angle_degY = angle_degY + angle_userY;
  } else {
    angle_degY = 0;
  }
  
///////////////////////////////////////// Z
  if (vert > 120 && horz > 120) {
    angle_userZ = -5;
  }
  else if (vert < 80 && horz <80){
    angle_userZ = 5;
  }
  else {
    angle_userZ = 0;
  }

  if (angle_degZ < 360) {
    angle_degZ = angle_degZ + angle_userZ;
  } else {
    angle_degZ = 0;
  }


  // calculate the points
  for (int i=0; i<8; i++) {
    // rotate 3d points around the Y axis (rotating X nad Z positions)
    rotated_3d_points [i][0] = orig_points [i][0] * cos(radians(angle_degX)) - orig_points [i][2] * sin(radians(angle_degX));
    rotated_3d_points [i][1] = orig_points [i][1];
    rotated_3d_points [i][2] = orig_points [i][0] * sin(radians(angle_degX)) + orig_points [i][2] * cos(radians(angle_degX));  

    // rotate 3d points around the Z axis (rotating X and Y positions)
    Zrotated_3d_points [i][0] = rotated_3d_points [i][0] * cos(radians(angle_degZ)) - rotated_3d_points [i][1] * sin(radians(angle_degZ));
    Zrotated_3d_points [i][1] = rotated_3d_points [i][0] * sin(radians(angle_degZ)) + rotated_3d_points [i][1] * cos(radians(angle_degZ));
    Zrotated_3d_points [i][2] = rotated_3d_points [i][2] ;

    // rotate 3d points around the X axis (rotating Y and Z positions)
    XZrotated_3d_points [i][0] = Zrotated_3d_points [i][0] ;
    XZrotated_3d_points [i][1] = Zrotated_3d_points [i][1] * cos(radians(angle_degY)) - Zrotated_3d_points [i][2] * sin(radians(angle_degY));
    XZrotated_3d_points [i][2] = Zrotated_3d_points [i][1] * sin(radians(angle_degY)) + Zrotated_3d_points [i][2] * cos(radians(angle_degY)) + z_offset;

    // project 3d points into 2d space with perspective divide -- 2D x = x/z,   2D y = y/z
    points[i][0] = round(64 + XZrotated_3d_points [i][0] / XZrotated_3d_points [i][2] * cube_size);
    points[i][1] = round(32 + XZrotated_3d_points [i][1] / XZrotated_3d_points [i][2] * cube_size);    
  }

  testdrawCube();

}

void testdrawCube(){
  display.clearDisplay();
  display.drawLine(points[ 0 ][ 0 ],points[ 0 ][ 1 ],points[ 1 ][ 0 ],points[ 1 ][ 1 ],SSD1306_WHITE); // 0 - 1
  display.drawLine(points[ 1 ][ 0 ],points[ 1 ][ 1 ],points[ 2 ][ 0 ],points[ 2 ][ 1 ],SSD1306_WHITE); // 1 - 2
  display.drawLine(points[ 2 ][ 0 ],points[ 2 ][ 1 ],points[ 3 ][ 0 ],points[ 3 ][ 1 ],SSD1306_WHITE); // 2 - 3
  display.drawLine(points[ 3 ][ 0 ],points[ 3 ][ 1 ],points[ 0 ][ 0 ],points[ 0 ][ 1 ],SSD1306_WHITE); // 3 - 0

  display.drawLine(points[ 4 ][ 0 ],points[ 4 ][ 1 ],points[ 5 ][ 0 ],points[ 5 ][ 1 ],SSD1306_WHITE); // 4 - 5
  display.drawLine(points[ 5 ][ 0 ],points[ 5 ][ 1 ],points[ 6 ][ 0 ],points[ 6 ][ 1 ],SSD1306_WHITE); // 5 - 6
  display.drawLine(points[ 6 ][ 0 ],points[ 6 ][ 1 ],points[ 7 ][ 0 ],points[ 7 ][ 1 ],SSD1306_WHITE); // 6 - 7
  display.drawLine(points[ 7 ][ 0 ],points[ 7 ][ 1 ],points[ 4 ][ 0 ],points[ 4 ][ 1 ],SSD1306_WHITE); // 7 - 4

  display.drawLine(points[ 0 ][ 0 ],points[ 0 ][ 1 ],points[ 4 ][ 0 ],points[ 4 ][ 1 ],SSD1306_WHITE); // 0 - 4
  display.drawLine(points[ 1 ][ 0 ],points[ 1 ][ 1 ],points[ 5 ][ 0 ],points[ 5 ][ 1 ],SSD1306_WHITE); // 1 - 5
  display.drawLine(points[ 2 ][ 0 ],points[ 2 ][ 1 ],points[ 6 ][ 0 ],points[ 6 ][ 1 ],SSD1306_WHITE); // 2 - 6
  display.drawLine(points[ 3 ][ 0 ],points[ 3 ][ 1 ],points[ 7 ][ 0 ],points[ 7 ][ 1 ],SSD1306_WHITE); // 3 - 7
  display.display();
}