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

struct vec3d
{
	float x, y, z;
};

struct triangle
{
	vec3d p[3];
};

struct mesh{
std::vector<triangle> exvector = {
    // SOUTH
		{ 0.0f, 0.0f, 0.0f,    0.0f, 1.0f, 0.0f,    1.0f, 1.0f, 0.0f },
		{ 0.0f, 0.0f, 0.0f,    1.0f, 1.0f, 0.0f,    1.0f, 0.0f, 0.0f },

		// EAST                                                      
		{ 1.0f, 0.0f, 0.0f,    1.0f, 1.0f, 0.0f,    1.0f, 1.0f, 1.0f },
		{ 1.0f, 0.0f, 0.0f,    1.0f, 1.0f, 1.0f,    1.0f, 0.0f, 1.0f },

		// NORTH                                                     
		{ 1.0f, 0.0f, 1.0f,    1.0f, 1.0f, 1.0f,    0.0f, 1.0f, 1.0f },
		{ 1.0f, 0.0f, 1.0f,    0.0f, 1.0f, 1.0f,    0.0f, 0.0f, 1.0f },

		// WEST                                                      
		{ 0.0f, 0.0f, 1.0f,    0.0f, 1.0f, 1.0f,    0.0f, 1.0f, 0.0f },
		{ 0.0f, 0.0f, 1.0f,    0.0f, 1.0f, 0.0f,    0.0f, 0.0f, 0.0f },

		// TOP                                                       
		{ 0.0f, 1.0f, 0.0f,    0.0f, 1.0f, 1.0f,    1.0f, 1.0f, 1.0f },
		{ 0.0f, 1.0f, 0.0f,    1.0f, 1.0f, 1.0f,    1.0f, 1.0f, 0.0f },

		// BOTTOM                                                    
		{ 1.0f, 0.0f, 1.0f,    0.0f, 0.0f, 1.0f,    0.0f, 0.0f, 0.0f },
		{ 1.0f, 0.0f, 1.0f,    0.0f, 0.0f, 0.0f,    1.0f, 0.0f, 0.0f },

		};
};

struct mat4x4
{
	float m[4][4] = { 0 };
};


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

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

  mesh tris;
  
  for (auto &&tri : tris.exvector)
    {
      Serial.printf("%f\t%f\t%f\n", tri.p[2].x, tri.p[2].y, tri.p[2].z);
    }


  // 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)

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

  if (horz > 120) {
    angle_user1 = -5;
  }
  else if (horz < 80){
    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;
  }

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

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


void testdrawline() {
  int16_t i;

  display.clearDisplay(); // Clear display buffer

  for(i=0; i<display.width(); i+=4) {
    display.drawLine(0, 0, i, display.height()-1, SSD1306_WHITE);
    display.display(); // Update screen with each newly-drawn line
    delay(1);
  }
  for(i=0; i<display.height(); i+=4) {
    display.drawLine(0, 0, display.width()-1, i, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  delay(250);

  display.clearDisplay();

  for(i=0; i<display.width(); i+=4) {
    display.drawLine(0, display.height()-1, i, 0, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  for(i=display.height()-1; i>=0; i-=4) {
    display.drawLine(0, display.height()-1, display.width()-1, i, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  delay(250);

  display.clearDisplay();

  for(i=display.width()-1; i>=0; i-=4) {
    display.drawLine(display.width()-1, display.height()-1, i, 0, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  for(i=display.height()-1; i>=0; i-=4) {
    display.drawLine(display.width()-1, display.height()-1, 0, i, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  delay(250);

  display.clearDisplay();

  for(i=0; i<display.height(); i+=4) {
    display.drawLine(display.width()-1, 0, 0, i, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  for(i=0; i<display.width(); i+=4) {
    display.drawLine(display.width()-1, 0, i, display.height()-1, SSD1306_WHITE);
    display.display();
    delay(1);
  }

  delay(2000); // Pause for 2 seconds
}

void testdrawtriangle(void) {
  display.clearDisplay();

  for(int16_t i=0; i<max(display.width(),display.height())/2; i+=5) {
    display.drawTriangle(
      display.width()/2  , display.height()/2-i,
      display.width()/2-i, display.height()/2+i,
      display.width()/2+i, display.height()/2+i, SSD1306_WHITE);
    display.display();
    delay(1);
  }

  delay(2000);
}

void testfilltriangle(void) {
  display.clearDisplay();

  for(int16_t i=max(display.width(),display.height())/2; i>0; i-=5) {
    // The INVERSE color is used so triangles alternate white/black
    display.fillTriangle(
      display.width()/2  , display.height()/2-i,
      display.width()/2-i, display.height()/2+i,
      display.width()/2+i, display.height()/2+i, SSD1306_INVERSE);
    display.display();
    delay(1);
  }

  delay(2000);
}

void MultiplyMatrixVector(vec3d &i)
	{
    Serial.print("i.x = ");
    Serial.println(i.x);
	}