#include <termios.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <sys/time.h>
#include <fcntl.h>

#include <servo.h>
#include <ik.h>
#include <spi.h>

int servo_power[NUM_SERVOS];

int
main(int argc, char **argv) {
   int fd;
   int i,j,l,s;
   int frame;
   uint32_t time;
   char c; 
   uint16_t pwm;
   struct termios t;
   int step=1;

   printf("welcome.\n");
   spi_open(0,33000000);
   
   for(i=0;i<NUM_SERVOS;i++) { servo_pwm[i]=0; servo_power[i]=0; }
   spi_update_servos();

   load_calibration("calibration.bin");

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

   l=0;

   do {
      printf("\f");
      for(i=0;i<NUM_SERVOS;i++) {
         if(!(i%3)) { 
            printf("\n");
            if((i/3)==l) { 
               if(servo_power[i]) {
                  printf("!> "); 
               } else {
                  printf(" > "); 
               }
            } else { 
               if(servo_power[i]) {
                  printf("!  "); 
               } else {
                  printf("   "); 
               }
            }
         }            
         printf("%04x (%d) ",servo_offsets[i],servo_offsets[i]);
      }
       printf("\n\nazsxdc: move servo\n+-: select leg(%d)\n[]: increase/decrease step size (%d)\np: power on/off leg\n12: save min/max value\nw: dump\nf: save to calibration.bin\nq: quit\n> ",l,step);
       c=getchar();
       printf("%c (%02x)\n",c,c);
       switch(c) {
          case 'a':
             servo_offsets[l*3]+=step;
             break;
          case 'z': 
             servo_offsets[l*3]-=step;
             break;
          case 's': 
             servo_offsets[l*3+1]+=step;
             break;
          case 'x': 
             servo_offsets[l*3+1]-=step;
             break;
          case 'd': 
             servo_offsets[l*3+2]+=step;
             break;
          case 'c': 
             servo_offsets[l*3+2]-=step;
             break;
          case '+':
             if((l+1)<NUM_LEGS) l++;
             break;
          case '-':
             if(l>0) l--;
             break;
          case '[':
             if(step>=10) step/=10;
             break;
          case ']':
             step*=10;
             break;
          case 'w':
             printf("/* Servo current values */\n\nuint16_t servo_base[]= {");
             for(i=0;i<NUM_SERVOS;i++) {
                if(!(i%3)) printf("\n");
                printf(" 0x%04x,",servo_pwm[i]);
             }
             printf("\n};\n\n");
             printf("/* press any key */\n");
             getchar();
             break;
          case 'f':
             unlink("calibration.bin.bak");
             rename("calibration.bin","calibration.bin.bak");
             fd=open("calibration.bin",O_WRONLY|O_CREAT|O_TRUNC);
             write(fd,servo_offsets,sizeof(servo_offsets));
             close(fd);
             break;
          case 'p':
             if(!servo_power[l*3]) {
                for(i=0;i<3;i++) servo_power[l*3+i]=1;
             } else {
                for(i=0;i<3;i++) servo_power[l*3+i]=0;
             }
             
       }
       for(i=0;i<NUM_SERVOS;i++) {
          servo_pwm[i]=servo_power[i]*servo_offsets[i];
       }
       spi_update_servos();
   } while(c!='q');

   spi_close();

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