/* **********************************************************************
 * *                                                                    * 
 * *       Generic Simulator of a Serial Radar/Antenna Controller       *
 * *                                                                    * 
 * **********************************************************************
 * File:  libs/antenna/antsim_xmt.c
 * 
 * COPYRIGHT (c) 1987, 1988, 1991, 1996, 1999, 2000, 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 <locale.h>
#include <math.h>
#include <netinet/in.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>

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

#include "antsim_global.h"

static void  antsim_xmt_loop
( int   iFD_a,
  SINT4 ixmt_size_a,
  UINT4 iformat_a,
  const char *sFormat_a );

static void set_xmt_antpos
( UINT1 str_a[],
  UINT4 iformat_a );
      
static void set_xmt_status
( UINT1 str_a[],
  UINT4 iformat_a );

static void set_xmt_v1_status
( UINT1 str_a[2] );

static void set_xmt_v2_status
( UINT1 str_a[4] );

static void set_xmt_v3_status
( UINT1 str_a[4] );

static void  set_xmt_moving_platform
( UINT1 str_a[] );

static void  compose_bite_packet
( UINT1 str_a[], SINT4* ilength_a, SINT2 iunit_a );

static void compose_qbite_packet
( UINT1 str_a[], SINT4* ilength_a, SINT2 iunit_a );

static void compose_time_packet
(UINT1 str_a[],    /* Packet returned */
 SINT4* ilength_a ); /* Number of bytes in the packet */

static void  ant_sim_tick_
( void );

/* ------------------------------
 * Simulate one of several styles of serial Radar/Antenna controller.
 * History:
 *     jmh   13 Sep 1991   Converted from FORTRAN to C.
 *     jmh    4 Apr 1991   Made into detached process
 *     jmh   24 Nov 1989   switched simulator structure to ant_g(nants_p+1)
 */
int  main()
{
  MESSAGE istatus;
  UINT4 iformat;
  UINT2 lcreated;
  int   iFD;
   
  /* Map to the global section, and assign process priority.
   */
  setlocale( LC_ALL, "" ) ;

  istatus = sig_run_as_operator() ;
  if( istatus != SS_NORMAL ) sig_signal(istatus) ;

  istatus = AntPowerUpMain( ANT_IAM_SIM_XMT, 0 );
  if( istatus != SS_NORMAL ) {
    sig_signal( istatus ) ; exit( EXIT_FAILURE ) ;
  }
  sig_establish( antSignalHandler, "ANTSIM_XMT" ) ;

  istatus = antsim_global_init(&lcreated);
  if( istatus != SS_NORMAL ) {
    pantsim_c->info.ixmt_status = istatus;
    sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
  }
  pantsim_c->info.ixmt_pid = getpid();

#ifdef HPUX
  if( pantsim_c->info.lauto ) ignore_next_signal( SIGHUP ) ;
#endif

  istatus = sig_setpriority( (SINT4)pantsim_c->setup.ipri_antxmt ) ;
  if( istatus != SS_NORMAL ) sig_signal(istatus) ;

  antWriteLog( "ANTSIM_XMT Process Starting" ) ;
  iFD = -1;

  /* Latch format so changes to shared memory do not give trouble
   */
  iformat = pantsim_c->setup.istatus_format;
  switch( iformat )
    {
    case ANTFMT_RCV01:
      pantsim_c->info.ixmt_status = SS_NORMAL;
      antsim_xmt_loop( iFD, 8, iformat, "RCV01" );
      break;

    case ANTFMT_RCV02:
    case ANTFMT_RCV04:
      pantsim_c->info.ixmt_status = SS_NORMAL;
      antsim_xmt_loop( iFD, 16, iformat, "RCV02" );
      break;

    case ANTFMT_RCV03:
      pantsim_c->info.ixmt_status = SS_NORMAL;
      antsim_xmt_loop( iFD, 47, iformat, "RCV03" );
      break;

    case ANTFMT_RCV05:
      pantsim_c->info.ixmt_status = SS_NORMAL;
      antsim_xmt_loop( iFD, 24, iformat, "RCV05" );
      break;

    default:
      pantsim_c->info.ixmt_status = ANT_BAD_STATUS;
      sig_signal( ANT_BAD_STATUS );
      exit(EXIT_FAILURE) ;
      break;
    }
  return 0;
}
 
/* ------------------------------
 */
