/*
 * Multiple bodies problem
 */

#include <v_lib.h>
#include <v_imath.h>

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

typedef struct particle_s
{
	vec3_t	force;		/* force acting to this particle */
	vec_t	mass;		/* gravital mass */
	vec_t	rm;			/* reciprocal inertial mass */
}	particle_t, particle_p;

#define		NUM_PARTICLES		5

particle_t	objects[NUM_PARTICLES];
/*
 * Phase: all positions and velocities, interleaved.
 */
vec_t		initial_phase[6 * NUM_PARTICLES];
vec_p		phase = initial_phase;

#define		PH_POS(i)	(phase + 6 * (i))		/* position of i-th particle */
#define		PH_VEL(i)	(phase + 6 * (i) + 3)	/* velocity of i-th particle */
/*
 * force[i] contains the force acting to particle i.
 */
vec3_t		forces[NUM_PARTICLES];

/*
 * Newton's law of gravitation.
 * F = r12 * m1 * m2 / abs(r12) ^ 3
 */
static void
gravity_force(vec3_t f12, vec3_t r12, vec_t m1, vec_t m2)
{
	vec_t abs_r12_3 = vec3Abs(r12);
	abs_r12_3 = abs_r12_3 * abs_r12_3 * abs_r12_3;
	vec3Scale(f12,r12,m1 * m2 / abs_r12_3);
}

/*
 * Computation of forces: part of the SODE right side routine.
 */
static void compute_forces(void)
{
	vec3_t fij, rij;
	int i, j, lastobj;
	for(i = 0;i < NUM_PARTICLES;i++)
		vec3Zero(forces[i]);
	lastobj = NUM_PARTICLES - 1;
	for(i = 0;i < lastobj;i++)
	{
		for(j = i + 1;j < NUM_PARTICLES;j++)
		{
			vec3Sub(rij,PH_POS(j),PH_POS(i));
			gravity_force(fij,rij,objects[i].mass,objects[j].mass);
			vec3Add(forces[i],forces[i],fij);
			vec3Sub(forces[j],forces[j],fij);
		}
	}
}

/*
 * -----------------------------------------------------------------------
 * Part 2: application of initial conditions
 */
vec_t initial_energy;

static vec_t full_energy();

static void
initial_conditions(void)
{
	int i;

    /*
     * Setup masses, positions and velocities.
     */
    
	objects[0].mass = 5;
	vec3Set(PH_POS(0),0,0,0);
	vec3Set(PH_VEL(0),0,0,0);

	objects[1].mass = 1;
	vec3Set(PH_POS(1),1,1,1);
	vec3Set(PH_VEL(1),-1,0,0);

	objects[2].mass = 1;
	vec3Set(PH_POS(2),-1,1,1);
	vec3Set(PH_VEL(2),0,-1,0);

	objects[3].mass = 1;
	vec3Set(PH_POS(3),-1,-1,-1);
	vec3Set(PH_VEL(3),1,0,0);

	objects[4].mass = 1;
	vec3Set(PH_POS(4),1,-1,-1);
	vec3Set(PH_VEL(4),0,1,0);

	/* setup reciprocal inertial masses */
    for(i = 0;i < NUM_PARTICLES;i++)
		objects[i].rm = (vec_t)(1.0 / objects[i].mass);

	/* compute initial energy of the system */
	initial_energy = full_energy();
}

/*
 * -----------------------------------------------------------------------
 * Part 3: Interface with SODE solver
 */

void V_CALL right_side(void *arg,vec_t deriv[],vec_t t,vec_t ph[])
{
	unsigned i;
	vec_t *vel = ph + 3;
	vec_t *dx_dt = deriv, *dv_dt = deriv + 3;
	phase = ph;
	/*
	 * Note: in real program we should pass both dimension
	 * (NUM_PARTICLES), masses and forces via pointer to struct
	 * stored in 'arg'.
	 */
	compute_forces();
	for(i = 0;i < NUM_PARTICLES;i++)
	{
		vec3Copy(dx_dt,vel); /* dx / dt = v */
		/* Newton's second law: dv / dt = F / m */
		vec3Scale(dv_dt,forces[i],objects[i].rm);
		vel += 6;
		dx_dt += 6;
		dv_dt += 6;
	}
}

/*
 * -----------------------------------------------------------------------
 * Part 4: Monitoring
 */

/*
 * Computes full energy of system
 */
