#define GLUT_API_VERSION 4

#include <iostream>
#include <strstream>
#include <vector>

#ifdef _WIN32
#include <windows.h>
#endif
#include <math.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#ifndef M_PI
#define M_PI            3.14159265358979323846
#endif 
#include <time.h>

#include "cvec2t.h"
#include "cvec3t.h"

typedef CVec2T<float> Vec2f;
typedef CVec3T<float> Vec3f;


namespace WindowParams {
  static int WindowWidth = 800;
  static int WindowHeight = 600;
  static int MainWindow; 
};

namespace Params { 
  const float WorldWidth = 1.33;
  const float WorldHeight = 1;

  const Vec3f BackgroundColor(0.5,0.5,0.5);

  const Vec2f CourtLowerLeftCorner(-WorldWidth*0.4,-WorldHeight*0.4);  
  const Vec2f CourtSize(WorldWidth*0.8, WorldHeight*0.8); 

  const Vec3f Gravity(0,-0.0001,0); 
  const int SubSteps = 4; // number of substeps of physics calculations per frame

};

float CurrentTime;


class Box; 

class RigidObject {
public:
    // if other derived object types are created, entries should be added to this type, along with 
	// appropriate collision functions

	enum ObjectType { OBJ_BOX };
  // update for continuous motion
  virtual update(float dt, const Vec3f& force, float torque) { 
	  if(_movable) {
        _position += _velocity*dt;
        _velocity +=  force/_mass*dt;
        _rotation += _ang_velocity*dt;
	    _ang_velocity += torque/_inertia*dt;
	  }
  }

  // update for collisions
  virtual impulse_update(const Vec3f& impulse, const Vec3f& point) {
      _velocity += impulse/_mass;
	 _ang_velocity += cross(point-_position,impulse).z()/_inertia;
  }

  float mass()     const  { return _mass; }
  float inertia()  const  { return _inertia; } 
  Vec3f position() const  { return _position;} 
  Vec3f velocity() const  { return _velocity; }
  float rotation() const  { return _rotation; }
  float ang_velocity() const { return _ang_velocity;}
  bool  movable() const { return _movable; }
  ObjectType type() const { return _type;}

  // velocity of a point p in the object, computed from velocity of c.m. and angular velocity
  Vec3f point_velocity(const Vec3f& p) const { 
    Vec3f r = p - _position;
    return _velocity+ Vec3f(-r.y(), r.x(),0)*_ang_velocity;
  }
  // abstract draw function, to force all derived classes to have it
  virtual void draw() = 0; 

  // general collision dispatch
  virtual bool collide(const RigidObject& ob,Vec3f& pos, Vec3f& normal) const { 
	  // other object types can be added here
	  switch( ob.type()) {
        case OBJ_BOX: return collide( (const Box&)(ob),pos,normal); break;
	  }
	  // if unfamiliar object ignore
	  return false;
  }

  // abstract collision function used when the other object is also Box, to force derived classes to have it
  // if other object types are added specific functions like this need to be defined
  virtual bool collide(const Box& box,Vec3f& pos, Vec3f& normal) const =0;
   
protected:
  float _mass; 
  float _inertia;
  Vec3f _position; 
  Vec3f _velocity; 
  float _rotation; 
  float _ang_velocity;
  bool  _movable;      // is the object fixed in its initial position? useful for making static objects
  ObjectType _type;    // object type, for collision dispatch 
};


// compute impulse for perfect elastic collision of two rigid objects
Vec3f collisionImpulse(const RigidObject& oA, const RigidObject& oB, const Vec3f& p, const Vec3f& n) { 
  Vec3f rA = p - oA.position();
  Vec3f rB = p - oB.position();
  Vec3f vA = oA.velocity() + Vec3f(-rA.y(), rA.x(),0)*oA.ang_velocity();
  Vec3f vB = oB.velocity() + Vec3f(-rB.y(), rB.x(),0)*oB.ang_velocity();
  Vec3f j = -2*(vA - vB).dot(n)*n/
 ( 1/oA.mass() + 1/oB.mass() + cross(rA,n).dot()/oA.inertia() +   cross(rB,n).dot()/oB.inertia());
  return j;
}




