/*-=-=-=-=-=-=-=-=-=-=-=- 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 "../h/robot_kynvar.h"
#include "../h/chase.h"
#include "../h/rciTask.h"
#include "../h/robotMath.h"
#include "../h/errorCodes.h"

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

robot_grav(t, rbt)
float *t;
RCI_RBT *rbt;
{
	char *doFriction;
	float maxForce;
	float diff;
	register int j;

	KYN *kyn = rbt->kyn;
	HOW *how = rbt->how;
	JLS *jls = rbt->jls;

	if ((doFriction = (char*)getMemByName("doFriction")) == NULL)
	 { rciAbort (0, "can't get memory 'doFriction'\n");
	   return;
	 }

	gravload (t, rbt->how->ang, kyn);
	
	for (j=0; j<jls->numJ; j++)
	 { 
#if SOFT_STOPS
	      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];
		 t[j] -= MIN(maxForce, (diff/STOP_RAMP_LENGTH)*maxForce);
		 break;
	       }
	      else if ((diff = jls->jmin[j]+STOP_MARGIN-how->ang[j]) > 0.0)
	       { maxForce = STOP_FORCE_FACTOR*kyn->gen->coulFriction[j];
		 t[j] += MIN(maxForce, (diff/STOP_RAMP_LENGTH)*maxForce);
		 break;
	       }
	    }
#endif
	   if (doFriction[j])
	    { if (how->angVel[j] < 0.0)
	       { t[j] -= kyn->gen->coulFriction[j];
	       }
	      else if (how->angVel[j] > 0.0)
	       { t[j] += kyn->gen->coulFriction[j];
	       }
	    }
	 }
}
