#include <v_lib.h>
#include "util_1.h"

vboolean	V_GLOBAL
vecStoreVec(vector_p *srcdst,vec_t val,u32_t idx)
{
	CALLER_NAME(vecStoreVec)
	vector_p vec;
	if(!srcdst)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"%s(%X,%g,%u): %s.\n",caller_name,srcdst,val,idx,error_incorrect_parameters);
#endif	/* CHECKED_BUILD */
		return false;
	}
	if(!*srcdst)
	{
#define	VEC_MIN_AUTOSIZE	512
		*srcdst = vec = vecAlloc((idx < VEC_MIN_AUTOSIZE) ? VEC_MIN_AUTOSIZE : idx);
#undef	VEC_MIN_AUTOSIZE
		if(!vec)
			return false;
	}
	else
	{
		if(idx >= (*srcdst)->size)
		{
			vec = vecAlloc((*srcdst)->size * 2);
			if(!vec)
				return false;
			memCopy(vec->elements, (*srcdst)->elements, (*srcdst)->size * sizeof(vec_t));
			memFree(*srcdst);
			*srcdst = vec;
		}
		else
			vec = *srcdst;
	}
	vec->elements[idx] = val;
	return true;
}

/*
 * Size of vector must be equal to number of columns of matrix
 */
void	V_GLOBAL
matCopyToRow(matrix_t *dst, vector_t *src,u32_t row)
{
	CALLER_NAME(matCopyToRow)
	if(!dst || !src || row >= dst->rows || src->size != dst->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"%s(%X,%X,%u): %s.\n",caller_name,dst,src,row,error_incorrect_parameters);
#endif	/* CHECKED_BUILD */
		return;
	}
	memCopy32(dst->cell[row], src->elements, (dst->cols * sizeof(vec_t)) >> 2);
}

/*
 * Size of vector must be equal to number of rows of matrix
 */
void	V_GLOBAL
matCopyToCol(matrix_t *dst,vector_t *src,u32_t col)
{
	CALLER_NAME(matCopyToCol)
	u32_t r;
	if(!dst || !src || col >= dst->cols || src->size != dst->rows)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"%s(%X,%X,%u): %s.\n",caller_name,dst,src,col,error_incorrect_parameters);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(r = 0;r < dst->rows;r++)
		dst->cell[r][col] = src->elements[r];
}

matrix_p	V_GLOBAL
matRowMatrixFromVecs(u32_t rows,u32_t cols,vec_p src)
{
	CALLER_NAME(matRowMatrixFromVecs)
	u32_t row, col, count;
	matrix_p retval;
	if(!rows || !cols || !src)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"%s(%u,%u,%X): %s.\n",caller_name,rows,cols,src,error_incorrect_parameters);
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	retval = matAlloc(rows, cols);
	if(!retval)
		return NULL;
	for(count = row = 0;row < rows;row++)
		for(col = 0;col < cols;col++)
			retval->cell[row][col] = src[count++];
	return retval;
}

/*
 * Matlab syntax:
 * >> v = [1 2 3]
 * >> v = [1,2,3]
 * v =
 *	1   2   3
 * Chars '[', ']' and ',' must be in delimiter string of "lex".
 * Either current or next token should be "[".
 */
vector_p	V_GLOBAL
vecParse(vhandle_t lex)
{
	CALLER_NAME(vecParse)
	u32_t cols = 0;
	vec_t val;
	vector_p first_row = NULL;
	vector_p retval = NULL;
	vboolean comma_allowed = false;
	if(LIBV_INVALID_HANDLE == lex)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"%s(NULL): %s.\n",caller_name,error_incorrect_parameters);
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	if(!lexLastWas(lex,"[") && !lexNextIs(lex,"["))
	{
		SYN_EXPECTED(lex, "[");
		return NULL;
	}
	while(lexNext(lex))
	{
		if(lexLastFloat(lex, &val))
		{
			vecStoreVec(&first_row,val,cols++);
			comma_allowed = true;
			continue;
		}
		if(lexLastWas(lex, ",") && comma_allowed)
		{
			comma_allowed = false;
			continue;
		}
		if(lexLastWas(lex, "]"))
			break;
		PARSE_ERROR(lex, "unknown token in 1st row");
		if(first_row)
			memFree(first_row);
		return NULL;
	}
	if(!cols)
	{
		PARSE_ERROR(lex, "empty vector");
		return NULL;
	}
	retval = vecAlloc(cols);
	first_row->size = cols;
	vecCopy(retval,first_row);
	memFree(first_row);
	return retval;
}

