/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

#include <math.h>
#include <sys/time.h>

#define DOLIVE	0

#include "../h/cdefs.h"
#include "../h/chase.h"
#include "../h/robotMath.h"
#include "../h/rciTask.h"
#include "../h/rciMonitor.h"
#include "../h/rciMacros.h"
#include "../h/robotControl.h"
#include "../h/IOdevice.h"

float computeMinimumTime();
float computeStopSegment();

typedef struct {
	int alive;
	float time;
	float p0;
	float v0;
	float a0;
} SEGMENT;

typedef struct {
	SEGMENT *seg;
	SEGMENT segbuf[4];
	int running;
	float time;
	float t0;
	float target;
} MOTION_DESC;

typedef struct {
	MOTION_DESC motions[MAXJNTS];
	CHASE_CTRL *cb;
} CONTROL_MEM;

CONTROL_MEM controlMem [MAX_CHASE_TASKS];

#define EPSILON	1.0e-7

chaseCtrlInit (cb)
CHASE_CTRL *cb;
{
	int j;
	for (j=0; j<MAXJNTS; j++)
	 { cb->angSpeed[j] = 1.0;    /* these values are about as harmless */
	   cb->angAccel[j] = 1.0;    /* as we can get */
	   cb->encSpeed[j] = 1.0;
	   cb->encAccel[j] = 1.0;
	 }
	cb->controlLevel = JOINT_LEVEL;
	cb->controlLevelChange = 0;
	cb->movecode = 0;
	cb->synchronize = YES;
	cb->block = NO;
	cb->compcode = 0x3f;
	cb->halt = NO;
	cb->stop = NO;
	cb->jTorqueFunction = NULLF;
	cb->mTorqueFunction = NULLF;
	cb->liveZone = 5.0*DEGTORAD;
	cb->monitorFxn = NULLF;
}

static unsigned short *csr = NULL;
#define SET_TRACE()	((csr) ? (*csr |= 0x02) : rciSetTrace(0x1))
#define CLEAR_TRACE()	((csr) ? (*csr &= ~0x02) : rciClearTrace(0x1))

chaseTrigger (rbt)
RCI_RBT *rbt;
{
	char *fxnName = "chaseTrigger() -- ";

	if ((*rbt->wakeup)(rbt->wakeupArg) < 0)
	 { rciAbort (0, "%s Can't signal robot\n", fxnName);
	 }
}

chaseStart (td)
RCI_DESC *td;
{
	CONTROL_MEM *cm = &controlMem[0];
	CHASE_CTRL *cb;
	RCI_RBT *rbt;
	int j, numJ;

	char *pfx = "chaseStart() -- ";

	if ((cb = (CHASE_CTRL*)getSharedMemory ("CHASE_CTRL")) == NULL)
	 { rciAbort (0, "%s can't get shared memory\n", pfx);
	   return (-1);
	 }
	td->tcb->sharedMem = (void*)cb;
	td->tcb->privateMem = (void*)cm;

	bzero ((char*)cm, sizeof(CONTROL_MEM));

	/* ---- Robot interface code ---- */

	if ((rbt = td->rbts[0]) == NULL)
	 { rciAbort (0, "%s no robot defined for task\n", pfx);
	   return (-1);
	 }
	if ((*rbt->startup)(td, rbt) < 0)
	 { return (-1);
	 }
	switch (td->tcb->scheduleDisp)
	 { 
	   case ON_TRIGGER:
	    { rciWakeupOn (td, rbt->ready, rbt->readyArg);
	      rciSetTrigger (chaseTrigger, (int)rbt);
	      break;
	    }
	   case ON_FUNCTION:
	    { rciWakeupOn (td, rbt->ready, rbt->readyArg);
	      break;
	    }
	   case ON_SIGNAL:
	    { rciWakeupOn (td, simRobotsReady, (int)td);
	      simSetTimer (td->tcb->interval, (struct timeval*)0);
	      break;
	    }
	 }
	return (1);
}

