#include "logger.h"

#include "stb_image.h"

#define _USE_MATH_DEFINES
#define GLM_SWIZZLE

#include <glm/mat4x4.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <GL/glew.h>
#include <GLFW/glfw3.h>

#include <cstdio>
#include <iostream>
#include <fstream>
#include <cmath>
#include <string>
#include <array>
#include <vector>

using namespace std;
using namespace glm;

#define ONE_DEG_IN_RAD (2.0 * M_PI) / 360.0 // 0.017444444

struct SceneObject {
   mat4 model_mat;
};

struct ObjectFace {
   unsigned int objectId;
   array<vec3, 3> points;
};

const bool FULLSCREEN = false;
int width = 640;
int height = 480;

vec3 cam_pos;

mat4 view_mat;
mat4 proj_mat;

vector<SceneObject> objects;
vector<ObjectFace> faces;

SceneObject* clickedObject = NULL;
SceneObject* selectedObject = NULL;

bool insideTriangle(vec3 p, ObjectFace* face);

GLuint loadShader(GLenum type, string file);
GLuint loadShaderProgram(string vertexShaderPath, string fragmentShaderPath);
unsigned char* loadImage(string file_name, int* x, int* y);

void printVector(string label, vec3 v);

float NEAR_CLIP = 0.1f;
float FAR_CLIP = 100.0f;

void glfw_error_callback(int error, const char* description) {
   gl_log_err("GLFW ERROR: code %i msg: %s\n", error, description);
}

void mouse_button_callback(GLFWwindow* window, int button, int action, int mods) {
   /*
   double mouse_x, mouse_y;
   glfwGetCursorPos(window, &mouse_x, &mouse_y);

   if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS) {
      cout << "Mouse clicked (" << mouse_x << "," << mouse_y << ")" << endl;

      float x = (2.0f*mouse_x) / width - 1.0f;
      float y = 1.0f - (2.0f*mouse_y) / height;
      cout << "x: " << x << ", y: " << y << endl;

      // Since the projection matrix gets applied before the view matrix,
      // treat the initial camera position (aka origin of the ray) as (0, 0, 0)

      // When getting the ray direction, you can use near and fov to get the
      // coordinates

      vec4 ray_clip = vec4(x, y, -1.0f, 1.0f); // this should have a z equal to the near clipping plane
      vec4 ray_eye = inverse(proj_mat) * ray_clip;
      ray_eye = vec4(ray_eye.xy(), -1.0f, 0.0f);
      vec3 ray_world = normalize((inverse(view_mat) * ray_eye).xyz());

      / * LATEST NOTES:
       *
       * Normalizing the world ray caused issues, although it should make sense with the projection
       * matrix, since the z coordinate has meaning there.
       *
       * Now, I need to figure out the correct intersection test in 2D space
       * Also, need to check that the global triangle points are correct
       * /

      // since ray_world is the end result we want anyway, we probably don't need to add cam_pos to
      // it, only to subtract it later

      vec3 click_point = cam_pos + ray_world;

      / * Now, we need to generate the constants for the equations describing
       * a 3D line:
       *   (x - x0) / a = (y - y0) / b = (z - z0) / c
       *
       * The line goes through the camera position, so
       * cam_pos = <x0, y0, z0>
       * /

      // upper right corner is 1, 1 in opengl

      cout << "Converted -> (" << ray_world.x << "," << ray_world.y << "," << ray_world.z << ")" << endl << endl;;
      cout << "Camera -> (" << cam_pos.x << "," << cam_pos.y << "," << cam_pos.z << ")" << endl;
      cout << "Click point -> (" << click_point.x << "," << click_point.y << "," << click_point.z << ")" << endl;

      float a = 1.0f;
      float b = a * (click_point.y - cam_pos.y) / (click_point.x - cam_pos.x);
      float c = a * (click_point.z - cam_pos.z) / (click_point.x - cam_pos.x);

      cout << "(x - " << cam_pos.x << ") / " << a << " = ";
      cout << "(y - " << cam_pos.y << ") / " << b << " = ";
      cout << "(z - " << cam_pos.z << ") / " << c << endl;;

      / * Now, we need to generate the constants for the equations describing
       * a 3D plane:
       * dx + ey +fz +g = 0
       * /

      vec3 fp1 = triangle_face[0];
      vec3 fp2 = triangle_face[1];
      vec3 fp3 = triangle_face[2];

      cout << "Points on the plane" << endl;
      cout << "(" << fp1.x << ", " << fp1.y << ", " << fp1.z << ")" << endl;
      cout << "(" << fp2.x << ", " << fp2.y << ", " << fp2.z << ")" << endl;
      cout << "(" << fp3.x << ", " << fp3.y << ", " << fp3.z << ")" << endl;

      float pa = (fp2.y-fp1.y)*(fp3.z-fp1.z) - (fp3.y-fp1.y)*(fp2.z-fp1.z);
      float pb = (fp2.z-fp1.z)*(fp3.x-fp1.x) - (fp3.z-fp1.z)*(fp2.x-fp1.x);
      float pc = (fp2.x-fp1.x)*(fp3.y-fp1.y) - (fp3.x-fp1.x)*(fp2.y-fp1.y);
      float pd = -(pa*fp1.x+pb*fp1.y+pc*fp1.z);

      cout << pa << "x+" << pb << "y+" << pc << "z+" << pd << "=0" << endl;

      // get intersection

      // the intersection this computes is incorrect
      // it doesn't match the equation of the plane
      vec3 i;
      i.z = -cam_pos.z - pc*pd/(pa*a+pb*b);
      i.x = cam_pos.x + a * (i.z-cam_pos.z) / c;
      i.y = cam_pos.y + b * (i.z-cam_pos.z) / c;

      cout << "The holy grail?" << endl;
      cout << "(" << i.x << "," << i.y << "," << i.z << ")" << endl;

      clicked = insideTriangle(i, triangle_face);
      cout << (clicked ? "true" : "false")  << endl;
   }
   */
}

