#include <string.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <ctype.h>
#include <stdlib.h>
#include "constant.h"
#include "lbl.h"
#include "nav.h"


void init_lbl(lbl_t *l)
{
	int i,k;
	/* init the position estimate */
	l->xhat.x=l->xhat.y=l->xhat.z=0.0;

	/* init the xpndr positions and parameters */
	for (i=0;i<NXP;i++)
	{
		l->xp[i].x=l->xp[i].y=l->xp[i].z=0.0; //positions
		l->tat[i]=0.003; // turn around times
		l->max_range[i] = 4000.0;  // maximum ranges
		l->good_range[i] = 0.0;  // last good range
		l->good_time[i] = -100.0;  // time of last good range
		l->good[i] = RANGE_OLD; // evaluation of range
		for(k=0;k<LBL_NUM_RANGES;k++)   // fill up the ring buffers
		{
			l->range[i][k] = -100.0;
			l->time[i][k] = -100.0;
		}
    }
		/* init the status */
		/* init the computed positions */
	for(i=0;i<LBL_NUM_RANGES;i++)
	{
		l->status[i] = 0;
		l->position[i].x=l->position[i].y=l->position[i].z=0.0;
	}
	l->depth = 0.0; // the measured depth
	l->sound_speed = 1500.0;  // sound speed
	l->range_tol = 30.0; // ranges that differ from the median by more than this
                        // are declared bad
	l->fix_tol = 20.0;   // fixes with least-squares errors higher than this are bad
	l->timeout = 99999.0;  // timeout (not used)
	l->error = 0.0;
	l->baseline.i0 = 0;
	l->baseline.i1 = 1;
	/* init the baseline, side, and index */
	l->side= CW;
	l->min_angle = 15.0*DTOR; // if the crossing angle of the rays to the 
                              // transponders are less than this, 2 element
                              // solution is too close to the baseline and
                              // declared bad
	l->alpha = .5;            // convergence parameter
	l->num_steps_pinv = 10; // pseudoinverse computed every 10 steps
	l->max_iter = 40;  // max number of iterations in the nonlinear LS
	l->index = 0;
	l->new_fix = 0;
	l->lost = 0;
}
/* routine to read the transponder positions from a file called xpndr.dat
it returns the number of xpndrs
*/
int read_xpndrs(lbl_t *lbl)
{
   FILE *fxp;
   int nxp;  // number of xpndrs
	fxp = fopen("xpndr.dat","r");
	if(fxp == NULL)
	{
		printf("xpndr.dat not found\n");
		exit(-1);
	}
	nxp = 0;
	printf("xpndrs\n");
	while(fscanf(fxp,"%lf%lf%lf",
			&(lbl->xp[nxp].x),&(lbl->xp[nxp].y),&(lbl->xp[nxp].z)) == 3)
	{
		printf("%d %7.1lf %7.2lf %7.2lf\n",nxp,lbl->xp[nxp].x,
         lbl->xp[nxp].y,lbl->xp[nxp].z);
		nxp++;
	}
	fclose(fxp);
   return(nxp);
}
// this routine increments the index used to access the ring buffers
int lbl_inc_index(lbl_t *lbl)
{
	(lbl->index)++;
	return(lbl->index = lbl->index%LBL_NUM_RANGES);
}
// low level routine used by lbl_set_baseline
void lbl_set_int(int i0,int i1,lbl_t *lbl)
{
	lbl->baseline.i0 = i0;
	lbl->baseline.i1 = i1;
}
// routine to set the baseline based on the two letter description
// AB, AC, AC, BC, BD, CC
int lbl_set_baseline(char *string,lbl_t *lbl)
{
	if(strncmp(string,"AB",2)==0){
		lbl_set_int(0,1,lbl);
		return(1);
	}
	if(strncmp(string,"AC",2)==0){
		lbl_set_int(0,2,lbl);
		return(1);
	}
	if(strncmp(string,"AD",2)==0){
		lbl_set_int(0,3,lbl);
		return(1);
	}
	if(strncmp(string,"BC",2)==0){
		lbl_set_int(1,2,lbl);
		return(1);
	}
	if(strncmp(string,"BD",2)==0){
		lbl_set_int(1,3,lbl);
		return(1);
	}
	if(strncmp(string,"CD",2)==0){
		lbl_set_int(2,3,lbl);
		return(1);
	}
	return(0);
}
// make the baseline string for the current baseline
void baseline_string(lbl_t *lbl, char string[])
{
 	string[0] = 'A' + lbl->baseline.i0;
 	string[1] = 'A' + lbl->baseline.i1;
	string[2] = '\0';
}
// set the baseline side, either CW or CCW
int lbl_set_side(char *string,lbl_t *lbl)
{
	if(strncmp(string,"CW",2)==0)
	{
		lbl->side=CW;
		return(1);
	}
	if(strncmp(string,"CCW",3)==0)
	{
		lbl->side=CCW;
		return(1);
	}
	return(0);
}
// check the new ranges to see if they are too old, too long, or if
// they fail the median test
range_status_t check_range(double r, double t, int channel, lbl_t *lbl)
{
	double med;
	range_status_t result;
	if(r < 1.0)result=RANGE_OLD;
	else
	{
		if(r > lbl->max_range[channel])result = RANGE_TOO_LONG;
		else
		{
			med = median(lbl->range[channel],LBL_NUM_RANGES);
			if( fabs(med-r) < lbl->range_tol)
			{
				lbl->good_range[channel] = r;
				lbl->good_time[channel] = t;
				result = RANGE_GOOD;
			}
			else result = RANGE_FAILED_MEDIAN;
		}
	}
	/* don't put the ranges and times in until after median filtering */
	lbl->time[channel][lbl->index] = t;
	lbl->range[channel][lbl->index] = r;
	return(result);
}
	