chaseDrive (td)
RCI_DESC *td;
{
	char *fxnName = "chaseDrive() --";

	RCI_RBT *rbt = td->rbts[0];
	CHASE_CTRL *cb;
	CONTROL_MEM *cm;
	register int j;
	int numJ;
	float interval;
	int torqueComputed = 0;
	struct timeval tv0;

	char *pfx = "chaseDrive() --";
	
	if (td->tcb->scheduleDisp == ON_SIGNAL)
	 { getSystemTime(&tv0);
	 }

	/* --- hack so we can see the function on the scope --- */

	if (strcmp ( ((IO_DEVICE*)rbt->dev)->name, "pri") == 0)
	 { csr = (unsigned short *)(((IO_DEVICE*)rbt->dev)->location);
	 }
	else
	 { csr = 0;
	 }

	numJ = rbt->jls->numJ;

	cb = (CHASE_CTRL*)td->tcb->sharedMem;
	cm = (CONTROL_MEM*)td->tcb->privateMem;
	if (cb == NULL || cm == NULL)
	 { rciAbort (0, 
	"%s NULL memory pointers. chaseStart() probably not called\n", pfx);
	   return (-1);
	 }

	SET_TRACE();

	/* ---- begin robot interface code ---- */

	if (td->tcb->scheduleDisp == ON_CLOCK)
	 { if (ROBOT_WAKEUP (rbt) < 0)
	    { rciAbort (0, "%s can't wakeup robot\n", fxnName);
	      CLEAR_TRACE(); return (-1);
	    }
	 }
	if ((*rbt->input)(td, rbt) < 0)
	 { CLEAR_TRACE(); return (-1);
	 }
 	if (td->tcb->cycleCount == 1)
	 { 
#if DOLIVE
	   JOINT_LEVEL_CONTROL(rbt);
	   for (j=0; j<numJ; j++)
	    { float angDiff;
   
	      cb->angSetpnt[j] = rbt->how->ang[j];
	      angDiff = rbt->how->lastAngSet[j] - rbt->how->ang[j];
	      if (FABS(angDiff) <= cb->liveZone)
	       { cb->angTarget[j] = rbt->how->lastAngSet[j];
		 cb->movecode |= (1<<j);
		 cb->compcode &= ~(1<<j);
	       }
	      else
	       { cb->angTarget[j] = rbt->how->ang[j];
	       }
	    }
#else
	   /* MS instead of halt, do initializations here
 	   cb->halt = YES; */
           {
	   float ang;
	   int enc;

	   for (j=0; j<numJ; j++)
	    { ang = rbt->how->ang[j];
	      cb->angTarget[j] = ang;
	      cb->angSetpnt[j] = ang;
	      cb->jTorque[j] = 0.0;

	      enc = rbt->how->enc[j];
	      cb->encTarget[j] = enc;
	      cb->encSetpnt[j] = enc;
	      cb->mTorque[j] = 0.0;

	      cb->torqueControl[j] = 0;
	      cm->motions[j].running = NO;
	      cb->movecode = 0;
	    }
	   }
	   if (!cb->controlLevelChange)
	    { cb->controlLevelChange = cb->controlLevel;
	    }
#endif
	 }
	if ((*rbt->output)(td, rbt) < 0)
	 { CLEAR_TRACE(); return (-1);
	 }

	SET_TRACE();

	if (td->tcb->scheduleDisp == ON_TRIGGER)
	 { rciSetTrigger (chaseTrigger, (int)rbt);
	 }

	/* ---- end robot interface code ---- */


	if (cb->monitorFxn)
	 { (*cb->monitorFxn)(cb, td, cb->monitorArg);
	 }

	if (cb->controlLevelChange && cb->compcode == 0x3f)
	 { 
	   switch (cb->controlLevelChange)
	    { case JOINT_LEVEL:
	       { JOINT_LEVEL_CONTROL(rbt);
		 break;
	       }
	      case MOTOR_LEVEL:
	       { MOTOR_LEVEL_CONTROL(rbt);
		 break;
	       }
	      default:
	       { rciAbort (0, 
	"%s bogus control mode (%d) specified\n", pfx,cb->controlLevelChange);
		 CLEAR_TRACE(); return (-1);
		 break;
	       }
	    }
	   cb->controlLevel = cb->controlLevelChange;
	   cb->controlLevelChange = 0;
	   /* MS This seems to halt the robot
	      before it gets started, not sure why its there 
	      cb->halt = YES; */
	 }

	if (cb->halt)
  	 { float ang;
	   int enc;

	   cb->halt = 0;

	   for (j=0; j<numJ; j++)
	    { ang = rbt->how->ang[j];
	      cb->angTarget[j] = ang;
	      cb->angSetpnt[j] = ang;
	      cb->jTorque[j] = 0.0;

	      enc = rbt->how->enc[j];
	      cb->encTarget[j] = enc;
	      cb->encSetpnt[j] = enc;
	      cb->mTorque[j] = 0.0;

	      cb->torqueControl[j] = 0;
	      cm->motions[j].running = NO;
	      cb->movecode = 0;
	    }
	   SET_STOP_ALL(rbt);

	   if (td->tcb->scheduleDisp == ON_SIGNAL)
	    { simSetTimer (td->tcb->interval, &tv0);
	    }
	   CLEAR_TRACE();
	   return (1);
	 }

	if (cb->block || !rbt->how->power)
	 { if (td->tcb->scheduleDisp == ON_SIGNAL)
	    { simSetTimer (td->tcb->interval, &tv0);
	    }
	   CLEAR_TRACE();
	   return (1);
	 }

	if (cb->stop)
	 { float p0[MAXJNTS];
	   float p1[MAXJNTS];
	   float v0[MAXJNTS];
	   float accel[MAXJNTS];
	   
	   if (cb->controlLevel == JOINT_LEVEL)
	    { 
	      for (j=0; j<numJ; j++)
	       { p0[j] = cb->angSetpnt[j];
		 v0[j] = rbt->how->angVel[j];
		 accel[j] = FABS(cb->angAccel[j]);
	       }
	    }
	   else
	    { 
	      for (j=0; j<numJ; j++)
	       { p0[j] = cb->encSetpnt[j];
		 v0[j] = rbt->how->encVel[j];
		 accel[j] = ABS(cb->encAccel[j]);
	       }
	    }

	   for (j=0; j<numJ; j++)
	    { float p1;
	      MOTION_DESC *motion = &cm->motions[j];

	      if (accel[j] < EPSILON)
	       { rciAbort (0, "%s acceleration is close to 0\n", pfx);
		 CLEAR_TRACE(); return (-1);
	       }	      
	      p1 = computeStopSegment(motion->segbuf, p0[j], v0[j], accel[j]);

	      motion->running = YES;
	      motion->time = 0.0;
	      motion->t0 = 0.0;
	      motion->seg = motion->segbuf;
	      motion->target = p1;
	      
	      if (cb->controlLevel == JOINT_LEVEL)
	       { cb->angTarget[j] = p1;
	       }
	      else
	       { cb->encTarget[j] = p1;
	       }
	      cb->torqueControl[j] = 0;
	    }
	   cb->movecode = 0;
	   cb->stop = NO;
	 }

	if (cb->movecode)
	 { MOTION_DESC *motion;
	   float p1[MAXJNTS];
	   float p0[MAXJNTS];
	   float v0[MAXJNTS];
	   float speed[MAXJNTS];
	   float accel[MAXJNTS];
	   float maxTime;

	   if (cb->controlLevel == JOINT_LEVEL)
	    { for (j=0; j<numJ; j++)
	       { p1[j] = cb->angTarget[j];
		 p0[j] = cb->angSetpnt[j];
		 v0[j] = rbt->how->angVel[j];
		 speed[j] = cb->angSpeed[j];
		 accel[j] = cb->angAccel[j];
	       }
	    }
	   else
	    { for (j=0; j<numJ; j++)
	       { p1[j] = cb->encTarget[j];
		 p0[j] = cb->encSetpnt[j];
		 v0[j] = rbt->how->encVel[j];
		 speed[j] = cb->encSpeed[j];
		 accel[j] = cb->encAccel[j];
	       }
	    }
	   if (cb->synchronize)
	    { maxTime = 0.0;
	      for (j=0; j<numJ; j++)
	       { float t;
		 if (!cb->torqueControl[j] && (cb->movecode & (1<<j)) )
		  { t = computeMinimumTime (p1[j], p0[j], v0[j],
					    speed[j], accel[j]);
		    if (t > maxTime)
		     { maxTime = t;
		     }
		  }
	       }
	      for (j=0; j<numJ; j++)
	       { if (!cb->torqueControl[j] && (cb->movecode & (1<<j)) )
		  { motion = &cm->motions[j];
		    computeSyncMotionSegments (motion->segbuf, p1[j], p0[j],
					       v0[j], accel[j], maxTime);
		    motion->running = YES;
		    motion->seg = motion->segbuf;
		    motion->time = 0.0;
		    motion->t0 = 0.0;
		    motion->target = p1[j];
		  }
	       }
	    }
	   else
	    { for (j=0; j<numJ; j++)
	       { if (!cb->torqueControl[j] && (cb->movecode & (1<<j)) )
		  { motion = &cm->motions[j];
		    computeAsyncMotionSegments (motion->segbuf, p1[j], p0[j],
					       v0[j], speed[j], accel[j]);
		    motion->running = YES;
		    motion->seg = motion->segbuf;
		    motion->time = 0.0;
		    motion->t0 = 0.0;
		    motion->target = p1[j];
		  }
	       }
	    }
	   cb->movecode = 0;
	 }

	interval = td->tcb->interval/1000.0;

	for (j=0; j<numJ; j++)
	 { float computedTorques[MAXJNTS];
	   int torqueComputed = NO;
	
	   if (!cb->torqueControl[j])
	    { 
	      MOTION_DESC *motion = &cm->motions[j];

	      if (motion->running)
	       { SEGMENT *seg;
		 float setpoint, t;

		 t = (motion->time += interval);
		 seg = motion->seg;

		 while (seg->alive && seg->time <= t)
		  { motion->t0 = seg->time;
		    seg++;
		  }
		 if (!seg->alive)
		  { motion->running = 0;
		    setpoint = motion->target;
		  }
		 else
		  { t -= motion->t0;
		    setpoint = seg->p0 + t*(seg->v0 + t*(0.5*seg->a0));
		    motion->seg = seg;
		  }
		 if (cb->controlLevel == JOINT_LEVEL)
		  { MOVE_ANGLE (rbt, setpoint, j);
		    cb->angSetpnt[j] = setpoint;
		  }
		 else
		  { MOVE_ENCODER (rbt, setpoint, j);
		    cb->encSetpnt[j] = setpoint;
		  }
	       }
	      else
	       { cb->compcode |= (1<<j);

		 if (cb->controlLevel == JOINT_LEVEL)
		  { MOVE_ANGLE (rbt, cb->angSetpnt[j], j);
		  }
		 else
		  { MOVE_ENCODER (rbt, cb->encSetpnt[j], j);
		  }
	       }
	    }
	   else
	    {
	      if (cb->controlLevel == JOINT_LEVEL)
	       { float ang;
		 switch (cb->torqueControl[j])
		  { case TORQUE_ZERO:
		     { SET_JTORQUE (rbt, 0.0, j);
		       break;
		     }
		    case TORQUE_SETVALUE:
		     { SET_JTORQUE (rbt, cb->jTorque[j], j);
		       break;
		     }
		    case TORQUE_FUNCTION:
		     { if (!torqueComputed)
			{ if (cb->jTorqueFunction)
			   { (*cb->jTorqueFunction)(computedTorques, rbt);
			   }
			  else
			   { int k;
			     for (k=0; k<numJ; k++)
			      { computedTorques[k] = 0.0;
			      }
			   }
			  torqueComputed = YES;
			}
		       SET_JTORQUE (rbt, computedTorques[j], j);
		       break;
		     }
		  }
		 ang = rbt->how->ang[j];
		 cb->angTarget[j] = ang;
		 cb->angSetpnt[j] = ang;
	       }
	      else
	       { int enc;
		 switch (cb->torqueControl[j])
		  { case TORQUE_ZERO:
		     { SET_MTORQUE (rbt, 0.0, j);
		       break;
		     }
		    case TORQUE_SETVALUE:
		     { SET_MTORQUE (rbt, cb->mTorque[j], j);
		       break;
		     }
		    case TORQUE_FUNCTION:
		     {        
		      if (!torqueComputed)
			{ if (cb->mTorqueFunction)
			   { (*cb->mTorqueFunction)(computedTorques, rbt);
			   }
			  else
			   { int k;
			     for (k=0; k<numJ; k++)
			      { computedTorques[k] = 0.0;
			      }
			   }
			  torqueComputed = YES;
			}
		       SET_MTORQUE (rbt, computedTorques[j], j);
		       break;
		     }
		  }
		 enc = rbt->how->enc[j];
		 cb->encTarget[j] = enc;
		 cb->encSetpnt[j] = enc;
	       }
	      cb->compcode |= (1<<j);
	    }
	 }
	if (td->tcb->scheduleDisp == ON_SIGNAL)
	 { simSetTimer (td->tcb->interval, &tv0);
	 }
	CLEAR_TRACE();
	return (1);
}

