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

extern int circleFxn();

#define CIRCLE_RADIUS   50

main()
{
        JNTS rcclpark1;
        JNTS rcclpark2;
        TRSF_PTR offset, e, track;
        POS_PTR mnpPos, objPos;
        MANIP_PTR mnp1, mnp2, obj;
        float tval, rval;

        char *robotName1 = "Rsun";                              /*1*/
        char *robotName2 = "Lsun";

        rcclSetOptions (RCCL_ERROR_EXIT);

        if (!getRobotPosition (rcclpark1.v, "rcclpark", robotName1))
         { printf ("position 'rcclpark' not defined for robot\n");
           exit(-1);
         }
        if (!getRobotPosition (rcclpark2.v, "rcclpark", robotName2))
         { printf ("position 'rcclpark' not defined for robot\n");
           exit(-1);
         }

        e = allocTransXyz (NULL, UNDEF, 0.0, 0.0, TOOLZ);
        track = allocTrans (NULL, UNDEF);
        offset = allocTrans (NULL, UNDEF);

        mnp1 = rcclCreate (robotName1, 0);
        mnp2 = rcclCreate (robotName2, 0);
        obj = rcclCreate ("foo", 0);                            /*2*/

        mnpPos = makePosition (NULL, T6, e, EQ, offset, obj->t6, TL, e);
        objPos = makePosition (NULL, T6, EQ, track, TL, T6);

        rcclStart();

        movej (mnp1, &rcclpark1);                               
        movej (mnp2, &rcclpark2);
        waitForCompleted (mnp1);                                /*3*/
        waitForCompleted (mnp2);

        setMod (mnp1, 'c');
        setTime (mnp1, F_DEFAULT, F_UNDEF);
        solveTrans (offset, mnpPos, offset, mnp1->t6);
        move (mnp1, mnpPos);
        setMod (mnp2, 'c');
        setTime (mnp2, F_DEFAULT, F_UNDEF);
        solveTrans (offset, mnpPos, offset, mnp2->t6);
        move (mnp2, mnpPos);                                    /*4*/

        getCartVel (mnp1, &tval, &rval);
        setCartVel (obj, tval, rval);
        getCartAccel (mnp1, &tval, &rval);
        setCartAccel (obj, tval, rval);                         /*5*/
           
        while (1)
         { distance (obj, "dx", 50.0);
           move (obj, obj->last);
           distance (obj, "dz", -100.0);
           move (obj, obj->last);
           distance (obj, "dx", -100.0);
           move (obj, obj->last);
           distance (obj, "dz", 100.0);
           move (obj, obj->last);
           distance (obj, "dx", 50.0);
           move (obj, obj->last);                               /*6*/

           waitForCompleted (obj);

           setTime (obj, F_DEFAULT, 2000.0);
           transMotionEval (track, circleFxn, CIRCLE_RADIUS, obj);
           move (obj, objPos);                                  /*7*/
           move (obj, obj->park);                               /*8*/
         }
}