/*
 * Matlab syntax:
 * >> m = [1 2 3;4 5 6]
 * >> m = [1,2,3;4,5,6]
 * m =
 *	1   2   3
 *	4   5   6
 * Chars '[', ']', ',' and ';' must be in delimiter string of "lex".
 * Either current or next token should be "[".
 */
matrix_p	V_GLOBAL
matParse(vhandle_t lex)
{
	CALLER_NAME(matParse)
	u32_t col, cols = 0, rows, count;
	vec_t val;
	vector_p first_row = NULL;
	matrix_p retval = NULL;
	vboolean comma_allowed = false;
	if(LIBV_INVALID_HANDLE == lex)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"%s(NULL): %s.\n",caller_name,error_incorrect_parameters);
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	if(!lexLastWas(lex,"[") && !lexNextIs(lex,"["))
	{
		SYN_EXPECTED(lex, "[");
		return NULL;
	}
	/* load 1st row */
	while(lexNext(lex))
	{
		if(lexLastFloat(lex, &val))
		{
			vecStoreVec(&first_row,val,cols++);
			comma_allowed = true;
			continue;
		}
		if(lexLastWas(lex, ",") && comma_allowed)
		{
			comma_allowed = false;
			continue;
		}
		if(lexLastWas(lex, ";") || lexLastWas(lex, "]"))
			break;
		PARSE_ERROR(lex, "unknown token in 1st row");
		if(first_row)
			memFree(first_row);
		return NULL;
	}
	if(!cols)
	{
		PARSE_ERROR(lex, "empty first row");
		return NULL;
	}
	if(lexLastWas(lex, "]"))
	{
		/* one row */
		retval = matAlloc(1, cols);
		first_row->size = cols;
		matCopyToRow(retval,first_row,0);
		memFree(first_row);
		return retval;
	}
	/* load other rows in one array */
	rows = 1;
	count = cols;
	comma_allowed = false;
	for(;;)
	{
		for(col = 0;col < cols;)
		{
			if(lexNextIs(lex,",") && comma_allowed)
			{
				comma_allowed = false;
				continue;
			}
			if(!lexLastFloat(lex, &val))
			{
				SYN_EXPECTED(lex, "floating point number");
				memFree(first_row);
				return NULL;
			}
			vecStoreVec(&first_row, val, count++);
			comma_allowed = true;
			col++;
		}
		rows++;
		if(lexNextIs(lex, ";")) /* load next row */
			continue;
		if(lexLastWas(lex,"]")) /* all done */
			break;
		SYN_EXPECTED(lex, "either ';' or ']'");
		memFree(first_row);
		return NULL;
	}
	/* store loaded data in matrix */
	retval = matRowMatrixFromVecs(rows,cols,first_row->elements);
	memFree(first_row);
	return retval;
}

void	V_GLOBAL
matWrite(vhandle_t out,matrix_p m)
{
	CALLER_NAME(matWrite)
	u32_t col, row;
	if(LIBV_INVALID_HANDLE == out || !m)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"%s(%X,%X): %s.\n",caller_name,out,m,error_incorrect_parameters);
#endif	/* CHECKED_BUILD */
		return;
	}
	filePrintf(out,"[");
	for(row = 0;row < m->rows - 1;row++)
	{
		for(col = 0;col < m->cols - 1;col++)
			filePrintf(out,"%g ", m->cell[row][col]);
		filePrintf(out,"%g; ", m->cell[row][col]);
	}
	for(col = 0;col < m->cols - 1;col++)
		filePrintf(out,"%g ", m->cell[row][col]);
	filePrintf(out,"%g]\n", m->cell[row][col]);
}

void	V_GLOBAL
vecWrite(vhandle_t out,vector_p v)
{
	CALLER_NAME(vecWrite)
	u32_t col;
	if(LIBV_INVALID_HANDLE == out || !v)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"%s(%X,%X): %s.\n",caller_name,out,v,error_incorrect_parameters);
#endif	/* CHECKED_BUILD */
		return;
	}
	filePrintf(out,"[");
	for(col = 0;col < v->size - 1;col++)
		filePrintf(out,"%g ", v->elements[col]);
	filePrintf(out,"%g]\n", v->elements[col]);
}