static void  antsim_xmt_loop
( int   iFD_a,
  SINT4 ixmt_size_a,
  UINT4 iformat_a,
  const char *sFormat_a )
{
#define MAX_XMT_SIZE (47)
  SINT2 iunit, iunit_cnt;
  SINT4 ilength, itick, icount, ixmt_size;
  
  UINT1 str[10 + BITE_MAX_LENGTH];

  /* --------------------------------------------------------
   */
  ixmt_size = MIN( MAX_XMT_SIZE, ixmt_size_a );
  
  /* The simulator will only work correctly if the data can get out
   * in the time below.  This means the baud rate must be at least
   * 19200 for RCV03, and 4800 for others.
   */
  itick = 50;
  
  while(TRUE)
    {
      /* Update the simulator state
       */
      ant_sim_tick_( );
      
      /* Compose a string to be transmitted
       */
      memset( str, 0, ixmt_size ) ; str[0] = ANT_SYNC_ANT;

      set_xmt_antpos( str, iformat_a );
      if (iformat_a == ANTFMT_RCV03) set_xmt_moving_platform( str );

      set_xmt_status( str, iformat_a );
      str[ixmt_size-1] = 0xFF;

      icount = WriteInt( iFD_a, str, ixmt_size, sFormat_a, htonl(INADDR_ANY) );
      if (icount==ixmt_size)
	pantsim_c->info.iout_cnt += ixmt_size;
      else
	pantsim_c->info.iout_ercnt += ixmt_size;


      /* We must loop through all of the available units
       */
      iunit_cnt = pantsim_c->bitex.iunit_cnt;
      if (iunit_cnt>BITE_MAX_UNITS) iunit_cnt = BITE_MAX_UNITS;
      for (iunit=0; iunit<iunit_cnt; iunit++)
	{
	  volatile struct bitesim_unit_info *pBite=&(pantsim_c->binfo.Units[iunit]);

	  if (pBite->lSendUpdate)
	    {
	      pBite->lSendUpdate=FALSE;
	      if (pantsim_c->bitex.units[iunit].lQBite)
		{
		  compose_qbite_packet( str, &ilength, iunit );
		  icount = WriteInt( iFD_a, str, ilength, "BITES", htonl(INADDR_ANY) );
		  pantsim_c->binfo.Units[iunit].iout_cnt += ilength;
		}
	      else
		{
		  compose_bite_packet( str, &ilength, iunit );
		  icount = WriteInt( iFD_a, str, ilength, "BITES", htonl(INADDR_ANY) );
		  pantsim_c->binfo.Units[iunit].iout_cnt += ilength;
		}
	    }
	  
	}  /* end for (iunit=0; ... */

      if (pantsim_c->info.lsend_time)
	{
	  pantsim_c->info.lsend_time = FALSE;
	  getbtime( &pantsim_c->info.itime_sent_time );

	  compose_time_packet( str, &ilength );
	  icount = WriteInt( iFD_a, str, ilength, "TIME", htonl(INADDR_ANY) );
	  if (icount==ilength)
	    pantsim_c->info.iout_cnt += ilength;
	  else
	    pantsim_c->info.iout_ercnt += ilength;
	}

      sig_microSleep( 1000 * itick ) ;
    }
}

/* ------------------------------
 */
static void set_xmt_antpos
( UINT1 str_a[],
  UINT4 iformat_a )
{
  SINT2 ivelocity;

  switch (iformat_a)
    {
    default:
      str_a[1] = ( pantsim_c->info.iaz >> 2 )& 0x7F;
      str_a[2] = ( pantsim_c->info.iaz >> 9 )& 0x7F;
      str_a[3] = ( pantsim_c->info.iel >> 2 )& 0x7F;
      str_a[4] = ( pantsim_c->info.iel >> 9 )& 0x7F;
      break;

    case ANTFMT_RCV03:
      str_a[2] = ( pantsim_c->info.iaz >> 2 )& 0x7F;
      str_a[3] = ( pantsim_c->info.iaz >> 9 )& 0x7F;
      str_a[4] = ( pantsim_c->info.iel >> 2 )& 0x7F;
      str_a[5] = ( pantsim_c->info.iel >> 9 )& 0x7F;
      break;
    }

  switch (iformat_a)
    {
    case ANTFMT_RCV03:
      ivelocity = (SINT2)(pantsim_c->info.fazv*ANTSIM02_VEL_UNITS);
      str_a[16] = ivelocity & 0x7f;
      str_a[17] = (ivelocity>>7) & 0x7f;
      ivelocity = (SINT2)(pantsim_c->info.felv*ANTSIM02_VEL_UNITS);
      str_a[18] = ivelocity & 0x7f;
      str_a[19] = (ivelocity>>7) & 0x7f;
      break;

    case ANTFMT_RCV02:
    case ANTFMT_RCV04:
    case ANTFMT_RCV05:
    default:
      ivelocity = (SINT2)(pantsim_c->info.fazv*ANTSIM02_VEL_UNITS);
      str_a[5] = ivelocity & 0x7f;
      str_a[6] = (ivelocity>>7) & 0x7f;
      ivelocity = (SINT2)(pantsim_c->info.felv*ANTSIM02_VEL_UNITS);
      str_a[7] = ivelocity & 0x7f;
      str_a[8] = (ivelocity>>7) & 0x7f;
      break;

    case ANTFMT_RCV01:
      /* Velocity is not reported
       */
      break;
    }
}

/* ------------------------------
 */
