/* SimplePhysics.C 
 * 
 */

// ------------------------------------------------
#include "glut.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "matrix.h"

// ------------------------------------------------
#define  PI  3.1415926535
#define  TRUE 1
#define  FALSE 0
#define  EPSILON  0.001
#define  RAND  ((rand()*1.0)/(RAND_MAX))

// ------------------------------------------------
#define DELTA_TIME  0.05
#define FORCE_MAGNITUDE 1.0
#define DO_ROTATION
// #define DEBUG_PROCESS
// #define DEBUG_INIT
// #define DEBUG_DISPLAY
// #define DEBUG_ORIENTATION



// ------------------------------------------------
typedef struct xyz_struct {
	float	x,y,z;
} xyz_td;

typedef struct rgb_struct {
	float	r,g,b;
} rgb_td;

// ------------------------------------------------
void display_animation();

// ================================================================================

typedef struct state_struct {
	float		size[3];
	float		mass;
	matrix3x3	inertiaTensor;
	matrix3x3	inertiaTensorInverse;

	float		position[3];
	matrix3x3	orientation;
	float		momentum[3];
	float		angularMomentum[3];
} state_td;

// ---------------------------------------------
void reseed();
void rerun();

// ---------------------------------------------

state_td	object;

// ------------------------------------------------
// INITIALIZE SIMPLE PHYSICS
// ------------------------------------------------
void initialize_animation()
{
	object.orientation = NULL;
	object.inertiaTensor = NULL;
	object.inertiaTensorInverse = NULL;
	
	reseed();
	
	rerun();
}

// -------------------------------------------------
// RERUN SIMPLE PHYSICS
// -------------------------------------------------
void rerun()
{
	matrix3x3	R,RT,Iobject,IobjectInv,It,ItInv,m;
	float		rx,ry,rz,sx,sy,sz;
	float 		Lt[3];
	float		velocity[3];
	float		mass;
	

	// -----------------------set dimensions
	sx = RAND*3+1;
	sy = RAND*3+1;
	sz = RAND*3+1;
	object.size[0] = sx;
	object.size[1] = sy;
	object.size[2] = sz;

	// --------------------set object mass
	mass = RAND*50 + 300;
	object.mass = mass;
	
	// -------------------------set position
	object.position[0] = RAND*5.0;
	object.position[1] = RAND*5.0;
	object.position[2] = 0.0;
	
	#ifdef DEBUG_INIT
	printf("position: %f %f %f\n",object.position[0],object.position[1],object.position[2]);
	printf("mass: %f; size: %f %f %f\n",mass,sx,sy,sz);
	#endif
		
	// ----------------------set orientation
	m = matrix3x3_allocate();
	R = matrix3x3_allocate();
	RT = matrix3x3_allocate();
	if (object.inertiaTensor == NULL) 
		object.inertiaTensor = matrix3x3_allocate();
	if (object.inertiaTensorInverse == NULL) 
		object.inertiaTensorInverse = matrix3x3_allocate();
	It = matrix3x3_allocate();
	ItInv = matrix3x3_allocate();
	
	rx = RAND*360;
	ry = RAND*360;
	rz = RAND*360;
	
	matrix3x3_set_x_rotate(R,rx);
	matrix3x3_set_y_rotate(m,ry);
	matrix3x3_multiply(R,R,m);
	matrix3x3_set_z_rotate(m,rz);
	matrix3x3_multiply(R,R,m);
	
	object.orientation = R;
	
	#ifdef DEBUG_INIT
	// print orientation matrix
	printf("\n orientation\n");
	matrix3x3_print(object.orientation);
	#endif
	
	// ----------------set linear momentum
	velocity[0] = RAND*10-5;
	velocity[1] = RAND*5;
	velocity[2] = 0.0;
	object.momentum[0] = velocity[0]*mass;
	object.momentum[1] = velocity[1]*mass;
	object.momentum[2] = velocity[2]*mass;
	
	#ifdef DEBUG_INIT
	// print orientation matrix
	printf("\n velocity\n");
	printf("position: %f %f %f\n",velocity[0],velocity[1],velocity[2]);
	#endif
	
	// ----------------set angular velocity
	velocity[0] = RAND;
	velocity[1] = RAND;
	velocity[2] = RAND;
	
	// ----------------set angular momentum
	// inertia tensor and its inverse for cuboid in object space
	matrix3x3_load(object.inertiaTensor,
		mass*(sy*sy+sz*sz)/12,0.0,0.0,
		0.0,mass*(sx*sx+sz*sz)/12,0.0,
		0.0,0.0,mass*(sx*sx+sy*sy)/12);
		
		
	// printf("InertiaTensor\n");
	// matrix3x3_print(object.inertiaTensor);
	matrix3x3_inverse(object.inertiaTensorInverse,object.inertiaTensor);
	// printf("InertiaTensorInverse\n");
	// matrix3x3_print(object.inertiaTensorInverse);
	// scanf("%d");
	
	
	// inertia tensor for cuboid in world space
	matrix3x3_transpose(RT,R);
	matrix3x3_multiply(It,R,object.inertiaTensor);
	matrix3x3_multiply(It,It,RT);
	matrix3x3_multiply(ItInv,R,object.inertiaTensorInverse);
	matrix3x3_multiply(ItInv,ItInv,RT);
	
	
	// compute angular momentum
	matrix3x3_multiply_vector(Lt,It,velocity);
	object.angularMomentum[0] = Lt[0];
	object.angularMomentum[1] = Lt[1];
	object.angularMomentum[2] = Lt[2];
	
	
	matrix3x3_deallocate(It);
	matrix3x3_deallocate(ItInv);
	matrix3x3_deallocate(m);
	matrix3x3_deallocate(RT);
}

