/* **********************************************************************
 * *                                                                    *
 * *             Miscellaneous Antenna Driver Routines                  *
 * *                                                                    *
 * **********************************************************************
 * File: libs/antenna/ant_misc.c
 *
 *  COPYRIGHT (c) 1989, 1991, 1997, 1998, 2001, 2002, 2003, 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>
#include <string.h>

#include "sigtypes.h"
#include "setup.h"
#include "headers.h"
#include "ingest.h"
#include "user_lib.h"
#include "antenna_lib.h"

#define ANT_GLOBAL_ALLOCATE 1
#include "ant_global.h"

/* ------------------------------
 * Logical function to test whether it would currently be reasonable to
 * set the system time from the received time information that we have
 * so far.
 */
#define MAX_TIME_ADJ (7 * 1000 * 86400) /* Thats one week max adjustment */
UINT2 lokay_to_set_time_int
(volatile struct antenna_info* pant_a)
{
  UINT2 lokay = FALSE ;

  /* A packet must have been received already.
   */
  if( pant_a->ltime_packet_received == TRUE ) {

    /* The last packet must not have already been used to set the
     * system time.  The reason for this is that the local time of
     * arrival of the last packet is invalid after the system time
     * becomes reset.
     */
    if( pant_a->ltime_set_from_packet == FALSE ) {

      /* Check the ITIME_ERROR field; it is correctly clipped to the
       * (approx) 24 day wraparound limit of milliseconds in a UINT4.
       * Do not make any adjustments greater than the limit.
       */
      if((pant_a->itime_error > -MAX_TIME_ADJ) &&
	 (pant_a->itime_error <  MAX_TIME_ADJ)  ) {

	/* Make sure that the time record itself was received sometime
	 * within the last hour.
	 */
	UINT4 ibtime ; SINT4 iage ;
	getbtime( &ibtime ) ; iage = btimeGrace(ibtime - pant_a->itime_btime) ;
	if( (iage >= 0) && (iage < 3600000) ) {
	  lokay = TRUE ;
	}
      }
    }
  }
  return( lokay ) ;
}

/* ------------------------------
 * Real function which bounds its second argument to have an absolute value
 * less than or equal to the first argument. 
 * entry parameters: 
 *     fmax_a - the limit on absolute value of fval_a
 *     fval_a - the number to be limited.
 */ 
FLT4 fbound( FLT4 fmax_a, FLT4 fval_a ) 
{     
  FLT4 freturn;

  freturn = fval_a ;
  if( fval_a >  fmax_a ) freturn =  fmax_a;
  if( fval_a < -fmax_a ) freturn = -fmax_a;
      
  return freturn;
}
      
/* ------------------------------
 * Compute the terms of the rotation velocity matrix.  Note that terms
 * which will be multiplied by zero are not computed.
 */
void velocity_matrix_compute
(FLT4 fmatrix_a[3][3],
 FLT4 frotation_vel[3],  /* Order pitch, roll, heading in degrees/second */
 FLT4 frotation_pos[3],  /* Order pitch, roll, heading in degrees */
 FLT4 fplatform_offset[3] /* Order forward, port, over, in meters */
 )
{
  FLT4 fsin_pit, fcos_pit, fsin_rol, fcos_rol, fsin_hed, fcos_hed;
  FLT4 fpit_vel, frol_vel, fhed_vel;

  /* Compute all the sines and cosines required
   */
  fsin_pit = sin( (FPI_P/180.0) * frotation_pos[0] );
  fcos_pit = cos( (FPI_P/180.0) * frotation_pos[0] );
  fsin_rol = sin( (FPI_P/180.0) * frotation_pos[1] );
  fcos_rol = cos( (FPI_P/180.0) * frotation_pos[1] );
  fsin_hed = sin( (FPI_P/180.0) * (frotation_pos[2]-90.0) );
  fcos_hed = cos( (FPI_P/180.0) * (frotation_pos[2]-90.0) );
  fpit_vel = (FPI_P/180.0) * frotation_vel[0];
  frol_vel = (FPI_P/180.0) * frotation_vel[1];
  fhed_vel = (FPI_P/180.0) * frotation_vel[2];
  
  if (0.0 != fplatform_offset[0] )
    {
      fmatrix_a[0][0] =
	(-fpit_vel*fsin_pit*fcos_hed) + (-fhed_vel*fcos_pit*fsin_hed);
      fmatrix_a[1][0] =
	( fpit_vel*fsin_pit*fsin_hed) + (-fhed_vel*fcos_pit*fcos_hed);
      fmatrix_a[2][0] = 
	(-fpit_vel*fcos_pit);
    }
  else
    {
      fmatrix_a[0][0] = 0.0;
      fmatrix_a[1][0] = 0.0;
      fmatrix_a[2][0] = 0.0;
    }
    
  if (0.0 != fplatform_offset[1] )
    {
      fmatrix_a[0][1] =
	(-frol_vel*fsin_rol*fsin_hed) +
	(fhed_vel*fcos_rol*fcos_hed) +
	(-frol_vel*fcos_rol*fsin_pit*fcos_hed) +
	(-fpit_vel*fsin_rol*fcos_pit*fcos_hed) +
	(fhed_vel*fsin_rol*fsin_pit*fsin_hed);
      fmatrix_a[1][1] =
	(-frol_vel*fsin_rol*fcos_hed) +
	(-fhed_vel*fcos_rol*fsin_hed) +
	(frol_vel*fcos_rol*fsin_pit*fsin_hed) +
	(fpit_vel*fsin_rol*fcos_pit*fsin_hed) +
	(fhed_vel*fsin_rol*fsin_pit*fcos_hed);

      fmatrix_a[2][1] = 
	(-frol_vel*fcos_rol*fcos_pit) +
	(fpit_vel*fsin_rol*fsin_pit);
    }
  else
    {
      fmatrix_a[0][1] = 0.0;
      fmatrix_a[1][1] = 0.0;
      fmatrix_a[2][1] = 0.0;
    }

  if (0.0 != fplatform_offset[2] )
    {
      fmatrix_a[0][2] =
	(frol_vel*fcos_rol*fsin_hed) +
	(fhed_vel*fsin_rol*fcos_hed) +
	(-frol_vel*fsin_rol*fsin_pit*fcos_hed) +
	(fpit_vel*fcos_rol*fcos_pit*fcos_hed) +
	(-fhed_vel*fcos_rol*fsin_pit*fsin_hed);
      fmatrix_a[1][2] =
	(frol_vel*fcos_rol*fcos_hed) +
	(-fhed_vel*fsin_rol*fsin_hed) +
	(frol_vel*fsin_rol*fsin_pit*fsin_hed) +
	(-fpit_vel*fcos_rol*fcos_pit*fsin_hed) +
	(-fhed_vel*fcos_rol*fsin_pit*fcos_hed);

      fmatrix_a[2][2] = 
	(-frol_vel*fsin_rol*fcos_pit) +
	(-fpit_vel*fcos_rol*fsin_pit);
    }
  else
    {
      fmatrix_a[0][2] = 0.0;
      fmatrix_a[1][2] = 0.0;
      fmatrix_a[2][2] = 0.0;
    }
}

