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

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

/*
#define USE_EARLY_OUT
#define DEBUG_EARLY_OUT
*/

static unsigned int compute_forces_call_count = 0;

void
compute_binary_dist(particle_system_p ps)
{
	u32_t i, j;
	vec_p posi, posj;
	vec_t dr, surf_dist;
	vec3_t rij;
	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(surf_dist <= 0)
			{
				puts("compute_binary_dist(): Current parameters maked RK45 to diverge, or incorrect IC was specified.");
				printf("compute_forces_call_count = %u\n", compute_forces_call_count);
				printf("Distance between centers %g, sum of sizes %g\n",dr,ps->particle_radius[j] + ps->particle_radius[i]);
				printf("Particles intersect each other. Phase trashed, aborting%s.\n",
					ps->final_filename[0] ? " without save" : "");
				fflush(stdout);
				exit(-1);
			}
			ps->dist->cell[i][j] = ps->dist->cell[j][i] = dr;
			ps->surf_dist->cell[i][j] = ps->dist->cell[j][i] = surf_dist;
		}
	}
}

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_ctlr_p bi = ps->binary_interaction;
	u32_t i, j;
	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;
				if(realdist < BII_MIN_DIST)
				{
					ps->fail_counter++;
					continue;
				}
				f = (bi->entry)(bi, realdist);
			/*
			}
			else
			{
				if(realdist < BII_MIN_DIST)
				{
					ps->fail_counter++;
					continue;
				}
				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);
				}
			}
			*/
			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);
		}
	}
}

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;
	for(i = 0;i < ps->num_particles;i++)
		ps->contact->cell[i][i] = 0;
	for(i = 0;i < ps->num_particles - 1;i++)
	{
		posi = PS_POS(phase,i);
		sizei = ps->particle_radius[i];
		for(j = i+1;j < ps->num_particles;j++)
		{
			posj = PS_POS(phase,j);
			sizej = ps->particle_radius[j];
			vec3Sub(rij,posj,posi);
			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)
			{
				puts("compute_contacts(): current parameters maked RK45 to diverge, or incorrect IC was specified.");
				printf("compute_forces_call_count = %u\n", compute_forces_call_count);
				printf("Distance between centers %g, sum of sizes %g, ps->dglue %g & %g.\n",
					dr, sizei + sizej, ps->dglue[i], ps->dglue[j]);
				printf("Particles intersect each other. Phase trashed, aborting%s.\n",
					ps->final_filename[0] ? " without save" : "");
				fflush(stdout);
				exit(-1);
			}
			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 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);
		}
	}
}

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;
	vec4_t plane, sphere;
	vec_p pf;
	for(i=0;i<ps->num_boundary_planes;i++)
	{
		vec4Copy(plane, ps->boundary_plane[i]);
		/*
			float sine50 {
				periodic 1e-1 50
			}
			if(ps->plane_shift[i])
				plane[3] += (ps->plane_shift[i]->entry)(ps->boundary_plane_ctrls[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 */
				ps->fail_counter++;
				continue;
			}
		}
	}
	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 */
				ps->fail_counter++;
				continue;
			}
			/* 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);
		}
	}
}

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)
{
#ifdef  USE_EARLY_OUT
	vboolean need_reject_step;
#endif	/* USE_EARLY_OUT */
	ps->fail_counter = 0;
	compute_forces_call_count++;
#ifdef  USE_EARLY_OUT
	need_reject_step = false;
	for(;;)
	{
		memZero(ps->force,ps->num_particles * sizeof(vec3_t));
		compute_binary_dist(ps);
		if(ps->binary_interaction)
			apply_binary_forces(ps);
		if(ps->fail_counter)
		{
			conlPrintf(LOG_TRACE,
				"compute_forces(): %u fails after binary_interaction()\n",
				ps->fail_counter);
			need_reject_step = true;
			break;
		}
		if(ps->tangent_forces)
		{
			compute_contacts(ps);
			apply_tangent_forces(ps);
		}
		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);
	if(ps->binary_interaction)
		apply_binary_forces(ps);
	if(ps->tangent_forces)
	{
		compute_contacts(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;
#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;
	}
}