chaseRelease(td, arg, code)
RCI_DESC *td;
int arg;
int code;
{
	RCI_RBT *rbt = td->rbts[0];
	int rcode;
	
	rciSetTrigger (NULLF, 0);	/* clear any trigger function */
	if (code == RELEASE_REQ)
	 { rcode = RBT_NORMAL;
	 }
	else
	 { rcode = RBT_HARDERROR;
	 }
	if ((*rbt->release)(td, rbt, rcode) < 0)
	 { return (-1);
	 }
	return (1);
}

#define FSGN(x)		((x) >= 0.0 ? 1.0 : -1.0)

computeSyncMotionSegments (seg, p1, p0, v0, amax, time)
register SEGMENT *seg;
float p1, p0, v0, amax;
float time;
{
	float vi, a, t0, Dp, Dpr, tr, D;
	float t1, t2, tc, a1, a2;

	Dp = p1 - p0;
	t0 = FABS(v0)/amax;		/* min. time to bring velocity to 0 */
	tr = time - t0;			/* time we have left after that */
	if (v0 < 0.0)
	 { vi = -v0;
	   a = -amax;
	   Dpr = -Dp - 0.5*vi*t0;	/* remaining displacement */
	 }
	else
	 { vi = v0;
	   a = amax;
	   Dpr = Dp - 0.5*vi*t0;	/* remaining displacement */
	 }

	if (Dpr < 0.0)			/* overshoot case */
	 { D = amax*tr*tr/4.0 + Dpr;
	   if (D <= 0.0)		/* no velocity clipping */
	    { t2 = tr/2.0;
	      t1 = time-t2;
	      tc = 0.0;
	    }
	   else				/* clip velocity to stretch time */
	    { t2 = tr/2.0 - sqrt(amax*D)/amax;
	      t1 = t0+t2;
	      tc = time-t1-t2;
	    }
	   a1 = -a;
	   a2 = a;
	 }
	else if (Dpr >= vi*tr)
	 { float t = t0+tr/2.0;
	   D = amax*t*t - vi*t0 - Dpr;
	   if (D <= 0.0)
	    { t1 = tr/2.0;
	      t2 = time - t1;
	      tc = 0.0;
	    }
	   else
	    { t1 = tr/2.0 - sqrt(amax*D)/amax;
	      t2 = t0+t1;
	      tc = time-t1-t2;
	    }
	   a1 = a;
	   a2 = -a;
	 }
	else
	 { if (FABS(tr) < EPSILON)
	    { t2 = 0.0;
	    }
	   else
	    { t2 = Dpr/(tr*amax);
	    }
	   tc = tr;
	   t1 = t0-t2;
	   a1 = -a;
	   a2 = -a;
	 }

	seg->alive = 1;
	seg->time = t1;
	seg->a0 = a1;
	seg->v0 = v0;
	seg->p0 = p0;
	seg++;

	if (tc != 0.0)
	 { seg->alive = 1;
	   seg->time = t1+tc;
	   seg->a0 = 0.0;
	   seg->v0 = v0 + t1*a1;
	   seg->p0 = p0 + t1*(v0 + t1*(0.5*a1));
	   seg++;
	 }
	
	seg->alive = 1;
	seg->time = time;
	seg->a0 = a2;
	seg->v0 = -a2*t2;
	seg->p0 = p0 + Dp - 0.5*seg->v0*t2;
	seg++;

	seg->alive = 0;
}

