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

#define FLAG    2

onSignal()                                                      /*2*/
{
        printf ("Column done\n");
}

#ifdef VXWORKS_CFG
#define main hex_560_
#endif

main()
{
        TRSF_PTR e, park, pickup, top, down;
        POS_PTR home, reload, center;
        MANIP *mnp;
        JNTS rcclpark;
        char *robotName;

        float hexsize = 15.0;
        int numRows = 5;
        int numCols = 5;
        int i, j;

        float sin_60;

        int mid;

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

        e = allocTransXyz ("E", UNDEF, 0.0, 0.0, TOOLZ);
        park = allocTrans ("PARK", UNDEF);                      /*4*/
        pickup = allocTrans ("PICKUP", UNDEF);
        top = allocTrans ("TOP", UNDEF);
        down = allocTrans ("DOWN", UNDEF);
                                                                
        home = makePosition ("home", T6, e, EQ, park, TL, e);   /*5*/
        reload = makePosition ("reload", T6, e, EQ, pickup, TL, e);
        center = makePosition ("center", T6, e, EQ, park, top, down, TL, e);

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

        movej (mnp, &rcclpark);                                 /*7*/
        waitForCompleted (mnp);

        solveTrans (park, home, park, mnp->here);               /*8*/

        rotToTrsf (pickup, zunit, 90.0);                        /*9*/
        multTrsf (pickup, pickup, park);
        multTrsfXyz (pickup, 0.0, 0.0, -125.0);
        multTrsfRot (pickup, xunit, 30.0);

        xyzToTrsf (top, 100.0, 0.0, 50.0);                      /*10*/
        multTrsfRot (top, xunit, 90.0);

        setJvelScale (mnp, 2.0);                                /*11*/
        setCartVel (mnp, 100.0, 50.0);
        signal (SIGUSR1, onSignal);

        sin_60 = sin(60.0*DEGTORAD);                            /*12*/

        for (j=0; j<numCols; j++)                               /*13*/
         { setConf (mnp, "n");                                  /*14*/

           distance (mnp, "dz", -50.0);                         /*15*/
           move (mnp, reload);
           mid = move (mnp, reload);
           stop (mnp, 2000.0);
           distance (mnp, "dz", -50.0);
           move (mnp, reload);

           waitForStop (mid);                                   /*16*/
           OPEN_HAND (mnp);
           delay (500.0);
           CLOSE_HAND (mnp);

           setConf (mnp, "f");                                  /*17*/

           top->p.x -= 2*hexsize*sin_60;
           identTrsf (down);                                    /*18*/
           for (i=0; i<numRows-1; i++)                          /*19*/
            { traceHex (mnp, center, hexsize, 50.0, UNDEF);
              if (i%2 == 0)                                     /*20*/
               { multTrsfXyz (down, hexsize*sin_60, 3/2.*hexsize, 0.0);
               }
              else
               { multTrsfXyz (down, -hexsize*sin_60, 3/2.*hexsize, 0.0);
               }
            }
           traceHex (mnp, center, hexsize, 50.0, FLAG);
           setStopSig (mnp, SIGUSR1, FLAG);                     /*21*/
         }

        setConf (mnp, "n");                                     /*22*/
        move (mnp, home);
        stop (mnp, 1000.0);
        waitForCompleted (mnp);
        rcclRelease (YES);
}

traceHex (mnp, p, hexsize, approach, flag)
MANIP *mnp;
POS *p;
float hexsize, approach;
int flag;
{
        TRSF offset;
        int saveMod;
        int mid;
        int theta;

        xyzToTrsf (identTrsf(&offset), 0.0, 0.0, -approach);
        setDistance (mnp, &offset);
        move (mnp, p);                                          /*23*/

        saveMod = getMod(mnp);                                  /*24*/
        setMod (mnp, 'c');                                      /*25*/

        offset.p.z = 0.0;                                       /*26*/
        for (theta=30; theta<=390; theta+=60)
         { offset.p.x = hexsize * cos(DEGTORAD*theta);
           offset.p.y = hexsize * sin(DEGTORAD*theta);
           setDistance (mnp, &offset);
           move (mnp, p);                                       /*27*/
         }
        xyzToTrsf (identTrsf(&offset), 0.0, 0.0, -approach);
        setDistance (mnp, &offset);
        if (flag != UNDEF)
         { setMotionFlag (mnp, flag);                           /*28*/
         }
        mid = move (mnp, p);                                    /*29*/
        setMod (mnp, saveMod);                                  /*30*/
        return (mid);
}
