% [sol,near,far] = invkin(WrelB,current) is a function which computes the
% inverse kinematics for the Chapter 4 Programming Exercises.  It accepts
% the desired pose of the {WRIST} frame relative to the {BASE} frame,
% matrix WrelB, and also the "current" joint angles (radians).  If no solution
% exists, "sol" is returned as "0", and near and far are null vectors [].
% If two solutions exist, then "sol" is returned as "2" and the 
% nearest solution to the current position is returned as 3x1 vector "near."
% The second (farthest) solution is returned as 3x1 vector "far."  
% If only one solution exists then "sol" is returned as "1" and the one
% solution is returned in both "near" and "far."

function [sol,near,far] = invkin(WrelB,current)

%disp('At the start of invkin, current:');
%current

L1 = 0.5;   % Link 1 length in meters
L2 = 0.5;   % Link 2 length in meters

DEG = 180/pi;   % Conversion factors
RAD = pi/180;

theta = zeros(3,2); % Temporary storage for the two solutions

x = WrelB(1,3);     % Extract x from matrix WrelB
y = WrelB(2,3);     % Extract y from matrix WrelB

c2 = (x^2+y^2-L1^2-L2^2)/(2*L1*L2); % Compute cos(theta2)

if abs(c2)>1    % Check for solution existence; if cos(theta2)
    sol = 0;    % is greater in magnitude than 1, then
    near = [];  % no solution exists.  Return sol = 0
    far = [];   % and null matrices for both
    return;     % solutions.  Return since we
end;            % go no further.

s2p = sqrt(1-c2^2);     % Compute the "positive" value for sin(theta2),
s2n = -sqrt(1-c2^2);    % then find the "negative" value.

theta(2,1) = atan2(s2p,c2); % Compute both solutions
theta(2,2) = atan2(s2n,c2); % for theta2 and save.

k(1,1) = L1+L2*cos(theta(2,1)); % Compute the two constants
k(2,1) = L2*sin(theta(2,1));    % k1 and k2 for both values
k(1,2) = L1+L2*cos(theta(2,2)); % of theta2.
k(2,2) = L2*sin(theta(2,2));

theta(1,1) = unwrap(atan2(y,x)-atan2(k(2,1),k(1,1)));   % Compute both values
theta(1,2) = unwrap(atan2(y,x)-atan2(k(2,2),k(1,2)));   % for theta1.

theta123 = atan2(WrelB(2,1),WrelB(1,1));    % Find theta123

theta(3,1) = unwrap(theta123-theta(1,1)-theta(2,1));    % Finally get both
theta(3,2) = unwrap(theta123-theta(1,2)-theta(2,2));    % values for theta3.

% Next check the joint angles against the +/- 170 degree limits.

soln1 = 1;  % Initialize solution "flags" for
soln2 = 1;  % both solutions.

for i = 1:3                        % First check solution "1"
    if abs(theta(i,1))>170*RAD  % If any of the angles
        soln1 = 0;              % in the "first" solution
%        disp('Positive limit violation!');
        break;                  % are outside the limits,
    end;                        % set a flag indicating that,
end;                            % and go on to second solution.

for i = 1:3                     % Next check solution "2"
    if abs(theta(i,2))>170*RAD  % If any of the angles
        soln2 = 0;              % in the "second" solution
%        disp('Negative limit violation!');
        break;                  % are outside the limits,
    end;                        % set a flag indicating that,
end;                            % then check for "near/far"

if (soln1 == 0) && (soln2 == 0)    % If both solutions outside joint limits,
    sol = 0;                % set sol = 0 and return.
    near = [];
    far = [];
    return;
end;

if (soln1 && soln2) == 1        % If both solutions within joint limits,
    diff = current-theta(:,1);  % find joint travel for solution 1,
    travel(1) = diff'*diff;     % using sum-of-squares.
    diff = current-theta(:,2);  % doing the same thing
    travel(2) = diff'*diff;     % for solution 2.
    if travel(1)<travel(2)      % Find out which solution is
        near = theta(:,1);      % nearest, and assign results
        far = theta(:,2);       % accordingly.
    else
        near = theta(:,2);
        far = theta(:,1);
    end;
    sol = 2;                    % Return flag indicating two solutions.
    return;
end;

if (soln1 | soln2) == 1         % If only one solution is within joint limits,
    if soln1                    % find out which one, and return both
        near = theta(:,1);      % "near" and "far" as that one solution.
        far = theta(:,1);
    else
        near = theta(:,2);
        far = theta(:,2);
    end;
    sol = 1;                    % Return flag indicating one solution.
    return;
end;