#include <rccl.h>                                               /*1*/
#include "manex.560.h"

#define BOXSIZE         150.0

main()
{
        TRSF_PTR e, b, c[4];                                    /*2*/
        POS_PTR corners[4], start;
        MANIP *mnp;
        char *robotName;
        int k, ia, iv;
                                                                /*3*/
        static float accelValues[] = { F_DEFAULT, 4000.0, 20.0 };
        static char *accelDesc[] = { "default", "high", "low" };
        static float velValues[] = { F_DEFAULT, 400.0, 40.0 };
        static char *velDesc[] = { "default", "fast", "slow" };

        rcclSetOptions (RCCL_ERROR_EXIT);                       /*4*/
        robotName = getDefaultRobot();                          /*5*/

        e = allocTransXyz ("E", UNDEF, 0.0, 0.0, TOOLZ);        /*6*/
        b = allocTransRot ("B", UNDEF, B_X, B_Y, B_Z, xunit, 180.0);

        start = makePosition ("start", T6, e, EQ, b, TL, e);    /*7*/

        c[0] = allocTransXyz (NULL, UNDEF,  BOXSIZE/2, 0.0, -BOXSIZE/2);
        c[1] = allocTransXyz (NULL, UNDEF, -BOXSIZE/2, 0.0, -BOXSIZE/2);
        c[2] = allocTransXyz (NULL, UNDEF, -BOXSIZE/2, 0.0,  BOXSIZE/2);
        c[3] = allocTransXyz (NULL, UNDEF,  BOXSIZE/2, 0.0,  BOXSIZE/2);
        for (k=0; k<4; k++)                                     /*8*/
         { corners[k] = makePosition (NULL, T6, e, EQ, b, c[k], TL, e);
         }

        mnp = rcclCreate (robotName, 0);                        /*9*/
        rcclStart();

        move (mnp, start);                                      /*10*/
        stop (mnp, 1000.0);
        waitForCompleted (mnp);                                 /*11*/

        setMod (mnp, 'c');                                      /*12*/

        for (ia=0; ia<ARRAY_SIZE(accelValues); ia++)            /*13*/
         { for (iv=0; iv<ARRAY_SIZE(velValues); iv++)
            { float vel, accel;

              setCartAccel (mnp, accelValues[ia], F_UNDEF);
              setCartVel (mnp, velValues[iv], F_UNDEF);
              getCartAccel (mnp, &accel, (float*)NULL);
              getCartVel (mnp, &vel, (float*)NULL);

              printf ("%s vel (%g), %s acceleration (%g):\n",
                      velDesc[iv], vel, accelDesc[ia], accel);

              moveSet (mnp, corners, ARRAY_SIZE(corners));      /*14*/
              waitForCompleted (mnp);                           /*15*/
            }
         }

        setMod (mnp, 'j');                                      /*16*/
        move (mnp, start);
        stop (mnp, 1000.0);
        waitForCompleted (mnp);                                 /*17*/

        rcclRelease (YES);
}

moveSet (mnp, p, size)
MANIP *mnp;
POS_PTR *p;
int size;
{
        int k;

        for (k=0; k<size; k++)
         { move (mnp, p[k]);
         }
}
