#include <unistd.h>
#include <math.h>
#include <stdio.h>
#include <fcntl.h>
#include <termios.h>

#include <sys/time.h>

#include "servo.h"
#include "spi.h"

#include "ik.h"
#include "dynamic_sequencer.h"

void ik_to_servos(bot *);
int get_time();

static bot idle_position = {
   {0,0,0},    // world position
   {0,0,0},    // world orientation
   {0,0,-20},  // body position
   {0,0,0},    // body orientation

   { // leg positions
      {{ 166, 110,   0},}, // leg 0
      {{   0, 160,   0},}, // leg 1        
      {{-166, 110,   0},}, // ...
      {{-166,-110,   0},},
      {{   0,-160,   0},},
      {{ 166,-110,   0},}
   }
};


int
main(int argc, char **argv) {
   struct termios t;
   int flags_stdio;

   vector3d startup_vector={0,0,-100};
   vector3d v;
   
   sequencer_walk_parameters wp;
   
   int quit=0;
   int frame=0;
   int t_frame_start, t_frame_end;

   char c;

   int rotmove;

   load_calibration("/etc/calibration.bin");

   spi_open(0,33000000);

   ik(&idle_position);
   ik_to_servos(&idle_position);

   spi_close();

   return 0;
}

void
ik_to_servos(bot *b) {
   int i,j;
   
   for(i=0;i<NUM_LEGS;i++) {
      for(j=0;j<3;j++) {
         if(!isnan(b->leg[i].ik_angle[j])) {
            servo_pwm[i*3+j]=b->leg[i].ik_angle[j]*8.5*1800.0/M_PI+servo_offsets[i*3+j];
         }
      }
   }
   spi_update_servos();
}