// make a fix structure from the information in the lbl structure
void make_fix(lbl_t *lbl, double t, fix_t *fix)
{
   int i, index;
	copy_vector(&(fix->position),&(lbl->position[lbl->index]));
	fix->last_time = fix->time;
   fix->side = lbl->side;
   fix->baseline = lbl->baseline;
   fix->last_time = fix->time;
	fix->time = t;
	fix->new_fix = 1;
   index = lbl->index;
   for(i=0;i<NXP;i++)
   {
      fix->good[i]=lbl->good[i];    
      fix->range[i]=lbl->range[i][index];
   }
}
/* returns the number of ranges that have been judged good */
int num_good(lbl_t *lbl)
{
	int i, result;
	result = 0;
	for (i=0;i<NXP;i++)if(lbl->good[i] == RANGE_GOOD)result++;
	return(result);
}
/* finds the indices of the first two good ranges*/

void get_indices(lbl_t *lbl)
{
	int i;
	
	for (i=0;i<NXP;i++)
	{
		if(lbl->good[i] == RANGE_GOOD)
		{
			lbl->baseline.i0 = i;
			break;
		}
	}
	for( i = lbl->baseline.i0+1; i < NXP; i++)
	{
		if(lbl->good[i] == RANGE_GOOD)
		{
			lbl->baseline.i1 = i;
			break;
		}
	}
}
// low level routine used in quick, linear least squares 
void ABmat(double r[], lbl_t *lbl, double A[NXP][3], double B[])
{
	int i;
	double good, xpx, xpy;
	for(i = 0; i < NXP;i++)
	{
		good = (lbl->good[i] == RANGE_GOOD) ? 1.0 : 0.0;
		xpx = lbl->xp[i].x;
		xpy = lbl->xp[i].y;
		A[i][0] = good;
		A[i][1] = -2.0*good*xpx;
		A[i][2] = -2.0*good*xpy;
		B[i] = good*(r[i]*r[i] - xpx*xpx - xpy*xpy);
	}
}
// psuedo inverse for 3x3
void psuedo3(double A[NXP][3], double B[NXP], double x[])
{
	double AtA[3][3], AtAinv[3][3], AtB[3], det;
	int i, j, k;
	/* compute Atranspose*A */
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			AtA[i][j] = 0.0;
			for(k=0;k<NXP;k++)AtA[i][j] += A[k][i]*A[k][j];
		}
	}
	/* compute Atranspose*B */
	for(i=0;i<3;i++)
	{
		AtB[i] = 0.0;
		for(j=0;j<NXP;j++)AtB[i] += A[j][i]*B[j];
	}
