/******************************************************************************
//	nav.c			handles the navigation functions
//	
//*****************************************************************************
//	
//	Jan2009, created
//
//
//==== Future Expansion =======================================================
//		last update: Jan2009
//
//	1. 
//  2. verify that waypoint and route exceptions are handled OK.
//	
******************************************************************************/
#include	<cfxbios.h>		// Persistor BIOS and I/O Definitions
#include	<cfxpico.h>		// Persistor PicoDOS Definitions

#include	<assert.h>
#include	<ctype.h>
#include	<errno.h>
#include	<float.h>
#include	<limits.h>
#include	<locale.h>
#include	<math.h>
#include	<setjmp.h>
#include	<signal.h>
#include	<stdarg.h>
#include	<stddef.h>
#include	<stdio.h>
#include	<stdlib.h>
#include	<string.h>
#include	<time.h>


#include    "cf2_qsm332.h"
#include    "spi_cmd.h"
#include    "setup.h"
#include    "nav.h"

// globals
extern char Verbose; // switch for displaying more info
extern char IRQ_Flag; // interrupt flag: says what caused the interrupt
extern short iparam [ MAX_PARAM ]; // global parameter settings
extern struct all_param all;


// internal prototypes
void test_navigation( void );

//*****************************************************************************
void   accum_dr(  float pa, float h, float dp )
{ // calculate dr over this interval and accumulate
  // pa = pitch
  // h = heading (magnetic)
  // dp = change in pressure
  struct nav_param *nav;
  float t;
  
  nav = all.nav; // point to the nav sub-struct
  
        // compute the glide angle
        pa = flt_abs(pa); // abs value
        // if (pa<0) pa = -pa;  //=abs value
        pa = pa + ATTACK_ANGLE; // rough estimate on glide angle
        if (pa<12) pa = 12;     // set to a minimum value
        // pa = pa*0.0175;  // glide angle, in rad
        t= tan_find( pa );  // tangent

        dp = flt_abs(dp);  // take abs(dp)
        h = (  h - iparam[ IA_mag_var  ] );  // True, in degrees
        dp = dp / t;      // horiz glide dist. [m]

        nav->drx += (dp*sin_find( h ) );  // dist. traveled east
        nav->dry += (dp*cos_find( h ) );  // dist. traveled north

  return;
} //***************************************************************************

short  xy2rb( float dx, float dy, float *range) //*****************************
{	// convert dx, dy into range and bearing...returns bearing
    // bearing range = 0..360
  double bear;
  short  theta;
  
     *range = (float) sqrt( dx*dx + dy*dy );
     // bear = atan2(dx,dy)*57.3;
     bear = atan_find(dx,dy);
     theta = ilimit360( (short) bear ); // limit to range = 0..360
     
  return( theta );
} //***************************************************************************

short  LonLat2rb(float x1, float y1, float x2, float y2, float *range)
{ // convert Lat, Lon from X1 to X2 to range and bearing...returns bearing
  // x1, x2, y1, y2 are decimal degrees
  // output bearing range = 0..360
  float  dx, dy, clat, y;
  short bear;
  
    y = (y1 + y2)/2;  // average latitude
    clat =  cos_find( y )*111.1; // average longitude scalar
    dx = (x2 - x1)*clat;    // km
    dy = (y2 - y1)*111.1;   // km
    bear = xy2rb(dx, dy, range);
  
  return(bear);
} //***************************************************************************

