// ac.c
//
// $Id: ac.c 123 2011-09-11 01:04:20Z csherwood $
// 
// arm control program
//
// requires a running gmsm-mx to provide input data from modtronix 
//  currently gets info from shared memory, written by gmsm-mx
//  (earlier version used a fifo pipe; code is here but not used)
// 
// csherwood@usgs.gov
// emontgomery.gov
// 10-Sept-2011
//

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <unistd.h>
#include <ctype.h>
#include <string.h>
#include <signal.h>
#include <math.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>

/* 
 * Must define one method for getting data from Modtronix:
 * DO_FIFO uses a file-based fifo pipe, and works with gmf.c
 * DO_PSHM uses a POSIX shared memory approach and works with gmsm.c
 */
#undef DO_FIFO
#define DO_PSHM

#if defined DO_FIFO
# include "fifo.h"
#endif

#if defined DO_PSHM
# include "pshm.h"
#endif

// Function declarations
int get_modx(void);
int sem_init( sem_t * sem, int pshared, unsigned value );
int apply_voltage( void );
int ramp_to(int updn,  int target_cvolts, int nsteps, int delay );
int move_arm( int value );
static void my_exit(int sig);
int my_output( FILE *fd, char *buf, int display_data );
int set_brake( int );
int stop_arm( void );
int set_voltage( int ivolts );
int send_command( void );

// These are defaults if the configuration file fails
#define TRAVEL_TIME 1600    // seconds (short for testing) allow about 10+ min for 20 deg
#define TARGET_TACH 550    // hopefully, a little faster
#define MIN_TACH 85
#define SCARY_AMPS 300     // want to stay below this (trips occur at ~340+)
#define NCYCLES 1
#define CYCLE_TIME 1200 // s
#define REF_ANGLE (0.0)
#define TOP_ANGLE (-20.0)  // This is good  (was +33)
#define BOT_ANGLE (-30.0) // This squashed the spring: try -33
#define MAXIMUM_CVOLTS 96	//72
#define MINIMUM_CVOLTS 24
#define MAX_FAILURES 2
#define ANGLE_PRECISION (0.25)
#define DISPLAY_DATA 1

#define NUM_VOLTS 21
#define MAX_MSG_SIZE 255
#define TRIPPED -1
#define NOT_TRIPPED 0
#define MAX_AMPS 350
#define ZERO    (0.0)

#define UP 1
#define DOWN -1
#define SUCCESS 1
#define FAILURE -1
#define BRAKE_OFF 1
#define BRAKE_ON 0

// Return codes for move_arm
#define UNKNOWN -1
#define NO_MODX 0
#define CANT_MOVE 2
#define TOO_LONG 3
#define MANY_FAILS 4
#define NO_TACH 5


#define MSG_SIZE 68         // data file lines must be this length

#define MAX_BUF_SIZE 255
#define MAX_FNAME_SIZE 127
#define MODX_IP "128.128.205.110"  // Value for WHOI node

#define min(m,n) ((m) < (n) ? (m) : (n))
#define max(m,n) ((m) > (n) ? (m) : (n))

//global variables (good idea?)
int present_cvolts=127; // present voltage requested (0 - 255)
int ibreaker;
int nfail;
// Declare variables to be read from .conf file and initialize with default values
int display_data=DISPLAY_DATA;		// 1=echo log entries to screen
int travel_time=TRAVEL_TIME;            
int target_tach=TARGET_TACH;
int min_tach=MIN_TACH;
int scary_amps=SCARY_AMPS;
int ncycles=NCYCLES;
int cycle_time=CYCLE_TIME;
int max_failures=MAX_FAILURES;
int maximum_cvolts=MAXIMUM_CVOLTS;
int minimum_cvolts=MINIMUM_CVOLTS;
float ref_angle=REF_ANGLE;
float top_angle=TOP_ANGLE;
float bot_angle=BOT_ANGLE;
float angle_precision=ANGLE_PRECISION;
int modx_ok=TRUE;
int itrust_angle=TRUE;
int itrust_modx_data=TRUE;

int dirn=1;  // Direction of arm motion: -1 = down, 1 = up
int fd=-1;
int* loc;
char *message, *fname;
char msg[MSG_SIZE], fn[10];
FILE *fdp_log, *fdp_cfg;
int fds_log;
char tmp[255];
char str1[10], str2[10], str3[10], str4[10], str5[10], str6[10];
char str7[10], str8[10], str9[10], str10[10], str11[10], str12[10], str13[10], str14[10];
char *conf_name="/var/sda/progs/ac.conf";

pid_t my_pid;
char data_msg[MAX_MSG_SIZE];

#if defined DO_FIFO
  int serverFd, dummyFd;
#endif

#if defined DO_PSHM
    int pshm_fd;
    char *addr;
    struct stat sb;
    sem_t *sem;
#endif

struct modx_data modx_data;

