#include <vector>
#include <Vector.h>
#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);
Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Duplicated buffer
static uint8_t displayBuffer[(SCREEN_WIDTH * SCREEN_HEIGHT + 7) / 8];

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

struct triangle
{
	vec3d p[3];
};

struct mesh{
std::vector<triangle> tris = {
    // 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 };
};

float angle_userX = 0.0f; //radians
float angle_userZ = 0.0f; //radians
float fNear = 0.1f;
float fFar = 1000.0f;
float fFov = 90.0f;
float fAspectRatio = (float)SCREEN_HEIGHT / (float)SCREEN_WIDTH;
float fFovRad = 1.0f / tanf(fFov * 0.5f / 180.0f * 3.14159f);
float fThetaX = 0.0f;
float fThetaZ = 0.0f;

vec3d vCamera;

void setup() {
  Serial.begin(9600);
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) {  // Address 0x3D for 128x64   ... and don t delete
    Serial.println(F("SSD1306 allocation failed")); // this line of code bcz dosen t works without it
    for(;;); // Don't proceed, loop forever
  }
  if(!display2.begin(SSD1306_SWITCHCAPVCC, 0x3D)) {  // Address 0x3D for 128x64   ... and don t delete
    Serial.println(F("SSD1306 allocation failed")); // this line of code bcz dosen t works without it
    for(;;); // Don't proceed, loop forever
  }

  display.clearDisplay();
  display2.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(selPressed){
    display.clearDisplay();
  }

  if (vert > 120) {
    angle_userX = -0.2;
  }
  else if (vert < 80){
    angle_userX = 0.2;
  }
  else {
    angle_userX = 0;
  }
  
  // increase the angle by 5° increments
  if (fThetaX < 360) {
    fThetaX = fThetaX + angle_userX;
  } 
  else {
    fThetaX = 0;
  }

 if (horz > 120) {
    angle_userZ = -0.2;
  }
  else if (horz < 80){
    angle_userZ = 0.2;
  }
  else {
    angle_userZ = 0;
  }
  
  // increase the angle by 5° increments
  if (fThetaZ < 360) {
    fThetaZ = fThetaZ + angle_userZ;
  } 
  else {
    fThetaZ = 0;
  }

  mesh meshCube;
  mat4x4 matProj;
  mat4x4 matRotZ, matRotX;
	
  matProj.m[0][0] = fAspectRatio * fFovRad;
	matProj.m[1][1] = fFovRad;
	matProj.m[2][2] = fFar / (fFar - fNear);
	matProj.m[3][2] = (-fFar * fNear) / (fFar - fNear);
	matProj.m[2][3] = 1.0f;
	matProj.m[3][3] = 0.0f;

	// Rotation Z
	matRotZ.m[0][0] = cosf(fThetaZ);
	matRotZ.m[0][1] = sinf(fThetaZ);
	matRotZ.m[1][0] = -sinf(fThetaZ);
	matRotZ.m[1][1] = cosf(fThetaZ);
	matRotZ.m[2][2] = 1;
	matRotZ.m[3][3] = 1;

	// Rotation X
	matRotX.m[0][0] = 1;
	matRotX.m[1][1] = cosf(fThetaX * 0.5f);
	matRotX.m[1][2] = sinf(fThetaX * 0.5f);
	matRotX.m[2][1] = -sinf(fThetaX * 0.5f);
	matRotX.m[2][2] = cosf(fThetaX * 0.5f);
	matRotX.m[3][3] = 1;

