/* *******************************************************************
 * *                                                                 *
 * *  ANTLIB: Functions to insert antenna information into memory    *
 * *                                                                 *
 * *******************************************************************
 * File: libs/antenna/AntInsert.c
 *
 *  COPYRIGHT (c) 1988, 1989, 1991, 1997, 1999, 2001, 2002, 2004 BY
 *         SIGMET INCORPORATED, WESTFORD MASSACHUSETTS, U.S.A.
 *
 * THIS SOFTWARE IS FURNISHED UNDER A LICENSE AND MAY BE USED AND  COPIED
 * ONLY  IN  ACCORDANCE WITH  THE  TERMS  OF  SUCH  LICENSE  AND WITH THE
 * INCLUSION OF THE ABOVE COPYRIGHT NOTICE.  THIS SOFTWARE  OR  ANY OTHER
 * COPIES  THEREOF MAY NOT BE PROVIDED OR OTHERWISE MADE AVAILABLE TO ANY
 * OTHER PERSON.  NO TITLE TO AND OWNERSHIP OF  THE  SOFTWARE  IS  HEREBY
 * TRANSFERED.
 */
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#define INCLUDE_STDIO_PROTOTYPES 1
#include "sigtypes.h"
#include "user_lib.h"
#include "antenna_lib.h"
#include "antenna_int.h"
#include "setup.h"

#include "ant_global.h"

static void AntFillVelocity
( struct rtpedx *pCurrentPedx_a,
  UINT4 iCurrentBtime_a );

static void AntFilterPedPos
( const struct rtpedx *pNewPos_a,    /* New measured position */
  UINT4 iNewTimeMs_a );              /* Btime of the new position */

static void AntFilterInu
( const struct rtship *pNewPos_a,    /* New measured position */
  UINT4 iNewTimeMs_a );              /* Btime of the new position */

static UINT1 lAnt_parse_antpos
( const UINT1 str_a[], UINT4 iformat_a, UINT4 iread_time_a,
  struct rtpedx *rtpedx_a ) ;

static void ant_parse_moving_platform
( const UINT1 str_a[],
  struct rtship *rtship_a,
  struct rtpedx *pRtpedx_a );

static void ant_parse_status
( const UINT1 str_a[], UINT4 iformat_a ) ;

static void ant_parse_v1_status
( const UINT1 str_a[2] ) ;

static void ant_parse_v2_status
( const UINT1 str_a[2] ) ;

static void ant_parse_v3_status
( const UINT1 str_a[4] ) ;

/* ------------------------------
 * Functions to insert position, velocity, and 'velocity present'
 * flags into the antenna driver.  Return value indicates whether the
 * data were successfully inserted.
 */
UINT1 AntInsertPosition
( BIN2 iEarthAzPos_a, BIN2 iEarthElPos_a,
  BIN2 iPedAzPos_a,   BIN2 iPedElPos_a,
  BIN2 iPedAzJump_a,  BIN2 iPedElJump_a, /* Position jumps due to encoder cal */
  UINT4 iBtime_a )		/* Time of packet */
{
  UINT1 lInsertedOkay = FALSE ;
  if( lAntCanInsertAngles() ) {
    volatile struct antenna_info *pant = &antGlobal_c->info ;
    struct rtpedx rtpedx ; lInsertedOkay = TRUE ;

    /* Optionally jump the history buffer
     */
    if (iPedAzJump_a||iPedElJump_a)
      {
	rtpedx.faz    = fDegFromBin2(iPedAzJump_a);
	rtpedx.fel    = fDegFromBin2(iPedElJump_a);
	rtpedx.fPedAz = rtpedx.faz;
	rtpedx.fPedEl = rtpedx.fel;
	rtpedx.fazv   = 0.0;
	rtpedx.felv   = 0.0;
	rtvecJumpData( &antClient_c.pedx, (void*)&rtpedx );
      }

    rtpedx.faz =  fPDegFromBin2( iEarthAzPos_a ) ; rtpedx.fazv  = 0.0 ;
    rtpedx.fel = fElDegFromBin2( iEarthElPos_a ) ; rtpedx.felv  = 0.0 ;

    rtpedx.fPedAz =  fPDegFromBin2( iPedAzPos_a ) ;
    rtpedx.fPedEl = fElDegFromBin2( iPedElPos_a ) ;

    AntFillVelocity( &rtpedx, iBtime_a );
    AntFilterPedPos( &rtpedx, iBtime_a );
    rtvecInsertPoint( &antClient_c.pedx, iBtime_a, (void *)&rtpedx ) ;
    pant->iBtimeAng        = iBtime_a ;

    /* Optional logging of inserted angles
     */
    if( antGlobal_c->setup.lLogDataAnt ) {
      char sLogText[120] ;
      sprintf( sLogText, "BTime:%10.10u, A:%6.2f, E:%5.2f",
	       iBtime_a, rtpedx.faz, rtpedx.fel ) ;
      antWriteLog( sLogText ) ;
    }
  }
  return( lInsertedOkay ) ;
}

