/*
 * Multiple bodies problem
 */

#include <v_lib.h>

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

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

#define		NUM_OBJECTS		3
ph_obj_t	obj[NUM_OBJECTS];

/*
 * F-vect = radius-vect * m1 * m2 / abs(radius-vect) ^ 3
 */
void gravity_force(ph_obj_t *subj,ph_obj_t *obj,vec3_t force_to_subj)
{
	vec3_t dr;
	vec_t r, rr, f;
	vec3Sub(dr,obj->pos,subj->pos);
	r = vec3Abs(dr);
	rr = 1/r;
	vec3Scale(dr,dr,rr);
	f = subj->mass * obj->mass / (r * r);
	vec3Scale(force_to_subj,dr,f);
}

/*
 * For each pairs of objects, compute force and accumulate it
 */
void compute_forces(void)
{
	vec3_t force;
	int i,j,lastobj;
	for(i=0;i<NUM_OBJECTS;i++)
		vec3Zero(obj[i].force);
	lastobj = NUM_OBJECTS-1;
	for(i=0;i<lastobj;i++)
	{
		for(j=i+1;j<NUM_OBJECTS;j++)
		{
			gravity_force(&obj[i],&obj[j],force);
			vec3Add(obj[i].force,obj[i].force,force);
			vec3Sub(obj[j].force,obj[j].force,force);
		}
	}
}

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

/*
 * Computes full energy of system
 */
vec_t full_energy(void)
{
	int i, j;
	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_squared = vec3Dot(obj[i].vel,obj[i].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,obj[j].pos,obj[i].pos);
			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(obj[0].pos,0,0,0);
	vec3Set(obj[0].vel,0,0,0);

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

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

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

/*
 * -----------------------------------------------------------------------
 * Part 3: DE solver
 */

/*
 * Dumb implementation of Euler method on non-adaptive time-domain grid.
 * Not working, of course :)
 */

void euler_step(vec_t timediff)
{
	int i;
	vec3_t v;
	compute_forces();
	for(i=0;i<NUM_OBJECTS;i++)
	{
		vec3Scale(v,obj[i].force,obj[i].rm * timediff);
		vec3Add(obj[i].vel,obj[i].vel,v);
		vec3Scale(v,obj[i].vel,timediff);
		vec3Add(obj[i].pos,obj[i].pos,v);
	}
}

/*
TODO
void solver_init()
{
}

void solver_step()
{
}

void solver_shutdown()
{
}
*/

/*
 * -----------------------------------------------------------------------
 * 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++)
		{
			glVertex3f(p[0]+i*dp[0],p[1],p[2]);
			glVertex3f(p[0]+i*dp[0],p[1]+height,p[2]);
		}

		/* grid with lines along Ox direction */
		for(i=0;i<num_lines;i++)
		{
			glVertex3f(p[0],p[1]+i*dp[1],p[2]);
			glVertex3f(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;
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);

	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);

	glLineWidth(1.5f);
	draw_xOy_grid(neworg,width,height,minor_lines);
	glLineWidth(3.5f);
	draw_xOy_grid(neworg,width,height,major_lines);

	glDisable(GL_BLEND);
}

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();
	glTranslatef(0,char_height-1,0);

	glColor4f(1,1,1,1);
	glLineWidth(1);
	glDisable(GL_LINE_SMOOTH);
	glDisable(GL_BLEND);
}

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

void draw_status()
{
	begin_overlay(30,25);
	vglPrintf("Full energy: %4.5g\n",full_energy());
	end_overlay();
}

void draw(void)
{
	int i;
	vec3_t mins, maxs, origin, center;
	vec_t scale;
	glClear(GL_COLOR_BUFFER_BIT);
    glLoadIdentity();

	/* Compute bounding box */
	bboxClear(mins,maxs);
	for(i=0;i<NUM_OBJECTS;i++)
		bboxAddPoint(mins,maxs,obj[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);
    glScalef(scale,scale,scale);
	glTranslatef(-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);
		GL_VERTEX3V(origin);
	glEnd();
	/* Draw masses */
	glPointSize(6);
	glColor3f(0,1,1);
	glBegin(GL_POINTS);
		for(i=0;i<NUM_OBJECTS;i++)
			GL_VERTEX3V(obj[i].pos);
	glEnd();
	/* Draw status */
	draw_status();
	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;
			}
			break;
		default:
			break;
	}
	return false;
}

void main_loop(void)
{
	int i;
	input_record_t in;
	for(;;)
	{
		while(vglPending())
		{
			vglNextEvent(&in);
			if(handle_event_check_break(&in))
				goto good_bye;
		}
		for(i=0;i<10;i++)
			euler_step(0.0005f);
		draw();
	}
good_bye:;
}

int main()
{
	logStartConsoleLog();
	logUpperLevel(LOG_TRACE);
	vglCreateWindow("Initial value problem for multiple masses",0,0,512,512);
	vglLoadDefaultFont();
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(-1,1,-1,1,-1,1);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glEnable(GL_POINT_SMOOTH);
	glAlphaFunc(GL_GREATER,0.5f);
	glEnable(GL_ALPHA_TEST);
	glEnable(GL_LINE_SMOOTH);

	initial_value();
	main_loop();

	vglDestroyWindow(vglGetCurrentWindow());

	return 0;
}
