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

/*
 * float controllers
 */
#include <../source/PAL/argsused.h>
vec_t CTRL_CALL zero_energy(struct float_ctrl_s *self,vec_t arg)
{
	return 0;
}

#include <../source/PAL/argsused.h>
vec_t CTRL_CALL constant_force(struct float_ctrl_s *self,vec_t arg)
{
	return self->params[0];
}

vec_t CTRL_CALL time_linear_force(struct float_ctrl_s *self,vec_t arg)
{
	vec_p p = self->params;
	vec_t t1 = p[0], t2 = p[1];
	vec_t cur_time = self->ps->cur_time;
	if(cur_time <= t1)
		return p[2];
	if(cur_time >= t2)
		return p[3];
	return p[2] + (cur_time - t1) * (p[3] - p[2]) / (t2 - t1);
}

vec_t CTRL_CALL inv_square_force(struct float_ctrl_s *self,vec_t arg)
{
	return self->params[0] / (arg * arg);
}

vec_t CTRL_CALL inv_square_energy(struct float_ctrl_s *self,vec_t arg)
{
	return - self->params[0] / arg;
}

/*
 * U_0(\frac{r_m}{r})^\alpha
 */
void pwr_setup(struct float_ctrl_s *self)
{
	/* Input:
	 *   [0] == U_0
	 *   [1] == r_m
	 *   [2] == alpha
	 * Output for pwr_force():
	 *   [3] == - alpha * U_0 * r_m ^ alpha
	 *   [4] == - alpha - 1
	 */
	vec_p p = self->params;
	vec_t u0 = p[0], rm = p[1], alpha = p[2];
	p[3] = - alpha * u0 * pow(rm, alpha);
	p[4] = - alpha - 1;
	p[5] = u0 * pow(rm, alpha);
}

vec_t CTRL_CALL pwr_force(struct float_ctrl_s *self,vec_t arg)
{
	vec_p p = self->params;
	assert(!IsZero(arg));
	return p[3] * pow(arg, p[4]);
}

vec_t CTRL_CALL pwr_energy(struct float_ctrl_s *self,vec_t arg)
{
	vec_p p = self->params;
	return p[5] * pow(arg, -p[2]);
}

void lj_setup(struct float_ctrl_s *self)
{
	/* Input:
	 *   [0] == \epsilon
	 *   [1] == r_m
	 * Output for lj_force():
	 *   [2] == - 12\epsilon(\rm)^(12)
	 *   [3] == 12\epsilon(\rm)^(6)
	 */
	vec_p p = self->params;
	vec_t eps = p[0], rm = p[1];
	p[2] = - 12 * eps * pow(rm, 12);
	p[3] = 12 * eps * pow(rm, 6);
}

vec_t CTRL_CALL lj_force(struct float_ctrl_s *self,vec_t arg)
{
	vec_p p = self->params;
	vec_t rd = (vec_t)1.0 / arg; /* reciprocal distance */
	vec_t rd3 = rd * rd * rd;
	vec_t rd6 = rd3 * rd3;
	vec_t rd7 = rd6 * rd;
	return (p[2] * rd6 + p[3]) * rd7;
}

vec_t CTRL_CALL lj_energy(struct float_ctrl_s *self,vec_t arg)
{
	vec_p p = self->params;
	vec_t rmr = p[1] / arg;
	vec_t rmr3 = rmr * rmr * rmr;
	vec_t rmr6 = rmr3 * rmr3;
	return p[0] * rmr6 * (1 - 2 * rmr6);
}

