/* **********************************************************************
 * *	                                                                *
 * *        Antenna SimulatorPosition and Motion Routines               *
 * *                                                                    *
 * **********************************************************************
 * File: libs/antenna/antsim_motion.c
 *
 *                       COPYRIGHT (c) 1991, 1997, 1999  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 "antenna_int.h"

#include "antsim_global.h"

/* ------------------------------
 * Get AZ and EL velocities
 * History:
 *     jmh   21 Oct 1991   Init
 *
 * Exit Parameters
 *   fazv_a, felv_a - Velocities on each axis in degrees/sec
 */
void antsim_get_azel_vel( FLT4* fazv_a, FLT4* felv_a )
{
  *fazv_a = pantsim_c->info.fazv ;
  *felv_a = pantsim_c->info.felv ;
}

/* ------------------------------
 * Get AZ and EL positions.  Extrapolation is used to fill in the current
 * position based on the last position and velocity updates, and the current
 * time.
 * History:
 *     jmh   21 Oct 1991   Init
 *
 * Exit Parameters
 *   iaz, iel - Antenna binary angle positions
 */
void antsim_get_azel_pos( BIN2* iaz_a, BIN2* iel_a )
{
  UINT4 ibtime, ibsamp ;
  FLT4 ftick, fmotion ;

  /* Loop if an update comes in during the calculation
   */
 loop:
  ibsamp = pantsim_c->info.ibtime ;
  getbtime( &ibtime ) ;

  /* Compute the time since the last update, and limit it to the setup value
   */
  ftick = ((FLT4)btimeGrace(ibtime - ibsamp)) / 1000.0 ;
  ftick = MIN( ftick, (ANT_MAXEXTRAPMS / 1E3) ) ;

  fmotion = pantsim_c->info.fazv * ftick ;
  *iaz_a = pantsim_c->info.iaz + iBin2FromFDeg( fmotion ) ;
  fmotion = pantsim_c->info.felv * ftick ;
  *iel_a = pantsim_c->info.iel + iBin2FromFDeg( fmotion ) ;
  if( ibsamp != pantsim_c->info.ibtime ) goto loop ;
}

/* ------------------------------
 * Get AZ command
 * History:
 *    jmh   21 Oct 1991   Init
 *
 * Exit Parameters
 *   imode_a - Mode for azimuth
 *   idaz_a  - Desired azimuth
 *   fdazv_a - Desired azimuth velocity
 */
void antsim_get_az_cmd( UINT2* imode_a, BIN2* idaz_a, FLT4* fdazv_a )
{
  *imode_a = pantsim_c->info.iazmode ;
  *idaz_a  = pantsim_c->info.idaz ;
  *fdazv_a = pantsim_c->info.fdazv ;
}

/* ------------------------------
 * Get EL command
 * History:
 *    jmh   21 Oct 1991   Init
 *
 * Exit Parameters
 *   imode_a - Mode for elevation
 *   idel_a  - Desired elevation
 *   fdelv_a - Desired elevation velocity
 */
void antsim_get_el_cmd( UINT2* imode_a, BIN2* idel_a, FLT4* fdelv_a )
{
  *imode_a = pantsim_c->info.ielmode ;
  *idel_a  = pantsim_c->info.idel ;
  *fdelv_a = pantsim_c->info.fdelv ;
}

/* ------------------------------
 * Get roll angle and velocity
 * History:
 *     jmh   4 May 1992   Init
 */
void antsim_get_roll( BIN2* iroll_a, FLT4* frollv_a )
{
  *iroll_a  = pantsim_c->info.iroll ;
  *frollv_a = pantsim_c->info.frollv ;
}

/* ------------------------------
 * Get pitch angle and velocity
 * History:
 *     jmh   4 May 1992   Init
 */
void antsim_get_pitch( UINT2* ipitch_a, FLT4* fpitchv_a )
{
  *ipitch_a  = pantsim_c->info.ipitch ;
  *fpitchv_a = pantsim_c->info.fpitchv ;
}

/* ------------------------------
 * Get heading angle and velocity
 * History:
 *     jmh   4 May 1992   Init
 */
