/*****************************************************************************/
/* focus.c                            Automatic control of Cassegrain focus. */
/* Augments manual control of the Control Room's  Focus Controller by means  */
/*  of single relays placed in parallel with the "In", "Out" and "Fast/Slow" */
/*  switches.  The GPIB equipment in the bottom of the Control Room rack     */
/*  allows this program to "write" to these 3 relays and "read" the signed,  */
/*  3.5 digit (14-bit binary-coded-decimal) focus position.  Refer to        */
/*  ntrack.c and GPIBports.h for more information.               JSW 11May10 */
/*****************************************************************************/

#include <ctype.h>
#include <slang/slang.h>
#include <stdio.h>
#include <sys/ioctl.h>
#include <termios.h>

#include "wirotypes.h"
#include "track.h"
#include "GPIBports.h"
#include "wiro.h"


void log_entry( char *comment );
double fabsolut();

struct  wiro_memory *tinfo, *get_tinfo();


main( int argc, char *argv[] ) {

  double val;
  int prec = 1;
  char	log_message[ 64 ];

  tinfo = get_tinfo();

  if( argc < 2 ) {
    printf("\n%6.3f    \n", FOCUS);
    exit( 0 );
  }

  else if(( argc == 2 ) || ( argc == 3 )) {
    if( sscanf( argv[ 1 ], "%lf", &val ) != 1 ) {
      help();
      exit( 1 );
    }
    if(( val < -0.870 ) || ( +1.120 < val )) {
      printf("\nArgument is out-of-range: %s\n", argv[ 1 ]);
      help();
      exit( 2 );
    }
    DES_FOCUS = val;
    if( argc == 3 ) {
      if( sscanf( argv[ 2 ], "%d", &prec ) != 1 ) {
        help();
        exit( 3 );
      }
      if( prec != 0 ) {
        printf("\nArgument is out-of-range: %s\n", argv[ 2 ]);
        help();
        exit( 4 );
      }
    }
  }

  else if (argc > 3) {
    printf("\nToo many arguments.\n");
    help( );
    exit( 5 );
  }

  sprintf( log_message,
	   "Begin focus setting:  %5.3lf", FOCUS);

  log_entry( log_message );

  HIFI[0] |= 0x04;   /* The first byte of HIFI contains flags which determine */
                     /* how trackloop will read or write to the HIFI D80.  Set*/
  focus_servo( prec );

  usleep( (unsigned long) 100000 );  /* Wait while trackloop sends the
                                        hold focus command. */

  HIFI[0] &= 0xfb;   /* then clear the bit which tells trackloop to write to */
                     /* the focus motor through HIFI D80 channel 2, port 5. */
  sprintf( log_message,
	   "End focus setting: %5.3lf", FOCUS);

  log_entry( log_message );

  printf("\n%6.3f    \a\n", FOCUS);
  exit( 0 );
}

/* end main */


help( ) {
  printf("\nCommand syntax: \"focus [desired [0], \"help\"]\"\n");
  printf("Description:  If given without an optional argument this command\n");
  printf(" displays the current focus position and exits.  If used with an\n");
  printf(" argument specifying a new focus position then said position is\n");
  printf(" obtained then displayed before exiting with a single beep.\n");
  printf(" If used with the argument \"help\" then this synopsis is\n");
  printf(" displayed before exiting.\n");
  printf("Argument:  A number between -0.870 and +1.120 which numbers\n");
  printf(" constitute the range of the Focus Controller.  Any other\n");
  printf(" argument causes the display of this synopsis before exiting.\n");
  printf("Precision: By default +/- 0.001; exact if \"0\" is specified\n");
  printf(" as an optional, second argument.\n");
  printf("Notes:  If the servo overshoots the desired position ten times\n");
  printf(" or if position feedback is not as expected then a diagnostic is\n");
  printf(" displayed before exiting with two beeps.  These diagnostics are\n");
  printf(" \"Instability\" and \"Malfunction: is Focus Controller switched to\n");
  printf(" \'remote\'?\" respectively.  This command makes use of the GPIB\n");
  printf(" equipment at the bottom of the Control Room rack as well as the\n");
  printf(" Cassegrain-focus sensor and actuator within the Secondary\n");
  printf(" Mirror Cell.\n\n");
}


focus_servo( int precision ) {

  double focus_error;
  double p = 0.001;
  double fastf_rate = 0.00774;
  double slowf_rate = 0.00454;
  double duration = 1.0;
  int i = 0;
  
  focus_error = DES_FOCUS - FOCUS;

  if( precision == 0 )
    p = 0.0;
    
  while(( fabsolut( focus_error )) > ( p + 0.0001 )) {

    printf("Precision: %7.4f    Error: %7.4f    ", p, focus_error);

    if( i++ >= 10 ) {
      printf("Instability.  Focus = %6.3f    \a\n", FOCUS);
      usleep( (unsigned long) 200000 );
      printf("\a");
      exit( 6 );
    }

    if(( duration = fabsolut( focus_error / fastf_rate )) > 5. )
      FOCUS_RATE = FASTF;
    else {
      FOCUS_RATE = SLOWF;
      duration = ( fabsolut( focus_error ) / slowf_rate );
    }

    printf("Duration: %7.4f\n", duration);

    if( focus_error > 0. )
      FOCUS_CMD = OUTF;
    else( FOCUS_CMD = INF );
    HIFI[5] = FOCUS_RATE | FOCUS_CMD;
    usleep( (unsigned long) (duration * 1000000.) );
    FOCUS_CMD = HOLDF;
    HIFI[5] = FOCUS_RATE | FOCUS_CMD;
    usleep( (unsigned long) 500000 );

    if(( focus_error > 0.003 ) && ( focus_error <= ( DES_FOCUS - FOCUS ))) {
      printf("Malfunction.  Focus = %6.3f    \n", FOCUS);
      printf(" Is Focus Controller switched to \'remote\'?\a\n");
      usleep( (unsigned long) 200000 );
      printf("\a");
      exit( 7 );
    }

    focus_error = DES_FOCUS - FOCUS;
  }
}


double fabsolut( double number ) {

  if( number < 0.0 )
    return( -1.0 * number );
  else
    return( number );
}