#include <v_lib.h>
#include "rh_side.h"
#include "phasesp.h"
#include "psys.h"

#define BII_MIN_DIST 1e-8 /* minimal distance for binary_interaction */
#define BNI_MIN_DIST 1e-8 /* minimal distance for boundary_interaction */

#if 1

/*
#define USE_EARLY_OUT
#define DEBUG_EARLY_OUT
*/

#ifdef	PS_PARALLEL

void do_job_parallel(particle_system_p ps);

void
apply_binary_forces_partial(particle_system_p ps,int *istart, int *sentinel)
{
	vec_p pos1, pos2;
	vec3_t r12, f12;
	vec_t dr, f, size1, size2;
	int *ptr;
	vec_p phase = ps->cur_phase;
	vec_t realdist;
	float_controller_t binary_interaction = ps->binary_interaction;
	vec_p f1, f2;
	int i, j;
	if(!binary_interaction)
		return;
	ptr = istart;
	while(ptr < sentinel);
	{
		i = ptr[0];
		j = ptr[1];
		pos1 = PS_POS(phase,i);
		f1 = ps->force[i];
		size1 = ps->particle_radius[i];
		pos2 = PS_POS(phase,j);
		f2 = ps->force[j];
		size2 = ps->particle_radius[j];
		vec3Sub(r12,pos2,pos1);
		dr = vec3Abs(r12);
		realdist = dr -  size1 - size2;
		if(realdist < BII_MIN_DIST)
		{
			ps->fail_counter++;
		}
		else
		{
			/* call controller */
			f = (*binary_interaction)(ps,realdist,ps->binary_interaction_arg);
			/*
			 * 'f' now contains modulus of the force acting to particle
			 * #1 from particle #2; exploit 3rd Newton's law and update
			 * both forces.
			 */
			vec3Scale(f12,r12,f / dr); /* normalize r12 and than scale by f */
			vec3Add(f1,f1,f12);
			vec3Sub(f2,f2,f12);
		}
		ptr += 2;
	}
}

#else	/* !PS_PARALLEL */

/*
 * FIXME: MODEL WITH GLUE USES SEQUIENTIAL EXECUTION ONLY!
 */

V_LOCAL void
compute_binary_dist(particle_system_p ps)
{
	u32_t i, j;
	u32_t inner_limit = ps->num_particles;
	u32_t outer_limit = ps->num_particles - 1;
	vec_p posi, posj;
	vec_t dr;
	vec3_t rij;
	for(i=0;i<outer_limit;i++)
	{
		posi = PS_POS(ps->cur_phase,i);
		for(j = i+1;j < inner_limit;j++)
		{
			posj = PS_POS(ps->cur_phase,j);
			vec3Sub(rij, posj, posi);
			dr = vec3Abs(rij);
			ps->dist->cell[i][j] = ps->dist->cell[j][i] = dr;
		}
	}
}

V_LOCAL void
apply_binary_forces(particle_system_p ps)
{
	vec_p pos1, pos2;
	vec3_t r12, f12;
	vec_t dr, f, size1, size2;
	vec_p phase = ps->cur_phase;
	vec_t realdist;
	float_controller_t binary_interaction = ps->binary_interaction;
	vec_p f1, f2;
	u32_t i, j;
	u32_t inner_limit = ps->num_particles;
	u32_t outer_limit = ps->num_particles - 1;
	if(!binary_interaction)
		return;
	for(i=0;i<outer_limit;i++)
	{
		pos1 = PS_POS(phase,i);
		f1 = ps->force[i];
		size1 = ps->particle_radius[i];
		for(j=i+1;j<inner_limit;j++)
		{
			pos2 = PS_POS(phase,j);
			f2 = ps->force[j];
			size2 = ps->particle_radius[j];
			vec3Sub(r12,pos2,pos1);
			/*
			dr = vec3Abs(r12);
			*/
			dr = ps->dist->cell[i][j];
			realdist = dr -  size1 - size2;
			if(realdist < BII_MIN_DIST)
			{
				ps->fail_counter++;
				continue;
			}
			/* call controller */
			f = (*binary_interaction)(ps, realdist, ps->binary_interaction_arg);
			/*
			 * 'f' now contains modulus of the force acting to particle
			 * #1 from particle #2; exploit 3rd Newton's law and update
			 * both forces.
			 */
			vec3Scale(f12,r12,f / dr); /* normalize r12 and than scale by f */
			vec3Add(f1,f1,f12);
			vec3Sub(f2,f2,f12);
		}
	}
}

