%% Slider-Crank Kinematic Analysis using Vector Loop
% -------------------------------------------------------------------------
% This script computes position, velocity, and acceleration of a slider-crank
% mechanism (crank-slider) using the vector-loop method.
%
% Units:
%   Lengths: mm
%   Angles:  rad (internally), degrees for display
%   Speed:   rad/s
%   Accel.:  rad/s^2
% -------------------------------------------------------------------------

clear; clc; close all;

%% 1) Given data -----------------------------------------------------------
% Digits from the student ID-like code (A,B,G,D)
A = 2;
B = 2;
G = 2;
D = 2;

% Concatenate digits as requested by the assignment convention
AB   = 10*A + B;     % e.g., A=2, B=2 -> AB = 22
GD   = 10*G + D;     % e.g., G=2, D=2 -> GD = 22

% Link dimensions from the handout
r2 = 40  + 0.1*AB;   % Crank length AB [mm]
r3 = 120 + 0.2*GD;   % Connecting rod length BC [mm]

% Input crank motion
omega2 = 1;       % [rad/s]
alpha2 = 0;      % [rad/s^2] Na to valete OLOI KAI OLES MIDEN

% Crank angle sweep for one full turn
N = 361;                          % Number of sample points
theta2 = linspace(0, 2*pi, N);    % [rad]

%% 2) Preallocate arrays ---------------------------------------------------
% Angular quantities
theta3 = zeros(1, N);
omega3 = zeros(1, N);
alpha3 = zeros(1, N);

% Slider kinematics
s    = zeros(1, N);
sdot = zeros(1, N);
sdd  = zeros(1, N);

% Point B (crank pin)
xB = zeros(1, N);
yB = zeros(1, N);
vBx = zeros(1, N);
vBy = zeros(1, N);
aBx = zeros(1, N);
aBy = zeros(1, N);

% Point C (slider pin)
xC = zeros(1, N);
yC = zeros(1, N);
vCx = zeros(1, N);
vCy = zeros(1, N);
aCx = zeros(1, N);
aCy = zeros(1, N);

% Midpoint kinematics of link 2
xG2 = zeros(1, N);
yG2 = zeros(1, N);
vG2x = zeros(1, N);
vG2y = zeros(1, N);
aG2x = zeros(1, N);
aG2y = zeros(1, N);

% Midpoint kinematics of link 3
xG3 = zeros(1, N);
yG3 = zeros(1, N);
vG3x = zeros(1, N);
vG3y = zeros(1, N);
aG3x = zeros(1, N);
aG3y = zeros(1, N);



%% 3) Main loop ------------------------------------------------------------
% Oi exiswseis thesis einai:
%   r2*cos(theta2) + r3*cos(theta3) = s
%   r2*sin(theta2) + r3*sin(theta3) = 0
%

