#include <time.h>
#include "../common/vector.h"
#include "../common/misc.h"
#include "simulation.h"
#include "solvers.h"
#include "../common/debug.h"

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

//vector b_pos;
//vector b_vel;
//vector b_accl;

//vector *bullet_pos = &b_pos;;
//vector *bullet_vel = &b_vel;
//vector *bullet_accl = &b_accl;


int running = 0;
double gravity = -9.81;
double powder = .5;
double air_friction = 10;
double bullet_mass = 100;
double cannon_azimuth = 0;
double cannon_elevation = 45;

extern int current_sim;
extern int sim_type;

#define CAM main_scene->camera

void translate_camera(vector *v)
{
	add_vectors(&CAM->lookP, &CAM->lookP, v);
	add_vectors(&CAM->pos, &CAM->pos, v);
}


void calc_cannon_force(vector *drag, vector *v)
{
	copy_vector(v, drag);
	multiply_vector(drag, drag, -air_friction);
}

void calc_cannon_acceleration(state *new_state, state *old_state)
{
	vector force;

	calc_cannon_force(&force, &old_state->velocity);

	//acceleration
	multiply_vector(&new_state->velocity, &force, 1/bullet_mass);
	new_state->velocity.y += gravity;
}



void run_cannon()
{
	vector old_position;
	vector translate_v;
	
	copy_vector(&current_state->position, &old_position);
	
	if(current_sim == RK4 && running)
		run_rk4((void* (*)(state*, state*))&calc_cannon_acceleration, current_state);
	else if(current_sim == EULER && running)
		run_euler((void* (*)(state*, state*))&calc_cannon_acceleration, current_state);
	else if(current_sim == MIDPOINT && running)
		run_midpoint((void* (*)(state*, state*))&calc_cannon_acceleration, current_state);

	copy_vector(&current_state->position, &BULLET_OBJ->pos);

	//if moving, print new values
	if(distance_between(&current_state->position, &old_position) > EPSILON)
	{
		capture_state(current_state);
	}
	
	//translate camera
	sub_vectors(&translate_v, &current_state->position, &main_scene->camera->lookP);
	translate_camera(&translate_v);
	
	//mark the ending
	if (current_state->position.y < -EPSILON)
	{
		MARKER_OBJ->pos.x = current_state->position.x;
		MARKER_OBJ->pos.z = current_state->position.z;
		translate_camera(sub_vectors(&translate_v, &MARKER_OBJ->pos, &main_scene->camera->lookP));
		running = 0;
	}
}

void recalc_cannon()
{
	double L;
	double b, Lx, Ly, Lz;
	double cosX,  cosY, cosZ;
	
	L = CANNON_OBJ->dist;
	b = L * cos((cannon_elevation / 180) * 3.14);
	Lx = b * cos(cannon_azimuth /180 * 3.14);
	Ly = L * cos((90 - cannon_elevation) / 180 * 3.14);
	Lz = b * sin(cannon_azimuth / 180 * 3.14);
	
	cosX = Lx/L;
	cosY = Ly/L;
	cosZ = Lz/L;

	CANNON_OBJ->norm.x = cosX;
	CANNON_OBJ->norm.y = cosY;
	CANNON_OBJ->norm.z = cosZ;
	//xe = L * cos(cannon_elevation /180 * 3.14) * cos(cannon_azimuth /180 * 3.14);
	//ze = L * cos(cannon_elevation /180 * 3.14) * sin(cannon_azimuth /180 * 3.14);
}


void reset_cannon_state()
{
	recalc_cannon();
	multiply_vector(&current_state->position, &current_state->position, 0);
	multiply_vector(&current_state->velocity, &current_state->position, 0);
	running = 0;
}

double initial_velocity()
{
	return (10000 * powder) / bullet_mass;  //mass in kg, 10000N * number of kg powder
}


void fire_cannon()
{
	//clear old values
	reset_cannon_state();
	
	copy_vector(normalize(&CANNON_OBJ->norm), &current_state->velocity);
	multiply_vector(&current_state->velocity, &current_state->velocity, initial_velocity());
	running = 1;
	
	copy_vector(&CANNON_OBJ->pos, &current_state->position);
	
	//bullet_accl->x = 0;
	//bullet_accl->y = gravity;
	//bullet_accl->z = 0;
}

