#ifndef _YOERGER_LBL_H
#define _YOERGER_LBL_H

/* this header file contains the data structures and function definitions
use in the lbl software. The main data structure is called lbl_t. It 
contains both the parameter settings, the current state, and significant
elements of the state from previous lbl cycles kept in ring buffers
*/
// this constant defines the number of ranges kept in the ring buffer
#define LBL_NUM_RANGES 5
// max number of xpndrs. I'm pretty certain this could be increased, 
// although I haven't tested it
#define NXP 4
// this structure type defines a simple 3D position or velocity vector
typedef struct
{
	double x,y,z;
} vector_t;
// this enumeration is used in 2 element fixes
typedef enum {CW, CCW, ON_BASELINE, SIDE_UNKNOWN} lbl_side_t ;
// this enumeration
typedef enum {RANGE_OLD,RANGE_TOO_LONG,
	RANGE_FAILED_MEDIAN, RANGE_HIGH_ERROR, RANGE_GOOD} range_status_t ;
// this structure definition is used to keep track of the indices used
// in 2 element navigation   
typedef struct
{
	int i0,i1;
}lbl_baseline_pair_t ;

// this is the big data structure 
typedef struct
{
   // these values are parameters that you will need to change.
	double sound_speed ;                      /* sound speed at bottom */
	double max_range[NXP] ;                   /* max range from each xpndr
                                             critical for keeping out bounces */
	vector_t xp[NXP] ;                        /* xpndr positions */
   // these  parameters could be reset but should be fine
	double tat[NXP] ;                         /* turn-around time (sec) */  
	double range_tol ;                        /* range tolerance used for median filter */
	double fix_tol ;                    	   /* fix tolerance used to judge ls soln */
	double timeout ;                          /* timeout (secs) */
	double min_angle ;								/* min crossing angle for a 2 xpndr fix */
	double alpha ;										/* convergence parameter  (0.5)*/
	int max_iter ;										/* max iterations for ls solution */ 
	int num_steps_pinv ;	 							/* # steps between psuedoinverse calc */ 
   // these values get set automatically, either by the high level call
   // compute_lbl_fix or internally
	double depth ;                            /* vehicle depth */    
	vector_t xhat ;                           /* estimated vehicle position*/
	range_status_t good[NXP];                 /* 1 if judged good, 0 if bad, 2 if high error */
	double range[NXP][LBL_NUM_RANGES] ;       /* array of ranges */
	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 */       
	lbl_baseline_pair_t baseline;             /* baseline pair used in 2 xpndr nav */
	lbl_side_t side ;                         /* baseline side used in 2 xpndr nav */
	double error ;                            /* error from LS position fix */
	int index ;                               /* current index for arrays */     
	int new_fix ;                             /* 1 if a new fix has been rcvd */
	int lost ;                                /* 1 if we're lost, not used */           
}lbl_t ;

// enumeration used to record and evaluate fixes
typedef enum {NAV_OFF, NAV_GOOD, NAV_STALE, NAV_TIMED_OUT} fix_status_t;

// fix structure, it contains information about the current state only,
// and would be easy to send to other tasks, perhaps it needs a bit of
// expansion
typedef struct
{
	vector_t position;  // current position
	lbl_side_t side;    // current baseline side if 2 range solution
   lbl_baseline_pair_t baseline; // current baseline pair if 2 range soln
   range_status_t good[NXP];  // evaluation of all ranges
   double range[NXP];  // ranges for this cycle
	double time, last_time;  // current time, time of last fix
	int new_fix, num_fixes;  // new fix is true if fix is new, num_fixes is the
                              // total number of fixes computed since init_lbl
	fix_status_t fix_status;   // fix status
} fix_t;

// Here are the function definitions that are used from the lbl calling
// program.
void init_lbl(lbl_t *l);
void compute_lbl_fix(double r[],lbl_t *lbl, double t, fix_t *lbl_fix,
	 int noisy);
int num_good(lbl_t *lbl);
int read_xpndrs(lbl_t *lbl);


#endif