V_LOCAL void
compute_contacts(particle_system_p ps)
{
	vec_p posi, posj;
	vec3_t rij;
	vec_t dr, d1, sizei, sizej, ri, rj;
	vec_p phase = ps->cur_phase;
	u32_t i, j;
	u32_t inner_limit = ps->num_particles;
	u32_t outer_limit = ps->num_particles - 1;
	for(i = 0;i < ps->num_particles;i++)
		ps->contact->cell[i][i] = 0;
	for(i = 0;i < outer_limit;i++)
	{
		posi = PS_POS(phase,i);
		sizei = ps->particle_radius[i];
		for(j = i+1;j < inner_limit;j++)
		{
			posj = PS_POS(phase,j);
			sizej = ps->particle_radius[j];
			vec3Sub(rij,posj,posi);
			/*
			dr = vec3Abs(rij);
			*/
			dr = ps->dist->cell[i][j];
			if(dr >= sizei + sizej + 2 * ps->dglue)
			{
				/* there is no contact */
				ps->contact->cell[i][j] = ps->contact->cell[j][i] = 0;
				continue;
			}
			/* compute surface of contact by eq. (3.15) */
			ri = sizei + ps->dglue;
			rj = sizej + ps->dglue;
			d1 = (ri * ri - rj * rj + dr * dr);
			d1 *= 0.25 * d1 / (dr * dr);
			if(ri * ri < d1)
			{
				puts("Incorrect initial conditions.");
				printf("Distance between centers %g, Sum of sizes %g, ps->dglue = %g\n",
					dr, sizei+sizej, ps->dglue);
				printf("ri * ri %g, d1 %g\n", ri * ri, d1);
				V_ASSERT(0);
			}
			d1 = ri * ri - d1;
			/* V_ASSERT(d1 >= 0); */
			ps->contact->cell[i][j] = M_PI * d1;
			ps->contact->cell[j][i] = ps->contact->cell[i][j];
		}
	}
}

V_LOCAL void
apply_tangent_forces(particle_system_p ps)
{
	vec_p posi, posj;
	vec3_t rij, rij0, vin, vit, fij;
	vec_p fi, vi;
	vec_t dr, f, sizei, sizej;
	vec_p phase = ps->cur_phase;
	u32_t i, j;
	if(ps->hglue == 0)
		return;
	for(i = 0;i < ps->num_particles;i++)
	{
		posi = PS_POS(phase,i);
        vi = PS_VEL(phase,i);
		fi = ps->force[i];
		sizei = ps->particle_radius[i];
		for(j = 0;j < ps->num_particles;j++)
		{
			/*
			if(i == j)
				continue;
			*/
			if(ps->contact->cell[i][j] <= 1e-30)
				continue;
			posj = PS_POS(phase,j);
			sizej = ps->particle_radius[j];
			vec3Sub(rij, posj, posi);
			/*
			dr = vec3Abs(r12);
			*/
			dr = ps->dist->cell[i][j];
			/* compute tangent velocity */
			vec3Scale(rij0, rij, 1 / dr);
			vec3Scale(vin, rij0, vec3Dot(vi, rij0));
			vec3Sub(vit, vi, vin);
			/* (3.16) */
			f = - ps->contact->cell[i][j] * ps->hglue / (dr - sizei - sizej);
			vec3Scale(fij, vit, f);
			vec3Add(fi,fi,fij);
		}
	}
}

#endif	/* !PS_PARRALEL */

/*
 * FIXME: change to use CSG boundary && fixed skeleton
 */
