/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	teachCtrl
 *
 * DESCRIPTION:	The trajectory generator for the rcclTeach() routine.
 *
 *		One might reasonably ask: "why a separate trajectory 
 *		generator for the teach routine?" There are a couple of
 *      	reasons. First, it is hard to make the canonical RCCL
 *      	trajectory generator (which lives mainly in the files
 *      	traject.c and setpointCtrl.c, for those interested) respond
 *      	nicely to teach pendant inputs. This is perhaps not as true
 *      	now as when I first wrote the teach software, since I have
 *      	added the 'jbias' routines to RCCL, but it would still
 *      	take a bit of work to integrate the two.
 *
 *		Similarly, the detection of limit stops that is done fairly
 *      	easily here is harder to do cleanly inside the main
 *      	trajectory generator.
 * 
 * CAVEATS:	This thing could be re-written for the better. What we
 *      	probably want to do is redesign the main trajectory generator
 *      	so that in fact it does handle teach pendant commands nicely.
 *
 *		I simply don't have time to comment this code properly right
 *      	now.
 *
 * AUTHOR/DATE:	John Lloyd, Winter 1985
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <math.h>
#include <signal.h>

#include "../h/cdefs.h"
#include "../h/robotMath.h"
#include "../h/rciTask.h"
#include "../h/rcclDefs.h"
#include "../h/pendant.h"
#include "../h/robot_kynvar.h"
#include "../h/errorCodes.h"

#include "../h/softStops.h"

#include "setpoint.h"
#include "kynMacros.h"

static TRSF ident = 			/* unit transform		*/
	{1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.};

#define T6	(&mpb->t6)
#define J6	(&mpb->j6)

#define ZEROG				/* allow zero gravity mode */

#define LOW_SPEED 	.2		/* Relative speeds		*/
#define MED_SPEED 	.6
#define HIGH_SPEED     1.0

#define EPSILON	1.0e-7			/* Small number for testing 0	*/
#define NTICKS		10		/* Number of ticks to respond   */

extern int rciCpu;

static update_display ();
static read_pendant();
static tp_tool_world_to_angle ();
static tp_free ();
static tp_joint();
static float compute_scale ();
static cp_tool_world_to_angle ();
static cp_joint ();
static set_cmove_times ();

teachInit(mpb)
MOTION_PARAMETER_BLOCK *mpb;
{
	register TEACH_BLOCK *tb = &mpb->tbk;
	register MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mpb->mcb;
	MANIP *mnp;
	RCI_RBT *rbt = mpb->rbt;
	JNTS jvel;
	float tvel, rvel;

	mnp = mpb->mnps[rciCpu];
	getCartVel (mnp, &tvel, &rvel);
	getJointVel (mnp, &jvel);

	degreeToRadian (tb->angleBaseSpeed, jvel.v, mnp->jls);

	tb->trsl_base_speed =   tvel;
	tb->rad_base_speed =    DEGTORAD*rvel;
	tb->angle_tick =       .1;
	tb->trsl_tick =	       .1;
	tb->rot_tick =	       .1;

	tb->joint = UNDEF;
	tb->plus = YES;

/*	tb->mode = 0; leave mode unchanged */
	tb->mode_req = tb->mode;
	tb->set_speed = 0.0;
	tb->set_speed_req = 0.0;
/*	tb->req_count = 0;*/
	tb->tick = NO;
	tb->cmove_completed = YES;
	tb->ctime = 0.0;
	tb->limit_message = NO;
	tb->display_word = (unsigned short)UNDEF;
	tb->last_display_mode = 0;

/*	tb->pendant_mode = 0; */
	tb->speed = 0.;
	tb->block_pendant = NO;

	mcb->t_state = STOPPED;
	mcb->t_speed = 1.0;

	read_angles (tb->old_joints.v, rbt->how);
	writeTrans (mpb->mnps[rciCpu]->tool, &ident);
	*J6 = tb->old_joints;


#if defined(linux)
	if(puma_pendant_startup() < 0) printf("Error initializing pendant serial IO\n");
#endif



}

