/*
 * Multiple bodies problem
 */

#include <v_lib.h>

/*
 * -----------------------------------------------------------------------
 * Part 1: physical model
 */

typedef struct ph_obj_s
{
	vec3_t	force; /* force acting to this object */
	vec_t	mass, rm; /* gravital mass and reciprocal inertial mass */
}	ph_obj_t;

typedef struct ph_obj_x_s
{
	vec3_t	pos;
	vec3_t	vel;
}	ph_obj_x_t;

#define		NUM_OBJECTS		5
ph_obj_t	obj[NUM_OBJECTS + 100];
ph_obj_x_t	depvars[NUM_OBJECTS + 100];

void gravity_force(vec3_t force_ret,int index,vec_t *x)
{
	int i;
	vec3_t dr,df;
	vec_t r, rr, f;
	vec_t *other_pos, *self_pos = &x[index * 6];
	vec3Zero(force_ret);
	for(i=0;i<NUM_OBJECTS;i++)
	{
		if(i == index)
			continue;
		other_pos = &x[i * 6];
		vec3Sub(dr,other_pos,self_pos);
		r = vec3Abs(dr);
		rr = 1 / r;
		vec3Scale(dr,dr,rr);
		f = obj[i].mass * obj[index].mass * rr * rr;
		vec3Scale(df,dr,f);
		vec3Add(force_ret,force_ret,df);
	}
}

/*
 * -----------------------------------------------------------------------
 * Part 2: intial conditons
 */

/*
 * Computes full energy of system
 */
vec_t full_energy(vec_t pos_vel[])
{
	int i, j;
	vec_p vel = &pos_vel[3];
	vec_t vel_squared, r, potential, energy = 0;
	vec3_t r_ji;
	/* Kinetic energy: summ_i(m_i * v_i ^ 2) / 2 */
	for(i=0;i<NUM_OBJECTS;i++,vel+=6)
	{
		vel_squared = vec3Dot(vel,vel);
		energy += obj[i].mass * vel_squared;
	}
	/* Potential energy */
	for(i=0;i<NUM_OBJECTS;i++)
	{
		potential = 0;
		for(j=0;j<NUM_OBJECTS;j++)
		{
			if(i==j)
				continue;
			vec3Sub(r_ji,&pos_vel[j*6],&pos_vel[i*6]);
			r = vec3Abs(r_ji);
			potential += obj[j].mass / r;
		}
		energy -= potential * obj[i].mass;
	}
	energy *= (vec_t)0.5;
	return energy;
}


void initial_value(void)
{
	int i;

    /* initial values for masses, positions and velocities */
    
	obj[0].mass = 5;
	vec3Set(depvars[0].pos,0,0,0);
	vec3Set(depvars[0].vel,0,0,0);

	obj[1].mass = 1;
	vec3Set(depvars[1].pos,1,1,1);
	vec3Set(depvars[1].vel,-1,0,0);

	obj[2].mass = 1;
	vec3Set(depvars[2].pos,-1,1,1);
	vec3Set(depvars[2].vel,0,-1,0);

	obj[3].mass = 1;
	vec3Set(depvars[3].pos,-1,-1,-1);
	vec3Set(depvars[3].vel,1,0,0);

	obj[4].mass = 1;
	vec3Set(depvars[4].pos,1,-1,-1);
	vec3Set(depvars[4].vel,0,1,0);

	/* setup reciprocal inertial masses */
    for(i=0;i<NUM_OBJECTS;i++)
		obj[i].rm = 1.0f / obj[i].mass;
}

/*
 * -----------------------------------------------------------------------
 * Part 3: DE solver
 */
vec_t simulation_time;

void V_CALL right_side(void_p arg,vec_t deriv[],vec_t t,vec_t x[])
{
	u32_t i;
	vec3_t frc;
	vec_t *pos = x, *vel = x+3;
	vec_t *pos_deriv = deriv, *vel_deriv = deriv+3;
	
	for(i=0;i<NUM_OBJECTS;i++)
	{
		gravity_force(frc,i,x);

		vec3Copy(pos_deriv,vel);
		vec3Scale(vel_deriv,frc,obj[i].rm);

		pos += 6;
		vel += 6;
		pos_deriv += 6;
		vel_deriv += 6;
	}
}

vec_t initial_energy;

vec_t max_diff = 0;
vec_t energy, diff;

void solver_status(vec_t *pos_vel)
{
	energy = full_energy(pos_vel);
	diff = energy - initial_energy;
	if(max_diff < v_abs(diff))
		max_diff = v_abs(diff);
}

/*
 * -----------------------------------------------------------------------
 * Part 4: drawing stuff
 */
