function [x,y,a1,a2] = trajectory_draw

figure(1); clf
fprintf('Click to input trajectory points.\n')
fprintf('Click to the right of x=300 to quit entering points.\n')

L1 = 178;  % mm
L2 = 88; % mm
a1range = pi/2+[-90 90]*pi/180; % artificial "recommended" joint limits
a2range = [0 140]*pi/180; % joint limits for second link, wrt a1

xp = [0 L1*cos(a1range(1))]; % Extreme limit for "elbow" location
yp = [0 L1*sin(a1range(1))];
avals = a2range(1)*[1:-.01:0]; % with elbow fixed, move L2 to fully extended
xp = [xp(end)+L2*cos(a1range(1)+avals)];
yp = [yp(end)+L2*sin(a1range(1)+avals)];
avals = a1range(1)+(a1range(2)-a1range(1))*[0:.01:1]; % move only L1 now
xp = [xp, (L1+L2)*cos(avals)];
yp = [yp, (L1+L2)*sin(avals)];
avals = a2range(2)*[0:.01:1]; % elbow fixed, move L2 to fully bent
xp = [xp, L1*cos(a1range(2))+L2*cos(a1range(2)+avals)];
yp = [yp, L1*sin(a1range(2))+L2*sin(a1range(2)+avals)];
pt1 = [xp(end), yp(end)];
r1 = (pt1(1)^2 + pt1(2)^2)^.5;
a1 = atan2(pt1(2), pt1(1));
avals = (a1range(1)-a1range(2))*[0:.01:1];
xp = [xp, r1*cos(a1+avals)];
yp = [yp, r1*sin(a1+avals)];
avals = a2range(2) + (a2range(1)-a2range(2))*[0:.01:1];
xp = [xp, L1*cos(a1range(1))+L2*cos(avals)];
yp = [yp, L1*sin(a1range(1))+L2*sin(avals)];
%xp = [xp, L1*cos(a1range(1))+

%p1 = plot(xp,yp,'b'); axis image; hold on
%p2 = plot(-xp,yp,'r--'); 
p1 = plot(-xp,yp,'b--'); axis image; hold on
title('Click a set of points to define a trajecory in the end effector workspace')

A = axis;
A(2) = 350;
axis(A);
plot(300+[0 0],[A(3) A(4)],'m--')

gg = [];
dostop = false;
while ~dostop
    gnew = ginput(1);
    title('Click to the right of x=300 to quit entering points.')
    [a1,a2] = xy2ang(gnew(1),gnew(2));
    if gnew(1)>=300
        dostop = true;
    else
        gg = [gg; gnew];
        if length(gg(:,1))==1
            pg = plot(gg(1),gg(2),'r.');
            pg2 = plot(gg(1),gg(2),'k.');
            pax = [0 L1*cos(a1)+[0 L2*cos(a1+a2)]];
            pay = [0 L1*sin(a1)+[0 L2*sin(a1+a2)]];
            parm = plot(pax,pay,'k.-');
        else
            tint = 1:length(gg(:,1));
            t = 1:.05:length(gg(:,1));
            psx = spline(tint,gg(:,1));
            psy = spline(tint,gg(:,2));
            x = ppval(psx,t);
            y = ppval(psy,t);
            set(pg2,'XData',gg(:,1),'YData',gg(:,2));%,...
               % 'LineStyle','-','Marker','None','LineWidth',2);
            set(pg,'XData',x,'YData',y,...
                'LineStyle','-','Marker','None','LineWidth',2);
            pax = [0 L1*cos(a1)+[0 L2*cos(a1+a2)]];
            pay = [0 L1*sin(a1)+[0 L2*sin(a1+a2)]];
            set(parm,'XData',pax,'YData',pay)
            
        end
    end
end
[a1,a2] = xy2ang(x,y);

