#include <v_lib.h>

vec_t	lower=-1,upper=1,step=0.1f;
matrix_t *linear,*subsquare,*square,*subcube,*cube,*subhyper;
vector_t *lv,*ssv,*sv,*scv,*cv,*shv;
matrix_t *X;

vec_t V_LOCAL
str_to_float(char *str)
{
	char buf[64];
	char *ptr;
	strCopy(buf,str,64);
	for(ptr=buf;*ptr;ptr++)
		if(*ptr==',')
			*ptr='.';
	return (vec_t)atof(buf);
}


void	expand_linear(vec_t *out,vec_t *in)
{
	out[0]=1;
	out[1]=in[0];
	out[2]=in[1];
}

void	expand_subsquare(vec_t *out,vec_t *in)
{
	expand_linear(out,in);
	out[3]=in[0]*in[1];
}

void	expand_square(vec_t *out,vec_t *in)
{
	expand_subsquare(out,in);
	out[4]=in[0]*in[0];
	out[5]=in[1]*in[1];
}

void	expand_subcube(vec_t *out,vec_t *in)
{
	expand_square(out,in);
	out[6]=out[4]*in[1];
	out[7]=out[5]*in[0];
}

void	expand_cube(vec_t *out,vec_t *in)
{
	expand_subcube(out,in);
	out[8]=in[0]*out[4];
	out[9]=in[1]*out[5];
}

void	expand_subhyper(vec_t *out,vec_t *in)
{
	expand_cube(out,in);
	out[10]=in[0]*out[9];
	out[11]=in[1]*out[8];
	out[12]=out[4]*out[4];
	out[13]=out[5]*out[5];
}

void	expand_all(matrix_t *X)
{
	u32_t row;
	linear=matAlloc(X->rows,3);
	subsquare=matAlloc(X->rows,4);
	square=matAlloc(X->rows,6);
	subcube=matAlloc(X->rows,8);
	cube=matAlloc(X->rows,10);
	subhyper=matAlloc(X->rows,14);
	lv=vecAlloc(3);
	ssv=vecAlloc(4);
	sv=vecAlloc(6);
	scv=vecAlloc(8);
	cv=vecAlloc(10);
	shv=vecAlloc(14);
	for(row=0;row<X->rows;row++)
	{
		expand_linear(linear->cell[row],X->cell[row]);
		expand_subsquare(subsquare->cell[row],X->cell[row]);
		expand_square(square->cell[row],X->cell[row]);
		expand_subcube(subcube->cell[row],X->cell[row]);
		expand_cube(cube->cell[row],X->cell[row]);
		expand_subhyper(subhyper->cell[row],X->cell[row]);
	}
}

void	read_code_matrix()
{
	bstack_t pairs;
	vec_t pair[2];
	char *next;
	u32_t i;
	vhandle_t p=tokParseFile("code_matrix","\t");
	if(!p)
		exit(-1);
	stackInit(&pairs,2*sizeof(vec_t));
	for(;;)
	{
		do {
			next=tokNext(p);
		} while(next && next[0]<=' ');
		if(!next)
			break;
		pair[0]=str_to_float(next);
		do {
			next=tokNext(p);
		} while(next && next[0]<=' ');
		if(!next)
			break;
		pair[1]=str_to_float(next);
		stackPush(&pairs,pair);
	}
	tokDestroy(p);
	i=stackCount(&pairs);
	conlPrintf(LOG_INFO,"read_code_matrix(): read in %u pairs.\n",i);
	X=matAlloc(i,2);
	for(i--;stackPop(&pairs,pair);i--)
	{
		X->cell[i][0]=pair[0];
		X->cell[i][1]=pair[1];
	}
	stackClear(&pairs);
}

matrix_t *make_covariance_matrix(matrix_t *X)
{
	matrix_t *C;
	matrix_t *Xt=matAlloc(X->cols,X->rows);
	matTranspose(Xt,X);
	C=matAlloc(X->cols,X->cols);
	matMul(C,Xt,X);
	C=matInverse(C,false);
	return C;
}

void	make_grid(char *filename,vector_t *dest)
{
	int i,j,k,l;
	vec_t xy[2];
	void (*expandproc)(vec_t *out,vec_t *in);
	file_t out=fileOpenWrite(filename);
	matrix_t *C;
	vector_t *dC;
	l=k=(u32_t)((upper-lower+0.0001)/step)+1;
	xy[1]=lower;
	dC=vecAlloc(dest->size);
	switch(dest->size)
	{
		case 3:
			expandproc=expand_linear;
			C=make_covariance_matrix(linear);
			break;
		case 4:
			expandproc=expand_subsquare;
			C=make_covariance_matrix(subsquare);
			break;
		case 6:
			expandproc=expand_square;
			C=make_covariance_matrix(square);
			break;
		case 8:
			expandproc=expand_subcube;
			C=make_covariance_matrix(subcube);
			break;
		case 10:
			expandproc=expand_cube;
			C=make_covariance_matrix(cube);
			break;
		case 14:
			expandproc=expand_subhyper;
			C=make_covariance_matrix(subhyper);
			break;
		default:
			puts("make_grid() !");
			exit(0);
	}
	/* x - labels */
	/*
	filePrintf(out,"0");
	xy[0]=lower;
	for(j=0;j<l;j++,xy[0]+=step)
		filePrintf(out,"\t%g",xy[0]);
	*/
	for(i=0;i<k;i++,xy[1]+=step)
	{
		/* y - label */
		/*
		filePrintf(out,"\n%g",xy[1]);
		*/
		xy[0]=lower;
		for(j=0;j<l;j++,xy[0]+=step)
		{
			expandproc(dest->elements,xy);
			matVecMul(dC,C,dest);
			/* filePrintf(out,"\t%g",vecDot(dC,dest)); */
			filePrintf(out,"%g,%g,%g\n",xy[0],xy[1],1 / vecDot(dC,dest));
		}
	}
	memFree(C);
	memFree(dC);
	fileClose(out);
}

int main()
{
	logStartConsoleLog();
	logUpperLevel(LOG_TRACE);
	read_code_matrix();
	expand_all(X);
	make_grid("linear",lv);
	puts("done linear");
	make_grid("subsqure",ssv);
	puts("done subsqure");
	make_grid("square",sv);
	puts("done square");
	make_grid("subcube",scv);
	puts("done subcube");
	make_grid("cube",cv);
	puts("done cube");
	make_grid("sybhyper",shv);
	puts("done sybhyper");
	return 0;
}
