clear;
clc;
close all;

%% DADOS DO MECANISMO

L1 = 60;      % mm
L2 = 20;      % mm
L3 = 70;      % mm
L4 = 90;      % mm

RPA = 60;     % mm
delta3 = deg2rad(30);

% velocidade angular da manivela
omega2 = 60*(2*pi/60);   % rad/s

% velocidade constante
alpha2 = 0;

%% DISCRETIZAÇÃO DA ROTAÇÃO COMPLETA

N = 36001;

theta2 = linspace(0,2*pi,N);

alpha3 = zeros(1,N);
alpha4 = zeros(1,N);

omega3 = zeros(1,N);
omega4 = zeros(1,N);

aPx = zeros(1,N);
aPy = zeros(1,N);
aP  = zeros(1,N);

theta3 = zeros(1,N);
theta4 = zeros(1,N);

%% MATRIZ PARA PRODUTO k x r
% Se r = [x;y]:
% k x r = [-y;x]

J = [0 -1;
     1  0];

O2 = [0;0];
O4 = [L1;0];

%% LOOP EM UMA VOLTA COMPLETA

for n = 1:N

    %% POSIÇÃO DO PONTO A

    r2 = L2*[cos(theta2(n));
             sin(theta2(n))];

    A = r2;

    %% ENCONTRAR PONTO B
    % Interseção:
    % círculo centrado em A com raio L3
    % círculo centrado em O4 com raio L4

    dvec = O4 - A;
    d = norm(dvec);

    aa = (L3^2 - L4^2 + d^2)/(2*d);

    h = sqrt(L3^2 - aa^2);

    P0 = A + aa*dvec/d;

    % vetor perpendicular unitário
    perp = [-dvec(2);
             dvec(1)]/d;

    % configuração aberta/superior
    B = P0 + h*perp;

    %% VETORES DOS ELOS

    r3 = B - A;
    r4 = B - O4;

    theta3(n) = atan2(r3(2),r3(1));
    theta4(n) = atan2(r4(2),r4(1));

    %% -------------------------
    % ANÁLISE DE VELOCIDADES
    % --------------------------

    % omega2*J*r2 + omega3*J*r3
    % = omega4*J*r4

    M = [J*r3, -J*r4];

    bvel = -omega2*J*r2;

    omega = M\bvel;

    omega3(n) = omega(1);
    omega4(n) = omega(2);

    %% -------------------------
    % ANÁLISE DE ACELERAÇÕES
    % --------------------------

    % alpha3*J*r3 - alpha4*J*r4 =
    %
    % omega2^2*r2
    % + omega3^2*r3
    % - omega4^2*r4

    bacel = ...
        omega2^2*r2 ...
        + omega3(n)^2*r3 ...
        - omega4(n)^2*r4;

    alfa = M\bacel;

    alpha3(n) = alfa(1);
    alpha4(n) = alfa(2);

    %% -------------------------
    % ACELERAÇÃO DO PONTO P
    % --------------------------

    % vetor A -> P
    thetaPA = theta3(n) + delta3;

    rPA = RPA*[cos(thetaPA);
               sin(thetaPA)];

    % aceleração de A
    aA = alpha2*J*r2 - omega2^2*r2;

    % aceleração de P
    aPvec = ...
        aA ...
        + alpha3(n)*J*rPA ...
        - omega3(n)^2*rPA;

    aPx(n) = aPvec(1);
    aPy(n) = aPvec(2);

    aP(n) = norm(aPvec);

end

%% CONVERSÃO DE THETA2 PARA GRAUS

theta2graus = rad2deg(theta2);

%% MÁXIMOS

[maxAlpha3, i3] = max(abs(alpha3));
[maxAlpha4, i4] = max(abs(alpha4));
[maxAP, iP] = max(aP);

fprintf('\n====================================\n');
fprintf('RESULTADOS MÁXIMOS\n');
fprintf('====================================\n');

fprintf('\n|alpha3|max = %.6f rad/s^2\n',maxAlpha3);
fprintf('theta2 = %.3f graus\n',theta2graus(i3));

fprintf('\n|alpha4|max = %.6f rad/s^2\n',maxAlpha4);
fprintf('theta2 = %.3f graus\n',theta2graus(i4));

fprintf('\n|aP|max = %.6f mm/s^2\n',maxAP);
fprintf('|aP|max = %.6f m/s^2\n',maxAP/1000);
fprintf('theta2 = %.3f graus\n',theta2graus(iP));

%% MOSTRAR TAMBÉM MÍNIMOS E MÁXIMOS COM SINAL

fprintf('\n------------------------------------\n');

fprintf('alpha3 minimo = %.6f rad/s^2\n',min(alpha3));
fprintf('alpha3 maximo = %.6f rad/s^2\n',max(alpha3));

fprintf('\nalpha4 minimo = %.6f rad/s^2\n',min(alpha4));
fprintf('alpha4 maximo = %.6f rad/s^2\n',max(alpha4));

%% GRÁFICO ALPHA 3

figure;
plot(theta2graus,alpha3,'LineWidth',1.5);
grid on;
xlabel('\theta_2 (graus)');
ylabel('\alpha_3 (rad/s^2)');
title('Aceleração angular do elo 3');

%% GRÁFICO ALPHA 4

figure;
plot(theta2graus,alpha4,'LineWidth',1.5);
grid on;
xlabel('\theta_2 (graus)');
ylabel('\alpha_4 (rad/s^2)');
title('Aceleração angular do elo 4');

%% GRÁFICO DA ACELERAÇÃO DE P

figure;
plot(theta2graus,aP/1000,'LineWidth',1.5);
grid on;
xlabel('\theta_2 (graus)');
ylabel('|a_P| (m/s^2)');
title('Módulo da aceleração do ponto P');