/*------------------------------------------------------------
  IO - a quick IO library for the RTD DM6810 digital I/O card,
  and the RTD 6420 Analog I/O card.
  
  Paul Dietz and Franz Hover
  Walt Disney Imagineering Research and Development Inc.
  Decemeber 2, 1997 
  
  Structure version made by fsh 4 February 1998
  --------------------------------------------------------------*/

#include "t2_struct.h"

/* gtob - gray code to binary. Converts an unsigned short gray code to
   an unsigned short binary. Based on Numerical Recipees in C code... */
//--------------------------------------------------------------------

unsigned short gtob(num)
     unsigned short num;
{
  unsigned short ish, ans, idiv;
  
  ish = 1;
  ans = num;
  
  for (;;) {
    ans ^= (idiv = ans >> ish);
    if (idiv <= 1 || ish == 16) return ans;
    ish <<= 1;
  }
}

//--------------------------------------------------------------------
/* enc - read an encoder. Returns a double which is the encoder angle
   in radians. */
//--------------------------------------------------------------------

double enc(encnum)
     unsigned short encnum;
{
  unsigned short num;
  double angle;
	int i ,junk,tempe ;
  
  switch (encnum) {
  case 0: 
    /* read from ports 0 and 2 (first digital board) */
    num = (inp(DM6810 + 4) << 8) | inp(DM6810);
    break;
  case 1:
    /* read from ports 1 and 3 (first digital board) */
    num = (inp(DM6810 + 5) << 8) | inp(DM6810 + 1);
    break;
  case 2: 
    /* read from ports 0 and 2 (second digital board) */
    num = (inp(DM6810_2 + 4) << 8) | inp(DM6810_2);
    break;
  case 3:
    /* read from ports 1 and 3 (second digital board) */
    num = (inp(DM6810_2 + 5) << 8) | inp(DM6810_2 + 1);
    break;
  default:
    fprintf(stderr,"Bad encoder number %d in module enc!\n", encnum);
    exit(2);
    break;
  }
  
  /* remove top 4 bits... */
  num &= 4095;

/* if(encnum==1){
	printf("Enc[1] num= %10d   : ",num );
	tempe = num ;
	for(i=11;i>=0;i--) {
		junk = (int)((float)tempe/pow(2.,(float)(i))) ;
		printf("%d", junk) ;
		tempe -= junk*pow(2.,(float)(i)) ; 
	}
}		
*/	
   
  /* Convert to radians */
  angle = gtob(num)/4096.*2.*PI ;

/*  
if (encnum == 1)
	printf("  %lf\n", angle) ;
*/

  if (angle >= PI) angle -= 2.*PI;
  if (angle < -PI) angle += 2.*PI ;

  return(angle);
}

//---------------------------------------------------------
// get_enc - gets four encoder values in rads, and corrects.
//---------------------------------------------------------

void get_enc(double encoder[], struct io_st io)
{
  encoder[0] = unwrap(enc(0) * io.enc_sign.hip  + io.enc_offset.hip) ;
  encoder[1] = unwrap(enc(1) * io.enc_sign.knee + io.enc_offset.knee) ;
  encoder[2] = unwrap(enc(2) * io.enc_sign.foot + io.enc_offset.foot) ;
  encoder[3] = unwrap(enc(3) * io.enc_sign.toe  + io.enc_offset.toe) ;
}

//----------------------------------------------------------------
// get_AD - fills up the AD data vector, USING ONLY THE FIRST 6420
//----------------------------------------------------------------

void get_AD(ADDatavec, ad)
     double ADDatavec[] ;
     struct ad_st ad ;
{	
  short j, temp ;
  
  for (j=0;j<ad.num_chan;j++) {
    temp = (short)(((inpw(BaseAddress + 4) << 1) &
		    65520))/16;
    ADDatavec[j] = (double)temp*ad.gain ;
  }
}

//----------------------------------------------------
// put_DA - sends three analog outputs to the D/A cards
//----------------------------------------------------

void put_DA(double DADatavec[], struct io_st io, struct ad_st ad)
{
  short temp ;
  char c ;
  
  DADatavec[0] -= io.act_sign.hip * io.act_offset.hip ;
  DADatavec[1] -= io.act_sign.knee * io.act_offset.knee ;
  DADatavec[2] -= io.act_sign.foot * io.act_offset.foot ;
  
  DADatavec[0] *= io.act_sign.hip ;
  DADatavec[1] *= io.act_sign.knee ;
  DADatavec[2] *= io.act_sign.foot ;

  temp = (short)(DADatavec[0]/ad.gain) + 2048 ;
  LoadDAC6420_2(0, temp); 
  
  temp = (short)(DADatavec[1]/ad.gain) + 2048 ;
  LoadDAC6420(1, temp);
  
  temp = (short)(DADatavec[2]/ad.gain) + 2048 ;
  LoadDAC6420(0, temp);
  
}

//-----------------------------------------------
// zero_DA - shut down the valves!
//-----------------------------------------------

