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

extern scene_data* main_scene;
extern double sim_step_size;

//puts the new acceleration in state.velocity and the 
//new velocity in state.position
void calc_accl_and_vel(void* (*calc_acceleration) (state *new_state, state *old_state), 
					   state *new_state, state *old_state)
{
	//get new acceleration (will become velocity)
	calc_acceleration(new_state, old_state);
	
	//get new velocity (will become position)
	copy_vector(&old_state->velocity, &new_state->position);
}

void run_euler(void* (*calc_acceleration) (state *new_state, state *old_state), 
			   state *old_state)
{
	state new_state;
	
	//get new acceleration and velocity
	calc_accl_and_vel(calc_acceleration, &new_state, old_state);

	//get the new deltas
	multiply_state(&new_state, &new_state, sim_step_size);

	//add deltas to old values
	add_states(old_state, &new_state, old_state);
}


void run_midpoint(void* (*calc_acceleration) (state *new_state, state *old_state), 
				  state *old_state)
{
	state midpoint;
	state new_state;
	
	//get new acceleration and velocity
	calc_accl_and_vel(calc_acceleration, &midpoint, old_state);
	//get the new deltas (midpoint)
	multiply_state(&midpoint, &midpoint, sim_step_size/2.0);
	//add deltas to old values (to get midpoint)
	add_states(&midpoint, &midpoint, old_state);

	//recalc acceleration/velocity at midpoint
	calc_accl_and_vel(calc_acceleration, &new_state, &midpoint);
	//get the new deltas
	multiply_state(&new_state, &new_state, sim_step_size);	
	//add deltas to old values
	add_states(old_state, &new_state, old_state);
}

void run_rk4(void* (*calc_acceleration) (state *new_state, state *old_state), 
				  state *old_state)
{
	state k1, k2, k3, k4;
	state temp_k;
	
	//get values for k1
	calc_accl_and_vel(calc_acceleration, &k1, old_state);
	multiply_state(&k1, &k1, sim_step_size);
	
	
	//get values for k2
	multiply_state(&temp_k, &k1, 0.5);
	add_states(&temp_k, &temp_k, old_state);
	calc_accl_and_vel(calc_acceleration, &k2, &temp_k);
	multiply_state(&k2, &k2, sim_step_size);
	
	
	//get values for k3
	multiply_state(&temp_k, &k2, 0.5);
	add_states(&temp_k, &temp_k, old_state);
	calc_accl_and_vel(calc_acceleration, &k3, &temp_k);
	multiply_state(&k3, &k3, sim_step_size);
	
	
	//get values for k4
	multiply_state(&temp_k, &k3, 1);
	add_states(&temp_k, &temp_k, old_state);
	calc_accl_and_vel(calc_acceleration, &k4, &temp_k);
	multiply_state(&k4, &k4, sim_step_size);
	
	
	//finally, scale all the values
	multiply_state(&k1, &k1, 1.0/6.0);
	multiply_state(&k2, &k2, 1.0/3.0);
	multiply_state(&k3, &k3, 1.0/3.0);
	multiply_state(&k4, &k4, 1.0/6.0);

	//and sum them
	add_states(&temp_k, &k2, &k1);
	add_states(&temp_k, &temp_k, &k3);
	add_states(&temp_k, &temp_k, &k4);
	
	//tack them on to the original values and pray it works
	add_states(old_state, &temp_k, old_state);
}