V_LOCAL void
apply_boundary_forces(particle_system_p ps)
{
	u32_t i, j;
	vec_p pos;
	vec3_t df;
	vec3_t rsp;
	vec_t drsp;
	vec_t f;
	vec_p phase = ps->cur_phase;
	vec_t realdist, dz = 0;
	vec_p c_arg, plane, sphere, pf;
	float_controller_t boundary_force;
	for(i=0;i<ps->num_boundary_planes;i++)
	{
		plane = ps->boundary_plane[i];
		boundary_force = ps->plane_interaction[i];
		c_arg = ps->plane_interaction_arg[i];
		for(j=0;j<ps->num_particles;j++)
		{
			pos = PS_POS(phase,j);
			pf = ps->force[j];
			if(ps->deltaz > 1e-20 && v_abs(v_abs(plane[2])-1) < 1e-20)
			{
				/* 50 HZ */
				dz = ps->deltaz * sin(ps->cur_time * 314.16);
			}
			realdist = vec3Dot(plane,pos) - plane[3] + dz - ps->particle_radius[j];
			if(realdist > BNI_MIN_DIST)
			{
				/* call controller */
				f = (*boundary_force)(ps,realdist,c_arg);
				vec3Scale(df,plane,f);
				vec3Add(pf,pf,df);
			}
			else
			{
				/* too close / other side */
				ps->fail_counter++;
				continue;
			}
		}
	}
	for(i=0;i<ps->num_boundary_spheres;i++)
	{
		sphere = ps->boundary_sphere[i];
		boundary_force = ps->sphere_interaction[i];
		c_arg = ps->sphere_interaction_arg[i];
		for(j=0;j<ps->num_particles;j++)
		{
			pos = PS_POS(phase,j);
			pf = ps->force[j];
			vec3Sub(rsp,pos,sphere);
			drsp = vec3Abs(rsp);
			if(drsp < BII_MIN_DIST)
			{
				/* particle too close to center: no work necessary */
				continue;
			}
			if(ps->deltaz > 1e-20)
			{
				/* 50 HZ */
				dz = ps->deltaz * sin(ps->cur_time * 314.16);
			}
			realdist = v_abs(drsp - sphere[3] - dz) - ps->particle_radius[j];
			if(realdist < BII_MIN_DIST)
			{
				/* particle too close to boundary: no work can be done */
				ps->fail_counter++;
				continue;
			}
			/* call controller */
			f = (*boundary_force)(ps,realdist,c_arg);
			/*
			 * 'f' now contains modulus of the force acting from sphere to particle 
			 */
			vec3Scale(df,rsp,- f / drsp);
			vec3Add(pf,pf,df);
		}
	}
}

V_LOCAL void
apply_unary_forces(particle_system_p ps)
{
	vec3_t f;
	vec3_controller_t unary_force = ps->unary_force;
	vec_p pf, unary_force_arg = ps->unary_force_arg;
	if(!ps->unary_force)
		return;
	for(ps->cur_idx=0;ps->cur_idx < ps->num_particles;ps->cur_idx++)
	{
		pf = ps->force[ps->cur_idx];
		vec3Zero(f);
		(*unary_force)(ps,unary_force_arg,f);
		vec3Add(pf,pf,f);
	}
}

V_LOCAL void
apply_unary_accels(particle_system_p ps)
{
	vec3_t a;
	vec3_controller_t unary_accel = ps->unary_accel;
	vec_p pf, unary_accel_arg = ps->unary_accel_arg;
	if(!ps->unary_accel)
		return;
	for(ps->cur_idx=0;ps->cur_idx < ps->num_particles;ps->cur_idx++)
	{
		pf = ps->force[ps->cur_idx];
		vec3Zero(a);
		(*unary_accel)(ps,unary_accel_arg,a);
		vec3Scale(a,a,ps->particle_mass[ps->cur_idx]);
		vec3Add(pf,pf,a);
	}
}

