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

char *key_buf[256];

#define MAXPOINTS       256

int numPoints = 0;
JNTS points[MAXPOINTS];                                         /*(2)*/
int handSet[MAXPOINTS];

main()
{
        char *keytree, *tree_match_parse(), *tree_match_err_at();
        float speed = 1.0;
        MANIP *mnp;

        numPoints = 0;
                                                                /*(3)*/
        keytree = tree_match_parse("                                    \
(       quit                                                            \
        set                                                             \
         (speed                                                         \
           (%f", &speed, ",0,10,        \"speed scale (1.0 nominal)\")  \
         )                                                              \
        show                                                            \
        record                                                          \
        playback                                                        \
)",0);
        if (keytree == 0)
         { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
           exit (-1);
         }

        if ((mnp = rcclCreate (getDefaultRobot(), 0)) == NULL)  /*(4)*/
         { printErrors();
           exit (-1);
         }
        if (rcclStart() < 0)
         { printErrors();
           exit (-1);
         }

        do
         { tree_match (keytree, "PLAYBACK> ", key_buf);         /*(5)*/

           if (COMMAND ("set speed"))                           /*(6)*/
            { setSpeed (mnp, speed);
            }
           else if (COMMAND("show"))                            /*(7)*/
            { printf ("speed = %g\n", speed);
              printf ("%d points stored\n", numPoints);
            }
           else if (COMMAND ("record"))                         /*(8)*/
            { int status;

              numPoints = 0;
              printf ("use command \"return\" to stop recording points\n");
              while ((status = rcclTeach(mnp, "RECORDING> ", NULL)) == 1)
               { points[numPoints] = *mnp->j6;                  /*(9)*/
                 handSet[numPoints] = mnp->handPos;
                 if (++numPoints >= MAXPOINTS)
                  { break;
                  }
               }
              if (status < 0)
               { printErrors();                                 /*(10)*/
                 exit(-1);
               }
            }
           else if (COMMAND ("playback"))                       /*(11)*/
            { int i;
              
              for (i=0; i<numPoints; i++)
               { movej (mnp, &points[i]);
                 if (handSet[i] != mnp->handPos)
                  { waitForCompleted (mnp);
                    if (handSet[i])
                     { OPEN_HAND(mnp);
                     }
                    else
                     { CLOSE_HAND(mnp);
                     }
                  }
               }
            }
         }
        while (!(COMMAND("quit")));

        rcclRelease(1);
}

