#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <forms.h>
#include "mainform.h"
#include "gaitgen.h"

/* function prototypes */
void Load(FL_OBJECT *, long);
void Save(FL_OBJECT *, long);
void Animate(FL_OBJECT *, long);
void AngleUpdate(FL_OBJECT *, long);
void About(FL_OBJECT *, long);
void HideAbout(int, void *);
void TimeDelta(FL_OBJECT *, long);
void Init(Window);
void DrawLinks(void);
void ChangeFrame(FL_OBJECT *, long);
void UpdateForm(void);
short AConvert(LINK, int);

/* variables and pointers */
FD_main *mainform;
FD_about *aboutform;
static GC gcBox;
Display *disp;
int displayScreen;
ViewInfo views;
XSegment sideLinks[SIDE_LINKS];
XSegment frontLinks[FRONT_LINKS];
XSegment topLinks[TOP_LINKS];
XArc heads[3];
int angle[NUM_LINKS];
int acc[NUM_ACC];
int time;
char buff[100];
const char *load;
const char *save;
const char *aboutInfo = 
"\n@c@m@bGait Generator v0.1\n
@c@iElliot Nicholls <nicho-ej@ee.uwa.edu.au>\n
@cThe University of Western Australia\n\n
@cThis program is used to generate gaits for the\n
@cbipedal walking robot. For more information see\n
@cthe documentation.";
ANGLES *angles;
ANGLES *currAngle;
int *times;
int *currTime;
ACCDATA *accData;
ACCDATA *currAccData;
int numFrames;
int offset[9] = {128, 128, 128, 128, 128, 128, 128, 128, 128};
int sFact[9] = {-508, -133, -133, 133, -423, -133, 133, 133, -508};

/* main program: create forms, call initialise variables/pointers and
   pass control to the event driving libforms (xforms) engine
*/
void main(int argc, char *argv[]) {

  disp = fl_initialize(&argc, argv, "Gait Generator", 0, 0);
  mainform = create_form_main();
  aboutform = create_form_about();
  fl_show_form(mainform->main,FL_PLACE_MOUSE,FL_FULLBORDER,"Main view");
  displayScreen = DefaultScreen(disp);
  Init(FL_ObjWin(mainform->sideView));
  DrawLinks();
  fl_do_forms();

}

/* init function: initialise variables/pointers, xforms houskeeping */
void Init(Window win) {

  int i;
  
  /* set-up window information */
  views.side.x = mainform->sideView->x + 2;
  views.side.y = mainform->sideView->y + 2;
  views.side.w = mainform->sideView->w - 4;
  views.side.h = mainform->sideView->h - 4;
  views.front.x = mainform->frontView->x + 2;
  views.front.y = mainform->frontView->y + 2;
  views.front.w = mainform->frontView->w - 4;
  views.front.h = mainform->frontView->h - 4;
  views.top.x = mainform->topView->x + 2;
  views.top.y = mainform->topView->y + 2;
  views.top.w = mainform->topView->w - 4;
  views.top.h = mainform->topView->h - 4;

  /* create a GC for drawing in the windows */
  gcBox = XCreateGC(disp, win, 0, 0);
  XMapWindow(disp, win);
  XFlush(disp);

  /* allocate memory for first frame and set-up frame pointers */
  times = (int *)malloc(sizeof(int));
  angles = (ANGLES *)malloc(sizeof(ANGLES));
  accData = (ACCDATA *)malloc(sizeof(ACCDATA));
  fprintf(stderr,"sizeof(ANGLES)=%d\n", sizeof(ANGLES));
  fprintf(stderr, "memory alloc: angles=%d, times=%d\n", sizeof(*angles), sizeof(*times));
  currAngle = angles;
  currTime = times;
  currAccData = accData;
  numFrames = 1;
  *currTime = DEFAULT_TIME_DELTA;
  time = DEFAULT_TIME_DELTA;
  sprintf(buff, "%d", *currTime);
  fprintf(stderr, "time=%d\n", *currTime);
  fl_set_input(mainform->time, buff);
  fl_set_counter_value(mainform->framePos, 1);

  /* set-up counter properly (fdesign bug) */
  fl_set_counter_step(mainform->framePos, 1, 10);
  fl_set_counter_return(mainform->framePos, FL_RETURN_CHANGED);
  fl_set_counter_bounds(mainform->framePos, 1, 2);
  mainform->NumFrames->label = "1";
  fl_redraw_object(mainform->NumFrames);
  
  /* set-up slider bounds properly */
  fl_set_slider_bounds(mainform->rHipT, (int)((255 - 128)*100/sFact[rHipT]), (int)((0 - 128)*100/sFact[rHipT]));
  fl_set_slider_bounds(mainform->rHipB, (int)((255 - 128)*100/sFact[rHipB]), (int)((0 - 128)*100/sFact[rHipB]));
  fl_set_slider_bounds(mainform->rKnee, (int)((255 - 128)*100/sFact[rKnee]), (int)((0 - 128)*100/sFact[rKnee]));
  fl_set_slider_bounds(mainform->rAnkle, (int)((0 - 128)*100/sFact[rAnkle]), (int)((255 - 128)*100/sFact[rAnkle]));
  fl_set_slider_bounds(mainform->torso, (int)((255 - 128)*100/sFact[torso]), (int)((0 - 128)*100/sFact[torso]));
  fl_set_slider_bounds(mainform->lAnkle, (int)((255 - 128)*100/sFact[lAnkle]), (int)((0 - 128)*100/sFact[lAnkle]));
  fl_set_slider_bounds(mainform->lKnee, (int)((0 - 128)*100/sFact[lKnee]), (int)((255 - 128)*100/sFact[lKnee]));
  fl_set_slider_bounds(mainform->lHipB, (int)((0 - 128)*100/sFact[lHipB]), (int)((255 - 128)*100/sFact[lHipB]));
  fl_set_slider_bounds(mainform->lHipT, (int)((255 - 128)*100/sFact[lHipT]), (int)((0 - 128)*100/sFact[lHipT]));
  
  
  /* set-up about box */
  fl_add_browser_line(aboutform->AboutText, aboutInfo);


}