/* ------------------------------
 * Compute the terms of the rotation position matrix.  This is the matrix that
 * will convert the earth relative vector to a ship relative one.
 */
void position_matrix_compute
(FLT4 fmatrix_a[3][3],
 FLT4 frotation_pos[3]   /* Order pitch, roll, heading in degrees */
 )
{
  FLT4 fsin_pit, fcos_pit, fsin_rol, fcos_rol, fsin_the, fcos_the;

  /* Compute all the sines and cosines required
   */
  fsin_pit = sin( (FPI_P/180.0) * frotation_pos[0] );
  fcos_pit = cos( (FPI_P/180.0) * frotation_pos[0] );
  fsin_rol = sin( (FPI_P/180.0) * frotation_pos[1] );
  fcos_rol = cos( (FPI_P/180.0) * frotation_pos[1] );
  fsin_the = sin( (FPI_P/180.0) * (frotation_pos[2]-90.0) );
  fcos_the = cos( (FPI_P/180.0) * (frotation_pos[2]-90.0) );
  
  /* This is A inverse in Rich's analysis
     fmatrix_a[0][0] = ( fcos_pit*fcos_the);
     fmatrix_a[1][0] = (-fcos_pit*fsin_the);
     fmatrix_a[2][0] = (-fsin_pit);
    
     fmatrix_a[0][1] = (-fsin_rol*fsin_pit*fcos_the) + (fcos_rol*fsin_the);
     fmatrix_a[1][1] = ( fsin_rol*fsin_pit*fsin_the) + (fcos_rol*fcos_the);
     fmatrix_a[2][1] = (-fsin_rol*fcos_pit);

     fmatrix_a[0][2] = ( fcos_rol*fsin_pit*fcos_the) + (fsin_rol*fsin_the);
     fmatrix_a[1][2] = (-fcos_rol*fsin_pit*fsin_the) + (fsin_rol*fcos_the);
     fmatrix_a[2][2] = ( fcos_rol*fcos_pit);
  */

  /* This is the A matrix in Rich's analysis
   */
  fmatrix_a[0][0] = ( fcos_pit*fcos_the);
  fmatrix_a[1][0] = ( fcos_rol*fsin_the) + (-fsin_rol*fsin_pit*fsin_the);
  fmatrix_a[2][0] = ( fsin_rol*fsin_the) + ( fcos_rol*fsin_pit*fcos_the);
    
  fmatrix_a[0][1] = (-fcos_pit*fsin_the);
  fmatrix_a[1][1] = ( fcos_rol*fcos_the) + ( fsin_rol*fsin_pit*fsin_the);
  fmatrix_a[2][1] = ( fsin_rol*fcos_the) + (-fcos_rol*fsin_pit*fsin_the);

  fmatrix_a[0][2] = (-fsin_pit);
  fmatrix_a[1][2] = (-fsin_rol*fcos_pit);
  fmatrix_a[2][2] = ( fcos_rol*fcos_pit);
}

