/******************************************************************************
//  gps.c	 : 13Nov2008 JTS
//  control of the GPS
//
// original includes add-ons from LAR/MCM for UBLOX compatibility
// jul09v3 includes trying ant switch again in get_gps if >2 minutes w/o fix
// 0903.5 nov11 added #sat used>3 for a good fix.
//
//==== Future Expansion =======================================================
//		last update: Jan2009
//	1. 
//
//****************************************************************************/


#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    "tcm2.h"
#include    "pitch_roll.h"
#include    "gpio_mux.h"
#include    "setup.h"

#include    "gps.h"
#include    "ublox.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;

short GPS_Baud = 96;

//=== internal prototypes ===============================
void   gps_menu   ( void );
void   gps_debug  ( void );
void   gps_d_menu ( void );

//! GPS_TYPE define here
#define  GPS_TYPE 2  // define type of GPS

#if GPS_TYPE == 2
  #define GBAUD  9600L
  #define GPARITY  'O'
  #define GTYPE  "U-blox Neo5"
  #define XS 1.0  // scalar for snr min & avg values
#endif


short   gps_on(  void  ) //****************************************************
{  // power up the gps module
   // returns error with SPI comms

   short ierr = 0;
   struct spi_param *ss;
   
  ss = all.sspi; // points to the spi comms struct
  setup_spi ( CS_GPIO, ss );
  //---feb12  set up the gps baud rate
	set_gps_baud ( GPS_Baud );
  ierr = spi_aux_pwr( AUX_ON_GPS, ss ); // gps now has power
  
  ierr = spi_mux( 1, SER_GPS, ss ); // turn on rs232 & configure mux
  
  return ( ierr );
} //***************************************************************************

short   gps_off(  void  ) //***************************************************
{  // turn off the gps module
   // returns error with SPI comms
   
   short ierr = 0;
   struct spi_param *ss;
   TUPort    *tup;
   
   ss = all.sspi; // points to the spi comms struct
   setup_spi ( CS_GPIO, ss );

    tup = all.tu_port[TU_GPS].tup;  // correct tpu uart port pointer
    // can send any commands before putting to sleep
	reset_gps_baud (); //---feb12, convert baud rate back
    
    ierr = spi_aux_pwr( -AUX_ON_GPS, ss );
    DelayMilliSecs(10);  //** wait 
  
    ierr = spi_mux( 1, SER_AUX1, ss ); 
    //switches MUX to AUX1, disconnecting the GPS lines

  return ( ierr );
} //***************************************************************************

