/******************************************************************************\
**	setup.c				general functions for the Main Spray PCB, v1.0
**	
*****************************************************************************
**	
**	Oct2008, created
**
*****************************************************************************
**	
**	
\******************************************************************************/
#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    "gpio_mux.h"
#include    "pitch_roll.h"
#include    "setup.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;



//*****************************************************************************
// functions specific to Spray 2008 CF2 controller
//*****************************************************************************

void init_pins (  void  ) //***************************************************
{ // initialize the pins to the desired states
	uchar		mirrorpins[] = { 1,  39,  0 };

	PIOSet   ( RES_WDOG ); // set the reset line HIGH to the wdog msp430
	PIOMirrorList(mirrorpins);
	// configure SPI with levels = low = off for Spray sense
	PIOClear ( 15 ); // pin15 = PCS2
	PIOClear ( 17 ); // pin17 = PCS3
	PIOClear ( 19 ); // pin19 = PCS1
	PIOClear ( 21 ); // pin21 = PCS0
	
	PIOClear ( 16 ); // pin16 = SCK SPI
	PIOClear ( 18 ); // pin18 = MOSI SPI
	PIORead  ( 20 ); // pin20 = MISO SPI
	
	// spare TPU: unassigned
	PIOClear ( TPU9  );
	PIOClear ( TPU10 );
	PIOClear ( TPU11 );
	PIOClear ( TPU12 );
	PIOClear ( TPU13 );
	
	// TPU serial Receive channels
	PIORead  ( TPUPinFromChan( FROM_SAT  ) );
	PIORead  ( TPUPinFromChan( FROM_MUX1 ) );
	PIORead  ( TPUPinFromChan( FROM_MUX2 ) );
	PIORead  ( TPUPinFromChan( FROM_SBE  ) );

	// TPU serial Transmit channels 
	PIOSet   ( TPUPinFromChan( TO_SAT  ) );
	PIOSet   ( TPUPinFromChan( TO_MUX1 ) );
	PIOSet   ( TPUPinFromChan( TO_MUX2 ) );
	PIOSet   ( TPUPinFromChan( TO_SBE  ) );
   
   backup_v7(); // set alternative voltage regulator
   
  return;
} //***************************************************************************



