#include <iostream>
#include <vector>

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

#define GLM_FORCE_RADIANS
#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtx/quaternion.hpp>

#include "Camera.h"
#include "GLSL.h"
#include "Program.h"
#include "MatrixStack.h"
#include "Shape.h"
#include "Component.h"
#include "Keyframe.h"

using namespace std;

bool keyToggles[256] = {false}; // only for English keyboards!

GLFWwindow *window; // Main application window
string RESOURCE_DIR = ""; // Where the resources are loaded from

shared_ptr<Program> progNormal;
shared_ptr<Program> progSimple;
shared_ptr<Camera> camera;
shared_ptr<Shape> bunny;
shared_ptr<Shape> heli_body1;
shared_ptr<Shape> heli_body2;
shared_ptr<Shape> heli_prop1;
shared_ptr<Shape> heli_prop2;

// As used in my A2, this can be used as a root
// component for objects with multiple children.
// I have redefined it to handle children with
// multiple shape objects as well.
Component root;

// Our global time variable
float t = glfwGetTime();

// I thought tpl "Time Per Loop" would be an appropriate name for a global
// defining tmax.
float tpl = 10.0f;

// Needed for converting between s and u
vector<pair<float, float> > usTable;

// Vector to hold all keyframes
vector<Keyframe> keyframes; 

static void error_callback(int error, const char *description)
{
	cerr << description << endl;
}

static void key_callback(GLFWwindow *window, int key, int scancode, int action, int mods)
{
	if(key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) {
		glfwSetWindowShouldClose(window, GL_TRUE);
	}
}

static void char_callback(GLFWwindow *window, unsigned int key)
{
	keyToggles[key] = !keyToggles[key];
}

static void cursor_position_callback(GLFWwindow* window, double xmouse, double ymouse)
{
	int state = glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT);
	if(state == GLFW_PRESS) {
		camera->mouseMoved(xmouse, ymouse);
	}
}

void mouse_button_callback(GLFWwindow* window, int button, int action, int mods)
{
	// Get the current mouse position.
	double xmouse, ymouse;
	glfwGetCursorPos(window, &xmouse, &ymouse);
	// Get current window size.
	int width, height;
	glfwGetWindowSize(window, &width, &height);
	if(action == GLFW_PRESS) {
		bool shift = mods & GLFW_MOD_SHIFT;
		bool ctrl  = mods & GLFW_MOD_CONTROL;
		bool alt   = mods & GLFW_MOD_ALT;
		camera->mouseClicked(xmouse, ymouse, shift, ctrl, alt);
	}
}

// Used for converting our global t to s
float t2s(float t)
{
  float tmax = tpl;
  float smax = usTable[usTable.size() - 1].second;
  float tNorm = std::fmod(t, tmax) / tmax;
  float sNorm = tNorm;
  float s = smax * sNorm;
  return s;
}

