%% FUNZIONE DI RISPOSTA IN FREQ GENERICA
% Analisi in frequenza del sistema
% da uno sweep in frequenza


close all
clear all
clc

%% Struttura dei file di input:
% tempo [s]
% channel 1: funzione imposta [V]
% channel 2: spostamento tip laser [V]
% channel 3: accelerometro su shaker [V]
% channel 4: harvester [V]

%sensibilit�
laser_sens= 2; %V/mm
acc_sens= 100e-3/9.81; %V/(m/s^2)

%SELEZIONE FILE DI INPUT
file=4;

sweep=[20 100];


%caricamento dati
dati=importdata(['dati/scope_' int2str(file) '.csv'],',',2);



% t=[.0025:.0025:5];
% frequenza_s=10;
% ampiezza_s=10;
% %clear dati
% for i=2:5
%     dati.data(:,i)=ampiezza_s*sin(2*pi*frequenza_s.*t);
% end



tempo=dati.data(:,1) - dati.data(1,1); %tempo traslato allo zero
funz=dati.data(:,2); %legge di moto imposta
dati.data(:,3)=dati.data(:,3)./laser_sens; 
spost=dati.data(:,3); %spostamento da laser
dati.data(:,4)=dati.data(:,4)./acc_sens;
accel=dati.data(:,4); %accelerometro
harv=dati.data(:,5); %uscita harvester


%% Analisi in frequenza
N=length(spost);
dt=tempo(2);
fsamp=1/dt;
df=fsamp/N;

freq=0:df:fsamp/2;
    
% trasformate (canali per colonne)
for i=1:4
    TF=fft(dati.data(:,i+1))/N;
    modulo(:,i)=abs(TF(1:N/2+1))*2;
    modulo(1,i)=abs(TF(1));
    modulo(N/2+1,i)=abs(TF(N/2+1));
    
    fase(:,i)=angle(TF(1:N/2+1));
end


% spostamento imposto e accelerazione
figure('position',[100,100,600,400])
subplot(311)
plot(tempo,funz,'y',tempo,accel,'c','linewidth',1)
xlabel('tempo [s]')
title('-------storia temporale-------')
grid
subplot(312)
plot(freq,modulo(:,1),'y',freq,modulo(:,3),'c','linewidth',1)
grid
xlabel('freq [Hz]')
ylabel('modulo spettro')
title('-------FFT------')
subplot(313)
plot(freq,fase(:,1),'y',freq,fase(:,3),'c','linewidth',1)
xlabel('freq [Hz]')
ylabel('fase spettro')
grid

% salvataggio EPS
%print -painters -depsc img/spettri_IN.eps
%print -painters -dpng img/spettri_IN.png


% spostamento laser e tensione harvester
figure('position',[200,75,600,400])
subplot(311)
plot(tempo,spost,'g',tempo,harv,'m','linewidth',1)
xlabel('tempo [s]')
title('-------storia temporale-------')
grid
subplot(312)
plot(freq,modulo(:,2),'g',freq,modulo(:,4),'m','linewidth',1)
grid
xlabel('freq [Hz]')
ylabel('modulo spettro')
title('-------FFT------')
subplot(313)
plot(freq,fase(:,2),'g',freq,fase(:,4),'m','linewidth',1)
xlabel('freq [Hz]')
ylabel('fase spettro')
grid

%% FRF
NFFT=round(fsamp/df);
wind=boxcar(NFFT);
over=0*NFFT/100;

[autosp,freqa]=pwelch(accel,wind,over,NFFT,fsamp);

[crossp,freqc]=cpsd(spost,accel,wind,over,NFFT,fsamp);

FRF=crossp./autosp;
modulo_frf=abs(FRF);
fase_frf=angle(FRF);

figure('position',[300,50,600,400])
subplot(2,1,1)
plot(freqa,modulo_frf,'r','LineWidth',1)
hold on

lim1=sweep(1);
lim2=sweep(2);
pos_f1=find(freqa<=lim1,1,'last');
pos_f2=find(freqa>=lim2,1);

[Ym mas]=max(modulo_frf(pos_f1:pos_f2));
Xm=freqa(pos_f1+mas-1);


plot(Xm,Ym,'gp')
testo=sprintf(' sweep (%.2f, %.5f)',Xm,Ym);
text(Xm-7,Ym,testo,'Color','g')
grid
xlim([lim1 lim2])
title('------FRF------')

subplot(2,1,2)
plot(freqc,fase_frf,'r','LineWidth',1)
grid
xlabel('Freq [Hz]')
xlim([lim1 lim2])