#include <time.h>
#include "../common/vector.h"
#include "../common/matrix.h"
#include "../common/misc.h"
#include "simulation.h"
#include "solvers.h"
#include "../render/intersect/sphere.h"
#include "../common/debug.h"
#include "../common/colors.h"
#include "../parse/parser.h"
#include "../common/list.h"

extern scene_data *main_scene;
extern int running;
extern state *current_state;

#define OCTREE

extern int current_sim;
extern int sim_type;
extern int shadow_cache;

double tessellation_degree;
double max_radius ;
double min_radius;
int num_sims;
int balls_needed;

int num_balls = 0;
int num_interations = 0;
double total_time = 0;
int first_run = 1;

#ifdef OCTREE
octant octants[8];
#endif

#define CAM main_scene->camera
double size_of_cube;

#ifdef OCTREE
//picks the correct octants for a ball
void assign_ball_to_octant(object *obj)
{
	int i;
	int assigned = 0;
	
	//remove from all bins
	for(i=0; i<8; i++)
	{
		list_delete_item(&octants[i].objects, obj);
	}

	//assign to new bins
	for(i=0; i<8; i++)
	{
		if( abs(octants[i].pos.x - obj->pos.x) < size_of_cube / 2.0 + obj->radius &&
			abs(octants[i].pos.y - obj->pos.y) < size_of_cube / 2.0 + obj->radius &&
			abs(octants[i].pos.z - obj->pos.z) < size_of_cube / 2.0 + obj->radius )
		{
			if(list_get_item(&octants[i].objects, obj) == NULL )
			   list_add_item(&octants[i].objects, obj, NULL);
			
			//printd(NORMAL, "in bin[%i]\n", i);
			//change color
			if(assigned)
			{
				make_color(&obj->amb, 250, 250, 250);
				make_color(&obj->diff, 250, 250, 250);
			}
			else
			{
				color_from_number(&obj->amb, i);
				color_from_number(&obj->diff, i);
				assigned = 1;
			}
		}
	}
	//printd(NORMAL, "---\n", i);
	
	if(!assigned)
	{
		make_color(&obj->amb, 0, 0, 0);
		make_color(&obj->diff, 50, 50, 50);
	}
}
#endif

//write the scene data to disk
void save_current_data()
{
	//record time taken
	save_ball_data(num_balls, tessellation_degree, min_radius, max_radius, 
				   (double)total_time/(double)num_interations);
	
	//reset for next interation
	num_interations = 0;
	total_time = 0;
}


/*
 test code
 used to generage the csv files for the graphs
 
void test_tessellate()
{
	int i;
	
	save_current_data();
	
	if(first_run)
	{
		tessellation_degree = 70;
		max_radius = 1.5;
		min_radius = 0.5;
		num_sims = 500;
		balls_needed = 20;

		for(i=0; i<balls_needed; i++)
			add_ball();
		
		first_run = 0;
	}
	
	adjust_tessellation(-1);
	
	if(tessellation_degree > 120 || tessellation_degree == 3)
		exit(0);
}

void test_num_objects()
{
	int i;
	
	save_current_data();

	if(first_run)
	{
		tessellation_degree = 10;
		max_radius = 0.4;
		min_radius = 0.2;
		num_sims = 1000;
		balls_needed = 5;

		first_run = 0;
	}
	
	for(i=0; i<balls_needed; i++)
		add_ball();
	
	if(main_scene->num_objects > 200)
		exit(0);
}

void test_radius()
{
	int i;
	
	save_current_data();
	
	max_radius += 0.05;
	min_radius += 0.05;
	
	if(first_run)
	{
		tessellation_degree = 10;
		max_radius = 0.5;
		min_radius = 0.0;
		num_sims = 1000;
		balls_needed = 10;
		
		first_run = 0;
	}
	
	for(i=0; i<balls_needed; i++)
		remove_ball();
	for(i=0; i<balls_needed; i++)
		add_ball();

	if(max_radius > 3.0)
		exit(0);
}*/

void basic_sim()
{
	int i;
	
	save_current_data();
	
	if(first_run)
	{
		tessellation_degree = 10;
		max_radius = 0.8;
		min_radius = 0.5;
		num_sims = 1;
		balls_needed = 15;
		
		for(i=0; i<balls_needed; i++)
			add_ball();
	
		first_run = 0;
	}
}




