#include <rccl.h>
#include <puma_kynvar.h>
#include <errorCodes.h>

#include "manex.260.h"

#define SENSOR_Z_OFFSET         90.0

#define NEWTONS_PER_POUND       4.448
#define LB_PER_UF               (1.0/40.0)
#define NEWTONS_PER_UF          (4.448/40.0)
#define MM_PER_INCH             25.4

computeForce (arg, mnp)
int arg;
MANIP *mnp;
{
        COMPLY_CTRL_BLK *cbk;
        FORCE force1;
        FORCE force2;
        char *fxnName = "computeForce()";
                                                                /*10*/
        if ((cbk = (COMPLY_CTRL_BLK*)getMemByAddr ((void*)arg)) == NULL)
         { rciAbort (EFatal, "%s -- can't find control block\n", fxnName);
           return;
         }

        if (cbk->init)
         { SET_FORCE_READ (mnp->rbt, 1);                        /*12*/
           cbk->init = NO;
           cbk->valid = 0;
         }
        else
         { if (mnp->how->forceOK)                               /*13*/
            { TRSF stoc;
              
              force1.f.x = mnp->how->forceInput[0]*NEWTONS_PER_UF;
              force1.f.y = mnp->how->forceInput[1]*NEWTONS_PER_UF;
              force1.f.z = mnp->how->forceInput[2]*NEWTONS_PER_UF;

              force1.m.x = mnp->how->forceInput[3]*NEWTONS_PER_UF*MM_PER_INCH;
              force1.m.y = mnp->how->forceInput[4]*NEWTONS_PER_UF*MM_PER_INCH;
              force1.m.z = mnp->how->forceInput[5]*NEWTONS_PER_UF*MM_PER_INCH;

              multRiTrsf (&stoc, mnp->tool, getTransByAddr(cbk->comply));
              stoc.p.z -= SENSOR_Z_OFFSET;                      /*14*/
              transForce (&force2, &force1, &stoc);
              accessMem (cbk, &force2, &cbk->force, 6*sizeof(float));
              cbk->valid = 1;                                   /*15*/
            }
           else
            { cbk->valid = 0;                                   /*16*/
            }
         }
}

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

        COMPLY_CTRL_BLK *cbk;
        FORCE complyValues;
        unsigned long complyMask;
        FORCE forceError;
        DIFF accomodate;
        TRSF accomTrsf;

        if ((cbk = (COMPLY_CTRL_BLK*)getMemByAddr ((void*)arg)) == NULL)
         { rciAbort (EFatal, "%s -- can't find control block\n", fxnName);
           return;
         }

        if (!cbk->valid)
         { if (cbk->badDataCount++ > 4)                         /*17*/
            { rciAbort (0, "bad force data for 4 consecutive cycles\n");
            }
           return;
         }
        cbk->badDataCount = 0;

        bzero ((char*)&forceError, sizeof(FORCE));

        getActiveComply (mnp, &complyMask, &complyValues);      /*18*/

        if (complyMask & ALONG_X)
         { forceError.f.x = (complyValues.f.x - cbk->force.f.x);
         }
        if (complyMask & ALONG_Y)
         { forceError.f.y = (complyValues.f.y - cbk->force.f.y);
         }
        if (complyMask & ALONG_Z)
         { forceError.f.z = (complyValues.f.z - cbk->force.f.z);
         }
        if (complyMask & ABOUT_X)
         { forceError.m.x = (complyValues.m.x - cbk->force.m.x);
         }
        if (complyMask & ABOUT_Y)
         { forceError.m.y = (complyValues.m.y - cbk->force.m.y);
         }
        if (complyMask & ABOUT_Z)
         { forceError.m.z = (complyValues.m.z - cbk->force.m.z);
         }                                                      /*19*/

        accomodate.t.x = forceError.f.x * cbk->tgain;
        accomodate.t.y = forceError.f.y * cbk->tgain;
        accomodate.t.z = forceError.f.z * cbk->tgain;

        accomodate.r.x = forceError.m.x * cbk->rgain;
        accomodate.r.y = forceError.m.y * cbk->rgain;
        accomodate.r.z = forceError.m.z * cbk->rgain;

        diffToTrsf (&accomTrsf, &accomodate);                   /*20*/

        multTrsf (t, &accomTrsf, t);                            /*21*/
}
