/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *
 * MODULE NAME:	moves
 *
 * DESCRIPTION: This file contains the RCCL primitives for actually
 *		requesting motions and stops. They are front ends to
 *		the general motionRequest() function which lives
 *		in motionRequest.c.
 *
 * AUTHOR:	Vincent Hayward, Purdue University, 1983.
 *		John Lloyd, McGill University, 1983
 *
 * MODIFICATION HISTORY:
 *
 *------------------------------------------------------------------------*/

#include "../h/cdefs.h"
#include "../h/rcclDefs.h"
#include "../h/errorCodes.h"
#include "../h/robot_kynvar.h"

#include "../rccl/motionRequest.h"
#include "../rccl/rcclAutoAbort.h"
#include "../rccl/rcclErrorMsgs.h"
#include "../rccl/kynMacros.h"

move(mnp, p)
MANIP *mnp;
POS *p;
{
	/* Basic motion request: have the manipulator move to the
	   target specified by the position 'p'.
	 */

	char *fxnName = "move()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	int mid;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();
	mvb->goalPosition = p;
	mvb->jointPositionSet = NO;
	mvb->doDrive = YES;
	mvb->stopMotion = NO;
	if ((mid = motionRequest(mnp)) < 0)
	 { errorMsg (getErrorCode(0), rcclMotionReqMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	RETURN_NORMAL(mid);
}

stop(mnp, t_segment)
MANIP *mnp;
double t_segment;
{
	/* Request a stop() for a given length of time at the last target
	   moved to. It the target is moving, then the manipulator will
	   still move in that it will track the target.
	 */

	char *fxnName = "stop()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	int mid;

       /* stop issues move request without pos, meaning goto last position */

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();
	if (t_segment < 0.0 && t_segment != F_DEFAULT
			    && t_segment != F_UNDEF)
	 { errorMsg (ERange, rcclArgNegMsg, fxnName);
	   RETURN_ERROR(-1);
	 }
	if (mvb->goalPosition == NULL && !mvb->jointPositionSet)
	 { RETURN_NORMAL(0);		/* there have been no prev. moves */
	 }
	if (t_segment != F_DEFAULT)
	 { if (t_segment == F_UNDEF)
	    { mvb->segmentTime = UNDEF;
	    }
	   else
	    { mvb->segmentTime = (int)t_segment;
	    }
	 }
	else
	 { if (mvb->segmentTimeSet)
	    { mvb->segmentTime = 0;
	    }
	 }
	mvb->segmentTimeSet = YES;
	mvb->stopMotion = 1;
	mvb->doDrive = NO;
	mvb->stopMotion = YES;
	if ((mid = motionRequest(mnp)) < 0)
	 { errorMsg (getErrorCode(0), rcclMotionReqMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	RETURN_NORMAL(mid);
}

maintain (mnp, p)
MANIP *mnp;
POS *p;
{
	/* Simply 'place' a manipulator at a particular position, and
	   keep it tracking that position until another motion request
	   comes along.
	 */
	char *fxnName = "maintain()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	int mid;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();
	mvb->goalPosition = p;
	mvb->jointPositionSet = NO;
	mvb->segmentTime = UNDEF;
	mvb->segmentTimeSet = YES;
	if (!mvb->transitionTimeSet)
	 { mvb->transitionTime = 0.0;
	   mvb->transitionTimeSet = YES;
	 }
	mvb->doDrive = NO;
	mvb->stopOnNextRequest = YES;
	mvb->stopMotion = NO;

	if (!mnp->objectFlag)
	 { errorMsg (ENotDefined, rcclNotVirtMnpMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	if ((mid = motionRequest(mnp)) < 0)
	 { errorMsg (getErrorCode(0), rcclMotionReqMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	RETURN_NORMAL(mid);
}

movej (mnp, jnts)
MANIP *mnp;
JNTS *jnts;
{
	/* Joint based motion request: Have the manipulator move
	   the target specified by the joint angles 'jnts'.
	 */
	char *fxnName = "movej()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	RCI_RBT *rbt;
	JLS *jls;
	float *jp;
	int j, mid;
	
	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();
	if ((rbt = MANIP_ROBOT(mnp)) == NULL)
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	jls = rbt->jls;
	jp = &(jnts->v[0]);
	for (j = 0; j < 6; j++)
	 { if (jp[j] <= jls->jmin[j] || jp[j] >= jls->jmax[j])
	    { errorMsg (ELimit, rcclRangeLimMsg, fxnName, jp[j], j+1);
	      RETURN_ERROR (-1);
	    }
	 }
	mvb->goalPosition = NULL;
	mvb->jointPositionSet = YES;
	mvb->jointPosition = *jnts;
	mvb->newShiftSet = NO;		/* cancel distance() requests */
	mvb->doDrive = YES;
	mvb->stopMotion = NO;

	/* --- do the implicit 'setConf()' --- */

	 { TRSF t6;
	   FWD_KINEMATICS (&t6, &mvb->config, jnts->v, NULL);
	   mvb->configSet = 0;
	 }

	if ((mid = motionRequest(mnp)) < 0)
	 { errorMsg (getErrorCode(0), rcclMotionReqMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	RETURN_NORMAL(mid);
}

checkMove(mnp, p, status)
MANIP *mnp;
POS *p;
int *status;
{
	/* Check to see if a motion to the position 'p' (given
	   the current values of 'p') would be within range */

	char *fxnName = "checkMove()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	RCI_RBT *rbt;
	JLS *jls;
	TRSF t6;
	JNTS j6;
	int code, conf;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if ((rbt = MANIP_ROBOT(mnp)) == NULL)
	 { return (1);
	 }
	jls = rbt->jls;
	solveTrans (&t6, p, T6, (TRSF*)NULL);
	conf = (mvb->config == UNDEF ? mnp->conf : mvb->config);
	code = INV_KINEMATICS (j6.v, &t6, conf, mnp->j6->v, NULL);
	if (!code & (KYN_JOINT_LIMIT | KYN_CANT_REACH))
	 { return (1);
	 }
	else
	 { if (status)
	    { *status = code;
	    }
	   return (0);
	 }
}