int main( int argc, char* argv[] )
{
  int i, tstat, tdif;
  int ir = -1, ier=0, result;
  time_t t,now;
  long ori_secs;
  int el_secs, rem_secs;

  struct tm gm, *gmp;
  struct timeval tv;
  char logfname[MAX_FNAME_SIZE];
  char buf[MAX_BUF_SIZE];
  const char svnId[80]="$Id: ac.c 123 2011-09-11 01:04:20Z csherwood $";

  // Initialize  
  signal(SIGINT,my_exit);  /* terminal interuppt (e.g., ctrl-C) */
  signal(SIGTERM,my_exit); /* terminate process */
 
  //first open the log file
  now=time(NULL);
  gmp = gmtime(&now);
  gm = *gmp;
  my_pid = getpid();
  // for use with cron, use absolute path to open a log file with a name that includes the PID
  snprintf(logfname,MAX_FNAME_SIZE,"/var/sda/logs/arm_ctrl_%04d%02d%02d%02d%02d%02d_%06d.log",
	   1900+gm.tm_year,gm.tm_mon+1,gm.tm_mday,gm.tm_hour,gm.tm_min,gm.tm_sec,(int)my_pid);
  
  fdp_log = fopen(logfname,"wt");
  if(fdp_log == NULL) {
    fprintf(stderr,"Problem opening log file %s\n",logfname);
    my_exit(SIGUSR1);
  }
  snprintf(buf,sizeof(buf),"%s",svnId);
  ier = my_output( fdp_log, buf, 1);

  
  // then open the config file (based on Sam Laney's code)
  snprintf(buf,sizeof(buf),"Config file name: %s",conf_name);
  ier = my_output( fdp_log, buf, 1);
  //ier = my_output( stdout, buf, 1);
  if( (fdp_cfg=fopen( conf_name, "rt")) == NULL ){
    snprintf(buf,sizeof(buf),"Could not open %s- this file must exist to provide inputs!", conf_name);
    ier = my_output( fdp_log, buf, 1 );
    //ier = my_output( stdout, buf, 1 );
    my_exit( SIGUSR1 );
  }
  else {
    // read and log the header
    snprintf(buf,sizeof(buf),"Opened config file: %s",conf_name );
    ier = my_output( fdp_log, buf, 1 );
       /* config file format expects a csv list- 1 line with 12 things:
       * travel_time, target_tach, scary_amps. ncycles, ref_angle,top_angle,
       * bot_angle, minimum_cvolts, maximum_cvolts, max_failures, 
       * angle_precision, and whether to display to screen (1) or not (0).
       * they go into str1-str12
       */
    // read header line from conf file
    fgets(tmp, 200, fdp_cfg);
    //then get the configuration parameters
    if (fgets(tmp, 200, fdp_cfg) == NULL){
      // if  EOF is reached on an fgets
      snprintf(buf,sizeof(buf),"No data in config file %s",argv[1]);
      ier = my_output( fdp_log, buf, display_data );
      my_exit( SIGUSR1 );
    }
    else {
      snprintf(buf,sizeof(buf), "%s", tmp);
      ier = my_output( fdp_log, buf, display_data );
    }
  }	    
  // put the config data into the right records:
  result = sscanf(tmp,"%[^','],%[^','],%[^','],%[^','],%[^','],%[^','],%[^','],%[^','],%[^','],%[^','],%[^','],%[^','],%[^','],%[^',']",
		  str1,str2,str3,str4,str5,str6,str7,str8,str9,str10,str11,str12,str13,str14);
  
  //now assign the strings to variables- use the # defines, if the conf file wasn't read right
  if (strlen(str1) > 0)
    travel_time=atoi(str1);
  else
    target_tach=TRAVEL_TIME;
  if (strlen(str2) > 0)
    target_tach=atoi(str2);
  else
    target_tach=TARGET_TACH;
  if (strlen(str3) > 0)
    min_tach=atoi(str3);
  else
    target_tach=MIN_TACH;
  if (strlen(str4) > 0)
    scary_amps=atoi(str4);
  else
    scary_amps=SCARY_AMPS;
  if (strlen(str5) > 0)
    ncycles=atoi(str5);
  else
    ncycles=NCYCLES;
  if (strlen(str6) > 0)
    cycle_time=atoi(str6);
  else
    cycle_time=CYCLE_TIME;
  if (strlen(str7) > 0)
    ref_angle=atof(str7);
  else
    ref_angle=REF_ANGLE;
  if (strlen(str8) > 0)
    top_angle=atof(str8);
  else
    top_angle=TOP_ANGLE;
  if (strlen(str9) > 0)
    bot_angle=atof(str9);
  else
    bot_angle=BOT_ANGLE;
  if (strlen(str10) > 0) 
    minimum_cvolts=atoi(str10);
  else
    minimum_cvolts=MINIMUM_CVOLTS;
  if (strlen(str11) > 0)
    maximum_cvolts=atoi(str11);
  else
    maximum_cvolts=MAXIMUM_CVOLTS;
  if (strlen(str12) > 0)
    max_failures=atoi(str12);
  else
    max_failures=MAX_FAILURES;
  if (strlen(str13) > 0)
    angle_precision=atof(str13);
  else
    angle_precision=ANGLE_PRECISION;
  if (strlen(str14) > 0)
    display_data=atoi(str14);
  else
    display_data=DISPLAY_DATA;
  
  // Write these values to the log file
  snprintf(buf,MAX_BUF_SIZE,"travel_time %d",travel_time);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"target_tach %d",target_tach);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"min_tach %d",min_tach);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"scary_amps %d",scary_amps);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"ncycles %d",ncycles);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"cycle_time %d",cycle_time);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"ref_angle %f",ref_angle);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"top_angle %f",top_angle);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"bot_angle %f",bot_angle);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"minimum_cvolts %d",minimum_cvolts);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"maximum_cvolts %d",maximum_cvolts);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"max_failures %d",max_failures);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"angle_precision %f",angle_precision);
  ier = my_output( fdp_log, buf, display_data );
  
  