//simple ball acceleration
void calc_ball_acceleration(state *new_state, state *old_state)
{
	//no new forces on the balls
	
	//acceleration
	multiply_vector(&new_state->velocity, &new_state->velocity, 0);

}

//create a PQP matrix from a double matrix
void fill_pqp_matrix(PQP_REAL pqp[3][3], double rot[3][3])
{
	pqp[0][0] = rot[0][0];
	pqp[0][1] = rot[0][1];
	pqp[0][2] = rot[0][2];

	pqp[1][0] = rot[1][0];
	pqp[1][1] = rot[1][1];
	pqp[1][2] = rot[1][2];

	pqp[2][0] = rot[2][0];
	pqp[2][1] = rot[2][1];
	pqp[2][2] = rot[2][2];
}

//constructs a PQP model from the objects tessellation
void add_pqp_data(object *obj)
{
	int j;
	PQP_REAL p1[3], p2[3], p3[3];	
	
	obj->pqp_data = new PQP_Model();
	obj->pqp_data->BeginModel();
	
	for(j=0; j<obj->polygon->num_triangles; j++)
	{
		p1[0] = obj->polygon->triangles[j].vertices[0]->x;
		p1[1] = obj->polygon->triangles[j].vertices[0]->y;
		p1[2] = obj->polygon->triangles[j].vertices[0]->z;
		
		p2[0] = obj->polygon->triangles[j].vertices[1]->x;
		p2[1] = obj->polygon->triangles[j].vertices[1]->y;
		p2[2] = obj->polygon->triangles[j].vertices[1]->z;
		
		p3[0] = obj->polygon->triangles[j].vertices[2]->x;
		p3[1] = obj->polygon->triangles[j].vertices[2]->y;
		p3[2] = obj->polygon->triangles[j].vertices[2]->z;
		
		obj->pqp_data->AddTri(p1, p2, p3, j);
	}
	
	obj->pqp_data->EndModel();
}

//plane collision resolution
void resolve_plane_collision(object *obj1, object *obj2, PQP_CollideResult *collision)
{
	vector plane_normal;
	double sphere_speed;
	
	//get the plane's normal
	obj2->normal(&plane_normal, obj2, &plane_normal);

	sphere_speed = vector_length(&obj1->st.velocity);

	normalize(&plane_normal);
	normalize(&obj1->st.velocity);
	
	//reflect sphere's velocity
	reflect_vector(&obj1->st.velocity, &plane_normal, &obj1->st.velocity);
	multiply_vector(&obj1->st.velocity, &obj1->st.velocity, sphere_speed);
	//invert_vector(&obj1->st.velocity);
	
	//reverse rotation
	obj1->st.rot_x = -obj1->st.rot_x;
	obj1->st.rot_y = -obj1->st.rot_y;
	obj1->st.rot_z = -obj1->st.rot_z;
	create_rotation_matrix(obj1->rotation, obj1->st.rot_x, obj1->st.rot_y, obj1->st.rot_z);
	
	obj1->collided = 1;
}

//sphere collision resolution
void resolve_sphere_collision(object *obj1, object *obj2, PQP_CollideResult *collision)
{
	vector normal;
	double speed1;
	
	//find normal by getting difference in position
	sub_vectors(&normal, &obj1->pos, &obj2->pos);
	
	speed1 = vector_length(&obj1->st.velocity);
	
	normalize(&normal);
	normalize(&obj1->st.velocity);
	
	//reflect shere1's velocity
	reflect_vector(&obj1->st.velocity, &normal, &obj1->st.velocity);
	multiply_vector(&obj1->st.velocity, &obj1->st.velocity, speed1);
	//invert_vector(&obj1->st.velocity);
	
	//reverse rotation
	obj1->st.rot_x = -obj1->st.rot_x;
	obj1->st.rot_y = -obj1->st.rot_y;
	obj1->st.rot_z = -obj1->st.rot_z;
	create_rotation_matrix(obj1->rotation, obj1->st.rot_x, obj1->st.rot_y, obj1->st.rot_z);
	
	obj1->collided = 1;
}


