#include <stdio.h>
#include <stdlib.h>

#include <math.h>

#include "parab.h"
#include "matrix.h"

#define	sqr(a)		((a)*(a))
#define FREE(a)		if ((a)!=NULL) free(a)
#define	SWAP(a,b)	swap((int*)&(a),(int*)&(b))
/*#define	SWAP(a,b)	swap_f((a),(b))*/
#define CROP(a)		((a)<0.0 ? 0.0 : ( (a)>1.0 ? 1.0 : (a) ) )

static void swap(int *a, int *b);
static void swap_f(Float a, Float b);
static void shift(void);
static int readln(int i);
static Float get_alfa(int p);

extern	int 	AbsTime;

int			tid;
Float *	data[EVAL_WINDOW];
char		tdata[MAX_DATA];
int			did;
size_t 		sz_data;

Float *	tmp_data;
static int	g_FirstTime;
static int	g_LastTime;

Matrix		g_B, g_D;			/* 3x3 alfa */
Matrix2		g_P1, g_P2;			/* [ P1 P2 P3 ]t */
Matrix		P11,P22;			/* g_B * P1 */
Matrix		g_R1,g_R2;			/* T * g_B * P1 */
Matrix		T1,T2;				/* t2 t 1 */

Float		g_alfa, g_beta;


/******************************************************************************/
int 	init_data(void)
{
	int i;
	
	tmp_data = calloc(sz_data,sizeof(Float));
	g_FirstTime = 1;
	g_LastTime = 0;

	g_P1.i=g_P2.i=sz_data;
	g_P1.j=g_P2.j=EVAL_WINDOW-1;
	
	data[0] = calloc(sz_data,sizeof(Float));

	for(i=0;i<EVAL_WINDOW-1;i++)
	{
		data[i+1] = calloc(sz_data,sizeof(Float));
		g_P1.a[i]=data[i];
		g_P2.a[i]=data[i+1];
		readln(i);
	}
	readln(EVAL_WINDOW-1);
		
	new_matrix(3,3,g_B);
	new_matrix(3,3,g_D);
	
	new_matrix(sz_data,3,P11);
	new_matrix(sz_data,3,P22);
	
	g_alfa = get_alfa(1);
	g_beta = get_alfa(2);

	new_matrix(sz_data,1,g_R1);
	new_matrix(sz_data,1,g_R2);

	new_matrix(3,1,T1);
	new_matrix(3,1,T2);

	return 0;
}

int		out_data(int NumLines,int PutTime,int PutPolar)
{
	static int line=0;
	Float *f,R,T,F;
	char *c;
	
	if(NumLines) printf("%d\t",line++);	
	
	if(PutPolar)
		for(f=tmp_data,c=tdata;f<tmp_data+sz_data ;)
		{
			switch(*(c++))
			{
				case 'c':
					f+=3;
					break;
				case 'p':

					*f -= tmp_data[0];
					*(f+1) -= tmp_data[1];
					*(f+2) -= tmp_data[2];

					R=pow( (	sqr(*(f))+sqr(*(f+1))+sqr(*(f+2))), 0.5);
					F=atan2(	*(f+1), *f );
					if ( F < 0 ) F += _2_M_PI;
					F *= 180.0/M_PI;
					T=acos (*(f+2)/R);
					if ( T < 0 ) T = M_PI_2 - T;
					T *= 180.0/M_PI;

					*f++ = R;
					*f++ = T;
					*f++ = F;

					break;
				case 'x':
				case 't':
					f++;
					break;
				default:
					break;
			}
		}


	for(f=tmp_data;f<tmp_data+sz_data;)
	{
		if( (f!=(tmp_data+tid)) || (PutTime) ) printf("%.16Lg\t",*f++);
		else f++;
	}
	
	printf("\n");fflush(stdout);
	return 0;
}

