/* *****************************************
 * *                                       *
 * *   IRIS DSP Speed/Performance Tester   *
 * *                                       *
 * *****************************************
 * file: utils/dsp/speed.C
 *      
 *   COPYRIGHT (c) 1994, 1995, 1996, 1997, 1999, 2000, 2002, 2003  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 <stdlib.h>
#include <string.h>
#include <errno.h>

#include "sigtypes.h"
#include "dsp.h"
#include "user_lib.h"
#include "dsp_lib.h"
#include "antenna_lib.h"

static void angleCmpTest( SINT4 iMaxCount_a ) ;
static void usage( void ) ;
static void exit_handler( void ) ;

static Dsp *dsp_c = NULL ;

/* ------------------------------
 * This program allows testing of the RVP processing speed.  It is
 * both a functional test utility as well as a code demonstration
 * utility.  However, because some of the tests that it performs are
 * tricky, parts of this code actually bypass the DSP driver calls and
 * perform lower level operations directly.  Those portions of code
 * are noted in the comments, and should not really be emulated for
 * use elsewhere.
 */
int main( int argc_a, char *argv_a[] ) 
{
#define MAXREADWORDS 100000
  SINT4 ii, iUnfold, iDelay, iMaxCount, iSetupBins, nFlushedRays=5 ;
  SINT4 iLoopCount, iBinCount, iReadCount, iSpeckleCount ;
  UINT2 iWordBuf[ MAXREADWORDS ], iProcCmd ; struct gparm Gparm;
  UINT1 lFreeRun, lSpeckle, lAngleSync, lFlagEQ, lAngleCmp ;
  UINT1 lSetVel, lLogRays ;
  struct ymds_time StartTime, EndTime, Time ;
  MESSAGE istatus ; FLT4 fSetVel ;

  /* Setup the signal handler and exit handler, and check that we're a
   * valid user.
   */
  sig_establish( NULL, "speed" ) ;

  if( atexit( exit_handler ) == -1 ) {
    sig_signal( errno_to_message_va( errno, 1, "speed->atexit()" ) ) ;
    exit(1) ;
  }
  if( !liris_operator( sig_username() ) ) {
    printf("Sorry, you must be an IRIS operator to run SPEED.\n") ;
    exit(EXIT_FAILURE) ;
  }
  /* Process arguments from the command line
   */
  iUnfold    = 1 ;
  iDelay     = 0 ;
  iMaxCount  = 100 ;
  lFreeRun   = FALSE ;
  lSpeckle   = FALSE ;
  lAngleSync = FALSE ;
  lAngleCmp  = FALSE ;
  lFlagEQ    = FALSE ;
  iSetupBins = 0 ;
  lSetVel    = FALSE ;
  lLogRays   = FALSE ;

  for( ii=1 ; ii < argc_a ; ii++ ) {
    int ilen = strlen( argv_a[ii] ) - 1 ;
    if( (argv_a[ii][0] == '-' || argv_a[ii][0] == '/') && (ilen>0) ) {

      if( 0 == strncasecmp( &argv_a[ii][1], "help", ilen ) ) {
	usage() ; exit(EXIT_SUCCESS) ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "freerun", ilen ) ) {
	lFreeRun = TRUE ; continue ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "speckle", ilen ) ) {
	lSpeckle = TRUE ; continue ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "angsyn", ilen ) ) {
	lAngleSync = TRUE ; continue ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "flageq", ilen ) ) {
	lFlagEQ = TRUE ; continue ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "anglecmp", ilen ) ) {
	lAngleCmp = TRUE ; continue ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "lograys", ilen ) ) {
	lLogRays = TRUE ; continue ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "unfold" , ilen ) ) {
	if( ++ii < argc_a ) {
	  iUnfold = atol( argv_a[ii] ) ;
	  if( (iUnfold < 1) || (iUnfold > 4) ) {
	    printf( "Invalid unfolding argument: %d\n", iUnfold ) ; exit(EXIT_FAILURE) ;
	  }
	} else {
	  printf( "Missing unfolding argument\n" ) ; exit(EXIT_FAILURE) ;
	}
	continue ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "delay" , ilen ) ) {
	if( ++ii < argc_a ) {
	  iDelay = atol( argv_a[ii] ) ;
	  if( (iDelay < 0) || (iDelay > 10000) ) {
	    printf( "Invalid delay argument: %d\n", iDelay ) ; exit(EXIT_FAILURE) ;
	  }
	} else {
	  printf( "Missing delay argument\n" ) ; exit(EXIT_FAILURE) ;
	}
	continue ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "count" , ilen ) ) {
	if( ++ii < argc_a ) {
	  iMaxCount = atol( argv_a[ii] ) ;
	  if( (iMaxCount < 0) || (iMaxCount > 10000000) ) {
	    printf( "Invalid count argument: %d\n", iMaxCount ) ; exit(EXIT_FAILURE) ;
	  }
	} else {
	  printf( "Missing count argument\n" ) ; exit(EXIT_FAILURE) ;
	}
	continue ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "bins" , ilen ) ) {
	if( ++ii < argc_a ) {
	  iSetupBins = atol( argv_a[ii] ) ;
	  if( (iSetupBins <= 0) || (iSetupBins > 2048) ) {
	    printf( "Invalid bin count: %d\n", iSetupBins ) ; exit(EXIT_FAILURE) ;
	  }
	} else {
	  printf( "Missing bin count\n" ) ; exit(EXIT_FAILURE) ;
	}
	continue ;
      }
      if( 0 == strncasecmp( &argv_a[ii][1], "setvel" , ilen ) ) {
	if( ++ii < argc_a ) {
	  fSetVel = atof( argv_a[ii] ) ; lSetVel = TRUE ; nFlushedRays = 0 ;
	} else {
	  printf( "Missing desired antenna velocity\n" ) ; exit(EXIT_FAILURE) ;
	}
	continue ;
      }

      printf( "Unrecognized arg: '%s'\n", &argv_a[ii][0] ) ;
      usage() ; exit(EXIT_FAILURE) ;

    } else {
      printf( "Unrecognized arg: '%s'\n", &argv_a[ii][0] ) ;
      usage() ; exit(EXIT_FAILURE) ;
    }
  }
  /* Attempt to open the antenna driver
   */
  istatus = AntPowerUpMain( ANT_IAM_CLIENT, 0 ) ;
  if( istatus != SS_NORMAL ) {
    sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
  }
  /* Attempt to open the DSP
   */
  dsp_c = DspOpenForIo( &istatus ) ;
  if( NULL == dsp_c ) {
    sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
  }
  /* Check for correct byte order
   */
  istatus = DspResetFifo( dsp_c );
  if( istatus == SS_NORMAL ) istatus = DspCheckByteOrder( dsp_c ) ;
  if( istatus != SS_NORMAL ) {
    sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
  }
  /* Optionally run the special angle comparison mode.  If chosen,
   * these tests run to completion and the program then exits.
   */
  if( lAngleCmp ) {
    angleCmpTest( iMaxCount ) ; exit(EXIT_SUCCESS) ;
  }
  /* Set up angle synchronization mode
   */
  istatus = dspw_syn_mode
    ( dsp_c, lAngleSync ? ANGSYN_DYNAMIC : ANGSYN_NONE, 0 ) ;
  if( istatus != SS_NORMAL ) {
    sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
  }
  /* Set the task name
   */
  istatus = DspWriteTaskId( dsp_c, "Speed", 0, 0, SCAN_PPI ) ;
  if( istatus != SS_NORMAL ) {
    sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
  }
  /* Set the header configuration
   */
  istatus = dspw_header_cfg( dsp_c, DSP_HDR_TAGS ) ;
  if( istatus != SS_NORMAL ) {
    sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
  }
  /* If a specific number of bins has been requested, then reload the
   * range mask with a uniform mask starting from range zero.
   */
  if( iSetupBins ) {
    UINT2 imask[2048] ;
    /* This code does not really work if the range mask at the current
     * pulsewidth is not 125 meters!
     */
    range_mask_gen( dsp_c, imask, 0.0, iSetupBins, 0.125, 0 ) ;
    istatus = dspw_range_mask( dsp_c, imask, 0 ) ;

    if( istatus != SS_NORMAL ) {
      sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
    }
  }
  /* Read the GPARM information from the DSP to get its present
   * configuration, and to compute the number of bytes to read each
   * time.
   */
  istatus = dspr_gparm( dsp_c, &Gparm, GPARM_LSTATUS_CLEAR ) ;
  if( istatus != SS_NORMAL ) {
    sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
  }
  iBinCount  = Gparm.ibin_out_num ;
  iReadCount = 4 + 2*iBinCount ;
  iReadCount = MIN( iReadCount, MAXREADWORDS ) ;

  /* Print information about the run we are about to begin.
   */
  printf( "Processor is configured for %d range bins.\n", iBinCount ) ;

  if( iUnfold == 1 ) printf( "Using fixed PRF" ) ;
                else printf( "Using x%d Dual-PRF", iUnfold ) ;
  printf( " at %.1f Hertz for %d rays.\n",
	  TREQSCL / (FLT4)MAX( Gparm.iprt_gen, (SINT4)(TREQSCL/100000.0) ),
	  iMaxCount ) ;

  if( iDelay == 0 ) printf( "No added delay between rays.\n" ) ;
  else              printf( "Adding %d-milliseconds delay between rays.\n", iDelay ) ;

  if( lFreeRun ) printf( "Using Free-Running PROC command\n" ) ;
  else           printf( "Using One-Ray-at-a-Time PROC command\n" ) ;

  if( lAngleSync ) printf( "Dynamic Angle Sync'ing is enabled\n" ) ;
  else             printf( "No angle synchronization will be used\n" ) ;

  /* Set the antenna speed, if requested.
   */
  if( lSetVel ) {
    printf( "Setting antenna scan speed to %.1f deg/sec\n", fSetVel ) ;
    ant_set_az_vel( fSetVel ) ;
  }
  /* Make up the PROC command to acquire data in the requested mode.
   * Send this command to the DSP to begin the acquisition process.
   *
   * NOTE: We make the command manually, rather than using the DSP
   * driver's dspw_proc() entry point, so that none of the internal
   * DSP settings get disturbed in the process.  This is necessary
   * because we want the DSP to retain all of its existing settings
   * when 'speed' is run.  This is NOT how typical application code
   * would use the driver.
   */
  iProcCmd  = ( OP_PROC | OP_PROC_ARCHIVE ) ;
  iProcCmd |= ( OP_PROC_UMODE & ((iUnfold - 1) << OP_PROC_UMODE_SHIFT) ) ;

  if( lFreeRun ) iProcCmd |= OP_PROC_FREEDOP ;
  else           iProcCmd |= OP_PROC_SYNCDOP ;

  istatus = dsp_write( dsp_c, &iProcCmd, 1 ) ;
  if( istatus != SS_NORMAL ) {
    sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
  }
  GetTimeYmds( &Time, TIME_FLG_UTC );  EndTime = StartTime = Time ;
  iSpeckleCount = 0 ;

  /* Loop reading from the DSP.  We start the reading from a small
   * negative ray number to get the pipelining and buffering
   * initialized.
   */
  for( iLoopCount = -nFlushedRays ; iLoopCount <= iMaxCount ; iLoopCount++ ) 
    {
      SINT4 iActualCount ;
      istatus = dsp_read( dsp_c, iWordBuf, iReadCount, &iActualCount ) ;
      if (istatus != SS_NORMAL) {
	sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
      }
      if( iLoopCount == -nFlushedRays ) {
	GetTimeYmds( &Time, TIME_FLG_UTC );
	printf( "Latency from PROC to FirstRead: %d ms.\n\n",
		iYmdsSubtractMs( &Time, &StartTime ) ) ;
      }
      if( iDelay > 0 ) sig_microSleep( 1000 * iDelay ) ;
      if( ! lFreeRun ) {
	istatus = dsp_write( dsp_c, &iProcCmd, 1 ) ;
	if( istatus != SS_NORMAL ) {
	  sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
	}
      }
      GetTimeYmds( &Time, TIME_FLG_UTC );
      
      /* Optionally check for speckles in the received reflectivity
       * data.
       */
      if( lSpeckle ) {
	for( ii=1 ; ii < iBinCount ; ii++ ) {
	  SINT4 ivalx = 0x00FF & iWordBuf[ 4 + 2*(ii-1) ] ;
	  SINT4 ivaly = 0x00FF & iWordBuf[ 4 + 2*(ii  ) ] ;
	  SINT4 idiff = (ivalx - ivaly) ;
	  if( (idiff < -5) || (idiff > 5) ) iSpeckleCount++ ;
	}
      }
      /* Print progress messages according to how we're running.  Do
       * this only once the pipeline actually gets flowing.
       */
      if( iLoopCount >= 0 ) {
	if( lAngleSync || lLogRays ) {
	  static SINT4 iMatchCount = 0 ;
	  char sTimeBuf[TIMENAME_SIZE];
	  SINT2 idiff = (SINT2)( iWordBuf[0] - iWordBuf[2] ) ;
	  FLT4 fBegAZ =  fPDegFromBin2( iWordBuf[0] ) ;
	  FLT4 fEndAZ =  fPDegFromBin2( iWordBuf[2] ) ;
	  FLT4 fBegEL = fElDegFromBin2( iWordBuf[1] ) ;
	  FLT4 fEndEL = fElDegFromBin2( iWordBuf[3] ) ;
	  shhmmssmmm_r( Time.isec, MS_FROM_MILLS(Time.imills), sTimeBuf );
	  
	  printf( "Ray %3d %s  Az(%6.2f to %6.2f)   El(%6.2f to %6.2f)",
		  iLoopCount, sTimeBuf+3, fBegAZ, fEndAZ, fBegEL, fEndEL ) ;
	  
	  /* Print equals sign, beep and total count of nearly identical
	   * angles while in angle sync mode.  This is to help view
	   * angle sampling details in the DSP.
	   */
	  if( lFlagEQ && (idiff >= -2) && (idiff <= 2) ) {
	    iMatchCount++ ; printf( " =%d\007", iMatchCount ) ;
	  } else {
	    if( iMatchCount ) printf( "  %d", iMatchCount ) ;
	  }
	  printf( "\n" ) ;
	  
	} else {
	  /* Print speed measurement messages, but limit the printout to
	   * the first, last, and every 200 milliseconds so that we
	   * don't add overhead that might affect the measurement.
	   */
	  static struct ymds_time PrintTime = {0} ;
	  if ( (iLoopCount == 0) || (iLoopCount == iMaxCount) ||
	       (iYmdsSubtractMs( &Time, &PrintTime ) > 200) ) {
	    FLT4 fazimuth   =  fPDegFromBin2( iWordBuf[0] ) ;
	    FLT4 felevation = fElDegFromBin2( iWordBuf[1] ) ;
	    printf("\rRay %3d  Az:%6.2f  El:%6.2f",
		   iLoopCount, fazimuth, felevation ) ;
	    PrintTime = Time ; fflush( stdout ) ;
	  }
	}
      }
      /* Record the starting and ending times of the loop during fully
       * pipelined operation.
       */
      if (iLoopCount == 0        ) StartTime = Time ;
      if (iLoopCount == iMaxCount) EndTime   = Time ;
    }

  /* Print summary statistics on exit
   */
  printf("\n") ;
  if( iMaxCount > 0 ) {
    printf( "Time per ray: %.4f seconds.\n",
	    ((FLT8)iYmdsSubtractMs( &EndTime, &StartTime )) /
	    (1000.0 * (FLT8)iMaxCount) ) ;
  }
  if( lSpeckle ) {
    if( iSpeckleCount )
      printf( "There were %d speckles observed.%c\n", iSpeckleCount, 0x07 ) ;
    else
      printf( "There were no speckles observed.\n" ) ;
  }

  exit(EXIT_SUCCESS) ;
}