void	V_GLOBAL
vecsWriteXY(char *outname,vec_p X,vec_p Y,u32_t num_elems)
{
	CALLER_NAME(vecsWriteXY)
	u32_t i;
	vhandle_t out;
	if(!outname || !X || !Y || !num_elems)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"%s(%X,%X,%X,%u): %s.\n",caller_name,
			outname,X,Y,num_elems,error_incorrect_parameters);
#endif	/* CHECKED_BUILD */
		return;
	}
	out = fileOpenWrite(outname);
	if(LIBV_INVALID_HANDLE == out)
		return;
	for(i = 0;i < num_elems;i++)
		filePrintf(out,"%g\t%g\n",X[i],Y[i]);
	fileClose(out);
}

void	V_GLOBAL
vecsWriteXYsvgEx(char *outname,vec_p X,vec_p Y,u32_t num_elems,
	vec_t x_ofs,vec_t x_scale,vec_t y_ofs,vec_t y_scale,
	char *stroke_color, char *stroke_width,vboolean draw_box,vboolean draw_axes)
{
	CALLER_NAME(vecsWriteXYsvgEx)
	u32_t i;
	vec3_t v, mins, maxs;
	vhandle_t out;
	char *def_stroke_color = "000000";
	char *def_stroke_width = "0.1pt";
	if(!outname || !X || !Y || !num_elems)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"%s(%X,%X,%X,%u): %s.\n",caller_name,
			outname,X,Y,num_elems,error_incorrect_parameters);
#endif	/* CHECKED_BUILD */
		return;
	}
	out = fileOpenWrite(outname);
	if(LIBV_INVALID_HANDLE == out)
		return;
	if(!stroke_color)
		stroke_color = def_stroke_color;
	if(!stroke_width)
		stroke_width = def_stroke_width;
	filePrintf(out, "<svg xmlns=\"http://www.w3.org/2000/svg\" width=\"210mm\" height=\"297mm\" viewBox=\"0 0 210 297\">\n");
	filePrintf(out, "\t<polyline points=\"");
	bboxClear(mins, maxs);
	for(i = 0;i < num_elems;i++)
	{
		vec3Set(v, x_ofs + x_scale * X[i], y_ofs + y_scale * Y[i], 0);
		bboxAddPoint(mins,maxs,v);
		filePrintf(out,"%g,%g ", v[0], v[1]);
	}
	filePrintf(out, "\" fill=\"none\" stroke=\"#%s\" stroke-width=\"%s\"/>\n", stroke_color, stroke_width);
	/* box */
	if(draw_box)
	{
		filePrintf(out, "\t<polyline points=\"");
			filePrintf(out,"%g,%g ", mins[0], mins[1]);
			filePrintf(out,"%g,%g ", maxs[0], mins[1]);
			filePrintf(out,"%g,%g ", maxs[0], maxs[1]);
			filePrintf(out,"%g,%g ", mins[0], maxs[1]);
			filePrintf(out,"%g,%g ", mins[0], mins[1]);
		filePrintf(out, "\" fill=\"none\" stroke=\"#000000\" stroke-width=\"0.1pt\"/>\n");
	}
	/* axes */ 
	if(draw_axes)
	{
		filePrintf(out, "\t<polyline points=\"");
			filePrintf(out,"%g,%g ", mins[0], y_ofs);
			filePrintf(out,"%g,%g ", maxs[0], y_ofs);
		filePrintf(out, "\" fill=\"none\" stroke=\"#800000\" stroke-width=\"0.4pt\"/>\n");
		filePrintf(out, "\t<polyline points=\"");
			filePrintf(out,"%g,%g ", x_ofs, mins[1]);
			filePrintf(out,"%g,%g ", x_ofs, maxs[1]);
		filePrintf(out, "\" fill=\"none\" stroke=\"#800000\" stroke-width=\"0.4pt\"/>\n");
	}
	filePrintf(out, "</svg>");
	fileClose(out);
}

void	V_GLOBAL
vecsWriteXYsvg(char *outname,vec_p X,vec_p Y,u32_t num_elems)
{
	vecsWriteXYsvgEx(outname,X,Y,num_elems,0,1,0,1,NULL,NULL,true,true);
}

/* Stepwise interpolation */
vec_t	V_GLOBAL
vecsStepwise(vec_p X,vec_p Y,u32_t num_points,vec_t x)
{
	u32_t idx, last_idx = num_points - 1;
	if(x < X[0])
		return Y[0];
	if(x >= X[last_idx])
		return Y[last_idx];
	for(idx = 1;;idx++)
		if(x < X[idx])
			break;
	return Y[idx];
}

