#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,-30},  // 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;

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

   spi_open(0,33000000);

   tcgetattr(0,&t);
   t.c_lflag&=~(ICANON|ECHO);
   tcsetattr(0,TCSANOW,&t);

   flags_stdio=fcntl(0, F_GETFL,0);
   flags_stdio|=O_NONBLOCK;
   fcntl(0, F_SETFL,flags_stdio);
                     

   sequencer_init();
   sequencer_move_body(0,100,&startup_vector);

   wp.step_direction.x=0; 
   wp.step_direction.y=0;
   wp.step_direction.z=0;
   wp.step_rotation=0;

   wp.step_duration=25;
   wp.step_height=20;

   sequencer_walk(&wp);

   while(!quit) {
      if(read(0,&c,1)==1) {
         switch(c) {
            case  27: 
               quit=1; 
               break;
            case 'w': wp.step_direction.x+=5; break;
            case 's': wp.step_direction.x-=5; break;
            case 'a': wp.step_direction.y+=5; break;
            case 'd': wp.step_direction.y-=5; break;

            case 'q': wp.step_rotation-=1.0*M_PI/180.0; break;
            case 'e': wp.step_rotation+=1.0*M_PI/180.0; break;
            case 'x': wp.step_direction.x=0;   wp.step_direction.y=0; wp.step_rotation=0;   break;

            case 'r': v.x=0; v.y=0; v.z=-10; sequencer_move_body(0,10,&v); break;
            case 'f': v.x=0; v.y=0; v.z=10; sequencer_move_body(0,10,&v); break;
            
               
         }
         printf("x: %f y: %f h: %f d: %i rot: %f\n",wp.step_direction.x, wp.step_direction.y, wp.step_height, wp.step_duration, wp.step_rotation);
      }
      t_frame_start=get_time();
      sequencer_run_frame(&idle_position);
      ik(&idle_position);
      t_frame_end=get_time();
      if(t_frame_end-t_frame_start>20000) {
         printf("frame %d slack %d\n",frame,t_frame_end-t_frame_start);
      }
      while(t_frame_end<t_frame_start+20000) t_frame_end=get_time();
      ik_to_servos(&idle_position);
      frame++;
   }
   spi_close();

   flags_stdio=fcntl(0, F_GETFL,0); flags_stdio&=~(O_NONBLOCK); fcntl(0,
   F_SETFL,flags_stdio);


   tcgetattr(0,&t);
   t.c_lflag|=(ICANON|ECHO|ECHOE|ISIG);
   tcsetattr(0,TCSANOW,&t);
            
   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();
}


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