/******************************************************************************
//	lifo.c			handles the LIFO (Last-In, First-Out) functions	
//	
//*****************************************************************************
//	
//	Jan2009, created
//
//==== Future Expansion =======================================================
//		last update: Jan2009
// oct13 added change to GPS packet
//
//	1. time-out in menu, to quit back to base menu
//	2. menu for bench-testing.
//	3. verify I can use the PID for profile #
//
******************************************************************************/
#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    "nav.h"
#include    "sbd.h"
#include    "setup.h"
#include    "dat_fnx.h"
#include    "sbe.h"
#include    "pump.h"
#include    "lifo.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_lifo( void );


// LAST-IN, FIRST-OUT = LIFO FUNCTIONS : SEE also lifo_notes.c
//****** MAY 2003 *******************************************
// lifo status of packet:
//	3 = queued, but not sent
//  2 = sent, but no ack, or ack=retry sending
//  1 = sent, ack= error in receive, but move on OR already tried 3x
//  0 = sent with no errors

#define MINSTATVAL 1  // send packet if status>MINSTATVAL

short alloc_lifo(struct lifo_param *lifo) //***********************************
{ // allocate memory for satellite LIFO buffer area
  // return TRUE if an error, FALSE if ok
  unsigned char *buf;
  long bufsize;
  
  // LIFO_NBYTE =sector size=2048 bytes, see setup.h
  // NSECTORS is max # of packets to store in LIFO buffer

  bufsize = ((long)LIFO_NBYTE )*(NSECTORS); //=max#bytes * # entries
  buf = (uchar *) malloc(bufsize  );
  if (buf == NULL ) // then allocation failed
  {
     cprintf("LIFO malloc of %ld bytes failed\n",bufsize);
     return(TRUE);
  }
  
  lifo->buf = buf; // save ptr
  printf("  LIFO ptr = %p \n",lifo->buf);
  
  return(FALSE);   
} //***************************************************************************

void  reset_lifo (struct lifo_param *lifo) //**********************************
{ // reset all of lifo  : any message in LIFO will be lost.
  short i;
  
  for (i=0; i<NSECTORS; i++ ) //=================================
  {  // clear LIFO buffer area
    lifo->nbytes[i] = 0;  // #bytes in message
    lifo->stat[i] = 0;   // if 0  means sent successfully
    lifo->dive[i] = 0;   // dive #
    lifo->ip[i] = 0;     // individual packet id #
    lifo->ntry[i] = 0;   // #transmissions attempted
  } //===========================================================

  return;
} //***************************************************************************


void  clear_lifo_stat (struct lifo_param *lifo) //*****************************
{ // If status>0 (has not been sent, or there's an error)
  // set status to MINSTATVAL = error, but do NOT send.
  // a value of MINSTATVAL keeps the message around as long
  // as possible before being over-written, thus allowing
  // a remote request to re-send a specific dive #.
  short i;
  
  for (i=0;i<NSECTORS;i++) //====================================
  { 
     if (lifo->stat[i] > 0)
         lifo->stat[i] = MINSTATVAL;   // do NOT send
  } //===========================================================

  return;
} //***************************************************************************


short check_lifo (struct lifo_param *lifo) //**********************************
{ // returns the count of all instances where lifo->stat >minstatval
  // = count of remaining packets that still should be transmitted
  short count, i;
  
	count=0; // zero accumulator
	for (i=0; i<NSECTORS; i++ ) 
		if ( lifo->stat[i]>MINSTATVAL ) count++;
		
  return(count);
} //***************************************************************************


short  oldest_lifo ( struct lifo_param *lifo ) //******************************
{ // finds the oldest packet (=smallest dive no.)
  // returns pointer to the next spot in LIFO to overwrite
  short next, statval, i, min_age, j;
  
  next = -1; // next location available : set to illegal value
  statval = 0;  // allowable status value to overwrite : 0=already sent
  min_age = 9999; // keeps track of minimum dive packet
  j = -1; // j points to sector of min_age
 
  while ( (next<0) && (statval<4) ) //=================================
  { // find the oldest packet, progressively selecting by status
  
    for ( i=0; i<NSECTORS; i++) //+++++++++++++++++++++++++++++++++
    { // step through all packets stored in buffer
       
       if (lifo->stat[i] == statval) //------------------------
       {  // then status is low enough to overwrite
          if ( min_age > lifo->dive[i] )
       	  { // then found one that's older
       		  min_age = lifo->dive[i]; // reset min_age
       		  j = i;  // and set j to this sector
       	  } // end if min_age
       	} //---------------------------------------------------
       	
    } //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    
    if (j>-1) next=j;  // we found a legal value
    statval++; // increment statval for the next loop, if needed
    
  } //=================================================================
  
  if (next<0) next = 0;  // make sure it's legal : this should never happen
  
  return(next);
} //***************************************************************************