//---feb12, hooks to set, reset GPS baud
void set_gps_baud ( short baud ) //********************************************
{ // set the baud rate for the GPS serial comms
  // baud = baud rate/100: 96 = 9600
   short istat = 0;
   long b100;
   TUChParams tuart_cfg;
   TUPort    *tup;

  tup = all.tu_port[TU_GPS].tup;  // correct tpu uart port pointer
  TUGetCurrentParams( tup, &tuart_cfg ); // get the config
  // GPS uses mux1, shared with ADP and Polyplus
  b100 = baud*100;
  cprintf("Setting the baud to %ld\n", b100);
  tuart_cfg.baud = b100; // change to new value
  TUConfigure( tup, &tuart_cfg ); // and set

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

void reset_gps_baud ( void ) //************************************************
{ //reset the baud rate to the default
  short istat = 0;
  TUChParams tuart_cfg;
  TUPort    *tup;

  tup = all.tu_port[TU_GPS].tup;  // correct tpu uart port pointer
  TUGetCurrentParams( tup, &tuart_cfg ); // get the config
  tuart_cfg.baud = BAUD_MUX1; // change back; should be 9600
  TUConfigure( tup, &tuart_cfg ); // and store
  
  return;
} //***************************************************************************


short   get_gps ( short wc ) //************************************************
{ // get a gps fix
  // return 1 if valid, else 0
  // wc = wing control flag: 0=NO, 1=YES, try periodically to readjust wing
  unsigned long tnow, tstart, tend, tnext, twc = 120;
  short  hdop, ngood, iwing;
  struct gps_param *gps;
  long twait=10;  // check every twait s
  short n_used;  //nov11, #satellites used in the fix
  short   irq_flag; //dec14, hibernate output   
  
  gps = all.gps;  // point to gps structure
      
  if (gps->valid) //=======================================================
  { // true if the last time we got a valid fix
    gps->ovalid = TRUE;
    gps->olat   = gps->xlat;
    gps->olon   = gps->xlon;
  } //=====================================================================

  tstart = tnow = RTCGetTime(0,0);
  tend = tnow + iparam [ IA_gps_tm ]*60;  // try GPS up to this time
  cprintf("#minutes to try gps=%d\n", iparam [ IA_gps_tm ] ); 

  tnext = tnow + twc;  // next time to check the wing
  iwing = all.wing_now[GPS_UNIT]; // the wing we're presently using.
  if (iwing != PORT_WING) iwing = STBD_WING; // set to a legal value

  gps_on( );
  DelayMilliSecs(2000); // wait 2 s for GPS to come up  

  UBX_init(  ); // set the UBX to a known state
  
  if ( all.exc->gps_cold ) //====================
  { //dec11, force a reset
    DelayMilliSecs(2000);
    UBX_cold();
    cprintf(" UBX cold reset performed \n");
    all.exc->gps_cold = 0;
    DelayMilliSecs(2000);
    UBX_init();
  } //===========================================
  
  // initialize parameters
  hdop = gps->hdop = 2500;  // hdop is used to decide whether fix is good
  gps->nsat        = 0;
  gps->smin        = 250;
  gps->savg        = 0;
  gps->smax        = 0;
  ngood            = 0;  // # of good fixes
  
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //++++ keep trying until a valid fix ++++++++++++++++++++++++++++++++++++++
  
  while ( RTCGetTime(0,0) < tend )  
  {  // keep trying or until hdop is accurate enough
     //--dec14, handle irq from the wdog
     irq_flag = hibernate( &tnow, twait, PIT_IRQ2 );  //wait twait seconds
     if ( irq_flag == IRQ2_F ) //----------------------------
     { // interrupt from msp430,
       //cprintf("Wake-up from CF2_ATTN\n");
       service_wdog(); // service the CF2
     } //----------------------------------------------------
     n_used = check_UBX( ); // nov11 n_used = #satellites used in the fix
     if (n_used>2) hdop = gps->hdop; // if successful, update hdop
     
     if (  hdop<500 && n_used>3 )  //============================
     { // got a reasonable fix; nov11 added n_used>3 = high-quality
       ngood++; // increment # of good fixes
       if (ngood==2)  
       {  // after 2nd fix, wait 20s more
          //--- sep11, update time first, then update end-time
          UBX_SetRTC(); // update the RTClock based on GPS time
          tnow  = RTCGetTime(0,0);
          tend  = tnow + 20; 
          tnext = tend + 10; // don't worry about the wing anymore
       }
       
     } //========================================================

     if ( tnow>tnext && wc==1  ) //------------------------------
     {  // then want wing-control & time to do it
        // jul09 v3 make sure gps is on & switch is right
        gps_on(); // make sure gps is on & mux is correctly set up
        ant_switch(GPS_UNIT,iwing);  // config ant switch for right unit, wing
        if ( iwing == PORT_WING ) cprintf("PORT ");
        else cprintf("STBD ");
        cprintf(" wing control for GPS\n");
        roll_wing(iwing,0); // roll the specified wing up,0=don't seek
        UBX_init(  ); // set the UBX to a known state
        tnext += twc; // set flag for next control time
        service_wdog(); // dec14 service the CF2
     } //--------------------------------------------------------
     
  } // end while ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  
  
  tend = tnow - tstart;
  gps->time = (short) (tend/10);  // time to acquire, [10 s] incr.
  gps->valid = (hdop<900 || ngood>0);
  // 0902, went from hdop<500 to <900, as fixes seem fine.
  // OR if already deemed we got a good fix (gets around variable hdop).
  gps->wing  = iwing; // used in the sbd gps message

  // if (gps->valid) set_time(gps); // update RTC to UTC
  // could keep track of wing error here as well
  // if want to decide if a wing antenna is bad/poor
  gps_off( ); // turn off gps
  
  return ( gps->valid );
} //***************************************************************************


void  show_gps  ( void ) //****************************************************
{ // display gps structure values
	short n;
	struct gps_param *gps;
	//oct13 update for +/-0 degree situations
	float xdlat, xdlon;
	
	gps = all.gps;
	
	// 0901 nsat = satellite info only: no rstat
	// 
	n = gps->nsat; 
	//rstat = ( gps->nsat & 0xf0 ) >> 4; // upper 4 bits, shift right by 4
	if (gps->valid) { printf("NEW"); }
	else { printf("OLD"); }

    //oct13, changed to floats, & not quite zero if on equator
	xdlat = ((float)gps->lat_deg+.001)*gps->lat_sign;  // signed latitude
	xdlon = ((float)gps->lon_deg+.001)*gps->lon_sign;  // signed longitude
	
	printf("week %6d day %1d, %02d:%02d:%02d %+3.0f:%06.3f  %+4.0f:%06.3f ",
	  gps->gps_week, gps->gps_day,
	  gps->utc_hr, gps->utc_min, gps->utc_sec,
	  xdlat, gps->lat_min,
	  xdlon, gps->lon_min );
	printf("  %1d %4d %4d %4d %4.1f \n", n, gps->smin,
	  gps->savg, gps->smax, gps->hdop*0.1 );
	
	return;
} //***************************************************************************

short calc_surf_drift ( void ) //**********************************************
{ // calculate the surface drift
  // returns 1=good
  //         0=either/both fixes NOT valid: no good
  
  short good;
  long dx;
  struct ublox_param *ubx;
  float ylat, clat;
  
  ubx = all.ubx;
  
  good = ( ubx->valid[0]==1 && ubx->valid[1] == 1 );
  if ( good ) //===========================================================
  { // both fixes are valid: process
    //  ( dx*1e-7 *60 (nm/deg) * 1852 (m/nm) )*90 = 1.000 m
    ylat = ubx->lat[1]*1e-7; // decimal degrees
    clat =  cos_find( ylat ); // cos of latitude
    
    dx = (ubx->lat[1] - ubx->lat[0] )/90;
    ubx->dy = (short) dx;  // scaling = 1m per count
    
    dx = (ubx->lon[1] - ubx->lon[0] )/90;
    dx = dx*clat; // scale from fraction of long. degrees to m.
    ubx->dx = (short) dx;
    dx = ubx->ti[1]  - ubx->ti[0];
    ubx->dt = (short) dx;
  } else //================================================================
  { // not valid
    ubx->dx = 0;
    ubx->dy = 0;
    ubx->dt = 0;
  } //=====================================================================
  
  return (good);
} //***************************************************************************

 

void   test_gps (  void  )  //*************************************************
{ // test the gps functions

  char ch;
  short err, tmo=0, n, val;

  struct gps_param *gps;
  struct ublox_param *ubx;
  gps = all.gps; // point to gps structure;
  ubx = all.ubx;

 
  gps_off(  );
  gps->hdop = 2500;
  gps->nsat = 0;
  gps->smin = 250;
  gps->savg = 0;
  gps->smax = 0;
  // max time comes from iparam [ IA_gps_tm ]
  
  cprintf("              Test of GPS %s : %s \n",GTYPE, __DATE__);
  gps_menu();
  while ( ch !='Q') //=======================================================
  {
	SerInFlush();  // flush any characters

        if (ch !=' ' && ch>0 ) cprintf("G_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) { // call the desired function
        case 'Q' : break;  // will quit the program
        case '?' : break;  // displays the directory prompt
        case 'U' : gps_menu(); break;
        case 'D' : gps_debug(); break;
        case  -1 :  break; // wait for another character
	    case '+' :  toggle_verbose(); // toggle verbose mode
				    break;
        case 'W' : 
		    gps_on(   );  
		    cprintf("gps is on\n");
		    break;  
        case 'O' : 
    	    gps_off(  ); 
    	    cprintf("gps is off\n");
    	    break;  


        case 'C' : 
    	    calc_surf_drift ( ); 
    	    cprintf("Surf dx, dy, dt = %d %d %d\n",
    	      ubx->dx, ubx->dy, ubx->dt);
    	    cprintf("Give 0=start, 1=end for next GPS fix\n");
    	    n = get_num ( &val );
    	    if (n==1) ubx->index = val;
    	    break;  

        case 'G' : 
    	    check_UBX(  ); 
    	    break;  

        case 'I' : 
		    err = UBX_init (   );  
		    cprintf("gps is initialized ");
		    if (err) cprintf(" OK\n");
		    else      cprintf(" BAD\n");
		    break;  

        case 'L' : 
    	    show_gps (  ); 
    	    break;  

        case 'P' : 
    	    // err = ant_select (  ANT_GPS_2_PORT ); 
    	    ant_switch ( GPS_UNIT, PORT_WING );
    	    cprintf("port wing antenna to GPS \n");
    	    //if (err !=SPI_CRC_OK) cprintf("port ant error=%c \n",err);
    	    break;  

        case 'S' : 
    	    //err = ant_select (  ANT_GPS_2_STBD ); 
    	    ant_switch ( GPS_UNIT, STBD_WING );
    	    cprintf("stbd wing antenna to GPS \n");
    	    //if (err !=SPI_CRC_OK) cprintf("stbd ant error=%c \n",err);
    	    break;  


        default  : cprintf(" input unknown \n");
    } // end switch +++++++++++++++++++++++++++++++++++++++++++++++++++++++
    
  }  //=== end while ========================================================


  gps_off(  );  // make sure gps is off
  return;
}  //**************************************************************************

void gps_menu(void) //*********************************************************
{ //print out menu selections
 cprintf("                  GPS MENU  \n"
 	     "Q.uit GPS test :  U.pdate menu   :  D.ebug Menu\n"
         "G.et Position  :  O.ff gps       :  W.akeup GPS\n"
         "I.nit GPS      :  L.ast Pos. Display : C.alc Drift \n" );
 cprintf( "P.ort Wing     :  S.tbd Wing\n");
 
 return;
}  //**************************************************************************

void   gps_debug (  void  )  //************************************************
{ // test the gps functions

  char ch;
  short err, tmo=0, n, val;

  struct gps_param *gps;
  gps = all.gps; // point to gps structure;
  GPS_Baud = 96; //---Feb12, play around w/GPS baud rate

 
  gps_off(  );
  gps->hdop = 2500;
  gps->nsat = 0;
  gps->smin = 250;
  gps->savg = 0;
  gps->smax = 0;
  // max time comes from iparam [ IA_gps_tm ]
  
  cprintf("              Debug GPS %s : %s \n",GTYPE, __DATE__);
  gps_d_menu();
  while ( ch !='Q') //=======================================================
  {
	SerInFlush();  // flush any characters

        if (ch !=' ' && ch>0 ) cprintf("G_2>\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) { // call the desired function
        case 'Q' : break;  // will quit the program
        case '?' : break;  // displays the directory prompt
        case 'U' : gps_d_menu(); break;
        case 'D' : gps_debug(); break;
        case  -1 :  break; // wait for another character
	    case '+' :  toggle_verbose(); // toggle verbose mode
				    break;


        case 'W' : 
		    gps_on(   );  
		    cprintf("gps is on\n");
		    break;  
        case 'O' : 
    	    gps_off(  ); 
    	    cprintf("gps is off\n");
    	    break;  
    	    
        case 'B' : 
			cprintf("   give baud rate/100 (i.e. 96) \n");
			n = get_num(&val);
			if (n==1) //then a legal read of value
			{  // send command 
				set_gps_baud( val);
				GPS_Baud = val;
			 }
		    break;  

        case 'C' : 
    	    UBX_cold(  ); 
    	    cprintf("GPS cold reset sent\n");
    	    break;  

        case 'G' : 
    	    check_UBX(  ); 
    	    break;  

        case 'I' : 
		    err = UBX_init (   );  
		    cprintf("gps is initialized ");
		    if (err) cprintf(" OK\n");
		    else      cprintf(" BAD\n");
		    break;  

        case 'R' : 
    	    UBX_SetRTC(  ); 
    	    break;  

        case 'P' : 
    	    ant_switch ( GPS_UNIT, PORT_WING );
    	    cprintf("port wing antenna to GPS \n");
    	    break;  

        case 'S' : 
    	    ant_switch ( GPS_UNIT, STBD_WING );
    	    cprintf("stbd wing antenna to GPS \n");
    	    break;  


      case 'X' : 
			gps_off();  // make sure is off
            get_gps( 1 );  show_gps( ); break;


        default  : cprintf(" input unknown \n");
    } // end switch +++++++++++++++++++++++++++++++++++++++++++++++++++++++
    
  }  //=== end while ========================================================


  gps_off(  );  // turn off the gps
  cprintf("GPS is off\n");
  
  return;
}  //**************************************************************************

void gps_d_menu(void) //*******************************************************
{ //print out menu selections
 cprintf("                  GPS Debug Menu  \n"
 	     "Q.uit  Debug   :  U.pdate menu   : C.old Reset  \n"
         "G.et Position  :  O.ff gps       :  W.akeup GPS\n"
         "I.nit GPS      :  X. execute cycle : R.TC set from GPS\n" );
 cprintf("P.ort Wing     :  S.tbd Wing     : B.aud Rate \n");
 
 return;
}  //**************************************************************************

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