computeAsyncMotionSegments (seg, p1, p0, v0, vmax, amax)
register SEGMENT *seg;
float p1, p0, v0, vmax, amax;
{
	float vi, a, t0, Dp, Dpr, D;
	float t1, t2, tc, a1, a2;

	Dp = p1 - p0;
	t0 = FABS(v0)/amax;
	if (v0 < 0.0)
	 { vi = -v0;
	   a = -amax;
	   Dpr = -Dp - 0.5*vi*t0;
	 }
	else
	 { vi = v0;
	   a = amax;
	   Dpr = Dp - 0.5*vi*t0;
	 }

	if (Dpr < 0.0)
	 { D = - Dpr - vmax*vmax/amax;
	   if (D < 0.0)
	    { t2 = sqrt (-Dpr/amax);
	      t1 = t2+t0;
	      tc = 0.0;
	    }
	   else
	    { t2 = vmax/amax;
	      t1 = t2+t0;
	      tc = D/vmax;
	    }
	   a1 = -a;
	   a2 = a;
	 }
	else if (vi <= vmax)
	 { D = Dpr - vmax*vmax/amax + amax*t0*t0;
	   if (D < 0.0)
	    { t2 = sqrt(Dpr/amax + t0*t0);
	      t1 = t2-t0;
	      tc = 0.0;
	    }
	   else
	    { t2 = vmax / amax;
	      t1 = t2-t0;
	      tc = D / vmax;
	    }
	   a1 = a;
	   a2 = -a;
	 }
	else
	 { t2 = vmax / amax;
	   tc = Dpr / vmax;
	   t1 = t0-t2;
	   a1 = -a;
	   a2 = -a;
	 }

	seg->alive = 1;
	seg->time = t1;
	seg->a0 = a1;
	seg->v0 = v0;
	seg->p0 = p0;
	seg++;

	if (tc != 0.0)
	 { seg->alive = 1;
	   seg->time = t1+tc;
	   seg->a0 = 0.0;
	   seg->v0 = v0 + t1*a1;
	   seg->p0 = p0 + t1*(v0 + t1*(0.5*a1));
	   seg++;
	 }

	seg->alive = 1;
	seg->time = t1+tc+t2;
	seg->a0 = a2;
	seg->v0 = -a2*t2;
	seg->p0 = p0 + Dp - 0.5*seg->v0*t2;
	seg++;

	seg->alive = 0;
}