short youngest_lifo ( struct lifo_param *lifo ) //*****************************
{ // returns pointer to the next packet in LIFO to transmit
  // returns -1 when there are no packets left that need to be sent
  // scan the lifo for the youngest packet =last-in, should be first-out
  
  short next, statval, i, max_age, j;
  next = -1; // next location available : set to illegal value
  statval = 3;  // 3 = packet has never been sent
  max_age = -1; // keeps track of maximum dive packet = youngest
  j = -1; // j points to sector of max_age
  
  // ONLY SEND packets w/statval > min
  // minstatval=1 means only send packets w/status=2 = request to resend
  // minstatval=2 means only send status=3 = never been sent
  
  while ( (next<0) && ( statval>MINSTATVAL ) ) //======================
  { // find the most recent packet, progressively selecting by status
  
    for ( i=0; i<NSECTORS; i++ ) //++++++++++++++++++++++++++++++++
    { // step through all packets stored in buffer
       
       if (lifo->stat[i] == statval) //------------------------
       {  // then has the right status
          
          if ( max_age < lifo->dive[i] )
       		{ // then found one that's more recent
       			max_age = lifo->dive[i]; // reset min_age
       			j = i;  // and set j to this sector
       		} // end if min_age
       		
       	} //---------------------------------------------------
       	
    } //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    
    if (j>-1) next=j;  // we found a legal value
    statval--; // decrement statval for the next loop, if needed
    
  } //=================================================================
  
  return(next);  //next =-1 if NO packets had status > minstatval
} //***************************************************************************


short  queue_lifo (struct lifo_param *lifo ) //********************************
{ // find the most-recent packet that needs to be sent
  // and queue it to the satellite output temporary buffer.
  // returns LIFO pointer that was queued.
  // returns -1 if NO packet was queued = none needed sending.
  short next;
  unsigned char *out;
  
  next = youngest_lifo(lifo); // find the next available spot
  if (next>-1) 
  { // then there is a packet that needs to be sent
    out = lifo->buf + (long) next* (long)LIFO_NBYTE; //=output location
    
  	format_out( out,'X',lifo->nbytes[next] ); // fill temp. buffer
  } // end if next>-1
  
  return(next);
} //***************************************************************************


void  retransmit_lifo ( short nprof ) //***************************************
{ // find all entries with profile #=nprof and set status=3 = re-transmit
  // NOTE that if there are more than one packet id for the same dive,
  // they all will be re-queued for transmit.
  short i;
  struct lifo_param *lifo;
  
  lifo = all.lifo; // point to the lifo struct
  
	for ( i=0; i<NSECTORS; i++) //=====================================
	{ // step through all entries
	
		if ( lifo->dive[i] == nprof )
		{ // then the dive # matches
		   lifo->stat[i] = 3; // reset status to NOT transmitted
		   lifo->ntry[i] = 0; // and NO attempts yet to transmit
		} // end if right dive #
		  
	} //===============================================================
	
  return;
} //***************************************************************************


short assign_lifo(struct lifo_param *lifo, short dive_no, short pid) //********
{ // find the oldest LIFO location to overwrite,
  // initialize the status arrays, setting packet length to 0
  // returns the lifo location 
  // USE TO INITIALIZE before append_lifo
  short next, n, sn;
  uchar  *out;
  
	next = oldest_lifo( lifo ); // = location to write to
	lifo->stat[next] = 3; // 3=packet is waiting to be sent
	lifo->dive[next] = dive_no;
	lifo->  ip[next] = pid; //=packet id
	lifo->ntry[next] = 0; // reset #transmission tries
  
  // Each packet starts off with
  // Spray s/n (2 bytes), dive number (2 bytes) & packet id:
  out = lifo->buf + (long) next* (long)LIFO_NBYTE; //=output location

  sn = iparam [ EA_serno ];
  n=0; // byte counter for #bytes written out
  n = b2_store( n, out, sn ); // store sn
  n = b2_store( n, out, dive_no ); // store dive#
  out[n++] = pid & 0xff; // store packet id
  
  lifo->nbytes[next] = n; // n = #bytes in the packet
		
  return(next);
} //***************************************************************************


