% Program 2.3
% R-RTR-RTR 
% Position analysis - input angle phi

clear; clc; close all

% Input data 
AB = 0.14 ; AC = 0.06 ; AE = 0.25 ; CD = 0.15 ;  %(m) 
phi = pi/6 ;  % (rad)
% Select the dimensions
DF=0.4 ; EG=0.5 ; % (m) 

xA = 0 ; yA = 0   ; rA = [xA yA 0] ; % Position of A  
xC = 0 ; yC = AC  ; rC = [xC yC 0] ; % Position of C  
xE = 0 ; yE = -AE ; rE = [xE yE 0] ; % Position of E 
xB = AB*cos(phi) ; yB = AB*sin(phi) ; rB = [xB yB 0] ; % Position of B 

% Position of joint D 
% Distance formula: CD=constant
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');
% solve symbolic solution of algebraic equations
% Two solutions for xD - vector form
xDpositions = eval(solD.xDsol);
% eval execute string as an expression or statement
% Two solutions for yD - vector form
yDpositions = eval(solD.yDsol);
% Separate the solutions in scalar form
xD1 = xDpositions(1); % first  component of the vector xDpositions
xD2 = xDpositions(2); % second component of the vector xDpositions
yD1 = yDpositions(1); % first  component of the vector yDpositions
yD2 = yDpositions(2); % second component of the vector yDpositions

% Select the correct position for D for the given input angle
if xD1 <= xC
    xD = xD1; yD=yD1;
else
    xD = xD2; yD=yD2;
end
rD = [xD yD 0]; % Position of D 

% Angles of the links with the horizontal
phi2 = atan((yB-yC)/(xB-xC));
phi3 = phi2;
phi4 = atan((yD-yE)/(xD-xE))+pi;
phi5 = phi4;

% Positions of the points F and G

xF = xD + DF*cos(phi3) ; 
yF = yD + DF*sin(phi3) ; 
rF = [xF yF 0]; % Position vector of F 

xG = xE + EG*cos(phi5) ; 
yG = yE + EG*sin(phi5) ;
rG = [xG yG 0]; % Position vector of G 




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('rB =  [ %g, %g, %g] (m)\n', rB);
fprintf('rD =  [ %g, %g, %g] (m)\n', rD);
fprintf('phi2 = phi3 = %g (degrees) \n', phi2*180/pi);
fprintf('phi4 = phi5 = %g (degrees) \n', phi4*180/pi);
fprintf('rF =  [ %g, %g, %g] (m)\n', rF);
fprintf('rG =  [ %g, %g, %g] (m)\n', rG);

% Graphic of the mechanism
plot([xA,xB],[yA,yB],'r-o','LineWidth',1.5) 
hold on % holds the current plot
plot([xD,xC],[yD,yC],'b-o','LineWidth',1.5)
hold on 
plot([xC,xB],[yC,yB],'b-o','LineWidth',1.5)
hold on 
plot([xB,xF],[yB,yF],'b-o','LineWidth',1.5)
hold on 
plot([xE,xD],[yE,yD],'g-o','LineWidth',1.5)
hold on 
plot([xD,xG],[yD,yG],'g-o','LineWidth',1.5)
grid on,...
xlabel('x (m)'), ylabel('y (m)'),...
title('positions for \phi = 30 (deg)'),...
text(xA,yA,'\leftarrow A = ground','HorizontalAlignment','left'),...
text(xB,yB,'  B'),...
text(xC,yC,'\leftarrow C = ground','HorizontalAlignment','left'),...
text(xD,yD,'  D'),...
text(xE,yE,'\leftarrow E = ground','HorizontalAlignment','left'),...
text(xF,yF,'  F'), text(xG,yG,'  G')