teachCompute(mpb)
MOTION_PARAMETER_BLOCK *mpb;
{
	register TEACH_BLOCK *tb = &mpb->tbk;
	register MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mpb->mcb;
	RCI_RBT *rbt = mpb->rbt;
	HOW *how = rbt->how;
	JLS *jls = rbt->jls;
	KYN *kyn = rbt->kyn;
	MANIP *mnp = mpb->mnps[rciCpu];

	int status = 0;
        short prompt;
	int j;

	/* compute observed angles and observed T6 */

	 { TRSF t6o;
	   FWD_KINEMATICS (&t6o, NULL, how->ang, NULL);
	   writeTrans (mnp->t6o, &t6o);
	   writeMem (mnp->j6o, how->ang);
	 }
	if (rbt && mnp->handPos != how->hand)
	 { setManipHandPos (mpb, how->hand);
	 }

	if (mcb->t_stop)
 	 { read_angles (tb->old_joints.v, how);
	   *J6 = tb->old_joints;
	   mcb->t_state = STOPPED;
	   mcb->t_stop = NO;
	   tb->block_pendant = NO;
	   tb->cmove_completed = YES;
	   update_mnp (mpb);
	   return;
	 }

	tb->panic_req	= NO;
	tb->plus_req	= YES;
	tb->joint_req	= UNDEF;

	if (!tb->block_pendant)
  	 { 
	   if (how->options & PENDANT_AVAIL)
#if defined(linux)
	     /* MS In Linux implementation, we read the teachpendant directly through
		serial port /dev/tty.  Insert this read here, filling  
		how->pendantInput instead of accepting this from RCI/LSI11
                display_word is sent to the pendant to control LED display
	     */

	     { puma_pendant_read(mpb->tbk.display_word,how->pendantInput);
	       read_pendant(mpb, how->pendantInput);
	     }

#else
	     { read_pendant(mpb, how->pendantInput);
	     }
	   
#endif
	   if (tb->panic_req)
	    { SET_STOP_ALL (rbt);
	      read_angles (tb->old_joints.v, how);
	      *J6 = tb->old_joints;
	      update_mnp (mpb);
	      return;
	    }
	 }

	tb->set_speed = tb->set_speed_req;
	tb->t6Computed = 0;
	
	switch (mcb->t_state)
  	 { case STOPPED:
	       { tb->speed = 0.;

		 if (tb->mode_req != 0)
		  { tb->last_mode = tb->mode;
		    tb->mode = /* tb->pendant_mode = */ tb->mode_req;
		  }
		 if (0 <= tb->joint_req && tb->joint_req <= 5 ||
		     (tb->mode == FREE_M && !mcb->t_motion_mode))
		  { /*tb->req_count = 0;*/
		    tb->joint = tb->joint_req;
		    tb->plus = tb->plus_req;

		    mcb->t_state = TP_MOVE;
		  }
		 else if (mcb->t_motion_mode)
		  { tb->last_mode = tb->mode;
		    tb->mode = mcb->t_motion_mode;

	 	    if (tb->last_mode != tb->mode && 
			(tb->mode == WORLD_M || tb->mode == TOOL_M))
	    	     { tb->old_joints = *J6;
		       FWD_KINEMATICS (T6, NULL, tb->old_joints.v, NULL);
	    	     }
		    tb->cmove_completed = NO;
		    set_cmove_times (mpb, tb->mode);
		    tb->ctime = 0.;
		    mcb->t_motion_mode = NO;
		    
		    mcb->t_state = CP_MOVE;
		  }
		 if (tb->last_mode != tb->mode && 
		    (tb->mode == WORLD_M || tb->mode == TOOL_M))
		  { tb->old_joints = *J6;
		    FWD_KINEMATICS (T6, NULL, tb->old_joints.v, NULL);
		  }
		 break;
	       }
	   case TP_MOVE:
	       { int req = (tb->joint_req != tb->joint || 
			    tb->mode_req != tb->mode || 
			    tb->plus_req  != tb->plus);

		 if (tb->mode == FREE_M || req)
		  { tb->set_speed = 0.0;
		  }
		 if (mcb->t_motion_mode)
		  { tb->set_speed = 0.0;
		    tb->block_pendant = YES;
		  }
		 if (tb->speed > (tb->set_speed + EPSILON))
		  { tb->speed -= LOW_SPEED / 2.0;
		  }
		 else if (tb->speed < (tb->set_speed - EPSILON))
		  { tb->speed += LOW_SPEED / 2.0;
		  }
		 if (tb->mode == FREE_M && 
		      (tb->mode_req != FREE_M || mcb->t_motion_mode))
		  { mcb->t_state = STOPPED;
		  }
		 else if (tb->tick && req)
		  { mcb->t_state = STOPPED;
		  }
		 else if (req && tb->speed < EPSILON)
		  { mcb->t_state = STOPPED;
		  }
		 break;
	       }
	      case CP_MOVE:
	       { tb->speed = 0.;
		 if (tb->cmove_completed)
		  { mcb->t_state = STOPPED;
		    tb->block_pendant = NO;
		    /* XXX we reset the pendant mode now instead
		       of waiting until the next cycle */	
		    if (tb->mode_req != 0)
		     { tb->last_mode = tb->mode;
		       tb->mode = /* tb->pendant_mode = */ tb->mode_req;
		     }
		  }
		 tb->ctime += 1.0;
		 break;
	       }
	 }

	/* Now perform the indicated actions */

	if (tb->last_mode != tb->mode)
	 { if (tb->mode == FREE_M)
	    { SET_CHECKING (rbt, (CHK_REQTOR | CHK_MAXTOR | 
				  CHK_REQVEL | CHK_MAXVEL));
	      for (j=0;j<6;j++)
	       { 
#ifdef ZEROG
	         tb->locked[j] = NO;
#else
		 tb->locked[j] = YES;
#endif
	       }
	    }
	   else if (tb->last_mode == FREE_M)
	    { SET_CHECKING (rbt, ~0);
	    }
	 }

	/* Calculate motion angles */

	switch (mcb->t_state)
  	 { case TP_MOVE:
	       { switch (tb->mode)
		  { case WORLD_M:
		    case TOOL_M:
		        { status = tp_tool_world_to_angle (mpb, tb->mode, 
						     tb->joint, tb->speed);
			  break;
			}
		    case FREE_M:
		        { status = tp_free(mpb);
			  break;
			}
		    case JOINT_M:
			{ status = tp_joint(mpb, tb->joint, tb->speed);
			  break;
			}
		  }
		 break;
	       }
	   case CP_MOVE:
	       { switch (tb->mode)
		  { case WORLD_M:
		    case TOOL_M:
			{ status = cp_tool_world_to_angle(mpb);
			  break;
			}
		    case FREE_M:
			{ status = tp_free(mpb);
			  break;
			}
		    case JOINT_M:
			{ status = cp_joint(mpb);
			  break;
			}
		  }
		 break;
	       }
	 }

	if (!tb->t6Computed)
	 { FWD_KINEMATICS (T6, &mpb->config, J6->v, NULL);
	 }
	else
	 { solveConf (&mpb->config, J6->v, rbt->kyn);
	 }
	update_mnp(mpb);

	tb->old_joints = *J6;
	update_display (mpb, mcb->t_state, status, tb->mode);

	if (how->pendantInput[0] & CLAMP1)
	 { SET_HANDPOS (rbt, 1);
	 }
 	else if (how->pendantInput[0] & CLAMP2)
	 { SET_HANDPOS (rbt, 0);
	 }

	if (tb->mode == FREE_M)
  	 { for (j=0; j<6; j++)
	    { if (tb->locked [j])
	       { MOVE_ANGLE (rbt, J6->v[j], j);
	       }
	      else
	       { 
#ifdef ZEROG
#if SOFT_STOPS
		 float diff, maxForce;

		 if (how->enc[j] < 1000 || how->enc[j] > (0x10000-1000))
		  { rciAbort (ELimit, 
	       "Encoder on joint %d = %d and is close to wrapping\n",
			      j+1, how->enc[j]);
		  }
		 if ((diff = how->ang[j]+STOP_MARGIN-jls->jmax[j]) > 0.0)
		  { maxForce = STOP_FORCE_FACTOR*kyn->gen->coulFriction[j];
		    tb->output_torque[j] -=
			     MIN(maxForce, (diff/STOP_RAMP_LENGTH)*maxForce);
		  }
		 else if ((diff = jls->jmin[j]+STOP_MARGIN-how->ang[j]) > 0.0)
		  { maxForce = STOP_FORCE_FACTOR*kyn->gen->coulFriction[j];
		    tb->output_torque[j] +=
			     MIN(maxForce, (diff/STOP_RAMP_LENGTH)*maxForce);
		  }
#endif
		 SET_JTORQUE (rbt, tb->output_torque[j], j);
#endif
	       }
	    }
	 }
        else
  	 { for (j=0; j<6; j++)
	    { MOVE_ANGLE (rbt, J6->v[j], j);
	    }
	 }
}

