function workspaceTrajectory_2Darm()
clc
close all

% Refer to RoboticsAndMechatronics_IndustrialRobotics_part3_2021 slide 33

% Fixed parameters
% =========================================================================
l1 = .2;
l2 = .4;
l3 = .3;
l4 = .2;
PE_start = [0.1, 0.3, -pi/4];
PE_end   = [0.9, 0.0, 0];

% Trajectory
% =========================================================================
% Here we assume a linear trajectory in workspace, with a pose orientation
% which changes smoothly between start- and end-orientations.
nSteps = 100; % Simulation steps
PE_trajectory(1,:) = linspace(PE_start(1), PE_end(1), nSteps);
PE_trajectory(2,:) = linspace(PE_start(2), PE_end(2), nSteps);
PE_trajectory(3,:) = linspace(PE_start(3), PE_end(3), nSteps);

% Base point
P0 = [0;0];

% Initialize arrays for speed of execution
P1_trajectory = zeros(2,nSteps);
P2_trajectory = zeros(2,nSteps);
P3_trajectory = zeros(2,nSteps);
P4_trajectory = zeros(2,nSteps);
phi1_array = zeros(1,nSteps);
phi2_array = zeros(1,nSteps);
for iStep = 1:nSteps
    % Perform analytical inverse kinematics
    % =====================================================================
    % Here we can compute the location of the known joints (P0, PE, P4 and
    % P2)
    PE = PE_trajectory(1:2,iStep);  % Pose [position]
    theta = PE_trajectory(3,iStep); % Pose [orientation]
    P1 = P0 + [l1 ; 0];
    P3 = PE - l4*[cos(theta) ; sin(theta)];
    Lvect = P3-P1;
    L = norm(Lvect);
    Lx = Lvect(1);
    Ly = Lvect(2);
    % At this point we can use the cosine theorem to find one of the
    % possible solutions of the planar part of the robot.
    tau = acos( (l2^2 + l3^2 - L^2) / (2*l2*l3) );
    phi2 = - ( pi - tau );
    xi = atan2(Ly,Lx);
    zeta = acos( (l2^2 + L^2 - l3^2) / (2*l2*L) );
    phi1 = xi + zeta;
    phi3 = theta - phi1 - phi2;

    % Calculate coordinates of points
    P2 = P1 + l2*[cos(phi1) ; sin(phi1)];
    
    % Save in arrays
    P1_trajectory(:,iStep) = P1;
    P2_trajectory(:,iStep) = P2;
    P3_trajectory(:,iStep) = P3;
    P4_trajectory(:,iStep) = PE;
    phi1_array(iStep) = phi1;
    phi2_array(iStep) = phi2;

    % Concatenate points for display purposes
    points = [P0 P1 P2 P3 PE];
    % Plot the configuration of the robot
    hold off
    plot(points(1,:),points(2,:),'-o')
    hold on;
    plot(P1_trajectory(1,1:iStep),P1_trajectory(2,1:iStep))
    plot(P2_trajectory(1,1:iStep),P2_trajectory(2,1:iStep))
    plot(P3_trajectory(1,1:iStep),P3_trajectory(2,1:iStep))
    plot(PE_trajectory(1,1:iStep),PE_trajectory(2,1:iStep))
    %hold on
    daspect([1 1 1]);
    grid on
    xlim([-.3 1.2])
    ylim([-.3 1.2])
    xlabel('X axis')
    ylabel('Y axis')
    drawnow
end

% Plot joint variables
figure
plot(phi1_array)
hold on
plot(phi2_array)
xlabel('Steps')
ylabel('Joint angles [rad]')
legend('phi1','phi2')