/************************************************************** 
 
  Controller for hydraulic leg test fixture.  
  
  Calling syntax:  control2(spool,x,p,logger, dt, data);

where 	spool = three spool Voltages
	x = [time, acceleration(3), encoder(4), pressure(4)]
  	p = control parameters structure 
	logger = logging parameters (sub)structure
	dt = time step (s)
	data = data storage matrix

****************************************************************/

#include "control_struct.h"
#include "t2_struct.h"

void control(double spool[], double x[], struct p_st p, 
	     struct logger_st logger, double dt, double data[][30],int program)
{
  double t, acc[3], enc[4], pressure[4] ; 
  static double enc_last[4], enc_llast[4] ;
  double traj[3], trajdot[3] ;  /* trajectory generator baggage */
  static double traj_last[3], trajdot_last[3] ;
  double co[3], si[3];  	/* compact cosines and sines */
  double goal[3] ; 		/* goal in radians */
  static double goal_orig[3] ; 	/*!!either cartesian or joint coords!!*/
  double xygoal[2] ;		/* goal in xy coords */
  double xy[2] ;      		/* actual xy coordinates */
  int i,j ;			/* utility */
  static long icount = 0 ;		/* counts rows in the data matrix */
  static int count_skip ;	/* counts calls to this function */		
  double zero_force_e32 ;		/* encoder difference at zero force */
  static double dx = 0. ;		/* cartesian deflection due to force */
  double Fdes = 200. ;                   /* Was 60 */
 
  /* parse the input vector */
  t = x[0] ;
  acc[0] = x[1] ;
  acc[1] = x[2] ;
  acc[2] = x[3] ;
  enc[0] = x[4] ;
  enc[1] = x[5] ;
  enc[2] = x[6] ;
  enc[3] = x[7] ;
  pressure[0] = x[8] ;
  pressure[1] = x[9] ;
  pressure[2] = x[10] ;
  pressure[3] = x[11] ;

 if (t < 1e-6)
	count_skip = logger.write_skip ;

  /* if nobody else modifies spool[], guarantee no movement */
  for (i=0 ; i<3 ; i++)
    {
      spool[i] = 0. ;
    }
  
  /* if inside the pose time, keep redefining goal origin point,
	so that cyclic motion is about the actual configuration */

   co[0] = cos(enc[0]) ;
   co[1] = cos(enc[0] + enc[1]) ;
   co[2] = cos(enc[0] + enc[1] + enc[2]) ;
   si[0] = sin(enc[0]) ;
   si[1] = sin(enc[0] + enc[1]) ;
   si[2] = sin(enc[0] + enc[1] + enc[2]) ;

  if ( 	(t < p.ol.t[0] + dt + p.pose.t) ||
	(t < p.ol.t[1] + dt + p.pose.t) ||
	(t < p.ol.t[2] + dt + p.pose.t)       )
    {
      if(p.move.CARTESIAN[program])
	{
	  goal_orig[0] = p.robot.l[0]*co[0] + p.robot.l[1]*co[1] + 
	    p.robot.l[2]*co[2] ;
	  goal_orig[1] = p.robot.l[0]*si[0] + p.robot.l[1]*si[1] + 
	    p.robot.l[2]*si[2] ;
	  goal_orig[2] = 0. ;  // (not used in cartesian mode)
	zero_force_e32 = enc[3] ;
				    }
      else
	{
	  for(i=0;i<3;i++)
	    {
	      goal_orig[i] = enc[i] ;
	    }
	}
    }
  
  /*---------------------------------------------------------
    // also, keep re-intitializing the goal, trajectory generator and 
    // ring buffers as long as there are open-loop commands going on. */
    
  if ( (t < p.ol.t[0] + dt) || (t < p.ol.t[1] + dt) ||
       (t < p.ol.t[2] + dt) ) 
    {
      for (i=0 ; i<3 ; i++)
	{
	  goal[i] = enc[i] ;
	  traj_last[i] = enc[i] ;
	  trajdot_last[i] = 0. ;
	  enc_last[i] = enc[i] ;
	  enc_llast[i] = enc[i] ;
	}
 	enc_last[3] = enc[3] ;
  	enc_llast[3] = enc[3] ;
    }

/* this is the cartesian deflection due to compression */
/* moved the gain off of the spring and onto the damping */