/* ------------------------------
 * Special test in which TAG angles are compared with AZ/EL angles
 * from the IRIS antenna driver.
 */
#define ANGPERIOD 40000		/* Update period (usec) */
static void angleCmpTest( SINT4 iMaxCount_a )
{
  MESSAGE istatus ; SINT4 iCount ; UINT4 iBtimeNow, iBtimeStart ;

  /* Loop reading angles from both sources and printing a comparison
   * table as we go.
   */
  sig_microSleep( ANGPERIOD ) ; getbtime( &iBtimeStart ) ;

  for( iCount=0 ; iCount < iMaxCount_a ; iCount++ ) {
    struct gparm gparm ; BIN2 iaz, iel, iazsamps[2], ielsamps[2] ;

    sig_microSleep( ANGPERIOD ) ; getbtime( &iBtimeNow ) ;

    ant_get_azel_pos( &iazsamps[0], &ielsamps[0] ) ;

    istatus = dspr_gparm( dsp_c, &gparm, GPARM_LSTATUS_CLEAR ) ;
    if( istatus != SS_NORMAL ) {
      sig_signal( istatus ) ; exit(EXIT_FAILURE) ;
    }
    ant_get_azel_pos( &iazsamps[1], &ielsamps[1] ) ;

    iaz = iazsamps[0] + (((SINT2)(iazsamps[1] - iazsamps[0])) >> 1) ;
    iel = ielsamps[0] + (((SINT2)(ielsamps[1] - ielsamps[0])) >> 1) ;

    printf( "%7.3f   TAGs:%7.2f%7.2f   IRIS:%7.2f%7.2f   Err: %6.2f%6.2f\n",
	    1E-3 * (FLT4)( iBtimeNow - iBtimeStart ),
	    fPDegFromBin2( gparm.itaga ), fElDegFromBin2( gparm.itagb ),
	    fPDegFromBin2( iaz         ), fElDegFromBin2( iel         ),	    
	    fDegFromBin2( gparm.itaga - iaz ),
	    fDegFromBin2( gparm.itagb - iel ) ) ;
    fflush( stdout ) ;
  }
}

