/* **********************************************************************
 * *	                                                                *
 * *              Antenna Position and Motion Routines                  *
 * *                                                                    *
 * **********************************************************************
 * File: libs/antenna/ant_motion.c
 *
 *     COPYRIGHT (c) 1989, 1991, 1997, 1998, 1999, 2001, 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 <stdio.h>

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

#include "ant_global.h"

/* ------------------------------
 * Get AZ and EL positions.  The RTVEC modeling package is used to
 * fill in the current positions based on the antenna history.
 *
 * Exit Parameters
 *   iaz, iel - Antenna binary angle positions
 */
void ant_get_azel_pos( volatile BIN2* iaz_a, volatile BIN2* iel_a )
{
  UINT4 iBtimeNow ; getbtime( &iBtimeNow ) ;
  ant_get_azel_pos_btime( iaz_a, iel_a, iBtimeNow ) ;
}

void ant_get_azel_pos_btime
( volatile BIN2* iaz_a, volatile BIN2* iel_a, UINT4 iBtime_a )
{
  /* Lookup the data point and return the position components
   */
  struct rtpedx rtpedx ; ant_get_rtpedx( &rtpedx, iBtime_a ) ;

  *iaz_a = iBin2FromFDeg( rtpedx.faz ) ;
  *iel_a = iBin2FromFDeg( rtpedx.fel ) ;

  antGlobal_c->info.iactive += 1 ;  
}

/* ------------------------------
 * Get AZ and EL velocities.  Very similar to above routines for
 * position.
 *
 * Exit Parameters
 *   fazv_a, felv_a - Velocities on each axis in degrees/sec
 */
void ant_get_azel_vel( volatile FLT4* fazv_a, volatile FLT4* felv_a )
{
  UINT4 iBtimeNow ; getbtime( &iBtimeNow ) ;
  ant_get_azel_vel_btime( fazv_a, felv_a, iBtimeNow ) ;
}

void ant_get_azel_vel_btime
( volatile FLT4* fazv_a, volatile FLT4* felv_a, UINT4 iBtime_a )
{
  /* Lookup the data point and return the velocity components
   */
  struct rtpedx rtpedx ; ant_get_rtpedx( &rtpedx, iBtime_a ) ;

  *fazv_a = rtpedx.fazv ;
  *felv_a = rtpedx.felv ;

  antGlobal_c->info.iactive += 1 ;  
}

/* ------------------------------
 * Get AZ and EL position error, i.e., the difference between the actual and
 * desired antenna positions.  If a position servo is not being implemented
 * then the position error is zero.
 *
 * Exit Parameters
 *   iaz, iel - Antenna binary angle positions
 */
void ant_get_azel_err( volatile UINT2* iazerr_a, volatile UINT2* ielerr_a )
{
  UINT2 iaz, iel, iazerr=0, ielerr=0 ;

  antGlobal_c->info.iactive += 1 ;
  ant_get_azel_pos( &iaz, &iel ) ;

  if( antGlobal_c->info.iazmode == ANTMODE_POS ) {
    iazerr = iaz - antGlobal_c->info.idaz ;
  }

  /* Elevation error must be computed specially because the direction
   * is always "over the top".
   */
  if( antGlobal_c->info.ielmode == ANTMODE_POS ) {
    SINT4 ierr = ( ((SINT4)(UINT2)(                  iel + 0x4000)) -
		   ((SINT4)(UINT2)(antGlobal_c->info.idel + 0x4000)) ) ;
    if( ierr >  32767 ) ierr =  32767 ;
    if( ierr < -32767 ) ierr = -32767 ;
    ielerr = (SINT2)ierr ;
  }
  *iazerr_a = iazerr ;
  *ielerr_a = ielerr ;
}

/* ------------------------------
 * Get AZ command
 *
 * Exit Parameters
 *   imode_a - Mode for azimuth
 *   idaz_a  - Desired azimuth
 *   fdazv_a - Desired azimuth velocity
 */
void ant_get_az_cmd
( volatile UINT2* imode_a, volatile BIN2* idaz_a, volatile FLT4* fdazv_a )
{
  antGlobal_c->info.iactive += 1 ;
  *imode_a = antGlobal_c->info.iazmode ;
  *idaz_a  = antGlobal_c->info.idaz ;
  *fdazv_a = antGlobal_c->info.fdazv ;
}

/* ------------------------------
 * Get AZ speed limits
 *
 * Exit Parameters
 *   fvel_limits_a - Minimum and maximum azimuth velocity
 */
void ant_get_az_limits( FLT4 fvel_limits_a[2] )
{
  antGlobal_c->info.iactive += 1 ;
  fvel_limits_a[0] = -antGlobal_c->setup.fazvelmax ;
  fvel_limits_a[1] =  antGlobal_c->setup.fazvelmax ;
}

