//---------------------------------------------------------
// Interrupt-Driven Version of test_io.c, with new timing 
// functions, and further developments.
//
// Franz Hover and Paul Dietz, December 1997
// Walt Disney Imagineering Research and Development, Inc.
// Major Mods February 1998 for force control of test fixture.
// Added command line interface for demos - abennett 9/98
//------------------------------------------------------------

#include "t2_struct.h"
#include "control_struct.h"
#include <stdio.h>
#include <string.h>

// ----------------------------------------------------------

double enc (unsigned short) ;
unsigned short gtob (unsigned short) ;
void control(double[],double[],struct p_st, struct logger_st,
	double dt, double data[][30], int option) ;
struct p_st get_p( int ) ;
struct h_st get_h(void) ;

//-----------------------------------------------------------------
// these must be global because signal() doesn't pass data, and we 
// need these quantities for exiting gracefully.

double y[3] ;  			/* spool voltages */
double data[55000][30] ;	/* logged data */
struct h_st h ; 		/* hardware structure */

// Randy Martin's timing stuff..---------------------------

double seconds_per_tick;
Long start_time64, stop_time64;

pid_t proxy1;    
volatile unsigned counter = 0;  /* for testing */

double bestcase=99999999, worstcase=0;
double diff;

// keyboard exit stuff ------------------------------------

sig_quit() 
{ 
	zero_DA(h.io, h.ad, y);
	write_data(h.logger,data) ;
	printf("\nQuit signal received.\n") ;
/*	printf("Worst-case Cycle Time = %lf sec, Best-case = %lf sec\n",
		 worstcase, bestcase );	
*/
	exit(0);
}

// more of Randy's timing stuff ---------------------------

#pragma off( check_stack );
pid_t far handler()
{
   	rdtsc64( &start_time64 );  // snap time

	++counter;
	//if ( !(counter % 1000) )
		Trace2( ((MYCODE <<12) | 1), 3, 0, counter );

	ClearIRQ16420();
	return(proxy1);
}
#pragma on( check_stack );

//------------------------------------------------------------------
//------------------------------------------------------------------
 
