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

/*
 * float controllers
 */

/*
 * float constant
 */
#include <../source/PAL/argsused.h>
vec_t   CTRL_CALL
float_constant(struct particle_system_s *model,vec_t arg,vec_p custom)
{
	/*
	 * custom[0] - float const
	 */
	return custom[0];
}

/*
 * float time-linear
 */
vec_t   CTRL_CALL
float_time_linear(struct particle_system_s *model,vec_t arg,vec_p custom)
{
	/*
	 * custom[0] - start time const
	 * custom[1] - end time const
	 * custom[2] - start value const
	 * custom[3] - end value const
	 */
	vec_t t = model->cur_time - custom[0];
	if(t <= 0)
		return custom[2];
	if(model->cur_time >= custom[1])
		return custom[3];
	t /= custom[1] - custom[0];
	return custom[2] + t * (custom[3]-custom[2]);
}

vec_t CTRL_CALL
LJ_modulus(struct particle_system_s *model,vec_t arg,vec_p custom)
{
	/*
	 * custom[0] == U0 const
	 * custom[1] == R0 const
	 * arg = dist;
	 */
	vec_t rd = (vec_t)1.0 / arg; /* reciprocal distance */
	vec_t r0r = custom[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 * custom[0] * rd * p6 * (1 - p6);
}

vec_t CTRL_CALL
LJ2_modulus(struct particle_system_s *model,vec_t arg,vec_p custom)
{
	/*
	 * custom[0] == U0 const
	 * custom[1] == R0 const
	 * custom[2] == EXTRA_HEIGHT const
	 * arg = dist;
	 */
	vec_t rd = (vec_t)1.0 / arg; /* reciprocal distance */
	vec_t r0r = custom[1] * rd;
	vec_t pwr, p6;
	/*
	 * d/dr(U0*((r0/r)^12 - 2*(r0/r)^6) + exp(-((r-1.15*r0)*10)^2))
	 */
	p6 = r0r * r0r * r0r;
	p6 *= p6;
	pwr = 10 * arg - 11.5 * custom[1];
	pwr *= pwr;
	return custom[0] * (12 * rd * p6 * (1 - p6) +
		+ custom[2] * (230-200*arg) * exp(-pwr));
}

vec_t CTRL_CALL
IS_modulus(struct particle_system_s *model,vec_t arg,vec_p custom)
{
	/*
	 * custom[0] == G const
	 * arg = dist
	 */
	vec_t dist = arg;
	return custom[0] / (dist * dist);
}

vec_t CTRL_CALL
power_modulus(struct particle_system_s *model,vec_t arg,vec_p custom)
{
	/*
	 * custom[0] == G const
	 * custom[1] == power of dist
	 * arg = dist
	 */
	vec_t dist = arg;
	return custom[0] * pow(dist, custom[1]);
}

/*
 * vec3 controllers
 */
void    CTRL_CALL
vec3_constant(struct particle_system_s *model,vec_p custom,
	vec3_t ret)
{
	/*
	 * custom[0,1,2] - vec3 const
	 */
	vec3Copy(ret,custom);
}

void    CTRL_CALL
vec3_time_linear(struct particle_system_s *model,vec_p custom,
	vec3_t ret)
{
	/*
	 * custom[0] - start time const
	 * custom[1] - end time const
	 * custom[2,3,4] - start value const
	 * custom[5,6,7] - end value const
	 */
	vec3_t v;
	vec_t t = model->cur_time - custom[0];
	if(t < 0)
	{
		vec3Copy(ret,custom + 2);
		return;
	}
	if(model->cur_time > custom[1])
	{
		vec3Copy(ret,custom + 5);
		return;
	}
	t /= custom[1] - custom[0];
	vec3Sub(v,custom + 5,custom + 2);
	vec3Scale(v,v,t);
	vec3Add(ret,v,custom + 2);
}

void    CTRL_CALL
vec3_pos_dep(struct particle_system_s *model,vec_p custom,
	vec3_t ret)
{
	/*
	 * model->cur_idx - index of the particle in model->cur_phase
	 * custom - matrix3x4_t const
	 */
	vec_p pos = PS_POS(model->cur_phase,model->cur_idx);
	mat3x4VecMul(ret,*(matrix3x4_t*)custom,pos);
}

void    CTRL_CALL
vec3_vel_dep(struct particle_system_s *model,vec_p custom,
	vec3_t ret)
{
	/*
	 * model->cur_idx - index of the particle in model->cur_phase
	 * custom - matrix3x4_t const
	 */
	vec_p vel = PS_VEL(model->cur_phase,model->cur_idx);
	mat3x4VecMul(ret,*(matrix3x4_t*)custom,vel);
}

void    CTRL_CALL
vec3_unit_pos_dep(struct particle_system_s *model,vec_p custom,
	vec3_t ret)
{
	/*
	 * model->cur_idx - index of the particle in model->cur_phase
	 * custom - matrix3x4_t const
	 */
	vec_t l, *pos = PS_POS(model->cur_phase,model->cur_idx);
	vec3_t u;
	l = vec3Abs(pos);
	if(IsZero(l))
		vec3Zero(ret);
	else
	{
		l = (vec_t)1.0 / l;
		vec3Scale(u,pos,l);
		mat3x4VecMul(ret,*(matrix3x4_t*)custom,u);
	}
}

void    CTRL_CALL
vec3_unit_vel_dep(struct particle_system_s *model,vec_p custom,
	vec3_t ret)
{
	/*
	 * model->cur_idx - index of the particle in model->cur_phase
	 * custom - matrix3x4_t const
	 */
	vec_t l, *vel = PS_VEL(model->cur_phase,model->cur_idx);
	vec3_t u;
	l = vec3Abs(vel);
	if(IsZero(l))
		vec3Zero(ret);
	else
	{
		l = (vec_t)1.0 / l;
		vec3Scale(u,vel,l);
		mat3x4VecMul(ret,*(matrix3x4_t*)custom,u);
	}
}

void    CTRL_CALL
vec3_linear_to_origin(struct particle_system_s *model,vec_p custom,
	vec3_t ret)
{
	/*
	 * custom[0] = potential_barrier_radius
	 * custom[1] = potential_barrier_height
	 * model->cur_idx - index of the current particle in model->cur_phase
	 */
	vec_p pos = PS_POS(model->cur_phase,model->cur_idx);
	vec_t r = custom[0] - vec3Abs(pos);
	if(r > 0) /* barrier_radius > cur_dist */
		vec3Zero(ret);
	else
		vec3Scale(ret,pos,custom[1] * r);
}

void    CTRL_CALL
vec3_exp_to_origin(struct particle_system_s *model,vec_p custom,
	vec3_t ret)
{
	/*
	 * custom[0] = potential_barrier_radius
	 * custom[1] = potential_barrier_height
	 * model->cur_idx - index of the current particle in model->cur_phase
	 */
	vec_p pos = PS_POS(model->cur_phase,model->cur_idx);
	vec_t d = vec3Abs(pos);
	vec_t r = custom[0] - d;
	if(r >= 0)
		vec3Zero(ret);
	else
		vec3Scale(ret,pos,custom[1] * exp(r) / d);
}

void    CTRL_CALL
vec3_rectlinear_to_origin(struct particle_system_s *model,vec_p custom,
	vec3_t ret)
{
	/*
	 * custom[0] = potential_barrier_radius
	 * custom[1] = potential_barrier_height
	 * model->cur_idx - index of the current particle in model->cur_phase
	 */
	int i;
	vec_p pos = PS_POS(model->cur_phase,model->cur_idx);
	vec_t pbwidth = custom[0];
	vec_t pbheight = custom[1];
	for(i=0;i<3;i++)
	{
		if(pos[i] < -pbwidth)
		{
			ret[i] = (-pbwidth - pos[i]) * pbheight; /* positive */
		} else if(pos[i] > pbwidth)
		{
			ret[i] = (pbwidth - pos[i]) * pbheight; /* negative */
		} else
		{
			ret[i] = 0;
		}
	}
}

typedef struct float_ctrl_rec_s
{
	char    name[MAX_CTRL_NAME];
	u32_t   num_scalar_consts;
	float_controller_t entry;
}   float_ctrl_rec_t, float_ctrl_rec_p;

typedef struct vec3_ctrl_rec_s
{
	char    name[MAX_CTRL_NAME];
	u32_t   num_scalar_consts;
	vec3_controller_t entry;
}   vec3_ctrl_rec_t, vec3_ctrl_rec_p;

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

V_LOCAL vec3_ctrl_rec_t vec3_controllers[]=
{
	{"vec3_constant",3,vec3_constant},
	{"vec3_time_linear",8,vec3_time_linear},
	{"vec3_pos_dep",12,vec3_pos_dep},
	{"vec3_vel_dep",12,vec3_vel_dep},
	{"vec3_linear_to_origin",2,vec3_linear_to_origin},
	{"vec3_exp_to_origin",2,vec3_exp_to_origin},
	{"vec3_rectlinear_to_origin",2,vec3_rectlinear_to_origin},
	{"",0,NULL}
};

float_controller_t
get_float_controller(char *ctrl_name,u32_t *requires_floats_ret)
{
	float_ctrl_rec_t *ptr = float_controllers;
	if(!ctrl_name)
		return NULL;
	while(ptr->entry)
	{
		if(!strcmp(ctrl_name,ptr->name))
		{
			if(requires_floats_ret)
				*requires_floats_ret = ptr->num_scalar_consts;
			return ptr->entry;
		}
		ptr++;
	}
	conlPrintf(LOG_ERROR,
		"get_float_controller(%s): unknown controller.\n",ctrl_name);
	return NULL;
}

vec3_controller_t
get_vec3_controller(char *ctrl_name,u32_t *requires_floats_ret)
{
	vec3_ctrl_rec_t *ptr = vec3_controllers;
	if(!ctrl_name)
		return NULL;
	while(ptr->entry)
	{
		if(!strcmp(ctrl_name,ptr->name))
		{
			if(requires_floats_ret)
				*requires_floats_ret = ptr->num_scalar_consts;
			return ptr->entry;
		}
		ptr++;
	}
	conlPrintf(LOG_ERROR,
		"get_vec3_controller(%s): unknown controller.\n",ctrl_name);
	return NULL;
}

void
help_controllers(void)
{
	float_ctrl_rec_t *fc = float_controllers;
	vec3_ctrl_rec_t *vc = vec3_controllers;
	puts("Float controllers:");
	while(fc->entry)
	{
		conPrintf("Name: '%s'; requires %u float(s).\n",
			fc->name,fc->num_scalar_consts);
		fc++;
	}
	puts("VEC3 controllers:");
	while(vc->entry)
	{
		conPrintf("Name: '%s'; requires %u float(s).\n",
			vc->name,vc->num_scalar_consts);
		vc++;
	}
}
