#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <sys/time.h>

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

int get_time();

static bot idle_position = {
   {0,0,-130},  // body position
   {0,0,0},     // body rotation

   { // 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) {
   int i,j;
   int start_time;
   int end_time;
   spi_open(0,0);
   ik(&idle_position);
   for(i=0;i<24;i++) servo_pwm[i]=0;
   for(i=0;i<NUM_LEGS;i++) {
      printf("leg %d\n",i);
      for(j=0;j<3;j++) {
         printf("   a[%d]=%.3f (%.3f deg)\n",j,idle_position.leg[i].ik_angle[j],idle_position.leg[i].ik_angle[j]*180/M_PI);
         if(!isnan(idle_position.leg[i].ik_angle[j])) {
            getchar();
            servo_pwm[i*3+j]=(idle_position.leg[i].ik_angle[j]*8.5*1800/M_PI)+servo_offsets[i*3+j];
            spi_update_servos();
         }
      }
   }

   printf("benchmarking...");
   start_time=get_time();
   for(i=0;i<1000;i++) {
      ik(&idle_position);
   }
   end_time=get_time();
   printf(" %d\n",end_time-start_time);
   spi_close();
}

int
get_time() {
   struct timeval t;
   gettimeofday(&t,NULL);
   return (t.tv_sec*1000000+t.tv_usec);
   
}