UINT1 AntInsertPosAndVel
( BIN2 iEarthAzPos_a, BIN2 iEarthElPos_a,
  BIN2 iPedAzPos_a,   BIN2 iPedElPos_a,
  BIN2 iPedAzJump_a,  BIN2 iPedElJump_a,   /* Position jumps due to encoder cal */
  FLT4 fAzVel_a, FLT4 fElVel_a, UINT4 iBtime_a )
{
  UINT1 lInsertedOkay = FALSE ;
  if( lAntCanInsertAngles() ) {
    volatile struct antenna_info *pant = &antGlobal_c->info ;
    struct rtpedx rtpedx ; lInsertedOkay = TRUE ;

    /* Optionally jump the history buffer
     */
    if (iPedAzJump_a||iPedElJump_a)
      {
	rtpedx.faz    = fDegFromBin2(iPedAzJump_a);
	rtpedx.fel    = fDegFromBin2(iPedElJump_a);
	rtpedx.fPedAz = rtpedx.faz;
	rtpedx.fPedEl = rtpedx.fel;
	rtpedx.fazv   = 0.0;
	rtpedx.felv   = 0.0;
	rtvecJumpData( &antClient_c.pedx, (void*)&rtpedx );
      }

    rtpedx.faz  =  fPDegFromBin2( iEarthAzPos_a ) ; rtpedx.fazv = fAzVel_a ;
    rtpedx.fel  = fElDegFromBin2( iEarthElPos_a ) ; rtpedx.felv = fElVel_a ;

    rtpedx.fPedAz =  fPDegFromBin2( iPedAzPos_a ) ;
    rtpedx.fPedEl = fElDegFromBin2( iPedElPos_a ) ;

    AntFilterPedPos( &rtpedx, iBtime_a );
    rtvecInsertPoint( &antClient_c.pedx, iBtime_a, (void *)&rtpedx ) ;
    pant->iBtimeAng        = iBtime_a ;

    /* Optional logging of inserted angles
     */
    if( antGlobal_c->setup.lLogDataAnt ) {
      char sLogText[120] ;
      sprintf( sLogText, "BTime:%10.10u, A:%6.2f, E:%5.2f",
	       iBtime_a, rtpedx.faz, rtpedx.fel ) ;
      antWriteLog( sLogText ) ;
    }
  }
  return( lInsertedOkay ) ;
}

UINT1 AntInsertRtship
( const struct rtship *pRtShip_a,
  UINT4 iBtime_a )
{
  UINT1 lInsertedOkay = FALSE ;
  if( lAntCanInsertAngles() ) {
    volatile struct antenna_info *pant = &antGlobal_c->info ;
    lInsertedOkay = TRUE ;

    AntFilterInu( pRtShip_a, iBtime_a );
    rtvecInsertPoint( &antClient_c.ship, iBtime_a, (void *)pRtShip_a ) ;
    pant->iBtimeShip = iBtime_a ;

    /* Optional logging of inserted angles
     */
    if( antGlobal_c->setup.lLogDataINU ) {
      char sLogText[120] ;
      sprintf( sLogText, "BTime:%10.10u, H:%6.2f, R:%5.2f, P:%5.2f",
	       iBtime_a, pRtShip_a->fheading, pRtShip_a->froll, pRtShip_a->fpitch );
      antWriteLog( sLogText ) ;
    }
  }
  return( lInsertedOkay ) ;
}

/* ------------------------------
 * Estimate the velocity based on the current and past position.
 */
static void AntFillVelocity
( struct rtpedx *pCurrentPedx_a,
  UINT4 iCurrentBtime_a )
{
#define VELTDIFF  500		/* Time span (ms) for velocity estimate */
  struct rtpedx PreviousPedx ; FLT8 fTDiff = 1E-3 * VELTDIFF ;
  rtvecLookupPoint( &antClient_c.pedx, iCurrentBtime_a - VELTDIFF, (void *)&PreviousPedx ) ;

  pCurrentPedx_a->fazv =
    fDegFromBin2( iBin2FromFDeg( pCurrentPedx_a->faz - PreviousPedx.faz ) ) / fTDiff ;
  pCurrentPedx_a->felv =
    fDegFromBin2( iBin2FromFDeg( pCurrentPedx_a->fel - PreviousPedx.fel ) ) / fTDiff ;
}

