#include "psmod.h"

#define	MAX_CTRL_TABLES		16
#define	CTRL_ARGS_END		(-1)
#define	CTRL_FLOAT_TABLE	(-2)
#define	CTRL_VEC3_TABLE		(-3)
typedef s32_t				argdef_t[MAX_CTRL_TABLES];

V_LOCAL void
ctlDescribeArgs(argdef_t argdef)
{
	u32_t i;
	for(i=0;;)
	{
		switch(argdef[i++])
		{
			case CTRL_FLOAT_TABLE:
				conPrintf("float_table[%i]",argdef[i]);
				break;
			case CTRL_VEC3_TABLE:
				conPrintf("vec3_table[%i]",argdef[i]);
				break;
			case CTRL_ARGS_END:
				return;
		}
		if(argdef[++i] != CTRL_ARGS_END)
			conPrintf(";");
	}
}

V_LOCAL u32_t
ctlCountFloats(argdef_t argdef)
{
	u32_t i, n_el = 0;
	for(i=0;;i++)
	{
		switch(argdef[i++])
		{
			case CTRL_FLOAT_TABLE:
				n_el += (u32_t)(argdef[i]);
				break;
			case CTRL_VEC3_TABLE:
				n_el += 3 * (u32_t)(argdef[i]);
				break;
			case CTRL_ARGS_END:
				return n_el;
		}
	}
}

V_LOCAL vboolean
ctlParseArgs(vhandle_t script,vec_p dest,argdef_t argdef)
{
	CALLER_NAME(ctlParseArgs)
	u32_t i, n_el;
	vec_p ptr = dest;
	for(i=0;;i++)
	{
		switch(argdef[i++])
		{
			case CTRL_FLOAT_TABLE:
				n_el = (u32_t)(argdef[i]);
				if(!icParseFloatTable(ptr,sizeof(vec_t),n_el,NULL,script))
				{
					SYN_EXPECTED(script,"float_table");
					return false;
				}
				ptr += n_el;
				break;
			case CTRL_VEC3_TABLE:
				n_el = (u32_t)(argdef[i]);
				if(!icParseVec3Table(ptr,sizeof(vec3_t),n_el,NULL,script))
				{
					SYN_EXPECTED(script,"vec3_table");
					return false;
				}
				ptr += n_el * 3;
				break;
			case CTRL_ARGS_END:
				return true;
		}
	}
}

/*
 * float controllers
 */

/*
 * float constant
 */
#include <../source/PAL/argsused.h>
V_LOCAL vec_t
float_constant(struct float_controller_s *self,particle_system_p ps)
{
	/*
	 * setup[0] - float const
	 */
	return self->setup[0];
}

/*
 * float time-linear
 */
V_LOCAL vec_t
float_time_linear(struct float_controller_s *self,particle_system_p ps)
{
	/*
	 * setup[0] - start time const
	 * setup[1] - end time const
	 * setup[2] - start value const
	 * setup[3] - end value const
	 */
	vec_p setup = self->setup;
	vec_t t = ps->solver.current_time - setup[0];
	if(t <= 0)
		return setup[2];
	if(ps->solver.current_time >= setup[1])
		return setup[3];
	t /= setup[1] - setup[0];
	return setup[2] + t * (setup[3]-setup[2]);
}

V_LOCAL vec_t
LJ_modulus(struct float_controller_s *self,particle_system_p ps)
{
	/*
	 * setup[0] == U0 const
	 * setup[1] == R0 const
	 * ps->ctl_args[0] = dist;
	 */
	vec_p setup = self->setup;
	vec_t rd = (vec_t)1.0 / ps->ctl_args[0]; /* reciprocal distance */
	vec_t r0r = setup[1] * rd;
	vec_t p6;
	/*
	 * First derivative of Lennard - Jones potential:
	 * d/dr(U0*((r0/r)^12 - 2*(r0/r)^6))
	 */
	p6 = r0r * r0r * r0r;
	p6 *= p6;
	return 12 * setup[0] * rd * p6 * (1 - p6);
}

