#include "jlib.h"

#include <GL/glut.h>
#include <iostream.h>
#include <fstream.h>
#include <math.h>
#include "vec3f.h"

//#define CANNON
#define SPRING

//
// Cannon definitions
//
#ifdef CANNON

const int MAX_TIME_STEPS = 1000;
const double DISP_PER_SIM = 0.5;	// Number of seconds of simulation time per second of display time.

const double FIELD_EXTENT = 100;

const double BASE_RADIUS = FIELD_EXTENT/10;
const double TARG_RADIUS = FIELD_EXTENT/15;
const double BULLET_RADIUS = FIELD_EXTENT/30;

const double FRICTION = 50;		// Friction coefficient (kg/s)
const double BARREL_DURATION = 0.03;	// (s) Amount of acceleration time within the gun barrel.

const double Pi = 3.14159265;

double theta = Pi/2;
double phi = Pi/4;
double thetadelta = Pi/180;;
double phidelta = Pi/180;
double view = 0;
double viewdelta = Pi/2/10;
double powder = 10.0;			// Powder (kg).  1 kg produces 10,000 N of force
double powderdelta = 1.0;
double mass = 100;				// Weight of projectile.  (kg)
double massdelta = 1;

Vec3f pBullet;
//Vec3f pBase(0,0,0);
//Vec3f pTarget(Random(-FIELD_EXTENT,+FIELD_EXTENT),0,Random(-FIELD_EXTENT,+FIELD_EXTENT));
Vec3f pBase(0,0,0);
Vec3f pTarget(0,0,+FIELD_EXTENT);

//
// Spring-Mass definitions.
//
#else

const int MAX_TIME_STEPS = 2000;
const double DISP_PER_SIM = 0.2;	// Number of seconds of simulation time per second of display time.

const double LINE_WIDTH = 10;
const double LINE_SPACING = 1;
const double MASS_RADIUS = 1.5;
const double THRESH = 0.1;		// Theshold to ensure that it's done moving.

double mass = 5;
double massdelta = 1;
double k = 15;
double kdelta = 1;
double damping = 1;

#endif

//
// General definitions.
//

const double GRAVITY = 9.8;		// Accel due to gravity (m/s^2)
double timestep = 0.03;	// (s)

Vec3f *ObjectPath = new Vec3f[MAX_TIME_STEPS];
Vec3f *ObjectVel = new Vec3f[MAX_TIME_STEPS];
int nSteps;
bool active = false;	// Bullet is not in the air.
bool firstperson = false;	// Show bullet from the first person mode.

typedef enum {EULER, RK2, RK3, RK4} METHOD;
METHOD Method = EULER;


