/* **********************************************************************
 * *	                                                                *
 * *                    Antenna Modelling Routines                      *
 * *                                                                    *
 * **********************************************************************
 * File: libs/antenna/ant_model.c
 *
 *                        COPYRIGHT (c) 2002  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 <math.h>

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

static SINT2 iDesVelForPosError
( SINT2 iPosError_a, volatile UINT1 *lHysteresisInner_a ) ;

/* ------------------------------
 * Routine to update one axis of a modelled antenna, simulating the
 * passage of FTICK seconds since the last call.
 */
void antAxisModelUpdate
( volatile struct antAxisModel *pModel_a, FLT4 fTick_a )
{
  SINT2 iDesVel ;

  /* Sanity check the tick increment, and do nothing if the time span
   * is outrageous.
   */
  if( (fTick_a < 0.0) || (fTick_a > 2.0) ) return ;

  /* From the servo mode, first compute a desired velocity.  The
   * remainder of the update routine will work from this desired
   * rate of rotation.
   */
  switch( pModel_a->iAntMode ) {
  case ANTMODE_STOP: default:
    /* For stopped mode, the desired velocity is zero.
     */
    iDesVel = 0 ; break ;

  case ANTMODE_VEL:
    /* For the velocity servo, the desired velocity is simply the
     * requested velocity.
     */
    iDesVel = pModel_a->iRequestedVel ; break ;

  case ANTMODE_POS:
    /* For the position servo, the desired velocity is a nonlinear
     * function of how much distance there is to travel.  If we are
     * supposed to move "Over the Top", then change the computation of
     * the angle error so that we will never cross the seam at -90
     * degrees.  Otherwise, always go for the shortest angular route.
     */
    {
      BIN2 iReq = pModel_a->iRequestedPos ; SINT4 iDiff ;
      BIN2 iPos = pModel_a->iPos32 >> 16 ;

      if( pModel_a->lOverTheTop ) {
	iDiff = ((SINT4)(UINT2)( iReq + 0x4000) ) -
	        ((SINT4)(UINT2)( iPos + 0x4000) ) ;
      } else {
	iDiff = (SINT4)(SINT2)( iReq - iPos ) ;
      }
      iDiff = MAX( MINSINT2, MIN( MAXSINT2, iDiff ) ) ;
      iDesVel = iDesVelForPosError( iDiff, &pModel_a->lHysteresisInner ) ;
    }
    break ;
  }
  /* Bound the desired velocity against the absolute maximum limit,
   * and also against the velocities that would be required to stop at
   * the optional soft limits.
   */
  if( pModel_a->lUseLimits ) {
    BIN2 iInner = iBin2OrderedMidPt( pModel_a->iPosLimits[0], pModel_a->iPosLimits[1] ) ;
    BIN2 iOuter = iBin2OrderedMidPt( pModel_a->iPosLimits[1], pModel_a->iPosLimits[0] ) ;
    BIN2   iPos = (pModel_a->iPos32 >> 16) ;

    if( lbin2_in_a_row( iOuter, iPos, iInner ) ) {
      SINT2 iLoLimit = iDesVelForPosError( pModel_a->iPosLimits[0] - iPos, NULL ) ;
      iDesVel = MAX( iDesVel, iLoLimit ) ;
    }
    if( lbin2_in_a_row( iInner, iPos, iOuter ) ) {
      SINT2 iHiLimit = iDesVelForPosError( pModel_a->iPosLimits[1] - iPos, NULL ) ;
      iDesVel = MIN( iDesVel, iHiLimit ) ;
    }
  }
  iDesVel = MAX( iDesVel, -pModel_a->iVelLimit ) ;
  iDesVel = MIN( iDesVel,  pModel_a->iVelLimit ) ;

  /* Update the current velocity by applying a (bounded) acceleration
   * due to the difference between the desired and current values.
   * The velocity update is clamped so that velocity overshoots will
   * never occur.
   */
  {
    SINT4 iDesVel32 = ((SINT4)iDesVel) << 16 ;
    FLT8 fVelErr = fDegFromBin4( iDesVel32 - pModel_a->iVel32 ) ;
    FLT8 fAccel  = ANTMOD_ACCELGAIN * fVelErr ;

    fAccel = MAX( fAccel, -ANTMOD_MAXACCEL ) ;
    fAccel = MIN( fAccel,  ANTMOD_MAXACCEL ) ;
    pModel_a->iVel32 += iBin4FromFDeg( fTick_a * fAccel ) ;

    if( fAccel > 0.0 ) {
      pModel_a->iVel32 = MIN( pModel_a->iVel32, iDesVel32 ) ;
    }
    if( fAccel < 0.0 ) {
      pModel_a->iVel32 = MAX( pModel_a->iVel32, iDesVel32 ) ;
    }
  }
  /* Finally, update the position based on the velocity and tick
   * interval.
   */
  pModel_a->iPos32 += NINT( fTick_a * (FLT4)pModel_a->iVel32 ) ;
}

/* ------------------------------
 * Return the desired velocity (signed binang/sec) required to correct
 * a given position error (signed binang).
 */
static SINT2 iDesVelForPosError
( SINT2 iPosError_a, volatile UINT1 *lHysteresisInner_a )
{
  SINT4 iDesVel ; FLT4 fErrAbs = fabs( (FLT4)iPosError_a ) ;
  UINT1 lHysteresisInner = TRUE ;

#define POSHYSTOUTER  12	/* Outer and inner hysteresis zones */
#define POSHYSTINNER   5	/*   (binary angles) */

  if( lHysteresisInner_a ) {
    if( fErrAbs > POSHYSTOUTER ) *lHysteresisInner_a = FALSE ;
    if( fErrAbs < POSHYSTINNER ) *lHysteresisInner_a = TRUE  ;
    lHysteresisInner = *lHysteresisInner_a ;
  }
  if( (    lHysteresisInner  && (fErrAbs < POSHYSTOUTER)) ||
      ( (! lHysteresisInner) && (fErrAbs < POSHYSTINNER)) ) iDesVel = 0 ;
  else {
#define POSBREAK 5.0		/* Break point of square root feedback */
    iDesVel = NINT( ((iPosError_a > 0) ? 1.0 : -1.0) * ANTMOD_POSGAIN *
		    (sqrt(SQUARE(POSBREAK) + fErrAbs) - POSBREAK) ) ;
  }
  return( iDesVel ) ;
}
