/* **********************************************************************
 * *	                                                                *
 * *    Instantaneous Automatic Gain Control (IAGC) Support Routines    *
 * *                                                                    *
 * **********************************************************************
 * File: libs/dsp/dsp_iagc.c
 *
 *              COPYRIGHT (c) 1993, 1994, 1997, 1998, 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 <math.h>
#include "sigtypes.h"
#include "dsp.h"
#include "dsp_lib.h"
#include "dsp_def.h"

/* ======================================================================
 C This routine sets up the digital attenuation levels when IAGC hardware
 C is present.  It should be called once to initially set up the attenuator
 C calibrations in the RVP6.
 C History:
 C     ads   13 Jun 1993  Init
 C Entry parameters
 C     FGAINS_A   Table of 64 attenuator gains in dB
 C     IFLAGS_A   Table of 64 usage flags (Bit 0 NZ ==> Use this setting)
 */
MESSAGE dsp_wiagc_gains_
  (Dsp *dsp_a,
   const FLT4 fgains_a[64],
   const UINT2 iflags_a[64] ) 
{
  MESSAGE istatus;
  UINT2 igains[64], ii, liagc;

  /* ---------------------------------------------------------------------- */
      
  liagc = ldsp_has_iagc_(dsp_a, &istatus);
  if (istatus != SS_NORMAL) return istatus;

  if ( liagc )
    {
      icmd_c[0] = OP_IAGCTRL + 0x100;
      istatus = dsp_write( dsp_a, icmd_c, 1 );
      if (istatus!=SS_NORMAL) return istatus;
      
      for (ii=0; ii<64; ii++ )
	{
	  igains[ii] = ( 0x7FFF & NINT( 100.0 * fgains_a[ii] ) ) | ( iflags_a[ii]<<15 );
	}
      istatus = dsp_write( dsp_a, igains, 64 );
    }
      
  return istatus;
}
/*
 * ======================================================================
 C Routine to switch out of IAGC mode and return to old-style analog
 C AGC/STC.
 C History:
 C     ads   13 Jun 1993  Init
 */
MESSAGE dsp_iagc_off_
  (Dsp *dsp_a )  /* Dsp structure */
{
  MESSAGE istatus;
  UINT2 liagc;

  /* ---------------------------------------------------------------------- */
  liagc = ldsp_has_iagc_(dsp_a, &istatus);
  if (istatus != SS_NORMAL) return istatus;

      if(0 != liagc )
	{
	  icmd_c[0] = OP_IAGCTRL + 0x0000;
	  istatus = dsp_write( dsp_a, icmd_c, 1 );
	}
      
  return istatus;
}
/*
 * ======================================================================
 * This routine sets up and runs a state transition table for the IAGC
 * control lines.  This serves primarilly as a diagnostic test of the
 * attenuator calibration and overall working of the hardware.
 * History:
 *     ads   13 Jun 1993  Init
 * Entry parameters
 *     ISTATES_A  Table of next states from each 6-bit state (origin 0)
 *     IFIRST_A   Initial value to start from
 */
MESSAGE dsp_iagc_run_states
  (Dsp *dsp_a,          /* Dsp structure */
   const UINT2 istates_a[64],
   UINT2 ifirst_a ) 
{
  MESSAGE istatus;
  UINT2 liagc;
  /* ----------------------------------------------------------------------*/
  liagc = ldsp_has_iagc_(dsp_a, &istatus);
  if (istatus != SS_NORMAL) return istatus;

  if ( liagc )
    {
      icmd_c[0] = OP_IAGCTRL + 0x0200;
      istatus = dsp_write( dsp_a, icmd_c, 1 );
      if (istatus != SS_NORMAL) return istatus;;

      istatus = dsp_write( dsp_a, istates_a, 64 );
      if (istatus != SS_NORMAL) return istatus;

      istatus = dsp_write( dsp_a, &ifirst_a, 1 );
    }
      
  return istatus;
}

