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

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

#warning code uses old leg numbering!

void move_to(struct bot_double*, struct bot_double*, int);
void demo();
int get_time();

static struct bot_double startup_position = {
   {0,0,0},  // body offset
   {0,0,0},  // world position
   {0,0,0},  // body rotation

   {   
   {                        // leg 0
      { 80, 40, 0},         // base offset
      {200,120,10},         // end effector position
      { 0,  0,  0}          // leg angles
      
   },{                      // leg 1
      {  0, 60, 0},
      { 0,200,10},
      { 0,  0,  0},         // leg angles
   },{
      {-80, 40, 0},
      {-200,120,10},
      { 0, 0,  0}           // leg angles

   },{
      { 80,-40, 0},
      {200,-120,10},
      { 0, 0,  0}           // leg angles

   },{
      {  0,-60, 0},
      {0,-200,10},
      { 0, 0,  0}           // leg angles

   },{
      {-80,-40, 0},
    {-200,-120,10},
      { 0, 0,  0}           // leg angles
   }}
};


static struct bot_double idle_position = {
   {0,0,0},  // body offset
   {0,0,0},  // world position
   {0,0,0},  // body rotation

   {   
   {                        // leg 0
      { 80, 40, 0},         // base offset
      {150,120,120},         // end effector position
      { 0,  0,  0}          // leg angles
      
   },{                      // leg 1
      {  0, 60, 0},
      { 0,160,120},
      { 0,  0,  0},         // leg angles
   },{
      {-80, 40, 0}, // leg 2
      {-150,120,120},
      { 0, 0,  0}           // leg angles

   },{
      { 80,-40, 0}, // leg 3
      {150,-120,120},
      { 0, 0,  0}           // leg angles

   },{
      {  0,-60, 0},
      {0,-160,120},
      { 0, 0,  0}           // leg angles

   },{
      {-80,-40, 0},
    {-150,-120,120},
      { 0, 0,  0}           // leg angles

   }}
};

static struct bot_double current_position;
static struct bot_double target_position;

int
main(int argc, char **argv) {
   int i,j;
   int frame;
   uint32_t time;
   
   printf("welcome.\n");
   
   spi_open(0,33000000);
   
   for(i=0;i<NUM_SERVOS;i++) servo[i]=0;
   spi_update_servos(servo);
   ik_double(&startup_position);

   for(i=0;i<NUM_LEGS;i++) {
      for(j=0;j<3;j++) {
         servo[i*3+j]=servo_directions[i*3+j]*(startup_position.leg[i].angle[j])*1800.0/M_PI+servo_offsets[i*3+j];
      }
   }
   spi_update_servos(servo);
   
   demo();
   spi_close();
}

void	
move_to(struct bot_double *current_position, struct bot_double *target_position, int num_frames) {
   struct bot_double frame_position;

   int frame;
   unsigned int frame_start, frame_end;
   struct timespec delay;
   
   int leg,i,j;
   memcpy(&frame_position,current_position,sizeof(struct bot_double));

   for(frame=1;frame<=num_frames;frame++) {
      frame_start=get_time();
      
      frame_position.position.x=current_position->position.x+frame*(target_position->position.x-current_position->position.x)/num_frames;
      frame_position.position.y=current_position->position.y+frame*(target_position->position.y-current_position->position.y)/num_frames;
      frame_position.position.z=current_position->position.z+frame*(target_position->position.z-current_position->position.z)/num_frames;
      frame_position.orientation.rx=current_position->orientation.rx+frame*(target_position->orientation.rx-current_position->orientation.rx)/num_frames;
      frame_position.orientation.ry=current_position->orientation.ry+frame*(target_position->orientation.ry-current_position->orientation.ry)/num_frames;
      frame_position.orientation.rz=current_position->orientation.rz+frame*(target_position->orientation.rz-current_position->orientation.rz)/num_frames;
      for(leg=0;leg<NUM_LEGS;leg++) {
         frame_position.leg[leg].position.x=current_position->leg[leg].position.x+frame*(target_position->leg[leg].position.x-current_position->leg[leg].position.x)/num_frames;
         frame_position.leg[leg].position.y=current_position->leg[leg].position.y+frame*(target_position->leg[leg].position.y-current_position->leg[leg].position.y)/num_frames;
         frame_position.leg[leg].position.z=current_position->leg[leg].position.z+frame*(target_position->leg[leg].position.z-current_position->leg[leg].position.z)/num_frames;
      }
      ik_double(&frame_position);
      for(i=0;i<NUM_LEGS;i++) {
         for(j=0;j<3;j++) {
            servo[i*3+j]=servo_directions[i*3+j]*(frame_position.leg[i].angle[j])*1800.0/M_PI+servo_offsets[i*3+j];
         }
      }
      frame_end=get_time();
      if(frame_end-frame_start>20000) { 
         printf("frame %d delay %d\n",frame,frame_end-frame_start);
      }
      while(frame_end-frame_start<20000) {
//         delay.tv_sec=0;
//         delay.tv_nsec=(frame_end-frame_start)*1000;
//         nanosleep(&delay,NULL);
         frame_end=get_time();
      }
      
      spi_update_servos(servo);

   }

   memcpy(current_position,target_position,sizeof(struct bot_double));
}