// Used as in L13 to build the usTable
// Modified to accept the looping keyframes
void buildTable()
{
	usTable.clear();
  int ncps = (int) keyframes.size();
  
  // Initialize B.
  glm::mat4 B;
  
  // Fill in B.
  B[0] = glm::vec4(0.0f, 2.0f, 0.0f, 0.0f);
  B[1] = glm::vec4(-1.0f, 0.0f, 1.0f, 0.0f);
  B[2] = glm::vec4(2.0f, -5.0f, 4.0f, -1.0f);
  B[3] = glm::vec4(-1.0f, 3.0f, -3.0f, 1.0f);
  B = 0.5f*B;

  glm::mat4 G;
  
  for(int c = 0; c < ncps - 1; c++)
  {

    if(c == 0)
    {
      G[0] = glm::vec4(keyframes[ncps - 2].getPos(), 0.0f);
      G[1] = glm::vec4(keyframes[c].getPos(), 0.0f);
      G[2] = glm::vec4(keyframes[c + 1].getPos(), 0.0f);
      G[3] = glm::vec4(keyframes[c + 2].getPos(), 0.0f);
    }
    else if(c == ncps - 3)
    {
      G[0] = glm::vec4(keyframes[c - 1].getPos(), 0.0f);
      G[1] = glm::vec4(keyframes[c].getPos(), 0.0f);
      G[2] = glm::vec4(keyframes[c + 1].getPos(), 0.0f);
      G[3] = glm::vec4(keyframes[0].getPos(), 0.0f);
    }
    else if(c == ncps - 2)
    {
      G[0] = glm::vec4(keyframes[c - 1].getPos(), 0.0f);
      G[1] = glm::vec4(keyframes[c].getPos(), 0.0f);
      G[2] = glm::vec4(keyframes[0].getPos(), 0.0f);
      G[3] = glm::vec4(keyframes[1].getPos(), 0.0f);
    }
    else if(c == ncps - 1)
    {
      G[0] = glm::vec4(keyframes[c - 1].getPos(), 0.0f);
      G[0] = glm::vec4(keyframes[c].getPos(), 0.0f);
      G[1] = glm::vec4(keyframes[0].getPos(), 0.0f);
      G[2] = glm::vec4(keyframes[1].getPos(), 0.0f);
      G[3] = glm::vec4(keyframes[2].getPos(), 0.0f);
    }
    else
    {
      G[0] = glm::vec4(keyframes[c - 1].getPos(), 0.0f);
      G[1] = glm::vec4(keyframes[c].getPos(), 0.0f);
      G[2] = glm::vec4(keyframes[c + 1].getPos(), 0.0f);
      G[3] = glm::vec4(keyframes[c + 2].getPos(), 0.0f);
    }

    for(int i = 0; i < 5; i++)
    {
      float ua = (float) i*0.2f;
      float ub = (float) (i + 1.0f)*0.2f;

      float sl = (ub - ua)/2.0f;
      float sr = 0;

      float xj;
      float wj;
    

      for(int j = 0; j < 3; j++)
      {
        switch(j)
        {
          case 0:
            xj = -1*sqrt(3.0f/5.0f);
            wj = 5.0f/9.0f;
            break;
          case 1:
            xj = 0;
            wj = 8.0f/9.0f;
            break;
          case 2:
            xj = sqrt(3.0f/5.0f);
            wj = 5.0f/9.0f;
            break;
          default:
            xj = 0;
            wj = 0;
            break;
        }

        float up = ((((ub - ua)*xj)/2.0f) + ((ua + ub)/2.0f));
      
        glm::vec4 uVecPrime(0.0f, 1.0f, 2.0f*up, 3.0f*up*up);
      
        glm::vec4 Pp = G*(B*uVecPrime);
      
        sr += (wj*(glm::length(Pp)));
      }
      
      if(usTable.size() == 0)
      {
        usTable.push_back(make_pair(0.0f, 0.0f));
      }
      else
      {
        float s = usTable[usTable.size() - 1].second + sl * sr;

        usTable.push_back(make_pair(c + ub, s));
      }
    }
  }
}

// Pretty much exactly the same as L13
float s2u(float s)
{
	// INSERT CODE HERE

  float s0 = 0.0f;
  float s1 = 0.0f;

  float u0 = 0.0f;
  float u1 = 0.0f;

  if(usTable.size() < 2)
  {
    return 0.0f;
  }

  for(unsigned int i = 1; i < usTable.size(); i++)
  {
    if(usTable[i].second > s)
    {
      s1 = usTable[i].second;
      s0 = usTable[i - 1].second;
      u1 = usTable[i].first;
      u0 = usTable[i - 1].first;
      break;
    }
  }

  if(s1 == 0.0f)
  {
    return 0.0f;
  }

  float alpha = (s - s0)/(s1 - s0);

  float u = (1.0f - alpha)*u0 + alpha*u1;

	return u;
}

// A helper function used in A2 to
// ensure children are assigned parents
// properly and visa-versa in component trees
static void setParents(Component& elem)
{
  for (unsigned int i = 0; i < elem.children.size(); ++i)
  {
    setParents(elem.children[i]);
    elem.children[i].parent = &elem;
  }
}

// Function that creates the helicopter
// with all shapes joined together appropriately
void createHeli(Component& body)
{
  root.shape = heli_body1;

  Component lbody;
  lbody.parent = &body;

  lbody.tp.y = 0.0f;

  lbody.s.x = 1.0;
  lbody.s.y = 1.0;
  lbody.s.z = 1.0;

  body.children.push_back(lbody);

  Component prop1;
  prop1.parent = &body;

  prop1.s.x = 1.0;
  prop1.s.y = 1.0;
  prop1.s.z = 1.0;

  body.children.push_back(prop1);

  Component prop2;
  prop2.parent = &body;
  
  prop2.s.x = 1.0;
  prop2.s.y = 1.0;
  prop2.s.z = 1.0;

  body.children.push_back(prop2);
 
  body.children[1].center = glm::vec3(0.0f, 0.4819f, 0.0f);
  body.children[2].center = glm::vec3(0.6228f, 0.1179f, 0.1365f);
  
  body.children[0].shape = heli_body2;
  body.children[1].shape = heli_prop1;
  body.children[2].shape = heli_prop2;

  Material m;

  m.setMaterial(glm::vec3(0.9f, 0.0f, 0.0f), glm::vec3(0.5f, 0.3f, 0.3f), glm::vec3(0.6f, 0.3f, 0.3f), 200.0f);
  body.material = m;
  
  m.setMaterial(glm::vec3(0.7f, 0.7f, 0.1f), glm::vec3(0.5f, 0.5f, 0.3f), glm::vec3(0.5f, 0.5f, 0.3f), 200.0f);
  body.children[0].material = m;

  m.setMaterial(glm::vec3(0.3f, 0.3f, 0.3f), glm::vec3(0.3f, 0.3f, 0.4f), glm::vec3(0.5f, 0.5f, 0.5f), 200.0f);
  body.children[1].material = m;

  m.setMaterial(glm::vec3(0.3f, 0.3f, 0.3f), glm::vec3(0.3f, 0.3f, 0.4f), glm::vec3(0.5f, 0.5f, 0.5f), 200.0f);
  body.children[2].material = m;

  float alpha = fmod(t, M_PI);

  body.children[1].r.y = alpha;
  
  setParents(root);
}

