% Position analysis
% R-RRT

clear; clc; close all;
% Input data 
AB = 0.5; % (m)
BC = 1;   % (m)
phi = pi/3; % (rad)

% position of joint A (origin)
xA = 0; yA = 0;

% position of joint B 
xB = AB*cos(phi);
yB = AB*sin(phi);

% position of joint C 
yC = 0;
% xC unknown
syms xCsol
% distance formula: BC=constant
eqnC = ( xB - xCsol )^2 + ( yB - yC )^2 - BC^2;
% solve equation eqnC
solC_ = solve(eqnC, xCsol);
% two solutions for xC 
% first  component of the vector solC
xC1 = eval(solC_(1));
%   -0.6514
% second component of the vector solC
xC2 = eval(solC_(2));
%    1.1514

% select the correct position for C 
% or the given input angle
if xC1 > xB xC = xC1; else xC = xC2; end 

% angle of the link 2 with the horizontal
phi2 = atan((yB-yC)/(xB-xC));

fprintf('Results \n\n') 
% Print the coordinates of B
fprintf('xB =  %6.3f (m)\n', xB)
fprintf('yB =  %6.3f (m)\n', yB)
% Print the coordinates of C
fprintf('xC =  %6.3f (m)\n', xC)
fprintf('yC =  %6.3f (m)\n', yC)
% Print the angle phi2
fprintf('phi2 = %6.3f (degrees) \n',180+phi2*180/pi);

% Graphic of the mechanism

plot([xA,xB],[yA,yB],'r-o','LineWidth',1.5);
hold on 
plot([xB,xC],[yB,yC],'b-o','LineWidth',1.5); 
hold on 
grid on
ad = 1.4;
axis([-ad ad -ad ad])

% plotting joint A 
pivot0_scale = 0.03;  
plot_pivot0( xA, yA, pivot0_scale, 0)
% plotting slider at C 
slider_scale = 0.15;
plot_slider( xC, yC, slider_scale, 0)

xlabel('x (m)') 
ylabel('y (m)')
text(xA,yA,'  A') 
text(xB,yB,'  B')
text(xC,yC,'     C')

% eof

% xB =   0.250 (m)
% yB =   0.433 (m)
% xC =   1.151 (m)
% yC =   0.000 (m)
% phi2 = 154.341 (degrees) 