void   append_lifo (  short n, uchar *dat ) //*********************************
{ // append n bytes of data and update byte counter for present lifo.
  // Will start a new message if this over-runs the lifo
  short i,j, nb, pid, iprof;
  unsigned char *out;  // points to output location
  struct lifo_param *lifo;
  
  lifo = all.lifo; // point to the lifo struct

	i = all.klifo; // points to the present output lifo buffer
	nb = lifo->nbytes[i]; // = #bytes presently in the packet
	
	//=================================================================
	if ( (nb+n)> (MAX_SBD_OUT-10) ) // then we have over-run
	{ // and we need to assign a new lifo buffer
      pid   = lifo->ip[i] +1; // increment the present packet id
      iprof = lifo->dive[i]; // use the present profile #
      all.klifo = assign_lifo( lifo, iprof, pid); //start the new packet
      i  = all.klifo; // i points to the new lifo buffer
      nb = lifo->nbytes[i]; // =#bytes in new buffer (=header bytes)
    } // end if we needed a new lifo buffer
	//=================================================================
	
    out = lifo->buf + (long) i* (long)LIFO_NBYTE +nb; //=output location
	for ( j=0; j<n; j++ )
		*out++ = *dat++;  // xfer the data
	lifo->nbytes[i] = nb + n; // adjust #bytes NOW in buffer
		
  return;
} //***************************************************************************


void new_msg_lifo ( short n, unsigned char *dat) //****************************
{ // mimics append_lifo, but forces it to create a new message.
  // This was written for creating a new message for the ADP, but
  // could be used for any need.
  short i,j, nb, pid, iprof;
  uchar *out;  // points to output location
  struct lifo_param *lifo;
  
  lifo = all.lifo; // point to the lifo struct

	i = all.klifo; // points to the present output lifo buffer
	{ // assign a new lifo buffer -----------------------------------------
      pid   = lifo->ip[i] +1; // increment the present packet id
      iprof = lifo->dive[i]; // use the present profile #
      all.klifo = assign_lifo( lifo,iprof,pid); //start the new packet
      i  = all.klifo; // i points to the new lifo buffer
      nb = lifo->nbytes[i]; // =#bytes in new buffer (=header bytes)
    } //-------------------------------------------------------------------
	
    out = lifo->buf + (long) i* (long)LIFO_NBYTE +nb; //=output location
	for (j=0;j<n;j++)
		*out++ = *dat++;  // transfer the data to the buffer
	lifo->nbytes[i] = nb + n; // adjust #bytes NOW in buffer
		
  return;
} //***************************************************************************


//*****************************************************************************
short put_lifo( short nbytes, short dive_no, short pid, uchar *dat )
{ // find the oldest LIFO location to overwrite,
  // and store nbytes of *dat there.
  // returns the lifo location
  short next;
  struct lifo_param *lifo;
  
  lifo = all.lifo; // point to the lifo struct
  
  next = assign_lifo( lifo, dive_no, pid);
  all.klifo = next; // store the lifo pointer
	// append the data
  append_lifo(   nbytes, dat);
		
  return(next);
} //***************************************************************************


void lifo_gps(  short iver, short taken )  //**********************************
{ //** store gps data into lifo area *******
  // iver = 0 = start mission, 
  // 1=start of dive, 2=end of dive, 3=abort mission
  // taken = 1 = gps attempt made,
  // taken = 0 = just filler for uniformity of messages
  uchar dat[256], sgn_stat, sgn_ver;
  short i,x,y,n;
  struct gps_param *gps;
  
  gps = all.gps;
  dat[0] = 0 + iver; //assign id = 0 for gps data + iver
  i = 3; // dat[1], dat[2] = #bytes in gps message

  x = 0;  // set to an invalid value
  if (gps->valid)  x = gps->lon_sign; //if valid set to +/-1
  if (!taken) 
  {  x=0; // data was not taken, set flag accordingly
     gps->time = 0; // 24JUL03 set time=0 as flag as FAKE
  }
  // oct13 mod to correctly handle +/-0 degrees latitude
  sgn_ver  = 0x10; // set MSN=1 =ver1 of lat/lon degrees sign
  // allows future expansion for other versions
  sgn_stat = 0; // LSN will be GPS sign/status
  if ( gps->lon_sign>0 ) sgn_stat |= 0x04; // set bit 2 ==1 if lon>0
  if ( gps->lat_sign>0 ) sgn_stat |= 0x02; // set bit 1 ==1 if lat>0
  if ( gps->valid>0    ) sgn_stat |= 0x01; // set bit 0 ==1 if valid
  sgn_stat += sgn_ver;
  //dat[i++] = x;  //OLD first byte = 0 if invalid, else sign of longitude
  dat[i++] = sgn_stat;  //oct13 new status byte
  
  dat[i++] = gps->lat_deg; //oct13, absolute value; range = 0..90
  //--end oct13 new code
	x= gps->lat_min; //truncate
	y= (gps->lat_min - x)*100;
  dat[i++] = x;
  dat[i++] = y;
  dat[i++] = gps->lon_deg;  //=range 0..180
	x= gps->lon_min; //truncate
	y= (gps->lon_min - x)*100;
  dat[i++] = x;
  dat[i++] = y;
  //jul03 added gps week + day + wing
  dat[i++] = all.roll_stat + gps->wing; //wing = 1=port, 0 = stbd
 cprintf("%d %d\n", all.roll_stat, gps->wing );
  // jun06, the wing byte includes the roll status: tells us if the wing was all way up.
  dat[i++] = (gps->gps_week & 0xff00) >> 8; // MSB
  dat[i++] = (gps->gps_week & 0x00ff); // LSB
  dat[i++] = (gps->gps_day  & 0x00ff); // day-of-week : 0..6 
  // back to old structure
  dat[i++] = gps->utc_hr;
  dat[i++] = gps->utc_min;
  dat[i++] = gps->time;  //'''=time, #sec/10 to get fix
  dat[i++] = gps->nsat;  //=#sat in view
  dat[i++] = gps->smin;  //=min sig. level
  dat[i++] = gps->savg;
  dat[i++] = gps->smax;
  x = gps->hdop/10; // 2008controller, gps->hdop = (hdop)*100; old was (hdop)*10
  if (x>127) x=127; // parsed as a signed value: keep below max
  dat[i++] = x;  // horiz. dilution of precision, should be ~50
  dat[i++] = ';'; // end-of-data char
  n = i; //=# of points to transmit : includes 0,1,2 bytes + ';'
  dat[1] = n >> 8; //=msb
  dat[2] = n & 0xff; //=lsb
  cprintf("lifo = %d GPS #bytes = %d\n",all.klifo, n);
  
  append_lifo( n,dat);
  if (iver==0 || iver==2) lifo_route( );  //  add route info
  // this gets added to the start-of-mission, plus each end-dive fix.

  return;
} //***************************************************************************