static void set_xmt_status
( UINT1 str_a[],
  UINT4 iformat_a )
{
  switch (iformat_a)
    {
    case ANTFMT_RCV01:
      set_xmt_v1_status( &(str_a[5]) );
      break;

    case ANTFMT_RCV02:
    case ANTFMT_RCV04:
    default:
      set_xmt_v1_status( &(str_a[ 9]) );
      set_xmt_v2_status( &(str_a[11]) );
      break;

    case ANTFMT_RCV03:
      set_xmt_v1_status( &(str_a[26]) );
      set_xmt_v2_status( &(str_a[28]) );
      break;

    case ANTFMT_RCV05:
      set_xmt_v1_status( &(str_a[ 9]) );
      set_xmt_v2_status( &(str_a[11]) );
      set_xmt_v3_status( &(str_a[15]) );
      break;
    }
}
      
/* ------------------------------
 * Routine to set the overall radar and antenna status, producing a
 * pair of bytes.
 */
static void set_xmt_v1_status
( UINT1 str_a[2] )
{
  str_a[0] = 0;
  if(  (pantsim_c->info.isbits & ASF_RADIATE   ) ) str_a[0] |=    1;
  if(  (pantsim_c->info.isbits & ASF_STANDBY   ) ) str_a[0] |=    2;
  if(  (pantsim_c->info.isbits & ASF_INTERLOCK ) ) str_a[0] |=    4;
  if(  (pantsim_c->info.isbits & ASF_LOCAL     ) ) str_a[0] |=    8;
  if(  (pantsim_c->info.isbits & ASF_SERVO     ) ) str_a[0] |= 0x10;
  if( !(pantsim_c->info.isbits & ASF_LOW_WGP   ) ) str_a[0] |= 0x20;
  if( !(pantsim_c->info.isbits & ASF_LOWAIRFLOW) ) str_a[0] |= 0x40;

  str_a[1] = 0;
  if( !(pantsim_c->info.isbits & ASF_MAGTFLT) ) str_a[1] |=    1;
  if(  pantsim_c->info.iActualPwbw & 2 )        str_a[1] |=    2;
  if(  pantsim_c->info.isbits & ASF_TRLOCAL )   str_a[1] |=    8;
  if(  pantsim_c->info.isbits & ASF_TRPOWER )   str_a[1] |= 0x10;
  if(  pantsim_c->info.iActualPwbw & 1 )        str_a[1] |= 0x20;
  if(  pantsim_c->info.isbits & ASF_SHUTDOWN )  str_a[1] |= 0x40;
}

/* ------------------------------
 * Routine to set the IRIS mode status, signal generator level, and
 * time stamp, producing an array of four bytes.
 */
static void set_xmt_v2_status
( UINT1 str_a[4] )
{
  UINT1 imode;

  str_a[0] = 0;
  if( pantsim_c->info.isbits & ASF_SIGCW    ) str_a[0] |=  1;
  if( pantsim_c->info.isbits & ASF_SIGGEN   ) str_a[0] |=  2;
  if( pantsim_c->info.isbits & ASF_SIGFAULT ) str_a[0] |=  4;

  /* Set the rst mode request
   */
  imode = (pantsim_c->info.irequested_rst_mode & 7) << 4 ;
  str_a[0] |= imode;

  /* Set the signal generator level
   */
  str_a[1] = (pantsim_c->setup.isiggen_max - pantsim_c->info.isiggen_value) & 0x7F ;

  /* Set the time stamp
   */
  str_a[2] = ( pantsim_c->info.isim_time      ) & 0x7F ;
  str_a[3] = ( pantsim_c->info.isim_time >> 7 ) & 0x7F ;
}
      
/* ------------------------------
 * Routine to set the Dual-System status, producing a trio of bytes.
 */
static void set_xmt_v3_status( UINT1 str_a[4] )
{
  volatile struct drcp_status *drcp = &pantsim_c->info.drcp ;

  UINT1 lOtherKnown =
    ( drcp->sysA.lKnown && (! drcp->lUnitA) ) ||
    ( drcp->sysB.lKnown && (  drcp->lUnitA) ) ;

  str_a[0] =
    (  drcp->lRunningDual    ? 0x40 : 0 ) +
    (( drcp->iDrcpMode & 0x03 ) << 4 )    +
    (  drcp->lUnitA          ? 0x08 : 0 ) +
    (  lOtherKnown           ? 0x04 : 0 ) +
    (  drcp->lPreferredA     ? 0x02 : 0 ) +
    (  drcp->sysB.lDisabled  ? 0x01 : 0 ) ;

  str_a[1] =
    ( drcp->sysB.lOkay       ? 0x40 : 0 ) +
    ((drcp->sysB.iActivity & 0x03) << 4 ) +
    ( drcp->sysA.lDisabled   ? 0x08 : 0 ) +
    ( drcp->sysA.lOkay       ? 0x04 : 0 ) +
    ((drcp->sysA.iActivity & 0x03) << 0 ) ;

  str_a[2] =
    ( drcp->lFlipAllowed     ? 0x40 : 0 ) +
    ( drcp->sysB.lRelinquish ? 0x20 : 0 ) +
    ( drcp->sysA.lRelinquish ? 0x10 : 0 ) +
    ( drcp->sysB.lWouldUse   ? 0x08 : 0 ) +
    ( drcp->sysA.lWouldUse   ? 0x04 : 0 ) ;

  if( pantsim_c->info.iActualPolar & 0x01 ) str_a[3] |= 0x01;
  if( pantsim_c->info.iActualPolar & 0x02 ) str_a[3] |= 0x02;

  if( pantsim_c->info.isbits & ASF_POLAR )  str_a[3] |= 0x08;

}