void
move_to_parabolic(struct bot_double *current_position, struct bot_double *target_position, int num_frames, double dx, double dy, double dz, double a, int leg_mask) {
   struct bot_double frame_position;
   int frame;
   int frame_start, frame_end;
   int leg,i,j;
      
   double t,d;
   
   memcpy(&frame_position,current_position,sizeof(struct bot_double));

   for(frame=1;frame<=num_frames;frame++) {
      frame_start=get_time();   
      t=2.0*frame/num_frames-1.0;
      d=a-(a*t*t);

      frame_position.position.x=current_position->position.x+frame*(target_position->position.x-current_position->position.x)/num_frames;
      frame_position.position.y=current_position->position.y+frame*(target_position->position.y-current_position->position.y)/num_frames;
      frame_position.position.z=current_position->position.z+frame*(target_position->position.z-current_position->position.z)/num_frames;
      frame_position.orientation.rx=current_position->orientation.rx+frame*(target_position->orientation.rx-current_position->orientation.rx)/num_frames;
      frame_position.orientation.ry=current_position->orientation.ry+frame*(target_position->orientation.ry-current_position->orientation.ry)/num_frames;
      frame_position.orientation.rz=current_position->orientation.rz+frame*(target_position->orientation.rz-current_position->orientation.rz)/num_frames;

      for(leg=0;leg<NUM_LEGS;leg++) {
         if(!((1<<leg)&leg_mask)) {
            frame_position.leg[leg].position.x=current_position->leg[leg].position.x+frame*(target_position->leg[leg].position.x-current_position->leg[leg].position.x)/num_frames;
            frame_position.leg[leg].position.y=current_position->leg[leg].position.y+frame*(target_position->leg[leg].position.y-current_position->leg[leg].position.y)/num_frames;
            frame_position.leg[leg].position.z=current_position->leg[leg].position.z+frame*(target_position->leg[leg].position.z-current_position->leg[leg].position.z)/num_frames;
         } else {
            frame_position.leg[leg].position.x=dx*d + current_position->leg[leg].position.x + frame*(target_position->leg[leg].position.x - current_position->leg[leg].position.x)/num_frames;
            frame_position.leg[leg].position.y=dy*d + current_position->leg[leg].position.y + frame*(target_position->leg[leg].position.y - current_position->leg[leg].position.y)/num_frames;
            frame_position.leg[leg].position.z=dz*d + current_position->leg[leg].position.z + frame*(target_position->leg[leg].position.z - current_position->leg[leg].position.z)/num_frames;

         }
      }

      ik_double(&frame_position);
      for(i=0;i<NUM_LEGS;i++) {
         for(j=0;j<3;j++) {
            servo[i*3+j]=servo_directions[i*3+j]*(frame_position.leg[i].angle[j])*1800.0/M_PI+servo_offsets[i*3+j];
         }
      }
      
      frame_end=get_time();
      if(frame_end-frame_start>20000) { 
         printf("frame %d delay %d\n",frame,frame_end-frame_start);
      }
      while(frame_end-frame_start<20000) {
//         delay.tv_sec=0;
//         delay.tv_nsec=(frame_end-frame_start)*1000;
//         nanosleep(&delay,NULL);
         frame_end=get_time();
      }
      
      spi_update_servos(servo);
   }

   memcpy(current_position,target_position,sizeof(struct bot_double));
}