#if defined DO_FIFO  
  /* Create well-known FIFO, and open it for reading */
  umask(0);     /* So we get the permissions we want */
  if (mkfifo(SERVER_FIFO, S_IRUSR | S_IWUSR | S_IWGRP) == -1
      && errno != EEXIST) {
    snprintf(buf,MAX_BUF_SIZE,"Error making mkfifo %s", SERVER_FIFO);
    ier = my_output( fdp_log, buf, display_data );
  }
  snprintf(buf,MAX_BUF_SIZE,"Made fifo");
  ier = my_output( fdp_log, buf, display_data );

  serverFd = open(SERVER_FIFO, O_RDONLY | O_NONBLOCK );
  if (serverFd == -1) {
    snprintf(buf,MAX_BUF_SIZE,"Error opening %s", SERVER_FIFO);
    ier = my_output( fdp_log, buf, display_data );
  }
  snprintf(buf,MAX_BUF_SIZE,"Opened serverFd");
  ier = my_output( fdp_log, buf, display_data );

  /* Open an extra write descriptor, so that we never see EOF */
  dummyFd = open(SERVER_FIFO, O_WRONLY);
  if (dummyFd == -1){
    snprintf(buf,MAX_BUF_SIZE,"Error opening %s", SERVER_FIFO);
    ier = my_output( fdp_log, buf, display_data );
  }
  snprintf(buf,MAX_BUF_SIZE,"Opened dummyFd");
  ier = my_output( fdp_log, buf, display_data );

  /* Let's find out about broken client pipe via failed write() */
  if (signal(SIGPIPE, SIG_IGN) == SIG_ERR){
    snprintf(buf,MAX_BUF_SIZE,"Problem with broken pipe signal");
    ier = my_output( fdp_log, buf, display_data );
  } 
  snprintf(buf,MAX_BUF_SIZE,"Checked SIGPIPE");
  ier = my_output( fdp_log, buf, display_data );
#endif /* DO_FIFO */

#if defined DO_PSHM
  /* Open existing shared memory object and semaphore */
  pshm_fd = shm_open(SHMEM_NAME, O_RDONLY, 0);    /* Open existing object */
  if (pshm_fd == -1){
    snprintf(buf,MAX_BUF_SIZE,"shm_open error");
    ier = my_output( fdp_log, buf, display_data );
    exit(-1);
  }
  
  /* Use shared memory object size as length argument for mmap()
     and as number of bytes to write() 
     (or, could use sizeof(moxa_data))
  */
  if (fstat(pshm_fd, &sb) == -1){
   snprintf(buf,MAX_BUF_SIZE,"fstat error");
   ier = my_output( fdp_log, buf, display_data );
   exit(-1);
  }
  
  addr = mmap(NULL, sb.st_size, PROT_READ, MAP_SHARED, pshm_fd, 0);
  if (addr == MAP_FAILED){
    snprintf(buf,MAX_BUF_SIZE,"mmap error");
    ier = my_output( fdp_log, buf, display_data );
    exit(-1);
  }
  
  if (close(pshm_fd) == -1){                    /* fd is no longer needed */
    snprintf(buf,MAX_BUF_SIZE,"sem close error");
   ier = my_output( fdp_log, buf, display_data );
   exit(-1);
  }
  
  sem = sem_open(SEM_NAME,O_RDWR,(S_IRUSR | S_IWUSR),1); /* open semaphore */
  if(sem==SEM_FAILED){
    snprintf(buf,MAX_BUF_SIZE,"sem_open error");
   ier = my_output( fdp_log, buf, display_data );
   exit(-1);
  }
