%% task 4
close all; clear all; clc
% Beam properties & first resonance frequency
L = 1; w = 0.02; h = 0.01; A = w*h;
V = A*L; rho = 7800; m = rho*V;
zeta = 1;
E = 200*10^9; Ix = (w*h^3)/12;
zero1 =fzero('cos(x).*cosh(x)+1',[1,2]);
f0=sqrt(zero1.^4.*E.*Ix./(rho*A))/(2*pi);

fs = 10000; % sampling frequency
N = 4; NN = 1024*512;
x = randn(512*1024,1); % creating force input
[B,A] = butter(N, 0.2, 'low');
Y = filter(B,A,x); % filtering signal
win = hann(NN); % Hanning window
[pxx, f] = pwelch(Y, win, NN/2, NN, fs); % calculating PSD
figure()
semilogy(f, pxx); xlim([0 1000]); title('PSD - N=1024*512 (Filtered force)');
xlabel('Frequency [Hz]'); ylabel('PSD log_1_0[N^2/Hz]')

% Calculating rms
pxx_rms = rms(pxx)
F_rms = rms(Y)


N = 16384; %FFT size
win = hann(N); % hanning window
OL = N/2; % overlap
df = fs/N;
f = 0:df:N*df-df; % frequency vector
xin = 1;
N_modes = 5;

for i = 1:10
    xout = (i-1)/10;
    [poles(:,i), residues(:,i)] = cantilever(m, f0, xin, xout, N_modes, zeta); % Calculating poles/residues
    [y(:,i),t(:,i)] = timeresv_rp(Y,fs,residues(:,i),poles(:,i),(1:N_modes)); % Time response/ time vector
    FRF(:,i) = tfestimate(t(:,i), y(:,i), win , N/2, N); % Calculating transfer function

    figure()
    semilogy(f(1:length(FRF(:,i))), abs(FRF(:,i))); xlim([0 1000])
end

xout = 1;
[pole, residue] = cantilever(m, f0, xin, xout, N_modes, zeta);
[y1,t1] = timeresv_rp(Y,fs,residue,pole,(1:N_modes));
[H,f1] = tfestimate(t1, y1, win , N/2, N, fs);
figure()
semilogy(f1, abs(H)); xlim([0 1000])


[coherence, f_coh] = mscohere(Y, y(:,10),[],[],[], fs); % Calculating coherence
figure()
plot(f_coh, coherence); xlim([0 1000])
title('Coherence \gamma^2'); xlabel('Frequency [Hz]'); ylabel('Coherence');
% w = f1.*(2*pi);
% s = (j*w);
% HM = H.*s;
% mif = modeind1(HM);
% figure()
% plot(f1, mif);xlim([0 100])
%
% [h,t,fs] = impresp(H,f1);
% hN = 2000; N = 25; flow = 0; fhi = 1000;
% poles = complexp(h,fs,hN,N,mif,f1,flow,fhi);


%[residues,residuals] = pol2resf(H,f,poles,flow,fhi,disptime)