class Box: public RigidObject { 
public:
 Box ( const Vec3f& sz,float m,   bool ismovable = true, bool isinterior=false,
	const Vec3f& init_position=Vec3f(0,0,0),  const Vec3f& init_velocity=Vec3f(0,0,0), 
	const float init_rotation=0, const float init_ang_velocity=0.0)
  { 
	_mass = m;
	_size = sz; 
	_position = init_position; 
	_velocity = init_velocity; 
	_rotation = init_rotation;
	_ang_velocity = init_ang_velocity;
	_inertia = (1/12.0)*(_size.x()*_size.x() + _size.y()*_size.y())*_mass;
	_movable = ismovable;
	_interior = isinterior;
	_type = OBJ_BOX;
  }
  
  virtual void draw() {
	  glPushMatrix();
	  glTranslatef(_position.x(),_position.y(),_position.z());
	  glRotatef(_rotation*180/M_PI, 0,0,1);
	  if(!_interior) { 
     	  glRectf(-0.5*_size.x(),-0.5*_size.y(),0.5*_size.x(),0.5*_size.y());
	  } else { 
	    glColor3f(1,0,0);
		glBegin(GL_LINE_LOOP); 
		glVertex3f(-0.5*_size.x(),-0.5*_size.y(),0); glVertex3f(0.5*_size.x(),-0.5*_size.y(),0); 
		glVertex3f(0.5*_size.x(),0.5*_size.y(),0); glVertex3f(-0.5*_size.x(),0.5*_size.y(),0); 
		glEnd();
	  }

	  glPopMatrix();
  }
  // convert from world coordinates to the local coordinate system with origin at the center of the box
  // and axes aligned with the axes of the box
  Vec3f toLocal(const Vec3f& pworld) const {
   Vec3f ptrans = pworld-_position;
   return Vec3f(cos(_rotation)*ptrans.x()+sin(_rotation)*ptrans.y(), 
		            -sin(_rotation)*ptrans.x()+cos(_rotation)*ptrans.y(), 0);
  }

  // convert from local coordinates to world
  Vec3f toWorld(const Vec3f& plocal) const {
   Vec3f prot =    Vec3f(cos(_rotation)*plocal.x()-sin(_rotation)*plocal.y(), 
		           sin(_rotation)*plocal.x()+cos(_rotation)*plocal.y(), 0);

   return prot+_position;
  }

  // conversion for vectors (no translation applied)
  Vec3f toLocalVec(const Vec3f& pworld) const {
   Vec3f ptrans = pworld;
   return Vec3f(cos(_rotation)*ptrans.x()+sin(_rotation)*ptrans.y(), 
		            -sin(_rotation)*ptrans.x()+cos(_rotation)*ptrans.y(), 0);
  }
  // conversion for vectors (no translation applied)
  Vec3f toWorldVec(const Vec3f& plocal) const {
   Vec3f prot =    Vec3f(cos(_rotation)*plocal.x()-sin(_rotation)*plocal.y(), 
		           sin(_rotation)*plocal.x()+cos(_rotation)*plocal.y(), 0);

   return prot;
  }
  // check if a point p is inside the box; (if the flag _interior is set, "inside" is swapped with "outside"
  bool collidePoint(const Vec3f& p, Vec3f& normal)  const { 
  Vec3f plocal = toLocal(p);
     // check if there is a collision, i.e. p's coordinates in box's local cood system are in the right range
	 bool collide = fabs(plocal.x()) <= 0.5*_size.x() && fabs(plocal.y()) <= 0.5*_size.y();
	 if(_interior) collide = !collide;
 
	 if(collide) { 
	   float d[4];
	   // list of outward facing normals to sides
	   const Vec3f edgenormal[4] = {Vec3f(-1,0,0), Vec3f(0,1,0),Vec3f(1,0,0),Vec3f(0,-1,0)};
	   // distances to box sides
	   d[0] = fabs(plocal.x()+0.5*_size.x());
	   d[1] = fabs(plocal.y()-0.5*_size.y());
	   d[2] = fabs(plocal.x()-0.5*_size.x());
	   d[3] = fabs(plocal.y()+0.5*_size.y());
	   // find closest side
	   int mindno = -1; 
	   float mind = FLT_MAX;
	   for(int i = 0; i < 4; i++) {
		   if (mind > d[i]) {
			   mindno=i; mind = d[i];
		   }
	   }
       // assign collision normal
	   normal = toWorldVec(edgenormal[mindno]);
	   if(_interior) normal = -normal; 
	 }
	 return collide;
  }