void check_waypt( void ) // ***************************************************
{ // see if we are 'at' the waypoint, & update to the next one.
  // Special case is when waypt = 0 = Home location.
  // This fnx is only called from 'get_theta_.'
  float wlat1, wlon1;
  float lat_now, lon_now;
  float range, diff, circle, max_dx;
  short ir, iw, theta, n, detect, iradius;
  short at_waypt;  // used to decide if we're at the waypt
  short at_end = FALSE; // set true when at the end-of-route
  short at_new_waypt = FALSE; // is set if we arrive at a new waypt
  struct nav_param *nav;
  struct route_param *route; 
  struct s_waypt *waypt; 
  struct gps_param *gps;

  gps = all.gps;        // point to the gps structure
  if (!gps->valid) return; // gps is NOT valid, return immediately
  
  verify_route(); //makes sure route is legal, & heads for home if bad

  nav = all.nav;        // point to the nav parameters
  route = nav->route;    // point to the route params
  waypt = nav->waypt;    // point to the waypoint list
  n = iparam[ IA_num_route ]; // = #of points in the route
  
  // compute a watch-circle radius based on last dive: used for auto-mode range=0
  max_dx = ((float)all.eng->zmax) *0.003;  // =last dive depth * 3.0 glide ratio
  // max_dx [km] = max expected horizontal distance traveled for 1/2 dive.
  iradius = 1 + (short) max_dx;  // equivalent to rounding max_dx up
  //=1 if zmax<333, =2 if zmax<666, =3 if zmax<999, =4 if zmax<1333
  if (iradius<1) iradius=1;  // make sure that it is legal
  if (iradius>4) iradius=4;  // make sure that it is legal
  
  printf("Auto-Watch-Circle R[km] =%4.1f = %d\n",max_dx, iradius);

     //get the present lat and lon in decimal degrees
     lat_now = gps->xlat;
     lon_now = gps->xlon;
     nav->xlat = lat_now;  //save values in nav structure as well
     nav->xlon = lon_now;
     // printf("present lat, lon = %8.3f  %8.3f\n",lat_now,lon_now);
     
     // compute bearing to HOME =waypt[0] & save just in case
     theta = LonLat2rb(lon_now,lat_now, waypt[0].lon, waypt[0].lat, &range);
     nav->home = theta; // True heading to home waypt
     
     
     ir  = iparam[ IA_route_now ]; // point to the present index into the route
     // ir=0 = go HOME
     
     at_waypt = TRUE; // set TRUE to enter the while loop
     while (at_waypt) // &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
     { // check waypt & increment to next if we're there
       // if we reach the end-of-route, will also break out of this loop
       iw  = route->rlist[ir];  // points to the waypoint index
       
       if ( iw<0 ||  iw>=MAX_WAYPTS    ||   ir<0 || ir>n ) 
       { // then either ir or iw are out-of range: something is broken
          iw =0; // = HOME waypoint index
          ir =0; // = head HOME
          iparam[ IA_route_now ] = 0;  // = HOME route entry
          iparam[ IA_route_dir ] = 1;  // make sure direction is legal
          iparam[ IA_steer_pt  ] = 0; // turn off using the steering pt
          // exception is noted on shore via heading for HOME!
       }  // end if bad indices
       
       // get information for this waypt
       wlat1  = waypt[iw].lat;   
       wlon1  = waypt[iw].lon;
       if (route->circle[ir] == 0 ) // then auto-ranging
            circle = (float) iradius; // use circle based on glide depth
       else circle = (float) route->circle[ir];
       detect = route->detect[ir];
       
       // compute range & bearing from our present location
       theta = LonLat2rb(lon_now,lat_now,wlon1,wlat1,&range);
       
       // check the route-line bearing, and set if undefined
       if (nav->bear < 0 || nav->bear>360 ) //--------------
       {  // bearing has yet to be defined -----------------
          if ( route->blist[ir] == 0)
          { // then we want auto-ranging
              nav->bear = theta; // set to our present bearing to the waypt
              if (nav->owlat<-90 || nav->owlat>90)
              {  // then ill-defined old waypt settings
                 nav->owlat = lat_now;
                 nav->owlon = lon_now;
                 nav->range = range;
              }
          } else // else we want to use  a manual value, defined by blist
          { nav->bear = route->blist[ir]; }
       } // end if bear<0 ----------------------------------
       if (at_new_waypt) //---------------------------------
       {  // then we want to keep track of old info for steering pt calcs
          nav->owlat = lat_now;
          nav->owlon = lon_now;
          nav->range = range;
          nav->sbear = theta; //=bearing to new waypt
       } // end if -----------------------------------------
       
       if (at_end) break; // if reached a route endpt, quit checking
       // this exits us out of an infinite loop if we're within all waypts
       // but gets us far enough to set the new nav-bear, iw, ir values
       
       // check to see if we're at this waypoint
       diff = theta - nav->bear;  // difference between waypt path & path NOW
       diff = limit180(diff); // limit to +/- 180 degrees
       diff = flt_abs(diff); // abs value
       // if (diff<0) diff = -diff;  // take absolute value
       printf("waypt %2d : route_angle= %5d, range=%5.1f D-theta=%5.1f\n",
          iw,nav->bear,range, diff);
          
       // if diff>90, then the waypt is aft of abeam ----------------
       switch(detect) //---------------------------------------------
       { // depending on route detect, decide if we're there
          case 0 : at_waypt = range<circle; // only use range circle
                   break;
          case 1 : // use either range circle or bisector angle
                   at_waypt = ( (range<circle) || (diff>90) );
                   break;
          case 2 : // use bisector angle only
                   at_waypt = diff>90;
                   break;
        }  //end switch ---------------------------------------------
        
        if (at_waypt) // then we're at this waypt -------------------
        { // try the next waypt -------------------------------------
             at_new_waypt = TRUE;  // we have arrived @ a new waypt
             printf("At Route %2d, Waypt %2d\n",ir, iw);
             iparam[ IA_man_theta ] = -1; // turn OFF manual steering
                                    // so we head to the next waypt.
             nav->bear = -999; // =we don't know the new bearing
             // if we're at the home waypt, abort !!
             if (ir==0)
             {  all.exc->abort = TRUE;  // HOME exception
                return; 
             } // return now
             
          ir +=  iparam[ IA_route_dir ];  // point to next waypt
          // check to see if we're past an endpt ^^^^^^^^^^^^^^^^^^^
          if ( ir>n || ir<1 ) // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
          { // then we're at an endpt of the route
             at_end= TRUE;  // set TRUE that we're at the end-of-route
             switch( iparam[ IA_route_end ]) //++++++++++++++++++
             { // depending on end choice, take appropriate step
              case 0 : // go to the HOME waypoint
                     ir = iw = 0; // go to HOME waypt
                     break;
              case 1 : // repeat transect from the other end
                     if (ir<1) ir = n; //going from 1 to npts
                     else ir=1;  //else going from npts to 1
                     break;
              case 2 : // reverse route
                     if (ir<1) ir = 2; //going from 1 to 2
                     else ir =n-1;  //else going  from npts to npts-1
                     // switch route direction
                     iparam[ IA_route_dir ] = -iparam[ IA_route_dir ]; 
                     break;
              case 3 : // stay at the last waypt
                     if (ir<1) ir=1;
                     else ir = n; 
                     break;
              case 4 : // means we're done with mission
                     all.exc->abort = TRUE;
                     break;
             } // end switch on end-of-route options +++++++++++++
          } // end if at end of route ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
          // ---------------------------------------------------------
        } // end deemed that at a waypt ------------------------------
     } // end while not at waypt &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
     
     // we're here because we found a waypt that we're NOT at, or at the end-of-route
     // Save the new values: note ir, iw have already been bounds-checked
     iparam[ IA_route_now ] = ir; // save the new waypoint value
     nav->wlat = waypt[iw].lat;   
     nav->wlon = waypt[iw].lon;
     
     printf("waypt %d : lat %8.3f lon %8.3f \n",iw,nav->wlat,nav->wlon);
       
   return;
} //***************************************************************************