// -------------------------------------------------
// RESEED
// -------------------------------------------------
void reseed()
{
	int			seed;

	printf("input random number seed: ");
	scanf("%d",&seed);
}

// -------------------------------------------------
// DISPLAY SIMPLE PHYSICS
// -------------------------------------------------
void display_animation()
{
	float	pp[8][3],p[3];
	float	kx,ky,kz,fx,fy,fz,tx,ty,tz;
	float	t[3],f[3];
	float	sx,sy,sz;
	int		i;
	vector3	pq;
	float	dt;
	float 	v[3];
	float	len,angle;
	float	temp[3];
	matrix3x3	ItInv;
	matrix3x3	RT;
	float	x,y,z,s,ca,sa;
	float	glmatrix[16];
	
	dt = DELTA_TIME;

	ItInv = matrix3x3_allocate();
	RT = matrix3x3_allocate();
	matrix3x3_transpose(RT,object.orientation);
	matrix3x3_multiply(ItInv,object.orientation,object.inertiaTensorInverse);
	matrix3x3_multiply(ItInv,ItInv,RT);
	
	// printf("InertiaTensorInverse\n");
	// matrix3x3_print(object.inertiaTensorInverse);
	
	// matrix3x3_print(ItInv);
	// printf("ItInv\n");
	// matrix3x3_print(ItInv);

	// get points in space
	sx = object.size[0];
	sy = object.size[1];
	sz = object.size[2];
	p[0] = sx;
	p[1] = sy;
	p[2] = sz;
	vector3_rigid_transform(pp[0],p,object.orientation,object.position);
	p[0] = -sx;
	p[1] = sy;
	p[2] = sz;
	vector3_rigid_transform(pp[1],p,object.orientation,object.position);
	p[0] = sx;
	p[1] = -sy;
	p[2] = sz;
	vector3_rigid_transform(pp[2],p,object.orientation,object.position);
	p[0] = -sx;
	p[1] = -sy;
	p[2] = sz;
	vector3_rigid_transform(pp[3],p,object.orientation,object.position);
	p[0] = sx;
	p[1] = sy;
	p[2] = -sz;
	vector3_rigid_transform(pp[4],p,object.orientation,object.position);
	p[0] = -sx;
	p[1] = sy;
	p[2] = -sz;
	vector3_rigid_transform(pp[5],p,object.orientation,object.position);
	p[0] = sx;
	p[1] = -sy;
	p[2] = -sz;
	vector3_rigid_transform(pp[6],p,object.orientation,object.position);
	p[0] = -sx;
	p[1] = -sy;
	p[2] = -sz;
	vector3_rigid_transform(pp[7],p,object.orientation,object.position);
	
	// collect forces and torques
	// zero out forces and torques
	f[0] = 0.0;
	f[1] = -9.8;
	f[2] = 0.0;
	t[0] = 0.0;
	t[1] = 0.0;
	t[2] = 0.0;
	// collect force and torque on point by point basis
	for (i=0; i<8; i++) {
		if (pp[i][0] > 0.0) {
			// get vector from Center of Mass to point
			kx = pp[i][0]-object.position[0];
			ky = pp[i][0]-object.position[0];
			kz = pp[i][0]-object.position[0];
			// use some function of position to determine force on point
			fx = -pp[i][0]*FORCE_MAGNITUDE;
			fy = pp[i][0]*FORCE_MAGNITUDE;
			fz = 0.0;
			// accumulate force
			f[0] += fx;
			f[1] += fy;
			f[2] += fz;
			// compute torque
			tx = ky*fz - kz*fy;
			ty = kz*fx - kx*fz;
			tz = kx*fy - ky*fx;
			// accumulate torque
			t[0] += tx;
			t[1] += ty;
			t[2] += tz;
		}
	}
	
	#ifdef DEBUG_PROCESS
	// print forces
	printf("force and torque\n");
	printf("%f %f %f\n",f[0],f[1],f[2]);
	printf("%f %f %f\n",t[0],t[1],t[2]);
	#endif
	
	// update linear momentum
	object.momentum[0] += f[0]*dt;
	object.momentum[1] += f[1]*dt;
	object.momentum[2] += f[2]*dt;
	
	// update angular momentum;
	object.angularMomentum[0] += t[0]*dt;
	object.angularMomentum[1] += t[1]*dt;
	object.angularMomentum[2] += t[2]*dt;
	
	#ifdef DEBUG_PROCESS
	printf("angular Momentum\n");
	printf("%f %f %f\n",
	object.angularMomentum[0],object.angularMomentum[1],object.angularMomentum[2]);
	#endif
	
	
	#ifdef DEBUG_ORIENTATION
	printf("\n");
	// printf axis angle
	printf("angular momentum\n");
	printf(" %f %f %f\n",
		object.angularMomentum[0],object.angularMomentum[1],object.angularMomentum[2]);
	printf("intertial tensor inverse (t)\n");
	matrix3x3_print(ItInv);
	#endif
	
	// get angular velocity
	matrix3x3_multiply_vector(v,ItInv,object.angularMomentum);
	
	#ifdef DEBUG_PROCESS
	printf("angular velocity\n");
	printf("%f %f %f\n",v[0],v[1],v[2]);
	#endif
	
	angle = vector3_norm(v);
	v[0] /= angle;
	v[1] /= angle;
	v[2] /= angle;
	angle *= dt;
	
	#ifdef DEBUG_PROCESS
	// printf axis angle
	printf("axis-angle\n");
	printf(" (%f %f %f), %f\n",v[0],v[1],v[2],angle);
	#endif
	
	#ifdef DEBUG_ORIENTATION
	// printf axis angle
	printf("axis-angle\n");
	printf(" (%f %f %f), %f\n",v[0],v[1],v[2],angle);
	#endif
	
	#ifdef DEBUG_PROCESS
	// print orientation matrix
	printf("orientation - before\n");
	matrix3x3_print(object.orientation);
	#endif
	#ifdef DEBUG_ORIENTATION
	// print orientation matrix
	printf("orientation - before\n");
	matrix3x3_print(object.orientation);
	#endif
	
	
	#ifdef DO_ROTATION
	// update orientation matrix
	temp[0] = object.orientation[0][0];
	temp[1] = object.orientation[1][0];
	temp[2] = object.orientation[2][0];
	#ifdef DEBUG_PROCESS
	printf("\nvector before\n");
	printf("%f %f %f\n",temp[0],temp[1],temp[2]);
	#endif
	vector3_axisangle_rotate(temp,temp,v,angle);
	#ifdef DEBUG_PROCESS
	printf("vector after\n");
	printf("%f %f %f\n",temp[0],temp[1],temp[2]);
	#endif
	object.orientation[0][0] = temp[0];
	object.orientation[1][0] = temp[1];
	object.orientation[2][0] = temp[2];

	temp[0] = object.orientation[0][1];
	temp[1] = object.orientation[1][1];
	temp[2] = object.orientation[2][1];
	#ifdef DEBUG_PROCESS
	printf("\nvector before\n");
	printf("%f %f %f\n",temp[0],temp[1],temp[2]);
	#endif
	vector3_axisangle_rotate(temp,temp,v,angle);
	#ifdef DEBUG_PROCESS
	printf("vector after\n");
	printf("%f %f %f\n",temp[0],temp[1],temp[2]);
	#endif
	object.orientation[0][1] = temp[0];
	object.orientation[1][1] = temp[1];
	object.orientation[2][1] = temp[2];

	temp[0] = object.orientation[0][2];
	temp[1] = object.orientation[1][2];
	temp[2] = object.orientation[2][2];
	#ifdef DEBUG_PROCESS
	printf("\nvector before\n");
	printf("%f %f %f\n",temp[0],temp[1],temp[2]);
	#endif
	vector3_axisangle_rotate(temp,temp,v,angle);
	#ifdef DEBUG_PROCESS
	printf("vector after\n");
	printf("%f %f %f\n",temp[0],temp[1],temp[2]);
	#endif
	object.orientation[0][2] = temp[0];
	object.orientation[1][2] = temp[1];
	object.orientation[2][2] = temp[2];
	#endif
	
	// get linear velocity
	v[0] = object.momentum[0]/object.mass;
	v[1] = object.momentum[0]/object.mass;
	v[2] = object.momentum[0]/object.mass;
	
	// update position
	object.position[0] += v[0]*dt;
	object.position[1] += v[1]*dt;
	object.position[2] += v[2]*dt;
	
	#ifdef DEBUG_PROCESS
	// print orientation matrix
	printf("orientation - after\n");
	matrix3x3_print(object.orientation);
	#endif
	
	#ifdef DEBUG_ORIENTATION
	// print orientation matrix
	printf("orientation - after\n");
	matrix3x3_print(object.orientation);
	#endif
	
	#ifdef DEBUG_DISPLAY
	// print orientation matrix
	printf("orientationr\n");
	matrix3x3_print(object.orientation);
	// print scales
	printf("translation\n");
	printf("%f %f %f\n",object.position[0],object.position[1],object.position[2]);
	// print scales
	printf("scales\n");
	printf("%f %f %f\n",sx,sy,sz);
	#endif
	
	glPushMatrix();
	glmatrix[0] = object.orientation[0][0];
	glmatrix[1] = object.orientation[1][0];
	glmatrix[2] = object.orientation[2][0];
	glmatrix[3] = 0.0;
	glmatrix[4] = object.orientation[0][1];
	glmatrix[5] = object.orientation[1][1];
	glmatrix[6] = object.orientation[2][1];
	glmatrix[7] = 0.0;
	glmatrix[8] = object.orientation[0][2];
	glmatrix[9] = object.orientation[1][2];
	glmatrix[10] = object.orientation[2][2];
	glmatrix[11] = 0.0;
	glmatrix[12] = 0.0;
	glmatrix[13] = 0.0;
	glmatrix[14] = 0.0;
	glmatrix[15] = 1.0;
	glTranslatef(object.position[0],object.position[1],object.position[2]);
	glMultMatrixf(glmatrix);
	glScalef(sx,sy,sz);
	draw_cube();
	glPopMatrix();
	
	#ifdef DEBUG_PROCESS
	scanf("%d");
	#endif
	
}