#include <unistd.h>
#include <math.h>
#include <stdio.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
#include <stdlib.h>
#include <sched.h>

#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/poll.h>
#include <sys/un.h>
#include <sys/resource.h>

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

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

#include "licks_message.h"

struct sched_param sched_p;
struct timespec timeout;

static licks_message message;

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

static bot bot_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) {
   sequencer_walk_parameters wp;
   vector3d move_vector, rotate_vector;
   char *endptr;
   int i,l,d;
   
   int power=0;
   int quit=0;
   int frame=0;
   int query=0;
   
   long t_frame_start, t_frame;
   char *sender;

   licks_socket_open();

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

   spi_open(0,15000000);

#ifdef LINUX
   setpriority(PRIO_PROCESS,0,-99);
   sched_p.sched_priority=99;
   if(sched_setscheduler(0,SCHED_FIFO,&sched_p)==-1) {
      perror("sched_setscheduler");
   }
#endif


   sequencer_init();

   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) {
      t_frame_start=get_time();
      sequencer_run_frame(&bot_position);
      ik(&bot_position);

      if(power) {
         ik_to_servos(&bot_position);
      } else {
         for(i=0;i<NUM_SERVOS;i++) servo_pwm[i]=0;
         spi_update_servos();
      }
      t_frame=get_time()-t_frame_start;

      while(licks_socket_poll()>0) {
         sender=receive_message(&message);
         switch(message.type) {
            case MSG_HELO:
               printf("HELO received\n");
               message.type='A';
               sprintf(message.parameters,"AH2U2");
               send_reply(&message);
               break;
            case MSG_QUIT:
               quit=1;
               break;
            case MSG_POWER:
               if(message.parameters[0]=='1') power=1; else power=0;
               break;
            case MSG_QUERY:
               printf("query\n");
               if(message.parameters[0]=='1') query=1; else query=0;
               break;
            case MSG_BODY:
               d=strtol(&(message.parameters[0]),&endptr,0);
               move_vector.x=strtod(endptr+1,&endptr);
               move_vector.y=strtod(endptr+1,&endptr);
               move_vector.z=strtod(endptr+1,&endptr);
               rotate_vector.x=strtod(endptr+1,&endptr);
               rotate_vector.y=strtod(endptr+1,&endptr);
               rotate_vector.z=strtod(endptr+1,&endptr);
               sequencer_move_body(0,d,&move_vector);
               sequencer_rotate_body(0,d,&rotate_vector);
               break;
            case MSG_LEG:
               d=strtol(&(message.parameters[0]),&endptr,0);
               l=strtol(endptr+1,&endptr,0);
               move_vector.x=strtod(endptr+1,&endptr);
               move_vector.y=strtod(endptr+1,&endptr);
               move_vector.z=strtod(endptr+1,&endptr);
               sequencer_move_leg(0,d,l,&move_vector);
               break;
            case MSG_GAIT:
               printf("gait\n");
               wp.step_duration=strtol(message.parameters,&endptr,0);
               wp.step_direction.x=strtod(endptr+1,&endptr);
               wp.step_direction.y=strtod(endptr+1,&endptr);
               wp.step_direction.z=strtod(endptr+1,&endptr);
               wp.step_rotation=strtod(endptr+1,&endptr);
               wp.step_height=strtol(endptr+1,&endptr,0);
               break;
            
         }
      }



      if(query&&((frame%10)==0)) {
         message.type='A';
         snprintf(message.parameters,255,
            "B%4.2f %4.2f %4.2f %2.3f %2.3f %2.3f",
            bot_position.body_position.x,
            bot_position.body_position.y,
            bot_position.body_position.z,
            bot_position.body_orientation.x,
            bot_position.body_orientation.y,
            bot_position.body_orientation.z);
         send_reply(&message);
         for(i=0;i<NUM_LEGS;i++) {
            snprintf(message.parameters,255,
               "L%d %4.2f %4.2f %4.2f %2.4f %2.4f %2.4f", i,
               bot_position.leg[i].position.x,
               bot_position.leg[i].position.y,
               bot_position.leg[i].position.z,
               bot_position.leg[i].ik_angle[0],
               bot_position.leg[i].ik_angle[1],
               bot_position.leg[i].ik_angle[2]
            );
            send_reply(&message);            
         }
         snprintf(message.parameters,255,
            "G%d %4.2f %4.2f %4.2f %2.4f %4.2f",
            wp.step_duration,
            wp.step_direction.x,
            wp.step_direction.y,
            wp.step_direction.z,
            wp.step_rotation,
            wp.step_height
         );
         send_reply(&message);
      }


      t_frame=get_time()-t_frame_start;

      timeout.tv_sec=0;
      timeout.tv_nsec=1000*(14000-t_frame);
      nanosleep(&timeout,NULL);

      while((t_frame=get_time()-t_frame_start)<20000);

//      if(t_frame>21000) printf("slack: %d\n",t_frame);
      frame++;
   }
   spi_close();
   licks_socket_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();
}


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