function J = invposkin(T)
% INVPOSKIN Inverse kinematics for the first 3 joints of the A150 arm.
%
% J = INVPOSKIN(T) finds the first 3 joint angles given the pose
% of the end effector T = T05.
%
% If no solution exists, J is the empty matrix.



% find the wrist center
P = wristcenter0(T);

% theta1; there may be two possible values of theta1
theta1 = solvetheta1(P)

% theta3
[theta1, theta2, theta3] = solvetheta23(P, theta1);
if isempty(theta3)
    J = [];
    return;
end


J = [theta1(:) theta2(:) theta3(:)];

end


function theta1 = solvetheta1(P)
% theta1: the waist angle
% range: -175 to +175 degrees

% looking at the robot from the top down we see that
% theta1 is the angle between the base x-axis (frame 0) and vector
% from the base origin to [P(1); P(2); 0]

% but there is a potential second solution that is +/- 180 degrees
% from the first solution

sol1 = atan2(P(2), P(1)) * 180/pi;
sol2 = sol1 - 180;
if sol1 < 0
    sol2 = sol1 + 180;
end

theta1 = [sol1 sol2];

% remove solutions that are out of range
idx = find(abs(theta1) > 175);
theta1(idx) = [];

end


function [theta1, theta2, theta3] = solvetheta23(P, theta1)
% theta3: the elbow
% range: 0 to -125 degrees (measured relative to link 2)

n = length(theta1);
theta2 = zeros(1, n);
theta3 = zeros(1, n);

for i = 1:n
    t1 = theta1(i);
    % we can move P into the x-z plane by rotating P by Rz(-theta1)
    Q = rz(-t1) * P;
    
    % we can remove the offset from the base to the center of the
    % shoulder joint by subtracting this height from z
    Q(3) = Q(3) - 10;
    
    % now use the geometric solution for a 2-link planar robot to find
    % theta3
    x = Q(1);
    y = Q(3);   % ok, this is really z, but our 2-link solution was
                % for the x-y plane
    L2 = 10;
    L3 = 10;
    c3 = (x*x + y*y - L2*L2 - L3*L3) / (2*L2*L3);
    
    if abs(c3) > 1
        % no solution exists; use an out of range angle
        theta3(i) = -Inf;
        theta2(i) = Inf;
        continue;
    end
    theta3(i) = atan2(-sqrt(1 - c3*c3), c3) * 180/pi
    
    % because theta3 must negative (or zero), s3 must be negative (or zero)
    s3 = -sqrt(1 - c3*c3);
    theta2(i) = (atan2(y, x) - atan2(L3*s3, L2 + L3*c3)) * 180/pi;
end

% remove solutions that are out of range
idx = find(theta3 < -125);
theta3(idx) = [];
theta2(idx) = [];
theta1(idx) = [];

idx = find(theta2 < 0 | theta2 > 110);
theta2(idx) = [];
theta3(idx) = [];
theta1(idx) = [];

end




function R = rz(deg, isHomogeneous)
% RZ Rotation matrix for rotation about Z axis.
%
% R = RZ(deg) computes the 3x3 rotation matrix for rotation about the Z
% axis by an amount deg degrees.
%
% R = RZ(deg, true) computes the 4x4 homogenous rotation matrix.

if nargin == 1
    isHomogeneous = false;
end

a = deg*pi/180;
ca = cos(a);
sa = sin(a);
R = [ca -sa 0; sa ca 0; 0 0 1];

if isHomogeneous
    R = [R [0; 0; 0]; 0 0 0 1];
end

end