void Display()
{
#ifdef CANNON

	static GLUquadric* quad = gluNewQuadric();

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	Vec3f pCamera;
	Vec3f pLook;
	Vec3f pUp;

	int j = int(ReadTimer()/DISP_PER_SIM/timestep);	// Which step of the flight movie we're on.  (if active).
	if (active &&  j < nSteps  &&  firstperson)
	{
		pCamera = ObjectPath[j];
		pLook = pTarget;
		pUp = Vec3f(0, 1, 0);
	}
	else
	{
		if (active &&  j >= nSteps)
		{
			active = false;
			cout << "stop" << endl;
		}
		double camDist = (pBase-pTarget).length();
		if (camDist < 100)
			camDist = 100;
		pLook = (pBase+pTarget) * 0.5;
		pCamera = pLook + Vec3f(camDist*sin(view), camDist*cos(view), 0);
		pUp = Vec3f(-1, 0, 0);
	}

	glLoadIdentity();
	gluLookAt(pCamera.x, pCamera.y, pCamera.z,
		      pLook.x, pLook.y, pLook.z,
			  pUp.x, pUp.y, pUp.z);

	if (!active || !firstperson)
	{
	glPushMatrix();
		glColor3f(0.0, 0.0, 1.0);
		glTranslatef(pBase.x, pBase.y, pBase.z);
		glScalef(BASE_RADIUS, BASE_RADIUS, BASE_RADIUS);
		gluSphere(quad, 1, 20, 20);
		glColor3f(1.0, 1.0, 1.0);
		glBegin(GL_LINES);
			glVertex3f(pBase.x, pBase.y, pBase.z);
			glVertex3f(pBase.x + 2*cos(phi)*cos(theta), pBase.y + 2*sin(phi), pBase.z + 2*cos(phi)*sin(theta));
		glEnd();
	glPopMatrix();
	}

	glPushMatrix();
		glColor3f(0.0, 1.0, 0.0);
		glTranslatef(pTarget.x, pTarget.y, pTarget.z);
		//gluSphere(quad, TARG_RADIUS, 20, 20);
		//glRotatef(90, 1, 0, 0);
		glutSolidTeapot(TARG_RADIUS);
	glPopMatrix();


	// Draw the playing field.
	glPushMatrix();
		glColor3f(0.5, 0.5, 0.5);
		glBegin(GL_LINES);
			const int NGRID = 20;
			const double GRIDSPACING = 2*FIELD_EXTENT / double(NGRID);
			for (int i = 0; i <= NGRID; ++i)
			{
				glVertex3f(-FIELD_EXTENT+i*GRIDSPACING, 0, -FIELD_EXTENT);
				glVertex3f(-FIELD_EXTENT+i*GRIDSPACING, 0, +FIELD_EXTENT);
				glVertex3f(-FIELD_EXTENT, 0, -FIELD_EXTENT+i*GRIDSPACING);
				glVertex3f(+FIELD_EXTENT, 0, -FIELD_EXTENT+i*GRIDSPACING);
			}
		glEnd();
	glPopMatrix();

	// Draw the projectile.
	if (active && !firstperson)
	{
		if (j >= nSteps)
		{
			active = false;
			cout << "stop" << endl;
		}
		else
		{
			glPushMatrix();
			glColor3f(1.0, 0, 0);
			glTranslatef(ObjectPath[j][0], ObjectPath[j][1], ObjectPath[j][2]);
			gluSphere(quad, BULLET_RADIUS, 20, 20);
			glPopMatrix();
		}
	}

	glFlush();

#else

	static GLUquadric* quad = gluNewQuadric();

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glLoadIdentity();
	gluLookAt(0, 0, 10,
		      0, 0, 0,
			  0, +1, 0);

	// Draw the marking lines.
	glPushMatrix();
		glBegin(GL_LINES);
			// Center Line.
			glColor3f(0.0, 0.0, 1.0);
			glVertex3f(-LINE_WIDTH/2.0,0,0);
			glVertex3f(+LINE_WIDTH/2.0,0,0);
			glColor3f(1.0, 1.0, 1.0);
			// Lines -10...-1,  +1...+10
			for (int i = 1; i <=10; ++i)
			{
				glVertex3f(-LINE_WIDTH/2.0, i*LINE_SPACING, 0);
				glVertex3f(+LINE_WIDTH/2.0, i*LINE_SPACING, 0);
				glVertex3f(-LINE_WIDTH/2.0, -i*LINE_SPACING, 0);
				glVertex3f(+LINE_WIDTH/2.0, -i*LINE_SPACING, 0);
			}
		glEnd();
	glPopMatrix();

	// Draw the mass.
	glPushMatrix();
		glColor3f(1.0, 0, 0);
		if (active)
		{
			int j = int(ReadTimer()/DISP_PER_SIM/timestep);
			if (j >= nSteps)
			{
				active = false;
				cout << "stop" << endl;
			}
			else
			{
				glTranslatef(ObjectPath[j][0], ObjectPath[j][1], ObjectPath[j][2]);
				gluSphere(quad, MASS_RADIUS, 20, 20);
			}
		}
		else
			gluSphere(quad, MASS_RADIUS, 20, 20);
	glPopMatrix();
		
	glFlush();

#endif
}

Vec3f Acceleration(const Vec3f &p, const Vec3f &v)
{
#ifdef CANNON

	Vec3f G(0, -mass*GRAVITY, 0);
	Vec3f Friction = -v * FRICTION;
	return (G + Friction) / mass;

#else

	Vec3f G(0, -mass*GRAVITY, 0);
	Vec3f Spring = -p * k;
	Vec3f Friction = -v * damping;
	return (G + Spring + Friction) / mass;

#endif
}

Vec3f Velocity(const Vec3f &p, const Vec3f &v)
{
#ifdef CANNON

	return v;

#else

	return v;

#endif
}

void DoRK2(Vec3f &p, Vec3f &v, double timestep)
{
	Vec3f k1, k2;
	Vec3f l1, l2;

	k1 = timestep * Velocity(p, v);
	l1 = timestep * Acceleration(p, v);
	k2 = timestep * Velocity(p + k1, v + l1);
	l2 = timestep * Acceleration(p + k1, v + l1);

	p += k1/2 + k2/2;
	v += l1/2 + l2/2;
}

void DoRK3(Vec3f &p, Vec3f &v, double timestep)
{
	Vec3f k1, k2, k3;
	Vec3f l1, l2, l3;

	k1 = timestep * Velocity(p, v);
	l1 = timestep * Acceleration(p, v);
	k2 = timestep * Velocity(p + k1/2, v + l1/2);
	l2 = timestep * Acceleration(p + k1/2, v + l1/2);
	k3 = timestep * Velocity(p + k2, v + l2);
	l3 = timestep * Acceleration(p + k2, v + l2);

	p += k1/6 + k2*2/3.0 + k3/6;
	v += l1/6 + l2*2/3.0 + l3/6;
}