/* ------------------------------
 */
static void set_xmt_moving_platform( UINT1 str_a[47] )
{
  SINT4 ivelocity, ialtitude;

  str_a[6] = ( pantsim_c->info.itr_order >> 2 )& 0x7F;
  str_a[7] = ( pantsim_c->info.itr_order >> 9 )& 0x7F;

  str_a[8] = ( pantsim_c->info.iel_order >> 2 )& 0x7F;
  str_a[9] = ( pantsim_c->info.iel_order >> 9 )& 0x7F;

  str_a[10] = ( pantsim_c->info.ipitch >> 2 )& 0x7F;
  str_a[11] = ( pantsim_c->info.ipitch >> 9 )& 0x7F;

  str_a[12] = ( pantsim_c->info.iroll >> 2 )& 0x7F;
  str_a[13] = ( pantsim_c->info.iroll >> 9 )& 0x7F;

  str_a[14] = ( pantsim_c->info.iheading >> 2 )& 0x7F;
  str_a[15] = ( pantsim_c->info.iheading >> 9 )& 0x7F;

  ivelocity = NINT(pantsim_c->info.fpitchv*ANTSIM02_VEL_UNITS);
  str_a[20] = ivelocity & 0x7e;
  str_a[21] = (ivelocity>>7) & 0x7f;

  ivelocity = NINT(pantsim_c->info.frollv*ANTSIM02_VEL_UNITS);
  str_a[22] = ivelocity & 0x7e;
  str_a[23] = (ivelocity>>7) & 0x7f;

  ivelocity = NINT(pantsim_c->info.fheadingv*ANTSIM02_VEL_UNITS);
  str_a[24] = ivelocity & 0x7e;
  str_a[25] = (ivelocity>>7) & 0x7f;

  /* Platform position
   */
  str_a[32] = 0x7f & (pantsim_c->info.ilatitude >> 11);
  str_a[33] = 0x7f & (pantsim_c->info.ilatitude >> 18);
  str_a[34] = 0x7f & (pantsim_c->info.ilatitude >> 25);
  str_a[35] = 0x7f & (pantsim_c->info.ilongitude >> 11);
  str_a[36] = 0x7f & (pantsim_c->info.ilongitude >> 18);
  str_a[37] = 0x7f & (pantsim_c->info.ilongitude >> 25);

  ialtitude = NINT(pantsim_c->info.faltitude);
  if (ialtitude < -8192) ialtitude= -8192;
  if (ialtitude > 8191) ialtitude= 8191;
  str_a[38] = ialtitude & 0x7f;
  str_a[39] = (ialtitude>>7) & 0x7f;

  ivelocity = NINT(100.0 * pantsim_c->info.fvel_east);
  if (ivelocity < -8192) ivelocity= -8192;
  if (ivelocity > 8191) ivelocity= 8191;
  str_a[40] = ivelocity & 0x7e;
  str_a[41] = (ivelocity>>7) & 0x7f;

  ivelocity = NINT(100.0 * pantsim_c->info.fvel_north);
  if (ivelocity < -8192) ivelocity= -8192;
  if (ivelocity > 8191) ivelocity= 8191;
  str_a[42] = ivelocity & 0x7e;
  str_a[43] = (ivelocity>>7) & 0x7f;

  ivelocity = NINT(100.0 * pantsim_c->info.fvel_up);
  if (ivelocity < -8192) ivelocity= -8192;
  if (ivelocity > 8191) ivelocity= 8191;
  str_a[44] = ivelocity & 0x7e;
  str_a[45] = (ivelocity>>7) & 0x7f;
}
      
/* ------------------------------
 * Compose a character string package for the appropriate bite unit
 */
static void compose_bite_packet
( UINT1 str_a[], SINT4* ilength_a, SINT2 iunit_a )
{
  *ilength_a = pantsim_c->bitex.units[iunit_a].iunit_length ;
  memmove( str_a, (void *)&pantsim_c->binfo.Units[iunit_a].sbite_reception[0], *ilength_a );
}
/* ------------------------------
 * Compose a character string package for the appropriate Q-BITE unit
 */
