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

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

V_LOCAL vboolean
compute_binary_dist_r(particle_system_p ps,u32_p recursion_counter)
{
	CALLER_NAME(compute_binary_dist_r)
	u32_t i, j;
	vec_p posi, posj;
	vec_t dr, surf_dist;
	vec3_t rij;
	vboolean do_recursion = false;
	for(i = 0;i < ps->num_particles - 1;i++)
	{
		posi = PS_POS(ps->cur_phase,i);
		for(j = i + 1;j < ps->num_particles;j++)
		{
			posj = PS_POS(ps->cur_phase,j);
			vec3Sub(rij, posj, posi);
			dr = vec3Abs(rij);
			surf_dist = dr - ps->particle_radius[j] - ps->particle_radius[i];
			if(NULL != ps->binary_interaction && surf_dist <= BII_MIN_DIST)
			{
				vec_t delta, di, dj, RjRi;
				vec3_t dpi, dpj;
				if(!recursion_counter)
				{
					conlPrintf(LOG_ERROR,"%s(time %g): intersection (%u,%u): r_ij == %g, r_i+r_j == %g (BII_MIN_DIST %g).\n",
							   caller_name,ps->cur_time,i,j,dr,ps->particle_radius[j] + ps->particle_radius[i], BII_MIN_DIST);
					return false;
				}
				/* try to correct positions of particles */
				if(dr <= BII_MIN_DIST)
				{
					conlPrintf(LOG_ERROR,"%s(): centers too close, unable to correct positions of soft particles.\n",
						caller_name);
					return false;
				}
				vec3Scale(rij,rij,1 / dr);
				delta = BII_MIN_DIST * (1 + ps->sp_repulsion_factor) - surf_dist;
				/* small particle moves to higher distance */
				RjRi = ps->particle_radius[j] / ps->particle_radius[i];
				di = RjRi * delta / (1 + RjRi);
				dj = delta - di;
				/*
				 * conlPrintf(LOG_TRACE,"%s(): d_i == %g, d_j == %g.\n",caller_name,di,dj);
				 */
				vec3Scale(dpi,rij,-di);
				vec3Scale(dpj,rij,dj);
				vec3Add(posi,posi,dpi);
				vec3Add(posj,posj,dpj);
				do_recursion = true;
			}
			ps->dist->cell[i][j] = ps->dist->cell[j][i] = dr;
			ps->surf_dist->cell[i][j] = ps->dist->cell[j][i] = surf_dist;
		}
	}
	if(do_recursion)
	{
		*recursion_counter = *recursion_counter + 1;
		if(*recursion_counter >= ps->sp_max_recursions)
		{
			conlPrintf(LOG_ERROR,"%s(): too many recursions (%u).\n",caller_name,*recursion_counter);
			return false;
		}
		return compute_binary_dist_r(ps,recursion_counter);
	}
	return true;
}

V_LOCAL void
apply_binary_forces(particle_system_p ps)
{
	vec_p ri, rj;
	vec_p fi, fj;
	vec3_t rij, fij;
	vec_t f, realdist;
	vec_p phase = ps->cur_phase;
	float_ctrl_p bi = ps->binary_interaction;
	u32_t count = 0, count_max;
	u32_t i, j;
	count_max = ps->num_particles * (ps->num_particles-1) / 2;
	for(i = 0;i < ps->num_particles - 1;i++)
	{
		ri = PS_POS(phase, i);
		fi = ps->force[i];
		/*
		bi_i = ps->binary_interaction[i];
		*/
		for(j = i + 1;j < ps->num_particles;j++)
		{
			realdist = ps->surf_dist->cell[i][j];
			/*
			bi_j = ps->binary_interaction[j];
			if(bi_i == bi_j)
			{
				bi = bi_i;
				if(NULL == bi)
					continue;
				*/
				if(bi->cutoff && realdist >= bi->cutoff_distance)
					continue;
				f = (bi->entry)(bi, realdist);
			/*
			}
			else
			{
				if(NULL == bi_i || (bi_i->cutoff && realdist >= bi_i->cutoff_distance))
				{
					if(NULL == bi_j || (bi_j->cutoff && realdist >= bi_j->cutoff_distance))
						continue;
					else
						f = (bi_j->entry)(bi_j, realdist);
				}
				else
				{
					if(NULL == bi_j || (bi_j->cutoff && realdist >= bi_j->cutoff_distance))
						f = - (bi_i->entry)(bi_i, realdist);
					else
						f = (bi_j->entry)(bi_j, realdist) - (bi_i->entry)(bi_i, realdist);
				}
			}
			*/
			count++;
			fj = ps->force[j];
			rj = PS_POS(phase, j);
			vec3Sub(rij, rj, ri);
			/*
			 * 'f' now contains modulus of the force acting to ith particle
			 * from jth particle; exploit 3rd Newton's law and update
			 * both forces.
			 */
			vec3Scale(fij, rij, f / ps->dist->cell[i][j]); /* normalize rij and than scale by f */
			vec3Add(fi,fi,fij);
			vec3Sub(fj,fj,fij);
		}
	}
	ps->bskip_ratio = 100 * (1 - (vec_t)count / (vec_t)count_max);
}