teachWrapup(mpb)
MOTION_PARAMETER_BLOCK *mpb;
{
	writeTrans (mpb->mnps[rciCpu]->here, T6);
	writeTrans (mpb->mnps[rciCpu]->lastTC, T6);

#if defined(linux)
	puma_pendant_release();
#endif

}

static update_display (mpb, state, status, mode)
MOTION_PARAMETER_BLOCK *mpb;
int state, status, mode;
{
	unsigned short old_display_word = mpb->tbk.display_word;

	if (mpb->tbk.display_word == (unsigned short)UNDEF)
	 { mpb->tbk.display_word = M_NULL;
	 }

	if (state != CP_MOVE && mpb->tbk.last_display_mode != mode)
         { mpb->tbk.display_word &= ~(0xe);
	   switch (mode)
	    { case TOOL_M:
		 mpb->tbk.display_word |= TOOL_LED;
		 break;
	      case WORLD_M:
		 mpb->tbk.display_word |= WORLD_LED;
		 break;
	      case JOINT_M:
		 mpb->tbk.display_word |= JOINT_LED;
		 break;
	      case FREE_M:
	       { mpb->tbk.display_word |= FREE_LED;
		 break;
	       }
	    }
	   mpb->tbk.last_display_mode = mode;
	 }
	else if (state == CP_MOVE && mpb->tbk.last_display_mode != COMP_M)
  	 { mpb->tbk.last_display_mode = COMP_M;
	   mpb->tbk.display_word &= ~(0xe);
	   mpb->tbk.display_word |= COMP_LED;
	 }
	if (status != 0)
 	 { mpb->tbk.display_word &= 0x00ff;
	   mpb->tbk.limit_message = YES;
	   if (status & 0x01)
	    { mpb->tbk.display_word |= (M_LIMIT1);
	    }
	   else if (status & 0x02)
	    { mpb->tbk.display_word |= (M_LIMIT2);
	    }
	   else if (status & 0x04)
	    { mpb->tbk.display_word |= (M_LIMIT3);
	    }
	   else if (status & 0x08)
 	    { mpb->tbk.display_word |= (M_LIMIT4);
	    }
	   else if (status & 0x10)
 	    { mpb->tbk.display_word |= (M_LIMIT5);
	    }
	   else if (status & 0x20)
 	    { mpb->tbk.display_word |= (M_LIMIT6);
	    }
 	   else if (status & (KYN_JOINT_LIMIT|KYN_CANT_REACH))
  	    { mpb->tbk.display_word |= (M_TOO_FAR);
	    }
	 }
 	else if (mpb->tbk.limit_message == YES)
	 { mpb->tbk.limit_message = NO;
	   mpb->tbk.display_word &= 0x00ff;
	   mpb->tbk.display_word |= M_NULL;
	 }
	if (mpb->tbk.display_word != old_display_word)
	 { 

#if defined(linux)

#else
	   PUT_PENDANT_MSG (mpb->rbt, (char*)&mpb->tbk.display_word, 
				      sizeof(short));	    
#endif


	 }
}