// Used for checking for "twists"
void checkNegate(int i)
{
  float tempquat;
  if(i > 0)
  {
    tempquat = glm::dot(glm::vec4(keyframes[i - 1].getQuat().x, keyframes[i - 1].getQuat().y, keyframes[i - 1].getQuat().z, keyframes[i - 1].getQuat().w),
                        glm::vec4(keyframes[i].getQuat().x, keyframes[i].getQuat().y, keyframes[i].getQuat().z, keyframes[i].getQuat().w));
  }
  else
  {
    int temp = (int) keyframes.size() - 1;
    tempquat = glm::dot(glm::vec4(keyframes[temp].getQuat().x, keyframes[temp].getQuat().y, keyframes[temp].getQuat().z, keyframes[temp].getQuat().w),
                        glm::vec4(keyframes[i].getQuat().x, keyframes[i].getQuat().y, keyframes[i].getQuat().z, keyframes[i].getQuat().w));
  }

  if(tempquat < 0.0f)
  {
    keyframes[i].setQuat(-keyframes[i].getQuat());
  }
}

// Function that creates and assigns keyframes
// to the global keyframes vector
void createKeyframes()
{	
  Keyframe key;

  for(int t = 0; t < 12; t++)
  {
    keyframes.push_back(key);
  }

	keyframes[0].setPos(glm::vec3(-1.0f, 0.0f, 0.0f));
	keyframes[1].setPos(glm::vec3( 1.0f, 0.0f, 0.0f));
  keyframes[2].setPos(glm::vec3(1.0f, 0.0f, -2.0f));
  keyframes[3].setPos(glm::vec3(-1.0f, 0.0f, -2.0f));
  keyframes[4].setPos(glm::vec3(2.0f, 0.0f, 3.0f));
  keyframes[5].setPos(glm::vec3(-1.0f, 2.3f, -2.0f));
  keyframes[6].setPos(glm::vec3(-1.0f, 0.0f, 3.2f));
  keyframes[7].setPos(glm::vec3(-2.0f, 1.0f, 1.35f));
  keyframes[8].setPos(glm::vec3(-1.0f, 2.0f, -0.5f));
  keyframes[9].setPos(glm::vec3(-0.0f, 1.0f, -2.35f));
  keyframes[10].setPos(glm::vec3(-1.0f, 0.0f, -4.2f));
	keyframes[11].setPos(glm::vec3(-1.0f, 0.0f, 0.0f));

  glm::vec3 xaxis(1.0f, 0.0f, 0.0f);
  glm::vec3 yaxis(0.0f, 1.0f, 0.0f);
  glm::vec3 zaxis(0.0f, 0.0f, 1.0f);

	keyframes[0].setQuat(glm::angleAxis(0.0f, zaxis));
  keyframes[1].setQuat(glm::angleAxis(30.0f, xaxis));
  keyframes[2].setQuat(glm::angleAxis(-90.0f, yaxis));
  keyframes[3].setQuat(glm::angleAxis(0.0f, yaxis));
  keyframes[4].setQuat(glm::angleAxis(90.0f, zaxis));
  keyframes[5].setQuat(glm::angleAxis(0.0f, xaxis));
  keyframes[6].setQuat(glm::angleAxis(0.0f, zaxis));
  keyframes[7].setQuat(glm::angleAxis(-90.0f, zaxis));
  keyframes[8].setQuat(glm::angleAxis(-180.0f, zaxis));
  keyframes[9].setQuat(glm::angleAxis(90.0f, zaxis));
  keyframes[10].setQuat(glm::angleAxis(0.0f, zaxis));
  keyframes[11].setQuat(keyframes[10].getQuat());
}