void  lifo_surf_drift( short valid )  //*************************************** 
{ //  store surface drift information
  //  valid = #'s should be good
  
  uchar dat[100];
  short i, n;
  struct ublox_param *ubx;
  
  ubx = all.ubx;
    
  dat[0] = SBD_SURF; //assign id = 0x08 = surface drift
  i = 3; // dat[1], dat[2] = #bytes in the message; set at end of fnx
  dat[i++] = valid;  // = if valid or not
  
  i = b2_store(i,dat, ubx->dt );
  i = b2_store(i,dat, ubx->dx );
  i = b2_store(i,dat, ubx->dy );
  
  
  dat[i++] = ';'; // end-of-data char
  n = i; //=# of bytes to transmit : includes 0,1,2 bytes + ';'
  i = b2_store(1,dat,n );
    
  append_lifo( n,dat); // append to present lifo
  flash_surf (); // store the ublox struct to flash

  return;
} //***************************************************************************

void  lifo_dive_mark( void )  //***********************************************
{ //  store surface#, profile#, time info
  
  uchar dat[100];
  short i, n;
      
  dat[0] = SBD_MARK; //assign id = 0x0f = dive marker
  i = 3; // dat[1], dat[2] = #bytes in the message; set at end of fnx
  
  i = b2_store(i,dat, iparam[ EA_surf_num ] ); // surfacing #
  i = b2_store(i,dat, all.ncyc ); // cycle #
  i = b2_store(i,dat, iparam[ EA_prof_num ] ); // profile #
  i = b4_store(i,dat, all.time->ti_cyc ); //start-profile time
  
  
  dat[i++] = ';'; // end-of-data char
  n = i; //=# of bytes to transmit : includes 0,1,2 bytes + ';'
  i = b2_store(1,dat,n );
  
  cprintf("lifo_dive_mark; #bytes=%d\n",n);
  for (i=0;i<n;i++) cprintf("%02x.",dat[i]);
  cprintf("\n");
    
  append_lifo( n,dat); // append to present lifo

  return;
} //***************************************************************************