void  get_theta( void )  // ***************************************************
{  // *** compute new heading given a new position in gps
   // updates nav->theta with new heading=Magnetic
   //         nav->true_theta = True
   // 0901, this is called BEFORE the dive_num is incremented: handle approp.
   
  float lat_now, lon_now, clat, dr, set;
  float droute, dd, d2w, wlat, wlon;
  float x, dx1, dy1, dx2, dy2, dr2;
  float  dxs, dys;
  short phi, theta, theta2, eps, gamma, use_set, nman;
  short steer_pt, iprof, set_correct, man_theta;
  struct gps_param *gps;
  struct nav_param *nav;

  nav = all.nav;  // assign same pointer value
  gps = all.gps;  // same here
  iprof = iparam [ EA_prof_num ] +1;  //0901 +1 added
  // the next profile #: the dive_num is incr. AFTER get_theta, 
  // therefore the +1 above.
  // This allows surface() to  update bad heading params,
  // which happens before the dive_num is incremented.

  nman = iparam[ IA_steer_nprof ]; // last dive# to use steering pt
  if (nman > 0 && nman >= iprof)
    {  
       steer_pt = iparam [ IA_steer_pt ]; 
       cprintf("Using steering pt = %d\n", steer_pt );
    }
  else //nman <iprof or illegal, don't use
    {  steer_pt = 0;  }  // 0 = DON'T use

  use_set = iparam[ IA_use_set ]; // store in local variable
  if (use_set !=0) use_set = 1; // make sure is only=0 or 1 = off/on
  
  // check whether we are in current-crossing mode:
  // current-crossing has the priority
  set_correct = iparam[ IA_cross ]; //crossing current or OFF if=0
  if ( iparam[ IA_set_nprof ] < iprof  || set_correct==0 ) 
  { // then we are NOT in current-crossing mode
    //set it to the current-bucking value
    set_correct = use_set; //=0 if NOT current-bucking, 1=we do
  }
  
  // set_correct = 1 if we want to use 'current-bucking mode' set correction
  // set_correct <0, or >1 if we want to steer relative to current:  'current-crossing'
  // set_correct = 0 if want to do neither
  
  nman = iparam[ IA_man_nprof]; // last dive# to steer manually
  if (nman > 0 && nman >= iprof)
    {  
       man_theta = iparam[ IA_man_theta ]; 
       cprintf("Using manual theta = %d\n", man_theta );
    }
  else //nman <iprof or illegal, don't use
    {  man_theta = -1; }  // -1 = DON'T use
  
  //&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
  if (gps->valid) //&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
  { // valid fix, compute new direction
     check_waypt( ); // if needed, update waypt
     lat_now = nav->xlat;  //values are from check_waypt()
     lon_now = nav->xlon;
     clat = cos_find( lat_now )*111.1;  // #km in one degree Lat.
     theta = LonLat2rb(lon_now,lat_now,nav->wlon,nav->wlat,&dr);
     // theta = angle(True) to the next waypoint (wlat, wlon)
     nav->dr = dr;  // range to waypoint, in km

 printf("now= %8.3f %8.3f, wpt= %8.3f %8.3f, range=%5.1f, true_theta=%d\n",
         lat_now, lon_now, nav->wlat, nav->wlon, dr, theta );

     // compute an alternate steering point rather than go to waypoint --------
     if (steer_pt>0) // -------------------------------------------------------
     { // then find distance down route line by km = steer_pt
       droute = nav->range; // distance between waypts on the route line
       if (droute<0.1) droute = 0.1; // make sure it is OK so no divide-by-zero
       eps = nav->bear - theta;  // angle difference between route line and present angle
       d2w = dr*cos_find( eps ); // distance to waypoint, along the route line
       dd = d2w - steer_pt; // distance between steer_pt & waypt, along route line
       // if >0, then steer_pt is before waypt, else, it is beyond the waypt
       dd = ( droute - dd)/ droute; //= fraction along route line to steering point
       // if dd>1, then steer beyond waypt, if dd<0, then steer before last waypt
       wlat = nav->owlat + dd*( nav->wlat - nav->owlat); // new latitude steering pt
       wlon = nav->owlon + dd*( nav->wlon - nav->owlon); 
       theta = LonLat2rb(lon_now,lat_now,wlon,wlat,&dr);
       // theta = new angle to head for
       // print out debug info
       printf("steer pt = %6.3f %6.3f range=%6.1f\n",wlat, wlon, dr);
     } // end if steer_pt ----------------------------------------------------
     // steer pt now acts as new waypoint: we have new theta,
     
     // correct for current if requested =====================================
     if ( (set_correct !=0 ) && (nav->ovalid) ) //============================
     {  //correct only if the last gps was good & we want to do it
     
       // compute the distance traveled over the ground
       dx1 = (lon_now - nav->olon)*clat; // x km traveled over last dive
       dy1 = (lat_now - nav->olat)*111.1;
       
       // compute the distance traveled by dead-reckoning
       dx2 = nav->drx*0.001; // in km
       dy2 = nav->dry*0.001;
       dr2 = (float) sqrt(dx2*dx2 + dy2*dy2); // dead-reckoning distance, km
       if (dr2 <0.1) dr2 = 0.1; // keep >0
       
       // estimate the amount of set
       dxs = dx1 - dx2;   // est. set east-west, km
       dys = dy1 - dy2;   // est. set N-S, km
       phi = xy2rb(dxs, dys, &set);
       // phi =  direction of the current
       // set =  magnitude of the set
       printf("Set direction & strength = %4d %4.1f km\n",phi, set);
       
       if ( set_correct ==1) //------------------------------------------
       { // correct heading by the set correction
         gamma = theta - phi;  // current angle relative to desired course
         x = (set/dr2)*sin_find( gamma );  // law of sines, x = sin(eps)
           // eps = angle between the dr vector and the desired course
         if ( x>1  || x<-1 ) // then we can't correct to get back: current is too large
         { //  do our best to get back to the route-line by going 90 
           all.exc->exc_stat |= EXC_HIGH_CURRENT; //jan13
           // 0x1000 = flag that we can't maintain against set
           if (x>1)   eps =  90; // max correction
             else     eps = -90;
           // these will be clipped in the code below
           
             /* old correction: if can't buck, then cross
             if (x>1) eps = 90 - gamma; // go perpendicular to the current
             else eps = -90 - gamma; /**/
         } //xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
         else // x is in bounds, compute the angle
         {   
            eps = asin_find(x); // returns degrees
            // eps = asin(x)*57.3; 
         } //xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
         
       }  //-------------------------------------------------------------
       else
       { // else, set_correct is the desired angle relative to the current direction
         x = phi + set_correct; // new heading relative to the current
         x = limit360(x); // keep within 0..360 value
         // jun05, make so bounds are the same as above: relative to theta
         eps = x - theta; // difference between desired and straight-line
         
       } // end if..else use_set ==1
       // we have eps = difference between desired & straightline course
         eps = limit180(eps); // make sure it's within 180 degrees
         if ( eps < iparam[ IA_set_min]) eps = iparam[ IA_set_min]; // limit correction
         if ( eps > iparam[ IA_set_max]) eps = iparam[ IA_set_max]; // limit correction
         theta2 = theta + eps; //=new course to steer
         printf("head = %d, %d w/o & w/set\n",theta, theta2);
         theta = ilimit360(theta2);
       
     } //======================================================================
     // end if use_set !=0 and the last fix was valid =========================
             
     //** save values for next time
     nav->olat = lat_now;
     nav->olon = lon_now;
     nav->ovalid = TRUE;  
     
   } //&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
   else // position is not valid, let nav->bad_gps decide new heading
   {  // bad_gps is set in surface, dependent upon IA_no_gps value
      theta = nav->bad_gps;  // True heading if no valid fix : if =-1, do circle
      if (theta==-3) //flag that we want to head HOME
         theta = nav->home;
      cprintf("position NOT valid, using nav->bad_gps = %d\n", theta);
      nav->ovalid = FALSE;  //set flag that we don't have valid position      
   } // end if valid position &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
   
   if ( man_theta == -2 ) theta=-1; // just do circles
   if ( man_theta  > -1 ) theta = man_theta; // manually steer !!!
   // 0702, moved here so manually steering does not require good GPS
   if ( theta > -1 )
   { // then valid
     nav->true_theta = theta; // true course to steer
     theta += iparam[ IA_mag_var  ]; // add in the local magnetic variation
     theta = ilimit360(theta); // limit to 0..360
   } else // theta==-1: man_theta=-2, or bad_gps = -1
   { nav->true_theta = -1; } // set as flag that we do circles
   
   nav->theta = theta;
  
  printf("Mag_head = %d, pitch = %d\n",nav->theta,
                          iparam[ IA_glide_angle ]);
  
  return;
} //***************************************************************************