#endif /* DO_PSHM */

  tstat=gettimeofday(&tv,NULL);
  gmp = gmtime(&tv.tv_sec);   
  gm=*gmp;
  snprintf(buf,MAX_BUF_SIZE,"started run at %s", asctime(gmp));
  ier = my_output( fdp_log, buf, display_data );
  
  // Check for Modtronix data
  i=0; ier=0; modx_ok = TRUE;
  while(i++<5 && ier != 1){
    ier=get_modx();
    sleep(1);
  }
  if(ier ==-1 ){
    snprintf(buf,MAX_BUF_SIZE,"No Modtronix data.");
    ier = my_output( fdp_log, buf, display_data );
    modx_ok = FALSE;
    my_exit( SIGUSR1 );
  }
  if(ier == 0){
    // See how stale the data is
    tdif = (int)(tv.tv_sec-modx_data.tv.tv_sec);
    snprintf(buf,MAX_BUF_SIZE,"data in shared memory is %d s different - restart gmsm-mx!",tdif);
    ier = my_output( fdp_log, buf, display_data );
    modx_ok = FALSE;
    my_exit( SIGUSR1 );
  }
  
  snprintf(buf,MAX_BUF_SIZE,"angle: %f status: %d amps: %d tach: %d cvolts: %d tripped: %d",
	   modx_data.angle[1],modx_data.istatus,modx_data.ad_current,
	   modx_data.ad_tach, modx_data.cvolts, modx_data.ibreaker);
  ier = my_output( fdp_log, buf, display_data );
  
  for (i=0; i<ncycles; i++)
    {
      t=time(NULL);
      ori_secs=(long)t;
      set_brake(BRAKE_OFF); // allow power to the motor - required prior to applying voltages

      ir = move_arm(DOWN);

      set_brake(BRAKE_ON); // turn off power to technadyne to set the brake
      snprintf(buf,MAX_BUF_SIZE,"Done moving down on cycle %d, ir = %d",i,ir);
      ier = my_output( fdp_log, buf, display_data );
      t=time(NULL);   //compute elapsed seconds from current time and start
      el_secs=(int)((long)(t-ori_secs));
      rem_secs = cycle_time-el_secs;
      if(rem_secs >= 1){
	snprintf(buf,MAX_BUF_SIZE,"cycle_time: %d el_secs: %d sleep for rem_secs = %d",
		 cycle_time,el_secs,rem_secs);
	ier = my_output( fdp_log, buf, display_data );
	sleep(rem_secs);
      }
      else {
	snprintf(buf,MAX_BUF_SIZE,"Problem: rem_secs =  %d",rem_secs);
	ier = my_output( fdp_log, buf, display_data );
      }

      t=time(NULL);
      ori_secs=(long)t;
      set_brake(BRAKE_OFF);

      ir = move_arm(UP);

      set_brake(BRAKE_ON); // turn off power to technadyne to set the brake
      snprintf(buf,MAX_BUF_SIZE,"Done moving up on cycle %d, ir = %d",i,ir);
      ier = my_output( fdp_log, buf, display_data );
      t=time(NULL);
      el_secs=(int)((long)(t-ori_secs));
      rem_secs = cycle_time-el_secs;
      if(rem_secs >= 1){
	snprintf(buf,MAX_BUF_SIZE,"cycle_time: %d el_secs: %d sleep for rem_secs = %d",
		 cycle_time,el_secs,rem_secs);
	ier = my_output( fdp_log, buf, display_data );
	sleep(rem_secs);
      }
      else {
	snprintf(buf,MAX_BUF_SIZE,"Problem: rem_secs =  %d",rem_secs);
	ier = my_output( fdp_log, buf, display_data );
      }
    }
  
  // finalize
  snprintf(buf,MAX_BUF_SIZE,"Graceful exit: ir = %d",(ir=0)); 
  ier = my_output( fdp_log, buf, display_data );
  my_exit(SIGUSR1);
  return( ir );
}