void backup_v7 ( void ) //*****************************************************
{ // presently just a shell
    PIOClear ( V7_BKUP ); //this leaves OFF
    // ideally, we want to detect when circuitry is cycling in this regulator
    // and set an eeprom/ram value that says it is time just to use it

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


//*****************************************************************************
//**** low-power hibernate/wait functions *************************************
//*****************************************************************************

short  go_lowpower ( short irq_enable ) //*************************************
{ // will wait in low-power until something wakes it up:
  // irq_enable defines which interrupts to enable
  //      = PIT_ONLY = 0 = just use PIT, don't enable any others
  //      = PIT_IRQ2 = 1 enable IRQ2 as well
  //      = PIT_IRQ2_RXD = 2 = also enable RXD
  // this draws ~ 0.3 mA, and is only awakened by rs232 or by
  // IRQ2 = controlled by msp430  = CF2_ATTN line
  
  // returns IRQ_Flag = what woke it up, see below

  // set up to go to sleep ======================================  
    IRQ_Flag = 0; // reset flag: 0= no interrupts yet
    // it is set by the various ISR's
    // IRQ2_F   2 // awoken from IRQ2 = CF2_ATTN line from msp430
    // IRQ4_F   4 // awoken by char detected on rs232 Rcv
    // SPUR_F   8 // awoken by spurious interrupt
    // it will remain 0 if serviced by the PIT

	cdrain();   // wait for completion of last SCI_Tx
	coflush();	// discard any output garbage characters
	ciflush();	// discard any input garbage characters
	cdrain();   // wait for completion of last SCI_Tx

	QSMStop();	//  shut down the QSM 
	TPURun(0);  // turn off the TPU module
	DelayMilliSecs( 10 );  // give everything time to finish up???

	// PERSISTOR flash notice for SiliconDrive cards
	*(ushort *) 0xffffe00c = 0xF000; // force CF card into Card-1 active mode

	// next line is to disable irq's, which are re-enabled by LPStop
	// CPUWriteInterruptMask(IEV_DISABLE);	// disables all interrupts
	CPUWriteInterruptMask(5);
	if ( irq_enable > PIT_ONLY && !all.exc->wdog_error ) 
	{  // then include IRQ2 as well = CF2_ATTN line
	   // it is only included if wdog_err = FALSE
       PinBus(IRQ2);
    }
	if ( irq_enable == PIT_IRQ2_RXD ) // enable rxd irq = irq4rxd
	   PinBus(IRQ4RXD);  // configuring as bus fnx turns on ISR
	// but interrupt mask ignores irq4, irq2 until FullStop
	// therefore, no spurious interrupts!

	EIAForceOff(true);  // 	Force off the RS232 transmitter on the CF2
    // put in low-power state =========================================
	LPStopCSE( FullStop );			// we're here until PIT awakens
	// back here after awakened! ======================================
	// Delay10us(); // slight delay 
	
	// could be awake either from PIT, RxD, or IRQ2
	// turn off all ISR pins (do here as don't know which ISR was called)

	PinIO(IRQ4RXD); // turns off ISR
	PinIO(IRQ2);    // turns off ISR


	EIAForceOff(false);  // Force RS232 transmitters on
	QSMRun();	
	//DelayMilliSecs( 10 );
	TPURun(1); // turn back on	

	ciflush();	// discard any input garbage characters
	
	// next line re-enables the interrupts
	CPUWriteInterruptMask(IEV_MASK_NONE);
	
  return ( IRQ_Flag );
} //***************************************************************************

// Following is for periodic interrupt, based on PIT **************************
// The hibernate_task function will be called by the PIT.  
// If you need to have to PIT
// do anything periodically then stick it in here, otherwise, it doesn't need
// to do anything but exist!
void hibernate_task(void)
{ // periodic task can be added here if wanted.
} //***************************************************************************

short  hibernate( long *time, short dt, short irq_en) //***********************
{  // set alarm to time+dt, update time to time=time+dt
   // this is used for synchronous sampling every dt seconds,
   // therefore *time should always be now, or in the recent past
   // dt is limited to MAX_HIBERNATE = 13 s = max for using PIT51ms
   // irq_en sets whether to use just PIT, or PIT+CF2_Attn, or PIT+Attn+Rxd
   
   // returns -1= no time to hibernate
   // else returns IRQ_Flag = what woke it up:
    //  0 = PIT, (normal)
    // IRQ2_F   2 = awoken from IRQ2 = CF2_ATTN line from msp430
    // IRQ4_F   4 = awoken by char detected on rs232 Rcv
   
  long tnow, tnew;  
  short n, irq_flag;

  tnew = *time +  dt; // time to hibernate until
  tnow = RTCGetTime(0,0);  // gets present time
  dt = tnew - tnow;  // sequential hibernate
  
  if (dt<1) //===================================================
  { // then not long enough to wait
    *time = tnow; // update the time
    return ( -1 );
  } //===========================================================
  
  if (dt > MAX_HIBERNATE ) // out-of-bounds
      dt = MAX_HIBERNATE; // limit
  n = dt*1000/ 51;  // # of 51ms ticks to wait
  //cprintf("%d\n",n);
  if (n>255) n=255; // keep in bounds
  PITSet51msPeriod(n);	// will wake up in dt seconds
  
  irq_flag = go_lowpower( irq_en ); // go to low-power 
  
  PITSet51msPeriod(PITOff); // disable the timer
	
  *time = RTCGetTime(0,0); // reset to the new time
	
  return ( irq_flag );
} //***************************************************************************


short sleep_check(short tmax, short PIT_set, short do_wdog) //*****************
{ // look for answer up to tmax seconds 
  // allows a wakeup dependent upon PIT_set (PIT_ONLY, PIT_IRQ2, PIT_IRQ2_RXD),
  // will service the watchdog if do_wdog = TRUE\
  // set tmax = STAY_LOWPWR, then will do an infinite loop
  
  // returns AWAKE_RXD if has been awakened by rs232
  
  short ch =   0;
  short dt =  10;  // wake up every 10 s
  short ms = 200;  // = milliseconds to wait for ans on keyboard wakeup
  short   irq_flag;    
  long tnow, tstop;

  tnow  = RTCGetTime(0,0); // present time
  tstop = tnow + tmax; // time to stop
  
  cprintf("will wake up every %d s and display '.', ", dt );
  if (tmax == STAY_LOWPWR ) //----------------------------------
  { // then we want to do an infinite loop
    // waiting to be awoken
    tstop = tnow + 60; // set tstop to 60s ahead of tnow
    PIT_set = PIT_IRQ2_RXD; // force to awaken from rs232
    cprintf(" you must ");
  } //----------------------------------------------------------
  else cprintf(" for %d s \n", tmax );

  if ( PIT_set == PIT_IRQ2_RXD )
     cprintf("hit '.' to exit \n");
  cdrain();   // wait for completion of last SCI_Tx
  // cdrain seems necessary to keep the CF2 from hanging
  // on an intermittent basis.  Could use further bench-testing

  while ( ch != '.'  && tnow <tstop ) //===================================
  { // wait for '.' or for max time
    irq_flag = hibernate( &tnow, dt, PIT_set );
      SCITxPutChar('.');
    if ( irq_flag == IRQ4_F ) //---------------------------------
    {  // true if char was detected on the rs232
	  ciflush();	// clear input
      SCITxPutChar('>');
      ch = SCIRxGetCharWithTimeout(ms); //**wait ms for w or W
      // SCITxPutChar('-');
    } //---------------------------------------------------------
    else if ( irq_flag == IRQ2_F ) //----------------------------
    { // interrupt from msp430,
      cprintf("Wake-up from CF2_ATTN\n");
      service_wdog(); // service the CF2
    } //---------------------------------------------------------
    else if ( irq_flag == SPUR_F ) // ---------------------------
    { // spurious interrupt
      // special action here ???
      cprintf("Spurious int \n");
    } //---------------------------------------------------------
    
    if (do_wdog) service_wdog(); // service the watchdog
	cdrain();   // wait for completion of last SCI_Tx
	if (tmax == STAY_LOWPWR ) tstop = tnow + 60; // keep tstop>tnow
    do_wdog = 0;
  } //=====================================================================
  
  if ( ch=='.' ) // then awoken by rs232
       irq_flag = 1;
  else irq_flag = 0;
  
 return( irq_flag ); // 1 means it was awakened correctly
} //***************************************************************************

short check_tmo ( char *c , short tmo ) //************************************
{ // tmo =#s since last user input,
  //*c = pointer to last char read in (=-1 if no char was inputted over the last s)
  // returns the new tmo
  //  and also changes *ch to upper case if needed
  char ch;
  ch = *c;
  
	    if ( ch < 0 )
	       tmo++; //increment the timeout counter
	    else // could be valid
	    {
           ch = ch_l2u( ch ); // make upper-case
           tmo = 0; // reset
        }
        
        if (tmo>300) // has been >5 minutes with no user
        {
	       cprintf("No user input for 300s, going to sleep...\n");
	       sleep_check ( STAY_LOWPWR, PIT_IRQ2_RXD, FALSE ); // go to low power
	       // we're here when we're awake
	       ch = 'U'; // update the menu
	       tmo = 0; // reset
	    }
  *c = ch; // update the value before returning
  
  return ( tmo );
} //***************************************************************************

void  Reset_CF2 ( void ) //****************************************************
{ // try to shut down everything in an orderly manner before resetting
  // this is called after we are done with surface comms

  flash_close (); // close the flash file
  BIOSReset(); // causes the reset
  
  return;
} //****************************************************************************


void  init_all ( void ) //*****************************************************
{ // initialize the params in the all struct to default settings
  // this is done regardless of op_mode
  float tau;
  
	all.necho      = 0; // no echo bytes to send back
	all.roll_stat  = 0; // roll status
	all.exc->wdog_error = 0; // NO error with wdog circuit
	
	all.exc->abort      = 0; // flag to go to the abort code
	all.exc->reset      = 0; // flag to reset

	all.gps->ovalid = FALSE; // assume no valid old GPS
	all.gps->valid  = FALSE; // assume no valid GPS
    
    all.flt->pmin  = PR_MIN;         // set to default surface cnts
    tau            = 2*DHDR*T_ACT;
    all.flt->tau   = tau;        //=time constant for heading error
    all.flt->etau  = ETAU_M*tau; //=time constant for integrated error esum MAY07 CHANGE
    all.flt->esum  = 0; // integrated error
    all.flt->ediv  = all.flt->etau; // feb11, keep copy here; gets modified by shore cmds
    all.flt->em    = 0; // feb11 init to 0
    all.flt->rm    = 0; // feb11 init to 0
    
    all.sbd->send_waypts = FALSE; // don't send the waypt file
    all.sbd->send_param  = FALSE; // don't send the parameter list
    
    all.sbd->save_waypt  = FALSE; // don't save the waypt file
    all.sbd->save_route  = FALSE; // don't save the parameter list
    all.sbd->calc_route  = FALSE; // don't calculate a new route
    
    all.ubx->index = 0;
    all.ubx->valid[0] = 0;
    all.ubx->valid[1] = 0;
    
    init_accum_err( );
    init_serr( );
    
    all.exc->gps_cold = 0; //dec11, init to NOT do cold-reset of GPS
    
    return;
} //***************************************************************************

void  display_dbg ( void ) //**************************************************
{ // dec14: display some of all parameters
  // used to see what changes before/after reset
  
  printf("==================================================\n");
  printf("ipp, klifo, necho = %d, %d, %d \n",all.ppb->ipp, all.klifo, all.necho);
  printf("exc_stat, roll_stat, ncyc = %d %d %d \n", all.exc->exc_stat, all.roll_stat, all.ncyc);
  printf("exc->wdog_error, n_reset = %d %d \n", all.exc->wdog_error, all.exc->n_reset);
  printf("==================================================\n");
      
    return;
} //***************************************************************************

void init_sensors ( void ) //**************************************************
{ // initialize the params on a normal entry (op_mode = 0 or -1)
  // these are NOT done if it is an abnormal
  // this way, if some sensors are on during an abnormal reset, they stay on.
  
  // the below flags control what is acquired in take_data()
    all.dat->sbe_pro_on = 0; // set OFF
    all.dat->take_adp   = 0; // set OFF
    all.adp->on_adp     = 0;  // set OFF
    all.ppb->ipp        = 0; // reset ping-pong buffer to empty
  
  return;
} //***************************************************************************

void  init_accum_err ( void ) //***********************************************
{ // initialize error accumulators : should only be done once
  // at the start of the mission
  short i;
  
  for (i=0;i<2;i++)
  {  
     // all.wing_err[i] = 0;  // can be used to decide that wing has failed
     all.wing_now[i] = STBD_WING; // initialize wing to use stbd
  }
  
  return;
} //***************************************************************************


void  init_serr ( void ) //****************************************************
{ // initialize the spi error accumulators : should be done every dive
  short i;
  
  for (i=0;i<5;i++)
  {  
     all.exc->serr[i] = 0; 
  }
  
  return;
} //***************************************************************************

void toggle_verbose ( void ) //************************************************
{ // toggle the global verbose setting
  // and display the new setting

    Verbose ^= 1;
    cprintf("Verbose mode is ");
    if (Verbose) cprintf("ON\n");
    else cprintf("OFF\n");
  
  return;
} //***************************************************************************


//*****************************************************************************
short  setup_tpu_uart_all ( void ) 
{ // jan13: used to set up the TPU_UART = TU arrays,
  // set up so it can be called multiple times if things get corrupted
  // ---returns ---
  //  error = 0 = OK,
  //  1 = error
  short i, err=0;
  TUPort *tup; // handle for TPU UART calls
  long taddr;
  
  // TUChParams    tuart_cfg;  // used to display the config info
  // init the local array to pre-compiler values
  struct tpu_uart tu_port[N_TU_PORTS] = {
	     FROM_SAT , TO_SAT , BAUD_SAT , 0,
	     FROM_MUX1, TO_MUX1, BAUD_MUX1, 0,
	     FROM_MUX2, TO_MUX2, BAUD_MUX2, 0, 
	     FROM_SBE,  TO_SBE,  BAUD_SBE,  0 }; // 4 TPU uart ports, defined as above


	for (i=0;i<N_TU_PORTS; i++) //iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiii
	{ // step through each TU port we want to assign
	  tup   = all.tu_port[i].tup; // present handle
	  taddr = (long) tup; // convert to long
	  if ( taddr >0 ) // also tested that TUClose is OK even if tup==0
	  { // already open, close it
	    cprintf("Closing UART %d addr=%lx \n", i, taddr );
	    TUClose( tup );
	  }
	  // now open again, so we know it has the correct settings
	  tup   = TUOpen( tu_port[i].rxch, tu_port[i].txch, tu_port[i].baud, 0);
	  taddr = (long) tup; // convert to long
       cprintf("    TUP: %2d    RX: %2u    TX: %2u    BAUD: %6lu ADDR: %lu\n",
			               i, tu_port[i].rxch, 
			tu_port[i].txch, tu_port[i].baud, taddr);
	  if ( tup == 0 )
	  { //ERROR!!
	    cprintf("TPU UART OPEN ERROR, port#=%d\n",i);
	    err = 1;
	  }
	  // save all of the info to the global
	  all.tu_port[i].tup   = tup; 
	  all.tu_port[i].rxch  = tu_port[i].rxch;
	  all.tu_port[i].txch  = tu_port[i].txch;
	  all.tu_port[i].baud  = tu_port[i].baud;
	  
	  /*  used for displaying settings
	  TUGetCurrentParams( tu_port[i].tup, &tuart_cfg ); // get the config
	  cprintf("%ld %d%c %d pr=%d %d,  size =%d %d %d\n",
	    tuart_cfg.baud,
	    tuart_cfg.bits,
	    tuart_cfg.parity,
	    tuart_cfg.autobaud,  // not implemented
	    tuart_cfg.rxpri,
	    tuart_cfg.txpri,
	    tuart_cfg.rxqsz,
	    tuart_cfg.txqsz,
	    tuart_cfg.tpfbsz ); */
	} //iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiii
  
  return ( err );
} //***************************************************************************

//*****************************************************************************
short  setup_one_tpu_uart ( short i ) 
{ // jan13: used to set up the TPU_UART for channel i
  // set up so it can be called multiple times if things get corrupted
  // ---returns ---
  //  error = 0 = OK,
  //  1 = error
  short  err=0;
  TUPort *tup; // handle for TPU UART calls
  long taddr;
  
  // init the local to pre-compiler values
  struct tpu_uart tu_port[N_TU_PORTS] = {
	     FROM_SAT , TO_SAT , BAUD_SAT , 0,
	     FROM_MUX1, TO_MUX1, BAUD_MUX1, 0,
	     FROM_MUX2, TO_MUX2, BAUD_MUX2, 0, 
	     FROM_SBE,  TO_SBE,  BAUD_SBE,  0 }; // 4 TPU uart ports, defined as above

  if ( i<0 || i >= N_TU_PORTS )
  { // NOT LEGAL PORT: declared in setup.h
    cprintf("Illegal tu_port chan=%d\n", i );
    err = 1;
  }
  else //--- else OK to proceed -----------------------------------
  {
	  tup = all.tu_port[i].tup; // present handle
	  taddr = (long) tup; // convert to long
	  if ( taddr >0 )
	  { // already open, close it
	    cprintf("Closing UART %d addr=%lx \n", i, taddr );
	    TUClose( tup );
	  }
	  // now open again, so we know it has the correct settings
	  tup   = TUOpen( tu_port[i].rxch, tu_port[i].txch, tu_port[i].baud, 0);
	  taddr = (long) tup; // convert to long
       cprintf("    TUP: %lX    RX: %2u    TX: %2u    BAUD: %6lu ADDR: %lu\n",
			tu_port[i].tup, tu_port[i].rxch, 
			tu_port[i].txch, tu_port[i].baud, taddr);
	  if ( tup == 0 )
	  { //ERROR!!
	    cprintf("TPU UART OPEN ERROR, port#=%d\n",i);
	    err = 1;
	  }
	  // save all of the info to the global
	  all.tu_port[i].tup   = tup; 
	  all.tu_port[i].rxch  = tu_port[i].rxch;
	  all.tu_port[i].txch  = tu_port[i].txch;
	  all.tu_port[i].baud  = tu_port[i].baud;
	  
  } //--------------------------------------------------------------  
  
  return ( err );
} //***************************************************************************

//*****************************************************************************
//*****************************************************************************
// functions to mimic TT8 calls
//*****************************************************************************


void SerInFlush  ( void ) //***************************************************
{ // flush the serial input buffer
  
   while ( SCIRxQueuedCount() )
       SCIRxFlush(); // delete any old stuff 
  
   return;
} //***************************************************************************

void DelayMilliSecs  ( long ms ) //********************************************
{ // wait for the desired millisecs
  long i;
  
  for (i=0;i<ms;i++)
    Delay1ms();
  
   return;
} //***************************************************************************

//***send str to TPU uart port ************************************************
void tput (TUPort    *tup, char * str)
{ //**** assumes no extra bytes are needed
   char *cs = str;  

   TUTxFlush( tup); // flush the transmit buffer
   
   while (*cs)  // will end on end-of-string = 0
     TUTxPutByte( tup, *cs++, 0); //0=don't wait for reply char
     
   
   return;
}  //**************************************************************************



//***send str to TPU uart port ************************************************
void tputs(TUPort  *tup, char * str)
{ //**** assumes str needs CR LF tacked on at the end 
   char *cs = str;  

   TUTxFlush( tup); // flush the transmit buffer
   
   while (*cs)  // will end on end-of-string = 0
     TUTxPutByte( tup, *cs++, 0); //0=don't wait for reply char
     
   TUTxPutByte( tup, '\r', 0); // \r = CR = 13
   TUTxPutByte( tup, '\n', 0); // \n = LF = 10
   
   return;
}  //**************************************************************************


//*** get string from TPU uart port *******************************************
//*** ends when eol or n characters are read in
//*** returns # of characters actually read 
short  tgets ( TUPort  *tup,  char *str,  short n, short tmo, 
                   short eol, short *found)
{ // get string from TPU Port *tup
  // n characters are read in or when eol is received
  // tmo = #sec to wait for 1st char
  // eol = end-of-line character, could be CR='\r', LF='\n', or new line prompt
  // eol = NO_EOL = -2 will result in no EOL being used,
  //       as c=-1 only if times out
  
  // returns # of bytes actually read
  // the string is in *str
  // found = TRUE if eol was found, else FALSE if timed-out
  
   char c=0, *cs = str; 	//* ch points to str buffer
   short i=0;  	//* i=# characters read in so far
   short ms, ok=1, reply;
   
   if (tmo>30) tmo=30; // limit time-out
   ms = tmo*1000; // #ms for the first char
   //if (Verbose) cprintf("tgets wait = %d ms\n",ms);

   while ( ok ) //=====================================================
   { //keep reading while ok

	   reply = TURxGetByteWithTimeout( tup, ms ); //get a byte
	   // cprintf("%02x ",reply); //value>0 unless not rcv'd, then=-1
	   ok = ( reply>=0  && reply !=eol ); // good char, store
	   if (ok) //------------------------------------------------
	   {  
	      c = reply;  // store as char
	      *cs++ = c;  // store the byte
	      i++;        // increment the counter
	      ms = 50;    // for rest of char, use 50ms timing
	      ok = (i<n); // make sure still room for next
	   } //------------------------------------------------------

   } 
   //==================================================================

   
   *cs = '\0';  //* tack on the end of string marker
   *found =  reply==eol; // true if eol char was the last read in.
   //if (Verbose) cprintf("tgets #bytes=%d, found=%d\n", i, *found);
   
   return (  i );
} //***************************************************************************
//*** get string from TPU uart port *******************************************
//*** ends when eol found, or n characters are read in
//*** returns # of characters actually read 
//*** jul17, differs from tgets by:
//***  tmo=max time to wait TOTAL (versus tgets waits tmo for the first char).
//***  this allows for uneven character streaming from the TPU port.
short  tgetsTmax  ( TUPort  *tup,  char *str,  short n, short tmo, 
                    short eol, short *found)
{ // get string from TPU Port *tup
  // n characters are read in, or until eol is received
  // tmo = #sec to wait for ALL char
  // eol = end-of-line character, could be CR='\r', LF='\n', or new line prompt
  // eol = NO_EOL = -2 will result in no EOL being used,
  
  // returns # of bytes actually read
  // the string is in *str
  // found = TRUE if eol was found, else FALSE if timed-out
  
   char  c=0, *cs = str; 	//* ch points to str buffer
   short i=0;  	//* i=# characters read in so far
   short ms, ok, reply, nms, done, k;
   
   if (tmo>30) tmo=30; // limit time-out to 30s
   ms  = 50; // #ms to wait between char
   nms = tmo*(1000/50); //max #loops to wait

   for (k=0; k<nms; k++) //============================================
   { //read up to max #cycles

	   reply = TURxGetByteWithTimeout( tup, ms ); //get a byte
	   ok = ( reply>=0  && reply !=eol ); // good char, store
	   if (ok) //------------------------------------------------
	   {  
	      c = reply;  // store as char
	      *cs++ = c;  // store the byte
	      i++;        // increment the counter
	   } //------------------------------------------------------
	   done = ( i>=n || reply==eol );
	   if (done) break; // no more room, or found the eol
   } 
   //==================================================================
   
   *cs = '\0';  //* tack on the end of string marker
   *found =  reply==eol; // true if eol char was the last read in.
   
   return ( i );
} //***************************************************************************



//*****************************************************************************
//*****************************************************************************
// generic functions
//*****************************************************************************

short YesNo  ( void ) //*** return TRUE if answer is 'y' or 'Y'****************
{ char ch;
  short yes;
  
  ch = SCIRxGetChar ( );
  yes = ( ( ch =='Y') || ( ch == 'y' ) );
  return(yes);
  
} //***************************************************************************

short hex2dec( char c ) //*****************************************************
{ // convert hex '0'...'9', 'A'...'F' to decimal value
  // returns the decimal value = 0..15 of the hex character
  short n;
  
  n = -1; // initialize to illegal value
  if ( c>='0' && c <='9' )  
  {   n = c - '0'; } // 0..9
  else if ( c>='a'  && c<='f' ) 
  {   n = c - 'a' + 10; }
  else if ( c>='A'  && c<='F' ) 
  {   n = c - 'A' + 10; }
  
  return (n);
} //***************************************************************************

//*****************************************************************************
short  fget_line(FILE *fp, char *line, short n) //**************************
{ // get line from input file
  // and drop the LF at the end ('0x0a')
  // returns #bytes read in
  // returns -1 if no line (=EOF)

	line[0]=0; // set to NULL length
	fgets( line,n,fp ); // reads up to a Newline = '\n' = LF = 0x0a
	n = strlen(line);  // n=#actual char read in
	if ( n<1) return(-1);  // did not read in any line
	
	n--; // point to the last char

	while ( ( (line[n]<' ') || (line[n]>'~') ) && (n>-1) ) 
	{   // throw away garbage at the end
		line[n--] = 0; // set =NULL & decrement #bytes
	}
    n = strlen(line);

	return(n);
} //***************************************************************************


short get_num(short *x) //*****************************************************
{ // get a number from stdin
  // return 1 if good, 0 if bad
  // stores number in *x
  
  char line[80];
  short n, status=0;
  
  SerInFlush();

  gets(line);
  n = sscanf(line,"%d",x);
  if (n==1)
  { cprintf(" input value = %d\n",*x);
    status = 1;
  }
  else
  { cprintf(" Bad value\n");
  }
  return(status);
} //***************************************************************************

short get_substr( char *str, char *out, short imax) //*************************
{ // read in from the str, passing up blank spaces,
  // and return the first sub-str to out[]
  // returns # of char read from str,
  // so subsequent calls can parse the line
  // returns 0 if NO substring was found
  
  short i=0, k=0, n, valid;
  
  n = strlen(str);
  out[0] = 0; // set to NULL str
  
  do //================================================================
  {  // look for a valid char
     valid = ( str[i]>' ' ) && ( str[i] < 127 );
     if (!valid) i++; // if no good, look for the next
  }  while ( i<n && !valid ); //=======================================
  
 if (!valid) return( 0 ); // nothing to parse!
  
  while ( valid && i<n && k<imax) //===================================
  {  // then found a valid char, keep reading
     out[k++] = str[i++]; 
     if (i<n) valid = ( str[i]>' ' ) && ( str[i] < 127 );
  } //=================================================================
  // i=next blank char into str
  out[k] = 0; // append the NULL char

  return( i );
} //***************************************************************************

short read_shorts( char *str, short imax, short *x ) //************************
{ // read up to imax values and store into x
  // returns # of values actually read
  
  short i=0;  // number actually read in
  short k=0, n;
  char  s[40]; // sub-string of answer
  
  comma2blank(str); //--convert commas to white spaces
  do //======================================================
  { // read in until not valid
    n = get_substr( &str[k], s, 40);  //read in a substring
    //cprintf("%d  %s___%s\n",n, &str[k], s );
    
    if (n>0) //----------------------------------------
    { // then we did read in a substr
      k += n; // next entry into str to start searching
      n = strlen(s); // substr length
      if (n>0) // verifies it is a valid substr
      { //nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn
         n = sscanf(s,"%d",&x[i]); // read in the value
         // cprintf("__%d %s %d\n",i,s,x[i] );
         if (n==1) i++; //yes, read in OK
      } //nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn
    } //-----------------------------------------------
    // n=0 if no string, or illegal number
    
  }  while (n>0 && i<imax ); //==============================
  
  return( i );
} //***************************************************************************

short read_hexes( char *str, short imax, short *x ) // ************************
{ // read up to imax hex values and store into x
  // returns # of values actually read
  
  short i=0;  // number actually read in
  short k=0, n;
  char  s[40]; // sub-string of answer
  
  
  comma2blank(str); //--convert commas to white spaces
  do //======================================================
  { // read in until not valid
    n = get_substr( &str[k], s, 40);  //read in a substring
    //cprintf("%d  %s___%s\n",n, &str[k], s );
    
    if (n>0) //----------------------------------------
    { // then we did read in a substr
      k += n; // next entry into str to start searching
      n = strlen(s); // substr length
      if (n>0) // verifies it is a valid substr
      { //nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn
         n = sscanf(s,"%x",&x[i]); // read in the value
         // cprintf("__%d %s %d\n",i,s,x[i] );
         if (n==1) i++; //yes, read in OK
      } //nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn
    } //-----------------------------------------------
    // n=0 if no string, or illegal number
    
  }  while (n>0 && i<imax ); //==============================
  
  return( i );
} //***************************************************************************

void    comma2blank  (  char *s  ) // *****************************************
{ // prep a string before parsing on blanks
  short j, n;
  
  n = strlen(s);
  
  for (j=0;j<n;j++)  { if ( s[j]==',' ) s[j] = ' '; }
  
  return;
} // **************************************************************************

char ch_l2u ( char ch ) //*****************************************************
{ // convert lower case to upper
  
  if ( ch>='a' && ch<='z' )
       ch -= 32; // convert to upper
       
  return(ch);
} //***************************************************************************



short bswap( short v ) //******************************************************
{ // swap bytes
  short msb, lsb, val;
  msb = ( v & 0xff00 ) >> 8;
  lsb =   v & 0x00ff;
  val = (lsb << 8) + msb;  // swapped! () around lsb<<8 are needed
  //printf ("%4x %2x %2x %4x\n",v, msb, lsb, val);
  
  return ( val );
} //***************************************************************************


void swap2ch ( uchar *a,  uchar *b ) //****************************************
{ // xfr 2 bytes of a to b, reversing the order

  b[1] = a[0];
  b[0] = a[1];
    
  return;
} //***************************************************************************

void swap4ch ( uchar *a,  uchar *b ) //****************************************
{ // xfr 4 bytes of a to b, reversing the order

  // for (i=0;i<4;i++) b[n-1-i] = a[i];
  // probably less code to do the loop? 4 lines seems pretty minimal
  b[3] = a[0];
  b[2] = a[1];
  b[1] = a[2];
  b[0] = a[3];
    
  return;
} //***************************************************************************

short b2_store(short i, unsigned char ch[], short n)  //***********************
{ // store n to the next 2 bytes in ch array
  // this does not care if you're on a word boundary or not
  ch[i++] = ( n & 0xff00 )  >> 8; // msb, shifted right
  ch[i++] =   n & 0x00ff; // lsb
  
  return(i);
} //***************************************************************************

short b4_store(short i, unsigned char ch[], long  n)  //***********************
{ // store n to the next 4 bytes in ch array
  // this does not care if you're on a word boundary or not
  
  unsigned char *b; // pointer to a single char
  short k;
  
  b = ( unsigned char *) &n; // point to the first byte of n
  
  for ( k=0; k<4; k++ )
     ch[i++] = *b++; // store the 4 bytes
  
  return(i);
} //***************************************************************************

//*****************************************************************************
//*****************************************************************************
//**************************************************************
// Oct06, code for atan, sin, cos functions
//**************************************************************
//**************************************************************

// atan_2 fnx equivalent ***************************************
#define N_ATAN 25 // # of points in the atan look-up table
#define R_ATAN_MAX 1.01 // max ratio for atan to create the table below
#define MX_ATAN 0.002 // multiplier used to create the table below

// OCT06 tests show the atan table, atan_find performs:
// 50% faster ( 1000 cycles in 6 s: 6 ms per call )
// 20% less space ( 950 bytes versus 1130 )
// accuracy is <0.02 degrees error

const short atan_tab[N_ATAN] = {
          0,   1205,  2406,  3598,  4778,  5941,  7085,  8207,  9303, 10372, 11411,
       12420, 13397, 14341, 15253, 16131, 16977, 17790, 18572, 19323, 20043, 20734,
       21397, 22033, 22643 };

float atan_find( float yt, float xt ) //************************
{  // use atan table to look up atan2 value
   // return the arctan [degrees], range = +/- 180 degrees
   
   float theta, r, dt, f;
   short s1=1, s2=1, s3=1, t2=0, t3=0, i;
   
   if ( yt< 0 ) // then in lower two quadrants
   { s1 = -1;  yt = -yt; } // set sign, force to upper two quadrants
   
   if ( xt< 0 ) // then in left quadrant
   { s2 = -1;  t2 = 180; xt = -xt; } // set sign/offset, force to right quad
   
   if ( yt > xt ) // then in the 45-90 degree octant
   { // force to the 0-45 deg octant
     s3 = -1; // set sign
     t3 = 90; // offset
     if (yt==0) r=0;
     else r = xt/yt; // ratio, always 0<=r <= 1
   } else // else we're in the 0-45 deg octant
   { if (xt==0) r=0;
     else r = yt/xt; } // ratio
   
   // now compute the index for the lookup table
   f = r/R_ATAN_MAX  *( N_ATAN - 1); // float value of index to table lookup
   i = ( short ) f; // truncates to the nearest integer
   // make sure i is legal for lookup entry
   if (i<0) i=0; 
   if (i>= (N_ATAN-1) ) i = N_ATAN-2; 
   
   // linear-interpolate
   dt = atan_tab[i+1] - atan_tab[i]; 
   f=  f - i; // fraction to interpolate beyond i
   theta = ( atan_tab[i] + f*dt )*MX_ATAN; // guess at theta, degrees
   
   // now get back into the correct octant & quadrant
   if (s3<0 ) theta = t3 - theta; // right octant, range=0-90
   if (s2<0 ) theta = t2 - theta; // range = 0-180
   if (s1<0 ) theta = -theta;  // range = -180..180
      
   return(theta);
} //************************************************************


//**************************************************************
//sine function follows ****************************************
//**************************************************************
//sin lookup takes 3.3 ms per call, and requires 1kB
//sin() fnx takes 6.6 ms per call, and requires 11kB for all of
//the required libraries.
//accuracy: error is < 6e-5 for any angle

#define N_SIN 91    // # points in the table
#define SIN_MAX 91  // max degrees in table below
#define MX_SIN 0.00004 // multiplier used to create the table below

const short sin_tab[N_SIN] = { //---------------------------------
         0,    436,    872,   1308,   1744,   2179,   2613,   3047, 
      3479,   3911,   4341,   4770,   5198,   5624,   6048,   6470, 
      6891,   7309,   7725,   8139,   8551,   8959,   9365,   9768, 
     10168,  10565,  10959,  11350,  11737,  12120,  12500,  12876, 
     13248,  13616,  13980,  14339,  14695,  15045,  15392,  15733, 
     16070,  16401,  16728,  17050,  17366,  17678,  17983,  18284, 
     18579,  18868,  19151,  19429,  19700,  19966,  20225,  20479, 
     20726,  20967,  21201,  21429,  21651,  21865,  22074,  22275, 
     22470,  22658,  22839,  23013,  23180,  23340,  23492,  23638, 
     23776,  23908,  24032,  24148,  24257,  24359,  24454,  24541, 
     24620,  24692,  24757,  24814,  24863,  24905,  24939,  24966, 
     24985,  24996,  25000 
     }; //---------------------------------------------------------
     
float  sin_find( float xt ) //**********************************
{  // input xt = degrees, +/- 180
   // return the sin value
   
   float s, dt, f;
   short s1=1, i;
   
   // get value between +/- 360 degrees
   if ( xt>360 )
   { //  get back to legal value
     i= ( short ) ( xt/ 360);
     xt = xt - i*360;
   } // is now < 360
   if ( xt< -360 )
   { //  get back to legal value
     i= ( short ) ( -xt/ 360);
     xt = xt + i*360;
   } // is now >-360
   
   if (xt> 180) xt = xt - 360; // limit to +/-180
   if (xt<-180) xt = xt + 360; 

   if ( xt< 0 ) // then in lower two quadrants
   { s1 = -1;  xt = -xt; } // set sign, force to upper two quadrants
   
   // now make sure   
   if ( xt > 90 ) // then in left quadrant
   {   xt = 180 - xt;  } // set sign/offset, force to right quad
      
   // now compute the index for the lookup table
   // hardwired for a 91-datum table, 1 entry per degree
   // f = xt/SIN_MAX * ( N_SIN - 1); // float value of index to table lookup
   i = ( short ) xt; // truncates to the nearest integer
   // make sure i is legal for lookup entry
   if (i<0) i=0; 
   if (i>= (N_SIN-1) ) i = N_SIN-2; 
   
   // linear-interpolate
   dt = sin_tab[i+1] - sin_tab[i]; 
   f=  xt - i; // fraction to interpolate beyond i
   s = ( sin_tab[i] + f*dt )*MX_SIN; // guess at sin(xt)
   
   // now get back into the correct octant & quadrant
   if (s1<0 ) s = -s;  // range = -180..180
      
   return(s);
} //************************************************************

float cos_find ( float xt ) //**********************************
{ // input xt = degrees
  // returns cos(xt)
  float ct;
    ct = sin_find( xt - 90 );
    ct = -ct;
  return(ct);
} // ***********************************************************


float tan_find ( float xt ) //**********************************
{ // compute the tangent, based on the lookup tables
  float ct, st;
  
  ct = cos_find( xt );
  if ( ct == 0 ) return( 999 );  // avoid divide by zero
  st = sin_find( xt );
  ct = st / ct; // = tangent
  
  return(ct);
} //************************************************************


//**************************************************************
// arcsine fnx *************************************************
//**************************************************************
//*** oct06 tests show the following code is:
//size =894+50 bytes code+table
//asin() size = 3200 bytes
// 2.2k reduction in code size
//speed:  runs >2x faster on average, taking 5 ms/call
//accuracy: biggest error = 0.033 degrees for 40-50 deg range

#define N_ASIN 16        // # of points in the asin look-up table
#define R_ASIN_MAX 0.80  // max ratio for asin to create the table below
#define MX_ASIN 0.005   // multiplier used to create the table below

const short asin_tab[N_ATAN] = {
         0,    611,   1225,   1841,   2464,   3093,   3733,   4384, 
      5051,   5737,   6446,   7184,   7958,   8779,   9660,  10626 };

float  asin_find ( float yt ) //********************************
{ // input yt = -1..1
  // return degrees: if out-of-bounds, returns +/-90 degrees
   float theta, dt, f;
   short s1=1, t2=0,  i;
   
   if ( yt< 0 ) // then in lower half
   { s1 = -1;  yt = -yt; } // set sign, force to upper half

   if ( yt>=1 ) //at or beyond a legal value
      { theta = 90*s1;   return(theta);   } 
      
   // lookup table is only for yt = sin(t) = 0..0.8.
   // if  y(t)>0.707, compute using yt = cos(t) to find t, return 90-t
   if ( yt>0.707 )  
   {  // then we're past 45 degrees
      yt = sqrt( 1 - yt*yt ); // = cos(t)
      t2 = 1; // use t2 as flag that we're getting cos(t)
   }
   
   // now compute the index for the lookup table
   f = yt/R_ASIN_MAX  *( N_ASIN -1); // float value of index to table lookup
   i = ( short ) f; // truncates to the nearest integer
   // make sure i is legal for lookup entry
   if (i<0) i=0; 
   if (i>= (N_ASIN-1) ) i = N_ASIN-2; 
   
   // linear-interpolate
   dt = asin_tab[i+1] - asin_tab[i]; 
   f=  f - i; // fraction to interpolate beyond i
   theta = ( asin_tab[i] + f*dt )*MX_ASIN; // guess at theta, degrees
   
   // correct if theta = cos(yt) 
   if (t2>0 ) theta = 90 - theta; // theta = sin(yt)
   if (s1<0 ) theta = -theta;  // range = -90..90
   
  return(theta);
} //************************************************************

//***END TRIG FNX's ***********************************************************
//*****************************************************************************
//*****************************************************************************



//  added functions to limit heading to 0..360 deg range
short ilimit360(short x) //****************************************************
{ // make sure heading x is within 0..360 degree limit
  if (x>360) x -= 360;
  if (x<0)	 x += 360;
  return(x);
} //***************************************************************************

float limit360(float x) //*****************************************************
{ // make sure heading x is within 0..360 degree limit
  if (x>360) x -= 360;
  if (x<0)	 x += 360;
  return(x);
} //***************************************************************************

short ilimit180(short x) //****************************************************
{ // make sure heading diff stays within +/-180
  if (x<-180) x +=360;
  if (x> 180) x -=360;
  return(x);
} //***************************************************************************

float limit180(float x) //*****************************************************
{ // make sure heading diff stays within +/-180
  if (x<-180) x +=360;
  if (x> 180) x -=360;
  return(x);
} //***************************************************************************

float flt_abs(float x) //******************************************************
{ // return absolute value of x, floating-point type
   if (x<0) x=-x;
   return(x);
} //***************************************************************************