void mie_setup(struct float_ctrl_s *self)
{
	/* Input:
	 *   [0] == \epsilon
	 *   [1] == r_m (\sigma = r_m / 2^(1/6))
	 *   [2] == \gamma_r
	 *   [3] == \gamma_a
	 * Output for mie_force():
	 *   [4] == C\epsilon\gamma_r(\sigma)^(\gamma_r - 1)
	 *   [5] == -C\epsilon\gamma_a(\sigma)^(\gamma_a - 1)
	 *   [6] == 1 - \gamma_r
	 *   [7] == 1 - \gamma_a
	 *   [8] == C
	 *   [9] == \sigma
	 */
	vec_p p = self->params;
	vec_t eps = p[0], rm = p[1], gr = p[2], ga = p[3];
	vec_t C = (gr/(gr - ga)) * pow(gr/ga,ga/(gr - ga));
	
	vec_t sigma = rm * pow(2, - 1.0/6.0);
	p[4] = C * eps * gr * pow(sigma, gr - 1);
	p[5] = - C * eps * ga * pow(sigma, ga - 1);
	p[6] = 1 - gr;
	p[7] = 1 - ga;
	p[8] = C;
	p[9] = sigma;
}

vec_t CTRL_CALL mie_force(struct float_ctrl_s *self,vec_t arg)
{
	vec_p p = self->params;
	return p[4] * pow(arg, p[6]) + p[5] * pow(arg, p[7]);
}

vec_t CTRL_CALL mie_energy(struct float_ctrl_s *self,vec_t arg)
{
	vec_p p = self->params;
	vec_t C = p[8], eps = p[0], gr = p[2], ga = p[3];
	vec_t sigma_by_r = p[9] / arg;
	return C * eps * (pow(sigma_by_r, gr) - 2 * pow(sigma_by_r, ga));
}

void morse_setup(struct float_ctrl_s *self)
{
	/*
	 * Input:
	 *	[0] == \epsilon
	 *	[1] == r_m
	 *	[2] == r_w
	 * Output:
	 *	[3] = 2 * \epsilon / r_w
	 *	[4] == \alpha = 1 / r_w
	 */
	vec_p p = self->params;
	vec_t eps = p[0], rw = p[2];
	vec_p two_eps_alpha = p + 3, alpha = p + 4;
	*alpha = 1 / rw;
	*two_eps_alpha = 2 * eps * *alpha;
}

vec_t CTRL_CALL
morse_force(struct float_ctrl_s *self,vec_t arg)
{
	vec_p p = self->params;
	vec_t rm = p[1], two_eps_alpha = p[3], alpha = p[4];
	vec_t exponent = exp(alpha * (rm - arg));
	return two_eps_alpha * exponent * (1 - exponent);
}

vec_t CTRL_CALL
morse_energy(struct float_ctrl_s *self,vec_t arg)
{
	vec_p p = self->params;
	vec_t eps = p[0], rm = p[1], alpha = p[4];
	vec_t exponent = exp(alpha * (rm - arg));
	return eps * exponent * (exponent - 2);
}

/*
 * vec3 controllers
 */
void CTRL_CALL
vec3_constant_entry(struct vec3_ctrl_s *self,vec_p ret_val)
{
	vec3Copy(ret_val, self->params);
}

void	CTRL_CALL
vec3_time_linear_entry(struct vec3_ctrl_s *self,vec_p ret_val)
{
	/*
	 * p[0] - start time const
	 * p[1] - end time const
	 * p[2,3,4] - start value const
	 * p[5,6,7] - end value const
	 */
	vec3_t v;
	vec_p p = self->params;
	vec_p vstart = p + 2, vend = p + 5;
	vec_t t1 = p[0], t2 = p[1];
	vec_t cur_time = self->ps->cur_time;
	vec_t dt = cur_time - t1;
	if(dt < 0)
	{
		vec3Copy(ret_val, vstart);
		return;
	}
	if(cur_time >= t2)
	{
		vec3Copy(ret_val, vend);
		return;
	}
	dt /= t2 - t1;
	vec3Sub(v,vend,vstart);
	vec3Scale(v,v,dt);
	vec3Add(ret_val,v,vstart);
}