int move_arm( int direction )  //up (==1); dn (==-1)
{
  /* Return codes:
   * 1 = SUCCESS (quit on angle test)
   * 2 = CANT_MOVE Can't move (quit on multiple tries to overcome tripped breaker)
   * 3 = TOO_LONG Took too long
   * 4 = MAY_FAILS nfails exceeded on ramp-up
   * 5 = NO_TACH power on, but not turning
   * 0 = NO_MODX Serious problem (e.g., no info from Modtronix)
   * -1 = UNKNOWN Should not happen
   */
  int i;
  int ir=-1, ier=0;
  int keep_moving;
  float target_angle=0;
  time_t t;
  char buf[80];
  long ori_secs;
  float req_angle;
  int el_secs;
  int ncvolts;
 
  nfail = 0;
  ibreaker = NOT_TRIPPED;

  t=time(NULL);
    
  if (direction == UP) {
    target_angle= top_angle + ref_angle;
  }
  else if( direction == DOWN ) {
    target_angle= bot_angle + ref_angle;
  }
  else {
   snprintf(buf,MAX_BUF_SIZE,"wtf?");
   ier = my_output( fdp_log, buf, display_data );
  }

  snprintf(buf,MAX_BUF_SIZE,"Before applying voltage:");
  ier = my_output( fdp_log, buf, display_data );
  modx_ok=get_modx();
  snprintf(buf,MAX_BUF_SIZE,"angle: %f status: %d amps: %d tach: %d cvolts: %d tripped: %d",
	   modx_data.angle[1],modx_data.istatus,modx_data.ad_current,
	   modx_data.ad_tach, modx_data.cvolts,modx_data.ibreaker);
  ier = my_output( fdp_log, buf, display_data );
  req_angle = (target_angle - modx_data.angle[1]);
  if((req_angle - angle_precision)>0.){
    direction = UP;
    snprintf(buf,MAX_BUF_SIZE,"Need to move up %f deg",req_angle);
    ier = my_output( fdp_log, buf, display_data );
  } else if((req_angle + angle_precision)<0.){
    direction = DOWN;
    snprintf(buf,MAX_BUF_SIZE,"Need to move down %f deg",req_angle);
    ier = my_output( fdp_log, buf, display_data );
  } else {
    snprintf(buf,MAX_BUF_SIZE,"No move required %f",req_angle);
    ier = my_output( fdp_log, buf, display_data );
    return SUCCESS;
  }

  // TODO - Estimate required travel time here

  //time start of arm movement
  ori_secs=(long)t;
  snprintf(buf,MAX_BUF_SIZE,"seconds at start = %ld",ori_secs);
  ier = my_output( fdp_log, buf, display_data );
  snprintf(buf,MAX_BUF_SIZE,"In move_arm: direction is %d:",direction);
  ier = my_output( fdp_log, buf, display_data );
  ibreaker=ramp_to( direction, 48, 8, 1 );
  if( ibreaker==TRIPPED ){
    nfail++;
    snprintf(buf,MAX_BUF_SIZE,"Tripped on initial ramp-up.");
    ier = my_output( fdp_log, buf, display_data );
    sleep(2);
    set_brake( BRAKE_OFF );
    ibreaker=ramp_to( direction, 16, 8, 2 );
    // TODO - handle no motion but not tripped during ramp-up
    if (ibreaker==TRIPPED){
      nfail++;
      snprintf(buf,MAX_BUF_SIZE,"Tripped on slower ramp-up.");
      ier = my_output( fdp_log, buf, display_data );
      sleep(2);
      set_brake( BRAKE_OFF );
    }
  }

  t=time(NULL);   //compute elapsed seconds from current time and start

  if(nfail > max_failures){
    keep_moving = FALSE;
    stop_arm();
    snprintf(buf,MAX_BUF_SIZE,"Quit during ramp_to with %d fails.",nfail);
    ier = my_output( fdp_log, buf, display_data );
    return (ir=MANY_FAILS);
  }

  el_secs=(int)((long)(t-ori_secs));
  snprintf(buf,MAX_BUF_SIZE,"Elapsed time for ramp_to: %d s",el_secs);
  ier = my_output( fdp_log, buf, display_data );

  keep_moving = TRUE;
  while (keep_moving)
  {
    sleep(1);    

    snprintf(buf,MAX_BUF_SIZE,"Elapsed time: %d, req. power (present_cvolts-127): %d",el_secs,present_cvolts-127);
    ier = my_output( fdp_log, buf, display_data );
    // try three times to get data from Modtronix
    for(i=0; i<3; i++){
      ier = get_modx();
      if(ier==1) break;
      sleep(1);
    }

    /* Test to see if Modtronix data is current */
    if(ier!=1){
      snprintf(buf,MAX_BUF_SIZE,"Quit because no Modtronix data: ier = %d",ier);
      ier = my_output( fdp_log, buf, display_data );
      return (ir=NO_MODX);
    }

    snprintf(buf,MAX_BUF_SIZE,"angle: %f status: %d amps: %d tach: %d cvolts: %d tripped: %d",
	   modx_data.angle[1],modx_data.istatus,modx_data.ad_current,
	   modx_data.ad_tach, modx_data.cvolts,modx_data.ibreaker);
    ier = my_output( fdp_log, buf, display_data );

    /* Test to see if arm motion is taking too long */
    if(el_secs > travel_time){
      keep_moving = FALSE;
      stop_arm();
      snprintf(buf,MAX_BUF_SIZE,"Quit because it took too long: %d s",el_secs);
      ier = my_output( fdp_log, buf, display_data );
      return (ir=TOO_LONG);
    }

    /* Test to see if target angle has been reached */
    if( fabs(target_angle - modx_data.angle[1]) < angle_precision ){
     keep_moving = FALSE;
     stop_arm();
     snprintf(buf,MAX_BUF_SIZE,"Should be at target_angle: %6.2f; modx_data.angle = %6.2f",
	      target_angle,modx_data.angle[1]);
     ier = my_output( fdp_log, buf, display_data );
     return (ir=SUCCESS);
    }

    /* Test to see if power is on but no motion */
    if( (modx_data.ad_tach < (uint16)min_tach) & 
	(abs(present_cvolts-127) > minimum_cvolts) &
	(modx_data.ibreaker!=(uint16)TRIPPED) ){
      keep_moving = FALSE;
      stop_arm();
      snprintf(buf,MAX_BUF_SIZE,"Power on, but not turning.");
      ier = my_output( fdp_log, buf, display_data );
      return (ir=NO_TACH);
    }

    /* Test to see if breaker has been tripped */
    if (modx_data.ibreaker==(uint16)TRIPPED){
      nfail++;
      snprintf(buf,MAX_BUF_SIZE,"Breaker is tripped, nfail = %d",nfail);
      ier = my_output( fdp_log, buf, display_data );

      /* Try again at lower power */
      // ncvolts = (int)( 0.5 * (float)(abs(present_cvolts-127)));
      ncvolts=16;
      snprintf(buf,MAX_BUF_SIZE,"Trying to ramp up again to ncvolts= %d",ncvolts);
      ier = my_output( fdp_log, buf, display_data );
      set_brake( BRAKE_OFF );
      ibreaker=ramp_to( direction, ncvolts, 8, 2 );
      if(modx_data.ibreaker==(uint16)TRIPPED){
	nfail++;
	stop_arm();
	keep_moving=FALSE;
	snprintf(buf,MAX_BUF_SIZE,"Quit because breaker is STILL tripped, nfail = %d",nfail);
	ier = my_output( fdp_log, buf, display_data );
	return (ir=CANT_MOVE);
      }
    }
    
    /* Test to see if too many failures have accumulated */
    if(nfail > max_failures){
      keep_moving = FALSE;
      stop_arm();
      snprintf(buf,MAX_BUF_SIZE,"Quit during move loop with %d fails.",nfail);
      ier = my_output( fdp_log, buf, display_data );
      return (ir=MANY_FAILS);
    }


    // TODO - should probably base these adjustments on running averages

    /* Reduce power if approaching current limits */
    if ( (int)modx_data.ad_current >= (int)scary_amps) {
      snprintf(buf,MAX_BUF_SIZE,"Amps too high.");
      ier = my_output( fdp_log, buf, display_data );
      if( abs(present_cvolts-127) > minimum_cvolts){
	// reduce power by 5%
	ncvolts = (int)( 0.95 * (float)(abs(present_cvolts-127)));
	snprintf(buf,MAX_BUF_SIZE,"Trying to reduce to %d",ncvolts);
	ier = my_output( fdp_log, buf, display_data );
	set_voltage( -direction*ncvolts + 127 );
      } else {
	snprintf(buf,MAX_BUF_SIZE,"...but can't do anything: at min volts now.");
	ier = my_output( fdp_log, buf, display_data );
      }      
    } //end if high current
    
    // Increase power if tach is too low and current is ok
    if ( (int)modx_data.ad_current <= (int)(0.9*scary_amps) &&
	 (int)modx_data.ad_tach <= (int)(0.95*target_tach) ){
      snprintf(buf,MAX_BUF_SIZE,"Tach too low and amps ok.");
      ier = my_output( fdp_log, buf, display_data );
      if( abs(present_cvolts-127) < maximum_cvolts){
	// increase power by 5%
	ncvolts = (int)( 1.05 * (float)(abs(present_cvolts-127)));
	snprintf(buf,MAX_BUF_SIZE,"Trying to increase to %d",ncvolts);
	ier = my_output( fdp_log, buf, display_data );
	set_voltage( -direction*ncvolts + 127 );
      } else {
	snprintf(buf,MAX_BUF_SIZE,"but can't do anything: at max volts now.");
	// need to do something here
        ier = my_output( fdp_log, buf, display_data );
      }      
    } //end if low tach

    t=time(NULL);   //compute elapsed seconds from current time and start
    el_secs=(int)((long)(t-ori_secs));
  } //end while
  
return (ir=UNKNOWN);
}