void test_navigation( void ) // ***********************************************
// test navigation function 
{  float wlat, wlon;
   short i, drx, dry, iprof;
   char line[80];
   struct nav_param *nav;

   all.eng->zmax = 1002; // set to a known qty
   nav = all.nav;  // point to same spot
   if (nav->route->valid != 99)
   { // then not a valid route, quit now
     printf("Route is not valid! \n");
     return;
   } // end not valid
   
   printf("give decimal latitude & longitude for present position >\n");
      SerInFlush(); // flush input
      gets(line);
      i = sscanf(line,"%f %f", &wlat, &wlon);
   printf("give profile #, drx, dry (m) >\n");
      SerInFlush(); // flush input
      gets(line);
      i = sscanf(line,"%d %d %d", &iprof, &drx, &dry);
      all.nav->drx = drx, all.nav->dry = dry;
      iparam [ EA_prof_num ] = iprof;
      
   all.gps->valid = TRUE;
   all.gps->xlat = wlat;
   all.gps->xlon = wlon;

   get_theta( );
   printf("range (km) to waypt, heading (M) to steer= %6.1f %d\n",
          nav->dr, nav->theta );
     //** save values for next time
     nav->olat = nav->xlat;
     nav->olon = nav->xlon;
     nav->ovalid = TRUE;  
   return;
} //***************************************************************************

void change_1_route( void ) //  ***********************************************
{ // change 1 route index values
  char line[80];
  short narg, x[6];
     cprintf("Enter route index, waypt index,approach angle,"
            " detection, radius >\n");
     SerInFlush(); // flush input
     gets(line);
     
     narg = read_shorts( line, 5, x ); // read in the params
     cmd_7 ( 0, narg, x); // and parse
     
  return;
} //***************************************************************************