void  lifo_route( void )  //*************************************************** 
{ //  store route data into lifo area and queue for transmit.
  uchar dat[256];
  short i, j, n ;
  struct route_param *route;
  
  route = all.nav->route;
  
  n= iparam [ IA_num_route ];  // = # of waypoints in the route
  if ( n>( MAX_WAYPTS-1)   ) 
       n = MAX_WAYPTS-1;  // limit size  to a legal value
  
  dat[0] = SBD_RTE; //assign id = 0xd1 = route info
  i = 3; // dat[1], dat[2] = #bytes in the route message; set at end of fnx
  dat[i++] = n;  // = # waypoints in the route
  dat[i++] = iparam[ IA_route_now ];   // = waypoint we're now heading to
  dat[i++] = iparam[ IA_route_end ];   // = action when we're at the end of route
  dat[i++] = iparam[ IA_route_dir ];   // = present direction thru the route
  
  // jul09 v3 changed from word to byte-store for IA_use_set
  // (fixes original error: now same format as old-style)
  dat[i++] = iparam[ IA_use_set   ]; //=0/1 = off/on
  i = b2_store(i,dat, iparam[ IA_cross     ] ); //crossing angle:0=off
  i = b2_store(i,dat, iparam[ IA_set_nprof ] );
  i = b2_store(i,dat, iparam[ IA_man_theta ] );
  i = b2_store(i,dat, iparam[ IA_man_nprof ] );
  i = b2_store(i,dat, iparam[ IA_steer_pt  ] );
  i = b2_store(i,dat, iparam[ IA_steer_nprof]);
  i = b2_store(i,dat, iparam[ IA_set_min   ] );
  i = b2_store(i,dat, iparam[ IA_set_max   ] );
  
  // now save the route list info ===============================
  for (j=1;j<=n; j++)
  { // step thru each waypoint in the route
    dat[i++] = route->rlist[j];   // waypoint #
    dat[i++] = route->detect[j];  // arrival-detection
    dat[i++] = route->circle[j];  // watch-circle radius [km]
    i = b2_store(i,dat,route->blist[j] ); // approach-angle
  } //===========================================================
  
  dat[i++] = ';'; // end-of-data char
  n = i; //=# of bytes to transmit : includes 0,1,2 bytes + ';'
  i = b2_store(1,dat,n );
  // *(short*) dat[1] = n; //=#bytes to transmit
  // dat[1] = n >> 8; //=msb
  // dat[2] = n & 0xff; //=lsb
  
  append_lifo( n,dat); // append to present lifo

  return;
} //***************************************************************************

void lifo_waypts( void )  //*************************************************** 
{ // store waypoint list into lifo area and queue for transmit
  uchar dat[256];
  short i,j,n, deg, f;
  struct s_waypt *waypt; 
  
  waypt = all.nav->waypt;
  
  for (j=0, n=0;j<MAX_WAYPTS;j++)
     if (waypt[j].valid==99) n++;  // count # of valid waypts
  
  dat[0] = SBD_WPT; //assign id = 0xd2 = waypoint list info
  i = 3; // dat[1], dat[2] = #bytes in the route message; set at end of fnx
  dat[i++] = n;  // = # valid waypoints in the list
  
  for (j=0;j<MAX_WAYPTS;j++) //============================================
  {
     if (waypt[j].valid==99) // this waypt is valid
     {  // store this data
        dat[i++] = j;  // waypoint #
        store_waypt(waypt[j].lat, &deg, &f);  // convert float to 2 shorts
        i = b2_store(i, dat, deg );
        i = b2_store(i, dat, f   );
        store_waypt(waypt[j].lon, &deg, &f);  // convert float to 2 shorts
        i = b2_store(i, dat, deg );
        i = b2_store(i, dat, f   );
      
     } // end if valid
     
  } //=====================================================================
  // The waypoint list is now stored
  
  dat[i++] = ';'; // end-of-data char
  n = i; //=# of bytes to transmit : includes 0,1,2 bytes + ';'
  i = b2_store(1,dat,n );
  // *(short*) dat[1] = n; //=#bytes to transmit
  // dat[1] = n >> 8; //=msb
  // dat[2] = n & 0xff; //=lsb
  
  append_lifo( n,dat); // append to present lifo= shore cmd list
  lifo_route( ); // append the route info as well
  all.sbd->send_waypts = FALSE; //set FALSE=we have now sent

  return;
} //***************************************************************************

