#include <math.h>
#include <stdlib.h>
#include "lbl.h"
#include "nav.h"
void copy_vector(vector_t *dest, vector_t *source)
{
	dest->x = source->x;
	dest->y = source->y;
	dest->z = source->z;

}
double distxyz(vector_t *a, vector_t *b)
{
	double dx,dy,dz;
	dx = a->x - b->x;
	dy = a->y - b->y;
	dz = a->z - b->z;
	return(sqrt(dx*dx + dy*dy + dz*dz));
}

// initialize a fix_t structure
void init_fix(fix_t *fix)
{
   int i;
	fix->position.x=fix->position.y=fix->position.z= 0.0;
	fix->time = fix->last_time = 0.0;
	fix->time=fix->last_time=-100.0;
	fix->side = SIDE_UNKNOWN;
	fix->new_fix=0;
	fix->num_fixes=0;
   for(i=0;i<NXP;i++)
   {
      fix->good[i] = RANGE_OLD;
      fix->range[i]=-1.0;
   }
   fix->fix_status=NAV_TIMED_OUT;
}
// invert a 2x2 matrix
int matinv2(double X[2][2],double Xi[2][2],double min_det, double *pdet)
{
    double d;
    int result;

    d = X[1][1] * X[0][0] - X[1][0] * X[0][1];

    Xi[0][0] =  X[1][1] / d;
    Xi[0][1] = -X[0][1] / d;
    Xi[1][0] = -X[1][0] / d;
    Xi[1][1] =  X[0][0] / d;
    *pdet = d;
    result = 1;
    if (fabs(d) < min_det) {
        Xi[0][0] = Xi[0][1] = Xi[1][0] = Xi[1][1] = 0.0;
        result = 0;
    }
    return result;
}
// compute the determinant of a 3x3 matrix
double det3(double a[][3])
{
	double result;
   result = a[0][0] * a[1][1] * a[2][2] +
          a[0][1] * a[1][2] * a[2][0] +
          a[0][2] * a[1][0] * a[2][1] -
          a[0][2] * a[1][1] * a[2][0] -
          a[0][0] * a[1][2] * a[2][1] -
          a[0][1] * a[1][0] * a[2][2];
	return(result);
}
// invert a 3x3 matrix
int matinv3(double a[][3], double ainv[][3], double min_det, double *pdet)
{
	double det;
	int i;
   int j;
   int result;

   *pdet = det = det3(a);

   if (fabs(det) > min_det) {
       result = 1;
       ainv[0][0] =  (a[1][1] * a[2][2] - a[1][2] * a[2][1]) / det;
       ainv[1][0] = -(a[1][0] * a[2][2] - a[1][2] * a[2][0]) / det;
       ainv[2][0] =  (a[1][0] * a[2][1] - a[1][1] * a[2][0]) / det;
       ainv[0][1] = -(a[0][1] * a[2][2] - a[0][2] * a[2][1]) / det;
       ainv[1][1] =  (a[0][0] * a[2][2] - a[0][2] * a[2][0]) / det;
       ainv[2][1] = -(a[0][0] * a[2][1] - a[0][1] * a[2][0]) / det;
       ainv[0][2] =  (a[0][1] * a[1][2] - a[0][2] * a[1][1]) / det;
       ainv[1][2] = -(a[0][0] * a[1][2] - a[0][2] * a[1][0]) / det;
       ainv[2][2] =  (a[0][0] * a[1][1] - a[0][1] * a[1][0]) / det;
   }
   else {
       result = 0;
       for (i = 0; i < 3; ++i) {
           for (j = 0; j < 3; ++j) {
               ainv[i][j] = 0.0;
            }
       }
   }
   return result;
}

// a version of atan2 that doesn't fail if you give it 0.0,0.0
double atan2_fix(double x, double y)
{
	if ( (fabs(x)<.0001) && (fabs(y)<.0001) ) return (0.0);
	return (atan2(x,y));
}

