% Program 2.3
% R-RTR-RTR 
% Position analysis - complete rotation
% Euclidian distance function 
% the program use the function: Dist(x1,y1,x2,y2) 
% the function is defined in the program Dist.m
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');

increment = 0 ; % at the initial moment phi=0 => increment = 0

step=pi/6; % the step has to be small for this method
for phi=0:step:2*pi,
fprintf('phi =  %g deegres \n', phi*180/pi);

% Position of joint B 
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 ';
eqnD2 = '( yB - yC ) / ( xB - xC ) = ( yDsol - yC ) / ( xDsol - xC )';
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 only for increment == 0
% the selection process is automatic for all the other steps

if increment == 0
   if xD1 <= xC xD = xD1; yD=yD1; else xD = xD2; yD=yD2; end
else
  dist1 = Dist(xD1,yD1,xDold,yDold);
  dist2 = Dist(xD2,yD2,xDold,yDold);
   if dist1 < dist2 xD = xD1; yD=yD1; else xD = xD2; yD=yD2; end
end
xDold=xD;
yDold=yD;

increment=increment+1;

rD = [xD yD 0];
fprintf('rD =  [ %g, %g, %g] (m)\n', rD);

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 30 (deg)'),...
text(xA,yA,'  A'),...
text(xB,yB,'  B'),...
text(xC,yC,'  C'),...
text(xD,yD,'  D'),...
text(xE,yE,'  E')

end