V_LOCAL vec_t
LJ2_modulus(struct float_controller_s *self,particle_system_p ps)
{
	/*
	 * U(r) = U0*((r0/r)^12 - 2*(r0/r)^6 + exp(-(r-r0)^2))
	 */
	/*
	 * setup[0] == U0 const
	 * setup[1] == R0 const
	 * ps->ctl_args[0] = dist;
	 */
	vec_p setup = self->setup;
	vec_t rd = (vec_t)1.0 / ps->ctl_args[0]; /* reciprocal distance */
	vec_t r0r = setup[1] * rd;
	vec_t r0sr = setup[1] - ps->ctl_args[0]; /* r_0 - r_ij */
	vec_t p6 = r0r * r0r * r0r;
	p6 *= p6;
	return setup[0] * (12 * rd * p6 * (1 - p6) +
		2 * r0sr * exp(- r0sr * r0sr));
}

V_LOCAL vec_t
IS_modulus(struct float_controller_s *self,particle_system_p ps)
{
	/*
	 * setup[0] == G const
	 * ps->ctl_args[0] = dist
	 */
	vec_t dist = ps->ctl_args[0];
	return self->setup[0] / (dist * dist);
}

V_LOCAL vec_t
power_modulus(struct float_controller_s *self,particle_system_p ps)
{
	/*
	 * setup[0] == G const
	 * setup[1] == power of dist
	 * ps->ctl_args[0] = dist
	 */
	vec_t dist = ps->ctl_args[0];
	return self->setup[0] * pow(dist, self->setup[1]);
}

/*
 * vec3 controllers
 */
V_LOCAL void
vec3_constant(struct vec3_controller_s *self,particle_system_p ps,vec3_t dest)
{
	/*
	 * setup[0,1,2] - vec3 const
	 */
	vec3Copy(dest,self->setup);
}

V_LOCAL void
vec3_time_linear(struct vec3_controller_s *self,particle_system_p ps,vec3_t dest)
{
	/*
	 * setup[0] - start time const
	 * setup[1] - end time const
	 * setup[2,3,4] - start value const
	 * setup[5,6,7] - end value const
	 */
	vec3_t v;
	vec_p setup = self->setup;
	vec_t t = ps->solver.current_time - setup[0];
	if(t < 0)
	{
		vec3Copy(dest,setup + 2);
		return;
	}
	if(ps->solver.current_time > setup[1])
	{
		vec3Copy(dest,setup + 5);
		return;
	}
	t /= setup[1] - setup[0];
	vec3Sub(v,setup + 5,setup + 2);
	vec3Scale(v,v,t);
	vec3Add(dest,v,setup + 2);
}

V_LOCAL void
vec3_pos_dep(struct vec3_controller_s *self,particle_system_p ps,vec3_t dest)
{
	/*
	 * ps->cur_idx - index of the particle in ps->current_phase
	 * setup - matrix3x4_t const
	 */
	vec_p pos = PS_POS(ps->current_phase,ps->cur_idx);
	mat3x4VecMul(dest,*(matrix3x4_t*)(self->setup),pos);
}

V_LOCAL void
vec3_vel_dep(struct vec3_controller_s *self,particle_system_p ps,vec3_t dest)
{
	/*
	 * ps->cur_idx - index of the particle in ps->current_phase
	 * setup - matrix3x4_t const
	 */
	vec_p vel = PS_VEL(ps->current_phase,ps->cur_idx);
	mat3x4VecMul(dest,*(matrix3x4_t*)(self->setup),vel);
}