/* ------------------------------
 * Filter a new pedestal position report to make sure it passes
 * sanity checks.  We are checking solely if the antenna could
 * have moved to this position based on the maximum likely
 * velocity.
 */
static void AntFilterPedPos
( const struct rtpedx *pNewPos_a,    /* New measured position */
  UINT4 iNewTimeMs_a )                /* Btime of the new position */
{
#define OVERMARGIN  1.500	/* Widen max velocity margin a bit */
#define FDELTATIME  0.101	/* Round up possible time quantization */
#define DELTATIMEMS   100	/*   from this many real miliseconds. */
#define SKIPVALIDRUN   10	/* Signal errors only after a run of valid inserts */

  UINT4 iPreviousTimeMs, iRtvecFlags;
  struct rtpedx PreviousPos;
  FLT8 fDeltaAz, fMaxDeltaAz, fDeltaEl, fMaxDeltaEl;

  iPreviousTimeMs = iNewTimeMs_a - DELTATIMEMS ;
  iRtvecFlags = ant_get_rtpedx( &PreviousPos, iPreviousTimeMs ) ;

  /* Skip checking if any flags are set
   */
  if (iRtvecFlags) return;

  fDeltaAz = pNewPos_a->fPedAz - PreviousPos.fPedAz ;
  if (fDeltaAz >  180.0) fDeltaAz -= 360.0;
  if (fDeltaAz < -180.0) fDeltaAz += 360.0;
  fDeltaAz = fabs(fDeltaAz);
  fMaxDeltaAz = OVERMARGIN * antGlobal_c->setup.fazvelmax * FDELTATIME ;
  fDeltaEl = fabs( pNewPos_a->fPedEl - PreviousPos.fPedEl ) ;
  fMaxDeltaEl = OVERMARGIN * antGlobal_c->setup.felvelmax * FDELTATIME ;

  /* Signal problems as well as their ultimate resolution.  Don't
   * signal errors until we see at least a minimal run of valid
   * inserted angles.
   */
  if( (fDeltaAz > fMaxDeltaAz) || (fDeltaEl > fMaxDeltaEl) )
    {
      if( antClient_c.iPedxSkipState == SKIPVALIDRUN )
	{
	  char sAngles[80];
	  sprintf( sAngles, "%4.2f,%4.2f was %4.2f,%4.2f",
		   pNewPos_a->fPedAz,pNewPos_a->fPedEl,
		   PreviousPos.fPedAz, PreviousPos.fPedEl );
	  sig_signal_va( ANT_PED_ANGLE_SKIPPED, 1, sAngles );
	  antClient_c.iPedxSkipState++ ;
	}
      else if( antClient_c.iPedxSkipState < SKIPVALIDRUN )
	antClient_c.iPedxSkipState = 0 ;
    }
  else
    {
      if( antClient_c.iPedxSkipState > SKIPVALIDRUN )
	{
	  char sAngles[80];
	  sprintf( sAngles, "%4.2f,%4.2f was %4.2f,%4.2f",
		   pNewPos_a->fPedAz,pNewPos_a->fPedEl,
		   PreviousPos.fPedAz, PreviousPos.fPedEl );
	  sig_signal_va( ANT_PED_ANGLE_OK, 1, sAngles );
	  antClient_c.iPedxSkipState = 0 ;
	}
      else if( antClient_c.iPedxSkipState < SKIPVALIDRUN )
	antClient_c.iPedxSkipState++ ;
    }
}

/* ------------------------------
 * Filter a new INU position report to make sure it passes sanity
 * checks.  We are checking solely to see if the roll, pitch or
 * headerin could to this position based on the maximum likely
 * velocity.
 */