void draw_xOy_grid(vec3_t origin,vec_t width,vec_t height,u32_t num_lines)
{
	u32_t i;
	vec3_t p, dp;
	vec3Copy(p,origin);
	p[0] -= (vec_t)0.5 * width;
	p[1] -= (vec_t)0.5 * height;
	vec3Set(dp,width,height,0);
	vec3Scale(dp,dp,(vec_t)1 / (num_lines-1));
	glBegin(GL_LINES);
		/* grid with lines along Oy direction */
		for(i=0;i<num_lines;i++)
		{
			VGL_VERTEX3(p[0]+i*dp[0],p[1],p[2]);
			VGL_VERTEX3(p[0]+i*dp[0],p[1]+height,p[2]);
		}

		/* grid with lines along Ox direction */
		for(i=0;i<num_lines;i++)
		{
			VGL_VERTEX3(p[0],p[1]+i*dp[1],p[2]);
			VGL_VERTEX3(p[0]+width,p[1]+i*dp[1],p[2]);
		}
	glEnd(); /* GL_LINES */
}

void draw_2_xOy_grids(vec3_t origin,vec3_t center,vec_t width,vec_t height,u32_t major_lines,u32_t minor_lines)
{
	vec_t dw, dh;
	vec3_t neworg;
	if(minor_lines < 2 || major_lines < 2)
		return;
	dw = width / (major_lines - 1);
	dh = height / (major_lines - 1);
	vec3Copy(neworg,center);
	neworg[0] -= (vec_t)fmod(center[0] - origin[0],dw);
	neworg[1] -= (vec_t)fmod(center[1] - origin[1],dh);

#ifdef AA_ENABLED
	glLineWidth(1.5f);
#endif /* AA_ENABLED */
	draw_xOy_grid(neworg,width,height,minor_lines);
#ifdef AA_ENABLED
	glLineWidth(3.5f);
#endif /* AA_ENABLED */
	draw_xOy_grid(neworg,width,height,major_lines);
}

void begin_overlay(int char_width, int char_height)
{
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glOrtho(0,char_width,0,char_height,-1,1);
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();
	VGL_TRANSLATE(0,char_height-1,0);
	glColor4f(1,1,1,1);
#ifdef AA_ENABLED
	glLineWidth(1);
#endif /* AA_ENABLED */
}

void end_overlay(void)
{
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
}

void draw_status(vec_t pos_vel[])
{
	solver_status(pos_vel);
	begin_overlay(40,30);
	vglPrintf("Time: %4.3g\nFull energy: %4.5g\nDifference: %g\n"
		"Max. en. diff.: %g",
		simulation_time,energy,diff,max_diff);
	end_overlay();
}

GLfloat alpha = 0, betta = 0;

void draw(ph_obj_x_t *depvars)
{
	int i;
	vec3_t mins, maxs, origin, center;
	vec_t scale;
	glClear(GL_COLOR_BUFFER_BIT);
    glLoadIdentity();
    
    glRotatef(alpha,1,0,0);

	/* Compute bounding box */
	bboxClear(mins,maxs);
	for(i=0;i<NUM_OBJECTS;i++)
		bboxAddPoint(mins,maxs,depvars[i].pos);
	/* translate to center all objects */
	vec3Zero(origin);
	vec3Sub(center,mins,maxs);
	scale = 1.0f / vec3Abs(center);
	if(scale > 1)
		scale = 1;
	vec3Add(center,mins,maxs);
	vec3Scale(center,center,0.5f);
    VGL_SCALE(scale,scale,scale);
    glRotatef(betta,0,0,1);
	VGL_TRANSLATE(-center[0],-center[1],-center[2]);
	/* Draw coordsys-aligned grid on xOy plane */

	vec3Set(origin,0,0,0);
	glColor4f(.5f,1.0f,.5f,0.7f);
	draw_2_xOy_grids(origin,center,8,8,5,25);
	glColor4f(1,1,1,0.7f);
	/* Origin */
	glBegin(GL_POINTS);
		VGL_VERTEX3V(origin);
	glEnd();
	/* Draw masses */
	glPointSize(6);
	glColor3f(0,1,1);
	glBegin(GL_POINTS);
		for(i=0;i<NUM_OBJECTS;i++)
			VGL_VERTEX3V(depvars[i].pos);
	glEnd();
#ifdef	AA_ENABLED
	glLineWidth(1);
#endif /* AA_ENABLED */
	glBegin(GL_LINES);
		for(i=0;i<NUM_OBJECTS;i++)
		{
			VGL_VERTEX3V(depvars[i].pos);
			glVertex3f((GLfloat)depvars[i].pos[0],(GLfloat)depvars[i].pos[1],0);
		}
	glEnd();
	/* Draw status */
	draw_status((vec_t*)depvars);
	vglSwapBuffers();
}