void lifo_eng( void ) //*******************************************************
{ //** queue the engineering message ****
  uchar *b;
  short i, nb, dt, ti_pump,  max_z;
  struct eng_param *eng, edum;
  
  eng = all.eng;
  
  nb = sizeof(edum)-2; //=#bytes to send : removes the 2 dummy bytes

  // nb = all->eng->nbytes;  //nb =#bytes in eng. structure
  	// this DOES  include the  header, end bytes
  	// but NOT the very first and last bytes of the structure, which
  	// are dummy bytes to put everything else on even word boundaries
  #if  ADP==1 // removed dependence on ALT for this setting
    all.exc->exc_stat |= EXC_ADP;
  #endif
    eng->isu_type = SBD_ENG; // includes 0901 changes
  // 0xe0 = jul03..15jul04 operation
  // 0xe1 = JUL04 eng mod = includes pump time and vacuum
  // 0xe2 = Jun05 eng mod = includes amp stats + ISU send time
  // 0xe3 = Jan06 ADP altimeter word is packed differently: see bottom turn.
  // 0xe5 = Aug06 includes exception status word
  // 0xe6 = 0711, z_at_alt, drift, vent parameters
  // 0xe7 = 0901: only change is ncyc replaces old miss_id.

  eng->nbytes = nb;  // make sure that it is correct
  eng->end = ';' ; //=end of sensor data character
  eng->iprof =   iparam[ EA_prof_num ]; // 0902 save profile #
  // eng->miss_id = iparam[ IA_miss_id ]; // OLD
  eng->ncyc = all.ncyc;

  //  compute hydraulic pump times here:
  //  this used to be in the 'surface' code, which didn't get called
  //  for multi-dive/non-surfacing events
  dt = (short) ( all.hyd->total/10); // total time (s/10) that the pump has run
  // pre-jun06, dt was JUST the ascent time: change in bottom_turn has this now=total time
  // we want to remove the pump time down deep, so this represents just the ascent pump
  ti_pump = eng->ti_pump/10;  // deep pump time (s/10) to get to neutral buoyancy.
  dt = dt - ti_pump;  // ascent time  (s/10)
  eng->ti_pump = (ti_pump << 8 ) + dt;  // MSB = deep pump, LSB = ascent pump

  eng->n_badamp = all.hyd->nbad; //#times high current was detected.
  #if DO6==1 // oct14, then SBE63 DO
    eng->n_badamp = (0xff & all.exc->sbe_err); // kluge for catching errors
    all.exc->sbe_err = 0; // reset to 0; all set for next dive
  #endif
  eng->exc_stat = all.exc->exc_stat; // save the exception status flag word
  all.exc->exc_stat = 0; // clear it out for the next cycle!
  
  max_z = eng->zmax;  // jun07, save a copy of max depth
  
  b = (uchar*) all.eng +1; // point to isu_type in array
  append_lifo( nb,b);  
  flash_eng( );  //  write to flash as well.

  for (i=0;i<nb;i++) b[i] = 0; // zero out the engineering buffer
  
  eng->zmax = max_z; // jun07, restore the max depth
  // this is needed for auto-ranging in check_waypt

  // 0711 = add drift structure if actually drifted.
  if (iparam[ IA_drift_tm ]>0 ) 
  {  
     lifo_drift( ); // add drift
     flash_drift(); // also write to flash
  }
  
  return;
} //***************************************************************************

void  lifo_drift ( void ) //***************************************************
{  // add the drift structure to the LIFO message
  uchar b[256], *d;
  short i, j, n, nb;
  struct drift_param *dd, ddum;
  
  dd = all.drift;
  nb = sizeof(ddum); //=#bytes 
  d = (uchar*) dd; // point to the drift structure
  
  b[0] = SBD_DRIFT; // assign id= 0xd4 = drift structure
  i=3; // start storing at b[3]
  for (j=0;j<nb;j++)
      b[i++] = d[j]; // xfr the bytes starting at b[3]
      
  b[i++] = ';'; // add end-of-data char
  n = i; //=total # of bytes : includes 0,1,2 bytes + ';'
  i = b2_store(1,b,n ); // store #of bytes @ b[1],b[2]
  
  append_lifo( n, b);  
  
  return;
} //***************************************************************************

#if ZOOG ==1 //ZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZ
void   lifo_zooCam ( void ) //*************************************************
{  // add the zooglider structure to the LIFO message
  uchar b[256], *d;
  short i, j, n, nb;
  struct zcam_param *dd, ddum;
  
  dd = all.zcam;
  nb = sizeof(ddum); //=#bytes 
  d = (uchar*) dd; // point to the zooCam structure
  
  b[0] = SBD_ZC_PARAM; // assign id
  i=3; // start storing at b[3]
  b[i++] = 0; // sep16, adding another ID byte: this could be the packet version,
  // now all zooGlider SBD packets have an extra ID.
  // Continue with transferring the data.
  for (j=0;j<nb;j++)
      b[i++] = d[j]; // xfr the bytes 
      
  b[i++] = ';'; // add end-of-data char
  n = i; //=total # of bytes : includes 0,1,2 bytes + ';'
  i = b2_store(1,b,n ); // store #of bytes @ b[1],b[2]
  
  append_lifo( n, b);  
  
  return;
} //***************************************************************************
#endif //ZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZ

void lifo_surface(void) //*****************************************************
{  // July14 add extra surface info
  uchar b[64];
  short i,  n, x;
	long tmp;
	float p;
	char *sptr;
	
	sptr  = ( char *) CPUReadStackReg(); // stack register
	//sptr  = ( char *) GetStackPtr(); // stack register
    tmp   = (long) sptr;
    //tmp = 9999;
    x     = 0xffff & tmp; // low address
    printf("SR = %ld = %u\n", tmp, x);
  
  b[0] = 0x0e; // assign id= 0x0e = surface info
  i=3; // start storing at b[3]
  b[i++] = 0; //Version #
  
  i = b2_store(i,b, all.exc->exc_stat ); // latest exception status
  i = b2_store(i,b, x ); // stack register
  //---add battery counts
  tmp = battery(); //volts
  x   = ( short ) tmp;
  i   = b2_store(i,b, x ); // battery volts now
  //---add pressure counts
  p   = sbe_fp(); // pressure, floating
  x   = intPval(p);  // integer: 250 = nominal surface
  i   = b2_store(i,b, x ); // battery volts now
     
  b[i++] = ';'; // add end-of-data char
  n = i; //=total # of bytes : includes 0,1,2 bytes + ';'
  i = b2_store(1,b,n ); // store #of bytes @ b[1],b[2]
  
  append_lifo( n, b);  
  
  return;
} //***************************************************************************

void lifo_abort(void) //**************************************
{  // JULY14
  uchar b[64];
  short i,  n, x, oil;
  long  tmp;
	
  
  b[0] = 0x0d; // assign id= 0x0d = abort info
  i=3; // start storing at b[3]
  b[i++] = 0; //Version #
  
  //---surface battery, pressure, already added in lifo_surface
  
  //---add vacuum counts
  tmp = vacuum(); //vacuum: in-Hg*100
  x   = ( short ) tmp;
  i   = b2_store(i,b, x ); // vacuum now
  
  //get oil and pump time
  x   = all.hyd->total;  // total time that the pump has run
  
  //---add oil counts
  get_hyd_ad ( ); // this samples all, even if pump is off
                  // if the pump is off, which it is in this case,
                  // it will zero the timer accumulators.  that
                  // is why i read the total time above.  But, to 
                  // keep universal on the GS side, I store the oil
                  // first
  oil = all.hyd->oil; 
  i   = b2_store(i,b, oil ); // oil now
  
  //---total pump time
  i   = b2_store(i,b, x ); // store the value
     
  b[i++] = ';'; // add end-of-data char
  n = i; //=total # of bytes : includes 0,1,2 bytes + ';'
  i = b2_store(1,b,n ); // store #of bytes @ b[1],b[2]
  
  append_lifo( n, b);  
  
  return;
} //***************************************************************************
void  lifo_pwr ( void ) //*****************************************************
{  // add the pwr structure and the SPI err accum to the LIFO message
  uchar b[256], *d;
  short i, j, n, nb;
  struct pwr_param *pwr, p ;
  
    pwr = all.pwr; // point to the structure
    nb  = sizeof( p ); //=#bytes to send 
    d   = (uchar*) pwr; // recast as correct pointer type
  
    pwr->watt[3] = 0; // not used
    pwr->ti[3]   = 0; // not used
  
  b[0] = SBD_PWR; // assign id= 0xd5 = power structure
  i=3; // start storing at b[3]
  for (j=0;j<nb;j++)
      b[i++] = d[j]; // xfr the bytes starting at b[3]
      
  //--- add spi err accum ---------------------------------
  for (j=0;j<5;j++)
      b[i++] = all.exc->serr[j];
  
  //-----------------------------------
  b[i++] = ';'; // add end-of-data char
  n = i; //=total # of bytes : includes 0,1,2 bytes + ';'
  i = b2_store(1,b,n ); // store #of bytes @ b[1],b[2]
  
  append_lifo( n, b);  
  //--- also store to flash -----------
  n = n - 4; // throw away 1st 3=pid+nb, & last=;
  flash_store( n, &b[3], CF_PWR );
  
  init_serr(); // zero-out spi err accum
  
  return;
} //***************************************************************************

void  lifo_echo ( void ) //****************************************************
{ // stores edat to a new lifo 
  short i, j, n;
  
  n= all.necho; //=#of char to transfer
  all.bdat[0] = SBD_ECHO; //assign id = 0xde = echo commands
  i = 3; // dat[1], dat[2] = #bytes in the route message; set at end of fnx
  
  cprintf("Transferring %d echo bytes\n",n);
  // transfer the buffers
  for (j=0;j<n; j++) 
  {
     all.bdat[i++] = all.edat[j];  
  }
  
  all.bdat[i++] = ';'; // add end-of-data char 
  n = i; //=# of bytes to transmit : includes 0,1,2 bytes + ';'
  i = b2_store(1, all.bdat, n ); //=#bytes to transmit
  put_lifo( n, iparam[ EA_prof_num ], 0, all.bdat); //write to a new lifo
  //0902, mark lifo with profile# (0901 was dive-set #=surfacing#)
  
  /* for testing, display buffer
  for (i=0; i<n; i++)
    cprintf("%02x ",all.bdat[i] );
  cprintf("\n"); /**/
  
  all.necho = 0; // reset: any new incoming messages will start over
  
  return;
} //***************************************************************************