main( int argc, char *argv[] )
{
  int j;
  int option;                   /* control type option */
  double DADatavec[3] ;		/* Volts */
  double ADDatavec[16] ; 	/* Volts */
  double encoder[4];		/* radians */
  double t ;			/* time */
  double x[12] ; 		/* measurements passed to control() */
  int id;			/* proxy id number */
  struct _osinfo osdata;	/* operating system stuff (Randy) */
  Long diffl;			/* 64-bit arithmetic for timing */
  double diffd;			/*  "  */
  static double enc_last[4] ;	/* previous encoder values (for rate) */
  struct p_st p ; 		/* control structure */
  double dt ;			/* time step, sec */

  /* Parse out the command line and handle appropriately. */

  if ( argc == 1 ) { /* No commands.  Run default demo */
    printf("argc is 1. Default option is bar\n");
    option = 1;
  } else if ( argc == 2 ) {
    if ( strstr( "step" , argv[1] ) != NULL ) {
//      printf ( "Your arg is: step\n");
      option = 0;
    } else if ( strstr( "bar" , argv[1] ) != NULL ) {
//      printf ( "Your arg is: bar\n");
      option = 1;
    } else if ( strstr( "lift" , argv[1] ) != NULL ) {
//      printf ( "Your arg is: lift\n");
      option = 2;
    } else if ( strstr( "bulb" , argv[1] ) != NULL ) {
//      printf ( "Your arg is: bulb\n");
      option = 3;
    } else if ( strstr( "crush" , argv[1] ) != NULL ) {
//      printf ( "Your arg is: crush\n");
      option = 4;
    } else if ( strstr( "hop" , argv[1] ) != NULL ) {
//      printf ( "Your arg is: hop");
      option = 5;
    } else {
      printf("Illegal command option.  Defaulting to bar.\n");
      option = 1;
    }
  } else {
    fprintf(stdout, " Too many args (argc = %d).  Usage: t2 [step lift bar]\n Defaulting to t2 bar\n", argc );
    option = 1;
  }


  p = get_p(option);
  h = get_h() ;

  dt = 1./h.ad.rate ;
  
if (option > 2)   /* constrain option since arrays have only 3 rows. */
	option -= 3 ;

  /* randy's proxy stuff ---------------------------------------------*/
  
  if ( ( proxy1 = qnx_proxy_attach(0,0,0,-1) ) == -1 ) {
    fprintf(stderr, "Unable to acquire proxy (%s)\n", strerror(errno));
    exit(-1);
  }
  
  if ( setprio( getpid(), HIGHEST_PRIORITY ) == -1 ) {
    fprintf(stderr, "\nUnable to set priority.\n\n");
    exit(-1);
  }

  printf("Beginning sync ...\n");
  sync_pentium(); 
  printf("Sync complete.\n");

    bpconfig(h.ad) ;
	printf("Configured cards OK with burst clock.\n");

LoadDAC6420_2(0, 2048);
LoadDAC6420(1, 2048);
LoadDAC6420(0, 2048);

printf("\n\n\n\n\n");
  printf("---------------------------------------------------\n");
  printf("   *** Test Fixture Control (%s):  %d Hz *** \n\n", 
	  argv[1], (int)h.ad.rate);
  printf("              Hit any key to begin.\n") ;
  
  while (!kbhit()) {}
  getch() ;
  
  printf("---------------------------------------------------\n") ;
  printf("               Hit CTRL-C to exit.\n") ;
  printf("---------------------------------------------------\n\n");

  ClearIRQ16420() ;	
  SetIRQ16420(0,RTD_IRQ) ;
  ClearIRQ16420() ;	

  signal(SIGQUIT, &sig_quit);
  signal(SIGTERM, &sig_quit);
  signal(SIGKILL, &sig_quit);
  signal(SIGINT,  &sig_quit);

  if ( ( id = qnx_hint_attach( RTD_IRQ, &handler, 
			       FP_SEG( &counter)) ) == -1 ) {
    fprintf(stderr, "Unable to attach to irq\n");
    exit(1);
  }

    get_enc(encoder,h.io) ;  /* initialize last encoder values */
	for (j=0;j<4;j++) 
	{
	enc_last[j] = encoder[j] ;
        }

  ClearADFIFO6420();
  ClearADFIFO6420_2();
 if ( !IsADFIFOEmpty6420() ) 
 {
      printf("FIFO not empty!\n");
      exit(0);
  }
  StartConversion6420();
  
  while (1) {
    
    Receive( proxy1, 0,0 ) ; /* wait for a hit from the 6420 IRQ */
    
    if ( IsADFIFOEmpty6420() ) 
	{
      printf("FIFO empty but IRQ hit received.  Aborting.\n");
	zero_DA(h.io, h.ad, y);
	write_data(h.logger,data) ;
      	}

    put_DA(DADatavec, h.io, h.ad) ;
    
    get_enc(encoder,h.io) ;

    get_AD(ADDatavec, h.ad) ;
    
    if (h.io.MONITOR) {
      printf("t: %4.1f ", t) ;
      printf("Enc: ");
      for(j=0;j<4;j++)
	printf("%5.3f ", encoder[j]) ;
        printf("DA: ");
      for(j=0;j<3;j++)
	printf("%4.1f ", DADatavec[j]) ;
/*      printf("AD: ");
      for(j=0;j<h.ad.num_chan;j++)
	printf("%4.1f ", ADDatavec[j]) ;
*/
      printf("\n");
    }
    
    /* Load up the control input vector with encoder and other
	signals you want to log, then call control(),
       // and parse out the returned control Voltages.
       //------------------------------------------------------------*/
    
    x[0] = t ;				
    x[1] = ADDatavec[h.logger.accel_chan] ;			 
    x[2] = ADDatavec[h.logger.accel_chan+1] ;
    x[3] = ADDatavec[h.logger.accel_chan+2] ;
    x[4] = encoder[0] ;		
    x[5] = encoder[1] ;
    x[6] = encoder[2] ;
    x[7] = encoder[3] ;
    x[8] = ADDatavec[h.logger.pressure_chan] ;  
    x[9] = ADDatavec[h.logger.pressure_chan+1] ;
    x[10] = ADDatavec[h.logger.pressure_chan+2] ;
    x[11] = ADDatavec[h.logger.pressure_chan+3] ;
    
    control(y, x, p, h.logger, dt, data, option) ;
    
    DADatavec[0] = y[0] ;
    DADatavec[1] = y[1] ;  
    DADatavec[2] = y[2] ;
    
    /* Check to make sure we still have an empty buffer */
    while ( !IsADFIFOEmpty6420() ) {
      printf("\nFIFO NOT EMPTY -- SAMPLE RATE TOO FAST.\n");
	    get_AD(ADDatavec, h.ad) ;
    }
    
    
    rdtsc64( &stop_time64 );    /* timing stuff */
    _Lsub( &diffl, stop_time64, start_time64 ); 
    diffd = (float)diffl.lo * seconds_per_tick;
    if ( diffd > worstcase ) 
      worstcase = diffd;
    if ( diffd < bestcase ) 
      bestcase = diffd;

	if (t > p.pose.t) 	       
		check_enc(encoder,y,t,h,data) ;
	
	if (t > 10*dt)
		check_enc_rate(encoder,enc_last,y,dt,h,data) ;
	 
	check_emergency(ADDatavec[h.limit.emergency_channel],y, h,data) ;
	 
	t += dt ;
	for (j=0;j<4;j++) /* update encoder history */
	{
	     enc_last[j] = encoder[j] ;
	}
	 
  }	// while (1) ;
  
}