V_LOCAL float_ctrl_rec_t float_controllers[]=
{
	{
		"constant",1,
		constant_force,zero_energy,NULL,
		"Returns its single parameter (i.e. 1st derivative of U(r)=Cr)"
	},
	{
		"time_linear",4,
		time_linear_force,zero_energy /* energy is not distance-dependent */, NULL,
		"\tTakes four scalar parameters: t_start, t_end, start, end.\n"
		"\tReturns: (t < t_start) ? start : (t > t_end) ? end : \\\n"
		"\t\tstart + ((t - t_start) / (t_end - t_start)) * (end - start)"
	},
	{
		"inv_square",1,
		inv_square_force,inv_square_energy,NULL,
		"\tReturns 1st derivative of gravitational potential: \\frac{U_0}{r}. Takes one paramer U_0."
	},
	{
		"pwr",3,
		pwr_force,pwr_energy,pwr_setup,
		"\tReturns 1st derivative of potential U_0(\\frac{r_m}{r})^\\alpha. Takes three paramers: U_0, r_m and \\alpha."
	},
	{
		"lj",2,
		lj_force,lj_energy,lj_setup,
		"\tReturns 1st derivative of Lennard-Jones potential:\n"
		 "\t\\epsilon(\\frac{r_m}{r}^12 - 2\\frac{r_m}{r}^6)\n"
		 "\tTakes two paramers: \\epsilon (depth of potential well) and r_m (position of well)."
	},
	
	{
		"mie",4,
		mie_force,mie_energy,mie_setup,
		"\tReturns 1st derivative of Mie potential:\n"
		 "\tC\\epsilon(\\frac{\\sigma}{r}^\\gamma_r - 2\\frac{\\sigma}{r}^\\gamma_a),\n"
		 "\twhere C=(\\frac{\\gamma_r}{\\gamma_r-\\gamma_a}\\frac{\\gamma_r}{\\gamma_a}^\\frac{\\gamma_a}{\\gamma_r-\\gamma_a}),\n"
		 "\t\\sigma=2^{-\\frac{1}{6}}r_m\n"
		 "\tTakes 4 paramers: \\epsilon -- depth of potential well, \\r_m, \\gamma_r and \\gamma_a (repulsion and attraction powers)"
	},
	{
		"morse",3,
		morse_force,morse_energy,morse_setup,
		"\tReturns 1st derivative of Morse potential:\n"
		 "\t\\epsilon(exp(-2\\frac{r-r_m}{r_w}) - 2 exp(-\\frac{r-r_m}{r_w})).\n"
		 "\tTakes three paramers: \\epsilon (depth of well), r_m (position of well) and r_w (width of well)."
	},
	{NULL,0,NULL,NULL,NULL}
};

V_LOCAL vec3_ctrl_rec_t vec3_controllers[]=
{
	{
		"constant",3,
		vec3_constant_entry,NULL,
		"\tReturns three specified parameters."
	},
	{
		"vec3_time_linear",8,
		vec3_time_linear_entry,NULL,
		"\tTakes 8 scalars: t_start, t_end, x1,y1,z1, x2,y2,z2.\n"
		"\tReturns: (t < t_start) ? v1 : (t > t_end) ? v2 : \\\n"
		"\t\tv1 + ((t - t_start) / (t_end - t_start)) * (v2 - v1)"
	},
	{NULL,0,NULL,NULL,NULL}
};

float_ctrl_rec_p
fcForName(char *name)
{
	u32_t i;
	for(i = 0;NULL != float_controllers[i].name;i++)
		if(!strcmp(float_controllers[i].name, name))
			return &(float_controllers[i]);
	return NULL;
}

vec3_ctrl_rec_p
vcForName(char *name)
{
	u32_t i;
	for(i = 0;NULL != vec3_controllers[i].name;i++)
		if(!strcmp(vec3_controllers[i].name, name))
			return &(vec3_controllers[i]);
	return NULL;
}

void clearControllersCache(struct particle_system_s *ps)
{
	void_p ptr;
	while(vpsPop(&(ps->float_controllers),&ptr))
		memFree(ptr);
	while(vpsPop(&(ps->vec3_controllers),&ptr))
		memFree(ptr);
	vpsClear(&(ps->float_controllers));
	vpsClear(&(ps->vec3_controllers));
}

