%% Parameters
prefix = '2016_10_20';  % name of the folder where result will be saved
gps0 = 1161034552;      % GPS time of clean data before excitation
gps1 = 1161034619;      % 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(e0)-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());

% spectrogram
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 = 6;
ncols = 7;
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)
    ii = (T > 0 );
    % choose X or Y
    if RX(i,1) > RY(i,1)
        a = RX(i,:);
    else
        a = RY(i,:);
    end
        
    % guess initial parameters

    % first do a simple exponential decay
    fun = @(p,t) p(1) * exp(-p(2) * t);
    err = @(p) mean((fun(p, T(ii)) - a).^2);
    p0 = [max(a), 0];
    p = fminsearch(err, p0, optimset('display', 'off'));
    tau0 = p(2); % mean time constant
    
    % continue only if there is a decay
    if tau0>0

        amean = p(1); % mean amplitude
        % difference
        aa = a - fun(p,T);
        da = (max(aa) - min(aa))/2; % beat amplitude
        amax = amean + da;      % max amplitude
        % find beat frequency as the maximum of PSD of residual
        np = 256;
        [ss,fr] = pwelch(aa, hanning(np), np/2, np, 2);
        [bb,b] = max(ss(2:end));
        fbeat = fr(b+1);

        % fit the full model, and keep doing it as long as the error gets better
        %ii = (T < 4/tau0);
        ii = T>0;
        a = a(ii);

        fun = @(p,t) sqrt( (p(1) .* exp(-p(3)*t) .* sqrt(1 + p(2)^2 .* exp(-2*abs(p(4))*t) + 2*p(2)*exp(-abs(p(4))*t).*cos(p(5)*t+p(6)))).^2 + p(7)^2) ;
        %fun = @(p,t) sqrt( (p(1) .* exp(-p(3)*t) .* sqrt(1 + p(2)^2 .* exp(-2*abs(0)*t) + 2*p(2)*exp(-abs(0)*t).*cos(p(5)*t+p(6)))).^2 + p(7)^2) ;
        err = @(p) mean((fun(p, T(ii)) - a).^2);
        p0 = [amax, da/amax, tau0,   0,  2*pi*fbeat, 0, 0];
        [p, err0] = fminunc(err, p0, optimset('display', 'off', 'maxfunevals', 1e6, 'maxiter', 1e6, 'TolX', 1e-9, 'TolFun', 1e-9));
        [p, err1] = fminsearch(err, p, optimset('display', 'off', 'maxfunevals', 1e6, 'maxiter', 1e6, 'TolX', 1e-9, 'TolFun', 1e-9));
        cc = 0;
        while err1<err0 && cc<10
            cc = cc + 1;
            err0 = err1;
            [p, err1] = fminsearch(err, p, optimset('display', 'off', 'maxfunevals', 1e6, 'maxiter', 1e6, 'TolX', 1e-9, 'TolFun', 1e-9));
        end

        scrsz = get(groot,'ScreenSize');
        f = figure('Position',[1 scrsz(4)/1.1 scrsz(3)/1.1 scrsz(4)/1.1]);
        plot(T(ii), a, '.', T(ii), fun(p, T(ii)), 'LineWidth', 2)
        xlabel('Time [s]')
        ylabel('Amplitude [a.u.]')
        legend('Data', 'Fit')
        title(sprintf('(%d) f_0=%.2f Hz (A_1=%.3f, A_2=%.3f) \\deltaf=%.3f Hz , Q_1=%.2e, Q_2=%.2e', i, freqs(i), p(1), p(1)*p(2), p(5)/2/pi, pi*freqs(i)/p(3), pi*freqs(i)/(p(3)+abs(p(4)))));
        set(gca, 'FontSize', 20);
        set(f, 'Color', [1 1 1]);
        img = getframe(gcf);
        imwrite(img.cdata, sprintf('%s/fit_%.0f.png', prefix, freqs(i)));

        q(i,1) = pi*freqs(i)/p(3);
        q(i,2) = pi*freqs(i)/(p(3)+abs(p(4)));

        pause
        close
    end
end

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

f = fopen([prefix, '/results.txt'], 'w');
fprintf(f, '%% Freq\t\tQq\t\tQ2\n');
for i=1:numel(freqs)
    fprintf(f, '%.1f\t\t%.4e\t\t%.4e\n',freqs(i), q(i,1), q(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(:,1), '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']);

% Q measured by Raymond @ Glasgow
gl1 = load('raymond1.txt');
gl2 = load('raymond2.txt');

hold all
semilogy(gl1(:,1), gl1(:,2), 'x', 'MarkerSize', 10, 'LineWidth', 2);
semilogy(gl2(:,1), gl2(:,2), 'v', 'MarkerSize', 10, 'LineWidth', 2);

% load typical disk meaurements
x = load('/Users/gvajente/Documents/MATLAB/CRIME/old_measurements/2016_08_23/ringdown_2016_08_23_8_45am.mat');
semilogy(x.freqs, x.q(:,1), '+', 'MarkerSize', 10, 'LineWidth', 2);
legend('Caltech GeNS', 'Glasgow nodal (first suspension)', 'Glasgow nodal (second suspension)', 'Typical GeNS (not flame polished nor annealed')