V_LOCAL void
vec3_unit_pos_dep(struct vec3_controller_s *self,particle_system_p ps,vec3_t dest)
{
	/*
	 * ps->cur_idx - index of the particle in ps->current_phase
	 * setup - matrix3x4_t const
	 */
	vec_t l, *pos = PS_POS(ps->current_phase,ps->cur_idx);
	vec3_t u;
	l = vec3Abs(pos);
	if(IsZero(l))
		vec3Zero(dest);
	else
	{
		l = (vec_t)1.0 / l;
		vec3Scale(u,pos,l);
		mat3x4VecMul(dest,*(matrix3x4_t*)(self->setup),u);
	}
}

V_LOCAL void
vec3_unit_vel_dep(struct vec3_controller_s *self,particle_system_p ps,vec3_t dest)
{
	/*
	 * ps->cur_idx - index of the particle in ps->current_phase
	 * setup - matrix3x4_t const
	 */
	vec_t l, *vel = PS_VEL(ps->current_phase,ps->cur_idx);
	vec3_t u;
	l = vec3Abs(vel);
	if(IsZero(l))
		vec3Zero(dest);
	else
	{
		l = (vec_t)1.0 / l;
		vec3Scale(u,vel,l);
		mat3x4VecMul(dest,*(matrix3x4_t*)(self->setup),u);
	}
}

V_LOCAL void
vec3_linear_to_origin(struct vec3_controller_s *self,particle_system_p ps,vec3_t dest)
{
	/*
	 * setup[0] = potential_barrier_radius
	 * setup[1] = potential_barrier_height
	 * ps->cur_idx - index of the current particle in ps->current_phase
	 */
	vec_p pos = PS_POS(ps->current_phase,ps->cur_idx);
	vec_t r = self->setup[0] - vec3Abs(pos);
	if(r > 0) /* barrier_radius > cur_dist */
		vec3Zero(dest);
	else
		vec3Scale(dest,pos,self->setup[1] * r);
}

V_LOCAL void
vec3_exp_to_origin(struct vec3_controller_s *self,particle_system_p ps,vec3_t dest)
{
	/*
	 * setup[0] = potential_barrier_radius
	 * setup[1] = potential_barrier_height
	 * ps->cur_idx - index of the current particle in ps->current_phase
	 */
	vec_p pos = PS_POS(ps->current_phase,ps->cur_idx);
	vec_t d = vec3Abs(pos);
	vec_t r = self->setup[0] - d;
	if(r >= 0)
		vec3Zero(dest);
	else
		vec3Scale(dest,pos,self->setup[1] * exp(r) / d);
}

V_LOCAL void
vec3_rectlinear_to_origin(struct vec3_controller_s *self,particle_system_p ps,vec3_t dest)
{
	/*
	 * setup[0] = potential_barrier_radius
	 * setup[1] = potential_barrier_height
	 * ps->cur_idx - index of the current particle in ps->current_phase
	 */
	int i;
	vec_p pos = PS_POS(ps->current_phase,ps->cur_idx);
	vec_t pbwidth = self->setup[0];
	vec_t pbheight = self->setup[1];
	for(i=0;i<3;i++)
	{
		if(pos[i] < -pbwidth)
		{
			dest[i] = (-pbwidth - pos[i]) * pbheight; /* positive */
		} else if(pos[i] > pbwidth)
		{
			dest[i] = (pbwidth - pos[i]) * pbheight; /* negative */
		} else
		{
			dest[i] = 0;
		}
	}
}