  if ( 	(t > p.ol.t[0] + dt + p.pose.t) &&
	(t > p.ol.t[1] + dt + p.pose.t) &&
	(t > p.ol.t[2] + dt + p.pose.t)       )
    {
/* Compliance Control ...  */
	if (program == 1 && p.move.amp[1][0] < 0.001) 
  		dx = -p.servo.panto[program]*unwrap(enc[3]-zero_force_e32) - 
			p.servo.pantod*unwrap(enc[3]-enc_llast[3])/2./dt ;

/*                    ... OR force control. */
     if (program == 0 && p.move.amp[0][0] < 0.001) {
       if ((t > dt + p.pose.t) && (t < dt + p.pose.t + 20.) ) {
	 dx -= dt*.03*36000/Fdes*((enc[3] - zero_force_e32)*.5 - Fdes/36000.) ;
       }
       else if (t > dt + p.pose.t + 20.) {
	 dx -= dt*sign(dx)*.03 ;
       }
     }
    }
  
  get_goal(t,goal,goal_orig,p,dx,enc,program,dt) ; 
  
  get_traj(goal, traj_last, trajdot_last, dt, traj, trajdot, t, p, program) ;

  get_spool(traj, enc, enc_last, enc_llast, p.servo, spool, dt) ;
  
  for (i=0;i<3;i++)
    {
      spool[i] += p.servo.vel_ffwd[i]*trajdot[i] ;
    }		
  
  open_loop(t, spool, p.ol) ;  /* can overwrite spool[], or pass through */
  
  add_dither(t,spool,p.servo) ;	/* always additive --> corrupts spool[] */
  
  for(i=0;i<3;i++)
    {
      spool[i] = ssat(spool[i],p.servo.max_spool) ;    
    }
  
  /* log the data */
  if ((count_skip >= logger.write_skip) && (icount < logger.rows)) {
      for (j=0;j<12;j++)
	data[icount][j] = x[j] ;
      for (j=0;j<3;j++)
	data[icount][j+12] = spool[j] ;
      for (j=0;j<3;j++)
	data[icount][j+15] = goal_orig[j] ;
      for (j=0;j<3;j++)
	data[icount][j+18] = goal[j] ;
      for (j=0;j<3;j++)
	data[icount][j+21] = traj[j] ;

      xygoal[0] = p.robot.l[0]*cos(goal[0]) + 
		p.robot.l[1]*cos(goal[0]+goal[1]) +
		p.robot.l[2]*cos(goal[0]+goal[1]+goal[2]) ;
      xygoal[1] = p.robot.l[0]*sin(goal[0]) + 
		p.robot.l[1]*sin(goal[0]+goal[1]) +
		p.robot.l[2]*sin(goal[0]+goal[1]+goal[2]);

      for (j=0;j<2;j++)
	data[icount][j+24] = xygoal[j] ;
      
      xy[0] = p.robot.l[0]*co[0] + p.robot.l[1]*co[1] +
	p.robot.l[2]*co[2] ;
      xy[1] = p.robot.l[0]*si[0] + p.robot.l[1]*si[1] +
	p.robot.l[2]*si[2];

      for (j=0;j<2;j++)
	data[icount][j+26] = xy[j] ;
  
      icount++ ;
      count_skip = 0 ;
   }
   count_skip++ ;
  
  /* update ring buffers */
  for (i=0;i<4;i++)
    {
      enc_llast[i] = enc_last[i] ;
      enc_last[i] = enc[i] ;
    }
  
  if (p.DEBUG) {
    printf("enc errors: %6.2f %6.2f %6.2f (rad) --> CONTROL: %6.4f %6.4f %6.4f (V)\n",  
		unwrap(enc[0]-traj[0]), unwrap(enc[1]-traj[1]),
		unwrap(enc[2]-traj[2]), spool[0], spool[1],spool[2]); 
  }
}

