#include <stdio.h>
#include "robotdef.h"
#include "legdefs.h"
#include "cfout.h"
/*
typedef struct walker_s {
   int        legs;
   int        legdof;
   point3d_t  acceleration;
   point3d_t  velocity;
   point3d_t  position ;
   point3d_t  orientation;  
   point3d_t *footPosition;   
   double    *jointAngle;
 
} walker_t;
*/

FILE * OpenFile ( char * FileName) {

 FILE * output;
 int i, j, k;
 
 char * jointNames[] = {"ely","shy","shz"}; 
 char * sideNames[]  = {"r","l"};
 char * axisNames[] =  {"x", "y", "z"};
 
 if ((output = fopen(FileName, "wt")) == NULL) {  //open text file 'param 1' w/ err chk
    //printf("Unable to open %s for input.\n", argv[1]);
    return NULL;
 }
  
 fprintf(output, "K \n0 \n50 \n-1 \n"); // write header
 fprintf(output, "vlegs\n");
 fprintf(output, "vlegdof\n");
 fprintf(output, "vaccelerationx\n");
 fprintf(output, "vaccelerationy\n");
 fprintf(output, "vaccelerationz\n");
 fprintf(output, "vvelocityx\n");
 fprintf(output, "vvelocityy\n");
 fprintf(output, "vvelocityz\n"); 
 fprintf(output, "vpositionx\n");
 fprintf(output, "vpositiony\n");
 fprintf(output, "vpositionz\n"); 
 fprintf(output, "vorientationx\n");
 fprintf(output, "vorientationy\n");
 fprintf(output, "vorientationz\n");  
 
 for(i=0; i < Sides; i++) {
   for(j=0; j < Legs/Sides; j++) {
      fprintf(output, "v%s%dfootx\n", sideNames[i], j);
      fprintf(output, "v%s%dfooty\n", sideNames[i], j);
      fprintf(output, "v%s%dfootz\n", sideNames[i], j);
   }
 }   
   
 for(i=0; i < Sides; i++) {
   for(j=0; j < Legs/Sides; j++) {
     for (k=0; k < DOF; k++) {
       fprintf(output, "v%s%d%s%s\n", sideNames[i], j, jointNames[k]);
     }
   }
 }
  
 return output;

}


void AddEntry(  FILE * output, walker_t *this) {

  int i, j, k;

  fprintf(output, "%0.2f, ", (float)this->legs );
  fprintf(output, "%0.2f, ", (float)this->legdof );
  fprintf(output, "%0.2f, ", (float)this->acceleration.x );
  fprintf(output, "%0.2f, ", (float)this->acceleration.y );
  fprintf(output, "%0.2f, ", (float)this->acceleration.z );
  fprintf(output, "%0.2f, ", (float)this->velocity.x );
  fprintf(output, "%0.2f, ", (float)this->velocity.y );
  fprintf(output, "%0.2f, ", (float)this->velocity.z );
  fprintf(output, "%0.2f, ", (float)this->position.x );
  fprintf(output, "%0.2f, ", (float)this->position.y );
  fprintf(output, "%0.2f, ", (float)this->position.z );
  fprintf(output, "%0.2f, ", (float)this->orientation.x );
  fprintf(output, "%0.2f, ", (float)this->orientation.y );
  fprintf(output, "%0.2f, ", (float)this->orientation.z );
  
  for(i=0; i < Sides; i++) {
   for(j=0; j < Legs/Sides; j++) {
      fprintf(output, "%0.2f, ", (float)this->footPosition[footIndex(this, i, j)].x);
      fprintf(output, "%0.2f, ", (float)this->footPosition[footIndex(this, i, j)].y);
      fprintf(output, "%0.2f, ", (float)this->footPosition[footIndex(this, i, j)].z);
   }
 } 
  
 for(i=0; i < Sides; i++) {
   for(j=0; j < Legs/Sides; j++) {
     for (k=0; k < DOF; k++) {
       fprintf(output, "%0.2f, ", (float)this->jointAngle[axisIndex(this, i, j, k)]) ;
     }
   }
 }
  fprintf(output, "\n");
}

void CloseFile(FILE * output) {
  fclose(output);
}