static void compose_qbite_packet
( UINT1 str_a[], SINT4* ilength_a, SINT2 iunit_a )
{
  SINT4 iField;
  *ilength_a = pantsim_c->bitex.units[iunit_a].iunit_length ;

  memset( str_a, 0, *ilength_a );
  str_a[0] = ANT_SYNC_QBITE;
  str_a[(*ilength_a)-1] = 0xFF;
  str_a[1] = pantsim_c->bitex.units[iunit_a].iunit_id ;

  for (iField=0; iField<BITE_NUM_FIELD_DEFS; iField++)
    {
      volatile struct bitex_field_def *pField = &pantsim_c->bitex.fields[iField];

      if ((iunit_a==pField->iunit)&&(pField->ldisabled == FALSE))
	{
	  SINT4 iValue, iByte, iSize, iCount;
	  iValue = pantsim_c->binfo.iQBiteValues[iField];
	  iByte  = pField->ibyte;
	  iSize  = pField->ibit;
	  for (iCount=0; iCount<iSize; iCount++)
	    {
	      str_a[iByte] = 0x7f & iValue;
	      iByte++; iValue = iValue>>7;
	    }
	}
    }
}

/* ------------------------------
 * Compose a character string package for the appropriate bite unit
 * History:
 *     jmh   19 Jul 1992   Init
 */
static void compose_time_packet
(UINT1 str_a[],    /* Packet returned */
 SINT4* ilength_a /* Number of bytes in the packet */
 )
{
  struct timeval TVal;
  time_t phase_time;
  struct tm BrokenTime;
  SINT4 iyear;
#define TIME_DRIFT_PERIOD (1800)
#define TIME_DRIFT_PEAK   (10)

  gettimeofday( &TVal, NULL );

  /* Shift time by a triangular version of sine wave. */
  phase_time = TVal.tv_sec % TIME_DRIFT_PERIOD;
  if (phase_time < 0) phase_time += TIME_DRIFT_PERIOD;
  if (phase_time<(TIME_DRIFT_PERIOD/4))
    {
      TVal.tv_sec += (4*TIME_DRIFT_PEAK*phase_time)/TIME_DRIFT_PERIOD;
    }
  else if (phase_time<((3*TIME_DRIFT_PERIOD)/4))
    {
      TVal.tv_sec += (4*TIME_DRIFT_PEAK*((TIME_DRIFT_PERIOD/2)-phase_time))/TIME_DRIFT_PERIOD;
    }
  else
    {
      TVal.tv_sec += (4*TIME_DRIFT_PEAK*(phase_time-TIME_DRIFT_PERIOD))/TIME_DRIFT_PERIOD;
    }
  /* adding 10 seconds to make the simulation to make it more
   * interesting.
   */
  /*  current_time += 10; */
  if (pantsim_c->setup.lTimePacketUtc)
    gmtime_r( &TVal.tv_sec, &BrokenTime );
  else
    localtime_r( &TVal.tv_sec, &BrokenTime );

  iyear = BrokenTime.tm_year + 1900;

  str_a[0]  = 0xb0;
  str_a[1]  = ( iyear  )& 0x7f;
  str_a[2]  = ( iyear >> 7 ) & 0x7f;
  str_a[3]  = ( BrokenTime.tm_mon+1 )& 0x7F;
  str_a[4]  = ( BrokenTime.tm_mday )& 0x7F;
  str_a[5]  = ( BrokenTime.tm_hour )& 0x7F;
  str_a[6]  = ( BrokenTime.tm_min )& 0x7F;
  str_a[7]  = ( BrokenTime.tm_sec )& 0x7F;
  str_a[8]  = (TVal.tv_usec/10000) & 0x7F;
  str_a[9]  = 0;
  str_a[10] = 0xff;
  *ilength_a = 11;
}

/* ------------------------------
 * Run another "tick" of internal antenna simulation
 * History:
 *     jmh    4 May 1992   Added velocity difference clipping to prevent oscillation.
 *     jmh    9 Sep 1991   Removed antenna_info argument
 *     jmh   17 Apr 1991   Removed time tick argument
 */