for k = 1:N
    th2 = theta2(1,k);

    % --- Position analysis -----------------------------------------------
    % From the y-closure equation:
    %   sin(theta3) = -(r2/r3)*sin(theta2)
    sin_th3 = -(r2/r3)*sin(th2);
    th3 = asin(sin_th3);
    cos_th3 = cos(th3);

    % Slider position from x-closure
    sk = r2*cos(th2) + r3*cos_th3;

    % --- Velocity analysis -----------------------------------------------
    % Oi exiswseis taxytitas einai:
    %   -r2*omega2*sin(theta2) - r3*omega3*sin(theta3) = sdot
    %    r2*omega2*cos(theta2) + r3*omega3*cos(theta3) = 0
    omega3k = -(r2*omega2*cos(th2)) / (r3*cos_th3);

    % Slider speed
    sdotk = -r2*omega2*sin(th2) - r3*omega3k*sin(th3);

    % --- Acceleration analysis -------------------------------------------
    % Oi exiswseis epitaxynsis ston y einai:
    %   r2*alpha2*cos(theta2) - r2*omega2^2*sin(theta2)
    % + r3*alpha3*cos(theta3) - r3*omega3^2*sin(theta3) = 0
    alpha3k = (-r2*alpha2*cos(th2) + r2*omega2^2*sin(th2) + r3*omega3k^2*sin(th3)) / (r3*cos_th3);

    % Oi exiswseis epitaxynsis ston x einai:
    %   -r2*alpha2*sin(theta2) - r2*omega2^2*cos(theta2)
    %   -r3*alpha3*sin(theta3) - r3*omega3^2*cos(theta3) = sdd
    sddk = -r2*alpha2*sin(th2) - r2*omega2^2*cos(th2) - r3*alpha3k*sin(th3) - r3*omega3k^2*cos_th3;

    % --- Point B kinematics ----------------------------------------------
    % Position of B
    xBk = r2*cos(th2);
    yBk = r2*sin(th2);

    % Velocity of B
    vBxk = -r2*omega2*sin(th2);
    vByk =  r2*omega2*cos(th2);

    % Acceleration of B
    aBxk = -r2*alpha2*sin(th2) - r2*omega2^2*cos(th2);
    aByk =  r2*alpha2*cos(th2) - r2*omega2^2*sin(th2);

    % --- Point C kinematics ----------------------------------------------
    % Slider point C is constrained to the x-axis
    xCk = sk;
    yCk = 0;

    % Velocity of C
    vCxk = sdotk;
    vCyk = 0;

    % Acceleration of C
    aCxk = sddk;
    aCyk = 0;

    % --- Store results ----------------------------------------------------
    theta3(1,k) = th3;
    omega3(1,k) = omega3k;
    alpha3(1,k) = alpha3k;
    s(1,k)      = sk;
    sdot(1,k)   = sdotk;
    sdd(1,k)    = sddk;

    xB(1,k) = xBk;  yB(1,k) = yBk;
    vBx(1,k)= vBxk; vBy(1,k)= vByk;
    aBx(1,k)= aBxk; aBy(1,k)= aByk;

    xC(1,k) = xCk;  yC(1,k) = yCk;
    vCx(1,k)= vCxk; vCy(1,k)= vCyk;
    aCx(1,k)= aCxk; aCy(1,k)= aCyk;
end

%% 4) Metatropi gwnivn se moires (gia kaliteri epeikonisi) ------------------------------------------
theta2_deg = rad2deg(theta2);
theta3_deg = rad2deg(theta3);

%% 5) Dimiourgia pinaka apotelesmatwn ----------------------------------------------
results = table(...
    theta2_deg(:), theta3_deg(:), s(:), sdot(:), sdd(:), ...
    omega3(:), alpha3(:), ...
    xB(:), yB(:), vBx(:), vBy(:), aBx(:), aBy(:), ...
    xC(:), yC(:), vCx(:), vCy(:), aCx(:), aCy(:), ...
    'VariableNames', { ...
        'theta2_deg','theta3_deg','s_mm','sdot_mm_s','sdd_mm_s2', ...
        'omega3_rad_s','alpha3_rad_s2', ...
        'xB_mm','yB_mm','vBx_mm_s','vBy_mm_s','aBx_mm_s2','aBy_mm_s2', ...
        'xC_mm','yC_mm','vCx_mm_s','vCy_mm_s','aCx_mm_s2','aCy_mm_s2'});

%% 6) Anafora dedomenwn provlimatos ---------------------------------------------
fprintf('Slider-crank kinematic analysis\n');
fprintf('--------------------------------\n');
fprintf('Crank length r2 = %.3f mm\n', r2);
fprintf('Rod length   r3 = %.3f mm\n', r3);
fprintf('Input omega2  = %.6f rad/s\n', omega2);
fprintf('Input alpha2  = %.6f rad/s^2\n', alpha2);
fprintf('Results table created with %d samples.\n\n', height(results));

disp(results(1:8,:));   % show first rows as a preview

%% 8) Grafimata ----------------------------------------------------------------
% 8.1 Slider position vs crank angle
figure;
plot(theta2_deg, s, 'LineWidth', 1.5);
grid on;
xlabel('\theta_2 [deg]');
ylabel('s [mm]');
title('Slider Position vs Crank Angle');

% 8.2 Slider velocity vs crank angle
figure;
plot(theta2_deg, sdot, 'LineWidth', 1.5);
grid on;
xlabel('\theta_2 [deg]');
ylabel('\dots [mm/s]');
title('Slider Velocity vs Crank Angle');

% 8.3 Slider acceleration vs crank angle
figure;
plot(theta2_deg, sdd, 'LineWidth', 1.5);
grid on;
xlabel('\theta_2 [deg]');
ylabel('\ddots [mm/s^2]');
title('Slider Acceleration vs Crank Angle');