/* calculate and draw the stick figure views */
void DrawLinks(void) {

  /* clear the windows by redrawing the links in the background colour */
  XSetForeground(disp, gcBox, fl_get_pixel(FL_LEFT_BCOL));
  /*XFillRectangle(disp, FL_ObjWin(mainform->sideView), gcBox, views.side.x, views.side.y, views.side.w, views.side.h);
  XFillRectangle(disp, FL_ObjWin(mainform->frontView), gcBox, views.front.x, views.front.y, views.front.w, views.front.h);
  XFillRectangle(disp, FL_ObjWin(mainform->topView), gcBox, views.top.x, views.top.y, views.top.w, views.top.h);*/
  /* draw side links and head*/
  XDrawSegments(disp, FL_ObjWin(mainform->sideView), gcBox, sideLinks, 7);
  XDrawArcs(disp, FL_ObjWin(mainform->sideView), gcBox, heads, 3);
  /* draw front links */
  XDrawSegments(disp, FL_ObjWin(mainform->frontView), gcBox, frontLinks, 8);
  /* draw top links */
  XDrawSegments(disp, FL_ObjWin(mainform->topView), gcBox, topLinks, 4);
  XSetForeground(disp, gcBox, fl_get_pixel(FL_BLUE));
  XFlush(disp);
  
  /* set up the side view links */
  /* torso link */
  sideLinks[storso].x1 = views.side.x + views.side.w / 2;
  sideLinks[storso].y1 = views.side.y + views.side.h / 2;
  sideLinks[storso].x2 = sideLinks[storso].x1 + (TORSO_LENGTH*cos(angle[torso]*M_PI/180))*sin(angle[rotSide]*M_PI/180);
  sideLinks[storso].y2 = sideLinks[storso].y1 - (TORSO_LENGTH*cos(angle[torso]*M_PI/180))*cos(angle[rotSide]*M_PI/180);
  /* rHipB link */
  sideLinks[srHipB].x1 = sideLinks[storso].x1;
  sideLinks[srHipB].y1 = sideLinks[storso].y1;
  sideLinks[srHipB].x2 = sideLinks[srHipB].x1 + LEG_LINK_LENGTH*sin((angle[rHipB]-angle[rotSide])*M_PI/180);
  sideLinks[srHipB].y2 = sideLinks[srHipB].y1 + LEG_LINK_LENGTH*cos((angle[rHipB]-angle[rotSide])*M_PI/180);
  /* rKnee link */
  sideLinks[srKnee].x1 = sideLinks[srHipB].x2;
  sideLinks[srKnee].y1 = sideLinks[srHipB].y2;
  sideLinks[srKnee].x2 = sideLinks[srKnee].x1 + LEG_LINK_LENGTH*sin((angle[rHipB]+angle[rKnee]-angle[rotSide])*M_PI/180);
  sideLinks[srKnee].y2 = sideLinks[srKnee].y1 + LEG_LINK_LENGTH*cos((angle[rHipB]+angle[rKnee]-angle[rotSide])*M_PI/180);
  /* rAnkle link */
  sideLinks[srAnkle].x1 = sideLinks[srKnee].x2;
  sideLinks[srAnkle].y1 = sideLinks[srKnee].y2;
  sideLinks[srAnkle].x2 = sideLinks[srAnkle].x1 + FOOT_LENGTH*sin((angle[rHipB]+angle[rKnee]+angle[rAnkle]-angle[rotSide]+90)*M_PI/180);
  sideLinks[srAnkle].y2 = sideLinks[srAnkle].y1 + FOOT_LENGTH*cos((angle[rHipB]+angle[rKnee]+angle[rAnkle]-angle[rotSide]+90)*M_PI/180);
  /* lHipB link */
  sideLinks[slHipB].x1 = sideLinks[storso].x1;
  sideLinks[slHipB].y1 = sideLinks[storso].y1;
  sideLinks[slHipB].x2 = sideLinks[slHipB].x1 + LEG_LINK_LENGTH*sin((angle[lHipB]-angle[rotSide])*M_PI/180);
  sideLinks[slHipB].y2 = sideLinks[slHipB].y1 + LEG_LINK_LENGTH*cos((angle[lHipB]-angle[rotSide])*M_PI/180);
  /* lKnee link */
  sideLinks[slKnee].x1 = sideLinks[slHipB].x2;
  sideLinks[slKnee].y1 = sideLinks[slHipB].y2;
  sideLinks[slKnee].x2 = sideLinks[slKnee].x1 + LEG_LINK_LENGTH*sin((angle[lHipB]+angle[lKnee]-angle[rotSide])*M_PI/180);
  sideLinks[slKnee].y2 = sideLinks[slKnee].y1 + LEG_LINK_LENGTH*cos((angle[lHipB]+angle[lKnee]-angle[rotSide])*M_PI/180);
  /* lAnkle link */
  sideLinks[slAnkle].x1 = sideLinks[slKnee].x2;
  sideLinks[slAnkle].y1 = sideLinks[slKnee].y2;
  sideLinks[slAnkle].x2 = sideLinks[slAnkle].x1 + 10*sin((angle[lHipB]+angle[lKnee]+angle[lAnkle]-angle[rotSide]+90)*M_PI/180);
  sideLinks[slAnkle].y2 = sideLinks[slAnkle].y1 + 10*cos((angle[lHipB]+angle[lKnee]+angle[lAnkle]-angle[rotSide]+90)*M_PI/180);
  /* head */
  heads[0].x = sideLinks[storso].x1 + ((TORSO_LENGTH+HEAD_RADIUS)*cos(angle[torso]*M_PI/180))*sin(angle[rotSide]*M_PI/180) - HEAD_RADIUS;
  heads[0].y = sideLinks[storso].y1 - (TORSO_LENGTH+HEAD_RADIUS)*cos(angle[torso]*M_PI/180)*cos(angle[rotSide]*M_PI/180) - HEAD_RADIUS;
  heads[0].width = HEAD_RADIUS*2;
  heads[0].height = HEAD_RADIUS*2;
  heads[0].angle1 = 0;
  heads[0].angle2 = 360*64;

  
  /* set up the front view links */
  /* torso link */
  frontLinks[ftorso].x1 = views.front.x + views.front.w / 2;
  frontLinks[ftorso].y1 = views.front.y + views.front.h / 2;
  frontLinks[ftorso].x2 = frontLinks[ftorso].x1 + TORSO_LENGTH*sin(angle[torso]*M_PI/180);
  frontLinks[ftorso].y2 = frontLinks[ftorso].y1 - TORSO_LENGTH*cos(angle[torso]*M_PI/180);
  /* pelvic link */
  frontLinks[fPelv].x1 = frontLinks[ftorso].x1 - HIP_WIDTH/2;
  frontLinks[fPelv].y1 = frontLinks[ftorso].y1;
  frontLinks[fPelv].x2 = frontLinks[fPelv].x1 + HIP_WIDTH;
  frontLinks[fPelv].y2 = frontLinks[fPelv].y1;
  /* rHipB link */
  frontLinks[frHipB].x1 = frontLinks[fPelv].x1;
  frontLinks[frHipB].y1 = frontLinks[fPelv].y1;
  frontLinks[frHipB].x2 = frontLinks[frHipB].x1;
  frontLinks[frHipB].y2 = frontLinks[frHipB].y1 + LEG_LINK_LENGTH*cos(angle[rHipB]*M_PI/180);
  /* rKnee link */
  frontLinks[frKnee].x1 = frontLinks[frHipB].x2;
  frontLinks[frKnee].y1 = frontLinks[frHipB].y2;
  frontLinks[frKnee].x2 = frontLinks[frKnee].x1;
  frontLinks[frKnee].y2 = frontLinks[frKnee].y1 + LEG_LINK_LENGTH*cos((angle[rHipB]+angle[rKnee])*M_PI/180);
  /* rAnkle link */
  frontLinks[frAnkle].x1 = frontLinks[frKnee].x2 - FOOT_WIDTH/2;
  frontLinks[frAnkle].y1 = frontLinks[frKnee].y2;
  frontLinks[frAnkle].x2 = frontLinks[frAnkle].x1 + FOOT_WIDTH;
  frontLinks[frAnkle].y2 = frontLinks[frAnkle].y1;
  /* lHipB link */
  frontLinks[flHipB].x1 = frontLinks[fPelv].x2;
  frontLinks[flHipB].y1 = frontLinks[fPelv].y2;
  frontLinks[flHipB].x2 = frontLinks[flHipB].x1;
  frontLinks[flHipB].y2 = frontLinks[flHipB].y1 + LEG_LINK_LENGTH*cos(angle[lHipB]*M_PI/180);
  /* lKnee link */
  frontLinks[flKnee].x1 = frontLinks[flHipB].x2;
  frontLinks[flKnee].y1 = frontLinks[flHipB].y2;
  frontLinks[flKnee].x2 = frontLinks[flKnee].x1;
  frontLinks[flKnee].y2 = frontLinks[flKnee].y1 + LEG_LINK_LENGTH*cos((angle[lHipB]+angle[lKnee])*M_PI/180);
  /* rAnkle link */
  frontLinks[flAnkle].x1 = frontLinks[flKnee].x2 - FOOT_WIDTH/2;
  frontLinks[flAnkle].y1 = frontLinks[flKnee].y2;
  frontLinks[flAnkle].x2 = frontLinks[flAnkle].x1 + FOOT_WIDTH;
  frontLinks[flAnkle].y2 = frontLinks[flAnkle].y1;
  /* head */
  heads[1].x = frontLinks[ftorso].x1 + (TORSO_LENGTH+HEAD_RADIUS)*sin(angle[torso]*M_PI/180) - HEAD_RADIUS;
  heads[1].y = frontLinks[ftorso].y1 - (TORSO_LENGTH+HEAD_RADIUS)*cos(angle[torso]*M_PI/180) - HEAD_RADIUS;
  heads[1].width = HEAD_RADIUS*2;
  heads[1].height = HEAD_RADIUS*2;
  heads[1].angle1 = 0;
  heads[1].angle2 = 360*64;

  /* set-up top view links */
  /* head */
  heads[2].x = views.top.x + views.top.w/2 - HEAD_RADIUS + (TORSO_LENGTH+HEAD_RADIUS)*sin(angle[torso]*M_PI/180);
  heads[2].y = views.top.y + views.top.h/2 - HEAD_RADIUS;
  heads[2].width = HEAD_RADIUS*2;
  heads[2].height = HEAD_RADIUS*2;
  heads[2].angle1 = 0;
  heads[2].angle2 = 360*64;
  /* rPelv */
  topLinks[trPelv].x1 = views.top.x + views.top.w/2;
  topLinks[trPelv].y1 = views.top.y + views.top.h/2;
  topLinks[trPelv].x2 = topLinks[trPelv].x1 - HEAD_RADIUS*2;
  topLinks[trPelv].y2 = topLinks[trPelv].y1;
  /* rAnkle */
  topLinks[trHipT].x1 = topLinks[trPelv].x2;
  topLinks[trHipT].y1 = topLinks[trPelv].y2;
  topLinks[trHipT].x2 = topLinks[trHipT].x1 + FOOT_LENGTH*sin(angle[rHipT]*M_PI/180);
  topLinks[trHipT].y2 = topLinks[trHipT].y1 + FOOT_LENGTH*cos(angle[rHipT]*M_PI/180);
  /* lPelv */
  topLinks[tlPelv].x1 = views.top.x + views.top.w/2;
  topLinks[tlPelv].y1 = views.top.y + views.top.h/2;
  topLinks[tlPelv].x2 = topLinks[tlPelv].x1 + HEAD_RADIUS*2;
  topLinks[tlPelv].y2 = topLinks[tlPelv].y1;
  /* lAnkle */
  topLinks[tlHipT].x1 = topLinks[tlPelv].x2;
  topLinks[tlHipT].y1 = topLinks[tlPelv].y2;
  topLinks[tlHipT].x2 = topLinks[tlHipT].x1 + FOOT_LENGTH*sin(angle[lHipT]*M_PI/180);
  topLinks[tlHipT].y2 = topLinks[tlHipT].y1 + FOOT_LENGTH*cos(angle[lHipT]*M_PI/180);

  /* draw side links and head*/
  XDrawSegments(disp, FL_ObjWin(mainform->sideView), gcBox, sideLinks, 7);
  XDrawArcs(disp, FL_ObjWin(mainform->sideView), gcBox, heads, 3);
  
  /* draw front links */
  XDrawSegments(disp, FL_ObjWin(mainform->frontView), gcBox, frontLinks, 8);
  
  /* draw top links */
  XDrawSegments(disp, FL_ObjWin(mainform->topView), gcBox, topLinks, 4);

  XFlush(disp);
  
}

