#include <stdio.h>
#include <stdlib.h>

//#define dodebug /* turn debug on */

#include "robotdef.h"  // physical info
#include "cfout.h"     // data file output
#include "legdefs.h"   // names for legs
#include "walkerbot.h" // physical definitons and ops
#include "debug.h"     // debug()


walker_t bot;

int main(void) {

   int i, j, k;
   FILE * output;  
   
   dt = 0.1;
   
   debug(("Initializing walker...\n"));
   setParamiters( &bot, Legs, DOF) ;
   walkerInit( &bot );
   
   debug(("Zeroing axies..\n"));
   for(i=0; i < Sides; i++) {
     for(j=0; j < Legs; j++) {
       for (k=0; k < DOF; k++) {
         bot.jointAngle[axisIndex(&bot, i, j, k)]= 0;
       }
     }
   }
   
   /*
  C = dist3d(0, 0, 0, px-len0, py, 0);
  c = elbow(C, len2, len1);
  b = elbow(len2, C, len1);
  t = dir(px-len0,py);

  printf("%f\n", c);

  t1 = t + b;
  t2 = 180 + c;
   
   */
   debug(("Settings joint angles...\n"));
   bot.jointAngle[ axisIndex(&bot, RIGHT, FIRST,  THRUST )] =  0;
   bot.jointAngle[ axisIndex(&bot, RIGHT, FIRST,  LIFT)   ] =  60;
   bot.jointAngle[ axisIndex(&bot, RIGHT, FIRST,  ELBOW)  ] = -110;
   bot.jointAngle[ axisIndex(&bot, RIGHT, SECOND, THRUST) ] =  0;
   bot.jointAngle[ axisIndex(&bot, RIGHT, SECOND, LIFT)   ] =  60; 
   bot.jointAngle[ axisIndex(&bot, RIGHT, SECOND, ELBOW)  ] = -110;
   bot.jointAngle[ axisIndex(&bot, RIGHT, THIRD,  THRUST) ] =  0;
   bot.jointAngle[ axisIndex(&bot, RIGHT, THIRD,  LIFT)   ] =  60;
   bot.jointAngle[ axisIndex(&bot, RIGHT, THIRD,  ELBOW)  ] = -110;
   bot.jointAngle[ axisIndex(&bot, LEFT,  THIRD,  THRUST) ] =  0;
   bot.jointAngle[ axisIndex(&bot, LEFT,  THIRD,  LIFT)   ] =  60;
   bot.jointAngle[ axisIndex(&bot, LEFT,  THIRD,  ELBOW)  ] = -110;
   bot.jointAngle[ axisIndex(&bot, LEFT,  SECOND, THRUST) ] =  0;
   bot.jointAngle[ axisIndex(&bot, LEFT,  SECOND, LIFT)   ] =  60;
   bot.jointAngle[ axisIndex(&bot, LEFT,  SECOND, ELBOW)  ] = -110;
   bot.jointAngle[ axisIndex(&bot, LEFT,  FIRST,  THRUST) ] =  0; 
   bot.jointAngle[ axisIndex(&bot, LEFT,  FIRST,  LIFT)   ] =  60;  
   bot.jointAngle[ axisIndex(&bot, LEFT,  FIRST,  ELBOW)  ] = -110;
   
   debug(("Opening output file...\n"));   
   output = OpenFile ( "stand.dat" ) ;
   
   
   AddEntry( output, &bot) ;
 
   debug(("Closing output file...\n"));   
   CloseFile(output) ;
   
   debug(("Cleaning up...\n"));  
   walkerFree( &bot );
   return 0;
   
}