V_LOCAL void
apply_viscous_forces(particle_system_p ps)
{
	vec3_t v;
	vec3_controller_t env_vel = ps->env_velocity;
	vec_p pf, pv, env_vel_arg = ps->env_velocity_arg;
	pv = PS_VEL(ps->cur_phase,0);
	if(!env_vel)
		return;
	for(ps->cur_idx=0;ps->cur_idx < ps->num_particles;ps->cur_idx++)
	{
		pf = ps->force[ps->cur_idx];
		vec3Zero(v);
		(*env_vel)(ps,env_vel_arg,v);
		vec3Sub(v,v,pv);
		vec3Scale(v,v,ps->particle_6_pi_eta_r[ps->cur_idx]);
		vec3Add(pf,pf,v);
		pv += PHASESPACE_DIM;
	}
}

/*
static call_count = 0;
*/

V_LOCAL vboolean
compute_forces(particle_system_p ps)
{
#ifdef  USE_EARLY_OUT
	vboolean need_reject_step;
#endif	/* USE_EARLY_OUT */
	ps->fail_counter = 0;
#ifdef  USE_EARLY_OUT
	need_reject_step = false;
	for(;;)
	{
		memZero(ps->force,ps->num_particles * sizeof(vec3_t));
		compute_binary_dist(ps);
#ifndef	PS_PARALLEL
		apply_binary_forces(ps);
		compute_contacts(ps);
		apply_tangent_forces(ps);
#else	/* PS_PARALLEL */
/*
		do_job_parallel(ps);
*/
#endif	/* PS_PARALLEL */
		if(ps->fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after binary_interaction()\n",
				ps->fail_counter);
			need_reject_step = true;
			break;
		}
		apply_boundary_forces(ps);
		if(ps->fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after boundary_interaction()\n",
				ps->fail_counter);
			need_reject_step = true;
			break;
		}
		apply_unary_forces(ps);
		if(ps->fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after unary_interaction()\n",
				ps->fail_counter);
			need_reject_step = true;
			break;
		}
		apply_unary_accels(ps);
		if(ps->fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after unary_accels()\n",
				ps->fail_counter);
			need_reject_step = true;
			break;
		}
		apply_viscous_forces(ps);
		if(ps->fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after environment_interaction()\n",
				ps->fail_counter);
			need_reject_step = true;
			break;
		}
		break;
	}
	if(need_reject_step)
	{
		if(ps->step_rejection_count == MAX_STEP_REJECTION)
			return false;
#ifdef	DEBUG_EARLY_OUT
		printf("fail_counter: %u; odeRejectStep()\n",ps->fail_counter);
#endif
		odeRejectStep(ps->ode_solver);
		ps->step_rejection_count++;
	}
	else
		ps->step_rejection_count = 0;
	return true;
#else   /* !USE_EARLY_OUT */
	memZero(ps->force,ps->num_particles * sizeof(vec3_t));
	compute_binary_dist(ps);
#ifndef	PS_PARALLEL
	apply_binary_forces(ps);
	compute_contacts(ps);
	apply_tangent_forces(ps);
#else	/* PS_PARALLEL */
	do_job_parallel(ps);
#endif	/* PS_PARALLEL */
	apply_boundary_forces(ps);
	apply_unary_forces(ps);
	apply_unary_accels(ps);
	apply_viscous_forces(ps);
	if(ps->fail_counter)
	{
		conlPrintf(LOG_TRACE,
			"compute_forces(): %u fails.\n",ps->fail_counter);
	}
	return true;
#endif  /* !USE_EARLY_OUT */
}

/*
 * Routine to call from the ODE solver
 */
void V_CALL
problem_right_side(void_p arg,vec_t deriv[],vec_t t,vec_t x[])
{
	u32_t idx;
	particle_system_p ps;
//	vec_p pos = PS_POS(x,0);
	vec_p vel = PS_VEL(x,0);
	vec_p pos_deriv = PS_POS(deriv,0);
	vec_p vel_deriv = PS_VEL(deriv,0);
	ps = (particle_system_p)arg;
	ps->cur_time = t;
	ps->cur_phase = x;
	if(!compute_forces(ps))
		odeTerminateIntegration(ps->ode_solver);
	for(idx=0;idx < ps->num_particles;idx++)
	{
		vec3Scale(vel_deriv,ps->force[idx],ps->particle_reciprocal_mass[idx]);
		vec3Copy(pos_deriv,vel);
//		pos += PHASESPACE_DIM;
		vel += PHASESPACE_DIM;
		pos_deriv += PHASESPACE_DIM;
		vel_deriv += PHASESPACE_DIM;
	}
}

