#include "U8g2lib.h"

#define button 34
#define CENTER_X 64
#define CENTER_Y 16
// U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST);
// U8GLIB_SSD1306_128X32 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST);
// U8G2_SSD1306_128X32_UNIVISION_1_HW_I2C u8g(U8G2_R0, U8X8_PIN_NONE, 22, 21); // [page buffer, size = 128 bytes]
U8G2_SSD1306_128X64_NONAME_1_HW_I2C u8g (U8G2_R0, U8X8_PIN_NONE, 22, 21);

int points[8][2];
float rotated_3d[8][3];
int angle_deg = 360;
const int z_offset = 4;
int multiplier = 40;

void cube(int angle_deg = 0);
void pyramid(int angle_deg = 0);
void plane(int angle_deg = 0);
void diagonal(int angle_deg = 0);
void rotate_points(int points_amount = 0, const int initial_points[][3] = 0);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.print("start");
  pinMode(button, INPUT_PULLUP);
  u8g.begin();
  // u8g.clear();
  u8g.setColorIndex(1);
  u8g.setFont(u8g_font_6x10);
  Serial.begin(115200);
  Serial.print("started");
}
void loop() {
  static byte mode;
  static uint32_t tmr,fps;
  static int fpsc,disp;
  static bool buttstate1;
  bool button1 = !digitalRead(button); 
  
  if (millis() - fps > 1000){
    fps = millis();
    Serial.println(fpsc);
    disp = fpsc;
    fpsc = 0;
  }

  if (button1 && !buttstate1 && millis() - tmr > 20){
    buttstate1 = true;    
    tmr = millis();    
    if(++mode >= 4) mode = 0;
  }
  if (!button1 && buttstate1 && millis() - tmr > 20){
    buttstate1 = false;    
    tmr = millis();
  }


  if(angle_deg > 0 + 5){
    angle_deg = angle_deg - 5;
  }else{
    angle_deg = 360;
  }

  u8g.firstPage();
  do{
    switch (mode)
    {  
    case 1:
      pyramid(angle_deg);
      break;
    case 2:
      plane(angle_deg);
      break;
    case 3:      
      diagonal(angle_deg);
      break;
    default:
      cube(angle_deg);
      break;
    }
    u8g.setCursor(0,10);
    u8g.println(disp);  
    // u8g.drawLine(0,32,128,32);
  }while(u8g.nextPage());
  fpsc += 1;
  delay(10);
  // put your main code here, to run repeatedly:
}

void rotate_points(int points_amount = 0 , const int initial_points[][3] = 0,const int x_offset = 0,const int y_offset = 0){
  for(int i = 0; i < points_amount; i++){
    rotated_3d[i][0] = initial_points[i][0] * cos(radians(angle_deg)) - initial_points[i][2] * sin(radians(angle_deg));
    rotated_3d[i][1] = initial_points[i][1];
    rotated_3d[i][2] = initial_points[i][0] * sin(radians(angle_deg)) + initial_points[i][2] * cos(radians(angle_deg)) + z_offset;

    points[i][0] = round(rotated_3d[i][0] / rotated_3d[i][2] * multiplier + CENTER_X + x_offset); //X coords
    points[i][1] = round(rotated_3d[i][1] / rotated_3d[i][2] * multiplier + CENTER_Y + y_offset); //Y coords
  }
}

void cube(int angle_deg){
  const int initial_points[8][3]={
    {-1,-1,1},
    {1,-1,1},
    {1,1,1},
    {-1,1,1},
    {-1,-1,-1},
    {1,-1,-1},
    {1,1,-1},
    {-1,1,-1}
  };
  rotate_points(8,initial_points,0,0);
  u8g.drawLine(points[0][0],points[0][1],points[1][0],points[1][1]);
  u8g.drawLine(points[1][0],points[1][1],points[2][0],points[2][1]);
  u8g.drawLine(points[2][0],points[2][1],points[3][0],points[3][1]);
  u8g.drawLine(points[3][0],points[3][1],points[0][0],points[0][1]);

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

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

void pyramid(int angle_deg){
  const int initial_points[5][3]={
    {-1,1,-1},
    {1,1,-1},
    {1,1,1},
    {-1,1,1},
    {0,-1,0}
  };
  rotate_points(5,initial_points,0,0);
  u8g.drawLine(points[0][0],points[0][1],points[1][0],points[1][1]);
  u8g.drawLine(points[1][0],points[1][1],points[2][0],points[2][1]);
  u8g.drawLine(points[2][0],points[2][1],points[3][0],points[3][1]);
  u8g.drawLine(points[3][0],points[3][1],points[0][0],points[0][1]);

  u8g.drawLine(points[4][0],points[4][1],points[1][0],points[1][1]);
  u8g.drawLine(points[4][0],points[4][1],points[2][0],points[2][1]);
  u8g.drawLine(points[4][0],points[4][1],points[3][0],points[3][1]);
  u8g.drawLine(points[4][0],points[4][1],points[0][0],points[0][1]);
}

void diagonal(int angle_deg){
  const int initial_points[2][3]={
    {-1,-1,-1},
    {1,1,1}
  };
  rotate_points(2,initial_points,0,0);
  u8g.drawLine(points[0][0],points[0][1],points[1][0],points[1][1]);
  u8g.drawLine(points[0][0],points[1][1],points[1][0],points[0][1]);
}

void plane(int angle_deg){
  const int initial_points[5][3]={
    {-1,1,-1},
    {1,1,-1},
    {1,1,1},
    {-1,1,1},
  };
  rotate_points(4,initial_points,0,0);
  u8g.drawLine(points[0][0],points[0][1],points[1][0],points[1][1]);
  u8g.drawLine(points[1][0],points[1][1],points[2][0],points[2][1]);
  u8g.drawLine(points[2][0],points[2][1],points[3][0],points[3][1]);
  u8g.drawLine(points[3][0],points[3][1],points[0][0],points[0][1]);
}