static void init()
{
	GLSL::checkVersion();
	
	// Set background color
	glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
	// Enable z-buffer test
	glEnable(GL_DEPTH_TEST);
	
	keyToggles[(unsigned)'c'] = true;
	
	// For drawing the bunny
	progNormal = make_shared<Program>();
	progNormal->setShaderNames(RESOURCE_DIR + "vert.glsl", RESOURCE_DIR + "frag.glsl");
	progNormal->setVerbose(true);
	progNormal->init();
	progNormal->addUniform("P");
	progNormal->addUniform("MV");
	progNormal->addAttribute("aPos");
	progNormal->addAttribute("aNor");
  progNormal->addUniform("ka");
 	progNormal->addUniform("kd");
 	progNormal->addUniform("ks");
 	progNormal->addUniform("s");
	progNormal->setVerbose(false);
	
	// For drawing the frames
	progSimple = make_shared<Program>();
	progSimple->setShaderNames(RESOURCE_DIR + "simple_vert.glsl", RESOURCE_DIR + "simple_frag.glsl");
	progSimple->setVerbose(true);
	progSimple->init();
	progSimple->addUniform("P");
	progSimple->addUniform("MV");
	progSimple->setVerbose(false);
	
	bunny = make_shared<Shape>();
	bunny->loadMesh(RESOURCE_DIR + "bunny.obj");
	bunny->init();
	
  heli_body1 = make_shared<Shape>();
	heli_body1->loadMesh(RESOURCE_DIR + "helicopter_body1.obj");
	heli_body1->init();
	
  heli_body2 = make_shared<Shape>();
	heli_body2->loadMesh(RESOURCE_DIR + "helicopter_body2.obj");
	heli_body2->init();
	
  heli_prop1 = make_shared<Shape>();
	heli_prop1->loadMesh(RESOURCE_DIR + "helicopter_prop1.obj");
	heli_prop1->init();
	
  heli_prop2 = make_shared<Shape>();
	heli_prop2->loadMesh(RESOURCE_DIR + "helicopter_prop2.obj");
	heli_prop2->init();
	
  // Connects and sets the parts of the helicopter with
  // root as the main component
  createHeli(root);
  
  // Initializes and sets the Keyframe objects
  // for the Keyframe global vector
  createKeyframes();
  
  // See L13, slightly modified
  buildTable();

  camera = make_shared<Camera>();
	
	// Initialize time.
	glfwSetTime(0.0);
	
	// If there were any OpenGL errors, this will print something.
	// You can intersperse this line in your code to find the exact location
	// of your OpenGL error.
	GLSL::checkError(GET_FILE_LINE);
}