#else /* In case of screwing up the above, here is the backup */

#define BII_MIN_DIST 1e-8 /* minimal distance for binary_interaction */
#define BNI_MIN_DIST 1e-8 /* minimal distance for boundary_interaction */

V_LOCAL void
compute_binary_dist(particle_system_p ps)
{
	u32_t i, j;
	u32_t inner_limit = ps->num_particles;
	u32_t outer_limit = ps->num_particles - 1;
	vec_p posi, posj;
	vec_t dr;
	vec3_t rij;
	for(i=0;i<outer_limit;i++)
	{
		posi = PS_POS(ps->cur_phase,i);
		for(j = i+1;j < inner_limit;j++)
		{
			posj = PS_POS(ps->cur_phase,j);
			vec3Sub(rij, posj, posi);
			dr = vec3Abs(rij);
			ps->dist->cell[i][j] = ps->dist->cell[j][i] = dr;
		}
	}
}

V_LOCAL void
apply_binary_forces(particle_system_p ps)
{
	vec_p pos1, pos2;
	vec3_t r12, f12;
	vec_t dr, f, size1, size2;
	vec_p phase = ps->cur_phase;
	vec_t realdist;
	float_controller_t binary_interaction = ps->binary_interaction;
	vec_p f1, f2;
	u32_t i, j;
	u32_t inner_limit = ps->num_particles;
	u32_t outer_limit = ps->num_particles - 1;
	if(!binary_interaction)
		return;
	for(i=0;i<outer_limit;i++)
	{
		pos1 = PS_POS(phase,i);
		f1 = ps->force[i];
		size1 = ps->particle_radius[i];
		for(j=i+1;j<inner_limit;j++)
		{
			pos2 = PS_POS(phase,j);
			f2 = ps->force[j];
			size2 = ps->particle_radius[j];
			vec3Sub(r12,pos2,pos1);
			/*
			dr = vec3Abs(r12);
			*/
			dr = ps->dist->cell[i][j];
			realdist = dr -  size1 - size2;
			if(realdist < BII_MIN_DIST)
			{
				ps->fail_counter++;
				continue;
			}
			/* call controller */
			f = (*binary_interaction)(ps, realdist, ps->binary_interaction_arg);
			/*
			 * 'f' now contains modulus of the force acting to particle
			 * #1 from particle #2; exploit 3rd Newton's law and update
			 * both forces.
			 */
			vec3Scale(f12,r12,f / dr); /* normalize r12 and than scale by f */
			vec3Add(f1,f1,f12);
			vec3Sub(f2,f2,f12);
		}
	}
}

V_LOCAL void
apply_boundary_forces(particle_system_p ps)
{
	u32_t i, j;
	vec_p pos;
	vec3_t df;
	vec3_t rsp;
	vec_t drsp;
	vec_t f;
	vec_p phase = ps->cur_phase;
	vec_t realdist;
	vec_p c_arg, plane, sphere, pf;
	float_controller_t boundary_force;
	for(i=0;i<ps->num_boundary_planes;i++)
	{
		plane = ps->boundary_plane[i];
		boundary_force = ps->plane_interaction[i];
		c_arg = ps->plane_interaction_arg[i];
		for(j=0;j<ps->num_particles;j++)
		{
			pos = PS_POS(phase,j);
			pf = ps->force[j];
			realdist = vec3Dot(plane,pos) - plane[3] - ps->particle_radius[j];
			if(realdist > BNI_MIN_DIST)
			{
				/* call controller */
				f = (*boundary_force)(ps,realdist,c_arg);
				vec3Scale(df,plane,f);
				vec3Add(pf,pf,df);
			}
			else
			{
				/* too close / other side */
				ps->fail_counter++;
				continue;
			}
		}
	}
	for(i=0;i<ps->num_boundary_spheres;i++)
	{
		sphere = ps->boundary_sphere[i];
		boundary_force = ps->sphere_interaction[i];
		c_arg = ps->sphere_interaction_arg[i];
		for(j=0;j<ps->num_particles;j++)
		{
			pos = PS_POS(phase,j);
			pf = ps->force[j];
			vec3Sub(rsp,pos,sphere);
			drsp = vec3Abs(rsp);
			if(drsp < BII_MIN_DIST)
			{
				/* particle too close to center: no work necessary */
				continue;
			}
			realdist = v_abs(drsp - sphere[3]) - ps->particle_radius[j];
			if(realdist < BII_MIN_DIST)
			{
				/* particle too close to boundary: no work can be done */
				ps->fail_counter++;
				continue;
			}
			/* call controller */
			f = (*boundary_force)(ps,realdist,c_arg);
			/*
			 * 'f' now contains modulus of the force acting from sphere to particle 
			 */
			vec3Scale(df,rsp,- f / drsp);
			vec3Add(pf,pf,df);
		}
	}
}