void
walk(double dx, double dy, double alpha, int frames, int mode) {
   int leg_mask;
   int i;

   double step_height=30;

   struct bot_double start_position;

   memcpy(&target_position, &current_position,sizeof(struct bot_double));
   memcpy(&start_position,&current_position,sizeof(struct bot_double));

   leg_mask=0;
         
   for(i=0;i<NUM_LEGS;i++) {
      if(i%2) {
         leg_mask|=(1<<i);
         target_position.leg[i].position.x=dx+(current_position.leg[i].position.x*cos(alpha/2)-current_position.leg[i].position.y*sin(alpha/2));
         target_position.leg[i].position.y=dy+(current_position.leg[i].position.x*sin(alpha/2)+current_position.leg[i].position.y*cos(alpha/2));
      }
   }   
   move_to_parabolic(&current_position,&target_position,frames,0,0,-1,step_height,leg_mask);

   target_position.position.x=start_position.position.x+dx/2;
   target_position.position.y=start_position.position.y+dy/2;
   target_position.orientation.rz=start_position.orientation.rz+alpha/2;
      
   move_to(&current_position,&target_position,frames);

   leg_mask=0;
   for(i=0;i<NUM_LEGS;i++) {
      if(!(i%2)) {
         leg_mask|=(1<<i);
         target_position.leg[i].position.x=dx+(current_position.leg[i].position.x*cos(alpha/2)-current_position.leg[i].position.y*sin(alpha/2));
         target_position.leg[i].position.y=dy+(current_position.leg[i].position.x*sin(alpha/2)+current_position.leg[i].position.y*cos(alpha/2));
      }
   }   

   move_to_parabolic(&current_position,&target_position,frames,0,0,-1,step_height,leg_mask);
   target_position.position.x=start_position.position.x+dx;
   target_position.position.y=start_position.position.y+dy;
   target_position.orientation.rz=start_position.orientation.rz+alpha;
   move_to(&current_position,&target_position,frames);

   memcpy(&target_position,&current_position,sizeof(struct bot_double));
}