int		get_data(Float time)
{
	int i;
	Float *f;
	Float r,s,t;
	
	/* ojo, esta funcion solo vale para parabolic blending */
		
	if (time>data[2][tid])
	{
		if(!g_LastTime)
		{
			shift();
		}
/*		else*/
/*		{*/
/*			if (time>data[3][tid])*/
/*				return(g_LastTime);*/
/*		}*/
	}
		t = (time-data[1][tid])/(data[2][tid]-data[1][tid]);
		r = ( 1.0 - g_alfa ) * t  + g_alfa;
		s = g_beta * t;

		el(2,0,&T1) = 1.0	* CROP(1-t);
		el(1,0,&T1) = r		* CROP(1-t);
		el(0,0,&T1) = r*r	* CROP(1-t);

		el(2,0,&T2) = 1.0	* CROP(t);
		el(1,0,&T2) = s		* CROP(t);
		el(0,0,&T2) = s*s	* CROP(t);

		ProdM( &T1, &P11 , &g_R1 );
		ProdM( &T2, &P22 , &g_R2 );
		
		for( i=0, f=tmp_data; i<sz_data; i++)
		{
			*f++ = el(i,0,&g_R1) + el(i,0,&g_R2);
		}
		
	return(g_LastTime && (r>=1.0));
}

int 	flush_data(void)
{
	FREE(tmp_data);
		
	dispose_matrix(g_B);
	dispose_matrix(g_D);
	
	dispose_matrix(P11);
	dispose_matrix(P22);
	
	dispose_matrix(g_R1);
	dispose_matrix(g_R2);

	dispose_matrix(T1);
	dispose_matrix(T2);
	return 0;
}

/******************************************************************************/
static Float get_alfa(int p)
{
	Float a, *f;
	
	if(p==1)
	{
		a = (data[0][tid]-data[1][tid])/(data[2][tid]-data[1][tid]);
		a = a/(a-1.0);
	}
	else
	{
		a = (data[3][tid]-data[1][tid])/(data[2][tid]-data[1][tid]);
		a = 1/a;
	}
	f = ( p==1 ? g_B.a : g_D.a );
	
	*f++ = 1.0/a;		*f++ = -1.0/a/(1.0-a);	*f++ = 1.0/(1.0-a);	
	*f++ = -(1.0+a)/a;	*f++ = 1.0/a/(1.0-a);	*f++ = -a/(1.0-a);	
	*f++ = 1.0;			*f++ = 0.0;				*f++ = 0.0;	

	if(p==1)
		ProdM2( &g_B, &g_P1 , &P11 );
	else
		ProdM2( &g_D, &g_P2 , &P22 );

	return( a );
}

/******************************************************************************/

static void swap(int *a, int *b) /* valid for int, float & char* */
{
	(*a) ^= (*b);
	(*b) ^= (*a);
	(*a) ^= (*b);
}

static void swap_f(Float a, Float b)
{
	Float c;

	c=a;
	a=b;
	b=c;
}

static void shift(void)
{
	int i,done=0;
	
	for(i=0;i<(EVAL_WINDOW-EVAL_STEP);i++)
		SWAP(data[i],data[EVAL_STEP+i]);
		
	for(i=0;i<(EVAL_WINDOW-EVAL_STEP);i++)
	{
		g_P1.a[i]=data[i];
		g_P2.a[i]=data[i+1];
	}
		
	for(;i<EVAL_WINDOW & !done;i++)
	{
		done = readln(i);
	}
	g_LastTime=done;

	SWAP(g_D.a,g_B.a);
	SWAP(P11.a,P22.a);
	g_alfa=g_beta;
	g_beta=get_alfa(2);
}

static int readln(int i)
{
	int done=0;
	Float *f,R,T,F;
	char *c;
	
		for(f=data[i],c=tdata;f<data[i]+sz_data & !done;)
		{
			switch(*(c++))
			{
				case 'c':
					scanf("%Lf %Lf %Lf",f,f+1,f+2);
					f+=3;
					break;
				case 'p':
					scanf("%Lf %Lf %Lf",&R,&T,&F);
					T *= M_PI/180.0;
					F *= M_PI/180.0;
					*f++ = data[i][0] + R*sin(T)*cos(F);
					*f++ = data[i][1] + R*sin(T)*sin(F);
					*f++ = data[i][2] + R*cos(T);
					break;
				case 'x':
					scanf("%Lf",f);
					f++;
					break;
				case 't':
					scanf("%Lf",f);
					if((i!=0)&&(!AbsTime))
						(*f) += data[i-1][tid];
					f++;
					break;
				default:
					break;
			}
			done=feof(stdin);
		}
		
	return done;
}