#include "U8glib.h" // u8g library, note there is a newer version u8g2, please use the older one


#define VERT_PIN A0
#define HORZ_PIN A1
#define SEL_PIN  2


U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST); 


void DrawTriangles(int x1, int y1,int x2, int y2,int x3, int y3){
  u8g.drawLine(x1,y1,x2,y2);
  u8g.drawLine(x2,y2,x3,y3);
  u8g.drawLine(x3,y3,x1,y1);
}


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

int orig_points_tri [4][3] = {
{-1,-1,-1},
{-1,1,-1},
{1,1,-1},
{-1,-1,1} //{0,0,1}
};

int orig_points_car [12][3] = {
{-1,-1,1},
{-1,-1,-2},
{-1,0,-2},
{-1,0,-1},
{-1,1,-1},
{-1,1,1},
{1,-1,1},
{1,-1,-2},
{1,0,-2},
{1,0,-1},
{1,1,-1},
{1,1,1}
};

int var = 1;
bool once;

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 angle_deg = 60.0;           // rotation around the Y axis
float angle_user1 = 0;
float z_offset = -5;            // offset on Z axis
float cube_size = 70.0;           // cube size (multiplier)
//float time_frame;               // ever increasing time value

void setup() {

  Serial.begin(9600);

  u8g.setColorIndex(1); // set color to white

  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, 1023, 0, 200);     // horz goes from 0 (right) to 200 (left)
  vert = map(vert, 0, 1023, 0, 200);     // vert goes from 0 (bottom) to 200 (top)

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

  if (horz > 150) {
    angle_user1 = -5;
  }
  else if (120 < horz && horz < 150 ) {
    angle_user1 = -2;
  }
  else if (50 < horz && horz < 80){
    angle_user1 = 2;
  }
  else if (horz < 50){
    angle_user1 = 5;
  }
  else {
    angle_user1 = 0;
  }
  

  // increase the angle by 5° increments
  if (angle_deg < 360) {
    angle_deg = angle_deg + angle_user1;
  } else {
    angle_deg = 0;
  }


  if (selPressed && !once){
    var += 1;
    if (var > 2){
      var = 1;
    }
    once = true;
  }

  if (!selPressed) {
    once = false;
  }

  switch (var) {
  case 1:

    // 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_deg)) - orig_points [i][2] * sin(radians(angle_deg));
    rotated_3d_points [i][1] = orig_points [i][1];
    rotated_3d_points [i][2] = orig_points [i][0] * sin(radians(angle_deg)) + orig_points [i][2] * cos(radians(angle_deg)) + z_offset;  

    // 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_deg)) - rotated_3d_points [i][1] * sin(radians(angle_deg));
    Zrotated_3d_points [i][1] = rotated_3d_points [i][0] * sin(radians(angle_deg)) + rotated_3d_points [i][1] * cos(radians(angle_deg));
    Zrotated_3d_points [i][2] = rotated_3d_points [i][2] ;

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


  u8g.firstPage();
  do {
      // connect the lines between the individual points
      u8g.drawLine(points[ 0 ][ 0 ], points[ 0 ][ 1 ] , points[ 1 ][ 0 ] , points[ 1 ][ 1 ] );  // connect points 0-1
      u8g.drawLine(points[ 1 ][ 0 ], points[ 1 ][ 1 ] , points[ 2 ][ 0 ] , points[ 2 ][ 1 ] );  // connect points 1-2  
      u8g.drawLine(points[ 2 ][ 0 ], points[ 2 ][ 1 ] , points[ 3 ][ 0 ] , points[ 3 ][ 1 ] );  // connect points 2-3      
      u8g.drawLine(points[ 3 ][ 0 ], points[ 3 ][ 1 ] , points[ 0 ][ 0 ] , points[ 0 ][ 1 ] );  // connect points 3-0      

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

      u8g.drawLine(points[ 0 ][ 0 ], points[ 0 ][ 1 ] , points[ 4 ][ 0 ] , points[ 4 ][ 1 ] );  // connect points 0-4
      u8g.drawLine(points[ 1 ][ 0 ], points[ 1 ][ 1 ] , points[ 5 ][ 0 ] , points[ 5 ][ 1 ] );  // connect points 1-5  
      u8g.drawLine(points[ 2 ][ 0 ], points[ 2 ][ 1 ] , points[ 6 ][ 0 ] , points[ 6 ][ 1 ] );  // connect points 2-6      
      u8g.drawLine(points[ 3 ][ 0 ], points[ 3 ][ 1 ] , points[ 7 ][ 0 ] , points[ 7 ][ 1 ] );  // connect points 3-7                 


  } while ( u8g.nextPage() );   // u8g library specific, has to be there
    break;

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

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

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

  u8g.firstPage();
  do {
      // connect the lines between the individual points
      u8g.drawLine(points[ 0 ][ 0 ], points[ 0 ][ 1 ] , points[ 1 ][ 0 ] , points[ 1 ][ 1 ] );  // connect points 0-1
      u8g.drawLine(points[ 1 ][ 0 ], points[ 1 ][ 1 ] , points[ 2 ][ 0 ] , points[ 2 ][ 1 ] );  // connect points 1-2  
      u8g.drawLine(points[ 2 ][ 0 ], points[ 2 ][ 1 ] , points[ 3 ][ 0 ] , points[ 3 ][ 1 ] );  // connect points 2-3   
      u8g.drawLine(points[ 3 ][ 0 ], points[ 3 ][ 1 ] , points[ 4 ][ 0 ] , points[ 4 ][ 1 ] );  // connect points 3-4
      u8g.drawLine(points[ 4 ][ 0 ], points[ 4 ][ 1 ] , points[ 5 ][ 0 ] , points[ 5 ][ 1 ] );  // connect points 4-5  
      u8g.drawLine(points[ 5 ][ 0 ], points[ 5 ][ 1 ] , points[ 0 ][ 0 ] , points[ 0 ][ 1 ] );  // connect points 5-0

      u8g.drawLine(points[ 6 ][ 0 ], points[ 6 ][ 1 ] , points[ 7 ][ 0 ] , points[ 7 ][ 1 ] );  // connect points 6-7
      u8g.drawLine(points[ 7 ][ 0 ], points[ 7 ][ 1 ] , points[ 8 ][ 0 ] , points[ 8 ][ 1 ] );  // connect points 7-8
      u8g.drawLine(points[ 8 ][ 0 ], points[ 8 ][ 1 ] , points[ 9 ][ 0 ] , points[ 9 ][ 1 ] );  // connect points 8-9
      u8g.drawLine(points[ 9 ][ 0 ], points[ 9 ][ 1 ] , points[ 10][ 0 ] , points[ 10][ 1 ] );  // connect points 9-10
      u8g.drawLine(points[ 10][ 0 ], points[ 10][ 1 ] , points[ 11][ 0 ] , points[ 11][ 1 ] );  // connect points 10-11            
      u8g.drawLine(points[ 11][ 0 ], points[ 11][ 1 ] , points[ 6 ][ 0 ] , points[ 6 ][ 1 ] );  // connect points 11-6

      u8g.drawLine(points[ 0 ][ 0 ], points[ 0 ][ 1 ] , points[ 6 ][ 0 ] , points[ 6 ][ 1 ] );  // connect points 0-6
      u8g.drawLine(points[ 1 ][ 0 ], points[ 1 ][ 1 ] , points[ 7 ][ 0 ] , points[ 7 ][ 1 ] );  // connect points 1-7  
      u8g.drawLine(points[ 2 ][ 0 ], points[ 2 ][ 1 ] , points[ 8 ][ 0 ] , points[ 8 ][ 1 ] );  // connect points 2-8   
      u8g.drawLine(points[ 3 ][ 0 ], points[ 3 ][ 1 ] , points[ 9 ][ 0 ] , points[ 9 ][ 1 ] );  // connect points 3-9
      u8g.drawLine(points[ 4 ][ 0 ], points[ 4 ][ 1 ] , points[ 10][ 0 ] , points[ 10][ 1 ] );  // connect points 4-10 
      u8g.drawLine(points[ 5 ][ 0 ], points[ 5 ][ 1 ] , points[ 11][ 0 ] , points[ 11][ 1 ] );  // connect points 5-11

  } while ( u8g.nextPage() );   // u8g library specific, has to be there
    break;

  default:

    break;
}

  

}