% analyze the data from the CO2 Y laser transfer function

old1 = 1;
if old1 == 1
    t0 = 1138508180;
    dt = 2400;
    
else

    % get a start time from last 200s or so
    [a,b] = system('gpstime');
    tnow = str2num(b(strfind(b, 'GPS')+4:end));
    dt = 200;
    t0 = round(tnow - dt - 100);
end

chans = {'H1:TCS-ITMY_CO2_RIN_INLOOP_OUT_DQ', 'H1:TCS-ITMY_CO2_RIN_OUTLOOP_OUT_DQ', ...
    'H1:CAL-DELTAL_CTRL_DQ', 'H1:SYS-MOTION_C_PICO_G_CURRENT_X_POSITION', ...
    'H1:SYS-MOTION_C_PICO_G_CURRENT_Y_POSITION', 'H1:TCS-ITMY_CO2_LSRPWR_MTR_OUT16'};

% get the data
result = get_data(chans, 'raw', t0, dt);

dtFFT = 10;
dtStep = 5;

% loop through the data and get FFTs for the CO2 and DELTAL signals
maxList = [];
dcList = [];
cplList = [];

% get the coupling coefficients for DELTAL
DELTALcal = [21     -131.335      163.924
          22     -133.261      166.091
          23     -135.095      168.301
          24     -136.843      170.547
          25     -138.513      172.821
          26      -140.11      175.119
          27     -141.639      177.436
          28     -143.106       179.77];
aCal = polyfit(DELTALcal(1:5, 1), 10.^(DELTALcal(1:5, 2)/20),2);

close all
for ii = 115:floor(dt/dtStep - dtFFT/dtStep)-1
    maxSub = [];
    dataDC = [];
    for kk = 1:6

        nps = result(kk).rate;
        i0 = (ii-1)*nps*dtStep + 1;
        i1 = i0 + nps*dtFFT - 1;

        dataSub = result(kk).data(i0:i1);
        if kk == 3
            dataCAL = dataSub;
        end
        if kk < 3
            dataSub = detrend(dataSub);
            dataSub = dataSub.*hamming(numel(dataSub));
            
            f1 = fft(dataSub);

            % find the local maximum in the spectrum 
            % rough guide to line
            Irough = [22*dtFFT:28*dtFFT];
            if kk == 1
                [B, IX] = sort(abs(f1(Irough)), 'descend');
                ind0 = IX(1) + Irough(1) - 1;
            end
            fPeak = (ind0-1)/dtFFT;
            dispCoeff = aCal(1)*fPeak^2 + aCal(2)*fPeak + aCal(3);

            if kk == 3
               f1 = f1*dispCoeff; 
%                plot(abs(f1(200:400)));
%                grid on
%                hold all
%                pause(0.1)
%                ylim([0 1E-9])
            end
            
            
            
            % get the maximum components
            maxSub = [maxSub, abs(f1(ind0))];
        elseif kk == 3
            dataCAL = detrend(dataCAL);
            [Pxx, w] = pwelch(dataCAL, 5*2^12, [], 5*2^12, 2^12);
            ind0 = find(w==fPeak);
            maxSub = [maxSub, sqrt(Pxx(ind0))];
%             plot(w, sqrt(Pxx));
%             grid on
%             axis([21, 27, 0, 2.5E-7])
%             hold all
%             pause(0.1)
        else
            
            dataDC = [dataDC, mean(dataSub)];

        end
    end
    maxList = [maxList; maxSub];
    dcList = [dcList; dataDC];   
    
    % work out the coupling coefficients
    cpl = [dataDC(1), dataDC(2), ...
        maxSub(3)/(maxSub(1)*dataDC(3)), ...
        maxSub(3)/(maxSub(2)*dataDC(3))];
    cplList = [cplList; cpl];
    
end


close all
figure('Position', [100, 200, 1900, 800])
subplot(2,2,1)
ind1 = find(cplList(:, 2) == 3E4);
PICOsc = 6E-3;
plot(cplList(ind1, 1)*PICOsc, smooth(cplList(ind1, 4),3), 'o');
grid on
xlabel('X-PICO (mm)')
ylabel('coupling')
title(['Coupling vs X-position: t0 = ', num2str(t0), '. dt = ', num2str(dt)])

subplot(2,2,3)
ind1 = find(cplList(:, 1) == -1E4);
plot(cplList(ind1, 2)*PICOsc, smooth(cplList(ind1, 4),3), 'o');
grid on
xlabel('Y-PICO (mm)')
ylabel('coupling')
title('Coupling vs Y-position')


subplot(2,2,2:2:4)
scatter(cplList(:, 1)*PICOsc, cplList(:, 2)*PICOsc, 100, ...
    maxList(:, 3), 'filled');
grid on
xlabel('X-PICO (mm)')
ylabel('Y-PICO (mm)')
title(['Coupling vs X/Y position: ', chans{4}(1:30)], 'Interpreter', 'none')
colorbar
theta = 0:0.001:2*pi;
x = 170*cos(theta) + (-5000*6E-3);
y = 170*sin(theta) + (31000*6E-3);
hold all
plot(x,y, '-k', 'LineWidth', 2)
plot([-5E3*6E-3, -5E3*6E-3],[max(y), min(y)], '-k', 'LineWidth', 2)
plot([max(x), min(x)],[31E3,31E3]*6E-3, '-k', 'LineWidth', 2)

print('-dpdf', [mfilename(), '_full.pdf'])


figure
plot(cplList(:, 1));
grid on;
hold all
plot(cplList(:, 2));






figure('Position', [100, 200, 1900, 800])
subplot(2,2,1)
ind1 = 198:225;
plot(cplList(ind1, 1), smooth(cplList(ind1, 4),3), 'o');

a = polyfit(cplList(ind1, 1), smooth(cplList(ind1, 4),3),2);
%centX = -0.5*a(1)/a(2);
hold all 
x = -4E4:2E4;
%plot(x, a(1)*x.^2 + a(2)*x + a(3))

grid on
xlabel('X-PICO (counts)')
ylabel('coupling')
title(['Coupling to DARM vs X-position for sweep: maximum ~ -5000'])

ind1 = 260:293;
subplot(2,2,3)
%ind1 = find(cplList(:, 1) == -1E4);
plot(cplList(ind1, 2), smooth(cplList(ind1, 4),3), 'o');
%a = lsqcurvefit(@gaussConv, [4E-9, 3.1E4, 4E-9], cplList(ind1, 1), cplList(ind1, 4),'TolFun', 1E-9);
%a = polyfit(cplList(ind1, 2), smooth(cplList(ind1, 4),3),2);
centY = -0.5*a(1)/a(2);
hold all
y = -1E4:6E4;
%plot(y, a(1)*y.^2 + a(2)*y + a(3))
grid on
xlabel('Y-PICO (counts)')
ylabel('coupling')
title('Coupling to DARM vs Y-position for sweep: maximum ~ 31000')


subplot(2,2,2:2:4)
scatter(cplList(:, 1), cplList(:, 2), 100, ...
    maxList(:, 3), 'filled');
grid on
xlabel('X-PICO (counts)')
ylabel('Y-PICO (counts)')
title(['Coupling vs X/Y position: ', chans{4}(1:30)], 'Interpreter', 'none')

theta = 0:0.001:2*pi;
x = 0.17*cos(theta) - (-5000*6E-3);
y = 0.17*sin(theta) - (31000*6E-3);
hold all
plot(x,y, 'LineWidth', 2)

colorbar
orient landscape
print('-dpdf', [mfilename(), '.pdf'])