void DoRK4(Vec3f &p, Vec3f &v, double timestep)
{
	Vec3f k1, k2, k3, k4;
	Vec3f l1, l2, l3, l4;

	k1 = timestep * Velocity(p, v);
	l1 = timestep * Acceleration(p, v);
	k2 = timestep * Velocity(p + k1/2, v + l1/2);
	l2 = timestep * Acceleration(p + k1/2, v + l1/2);
	k3 = timestep * Velocity(p + k2/2, v + l2/2);
	l3 = timestep * Acceleration(p + k2/2, v + l2/2);
	k4 = timestep * Velocity(p + k3, v + l3);
	l4 = timestep * Acceleration(p + k3, v + l3);

	p += k1/6 + k2/3 + k3/3 + k4/6;
	v += l1/6 + l2/3 + l3/3 + l4/6;
}

void DoEuler(Vec3f &p, Vec3f& v, double timestep)
{
	Vec3f pnew = p + Velocity(p,v)*timestep;
	v += Acceleration(p,v)*timestep;
	p = pnew;
}

void DoSimulation()
{
#ifdef CANNON

	double muzzlespeed = powder*10000/ mass * BARREL_DURATION;
	Vec3f p(pBase.x, pBase.y, pBase.z);
	Vec3f v(muzzlespeed*cos(phi)*cos(theta), muzzlespeed*sin(phi), muzzlespeed*cos(phi)*sin(theta));

	ObjectPath[0] = p;
	ObjectVel[0] = v;

	for (nSteps = 1; nSteps < MAX_TIME_STEPS; ++nSteps)
	{

		if (Method == EULER)
			DoEuler(p,v,timestep);
		else if (Method == RK2)
			DoRK2(p, v, timestep);
		else if (Method == RK3)
			DoRK3(p, v, timestep);
		else if (Method == RK4)
			DoRK4(p, v, timestep);
		else
			exit(1);

		ObjectPath[nSteps] = p;
		ObjectVel[nSteps] = v;

		if (p[1] <= 0.0)	// hit the ground.
		{
			cout << "Landed at (" << p[0] << "," << p[2] << ")  after " << timestep*nSteps << " sec" << endl;
			break;
		}
	}

#else

	Vec3f p(0,0,0);
	Vec3f v(0,0,0);

	ObjectPath[0] = p;
	ObjectVel[0] = v;

	for (nSteps = 1; nSteps < MAX_TIME_STEPS; ++nSteps)
	{

		if (Method == EULER)
			DoEuler(p,v, timestep);
		else if (Method == RK2)
			DoRK2(p, v, timestep);
		else if (Method == RK3)
			DoRK3(p, v, timestep);
		else if (Method == RK4)
			DoRK4(p, v, timestep);
		else
			exit(1);

		ObjectPath[nSteps] = p;
		ObjectVel[nSteps] = v;

		// Stop when the mass comes (almost) to a rest.
		if (fabs(v.y) < THRESH && Acceleration(p,v).length() < THRESH)
			break;

		if (p.y > 0.1)
		{	// Test for instability (going far above the initial release point)
			cout << nSteps * timestep << " seconds of stability." << endl;
			break;
		}
	}

	if (nSteps == MAX_TIME_STEPS)
		cout << "Didn't come to a rest within " << timestep*nSteps << " sec." << endl;
	else
		cout << "Came to rest after " << timestep*nSteps << " sec." << endl;

#endif
}

// Runs the simulation at a very very small time step using Runga-Kutta 4.
// Compares the results to the values stored in ObjectPath (i.e. those computed with the current timestep size).
// Returns the max displacement error.
double GoldStandard()
{
	const int timestepFraction = 1000;
	double tinydt = timestep / timestepFraction;
	double pErrMax = 0;

#	ifdef CANNON
		double muzzlespeed = powder*10000/ mass * BARREL_DURATION;
		Vec3f p(pBase.x, pBase.y, pBase.z);
		Vec3f v(muzzlespeed*cos(phi)*cos(theta), muzzlespeed*sin(phi), muzzlespeed*cos(phi)*sin(theta));
#	else
		Vec3f p(0,0,0);
		Vec3f v(0,0,0);
#	endif


	for (int j = 1; j < nSteps; ++j)
	{
		for (int i=0; i < timestepFraction; ++i)
		{
			DoRK4(p,v,tinydt);
		}
		double err = (ObjectPath[j]-p).length();
		if (err > pErrMax)
			pErrMax = err;
	}

	return pErrMax;
}