void antsim_get_heading( BIN2* iheading_a, FLT4* fheadingv_a )
{
  *iheading_a  = pantsim_c->info.iheading ;
  *fheadingv_a = pantsim_c->info.fheadingv ;
}

/* ------------------------------
 * Get elevation order angle
 * History:
 *     jmh   4 May 1992   Init
 */
void antsim_get_elor( BIN2* ielor_a )
{
  *ielor_a  = pantsim_c->info.iel_order ;
}

/* ------------------------------
 * Get train order angle
 * History:
 *     jmh   4 May 1992   Init
 */
void antsim_get_tror( BIN2* itror_a )
{
  *itror_a  = pantsim_c->info.itr_order ;
}

/* ------------------------------
 * Get velocity of the inu.
 * History:
 *     jmh   5 May 1992   Init
 */
void antsim_get_inu_vel( FLT4 fvelocity_a[3] )
{
  fvelocity_a[0] = pantsim_c->info.fvel_east ;
  fvelocity_a[1] = pantsim_c->info.fvel_north ;
  fvelocity_a[2] = pantsim_c->info.fvel_up ;
}

/* ------------------------------
 * Set the simulated velocity of the inu.
 * History:
 *     jmh   5 May 1992   Init
 */
void antsim_set_inu_vel( FLT4 fvelocity_a[3] )
{
  pantsim_c->info.fvel_east = fvelocity_a[0] ;
  pantsim_c->info.fvel_north = fvelocity_a[1] ;
  pantsim_c->info.fvel_up = fvelocity_a[2] ;
}

/* ------------------------------
 * Set the simulated fixed tilt of the moving platform.
 * History:
 *     jmh   6 May 1992   Init
 */
void antsim_set_deck_tilt( FLT4 fposition_a[3] )
{
  pantsim_c->info.fsim_tilt_pitch   = fposition_a[0] ;
  pantsim_c->info.fsim_tilt_roll    = fposition_a[1] ;
  pantsim_c->info.fsim_tilt_heading = fposition_a[2] ;
}

/* ------------------------------
 * Set the simulated rocking amplitude of the moving platform.
 * History:
 *     jmh   5 May 1992   Init
 */
void antsim_set_deck_rock( FLT4 fvelocity_a[3] )
{
  pantsim_c->info.fsim_rock_pitch   = fvelocity_a[0] ;
  pantsim_c->info.fsim_rock_roll    = fvelocity_a[1] ;
  pantsim_c->info.fsim_rock_heading = fvelocity_a[2] ;
}

/* ------------------------------
 * Get velocity of the pedestal.
 * History:
 *     jmh   5 May 1992   Init
 */
void antsim_get_pedestal_vel( FLT4 fvelocity_a[3] )
{
  FLT4 frotation_vel[3], frotation_pos[3], flocal_velocity[3] ;
  FLT4 fplatform_offset[3] ;
  FLT4 fmatrix[3][3] ;

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

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

  fplatform_offset[0] = (0.01)*(FLT4)pantsim_c->setup.iplatform_offset[0] ;
  fplatform_offset[1] = (0.01)*(FLT4)pantsim_c->setup.iplatform_offset[1] ;
  fplatform_offset[2] = (0.01)*(FLT4)pantsim_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] = pantsim_c->info.fvel_east  + flocal_velocity[0] ;
  fvelocity_a[1] = pantsim_c->info.fvel_north + flocal_velocity[1] ;
  fvelocity_a[2] = pantsim_c->info.fvel_up    + flocal_velocity[2] ;
}

/* ------------------------------
 * Return the position of the inu.
 * Vertical position is in meters.
 * History:
 *     jmh   6 May 1992 init
 */
void  antsim_get_inu_pos( FLT4 fposition_a[3] )
{
  fposition_a[0] = fDegFromBin4(pantsim_c->info.ilongitude) ;
  fposition_a[1] = fDegFromBin4(pantsim_c->info.ilatitude) ;
  fposition_a[2] = pantsim_c->info.faltitude ;
}