/* load a file */
void Load(FL_OBJECT *x, long num) {

  FILE *loadLnk;
  int i, tmp;
  ANGLES readAngle;
  int readTime;
  ACCDATA readAcc;

  load = fl_show_fselector("Load file...","","","");
  fprintf(stderr, "load file: %s\n", load);
  numFrames = 0;
  free(angles); angles = NULL;
  free(times); times = NULL;
  free(accData); accData = NULL;
  loadLnk = fopen(load, "r");
  do {
    if((tmp = fscanf(loadLnk, "#%*[^\n]\n")) == EOF) {
      fprintf(stderr, "Unexpected EOF\n");
    } else if(tmp == 0) {
      if((tmp = fscanf(loadLnk, "%d", &readTime)) == EOF) {
	fprintf(stderr, "Unexpected EOF\n");
      }	else if(tmp == 0) {
	fprintf(stderr, "File format incorrect - no times or angles found.\n");
	tmp = EOF;
      } else {
	for(i=0; i<NUM_LINKS; i++) {
	  if((tmp = (fscanf(loadLnk, "%d", &readAngle[i]))) == EOF) {
	    fprintf(stderr, "Unexpected EOF\n");
	    break;
	  } else if(tmp == 0) {
	    fprintf(stderr, "File format incorrect - not enough angles.\n");
	    tmp = EOF;
	    break;
	  }
	}
	for(i=0; i<NUM_ACC; i++) {
	  if((tmp = (fscanf(loadLnk, "%d", &readAcc[i]))) == EOF) {
	    fprintf(stderr, "Unexpected EOF\n");
	    break;
	  } else if(tmp == 0) {
	    fprintf(stderr, "File format incorrect - not enough angles.\n");
	    tmp = EOF;
	    break;
	  }
	}
	if(tmp != EOF) {
	  ++numFrames;
	  angles = (ANGLES *)realloc(angles, numFrames*sizeof(ANGLES));
	  currAngle = angles + numFrames - 1;
	  times = (int *)realloc(times, numFrames*sizeof(int));
	  currTime = times + numFrames - 1;
	  accData = (ACCDATA *)realloc(accData, numFrames*sizeof(ACCDATA));
	  currAccData = accData + numFrames - 1;
	  for(i=0; i<NUM_LINKS; i++) (*currAngle)[i] = readAngle[i];
	  for(i=0; i<NUM_ACC; i++) (*currAccData)[i] = readAcc[i];
	  *currTime = readTime;
	}
      }
    }
  } while(tmp != EOF);
  if(angles == NULL) {
    Init(FL_ObjWin(mainform->sideView));
  } else {
    currAccData = accData;
    currTime = times;
    currAngle = angles;
    fl_set_counter_value(mainform->framePos, 1);
    for(i=0; i<NUM_LINKS; i++) angle[i] = (*currAngle)[i];
    for(i=0; i<NUM_ACC; i++) acc[i] = (*currAccData)[i];
    time = *currTime;
    ChangeFrame(mainform->framePos, 0);
  }
}

