% Program 2.4
% R-RTR-RTR 
% Position analysis - complete rotation
clear; clc; close all

% Input data 
AB=0.14; AC=0.06; AE=0.25; CD=0.15; %(m) 
xA = 0; yA = 0; rA = [xA yA 0]; % Position vector of A 
xC = 0 ; yC = AC ; rC = [xC yC 0]; % Position vector of C 
xE = 0 ; yE = -AE ; rE = [xE yE 0]; % Position vector of E 

fprintf('Results \n') ; fprintf('\n');
fprintf('rA =  [ %g, %g, %g] (m)\n', rA);
fprintf('rC =  [ %g, %g, %g] (m)\n', rC);
fprintf('rE =  [ %g, %g, %g] (m)\n', rE); fprintf('\n');

% complete rotation phi=0 to 2*pi step pi/3
for phi=0:pi/3:2*pi,
% for    repeat statements a specific number of times
     
fprintf('phi =  %g deegres \n', phi*180/pi);

% Position of joint B - position of the driver link
xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0]; 
fprintf('rB =  [ %g, %g, %g] (m)\n', rB);

% Position of joint D 
eqnD1 = '( xDsol - xC )^2 + ( yDsol - yC )^2 = CD^2 ';
% Slope formula: B, C, and D are on the same straight line 
eqnD2 = '( yB - yC ) / ( xB - xC ) = ( yDsol - yC ) / ( xDsol - xC )';
% Simultaneously solve above equations
solD = solve(eqnD1, eqnD2, 'xDsol, yDsol');
xDpositions = eval(solD.xDsol);
yDpositions = eval(solD.yDsol);
% Separate the solutions in scalar form
xD1 = xDpositions(1); xD2 = xDpositions(2);
yD1 = yDpositions(1); yD2 = yDpositions(2);

% Select the correct position for D for the angle phi
% see the drawings for each quadrant

if (phi>=0 && phi<=pi/2)||(phi >= 3*pi/2 && phi<=2*pi)
   if xD1 <= xC xD = xD1; yD=yD1; else xD = xD2; yD=yD2; end
   else
   if xD1 >= xC xD = xD1; yD=yD1; else xD = xD2; yD=yD2; end
end
% &&  short-circuit logical AND           
% ||  short-circuit logical OR          
             
rD = [xD yD 0]; 
fprintf('rD =  [ %g, %g, %g] (m)\n', rD);

% Angles of the links with the horizontal
phi2 = atan((yB-yC)/(xB-xC));
phi3 = phi2;
fprintf('phi2 = phi3 = %g (degrees) \n', phi2*180/pi);
phi4 = atan((yD-yE)/(xD-xE));
phi5 = phi4;
fprintf('phi4 = phi5 = %g (degrees) \n', phi4*180/pi);
fprintf('\n');

% Graphic of the mechanism

plot([xA,xB],[yA,yB],'r-o',[xB,xC],[yB,yC],'b-o',[xC,xD],[yC,yD],'b-o')
hold on
plot([xD,xE],[yD,yE],'g-o') 
xlabel('x (m)'),...
ylabel('y (m)'),...
title('positions for \phi = 0 to 360 step 60 (deg)'),...
text(xA,yA,'  A'),...
text(xB,yB,'  B'),...
text(xC,yC,'  C'),...
text(xD,yD,'  D'),...
text(xE,yE,'  E')

end % end for