/*
 * ======================================================================
 C This routine sets up and runs a signal feedback algorithm for the IAGC
 C control lines.  This is the primary way that IAGC is used during real
 C data processing.
 C History:
 C     ads   13 Jun 1993  Init
 C Entry parameters
 C     FLOW_A     Low feedback hysterisis point
 C     FGOAL_A    Goal feedback point
 C     FHI_A      High feedback hysterisis point
 C     IFIRST_A   Initial value to start from
 */
MESSAGE dsp_iagc_run_feedback
  (Dsp *dsp_a,
   FLT4 flow_a,
   FLT4 fgoal_a,
   FLT4 fhi_a ) 
{
  MESSAGE istatus;
  UINT2 liagc;
  /* ----------------------------------------------------------------------*/
  liagc = ldsp_has_iagc_(dsp_a, &istatus );
  if (istatus != SS_NORMAL) return istatus;

  if( liagc )
    {
      icmd_c[0] = OP_IAGCTRL + 0x0300;
      icmd_c[1] = NINT( 100.0 * flow_a  );
      icmd_c[2] = NINT( 100.0 * fgoal_a );
      icmd_c[3] = NINT( 100.0 * fhi_a   );
      istatus = dsp_write( dsp_a, icmd_c, 4 );
    }
      
  return istatus;
}

/* ======================================================================
 * Routine returns nonzero if the DSP has instantaneous gain control
 C History:
 C     ads   13 Jun 1993  Init
 */
UINT2 ldsp_has_iagc_
  (Dsp *dsp_a,           /* Dsp structure */
   MESSAGE *istatus_a )  /* Message return value */
{
  UINT2 lreturn;

  /* Assume that the DSP does not have IAGC
   */
  *istatus_a = SS_NORMAL ;
  lreturn = FALSE ;

  /* Check that the processor is an RVP6 of some sort.
   */
  if( dsp_a->itype == DSP_TYPE_RVP6   ||
      dsp_a->itype == DSP_TYPE_RVP6_2 )
    {
      
      /* Verify that the GPARM flag indicates that IAGC is
       * available.  Not all RVP6's have it, and it can only be
       * determined at run time.
       */
      *istatus_a = dspr_pram_internal( dsp_a ) ;
      
      if( *istatus_a == SS_NORMAL )
	{
	  if( gparm_c.istat_i & GIS_HASIAGC ) lreturn = TRUE;
	}
    }
  return( lreturn ) ;
}

/* =================================================================== 
 * This function determines the IAGC attenuator setting (0 - 63) which
 * will best produce the gain requested.
 *
 *  ads  21 Jun 1993   Init
 * Entry parameters: 
 *     FGAINTAB_A - Gains for each attenuator setting
 *     IFLAGS_A   - Array of 64 flags (LSB ==> setting is used)
 *     FGAIN_A    - The gain value we wish to find.
 */
UINT2 iiagc_search
  (const FLT4 fgaintab_a[64],
   const UINT2 iflags_a[64],
   FLT4 fgain_a,
   UINT2 *lbad_a   /* Set to TRUE if no gain value can be found, otherwise unchanged */
   ) 
{
  UINT2 lfirst, iresult;
  UINT4 ii;
  FLT4 fminimum, ff;
      
  /* ---------------------------------------------------------------------- */

  lfirst = TRUE;
  for (ii=0;ii<64;ii++)
    {
      if( iflags_a[ii]&1 )
	{
	  ff = fabs( fgaintab_a[ii] - fgain_a );
	  if( lfirst )
	    {
	      iresult  = ii;
	      lfirst   = FALSE;
	      fminimum = ff;
	    }
	  else
	    {
	      if( ff < fminimum )
		{
                  iresult  = ii;
                  fminimum = ff;
		}
	    }
	}
    }

  if( lfirst )
    {
      iresult = 0;
      *lbad_a  = TRUE;
    }

  return iresult;
}
