#include <rccl.h>
#include "manex.260.h"

extern int touchFxn();

#ifdef VXWORKS_CFG
#define main guarded_260_
#endif

main()
{
        TRSF_PTR target, e, contact[2];
        TRSF start[2];
        POS_PTR p0;
        MANIP *mnp;
        JNTS rcclpark;
        char *robotName;
        int mid;
        int bothTouched;
        int i;

        rcclSetOptions (RCCL_ERROR_EXIT);
        robotName = getDefaultRobot();
        if (!getRobotPosition (rcclpark.v, "rcclpark", robotName))
         { printf ("position 'rcclpark' not defined for robot\n");
           exit(-1);
         }

        target = allocTrans (NULL, UNDEF);
        e = allocTrans (NULL, UNDEF);
        contact[0] = allocTrans (NULL, UNDEF);
        contact[1] = allocTrans (NULL, UNDEF);

        p0 = makePosition (NULL, T6, e, EQ, target, TL, e);

        mnp = rcclCreate (robotName, 0);
        rcclStart();

        setJvelScale (mnp, 2.0);
        movej (mnp, &rcclpark);
        waitForCompleted (mnp);

        xyzToTrsf (e, 0.0, 0.0, TOOLZ);
        solveTrans (&start[0], p0, target, mnp->t6);
        solveTrans (&start[1], p0, target, mnp->t6);
        multTrsfXyz (&start[0], -350.0,  50.0, 0.0);
        multTrsfXyz (&start[1], -350.0, 350.0, 0.0);

        /* turn on 1st ADC channel, *if* we have one */

        if (mnp->how->numAdc > 0)
         { SET_ADC_CHAN (mnp->rbt, 0x1);
         }
        else
         { printf ("no real ADC channel; going to fake it\n");
         }

        bothTouched = 1;
        mid = 0;
        for (i=0; i<2; i++)
         { *target = start[i];
           move (mnp, p0);

           setMod (mnp, 'c');
           setCartVel (mnp, 10.0, F_DEFAULT);
           setMotionFlag (mnp, ++mid);
           runMotionFxn (mnp, touchFxn, mid);
           updateTrans (mnp, contact[i], p0, target, mnp->t6);
           distance (mnp, "dz", 100.0);
           move (mnp, p0);

           setTime (mnp, 0.0, F_DEFAULT);
           distance (mnp, "dz", -20.0);
           move (mnp, mnp->last);

           setMod (mnp, 'j');
           *target = start[i];
           move (mnp, p0);

           waitForStop (mid);
           if (motionStopCode(mid) == 1)
            { printf ("Motion %d touched\n", i+1);
            }
           else
            { printf ("Motion %d did not touch\n", i+1);
              bothTouched = 0;
            }
         }
        waitForCompleted (mnp);

        if (bothTouched)
         { printf ("playing back ...\n");

           setMod (mnp, 'c');
           setCartVel (mnp, F_DEFAULT, F_DEFAULT);
           *target = start[0];
           move (mnp, p0);
           *target = *contact[0];
           target->p.z += 5.0;
           move (mnp, p0);
           *target = *contact[1];
           target->p.z += 5.0;
           move (mnp, p0);
           *target = start[1];
           move (mnp, p0);
           waitForCompleted (mnp);
           setMod (mnp, 'j');
         }
        movej (mnp, &rcclpark);
        stop (mnp, 1000.0);
        waitForCompleted (mnp);

        rcclRelease (YES);
}