//------------------------------------------------------------

void write_data(struct logger_st logger, double data[][30]) 
{
  long int i;
  int j ;
  FILE *fout, *fopen() ;
 /* 
  fout = fopen("/home/ftp/pub/May14_logfiles/control.log","w") ;
 */
 fout = fopen("/home/ftp/pub/control.log","w") ;
  for(i=0;i<logger.rows;i++)
    {
      for(j=0;j<logger.cols;j++)
	{	
	  fprintf(fout,"%lf  ", data[i][j]) ;
	}
      fprintf(fout,"\n");
    }
  fclose(fout) ;
}

//--------------------------------------------------------
		
set_my_priority( int priority ) {
  int ret;
  
  ret = setprio( getpid(), priority );
  if ( ret == -1 ) {
    fprintf(stderr,"Error setting priority. (%s)\n",strerror(errno));
    exit(-1);
  }
  return(ret);
}

//---------------------------------------------------------

sync_pentium() {
  Long diff;
  double ticks_per_second;
  int previous_priority;
  
  /* get synchronization values for this Pentium */
  sleep(1);
  do {
    
    previous_priority = set_my_priority( HIGHEST_PRIORITY );
    rdtsc64( &start_time64 );  
    sleep(1);
    rdtsc64( &stop_time64 );  
    set_my_priority( previous_priority );
    
  } while (start_time64.lo > stop_time64.lo);
  
  _Lsub( &diff, stop_time64, start_time64 ); 
  
  ticks_per_second = (float)diff.lo;
  seconds_per_tick = 1.000000000 / ticks_per_second;
  
/*  printf( "\nPentium Clock Speed Measurement:\n\n" );
	printf( "1 second took %10.0lf cycles on this machine.\n", ticks_per_second );
	printf( "Each cycle is %8.3lf nanoseconds.\n",
              seconds_per_tick * NANOS_PER_SEC );
	printf( "This is a %3.1f MHz Pentium.\n\n",
		(float) ticks_per_second / 1000000.0);
*/
}