/* save a file, emit .[cgh] files */
void Save(FL_OBJECT *x, long num) {

  FILE *saveLnk;
  int i, j, tmp;
  char *saveTmp;
  char saveHead[100], tmpBuff[100];
  char **searchBase, *basePos;

  save = fl_show_fselector("Save file...","","","");
  fprintf(stderr, "save file: %s\n", save);
  saveTmp = (char *)malloc((5 + strlen(save))*sizeof(char));

  /* write the gaitgenerator .gg link angle file */
  sprintf(saveTmp, "%s.gg", save);
  if((saveLnk = fopen(saveTmp, "w")) == NULL) {
    fprintf(stderr, "Error - cannot open file %s\n", saveTmp);
  } else {
    fprintf(saveLnk, "# gait generator link file - this file was automatically generated\n");
    currAngle = angles;
    currTime = times;
    currAccData = accData;
    for(j=0; j<numFrames; j++) {
      fprintf(saveLnk, "%d", *(times + j));
      for(i=0; i<NUM_LINKS; i++) {
	fprintf(saveLnk, " %d", (*(currAngle + j))[i]);
      }
      for(i=0; i<NUM_ACC; i++) {
	fprintf(saveLnk, " %d", (*(currAccData + j))[i]);
      }
      fprintf(saveLnk, "\n");
    } 
    fclose(saveLnk);
  }

  /* write the .c link angle file for compiling */
  sprintf(saveTmp, "%s.c", save);
  basePos = strrchr(save, '/');
  sprintf(saveHead, "%s", basePos+1);
  fprintf(stderr, "saveHead=%s\n", saveHead);
  if((saveLnk = fopen(saveTmp, "w")) == NULL) {
    fprintf(stderr, "Error - cannot open file %s\n", saveTmp);
  } else {
    fprintf(saveLnk, "/* gait generator link file - this file was automatically generated */\n");
    fprintf(saveLnk, "\n#include \"gaitgen.h\"\n");
    fprintf(saveLnk, "#include \"%s.h\"\n", saveHead);
    fprintf(saveLnk, "\nANGLES states[NUM_FRAMES] = {\n"); 
    currAngle = angles;
    currTime = times;
    currAccData = accData;
    for(j=0; j<numFrames; j++) {
      if(j) fprintf(saveLnk, ",\n");
      fprintf(saveLnk, "\t{");
      for(i=0; i<lHipT+1; i++) {
	if(i) fprintf(saveLnk, ", ");
	fprintf(saveLnk, "%d", AConvert(i, (*(currAngle + j))[i]));
      }
      fprintf(saveLnk, "}");
    } 
    fprintf(saveLnk, "\n\t};\n\n");
    fprintf(saveLnk, "int times[NUM_FRAMES] = {\n");
    for(j=0; j<numFrames; j++) {
      if(j) fprintf(saveLnk, ",\n");
      fprintf(saveLnk, "\t%d", *(times + j));
    }
    fprintf(saveLnk, "\n\t};\n");
    fprintf(saveLnk, "\nACCDATA acc[NUM_FRAMES] = {\n"); 
    for(j=0; j<numFrames; j++) {
      if(j) fprintf(saveLnk, ",\n");
      fprintf(saveLnk, "\t{");
      for(i=0; i<NUM_ACC; i++) {
	if(i) fprintf(saveLnk, ", ");
	fprintf(saveLnk, "%d", (*(currAccData + j))[i] + 512);
      }
      fprintf(saveLnk, "}");
    } 
    fprintf(saveLnk, "\n\t};\n\n");
    fclose(saveLnk);
  }

  /* write the .h link angle file for compiling */
  sprintf(saveTmp, "%s.h", save);
  if((saveLnk = fopen(saveTmp, "w")) == NULL) {
    fprintf(stderr, "Error - cannot open file %s\n", saveTmp);
  } else {
    fprintf(saveLnk, "/* gait generator link file - this file was automatically generated */\n");
    fprintf(saveLnk, "\n#define NUM_FRAMES %d\n", numFrames);
    fprintf(saveLnk, "\nextern ANGLES states[NUM_FRAMES];\n");
    fprintf(saveLnk, "\nextern int times[NUM_FRAMES];\n");
    fprintf(saveLnk, "\nextern ACCDATA acc[NUM_FRAMES];\n");
    fclose(saveLnk);
  }
}

