clear all
close all
clc
 
 
Mass = 10^7.*[2 0 0 0;... 
        0 2 0 0 ;...
        0 0 2 0 ; ...
        0 0 0 1 ];
 
K =  10.*[ 2 -1  0 0;... 
      -1  2 -1 0 ;...
       0 -1  2 -1 ;...
       0  0  -1 1 ];
 
[EV, w_sq] = eig(Mass,K);
 
w = w_sq.^.5;
 
% set up freq vector in Hz, and in Rad.
f = 0:0.5:1500; % Frequency in Hz - Make sure that you go to at least 2*highest freq
w_samp = f*(2*pi);
 

% Damping ratios calculated from real and imaginary components of FRF
% here they are just reasonable random numbers 
d1 = 0.008526;
d2 = 0.0051557;
d3 = 0.003676;
d4 = 0.001;
 
d = [d1 d2 d3 d4 ];
 
for j = 1:length(EV)
    for k = 1:length(EV)
        for u=1:1:length(w_samp)
        
        rec_frf(j,k,u) = ((2*(d(k))*w(k,k)^2) * ( EV(:,k).' *  EV(:,j) ) / ( (-w_samp(u).^2 + (w(j,j))^2) + i*(2*(d(j))*w(j,j))*w_samp(u)));
        
        end
    end
end
        
mag1 = 20*log10(abs(squeeze(rec_frf(1,1,:))));
phase1 = (180/pi).*unwrap(squeeze(angle(rec_frf(1,1,:))));
 
mag2 = 20*log10(abs(squeeze(rec_frf(2,1,:))));
phase2 = (180/pi).*unwrap(squeeze(angle(rec_frf(2,1,:))));
 
mag3 = 20*log10(abs(squeeze(rec_frf(3,1,:))));
phase3 = (180/pi).*unwrap(squeeze(angle(rec_frf(3,1,:))));
 
mag4 = 20*log10(abs(squeeze(rec_frf(4,1,:))));
phase4 = (180/pi).*unwrap(squeeze(angle(rec_frf(4,1,:))));
 
total_frf = squeeze(rec_frf(1,1,:)) + squeeze(rec_frf(2,1,:)) + squeeze(rec_frf(3,1,:)) + squeeze(rec_frf(4,1,:));
 
mag_total = 20*log10(abs(total_frf));
phase_total = (180/pi).*unwrap(angle(total_frf));
 
c= length(w_samp);
 
figure(1)
suptitle('Summation of Modes for Node 2')
set(gcf, 'Position', [50 50 900 900])
subplot(5,2,1)
plot(f(1:c), mag1(1:c),'-b')
hold on
%plot(f(1:c), 20*log10(abs(squeeze(rec_frf(1,1,1:c)))),'-r')
ylabel('Mag, dB')
title('1st mode')
 
subplot(5,2,2)
plot(f(1:c), phase1(1:c),'-b')
hold on
%plot(f(1:c), (180/pi)*angle(squeeze(rec_frf(1,1,1:c))),'-r')
ylabel('Phase, deg')
 
subplot(5,2,3)
plot(f(1:c), mag2(1:c),'-b')
ylabel('Mag, dB')
title('2nd mode')
 
subplot(5,2,4)
plot(f(1:c), phase2(1:c),'-b')
ylabel('Phase, deg')
 
subplot(5,2,5)
plot(f(1:c), mag3(1:c),'-b')
ylabel('Mag, dB')
title('3rd mode')
 
subplot(5,2,6)
plot(f(1:c), phase3(1:c),'-b')
ylabel('Phase, deg')
 
subplot(5,2,7)
plot(f(1:c), mag4(1:c),'-b')
ylabel('Mag, dB')
title('4th mode')
 
subplot(5,2,8)
plot(f(1:c), phase4(1:c),'-b')
ylabel('Phase, deg')
 
subplot(5,2,9)
plot(f(1:c), mag_total(1:c),'-b')
hold on
plot(f(1:c), 20*log10(abs(squeeze(rec_frf(1,1,1:c) + rec_frf(2,1,1:c) + rec_frf(3,1,1:c) + rec_frf(4,1,1:c)))),'-m')
 
ylabel('Mag, dB')
xlabel('Frequency, Hz')
title('Recombine FRFs')
 
subplot(5,2,10)
plot(f(1:c), phase_total(1:c),'-b')
hold on
plot(f(1:c), (180/pi)*unwrap(angle(squeeze(rec_frf(1,1,1:c) + rec_frf(2,1,1:c) + rec_frf(3,1,1:c) + rec_frf(4,1,1:c)))),'-m')
ylabel('Phase, deg')
xlabel('Frequency, Hz')
 
 
figure(2)
suptitle('Summation of Modes for node 2')
set(gcf, 'Position', [50 50 900 900])
subplot(2,1,1)
plot(f(1:c), 20*log10(abs(squeeze(rec_frf(1,1,1:c)))),'-r')
hold on
plot(f(1:c), 20*log10(abs(squeeze(rec_frf(2,1,1:c)))),'-g')
plot(f(1:c), 20*log10(abs(squeeze(rec_frf(3,1,1:c)))),'-b')
plot(f(1:c), 20*log10(abs(squeeze(rec_frf(4,1,1:c)))),'-k')
%plot(f(1:c), mag1(1:c),'-*r')
 
plot(f(1:c), 20*log10(abs(squeeze(rec_frf(1,1,1:c) + rec_frf(2,1,1:c) + rec_frf(3,1,1:c) + rec_frf(4,1,1:c)))),'-om')
 
ylabel('Mag, dB')
title('Node 2')
 
subplot(2,1,2)
plot(f(1:c), (180/pi)*angle(squeeze(rec_frf(1,1,1:c))),'-r')
hold on
plot(f(1:c), (180/pi)*angle(squeeze(rec_frf(2,1,1:c))),'-g')
plot(f(1:c), (180/pi)*angle(squeeze(rec_frf(3,1,1:c))),'-b')
plot(f(1:c), (180/pi)*angle(squeeze(rec_frf(4,1,1:c))),'-k')
%plot(f(1:c), phase1(1:c),'-*r')
 
plot(f(1:c), (180/pi)*angle(squeeze(rec_frf(1,1,1:c) + rec_frf(2,1,1:c) + rec_frf(3,1,1:c) + rec_frf(4,1,1:c))),'-om')
 
ylabel('Phase, deg')
xlabel('Frequency, Hz')
legend('2,2','2,3','2,4','2,5','Mag 2')
 

% IFFT section %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
total_resp(1,:) = squeeze(rec_frf(1,1,1:c) + rec_frf(2,1,1:c) + rec_frf(3,1,1:c) + rec_frf(4,1,1:c)).';
total_resp(2,:) = squeeze(rec_frf(1,2,1:c) + rec_frf(2,2,1:c) + rec_frf(3,2,1:c) + rec_frf(4,2,1:c)).';
total_resp(3,:) = squeeze(rec_frf(1,3,1:c) + rec_frf(2,3,1:c) + rec_frf(3,3,1:c) + rec_frf(4,3,1:c)).';
total_resp(4,:) = squeeze(rec_frf(1,4,1:c) + rec_frf(2,4,1:c) + rec_frf(3,4,1:c) + rec_frf(4,4,1:c)).';
 
% total_resp(5,:) = squeeze(rec_frf(1,1,1:c) + rec_frf(1,2,1:c) + rec_frf(1,3,1:c) + rec_frf(1,4,1:c)).';
 
freq = f(1:c).';
 
figure(3)
set(gcf, 'Position', [50 50 900 900])
suptitle('5 node Fixed-Free Beam Frequency Response')
subplot(2,2,1)
plot(f(1:c), 20*log10(abs(total_resp(1,:))),'-om','Markersize',3)
ylabel('Mag of disp, dB')
title('node 2')
 
subplot(2,2,2)
plot(f(1:c), 20*log10(abs(total_resp(2,:))),'-om','Markersize',3)
ylabel('Mag of disp, dB')
title('node 3')
 
subplot(2,2,3)
plot(f(1:c), 20*log10(abs(total_resp(3,:))),'-om','Markersize',3)
ylabel('Mag of disp, dB')
xlabel('Freq, Hz')
title('node 4')
 
subplot(2,2,4)
plot(f(1:c), 20*log10(abs(total_resp(4,:))),'-om','Markersize',3)
ylabel('Mag of disp, dB')
xlabel('Freq, Hz')
title('node 5')
 
% figure(100)
% plot(f(1:c), 20*log10(abs(total_resp(5,:))),'-om','Markersize',3)
% ylabel('Mag of disp, dB')
% xlabel('Freq, Hz')
% title('IGNORE')
% 
 
dims = size(total_resp);
response = zeros(dims(1), 2*dims(2));
 
[response(1,:), time] = IFFT_time_resp(total_resp(1,1:c),f(1:c));
[response(2,:), time] = IFFT_time_resp(total_resp(2,1:c),f(1:c));
[response(3,:), time] = IFFT_time_resp(total_resp(3,1:c),f(1:c));
[response(4,:), time] = IFFT_time_resp(total_resp(4,1:c),f(1:c));
% [response(5,:), time] = IFFT_time_resp(total_resp(5,1:c),f(1:c));
 

figure(4)
set(gcf, 'Position', [50 50 900 900])
suptitle('5 node Fixed-Free Beam Time Response')
subplot(2,2,1)
plot(time, response(1,:),'-om','Markersize',3)
ylabel('disp, m')
title('node 2')
 
subplot(2,2,2)
plot(time, response(2,:),'-om','Markersize',3)
ylabel('disp, m')
title('node 3')
 
subplot(2,2,3)
plot(time, response(3,:),'-om','Markersize',3)
ylabel('disp, m')
xlabel('time, s')
title('node 4')
 
subplot(2,2,4)
plot(time, response(4,:),'-om','Markersize',3)
ylabel('disp, m')
xlabel('time, s')
title('node 5')
 

% figure(101)
% plot(time, response(5,:),'-om','Markersize',3)
% ylabel('disp, m')
% xlabel('time, s')
% title('IGNORE')