/* invert Atranspose*A */
	matinv3(AtA,AtAinv,.00001,&det);
	/* x = inv(At*A)*At*B */
	for(i=0;i<3;i++)
	{
		x[i] = 0.0;
		for(j=0;j<3;j++)x[i] += AtAinv[i][j]*AtB[j];
	}
}
// approximate solution 
int lsquick2(double r[], lbl_t *lbl, double *perror)
{
	int ngood, result, ind, i;
	double A[NXP][3], B[NXP], x1[3], rtest[NXP];
	double x, y, dx, dy, dr, error;
	ngood = num_good(lbl);
	if(ngood	 < 3)
	{
		result = -1;
		return(result);
	}

	ABmat(r,lbl,A,B);
	psuedo3(A,B,x1);
	ind = lbl->index;
	x=lbl->position[ind].x = x1[1];
	y=lbl->position[ind].y = x1[2];
	result = ngood;
	error = 0.0;
	for(i=0;i<NXP;i++)
	{
		dx = x-lbl->xp[i].x;
		dy = y-lbl->xp[i].y;
		rtest[i] = sqrt(dx*dx + dy*dy);
		dr = r[i]-rtest[i];
		if(lbl->good[i] == RANGE_GOOD)error += dr*dr;
	}
	error = sqrt(error/(double)ngood);
	*perror = error;
	return(1);
}
// psuedo inverse for 2x2
int psuedo2(double J[NXP][2], double Jpinv[2][NXP])
{
	double JtJ[2][2], JtJinv[2][2], det;
	int i, j, k;
	/* compute Jtranspose*J */
	for(i=0;i<2;i++)
	{
		for(j=0;j<2;j++)
		{
			JtJ[i][j] = 0.0;
			for(k=0;k<NXP;k++)JtJ[i][j] += J[k][i]*J[k][j];
		}
	}
	/* compute inverse of JtJ */ 
	if(matinv2(JtJ,JtJinv,0.00001,&det) < 0)return(0);
	/* compute Jpinv = JtJinv*Jt */
	for(i=0;i<2;i++)
	{
		for(j=0;j<NXP;j++)
		{
			Jpinv[i][j]=0.0;
			for(k=0;k<2;k++)
				Jpinv[i][j] += JtJinv[i][k]*J[j][k];
		}
	}
	return(1);
}
// planar least-squares solution

int ls_soln2(double r[], double xstart, double ystart, lbl_t *lbl, double *perror,int *num_iter)
{

	double J[NXP][2],Jpinv[2][NXP],rtest[NXP];
	double x[2];
	double ngood, good[NXP],dx,dy,dr,error_last;
	int i,j,k,ind;
	ngood = 0.0;
#ifdef DEBUG
	printf("checking ranges\n");
#endif
	for(i=0;i<NXP;i++)
	{
		good[i] = 0.0;
		if(lbl->good[i] == RANGE_GOOD)
		{
			good[i] = 1.0;
			ngood += 1.0;
		}
	}
	x[0] = xstart;
	x[1] = ystart;
	j=0;
	while(1)
	{
#ifdef DEBUG
		printf("cmputing rtest\n");
#endif
		for(i=0;i<NXP;i++)
		{
			dx = x[0]-lbl->xp[i].x;
			dy = x[1]-lbl->xp[i].y;
			rtest[i] = sqrt(dx*dx + dy*dy);
			if(j%(lbl->num_steps_pinv) == 0)
			{
#ifdef DEBUG
				printf("computing J\n");
#endif
			if(rtest[i] > 1.0)
				{
					J[i][0] = good[i]*dx/rtest[i];
					J[i][1] = good[i]*dy/rtest[i];
				}
				else J[i][0]=J[i][1]=0.0;
			}
		}
		if(j%(lbl->num_steps_pinv) == 0)
		{
#ifdef DEBUG
			printf("computing Jpinv\n");
#endif
			if(!psuedo2(J,Jpinv))
			{
				lbl->error = 9999.;
				return(-1);
			}
		}
#ifdef DEBUG
		printf("updating\n");
#endif
		for(i=0;i<2;i++)
			for(k=0;k<NXP;k++)
				x[i] += lbl->alpha*Jpinv[i][k]*(r[k]-rtest[k]);
		*perror = 0.0;
#ifdef DEBUG
		printf("computing error\n");
#endif
		for(i=0;i<NXP;i++)
		{
			dr = rtest[i]-r[i];
			*perror += good[i]*dr*dr;
		}
		*perror = sqrt(*perror/ngood);
		*num_iter = j++;
		/* if error has stopped decreasing, we're done */
		if(j > 2)
			if( (error_last-*perror)/error_last < .01)break;
		if(j > lbl->max_iter)return(-2);
		error_last = *perror;
	}
	ind = lbl->index;
	lbl->position[ind].x = x[0];
	lbl->position[ind].y = x[1];
	return((int)ngood);
}
/*
returns the index of the range that disagrees with the estimated
position by the largest amount
*/
int findwrst(lbl_t *lbl,double r[])
{
	double dx, dy, biggest, rdiff[NXP], rtest[NXP];
	int i,iw;
	for(i=0;i<NXP;i++)
	{
		dx = lbl->xhat.x-lbl->xp[i].x;
		dy = lbl->xhat.y-lbl->xp[i].y;
		rtest[i] = sqrt(dx*dx + dy*dy);
		rdiff[i] = (lbl->good[i] == RANGE_GOOD) ? fabs(rtest[i]-r[i]) : 0.0;
	}
	iw = 0;
	biggest = rdiff[0];
	for(i=1;i<NXP;i++)
	{
		if(rdiff[i] > biggest)
		{
			iw = i;
			biggest = rdiff[i];
		}
	}
	return(iw);
}
/*
this routine is used when 2 xpndr nav is being used.
if changes the baseline side based on an estimated position
contained in xhat
*/
void chside(vector_t *xhat, lbl_t *lbl)
{
	int i0, i1;
	double distance_from_baseline;
	i0 = lbl->baseline.i0;
	i1 = lbl->baseline.i1;
	distance_from_baseline = oline(&(lbl->xp[i0]), &(lbl->xp[i1]), xhat);
	lbl->side = (distance_from_baseline < 0.0) ? CW : CCW;
}
/* 
computes a deterministic xy solution from 2 ranges, the two
xpndr positions, the two ranges, and the baseline side
must be provided
*/

