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

#define PEG_LENGTH      230.0   /* length of comply peg */
#define PEG_RADIUS        9.5   /* radius of comply peg */
#define DROP_DEPTH       35.0   /* vertical dist. to probe points */
#define MAX_RADIUS       70.0   /* max. likely radius of cylinder */
#define NUM_TURNS           2   /* no. of times to go around cylinder */
#define MSEC_PER_REV  12000.0   /* speed to go around cylinder at */

extern forceMonitor();
extern rotzFxn();
extern COMPLY_CTRL_BLK *complyInit();

float square(x)
float x;
{
        return (x*x);
}

main ()
{
        MANIP *mnp;
        JNTS rcclpark;
        char *robotName;
        COMPLY_CTRL_BLK *cbk;

        TRSF_PTR start, rotz, offset, peg, touch;
        float xval[3], yval[3];
        POS_PTR idle, seek, turn;
        int i;

        float centerX;
        float centerY;
        float radius;

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

        start = allocTrans (NULL, UNDEF);
        offset = allocTrans (NULL, UNDEF);
        rotz = allocTrans (NULL, UNDEF);
        peg = allocTransXyz (NULL, UNDEF, 0.0, 0.0, PEG_LENGTH);
        touch = allocTrans (NULL, UNDEF);

        mnp = rcclCreate (robotName, 0);
        cbk = complyInit (mnp, 0.005, 0.00);                    /*1*/   

        idle = makePosition (NULL, T6, peg, EQ, start, TL, peg);
        seek = makePosition (NULL, T6, peg, rotz, EQ, 
                                   start, rotz, offset, TL, rotz);
        turn = makePosition (NULL, T6, peg, rotz, cbk->comply, EQ,
                                   start, rotz, offset, TL, cbk->comply);

        rcclStart ();
        complyRun (cbk);                                        /*2*/
        
        printf ("Position robot about %g mm. above cylinder, with peg down\n",
                 DROP_DEPTH-10.0);
        rcclTeach (mnp, "TEACH> ", NULL);                       /*3*/
        solveTrans (start, seek, start, mnp->here);             /*4*/

        /* locate the points around the cylinder */

        runMonitorFxn (mnp, forceMonitor, (int)cbk);            /*5*/

        setMod (mnp, 'c');
        offset->p.x = MAX_RADIUS;                               /*6*/
        for (i=0; i<3; i++)
         { int mid;
           TRSF delta;

           rotToTrsf (rotz, zunit, 120.0*i);                    /*7*/
           move (mnp, seek);                                    /*8*/
           offset->p.z = DROP_DEPTH;
           move (mnp, seek);                                    /*9*/

           limit (mnp, "fx", 5.0);
           setCartVel (mnp, 10.0, F_DEFAULT);                   /*10*/
           distance (mnp, "dx", -MAX_RADIUS);
           updateTrans (mnp, touch, idle, start, mnp->t6);      /*11*/
           mid = move (mnp, seek);                              /*12*/
           setCartVel (mnp, F_DEFAULT, F_DEFAULT);
           setTime (mnp, 0.0, F_DEFAULT);
           move (mnp, seek);                                    /*13*/
           offset->p.z = 0.0;
           move (mnp, seek);                                    /*14*/
           waitForStop (mid);
           if (motionStopCode(mid) != ON_FORCE)
            { printf ("Did not touch cylinder\n");
              rcclRelease (1);
              exit (-1);
            }
           multLiTrsf (&delta, start, touch);
           xval[i] = delta.p.x;       
           yval[i] = delta.p.y;                                 /*15*/
           printf ("point %d: x = %g, y = %g\n", i+1, xval[i], yval[i]);
         }
        offset->p.x = 0.0;
        move (mnp, seek);
        waitForCompleted (mnp);

        computeCenter (xval[0], yval[0], xval[1], yval[1],      /*16*/
                       xval[2], yval[2], &centerX, &centerY);

        printf ("Cylinder center is at (%g, %g)\n", centerX, centerY);

        multTrsfXyz (start, centerX, centerY, 0.0);             /*17*/

        radius =  sqrt (square(xval[0] - centerX) + square(yval[0] - centerY))
                - PEG_RADIUS;

        printf ("radius = %g\n", radius);

        rcclSetModes (mnp, TRACKING_MODE);                      /*18*/

        setCartVel (mnp, 10.0, F_DEFAULT);

        identTrsf (rotz);
        move (mnp, turn);
        offset->p.x = radius + PEG_RADIUS + 5.0;
        move (mnp, turn);
        offset->p.z = DROP_DEPTH;
        move (mnp, turn);
        comply (mnp, "fx", 2.0);                                /*19*/
        offset->p.x = radius;
        move (mnp, turn);
        waitForCompleted (mnp);

        transMotionEval (rotz, rotzFxn, 0, mnp);                /*20*/
        comply (mnp, "fx", 10.0);                               /*21*/
        setTime (mnp, F_DEFAULT, NUM_TURNS*MSEC_PER_REV);
        move (mnp, turn);

        waitForCompleted (mnp);
        delay (1000.0);                                         /*22*/

        setCartVel (mnp, F_DEFAULT, F_DEFAULT);
        lock (mnp, "fx");
        setTransConst (rotz);
        offset->p.x = radius + PEG_RADIUS + 5.0;
        move (mnp, seek);
        offset->p.z = 0.0;
        move (mnp, seek);
        offset->p.x = 0.0;
        move (mnp, seek);                                       /*23*/
        stop (mnp, 1000.0);

        waitForCompleted (mnp);
        rcclRelease (1);
}
