#include <stdio.h>
#include <math.h>

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

#define MAX_ENTRIES 100

void sequencer_walk_callback(sequencer_entry *, bot *);

static sequencer_entry sequencer_list[MAX_ENTRIES];

void
sequencer_init() {
   int i;
   for(i=0;i<MAX_ENTRIES;i++) {
      sequencer_list[i].command=seq_nop;
   }
}

void
sequencer_dump() {
   int i;
   for(i=0;i<MAX_ENTRIES;i++) {
      printf("i: %d frame_delay: %d frame_t: %d frame_duration: %d cmd:",i,sequencer_list[i].frame_delay, sequencer_list[i].frame_t, sequencer_list[i].frame_duration);
      switch(sequencer_list[i].command) {
         case seq_nop: printf("nop\n"); break;
         case seq_move_body: printf("move body\n"); break;
         case seq_move_leg: printf("move leg %d %f %f %f\n",sequencer_list[i].arg_int,sequencer_list[i].arg_vector3d.x,sequencer_list[i].arg_vector3d.y,sequencer_list[i].arg_vector3d.z); break;
   
      }
   }
}

void
sequencer_run_frame(bot *b) {
   int i;
   double t;
   vector3d temp;
   int leg;
   
   for(i=0;i<MAX_ENTRIES;i++) {
      if(sequencer_list[i].command!=seq_nop) {
         if(sequencer_list[i].frame_delay) {
            sequencer_list[i].frame_delay--;
         } else {
            if(sequencer_list[i].frame_t>sequencer_list[i].frame_duration) sequencer_list[i].command=seq_nop;

            switch(sequencer_list[i].command) {
               case seq_walk:
                  sequencer_walk_callback(&sequencer_list[i], b);
                  break;
               case seq_move_body:
                  add_vector3d(&b->body_position,&sequencer_list[i].arg_vector3d,&b->body_position);
                  break;
               case seq_rotate_body:
                  add_vector3d(&b->body_orientation,&sequencer_list[i].arg_vector3d,&b->body_orientation);
                  break;
               case seq_move_leg:
                  add_vector3d(&b->leg[sequencer_list[i].arg_int].position,&sequencer_list[i].arg_vector3d,&b->leg[sequencer_list[i].arg_int].position);
                  break;
               case seq_move_leg_parabolic:
                  t = (2.0*sequencer_list[i].frame_t/sequencer_list[i].frame_duration)- 1.0;
                  t = 1.0 - (t*t);
                  scalar_mult_vector3d(&sequencer_list[i].arg_vector3d,t-sequencer_list[i].arg_double,&temp);
                  sequencer_list[i].arg_double=t;

                  add_vector3d(&b->leg[sequencer_list[i].arg_int].position,&temp,&b->leg[sequencer_list[i].arg_int].position);
                  break;
               case seq_rotate_leg:
                  if(sequencer_list[i].frame_t==0) {
                     rot_x_vector3d(&b->leg[sequencer_list[i].arg_int].position,sequencer_list[i].arg_vector3d.x,&temp);
                     rot_y_vector3d(&temp,sequencer_list[i].arg_vector3d.y,&temp);
                     rot_z_vector3d(&temp,-sequencer_list[i].arg_vector3d.z,&temp);
                     sub_vector3d(&temp,&b->leg[sequencer_list[i].arg_int].position,&temp);
                     scalar_mult_vector3d(&temp,1.0/sequencer_list[i].frame_duration,&sequencer_list[i].arg_vector3d);
                     printf("x: %f y: %f z: %f\n",temp.x, temp.y, temp.z);
                  }
                  add_vector3d(&b->leg[sequencer_list[i].arg_int].position,&sequencer_list[i].arg_vector3d,&b->leg[sequencer_list[i].arg_int].position);
                  break;
                  
            }
            sequencer_list[i].frame_t++;
         }
      }
   } 
}


sequencer_entry *
sequencer_new_entry() {
   int i=0;
   for(i=0;i<MAX_ENTRIES;i++) {
      if(sequencer_list[i].command==seq_nop) {
         sequencer_list[i].frame_t=0;
         return &sequencer_list[i];
      }
   }
   return 0;
}

void
sequencer_move_body(int frame_delay, int frame_duration, vector3d *d) {
   sequencer_entry *e;
   e=sequencer_new_entry();
   if(e) {
      e->command=seq_move_body;
      e->frame_delay=frame_delay;
      e->frame_duration=frame_duration;

      scalar_mult_vector3d(d,1.0/frame_duration,&e->arg_vector3d);
   }
}

void
sequencer_rotate_body(int frame_delay, int frame_duration, vector3d *d) {
   sequencer_entry *e;
   e=sequencer_new_entry();
   if(e) {
      e->command=seq_rotate_body;
      e->frame_delay=frame_delay;
      e->frame_duration=frame_duration;

      scalar_mult_vector3d(d,1.0/frame_duration,&e->arg_vector3d);
   }
}

void
sequencer_rotate_deg_body(int frame_delay, int frame_duration, vector3d *d) {
   sequencer_entry *e;
   e=sequencer_new_entry();
   if(e) {
      e->command=seq_rotate_body;
      e->frame_delay=frame_delay;
      e->frame_duration=frame_duration;

      scalar_mult_vector3d(d,(M_PI/180.0)/frame_duration,&e->arg_vector3d);
   }
}


