/*****************************************************************************/
/* 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 );

struct  wiro_memory *tinfo, *get_tinfo();


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

  double val;
  char	log_message[ 64 ];

  tinfo = get_tinfo();

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

  else if (argc == 2) {
    if (sscanf( argv[ 1 ], "%lf", &val) != 1) {
      printf("\nArgument not recognized: %s\n", argv[ 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;
  }

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

  sprintf( log_message,
	   "Begin focus setting:  %5.2lf", 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( );

  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.2lf", FOCUS);

  log_entry( log_message );

  /* end main */ }


help( ) {
  printf("\nCommand syntax: \"focus [desired]\"\n");
  printf("Description:  If given without the 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("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("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( ) {

  double focus_error;
  double fastf_rate = 0.010;
  double slowf_rate = 0.005;
  double duration;
  
  focus_error = DES_FOCUS - FOCUS;

  while( fabs( focus_error ) > 0.001 ) {
    if( duration = fabs( focus_error ) / fastf_rate > 5. )
      FOCUS_RATE = FASTF;
    else {
      FOCUS_RATE = SLOWF;
      duration = ( fabs( focus_error ) / slowf_rate );
    }
    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 );
    focus_error = DES_FOCUS - FOCUS;
  }
}