void render()
{
	// Update time.
	t = glfwGetTime();
	
	// Get current frame buffer size.
	int width, height;
	glfwGetFramebufferSize(window, &width, &height);
	glViewport(0, 0, width, height);
	
	// Use the window size for camera.
	glfwGetWindowSize(window, &width, &height);
	camera->setAspect((float)width/(float)height);
	
	// Clear buffers
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	if(keyToggles[(unsigned)'c']) {
		glEnable(GL_CULL_FACE);
	} else {
		glDisable(GL_CULL_FACE);
	}
	if(keyToggles[(unsigned)'l']) {
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	} else {
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	}
	
	auto P = make_shared<MatrixStack>();
	auto MV = make_shared<MatrixStack>();
	
	// Apply camera transforms
	P->pushMatrix();
	camera->applyProjectionMatrix(P);
	MV->pushMatrix();
	camera->applyViewMatrix(MV);
	
	// Draw origin frame
	progSimple->bind();
	glUniformMatrix4fv(progSimple->getUniform("P"), 1, GL_FALSE, glm::value_ptr(P->topMatrix()));
	glUniformMatrix4fv(progSimple->getUniform("MV"), 1, GL_FALSE, glm::value_ptr(MV->topMatrix()));
	glLineWidth(2);
	glBegin(GL_LINES);
	glColor3f(1, 0, 0);
	glVertex3f(0, 0, 0);
	glVertex3f(1, 0, 0);
	glColor3f(0, 1, 0);
	glVertex3f(0, 0, 0);
	glVertex3f(0, 1, 0);
	glColor3f(0, 0, 1);
	glVertex3f(0, 0, 0);
	glVertex3f(0, 0, 1);
	glEnd();
	
  // Draw grid frame
  glLineWidth(1);
  glBegin(GL_LINES);
  glColor3f(0.75, 0.75, 0.75);

  // Drawing z axis lines
  glVertex3f(-25, -10, -25);
  glVertex3f(-25, -10, 25);
  glVertex3f(-20, -10, -25);
  glVertex3f(-20, -10, 25);
  glVertex3f(-15, -10, -25);
  glVertex3f(-15, -10, 25);
  glVertex3f(-10, -10, -25);
  glVertex3f(-10, -10, 25);
  glVertex3f(-5, -10, -25);
  glVertex3f(-5, -10, 25);
  glVertex3f(0, -10, -25);
  glVertex3f(0, -10, 25);
  glVertex3f(25, -10, -25);
  glVertex3f(25, -10, 25);
  glVertex3f(20, -10, -25);
  glVertex3f(20, -10, 25);
  glVertex3f(15, -10, -25);
  glVertex3f(15, -10, 25);
  glVertex3f(10, -10, -25);
  glVertex3f(10, -10, 25);
  glVertex3f(5, -10, -25);
  glVertex3f(5, -10, 25); 
  
  // Drawing x axis lines
  glVertex3f(-25, -10, -25);
  glVertex3f(25, -10, -25);
  glVertex3f(-25, -10, -20);
  glVertex3f(25, -10, -20);
  glVertex3f(-25, -10, -15);
  glVertex3f(25, -10, -15);
  glVertex3f(-25, -10, -10);
  glVertex3f(25, -10, -10);
  glVertex3f(-25, -10, -5);
  glVertex3f(25, -10, -5);
  glVertex3f(-25, -10, 0);
  glVertex3f(25, -10, 0);
  glVertex3f(-25, -10, 5);
  glVertex3f(25, -10, 5);
  glVertex3f(-25, -10, 10);
  glVertex3f(25, -10, 10);
  glVertex3f(-25, -10, 15);
  glVertex3f(25, -10, 15);
  glVertex3f(-25, -10, 20);
  glVertex3f(25, -10, 20);
  glVertex3f(-25, -10, 25);
  glVertex3f(25, -10, 25); 
  
  glEnd();

  // Initialize beginning keyframes.
  glm::vec3 pbegin = keyframes[0].p;
  glm::vec3 pend = keyframes[1].p;
	
  // Alpha is the linear interpolation parameter between 0 and 1
	float alpha = std::fmod(0.5f*t, (float)keyframes.size() - 1);
	
  // Drawing the spline

  // Initialize B.
  glm::mat4 B;

  // Initialize Frenet Frame variables
  glm::vec3 p1;
  glm::vec3 p2;
  glm::vec4 uVec1;
  glm::vec4 uVec2;
  glm::vec3 Tan;
  glm::vec3 Bin;
  glm::vec3 Norm;

  // Provided code for using global time variable t
  // Modified to handle interpolation
  float ncps = keyframes.size();
  float kfloat;
  float uu = std::modf(std::fmod(s2u(t2s(t)), ncps - 1), &kfloat);
  int k = (int)std::floor(kfloat);
  
  // Before we start interpolating, check quaternions.
  for(unsigned int qc = ncps; qc > 0; qc --)
  {
    int temp = ((k - qc) + ((int)ncps - 1))%((int)ncps);
    checkNegate(temp);
  }

  // Initialize variables used for storing
  // quaternions and the resulting matrix
  // after interpolation
  glm::quat q;
  glm::mat4 E;

  // Initialize position vectors
  glm::vec4 p;
  glm::vec4 pp;

  if(keyframes.size() > 3)
  {
    // Fill in B.
    B[0] = glm::vec4(0.0f, 2.0f, 0.0f, 0.0f);
    B[1] = glm::vec4(-1.0f, 0.0f, 1.0f, 0.0f);
    B[2] = glm::vec4(2.0f, -5.0f, 4.0f, -1.0f);
    B[3] = glm::vec4(-1.0f, 3.0f, -3.0f, 1.0f);
    B = 0.5f*B;

    // Initialize G.
    glm::mat4 G;

    for(int i = 0; i < ncps - 1; i++)
    {
      // Fill in G
      if(i == ncps - 4)
      {
        G[0] = glm::vec4(keyframes[i].getPos(), 0.0f);
        G[1] = glm::vec4(keyframes[i + 1].getPos(), 0.0f);
        G[2] = glm::vec4(keyframes[i + 2].getPos(), 0.0f);
        G[3] = glm::vec4(keyframes[0].getPos(), 0.0f);
      }
      else if(i == ncps - 3)
      {
        G[0] = glm::vec4(keyframes[i].getPos(), 0.0f);
        G[1] = glm::vec4(keyframes[i + 1].getPos(), 0.0f);
        G[2] = glm::vec4(keyframes[0].getPos(), 0.0f);
        G[3] = glm::vec4(keyframes[1].getPos(), 0.0f);
      }
      else if(i == ncps - 2)
      {
        G[0] = glm::vec4(keyframes[i].getPos(), 0.0f);
        G[1] = glm::vec4(keyframes[0].getPos(), 0.0f);
        G[2] = glm::vec4(keyframes[1].getPos(), 0.0f);
        G[3] = glm::vec4(keyframes[2].getPos(), 0.0f);
      }
      else
      {
        G[0] = glm::vec4(keyframes[i].getPos(), 0.0f);
        G[1] = glm::vec4(keyframes[i + 1].getPos(), 0.0f);
        G[2] = glm::vec4(keyframes[i + 2].getPos(), 0.0f);
        G[3] = glm::vec4(keyframes[i + 3].getPos(), 0.0f);
      }
      
      if(keyToggles[(unsigned)'k'])
      {
        // Create uVec. Draw curve.
        glBegin(GL_LINE_STRIP);
          for(float u = 0; u <= 1.0f; u += 0.01f)
          {
            glm::vec4 uVec(1.0f, u, u*u, u*u*u);

            glm::vec3 p = G*(B*uVec);

            glColor3f(0.1f, 0.1f, 1.0f);
            glVertex3f(p.x, p.y, p.z);
          }
        glEnd();
      }
    }

    // Interpolating qauternions
    
    // Fill in G
    if(k == 0)
    {
      G[0] = glm::vec4(keyframes[ncps - 1].getQuat().x, keyframes[ncps - 1].getQuat().y, keyframes[ncps - 1].getQuat().z, keyframes[ncps - 1].getQuat().w);
      G[1] = glm::vec4(keyframes[k].getQuat().x, keyframes[k].getQuat().y, keyframes[k].getQuat().z, keyframes[k].getQuat().w);
      G[2] = glm::vec4(keyframes[k + 1].getQuat().x, keyframes[k + 1].getQuat().y, keyframes[k + 1].getQuat().z, keyframes[k + 1].getQuat().w);
      G[3] = glm::vec4(keyframes[k + 2].getQuat().x, keyframes[k + 2].getQuat().y, keyframes[k + 2].getQuat().z, keyframes[k + 2].getQuat().w);
    }
    else if(k == ncps - 3)
    {
      G[0] = glm::vec4(keyframes[k - 1].getQuat().x, keyframes[k - 1].getQuat().y, keyframes[k - 1].getQuat().z, keyframes[k - 1].getQuat().w);
      G[1] = glm::vec4(keyframes[k].getQuat().x, keyframes[k].getQuat().y, keyframes[k].getQuat().z, keyframes[k].getQuat().w);
      G[2] = glm::vec4(keyframes[k + 1].getQuat().x, keyframes[k + 1].getQuat().y, keyframes[k + 1].getQuat().z, keyframes[k + 1].getQuat().w);
      G[3] = glm::vec4(keyframes[0].getQuat().x, keyframes[0].getQuat().y, keyframes[0].getQuat().z, keyframes[0].getQuat().w);
    }
    else if(k == ncps - 2)
    {
      G[0] = glm::vec4(keyframes[k - 1].getQuat().x, keyframes[k - 1].getQuat().y, keyframes[k - 1].getQuat().z, keyframes[k - 1].getQuat().w);
      G[1] = glm::vec4(keyframes[k].getQuat().x, keyframes[k].getQuat().y, keyframes[k].getQuat().z, keyframes[k].getQuat().w);
      G[2] = glm::vec4(keyframes[0].getQuat().x, keyframes[0].getQuat().y, keyframes[0].getQuat().z, keyframes[0].getQuat().w);
      G[3] = glm::vec4(keyframes[1].getQuat().x, keyframes[1].getQuat().y, keyframes[1].getQuat().z, keyframes[1].getQuat().w);
    }
    else if(k == ncps - 1)
    {
      G[0] = glm::vec4(keyframes[k - 1].getQuat().x, keyframes[k - 1].getQuat().y, keyframes[k - 1].getQuat().z, keyframes[k - 1].getQuat().w);
      G[1] = glm::vec4(keyframes[0].getQuat().x, keyframes[0].getQuat().y, keyframes[0].getQuat().z, keyframes[0].getQuat().w);
      G[2] = glm::vec4(keyframes[1].getQuat().x, keyframes[1].getQuat().y, keyframes[1].getQuat().z, keyframes[1].getQuat().w);
      G[3] = glm::vec4(keyframes[2].getQuat().x, keyframes[2].getQuat().y, keyframes[2].getQuat().z, keyframes[2].getQuat().w);
    }
    else
    {
      G[0] = glm::vec4(keyframes[k - 1].getQuat().x, keyframes[k - 1].getQuat().y, keyframes[k - 1].getQuat().z, keyframes[k - 1].getQuat().w);
      G[1] = glm::vec4(keyframes[k].getQuat().x, keyframes[k].getQuat().y, keyframes[k].getQuat().z, keyframes[k].getQuat().w);
      G[2] = glm::vec4(keyframes[k + 1].getQuat().x, keyframes[k + 1].getQuat().y, keyframes[k + 1].getQuat().z, keyframes[k + 1].getQuat().w);
      G[3] = glm::vec4(keyframes[k + 2].getQuat().x, keyframes[k + 2].getQuat().y, keyframes[k + 2].getQuat().z, keyframes[k + 2].getQuat().w);
    }
    
    // Solve for p
    glm::vec4 uVec(1.0f, uu, uu*uu, uu*uu*uu);

    p = G*(B*uVec);

    // Store q and E
    q = glm::quat(p[3], p[0], p[1], p[2]);
    E = glm::toMat4(glm::normalize(q));

    // Interpolating position

    // Fill in G
    if(k == 0)
    {
      G[0] = glm::vec4(keyframes[ncps - 1].getPos().x, keyframes[ncps - 1].getPos().y, keyframes[ncps - 1].getPos().z, 1.0f);
      G[1] = glm::vec4(keyframes[k].getPos().x, keyframes[k].getPos().y, keyframes[k].getPos().z, 1.0f);
      G[2] = glm::vec4(keyframes[k + 1].getPos().x, keyframes[k + 1].getPos().y, keyframes[k + 1].getPos().z, 1.0f);
      G[3] = glm::vec4(keyframes[k + 2].getPos().x, keyframes[k + 2].getPos().y, keyframes[k + 2].getPos().z, 1.0f);
    }
    else if(k == ncps - 3)
    {
      G[0] = glm::vec4(keyframes[k - 1].getPos().x, keyframes[k - 1].getPos().y, keyframes[k - 1].getPos().z, 1.0f);
      G[1] = glm::vec4(keyframes[k].getPos().x, keyframes[k].getPos().y, keyframes[k].getPos().z, 1.0f);
      G[2] = glm::vec4(keyframes[k + 1].getPos().x, keyframes[k + 1].getPos().y, keyframes[k + 1].getPos().z, 1.0f);
      G[3] = glm::vec4(keyframes[0].getPos().x, keyframes[0].getPos().y, keyframes[0].getPos().z, 1.0f);
    }
    else if(k == ncps - 2)
    {
      G[0] = glm::vec4(keyframes[k - 1].getPos().x, keyframes[k - 1].getPos().y, keyframes[k - 1].getPos().z, 1.0f);
      G[1] = glm::vec4(keyframes[k].getPos().x, keyframes[k].getPos().y, keyframes[k].getPos().z, 1.0f);
      G[2] = glm::vec4(keyframes[0].getPos().x, keyframes[0].getPos().y, keyframes[0].getPos().z, 1.0f);
      G[3] = glm::vec4(keyframes[1].getPos().x, keyframes[1].getPos().y, keyframes[1].getPos().z, 1.0f);
    }
    else if(k == ncps - 1)
    {
      G[0] = glm::vec4(keyframes[k - 1].getPos().x, keyframes[k - 1].getPos().y, keyframes[k - 1].getPos().z, 1.0f);
      G[1] = glm::vec4(keyframes[0].getPos().x, keyframes[0].getPos().y, keyframes[0].getPos().z, 1.0f);
      G[2] = glm::vec4(keyframes[1].getPos().x, keyframes[1].getPos().y, keyframes[1].getPos().z, 1.0f);
      G[3] = glm::vec4(keyframes[2].getPos().x, keyframes[2].getPos().y, keyframes[2].getPos().z, 1.0f);
    }
    else
    {
      G[0] = glm::vec4(keyframes[k - 1].getPos().x, keyframes[k - 1].getPos().y, keyframes[k - 1].getPos().z, 1.0f);
      G[1] = glm::vec4(keyframes[k].getPos().x, keyframes[k].getPos().y, keyframes[k].getPos().z, 1.0f);
      G[2] = glm::vec4(keyframes[k + 1].getPos().x, keyframes[k + 1].getPos().y, keyframes[k + 1].getPos().z, 1.0f);
      G[3] = glm::vec4(keyframes[k + 2].getPos().x, keyframes[k + 2].getPos().y, keyframes[k + 2].getPos().z, 1.0f);
    }

    // Solve for p
    glm::vec4 upVec(1.0f, uu, uu*uu, uu*uu*uu);

    pp = G*(B*upVec);
    
    if(keyToggles[(unsigned)'k'])
    {
      // Solve for frenet frame variables
      uVec1 = glm::vec4(0.0f, 1.0f, (uu)*2.0f, (uu*uu)*3.0f);
      p1 = G*(B*uVec1);
      uVec2 = glm::vec4(0.0f, 0.0f, 2.0f, uu*6.0f);
      p2 = G*(B*uVec2);
    
      Tan = glm::normalize(p1);
      Bin = glm::normalize(glm::cross(p1, p2));
      Norm = glm::cross(Bin, Tan);

      // Draw frenet frame
      glBegin(GL_LINE_STRIP);
        glColor3f(1.0, 0.0, 0.0);
        glVertex3f(pp.x, pp.y, pp.z);
        glVertex3f(pp.x + Tan.x, pp.y + Tan.y, pp.z + Tan.z);
      glEnd();

      glBegin(GL_LINE_STRIP);
        glColor3f(0.0, 0.0, 1.0);
        glVertex3f(pp.x, pp.y, pp.z);
        glVertex3f(pp.x + Bin.x, pp.y + Bin.y, pp.z + Bin.z);
      glEnd();

      glBegin(GL_LINE_STRIP);
        glColor3f(0.0, 1.0, 0.0);
        glVertex3f(pp.x, pp.y, pp.z);
        glVertex3f(pp.x + Norm.x, pp.y + Norm.y, pp.z + Norm.z);
      glEnd();
    }
  }

	progSimple->unbind();
	GLSL::checkError(GET_FILE_LINE);

	// Draw the helicopter
	progNormal->bind();
	// Send projection matrix (same for all helicopters)
	glUniformMatrix4fv(progNormal->getUniform("P"), 1, GL_FALSE, glm::value_ptr(P->topMatrix()));
	
	// I don't know what the center is so for now it's (0.0f, 0.0f, 0.0f)
	glm::vec3 center(0.0f, 0.0f, 0.0f);

  // Find which keyframes we're interpolating between
  for(unsigned int i = 1; i < keyframes.size(); i++)
  {
    if(alpha > i - 1 && alpha < i)
    {
      pbegin = keyframes[i-1].getPos();
      pend = keyframes[i].getPos();
    }
  }

  // Make the propellers go "woosh"
  root.children[1].r.y = fmod(t*8, 2*M_PI);
  root.children[2].r.z = fmod(t*8, 2*M_PI);

	// Draw all the frames
  if(keyToggles[(unsigned)'k'])
  {
    for(unsigned int j = 0; j < keyframes.size() - 1; j++)
    {
      MV->pushMatrix();
        glm::mat4 R = glm::toMat4(keyframes[j].getQuat());
        R[3] = glm::vec4((keyframes[j].getPos() - center), 1.0f);
        MV->multMatrix(R);
        glUniformMatrix4fv(progNormal->getUniform("MV"), 1, GL_FALSE, glm::value_ptr(MV->topMatrix()));
        root.draw(MV, P, progNormal);
      MV->popMatrix();
    }
  }

	// INTERPOLATED
	MV->pushMatrix();
    E[3] = glm::vec4((glm::vec3(pp.x, pp.y, pp.z) - center), 1.0f);
    MV->multMatrix(E);
	  glUniformMatrix4fv(progNormal->getUniform("MV"), 1, GL_FALSE, glm::value_ptr(MV->topMatrix()));
	  root.draw(MV, P, progNormal);
    if(keyToggles[(unsigned)' '])
    {
      camera->followMe(E);
    }
    else
    {
      MV->pushMatrix();
        MV->loadIdentity();
        camera->followMe(MV->topMatrix());
      MV->popMatrix();
    }
	MV->popMatrix();
	
	progNormal->unbind();
	
	// Pop stacks
	MV->popMatrix();
	P->popMatrix();
	
	GLSL::checkError(GET_FILE_LINE);
}