/* ------------------------------
 * Get AZ speed limits
 *
 * Exit Parameters
 *   fvel_limits_a - Minimum and maximum elevation velocity
 *   fpos_limits_a = Minimum and maximum elevation positions (signed degrees)
 */
void ant_get_el_limits
(FLT4 fvel_limits_a[2],
 FLT4 fpos_limits_a[2] )
{
  antGlobal_c->info.iactive += 1 ;
  fvel_limits_a[0] = -antGlobal_c->setup.felvelmax ;
  fvel_limits_a[1] =  antGlobal_c->setup.felvelmax ;

  fpos_limits_a[0] = fElDegFromBin2( antGlobal_c->setup.ielmin ) ;
  fpos_limits_a[1] = fElDegFromBin2( antGlobal_c->setup.ielmax ) ;
}

/* ------------------------------
 * Get EL command
 *
 * Exit Parameters
 *   imode_a - Mode for elevation
 *   idel_a  - Desired elevation
 *   fdelv_a - Desired elevation velocity
 */
void ant_get_el_cmd
( volatile UINT2* imode_a, volatile BIN2* idel_a, volatile FLT4* fdelv_a )
{
  antGlobal_c->info.iactive += 1 ;
  *imode_a = antGlobal_c->info.ielmode ;
  *idel_a  = antGlobal_c->info.idel ;
  *fdelv_a = antGlobal_c->info.fdelv ;
}

/* ------------------------------
 * Get realtime pos/vel data and shipboard data for the antenna, while
 * enforcing a maximum extrapolation time limit.  Limit how far ahead
 * we are willing to extrapolate.  When the limit is exceeded, just go
 * back to reporting the most recently received value.  Returns age of
 * last update in ms.
 */
UINT4 ant_get_rtpedx( struct rtpedx *rtpedx_a, UINT4 iBtime_a )
{
  UINT4 iRtvecFlags;
  UINT4 iBtimeLookup = iBtime_a ; 
  SINT4 iBtimeAhead = btimeGrace( iBtime_a - antGlobal_c->info.iBtimeAng ) ;

  if( iBtimeAhead > ANT_MAXEXTRAPMS ) iBtimeLookup = antGlobal_c->info.iBtimeAng ;

  iRtvecFlags = rtvecLookupPoint( &antClient_c.pedx, iBtimeLookup, (void *)rtpedx_a ) ;

  return iRtvecFlags;
}

UINT4 ant_get_rtship( struct rtship *rtship_a, UINT4 iBtime_a )
{
  UINT4 iRtvecFlags;
  UINT4 iBtimeLookup = iBtime_a ; 
  SINT4 iBtimeAhead = btimeGrace( iBtime_a - antGlobal_c->info.iBtimeShip ) ;

  if( iBtimeAhead > ANT_MAXEXTRAPMS ) iBtimeLookup = antGlobal_c->info.iBtimeShip ;

  iRtvecFlags = rtvecLookupPoint( &antClient_c.ship, iBtimeLookup, (void *)rtship_a ) ;
  return iRtvecFlags;
}

/* ------------------------------
 * Get velocity of the pedestal.
 */
void ant_get_pedestal_vel
( FLT4 fvelocity_a[3], const struct rtship *rtship_a )
{
  FLT4 frotation_vel[3], frotation_pos[3], flocal_velocity[3] ;
  FLT4 fplatform_offset[3], fmatrix[3][3] ;

  antGlobal_c->info.iactive += 1 ;

  /* First compute the elements of the velocity matrix.  Note that we
   * need to latch the orientation angles and velocities
   */
  frotation_vel[0] = rtship_a->fpitchv ;
  frotation_vel[1] = rtship_a->frollv ;
  frotation_vel[2] = rtship_a->fheadingv ;

  frotation_pos[0] = rtship_a->fpitch ;
  frotation_pos[1] = rtship_a->froll ;
  frotation_pos[2] = rtship_a->fheading ;

  fplatform_offset[0] = (0.01)*(FLT4)antGlobal_c->setup.iplatform_offset[0] ;
  fplatform_offset[1] = (0.01)*(FLT4)antGlobal_c->setup.iplatform_offset[1] ;
  fplatform_offset[2] = (0.01)*(FLT4)antGlobal_c->setup.iplatform_offset[2] ;

  velocity_matrix_compute( fmatrix, frotation_vel, frotation_pos, fplatform_offset ) ;

  matrix_mul3( flocal_velocity, fmatrix, fplatform_offset ) ;

  fvelocity_a[0] = rtship_a->fvel_east  + flocal_velocity[0] ;
  fvelocity_a[1] = rtship_a->fvel_north + flocal_velocity[1] ;
  fvelocity_a[2] = rtship_a->fvel_up    + flocal_velocity[2] ;
}

/* ------------------------------
 * Return the position/velocity of the inu.
 */