//lifo_store_param is in param.c, so it has access to the param_table


void  display_lifo ( struct lifo_param *lifo )  //*****************************
{ // display status of the lifo buffers
  short i, nb, stat,dive, ip, ntry;
  
  for (i=0;i<NSECTORS;i++) //==========================================
  {  // get info for each sector
    nb   = lifo->nbytes[i];  // #bytes in message
    stat = lifo->stat[i];   // if 0  means sent successfully
    dive = lifo->dive[i];   // dive #
    ip   = lifo->ip[i];     // individual packet id #
    ntry = lifo->ntry[i];   // # of transmission attempts
    cprintf("i %2d dive %4d ip %4d nb %4d   stat=%3d  ntry=%2d\n",
       i,dive,ip,nb,stat, ntry);
  } //=================================================================

  return;
} //***************************************************************************

#if BENTHOS==1 || NPS==1 //==============================================================
 //=== BENTHOS adds hex dump of data to parsed output =========================
 void parse_lifo_benthos (struct lifo_param *lifo, short i)  //****************
 { // parse contents of sector i
  short nb, stat,dive, ip, j=0, n1, n, sn;
  unsigned char *out,*dptr;
  
  out = lifo->buf + (long) i* (long)LIFO_NBYTE; //=output location
  nb   = lifo->nbytes[i];  // #bytes in message
  stat = lifo->stat[i];   // if 0  means sent successfully
  dive = lifo->dive[i];   // dive #
  ip   = lifo->ip[i];     // individual packet id #
  printf("i %2d dive %4d ip %4d nb %4d   stat=%3d\n",
                       i,dive,ip,nb,stat); 
  if ( nb <= 0 ) return;//nothing to show if no data in LIFO
 
  dptr=out;  //do a char dump
  printf("CHAR DUMP (printable chars only:\n");
  for(n=0;n<nb;n++)
  { if( (out[n]>9) && (out[n]<126) )printf("%c",out[n]);
    //if(n%16==15)printf("\n");
  }
  //if(n%16!=0)printf("\n");
  printf("\n\n");
  
  cprintf("1. for hex dump:");
  get_num(&n1);
  cprintf("\n");
  if(n1)
   {     
    dptr=out;  //do a raw dump
    printf("RAW HEX DUMP:\n");
    for(n=0;n<nb;n++)
     { printf("%02x ",*dptr++);
     }
    printf("\n\n");
   }
   
  // JUL03 : first bytes in LIFO are s/n, dive, packet id
  sn = out[0]*256 + out[1];
  dive = out[2]*256 + out[3];
    
  printf("s/n %4d  dive# %4d  pid= %3d\n",
       sn, dive, out[4]);


  return;
 } // end BENTHOS version of parse_lifo ***********************************
//  #else //===================================================================
//=== jan13, make a separate benthos parse fnx above
#endif //======================================================================

//=== non-BENTHOS has no hex dump of data =====================================
 void  parse_lifo ( struct lifo_param *lifo, short i)  //******************
 { // parse contents of sector i
  short nb, stat,dive, ip, j=0, s, n1, n2, n, sn;
  unsigned char *out;
  
    out = lifo->buf + (long) i* (long)LIFO_NBYTE; //=output location
    nb   = lifo->nbytes[i];  // #bytes in message
    stat = lifo->stat[i];   // if 0  means sent successfully
    dive = lifo->dive[i];   // dive #
    ip   = lifo->ip[i];     // individual packet id #
    printf("i %2d dive %4d ip %4d nb %4d   stat=%3d\n",
       i,dive,ip,nb,stat);
    // first bytes in LIFO are s/n, dive, packet id
    sn = out[0]*256 + out[1];
    dive = out[2]*256 + out[3];
    
    cprintf("s/n %4d  dive# %4d  pid= %3d\n",
       sn, dive, out[4]);


	// parse contents, looking for ';' delimiters
	j=5;
	while (j<nb)
	{ // assume we are pointing at a sensor type:
	  s = out[j++]; // = sensor type
	  n1 = out[j++]; n2=out[j++];
	  n = n1*256 + n2; //=#total bytes for sensor
	  // this includes 1 byte header, 1 byte end, 2 bytes num
	  n = n-4; // = # of data bytes
	  cprintf("%02x %d :",s,n );
	  while ( (j<nb) && (out[j] != ';') )
	    cprintf("%02x ",out[j++] );
	  if ( out[j] == ';') { cprintf(";\n"); j++; }
	} // end while
  return;
 } //**********************************************************************


//*****END LIFO FUNCTIONS *****************************************************