/* ******************************************************************
 * *                                                                *
 * *    Program to move the antenna in a sector scan                *
 * *                                                                *
 * ******************************************************************
 * File: utils/examples/sector.C
 *
 *                      COPYRIGHT (c) 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 <locale.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "sigtypes.h"
#include "user_lib.h"

#include "antenna_lib.h"


#define CLOCKWISE        (0)
#define COUNTERCLOCKWISE (1)
#define MAX_ELEVATIONS   (20)

/* This holds information to customize the sector */
struct command_line
{
  FLT4  fSectorLeft;
  FLT4  fSectorRight;
  FLT4  fElList[MAX_ELEVATIONS];
  FLT4  fScanSpeed;
  SINT4 iSweepCount;
};

/* Local function prototypes
 */
static UINT1 lContinueSweep
( const struct command_line *pCommand_a,
  UINT1 iDirection_a,
  UINT1 lFirst_a );     /* First call for this sweep */

static void ProcessCommandLine
(int iArgc, 
 char *sArgv[],
 struct command_line *pCommand_a );

static void Usage( void );

/* ===============================================================
 * Main program starts here.
 */
int main(int iArgc, char *sArgv[])
{
  MESSAGE istatus;
  struct command_line Command;
  setlocale( LC_ALL, "" ) ;

  /* Protect against the wrong people running it.
   */
  if ( !liris_operator(sig_username()))
    {
      printf("Sorry, you must be an operator to run sector.\n");
      exit(EXIT_FAILURE);
    }

  /* Process the command line arguments */
  ProcessCommandLine( iArgc, sArgv, &Command );

  /* Power up the antenna driver
   */
  istatus = AntPowerUpMain( ANT_IAM_CLIENT, 0 ) ;

  if ( istatus != SS_NORMAL )
    {
      sig_signal( istatus );
      exit(EXIT_FAILURE);
    }
  
  /* Run this task forever */
  while (TRUE)
    {
      BIN2 iAzimuth, iElevation;
      SINT2 iAzError, iElError;
      UINT1 iDirection;
      SINT4 iSweep;
      /* This program runs sector scans.  The first sweep will always be
       * clockwise.
       */
      /* Set the initial position which is 5 degrees CCW of the left limit
       * of the sector, and the desired elevation.
       */
      iAzimuth = iBin2FromFDeg( Command.fSectorLeft - 5.0 );
      ant_set_az_pos( iAzimuth );
      iElevation = iBin2FromFDeg( Command.fElList[0] );
      ant_set_el_pos( iElevation );
      
      ant_get_azel_err( (BIN2 *)&iAzError, (BIN2 *)&iElError );
      /* Wait until we are within 1 degree in azimuth, and 0.5 degrees in elevation */
      while( (abs(iAzError) > 182) || (abs(iElError) > 91 ) )
	{
	  sig_microSleep( 100000 );
	  ant_get_azel_err( (BIN2 *)&iAzError, (BIN2 *)&iElError );
	}
      
      iSweep = 0;
      iDirection = CLOCKWISE;
      while (iSweep<Command.iSweepCount)
	{
	  iElevation = iBin2FromFDeg( Command.fElList[iSweep] );
	  ant_set_el_pos( iElevation );
	  
	  ant_get_azel_err( (BIN2 *)&iAzError, (BIN2 *)&iElError );
	  /* Wait until we are within 0.5 degrees in elevation */
	  while (abs(iElError) > 91 )
	    {
	      sig_microSleep( 100000 );
	      ant_get_azel_err( (BIN2 *)&iAzError, (BIN2 *)&iElError );
	    }
	  
	  /* Start the scan */
	  if (iDirection==CLOCKWISE)
	    ant_set_az_vel( Command.fScanSpeed );
	  else
	    ant_set_az_vel( -Command.fScanSpeed );
	  
	  lContinueSweep( &Command, iDirection, TRUE );
	  sig_microSleep( 100000 );
	  /* Check 10 times a second to see if we are done */
	  while( lContinueSweep( &Command, iDirection, FALSE ) )
	    {
	      sig_microSleep( 100000 );
	    }
	  
	  /* Stop the scan, increment and reverse for the next sweep */
	  ant_stop_az();
	  iSweep++;
	  if (iDirection==CLOCKWISE)
	    iDirection=COUNTERCLOCKWISE;
	  else
	    iDirection=CLOCKWISE;
	  
	}  /* End  while (iSweep<Command.iSweepCount) */
      
    }  /* End  while (TRUE) */
}

/* ==============================================================
 * Returns TRUE if it thinks the sweep if done.  Special code required
 * to make sure a full sector which overlaps at the ends will work.
 */