int nav2_xy(vector_t *pa, vector_t *pb, double ra, double rb,
	lbl_side_t nside, double min_angle, vector_t *pd)
{

	double ra2,rb2;
	double x0,y0,dx,dy,theta;
	double s,c,rab,rab2;
	double xa, ya, xb, yb, maga, magb, x2, test;
	double arg;
	/* net rotation parameters */
	dx = pb->x-pa->x;
	dy = pb->y-pa->y;
	theta = atan2_fix(dy,dx);
	s = sin(theta);
	c = cos(theta);

	ra2 = ra*ra;
	rb2 = rb*rb;

/* compute solution in net coordinates */
	rab2 = dx*dx+dy*dy;
	rab = sqrt(rab2);
	if (rab2==0) x0=0;
	else x0 = (rab2+ra2-rb2)/(2.0*sqrt(rab2));
	arg=ra2-x0*x0;

	if (arg<0.0)return(-1);
	y0 = sqrt(arg);
/* determine the cross product of the unit vectors pointing at the xpndrs */
	maga = sqrt(x0*x0 + y0*y0);
	if(maga < 1.0)return(-2);
	xa = x0/maga;
	ya = y0/maga;
	x2 = x0-rab;
	magb = sqrt(x2*x2 + y0*y0);
	if(magb < 1.0)return(-3);
	xb = x2/magb;
	yb = y0/magb;
	test = xa*yb - xb*ya;
	/* angle has to be between min_angle and pi-min_angle */
	if(fabs(test) < sin(min_angle))return(-4);
	if(nside == CW)y0 = -y0;

/* transform to world coordinates */
	pd->x = pa->x + c*x0 - s*y0;
	pd->y = pa->y + s*x0 + c*y0;
	return(1);

}

