%
%
% PLL_CTRL  ---- (+) ------ * -- (VCO_FREQ)
%                 |
%                 |
%              VCO_OFFSET
%
%
%


%% cavity scan of LHO
t1 = 1172864670 - 15*60;
t0 = t1 - 3600;

chanSLOW = {'H1:TCS-SIM_IFO_XARM_HOM_SPACING_HZ_OUTPUT.mean,s-trend', 'H1:TCS-SIM_IFO_YARM_HOM_SPACING_HZ_OUTPUT.mean,s-trend'};
chanSCAN1 = {'H1:ALS-C_COMM_VCO_TUNEOFS','H1:ALS-C_COMM_VCO_FREQUENCY'};

chanOUT = {'H1:ASC-X_TR_A_NSUM_OUT_DQ', 'H1:ASC-Y_TR_A_NSUM_OUT_DQ'},
% chanPLL = {'H1:ALS-C_COMM_PLL_CTRL_OUT_DQ','H1:ALS-C_COMM_PLL_ERR_OUT_DQ',...
%            'H1:ALS-C_COMM_A_RF_ERR_OUT_DQ', 'H1:ALS-C_REFL_DC_ERR_OUT_DQ'};

tic
[dataSIM, tSIM, info] = get_data(t0,t1, chanSLOW, []);
toc
[dataSCAN, tSC, info] = get_data(t0,t1, chanSCAN1, []);
toc
[dataTRANS, tTR, info] = get_data(t0,t1, chanOUT, []);
toc
% [dataPLL, tPLL, info] = get_data(t0,t1, chanPLL, []);
% toc

%% get the frequency
tloss = 872.15;
tscan1 = 1095;
tscan2 = 2983;

indTR = find((tTR >= tscan1).*(tTR<tscan2));
indSC = find((tSC >= tscan1).*(tSC<tscan2));
% indPLL = find((tPLL >= tscan1).*(tPLL<tscan2));

%% get the scan frequency 
tSCSub = tSC(indSC);
tTRSub = tTR(indTR);
% tPLLSub = tPLL(indPLL);

fSCAN = smooth(dataSCAN(indSC, 2), 32);
freqSCAN = dataSCAN(indSC, 2); 
freq2Knom = interp1(tSCSub(1:16:end), freqSCAN(1:16:end), tTRSub, 'cubic');
freq2K = interp1(tSCSub(1:16:end)-1, freqSCAN(1:16:end), tTRSub, 'cubic');
%freq2K = interp1(tSCSub,fSCAN, tTRSub, 'cubic');   % fSCAN rescaled to 2048 Hz

dataX = dataTRANS(indTR, 1);
dataY = dataTRANS(indTR, 2);


% 
%% get the peaks
[pks,indMax] = findpeaks(freq2K);
[pks,indMin] = findpeaks(-freq2K);

close all
plot(indMax, 'o'); hold all
plot(indMin, 'o')

% scans span the following segements
segList = [1 indMax(1)];
for ii = 1:numel(indMin)-1
    segList = [segList; indMax(ii) indMin(ii)];
    %segList = [segList; indMin(ii) indMax(ii+1)];
end
Nseg = size(segList,1);
close all


% got through the segements
FListX = [];
FListY = [];

tListX = [];
tListY = [];

for ii = 3:size(segList,1)
    
    for kk = 1:2
        ind0 = segList(ii,1);
        ind1 = segList(ii,2);

        % get the frequencies
        freq2KSUB = freq2K(ind0:ind1);

        if kk == 1
            subplot(2,1,1)
            TRXSub = dataX(ind0:ind1);
        else 
            TRXSub = dataY(ind0:ind1);
            subplot(2,1,2)
        end
        [pks, loc] = findpeaks(TRXSub.*(TRXSub>2));
        loc = loc(end-5:end);
        
        locAdd = [];
        for mm = 1:numel(loc)
           F0 = freq2KSUB(loc(mm));
           incList = abs(freq2KSUB-F0 + 1E4)/2<1000;
           outL = (TRXSub.*(TRXSub>0.5).*incList);
           indA = find(outL == max(outL));
           locAdd = [locAdd; indA(1)];
           
           if kk == 1
              tListX = [tListX; loc(mm) + ind0]; 
           else
              tListY = [tListY; loc(mm) + ind0];                
           end
           
        end
        df = (freq2KSUB(loc) - freq2KSUB(locAdd))/2;
        if kk == 1
            FListX = [FListX; df];
        else
            FListY = [FListY; df];
        end
        