static read_pendant(mpb, pendant)
MOTION_PARAMETER_BLOCK *mpb;
long *pendant;
{
	register TEACH_BLOCK *tb = &mpb->tbk;

	if ((pendant[0] & RECORD))
	 { MOTION_CONTROL_BLOCK *mcb = mpb->mcb;
	   if (!mcb->mcbregs[0])
	    { mcb->mcbregs[0] = 1;
	      rciSignal (SIGUSR1);
	    }
	   return;
	 }
 	if ((pendant[0] & PANIC1) && (pendant[1] & PANIC5))
	 { tb->panic_req = YES;
	 }
	switch ((int)(pendant[0] & 0xffffff))
	 { case X_MINUS:
	      tb->joint_req = 0;
	      tb->plus_req = NO;
	      break;
	   case X_PLUS:
	      tb->joint_req = 0;
	      break;
	   case Y_MINUS:
	      tb->joint_req = 1;
	      tb->plus_req = NO;
	      break;
	   case Y_PLUS:
	      tb->joint_req = 1;
	      break;
	   case Z_MINUS:
	      tb->joint_req = 2;
	      tb->plus_req = NO;
	      break;
	   case Z_PLUS:
	      tb->joint_req = 2;
	      break;
	   case RX_MINUS:
	      tb->joint_req = 3;
	      tb->plus_req = NO;
	      break;
	   case RX_PLUS:
	      tb->joint_req = 3;
	      break;
	   case RY_MINUS:
	      tb->joint_req = 4;
	      tb->plus_req = NO;
	      break;
	   case RY_PLUS:
	      tb->joint_req = 4;
	      break;
	   case RZ_MINUS:
	      tb->joint_req = 5;
	      tb->plus_req = NO;
	      break;
	   case RZ_PLUS:
	      tb->joint_req = 5;
	      break;
	   case TOOL_BUTTON:
	      tb->mode_req = TOOL_M;
	      break;
	   case WORLD_BUTTON:
	      tb->mode_req = WORLD_M;
	      break;
	   case JOINT_BUTTON:
	      tb->mode_req = JOINT_M;
	      break;
	   case FREE_BUTTON:
	      tb->mode_req = FREE_M;
	      break;
	 }

	tb->set_speed_req = (((unsigned)(pendant[0]&0xff000000))>>24)/255.0;

}

