Cleaned up IK solve code. Easier to read now.