#pragma hdrstop
#include <condefs.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "lbl.h"
#include "nav.h"
#include "constant.h"

/* This program demonstrates use of the lbl routines. It generates a vehicle
trajectory, computes ranges (with some noise and drop-outs thrown in), 
then computes a fix, and finally propagates an estimate
*/

// this is some c++ builder stuff, probably irrelevant
//---------------------------------------------------------------------------
USEUNIT("lbl.cpp");
USEUNIT("nav.cpp");
//---------------------------------------------------------------------------
#pragma argsused
// this is the time step for the estimator, normally the dt of the
// overall real-time loop
#define DT .1
// this is the time step for lbl updates
#define DT_LBL 5.0
// one-sigma noise level on ranges created by simulation
#define RNOISE 1.0

// here's the main (you knew that...)
//int main(int argc, char* argv[])
int main()
{
	vector_t sim_pos, est_pos ; // simulated and estimated position
	FILE *ffix;   // file pointers for output
	int i,k, noisy; // counters, number of xpndrs, "noisy" flag
	int idum ; // seed for random number generators
	double t, r[NXP]; // time, ranges
	double Kpos; // estimator gain
   double sim_hdg, est_hdg; // heading for simulation and estimator
   double uest, usim ; // vehicle speed for estimator and simulation
	fix_t fix;     // structure containing results
   lbl_t lbl; // structure containing all lbl parameters and states

	noisy = 0; // turn off debugging messages
	idum = 0;  // initialize random number seed
	Kpos = 1.0-exp(-DT/30.0);  // estimator gain, 30 sec time constant
   // initialize the lbl structure
	init_lbl(&lbl)	;
   // read in xpndr locations from xpndr.dat
   read_xpndrs(&lbl);
   // open the output file
	ffix = fopen("fixes.out","w");
   // initialize the simulated position relative to the first xpndr 
	sim_pos.x= lbl.xp[0].x - 400.;
	sim_pos.y= lbl.xp[0].x + 400.;
	sim_pos.z= 600.;
   // initialize the fix structure
	init_fix(&fix);
   // set the heading for simulated motion, in radians
	sim_hdg = 90.0*DTOR;
   // the estimated heading has a small error
	est_hdg = sim_hdg + .05;
   // the speed for simulated motion
	usim = .8;
   // the estimated speed is slightly in error
	uest = .9*usim;
	t = 0;
   // the main loop runs at DT, the real-time loop speed (not the
   // lbl loop rate)
	for(i=0;i<12000;i++)
	{
      // propagate the simulated position
		sim_pos.x += usim*sin(sim_hdg)*DT;
		sim_pos.y += usim*cos(sim_hdg)*DT;
      // set the depth in the lbl structure, in the real situation
      // this depth would come from the vehicle's pressure sensor
		lbl.depth = sim_pos.z;
      // default, no new fix has been received
      fix.new_fix=0;
      // every DT_LBL seconds, simulate an LBL hit
		if ( i%(int)(DT_LBL/DT) == 0)
		{
         // compute the ranges based on the simulated position
			for(k=0;k<4;k++)r[k]=distxyz(&sim_pos,&(lbl.xp[k]))
            +RNOISE*gasdev(&idum);
         // corrupt the ranges in various ways 
			if( (i > 1000) && (i < 3000))
				r[3] += 500.0;
         if( (i > 4000) && (i < 5000))
            r[1] += 600.0; 
         if( (i > 6000) && (i < 7000))
            r[2] *= ran1(&idum);
         if( (i > 8000) && (i < 9000))
            r[2]=r[3]=0.0;
#ifdef DEBUG
			printf("ready to compute fix\n");
#endif
         // this statement computes the fix
			compute_lbl_fix(r,&lbl,t,&fix,noisy);
		}
      /* every DT, propagate the the vehicle position forward based on
      the estimated speed. If we had an improved speed estimate from a
      doppler or prop speed calibration, we would use that
      */
		est_pos.x += uest*sin(est_hdg)*DT;
		est_pos.y += uest*cos(est_hdg)*DT;
		if(fix.fix_status == NAV_GOOD)
		{
         // for the first good fix, set the estimated position to xhat
         if(fix.num_fixes==1)
            copy_vector(&(lbl.xhat),&est_pos);
         // for good fixes, supplement the open-loop estimate with the
         // first-order filtered fixes 
			est_pos.x += Kpos*(fix.position.x-est_pos.x);
			est_pos.y += Kpos*(fix.position.y-est_pos.y);
		}
      // print stuff out
      if(fix.new_fix)
      {
			fprintf(ffix,"%7.1lf %d %d %d %d %d %d %d %7.2lf %7.2lf %7.2lf %7.2lf",
				t,fix.fix_status,num_good(&lbl),lbl.side,
            lbl.good[0],lbl.good[1],lbl.good[2],lbl.good[3],
            r[0],r[1],r[2],r[3]);
			fprintf(ffix," %7.1lf %7.1lf %7.1lf %7.1lf %7.1lf %7.1lf",
				sim_pos.x,sim_pos.y,sim_pos.z,
				fix.position.x,fix.position.y,fix.position.z);
			fprintf(ffix," %.2lf %.2lf\n",
				est_pos.x, est_pos.y);
      }
		t += DT;
	}

   return 0;
}