% 8.4 Connecting rod angle
figure;
plot(theta2_deg, theta3_deg, 'LineWidth', 1.5);
grid on;
xlabel('\theta_2 [deg]');
ylabel('\theta_3 [deg]');
title('Connecting Rod Angle vs Crank Angle');

% 8.5 Connecting rod angular velocity
figure;
plot(theta2_deg, omega3, 'LineWidth', 1.5);
grid on;
xlabel('\theta_2 [deg]');
ylabel('\omega_3 [rad/s]');
title('Connecting Rod Angular Velocity vs Crank Angle');

% 8.6 Connecting rod angular acceleration
figure;
plot(theta2_deg, alpha3, 'LineWidth', 1.5);
grid on;
xlabel('\theta_2 [deg]');
ylabel('\alpha_3 [rad/s^2]');
title('Connecting Rod Angular Acceleration vs Crank Angle');

%% 9) Apeikonisi mixanismou se theseis -------------
% Requested angles: 30, 60, 125, 220 degrees
snapshot_angles_deg = [30 60 125 220];

figure;
hold on; grid on; axis equal;
xlabel('x [mm]'); ylabel('y [mm]');
title('Mechanism Snapshots');

% Draw reference x-axis (slider line)
plot([0 max(s)+r3], [0 0], 'k--');

for i = 1:numel(snapshot_angles_deg)
    th2s = deg2rad(snapshot_angles_deg(i));
    sin_th3 = -(r2/r3)*sin(th2s);
    sin_th3 = max(min(sin_th3, 1), -1);
    cos_th3 = sqrt(max(0, 1 - sin_th3^2));
    th3s = atan2(sin_th3, cos_th3);
    ss = r2*cos(th2s) + r3*cos_th3;
    
    % Points
    O = [0 0];
    Bp = [r2*cos(th2s), r2*sin(th2s)];
    Cp = [ss, 0];
    
    % Plot links
    plot([O(1) Bp(1)], [O(2) Bp(2)], 'LineWidth', 1.5);
    plot([Bp(1) Cp(1)], [Bp(2) Cp(2)], 'LineWidth', 1.5);
    plot(Cp(1), Cp(2), 'o', 'MarkerSize', 6, 'MarkerFaceColor', 'r');
    text(Cp(1), Cp(2)+4, sprintf('%d$^\\circ$', snapshot_angles_deg(i)), ...
     'Interpreter', 'latex');
end
hold off;

%% Kinhmatika kentrwn varous ravdwn
% ------------------------------------------------------------
% G2 : meso AB
% G3 : meso BC
%
% Genikes sxeseis opws sto mathima:
%
% v_G = v_A + omega x r_G/A
%
% a_G = a_A + alpha x r_G/A
%             + omega x (omega x r_G/A)
%
% ------------------------------------------------------------

for k = 1:N

    th2 = theta2(k);
    th3 = theta3(k);

    %% =======================================================
    %  LINK 2 (CRANK AB)
    %  Reference point : O (fixed pivot)
    %% =======================================================

    % Position of midpoint G2
    xG2(k) = (r2/2)*cos(th2);
    yG2(k) = (r2/2)*sin(th2);

    % Velocity of midpoint G2
    vG2x(k) = -(r2/2)*omega2*sin(th2);
    vG2y(k) =  (r2/2)*omega2*cos(th2);

    % Acceleration of midpoint G2
    % Tangential + Centripetal

    aG2x(k) = -(r2/2)*alpha2*sin(th2) ...
              -(r2/2)*omega2^2*cos(th2);

    aG2y(k) =  (r2/2)*alpha2*cos(th2) ...
              -(r2/2)*omega2^2*sin(th2);


    %% =======================================================
    %  LINK 3 (CONNECTING ROD BC)
    %  Reference point : B
    %% =======================================================

    % Position vector from B to G3
    rx = (r3/2)*cos(th3);
    ry = (r3/2)*sin(th3);

    % Position of midpoint G3
    xG3(1,k) = xB(1,k) + rx;
    yG3(1,k) = yB(1,k) + ry;

    % Velocity of midpoint G3
    %
    % v_G3 = v_B + omega3 x r_G3/B

    vG3x(1,k) = vBx(1,k) - omega3(1,k)*ry;

    vG3y(1,k) = vBy(1,k) + omega3(1,k)*rx;


    % Tangential acceleration component
    %
    % alpha3 x r_G3/B

    aTx = -alpha3(1,k)*ry;
    aTy =  alpha3(1,k)*rx;


    % Centripetal acceleration component
    %
    % omega3 x (omega3 x r_G3/B)

    aCx = -omega3(1,k)^2*rx;
    aCy = -omega3(1,k)^2*ry;


    % Total acceleration of midpoint G3

    aG3x(1,k) = aBx(1,k) + aTx + aCx;

    aG3y(1,k) = aBy(1,k) + aTy + aCy;

