% the following code can be appended to run_Newton.m for plotting:
% please fill in the five "%TO_BE_COMPLETED%" entries with the appropriate expressions

figure(1), clf;
hold on;
%plot home in blue
Xi = robot_f(Qvec_expanded(:,1),[0;0]);
htraj_home = plot(Xvec_expanded(1,1),Xvec_expanded(2,1),'bo','linewidth',2);
harm_home = plot([0 L1*cos(Qvec_expanded(1,1)),Xi(1)],[0 L1*sin(Qvec_expanded(1,1)),Xi(2)],'b','linewidth',2);
axis equal;
for i = 2:size(Xvec_expanded,2)
    Xi = robot_f(Qvec_expanded(:,i),[0;0]);
    if (%TO_BE_COMPLETED%) % i_th point in Xvec_expanded is original trajectory point)
        %plot trajectory point (red)
        htraj_orig = plot(%TO_BE_COMPLETED%);
        %plot arm (red)
        harm_orig = plot(%TO_BE_COMPLETED%);
    else % i_th point in Xvec_expanded is expanded trajectory point)
        %plot expanded trajectory point (black)
        htraj_exp = plot(%TO_BE_COMPLETED%);
        %plot arm (black, dashed)
        harm_exp = plot(%TO_BE_COMPLETED%);
    end
end
legend([htraj_home; harm_home; htraj_orig; harm_orig; htraj_exp; harm_exp], ...
    'home', 'home solution', 'original trajectory',...
'original trajectory solution', 'inserted trajectory',...
    'inserted trajectory solution', 'Location', 'Best');

xlabel('$X_1$ (inches)','interpreter','latex','fontsize',14);
ylabel('$X_2$ (inches)','interpreter','latex','fontsize',14);
axis([-6 2 0 7]);
hold off;