static vec_t
full_energy()
{
	int i, j;
	vec_t *vel = PH_VEL(0);
	vec_t vel_squared, r, potential, energy = 0;
	vec3_t rji;
	/*
	 * Kinetic energy: summ_i(m_i * v_i ^ 2) / 2
	 */
	for(i = 0;i < NUM_PARTICLES;i++,vel += 6)
	{
		vel_squared = vec3Dot(vel,vel);
		energy += objects[i].mass * vel_squared;
	}
	/* Potential energy */
	for(i = 0;i < NUM_PARTICLES;i++)
	{
		potential = 0;
		for(j = 0;j < NUM_PARTICLES;j++)
		{
			if(i == j)
				continue;
			vec3Sub(rji,PH_POS(j),PH_POS(i));
			r = vec3Abs(rji);
			potential += objects[j].mass / r;
		}
		energy -= potential * objects[i].mass;
	}
	energy *= (vec_t)0.5;
	return energy;
}

vec_t max_diff = 0;
vec_t energy, diff;

static void solver_status()
{
	energy = full_energy();
	diff = (vec_t)fabs(energy - initial_energy);
	if(max_diff < diff)
		max_diff = diff;
}

u32_t last_draw_tick;
vec_t simulation_time;

static vboolean handle_event_check_break(ui_event_p ev);
static void draw(void);

/*
 * Called from SODE solver.
 */
vboolean V_CALL monitor(void *arg,vec_t t,vec_t *x)
{
	u32_t tick;
	ui_event_t ev;
	simulation_time = t;
	while(uiPending())
	{
		uiNextEvent(&ev);
		if(handle_event_check_break(&ev))
			return false;
	}
	tick = timeTickCount();
	/* limit FPS to 33 */
	if(tick - last_draw_tick > 30)
	{
		solver_status();
		draw();
		last_draw_tick = tick;
	}
	return true;
}

/*
 * -----------------------------------------------------------------------
 * Part 5: drawing stuff
 */

static 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 */
}

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

static 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 */
}

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

static void
draw_status()
{
	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, beta = 0;

static void draw()
{
	int i;
	vec3_t mins, maxs, origin, center, projz;
	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_PARTICLES;i++)
		bboxAddPoint(mins,maxs,PH_POS(i));
	/* 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(beta,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 points */
	glPointSize(6);
	glColor3f(0,1,1);
	glBegin(GL_POINTS);
		for(i = 0;i < NUM_PARTICLES;i++)
			VGL_VERTEX3V(PH_POS(i));
	glEnd();

#ifdef	AA_ENABLED
	glLineWidth(1);
#endif /* AA_ENABLED */
	glBegin(GL_LINES);
		for(i = 0;i < NUM_PARTICLES;i++)
		{
			vec3Copy(projz,PH_POS(i));
			VGL_VERTEX3V(projz);
			projz[2] = 0;
			VGL_VERTEX3V(projz);
		}
	glEnd();
	/* Draw status */
	draw_status();
	vglSwap();
}

/*
 * -----------------------------------------------------------------------
 * Part 5: UI
 */
static vboolean
handle_event_check_break(ui_event_p ev)
{
	switch(ev->type)
	{
		case ui_key_event:
			switch(ev->key.scan)
			{
				case sc_escape:
					return true;
				case sc_up:
					alpha += 10;
					break;
				case sc_down:
					alpha -= 10;
					break;
				case sc_left:
					beta += 10;
					break;
				case sc_right:
					beta -= 10;
					break;
				case sc_space:
					alpha = beta = 0;
					break;
			}
			break;
		default:
			break;
	}
	return false;
}

int main(int argc,char *argv[])
{
	int i;
	vhandle_t solver;
	vec_t diff_en_limit = 1e6;
	vec_t timelimit = 1e6;
	for(i = 1;i < argc;i++)
	{
		char *cmd = argv[i];
		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_conditions();

	solver = odeCreateSolver(6 * NUM_PARTICLES,right_side,false);
	odeSetReal(solver,ODE_TIMEOUT,20);
	odeSetMonitor(solver,monitor,NULL,0.0005,true);

	vglInit("Initial value problem for multiple masses (RKEMBED)",
		0,0,512,512,
		false,false,
		false,false,false);
	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 */

	odeIntegrate(solver,phase,0,10000,1e-12,1e-6,1e-15,NULL);
	odeDestroySolver(solver);

	vglDestroy();
	return 0;
}

