clear all
clc
format long g
set(gca,'FontSize',18)

r1 = 0.4;
r2 = 0.1;
r3 = 0.6; 
r4 = 0.5;

th1 = pi;
th2 = 0.01:(2*pi/100):2.01*pi;
omega2 = 60*2*pi/60;


%POSITION ANALYSIS
bx = r1*cos(th1+pi) + r2*cos(th2+pi);
by = r1*sin(th1+pi) + r2*sin(th2+pi);
b = sqrt(bx.^2+by.^2);

alphas = acos(abs(bx./b));
cosa = bx./b;
sina = by./b;

for i = 1:length(th2)
    
if cosa(i)>0 && sina(i)>=0
    alpha(i) = alphas(i);
end

if cosa(i)<0 && sina(i)>=0
    alpha(i) = pi-alphas(i);
end

if cosa(i)<0 && sina(i)<=0
    alpha(i) = pi+alphas(i);
end

if cosa(i)>0 && sina(i)<=0
    alpha(i) = 2*pi-alphas(i);
end

end

A = (b.^2-r3^2+r4^2)./(2*b*r4);
B = (b-r4*A)/r3;
C = sqrt(1-A.^2)*r4/r3;

th3s = alpha - acos(abs(B));
for i = 1:length(th2)
    
if B(i)>0 && C(i)>=0
    th3(i) = th3s(i);
end

if B(i)<0 && C(i)>=0
    th3(i) = pi-th3s(i);
end

if B(i)<0 && C(i)<=0
    th3(i) = pi+th3s(i);
end

if B(i)>0 && C(i)<=0
    th3(i) = 2*pi-th3s(i);
end

end

costh4 = (b/r4).*cos(alpha)-(r3/r4)*cos(th3);
sinth4 = (b/r4).*sin(alpha)-(r3/r4)*sin(th3);

th4 = atan2(sinth4,costh4);

for i = 1:length(th2)
    if th3(i)>0
        th3(i) = th3(i) - 2*pi;
    end
%     if th4(i)>0
%         th4(i) = th4(i) - 2*pi;
%     end
end

figure(1)
plot(th2*180/pi,th3*180/pi)
xlabel('\theta_{2} [deg]')
ylabel('\theta_{3} [deg]')
xlim([0 360])
grid on

figure(2)
plot(th2*180/pi,th4*180/pi)
xlabel('\theta_{2} [deg]')
ylabel('\theta_{4} [deg]')
xlim([0 360])
grid on


% VELOCITY ANALYSIS
omega3 = (r2*omega2*sin(th2-th4))./(r3*sin(th4-th3));
omega4 = (r2*omega2*sin(th2-th3))./(r4*sin(th3-th4));

figure(3)
plot(th2*180/pi,omega3)
xlabel('\theta_{2} [deg]')
ylabel('\Omega_{3} [rad/s]')
xlim([0 360])
grid on

figure(4)
plot(th2*180/pi,omega4)
xlabel('\theta_{2} [deg]')
ylabel('\Omega_{4} [rad/s]')
xlim([0 360])
grid on

%ACCELERATION ANALYSIS
a2 = 0;
a3 = (-r3*omega3.*(omega4-omega3).*cos(th4-th3)+r2*a2.*sin(th2-th4)+r2*omega2.*(omega2-omega4).*cos(th2-th4))./(r3*sin(th4-th3));
a4 = (-r4*omega4.*(omega3-omega4).*cos(th3-th4)+r2*a2.*sin(th2-th3)+r2*omega2.*(omega2-omega3).*cos(th2-th3))./(r3*sin(th3-th4));

r2tt_x = -r2*(omega2.^2).*cos(th2 + pi) + r2*a2.*cos(th2+pi/2);
r2tt_y = -r2*(omega2.^2).*sin(th2 + pi) + r2*a2.*sin(th2+pi/2);
r3tt_x = -r3*(omega3.^2).*cos(th3 + pi) + r3*a3.*cos(th3+pi/2);
r3tt_y = -r3*(omega3.^2).*sin(th3 + pi) + r3*a3.*sin(th3+pi/2);
r4tt_x = -r4*(omega4.^2).*cos(th4 + pi) + r4*a4.*cos(th4+pi/2);
r4tt_y = -r4*(omega4.^2).*sin(th4 + pi) + r4*a4.*sin(th4+pi/2);

rG2tt_ksi = (r2/2)*omega2.^2;
rG2tt_hta = (r2/2)*a2;
rG3tt_ksi = (r3/2)*omega3.^2;
rG3tt_hta = (r3/2)*a3;
rG4tt_ksi = (r4/2)*omega4.^2;
rG4tt_hta = (r4/2)*a4;

figure(5)
set(gca,'FontSize',18)
plot(th2*180/pi,a3)
xlabel('\theta_{2} [deg]')
ylabel('\alpha_{3} [rad/s^{2}]')
xlim([0 360])
grid on

figure(6)
set(gca,'FontSize',18)
plot(th2*180/pi,a4)
xlabel('\theta_{2} [deg]')
ylabel('\alpha_{4} [rad/s^{2}]')
xlim([0 360])
grid on

figure(61)
set(gca,'FontSize',18)
plot(th2*180/pi,r2tt_x,th2*180/pi,r2tt_y,'r')
xlabel('\theta_{2} [deg]')
ylabel('$\stackrel{..}{r}_{2,x}, \stackrel{..}{r}_{2,y} [m/s^{2}]$','interpreter','latex');
xlim([0 360])
grid on

figure(62)
set(gca,'FontSize',18)
plot(th2*180/pi,rG3tt_ksi,th2*180/pi,rG3tt_hta,'r')
xlabel('\theta_{2} [deg]')
ylabel('$\stackrel{..}{r}_{G3,\xi}, \stackrel{..}{r}_{G3,\eta} [m/s^{2}]$','interpreter','latex');
xlim([0 360])
grid on

figure(63)
set(gca,'FontSize',18)
plot(th2*180/pi,rG4tt_ksi,th2*180/pi,rG4tt_hta,'r')
xlabel('\theta_{2} [deg]')
ylabel('$\stackrel{..}{r}_{G4,\xi}, \stackrel{..}{r}_{G4,\eta} [m/s^{2}]$','interpreter','latex');
xlim([0 360])
grid on

figure(631)
plot(th2*180/pi,r4tt_x,th2*180/pi,r4tt_y,'r')
xlabel('\theta_{2} [deg]')
ylabel('$\stackrel{..}{r}_{4,x}, \stackrel{..}{r}_{4,y} [m/s^{2}]$','interpreter','latex');
xlim([0 360])
grid on

%FORCE ANALYSIS