void set_route( void )  // ****************************************************
{ // set route parameters : User input
  // route->rlist[] indices go from 1..n
    struct route_param *route;
    short i, m, n, k, angle, detect, rad;
    char line[80];

  route = all.nav->route;
  n = iparam [ IA_num_route ]; // # of points in the route
  cprintf("Use cmd C to update the waypoint list.\n");
  cprintf("#Waypts in Route  = %d\n",n);

  if ( n<1 || n>(MAX_WAYPTS-1) )
  { cprintf("Illegal #route points\n"); return; }
  
  cprintf("Enter Waypoint index, approach angle, arrival-detect, & watch-circle R\n");
  cprintf("Approach angle=0 = auto-set, else 1-360=degrees T heading to waypoint\n");
  cprintf("Detect values: 0=range only, 1=range+angle, 2=angle only\n");
  cprintf("Watch-circle radius values:  0=auto-range, else = [km] for radius\n");
  route->valid = 99;  // 99=valid; start w/assuming it is valid
  route->rlist[0] = 0; // 1st position always points to HOME=0
  route->blist[0] = 0; // and always use auto-bearing
  route->detect[0] = 0; // use range-only
  route->circle[0] = 0; // auto-ranging
  
  // get user info for the route list
  i=1; // first route index is always 1
  do { // get the route while entries are valid
    cprintf("  Route pt %d: Give waypt[i],approach angle, detection, radius >\n", i);
    SerInFlush(); // flush input
    gets(line);
    m = sscanf(line,"%d %d %d %d", &k, &angle, &detect, &rad );
    if ( m==4 && (k>0 && k<MAX_WAYPTS) 
              && (angle>-1 && angle<361)
              && (detect>-1 && detect<4)
              && (rad>-1 && rad<20)
       )
    { // then all are valid
      route->rlist[i] = k;  // point to this waypoint
      route->blist[i] = angle;  // with this approach angle
      route->detect[i] = detect; // arrival-detect choice
      route->circle[i] = rad; // watch-circle radius
    }
    else route->valid = 0; // set flag that route is not valid
  } while ( i++<n && route->valid == 99 );
  
  verify_route(); // make sure the route makes sense
  if (route->valid != 99)
    { cprintf("Set Route failed! Try again.\n"); }
  else 
    { display_route( FALSE ); } // show the route, but don't write to file
  
  return;
} //***************************************************************************

//*****************************************************************************
short insert_waypt(struct s_waypt *waypt, short iw, float xlat, float xlon) 
{ // take waypt index=iw, xlat, xlon, verify ranges
  // & if valid, update the waypoint list
  // returns 1 if good, else 0
  short good = FALSE; // assume it is bad
  
    if (   (  iw>=0     &&  iw<MAX_WAYPTS )  // waypoint OK
        && ( xlat>-90 &&  xlat<90 )  // lat is OK
        && ( xlon>-180.01 && xlon<180.01 ) // lon is OK
        ) 
    { // values are good
    	good = TRUE;
    	waypt[iw].valid= 99; // set to VALID flag
    	waypt[iw].lat = xlat;
    	waypt[iw].lon = xlon;
    	printf("%3d %9.3f %9.3f : Waypoint is now set\n",iw,xlat,xlon);
    	if (iw ==0) cprintf("HOME waypoint is now set\n");
    }
    else printf("Bad waypt values\n");
    
  return(good);
} //***************************************************************************

void change_waypt( struct s_waypt *waypt )  // ********************************
{   // Change the lat/lon values for a single waypoint
    short i, stat=0, n;
    float xlat, xlon;
    char line[80];
  
  cprintf("\nGive waypoint #, decimal Lat, Lon >\n");
  SerInFlush(); // flush input
  gets(line);
  n = sscanf(line,"%d %f %f",&i,&xlat, &xlon );
  waypt[i].valid = 0;  // set to NOT valid
  if (n==3)
  { // we read in right # of values
    stat = insert_waypt(waypt,i, xlat, xlon);  // returns TRUE if good
  } // end if n==3
  // else we're here because of a bad value/illegal input
  if (!stat) printf("-1 -1 -1  : BAD INPUT LINE %s\n",line);
  
  return;
} //***************************************************************************

void waypt_display( short yes_file ) // ***************************************
{ // display the waypoint list
  // if *fp is valid, write to a file as well
    short i;
    char s[80]; // output string
    char name[20];
    struct s_waypt *waypt;
    FILE *fp;
    
    waypt = all.nav->waypt;
    
    if (yes_file)  //------------------------------------
    { // then open a file to write out
      sprintf(name,"%04d.wpt", iparam[ EA_serno ] );
      fp = fopen(name,"w"); // over-write an old one if needed
      if (fp==NULL) cprintf("could not open %s\n",s);
      write_header( fp );
    } //-------------------------------------------------
    
  cprintf("#  i  Lat     Lon\n");
  for (i=0;i<MAX_WAYPTS;i++) //========================================
  {  // show data for ea waypt
  
     if (waypt[i].valid == 99 )
     {  // then it is valid
        sprintf(s,"@w%02d %8.3f %8.3f", i, waypt[i].lat, waypt[i].lon );
        cprintf("%s\n", s); // print to screen,
        if ( yes_file && fp !=NULL )
          fprintf(fp,"%s\r\n", s);
     }  // end if
     
  } //=================================================================
  
  if ( yes_file) 
  {
    cprintf("waypoint list written to %s\n",name);
    fclose( fp ); // close the file
    all.sbd->save_waypt = FALSE; // set flag that it is saved.
  }
  
  return;
} //***************************************************************************

