% SDOF System - Frequency Response Function (analytical approach)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

M = 1;  % Mass  
K = 1;  % Stiffness

Nf = 500;               % Number of frequencies
w = linspace(0,2,Nf)';  % Angular frequencies (Nf values between 0 and 2)

No = 1;  % Number of inputs
Ni = 1;  % Number of outputs

H = zeros(Nf,No,Ni);  % Initialization of H (FRF) to zero

% Computation of H for all frequencies
for k = 1:Nf
    H(k) = 1/(-w(k)^2*M+K);
end

% Plot the result in Figure 1
figure(1)
semilogy(w,abs(H))


% 2DOF System
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

M = eye(2);       % Mass matrix: 2x2 identity matrix (masses normalized to 1)
K = [2,-1;-1,1];  % Stiffness matrix (2x2)

Nf = 500;
w = linspace(0,2,Nf)';

No = 2;  % Number of inputs
Ni = 2;  % Number of outputs

H = zeros(Nf,No,Ni);  % Initialization of H

% Computation of H for all frequencies 
for k = 1:Nf
    Hk = inv(-w(k)^2*M+K);   % Hk is a 2x2 matrix
    H(k,:) = Hk(:).';        % The 2x2 matrix Hk is stored in the array H for all frequencies
end

% Plot the result in Figure 2
figure(2)
subplot(2,2,1)
semilogy(w,abs(H(:,1,1)))
subplot(2,2,2)
semilogy(w,abs(H(:,2,1)))
subplot(2,2,3)
semilogy(w,abs(H(:,1,2)))
subplot(2,2,4)
semilogy(w,abs(H(:,2,2)))


% MDOF System
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

No = 5;  % 5 DOFs -> No=Ni=5
Ni = 5;

M = eye(No);
K = 2*eye(No)-diag(ones(No-1,1),-1)-diag(ones(No-1,1),1);
K(No,No) = 1;

Nf = 500;
w = linspace(0,2,Nf)';

H = zeros(Nf,No,Ni);

for k = 1:Nf
    Hk = inv(-w(k)^2*M+K);
    H(k,:) = Hk(:).';
end

% Plot the result in Figure 3
figure(3)
subplot(2,2,1)
semilogy(w,abs(H(:,3,3)))
subplot(2,2,2)
semilogy(w,abs(H(:,5,3)))
subplot(2,2,3)
semilogy(w,abs(H(:,3,5)))
subplot(2,2,4)
semilogy(w,abs(H(:,5,5)))


% Plot the result in Figure 4 - x-axis equals the vector index
figure(4)
semilogy(abs(H(:,5,5)))

% index corresponding with the 1st, 2nd, 3rd and 4th resonance
k1 = 72
k2 = 208
k3 = 328
k4 = 421

% Plot the operational deflection shape at the resonances
figure(5)
subplot(2,2,1)
plot(real(H(k1,:,1)))
subplot(2,2,2)
plot(real(H(k2,:,1)))
subplot(2,2,3)
plot(real(H(k3,:,1)))
subplot(2,2,4)
plot(real(H(k4,:,1)))


% Solve an eigenvalue problem to determine the mode shapes
[V,D]=eig(M\K);

% Plot the first 4 mode shapes
figure(6)
subplot(2,2,1)
plot(real(V(:,1)))
subplot(2,2,2)
plot(real(V(:,2)))
subplot(2,2,3)
plot(real(V(:,3)))
subplot(2,2,4)
plot(real(V(:,4)))

% Modal frequencies
modal_frequencies = sqrt(diag(D))/2/pi

% Modal mass
modal_mass = diag(V.'*M*V)
% Modal stiffness 
modal_stiffness = diag(V.'*K*V)


i = sqrt(-1);
s = i*w;       % Laplace variable

% Compute the frequency response matrix using the modal-parameter model
Hmodal = zeros(Nf,No,Ni);

for k = 1:Nf
    Hk = zeros(No);
    for m = 1:No  % Summation over the number of modes   
        p_m = i*sqrt(D(m,m)); % Pole of mode m
        phi_m = V(:,m);       % Mode shape of mode m
        Hk = Hk+1/(2*p_m*modal_mass(m))*phi_m*phi_m.'/(s(k)-p_m)+conj(1/(2*p_m*modal_mass(m))*phi_m*phi_m.')/(s(k)-conj(p_m));
    end
    Hmodal(k,:) = Hk(:).';
end

% Plot the result in Figure 7
figure(7)
subplot(2,2,1)
semilogy(w,abs(Hmodal(:,1,1)))
subplot(2,2,2)
semilogy(w,abs(Hmodal(:,5,1)))
subplot(2,2,3)
semilogy(w,abs(Hmodal(:,1,5)))
subplot(2,2,4)
semilogy(w,abs(Hmodal(:,5,5)))
