#include "ikLib.h"

/*
length units are mm
angle units are degrees

len   = dist3d(dist p1x, dist p1y, dist p1z, dist p2x, dist p2y, dist p2z);
angle = elbow(dist d, dist l1, dist l2);
angle = dir(dist x, dist y);
 
 
 <wildmage>  theta = acos(-vec1[0]);
<wildmage>   if ( asin(vec1[1]) > 0.0 ) 
<wildmage>    theta = -theta;
<wildmage> after you normalize the vector first
<wildmage> the first negative is not necessary
<wildmage> angle = acos(x)
<wildmage> if ( asin(y) > 0 ):
<wildmage>    angle = -angle
<wildmage> that's it
<wildmage> x,y needs to be normalized
<wildmage> mag = sqrt(x^2 + y^2)
<wildmage> x = x/mag
<wildmage> y = y/mag
 
*/

dist dist3d(dist p1x, dist p1y, dist p1z, dist p2x, dist p2y, dist p2z){
  return sqrt(sqr(p1x-p2x) + sqr(p1y-p2y) + sqr(p1z-p2z));
}

angle elbow(dist C, dist A, dist B){ // returns angle c
  if (C > A+B) return 0;
  return  fromRads(  acos(((double)sqr(A)+(double)sqr(B)-(double)sqr(C)) / ((double)2*(double)A*(double)B))  );
}

angle dir(dist x, dist y){
 // printf("%d, %d\n", x, y);
  return fromRads(atan2(y, x));
}