// MY_EXIT - Clean up, close files, write to log, exit
static void my_exit(int sig)
{
	int ier;
  // sig is assigned by signal or passed by user
  // UNSAFE use of printf() flush(), close(), and exit()
  // TODO - Do this with async-safe calls?
  // TODO - send these to the log

  // (1) identify cause of exit and close log file
   stop_arm();
   set_brake(BRAKE_ON);
 switch (sig) {
  case SIGINT : {
    snprintf(tmp,sizeof(tmp), " ac-mx caught SIGINT (ctrl-C forced exit), exiting\n");
    ier = my_output( fdp_log, tmp, display_data );
    if (munmap(addr, sb.st_size) == -1) {
	    perror("Error un-mmapping the file");
    }
    close(pshm_fd);
    fclose(fdp_log);
    fflush(NULL);
    exit(0);
  }
  case SIGTERM : {
    snprintf(tmp,sizeof(tmp), " ac-mx caught SIGTERM (external kill forced exit), exiting\n");
    ier = my_output( fdp_log, tmp, display_data );
   if (munmap(addr, sb.st_size) == -1) {
	    perror("Error un-mmapping the file");
    }
    close(pshm_fd);
    fclose(fdp_log);
    fflush(NULL);
    exit(0);
   }
  case SIGUSR1 : {
    snprintf(tmp,sizeof(tmp)," ac-mx caught SIGUSR1 (program logic forced exit), exiting\n");
    ier = my_output( fdp_log, tmp, display_data );
    if (munmap(addr, sb.st_size) == -1) {
	    perror("Error un-mmapping the file");
    }
    close(pshm_fd);
    fclose(fdp_log);
    fflush(NULL);
    exit(0);
   }
  default : {
    snprintf(tmp,sizeof(tmp),"ac-mx: not sure why here, but exiting\n");
    ier = my_output( fdp_log, tmp, display_data );

    exit(0);
   }
}
}