void collide_objects(object *obj1, object *obj2)
{
	int i, j;
	static PQP_REAL R1[3][3], R2[3][3], T1[3], T2[3];
	static PQP_CollideResult collision;

	T1[0] = obj1->pos.x;
	T1[1] = obj1->pos.y;
	T1[2] = obj1->pos.z;
	
	T2[0] = obj2->pos.x;
	T2[1] = obj2->pos.y;
	T2[2] = obj2->pos.z;
	
	//R1[0][0] = R1[1][1] = R1[2][2] = 1.0;
	//R1[0][1] = R1[1][0] = R1[2][0] = 0.0;
	//R1[0][2] = R1[1][2] = R1[2][1] = 0.0;
	
	//R2[0][0] = R2[1][1] = R2[2][2] = 1.0;
	//R2[0][1] = R2[1][0] = R2[2][0] = 0.0;
	//R2[0][2] = R2[1][2] = R2[2][1] = 0.0;
	
	fill_pqp_matrix(R1, obj1->rotation);
	fill_pqp_matrix(R2, obj2->rotation);
	
	PQP_Collide(&collision, R1, T1, obj1->pqp_data , R2, T2, obj2->pqp_data, PQP_FIRST_CONTACT);
	
	//debug code
	//printf("obj[%i] and obj[%i]\n", obj_num, j);
	//printf("Num BV tests: %d\n", collision.NumBVTests());
	//printf("Num Tri tests: %d\n", collision.NumTriTests());
	//printf("Num contact pairs: %d\n\n", collision.NumPairs());
	
	//resolve collision if needed
	if(collision.NumPairs() > 0)
	{
		//printd(NORMAL, "collision of obj[%i] and obj[%i]\n", obj_num, j);
		
		//resolve_collision(obj1, obj2, &collision);
		if(obj2->obj_type == SPHERE)
			resolve_sphere_collision(obj1, obj2, &collision);
		if(obj2->obj_type == RECTANGLE)
			resolve_plane_collision(obj1, obj2, &collision);
	}
}

void check_collisions(object *obj1, int obj_num)
{
	int i, j;
	object *obj2;
	
	//check collisions
	for(j=1; j<main_scene->num_objects; j++)
	{
		if(j==obj_num)
			continue; //don't self collide
		
		obj2 = main_scene->models[j];
		
		collide_objects(obj1, obj2);
	}
}

#ifdef OCTREE
void check_octree_collisions(object *obj1, int oct_num, int obj_num)
{
	int i, j;
	object *obj2;

	//check bins for collisions
	for(j=0; j<octants[oct_num].objects.item_count; j++)
	{
		obj2 = (object*)list_get_index(&octants[oct_num].objects, j);
		if(obj1 == obj2)
			continue; //don't self collide

		collide_objects(obj1, obj2);
	}
	
	//check planes for collisions
	for(j=1; j<7; j++)
	{
		obj2 = main_scene->models[j];		
		collide_objects(obj1, obj2);
	}
}
#endif

//perform single simulation cycle
void simulate_balls()
{
	int i, j;
	object *obj;

	//loop over all the spheres
	for(i=7; main_scene->models[i] != NULL; i++)
	{
		if(main_scene->models[i]->obj_type == SPHERE)
		{
			//move and rotate the spheres
			run_euler((void* (*)(state*, state*))&calc_ball_acceleration, &main_scene->models[i]->st);
			copy_vector(&main_scene->models[i]->st.position, &main_scene->models[i]->pos);
			matrix_mul_vector(&main_scene->models[i]->norm, main_scene->models[i]->rotation, &main_scene->models[i]->norm);
			matrix_mul_vector(&main_scene->models[i]->up, main_scene->models[i]->rotation, &main_scene->models[i]->up);
			
			main_scene->models[i]->collided = 0;
#ifdef OCTREE
			assign_ball_to_octant(main_scene->models[i]);
#endif
		}
	}
	
	//collide the spheres
#ifndef OCTREE
	for(i=7; main_scene->models[i] != NULL; i++)
	{
		if(main_scene->models[i]->obj_type == SPHERE)
		{
			check_collisions(main_scene->models[i], i);
		}
	}
#else
	for(i=0; i<8; i++)
	{
		for(j=0; j<octants[i].objects.item_count; j++)
		{
			obj = (object*)list_get_index(&octants[i].objects, j);
			if(obj->collided == 0)
				check_octree_collisions(obj , i, j);
		}
	}
#endif

}

