#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 */

/*
#define USE_EARLY_OUT
#define DEBUG_EARLY_OUT
*/

V_LOCAL void
apply_binary_forces(particle_system_p ps,u32_t *fail_counter)
{
	vec_p pos1, pos2;
	vec3_t r12, f12;
	vec_t dr, f, size1, size2;
	vec_p phase = ps->cur_phase;
	vec_p dist_ptr = ps->common;
	vec_p c_arg = ps->binary_interaction_arg;
	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);
			*dist_ptr = dr -  size1 - size2;
			if(*dist_ptr < BII_MIN_DIST)
			{
				(*fail_counter)++;
				continue;
			}
			/* call controller */
			f = (*binary_interaction)(ps,c_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 *fail_counter)
{
	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_p dist_ptr = ps->common;
	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];
			*dist_ptr = vec3Dot(plane,pos) - plane[3] - ps->particle_radius[j];
			if(*dist_ptr > BNI_MIN_DIST)
			{
				/* call controller */
				f = (*boundary_force)(ps,c_arg);
				vec3Scale(df,plane,f);
				vec3Add(pf,pf,df);
			}
			else
			{
				/* too close / other side */
				(*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;
			}
			*dist_ptr = v_abs(drsp - sphere[3]) - ps->particle_radius[j];
			if(*dist_ptr < BII_MIN_DIST)
			{
				/* particle too close to boundary: no work can be done */
				(*fail_counter)++;
				continue;
			}
			/* call controller */
			f = (*boundary_force)(ps,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,u32_t *fail_counter)
{
	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,u32_t *fail_counter)
{
	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,u32_t *fail_counter)
{
	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;
	}
}

V_LOCAL vboolean
compute_forces(particle_system_p ps)
{
	u32_t fail_counter = 0;
#ifdef  USE_EARLY_OUT
	vboolean need_reject_step = false;
	for(;;)
	{
		memZero(ps->force,ps->num_particles * sizeof(vec3_t));
		fail_counter = 0;
		apply_binary_forces(ps,&fail_counter);
		if(fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after binary_interaction()\n",
				fail_counter);
			need_reject_step = true;
			break;
		}
		apply_boundary_forces(ps,&fail_counter);
		if(fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after boundary_interaction()\n",
				fail_counter);
			need_reject_step = true;
			break;
		}
		apply_unary_forces(ps,&fail_counter);
		if(fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after unary_interaction()\n",
				fail_counter);
			need_reject_step = true;
			break;
		}
		apply_unary_accels(ps,&fail_counter);
		if(fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after unary_accels()\n",
				fail_counter);
			need_reject_step = true;
			break;
		}
		apply_viscous_forces(ps,&fail_counter);
		if(fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after environment_interaction()\n",
				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",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));
	apply_binary_forces(ps,&fail_counter);
	apply_boundary_forces(ps,&fail_counter);
	apply_unary_forces(ps,&fail_counter);
	apply_unary_accels(ps,&fail_counter);
	apply_viscous_forces(ps,&fail_counter);
	if(fail_counter)
	{
		conlPrintf(LOG_TRACE,
			"compute_forces(): %u fails.\n",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;
	}
}