/* REFACTORING PLAN:
 *
 * Have an array of object structs
 * Each object struct has:
 *    -a model matrix
 *    -a selected boolean
 * Eventually, maybe also want to store a reference to the correct shader
 * or whatever other info I need to properly render it
 *
 * Have an array of face structs
 * Each face struct has
 *    -an object index indicating which object it is a part of
 *    -an array of three points
 *
 * The mouse button callback will:
 *    -Set all selected flags in the objects array to false
 *    -iterate through the faces array
 *    -For each face, it will call faceClicked() with the following params:
 *       -Probably a world ray created from the mouse click coordinates
 *       -An array of 3 points representing the face
 *       -The object struct represnting the object the face is a part of
 *
 *       -Really, all I need to pass in are the world ray and an ObjectFace reference
 *       -The world ray will first need to be multiplied by the view and projection matrices before being passed in
 */

void mouse_button_callback_new(GLFWwindow* window, int button, int action, int mods) {
   double mouse_x, mouse_y;
   glfwGetCursorPos(window, &mouse_x, &mouse_y);

   ObjectFace* face = &faces[0];
   SceneObject* obj = &objects[face->objectId];

   if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS) {
      cout << "Mouse clicked (" << mouse_x << "," << mouse_y << ")" << endl;
      selectedObject = NULL;

      float x = (2.0f*mouse_x) / width - 1.0f;
      float y = 1.0f - (2.0f*mouse_y) / height;

      cout << "x: " << x << ", y: " << y << endl;

      // Since the projection matrix gets applied before the view matrix,
      // treat the initial camera position (aka origin of the ray) as (0, 0, 0)

      // When getting the ray direction, you can use near and fov to get the
      // coordinates

      // vec4 ray_clip = vec4(x, y, -1.0f, 1.0f); // this should have a z equal to the near clipping plane
      // vec4 ray_eye = inverse(proj_mat) * ray_clip;
      // ray_eye = vec4(ray_eye.xy(), -1.0f, 0.0f);
      // vec3 ray_world = normalize((inverse(view_mat) * ray_eye).xyz());

      vec4 ray_clip = vec4(x, y, NEAR_CLIP, 1.0f); // this should have a z equal to the near clipping plane
      vec4 ray_eye = ray_clip;
      vec3 ray_world = (inverse(obj->model_mat) * inverse(view_mat) * ray_eye).xyz();

      /* LATEST NOTES:
       *
       * Normalizing the world ray caused issues, although it should make sense with the projection
       * matrix, since the z coordinate has meaning there.
       * Plus, we really want to normalize it only once we recompute it below as the difference of two points,
       * although doing so shouldn't effect the results. Check the book to see if there is a good reason for doing so.
       */

      printVector("Initial world ray:", ray_world);

      vec4 cam_pos_origin = vec4(x, y, 0.0f, 1.0f);
      vec3 cam_pos_temp = (inverse(obj->model_mat) * inverse(view_mat) * cam_pos_origin).xyz();

      ray_world = ray_world-cam_pos_temp;

      cout << "Ray clip -> (" << ray_clip.x << "," << ray_clip.y << "," << ray_clip.z << ")" << endl << endl;;
      cout << "Ray world -> (" << ray_world.x << "," << ray_world.y << "," << ray_world.z << ")" << endl << endl;;
      cout << "Camera -> (" << cam_pos_temp.x << "," << cam_pos_temp.y << "," << cam_pos_temp.z << ")" << endl;

      vec3 fp1 = face->points[0];
      vec3 fp2 = face->points[1];
      vec3 fp3 = face->points[2];

      cout << "Points on the plane" << endl;
      cout << "(" << fp1.x << ", " << fp1.y << ", " << fp1.z << ")" << endl;
      cout << "(" << fp2.x << ", " << fp2.y << ", " << fp2.z << ")" << endl;
      cout << "(" << fp3.x << ", " << fp3.y << ", " << fp3.z << ")" << endl;

      // LINE EQUATION:		P = O + Dt
      // O = cam_pos
      // D = ray_world

      // PLANE EQUATION:	P dot n + d = 0  (n is the normal vector and d is the offset from the origin)

      // Take the cross-product of two vectors on the plane to get the normal
      vec3 v1 = fp2 - fp1;
      vec3 v2 = fp3 - fp1;

      vec3 normal = vec3(v1.y*v2.z-v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x);
      printVector("v1", v1);
      printVector("v2", v2);
      printVector("Cross", normal);
      cout << "Test theory: " << glm::dot(cam_pos_temp, normal) << endl;
      cout << "Test 2: " << glm::dot(ray_world, normal) << endl;

      float d = -glm::dot(fp1, normal);
      cout << "d: " << d << endl;

      float t = - (glm::dot(cam_pos_temp, normal) + d) / glm::dot(ray_world, normal);
      cout << "t: " << t << endl;

      vec3 intersection = cam_pos_temp+t*ray_world;
      printVector("Intersection", intersection);

      if (insideTriangle(intersection, face)) {
         clickedObject = obj;
      }
      cout << (obj == clickedObject ? "true" : "false") << endl;
   }
}