/* run through frame animation */
void Animate(FL_OBJECT *x, long num) {

  ANGLES new;
  FANGLES step, fangle;
  int frame, stepLength, numSteps, i, j, k;

  stepLength = 1000/FPS;

  /* start from current frame */
  ChangeFrame(mainform->framePos, 0);
  frame = (int)fl_get_counter_value(mainform->framePos);
  fprintf(stderr, "initial frame=%d\n", frame);

  for(k=frame; k<numFrames; k++) {
    fprintf(stderr, "frame=%d\n", k);
    for(i=0; i<NUM_LINKS; i++) angle[i] = (*currAngle)[i];
    
    numSteps = *(currTime + 1) * FPS/1000;
    fprintf(stderr, "numsteps=%d\n", numSteps);
    for(i=0; i<NUM_LINKS; i++) new[i] = (*(currAngle + 1))[i];
    for(i=0; i<NUM_LINKS; i++) step[i] = (float)(new[i] - angle[i]) / numSteps;
    for(i=0; i<NUM_LINKS; i++) fangle[i] = (float)angle[i];
    fprintf(stderr, "fangle=%f step=%f new=%d\n", fangle[1], step[1], new[1]);
    
    for(j=0; j<numSteps; j++) {
      for(i=0; i<NUM_LINKS; i++) fangle[i] += step[i];
      for(i=0; i<NUM_LINKS; i++) angle[i] = (int)fangle[i];
      usleep(stepLength*1000);
      UpdateForm();
      DrawLinks();
    }
    currAngle++;
    currTime++;
    time = *currTime;
    fl_set_counter_value(mainform->framePos, ++frame);
  }
  for(i=0; i<NUM_LINKS; i++) angle[i] = (*currAngle)[i];
  ChangeFrame(mainform->framePos, 0);
  
}