int my_output( FILE *fd, char *buf, int disp_data )
{
  // Prepend time stamp, print contents of buf, add CR
  struct tm gm, *gmp;
  struct timeval tv;
  int iw=1, tstat, nlen=0;
  char tbuf[255];
   
  tstat=gettimeofday(&tv,NULL);
  // pointing at the tv seconds gets the time from gettimeofday, so usecs will be synched
  gmp = gmtime(&tv.tv_sec);   
  gm = *gmp; // save local copy...see TLPI p. 192
 
  snprintf(tbuf,29,"%4d %02d %02d %02d %02d %10.7f ",
 	  1900+gm.tm_year,gm.tm_mon+1,gm.tm_mday,gm.tm_hour,gm.tm_min,(gm.tm_sec+((double)tv.tv_usec/1000000)));

  //check that concatenated string isn't too big
  nlen=strlen(buf);
  if ((strlen(tbuf) + strlen(buf)) < sizeof(tbuf)-1)
  { 
   strcat(tbuf,buf);
   strcat(tbuf,"\n");
    if (disp_data ==1)				//display data and write to log file
    {
     iw = fprintf(stdout,"%s",tbuf);
     iw = fprintf(fd,"%s",tbuf);
    } else {
     iw = fprintf(fd,"%s",tbuf);	//just write to the log file
    }
   fflush(fd);
  }
  else
  {
    printf("string too long- look for error in data!\n");
    //email manager that something's wrong
     iw=-1;
  }
  return(iw);
}

// in these, send_command must precede my_output, since there's fflush in there
int set_brake( int val )
{ 
  // To reset, val = BRAKE_OFF (= 1)
  // to turn off power, val = BRAKE_ON (= 0)
  //
  // Initially: toggle B2 to 1 then 0, which resets the trip to 0
  // turning on the motor can only happen AFTER this is done.  
  int ier;
  if (val==BRAKE_OFF) // toggle tripped breaker   
    {
      snprintf(data_msg,MAX_MSG_SIZE,"b2=1");
        ier = send_command();
      //ier = my_output( fdp_log, data_msg, display_data );
     sleep(1);
      snprintf(data_msg,MAX_MSG_SIZE,"b2=0");
        ier = send_command();
      //ier = my_output( fdp_log, data_msg, display_data );
          sleep(1); 
    }
  // Instrument power on (val=1) or off (val=0)
  snprintf(data_msg,MAX_MSG_SIZE,"b3=%d",val);
  ier = send_command();
  //ier = my_output( fdp_log, data_msg, display_data );
   return ier;
}

int stop_arm( void )
{
  // Hex value of 127 should be control voltage for zero
  snprintf(data_msg,MAX_MSG_SIZE,"f=%02x",(int)127);
  return (send_command() );
}

int set_voltage( int iv )
{
  int iret=FAILURE;
  if(iv<0) iv=0;
  if(iv>255) iv=255;
  snprintf(data_msg,MAX_MSG_SIZE,"f=%02x",iv);
   if ( (iret=send_command()) == SUCCESS ){
    present_cvolts = iv;
  }
 return iret;
}

#if defined DO_FIFO
//TODO - The return value is not consistent with the shared memory version
int get_modx( void )
{
  // Get Motronix data from fifo pipe
  int ier = -1;
  int iret;
  int nfound;
  int i;
  int NATTEMPTS=100;
  char buf[MAX_BUF_SIZE];
  struct timespec sleep_time, remain_time;

  for (i=0, nfound=0;i<NATTEMPTS;i++){
    // sleep for .01 sec (TLPI, p. 494)
    sleep_time.tv_sec =0;
    sleep_time.tv_nsec =1000000;
    
    ier = nanosleep(&sleep_time, &remain_time );
    if(ier !=0 ){
      if(ier == EINTR){
	snprintf(buf,MAX_BUF_SIZE,"nanosleep interuppted by sig.handler");
	iret = my_output( fdp_log, buf, display_data );
      }
      else {
	snprintf(buf,MAX_BUF_SIZE,"Nanosleep error %d",ier);
	iret = my_output( fdp_log, buf, display_data );
      }
    }
    
    /* Read requests and send responses */
    if (read(serverFd, &modx_data, sizeof(struct modx_data))
	!= sizeof(struct modx_data)) {
      //printf(buf,MAX_BUF_SIZE,"Error reading modx_data; discarding\n");
      //snprintf(buf,MAX_BUF_SIZE,"Error reading modx_data; discarding");
      //iret = my_output( fdp_log, buf, display_data );
    }
    else
      nfound++;
 
  }
  if(nfound>0) ier=nfound;
  return ier;
}
#endif

