#include <math.h>
#include "ik.h"

bot_configuration conf = {
 {
   { 80, 40,  0},
   {  0, 60,  0},
   {-80, 40,  0},
   {-80,-40,  0},
   {  0,-60,  0},
   { 80,-40,  0}
 }
};

void
ik(bot *b) {
   int i;

   vector3d v;

   double r;
   double r2z2,sr2z2;
   double acos_1,acos_2;

   const double c=20;
   const double f=80;
   const double t=130;

   const double f2=f*f;
   const double t2=t*t;

   for(i=0;i<NUM_LEGS;i++) {
      sub_vector3d(&b->leg[i].position,&b->body_position,&v);

      rot_x_vector3d(&v,b->body_orientation.x,&v);
      rot_y_vector3d(&v,b->body_orientation.y,&v);
      rot_z_vector3d(&v,b->body_orientation.z,&v);
         
      sub_vector3d(&v,&conf.leg_offset[i],&v);

      if(i<NUM_LEGS/2) {
         b->leg[i].ik_angle[0]=-atan2(v.x,v.y);
      } else {
         b->leg[i].ik_angle[0]=atan2(v.x,-v.y);
      }

      r=sqrt(v.x*v.x+v.y*v.y)-c;

      r2z2=r*r+v.z*v.z;
      sr2z2=sqrt(r2z2);
      
      acos_1=(r2z2+f2-t2)/(2*f*sr2z2);
      acos_2=(r2z2-f2-t2)/(2*f*t);
      if(i<NUM_LEGS/2) {
         b->leg[i].ik_angle[1]=M_PI/2 - (acos(acos_1) + atan2(r,v.z));
         b->leg[i].ik_angle[2]=-acos(acos_2)+M_PI/2;
      } else {
         b->leg[i].ik_angle[1]=-M_PI/2 + (acos(acos_1) + atan2(r,v.z));
         b->leg[i].ik_angle[2]=acos(acos_2)-M_PI/2;
      }
   }
}