static tp_tool_world_to_angle (mpb, mode, joint, speed)
MOTION_PARAMETER_BLOCK *mpb;
int mode, joint;
float speed;
{	
	MANIP *mnp = mpb->mnps[rciCpu];

	RCI_RBT *rbt = mpb->rbt;
	TRSF temp, change, world_trsl, world_rot, old_t6;
	float vscale;
	float delta_pos;
 	int status = 0;

	switch (joint)
 	 { case 0:
	   case 1:
	   case 2:
	       { vscale = mpb->tbk.trsl_base_speed * mpb->sampleTime / 1000.0;
		 if (mpb->tbk.tick)
		  { delta_pos = speed * vscale * mpb->tbk.trsl_tick;
		  }
		 else
		  { delta_pos = speed * vscale;
		  }
		 break;
	       }
	   case 3:
	   case 4:
	   case 5:
	       { vscale = mpb->tbk.rad_base_speed * mpb->sampleTime / 1000.0;
		 if (mpb->tbk.tick)
		  { delta_pos = speed * vscale * mpb->tbk.rot_tick;
		  }
		 else
		  { delta_pos = speed * vscale;
		  }
		 break;
	       }
	 }
 	if (!mpb->tbk.plus)
 	 { delta_pos = -delta_pos;
	 }
	
	copyTrsf (&change, &ident);
 	copyTrsf (&old_t6, T6);

 	switch (joint)
	 { case 0:
	      multTrsfXyz (&change, delta_pos, 0., 0.);
	      break;
	   case 1:
	      multTrsfXyz (&change, 0., delta_pos, 0.);
	      break;
	   case 2:
	      multTrsfXyz (&change, 0., 0., delta_pos);
	      break;
	   case 3:
	      multTrsfRpy (&change, 0., 0., RADTODEG*delta_pos);
	      break;
	   case 4:
	      multTrsfRpy (&change, 0., RADTODEG*delta_pos, 0.);
	      break;
	   case 5:
	      multTrsfRpy (&change, RADTODEG*delta_pos, 0., 0.);
	      break;
	 }
	if (mode == WORLD_M)
	 { multTrsf    (&temp, T6, mnp->tool);
	   world_trsl = ident;
	   world_rot  = ident;
	   copyTrsfXyz  (&world_trsl, &temp);
	   copyTrsfNoa   (&world_rot,  &temp);
	   multTrsf    (&temp, &world_trsl, &change);
	   multTrsf    (&temp, &temp, &world_rot);
	   multRiTrsf (&temp, &temp, mnp->tool);
	   *T6 = temp;
	 }
	else
	 { multTrsf    (&temp, T6, mnp->tool);
	   multTrsf    (&temp, &temp, &change);
	   multRiTrsf (&temp, &temp, mnp->tool);
	   *T6 = temp;
	 }

	status = INV_KINEMATICS (J6->v, T6, mpb->config, 
					 mpb->tbk.old_joints.v, NULL);
	status &= ~(KYN_JOINT_LIMIT | KYN_SINGULAR);
	status = check_limits (status, J6->v, mpb->tbk.old_joints.v, 
					    rbt->jls);
	mpb->tbk.t6Computed = 1;
	if (status != 0)
  	 { *T6 = old_t6;
	   *J6 = mpb->tbk.old_joints;
	 }
 	return (status);
}

