#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"

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

int ad_fd;

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

   { // leg positions
      {{ 166, 130,   0},}, // leg 0
      {{   0, 180,   0},}, // leg 1        
      {{-166, 130,   0},}, // ...
      {{-166,-130,   0},},
      {{   0,-180,   0},},
      {{ 166,-130,   0},}
   }
};


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

   int quit=0;
   int frame=0;
   int t_frame_start, t_frame_end;

   int dump_fd;

   char c;
   int i, int_z;
   int leg;
   unsigned char adbuffer[12];
   unsigned char ad_max[6]={0,0,0,0,0,0};
   unsigned char ad_min[6]={255,255,255,255,255,255};
   
   load_calibration("calibration.bin");

   spi_open(0,33000000);

   ad_fd=open("/dev/ttyS2",O_RDWR);
   if(ad_fd<0) {
      printf("can't open /dev/ttyS2.");
      exit(2);
   }

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

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

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

   leg=0;
                     
   while(!quit) {
      write(ad_fd,&c,1);
      read(ad_fd,&adbuffer,12);
      printf("\f");
      printf("   val rval  min max   avg diff  z\n");
      for(i=0;i<6;i++) {
         printf("%d: %3d %3d   %3d %3d   %3d %3d    %3.2f\n",adbuffer[2*i],adbuffer[2*i+1],adbuffer[2*i+1]-ad_min[i],ad_min[i], ad_max[i],(ad_min[i]+ad_max[i])/2,ad_max[i]-ad_min[i],idle_position.leg[i].position.z);
         if(ad_min[i]>adbuffer[2*i+1]) ad_min[i]=adbuffer[2*i+1];
         if(ad_max[i]<adbuffer[2*i+1]) ad_max[i]=adbuffer[2*i+1];
      }
      printf("\nleg (+/-): %d\nd: dump to ad.bin\n",leg);
      
      if(read(0,&c,1)==1) {
         switch(c) {
            case 27: 
            case 'q':
               quit=1;
               break;
            case 'a':
               idle_position.leg[leg].position.z++;
               break;
            case 'z':
               idle_position.leg[leg].position.z--;
               break;
            case '+':
               if(leg<5) leg++;
               break;
            case '-':
               if(leg>0) leg--;
               break;
            case 'r': 
               for(i=0;i<6;i++) {
                  for(int_z=0;int_z<20;int_z++) {
                     idle_position.leg[i].position.z=int_z;

                     t_frame_start=get_time();
                     ik(&idle_position);
                     t_frame_end=get_time();
                     while(t_frame_end<t_frame_start+50000) t_frame_end=get_time();
                     ik_to_servos(&idle_position);
                     write(ad_fd,&c,1);
                     read(ad_fd,&adbuffer,12);
                     if(ad_min[i]>adbuffer[2*i+1]) ad_min[i]=adbuffer[2*i+1];
                     if(ad_max[i]<adbuffer[2*i+1]) ad_max[i]=adbuffer[2*i+1];
                  }
                  for(int_z=20;int_z>0;int_z--) {
                     idle_position.leg[i].position.z=int_z;

                     t_frame_start=get_time();
                     ik(&idle_position);
                     t_frame_end=get_time();
                     while(t_frame_end<t_frame_start+50000) t_frame_end=get_time();
                     ik_to_servos(&idle_position);
                     write(ad_fd,&c,1);
                     read(ad_fd,&adbuffer,12);
                     if(ad_min[i]>adbuffer[2*i+1]) ad_min[i]=adbuffer[2*i+1];
                     if(ad_max[i]<adbuffer[2*i+1]) ad_max[i]=adbuffer[2*i+1];
                  }
               }
            case 'd':
               dump_fd=open("ad.bin",O_WRONLY|O_CREAT|O_TRUNC);
               if(dump_fd>=0) {
                  write(dump_fd,ad_min,sizeof(ad_min));
                  write(dump_fd,ad_max,sizeof(ad_max));
               }
               
         }
      }
      t_frame_start=get_time();
      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++;
   }
   close(ad_fd);
   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);
   
}