static UINT1 lContinueSweep
( const struct command_line *pCommand_a,
  UINT1 iDirection_a,
  UINT1 lFirst_a )     /* First call for this sweep */
{
  static UINT1 lBegun;
  static SINT2 iEndErr;

  BIN2 iAzPos, iElPos;
  SINT2 iLastEndErr;

  /* Sweep has not begun if this is the first call. */
  if (lFirst_a) { lBegun = FALSE; iEndErr=0; }

  ant_get_azel_pos( &iAzPos, &iElPos );

  /* Compute the current and last ending error */
  iLastEndErr = iEndErr;
  if (iDirection_a==CLOCKWISE)
    {
      BIN2 iEndAz = iBin2FromFDeg( pCommand_a->fSectorRight );
      iEndErr = iAzPos - iEndAz;
    }
  else
    {
      BIN2 iEndAz = iBin2FromFDeg( pCommand_a->fSectorLeft );
      iEndErr = iEndAz - iAzPos;
    }


  if (!lBegun)
    {
      /* Sweep is considered to begin if the azimuth is in the semicircle past the
       * starting angle:
       */
      SINT2 iStartErr;
      BIN2  iStartAz;
      if (iDirection_a==CLOCKWISE)
	{
	  iStartAz = iBin2FromFDeg( pCommand_a->fSectorLeft );
	  iStartErr = iAzPos - iStartAz;
	  if (iStartErr >=0) lBegun=TRUE;
	}
      else
	{
	  iStartAz = iBin2FromFDeg( pCommand_a->fSectorRight );
	  iStartErr = iAzPos - iStartAz;
	  if (iStartErr <=0) lBegun=TRUE;
	}
    }
  else
    {
      /* Sweep is considered to end if it has begun, and the azimuth crosses
       * the ending azimuth.
       */
      if ( (iLastEndErr <= 0) && (iEndErr > 0) )  return FALSE;

    }

  return TRUE;

}
/* ======================================================================
 * Function to process the command line arguments.
 */
static void ProcessCommandLine
(int iArgc_a, 
 char *sArgv_a[],
 struct command_line *pCommand_a )
{
  
  int iArg; 
  memset( pCommand_a, 0, sizeof(*pCommand_a) );
  /* Fill in defaults.  0 to 90 degree sector at 1 degree, 1 rpm */
  pCommand_a->fSectorLeft  = 0.0;
  pCommand_a->fSectorRight = 90.0;
  pCommand_a->fElList[0]   = 1.0;
  pCommand_a->fScanSpeed   = 6.0;
  pCommand_a->iSweepCount  = 1;
  
  /* Parse command line options
   */
  for( iArg=1 ; iArg<iArgc_a ; iArg++ )
    {
      const char *sArg = sArgv_a[iArg];
      char *pEnd;
      /* fprintf( stderr, "Processing %d: '%s'\n", iArg, sArg ); */
      
      if (0 == strncmp( sArg, "-h", 2 ))
	{
	  Usage();
	}
      else if (0 == strncmp( sArg, "-left=", 6 ))
	{
	  pCommand_a->fSectorLeft = strtod( &sArg[6], &pEnd );
	  if ((pEnd == &sArg[6]) || (pCommand_a->fSectorLeft < 0.0) ||
	      (pCommand_a->fSectorLeft > 360.0) )
	    fprintf( stderr, "Command line syntax error:'%s'\n", sArg );
	}
      else if (0 == strncmp( sArg, "-right=", 7 ))
	{
	  pCommand_a->fSectorRight = strtod( &sArg[7], &pEnd );
	  if ( (pEnd == &sArg[7]) || (pCommand_a->fSectorRight < 0.0) ||
	       (pCommand_a->fSectorRight > 360.0) )
	  fprintf( stderr, "Command line syntax error:'%s'\n", sArg );
	}
      else if (0 == strncmp( sArg, "-speed=", 7 ))
	{
	  /* Speed must be positive, less than 10 rpm */
	  pCommand_a->fScanSpeed = strtod( &sArg[7], &pEnd );
	  if ((pEnd == &sArg[7]) || (pCommand_a->fScanSpeed <= 0.0) ||
	       (pCommand_a->fScanSpeed > 60.0) )
	    fprintf( stderr, "Command line syntax error:'%s'\n", sArg );
	}
      else if (0 == strncmp( sArg, "-el=", 4 ) )
	{
	  char sString[200], *sSubArg;
	  SINT4 iSweep;

	  /* Copy the argument for detailed parsing */
	  strncpy( sString, &sArg[4], 199); sString[199]=0;

	  for (iSweep=0; iSweep<MAX_ELEVATIONS; iSweep++)
	    {
	      if (iSweep==0)
		sSubArg = strtok( sString, "," );
	      else
		sSubArg = strtok( NULL, "," );
	      if (sSubArg==NULL)
		{
		  /* End when we exhaust the list.  Error if zero args */
		  if (iSweep==0) fprintf( stderr, "Command line syntax error:'%s'\n", sArg );
		  break;
		}
	      pCommand_a->fElList[iSweep] = strtod( sSubArg, &pEnd );
	      if ((sSubArg==pEnd)||(pCommand_a->fElList[iSweep] <= -90.0)||
		  pCommand_a->fElList[iSweep] >= 270.0)
		{
		  fprintf( stderr, "Command line syntax error:'%s'\n", sArg );
		  break;
		}
	      pCommand_a->iSweepCount = iSweep+1;
	    }
	}
      
    }
  return;
}
/* ==================================================
 */
static void Usage( void )
{
  fprintf( stderr, "sector options:\n" );
  fprintf( stderr, "  -help           -- Print this list\n" );
  fprintf( stderr, "  -el=el1,el2,... -- Specify list of elevation angles\n" );
  fprintf( stderr, "  -left=<azimuth> -- Specify left limit of sector scan\n" );
  fprintf( stderr, "  -right=<az>     -- Specify right limit of sector scan\n" );
  fprintf( stderr, "  -speed=<speed>  -- Specify azimuth scanning speed (deg/sec)\n" );
  fprintf( stderr, "   (all other arguments ignored)\n" );
  exit(EXIT_FAILURE) ;
}