V_LOCAL void
vec3_swirl(struct vec3_controller_s *self,particle_system_p ps,vec3_t dest)
{
	/*
	 * setup[0,1,2] = axis
	 * setup[3,4,5] = origin
	 * setup[6] = near dist
	 * setup[7] = far dist
	 * setup[8] = near spd
	 * setup[11] = far spd
	 * ps->cur_idx - index of the current particle in ps->current_phase
	 */
	vec_t dist, axial_dist;
	vec3_t delta, prj;
	vec_p pos = PS_POS(ps->current_phase,ps->cur_idx);
	vec_p axis_dir = self->setup;
	vec_p axis_origin = self->setup + 3;
	vec_t near_dist = self->setup[6], far_dist = self->setup[7];
	vec_t near_spd = self->setup[8], far_spd = self->setup[9];
	/* Compute distance from axis */
	vec3Sub(delta,pos,axis_origin);
	dist = vec3Dot(axis_dir,delta);
	vec3Scale(prj,axis_dir,dist);
	vec3Sub(prj,delta,prj);
	axial_dist = vec3Abs(prj);
	/* catch degenerate case when point lies on axis */
	if(axial_dist < FLOAT_ZERO_EPSILON)
	{
		vec3Zero(dest);
		return;
	}
	vec3Scale(prj,prj,1/axial_dist); /* unify prj */
	vec3Cross(dest,axis_dir,prj);
	if(axial_dist <= near_dist)
		vec3Scale(dest,dest,near_spd);
	else if(axial_dist >= far_dist)
		vec3Scale(dest,dest,far_spd);
	else
	{
		dist = (axial_dist - near_dist) / (far_dist - near_dist);
		vec3Scale(dest,dest,dist);
	}
}

typedef struct float_ctrl_rec_s
{
	char    name[MAX_ID];
	argdef_t args;
	float_controller_entry_t entry;
}   float_ctrl_rec_t, float_ctrl_rec_p;

typedef struct vec3_ctrl_rec_s
{
	char    name[MAX_ID];
	argdef_t args;
	vec3_controller_entry_t entry;
}   vec3_ctrl_rec_t, vec3_ctrl_rec_p;


V_LOCAL float_ctrl_rec_t float_controllers[]=
{
	{"const",{CTRL_FLOAT_TABLE,1,CTRL_ARGS_END},float_constant},
	{"float_time_linear",{CTRL_FLOAT_TABLE,4,CTRL_ARGS_END},float_time_linear},
	{"lj",{CTRL_FLOAT_TABLE,2,CTRL_ARGS_END},LJ_modulus},
	{"lj2",{CTRL_FLOAT_TABLE,3,CTRL_ARGS_END},LJ2_modulus},
	{"inverse_square",{CTRL_FLOAT_TABLE,1,CTRL_ARGS_END},IS_modulus},
	{"custom_power",{CTRL_FLOAT_TABLE,2,CTRL_ARGS_END},power_modulus},
	{"",{CTRL_ARGS_END},NULL}
};

V_LOCAL vec3_ctrl_rec_t vec3_controllers[]=
{
	{"const",{CTRL_VEC3_TABLE,1,CTRL_ARGS_END},vec3_constant},
	{"vec3_time_linear",{CTRL_FLOAT_TABLE,2,CTRL_VEC3_TABLE,2,CTRL_ARGS_END},vec3_time_linear},
	{"vec3_pos_dep",{CTRL_FLOAT_TABLE,12,CTRL_ARGS_END},vec3_pos_dep},
	{"vec3_vel_dep",{CTRL_FLOAT_TABLE,12,CTRL_ARGS_END},vec3_vel_dep},
	{"vec3_unit_pos_dep",{CTRL_FLOAT_TABLE,12,CTRL_ARGS_END},vec3_unit_pos_dep},
	{"vec3_unit_vel_dep",{CTRL_FLOAT_TABLE,12,CTRL_ARGS_END},vec3_unit_vel_dep},
	{"vec3_linear_to_origin",{CTRL_FLOAT_TABLE,2,CTRL_ARGS_END},vec3_linear_to_origin},
	{"vec3_exp_to_origin",{CTRL_FLOAT_TABLE,2,CTRL_ARGS_END},vec3_exp_to_origin},
	{"vec3_rectlinear_to_origin",{CTRL_FLOAT_TABLE,2,CTRL_ARGS_END},vec3_rectlinear_to_origin},
	{"vec3_swirl",{CTRL_VEC3_TABLE,2,CTRL_FLOAT_TABLE,4,CTRL_ARGS_END},vec3_swirl},
	{"",{CTRL_ARGS_END},NULL}
};