void helpControllers()
{
	float_ctrl_rec_t *fc = float_controllers;
	vec3_ctrl_rec_t *vc = vec3_controllers;
	conPuts("Syntax of controller (in case of float one -- potential cuve) definition:\n");
	conPuts("float|vec3 <ID> { <definition> }\n");
	conPuts("Where <ID> -- identifier of controller, and definition is:\n"
		"\t<name>;\n"
		"\t[cutoff <float cutoff_distance>;] -- use cutoff (float controllers only)\n"
		"\tparameters <float param_1> ... <float param_n>;\n");
	conPuts("Float controllers:\n");
	while(fc->name)
	{
		conPrintf("Name: '%s', requires %u float parameters.\n",fc->name,fc->num_scalar_params);
		conPuts(fc->desc);
		conPuts("\n");
		fc++;
	}
	conPuts("VEC3 controllers:\n");
	while(vc->name)
	{
		conPrintf("Name: '%s', requires %u float parameters.\n",vc->name,vc->num_scalar_params);
		conPuts(vc->desc);
		conPuts("\n");
		vc++;
	}
}

V_LOCAL float_ctrl_p
fcNew(vhandle_t lex,char *fctype,vector_p parameters,vboolean cutoff,vec_t cutoff_distance,char *id)
{
	CALLER_NAME(fcNew)
	float_ctrl_p fc = NULL;
	float_ctrl_rec_p fcrec = NULL;
	if(!fctype || !parameters)
	{
		PARSE_ERROR(lex, "incomplete definition of float contoller");
		return NULL;
	}
	fcrec = fcForName(fctype);
	if(!fcrec)
	{
		PARSE_ERROR_2ARG(lex,"unknown type of float controller: ", fctype);
		return NULL;
	}
	if(fcrec->num_scalar_params != parameters->size)
	{
		PARSE_ERROR_2ARG(lex, "incorrect number of parameters for float contoller ", id);
		return NULL;
	}
	fc = memAllocZ(sizeof(float_ctrl_t));
	fc->entry = fcrec->entry;
	fc->energy = fcrec->energy;
	memCopy(fc->params, parameters->elements, parameters->size * sizeof(vec_t));
	if(fcrec->setup)
		fcrec->setup(fc);
	fc->cutoff = cutoff;
	fc->cutoff_distance = cutoff_distance;
	strCopy(fc->id, id, MAX_CTRL_NAME);
	conlPrintf(LOG_TRACE,"%s(): controller \"%s\" initialized.\n",caller_name,id);
	return fc;
}

