/*=-=-=-=-=-=-=-=-=-=-=-  RCI/RCCL Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987 by John E. Lloyd. Permission to use, distribute  *
 *	and copy is granted for non-commercial and research purposes only,  *
 *	providing the authors are acknowledged and this copyright notice is *
 *	displayed. The authors provide this software in hope that it will   *
 *	useful, but do not assume responsibility for its use.		    *
 *									    *
 *	This software was developed using the facilities of the McGill 	    *
 *	University Research Center for Intelligent Machines, Montreal,	    *
 *	Canada, for a project undertaken and supported by the RCA Advanced  *
 *	Technology Laboratories, Moorestown, New Jersey.		    *
 *									    *
 *=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-*/


#include <math.h>
#include <rci.h>
#include <robot_kynvar.h>
#include "gravity.h"

startupFxn(td, arg)
RCI_DESC *td;
int arg;
{
	COMBLK *cbk;
        RCI_RBT *rbt;
	register int j;

	if ((cbk = (COMBLK*)getMemByAddr((void*)arg)) == NULL)
	 { rciAbort (0, "Can't find control block\n");
	   return (-1);
	 }
	cbk->scale = 1.0;
	cbk->block = 1;

	rbt = TASKROBOT (td, 0);
        (*rbt->startup)(td, rbt);
	JOINT_LEVEL_CONTROL(rbt);
}

zerogravFxn(td, arg)
RCI_DESC *td;
int arg;
{
        RCI_RBT *rbt = TASKROBOT (td, 0);
        HOW *how = rbt->how;
        KYN *kyn = rbt->kyn;
	COMBLK *cbk;

 	register int i;
 	static float setpos[MAXJNTS];
	static int first = YES;
 	static float gload [MAXJNTS];
 	static float fload [MAXJNTS];

	if ((cbk = (COMBLK*)getMemByAddr((void*)arg)) == NULL)
	 { rciAbort (0, "Can't find control block\n");
	   return (-1);
	 }

	if (ROBOT_WAKEUP(rbt) < 0)
	 { rciAbort (0, "Can't wake up robot\n");
	   return (-1);
	 }
/*	while (!(ROBOT_READY(rbt)))
	   ;
*/
        (*rbt->input)(td, rbt);

	if (first)
	 { first = NO;
	   for (i=0; i<rbt->jls->numJ; i++)
	    { setpos[i] = how->ang[i];
	      cbk->current[i] = 0;
	      cbk->current_mode[i] = NO;
	    }
	 }

	if (cbk->block)
 	 { (*rbt->output) (td, rbt);
	   return;
	 }

	gravload (gload, how->ang, kyn);

	gload[1] *= cbk->scale;
	gload[2] *= cbk->scale;
	gload[3] *= cbk->scale;
	gload[4] *= cbk->scale;

	frictionLoad (fload, how->angVel, kyn);

	for (i=0; i<rbt->jls->numJ; i++)
	 { if (cbk->current_mode[i] == C_ZEROG)
	    { setpos[i] = how->ang[i];
	      cbk->torque[i] = gload[i];
	      SET_JTORQUE (rbt, gload[i], i);
	    }
	   else if (cbk->current_mode[i] == C_TORQUE)
	    { setpos[i] = how->ang[i];
	      cbk->torque[i] = gload[i] + fload[i];
	      SET_JTORQUE (rbt, gload[i] + fload[i], i);
	    }
	   else
	    { MOVE_ANGLE (rbt, setpos[i], i);
	    }
	 }

        (*rbt->output) (td, rbt);
}
		    
releaseFxn (td, arg, code)
RCI_DESC *td;
int arg, code;
{
        RCI_RBT *rbt = TASKROBOT (td, 0);
        int rcode;

        if (code == RELEASE_REQ)
         { rcode = RBT_NORMAL;
         }
        else
         { rcode = RBT_HARDERROR;
         }
        (*rbt->release)(td, rbt, rcode);
        return (1);
}

frictionLoad (load, vel, kyn)
float *load;
float *vel;
KYN *kyn;
{
	register float *coulFriction = kyn->gen->coulFriction;

	if (vel[0] > 0.0)
	 { load[0] = -coulFriction[0];
	 }
	else if (vel[0] < 0.0)
	 { load[0] = coulFriction[0];
	 }
	else
	 { load[0] = 0.0;
	 }

	if (vel[1] > 0.0)
	 { load[1] = coulFriction[1];
	 }
	else if (vel[1] < 0.0)
	 { load[1] = -coulFriction[1];
	 }
	else
	 { load[1] = 0.0;
	 }

	if (vel[2] > 0.0)
	 { load[2] = -coulFriction[2];
	 }
	else if (vel[2] < 0.0)
	 { load[2] = coulFriction[2];
	 }
	else
	 { load[2] = 0.0;
	 }

	if (vel[3] > 0.0)
	 { load[3] = coulFriction[3];
	 }
	else if (vel[3] < 0.0)
	 { load[3] = -coulFriction[3];
	 }
	else
	 { load[3] = 0.0;
	 }

	if (vel[4] > 0.0)
	 { load[4] = coulFriction[4];
	 }
	else if (vel[4] < 0.0)
	 { load[4] = -coulFriction[4];
	 }
	else
	 { load[4] = 0.0;
	 }

	if (vel[5] > 0.0)
	 { load[5] = coulFriction[5];
	 }
	else if (vel[5] < 0.0)
	 { load[5] = -coulFriction[5];
	 }
	else
	 { load[5] = 0.0;
	 }
}
