/*-----------------------------------------------------------------------*
  Copyright (C) 1994-1998, Massachusetts Institute of Technology.
  Proprietary to Sea Grant AUV Laboratory.  All rights reserved.
  $Id: bam-dy.h,v 1.1 2000/08/19 01:16:13 rob Exp $
 *-----------------------------------------------------------------------*/

/*-----------------------------------------------------------------------*
  Modifications for GOATS 2000 (JV)
        NXP = 6 instead of 4
        deleted useless lines
 *-----------------------------------------------------------------------*/

//#define MAX_BEACON_NUMBER 10

//typedef struct {
//              int number_of_beacon;
//              int newdata;
//              double sos;
//              double ping_time;
//              double tof_var;
//              double north_init,east_init;
//              double nb[MAX_BEACON_NUMBER],eb[MAX_BEACON_NUMBER],db[MAX_BEACON_NUMBER];
//              double sr[MAX_BEACON_NUMBER],tof[MAX_BEACON_NUMBER];
//              double turn_around_time[MAX_BEACON_NUMBER];
//              double fixgain;
//              double cycle,range_tol,fix_err,timeout;
//} lbl_array98;
#include "lbl_array.h"

#define LBL_NUM_RANGES 5
#define NXP  6  /* made 6 instead of 4 for GOATS 2000 by JV */

typedef struct {
    double x, y, z;
} vector_t;

typedef enum {
    NAV_OFF, NAV_GOOD, NAV_STALE, NAV_TIMED_OUT
} fix_status_t ;

typedef struct
{
    double range, bearing, range_rate ;
    vector_t position ;
    double time, last_time ;
    int new, num_fixes ;
    fix_status_t fix_status ;
} fix_t ;

typedef enum {CW, CCW, ON_BASELINE} lbl_side_t ;
typedef enum {
    RANGE_OLD, RANGE_TOO_LONG, RANGE_FAILED_MEDIAN, RANGE_HIGH_ERROR,
    RANGE_GOOD
} range_status_t;

typedef struct {
    int i0,i1;
} lbl_baseline_pair_t ;

typedef struct
{
    vector_t xp[NXP] ;          /* xpndr positions */
    vector_t xhat ;             /* estimated vehicle position*/
    double tat[NXP] ;           /* turn-around time (sec) */
    range_status_t good[NXP];   /* see enum def above for values */
    double range[NXP][LBL_NUM_RANGES] ; /* array of ranges */
    double max_range[NXP] ;     /* max range from each xpndr */
    double time[NXP][LBL_NUM_RANGES] ; /* time each range was rcv'd */
    vector_t position[LBL_NUM_RANGES] ; /* position from each set of ranges */
    int status[LBL_NUM_RANGES] ; /* status of each set of ranges */
    double good_range[NXP] ;    /* last range declared good for each xpndr*/
    double good_time[NXP] ;     /* times for the good ranges */
    double depth ;              /* vehicle depth */
    double sound_speed ;        /* sound speed at bottom */
    double range_tol ;          /* range tolerance used for median filter */
    double fix_tol ;            /* fix tolerance used to judge ls soln */
    double timeout ;            /* timeout (secs) */
    lbl_baseline_pair_t baseline; /* baseline pair used in 2 xpndr nav */
    lbl_side_t side ;           /* baseline side used in 2 xpndr nav */
    double min_angle ;          /* min crossing angle for a 2 xpndr fix */
    double error ;              /* error from LS position fix */
    double alpha ;              /* convergence parameter  (0.5)*/
    int max_iter ;              /* max iterations for ls solution */
    int num_steps_pinv ;        /* # steps between psuedoinverse calc */
    int index ;                 /* current index for arrays */
    int new_fix ;               /* 1 if a new fix has been rcvd */
    int lost ;                  /* 1 if we're lost */
} lbl_t ;

int num_good(lbl_t *lbl);
int lbl_inc_index(lbl_t *l);
int lbl_set_baseline(char *string,lbl_t *l);
void baseline_string(lbl_t *lbl, char string[]);
int lbl_set_side(char *string,lbl_t *l);
void compute_lbl_fix(double r[],lbl_t *lbl, double t, fix_t *lbl_fix,int noisy);

range_status_t check_range(double r, double t, int channel, lbl_t *lbl);
void make_fix(lbl_t *lbl, double t, fix_t *fix);
void get_indices(lbl_t *lbl);
void chside(vector_t *xhat, lbl_t *lbl);
void ABmat(double r[], lbl_t *lbl, double A[NXP][3], double B[]);
void psuedo3(double A[NXP][3], double B[NXP], double x[]);

double sgn(double x);
void init_fix(fix_t *fix);
void copy_vector(vector_t *dest, vector_t *source);
double shape(double x, double y, int n, double x0, double y0, double rx, double ry) ;
void init_rls2_matrices(double psi[], double R[2][2], double Theta[]) ;
void update_rls2_soln(double R[2][2], double Theta[], int n, double gamma, double psi[], double y, double *perror);
double det3(double a[][3]);
void init_rls3_matrices(double R[][3], double Theta[]) ;
void update_rls3_soln(double R[][3], double Theta[], int n, double gamma, double psi[], double y, double *perror) ;
void init_ls3_matrices(double AtA[][3], double AtB[]) ;
void update_ls3_matrices(double a[], double b,double AtA[][3], double AtB[]);
int solve_ls3(double AtA[][3], double AtB[], double x[], double *pdet);
double ls3_error(double Theta[], double x[], double y) ;
double oline(vector_t *x0, vector_t *x1, vector_t *x) ;
double median(double x[],int n) ;
double atan2_fix(double x, double y);
void nav2_depth(vector_t *pa, vector_t *pb, double ra, double rb, double depth, int nside, vector_t *pd);
double gauss2d(double x, double y) ;
int matinv3(double a[][3], double ainv[][3], double min_det, double *pdet);
int matinv2(double a[2][2], double ainv[2][2], double min_det, double *pdet);