void  matrix_mul3
(FLT4 fvelocity_a[3],     /* resulting velocity */
 FLT4 fmatrix_a[3][3],
 FLT4 fplatform_offset_a[3]
 )
{
  fvelocity_a[0] =
    fmatrix_a[0][0]*fplatform_offset_a[0] +
    fmatrix_a[0][1]*fplatform_offset_a[1] +
    fmatrix_a[0][2]*fplatform_offset_a[2];

  fvelocity_a[1] =
    fmatrix_a[1][0]*fplatform_offset_a[0] +
    fmatrix_a[1][1]*fplatform_offset_a[1] +
    fmatrix_a[1][2]*fplatform_offset_a[2];

  fvelocity_a[2] =
    fmatrix_a[2][0]*fplatform_offset_a[0] +
    fmatrix_a[2][1]*fplatform_offset_a[1] +
    fmatrix_a[2][2]*fplatform_offset_a[2];
}


/* ------------------------------
 * Return text description of a dual system mode
 */
const char *AntGetDrcpModeString( UINT2 iDrcpMode_a )
{
  static char smode0[] = { "Void"  } ;
  static char smode1[] = { "A-Sys" } ;
  static char smode2[] = { "B-Sys" } ;
  static char smode3[] = { "Auto"  } ;
  static char smode4[] = { "???"   } ;
  switch( iDrcpMode_a ) {
  case DRCP_MODE_VOID: return( smode0 ) ; break ;
  case DRCP_MODE_A:    return( smode1 ) ; break ;
  case DRCP_MODE_B:    return( smode2 ) ; break ;
  case DRCP_MODE_AUTO: return( smode3 ) ; break ;
  default:             return( smode4 ) ; break ;
  }
}

/* ------------------------------
 * Return antenna state flag and string.
 */
UINT4 AntGetStateString
( char  sName_a[16] )
{
  UINT4 iStatus;
  UINT4 icmd, istat;

  if( !antGlobal_c->setup.lAntAvailable ) 
    {
      iStatus = DEV_NULL;
    }
  else
    {
      UINT4 iAgePkt, iAgeAng, iAgeInu ; UINT1 iMask;
      
      iStatus = DEV_OK;
      iMask = AntGetAges( &iAgePkt, &iAgeAng, &iAgeInu );

      /* Skip check entirely if no data is expected */
      if (iMask)
	{
	  UINT1 lAllFaults=TRUE;
	  char sFaultTypes[20];

	  sFaultTypes[0]=0;

	  if (iMask & ANT_EXPECT_PKT)
	    {
	      if( iAgePkt > ANT_TIMEOUT_MS )
		strcat( sFaultTypes, " PKT" );
	      else
		lAllFaults=FALSE;
	    }
	  
	  if (iMask & ANT_EXPECT_ANG) 
	    {
	      if( iAgeAng > ANT_TIMEOUT_MS )
		strcat( sFaultTypes, " ANG" );
	      else
		lAllFaults=FALSE;
	    }
	  
	  if (iMask & ANT_EXPECT_INU)
	    {
	      if( iAgeInu > ANT_TIMEOUT_MS )
		strcat( sFaultTypes, " INU" );
	      else
		lAllFaults=FALSE;
	    }
  
	  if (lAllFaults)
	    {
	      /* If the RCP has timed out, then we can return now */
	      iStatus = DEV_CRITICAL;
	      strcpy( sName_a, "DEAD" );
	      return iStatus;
	    }
	  else if (sFaultTypes[0])
	    {
	      /* If the RCP has timed out, then we can return now */
	      iStatus = DEV_CRITICAL;
	      strcpy( sName_a, "DEAD" );
	      strcat( sName_a, sFaultTypes );
	      return iStatus;
	    }
	}
    }
      
  icmd  = iant_cmd();
  istat = iant_stat();
  
  if ( istat & ASF_SHUTDOWN )
    {
      UINT4 iCritical;
      iCritical = iant_stat_critical();
      strcpy( sName_a, "Shutdown" );
      if (iCritical & ASF_SHUTDOWN)
	iStatus = DEV_CRITICAL;
      else
	iStatus = DEV_FAULT;

    }
  else if ( istat & ASF_LOCAL )
    strcpy( sName_a, "Local" );
  else
    {
      if ( icmd & ACF_IRIS )
	strcpy( sName_a, "IRIS" );
      else
	strcpy( sName_a, "Computer" );
    }
  return iStatus;
}

/* ------------------------------
 * Return flag indicating that the antlib is shutting down.
 * This will allow application programs to exit, if they
 * want.
 */
UINT1 lAntLibShutDown(void)
{
  return antGlobal_c->info.lShuttingDown;
}

/* ------------------------------
 * Return a flag indicating that we are the controlling host.
 */
UINT1 lAntControllingHost(void)
{
  return (0 != antGlobal_c->setup.lControllingHost);
}