static void AntFilterInu
( const struct rtship *pNewPos_a,    /* New measured position */
  UINT4 iNewTimeMs_a )               /* Btime of the new position */
{
#define MAXINUVELDEGSEC 10.0	/* Assumed max INU velocities in deg/sec */

  UINT4 iPreviousTimeMs, iRtvecFlags;
  struct rtship PreviousPos;
  FLT8 fDeltaPitch, fMaxDelta, fDeltaRoll, fDeltaHeading;

  iPreviousTimeMs = iNewTimeMs_a - DELTATIMEMS ;
  iRtvecFlags = ant_get_rtship( &PreviousPos, iPreviousTimeMs ) ;

  /* Skip checking if any flags are set
   */
  if (iRtvecFlags) return;

  fMaxDelta = OVERMARGIN * MAXINUVELDEGSEC * FDELTATIME ;

  fDeltaPitch   = pNewPos_a->fpitch - PreviousPos.fpitch ;
  fDeltaPitch   = fabs(fDeltaPitch);

  fDeltaRoll    = pNewPos_a->froll  - PreviousPos.froll ;
  fDeltaRoll    = fabs(fDeltaRoll);

  fDeltaHeading = pNewPos_a->fheading - PreviousPos.fheading ;
  if (fDeltaHeading >  180.0) fDeltaHeading -= 360.0;
  if (fDeltaHeading < -180.0) fDeltaHeading += 360.0;
  fDeltaHeading = fabs(fDeltaHeading);

  /* Signal problems as well as their ultimate resolution.  Don't
   * signal errors until we see at least a minimal run of valid
   * inserted angles.
   */
  if( (fDeltaPitch   > fMaxDelta) || (fDeltaRoll > fMaxDelta) ||
      (fDeltaHeading > fMaxDelta) )
    {
      if( antClient_c.iShipSkipState == SKIPVALIDRUN )
	{
	  char sAngles[80];
	  sprintf( sAngles, "%4.2f,%4.2f,%4.2f was %4.2f,%4.2f,%4.2f",
		   pNewPos_a->froll  , pNewPos_a->fpitch  , pNewPos_a->fheading,
		   PreviousPos.froll , PreviousPos.fpitch , PreviousPos.fheading );
	  sig_signal_va( ANT_INU_ANGLE_SKIPPED, 1, sAngles );
	  antClient_c.iShipSkipState++ ;
	}
      else if( antClient_c.iShipSkipState < SKIPVALIDRUN )
	antClient_c.iShipSkipState = 0 ;
    }
  else
    {
      if( antClient_c.iShipSkipState > SKIPVALIDRUN )
	{
	  char sAngles[80];
	  sprintf( sAngles, "%4.2f,%4.2f,%4.2f was %4.2f,%4.2f,%4.2f",
		   pNewPos_a->froll  , pNewPos_a->fpitch  , pNewPos_a->fheading,
		   PreviousPos.froll , PreviousPos.fpitch , PreviousPos.fheading );
	  sig_signal_va( ANT_INU_ANGLE_OK, 1, sAngles );
	  antClient_c.iShipSkipState = 0 ;
	}
      else if( antClient_c.iShipSkipState < SKIPVALIDRUN )
	antClient_c.iShipSkipState++ ;
    }
}

/* ------------------------------
 * Function to process an antenna packet in any of the RCVxx formats,
 * once we know that it is formatted correctly.  The contents are
 * inserted into the antenna information database.
 */
void AntInsertPacket
( const UINT1 str_a[],		/* Array of data received */
  UINT4 iBtime_a,               /* Time the packet arrived */
  UINT4 iformat_a,		/* One of ANTFMT_RCVxxx */
  SINT4 iLength_a )		/* Total length of the packet */
{
  struct rtpedx rtpedx ; struct rtship rtship ; char sLogText[120] ;
  volatile struct antenna_info *pant = &antGlobal_c->info ;

  /* Set the arrival time of the most recent packet, and make a
   * history entry holding the most recent packet.
   */
  pant->iBtimePkt = iBtime_a ;
  antMakeHistoryEntry
    ( str_a, iLength_a, ANTGET_RCV_MASK, &antGlobal_c->vdat.updHist.ant ) ;

  /* Optionally log the I/O packet.
   */
  if( antGlobal_c->setup.lLogPacketsAnt ) {
    antWriteLogBinary( ":", str_a, iLength_a ) ;
  }
  /* Get all the position information, and insert it into the realtime
   * history.  Do this only if we are the correct client type.
   */
  if( lAntCanInsertAngles() ) {
    UINT1 lVelocityAvailable =
      lAnt_parse_antpos( str_a, iformat_a, iBtime_a, &rtpedx ) ;

    if( iformat_a == ANTFMT_RCV03 ) {
      /* Get all the moving platform information if the receive format
       * provides it to us.
       */
      ant_parse_moving_platform( str_a, &rtship, &rtpedx );
      rtvecInsertPoint( &antClient_c.ship, iBtime_a, (void *)&rtship ) ;
      pant->iBtimeShip = iBtime_a ;
    } else {
      /* If we don't have independent pedestal angles, then copy them
       * to match the Earth angles since we assume they're the same.
       */
      rtpedx.fPedAz = rtpedx.faz ;
      rtpedx.fPedEl = rtpedx.fel ;
    }
    if( ! lVelocityAvailable ) AntFillVelocity( &rtpedx, iBtime_a ) ;
    AntFilterPedPos( &rtpedx, iBtime_a ) ;
    rtvecInsertPoint( &antClient_c.pedx, iBtime_a, (void *)&rtpedx ) ;

    pant->iBtimeAng = iBtime_a ;

    /* Optional data logging
     */
    switch( (antGlobal_c->setup.lLogDataAnt ? 1 : 0) +
	    (antGlobal_c->setup.lLogDataINU ? 2 : 0) ) {
    case 1:
      sprintf( sLogText,
	       "BTime:%10.10u, A:%6.2f, E:%5.2f",
	       iBtime_a, rtpedx.faz, rtpedx.fel ) ;
      antWriteLog( sLogText ) ;
      break ;
    case 2:
      sprintf( sLogText,
	       "BTime:%10.10u, H:%6.2f, R:%5.2f, P:%5.2f",
	       iBtime_a, rtship.fheading, rtship.froll, rtship.fpitch ) ;
      antWriteLog( sLogText ) ;
      break ;
    case 3:
      sprintf( sLogText,
	       "BTime:%10.10u, A:%6.2f, E:%5.2f, H:%6.2f, R:%5.2f, P:%5.2f",
	       iBtime_a, rtpedx.faz, rtpedx.fel,
	       rtship.fheading, rtship.froll, rtship.fpitch ) ;
      antWriteLog( sLogText ) ;
      break ;
    }
  }
  /* Get all of the status information
   */
  ant_parse_status( str_a, iformat_a );
}