void zero_DA(struct io_st io, struct ad_st ad, double spool[]) 
{
  double v[3], vg[3], xg[3], x[3] ;
  double junk[16] ;  
  double bw ;
  double xtemp[3] ;
  double dt ;
  int i, j, n ;
  
  dt = 1. / ad.rate ;
  bw = 6.7/io.stop_time ;
  
  n = (int) (io.stop_time/dt) ;
  
  for (i=0;i<3;i++)
    {
      v[i] = 0. ;
      x[i] = spool[i]  ;
    }
  
  for (j=0;j<n;j++) 
    {

      while(IsADFIFOEmpty6420()) {}
      get_AD(junk, ad) ;  
      if (!IsADFIFOEmpty6420()) {}
      
      for(i = 0 ; i<3 ; i++)
	{
	  /* Euler method guess ... */
	  xg[i] = x[i] + dt*v[i] ;
	  vg[i] = v[i] + dt*(-bw*bw*x[i] - 2.*bw*v[i]) ;
	  
	  /* then make one correction ... */
	  x[i] += dt*(v[i] + vg[i])/2. ;
	  v[i] += dt*(-bw*bw*(x[i]+xg[i])/2. - 
		      2.*bw*(v[i]+vg[i])/2.) ;
	  
	  xtemp[i] = x[i] ;
	}
      
      put_DA(xtemp,io,ad) ;    /* put_DA corrupts the passed variable!!!*/
    }
	xtemp[0] = 0. ;
	xtemp[1] = 0. ;
	xtemp[2] = 0. ;
	put_DA(xtemp,io,ad) ;
}

//----------------------------------------------------------------------
// bpconfig - configure the analog board using both the burst clock and
// the pacer clock.  The only thing we do for the second 6420 board
// is initialize it, so we can do D/A conversions.
//----------------------------------------------------------------------

void bpconfig(struct ad_st ad) 
{
  ADTableRow ADTable[16] ;
  int i ;
  
  SetBaseAddress(DM6420) ;
  SetBaseAddress_2(DM6420_2) ;
  InitBoard6420() ;
  InitBoard6420_2() ;
  
  for(i=0;i<ad.num_chan;i++) {	
    ADTable[i].Channel = ad.first_chan + i ;
    ADTable[i].Gain = 0 ;		
    ADTable[i].Skip = 0 ;		
    ADTable[i].ADRange = 1 ; 	
    ADTable[i].Se_Diff = 0 ;	
    ADTable[i].Pause = 0 ;		
  }
  
  LoadADSampleCounter6420(ad.num_chan) ;
  
  SetPacerClock6420((float)ad.rate) ;
  SetBurstClock6420((float)ad.burst_rate) ;
  SetBurstTrigger6420(1) ;		
  SetStartTrigger6420(0);			
  SetStopTrigger6420(0);
  SetConversionSelect6420(2) ;            
  LoadADTable6420(ad.num_chan,ADTable) ;
  EnableTables6420(1,0) ;
  ClearADFIFO6420() ;

}

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

void check_enc(double enc[],double spool[],double t, struct h_st h, 
	double data[][30]) 
{
  
  if (enc[0] > h.limit.max_enc[0] ||
      enc[1] > h.limit.max_enc[1] ||
      enc[2] > h.limit.max_enc[2] ||
      enc[0] < h.limit.min_enc[0] ||
      enc[1] < h.limit.min_enc[1] ||
      enc[2] < h.limit.min_enc[2] ) 
    {
      
      zero_DA(h.io,h.ad,spool) ;
	printf("%lf %lf %lf %lf\n", enc[0],enc[1],enc[2],enc[3]) ;
      write_data(h.logger, data) ;
      printf("\n\n*****************************************************\n");
      printf("ENCODER BOUNDS EXCEEDED -- SHUTDOWN VALVES AND ABORT.\n");
      printf("*****************************************************\n");
      
      exit(2) ;
      
    }
}

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

void check_enc_rate(double enc[], double enc_last[],
		    double spool[], double dt,struct h_st h, double data[][30]) 
{
  if ((fabs(unwrap(enc[0] - enc_last[0]))/dt > h.limit.max_enc_rate[0]) ||
      (fabs(unwrap(enc[1] - enc_last[1]))/dt > h.limit.max_enc_rate[1]) ||
      (fabs(unwrap(enc[2] - enc_last[2]))/dt > h.limit.max_enc_rate[2]) ||
      (fabs(unwrap(enc[3] - enc_last[3]))/dt > h.limit.max_enc_rate[3]) ) 
    {
	printf("%lf %lf %lf %lf\n", unwrap(enc[0] - enc_last[0]), 
		unwrap(enc[1] - enc_last[1]), 
		unwrap(enc[2] - enc_last[2]), 
		unwrap(enc[3] - enc_last[3])) ;

      zero_DA(h.io,h.ad,spool) ;
      write_data(h.logger, data) ;			
      printf("\n\n*******************************************************\n");
      printf("MAX ENCODER RATE EXCEEDED -- SHUTDOWN VALVES AND ABORT.\n");
      printf("*******************************************************\n");
      exit(2) ;
    }
}

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

void check_emergency(double volts, double spool[], 
		     struct h_st h, double data[][30]) 
{
  if (fabs(volts) < h.limit.emergency_minV) 
    {
      zero_DA(h.io,h.ad,spool) ;
      write_data(h.logger,data) ;
      printf("\n************************************\n");
      printf("  EMERGENCY EXIT CONDITION DETECTED.\n");
      printf("************************************\n");
      exit(2) ;
    }
}

	