/**********************************************************/

/**********************************************************/

double ssat(double x, double max)
{
	if (x > max)
		return(max) ;
	if (x < -max)
		return(-max) ;
	return(x) ;
}

/**********************************************************/

void add_dither(double t, double spool[],struct servo_st servo) 
{
	int i ;
	for (i=0;i<3;i++)
	{
		spool[i] += servo.dither_amp[i]*sin(t*servo.dither_freq[i]) ;	
	}
}

/**********************************************************/

double unwrap(double x) 
{  
  while(x > PI)
    { 
      x -= 2.*PI ;
    }
  while(x < -PI)
    {
      x += 2.*PI;
    }
  return(x);
}

/***********************************************************/
/* Open-loop spool commands                                 */
/************************************************************/

void open_loop(double t, double spool[],struct ol_st ol) 
{ 
	int i ;
	for (i=0;i<3;i++)
	{
		// Either write some open-loop spool command ...

  		if ( t < ol.t[i] ) 
		{
			if(!ol.STEP)
			{
	    			spool[i] = ol.amp[i]*sin(t*ol.freq[i]) ;
			}
			else
			{
				if (t < ol.t[i]/4.)
					spool[i] = ol.amp[i] ;
				else if (t < ol.t[i]*2./4.)
					spool[i] = -ol.amp[i] ;
				else if (t < ol.t[i]*3./4.)
					spool[i] = ol.amp[i] ;
				else 
					spool[i] = -ol.amp[i] ;
 			}
    		}	
	}
	// OR just pass existing commands through.
}

/*********************************************************/
/* Highest-level position commands                       */
/*********************************************************/

void get_goal(double t, double goal[],double goal_orig[],struct p_st p,
	double dx,double enc[],int program,double dt) 
{
  double t_temp ;
  double env ;
  static double tempx, tempy ;
  int i ;
  double xygoal[2] ;
  double DX_THRESH = 0.01 ; 
  double DROP_SPEED = 0.01 ;
  static int flag = 0 ;
  static double x_last ;
  static double yfilt = 0. ;
  double alpha = 1. ; // 1 for all-pass, 0 for no-pass
  double period ;
  
  // first, get a time scale for use in this routine:
  // t_temp = 0 corresponds with the end of open-loop motions.
  t_temp = t - p.ol.t[0] ;
  if ( (t - p.ol.t[1]) < t_temp) 
    {
      t_temp = t - p.ol.t[1] ;
    }
  if ( (t - p.ol.t[2]) < t_temp)
    {
      t_temp = t - p.ol.t[2] ;
    }
  
  if ( t_temp > 0. )
    {
      /* pose motion */
      if (t_temp < p.pose.t)	
	{
	  goal[0] = p.pose.enc[program][0] ;
	  goal[1] = p.pose.enc[program][1] ;
	  goal[2] = p.pose.enc[program][2] ;
	}
      /* or cyclic motion */
      else
	{
	  env = (1. - exp(-(t_temp-p.pose.t)/p.move.env_tau)) ;
	  if (p.move.CARTESIAN[program]) 
	    {	      
	      /* A:  this part for akhil's demo 
		 if ((dx < -DX_THRESH) || flag) 
		 {
		 flag = 1 ;
		 xygoal[0] = x_last ;
		 }	
		 else if (!flag)
		 {
		 xygoal[0] = goal_orig[0] + DROP_SPEED*
		 (t_temp-p.pose.t) ;
		 x_last = xygoal[0] ; 
		 }
		 
		 xygoal[1] = goal_orig[1] ;
	      */
/* B:  this part for original stuff ...*/
	      if (program == 0){
		period = 2.*PI/p.move.freq[program][1] ;
		if ( (t_temp-p.pose.t)/period < 1. ||
		     (int)(((t_temp-p.pose.t)/period-.5)*2.)%4 < 1 ) { 
		  
		  tempx = sin(p.move.freq[program][0]*
			      (t_temp-p.pose.t) + p.move.phase[program][0]) ;
		  tempy = sin(p.move.freq[program][1]*
			      (t_temp-p.pose.t) + p.move.phase[program][1]) ;
		  
		}
		else {
		  tempy -= dt*2./3./period*2. ;
		}
	      }
	      else {
		tempx = sin(p.move.freq[program][0]*
			    (t_temp-p.pose.t) + p.move.phase[program][0]) ;
		tempy = sin(p.move.freq[program][1]*
			    (t_temp-p.pose.t) + p.move.phase[program][1]) ;
	      }
/*	printf("t: %f   tempx:  %f   tempy:  %f\r", t_temp-p.pose.t,
						tempx,tempy) ;
	      */
	      
// Step program:  half-circle by cutting off negative part.
	      if (program <= 0.5){ 
		if (tempx > 0.5)
		  {
		    tempx = 0.5 ;
		  }
		if (tempy > 1.)
		  {
		    tempy = 1. ;
		  }
	      }
	      
	      yfilt = (1.-alpha)*yfilt + 
		alpha*p.move.amp[program][1]*env*tempy ;
	      
	      xygoal[0] = goal_orig[0] + 
		p.move.amp[program][0]*env*tempx + dx ;
	      xygoal[1] = goal_orig[1] + yfilt ;
	      
/* this part for either */
	      invkin(xygoal,goal,p.robot,p.pose,enc,program) ;
	    }
	  else  
	    {
	      for(i=0;i<3;i++)
		{
// for step, use sign(sin(p.move.freq[program][i]*
// for sine wave, omit 'sign'  - aab Feb 13 98
		  goal[i] = goal_orig[i] + 
		    p.move.amp[program][i]*env*
		    (sin(p.move.freq[program][i]*
			 (t_temp-p.pose.t)+
			 p.move.phase[program][i])) ;
		}
	    }
	}
    }
  
  // otherwise, don't change anything.  control() will keep
  // re-initializing goal and goal_orig until the open-loop
  // move is done.
}