/* ------------------------------
 * Get normal antenna position and velocity information from the
 * format.  Return value is TRUE if a measured velocity value was
 * available.
 */
static UINT1 lAnt_parse_antpos
( const UINT1 str_a[], UINT4 iformat_a, UINT4 iread_time_a,
  struct rtpedx *rtpedx_a )
{
  SINT2 ivelocity ; UINT1 lVelocityAvailable = TRUE ;

  /* First extract the reported positions.
   */
  switch (iformat_a)
    {
    default:
      rtpedx_a->faz =  fPDegFromBin2( (((UINT2)str_a[2])<<9) | (((UINT2)str_a[1])<<2) ) ;
      rtpedx_a->fel = fElDegFromBin2( (((UINT2)str_a[4])<<9) | (((UINT2)str_a[3])<<2) ) ;
      break;

    case ANTFMT_RCV03:
      rtpedx_a->faz =  fPDegFromBin2( (((UINT2)str_a[3])<<9) | (((UINT2)str_a[2])<<2) ) ;
      rtpedx_a->fel = fElDegFromBin2( (((UINT2)str_a[5])<<9) | (((UINT2)str_a[4])<<2) ) ;
      break;
    }

  /* Read the reported antenna velocities, or compute them if they are
   * not supplied in the format.
   */
  switch (iformat_a)
    {
    case ANTFMT_RCV03:
      ivelocity = ((SINT2)((((UINT2)str_a[16])<<2) | (((UINT2)str_a[17])<<9)))/4 ;
      rtpedx_a->fazv = ((FLT4)ivelocity) / ANT02_VEL_UNITS ;

      ivelocity = ((SINT2)((((UINT2)str_a[18])<<2) | (((UINT2)str_a[19])<<9)))/4 ;
      rtpedx_a->felv = ((FLT4)ivelocity) / ANT02_VEL_UNITS ;
      break;

    case ANTFMT_RCV02:
    case ANTFMT_RCV04:
    case ANTFMT_RCV05:
    default:
      ivelocity = ((SINT2)((((UINT2)str_a[5])<<2) | (((UINT2)str_a[6])<<9)))/4 ;
      rtpedx_a->fazv = ((FLT4)ivelocity) / ANT02_VEL_UNITS ;

      ivelocity = ((SINT2)((((UINT2)str_a[7])<<2) | (((UINT2)str_a[8])<<9)))/4 ;
      rtpedx_a->felv = ((FLT4)ivelocity) / ANT02_VEL_UNITS ;
      break;

    case ANTFMT_RCV01:
      lVelocityAvailable = FALSE ;
      rtpedx_a->fazv = 0.0 ;
      rtpedx_a->felv = 0.0 ;
      break;
    }
  return( lVelocityAvailable ) ;
}

/* ------------------------------
 * Decode the moving platform parameters from a serial record.
 */