void store_waypt(float x, short *xdeg, short *xf)  //**************************
{ // store a float in 2 shorts : degrees + fraction
  // x = float input, decimal degrees
  // xdeg = signed degrees
  // xf = unsigned fraction of degrees  *1000
  // used by lifo_waypts to store values as ints
  short xsgn=1, xmin, deg;

    if (x<0) 
    { xsgn = -1; x = -x; } // keep sign, make x>0
    
    deg = x;  // unsigned integer of degrees
    xmin = ( x - deg)*1000.; // fraction of degree, *1000
    
    *xdeg = deg*xsgn;  // make output the signed value
    *xf   = xmin;  // and store the fractional value as well

  return;
} //***************************************************************************

short fix_bad_route ( void ) //************************************************
{ // if the route-valid flag is bad,
  // set the route to head HOME.
  // if HOME is not valid, also set that
  
  // returns TRUE if had to fix it
  short ierr = 0, n, k, ok;
  
   struct s_waypt *waypt;
   struct route_param *route;
   struct nav_param *nav;

   nav = all.nav;
   route = nav->route;
   waypt = nav->waypt;
   n = iparam [ IA_num_route ]; // # of waypoints in the route
   k = iparam [ IA_route_now ];
   ok = ( n<MAX_WAYPTS && k>=0 && k<=n ); // legal
   route->rlist [ 0 ] = 0; // make sure 0-entry points to HOME

     if ( route->valid != 99 || !ok ) //===============================
     { // NOT VALID, force to HOME 
        iparam [ IA_route_now ] = 0;  // route-entry=0=points to HOME
        iparam [ IA_route_dir ] = 1;
        iparam [ IA_route_end ] = 3;
        iparam [ IA_num_route ] = 1; // only one point!

        if ( waypt[0].valid !=99 )
        { // HOME is NOT a VALID waypoint!
          waypt[0].lat   = 0.0;
          waypt[0].lon   = 0.0;
          waypt[0].valid = 99; // set valid
        }

        cprintf("Not Valid Route.  Setting to head HOME\n");
        route->valid = 99; // fixed
        ierr = TRUE;
      } //=============================================================
  
  return ( ierr );
} //***************************************************************************

short verify_route( void ) //**************************************************
{ // verify that the route's waypoints are not garbage
  // returns TRUE if OK, else FALSE

   short i,n, iw,  i_ok=TRUE;
   struct s_waypt *waypt;
   struct route_param *route;
   struct nav_param *nav;

   nav = all.nav;
   route = nav->route;
   waypt = nav->waypt;
   
   fix_bad_route();  // if route is not valid, sets to head HOME
   n = iparam [ IA_num_route ]; // # of waypoints in the route

   i =1; // verify that the route->waypt indices make sense
   do //===============================================================
   { // step thru i and verify
     iw = route->rlist[i]; // waypoint in the route
     i_ok = (iw>=0 && iw<MAX_WAYPTS ); // valid range
     
     if (i_ok) // verify the values make sense
       i_ok = ( waypt[iw].valid==99 &&
                waypt[iw].lat>-90   && waypt[iw].lat<90  &&
                waypt[iw].lon>-180  && waypt[iw].lon<180  );
   } while (i_ok && i++<n ); //========================================
   
   if (!i_ok) // then something is bad
   {   cprintf("Route List ill-defined at route %d, waypt %d\n", i, iw);
       fix_bad_route(); // head for HOME
   }
   all.sbd->calc_route = FALSE; //clear flag that might have been set via shore cmd
   
  return ( i_ok );
} //***************************************************************************
   
void display_route( short yes_file ) // ***************************************
{  // display the route and write to file if wanted

   short ir, iw, n;
   struct s_waypt *waypt;
   struct route_param *route;
   struct nav_param *nav;
   char name[20];
   char s[80];
   FILE *fp;

   nav = all.nav;
   route = nav->route;
   waypt = nav->waypt;
   n = iparam [ IA_num_route ]; // # of waypoints in the route
   

    if (yes_file)  //------------------------------------
    { // then open a file to write out
      sprintf(name,"%04d.rte", iparam[ EA_serno ] );
      fp = fopen(name,"w"); // over-write an old one if needed
      if (fp==NULL) cprintf("could not open %s\n",s);
      write_header( fp );
    } //-------------------------------------------------
   
   sprintf(s,"@R %3d %3d %3d %3d  "
          "=#wpts in rte, next wpt, end-rte-flag, dir.",
     n, iparam[ IA_route_now ], iparam[ IA_route_end ], iparam[ IA_route_dir ] );
   cprintf("%s\n",s);
   if ( yes_file && fp !=NULL )  fprintf(fp,"%s\r\n", s);

   sprintf(s,"# ir,iw, approach_angle, detection, range");   
   cprintf("%s\n",s);
   if ( yes_file && fp !=NULL )  fprintf(fp,"%s\r\n", s);

   for (ir=1; ir<=n;ir++) //===========================================
   { // Display the route info for all points
      iw = route->rlist[ir];

     sprintf(s, "@r%02d %02d %1d %1d %3d",
        ir,iw, route->blist[ir], 
        route->detect[ir], route->circle[ir] );
     cprintf("%s\n",s);
     if ( yes_file && fp !=NULL )  fprintf(fp,"%s\r\n", s);
     
   } //================================================================


  if ( yes_file ) 
  {
    cprintf("route list written to %s\n",name);
    fclose( fp ); // close the file
    all.sbd->save_route = FALSE; // set flag that it has been saved.
  }
   
   cprintf("Steering Mode Settings:\n");
   cprintf("use_set = %d\n", iparam [ IA_use_set ] );
   cprintf("steer_current = %d, tmo = %d\n",iparam [ IA_cross ] , iparam[IA_set_nprof ] );
   cprintf("manual theta = %d, tmo = %d \n",iparam[ IA_man_theta ], iparam[IA_man_nprof ]);
   cprintf("steer pt km =%d, tmo = %d \n",iparam[IA_steer_pt], iparam[IA_steer_nprof] );
   cprintf("Heading limits: min = %d, max = %d \n", iparam[ IA_set_min], iparam[ IA_set_max]);
   
   return;
} //***************************************************************************

