 clear all
 close all
 dRaw=h5read('/ligo/home/andrei.danilin/Downloads/0001_ZM5.hdf5', '/H1:OMC-DCPD_SUM_OUT_DQ');
 dRaw2=h5read('/ligo/home/andrei.danilin/Downloads/0002_ZM2.hdf5', '/H1:OMC-DCPD_SUM_OUT_DQ');
 dRaw_ref = h5read('/ligo/home/andrei.danilin/Downloads/0000_No_excitation.hdf5', '/H1:OMC-DCPD_SUM_OUT_DQ');
 
 load('/ligo/home/andrei.danilin/Downloads/DARM_OLG_May2024.mat')
 load('/ligo/home/andrei.danilin/Downloads/freq_DARMOLG_May2024.mat')
 
 testvar(:,1) = abs(freq);
 testvar(:,2) = new_Open_loop;
 
%  OLmag=load('OL_DARM_mag.txt');
%  OLphase=load('OL_DARM_phase.txt');
 
 OLmag(:,2)=abs(new_Open_loop);
 OLphase(:,2)=angle(new_Open_loop);
 OLmag(:,1)=abs(freq)
 OLphase(:,1)=abs(freq)
 
 t=linspace(0, 300, length(dRaw));
%  dRaw(:,1)=[]; %delete times
 dRaw = [dRaw dRaw2 dRaw_ref];

%put dRaw into RIN

dmean=mean(dRaw); 
drin=[];
for i=1:length(dmean)
    drin(:,i)=dRaw(:,i)./dmean(i);
end

lambda=1064e-9;

Fs=round(1/t(2));
Fs=16384;  %sampling frequency
T=1/Fs;    %sample time
L=50*Fs;   % #samples in 20 %32 seconds %why was this 30*Fs?
t=t;  %vector of time
fm=1;      %moduation frequency and (1 Hz 3 V pp since wave to PZT dl   river)
m=(4*pi/lambda)*1.2e-6 ;     %modulation depth  (peak to peak drive in radians, 18 microns)
r=1e-4.*ones(1, length(dmean));
rm2 = 1e-6;

x=@(rm)2*rm*sin(m*sin(2*pi*fm*t));

stepSize = 0.5; %1 second
overlap = 0.5;
NFFT = stepSize*Fs;
w = hanning(NFFT);


Pxx=[];
for i=1:1 %1:length(dmean)
    [Pxx(:,i),f]=pwelch(x(r(i)),w,[],[],Fs);
end
f(2)-f(1)
a=sqrt(Pxx);

%real data
Pd=[];
for i=1:length(dmean)
    [Pd(:,i), f]=pwelch(drin(:,i),w,overlap*NFFT,NFFT,Fs);
end

b=sqrt(Pd);

%take out darm????
% f(1:20)=[]; %delte frequcies we dont' have darm for
% b(1:20, :)=[];
%a(1:20, :)=[];
DarmTF=OLmag(:,2).*exp(1i.*2.*pi.*OLphase(:,2)/360); %
darmOL=frd(DarmTF, 2.*pi.*OLmag(:,1));

P= bodeoptions;
P.FreqUnits = 'Hz';
figure(1)
bodeplot(darmOL, P)

darmGp=1+darmOL;
wfreq=2.*pi.*f;
[magDarmGp, phaseDarmGp]=bode(darmGp, wfreq);

bDarmOut=[];
for i=1:length(f)
    bDarmOut(i, :)=b(i, :).*magDarmGp(:, :, i);
end


% import reference of OSEM, to get normal rms velocity
osem = load('OMC_L_OSEM_ref.txt');

% %cut out everything above 20 Hz, its sensor noise
% index_cut = find(osem(:,1)>20);
% osem(index_cut, :) = [];
% %multiply spectrum by frequency to get velocity
% osem_velocity = osem(:,2).*osem(:,1);
% rms_osem_velocity = sqrt(cumtrapz(osem(:,1), osem_velocity.^2));
% ref_velocity = max(rms_osem_velocity);
m_norm = (2*pi/lambda)*7e-6;  %ampltiude at 0.74 Hz is 0.6 um (assuming osem calibration is wrong by a factor of 3)
fm_norm= 1;
x_norm=@(rm)2*rm*sin(m_norm*sin(2*pi*fm_norm*t))+2*rm2*sin(2*m_norm*sin(2*pi*fm_norm*t));

Pxx_norm=[];
for i=1:1 %1:length(dmean)
    [Pxx_norm(:,i),f]=pwelch(x_norm(r(i)),w,[],[],Fs);
end

a_norm=sqrt(Pxx_norm);

% figure(3)
% loglog(osem(:,1), osem_velocity, osem(:,1), rms_osem_velocity)

figure(2)
loglog(f, bDarmOut(:,1), f, a,f, bDarmOut(:,2), f, bDarmOut(:,3), 'LineWidth',2) %f, a_norm
title('OM1 excitation', 'FontSize', 20)
grid on
legend('measured RIN with excitation on', 'fringe wrapping model, r = 1e-5', 'DC PD RIN no excitation')
xlabel('Frequency [Hz]', 'FontSize', 15)
ylabel('OMC DC PD RIN', 'FontSize', 15)
xlim([3, 300])
% print('-dpng', 'OM1_L_fringe_wrapping')



