#include <rccl.h>
#include <fastmath.h>
#include "manex.560.h"

float clip (delta, maxDelta)                                    /*5*/
float delta;
float maxDelta;
{
        return (delta >= 0.0 ? MIN(delta, maxDelta) : -MIN(-delta, maxDelta));
}

trackFxn (t, arg, mnp)
TRSF *t;
int arg;
MANIP *mnp;
{
        char *fxnName = "trackFxn()";

        TRACK_CTRL_BLK *tcb;
        RCI_RBT *rbt;
        float maxDelta;
        float delta;
        float x, y;
                                                                /*6*/
        if ((tcb = (TRACK_CTRL_BLK*)getMemByAddr((void*)arg)) == NULL)
         { rciAbort (0, "%s -- Can't find paramter block\n", fxnName);
           return;
         }
        rbt = (getManipByAddr(tcb->mnpAddr))->rbt;              /*7*/

        if (tcb->tracking)
         { getInput (rbt, &x, &y);
           tcb->xval = x*tcb->gain;
           tcb->yval = y*tcb->gain;                             /*8*/

           maxDelta = FABS(tcb->vlimit) * rcclGetInterval() / 1000.0;
           t->p.x += clip (tcb->xval - t->p.x, maxDelta);
           t->p.y += clip (tcb->yval - t->p.y, maxDelta);       /*9*/

           if (!tcb->compute)                                   /*10*/
            { turnInputOff (rbt);
              tcb->tracking = 0;
            }
         }
        else
         { if (tcb->compute)                                    /*11*/
            { turnInputOn (rbt);
              tcb->tracking = 1;
            }
         }
}

static float faket;
static int setBias;
static int xbias;
static int ybias;

turnInputOn(rbt)
RCI_RBT *rbt;
{
        if (rbt->how->numAdc > 0)
         { SET_ADC_CHAN (rbt, 0x3);                             /*12*/
	   setBias = 1;
         }
        else
         { rciPrintf ("will fake the input\n");                 /*13*/
           faket = 0.0;
         }
}

turnInputOff(rbt)
RCI_RBT *rbt;
{
        if (rbt->how->numAdc > 0)
         { SET_ADC_CHAN (rbt, 0x0);                             /*14*/
         }
}

getInput(rbt, x, y)
RCI_RBT *rbt;
float *x, *y;
{
        if (rbt->how->numAdc > 0)
         { if (rbt->how->adc & 0x3)
	    { if (setBias)
	       { xbias = rbt->how->adcInput[0];
		 ybias = rbt->how->adcInput[1];
		 setBias = 0;
	       }
              *x = rbt->how->adcInput[0] - xbias;
              *y = rbt->how->adcInput[1] - ybias;                /*15*/
            }
         }
        else
         { /* fake the input ... draw an ellipse */
           faket += rcclGetInterval()/2000.0;
           *x = 100*COS(PI*faket);
           *y = -50*SIN(PI*faket);
         }
}
