%#ok<*UNRCH>
if 0 % set(0, '...')
    addpath('../')
    addpath('../../')

    set(0,'defaulttextinterpreter','tex')
    set(groot, 'defaultAxesTickLabelInterpreter','tex');
    set(groot, 'defaultLegendInterpreter','tex');
    % set(0,'defaultfigureposition',[400 300 550 400]);
    set(0, 'DefaultFigureWindowStyle', 'docked')

    set(0, 'DefaultAxesFontName', 'Times');
    % or any of the following...
    set(0, 'defaultUicontrolFontName', 'Times');
    set(0, 'defaultUitableFontName', 'Times');
    set(0, 'defaultAxesFontName', 'Times');
    set(0, 'defaultTextFontName', 'Times');
    set(0, 'defaultUipanelFontName', 'Times');

    set(0, 'DefaultLineLineWidth', 1.5);


    set(0,'DefaultFigureWindowStyle','docked')
    % set(0,'DefaultFigureWindowStyle','default')

    xfcd.red = hex2dec({'e5' '00' '00'})'/255;
    xfcd.orange = hex2dec({'f9' '73' '06'})'/255;
    xfcd.dullorange = hex2dec({'d8' '86' '3b'})'/255;
    xfcd.paleorange = hex2dec({'ff' 'a7' '56'})'/255;
    xfcd.lightorange = hex2dec({'fd' 'aa' '48'})'/255;

    xfcd.green = hex2dec({'15' 'b0' '1a'})'/255;
    xfcd.lightgreen = hex2dec({'96' 'f9' '7b'})'/255;
    xfcd.leafygreen = hex2dec({'5c' 'a9' '04'})'/255;
    xfcd.palegreen = hex2dec({'c7' 'fd' 'b5'})'/255;
    xfcd.seagreen = hex2dec({'53' 'fc' 'a1'})'/255;
    xfcd.aquamarine = hex2dec({'04' 'd8' 'b2'})'/255;

    xfcd.blue = hex2dec({'03' '43' 'df'})'/255;
    xfcd.lightblue = hex2dec({'95' 'd0' 'fc'})'/255;
    xfcd.cerulean = hex2dec({'04' '85' 'd1'})'/255;
    xfcd.sky = hex2dec({'82' 'ca' 'fc'})'/255;

    xfcd.magenta = hex2dec({'c2' '00' '78'})'/255;
    xfcd.lightmagenta = hex2dec({'fa' '5f' 'f7'})'/255;
    xfcd.purple = hex2dec({'7e' '1e' '9c'})'/255;
    xfcd.lightviolet = hex2dec({'d6' 'b4' 'fc'})'/255;

    xfcd.pink = hex2dec({'ff' '81' 'c0'})'/255;
    xfcd.babypink = hex2dec({'ff' 'b7' 'ce'})'/255;

    xfcd.brown = hex2dec({'65' '37' '00'})'/255;
    xfcd.lightbrown = hex2dec({'ad' '81' '50'})'/255;
    xfcd.darkbrown = hex2dec({'34' '1c' '02'})'/255;

    defaultColor = {[0, 0.4470, 0.7410];
        [0.8500, 0.3250, 0.0980];
        [0.9290, 0.6940, 0.1250];
        [0.4940, 0.1840, 0.5560];
        [0.4660, 0.6740, 0.1880];
        [0.3010, 0.7450, 0.9330];
        [0.6350, 0.0780, 0.1840];
        xfcd.lightmagenta;
        xfcd.aquamarine;
        [0, 0, 0]
        };

    defaultLine = {'--', '-', ':', '-.'};

    return;
end


if 0 % get data
    %     chan = {'L1:GDS-CALIB_STRAIN_CLEAN'};
    %     chan = {'H1:OAF-CAL_DARM_DQ';
    %         'H1:OAF-CLEAN_DELTAL_EXTERNAL_DQ'};
    chan = {'H1:OAF-CLEAN_DELTAL_EXTERNAL_DQ'};
    %     chan = {'L1:OMC-DCPD_SUM_OUT_DQ';
    %         'L1:CAL-PCALY_RX_PD_OUT_DQ'};
    %     chan = {'L1:CAL-DELTAL_A_DQ';
    %         'L1:CAL-DELTAL_B_DQ'};


    %         t0 = 1360830700; t1 = t0 + 900; % FIS optimized HF
    %         t0 = 1360833930; t1 = t0 + 900; % FIAS
    %         t0 = 1360822844; t1 = t0 + 900; % FDS optimized HF
    t0 = 1360825873; t1 = t0 + 900; % FDAS
    %     t0 = 1360834978; t1 = t0 + 3600; % No sqz


    %     t1 = t0 + 2^11;
    dur = t1 - t0;

    fs = 2^14;
    maxdatanum = 2^25;

    interval = maxdatanum/fs;

    %     raw = {};

    for k = 1:length(chan)
        cdsdata = [];
        start = t0;
        for j = 1:ceil(dur/interval)
            d = min(t1-start, interval);
            cds = get_data2(chan{k}, 'raw', start, d, 'lho');
            cdsdata = [cdsdata; cds.data];
            start = start + d;
            clear cds
        end
        raw = [raw; cdsdata];
    end

    return;
