// leg is the index of the current leg
// legpos[6][3] gives the root position of each leg in 3 dimensions, legpos[leg][*] picks a specific leg
// target is a float[3] giving absolute position for the foot
// return whether or not the solver was successful
bool hexapod::IKSolve (int leg, float *target)
{
  int iter;
  bool converged;
  float diff;
  float targetr, targetz, targetang;
  float fkpos[3], fkangles[3], J[2][2], inv[2][2], delta[2], p[2];
  float posr, posz, ang1, ang2, det;

  // convert absolute position to polar around leg root
  // in polar coordinates, one of the three dimensions is trivial to solve for
  targetz = target[2] - legpos[leg][2];
  targetr = sqrt(pow(target[0]-legpos[leg][0],2) + pow(target[1]-legpos[leg][1],2));
  targetang = atan2(target[1]-legpos[leg][1],target[0]-legpos[leg][0]) - legang[leg];

	// easy part: can the coxa servo not get to the correct angle?
  if (targetang > angleub[0] || targetang < anglelb[0]) return false;
  // else, go ahead and set coxa servo. One out of three angles done!
  angle[leg*3] = targetang;

  // begin 2-joint IK solver using jacobian pseudo-inverse
  // whenever we call FKSolve, need to convert to polar coords around leg root
  fkangles[0] = angle[leg*3]; // already solved for!
  fkangles[1] = angle[leg*3+1];
  fkangles[2] = angle[leg*3+2];
  // call forward kinematics on leg 'leg', servo angles 'fkangles', return result in 'fkpos'
  FKSolve(leg, fkangles, fkpos);
  // convert FK result to polar coordinates
  posz = fkpos[2] - legpos[leg][2];
  posr = sqrt(pow(fkpos[0]-legpos[leg][0],2) + pow(fkpos[1]-legpos[leg][1],2));

  // distance between target and current foot position
  diff = sqrt(pow(targetr-posr,2) + pow(targetz-posz,2));

  // ITERATE
  converged = false;
  // MAXITER is usually 10^2 to 10^3
  for (iter=0; iter<MAXITER && !converged; iter++)
  {
    // compute jacobian
    p[0] = targetr - posr;
    p[1] = targetz - posz;
    ang1 = fkangles[1]-femurangle;
    ang2 = fkangles[2]-tibiaangle;
    J[0][0] = -length[1]*sin(ang1) - length[2]*sin(ang1+ang2); // dr/dang1
    J[1][0] = -length[2]*sin(ang1+ang2); // dr/dang2
    J[0][1] = length[1]*cos(ang1) + length[2]*cos(ang1+ang2); // dz/dang2
    J[1][1] = length[2]*cos(ang1+ang2); // dz/dang2
    // compute inverse of jacobian
    det = 1.0/(J[0][0]*J[1][1]-J[0][1]*J[1][0]);
    inv[0][0] = J[1][1]*det;
    inv[1][0] = -J[1][0]*det;
    inv[0][1] = -J[0][1]*det;
    inv[1][1] = J[0][0]*det;
    // compute step for each angle
		delta[0] = p[0]*inv[0][0] + p[1]*inv[0][1];
		delta[1] = p[0]*inv[1][0] + p[1]*inv[1][1];
    // apply step to temporary angles
    fkangles[1] += delta[0]*0.5;
    fkangles[2] += delta[1]*0.5;
    // enforce bounds, ANGEPS is small, forces angle to stay within bounds, not just on
    if (fkangles[1] >= angleub[1]) 
      fkangles[1] = angleub[1]-ANGEPS;
    if (fkangles[1] <= anglelb[1]) 
      fkangles[1] = anglelb[1]+ANGEPS;
    if (fkangles[2] >= angleub[2]) 
      fkangles[2] = angleub[2]-ANGEPS;
    if (fkangles[2] <= anglelb[2]) 
      fkangles[2] = anglelb[2]+ANGEPS;
    // FK on new angles, see where the foot ends up
    FKSolve(leg, fkangles, fkpos);
    // convert to polar, once again
    posz = fkpos[2] - legpos[leg][2];
    posr = sqrt(pow(fkpos[0]-legpos[leg][0],2) + pow(fkpos[1]-legpos[leg][1],2));
    // how far away are we now?
    diff = sqrt(pow(targetr-posr,2) + pow(targetz-posz,2));
    // if we are close enough, we are done!
    if (diff < TOLERANCE) converged = true; // 1 mm tolerance, usually
  }

  if (converged)
  {
    // set angles for real
    angle[leg*3+1] = fkangles[1];
    angle[leg*3+2] = fkangles[2];
  }

  return converged;
}