V_LOCAL vboolean
compute_contacts(particle_system_p ps)
{
	CALLER_NAME(compute_contacts)
	vec_t dr, d1, sizei, sizej, ri, rj;
	u32_t i, j;
	for(i = 0;i < ps->num_particles;i++)
		ps->contact->cell[i][i] = 0;
	for(i = 0;i < ps->num_particles - 1;i++)
	{
		sizei = ps->particle_radius[i];
		for(j = i+1;j < ps->num_particles;j++)
		{
			sizej = ps->particle_radius[j];
			if(ps->surf_dist->cell[i][j] >= ps->dglue[i] + ps->dglue[j])
			{
				/* there is no contact */
				ps->contact->cell[i][j] = ps->contact->cell[j][i] = 0;
				continue;
			}
			dr = ps->dist->cell[i][j];
			/* compute surface of contact */
			ri = sizei + ps->dglue[i];
			rj = sizej + ps->dglue[j];
			d1 = (ri * ri - rj * rj + dr * dr);
			d1 *= 0.25 * d1 / (dr * dr);
			if(ri * ri < d1)
			{
				conlPrintf(LOG_ERROR,"%s(): r_ij %g, r_i+r_j %g, ps->dglue %g & %g.\n",
						   caller_name, dr, sizei + sizej, ps->dglue[i], ps->dglue[j]);
				return false;
			}
			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];
		}
	}
	return true;
}

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 f;
	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];
		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);
			vec3Sub(rij, posj, posi);
			/* compute tangent velocity */
			vec3Scale(rij0, rij, 1 / ps->dist->cell[i][j]);
			vec3Scale(vin, rij0, vec3Dot(vi, rij0));
			vec3Sub(vit, vi, vin);
			/* tangent force */
			f = - ps->contact->cell[i][j] * ps->hglue / ps->surf_dist->cell[i][j];
			vec3Scale(fij, vit, f);
			vec3Add(fi,fi,fij);
		}
	}
}

/*
 * FIXME: change to use CSG boundary && fixed skeleton
 */