end


if 0 % Welch PSD

    %     freq = logspace(log10(20), log10(200), 101);
    %     freq = [freq(1:end-1), logspace(log10(200), log10(600), 241)];
    %     freq = [freq(1:end-1), logspace(log10(600), log10(6000), 80)];
    %     freq = [freq(1:end-1), logspace(log10(200), log10(6000), 900)];


    freq = logspace(log10(20), log10(6000), 100);

    %     freq = 434.8:0.01:435.2;

    %     allS = {};
    % allS16 = {};
    allS50 = {};
    % allS84 = {};
    %     allSavg = {};

    calibration = 'OAF';

    for j = 1:length(raw)
        %     for j = 1

        data = raw{j};
        %
        % t = 0:1/fs:5;
        % data = sin(2*pi*100*t');
        %         data = alldata(1:fs*2^11);
        %         data = oaf;


        fs = 2^14;

        %     avgCycles = 2^5; % oscillation cycles in a segment
        % more cycles -> higher spectral resolution, but less
        % segments available, less statistical stability.



        %         S = {};
        %         S16 = [];
        S50 = [];
        %         S84 = [];
        %         Savg = [];

        for f = freq

            Pm = [];

            %         windowSize = ceil(log2(avgCycles*fs/f));
            %         windowSize = 2^windowSize;

            windowSize = fs;
            window = hanning(windowSize);

            frames = floor(length(data)/(windowSize/2));

            for m = 1:frames-1
                sample = data((m-1)/2*windowSize+1 : (m+1)/2*windowSize);

                %         P = fft(sample, windowSize);
                %         P = abs(P).^2;

                % single-sided PSD
                P = abs(sum( window.*sample.*exp(-1i*2*pi*(0:windowSize-1)*f/fs)' ))^2 /sum(window.^2);

                P = P*(1/80)^2; % normalization factor

                %         P = abs(sum( sample.*exp(-1i*2*pi*(0:windowSize-1)*f/fs)' ))^2;
                Pm = [Pm; P]; % list of individual segment PSD at f
            end

            %             S = [S, Pm];
            perc = quantile(Pm, [0.16 0.5 0.84]);
            %             S16 = [S16, perc(1)];
            S50 = [S50, perc(2)];
            %             S84 = [S84, perc(3)];
            %             Savg = [Savg, mean(Pm)];

            fprintf('PSD done at freq = %g\n', f)
        end

        if strcmp(calibration, 'OAF')
            tf = abs(1-1i*freq/30).^12./abs(1-1i*freq/0.3).^12;
            S50 = S50.*tf/4000^2; % to strain^2/Hz
        end


        %         allS = [allS; S];
        %     allS16 = [allS16; S16];
        allS50 = [allS50; S50];
        %     allS84 = [allS84; S84];
        %         allSavg = [allSavg; Savg];


    end
    return;
end


if 0 % fitting

    arg = {'P_IM4'; 57;
        'L_FC'; 295;
        'L_OMC'; 1.131/2;
        'T_ETMX'; 3.9e-6;
        'T_ETMY'; 3.8e-6;
        'T_ITMX'; 0.015;
        'T_ITMY'; 0.015;
        'T_SR'; 0.323;
        'eta_IM4'; 1;
        'loss_FC'; 24e-6;
        'loss_INJ'; 0.045;
        'loss_OMCtrans'; 0.041;
        'mismatch_FC'; 0.03;
        };

    % find the solution in vector


    flt = length(freq)/100*3;
    S_fisdiff = S_fis - S_nosqz;
    S_fisdiff = medfilt1(hampel(S_fisdiff, flt), flt);

    S_fiasdiff = S_fias - S_nosqz;
    S_fiasdiff = medfilt1(hampel(S_fiasdiff, flt), flt);

    S_fdsdiff = S_fds - S_nosqz;
    S_fdsdiff = medfilt1(hampel(S_fdsdiff, flt), flt);

    S_fdasdiff = S_fdas - S_nosqz;
    S_fdasdiff = medfilt1(hampel(S_fdasdiff, flt), flt);

    id = freq > 800 & freq < 1500;
    f_nosqz = freq(id); % high frequency part to match shot noise
    clasFacASD = 1/5; % portion of measured ASD that's classical noise
    temp = medfilt1(hampel(S_nosqz, flt), flt);
    S_nosqz_HF = temp(id)*(1-clasFacASD^2);

    id = freq > 30 & freq < 2000;
    f_fis = freq(id); % remove edge from medfilt1
    S_fisdiff = S_fisdiff(id);

    S_fdsdiff = S_fdsdiff(id);
    S_fdasdiff = S_fdasdiff(id);

    id = freq > 30 & freq < 2000;
    f_fias = freq(id);
    S_fiasdiff = S_fiasdiff(id);



    argin = @(x) {'loss_RO'; x(1)*1e-3; 'mismatch_OMC'; x(2)*1e-3; 'phi_SR'; 90 + x(3)*1e-2; ...
        'loss_yarm'; x(4)*1e-6; 'loss_xarm'; x(4)*1e-6; 'loss_SQZUnknown'; x(5)*1e-3; ...
        'mismatch_FC2OMC'; x(6)*1e-3; 'mismatchPsi_FC'; x(7);
        'T_FC1'; 0; 'z_eff_dB'; 16.9};

    lowerbound = [ 50,  50, -50,  60,  0,  10,   0,  80];
    startpoint = [ 50,  85, 6.2,  90,  0,  80, 3.4, 125];
    upperbound = [200, 300,  50, 100, 90, 150,  80, 140];


    func = @(x) db(abs(...
        [o_O('param', [arg; argin(x); 'z_eff_dB'; 0], 'omegas', 2*pi*f_nosqz, 'traces', 'S_h')'; ...
        o_O('param', [arg; argin(x); 'theta_SQZ'; 0;], 'omegas', 2*pi*f_fis, 'traces', 'S_h')'-o_O('param', [arg; argin(x); 'z_eff_dB'; 0], 'omegas', 2*pi*f_fis, 'traces', 'S_h')'; ...
        o_O('param', [arg; argin(x); 'theta_SQZ'; x(8);], 'omegas', 2*pi*f_fias, 'traces', 'S_h')'-o_O('param', [arg; argin(x); 'z_eff_dB'; 0], 'omegas', 2*pi*f_fias, 'traces', 'S_h')']...
        ))/2;



    %     figure;
    %     plot(data);
    %     hold on; grid on;
    %     plot(func(startpoint))
    %     id = 3;
    %     for j = linspace(lowerbound(id), upperbound(id), 10)
    %         startpoint(id) = j;
    %         plot(func(startpoint));
    %         loglog(f_fis, sqrt(o_O('param', [arg; argin(startpoint); 'theta_SQZ'; 90], 'omegas', 2*pi*f_fis, 'traces', 'S_h')'))
    %         set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log')
    %     end
    %     return;

    ctl = [  1,   3,  4,   5,  6];
    val = [140, -54, 75, 130, 80];

    %     fisargs = {};
    %     fiasargs = {};
    %     fdsargs = {};
    %     fdasargs = {};
    %     fisres = [];
    %     fdsres = [];

    for j = 1:length(ctl)
        st = startpoint;
        lb = lowerbound;
        ub = upperbound;
        st(ctl(j)) = val(j);
        lb(ctl(j)) = val(j);
        ub(ctl(j)) = val(j);

        data = [S_nosqz_HF, S_fisdiff, S_fiasdiff];
        data = db(abs(data))/2;

        options = optimoptions(@lsqnonlin,'Algorithm','levenberg-marquardt');
        [result,~,residual,~,~,~,jacobian] = lsqnonlin(@(x) abs(func(x) - data'), ...
            st, lb, ub, options);
        %     CI = nlparci(result,residual,'jacobian',jacobian);


        fisarg = [argin(result); 'theta_SQZ'; 0];
        fiasarg = [argin(result); 'theta_SQZ'; result(8)];
        fisres = [fisres; sum(residual)];

        fisargs = [fisargs; {fisarg}];
        fiasargs = [fiasargs; {fiasarg}];

        data = [S_fdsdiff, S_fdasdiff];
        data = db(abs(data))/2;




        [result,~,residual,~,~,~,jacobian] = lsqnonlin(@(x) db(abs( ...
            [o_O('param', [arg; fisarg; 'T_FC1'; 909e-6; 'deltaOmega_FC'; -2*pi*x(1)], 'omegas', 2*pi*f_fis, 'traces', 'S_h')' - o_O('param', [arg; fisarg; 'z_eff_dB'; 0], 'omegas', 2*pi*f_fis, 'traces', 'S_h')'; ...
            o_O('param', [arg; fisarg; 'theta_SQZ'; x(2); 'T_FC1'; 909e-6; 'deltaOmega_FC'; -2*pi*x(1)], 'omegas', 2*pi*f_fis, 'traces', 'S_h')' - o_O('param', [arg; fisarg; 'z_eff_dB'; 0], 'omegas', 2*pi*f_fis, 'traces', 'S_h')'] ))/2 ...
            - data', ...
            [20, 90], [0, 60], [100, 120], options);
        %     CI = nlparci(result,residual,'jacobian',jacobian);

        x = result;

        fdsarg = [fisarg; 'T_FC1'; 909e-6; 'deltaOmega_FC'; -2*pi*x(1)];
        fdasarg = [fisarg; 'theta_SQZ'; x(2); 'T_FC1'; 909e-6; 'deltaOmega_FC'; -2*pi*x(1)];
        fdsargs = [fdsargs; {fdsarg}];
        fdasargs = [fdasargs; {fdasarg}];
    end

    return;
end


if 1 % plot

    set(0,'DefaultFigureWindowStyle','normal')

    figure;

    for j = 1:length(fisargs)
        subplot(ceil(length(fisargs)/2),2,j)
        fisarg = fisargs{j};
        fiasarg = fiasargs{j};
        fdsarg = fdsargs{j};
        fdasarg = fdasargs{j};

        Sclas = S_nosqz - o_O('param', [arg; fisarg; 'z_eff_dB'; 0], 'omegas', 2*pi*freq, 'traces', 'S_h');
        Sshot = o_O('param', [arg; fisarg; 'z_eff_dB'; 0], 'omegas', 2*pi*freq, 'traces', 'S_h_shot');



        alpha = 0.3;
        %     temp = semilogx(freq, db(sqrt((Snosqz_0122 - classical)./Sshot)), ...
        %         'color', defaultColor{1});
        %     temp.Color(4) = alpha;
        temp = semilogx(freq, db(sqrt((S_fis - Sclas)./Sshot)), ...
            'color', defaultColor{1}, 'linewidth', 1);
        temp.Color(4) = alpha;
        hold on;
        temp = semilogx(freq, db(sqrt((S_fias - Sclas)./Sshot)), ...
            'color', defaultColor{4}, 'linewidth', 1);
        temp.Color(4) = alpha;
        temp = semilogx(freq, db(sqrt((S_fds - Sclas)./Sshot)), ...
            'color', defaultColor{2}, 'linewidth', 1);
        temp.Color(4) = alpha;
        temp = semilogx(freq, db(sqrt((S_fdas - Sclas)./Sshot)), ...
            'color', defaultColor{3}, 'linewidth', 1);
        temp.Color(4) = alpha;

        semilogx(freq, db(sqrt( o_O('param', [arg; fisarg], 'omegas', 2*pi*freq, 'traces', 'S_rel_SQL'))), 'color', xfcd.aquamarine)

        semilogx(freq, db(sqrt( o_O('param', [arg; fisarg], 'omegas', 2*pi*freq, 'traces', 'S_rel'))), '--', 'color', defaultColor{1})
        semilogx(freq, db(sqrt( o_O('param', [arg; fiasarg;], 'omegas', 2*pi*freq, 'traces', 'S_rel'))), '--', 'color', defaultColor{4})
        semilogx(freq, db(sqrt( o_O('param', [arg; fdsarg], 'omegas', 2*pi*freq, 'traces', 'S_rel'))), '--', 'color', defaultColor{2})
        semilogx(freq, db(sqrt( o_O('param', [arg; fdasarg;], 'omegas', 2*pi*freq, 'traces', 'S_rel', 'echo', 0))), '--', 'color', defaultColor{3})
        semilogx(freq, db(sqrt( o_O('param', arg, 'omegas', 2*pi*freq, 'traces', 'S_rel', 'echo', 0))), 'k--')

        ylim([-10 20])

        grid on; box on; grid(gca, 'minor');
        xlabel('{\it f} [Hz]')
        ylabel('Relative DARM QN [dB]')

        %             legend('FIS measured', 'FIAS measured', 'FDS measured', 'FDAS measured', 'SQL', 'FIS theory', 'FIAS theory', 'FDS theory', 'FDAS theory', 'No SQZ', ...
        %                 'fontsize', 10, 'NumColumns', 2)
        %         legend('FIS measured', 'FIAS measured', 'FDS measured', 'FDAS measured', 'FIS theory', 'FIAS theory', 'FDS theory', 'FDAS theory', 'fontsize', 10, 'NumColumns', 2)
        xlim([20 3000])
        ylim([-10 30])
        %     legend boxoff
        %         title('250 kW, Frequency (In)Dependent Squeezing')
        title(arg_name{j})

        set(gca, 'fontsize', 12)
        %     exportgraphics(gcf, "all.png", 'Resolution', 600)
    end

    return



    figure;

    %     for j = 1:length(fisargs)
    for j = 5
        %         subplot(ceil(length(fisargs)/2),2,j)
        fisarg = fisargs{j};
        fiasarg = fiasargs{j};
        fdsarg = fdsargs{j};
        fdasarg = fdasargs{j};

        Sclas = S_nosqz - o_O('param', [arg; fisarg; 'z_eff_dB'; 0], 'omegas', 2*pi*freq, 'traces', 'S_h');
        Sshot = o_O('param', [arg; fisarg; 'z_eff_dB'; 0], 'omegas', 2*pi*freq, 'traces', 'S_h_shot');

        alpha = 0.6;
        %     temp = semilogx(freq, db(sqrt((Snosqz_0122 - classical)./Sshot)), ...
        %         'color', defaultColor{1});
        %     temp.Color(4) = alpha;
        temp = loglog(freq, sqrt(S_nosqz), ...
            'k-', 'linewidth', 1);
        temp.Color(4) = alpha;
        hold on;

        temp = loglog(freq, sqrt(S_fis), ...
            'color', defaultColor{1}, 'linewidth', 1);
        temp.Color(4) = alpha;

        temp = loglog(freq, sqrt(S_fias), ...
            'color', defaultColor{4}, 'linewidth', 1);
        temp.Color(4) = alpha;
        temp = loglog(freq, sqrt(S_fds), ...
            'color', defaultColor{2}, 'linewidth', 1);
        temp.Color(4) = alpha;
        temp = loglog(freq, sqrt(S_fdas), ...
            'color', defaultColor{3}, 'linewidth', 1);
        temp.Color(4) = alpha;

        loglog(freq, sqrt( o_O('param', [arg; fisarg; 'z_eff_dB'; 0], 'omegas', 2*pi*freq, 'traces', 'S_h')), 'k--')
        loglog(freq, sqrt( o_O('param', [arg; fisarg], 'omegas', 2*pi*freq, 'traces', 'S_h')), '--', 'color', defaultColor{1})
        loglog(freq, sqrt( o_O('param', [arg; fiasarg], 'omegas', 2*pi*freq, 'traces', 'S_h')), '--', 'color', defaultColor{4})
        loglog(freq, sqrt( o_O('param', [arg; fdsarg], 'omegas', 2*pi*freq, 'traces', 'S_h')), '--', 'color', defaultColor{2})
        loglog(freq, sqrt( o_O('param', [arg; fdasarg], 'omegas', 2*pi*freq, 'traces', 'S_h', 'echo', 0)), '--', 'color', defaultColor{3})

        %     ylim([-10 20])

        grid on; box on;
        xlabel('{\it f} [Hz]')
        ylabel('DARM [strain/rtHz]')

        legend('DARM, No SQZ', 'DARM, FIS', 'DARM, FIAS', 'DARM, FDS', 'DARM, FDAS', 'QN, No SQZ', 'QN, FIS', 'QN, FIAS', 'QN, FDS', 'QN, FDAS', ...
            'fontsize', 10, 'NumColumns',2, 'location', 'northwest')
        %     xlim([20 6000])
        ylim([2e-24, 4e-22])
        %     title('250 kW, Frequency (In)Dependent Squeezing')
        title(arg_name{j})

        set(gca, 'fontsize', 12)

        %     exportgraphics(gcf, "DARM.png", 'Resolution', 600)

    end




end