V_LOCAL float_ctrl_p
fcLoad(vhandle_t lex, char *id)
{
	CALLER_NAME(fcLoad)
	float_ctrl_p fc = NULL;
	vec_t cutoff_distance = 0;
	vboolean cutoff = false, dump = false;
	/* make VC6 happy */
	vec_t dmin = 0, dmax = 0;
	s32_t dsteps = -1;
	vector_p parameters = NULL;
	char *fctype = NULL;
	char dname[_MAX_PATH];
	conlPrintf(LOG_TRACE,"%s(): loading controller \"%s\".\n",caller_name,id);
	if(!lexNextIs(lex, open_brace))
	{
		SYN_EXPECTED(lex, open_brace);
		return NULL;
	}
	while(lexNext(lex))
	{
		if(lexLastWas(lex,close_brace))
		{
			if(!fc)
				fc = fcNew(lex,fctype,parameters,cutoff,cutoff_distance,id);
			if(parameters)
				memFree(parameters);
			if(fctype)
				memFree(fctype);
			if(dump && fc)
			{
				s32_t i;
				vec_t x, dx;
				char fname[_MAX_PATH];
				vector_p xi;
				vector_p yi;
				xi = vecAlloc(dsteps + 1);
				yi = vecAlloc(dsteps + 1);
				conlPrintf(LOG_INFO,"%s(): dumping force & energy of \"%s\" to \"%s\".\n",caller_name,id,dname);
				dx = (dmax - dmin) / dsteps;
				for(i = 0, x = dmin;i <= dsteps;i++, x += dx)
					xi->elements[i] = x;
				logFormatString(fname,"%s.force",dname);
				for(i = 0;i <= dsteps;i++)
					yi->elements[i] = (fc->entry)(fc,xi->elements[i]);
				vecsWriteXY(fname,xi->elements,yi->elements,xi->size);
				logFormatString(fname,"%s.energy",dname);
				for(i = 0;i <= dsteps;i++)
					yi->elements[i] = (fc->energy)(fc,xi->elements[i]);
				vecsWriteXY(fname,xi->elements,yi->elements,xi->size);
				memFree(xi);
				memFree(yi);
			}
			return fc;
		}
		if(lexLastWas(lex,"cutoff"))
		{
			if(cutoff)
			{
				PARSE_ERROR(lex,"redefinition of cutoff");
				goto err_exit;
			}
			cutoff = true;
			if(!lexNextFloat(lex,&cutoff_distance))
			{
				SYN_EXPECTED(lex,"float cutoff_distance");
				goto err_exit;
			}
			LEX_CHECK_SEMICOLON(lex);
			continue;
		}
		if(lexLastWas(lex,"parameters"))
		{
			if(parameters)
			{
				PARSE_ERROR(lex,"redefinition of parameters");
				goto err_exit;
			}
			parameters = vecParse(lex);
			if(!parameters)
				return NULL;
			LEX_CHECK_SEMICOLON(lex);
			continue;
		}
		if(lexLastWas(lex,"dump"))
		{
			if(!lexNext(lex))
			{
				SYN_EXPECTED(lex,"string output_file_name");
				goto err_exit;
			}
			strCopy(dname,lexLast(lex),_MAX_PATH - 8);
			if(!lexNextFloat(lex,&dmin) || !lexNextFloat(lex,&dmax))
			{
				SYN_EXPECTED(lex,"float dmin dmax");
				goto err_exit;
			}
			if(!lexNextInteger(lex,&dsteps))
			{
				SYN_EXPECTED(lex,"int dsteps");
				goto err_exit;
			}
			if(dsteps <= 0)
			{
				SYN_EXPECTED(lex,"int dsteps > 0");
				goto err_exit;
			}
			LEX_CHECK_SEMICOLON(lex);
			dump = true;
			continue;
		}
		if(fctype)
		{
			PARSE_ERROR(lex,"redefinition of controller's type");
			goto err_exit;
		}
		fctype = strDup(lexLast(lex));
		LEX_CHECK_SEMICOLON(lex);
	}
	SYN_UNEXPECTED_EOF(lex);
err_exit:;
	if(fc)
		memFree(fc);
	if(parameters)
		memFree(parameters);
	if(fctype)
		memFree(fctype);
	return NULL;
}

V_LOCAL float_ctrl_p
fcParse(vhandle_t lex,char *id)
{
	CALLER_NAME(fcParse)
	float_ctrl_p fc = NULL;
	if(!lexSearchNamedEntry(lex,"float",id))
	{
		PARSE_ERROR_3ARG(lex,"float controller \"",id,"\" undefined");
		return NULL;
	}
	fc = fcLoad(lex, id);
	while(lexSearchNamedEntry(lex,"float",id))
	{
		GENERIC_PARSE_WARNING(lex,"float controller redefinition ignored");
	}
	return fc;
}

float_ctrl_p
fcGetById(vhandle_t lex,struct particle_system_s *ps,char *id)
{
	CALLER_NAME(fcGetById)
	u32_t i;
	float_ctrl_p fc = NULL;
	vp_stack_p cache = &ps->float_controllers;
	char *lid = NULL;
	for(i = 0;i < vpsCount(cache);i++)
	{
		fc = (float_ctrl_p)vpsGet(cache, i);
		if(!strcmp(fc->id, id))
		{
			conlPrintf(LOG_TRACE,"%s(): controller \"%s\" found in cache.\n",caller_name,id);
			return fc;
		}
	}
	lexPush(lex,LEX_POSITION_BIT);
	lexSetInteger(lex,LEX_CURRENT_LINE,1);
	lid = strDup(id);
	fc = fcParse(lex, lid);
	memFree(lid);
	lexPop(lex,LEX_POSITION_BIT);
	if(NULL != fc)
	{
		fc->ps = ps;
		vpsPush(cache, fc);
	}
	return fc;
}