/* ------------------------------
 * Give correct usage message, and error exit.
 */
static void usage()
{
  fprintf( stderr,
	   "IRIS DSP Performance Measurement Tool\n"
	   "-------------------------------------\n"
	   "  Overall Test Procedure Options:\n"
	   "       -count <N>: Use N-rays for the overall test\n"
	   "       -delay <N>: Add N-milliseconds delay between rays\n"
	   "     -speckle    : Check data for speckles\n"
	   "     -lograys    : Show log of all rays acquired\n"
	   "\n"
	   "  Processor Configuration Options:\n"
	   "        -bins <N>: Setup a range mask with N-bins\n"
	   "      -unfold <N>: Use times-N PRF unfolding\n"
	   "     -freerun    : Use Free-Running PROC command\n"
	   "\n"
	   "  Angle Synchronization Options:\n"
	   "      -angsyn    : Run with dynamic angle syncing\n"
	   "      -flageq    : Flag rays with nearly equal start/end AZ\n"
	   "    -anglecmp    : Compare TAG angles w/IRIS Antenna\n"
	   "      -setvel <V>: Set antenna scan speed to V deg/sec\n"
	   ) ;
}

/* ------------------------------
 * Clean up the signal processor if it is still trying to output more
 * words to us.
 */
static void exit_handler( void )
{
  MESSAGE istatus = SS_NORMAL ;
  if( dsp_c ) istatus = DspResetFifo( dsp_c ) ;
  if( istatus != SS_NORMAL ) sig_signal( istatus ) ;
}