// compute a planar nav solution using explicit depth measurement for
// a 2 element net

void nav2_depth(vector_t *pa, vector_t *pb, double ra, double rb, double depth,
		int nside, vector_t *pd)
{

	double da,db,ra2,rb2;
	double x0,y0,dx,dy,theta;
	double s,c,rab2;
	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);

/* correct ranges for depth */

	da = depth-pa->z;
	db = depth-pb->z;

	ra2 = ra*ra - da*da;
	rb2 = rb*rb - db*db;

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

	if (arg<0.0) arg=0.0;
	y0 = sqrt(arg);
	if(nside < 1)y0 = -y0;

/* transform to world coordinates */
	pd->x = pa->x + c*x0 - s*y0;
	pd->y = pa->y + s*x0 + c*y0;
	pd->z = depth;
}


// sort routine, used for median calculation
void sort(int n, double ra[])
{
	int l,j,ir,i;
	double rra;

	l=(n >> 1)+1;
	ir=n;
	for (;;) {
		if (l > 1)
			rra=ra[--l];
		else {
			rra=ra[ir];
			ra[ir]=ra[1];
			if (--ir == 1) {
				ra[1]=rra;
				return;
			}
		}
		i=l;
		j=l << 1;
		while (j <= ir) {
			if (j < ir && ra[j] < ra[j+1]) ++j;
			if (rra < ra[j]) {
				ra[i]=ra[j];
				j += (i=j);
			}
			else j=ir+1;
		}
		ra[i]=rra;
	}
}
double store[10];
// compute median, limited to a length of 10
double median(double x[],int n)
{
	int n2,n2p;
	int i;
	double result;
	for(i=0;i<n;i++)store[i+1]=x[i];
	sort(n,store);
	n2p=(n2=n/2)+1;
	result=(n % 2 ? store[n2p] : 0.5*(store[n2]+store[n2p]));
	return(result);
}

#define IA 16807
#define IM 2147483647
#define AM (1.0/IM)
#define IQ 127773
#define IR 2836
#define NTAB 32
#define NDIV (1+(IM-1)/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)
// random number generator: ripped off, no guarantees!
double ran1(int *idum)
{
	int j,k;
	static int iy=0;
	static int iv[NTAB];
	double temp;
	if (*idum <= 0 || !iy) {
		if (-(*idum) < 1) *idum=1;
		else *idum = -(*idum);
		for (j=NTAB+7;j>=0;j--) {
			k=(*idum)/IQ;
			*idum=IA*(*idum-k*IQ)-IR*k;
			if (*idum < 0) *idum += IM;
			if (j < NTAB) iv[j] = *idum;
		}
		iy=iv[0];
	}
	k=(*idum)/IQ;
	*idum=IA*(*idum-k*IQ)-IR*k;
	if (*idum < 0) *idum += IM;
	j=iy/NDIV;
	iy=iv[j];
	iv[j] = *idum;
	if ((temp=AM*iy) > RNMX) return RNMX;
	else return temp;
}
// gaussian random number generator, sigma = 1
double gasdev(int *idum)
{
	static int iset=0;
	static double gset;
	double fac,rsq,v1,v2;

	if  (iset == 0) {
		do {
			v1=2.0*ran1(idum)-1.0;
			v2=2.0*ran1(idum)-1.0;
			rsq=v1*v1+v2*v2;
		} while (rsq >= 1.0 || rsq == 0.0);
		fac=sqrt(-2.0*log(rsq)/rsq);
		gset=v1*fac;
		iset=1;
		return v2*fac;
	} else {
		iset=0;
		return gset;
	}
}
// compute distance from a point (x) to a line defined by
// two points (x0, x1)
double oline(vector_t *x0, vector_t *x1, vector_t *x)
{
	double alpha;
	alpha = atan2(x1->x-x0->x, x1->y-x0->y);
	return(-cos(alpha)*(x->x-x0->x) + sin(alpha)*(x->y-x0->y));
}