short  read_wpt( void ) //*****************************************************
{ // read the waypoint list from the file 
  //  <XXXX.wpt> where XXXX is the serial #
  // returns TRUE if no errors
  short ok = true, err;
  short sn;
  FILE *fp;
  char name[20];
  

  sn = (short) VEEFetchLong( "SERNO", NO_VEE_VAL );
  if (sn == NO_VEE_VAL) //------------------------------------
  { // then NOT found !!
      sn = 999; // set to default
      cprintf("Illegal s/n, setting to default = %d", sn);
  } //-------------------------------------------------------
  
  // compose the waypt name -------------------------------------
  { sprintf(name,"%04d.wpt",sn); }
  
  //--- open the file -------------------------------------------
  fp = fopen(name, "r"); // read
  if (fp) //-----------------------------------------------------
  { // then it is valid, we can read in
    err = rd_wpt( fp ); // read in all lines
    if (err) 
    {  cprintf("WPT err = %d\n",err);
       ok= FALSE;
    }
  }
  else 
  {
    cprintf("Waypt File %s could not be opened \n",name);
    ok = FALSE; // open error
  }
  
  all.sbd->save_waypt = FALSE; // clear flag
  fclose( fp );
   
  return ( ok );
} //***************************************************************************
    
short rd_wpt(FILE *fp ) //*****************************************************
{ // read in from file until no more lines
  // returns 0 if OK, else error status
  // err=1 = bad input
  // err=3 = incorrect #arg
  
  short n, num, err=0, ok;
  char str[80];
  short iw;
  float xlat, xlon;
  struct s_waypt *wpt;
    
  wpt = all.nav->waypt;
  
  do //====================================================================
  {  // read until EOF
    n = fget_line( fp, str, 80 ); // read up to 80 char
    
    if (n>6 && str[0] == '@' ) //++++++++++++++++++++++++++++++++
    { // then could be a legal line
      num = sscanf(str,"@w%d %f %f",&iw, &xlat, &xlon );
      
      if (num==3) //333333333333333333333333333333333333
      { // then read in the correct # of arguments
         ok = insert_waypt(wpt,iw, xlat, xlon);  // returns TRUE if good
         if (!ok) err = 1; // any bad value sets return value
        
      }  
      else err=3; //333333333333333333333333333333333333
      
    } //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    
  } while (n>=0); //=======================================================

  
  return ( err );
} //***************************************************************************

short  read_rte( void ) //*****************************************************
{ // read the route list from the file 
  //  <XXXX.rte> where XXXX is the serial #
  // returns TRUE if no errors
  short ok = true, err;
  short sn;
  FILE *fp;
  char name[20];
  

  sn = (short) VEEFetchLong( "SERNO", NO_VEE_VAL );
  if (sn == NO_VEE_VAL) //------------------------------------
  { // then NOT found !!
      sn = 999; // set to default
      cprintf("Illegal s/n, setting to default = %d", sn);
  } //-------------------------------------------------------
  
  // compose the route name -------------------------------------
  { sprintf(name,"%04d.rte",sn); }
  
  //--- open the file -------------------------------------------
  fp = fopen(name, "r"); // read
  if (fp) //-----------------------------------------------------
  { // then it is valid, we can read in
    err = rd_rte( fp ); // read in all lines
    if (err) 
    {  cprintf("RTE err = %d\n",err);
       ok= FALSE;
    }
  }
  else 
  {
    cprintf("File %s could not be opened \n",name);
    ok = FALSE; // open error
  }
  
  all.sbd->save_route = FALSE; // clear flag
  all.sbd->calc_route = FALSE; // clear flag
  fclose( fp );
   
  return ( ok );
} //***************************************************************************
    
short rd_rte(FILE *fp ) //*****************************************************
{ // read in from file until no more lines
  // returns 0 if OK, else error status
  // err=1 = bad input
  // err=3 = incorrect #arg
  
  short n, err=0, e;
  char str[80];
  // short ir, iw, a, d, r, num;
  
  do //====================================================================
  {  // read until EOF
    n = fget_line( fp, str, 80 ); // read up to 80 char
    
    if (n>6 && str[0] == '@' ) //++++++++++++++++++++++++++++++++
    { // then could be a legal line
    
      if ( str[1] == 'R' ) //--------------------------------
      { // main route parameters
        // fake into the 'W' command
        str[0] = 'W'; str[1] = ' ';
        e  = parse_cmd(  str );
        if (e) err=1;
        
      } else if ( str[1] == 'r' ) //-------------------------
      { // then individual route values
        //num= sscanf(str,"@r%d %d %d %d %d",
        //   &ir, &iw, &a, &d, &r );
        //sprintf(str,"7 %d %d %d %d %d",ir,iw,a,d,r);
        // fake into '7' command
        str[0] = '7'; str[1] = ' ';

        e  = parse_cmd(  str );
        if (e) err=2;
      } //---------------------------------------------------
      
    } //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    
  } while (n>=0); //=======================================================
  
  return ( err );
} //***************************************************************************