  // test if any corner of this box is inside another box Box
  bool collideAllCorners(const Box& box,Vec3f& pos, Vec3f& normal) const { 
	  Vec3f corner, n;
      corner = toWorld(0.5*Vec3f(-_size.x(),-_size.y(), _size.z()));
	  if( box.collidePoint(corner,normal)) { pos = corner; return true;}
      corner = toWorld(0.5*Vec3f(-_size.x(), _size.y(), _size.z()));
	  if( box.collidePoint(corner,normal)) { pos = corner; return true;}
	  corner = toWorld(0.5*Vec3f( _size.x(), _size.y(), _size.z()));
	  if( box.collidePoint(corner,normal)) { pos = corner; return true;}
      corner = toWorld(0.5*Vec3f( _size.x(), -_size.y(), _size.z()));
	  if( box.collidePoint(corner,normal )) { pos = corner; return true;}
	  return false;
	}

  // check if any corner of this box is inside the other box Box, and the other way around
  virtual bool collide(const Box& box,Vec3f& pos, Vec3f& normal) const {  
	  // need to flip the normal, so that it always points toward the interior of this box
	  if (collideAllCorners(box,pos,normal)) { normal = -normal; return true;}
	if (box.collideAllCorners(*this,pos,normal)) return true;
	return false;
  }

  
  Vec3f size() const { return _size;}
  bool interior() const { return _interior;}
private: 
	Vec3f _size;
	bool _interior;
};




void Reshape(int width, int height) {
  WindowParams::WindowWidth = width;
  WindowParams::WindowHeight = height;
  glViewport(0,0,width,height);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(-Params::WorldWidth/2, Params::WorldWidth/2,-Params::WorldHeight/2,Params::WorldHeight/2);
}




// structure to store collision information: object numbers, collision point and normal vector
struct Collision { 
   int A,B;
   Vec3f p, n;
   Collision(int a, int b, const Vec3f& pc, const Vec3f& nc): A(a), B(b), p(pc), n(nc) {} 
};

class Game { 
public:
   // list of all objects; can have different types derived from RigidObject
   vector<RigidObject*> objects;
   // collision list
   vector<Collision> collisions;

  Game()
  {   
	  // create 5 boxes
	  for(int i = 0; i < 5; i++) {
     	objects.push_back(new Box( 
			Vec3f(0.1*i,0.05,0.01), // dimensions
			i,            // mass 
			true,         // does this move?
			false,        // is this inside-out box?
			Vec3f(0,0.2-0.1*i,0), //initial position
			Vec3f(0,0,0), //initial velocity
			0.0,          // initial rotation angle
			0.0));        // initial ang. velocity
	  }
	  // very heavy box (one can make an "infinite mass" box by setting the inverse mass to 0
	  objects.push_back(new Box(Vec3f(Params::CourtSize.x(),Params::CourtSize.y(),0),1e6,false, true));
  }

  ~Game() { 
	  for(int i = 0; i < objects.size(); i++) { 
         delete objects[i];
	  }
  }

  void draw() { 
	for(int i = 0; i < objects.size(); i++) 
		objects[i]->draw();
  }