static tp_free (mpb)
MOTION_PARAMETER_BLOCK *mpb;
{
	RCI_RBT *rbt = mpb->rbt;
	float *J6angles = J6->v;
	int j;

	gravload (mpb->tbk.output_torque, rbt->how->ang, rbt->kyn);

	if (mpb->tbk.joint_req >= 0 && mpb->tbk.joint_req <= 5)
	 { 
#ifdef ZEROG
	   mpb->tbk.locked[mpb->tbk.joint_req] = 
					(mpb->tbk.plus_req ? YES : NO );
#endif
	 }

	for (j=0;j<6;j++)
	 { if (!(mpb->tbk.locked[j]))
	    { J6angles[j] = rbt->how->ang[j];
	    }
	 }
	return (rrange_check (J6->v, rbt->jls));
}

static tp_joint(mpb, joint, speed)
MOTION_PARAMETER_BLOCK *mpb;
int joint;
float speed;
{
	int status = 0;
	float delta_pos;
	float *J6angles = J6->v;
	float vscale;

	vscale = mpb->tbk.angleBaseSpeed[joint] * mpb->sampleTime / 1000.0;
	if (mpb->tbk.tick)
	 { delta_pos = speed * vscale * mpb->tbk.angle_tick;
	 }
	else
	 { delta_pos = speed * vscale;
	 }
	if (!mpb->tbk.plus)
 	 { delta_pos = -delta_pos;
	 }
	J6angles[joint] += delta_pos;
	status = check_limits (status, J6->v, mpb->tbk.old_joints.v, 
					    mpb->rbt->jls);
	if (status != 0)
 	 { *J6 = mpb->tbk.old_joints;
	 }
 	return (status);
}

static float compute_scale (tb, ctime)
register TEACH_BLOCK *tb;
float ctime;
{
	float ratio;

 	if (ctime < tb->time1)
	 { ratio = tb->a_rel * ctime * ctime / 2.0;
	 }
 	else if (ctime < tb->time2)
 	 { ratio = tb->a_rel * tb->time1 * tb->time1 / 2.0 + 
		   tb->v_rel * (ctime - tb->time1);
	 }
 	else if (ctime < tb->time3)
	 { ratio =   tb->a_rel * tb->time1 * tb->time1 / 2.0 + 
		     tb->v_rel * (ctime- tb->time1) -
		     tb->a_rel * (ctime- tb->time2) * (ctime - tb->time2)/2.0;
	 }
 	else
  	 { ratio = 1.0;
	 }
 	return (1.0 - ratio);
	
}