//head function for simulation
void run_balls()
{
	int i;
	double new_time;
	clock_t start_time;
	clock_t end_time;

	//test_num_objects();
	//test_tessellate();
	//test_radius();
	basic_sim();	

	//simulate
	start_time = clock();
	for(i=0; i<num_sims; i++)
		simulate_balls();
	end_time = clock();
	
	num_interations += num_sims;
	new_time = (double)(end_time - start_time);
	total_time += new_time;
}


//checks if a possible sphere position collides with a current sphere
char touching_other_sphere(double new_radius, vector *new_position)
{
	int i;
	double radius_distance;
	double position_distance;
	
	for(i=7; i<main_scene->num_objects; i++) //spheres start at 7
	{
		radius_distance = new_radius + main_scene->models[i]->radius;
		position_distance = distance_between(new_position, &main_scene->models[i]->pos);

		if(radius_distance * 1.2 > position_distance)
			return 1;  //currently colliding
	}
	
	return 0;  //new position is a good position
}


//adds a new ball to the scene
void add_ball()
{
	double new_radius;
	vector new_position;
	int i;
	int max_tries = 2000;
	
	//create a new position
	new_radius = ( (double)rand()/RAND_MAX) * (max_radius-min_radius) + min_radius;
	random_vector(&new_position);
	multiply_vector(&new_position, &new_position, size_of_cube - 1.2*new_radius);
	
	//try to place the ball in the scene
	for(i=0; i<max_tries; i++)
	{
		//if new position is no good, make another one
		if(touching_other_sphere(new_radius, &new_position))
		{
			if(i == max_tries - 1)
				exit(1);

			new_radius = ( (double)rand()/RAND_MAX) * (max_radius-min_radius) + min_radius;
			random_vector(&new_position);
			multiply_vector(&new_position, &new_position, size_of_cube - 1.2*new_radius);
		}
		else
			break;
	}
	
	object *obj = sphere_create();
	
	obj->radius = new_radius;
	copy_vector(&new_position, &obj->pos);
	//multiply_vector(&obj->pos, &obj->pos, 0);
	//if(num_balls < 8)
	//	copy_vector(&bins[num_balls].pos, &obj->pos);
	copy_vector(&obj->pos, &obj->st.position);
	random_vector(&obj->st.velocity);
	
	//debug code
	//multiply_vector(&obj->st.velocity, &obj->st.velocity, 0.1);
	//if(num_balls < 8)
	//	multiply_vector(&obj->st.velocity, &obj->st.velocity, 0);

	obj->st.rot_x = (double)rand()/RAND_MAX;
	obj->st.rot_y = (double)rand()/RAND_MAX;
	obj->st.rot_z = (double)rand()/RAND_MAX;
	
	create_rotation_matrix(obj->rotation, obj->st.rot_x, obj->st.rot_y, obj->st.rot_z);

/*	//debug code
	multiply_vector(&obj->pos, &obj->pos, 0);
	obj->st.velocity.x = 0;
	obj->st.velocity.y = 1;
	obj->st.velocity.z = 0;
	obj->radius = 1;
	multiply_vector(&obj->st.velocity, &obj->st.velocity, 0.1);
*/
	
	//tessellate the new sphere
	obj->polygon = sphere_polygon(obj, (int)tessellation_degree);	
	add_pqp_data(obj);
	
	//add sphere to scene models
	obj->id = main_scene->num_objects;
	//printd(NORMAL, "adding object[%i]\n", obj->id);
	main_scene->models[main_scene->num_objects] = obj;
	main_scene->num_objects++;
	
	color_from_number(&obj->amb, obj->id);
	color_from_number(&obj->diff, obj->id);
				
	num_balls++;
	
#ifdef OCTREE
	assign_ball_to_octant(obj);
#endif
	
	//process_command_string("set_all_shaders objnum");
}