V_LOCAL void
compute_contacts(particle_system_p ps)
{
	vec_p posi, posj;
	vec3_t rij;
	vec_t dr, d1, sizei, sizej, ri, rj;
	vec_p phase = ps->cur_phase;
	u32_t i, j;
	u32_t inner_limit = ps->num_particles;
	u32_t outer_limit = ps->num_particles - 1;
	for(i = 0;i < ps->num_particles;i++)
		ps->contact->cell[i][i] = 0;
	for(i = 0;i < outer_limit;i++)
	{
		posi = PS_POS(phase,i);
		sizei = ps->particle_radius[i];
		for(j = i+1;j < inner_limit;j++)
		{
			posj = PS_POS(phase,j);
			sizej = ps->particle_radius[j];
			vec3Sub(rij,posj,posi);
			/*
			dr = vec3Abs(rij);
			*/
			dr = ps->dist->cell[i][j];
			if(dr >= sizei + sizej + 2 * ps->dglue)
			{
				/* there is no contact */
				ps->contact->cell[i][j] = ps->contact->cell[j][i] = 0;
				continue;
			}
			/* compute surface of contact by eq. (3.15) */
			ri = sizei + ps->dglue;
			rj = sizej + ps->dglue;
			d1 = (ri * ri - rj * rj + dr * dr);
			d1 *= 0.25 * d1 / (dr * dr);
			if(ri * ri < d1)
			{
				printf("dr = %g, size1+size2 = %g, ps->dglue = %g\n", dr, sizei+sizej, ps->dglue);
				printf("ri * ri %g, d1 %g\n", ri * ri, d1);
				V_ASSERT(0);
			}
			d1 = ri * ri - d1;
			V_ASSERT(d1 >= 0); /* sanity check */
			ps->contact->cell[i][j] = M_PI * d1;
			ps->contact->cell[j][i] = ps->contact->cell[i][j];
		}
	}
}

V_LOCAL void
apply_tangent_forces(particle_system_p ps)
{
	vec_p posi, posj;
	vec3_t rij, rij0, vin, vit, fij;
	vec_p fi, vi;
	vec_t dr, f, sizei, sizej;
	vec_p phase = ps->cur_phase;
	u32_t i, j;
	for(i = 0;i < ps->num_particles;i++)
	{
		posi = PS_POS(phase,i);
        vi = PS_VEL(phase,i);
		fi = ps->force[i];
		sizei = ps->particle_radius[i];
		for(j = 0;j < ps->num_particles;j++)
		{
			/*
			if(i == j)
				continue;
			*/
			if(ps->contact->cell[i][j] <= 1e-30)
				continue;
			posj = PS_POS(phase,j);
			sizej = ps->particle_radius[j];
			vec3Sub(rij, posj, posi);
			/*
			dr = vec3Abs(r12);
			*/
			dr = ps->dist->cell[i][j];
			/* compute tangent velocity */
			vec3Scale(rij0, rij, 1 / dr);
			vec3Scale(vin, rij0, vec3Dot(vi, rij0));
			vec3Sub(vit, vi, vin);
			/* (3.16) */
			f = - ps->contact->cell[i][j] * ps->hglue / (dr - sizei - sizej);
			vec3Scale(fij, vit, f);
			vec3Add(fi,fi,fij);
		}
	}
}