V_LOCAL vec3_ctrl_p
vcLoad(vhandle_t lex, char *id)
{
	CALLER_NAME(vcLoad)
	vec3_ctrl_p vc = NULL;
	vec3_ctrl_rec_p vcrec;
	vector_p parameters = NULL;
	char *vctype = NULL;
	conlPrintf(LOG_TRACE,"%s(): loading controller \"%s\".\n",caller_name,id);
	if(!lexNextIs(lex, open_brace))
	{
		SYN_EXPECTED(lex, open_brace);
		return NULL;
	}
	while(lexNext(lex))
	{
		if(lexLastWas(lex,close_brace))
		{
			if(!vctype || !parameters)
			{
				PARSE_ERROR(lex, "incomplete definition of vec3 contoller");
				goto err_exit;
			}
			vcrec = vcForName(vctype);
			if(!vcrec)
			{
				PARSE_ERROR(lex,"unknown type vec3 of controller");
				goto err_exit;
			}
			if(vcrec->num_scalar_params != parameters->size)
			{
				PARSE_ERROR_2ARG(lex, "incorrect number of parameters for vec3 contoller ", id);
				goto err_exit;
			}
			vc = memAllocZ(sizeof(vec3_ctrl_t));
			vc->entry = vcrec->entry;
			memCopy(vc->params, parameters->elements, parameters->size * sizeof(vec_t));
			strCopy(vc->id, id, MAX_CTRL_NAME);
			conlPrintf(LOG_TRACE,"%s(): controller \"%s\" loaded.\n",caller_name,id);
			memFree(parameters);
			memFree(vctype);
			return vc;
		}
		if(lexLastWas(lex,"parameters"))
		{
			if(parameters)
			{
				PARSE_ERROR(lex,"redefinition of parameters");
				goto err_exit;
			}
			parameters = vecParse(lex);
			if(!parameters)
				return NULL;
			LEX_CHECK_SEMICOLON(lex);
			continue;
		}
		if(vctype)
		{
			PARSE_ERROR(lex,"redefinition of controller's type");
			goto err_exit;
		}
		vctype = strDup(lexLast(lex));
		LEX_CHECK_SEMICOLON(lex);
	}
	SYN_UNEXPECTED_EOF(lex);
err_exit:;
	if(parameters)
		memFree(parameters);
	if(vctype)
		memFree(vctype);
	return NULL;
}

V_LOCAL vec3_ctrl_p
vcParse(vhandle_t lex,char *id)
{
	CALLER_NAME(vcParse)
	vec3_ctrl_p vc = NULL;
	if(!lexSearchNamedEntry(lex,"vec3",id))
	{
		PARSE_ERROR_3ARG(lex,"vec3 controller \"",id,"\" undefined");
		return NULL;
	}
	vc = vcLoad(lex, id);
	while(lexSearchNamedEntry(lex,"vec3",id))
	{
		GENERIC_PARSE_WARNING(lex,"vec3 controller redefinition ignored");
	}
	return vc;
}

vec3_ctrl_p
vcGetById(vhandle_t lex,struct particle_system_s *ps,char *id)
{
	CALLER_NAME(vcGetById)
	u32_t i;
	vec3_ctrl_p vc = NULL;
	char *lid = NULL;
	vp_stack_p cache = &ps->vec3_controllers;
	for(i = 0;i < vpsCount(cache);i++)
	{
		vc = (vec3_ctrl_p)vpsGet(cache, i);
		if(!strcmp(vc->id, id))
		{
			conlPrintf(LOG_TRACE,"%s(): controller \"%s\" found in cache.\n",caller_name,id);
			return vc;
		}
	}
	lexPush(lex,LEX_POSITION_BIT);
	lexSetInteger(lex,LEX_CURRENT_LINE,1);
	lid = strDup(id);
	vc = vcParse(lex, lid);
	memFree(lid);
	lexPop(lex,LEX_POSITION_BIT);
	if(NULL != vc)
	{
		vc->ps = ps;
		vpsPush(cache, vc);
	}
	return vc;
}