static cp_tool_world_to_angle (mpb)
MOTION_PARAMETER_BLOCK *mpb;
{
	register MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mpb->mcb;
	RCI_RBT *rbt = mpb->rbt;

	TRSF drive_trans, old_t6;
	DRIVEP temp_drive;
	float scale;
	int status = 0;
 
	scale = compute_scale (&mpb->tbk, mpb->tbk.ctime);

	temp_drive 	 = mpb->tbk.drvp;
	temp_drive.dx   *= scale;
 	temp_drive.dy   *= scale;
 	temp_drive.dz   *= scale;
 	temp_drive.dthe *= scale;
 	temp_drive.dphi *= scale;

	driveToTrsf (&drive_trans, &temp_drive);
	old_t6 = *T6;

	multTrsf (T6, &mcb->t_dest_t6, &drive_trans);
	status = INV_KINEMATICS (J6->v, T6, mpb->config,
					mpb->tbk.old_joints.v, NULL);
	status &= ~(KYN_SINGULAR);
	status = check_limits (status, J6->v, mpb->tbk.old_joints.v, 
					    rbt->jls);

	mpb->tbk.t6Computed = 1;

	if (status != 0)
 	 { *J6 = mpb->tbk.old_joints;
	   *T6 = old_t6;
	   mpb->tbk.cmove_completed = YES;
	   return (status);
	 }
	if (scale < EPSILON)
	 { mpb->tbk.cmove_completed = YES;
	 }
	return (0);
}

check_limits (status, set_angles, old_joints, jls)
int status;
float *set_angles;
float *old_joints;
JLS *jls;
{
 	if (status == 0)
 	 { float diff;
	   float rate = getCurrentTask()->tcb->interval / 1000.0;

	   status = rrange_check (set_angles, jls);
 
	   diff = old_joints[3] - set_angles[3];
	   if (diff < 0.)
	    { diff = -diff;
	    }
	   if (diff >= jls->maxVel[3] * rate)
	    { status |= 0x8;
	    }
	   diff = old_joints[5] - set_angles[5];
	   if (diff < 0.)
	    { diff = -diff;
	    }
	   if (diff >= jls->maxVel[5] * rate)
	    { status |= 0x20;
	    }
	 }
 	return (status);
}

static cp_joint (mpb)
MOTION_PARAMETER_BLOCK *mpb;
{
 	float scale;
	JNTS jnts;
	int status = 0;

	scale = compute_scale(&mpb->tbk, mpb->tbk.ctime);
	scaleJnts (&jnts, scale, &mpb->tbk.diff_angles);
	subtractJnts (J6, &mpb->mcb->t_angles, &jnts);

	status = check_limits (status, J6->v, mpb->tbk.old_joints.v, 
					    mpb->rbt->jls);
 	if (status != 0)
	 { *J6 = mpb->tbk.old_joints;
	   mpb->tbk.cmove_completed = YES;
	   return (status);
	 }

	if (scale < EPSILON)
  	 { mpb->tbk.cmove_completed = YES;
	 }
 	return (0);
}
	