 // find all intersecting objects A,B for which the direction of the velocity of the point of A colliding with B
 // relative to B is towards the interior of B, i.e. objects are moving towards each other
 // store the list in collisions
  void detectCollisions() { 
	Vec3f p,n;
	collisions.clear();
	// for all pairs of objects
	for(int i = 0; i < objects.size(); i++) { 
       for(int j = i+1; j < objects.size(); j++) { 
		   // if there is overlap
		   if( objects[i]->collide(*(objects[j]),p,n) && 
			  // and the relative velocity at p is towards interior of B
			  (objects[i]->point_velocity(p)- objects[j]->point_velocity(p) ).dot(n) >= 0 ) {
               // add point to the list
       	       collisions.push_back(Collision(i,j,p,n));
			  }
		   }
	   }
    }
  

   // update all object positions; if there are forces other than gravity,
   // they should be added here
   void update(float dt) { 
      for(int i = 0; i < objects.size()-1; i++) 
		  objects[i]->update(dt, objects[i]->mass()*Params::Gravity,0); 
   }


   // advance simulation; check for collisions 
   // and if there are any (there may be multipel per time step), try to resolve them by applying impulses.
   // Note that applying impulses in principle can create more
   // collisions so iterate the process
   // there is no guarantee that this iteration conerges
   // so do it a fixed number of times, which should work for almost
   // all configurations
   // in the unusual event these iterations fail, the effect 
   // will be that object will fly through another with a chance
   // of getting stuck.
   // A better apporach would be to refine the time step 
   // but to keep the code simple we do not do this

   void processCollisions(float dt) { 
      float step = dt;
	  int i;
	  // move objects
      update(dt);
	  // check for collisions
	  detectCollisions();
	  int iter = 0;
      // if there are any try to resolve
	  while (collisions.size() > 0 && iter < 4) { 
	    iter++;
		// go through the list of collisions and apply impulses
		// this does not remove overlaps, but makes them fly avay from each other
		for(int k = 0; k < collisions.size(); k++) {
           Vec3f j = collisionImpulse( *(objects[collisions[k].A]),*(objects[collisions[k].B]), collisions[k].p, collisions[k].n);
		   objects[collisions[k].A]->impulse_update(j,collisions[k].p);
		   objects[collisions[k].B]->impulse_update(-j,collisions[k].p);
		}
		// check for collisions again; if now all objects are flying away from each other
		// there will be no collisions; if changing velocities made some objects collide, 
		// this will force another iteration 
		detectCollisions();
	  }
   }

  void advance(float dt) {
	  for( int i = 0; i < Params::SubSteps; i++)
		  processCollisions(dt/float(Params::SubSteps)); 
  }
};

Game* game;

// gets called when a key is pressed down
void Keyboard( unsigned char key, int, int) { 

}


void Draw() {
  glClearColor( 0.5, 0.5,0.5,0.0);
  glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
  game->draw();
  glutSwapBuffers();
}


void AnimateVariable() {
  float dt = clock()-CurrentTime; 
  CurrentTime +=dt;
  game->advance(60*dt/float(CLOCKS_PER_SEC));
  glutPostRedisplay();
}



int main(int argc, char* argv[]) {

  // initialize glut and parse command-line aguments that glut understands
  glutInit(&argc, argv);

  // initialize dislay mode: 4 color components, double buffer and depth buffer
  glutInitDisplayMode(GLUT_RGBA|GLUT_DOUBLE|GLUT_DEPTH);

  glutInitWindowSize(WindowParams::WindowWidth,WindowParams::WindowHeight);

  WindowParams::MainWindow = glutCreateWindow("Physics2D");

  // register all callbacks

  // gets called whenever the window needs to be redrawn
  glutDisplayFunc(Draw); 
  // gets called whenever the window changes shape
  glutReshapeFunc(Reshape);
  glutKeyboardFunc(Keyboard);    
  // this insures that Animate is called the first time
  CurrentTime = clock();
  glutIdleFunc(AnimateVariable);
  game = new Game; 
  // this is an infinite loop get event - dispatch event which never returns
  glutMainLoop();
  return 0;
}