int main(int argc, char **argv)
{
	if(argc < 2) {
		cout << "Please specify the resource directory." << endl;
		return 0;
	}
	RESOURCE_DIR = argv[1] + string("/");
	
	// Set error callback.
	glfwSetErrorCallback(error_callback);
	// Initialize the library.
	if(!glfwInit()) {
		return -1;
	}
	// Create a windowed mode window and its OpenGL context.
	window = glfwCreateWindow(640, 480, "Alex Huddleston Assignment 5", NULL, NULL);
	if(!window) {
		glfwTerminate();
		return -1;
	}
	// Make the window's context current.
	glfwMakeContextCurrent(window);
	// Initialize GLEW.
	glewExperimental = true;
	if(glewInit() != GLEW_OK) {
		cerr << "Failed to initialize GLEW" << endl;
		return -1;
	}
	glGetError(); // A bug in glewInit() causes an error that we can safely ignore.
	cout << "OpenGL version: " << glGetString(GL_VERSION) << endl;
	cout << "GLSL version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << endl;
	// Set vsync.
	glfwSwapInterval(1);
	// Set keyboard callback.
	glfwSetKeyCallback(window, key_callback);
	// Set char callback.
	glfwSetCharCallback(window, char_callback);
	// Set cursor position callback.
	glfwSetCursorPosCallback(window, cursor_position_callback);
	// Set mouse button callback.
	glfwSetMouseButtonCallback(window, mouse_button_callback);
	// Initialize scene.
	init();
	// Loop until the user closes the window.
	while(!glfwWindowShouldClose(window)) {
		// Render scene.
		render();
		// Swap front and back buffers.
		glfwSwapBuffers(window);
		// Poll for and process events.
		glfwPollEvents();
	}
	// Quit program.
	glfwDestroyWindow(window);
	glfwTerminate();
	return 0;
}