int main(int argc, char* argv[]) {
   cout << "New OpenGL Game" << endl;

   if (!restart_gl_log()) {}
   gl_log("starting GLFW\n%s\n", glfwGetVersionString());

   glfwSetErrorCallback(glfw_error_callback);
   if (!glfwInit()) {
      fprintf(stderr, "ERROR: could not start GLFW3\n");
      return 1;
   }

#ifdef __APPLE__
   glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
   glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
   glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
   glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
#endif

   glfwWindowHint(GLFW_SAMPLES, 4);

   GLFWwindow* window = NULL;

   if (FULLSCREEN) {
      GLFWmonitor* mon = glfwGetPrimaryMonitor();
      const GLFWvidmode* vmode = glfwGetVideoMode(mon);

      cout << "Fullscreen resolution " << vmode->width << "x" << vmode->height << endl;
      window = glfwCreateWindow(vmode->width, vmode->height, "Extended GL Init", mon, NULL);

      width = vmode->width;
      height = vmode->height;
   } else {
      window = glfwCreateWindow(width, height, "Hello Triangle", NULL, NULL);
   }

   if (!window) {
      fprintf(stderr, "ERROR: could not open window with GLFW3\n");
      glfwTerminate();
      return 1;
   }

   bool squareSelected = false;

   glfwSetMouseButtonCallback(window, mouse_button_callback_new);

   glfwMakeContextCurrent(window);
   glewExperimental = GL_TRUE;
   glewInit();

   // Check if we might ever need this. If not, remove it.
   // glViewport(0, 0, width*2, height*2);

   const GLubyte* renderer = glGetString(GL_RENDERER);
   const GLubyte* version = glGetString(GL_VERSION);
   printf("Renderer: %s\n", renderer);
   printf("OpenGL version supported %s\n", version);

   glEnable(GL_DEPTH_TEST);
   glDepthFunc(GL_LESS);

   glEnable(GL_CULL_FACE);
   // glCullFace(GL_BACK);
   // glFrontFace(GL_CW);

   int x, y;
   unsigned char* texImage = loadImage("test.png", &x, &y);
   if (texImage) {
     cout << "Yay, I loaded an image!" << endl;
     cout << x << endl;
     cout << y << endl;
     printf ("first 4 bytes are: %i %i %i %i\n", texImage[0], texImage[1], texImage[2], texImage[3]);
   }

   GLuint tex = 0;
   glGenTextures(1, &tex);
   glActiveTexture(GL_TEXTURE0);
   glBindTexture(GL_TEXTURE_2D, tex);
   glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, x, y, 0, GL_RGBA, GL_UNSIGNED_BYTE, texImage);

   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
   glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
   glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);

   GLfloat points[] = {
       0.0f,  0.5f,  0.0f,
      -0.5f, -0.5f,  0.0f,
       0.5f, -0.5f,  0.0f,
       0.5f, -0.5f,  0.0f,
      -0.5f, -0.5f,  0.0f,
       0.0f,  0.5f,  0.0f,
   };

   GLfloat colors[] = {
      1.0, 0.0, 0.0,
      0.0, 0.0, 1.0,
      0.0, 1.0, 0.0,
      0.0, 1.0, 0.0,
      0.0, 0.0, 1.0,
      1.0, 0.0, 0.0,
   };

   GLfloat colors_new[] = {
      0.0, 1.0, 0.0,
      0.0, 1.0, 0.0,
      0.0, 1.0, 0.0,
      0.0, 1.0, 0.0,
      0.0, 1.0, 0.0,
      0.0, 1.0, 0.0,
   };

   // Each point is made of 3 floats
   int numPoints = (sizeof(points) / sizeof(float)) / 3;

   GLfloat points2[] = {
      0.5f,  0.5f,  0.0f,
      -0.5f,  0.5f,  0.0f,
      -0.5f, -0.5f,  0.0f,
      0.5f,  0.5f,  0.0f,
      -0.5f, -0.5f,  0.0f,
      0.5f, -0.5f,  0.0f,
   };

   GLfloat colors2[] = {
      0.0, 0.9, 0.9,
      0.0, 0.9, 0.9,
      0.0, 0.9, 0.9,
      0.0, 0.9, 0.9,
      0.0, 0.9, 0.9,
      0.0, 0.9, 0.9,
   };

   GLfloat texcoords[] = {
      1.0f, 1.0f,
      0.0f, 1.0f,
      0.0, 0.0,
      1.0, 1.0,
      0.0, 0.0,
      1.0, 0.0
   };

   // Each point is made of 3 floats
   int numPoints2 = (sizeof(points2) / sizeof(float)) / 3;

   // initialize global variables for click intersection tests

   mat4 T_model, R_model;

   // triangle
   objects.push_back(SceneObject());

   /*
   mat4 R_model = rotate(mat4(), 4.0f, vec3(0.0f, 1.0f, 0.0f));
   */
   T_model = translate(mat4(), vec3(0.5f, 0.0f, 0.0f));
   R_model = rotate(mat4(), 0.0f, vec3(0.0f, 1.0f, 0.0f));
   objects[0].model_mat = T_model*R_model;

   faces.push_back(ObjectFace());
   faces[0].objectId = 0;
   faces[0].points = {
      vec3(points[0], points[1], points[2]),
      vec3(points[3], points[4], points[5]),
      vec3(points[6], points[7], points[8]),
   };

   // square
   objects.push_back(SceneObject());

   // mat4 T_model2 = translate(mat4(), vec3(-1.0f, 0.0f, 0.0f));
   T_model = translate(mat4(), vec3(-0.5f, 0.0f, 0.0f));
   R_model = rotate(mat4(), 0.0f, vec3(0.0f, 1.0f, 0.0f));
   objects[1].model_mat = T_model*R_model;

   faces.push_back(ObjectFace());
   faces[1].objectId = 1;
   faces[1].points = {
      vec3(points2[0], points2[1], points2[2]),
      vec3(points2[3], points2[4], points2[5]),
      vec3(points2[6], points2[7], points2[8]),
   };

   faces.push_back(ObjectFace());
   faces[2].objectId = 1;
   faces[2].points = {
      vec3(points2[9], points2[10], points2[11]),
      vec3(points2[12], points2[13], points2[14]),
      vec3(points2[15], points2[16], points2[17]),
   };

   GLuint points_vbo = 0;
   glGenBuffers(1, &points_vbo);
   glBindBuffer(GL_ARRAY_BUFFER, points_vbo);
   glBufferData(GL_ARRAY_BUFFER, sizeof(points), points, GL_STATIC_DRAW);

   GLuint colors_vbo = 0;
   glGenBuffers(1, &colors_vbo);
   glBindBuffer(GL_ARRAY_BUFFER, colors_vbo);
   glBufferData(GL_ARRAY_BUFFER, sizeof(colors), colors, GL_STATIC_DRAW);

   GLuint vao = 0;
   glGenVertexArrays(1, &vao);
   glBindVertexArray(vao);
   glBindBuffer(GL_ARRAY_BUFFER, points_vbo);
   glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, NULL);
   glBindBuffer(GL_ARRAY_BUFFER, colors_vbo);
   glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, NULL);

   glEnableVertexAttribArray(0);
   glEnableVertexAttribArray(1);

   GLuint points2_vbo = 0;
   glGenBuffers(1, &points2_vbo);
   glBindBuffer(GL_ARRAY_BUFFER, points2_vbo);
   glBufferData(GL_ARRAY_BUFFER, sizeof(points2), points2, GL_STATIC_DRAW);

   GLuint colors2_vbo = 0;
   glGenBuffers(1, &colors2_vbo);
   glBindBuffer(GL_ARRAY_BUFFER, colors2_vbo);
   glBufferData(GL_ARRAY_BUFFER, sizeof(colors2), colors2, GL_STATIC_DRAW);

   GLuint vt_vbo;
   glGenBuffers(1, &vt_vbo);
   glBindBuffer(GL_ARRAY_BUFFER, vt_vbo);
   glBufferData(GL_ARRAY_BUFFER, sizeof(texcoords), texcoords, GL_STATIC_DRAW);

   GLuint vao2 = 0;
   glGenVertexArrays(1, &vao2);
   glBindVertexArray(vao2);
   glBindBuffer(GL_ARRAY_BUFFER, points2_vbo);
   glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, NULL);
   // glBindBuffer(GL_ARRAY_BUFFER, colors2_vbo);
   // glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, NULL);
   glBindBuffer(GL_ARRAY_BUFFER, vt_vbo);
   glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 0, NULL);

   glEnableVertexAttribArray(0);
   glEnableVertexAttribArray(1);

   GLuint shader_program = loadShaderProgram("./color.vert", "./color.frag");
   GLuint shader_program2 = loadShaderProgram("./texture.vert", "./texture.frag");

   float speed = 1.0f;
   float last_position = 0.0f;

   float cam_speed = 1.0f;
   float cam_yaw_speed = 60.0f*ONE_DEG_IN_RAD;

   //cam_pos = vec3(0.0f, 0.0f, 2.0f);
   cam_pos = vec3(0.0f, 0.0f, 0.3f);
   float cam_yaw = 0.0f * 2.0f * 3.14159f / 360.0f;

   mat4 T = translate(mat4(), vec3(-cam_pos.x, -cam_pos.y, -cam_pos.z));
   mat4 R = rotate(mat4(), -cam_yaw, vec3(0.0f, 1.0f, 0.0f));
   /*
   mat4 T = translate(mat4(), vec3(0.0f, 0.0f, 0.0f));
   mat4 R = rotate(mat4(), 0.0f, vec3(0.0f, 1.0f, 0.0f));
   */
   view_mat = R*T;

   float fov = 67.0f * ONE_DEG_IN_RAD;
   float aspect = (float)width / (float)height;

   float range = tan(fov * 0.5f) * NEAR_CLIP;
   float Sx = NEAR_CLIP / (range * aspect);
   float Sy = NEAR_CLIP / range;
   float Sz = -(FAR_CLIP + NEAR_CLIP) / (FAR_CLIP - NEAR_CLIP);
   float Pz = -(2.0f * FAR_CLIP * NEAR_CLIP) / (FAR_CLIP - NEAR_CLIP);

   /*
   float proj_arr[] = {
     Sx, 0.0f, 0.0f, 0.0f,
     0.0f, Sy, 0.0f, 0.0f,
     0.0f, 0.0f, Sz, -1.0f,
     0.0f, 0.0f, Pz, 0.0f,
   };
   */
   float proj_arr[] = {
     1.0f, 0.0f, 0.0f, 0.0f,
     0.0f, 1.0f, 0.0f, 0.0f,
     0.0f, 0.0f, 1.0f, 0.0f,
     0.0f, 0.0f, 0.0f, 1.0f,
   };
   proj_mat = make_mat4(proj_arr);

   GLint model_test_loc = glGetUniformLocation(shader_program, "model");
   GLint view_test_loc = glGetUniformLocation(shader_program, "view");
   GLint proj_test_loc = glGetUniformLocation(shader_program, "proj");

   GLint model_mat_loc = glGetUniformLocation(shader_program2, "model");
   GLint view_mat_loc = glGetUniformLocation(shader_program2, "view");
   GLint proj_mat_loc = glGetUniformLocation(shader_program2, "proj");

   glUseProgram(shader_program);
   glUniformMatrix4fv(model_test_loc, 1, GL_FALSE, value_ptr(objects[0].model_mat));
   glUniformMatrix4fv(view_test_loc, 1, GL_FALSE, value_ptr(view_mat));
   glUniformMatrix4fv(proj_test_loc, 1, GL_FALSE, value_ptr(proj_mat));

   glUseProgram(shader_program2);
   glUniformMatrix4fv(model_mat_loc, 1, GL_FALSE, value_ptr(objects[1].model_mat));
   glUniformMatrix4fv(view_mat_loc, 1, GL_FALSE, value_ptr(view_mat));
   glUniformMatrix4fv(proj_mat_loc, 1, GL_FALSE, value_ptr(proj_mat));

   bool cam_moved = false;

   double previous_seconds = glfwGetTime();
   while (!glfwWindowShouldClose(window)) {
      double current_seconds = glfwGetTime();
      double elapsed_seconds = current_seconds - previous_seconds;
      previous_seconds = current_seconds;

      if (fabs(last_position) > 1.0f) {
         speed = -speed;
      }

      if (clickedObject == &objects[0]) {
         selectedObject = &objects[0];
      }

      // At some point, I should change this to only rebind the buffer once per click, not once per frame
      glBindBuffer(GL_ARRAY_BUFFER, colors_vbo);
      if (selectedObject == &objects[0]) {
         glBufferData(GL_ARRAY_BUFFER, sizeof(colors), colors_new, GL_STATIC_DRAW);
      }
      else {
         glBufferData(GL_ARRAY_BUFFER, sizeof(colors), colors, GL_STATIC_DRAW);
      }

      /*
      model[12] = last_position + speed*elapsed_seconds;
      last_position = model[12];
      */

      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

      glUseProgram(shader_program);

      // this is temporary.
      // It's needed to offset the code for the recoloring of the square working during click detection
      glUniformMatrix4fv(model_test_loc, 1, GL_FALSE, value_ptr(objects[0].model_mat));

      glBindVertexArray(vao);

      glDrawArrays(GL_TRIANGLES, 0, numPoints);

      if (clickedObject == &objects[1]) {
         squareSelected = !squareSelected;
         selectedObject = &objects[1];
      }

      if (selectedObject == &objects[1]) {
         glUseProgram(shader_program);

         // this is temporary.
         // It's needed to get the recoloring of the square working during click detection
         glUniformMatrix4fv(model_test_loc, 1, GL_FALSE, value_ptr(objects[1].model_mat));

         glBindVertexArray(vao2);

         glBindBuffer(GL_ARRAY_BUFFER, colors2_vbo);
         glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, NULL);
      } else {
         glUseProgram(shader_program2);

         glBindVertexArray(vao2);

         glBindBuffer(GL_ARRAY_BUFFER, vt_vbo);
         glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 0, NULL);
      }

      glDrawArrays(GL_TRIANGLES, 0, numPoints2);

      clickedObject = NULL;

      glfwPollEvents();
      glfwSwapBuffers(window);

      if (GLFW_PRESS == glfwGetKey(window, GLFW_KEY_ESCAPE)) {
         glfwSetWindowShouldClose(window, 1);
      }

      float dist = cam_speed * elapsed_seconds;
      if (glfwGetKey(window, GLFW_KEY_A)) {
         cam_pos.x -= cos(cam_yaw)*dist;
         cam_pos.z += sin(cam_yaw)*dist;
         cam_moved = true;
      }
      if (glfwGetKey(window, GLFW_KEY_D)) {
         cam_pos.x += cos(cam_yaw)*dist;
         cam_pos.z -= sin(cam_yaw)*dist;
         cam_moved = true;
      }
      if (glfwGetKey(window, GLFW_KEY_W)) {
         cam_pos.x -= sin(cam_yaw)*dist;
         cam_pos.z -= cos(cam_yaw)*dist;
         cam_moved = true;
      }
      if (glfwGetKey(window, GLFW_KEY_S)) {
         cam_pos.x += sin(cam_yaw)*dist;
         cam_pos.z += cos(cam_yaw)*dist;
         cam_moved = true;
      }
      if (glfwGetKey(window, GLFW_KEY_LEFT)) {
         cam_yaw += cam_yaw_speed * elapsed_seconds;
         cam_moved = true;
      }
      if (glfwGetKey(window, GLFW_KEY_RIGHT)) {
         cam_yaw -= cam_yaw_speed * elapsed_seconds;
         cam_moved = true;
      }
      if (cam_moved) {
         T = translate(mat4(), vec3(-cam_pos.x, -cam_pos.y, -cam_pos.z));
         R = rotate(mat4(), -cam_yaw, vec3(0.0f, 1.0f, 0.0f));
         // view_mat = R*T;

         glUniformMatrix4fv(view_mat_loc, 1, GL_FALSE, value_ptr(view_mat));
         cam_moved = false;
      }
   }

   glfwTerminate();
   return 0;
}