/*
 * -----------------------------------------------------------------------
 * Part 5: UI
 */
vboolean handle_event_check_break(input_record_t *in)
{
	switch(in->event)
	{
		case ev_key_down:
			switch(in->first_code)
			{
				case key_escape:
					return true;
				case key_up:
					alpha += 10;
					break;
				case key_down:
					alpha -= 10;
					break;
				case key_left:
					betta += 10;
					break;
				case key_right:
					betta -= 10;
					break;
				case key_space:
					alpha = betta = 0;
					break;
			}
			break;
		default:
			break;
	}
	return false;
}

u32_t last_draw_time;

vboolean V_CALL monitor(void *param,vec_t t,vec_t *x)
{
	u32_t time, wait_time;
	input_record_t in;
	simulation_time = t;
	while(vglPending())
	{
		vglNextEvent(&in);
		if(handle_event_check_break(&in))
			return false;
	}
	time = timeTickCount();
	solver_status(x);
	draw((ph_obj_x_t*)x);
	wait_time = time - last_draw_time;
	if(wait_time < 30)
		timeDelay(30 - wait_time);
	last_draw_time = time;
	return true;
}


void main_loop(void)
{
	u32_t time;
	input_record_t in;
	vhandle_t solver;
	
	initial_value();
	initial_energy = full_energy((vec_t*)depvars);

	solver = odeCreateSolver(6 * NUM_OBJECTS,right_side,false);
	odeSetReal(solver,ODE_TIMEOUT,20);
	odeSetMonitor(solver,monitor,NULL,0.005,true);
	last_draw_time = timeTickCount();
	odeIntegrate(solver,(vec_t*)depvars,0,10000,1e-10,1e-4,1e-15,NULL);
	odeDestroySolver(solver);
}

vboolean nodraw = false;
vboolean noburn = false;
vec_t	diff_en_limit = 1e6;
vec_t	timelimit = 1e6;

vboolean V_CALL monitor2(void *param,vec_t t,vec_t x[])
{
	solver_status(x);
	conPrintf("Sim. time: %4.3g\tFull en.: %4.9g\t"
		"Diff.: %g\t\tMax. diff.: %g\n",
		t,energy,diff,max_diff);
	if(max_diff > diff_en_limit)
	{
		conPrintf("max_diff > diff_el_limit (%g), exiting\n",diff_en_limit);
		return false;
	}
	return true;
}

int main(int argc,char *argv[])
{
	int i;
	for(i=1;i<argc;i++)
	{
		char *cmd = argv[i];
		if(!strcmp(cmd,"nodraw"))
			nodraw = true;
		else if(!strcmp(cmd,"noburn"))
			noburn = true;
		else if(!strcmp(cmd,"diff_en_limit"))
			diff_en_limit = atof(argv[++i]);
		else if(!strcmp(cmd,"timelimit"))
			timelimit = atof(argv[++i]);
	}
	logStartConsoleLog();
	logUpperLevel(LOG_TRACE);
	initial_value();
	if(nodraw)
	{
		int i,st;
		u32_t starttime = timeTickCount();
		vhandle_t solver;
		conPrintf("Diff. en. limit %g\nTimelimit %g\n",diff_en_limit,timelimit);
		initial_value();
		initial_energy = full_energy((vec_t*)depvars);
		solver = odeCreateSolver(6 * NUM_OBJECTS,right_side,false);
		odeSetMonitor(solver,monitor2,NULL,0.5,false);
		odeSetReal(solver,ODE_TIMEOUT,20);
		odeIntegrate(solver,(vec_t*)depvars,0,timelimit,1e-4,1e-4,1e-10,NULL);
	}
	else
	{
		vglCreateWindow(
			"Initial value problem for multiple masses (RKEMBED)",
			0,0,512,512);
		vglLoadDefaultFont();
		glMatrixMode(GL_PROJECTION);
		glLoadIdentity();
		glOrtho(-1,1,-1,1,-1,1);
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
#ifdef	AA_ENABLED
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
		glEnable(GL_POINT_SMOOTH);
		glEnable(GL_LINE_SMOOTH);
#endif	/* AA_ENABLED */
		main_loop();
		vglDestroyWindow(vglGetCurrentWindow());
	}
	solver_status((vec_p)depvars);
	conPrintf("Sim. time: %4.3g\tFull en.: %4.5g\t"
		"Diff.: %g\t\tMax. diff.: %g\n",
		simulation_time,energy,diff,max_diff);
	return 0;

}