#if defined DO_PSHM
int get_modx( void )
{
  // Get Modtronix data from shared memory
  // Return values as follows:
  // -1 = Serious problem
  //  0 = Got data, but is stale
  //  1 = Got data and is current

  int ier = -1;
  int i, tdif;
  char buf[MAX_BUF_SIZE];
  struct timespec abs_timeout;

  // check system clock and calculate timeout
  clock_gettime( CLOCK_REALTIME, &abs_timeout );
  abs_timeout.tv_sec+=0;                  // seconds
  abs_timeout.tv_nsec+=100000;            // about 1/10 sec
  ier=sem_timedwait( sem, &abs_timeout ); // blocks until sem>0, then decrements to zero
  if(ier==-1){
    // No modtronix data is forthcoming
    snprintf(buf,MAX_BUF_SIZE,"sem_timedwait > 1.1 sec");
    i = my_output( fdp_log, buf, display_data );
  }
  else {
    // update shared memory here
    memcpy(&modx_data, addr, sb.st_size);
    sem_post( sem ); // unblocks
    ier = 0;
    tdif = (int)(abs_timeout.tv_sec-modx_data.tv.tv_sec);
    if( abs(tdif)<2 ) ier = 1;
  }
  return ier;
}
#endif

// should the snprintf be in here or where called from?
// command to send is from global data_msg
int send_command( void )
{
  int sockfd;
  char *ip=MODX_IP;
  struct sockaddr_in their_addr;
  struct hostent *he;
  int numbytes;
  char buf[80];
  uint16_t MYPORT=54123;
  int ier=-1;

  /* get the host info */
  if ((he = gethostbyname(ip)) == NULL) {
    perror("Client-gethostbyname() error!");
    return FAILURE;
  }
  else {
    //printf("Client-gethostname() is OK...\n");    
  }
  
  if((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
    perror("Client-socket() error!");
    return FAILURE;
  }
  else {
    //printf("Client-socket() sockfd is OK...\n");    
  }
  
  /* host byte order */
  their_addr.sin_family = AF_INET;
  /* short, network byte order */
  their_addr.sin_port = htons(MYPORT);
  their_addr.sin_addr = *((struct in_addr *)he->h_addr);
  //printf("Using port: %d\n",MYPORT); 
  /* zero the rest of the struct */
  memset(&(their_addr.sin_zero), '\0', 8);
  
  if((numbytes = sendto(sockfd, data_msg, strlen(data_msg), 0, (struct sockaddr *)&their_addr, sizeof(struct sockaddr))) == -1){
    perror("Client-sendto() error!");
    return FAILURE;
  }
  else {
    // printf("Client-sendto() is OK...\n");
  }
   snprintf(buf,MAX_BUF_SIZE,"sent %s to %s", data_msg, inet_ntoa(their_addr.sin_addr));
   ier = my_output( fdp_log, buf, display_data );
  
  if (close(sockfd) != 0){
    printf("Client-sockfd closing failed!\n");
    return FAILURE;
  }
  else {
    //printf("Client-sockfd successfully closed!\n");
  }
  fflush(NULL);
  return SUCCESS;
}

// RAMP_TO - gradually increase voltage to target_val
int ramp_to(int updn,  int target_cvolts, int nsteps, int delay )
{
  int i;
  int ncvolts, delta_cvolts;

  // Requested control voltage will be 127+target_cvolts
  // remember: volts < 127 move arm UP
  // #define DOWN -1
  // #define UP 1
  // good idea to have target_cvolts and nsteps divisible by 2
  // TODO: the integer math is not bombproof

  // reasonable limits
  if (nsteps<=0)
    nsteps=1;
  if (nsteps>(abs(target_cvolts)))
    nsteps=abs(target_cvolts);
  if (delay<=0)
    delay=1;
  if (delay>10)
    delay=10;
  
  delta_cvolts = (-updn*target_cvolts)/nsteps;
  
  for (i=1; i<nsteps+1; i++){
    //    ncvolts = present_cvolts+delta_cvolts;
    ncvolts = 127+i*delta_cvolts;
    set_voltage( ncvolts );
    sleep(delay);
    get_modx();
    if( modx_data.ibreaker != (uint16)TRIPPED ){
      present_cvolts = ncvolts;
    } else {
      return( TRIPPED );
    }
  }
  return NOT_TRIPPED;
}