static void ant_parse_moving_platform
( const UINT1 str_a[],
  struct rtship *rtship_a,
  struct rtpedx *pRtpedx_a )
{
  SINT2 ivelocity, ialtitude ;
  UINT2 iInuFaultBits=0;

  pRtpedx_a->fPedAz   =  fPDegFromBin2( (((UINT2)str_a[ 7])<<9) | (((UINT2)str_a[ 6])<<2));
  pRtpedx_a->fPedEl   = fElDegFromBin2( (((UINT2)str_a[ 9])<<9) | (((UINT2)str_a[ 8])<<2));
  rtship_a->fpitch    =   fDegFromBin2( (((UINT2)str_a[11])<<9) | (((UINT2)str_a[10])<<2) ) ;
  rtship_a->froll     =   fDegFromBin2( (((UINT2)str_a[13])<<9) | (((UINT2)str_a[12])<<2) ) ;
  rtship_a->fheading  =  fPDegFromBin2( (((UINT2)str_a[15])<<9) | (((UINT2)str_a[14])<<2) ) ;

  /* Read the reported antenna velocities
   */
  ivelocity = ((SINT2)((((UINT2)(0x7e&str_a[20]))<<2) | (((UINT2)str_a[21])<<9)))/4 ;
  rtship_a->fpitchv   = ((FLT4)ivelocity) / ANT02_VEL_UNITS ;

  ivelocity = ((SINT2)((((UINT2)(0x7e&str_a[22]))<<2) | (((UINT2)str_a[23])<<9)))/4 ;
  rtship_a->frollv    = ((FLT4)ivelocity) / ANT02_VEL_UNITS ;
  if (0x01&str_a[22]) iInuFaultBits |= ANT_INU_INVROLL;

  ivelocity = ((SINT2)((((UINT2)(0x7e&str_a[24]))<<2) | (((UINT2)str_a[25])<<9)))/4 ;
  rtship_a->fheadingv = ((FLT4)ivelocity) / ANT02_VEL_UNITS ;
  if (0x01&str_a[24]) iInuFaultBits |= ANT_INU_INVHEAD;

  /* Velocity on the earth: east, north, and up
   */
  ivelocity = (SINT2)((((UINT2)(0x7e&str_a[40]))<<2) | (((UINT2)str_a[41])<<9)) ;
  rtship_a->fvel_east  = 0.0025 * (FLT4)ivelocity ;
  if (0x01&str_a[40]) iInuFaultBits |= ANT_INU_INVHORZ;

  ivelocity = (SINT2)((((UINT2)(0x7e&str_a[42]))<<2) | (((UINT2)str_a[43])<<9)) ;
  rtship_a->fvel_north = 0.0025 * (FLT4)ivelocity ;

  ivelocity = (SINT2)((((UINT2)(0x7e&str_a[44]))<<2) | (((UINT2)str_a[45])<<9)) ;
  rtship_a->fvel_up    = 0.0025 * (FLT4)ivelocity ;
  if (0x01&str_a[44]) iInuFaultBits |= ANT_INU_INVVERT;

  /* Position on the earth: latitude, longitude, altitude.  Override
   * the position if invalid
   */
  if (iInuFaultBits & ANT_INU_INVHORZ)
    {
      rtship_a->flatitude  = fDegFromBin4(antGlobal_c->setup.ilat);
      rtship_a->flongitude = fDegFromBin4(antGlobal_c->setup.ilon);
    }
  else
    {
      rtship_a->flatitude  = fDegFromBin4
	( (((UINT4)str_a[32])<<11) | (((UINT4)str_a[33])<<18) | (((UINT4)str_a[34])<<25) ) ;

      rtship_a->flongitude = fDegFromBin4
	( (((UINT4)str_a[35])<<11) | (((UINT4)str_a[36])<<18) | (((UINT4)str_a[37])<<25) ) ;
    }

  ialtitude = (SINT2)((((UINT2)str_a[38])<<2) | (((UINT2)str_a[39])<<9)) ;
  rtship_a->faltitude = (FLT4)(ialtitude/4) +
    ((FLT4)antGlobal_c->setup.inu_height_offset) / 100.0 ;

  /* Do indivisible assignment
   */
  antGlobal_c->info.iInuFaultBits = iInuFaultBits;
}

/* ------------------------------
 * Process status bytes for all formats
 */
