%% Parameters
prefix = '2016_10_19';  % name of the folder where result will be saved
gps0 = 1160972805;      % GPS time of clean data before excitation
gps1 = 1160972852;      % GPS time right after excitation
dt = 30;                % how much data to be used to search peaks

minsnr = 6;             % minimum peak SNR
minfr = 1000;           % minimum peak frequency
Dt = 3600;              % total amount of time for the ringdown measurement

%% Compare data right before and right after the excitation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get data
c = nds2.connection('cymac3.ligo.caltech.edu', 8088);
data0 = c.fetch(gps0, gps0+dt, {'X3:CR1-X_NORM_OUT_DQ', 'X3:CR1-Y_NORM_OUT_DQ', 'X3:CR1-ESD_OUT_DQ'});
data1 = c.fetch(gps1, gps1+dt, {'X3:CR1-X_NORM_OUT_DQ', 'X3:CR1-Y_NORM_OUT_DQ', 'X3:CR1-ESD_OUT_DQ'});

x0 = double(data0(1).getData());
x1 = double(data1(1).getData());
y0 = double(data0(2).getData());
y1 = double(data1(2).getData());
e0 = double(data0(3).getData());
e1 = double(data1(3).getData());
t = (0:length(e1)-1)/65536;

%% Find peaks %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute spectra
fs = 65536;
np = fs;
[spx0, fr] = pwelch(x0, blackman(np), np/2, np, fs);
spx0 = sqrt(spx0);
[spx1, fr] = pwelch(x1, blackman(np), np/2, np, fs);
spx1 = sqrt(spx1);
[spy0, fr] = pwelch(y0, blackman(np), np/2, np, fs);
spy0 = sqrt(spy0);
[spy1, fr] = pwelch(y1, blackman(np), np/2, np, fs);
spy1 = sqrt(spy1);

% plot comparison
scrsz = get(groot, 'ScreenSize');
figure('Position', [1, scrsz(4)/2 scrsz(3)/2, scrsz(4)/1.5], 'Color', 'w')
ax(1) = subplot(211);
loglog(fr, spx1, fr, spx0)
xlabel('Frequency [Hz]')
ylabel('QPD X')
legend('After exc.', 'before exc.')
set(gca, 'FontSize', 17)
ax(2) = subplot(212);
loglog(fr, spy1, fr, spy0)
xlabel('Frequency [Hz]')
ylabel('QPD Y')
legend('After exc.', 'before exc.')
set(gca, 'FontSize', 17)
linkaxes(ax)
xlim([500, 30000])
ylim([1e-10, 1e-4])
img = getframe(gcf);
imwrite(img.cdata, [prefix, '/spectra.png']);

% whiten spectrum with reference time
swx = spx1./spx0;
swy = spy1./spy0;

% estimate the smooth background
df = fr(2);
F = 50;
Nf = round(F / df);
ww = hanning(Nf);
ww = ww/sum(ww);
sbgx = exp(conv(log(swx), ww, 'same'));
sbgy = exp(conv(log(swy), ww, 'same'));

% further whiten using the background
swx = swx ./ sbgx;
swy = swy ./ sbgy;

% find peaks in X
idx = find(swx > minsnr & fr> minfr);
% aggregate
di = diff(idx);
dix = find(di > 1);
pk = {};
pk{1} = idx(1:dix(1));
for i=1:length(dix)-1
    pk{end+1} = idx(dix(i)+1:dix(i+1));
end
pk{end+1} = idx(dix(end)+1:end);
% compute mean frequency and total SNR
freqsx = cellfun(@(x) sum(fr(x).*swx(x).^2)./sum(swx(x).^2), pk);
amplsx = cellfun(@(x) sqrt(sum(swx(x).^2)), pk);

% find peaks in Y
idx = find(swy > minsnr & fr> minfr);
% aggregate
di = diff(idx);
dix = find(di > 1);
pk = {};
pk{1} = idx(1:dix(1));
for i=1:length(dix)-1
    pk{end+1} = idx(dix(i)+1:dix(i+1));
