#include <rccl.h>
#include <ctree.h>                                              /*(1)*/
#include <signal.h>

char key_buf[256];

#ifdef VXWORKS_CFG
#define main jmove_
#endif

main(argc, argv)
int argc; char **argv;
{
        MANIP *mnp;
        JNTS angles;
        char *robotName;
        char *keytree;

        int movejRequest = 0;
        int jnt;
        int mid = UNDEF;
        int newMid = UNDEF;

        float angval;
        float speed;
        char conf[4];

        rcclSetOptions (RCCL_ERROR_EXIT);               
        if (argc != 2)                                          /*(2)*/
         { fprintf (stderr, "USAGE: %s <robotName>\n", argv[0]);
           exit (-1);
         }
        robotName = *++argv;                            
                                                                /*(3)*/
        keytree = tree_match_parse("                                    \
(       quit                                                            \
        show                                                            \
        speed                                                           \
         (%f", &speed, ",0,,            \"speed scale\")                \
        move                                                            \
         (all                                                           \
          %d", &jnt, ",1,6,             \"joint number\"                \
           ([by to]                                                     \
             (%f", &angval, ",,,        \"angle value (degrees)\"))     \
          )                                                             \
        setConf                                                         \
         (%s", conf, ",                 \"configuaration string\")      \
)",0);
        if (keytree == 0)                                       /*(4)*/
         { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
           exit (-1);
         }
	tree_match_catch_sig (0, sigmask(SIGALRM));

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

        angles = *mnp->j6;                                      /*(6)*/

        do
         { tree_match (keytree, "JMOVE> ", key_buf);            /*(7)*/

           if (COMMAND("show"))                                 /*(8)*/
            { float deg[6];

              getActiveConf (mnp, conf);
              printf ("speed: %g\n", getSpeed(mnp));
              printf ("conf: %s\n\n", conf);
              printTrsf ("T6:    %m\n\n", mnp->t6);
              scaleVf (deg, RADTODEG, mnp->j6->v, 6);
              printVf ("J6:    %8.3f\n", deg, 6);
            }
           else if (COMMAND("speed"))                           /*(9)*/
            { setSpeed (mnp, speed);
            }
           else if (COMMAND("move all"))                        /*(10)*/
            { float deg[6];
              printf ("Joint values> ");
              if (scanVf (deg, 6) != 1)
               { printf ("Error reading values\n");
                 continue;
               }
              scaleVf (angles.v, DEGTORAD, deg, 6);
              movejRequest = 1;                                 /*(11)*/
            }
           else if (COMMAND("move by"))                         /*(12)*/
            { angles.v[jnt-1] = DEGTORAD*angval + mnp->j6->v[jnt-1];
              movejRequest = 1;
            }
           else if (COMMAND("move to"))                         /*(13)*/
            { angles.v[jnt-1] = DEGTORAD*angval;
              movejRequest = 1;
            }
           else if (COMMAND("setConf"))                         /*(14)*/
            { stopCurrentMotion (mnp, 0);
              waitForCompleted (mnp);                           /*(15)*/

              setConf (mnp, conf);                              /*(16)*/
              move (mnp, mnp->last);                            /*(17)*/
              waitForCompleted (mnp);
              angles = *(mnp->j6);                              /*(18)*/
            }
           if (movejRequest)                                    /*(19)*/
            { 
              if ((newMid = movej (mnp, &angles)) < 0)
               { printErrors();                                 /*(20)*/
                 clearErrors();
               }
              else
               { if (mid != UNDEF && motionStatus(mid) != M_FINISHED)
                  { stopMotion(mid, 0);                         /*(21)*/
                  }
                 mid = newMid;
               }
              movejRequest = 0;
            }         
         }
        while (!(COMMAND("quit")));                             /*(22)*/

        stopCurrentMotion (mnp, 0);
        waitForCompleted (mnp);
        rcclRelease (YES);
}