void ant_get_inu_pos( FLT8 fposition_a[3] )
{
  UINT4 iBtimeNow ; struct rtship rtship ;
  getbtime( &iBtimeNow ) ; ant_get_rtship( &rtship, iBtimeNow ) ;

  antGlobal_c->info.iactive += 1 ;

  fposition_a[0] = rtship.flongitude ;
  fposition_a[1] = rtship.flatitude ;
  fposition_a[2] = rtship.faltitude ;
}

void ant_get_inu_vel( FLT4 fvelocity_a[3] )
{
  UINT4 iBtimeNow ; struct rtship rtship ;
  getbtime( &iBtimeNow ) ; ant_get_rtship( &rtship, iBtimeNow ) ;

  antGlobal_c->info.iactive += 1 ;

  fvelocity_a[0] = rtship.fvel_east ;
  fvelocity_a[1] = rtship.fvel_north ;
  fvelocity_a[2] = rtship.fvel_up ;
}

/* ------------------------------
 * Get the offset between the the pedestal and the INU position.
 * Units are in meters.
 */
void ant_get_pedestal_off( FLT4 foffset_a[3] )
{
  antGlobal_c->info.iactive += 1 ;
  foffset_a[0] = (0.01)*(FLT4)antGlobal_c->setup.iplatform_offset[0] ;
  foffset_a[1] = (0.01)*(FLT4)antGlobal_c->setup.iplatform_offset[1] ;
  foffset_a[2] = (0.01)*(FLT4)antGlobal_c->setup.iplatform_offset[2] ;
}

/* ------------------------------
 * Set up a velocity servo at a particular speed on the AZ/EL axis.
 *   fspeed_a  - Desired antenna speed in degrees/second
 */
void   ant_set_az_vel( FLT4 fspeed_a ) { AntSetArg( ANTSET_AZCMD " VEL", fspeed_a ) ; }
void __ant_set_az_vel( FLT4 fspeed_a )
{
  FLT4 fspeed = fbound( antGlobal_c->setup.fazvelmax, fspeed_a ) ;
  antGlobal_c->info.fdazv   = fspeed ;
  antGlobal_c->info.iazmode = ANTMODE_VEL ;
}

void   ant_set_el_vel( FLT4 fspeed_a ) { AntSetArg( ANTSET_ELCMD " VEL", fspeed_a ) ; }
void __ant_set_el_vel( FLT4 fspeed_a )
{
  FLT4 fspeed = fbound( antGlobal_c->setup.felvelmax, fspeed_a ) ;
  antGlobal_c->info.fdelv   = fspeed ;
  antGlobal_c->info.ielmode = ANTMODE_VEL ;
}

/* ------------------------------
 * Set up a position servo at a particular angle on the AZ/EL axis.
 *   iangle_a  - Desired binary angle for antenna 
 */
void   ant_set_az_pos( BIN2 iangle_a ) { AntSetArg( ANTSET_AZCMD " POS",
						    fPDegFromBin2( iangle_a ) ) ; }
void __ant_set_az_pos( BIN2 iangle_a )
{
  antGlobal_c->info.idaz    = iangle_a ;
  antGlobal_c->info.iazmode = ANTMODE_POS ;
}

void   ant_set_el_pos( BIN2 iangle_a ) { AntSetArg( ANTSET_ELCMD " POS",
						    fElDegFromBin2( iangle_a ) ) ; }
void __ant_set_el_pos( BIN2 iangle_a )
{
  BIN2 iangle = iangle_a ;

  /* Check against limits.  Do this correctly on the full sector of
   * valid lower and upper limits.
   */
  if( ! lbin2_in_a_row( antGlobal_c->setup.ielmin, iangle,
			antGlobal_c->setup.ielmax ) ) {
    BIN2 imidpt = 0x8000 ^ (UINT2)
      ( ( ((SINT4)(UINT2)(antGlobal_c->setup.ielmin + 0x4000)) +
	  ((SINT4)(UINT2)(antGlobal_c->setup.ielmax + 0x4000)) ) >> 1 ) ;

    if( lbin2_in_a_row( imidpt, iangle, antGlobal_c->setup.ielmin ) )
      iangle = antGlobal_c->setup.ielmin ;
    else
      iangle = antGlobal_c->setup.ielmax ;
  }
  antGlobal_c->info.idel    = iangle ;
  antGlobal_c->info.ielmode = ANTMODE_POS ;
}

/* ------------------------------
 * Stop antenna gracefully.  If the antenna is placed in local mode
 * and moved around, then dont fight it.
 */
void   ant_stop_az( void ) { AntSet( ANTSET_AZCMD " STOP" ) ; }
void __ant_stop_az( void )
{
  antGlobal_c->info.iazmode = ANTMODE_STOP ;
}
 
void   ant_stop_el( void ) { AntSet( ANTSET_ELCMD " STOP" ) ; }
void __ant_stop_el( )
{
  antGlobal_c->info.ielmode = ANTMODE_STOP ;
}