V_LOCAL vboolean
apply_boundary_forces(particle_system_p ps)
{
	CALLER_NAME(apply_boundary_forces)
	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;
	vec4_t plane, sphere;
	vec_p pf;
	/* FIXME handle soft_particles */
	for(i=0;i<ps->num_boundary_planes;i++)
	{
		vec4Copy(plane, ps->boundary_plane[i]);
		/*
			if(ps->plane_shift[i])
				plane[3] += (ps->plane_shift[i]->entry)(ps->plane_shift[i],0);
			FIXME? cur_idx && calls in inner loop???
		*/
		for(j=0;j<ps->num_particles;j++)
		{
			pf = ps->force[j];
			pos = PS_POS(phase,j);
			realdist = vec3Dot(plane,pos) - plane[3] - ps->particle_radius[j];
			if(realdist > BNI_MIN_DIST)
			{
				/* call controller */
				f = (ps->plane_interaction[i]->entry)(ps->plane_interaction[i],realdist);
				vec3Scale(df,plane,f);
				vec3Add(pf,pf,df);
			}
			else
			{
				/* too close / other side */
				conlPrintf(LOG_ERROR,"%s(): particle %u, plane %u: dist %g, BNI_MIN_DIST %g.\n",
						   caller_name, j, i, realdist, BNI_MIN_DIST);
				return false;
			}
		}
	}
	for(i=0;i<ps->num_boundary_spheres;i++)
	{
		vec4Copy(sphere, ps->boundary_sphere[i]);
		/*
			vec3_t cshift;
			if(ps->sphere_radius_shift[i])
				sphere[3] += (ps->sphere_radius_shift[i]->entry)(ps->sphere_radius_shift[i],0);
			// spheres can move!
			if(ps->sphere_center_shift[i])
			{
				(ps->sphere_center_shift[i]->entry)(ps->sphere_center_shift[i],cshift);
				vec3Add(sphere,sphere,cshift);
			}
			FIXME? cur_idx && calls in inner loop???
		*/
		for(j=0;j<ps->num_particles;j++)
		{
			pf = ps->force[j];
			pos = PS_POS(phase,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 / intersects: no work can be done */
				conlPrintf(LOG_ERROR,"%s(): particle %u, sphere %u: dist %g, BNI_MIN_DIST %g.\n",
						   caller_name, j, i, realdist, BNI_MIN_DIST);
				return false;
			}
			/* call controller */
			f = (ps->sphere_interaction[i]->entry)(ps->sphere_interaction[i],realdist);
			/*
			 * 'f' now contains modulus of the force acting from sphere to particle 
			 */
			vec3Scale(df,rsp,- f / drsp);
			vec3Add(pf,pf,df);
		}
	}
	return true;
}

V_LOCAL void
apply_unary_forces(particle_system_p ps)
{
	vec3_t f;
	vec_p pf;
	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);
		(ps->unary_force->entry)(ps->unary_force,f);
		vec3Add(pf,pf,f);
	}
}

V_LOCAL void
apply_unary_accels(particle_system_p ps)
{
	vec3_t a;
	vec_p pf;
	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);
		(ps->unary_accel->entry)(ps->unary_accel,a);
		vec3Scale(a,a,ps->particle_mass_delta[ps->cur_idx]);
		vec3Add(pf,pf,a);
	}
}

V_LOCAL void
apply_viscous_forces(particle_system_p ps)
{
	vec3_t v;
	vec_p pf, pv;
	pv = PS_VEL(ps->cur_phase,0);
	if(!ps->env_velocity)
		return;
	for(ps->cur_idx = 0;ps->cur_idx < ps->num_particles;ps->cur_idx++)
	{
		pf = ps->force[ps->cur_idx];
		vec3Zero(v);
		(ps->env_velocity->entry)(ps->env_velocity,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 recursion_counter = 0;
	memZero(ps->force,ps->num_particles * sizeof(vec3_t));
	if(ps->soft_particles)
	{
		if(!compute_binary_dist_r(ps,&recursion_counter))
			return false;
	}
	else
	{
		if(!compute_binary_dist_r(ps,NULL))
			return false;
	}
	apply_binary_forces(ps);
	if(ps->tangent_forces)
	{
		if(!compute_contacts(ps))
			return false;
		apply_tangent_forces(ps);
	}
	if(!apply_boundary_forces(ps))
		return false;
	apply_unary_forces(ps);
	apply_unary_accels(ps);
	apply_viscous_forces(ps);
	return true;
}

/*
 * Routine to call from the ODE solver
 */
vboolean V_CALL
vdsRightSide(void_p arg,vec_p dxdt_dst,vec_t t,vec_t x[])
{
	CALLER_NAME(vdsRightSide)
	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(dxdt_dst,0);
	vec_p vel_deriv = PS_VEL(dxdt_dst,0);
	ps = (particle_system_p)arg;
	ps->cur_time = t;
	ps->cur_phase = x;
	if(!compute_forces(ps))
		return false;
	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;
	}
	return true;
}