V_LOCAL float_controller_entry_t
get_float_controller(char *ctrl_name,u32_t *requires_floats_ret,
	argdef_t **args_ret)
{
	CALLER_NAME(get_float_controller)
	float_ctrl_rec_t *ptr = float_controllers;
	if(!ctrl_name)
		return NULL;
	while(ptr->entry)
	{
		if(STR_EQ(ctrl_name,ptr->name))
		{
			*requires_floats_ret = ctlCountFloats(ptr->args);
			*args_ret = &ptr->args;
			return ptr->entry;
		}
		ptr++;
	}
	ERROR_INVALID_STR("unknown controller",ctrl_name);
	return NULL;
}

V_LOCAL vec3_controller_entry_t
get_vec3_controller(char *ctrl_name,u32_t *requires_floats_ret,
	argdef_t **args_ret)
{
	CALLER_NAME(get_vec3_controller)
	vec3_ctrl_rec_t *ptr = vec3_controllers;
	if(!ctrl_name)
		return NULL;
	while(ptr->entry)
	{
		if(STR_EQ(ctrl_name,ptr->name))
		{
			*requires_floats_ret = ctlCountFloats(ptr->args);
			*args_ret = &ptr->args;
			return ptr->entry;
		}
		ptr++;
	}
	ERROR_INVALID_STR("unknown controller",ctrl_name);
	return NULL;
}

void ctlHelp(void)
{
	float_ctrl_rec_t *fc = float_controllers;
	vec3_ctrl_rec_t *vc = vec3_controllers;
	puts("Float controllers:");
	while(fc->entry)
	{
		conPrintf("Name: '%s'; args: ",fc->name);
		ctlDescribeArgs(fc->args);
		conPuts("\n");
		fc++;
	}
	puts("VEC3 controllers:");
	while(vc->entry)
	{
		conPrintf("Name: '%s'; args: ",vc->name);
		ctlDescribeArgs(vc->args);
		conPuts("\n");
		vc++;
	}
}

void ctlFree(void_p ctl)
{
	memFree(ctl);
}

float_controller_p ctlParseFloat(vhandle_t script)
{
	CALLER_NAME(ctlParseFloat)
	u32_t nargs;
	argdef_t *args_ptr;
	float_controller_p ctl;
	float_controller_entry_t entry;
	if(!lexNextIs(script,"("))
	{
		SYN_EXPECTED(script,"(");
		return NULL;
	}
	entry = get_float_controller(lexNext(script),&nargs,&args_ptr);
	if(!entry)
		return NULL;
	ctl = memAllocZ(sizeof(float_controller_t)+nargs*sizeof(vec_t));
	ctl->entry = entry;
	ctl->setup = (vec_p)(ctl+1);
	if(!ctlParseArgs(script,ctl->setup,*args_ptr))
	{
		memFree(ctl);
		return NULL;
	}
	if(!lexNextIs(script,")"))
	{
		SYN_EXPECTED(script,")");
		memFree(ctl);
		return NULL;
	}
	return ctl;
}

vec3_controller_p ctlParseVec3(vhandle_t script)
{
	CALLER_NAME(ctlParseVec3)
	u32_t nargs;
	argdef_t *args_ptr;
	vec3_controller_p ctl;
	vec3_controller_entry_t entry;
	if(!lexNextIs(script,"("))
	{
		SYN_EXPECTED(script,"(");
		return NULL;
	}
	entry = get_vec3_controller(lexNext(script),&nargs,&args_ptr);
	if(!entry)
		return NULL;
	ctl = memAllocZ(sizeof(vec3_controller_t)+nargs*sizeof(vec_t));
	ctl->entry = entry;
	ctl->setup = (vec_p)(ctl+1);
	if(!ctlParseArgs(script,ctl->setup,*args_ptr))
	{
		memFree(ctl);
		return NULL;
	}
	if(!lexNextIs(script,")"))
	{
		SYN_EXPECTED(script,")");
		memFree(ctl);
		return NULL;
	}
	return ctl;
}