void
sequencer_move_leg(int frame_delay, int frame_duration, int i, vector3d *d) {
   sequencer_entry *e;
   e=sequencer_new_entry();
   if(e) {
      e->command=seq_move_leg;
      e->frame_delay=frame_delay;
      e->frame_duration=frame_duration;

      e->arg_int=i;
      scalar_mult_vector3d(d,1.0/frame_duration,&e->arg_vector3d);
   }
}

void
sequencer_rotate_leg(int frame_delay, int frame_duration, int i, vector3d *d) {
   sequencer_entry *e;
   e=sequencer_new_entry();
   if(e) {
      e->command=seq_rotate_leg;
      e->frame_delay=frame_delay;
      e->frame_duration=frame_duration;

      e->arg_int=i;
      e->arg_double=0;
      e->arg_vector3d.x=d->x;
      e->arg_vector3d.y=d->y;
      e->arg_vector3d.z=d->z;
   }
}

void
sequencer_move_leg_parabolic(int frame_delay, int frame_duration, int i, vector3d *d) {
   sequencer_entry *e;
   e=sequencer_new_entry();
   if(e) {
      e->command=seq_move_leg_parabolic;
      e->frame_delay=frame_delay;
      e->frame_duration=frame_duration;

      e->arg_int=i;
      e->arg_double=0;
      e->arg_vector3d.x=d->x;
      e->arg_vector3d.y=d->y;
      e->arg_vector3d.z=d->z;
   }
}

void
sequencer_walk(sequencer_walk_parameters *p) {
   sequencer_entry *e;
   if(e=sequencer_new_entry()) {
      e->command=seq_walk;
      e->arg_pointer=p;
      e->arg_int=0;
      e->frame_delay=0;
      e->frame_t=0;
      e->frame_duration=1;
   }
}

void
sequencer_walk_callback(sequencer_entry *e, bot *b) {
   sequencer_walk_parameters *p;
   
   p=e->arg_pointer;

   vector3d v;

   int l;

   e->frame_t=-1;
   e->frame_duration=1;
   e->frame_delay=p->step_duration;

   switch(e->arg_int) {
      case 0:
         if(((p->step_direction.x*p->step_direction.x)+(p->step_direction.y*p->step_direction.y))>=1.0||(abs(p->step_rotation)>0.01)) {
            e->arg_int=1;

            for(l=0;l<NUM_LEGS;l++) {
               if(l&1) {
                  rot_z_vector3d(&b->leg[l].position,p->step_rotation,&v);
                  add_vector3d(&v,&p->step_direction,&v);
               } else {
                  rot_z_vector3d(&b->leg[l].position,-p->step_rotation,&v);
                  sub_vector3d(&v,&p->step_direction,&v);
               }
               sub_vector3d(&v,&b->leg[l].position,&v);
               sequencer_move_leg(0,p->step_duration,l,&v);
               
               if(l&1) {
                  v.x=0;
                  v.y=0;
                  v.z=-p->step_height;
                  sequencer_move_leg_parabolic(0,p->step_duration,l,&v);
               }
            }
         }
         break;
      case 1:
         for(l=0;l<NUM_LEGS;l++) {
            if(l&1) {
               sub_vector3d(&b->leg[l].position,&p->last_step_direction,&v);
               rot_z_vector3d(&v,-(p->last_step_rotation+p->step_rotation),&v);
               sub_vector3d(&v,&p->step_direction,&v);
            } else {
               add_vector3d(&b->leg[l].position,&p->last_step_direction,&v);
               rot_z_vector3d(&v,(p->last_step_rotation+p->step_rotation),&v);
               add_vector3d(&v,&p->step_direction,&v);
            }
            sub_vector3d(&v,&b->leg[l].position,&v);
            sequencer_move_leg(0,p->step_duration,l,&v);
               
            if(!(l&1)) {
               v.x=0;
               v.y=0;
               v.z=-p->step_height;
               sequencer_move_leg_parabolic(0,p->step_duration,l,&v);
            }
         }
         
         if(((p->step_direction.x*p->step_direction.x)+(p->step_direction.y*p->step_direction.y))>=1.0||(abs(p->step_rotation)>0.01)) {
            e->arg_int=2;
         } else {
            e->arg_int=0;
         }
         break;
      case 2:
         for(l=0;l<NUM_LEGS;l++) {
            if(l&1) {
               add_vector3d(&b->leg[l].position,&p->last_step_direction,&v);
               rot_z_vector3d(&v,(p->last_step_rotation+p->step_rotation),&v);
               add_vector3d(&v,&p->step_direction,&v);
            } else {
               sub_vector3d(&b->leg[l].position,&p->last_step_direction,&v);
               rot_z_vector3d(&v,-(p->step_rotation+p->last_step_rotation),&v);
               sub_vector3d(&v,&p->step_direction,&v);
            }
            sub_vector3d(&v,&b->leg[l].position,&v);
            sequencer_move_leg(0,p->step_duration,l,&v);
               
            if(l&1) {
               v.x=0;
               v.y=0;
               v.z=-p->step_height;
               sequencer_move_leg_parabolic(0,p->step_duration,l,&v);
            }
         }


         if((((p->step_direction.x*p->step_direction.x)+(p->step_direction.y*p->step_direction.y))>=1.0)||(abs(p->step_rotation)>0.01)) {
            e->arg_int=1;
         } else {
            e->arg_int=0;
         }
         break;
   }
   copy_vector3d(&p->step_direction,&p->last_step_direction);
   p->last_step_rotation=p->step_rotation;
}