  for (auto tri : meshCube.tris)
	{
		triangle triProjected, triTranslated, triRotatedZ, triRotatedZX;

		// Rotate in Z-Axis
		MultiplyMatrixVector(tri.p[0], triRotatedZ.p[0], matRotZ);
		MultiplyMatrixVector(tri.p[1], triRotatedZ.p[1], matRotZ);
		MultiplyMatrixVector(tri.p[2], triRotatedZ.p[2], matRotZ);

    // Rotate in X-Axis
		MultiplyMatrixVector(triRotatedZ.p[0], triRotatedZX.p[0], matRotX);
		MultiplyMatrixVector(triRotatedZ.p[1], triRotatedZX.p[1], matRotX);
		MultiplyMatrixVector(triRotatedZ.p[2], triRotatedZX.p[2], matRotX);

		// Offset into the screen
		triTranslated = triRotatedZX;
		triTranslated.p[0].z = triRotatedZX.p[0].z + 2.0f;
		triTranslated.p[1].z = triRotatedZX.p[1].z + 2.0f;
		triTranslated.p[2].z = triRotatedZX.p[2].z + 2.0f;

    // Use Cross-Product to get surface normal
		vec3d normal, line1, line2;
		line1.x = triTranslated.p[1].x - triTranslated.p[0].x;
		line1.y = triTranslated.p[1].y - triTranslated.p[0].y;
		line1.z = triTranslated.p[1].z - triTranslated.p[0].z;

		line2.x = triTranslated.p[2].x - triTranslated.p[0].x;
		line2.y = triTranslated.p[2].y - triTranslated.p[0].y;
		line2.z = triTranslated.p[2].z - triTranslated.p[0].z;

		normal.x = line1.y * line2.z - line1.z * line2.y;
		normal.y = line1.z * line2.x - line1.x * line2.z;
		normal.z = line1.x * line2.y - line1.y * line2.x;

		// It's normally normal to normalise the normal; calculate lenght of the normal
		float l = sqrtf(normal.x*normal.x + normal.y*normal.y + normal.z*normal.z);
    // divide the individual components of the normal by the lenght
		normal.x /= l; normal.y /= l; normal.z /= l;

    //if (normal.z < 0)
		if(normal.x * (triTranslated.p[0].x - vCamera.x) + 
			normal.y * (triTranslated.p[0].y - vCamera.y) +
			normal.z * (triTranslated.p[0].z - vCamera.z) < 0.0f){

			// Project triangles from 3D --> 2D
			MultiplyMatrixVector(triTranslated.p[0], triProjected.p[0], matProj);
			MultiplyMatrixVector(triTranslated.p[1], triProjected.p[1], matProj);
			MultiplyMatrixVector(triTranslated.p[2], triProjected.p[2], matProj);

			// Scale into view
			triProjected.p[0].x += 1.0f; triProjected.p[0].y += 1.0f;
			triProjected.p[1].x += 1.0f; triProjected.p[1].y += 1.0f;
			triProjected.p[2].x += 1.0f; triProjected.p[2].y += 1.0f;
			triProjected.p[0].x *= 0.5f * (float)SCREEN_WIDTH;
			triProjected.p[0].y *= 0.5f * (float)SCREEN_HEIGHT;
			triProjected.p[1].x *= 0.5f * (float)SCREEN_WIDTH;
			triProjected.p[1].y *= 0.5f * (float)SCREEN_HEIGHT;
			triProjected.p[2].x *= 0.5f * (float)SCREEN_WIDTH;
			triProjected.p[2].y *= 0.5f * (float)SCREEN_HEIGHT;
      
      //writing to display 2
      display2.drawTriangle(
      triProjected.p[0].x, triProjected.p[0].y,
			triProjected.p[1].x, triProjected.p[1].y,
			triProjected.p[2].x, triProjected.p[2].y, SSD1306_WHITE);
      
    }
  }

// Copy over the contents of the display2 buffer to the duplicate buffer
memcpy(displayBuffer, display2.getBuffer(), sizeof(displayBuffer));

display2.clearDisplay();

// Copy from the duplicate buffer
memcpy(display.getBuffer(), displayBuffer, sizeof(displayBuffer));

// Display it
display.display();
}

void MultiplyMatrixVector(vec3d &i, vec3d &o, mat4x4 &m){
	o.x = i.x * m.m[0][0] + i.y * m.m[1][0] + i.z * m.m[2][0] + m.m[3][0];
	o.y = i.x * m.m[0][1] + i.y * m.m[1][1] + i.z * m.m[2][1] + m.m[3][1];
	o.z = i.x * m.m[0][2] + i.y * m.m[1][2] + i.z * m.m[2][2] + m.m[3][2];
	float w = i.x * m.m[0][3] + i.y * m.m[1][3] + i.z * m.m[2][3] + m.m[3][3];

	if (w != 0.0f){
		o.x /= w; o.y /= w; o.z /= w;
	}
}