//removes the last added ball from the scene
void remove_ball()
{
	int i;
	object *obj;
	
	//only remove spheres, not the planes
	if(main_scene->num_objects < 8)
		return;
	
#ifdef OCTREE
	//remove from octree
//	for(i=0; i<8; i++)
//		list_delete_item(&octants[i].objects, obj);
	printd(NORMAL, "Cannot delete from octree\n");
	return;
#endif

	obj = main_scene->models[main_scene->num_objects-1];
	//printd(NORMAL, "removing object[%i]\n", obj->id);

	delete obj->pqp_data;
	obj->pqp_data = NULL;
	free(obj->polygon->points);
	free(obj->polygon->triangles);
	free(obj);
	
	//don't want the shadow cache to check this object again
	shadow_cache = -1;
	
	main_scene->models[main_scene->num_objects-1] = NULL;
	main_scene->num_objects--;
	num_balls--;
}


//increases or decreases the tessellation of the spheres
void adjust_tessellation(int delta)
{
	object *obj;
	int i;
	
	tessellation_degree += delta;
	
	if(tessellation_degree < 3)
		tessellation_degree = 3;
	
	//if there are no spheres, we don't retessellate anything
	if(main_scene->num_objects < 8)
		return;
	
	//retessellate all the spheres
	for(i=7; i<main_scene->num_objects; i++)
	{
		
		obj = main_scene->models[i];
		//printd(NORMAL, "retessellating obj[%i]\n", obj->id);
		
		//remove the old tessellation
		delete obj->pqp_data;
		obj->pqp_data = NULL;
		free(obj->polygon->points);
		free(obj->polygon->triangles);
		
		//add the new data
		obj->polygon = sphere_polygon(obj, (int)tessellation_degree);	
		add_pqp_data(obj);
	}
}

//setup the initial scene
void setup_rects()
{
	int i;
#ifdef OCTREE
	double half_cube_size;
#endif
	
	//tessellate all the rectangles in the scene
	for(i=0; main_scene->models[i] != NULL; i++)
	{
		if(main_scene->models[i]->obj_type == RECTANGLE)
		{
			add_pqp_data(main_scene->models[i]);
			size_of_cube = (int) main_scene->models[i]->pos.x + 
						main_scene->models[i]->pos.y + 
						main_scene->models[i]->pos.z;
			size_of_cube = abs(size_of_cube);
			
			main_scene->models[i]->rotation[0][0] = 1;
			main_scene->models[i]->rotation[0][1] = 0;
			main_scene->models[i]->rotation[0][2] = 0;
			main_scene->models[i]->rotation[1][0] = 0;
			main_scene->models[i]->rotation[1][1] = 1;
			main_scene->models[i]->rotation[1][2] = 0;
			main_scene->models[i]->rotation[2][0] = 0;
			main_scene->models[i]->rotation[2][1] = 0;
			main_scene->models[i]->rotation[2][2] = 1;
		}
	}
	
#ifdef OCTREE
	half_cube_size = size_of_cube / 2.0;
	set_tri(&octants[0].pos, half_cube_size, half_cube_size, half_cube_size);
	set_tri(&octants[1].pos, -half_cube_size, half_cube_size, half_cube_size);
	set_tri(&octants[2].pos, half_cube_size, -half_cube_size, half_cube_size);
	set_tri(&octants[3].pos, -half_cube_size, -half_cube_size, half_cube_size);
	set_tri(&octants[4].pos, half_cube_size, half_cube_size, -half_cube_size);
	set_tri(&octants[5].pos, -half_cube_size, half_cube_size, -half_cube_size);
	set_tri(&octants[6].pos, half_cube_size, -half_cube_size, -half_cube_size);
	set_tri(&octants[7].pos, -half_cube_size, -half_cube_size, -half_cube_size);

	list_make(&octants[0].objects, 200);
	list_make(&octants[1].objects, 200);
	list_make(&octants[2].objects, 200);
	list_make(&octants[3].objects, 200);
	list_make(&octants[4].objects, 200);
	list_make(&octants[5].objects, 200);
	list_make(&octants[6].objects, 200);
	list_make(&octants[7].objects, 200);

#endif
}