short  init_wpt_rte ( short reset_param ) //***********************************
{ // read in the waypt and route files if available
  // also initializes other params
  // dec11, reset_param=TRUE, then will re-init waypoints, route to not-valid.
  // ..this is meant to happen on normal start-up, but NOT on spurious, remote
  // ..so the last waypoint/route info stays in RAM if files are corrupted.
  // returns error
  // 0 = no error
  // 1 = could not read waypt file
  // 2 = could not read route file
  // 3 = could not verify the route
  short ierr = 0, ok=0, iw;
  
   struct s_waypt     *waypt;
   struct route_param *route;
   struct nav_param     *nav;

   nav = all.nav;
   route = nav->route;
   waypt = nav->waypt;
   
   nav->bad_gps = iparam [ IA_no_gps ];  // what to do if no good gps
   nav->ovalid = 0; // assume no valid old fix is stored.
   nav->owlat  =-99;
   nav->owlon  =-99;

   nav->drx    = 0; // reset the dead-reckoning
   nav->dry    = 0;
   
 
   if (  reset_param ) //========================================
   { // then normal start-up.  before reading in,
     // init to not-valid values
       for (iw=0; iw< MAX_WAYPTS; iw++ )
       {  // this sets HOME as not valid as well
          waypt[iw].valid = -1; // NOT valid
    	  waypt[iw].lat = 0;
    	  waypt[iw].lon = 0;
          route->blist [iw] = 0;
          route->rlist [iw] = 0;  // 0=HOME waypt
          route->detect[iw] = 2;
          route->circle[iw] = 0;
       }
       ok = FALSE; // waypt and route are NOT ok: not set
     
   } //==========================================================
   // this allows old data in RAM to stay on a remote reset,
   // even if the files cannot be read.
   else //=======================================================
   { // it is out of a mid-mission reset.
     // see if the route seems to be valid.
     ok =  ( route->valid ==99 ); // TRUE if could be valid route
     if ( ok ) // could be valid
       ok = verify_route(); // confirm
     if (ok) display_route( FALSE ); // display the route
   } //==========================================================
     
   // ok = TRUE if the route verified OK
   if ( ok ) return (0); // return now, no errors
   // else the route is NOT valid,
   // try reading in from the files   
   
   ok = read_wpt( ); // returns TRUE good file read
   if ( ok )  //=================================================
   {  // then read in the route
      ok = read_rte();
      if ( ok ) //-------------------------------------
      { // verify and display
        if (  verify_route() )
             display_route( FALSE );
        else ierr = 3;
        
      } else //----------------------------------------
      { // set flag that route is not valid
        ierr = 2;
      } //---------------------------------------------
      
   } //==========================================================
   else
   { // no valid waypt file =====================================
     // initialized to NOT valid settings above if op_mode==0
     ierr = 1;
   } //==========================================================
   if (Verbose) cprintf("init_wpt_rte error = %d\n",ierr);
   
   if (ierr) fix_bad_route(); // this will set route to HOME
   
   return ( ierr );
} //***************************************************************************

void menu_nav(void);

void test_nav( void ) //*******************************************************
{ char ch=' ';
  short val, tmo=0;

  cprintf("              Test of NAV   : %s \n", __DATE__);
  all.nav->bear = -999;  // set to value = NOT Determined yet
  menu_nav();
  while ( ch !='Q') //=======================================================
  {
	SerInFlush();  // flush any characters

        if (ch !=' ' && ch>0 ) cprintf("N_1>\n"); // display directory prompt
	    ch = -1; // illegal value - means it timed out
	    ch = SCIRxGetCharWithTimeout( 1000 ); // get a new character
	    tmo = check_tmo ( &ch, tmo );
	    // increments the tmo counter if ch<0, and
	    // goes to sleep mode after max_tmo.  Returns *ch as upper-case

    switch (ch) { //+++++++++++++++++++++++++++++++++++++++++
        case 'Q' : break;  // will quit the program
        case '?' : break; // will display the directory prompt
        case 'U' : menu_nav(); break;
        case  -1 : break; // wait for another character
	    case '+' :  toggle_verbose(); // toggle verbose mode
				    break;

        case 'C' : 
		   change_waypt(all.nav->waypt); 
		   break;  // change waypoint
		   
        case 'D' : 
		   cprintf("Write to file as well ? \n");
		   val = YesNo();
		   display_route( val ); break; 
		   
        case 'F' : 
		   init_wpt_rte( true ); // read in the waypt and route
		   break;  
		   
        case 'N' : 
           test_navigation( ); break;
           
        case 'S' : // set the entire route
                   printf("Resets the entire route: This will erase the old route\n");
                   printf("Do you want to continue (y/n)? >\n");
                   if (YesNo() ) set_route( );
                   break;
                   
		case 'R' :
		   change_1_route( ); break;  // change one route point
		   
        case 'W' :  // display the waypt list
		   cprintf("Write to file as well ? \n");
		   val = YesNo();
		   waypt_display( val ); 
		   break; 
		   
	default  : cprintf(" input unknown \n");
     } // end switch +++++++++++++++++++++++++++++++++++++++++
  }  // end while ------------------------------------------------ 

   return;
} //***************************************************************************

void menu_nav(void)
{ //print out menu selections *************************************************
cprintf("        NAV MENU    Q.uit Nav     :  U.pdate menu\n"
        "  C.hange Waypt  : D.isplay Route :  N.avigate tests\n" 
        "  R.oute edit(1) : S.et Route     :  W.aypt Display\n"
        "  F.ile Read .rte, .wpt\n" );
 return;
} //***************************************************************************


//*** end *********************************************************************