/* Piecewise linear interpolation */
vec_t	V_GLOBAL
vecsLerp(vec_p X,vec_p Y,u32_t num_points,vec_t x)
{
	u32_t idx, right, last_idx = num_points - 1;
	if(x <= X[0])
		return Y[0];
	if(x >= X[last_idx])
		return Y[last_idx];
	for(idx = 1;;idx++)
		if(x < X[idx])
			break;
	right = idx--;
	return Y[idx] + (x - X[idx]) * (Y[right] - Y[idx]) / (X[right] - X[idx]);
}

/*
 * Returns 2 x (n+1) matrix of values that can be used for estimation of stepwise inverse
 * cumilative probability distribution function in case of target probability density function is
 * f(x) = \sum\limits_{i=1}^{n} \alpha_i \delta(x-x_i),
 * where \delta is Dirac delta, \sum\limits_{i=1}^{n} \alpha_i = 1.
 * Both x_i and \alpha_i are get from "PDF" 2 x n matix, \alpha's can be non-normalized,
 * but should be positive.
 * All x_i must be specified in ascending order.
 * The result if suitable for later vecsStepwise(m->cell[0], m->cell[1], m->cols, prob).
 */
matrix_p	V_GLOBAL
stepwiseICP(matrix_p PDF)
{
	CALLER_NAME(stepwiseICP)
	u32_t col;
	vec_t scale = 0;
	matrix_p ICP;
	if(!PDF)
		return NULL;
	for(col = 0;col < PDF->cols;col++)
	{
		if(PDF->cell[1][col] < FLOAT_ZERO_EPSILON)
		{
			conlPrintf(LOG_ERROR,"%s(): all probabilities must be positive.\n", caller_name);
			return NULL;
		}
		scale += PDF->cell[1][col];
	}
	for(col = 1;col < PDF->cols;col++)
	{
		if((PDF->cell[0][col - 1] + FLOAT_ZERO_EPSILON) >= PDF->cell[0][col])
		{
			conlPrintf(LOG_ERROR,"%s(): abscissas of Dirac's \\delta must be in ascending order.\n", caller_name);
			return NULL;
		}
	}
	scale = 1 / scale;
	ICP = matAlloc(2, PDF->cols + 1);
	ICP->cell[0][0] = FLOAT_ZERO_EPSILON;
	ICP->cell[1][0] = PDF->cell[0][0];
	for(col = 1;col < ICP->cols;col++)
	{
		ICP->cell[0][col] = ICP->cell[0][col - 1] + PDF->cell[1][col - 1] * scale;
		ICP->cell[1][col] = PDF->cell[0][col - 1];
	}
	return ICP;
}

V_LOCAL matrix_p
parseICP(vhandle_t lex,char *caller_name,matrix_p (V_GLOBAL *icp_constructor)(matrix_p in))
{
	matrix_p ICP, PDF = matParse(lex);
	if(!PDF)
		return NULL;
	if(PDF->rows != 2)
	{
		conlPrintf(LOG_ERROR,"%s(): expected 2 x N matrix, got %u x %u.\n",
			caller_name, PDF->rows, PDF->cols);
		memFree(PDF);
		return NULL;
	}
	ICP = (*icp_constructor)(PDF);
	memFree(PDF);
	return ICP;
}

/*
 * Reads x_i and \alpha_i from lexer "lex" as 2 x n matix and returns
 * stepestICP() of the result.
 */
matrix_p	V_GLOBAL
parseStepwiseICP(vhandle_t lex)
{
	CALLER_NAME(parseStepwiseICP)
	return parseICP(lex,caller_name,stepwiseICP);
}

/*
	matrix_p ICP, PDF = matParse(lex);
	if(!PDF)
		return NULL;
	if(PDF->rows != 2)
	{
		conlPrintf(LOG_ERROR,"%s(): expected 2 x N matrix, got %u x %u.\n",
			caller_name, PDF->rows, PDF->cols);
		memFree(PDF);
		return NULL;
	}
	ICP = stepwiseICP(PDF);
	memFree(PDF);
	return ICP;
*/

