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

TRSF *computeU5();

int numberOfObjects;

main()
{
        TRSF_PTR z, e, cam, o, coord, expect, drop;
        POS_PTR look, get, put;
        MANIP *mnp;
        TRSF t6r, u5;
        JNTS rcclpark, j6r;
        char *robotName;
        int putId, lookId, getId;

        numberOfObjects = 5;

        rcclSetOptions (RCCL_ERROR_EXIT);

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

        z = allocTransXyz (NULL, UNDEF, 0.0, 0.0, ZBASE);
        e = allocTransXyz (NULL, UNDEF, 0.0, 0.0, TOOLZ);
        cam = allocTransRot (NULL, UNDEF, 50.0, 0.0, 0.0, xunit, 90.0);
        expect = allocTransRot (NULL, UNDEF, 400.0, 400.0, 700.0,
                                          xunit, 180.0);
        drop = allocTransRot (NULL, UNDEF, -400.0, 400.0, 700.0,
                                          xunit, 180.0);
        o = allocTrans (NULL, UNDEF);
        coord = allocTrans (NULL, UNDEF);

        look = makePosition (NULL, z, T6, e, EQ, expect, TL, e);
        get = makePosition (NULL, T6, e, EQ, coord, cam, o, TL, e);
        put = makePosition (NULL, z, T6, e, EQ, drop, TL, e);

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

        setSpeed (mnp, 2.0);                                    /*1*/

        OPEN_HAND(mnp);

        for ( ; ; )                                             /*2*/
         { lookId = move (mnp, look);
           waitFor (motionScale(lookId) > 0.7)                  /*3*/

           snapshot();                                          /*4*/
           readTrans (mnp->t6, &t6r);                           /*5*/
           readMem (mnp->j6, &j6r);
           computeU5 (&u5, j6r.v[4], j6r.v[5]);                 /*6*/
           multRiTrsf (coord, &t6r, &u5);

           if (!getobj(o))
            { break;
            }

           distance (mnp, "dz", -50.0);                         /*7*/
           move (mnp, get);

           getId = move (mnp, get);
           stop (mnp, 500.0);

           distance (mnp, "dz", -50.0);
           move (mnp, get);

           waitForStop (getId);                                 /*8*/

           CLOSE_HAND (mnp);

           move (mnp, put);
           putId = stop (mnp, 500.0);
           stop (mnp, 500.0);
           waitForStop (putId);

           OPEN_HAND (mnp);                                     /*9*/
         }

        movej (mnp, &rcclpark);
        stop (mnp, 1000.0);
        waitForCompleted (mnp);

        rcclRelease (1);
}

snapshot()
{
        printf ("\nTAKING PICTURE\n");
}

getobj(t)
TRSF_PTR t;
{
        extern double frandom();
        double rand;

        /* fake a camera coordinate reading */
        rand = frandom (-20.0, 20.0);
        xyzToTrsf (t, rand, rand, 150.0+rand);
        rpyToTrsf (t, 90.0, -25.0+rand, 0.0);
        printTrsf ("Object is at %m\n", t);

        return (numberOfObjects--);
}

TRSF *computeU5 (u5, ang5, ang6)
TRSF *u5;
float ang5, ang6;
{
        /* Compute U5 for a PUMA 560 (old coordinates) */

        float s5, c5, s6, c6;

        SINCOS (ang5, &s5, &c5);
        SINCOS (ang6, &s6, &c6);

        u5->n.x =  c5 * c6;
        u5->n.y =  s5 * c6;
        u5->n.z =  s6;
        u5->o.x = -c5 * s6;
        u5->o.y = -s5 * s6;
        u5->o.z =  c6;
        u5->a.x =  s5;
        u5->a.y = -c5;
        u5->a.z =  0.0;
        u5->p.x =  0.0;
        u5->p.y =  0.0;
        u5->p.z =  0.0;

        return (u5);
}