end
pk{end+1} = idx(dix(end)+1:end);
% compute mean frequency and total SNR
freqsy = cellfun(@(x) sum(fr(x).*swy(x).^2)./sum(swy(x).^2), pk);
amplsy = cellfun(@(x) sqrt(sum(swy(x).^2)), pk);

% plot all detected peaks
scrsz = get(groot, 'ScreenSize');
figure('Position', [1, scrsz(4)/2 scrsz(3)/2, scrsz(4)/1.5], 'Color', 'w')
ax(1) = subplot(211);
loglog(fr, swx)
hold all
loglog(freqsx, amplsx, 'o')
xlabel('Frequency [Hz]')
ylabel('QPD X whitened')
legend('Whitened spectrum', 'Detected peaks')
set(gca, 'FontSize', 17)
ax(2) = subplot(212);
loglog(fr, swy)
hold all
loglog(freqsy, amplsy, 'o')
xlabel('Frequency [Hz]')
ylabel('QPD Y')
legend('Whitened spectrum', 'Detected peaks')
set(gca, 'FontSize', 17)
linkaxes(ax)
xlim([500, 30000])
ylim([0.5, 1000])
img = getframe(gcf);
imwrite(img.cdata, [prefix, '/peaks.png']);

% merge the X and Y frequencies
freqs = [freqsx, freqsy];
ampls = [amplsx, amplsy];
[freqs,i] = sort(freqs);
ampls = ampls(i);
% remove duplicates
ii = find(diff(freqs) < 1);
ii = setdiff(1:length(freqs), ii);
freqs = freqs(ii);
ampls = ampls(ii);

%% save the detected peaks %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
save([prefix, '/detected_peaks.mat'], 'freqs', 'ampls', 'freqsx', 'freqsy', 'amplsx', 'amplsy');

f = fopen([prefix, 'detected_peaks.txt'], 'w');
fprintf(f, '%% Freq\t\tSNR\n');
for i=1:numel(freqs)
    fprintf(f, '%.1f\t\t%.1f\n',freqs(i), ampls(i));
end
fclose(f);

%% Analyze complete ringdown %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% get all data
c = nds2.connection('cymac3.ligo.caltech.edu', 8088);
data0 = c.fetch(gps1, gps1+Dt, {'X3:CR1-X_NORM_OUT_DQ'});
x = double(data0(1).getData());
data0 = c.fetch(gps1, gps1+Dt, {'X3:CR1-Y_NORM_OUT_DQ'});
y = double(data0(1).getData());

% spectrograms
fs = 65536;
np = fs;
[SX,F,T] = spectrogram(x, hanning(np), np/2, np, fs);
SX = abs(SX);
[SY,F,T] = spectrogram(y, hanning(np), np/2, np, fs);
SY = abs(SY);

%% extract the mode amplitude as a function of time %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbins = 1;
RX = [];
RY = [];
for i=1:numel(freqs)
    idx(i) = find(F == round(freqs(i)), 1);
    RX(i,:) = sqrt(sum(SX(idx(i)-nbins:idx(i)+nbins,:).^2));
    RY(i,:) = sqrt(sum(SY(idx(i)-nbins:idx(i)+nbins,:).^2));
end

save([prefix, '/ringdown.mat'], 'freqs', 'ampls', 'freqsx', 'freqsy', 'amplsx', 'amplsy', 'T', 'RX', 'RY');

% plot all ring downs
nrows = 4;
ncols = 6;
scrsz = get(groot, 'ScreenSize');
figure('Position', [1, 0 scrsz(3), scrsz(4)], 'Color', 'w')
for i=1:numel(freqs)
    subplot(nrows, ncols, i)
    plot(T, RX(i,:), '.')
    hold all
    plot(T, RY(i,:), '.')
    title(sprintf('%d - %.2f Hz', i,freqs(i)))
    xlabel('Time [s]')
    ylabel('Peak ampl.')
    legend('X', 'Y')
end
img = getframe(gcf);
imwrite(img.cdata, [prefix, '/ringdowns.png']);

% write here the list of modes that are misidentified, if any
excluded = [];
idx = ~arrayfun(@(x) any(find(floor(x) == excluded)), freqs);
freqs = freqs(idx);
ampls = ampls(idx);
RX = RX(idx,:);
RY = RY(idx,:);