/* respond to change in angles and redraw the stick figure views */
void AngleUpdate(FL_OBJECT *x, long num) {

  angle[torso] = fl_get_slider_value(mainform->torso);
  angle[rHipT] = fl_get_slider_value(mainform->rHipT);
  angle[rHipB] = fl_get_slider_value(mainform->rHipB);
  angle[rKnee] = fl_get_slider_value(mainform->rKnee);
  angle[rAnkle] = fl_get_slider_value(mainform->rAnkle);
  angle[lHipT] = fl_get_slider_value(mainform->lHipT);
  angle[lHipB] = fl_get_slider_value(mainform->lHipB);
  angle[lKnee] = fl_get_slider_value(mainform->lKnee);
  angle[lAnkle] = fl_get_slider_value(mainform->lAnkle);
  angle[rArm] = fl_get_slider_value(mainform->rArm);
  angle[lArm] = fl_get_slider_value(mainform->lArm);
  angle[rotSide] = fl_get_slider_value(mainform->rotSide);

  acc[sideAcc] = fl_get_slider_value(mainform->sideAcc);
  acc[frontAcc] = fl_get_slider_value(mainform->frontAcc);

  DrawLinks();

}

/* show the about box for 5 seconds */
void About(FL_OBJECT *x, long num) {

  fl_add_timeout(5000, HideAbout, 0);
  fl_show_form(aboutform->about,FL_PLACE_MOUSE,FL_NOBORDER,"About gait generator");
  
}