GLuint loadShader(GLenum type, string file) {
  cout << "Loading shader from file " << file << endl;

  ifstream shaderFile(file);
  GLuint shaderId = 0;

  if (shaderFile.is_open()) {
    string line, shaderString;

    while(getline(shaderFile, line)) {
      shaderString += line + "\n";
    }
    shaderFile.close();
    const char* shaderCString = shaderString.c_str();

    shaderId = glCreateShader(type);
    glShaderSource(shaderId, 1, &shaderCString, NULL);
    glCompileShader(shaderId);

    cout << "Loaded successfully" << endl;
  } else {
    cout << "Failed to loade the file" << endl;
  }

  return shaderId;
}

GLuint loadShaderProgram(string vertexShaderPath, string fragmentShaderPath) {
   GLuint vs = loadShader(GL_VERTEX_SHADER, vertexShaderPath);
   GLuint fs = loadShader(GL_FRAGMENT_SHADER, fragmentShaderPath);

   GLuint shader_program = glCreateProgram();
   glAttachShader(shader_program, vs);
   glAttachShader(shader_program, fs);

   glLinkProgram(shader_program);

   return shader_program;
}

unsigned char* loadImage(string file_name, int* x, int* y) {
  int n;
  int force_channels = 4;
  unsigned char* image_data = stbi_load(file_name.c_str(), x, y, &n, force_channels);
  if (!image_data) {
    fprintf(stderr, "ERROR: could not load %s\n", file_name.c_str());
  }
  return image_data;
}

bool insideTriangle(vec3 p, ObjectFace* face) {
   vec3 v21 = face->points[1]- face->points[0];
   vec3 v31 = face->points[2]- face->points[0];
   vec3 pv1 = p- face->points[0];

   float y = (pv1.y*v21.x - pv1.x*v21.y) / (v31.y*v21.x - v31.x*v21.y);
   float x = (pv1.x-y*v31.x) / v21.x;

   cout << "(" << x << ", " << y << ")" << endl;

   return x > 0.0f && y > 0.0f && x+y < 1.0f;
}

void printVector(string label, vec3 v) {
   cout << label << " -> (" << v.x << "," << v.y << "," << v.z << ")" << endl;
}