static void ant_sim_tick_(void)
{
#define VTHRESH_P (0.02)
  UINT4 ibtime, iage;
  FLT4  fTick, fscale ;
  FLT4 fearth_vector[3], fship_vector[3], fposition[3], fmatrix[3][3];
  FLT4 fel_rad, faz_rad, fvelocity[3];

  static struct antAxisModel modelAZ, modelEL ;

  /* -----------------------------------
   * Get current time, and update it in internal storage:
   */
  getbtime( &ibtime );
  fTick = ((FLT4)btimeGrace(ibtime - pantsim_c->info.isim_time)) / 1000. ;
  if (fTick < 0.) fTick = 0.;
  pantsim_c->info.isim_time = ibtime;

  /* Reset has the effect of taking simulator out of shutdown state
   */
  if( pantsim_c->info.lwe_should_reset ) {
    pantsim_c->info.lwe_should_reset = FALSE;
    pantsim_c->info.isbits &= ~ASF_SHUTDOWN;
  }

  /* Simulate the Azimuth axis
   */
  modelAZ.iAntMode      = pantsim_c->info.iazmode ;
  modelAZ.iRequestedPos = pantsim_c->info.idaz ;
  modelAZ.iRequestedVel = iBin2FromFDeg( pantsim_c->info.fdazv ) ;
  modelAZ.iVelLimit     = iBin2FromFDeg( pantsim_c->setup.fazvelmax ) ;
  modelAZ.lOverTheTop   = FALSE ;
  modelAZ.lUseLimits    = FALSE ;

  antAxisModelUpdate( &modelAZ, fTick ) ;

  pantsim_c->info.iaz   = ( modelAZ.iPos32 + 0x8000 ) >> 16 ;
  pantsim_c->info.fazv  = fDegFromBin4( modelAZ.iVel32 ) ;

  /* Simulate the Elevation axis
   */
  modelEL.iAntMode      = pantsim_c->info.ielmode ;
  modelEL.iRequestedPos = pantsim_c->info.idel ;
  modelEL.iRequestedVel = iBin2FromFDeg( pantsim_c->info.fdelv ) ;
  modelEL.iVelLimit     = iBin2FromFDeg( pantsim_c->setup.felvelmax ) ;
  modelEL.lOverTheTop   = TRUE ;
  modelEL.lUseLimits    = TRUE ;
  modelEL.iPosLimits[0] = pantsim_c->setup.ielmin ;
  modelEL.iPosLimits[1] = pantsim_c->setup.ielmax ;

  antAxisModelUpdate( &modelEL, fTick ) ;

  pantsim_c->info.iel   = ( modelEL.iPos32 + 0x8000 ) >> 16 ;
  pantsim_c->info.felv  = fDegFromBin4( modelEL.iVel32 ) ;

  /* Pulse width
   */
  if (pantsim_c->info.iDesiredPwbw != ANT_PWBW_UNCHANGED)
    pantsim_c->info.iActualPwbw = pantsim_c->info.iDesiredPwbw;

  /* Polarization. */
  if (pantsim_c->info.iDesiredPolar != POL_UNCHANGED)
    {
      static UINT4 iBTimeChange=0;
      if (pantsim_c->info.iActualPolar != pantsim_c->info.iDesiredPolar)
	iBTimeChange = ibtime;

      /* Set invalid for 2 seconds after each change */
      if ( btimeGrace(ibtime - iBTimeChange)<2000 )
	pantsim_c->info.isbits &= ~ASF_POLAR;
      else
	pantsim_c->info.isbits |=  ASF_POLAR;

      pantsim_c->info.iActualPolar = pantsim_c->info.iDesiredPolar;
    }
  else
    {
      /* Set valid before the first command */
      pantsim_c->info.isbits |=  ASF_POLAR;
    }

  /* Signal generator value
   */
  pantsim_c->info.isiggen_value = pantsim_c->info.idsiggen_value;
 
  /* Signal generator control bits
   */
  if (pantsim_c->info.icbits&ACF_SIGGEN) 
    pantsim_c->info.isbits |= ASF_SIGGEN;
  else
    pantsim_c->info.isbits &= ~ASF_SIGGEN;

  if (pantsim_c->info.icbits&ACF_SIGCW) 
    pantsim_c->info.isbits |=  ASF_SIGCW;
  else
    pantsim_c->info.isbits &= ~ASF_SIGCW;


  /* Radiate control.  This can only be controlled if T/R power is on.
   * The simulator requires that T/R power control is available.
   */
  if (pantsim_c->info.isbits & ASF_TRPOWER)
    {
      /* When the T/R power is on, and the standby is true, and
       * radiation is off, then the radiation can be controlled by the
       * radiate control bit.  Also if T/R power is on, and radiation
       * is on, it can be controlled.  The standby bit is only
       * reliably defined when T/R power is on and Radiate is off.
       */
      if ( (pantsim_c->info.isbits&ASF_RADIATE) || 
	   (pantsim_c->info.isbits&ASF_STANDBY) )
	{
	  if (pantsim_c->info.icbits&ACF_RADIATE)
	    pantsim_c->info.isbits |=  ASF_RADIATE;
	  else
	    pantsim_c->info.isbits &= ~ASF_RADIATE;
	}
      else
	{
	  pantsim_c->info.isbits &= ~ASF_RADIATE;
	}
    }
 
  /* Servo power control
   */
  if( pantsim_c->info.icbits & ACF_SERVO ) 
    pantsim_c->info.isbits |=  ASF_SERVO;
  else
    pantsim_c->info.isbits &= ~ASF_SERVO;
 
  /* T/R power control
   */
  if( pantsim_c->info.icbits & ACF_TRPOWER )
    {
      /* If T/R power has been on, and we are not standby yet, then
       * check to see if the warmup time has passed.  If T/R power is
       * just being turned on, then save away the current time and set
       * the standby bit off, and set the radiate bit off.
       */
      if (pantsim_c->info.isbits&ASF_TRPOWER)
	{
	  if (!(pantsim_c->info.isbits&ASF_STANDBY))
	    {
	      if ( btimeGrace(ibtime - pantsim_c->info.itr_on_time) > 60000 )
		pantsim_c->info.isbits |= ASF_STANDBY;
	    }
	}
      else
	{
	  pantsim_c->info.isbits &= ~(ASF_STANDBY|ASF_RADIATE);
	  pantsim_c->info.itr_on_time = ibtime;
	}
      pantsim_c->info.isbits |= ASF_TRPOWER;
    }
  else
    pantsim_c->info.isbits &= ~ASF_TRPOWER;
     
  /* Simulate moving platform variables only if this is such a system
   */
  if (0 != lantsim_moving_platform() )
    {
      /* Implement the base motion of the platform, by integrating the
       * velocity.  Note that to convert distance to lat/long binary
       * angle, we must multiply by 2 to the 31 over pi times the
       * earth radius.  (the 1000 is to convert kilometers to meters.
       */
      fscale = 2147483648.0 / ( 1000.0 * FPI * FEARTH_MEAN_RADIUS );
      pantsim_c->info.ilatitude  += NINT(fscale * pantsim_c->info.fvel_north * fTick);

      /* Clip latitude just below the poles
       */
      if ((SINT4)pantsim_c->info.ilatitude > (SINT4)0x3fff0000 )
	pantsim_c->info.ilatitude = (UINT4)0x3fff0000;
      if ((SINT4)pantsim_c->info.ilatitude < -(SINT4)0x3fff0000 )
	pantsim_c->info.ilatitude = (UINT4)-(SINT4)0x3fff0000;

      pantsim_c->info.ilongitude += NINT(fscale * pantsim_c->info.fvel_east * fTick);
      pantsim_c->info.faltitude  += (pantsim_c->info.fvel_up * fTick);

      /* Clip the altitude at +30000 to -1000 meters
       */
      if (pantsim_c->info.faltitude > 30000.0) pantsim_c->info.faltitude = 30000.0;
      if (pantsim_c->info.faltitude < -1000.0) pantsim_c->info.faltitude = -1000.0;

      /* Compute the new simulated roll, pitch, and heading, and the
       * velocities.  The rock period is 60 seconds.
       */
      pantsim_c->info.fsim_rock_phase += 2.0 * FPI * fTick/60.0;
      if (pantsim_c->info.fsim_rock_phase >= 2.0*FPI)
	pantsim_c->info.fsim_rock_phase -= 2.0*FPI;

      fposition[0] = pantsim_c->info.fsim_tilt_pitch +
	pantsim_c->info.fsim_rock_pitch * sin(pantsim_c->info.fsim_rock_phase);
      fvelocity[0] = (2.0*FPI/60.0) *
	pantsim_c->info.fsim_rock_pitch * cos(pantsim_c->info.fsim_rock_phase);
      fposition[1] = pantsim_c->info.fsim_tilt_roll +
	pantsim_c->info.fsim_rock_roll * sin(pantsim_c->info.fsim_rock_phase+0.5);
      fvelocity[1] = (2.0*FPI/60.0) *
	pantsim_c->info.fsim_rock_roll * cos(pantsim_c->info.fsim_rock_phase+0.5);
      fposition[2] = pantsim_c->info.fsim_tilt_heading +
	pantsim_c->info.fsim_rock_heading * sin(pantsim_c->info.fsim_rock_phase+1.0);
      fvelocity[2] = (2.0*FPI/60.0) *
	pantsim_c->info.fsim_rock_heading * cos(pantsim_c->info.fsim_rock_phase+1.0);

      pantsim_c->info.ipitch   = iBin2FromFDeg( fposition[0] );
      pantsim_c->info.iroll    = iBin2FromFDeg( fposition[1] );
      pantsim_c->info.iheading = iBin2FromFDeg( fposition[2] );

      pantsim_c->info.fpitchv   = fvelocity[0];
      pantsim_c->info.frollv    = fvelocity[1];
      pantsim_c->info.fheadingv = fvelocity[2];

      /* Compute the train-order and the elevation-order from the
       * other known angles.  First compute a unit vector in the
       * direction of the earth relative number, then rotate it, then
       * convert back to polar.
       */
      faz_rad = fRadianFromBin2((BIN2)pantsim_c->info.iaz);
      fel_rad = fRadianFromBin2((BIN2)pantsim_c->info.iel);

      fposition[0] = fDegFromBin2( pantsim_c->info.ipitch );
      fposition[1] = fDegFromBin2( pantsim_c->info.iroll );
      fposition[2] = fDegFromBin2( pantsim_c->info.iheading );

      fearth_vector[0] = sin(faz_rad) * cos(fel_rad);
      fearth_vector[1] = cos(faz_rad) * cos(fel_rad);
      fearth_vector[2] =                sin(fel_rad);
      position_matrix_compute( fmatrix, fposition );

      matrix_mul3( fship_vector, fmatrix, fearth_vector );

      faz_rad = -atan2( fship_vector[1], fship_vector[0] );
      fel_rad =  asin( fship_vector[2] );

      pantsim_c->info.itr_order = iBin2FromRadian( faz_rad );
      pantsim_c->info.iel_order = iBin2FromRadian( fel_rad );
    }

  /* Request that time is sent every 60 seconds.  Note that "iage" is
   * unsigned in order to handle the start up problem.
   */
  iage = btimeGrace( ibtime - pantsim_c->info.itime_sent_time ) ;
  if (iage > 60000) pantsim_c->info.lsend_time = TRUE;

  /* Simulate a simple dual RCP system
   */
  {
    volatile struct drcp_status *drcp = &pantsim_c->info.drcp ;
    
    /* Setup the static state of the dual system.  Make it look like
     * this is the "A" unit, that each unit is functioning according
     * to its respective NST status bit, and that the units are
     * talking and not disabled.
     */
    drcp->lRunningDual     = TRUE ;
    drcp->lUnitA           = TRUE ;
    drcp->lPreferredA      = TRUE ;
    drcp->lFlipAllowed     = TRUE ;

    drcp->sysA.lKnown      = TRUE ;
    drcp->sysA.lDisabled   = FALSE ;
    drcp->sysA.lOkay       = BTEST( pantsim_c->info.icurrent_nst_bits, 3 ) ;
    drcp->sysA.lRelinquish = FALSE ;
    drcp->sysA.lWouldUse   = TRUE ;

    drcp->sysB.lKnown      = TRUE ;
    drcp->sysB.lDisabled   = FALSE ;
    drcp->sysB.lOkay       = BTEST( pantsim_c->info.icurrent_nst_bits, 2 ) ;
    drcp->sysB.lRelinquish = FALSE ;
    drcp->sysB.lWouldUse   = TRUE ;

    /* Grab any non-void mode status that has arrived, and switch over
     * to use it.
     */
    if( pantsim_c->info.iDrcpModeReq != DRCP_MODE_VOID ) {
      drcp->iDrcpMode = pantsim_c->info.iDrcpModeReq ;
      pantsim_c->info.iDrcpModeReq = DRCP_MODE_VOID ;
    }

    {
      UINT1 lActiveA, lActiveB, lwarmup ;
      static UINT1 lPrevActiveA, lPrevActiveB ;
      static UINT4 warmupBtime ;

      /* If "A" or "B" are selected, then make those systems active
       * right away if possible.  For "Auto", toggle back and forth on
       * our own.  But in all cases, never elect to use a system that
       * is not okay.
       */
      switch( drcp->iDrcpMode ) {
      default:
	lActiveA = FALSE ;
	lActiveB = FALSE ;
	break ;
      case DRCP_MODE_A:
	lActiveA = drcp->sysA.lOkay ;
	lActiveB = FALSE ;
	break ;
      case DRCP_MODE_B:
	lActiveA = FALSE ;
	lActiveB = drcp->sysB.lOkay ;
	break ;
      case DRCP_MODE_AUTO:
	{
	  UINT1 lUnitA = ((ibtime % 60000) < 30000) ;
	  lActiveA =     lUnitA   && drcp->sysA.lOkay ;
	  lActiveB = ( ! lUnitA ) && drcp->sysB.lOkay ;
	}
	break ;
      }
      /* Simulate a brief warmup interval whenever either system first
       * becomes active.
       */
      if( (lActiveA && ! lPrevActiveA) ||
	  (lActiveB && ! lPrevActiveB ) ) warmupBtime = ibtime ;
      lwarmup = ( btimeGrace(ibtime - warmupBtime) < 3000 ) ;

      if( ! lActiveA ) drcp->sysA.iActivity = DRCP_ACT_INACTIVE ;
      else
	drcp->sysA.iActivity = lwarmup ? DRCP_ACT_WARMUP : DRCP_ACT_ACTIVE ;

      if( ! lActiveB ) drcp->sysB.iActivity = DRCP_ACT_INACTIVE ;
      else
	drcp->sysB.iActivity = lwarmup ? DRCP_ACT_WARMUP : DRCP_ACT_ACTIVE ;

      lPrevActiveA = lActiveA ; lPrevActiveB = lActiveB ;
    }
  }

  /* Update the Q-Bite simulation only once a second or so. */


  if ( btimeGrace(ibtime - pantsim_c->info.iQBiteSimTime) > 1000 )
    {
      SINT4 iField;
      pantsim_c->info.iQBiteSimTime = ibtime;
      /* Only do the first 10 fields, slope increases */
      pantsim_c->binfo.iQBiteValues[0] += 1;
      for (iField=1; iField<10; iField++)
	{
	  pantsim_c->binfo.iQBiteValues[iField] += 10*iField;
	}
    }

}