V_LOCAL void
apply_unary_forces(particle_system_p ps)
{
	vec3_t f;
	vec3_controller_t unary_force = ps->unary_force;
	vec_p pf, unary_force_arg = ps->unary_force_arg;
	if(!ps->unary_force)
		return;
	for(ps->cur_idx=0;ps->cur_idx < ps->num_particles;ps->cur_idx++)
	{
		pf = ps->force[ps->cur_idx];
		vec3Zero(f);
		(*unary_force)(ps,unary_force_arg,f);
		vec3Add(pf,pf,f);
	}
}

V_LOCAL void
apply_unary_accels(particle_system_p ps)
{
	vec3_t a;
	vec3_controller_t unary_accel = ps->unary_accel;
	vec_p pf, unary_accel_arg = ps->unary_accel_arg;
	if(!ps->unary_accel)
		return;
	for(ps->cur_idx=0;ps->cur_idx < ps->num_particles;ps->cur_idx++)
	{
		pf = ps->force[ps->cur_idx];
		vec3Zero(a);
		(*unary_accel)(ps,unary_accel_arg,a);
		vec3Scale(a,a,ps->particle_mass[ps->cur_idx]);
		vec3Add(pf,pf,a);
	}
}

V_LOCAL void
apply_viscous_forces(particle_system_p ps)
{
	vec3_t v;
	vec3_controller_t env_vel = ps->env_velocity;
	vec_p pf, pv, env_vel_arg = ps->env_velocity_arg;
	pv = PS_VEL(ps->cur_phase,0);
	if(!env_vel)
		return;
	for(ps->cur_idx=0;ps->cur_idx < ps->num_particles;ps->cur_idx++)
	{
		pf = ps->force[ps->cur_idx];
		vec3Zero(v);
		(*env_vel)(ps,env_vel_arg,v);
		vec3Sub(v,v,pv);
		vec3Scale(v,v,ps->particle_6_pi_eta_r[ps->cur_idx]);
		vec3Add(pf,pf,v);
		pv += PHASESPACE_DIM;
	}
}

static call_count = 0;

V_LOCAL vboolean
compute_forces(particle_system_p ps)
{
	ps->fail_counter = 0;
	memZero(ps->force,ps->num_particles * sizeof(vec3_t));
	compute_binary_dist(ps);
	compute_contacts(ps);
	apply_binary_forces(ps);
	apply_tangent_forces(ps);
	apply_boundary_forces(ps);
	apply_unary_forces(ps);
	apply_unary_accels(ps);
	apply_viscous_forces(ps);
	if(ps->fail_counter)
	{
		conlPrintf(LOG_TRACE,
			"compute_forces(): %u fails.\n",ps->fail_counter);
	}
	return true;
}

/*
 * Routine to call from the ODE solver
 */
void V_CALL
problem_right_side(void_p arg,vec_t deriv[],vec_t t,vec_t x[])
{
	u32_t idx;
	particle_system_p ps;
//	vec_p pos = PS_POS(x,0);
	vec_p vel = PS_VEL(x,0);
	vec_p pos_deriv = PS_POS(deriv,0);
	vec_p vel_deriv = PS_VEL(deriv,0);
	ps = (particle_system_p)arg;
	ps->cur_time = t;
	ps->cur_phase = x;
	if(!compute_forces(ps))
		odeTerminateIntegration(ps->ode_solver);
	for(idx=0;idx < ps->num_particles;idx++)
	{
		vec3Scale(vel_deriv,ps->force[idx],ps->particle_reciprocal_mass[idx]);
		vec3Copy(pos_deriv,vel);
//		pos += PHASESPACE_DIM;
		vel += PHASESPACE_DIM;
		pos_deriv += PHASESPACE_DIM;
		vel_deriv += PHASESPACE_DIM;
	}
}

#endif