static set_cmove_times (mpb, mode)
MOTION_PARAMETER_BLOCK *mpb;
int mode;
{
	register TEACH_BLOCK *tb = &(mpb->tbk);
	register MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mpb->mcb;
	float v_max, a_max, vscale;
	float max_dis = 0.;

	vscale = mpb->sampleTime / 1000.0;
	
 	switch (mode)
	 { case JOINT_M:
	       { float *diffAng = tb->diff_angles.v;
		 float maxTime;
		 int j, jmax;

		 subtractJnts (&tb->diff_angles, &mcb->t_angles, J6);

		 maxTime = -1.0;
		 for (j=0; j<6; j++)
		  { if ((FABS(diffAng[j])/tb->angleBaseSpeed[j]) > maxTime)
		     { maxTime = FABS (diffAng[j])/tb->angleBaseSpeed[j];
		       jmax = j;
		     }
		  }
		 v_max = mcb->t_speed * vscale * tb->angleBaseSpeed[jmax];
		 a_max = LOW_SPEED * vscale * tb->angleBaseSpeed[jmax];
		 max_dis = FABS(diffAng[jmax]);
		 break;
	       }
	   case WORLD_M:
	   case TOOL_M:
	       { float max_trsl = 0.;
		 float max_rot = 0.;
		 TRSF tmp;

		 trsfToDrive (&tb->drvp, multLiTrsf(&tmp,&mcb->t_dest_t6,T6));
/*		 set_driveparams (&tb->drvp, &mcb->t_dest_t6, T6); */

		 if (FABS(tb->drvp.dx) > max_trsl)
		  { max_trsl = FABS(tb->drvp.dx);
		  }
		 if (FABS(tb->drvp.dy) > max_trsl)
		  { max_trsl = FABS(tb->drvp.dy);
		  }
		 if (FABS(tb->drvp.dz) > max_trsl)
		  { max_trsl = FABS(tb->drvp.dz);
		  }
	 	
		 if (FABS(tb->drvp.dphi) > max_rot)
		  { max_rot = FABS(tb->drvp.dphi);
		  }
		 if (FABS(tb->drvp.dthe) > max_rot)
		  { max_rot = FABS(tb->drvp.dthe);
		  }

		 if (max_trsl/tb->trsl_base_speed > 
		     max_rot/tb->rad_base_speed)
		  { v_max = mcb->t_speed * tb->trsl_base_speed * vscale;
		    a_max = LOW_SPEED * tb->trsl_base_speed * vscale;
		    max_dis = max_trsl;
		  }
		 else
		  { v_max = mcb->t_speed * tb->rad_base_speed * vscale;
		    a_max = LOW_SPEED * tb->rad_base_speed * vscale;
		    max_dis = max_rot;
		  }
		 break;
	       }
	 }

	if (mcb->t_speed == 0. || max_dis == 0.)
  	 { tb->time1 = tb->time2 = tb->time3 = 0.;
	   return;
	 }

	tb->time1 = v_max / a_max;
	if (a_max * tb->time1 * tb->time1 > max_dis)
	 { tb->time1 = sqrt (max_dis / a_max);
	   tb->time2 = tb->time1;
	   v_max = a_max * tb->time1;
	 }
	else
	 { tb->time2 = (max_dis - a_max * tb->time1 * tb->time1) / v_max + 
		       tb->time1;
	 }

	tb->time3 = tb->time1 + tb->time2;	

	mpb->tbk.v_rel = v_max / max_dis;
	mpb->tbk.a_rel = a_max / max_dis;
}
	
read_angles(ang, how)
float *ang;
HOW *how;
{
	ang[0] = how->ang[0];
	ang[1] = how->ang[1];
	ang[2] = how->ang[2];

	ang[3] = how->ang[3];
	ang[4] = how->ang[4];
	ang[5] = how->ang[5];
}

rrange_check (ang, jls)
float *ang;
JLS *jls;
{
	int status;
	float margin;

	register int j;
	
	status = 0;
	for (j=0; j<6; j++)
	 { if (jls->jtype[j] == 'R')
	    { margin = DEGTORAD*2.5;
	    }
	   else
	    { margin = 0.01*jls->jrng[j];
	    }
	   if (ang[j] <= (jls->jmin[j]+margin) || 
	       ang[j] >= (jls->jmax[j]-margin) )
	    { status |= (1<<j);
	    }
	 }
	return (status);
}

update_mnp(mpb)
MOTION_PARAMETER_BLOCK *mpb;
{
	JNTS jvel;
	MANIP *mnp = mpb->mnps[rciCpu];

	subtractJnts (&jvel, J6, &mpb->tbk.old_joints);

	writeMem (mnp->j6, J6);
	writeMem (mnp->jvel, &jvel);
	if (mnp->conf != mpb->config)
	 { setManipConf (mpb, mpb->config);
	 }
	writeTrans (mnp->t6, T6);
}