float computeStopSegment (seg, p0, v0, amax)
SEGMENT *seg;
float p0, v0, amax;
{
	float t0, a1;

	t0 = FABS(v0)/amax;

	seg->alive = 1;
	seg->time = t0;
	seg->p0 = p0;
	seg->v0 = v0;
	seg->a0 = (v0 < 0.0) ? amax : -amax;
	seg++;

	seg->alive = 0;

	return (p0 + 0.5*v0*t0);
}


float computeMinimumTime (p1, p0, v0, vmax, amax)
float p1, p0, v0, vmax, amax;
{
	float t0, D, dP, dP0, time;

	dP = p1-p0;
	if (dP == 0.0)
	 { return (0.0);
	 }

	t0 = FABS(v0)/amax;
	dP0 = 0.5*v0*t0;

	if (FSGN(dP-dP0) == FSGN(dP0))
	 { if (FABS(v0) > vmax)
	    { return (t0 + FABS(dP-dP0)/vmax);
	    }
	   else
	    { D = FABS(dP+dP0);
	      time = -t0;
	    }
	 }
	else
	 { D = FABS(dP-dP0);
	   time = t0;
	 }
	if (D <= (vmax*vmax)/amax)
	 { time += 2*sqrt(D/amax);
	 }
	else
	 { time += vmax/amax + D/vmax;
	 }
	return (time);
}