static void ant_parse_status
(const UINT1 str_a[],
 UINT4 iformat_a )
{
  switch (iformat_a)
    {
    case ANTFMT_RCV01:
      ant_parse_v1_status( &str_a[5] );
      /* Copy the az to el encoder calibration bit */
      if ( antGlobal_c->info.isbits | ASF_CALIB_AZ)
	antGlobal_c->info.isbits |=  ASF_CALIB_EL;
      else
	antGlobal_c->info.isbits &= ~ASF_CALIB_EL;
      break;

    case ANTFMT_RCV02:
    case ANTFMT_RCV04:
    default:
      ant_parse_v1_status( &str_a[9] );
      ant_parse_v2_status( &str_a[11] );
      break;

    case ANTFMT_RCV03:
      ant_parse_v1_status( &str_a[26] );
      ant_parse_v2_status( &str_a[28] );
      break;

    case ANTFMT_RCV05:
      ant_parse_v1_status( &str_a[9] );
      ant_parse_v2_status( &str_a[11] );
      ant_parse_v3_status( &str_a[15] );
      break;
    }
}

/* ------------------------------
 * Routine to parse the overall radar and antenna status from a pair
 * of bytes.
 */
static void  ant_parse_v1_status
( const UINT1 str_a[2] )
{
  UINT2 lbset, ipwbw;

  /* Set low airflow bit
   */
  lbset = ( BTEST( str_a[0], 6 ) ==
	    (0!=antGlobal_c->setup.status_low_airflow.lhigh) );
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_LOWAIRFLOW;
  else
    antGlobal_c->info.isbits &= ~ASF_LOWAIRFLOW;

  /* Check low waveguide pressure bit
   */
  lbset = ( BTEST( str_a[0], 5 ) ==
	    (0!=antGlobal_c->setup.status_low_wgp.lhigh)) ;
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_LOW_WGP;
  else
    antGlobal_c->info.isbits &= ~ASF_LOW_WGP;

  /* Check servo power bit
   */
  lbset = ( BTEST( str_a[0], 4 ) ==
	    (0!=antGlobal_c->setup.status_servo_power.lhigh)) ;
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_SERVO;
  else
    antGlobal_c->info.isbits &= ~ASF_SERVO;

  /* Check antenna local bit
   */
  lbset = ( BTEST( str_a[0], 3 ) ==
	    (0!=antGlobal_c->setup.status_local.lhigh));
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_LOCAL;
  else
    antGlobal_c->info.isbits &= ~ASF_LOCAL;

  /* Check interlock bit
   */
  lbset = ( BTEST( str_a[0], 2 ) ==
	    (0!=antGlobal_c->setup.status_interlock_open.lhigh) );
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_INTERLOCK;
  else
    antGlobal_c->info.isbits &= ~ASF_INTERLOCK;

  /* Check standby bit
   */
  lbset = ( BTEST( str_a[0], 1 ) ==
	    (0!=antGlobal_c->setup.status_standby.lhigh) );
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_STANDBY;
  else
    antGlobal_c->info.isbits &= ~ASF_STANDBY;

  /* Check radiate bit
   */
  lbset = ( BTEST( str_a[0], 0 ) ==
	    (0!=antGlobal_c->setup.status_radiate.lhigh) );
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_RADIATE;
  else
    antGlobal_c->info.isbits &= ~ASF_RADIATE;

  /* Check shutdown bit
   */
  lbset = ( BTEST( str_a[1], 6 ) ==
	    (0!=antGlobal_c->setup.status_shutdown.lhigh) );
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_SHUTDOWN;
  else
    antGlobal_c->info.isbits &= ~ASF_SHUTDOWN;

  /* Check pulse width LSB
   */
  if (BTEST( str_a[1], 5) )
    ipwbw = 1;
  else
    ipwbw = 0;

  /* Check t/r power bit
   */
  lbset = ( BTEST( str_a[1], 4 ) ==
	    (0!=antGlobal_c->setup.status_tr_power.lhigh) );
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_TRPOWER;
  else
    antGlobal_c->info.isbits &= ~ASF_TRPOWER;

  /* Check T/R local bit
   */
  lbset = ( BTEST( str_a[1], 3 ) ==
	    (0!=antGlobal_c->setup.status_trlocal.lhigh));
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_TRLOCAL;
  else
    antGlobal_c->info.isbits &= ~ASF_TRLOCAL;

  /* Check azimuth encoder bit
   */
  lbset = ( BTEST( str_a[1], 2 ) ==
	    (0!=antGlobal_c->setup.status_calib_az.lhigh));
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_CALIB_AZ;
  else
    antGlobal_c->info.isbits &= ~ASF_CALIB_AZ;

  /* Check pulse width MSB
   */
  if (BTEST( str_a[1], 1) ) ipwbw |= 2;
  antGlobal_c->info.iActualPwbw = ipwbw;

  /* Check magnitron current bit
   */
  lbset = ( BTEST( str_a[1], 0 ) ==
	    (0!=antGlobal_c->setup.status_mag_current.lhigh) );
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_MAGTFLT;
  else
    antGlobal_c->info.isbits &= ~ASF_MAGTFLT;
}