/*********************************************************/
/* Trajectory-generator:  traj[] tries to follow goal[], */
/*  constrained by max_speed and a time constant tau.    */
/*********************************************************/

void get_traj(double goal[], double traj_last[], double trajdot_last[],
	      double dt, double traj[], double trajdot[],double t,
	      struct p_st p, int program) 
{  
  	double alpha, beta ;
  	double dx ;
  	int i ;
	double tmax_speed[3], ttau[3] ;

  // if posing, slow trajectory way down 
  	if ( 	(t < (p.pose.t + p.ol.t[0])) &&
		(t < (p.pose.t + p.ol.t[1])) &&
		(t < (p.pose.t + p.ol.t[2]))    ) 
	{
		for (i=0;i<3;i++)
		{
			tmax_speed[i] = .1 ;
			ttau[i] = .5 ;
		}
  	}
	else
	{
		for (i=0;i<3;i++) 
		{
			tmax_speed[i] = p.servo.max_speed[i] ;
			ttau[i] = p.servo.tau[program][i] ;
		}
 	}

 	for (i=0 ; i<3 ; i++) 
	{ 
		alpha = exp(-dt/ttau[i]) ;
		beta = 1. - alpha ;

      		dx = goal[i] - traj_last[i] ;
      		trajdot[i] = alpha*trajdot_last[i] + 
			beta*tmax_speed[i]*sign(dx) ; 
      		if (fabs(dx/ttau[i]) > tmax_speed[i]) 
		{
	  		traj[i] = traj_last[i] + trajdot[i]*dt ;
		}
      		else 
		{
	  		traj[i] = alpha*traj_last[i] + beta*goal[i] ;
	  		trajdot[i] = (traj[i]-traj_last[i])/dt ;
		}
		traj_last[i] = traj[i] ;
		trajdot_last[i] = trajdot[i] ;
   	}
}

/***********************************************************/

double sign(double x) {
	if (x < 0.)
		return(-1.) ;
	if (x > 0.)
		return(1.) ;
	return(0.) ;
}

