% quantizationNoise.m -- quantization noise study for LHO alog 7154

%% Generate white noise excitation
Fs = 16384;
nFFT = Fs*30;
input = 200e3 * ...           % 100000 ct-pk excitation
    (rand([1000*Fs, 1])-0.5); % 1000 sec duration, dc offset subtracted

%% Define filters to shape and notch the noise
% foton string: zpk([1.3;1.3;1.3;1.3;1.3;1.3],[0.3;0.3;0.3;0.3;0.3;0.3],1,"n")ellip("LowPass",4,1,80,100)
shapeFilt = zpk([0+1i*2874.58, 0-1i*2874.58, 0+1i*6789.26, 0-1i*6789.26, -8.16814, -8.16814, -8.16814, -8.16814, -8.16814,...
    -8.16814], [-213.528+1i*260.263, -213.528-1i*260.263, -85.3399+1i*618.665, -85.3399-1i*618.665, -1.88496,...
    -1.88496, -1.88496, -1.88496, -1.88496, -1.88496], 1.56214e-08);

% foton string: notch(30,5,200)notch(32,5,200)notch(34,2.5,200)
notchFilt = zpk([-3.7928e-09+1i*188.496, -3.7928e-09-1i*188.496, -4.03733e-09+1i*201.062, -4.03733e-09-1i*201.062,...
    -8.72397e-09+1i*213.628, -8.72397e-09-1i*213.628],...
    [-37.8895+1i*184.648, -37.8895-1i*184.648, -40.4155+1i*196.958, -40.4155-1i*196.958, -87.2316+1i*195.007,...
    -87.2316-1i*195.007], 1);

filt = shapeFilt*notchFilt;

% bodeplot(filt); setoptions(h, 'FreqUnits', 'Hz');

%% Discretize filter and apply to input
digFilt = c2d(filt, 1/Fs, 'tustin');
[digFiltZ, digFiltP, digFiltK] = zpkdata(digFilt, 'v');
digFilt = zp2sos(digFiltZ, digFiltP, digFiltK);
outputDouble = sosfilt(digFilt, input);

%% Interpolate and round off
Fs = Fs * 4;
nFFT = nFFT * 4;
outputDouble = interp(outputDouble, 4);
outputInt = round(outputDouble);

%% Plot spectra
countsToVolts = 20/2^18;
[pDouble, f] = pwelch(outputDouble*countsToVolts, hann(nFFT), nFFT/2, nFFT, Fs);
[pInt, ~] = pwelch(outputInt*countsToVolts, hann(nFFT), nFFT/2, nFFT, Fs);
[pQNoise, ~] = pwelch((outputDouble-outputInt)*countsToVolts, hann(nFFT), nFFT/2, nFFT, Fs);

loglog(f, sqrt(pDouble));
hold on;
loglog(f, sqrt(pInt), 'r');
loglog(f, sqrt(pQNoise), 'r:');

%% Compare to measurement
dataPath = '/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/QUADTST/Common/Data/18Bit_DACNoise_2013-07-21/';
dataLF = load([dataPath 'SCRN0097.TXT']);
dataHF = load([dataPath 'SCRN0109.TXT']);
zeroOutLF = load([dataPath 'SCRN0098.TXT']);
zeroOutHF = load([dataPath 'SCRN0108.TXT']);
% exc = load([dataPath '2013-07-21_H1SUSQUADTST_DACNoise_COILOUTF_AvgASDw30-32-34HzTripleNotch_ASDs_asd.txt']);

sr560Gain = 5;
loglog(dataLF(:,1), dataLF(:,2)/sr560Gain, 'k');
loglog(dataHF(:,1), dataHF(:,2)/sr560Gain, 'k');
loglog(zeroOutLF(:,1), zeroOutLF(:,2)/sr560Gain, 'k:');
loglog(zeroOutHF(:,1), zeroOutHF(:,2)/sr560Gain, 'k:');
% loglog(exc(:,1), exc(:,2)*countsToVolts, 'm');

%% Format plot
legend('Excitation, double precision', 'Excitation, 18 bits', 'Quantization noise floor',...
    'Meas. DAC out, loFreq', 'Meas. DAC out, hiFreq',...
    'Meas. DAC out, no exc, loFreq', 'Meas. DAC out, no exc, hiFreq');
xlabel('Frequency [Hz]');
ylabel('Voltage at AI chassis output [V/rtHz]');
xlim([1e-1 1e3]);
ylim([1e-8 1e-1]);
grid on;