void
demo() {
   int i,j;
   
   // stand up
   memcpy(&current_position,&startup_position,sizeof(struct bot_double));
   memcpy(&target_position,&current_position,sizeof(struct bot_double));

// lift up      
   for(i=0;i<NUM_LEGS;i++) {
      target_position.leg[i].position.z=130;
   }
   move_to(&current_position,&target_position,50);
      

// move legs to idle position
   for(i=0;i<(NUM_LEGS/2);i++) {
      memcpy(&target_position.leg[i],&idle_position.leg[i],sizeof(struct leg_double));         
      memcpy(&target_position.leg[NUM_LEGS-1-i],&idle_position.leg[NUM_LEGS-1-i],sizeof(struct leg_double));         
      move_to_parabolic(&current_position,&target_position,25,0,0,-1,30,(1<<i)|(1<<(NUM_LEGS-1-i)));
   }

   
   // walk a square
   move_to(&current_position,&target_position,25);
   walk(0,50,0,20,0);
   target_position.position.z=20;
   move_to(&current_position,&target_position,25);
   walk(-50,0,0,20,0);
   target_position.position.z=40;
   move_to(&current_position,&target_position,25);
   walk(0,-50,0,20,0);
   target_position.position.z=-20;
   move_to(&current_position,&target_position,50);
   walk(50,0,0,20,0);


   // downward spiral
   sleep(1);

   target_position.position.z=0;
   target_position.orientation.rx=5*M_PI/180.0;
   move_to(&current_position,&target_position,25);

   
   for(j=0;j<3;j++) {
      for(i=0;i<16;i++) {
         target_position.orientation.rx=sin(M_PI*i/8.0)*5.0*M_PI/180.0;
         target_position.orientation.ry=cos(M_PI*i/8.0)*5.0*M_PI/180.0;
         target_position.position.z=i+j*16;
         move_to(&current_position,&target_position,8);
      }
   }   
   target_position.orientation.rx=0;
   target_position.orientation.ry=0;
   target_position.position.z=0;
   move_to(&current_position,&target_position,25);

   // shake it, baby
   for(i=0;i<4;i++) {
      target_position.position.z=10*i;
      if(i%2) {
         target_position.orientation.rz=5.0*M_PI/180.0;
      } else {
         target_position.orientation.rz=-5.0*M_PI/180;
      }
      move_to(&current_position,&target_position,25);
   }

   target_position.orientation.rz=0;
   move_to(&current_position,&target_position,5);

   sleep(1);

   // and some more..
   for(i=3;i>=0;i--) {
      target_position.position.z=10*i;
      if(i%2) {
         target_position.position.y=20;
      } else {
         target_position.position.y=-20;
      }
      move_to(&current_position,&target_position,25);
   }

   target_position.position.y=0;
   move_to(&current_position,&target_position,25);

   // lift middle legs
   target_position.leg[1].position.z-=80;
   target_position.leg[1].position.y+=20;
   target_position.leg[4].position.z-=80;
   target_position.leg[4].position.y-=20;
   move_to(&current_position,&target_position,100);

   target_position.leg[1].position.y+=80;
   target_position.leg[4].position.y-=80;
   move_to(&current_position,&target_position,100);

   target_position.position.z+=30;
   move_to(&current_position,&target_position,25);
   target_position.position.z-=60;
   move_to(&current_position,&target_position,50);
   target_position.position.z+=30;
   move_to(&current_position,&target_position,25);
   

   target_position.leg[1].position.y-=80;
   target_position.leg[4].position.y+=80;
   move_to(&current_position,&target_position,100);
   

   memcpy(&target_position,&idle_position,sizeof(struct bot_double));
   move_to(&current_position,&target_position,50);

/*
   walk(50,50,-15*91,25,0);
   walk(50,50,-15*91,25,0);
   walk(-50,-50,15*91,25,0);
   walk(-50,-50,15*91,25,0);
   walk(50,-50,15*91,25,0);
   walk(50,-50,15*91,25,0);
   walk(-50,50,-15*91,25,0);
   walk(-50,50,-15*91,25,0);
*/
   // lean back and wave
   target_position.position.x=-40;
   move_to(&current_position,&target_position,25);
   sleep(1);
   
   target_position.leg[0].position.x=180;
   target_position.leg[0].position.y=150;
   target_position.leg[0].position.z=-120;

   target_position.leg[3].position.x=180;
   target_position.leg[3].position.y=-150;
   target_position.leg[3].position.z=-120;
   move_to(&current_position,&target_position,50);

   for(i=0;i<3;i++) {
      target_position.leg[0].position.x+=20;
      target_position.leg[0].position.y-=60;
      target_position.leg[3].position.x+=20;
      target_position.leg[3].position.y+=60;
      move_to(&current_position,&target_position,12);

      target_position.leg[0].position.y+=60;
      target_position.leg[0].position.x-=20;
      target_position.leg[3].position.y-=60;
      target_position.leg[3].position.x-=20;

      move_to(&current_position,&target_position,12);
   }

   sleep(1);
   
   memcpy(&target_position,&idle_position,sizeof(struct bot_double));
   target_position.position.x=-40;
   move_to(&current_position,&target_position,50);
   target_position.position.x=0;
   move_to(&current_position,&target_position,25);

   target_position.position.z=100;
   move_to(&current_position,&target_position,50);

   for(i=0;i<NUM_SERVOS;i++) { servo[i]=0; }
   spi_update_servos(servo);
   
}

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