/* C Sociedad Protectora de Diplodocus soft. nov. 1991 */
#include <math.h>
#include <stdio.h>

#include "points.h"
#include "fmath.h"
#define _X 0
#define _Y 1
#define _Z 2
#ifndef	M_PI
#  define M_PI          3.14159265358979323846
#endif

#define SQR3 1.732050807568877293527446341505872366942805
#define OPS(a,b)	(a>=0? (a>=b ? ( b>=0? b:a):a):b) /* minimo positivo */
#define FEQ(a,b)	(fabs((a)-(b))<TOL)

/*___________________________________________________________________ */
/*	fcuad devuelve 	0 si ecuacion de segundo grado tiene solucion
					-1 si no tiene solucion real
					a debe ser no nulo
*/
short int fcuad(
	double a, double b, double c,
	double *x1,	/* -sqrt */
	double *x2		/* +sqrt */
	)
{
double d;

	if ( a<0 ) return(FCUAD_ANUL);
	
	d = b*b - 4.0*a*c;
	
	if ( d < 0 )
	{
		if( d < -TOL )
		{
			fprintf(stderr,"fcuad: %lf\n",d);
			return( FCUAD_DNEG );
		}
		else d=0.0;
	}
	d = sqrt(d);
	*x1 = ( -b - d )/a/2.0;
	*x2 = ( -b + d )/a/2.0;
	
	return(NO_ERR);
}

/*___________________________________________________________________ */
/* ffsen resuelve 1 = a sen - b cos     en sen	*/

short int ffsen(double a,double b,double *ang1, double *ang2 )
{
double aux_cos;

	if (( a==0.0 ) && ( b==0.0 )) return( FFSEN_ABNUL );
	if ( fabs(b) < TOL )
	{
		*ang1 = asin( 1.0 / a );
		*ang2 = M_PI - *ang1 ;
	}
	else if ( fabs(a) < TOL )
	{
		*ang1 = acos( -1.0 / b );
		*ang2 = - *ang1 ;
	}
	else
	{
		if (fcuad(a*a+b*b, -2.0*a, 1-b*b ,ang1,ang2)!=NO_ERR)
			return(FFSEN_DNEG);
		
		aux_cos=(a*(*ang1)-1.0)/b;*ang1=atan2(*ang1,aux_cos);
		aux_cos=(a*(*ang2)-1.0)/b;*ang2=atan2(*ang2,aux_cos);
		return(NO_ERR);
	}
}
/*___________________________________________________________________ */
/* fesf resuelve, para el punto i el sistema	*/
/*	
	vk vi . vj vi = 0
	| vj vi | = SQRT2
	
	Minimizando el angulo vl vj vi
*/

short int fesf(Point3D vi,Point3D vj,Point3D vk,Point3D vl)
{
double a,c,u,v,ang1,ang2,ang3,ang4;
double x1,x2;

	x1 = vk[_Z] - vj[_Z];
	x2 = ( vk[_X]/SQR3 + vk[_Y] - vj[_Y] );

	if (x1 ==  0) 
	{
		if (x2==0) return( FESF_ZZNUL );
		
		vi[_Y] = vj[_Y]*(vk[_Y]-vj[_Y])/x2;
		vi[_X] = vi[_Y] / SQR3;
		vi[_Z] = vj[_Z] - sgn(vk[_Z])*
			pow(2.0 - vi[_X]*vi[_X] - (vi[_Y]-vj[_Y])*(vi[_Y]-vj[_Y]) , .5 );
		
		if (vi[_Y] < 0 ) return( FESF_EQYNEG );
	}
	else
	{
		a = x2 / x1;
		c = vj[_Z] + vj[_Y] * ( vk[_Y] - vj[_Y] ) / x1;
		u = vj[_Y];
		v = vj[_Z];
	
		if ( fcuad(	4.0/3.0+a*a,
					2.0*(a*(v-c)-u),
					u*u + (c-v)*(c-v)-2.0,
					&x1,&x2 )				!=NO_ERR) return( FESF_NOSOL );
		
  			{
			Point3D vjm, vji;
			Float ag1, ag2;
			
			
				vi[_Y] = x1;
				vi[_Z] = c - a * vi[_Y] ;
				vi[_X] = vi[_Y] / SQR3;
				
				ResV( vl, vj, vjm );
				ResV( vi, vj, vji );
				ag1=AngrV(vjm,vji);
				
				vi[_Y] = x2;
				vi[_Z] = c - a * vi[_Y] ;
				vi[_X] = vi[_Y] / SQR3;
				
				ResV( vl, vj, vjm );
				ResV( vi, vj, vji );
				ag2=AngrV(vjm,vji);
				
				u = ( ag1 < ag2 ? x1:x2 );
			}
			
		vi[_Y] = u;
		vi[_Z] = c - a * vi[_Y] ;
		vi[_X] = vi[_Y] / SQR3;
		
		
	}
	return(NO_ERR);
}
/*___________________________________________________________________ */
/* falfa devuelve el valor minimo de alfa para delta segun y4>0	*/
short int falfa( double delta , double *alfa )
{
double a,b,a1,a2;

		if (( -M_PI/4.0 <= delta ) && ( delta <= M_PI/4.0 ))
			*alfa = 0.0;
		else if ( FEQ(delta, M_PI/2.0) ) *alfa = asin( 1.0/SQR3 );
		else
		{
			a = sqrt(3.0)/sin(fabs(delta));
			b = - cos(delta)/sin(fabs(delta));
			if ( ffsen(a,b,&a1,&a2)!=NO_ERR)  
				return(NO_ALFA);
			else
			{
				*alfa = OPS(a1,a2); 
				if ( *alfa < 0 ) return ( ALFA_NEG );
			}
		}
		return ( NO_ERR );
}
/*___________________________________________________________________ */
void MakeAlfa( void )
{
double dd,aa,ay4_pos,ay5eq2max,ay5eq2min,aexp;
FILE *archivo,*a2;
			
		if((archivo=fopen("alfa.in","r"))==NULL)
		{
			fprintf(stderr,"alfa.in not found\n");
			exit(-1);
		}
		if((a2=fopen("alfa.dat","w"))==NULL)
		{
			fprintf(stderr,"alfa.out not created\n");
			exit(-1);
		}

		while(2==fscanf(archivo,"%lf%lf",&dd,&aa))
		{
			if ((dd>=55.0) && (dd<=120.0))
			{
				double s;
				
				s = cos((dd-55.0)/(120.0-55.0)*2.0*M_PI);
				aa -= DEGS(0.03)*(1.00-s)/2.0;
			}
			fprintf(a2,"%lf\t%lf\n",dd,aa);
		}
		fclose(archivo);
		fclose(a2);
}
/*___________________________________________________________________ */
/* GetAlfa devuelve el valor de alfa para delta */
double GetAlfa( double delta )
{
static double d[256],a[256];
static char init=0;
int index;

	if (!init)
	{
		FILE *archivo;
			
		if((archivo=fopen("alfa.dat","r"))==NULL)
		{
			fprintf(stderr,"alfa.dat not found\n");
			exit(-1);
		}
		index=0;
		while(2==fscanf(archivo,"%lf%lf",&d[index],&a[index]))
		{
			d[index]=RADS(d[index]);
			a[index]=RADS(a[index]);
			index++;
		}
		fclose(archivo);
		init=1;
	}
	delta=fabs(delta);
	index=0;
	while( delta >= d[index] ) index++;

	return( (delta-d[index-1])/(d[index]-d[index-1])*(a[index]-a[index-1])+a[index-1]);
}