void TimerFunction(int i)
{
	if (active)
	{
		glutPostRedisplay();
		glutTimerFunc(100, TimerFunction, 0);
	}
}

void KeyInputHandler(unsigned char Key, int x, int y)
{
	switch(Key)
	{
		case 27:		// Escape quits.
			exit(0);
			break;

		case '1':
			Method = EULER;
			cout << "Euler's method" << endl;
			break;
		case '2':
			Method = RK2;
			cout << "Runga-Kutta 2" << endl;
			break;
		case '3':
			Method = RK3;
			cout << "Runga-Kutta 3" << endl;
			break;
		case '4':
			Method = RK4;
			cout << "Runga-Kutta 4" << endl;
			break;

#ifdef CANNON

		case 'u':
			phi += phidelta;
			break;
		case 'j':
			phi -= phidelta;
			break;
		case 'k':
			theta += thetadelta;
			break;
		case 'h':
			theta -= thetadelta;
			break;

		case 'g':
			powder += powderdelta;
			break;
		case 'd':
			powder -= powderdelta;
			if (powder < 0)
				powder = 0;
			break;

		case 'f':
			view += viewdelta;
			if (view > Pi/2)
				view = Pi/2;
			break;
		case 'r':
			view -= viewdelta;
			if (view < 0)
				view = 0;
			break;

		case '[':
			mass -= massdelta;
			if (mass < 0)
				mass = 0;
			break;
		case ']':
			mass += massdelta;
			break;

		case '\\':
			firstperson = !firstperson;
			cout << "First person view " << (firstperson ? "ON" : "OFF") << endl;
			break;

#else

		case 'h':
			mass -= massdelta;
			if (mass < 0)
				mass = 0;
			break;
		case 'k':
			mass += massdelta;
			break;

		case 'j':
			k -= kdelta;
			break;
		case 'u':
			k += kdelta;
			break;

		case '[':
			damping /= 2;
			break;
		case ']':
			damping *= 2;
			break;

#endif

		case ',':
			timestep /= 1.1;
			cout << "Timestep = " << int(timestep*1000) << " msec.\n";
			break;
		case '.':
			timestep *= 1.1;
			cout << "Timestep = " << int(timestep*1000) << " msec.\n";
			break;

		case ' ':
			if (active)
			{
				active = false;
			}
			else
			{
				DoSimulation();
				//cout << GoldStandard() << "m max deviation of position." << endl;
				active = true;
				StartTimer();
				glutTimerFunc(0, TimerFunction, 0);
			}
			break;

		default:
			;
	}
#ifdef CANNON
	cout << "az: " << theta*180/Pi << ",  el: " << phi*180/Pi << ",  pow: " << powder << ",  mass=" << mass << endl;
#else
	cout << "mass: " << mass << ",  k: " << k << ",  damp: " << damping << endl;
#endif
	Display();
}

void myReshape(int w, int h)
{
    glViewport(0, 0, w, h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(90, (GLfloat)w/(GLfloat)h, 0.1, 400.0);
    glMatrixMode(GL_MODELVIEW);
}


void main(int argc, char* argv[])
{
	int width=500, height=500;

//	glutInitDisplayMode(GLUT_RGB);
	glutInitWindowPosition(500, 100);
	glutInitWindowSize(width, height);

	glutCreateWindow("Cannon");

	glutDisplayFunc(Display);
	glutKeyboardFunc(KeyInputHandler);
	glutTimerFunc(0, TimerFunction, 0);
	glutReshapeFunc(myReshape);

	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glDisable(GL_CULL_FACE);
	glEnable(GL_DEPTH_TEST);
	glClearColor(0.0, 0.0, 0.0, 0.0);

	glMatrixMode(GL_PROJECTION);
	gluPerspective(90, 1, .1, 400);
	//glOrtho(-150, +150, -150, +150, 0.1, 400);
	glViewport(0, 0, width, height);
	glMatrixMode(GL_MODELVIEW);

	cout << "CONTROLS: " << endl;
#ifdef CANNON
	cout << "Azim   -/+ :  h/k\n";
	cout << "Elev   -/+ :  j/u\n";
	cout << "Powder -/+ :  d/g\n";
	cout << "Mass   -/+ :  [/]\n";
	cout << "View   -/+ :  f/r\n";
#else
	cout << "Mass   -/+ :  h/k\n";
	cout << "K      -/+ :  j/u\n";
	cout << "damp   -/+ :  [/]\n";
#endif
	cout << "tStep  -/+ :  ,/.\n";
	cout << endl;
	glutMainLoop();
}