/*
 * Returns 2 x n matrix of values that can be used for estimation of piecewise linear
 * inverse cumilative probability distribution function in case of target probability
 * density function is
 * f(x) = 
\begin{cases}
	0, x < x_1 \\
	p_i, x_i \leq x < x_{i+1}, i = \overline{1,n - 1} \\
	0, p_n \leq x
\end{cases}
 * Last probability value p_n is ignored (assumed to be zero).
 * Both x_i and p_i are get from "PDF" 2 x n matix, p_n's can be non-normalized,
 * p_1 must be positive and adjacent p_i must be different (thus, p_{n-1} must be
 * positive).
 * The result if suitable for later vecsLerp(m->cell[0], m->cell[1], m->cols, prob).
 */
matrix_p	V_GLOBAL
linearICP(matrix_p PDF)
{
	CALLER_NAME(linearICP)
	u32_t col, maxcol, nextcol;
	vec_t scale;
	matrix_p ICP;
	if(!PDF)
		return NULL;
	maxcol = PDF->cols - 1;
	if(PDF->cell[1][0] < FLOAT_ZERO_EPSILON || PDF->cell[1][maxcol - 1] < FLOAT_ZERO_EPSILON)
	{
		conlPrintf(LOG_ERROR,"%s(): first and last but one probabilities must be positive.\n", caller_name);
		return NULL;
	}
	if(PDF->cell[1][maxcol] > 0)
		conlPrintf(LOG_WARNING,"%s(): warning: last probability (%g) ignored.\n", caller_name, PDF->cell[1][maxcol]);
	PDF->cell[1][maxcol] = 0;
	for(col = 0;col < maxcol;col++)
	{
		nextcol = col + 1;
		if(PDF->cell[1][nextcol] < 0)
		{
			conlPrintf(LOG_ERROR,"%s(): negative probability \"%g\" at column %u.\n",
				caller_name, PDF->cell[1][nextcol], nextcol);
			return NULL;
		}
		if(IsEqualEpsilon(PDF->cell[1][col],PDF->cell[1][nextcol],FLOAT_ZERO_EPSILON))
		{
			conlPrintf(LOG_ERROR,"%s(): adjacent probabilities must be different.\n", caller_name);
			return NULL;
		}
		if((PDF->cell[0][col] + FLOAT_ZERO_EPSILON) >= PDF->cell[0][nextcol])
		{
			conlPrintf(LOG_ERROR,"%s(): abscissas must be in ascending order.\n", caller_name);
			return NULL;
		}
	}
	ICP = matAlloc(2, PDF->cols);
	ICP->cell[0][0] = 0;
	ICP->cell[1][0] = PDF->cell[0][0];
	for(col = 0;col < maxcol;col++)
	{
		ICP->cell[0][col + 1] = ICP->cell[0][col] + PDF->cell[1][col] * (PDF->cell[0][col + 1] - PDF->cell[0][col]);
		ICP->cell[1][col + 1] = PDF->cell[0][col + 1];
	}
	scale = 1 / ICP->cell[0][maxcol];
	for(col = 1;col <= maxcol;col++)
		ICP->cell[0][col] *= scale;
	return ICP;
}

/*
 * Reads x_i and p_i from lexer "lex" as 2 x n matix and returns
 * linearICP() of the result.
 */
matrix_p	V_GLOBAL
parseLinearICP(vhandle_t lex)
{
	CALLER_NAME(parseLinearICP)
	return parseICP(lex,caller_name,linearICP);
}

/*
 * Calls parseStepwiseICP() and than generates "numvals" random numbers.
 */
vboolean	V_GLOBAL
vecsDirac(vhandle_t lex,vec_p dst,u32_t numvals)
{
	u32_t i;
	matrix_p icp = parseStepwiseICP(lex);
	if(!icp)
		return false;
	for(i = 0;i < numvals; i++)
		dst[i] = vecsStepwise(icp->cell[0], icp->cell[1], icp->cols, ranNextFloat(rhandle));
	memFree(icp);
	return true;
}

/*
 * Calls parseLinearICP() and than generates "numvals" random numbers.
 */
vboolean	V_GLOBAL
vecsHeaviside(vhandle_t lex,vec_p dst,u32_t numvals)
{
	u32_t i;
	matrix_p icp = parseLinearICP(lex);
	if(!icp)
		return false;
	for(i = 0;i < numvals; i++)
		dst[i] = vecsLerp(icp->cell[0], icp->cell[1], icp->cols, ranNextFloat(rhandle));
	memFree(icp);
	return true;
}