/*
this is the high level function to determine an lbl fix
*/
void compute_lbl_fix(double rxyz[],lbl_t *lbl, double t, fix_t *fix, int noisy)
{
	double r[NXP], dz, xq, yq;
	int i, i0,i1, ngood, iworst, result,num_iter,lsresult;
	lbl_inc_index(lbl);
	lbl->new_fix = 0;
	/* convert from slant ranges into xy ranges */
	for(i=0;i<NXP;i++)
	{
		dz = lbl->xp[i].z - lbl->depth;
		r[i] = 0.0;
		if( rxyz[i] > fabs(dz) )r[i] = sqrt(rxyz[i]*rxyz[i] - dz*dz); 
	}
#ifdef DEBUG	
	printf("xy ranges computed\n");
#endif
	lbl->position[lbl->index].z = lbl->depth;
	/* ranges are declared not good if they fail the median
		filter test or if they are too old
	*/
#ifdef DEBUG
	printf("running median filter\n");
#endif
	for (i=0; i < NXP; i++)
	{
		lbl->good[i]=check_range(r[i],t,i,lbl);
		if( (t-lbl->good_time[i]) > lbl->timeout)
					lbl->good[i] = RANGE_OLD;
	}
	lbl->status[lbl->index] = 0;
	/* try to get a soln with more than 2 ranges */
	while( (ngood=num_good(lbl)) > 2)
	{
#ifdef DEBUG
		printf("running lsquick2\n");
#endif
      // the lsquick2 solution uses a linear least-squares solution
      // the answer is incorrect for our purposes but serves as a
      // good starting position for the nonlinear least-square
		lsquick2(r,lbl,&(lbl->error));
		/* set the starting point for ls_soln to the lsquick2 solution */
		xq = lbl->position[lbl->index].x;
		yq = lbl->position[lbl->index].y;
#ifdef DEBUG
		printf("running ls_soln2\n");
#endif
		lsresult=ls_soln2(r,xq,yq,lbl,&(lbl->error),&num_iter);
		/* if the error is below the set tolerance, soln is good */
		if(lbl->error < lbl->fix_tol)
		{
			lbl->lost = 0;  /* definately not lost! */
			lbl->new_fix = lbl->status[lbl->index] = 1;
			lbl->baseline.i0=lbl->baseline.i1=0;
         lbl->side = SIDE_UNKNOWN;
			/* we believe in LS fixes that meet the error criteria ! */
			lbl->xhat.x = lbl->position[lbl->index].x;
			lbl->xhat.y = lbl->position[lbl->index].y;
#ifdef LBLPRT
			if(noisy)printf("good soln with %d ranges\n",ngood);
#endif
			break;
		}
		else
		{
		  iworst = findwrst(lbl,r); /* pick worst range based on xhat */
		  lbl->good[iworst] = RANGE_HIGH_ERROR;  /* make it not good */
#ifdef LBLPRT
		  if(noisy)printf("range %d is bad\n",iworst);
#endif
		}
		/* the while loop will exit if ngood is not > 2 */	
	}
	/* only do the 2-range soln if not lost */
	if( (ngood == 2) && (!lbl->lost))
	{
 		lbl->error = 0.;
#ifdef LBLPRT
		if(noisy)printf("computing 2 range solution ");
#endif
		/* get the indices of the first two good ranges */
		get_indices(lbl);
		/* determine the baseline side based on xhat */
		chside(&(lbl->xhat), lbl);
		i0 = lbl->baseline.i0;
		i1 = lbl->baseline.i1;
#ifdef LBLPRT
		if(noisy)printf("2 range soln with xpndrs %d %d ranges %.1lf %.1lf\n",
			i0,i1,r[i0],r[i1]);
#endif
		/* crunch the 2-range soln, it returns false if you're too
			close to the baseline */
		if((result=nav2_xy(lbl->xp+i0, lbl->xp+i1,
							lbl->good_range[i0], lbl->good_range[i1], 
							lbl->side,lbl->min_angle,
							&(lbl->position[lbl->index])))==1)
		{
			lbl->new_fix = lbl->status[lbl->index] = 1;
		}
		else 
		{
			lbl->new_fix =  0;
			lbl->status[lbl->index] = result;
			lbl->side = ON_BASELINE;
		}
	}
	if(lbl->new_fix)
	{
		make_fix(lbl,t,fix);
		fix->num_fixes++;
		fix->fix_status = NAV_GOOD;
	}
	else
	{
		fix->new_fix = 0;
		fix->fix_status = NAV_STALE;
	}
	if(fix->time-fix->last_time > lbl->timeout)fix->fix_status = NAV_TIMED_OUT;
#ifdef LBLPRT
	if(noisy)
	{
		printf("index: %d\n",lbl->index);
		printf("     ra     rb     rc    rd     x       y      z     ta     tb     tc  status\n");
		for(i=0;i<LBL_NUM_RANGES;i++)
			printf("%d %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf   %d\n",i,
			lbl->range[0][i],lbl->range[1][i],lbl->range[2][i],lbl->range[3][i],
			lbl->position[i].x,lbl->position[i].y,lbl->position[i].z,	
			lbl->time[0][i],lbl->time[1][i],lbl->time[2][i],lbl->status[i]);
	}
#endif
}