/* ------------------------------
 * Routine to parse the IRIS mode status and signal generator level
 * from a pair of bytes.
 */
static void ant_parse_v2_status
( const UINT1 str_a[2] )
{
  UINT2 lbset;

  /* Elevation encoder bit
   */
  lbset = ( BTEST( str_a[0], 3 ) ==
	    (0!=antGlobal_c->setup.status_calib_el.lhigh));
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_CALIB_EL;
  else
    antGlobal_c->info.isbits &= ~ASF_CALIB_EL;

  /* Signal generator fault
   */
  lbset = ( BTEST( str_a[0], 2 ) ==
	    (0!=antGlobal_c->setup.status_sigfault.lhigh) );
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_SIGFAULT;
  else
    antGlobal_c->info.isbits &= ~ASF_SIGFAULT;

  /* Signal generator status
   */
  lbset = ( BTEST( str_a[0], 1 ) ==
	    (0!=antGlobal_c->setup.status_siggen.lhigh) );
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_SIGGEN;
  else
    antGlobal_c->info.isbits &= ~ASF_SIGGEN;

  /* Signal generator mode
   */
  lbset = ( BTEST( str_a[0], 0 ) ==
	    (0!=antGlobal_c->setup.status_sigcw.lhigh) );
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_SIGCW;
  else
    antGlobal_c->info.isbits &= ~ASF_SIGCW;

  /* Requested rst mode
   */
  antGlobal_c->info.irequested_rst_mode = ((UINT4)str_a[0])>>4;

  /* Signal generator value
   */
  {
    SINT4 ivalue = (SINT4)(UINT4)str_a[1] ;
    antGlobal_c->info.isiggen_value = (antGlobal_c->setup.isiggen_max - ivalue) ;
  }
}

/* ------------------------------
 * Routine to parse the Dual-System status from a trio of bytes.
 */
static void ant_parse_v3_status
(const UINT1 str_a[4] )
{
  volatile struct drcp_status *drcp = &antGlobal_c->info.drcp ;
  UINT1 lbset;

  drcp->lRunningDual     =   BTEST( str_a[0], 6 ) ;
  drcp->iDrcpMode        = ( BTEST( str_a[0], 5 ) ? 2 : 0 ) +
                           ( BTEST( str_a[0], 4 ) ? 1 : 0 ) ;
  drcp->lUnitA           =   BTEST( str_a[0], 3 ) ;
  drcp->sysB.lKnown      =   BTEST( str_a[0], 2 ) || (! drcp->lUnitA) ;
  drcp->sysA.lKnown      =   BTEST( str_a[0], 2 ) || (  drcp->lUnitA) ;
  drcp->lPreferredA      =   BTEST( str_a[0], 1 ) ;


  drcp->sysB.lDisabled   =   BTEST( str_a[0], 0 ) ;
  drcp->sysB.lOkay       =   BTEST( str_a[1], 6 ) ;
  drcp->sysB.iActivity   = ( BTEST( str_a[1], 5 ) ? 2 : 0 ) +
                           ( BTEST( str_a[1], 4 ) ? 1 : 0 ) ;


  drcp->sysA.lDisabled   =   BTEST( str_a[1], 3 ) ;
  drcp->sysA.lOkay       =   BTEST( str_a[1], 2 ) ;
  drcp->sysA.iActivity   = ( BTEST( str_a[1], 1 ) ? 2 : 0 ) +
                           ( BTEST( str_a[1], 0 ) ? 1 : 0 ) ;

  drcp->lFlipAllowed     =   BTEST( str_a[2], 6 ) ;
  drcp->sysB.lRelinquish =   BTEST( str_a[2], 5 ) ;
  drcp->sysA.lRelinquish =   BTEST( str_a[2], 4 ) ;
  drcp->sysB.lWouldUse   =   BTEST( str_a[2], 3 ) ;
  drcp->sysA.lWouldUse   =   BTEST( str_a[2], 2 ) ;

  antGlobal_c->info.iActualPolar = str_a[3] & 0x07;

  /* Polarization is OK
   */
  lbset = str_a[3] & 0x08;
  if (lbset)
    antGlobal_c->info.isbits |=  ASF_POLAR;
  else
    antGlobal_c->info.isbits &= ~ASF_POLAR;
}
