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

char *key_buf[256];

#define MAXPOINTS       256

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

#ifdef VXWORKS_CFG
#define main playback_
#endif

main()
{
        char *keytree, *tree_match_parse(), *tree_match_err_at();
	char filename[256];
        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                                                        \
	[save load] 							\
	 (%s", filename, ", \"file name\")				\
)",0);
        if (keytree == 0)
         { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
           exit (-1);
         }
	tree_match_catch_sig (0, sigmask(SIGALRM));

        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);
                     }
                  }
               }
            }
	   else if (COMMAND ("save"))
	    { FILE *fp;
	      float buf[NUM_JNTS];
	      int k, j;

	      if ((fp = fopen (filename, "w")) == NULL)
	       { fprintf (stderr, "Can't open file %s\n", filename);
		 continue;
	       }
	      for (k=0; k<numPoints; k++)
	       { for (j=0; j<NUM_JNTS; j++)
		  { buf[j] = points[k].v[j];
		    if (mnp->j6->type[j] == 'R')
		     { buf[j] *= RADTODEG;
		     }
		  }
		 fprintVf (fp, "%9.4f", buf, NUM_JNTS);
		 fprintf  (fp, " %d\n", handSet[k]);
	       }
	      fclose(fp);
	    }
	   else if (COMMAND ("load"))
	    { FILE *fp;
	      float buf[NUM_JNTS];
	      int hand, j, k, status;

	      if ((fp = fopen (filename, "r")) == NULL)
	       { fprintf (stderr, "Can't open file %s\n", filename);
		 continue;
	       }
	      numPoints = 0;
	      k = 0;
	      while (1)
	       { 
		 status = fscanVf (fp, buf, NUM_JNTS);
		 if (status == 1)
		  { status = fscanf (fp, "%d", &hand);
		  }
		 if (status == EOF)
		  { numPoints = k;
		    break;
		  }
		 else if (status != 1)
		  { fprintf (stderr, "Error reading points from file\n");
		    break;
		  }
		 else if (k == MAXPOINTS)
		  { fprintf (stderr, "Too many points: truncating\n");
		    numPoints = k;
		    break;
		  }
		 else
		  { initJntsByManip (&points[k], mnp);
		    for (j=0; j<NUM_JNTS; j++)
		     { if (mnp->j6->type[j] == 'R')
			{ buf[j] *= DEGTORAD;
			}
		       points[k].v[j] = buf[j];
		     }
		    handSet[k] = hand;
		  }
	       }
	      fclose(fp);
	    }
         }
        while (!(COMMAND("quit")));

        rcclRelease(1);
}