/* remove the about box */
void HideAbout(int id, void *data) {

  fl_hide_form(aboutform->about);

}

/* respond to the user changing the time */
void TimeDelta(FL_OBJECT *x, long num) {

  strcpy(buff, fl_get_input(x));
  fprintf(stderr, "time string entered: %s\n", buff);
  if(sscanf(buff, "%d", &time)) {
    sprintf(buff, "%d", time);
    fprintf(stderr, "time=%d\n", time);
    fl_set_input(x, buff);
  } else {
    fprintf(stderr, "invalid time string entered: %s\n", buff);
    fl_show_alert("Error","Invalid time entered. Time is in milliseconds and must be an integer.","Please re-enter this number.",0);
  }

}

/* respond to the user changing the current frame */
void ChangeFrame(FL_OBJECT *x, long num) {

  int value, i;

  /* read user input value */
  value = (int)fl_get_counter_value(x);
  
  /* put current angle and time values into angles and times */
  for(i=0; i<NUM_LINKS; i++) {
    (*currAngle)[i] = angle[i];
  }
  for(i=0; i<NUM_ACC; i++) {
    (*currAccData)[i] = acc[i];
  }
  *currTime = time;

  /* update pointers and other junk */
  if(value > numFrames) {
    /* add new frame */
    fl_set_counter_value(x, ++numFrames);
    fprintf(stderr, "numFrames=%d\n", numFrames);
    fl_set_counter_bounds(x, 1, numFrames + 1);
    sprintf(buff, "%d", numFrames);
    mainform->NumFrames->label = buff;
    fl_redraw_object(mainform->NumFrames);
    angles = (ANGLES *)realloc(angles, numFrames*sizeof(ANGLES));
    times = (int *)realloc(times, numFrames*sizeof(int));
    accData = (ACCDATA *)realloc(accData, numFrames*sizeof(ACCDATA));
    currAngle = angles + (numFrames - 1);
    currTime = times + (numFrames - 1);
    currAccData = accData + (numFrames - 1);
    for(i=0; i<NUM_LINKS; i++) {
      (*currAngle)[i] = (*(currAngle - 1))[i];
    }
    for(i=0; i<NUM_ACC; i++) {
      (*currAccData)[i] = (*(currAccData - 1))[i];
    }
    *currTime = *(currTime - 1);
  } else if(value == numFrames) {
    fl_set_counter_bounds(x, 1, numFrames + 1);
    currAngle = angles + (value - 1);
    currTime = times + (value - 1);
    currAccData = accData + (value - 1);
  } else {
    /* old frame */
    fl_set_counter_bounds(x, 1, numFrames);
    currAngle = angles + (value - 1);
    currTime = times + (value - 1);
    currAccData = accData + (value - 1);
  }
  
  /* get current angle and time values from angles and times and redraw */
  for(i=0; i<NUM_LINKS; i++) {
    angle[i] = (*currAngle)[i];
  }
  for(i=0; i<NUM_ACC; i++) {
    acc[i] = (*currAccData)[i];
  }
  time = *currTime;

  /*  for(i=0; i<NUM_LINKS; i++) fprintf(stderr, "read (*currAngle)[%d]=%d\n", i, (*currAngle)[i]);*/
  UpdateForm();
  DrawLinks();

}