%         plot(-(freq2KSUB - freq2KSUB(loc(2)))/2, TRXSub);
%         xlim([4.5E3 5.5E3])
%         %FList = [FList, diff(freq2K(loc(1:5)))/2];
%         hold all
%         grid on
%         pause(0.2)
%         
% 
%         hold all
        
        


    end    
    
    %pause(0.1)
end

% clean up any obviously wrong spots
indERR = find(abs(FListX-median(FListX)) > 1500);
for ii = 1:numel(indERR)
   FListX(indERR) = mean( [FListX(indERR-1), FListX(indERR+1)]);
end


%% plot the data and smoothed data
% fit the data
tfitX = tTRSub(tListX) - tloss;
[aX,s] = polyfit(tfitX, FListX,1);
steX = diag(sqrt(inv(s.R)*(inv(s.R))'*s.normr^2/s.df));
% get change in frequency
dFfitX0 = (aX(1)+([-steX(1), 0, +steX(1)]))*(tfitX(end) - tfitX(1));
dFSIMX = dataSIM(find(round(tSIM-tloss) == round(tfitX(end))),1) - ...
     dataSIM(find(round(tSIM-tloss) == round(tfitX(1))),1);
relAbsX = dFfitX0/dFSIMX;

 
tfitY = tTRSub(tListY) - tloss;
[aY,s] = polyfit(tfitY, FListY, 1);
steY = diag(sqrt(inv(s.R)*(inv(s.R))'*s.normr^2/s.df));
% get change in frequency
dFfitY0 = (aY(1)+([-steY(1), 0, +steY(1)]))*(tfitY(end) - tfitY(1))
dFSIMY = dataSIM(find(round(tSIM-tloss) == round(tfitY(end))),2) - ...
     dataSIM(find(round(tSIM-tloss) == round(tfitY(1))),2);
relAbsY = dFfitY0/dFSIMY;



close all
figure
subplot(2,1,1)
plot(tfitX, FListX, '-o');
hold all; 
plot(tfitX, aX(1)*tfitX + aX(2), 'LineWidth', 2)
plot(tSIM-tloss, dataSIM(:, 1))
grid on; axis([0 2000 4000 5500])
title(['XARM HOM frequency vs time, t0 = ', num2str(t0 + tloss)])
xlabel('Time since lock-loss (s)')
ylabel('HOM Frequency (Hz)')
legend('Measurement', 'Linear Fit', 'raw SIM data - XARM')
text(0.05, 0.1, ['Total XARM absorption = ', num2str(relAbsX(2), '%2.1f'), '\pm ', ...
          num2str(abs(relAbsX(2) - relAbsX(1)), '%2.1f'), ...
          ' times SIM model'], 'units', 'normalized', 'FontSize', 14, ...
          'BackgroundColor', [1,0.5,1])


subplot(2,1,2)
plot(tTRSub(tListY) - tloss, FListY, '-o');
hold all
plot(tfitY, aY(1)*tfitY +aY(2), 'LineWidth', 2)
plot(tSIM-tloss, dataSIM(:, 2))
grid on; axis([0 2000 4250 5750])
title(['YARM HOM frequency vs time, t0 = ', num2str(t0 + tloss)])
legend('Measurement', 'Linear Fit', 'raw SIM data - YARM')
xlabel('Time since lock-loss (s)')
ylabel('HOM Frequency (Hz)')
text(0.05, 0.1, ['Total YARM absorption = ', num2str(relAbsY(2), '%2.1f'), '\pm ', ...
          num2str(abs(relAbsY(2) - relAbsY(1)), '%2.1f'), ...
          ' times SIM model'], 'units', 'normalized', 'FontSize', 14, ...
          'BackgroundColor', [1,0.5,1])
orient landscape
print('-dpdf', ['LHO_cavity_scan_Data.pdf'])


    
    