/***********************************************************/
/*                  position controller                    */
/***********************************************************/

void get_spool(double traj[], 
	       double enc[], double enc_last[], double enc_llast[], 
	       struct servo_st servo, double spool[],
	       double dt) 
{
  int i ;
  
  for (i=0;i<3;i++)
    {   
    spool[i] = -servo.kp[i] * unwrap(enc[i] - traj[i]) - 
		servo.kd[i] * unwrap(enc[i] - enc_last[i])/dt - 
		servo.kdd[i] * unwrap(enc[i] - 2.*enc_last[i] + 
			enc_llast[i])/dt/dt ;
    }
}  

//-------------------------------------------------------------
// invkin -- inverse kinematics 
//-------------------------------------------------------------

void invkin(double xy[],double theta[], struct robot_st robot,
	struct pose_st pose,double enc[],int program)
{
	double c2, s2, k1, k2 ;
	double xyt[2] ;

	/* CASE 1:  use the hip and knee alone */
	/* We assume the foot is actuated */

	if (robot.ii < 1) 
	{
		/* assuming foot stays fixed in inertial frame,
		subtract its component of xy --> xyt is heel joint.*/

		xyt[0] = xy[0] - robot.l[2]*cos(pose.enc[program][0] +
			pose.enc[program][1] + pose.enc[program][2]) ;
		xyt[1] = xy[1] - robot.l[2]*sin(pose.enc[program][0] +
			pose.enc[program][1] + pose.enc[program][2]) ;

		/* use Craig's formulas to get theta[0] and theta[1] */

		c2 = (xyt[0]*xyt[0] + xyt[1]*xyt[1] - 
			robot.l[0]*robot.l[0] - robot.l[1]*robot.l[1]) / 
			2. / robot.l[0] / robot.l[1] ;
		if (fabs(c2) > 1.) // do the best you can if outside workspace
			c2 = sign(c2) ;
		s2 = robot.sign_s2*sqrt(1. - c2*c2) ;
		theta[1] = atan2(s2,c2) ;
		k1 = robot.l[0] + robot.l[1]*c2 ;
		k2 = robot.l[1]*s2 ;
		theta[0] = atan2(xyt[1],xyt[0]) - atan2(k2,k1) ;
	
		/* finally, make sure theta[2] will keep the foot 
		oriented properly in the inertial frame. */

		theta[2] = pose.enc[program][2] - 
			(theta[0] - pose.enc[program][0]) -
			(theta[1] - pose.enc[program][1]) ;
	}

	/* CASE 2:  use the knee and foot alone */
	/* We assume the hip is free to drift (on Sept 22, no hip actuation!) */
    /* If you have control of hip, change enc[0] below to pose.enc[program][0]
	 	at all three opportunties */
	else
	{
		/* assuming the hip stays in the same unregulated place, subract		
		its effect on xy --> xyt is toe location ref the knee. */

		xyt[0] = xy[0] - robot.l[0]*cos(enc[0]) ;
		xyt[1] = xy[1] - robot.l[0]*sin(enc[0]) ;

		/* use Craig's formulas directly for theta[1] and [2] */
		c2 = (xyt[0]*xyt[0] + xyt[1]*xyt[1] - 
			robot.l[1]*robot.l[1] - robot.l[2]*robot.l[2]) / 
			2. / robot.l[1] / robot.l[2] ;
		if (fabs(c2) > 1.) // do the best you can if outside workspace
			c2 = sign(c2) ;
		s2 = robot.sign_s2*sqrt(1. - c2*c2) ;	
		theta[2] = atan2(s2,c2) ;
		k1 = robot.l[1] + robot.l[2]*c2 ;
		k2 = robot.l[2]*s2 ;
		theta[1] = atan2(xyt[1],xyt[0]) - atan2(k2,k1) ;

		/* don't move the hip at all */
		theta[0] = enc[0] ;

		/* finally, make a correction to knee to account for hip */
		theta[1] -= theta[0] ; 
	}
	
}