void UpdateForm(void) {

  fl_set_slider_value(mainform->torso, angle[torso]);
  fl_set_slider_value(mainform->rHipT, angle[rHipT]);
  fl_set_slider_value(mainform->rHipB, angle[rHipB]);
  fl_set_slider_value(mainform->rKnee, angle[rKnee]); 
  fl_set_slider_value(mainform->rAnkle, angle[rAnkle]);
  fl_set_slider_value(mainform->lHipT, angle[lHipT]);
  fl_set_slider_value(mainform->lHipB, angle[lHipB]);
  fl_set_slider_value(mainform->lKnee, angle[lKnee]);
  fl_set_slider_value(mainform->lAnkle, angle[lAnkle]);
  fl_set_slider_value(mainform->rArm, angle[rArm]);
  fl_set_slider_value(mainform->lArm, angle[lArm]);
  fl_set_slider_value(mainform->rotSide, angle[rotSide]);

  fl_set_slider_value(mainform->sideAcc, acc[sideAcc]);
  fl_set_slider_value(mainform->frontAcc, acc[frontAcc]);

  sprintf(buff, "%d", time);
  fl_set_input(mainform->time, buff);
  sprintf(buff, "%d", numFrames);
  mainform->NumFrames->label = buff;
  fl_redraw_object(mainform->NumFrames);

}

short AConvert(LINK name, int angle) {

  return (angle * sFact[name])/100 + offset[name];

}