end

%% Ypologismos diatomis
% ------------------------------------------------------------
% This block finds the worst positions of the mechanism based on the
% resultant acceleration at the centroid of each rod, then performs a
% simplified static sizing check using Newton's 2nd law.
%
% IMPORTANT:
% If the rod mass is not given, the required area cannot be determined
% uniquely. Therefore m2_kg and m3_kg must be provided as design inputs.
% ------------------------------------------------------------

% Material properties (steel)
sigma_y = 250e6;     % Yield strength [Pa] (change if another steel grade is used)
FS = 2.0;            % Safety factor
sigma_allow = sigma_y / FS;   % Allowable normal stress [Pa]

% Masses of the rods - user-defined design inputs
% Replace these with the actual masses of the links [kg]
m2_kg = 1.0;   % Mass of link 2 [kg]
m3_kg = 1.0;   % Mass of link 3 [kg]

% Convert acceleration from mm/s^2 to m/s^2 for force calculation
aG2_res = sqrt(aG2x.^2 + aG2y.^2);   % [mm/s^2]
aG3_res = sqrt(aG3x.^2 + aG3y.^2);   % [mm/s^2]

aG2_res_m = aG2_res / 1000;          % [m/s^2]
aG3_res_m = aG3_res / 1000;          % [m/s^2]

% Find worst-case positions
[aG2_max, idxG2] = max(aG2_res_m);
[aG3_max, idxG3] = max(aG3_res_m);

theta2_G2_deg = theta2_deg(idxG2);
theta2_G3_deg = theta2_deg(idxG3);

% Newton's 2nd law: equivalent inertia force
F2_max = m2_kg * aG2_max;   % [N]
F3_max = m3_kg * aG3_max;   % [N]

% Required areas from sigma = F/A
A2_req_m2 = F2_max / sigma_allow;   % [m^2]
A3_req_m2 = F3_max / sigma_allow;   % [m^2]

% Convert to mm^2 for reporting
A2_req_mm2 = A2_req_m2 * 1e6;       % [mm^2]
A3_req_mm2 = A3_req_m2 * 1e6;       % [mm^2]

% Equivalent rectangular section assumption
% Choose aspect ratio h/b = 2
aspect = 2.0;

b2_mm = sqrt(A2_req_mm2 / aspect);
h2_mm = aspect * b2_mm;

b3_mm = sqrt(A3_req_mm2 / aspect);
h3_mm = aspect * b3_mm;

% Display results
fprintf('\n===== Simplified strength check =====\n');
fprintf('Allowable stress = %.2f MPa\n', sigma_allow/1e6);

fprintf('\nLink 2:\n');
fprintf('  Worst angle theta2 = %.2f deg\n', theta2_G2_deg);
fprintf('  Max resultant acceleration = %.4f m/s^2\n', aG2_max);
fprintf('  Equivalent force = %.4f N\n', F2_max);
fprintf('  Required area = %.4f mm^2\n', A2_req_mm2);
fprintf('  Rectangular section (h/b = %.1f): b = %.4f mm, h = %.4f mm\n', ...
        aspect, b2_mm, h2_mm);

fprintf('\nLink 3:\n');
fprintf('  Worst angle theta2 = %.2f deg\n', theta2_G3_deg);
fprintf('  Max resultant acceleration = %.4f m/s^2\n', aG3_max);
fprintf('  Equivalent force = %.4f N\n', F3_max);
fprintf('  Required area = %.4f mm^2\n', A3_req_mm2);
fprintf('  Rectangular section (h/b = %.1f): b = %.4f mm, h = %.4f mm\n', ...
        aspect, b3_mm, h3_mm);