% Program 2.2
% R-RRR-RRT
% Position analysis

clear; clc; close all 

% Input data 
AB=0.15; BC=0.40; CD=0.37; CE=0.23; EF=CE;
La=0.30; Lb=0.45; Lc=CD;	
phi = pi/4 ;

xA = 0; yA = 0; rA = [xA yA 0]; 
xD = La ; yD = Lb ; rD = [xD yD 0]; 
xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0]; 

% Position of joint C 
% Distance formula: BC=constant
eqnC1 = '( xCsol - xB )^2 + ( yCsol - yB )^2 = BC^2 ';
% Distance formula: CD=constant
eqnC2 = '( xCsol - xD )^2 + ( yCsol - yD )^2 = CD^2 ';

% Simultaneously solve above equations
solC = solve(eqnC1, eqnC2, 'xCsol, yCsol');
% Two solutions for xC - vector form
xCpositions = eval(solC.xCsol);
% Two solutions for yC - vector form
yCpositions = eval(solC.yCsol);
% Separate the solutions in scalar form
xC1 = xCpositions(1); % first  component of the vector xCpositions
xC2 = xCpositions(2); % second component of the vector xCpositions
yC1 = yCpositions(1); % first  component of the vector yCpositions
yC2 = yCpositions(2); % second component of the vector yCpositions

% Select the correct position for C for the given input angle
% see the drawings for each quadrant

if xC1 < xD
    xC = xC1; yC=yC1;
else
    xC = xC2; yC=yC2;
end
rC = [xC yC 0]; % Position vector of C 

% Position of joint E 
% Distance formula: CE=constant
eqnE1 = '( xEsol - xC )^2 + ( yEsol - yC )^2 = CE^2 ';
% Slope formula: E, C, and D are on the  same straight line 
eqnE2 = '( yD - yC ) / ( xD - xC ) = ( yEsol - yC ) / ( xEsol - xC )';
solE = solve(eqnE1, eqnE2, 'xEsol, yEsol');
xEpositions = eval(solE.xEsol); yEpositions = eval(solE.yEsol);
xE1 = xEpositions(1); xE2 = xEpositions(2); 
yE1 = yEpositions(1); yE2 = yEpositions(2); 

if xE1 < xC
    xE = xE1; yE=yE1;
else
    xE = xE2; yE=yE2;
end
rE = [xE yE 0]; % Position vector of E 


% Position of joint F 
xF = - Lc ;
% Distance formula: EF=constant
eqnF = '( xF - xE )^2 + ( yFsol - yE )^2 = EF^2 ';
solF = solve(eqnF,'yFsol');
yFpositions=eval(solF);
yF1 = yFpositions(1); yF2 = yFpositions(2); 

if yF1 < yE
     yF=yF1;
else
     yF=yF2;
end
rF = [xF yF 0]; 

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

fprintf('Results \n'); fprintf('\n');

fprintf('rA =  [ %g, %g, %g] (m)\n', rA);
fprintf('rD =  [ %g, %g, %g] (m)\n', rD);
fprintf('rB =  [ %g, %g, %g] (m)\n', rB);
fprintf('rC =  [ %g, %g, %g] (m)\n', rC);
fprintf('rE =  [ %g, %g, %g] (m)\n', rE);
fprintf('rF =  [ %g, %g, %g] (m)\n', rF);
fprintf('phi2 = %g (degrees) \n', phi2*180/pi);
fprintf('phi3 = %g (degrees) \n', phi3*180/pi);
fprintf('phi4 = %g (degrees) \n', phi4*180/pi);

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