%% Fit all ring downs %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:length(freqs)
    % choose X or Y depending on which has larger initial amplitude
    if RX(i,1) > RY(i,1)
        a = RX(i,:);
    else
        a = RY(i,:);
    end
    ii = T > 0;
    
    % simple exponential decay
    fun = @(p,t) sqrt((p(1)*exp(-t*p(2))).^2 + p(3).^2);
    opts = optimset('display', 'off', 'tolx', 1e-9, 'tolfun', 1e-9);
    
    p0 = [max(a), pi*freqs(i)/1e6, min(a)];
    % fit the exponential
    err = @(p) mean( (a(ii) - fun(p,T(ii))).^2 );
    [p,e0] = fminsearch(err, p0, opts);
    [p,e1] = fminsearch(err, p, opts);
    while e1<e0
        e0 = e1;
        [p,e1] = fminsearch(err, p, opts);
    end
    
    % compute non linear fit and confidence intervals
    [p1,r,J,cov,mse] = nlinfit(T(ii),a(ii),fun,p);
    ci = nlparci(p1, r, 'covariance', cov);
    
    dfr = 0.5; % error on frequency estimation
    q(i) = pi*freqs(i)/p1(2);       % estimated Q
    q_ci(i,1) = pi*(freqs(i) - dfr) / ci(2,2);  % confidence interval
    q_ci(i,2) = pi*(freqs(i) + dfr) / ci(2,1);
    
    scrsz = get(groot,'ScreenSize');
    f = figure('Position',[1 scrsz(4)/1.1 scrsz(3)/1.1 scrsz(4)/1.1]);
    semilogy(T(ii), a(ii), 'o', T(ii), fun(p, T(ii)), 'LineWidth', 3)
    xlabel('Time [s]')
    ylabel('Amplitude [a.u.]')
    legend('Data', 'Fit')
    title(sprintf('(%d) f_0=%.2f Hz Q=%.2e [%.4e < Q < %.4e]', i, freqs(i), q(i), q_ci(i,1), q_ci(i,2)));
    set(gca, 'FontSize', 20);
    set(f, 'Color', [1 1 1]);
    img = getframe(gcf);
    imwrite(img.cdata, sprintf('%s/fit_%.0f.png', prefix, freqs(i)));
    pause
    close
end

%% save final results %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
save([prefix, '/ringdown.mat'], 'freqs', 'ampls', 'freqsx', 'freqsy', 'amplsx', 'amplsy', 'T', 'RX', 'RY', 'q', 'q_ci');

f = fopen([prefix, '/results.txt'], 'w');
fprintf(f, '%% Freq\t\tQ\t\tQlow\t\tQhi\n');
for i=1:numel(freqs)
    fprintf(f, '%.1f\t\t%.4e\t\t%.4e\t\t%.4e\n',freqs(i), q(i), q_ci(i,1), q_ci(i,2));
end
fclose(f);

% plot all Q values
scrsz = get(groot, 'ScreenSize');
figure('Position', [1, scrsz(4)/2 scrsz(3)/2, scrsz(4)/1.5], 'Color', 'w')
l = semilogy(freqs, q, 'o', 'MarkerSize', 10, 'LineWidth', 2);
l.MarkerFaceColor = l.Color;
xlabel('Frequency [Hz]')
ylabel('Q value')
set(gca, 'FontSize', 17)
img = getframe(gcf);
imwrite(img.cdata, [prefix, '/Qs.png']);

scrsz = get(groot, 'ScreenSize');
figure('Position', [1, scrsz(4)/2 scrsz(3)/2, scrsz(4)/1.5], 'Color', 'w')
errorbar(freqs/1000, q, q - q_ci(:,1)', q_ci(:,2)' - q, '.', 'MarkerSize', 20);
set(gca, 'yscale', 'log')
xlabel('Frequency [Hz]')
ylabel('Q value')
set(gca, 'FontSize', 17)
img = getframe(gcf);
imwrite(img.cdata, [prefix, '/Qs_errorbars.png']);