close all
clear all
clc

%%
printFigs = true;
svnDir = '/ligo/svncommon/SusSVN/sus/trunk/';
resultsDir = [svnDir 'HXDS/Common/Data/'];
figTag = [resultsDir '2021-11-05_HXDSSliderCalibration'];

inches_to_meters = 0.0254;
meters_to_microns = 1e6;

%% Raw data
ZM5HR_to_ZM6HR_value.inches = 184.64; % [in] D1900436.
ZM5HR_to_ZM6HR_unc.inches = 0.5; % [in] JSK guess of cookie-cutter accuracy, 
                                 % reduced from 3 [in] in LHO:60453 

ZM6HR_to_ZM6BaffleAR_value.m = 62.76e-3; % [m] As the beam flies, the distance 
                                         % between the entrance to the baffle 
                                         % and the HR surface of the optic 
                                         % (symmetric "in" and "out")
ZM6HR_to_ZM6BaffleAR_unc.m = 1e-3;                                       
                              

pit_arc_span_value.m = 15e-3; % [m] LHO:60480
pit_arc_span_unc.m = 2e-3; % [m] LHO:60480
pit_slider_span_value.DACct = 5000; % [ct] LHO:60480
pit_slider_value.DACct = [-2871 129 2129]; % [ct] LHO:60480
pit_arc_value.m = 1e-3*[15 24 30]; % [ct] LHO:60480

yaw_arc_value.m = 2e-2; % [m] LHO:60446
yaw_arc_unc.m = 2e-3; % [m] LHO:60446
yaw_slider_span_value.DACct = 4700; % [ct] LHO:60446

                              
%% Compute lever arm
ZM5HR_to_ZM6HR_value.m = ZM5HR_to_ZM6HR_value.inches * inches_to_meters;
ZM5HR_to_ZM6HR_unc.m = ZM5HR_to_ZM6HR_unc.inches * inches_to_meters;

ZM5HR_to_ZM6BaffleAR_value.m = ZM5HR_to_ZM6HR_value.m - ZM6HR_to_ZM6BaffleAR_value.m;
ZM5HR_to_ZM6BaffleAR_unc.m = ZM5HR_to_ZM6BaffleAR_value.m ...
                             * quadsum([ZM5HR_to_ZM6HR_unc.m, ...
                                        ZM6HR_to_ZM6BaffleAR_unc.m]);

%% Compute angle span
pit_angle_span_value.rad = pit_arc_span_value.m / ZM5HR_to_ZM6BaffleAR_value.m;
pit_angle_span_unc.rel = quadsum([(pit_arc_span_unc.m / pit_arc_span_value.m), ...
                                 (ZM5HR_to_ZM6BaffleAR_unc.m / ZM5HR_to_ZM6BaffleAR_value.m)]);   
pit_angle_span_unc.rad = pit_angle_span_value.rad * pit_angle_span_unc.rel;

yaw_angle_span_value.rad = yaw_arc_value.m / ZM5HR_to_ZM6BaffleAR_value.m;
yaw_angle_span_unc.rel = quadsum([(yaw_arc_unc.m / yaw_arc_value.m), ...
                                 (ZM5HR_to_ZM6BaffleAR_unc.m / ZM5HR_to_ZM6BaffleAR_value.m)]);
yaw_angle_span_unc.rad = yaw_angle_span_value.rad * yaw_angle_span_unc.rel;

%% Compute calibration (angle span per slider span)
pit_calibration_value.rad_per_ct = pit_angle_span_value.rad / pit_slider_span_value.DACct;
pit_calibration_unc.rel = pit_angle_span_unc.rel;
pit_calibration_unc.rad_per_ct = pit_calibration_value.rad_per_ct * pit_angle_span_unc.rel;

yaw_calibration_value.rad_per_ct = yaw_angle_span_value.rad / yaw_slider_span_value.DACct;
yaw_calibration_unc.rel = yaw_angle_span_unc.rel;
yaw_calibration_unc.rad_per_ct = yaw_calibration_value.rad_per_ct * yaw_angle_span_unc.rel;


%% Cross-check by fitting the data directly, rather than computing in such a way that gives uncertainty
pit_fitCoeffs = polyfit(pit_slider_value.DACct,1e6*pit_arc_value.m/ZM5HR_to_ZM6BaffleAR_value.m,1);

%% 
pit_calibration_collection_value.urad_per_ct = [meters_to_microns*pit_calibration_value.rad_per_ct,...
                    4.498e-1];
pit_calibration_collection_unc.urad_per_ct = [meters_to_microns*pit_calibration_unc.rad_per_ct,...
                    4.498e-1];
pit_calibration_collection_xticks = {'ZM5-ZM6 Baffle (today)',...
                                     'Electronics Dead Reckon'};
 
[pit_calibration_collection_weightedmean, ...
    pit_calibration_collection_weightederr] = weightedmean( ...
                                              pit_calibration_collection_value.urad_per_ct,...
                                              pit_calibration_collection_unc.urad_per_ct);

yaw_calibration_collection_value.urad_per_ct = [meters_to_microns*yaw_calibration_value.rad_per_ct,...
                    9.075e-1,...
                    7.857e-1,...
                    4.0127e-1];
yaw_calibration_collection_unc.urad_per_ct = [meters_to_microns*yaw_calibration_unc.rad_per_ct,...
                    2.274e-1,...
                    5.736e-2,...
                    4.0127e-1];
yaw_calibration_collection_xticks = {'ZM5-ZM6 Baffle (today)',...
                                     'ZM5-ZM6 Baffle (LHO:60453)',...
                                     'ZM4-ZM5',...
                                     'Electronics Dead Reckon'};
                
[yaw_calibration_collection_weightedmean, ...
    yaw_calibration_collection_weightederr] = weightedmean( ...
                                              yaw_calibration_collection_value.urad_per_ct([1 3 4]),...
                                              yaw_calibration_collection_unc.urad_per_ct([1 3 4]));
                                          
                                          
disp('DOF       Unit        Value      Abs. Unc.  (Rel. Unc.)')
disp(['PIT       [urad/ct]   ' num2str(pit_calibration_collection_weightedmean,4) '     ' num2str(pit_calibration_collection_weightederr,3) '     (' num2str(100*(pit_calibration_collection_weightederr/pit_calibration_collection_weightedmean),3) '%)'])
disp(['YAW       [urad/ct]   ' num2str(yaw_calibration_collection_weightedmean,4) '     ' num2str(yaw_calibration_collection_weightederr,3) '     (' num2str(100*(yaw_calibration_collection_weightederr/yaw_calibration_collection_weightedmean),3) '%)'])

%%

figure(1)
figName{1} = [figTag '_ZM5toZM6Baffle_Pitch'];
plot(pit_slider_value.DACct,1e6*pit_arc_value.m/ZM5HR_to_ZM6BaffleAR_value.m,'*',...
     pit_slider_value.DACct,1e6*pit_slider_value.DACct*pit_calibration_value.rad_per_ct + pit_fitCoeffs(end),...
     pit_slider_value.DACct,polyval(pit_fitCoeffs,pit_slider_value.DACct),'--')
xlabel('PIT Slider [ct]')
ylabel('Displacement [urad]')
legend('LHO:60480 "Ruler Position" Data / (ZM5HR to ZM6BaffleAR)',...
       [num2str(meters_to_microns*pit_calibration_value.rad_per_ct,4) ' [urad/ct] Calibration + Arb. Offset'],...
       'Direct Fit to LHO:60480 "Ruler Position" Data / (ZM5HR to ZM6BaffleAR)',...
       'Location','SouthEast')
title({'HXDS Pitch Slider Calibration';'ZM5 to ZM6 Baffle (2021-11-05)'})

   
figure(2)
figName{2} = [figTag '_WeightedMean'];
subplot(211)
errorbar(1:2,pit_calibration_collection_value.urad_per_ct,...
             pit_calibration_collection_unc.urad_per_ct,'b*');
hold on
plot(0:5,ones(1,6)*pit_calibration_collection_weightedmean,'r-',...
     0:5,ones(1,6)*pit_calibration_collection_weightedmean+pit_calibration_collection_weightederr,'r--',...
     0:5,ones(1,6)*pit_calibration_collection_weightedmean-pit_calibration_collection_weightederr,'r--')
hold off
set(gca,'XTick',1:2,'XTickLabel',pit_calibration_collection_xticks,'FontSize',10)
xlim([0 5])


xlabel('Measurement')
ylabel('Calibration [urad/ct]')
title({'HXDS Slider Calibration';'Pitch'})
legend('Calculations',...
       ['Weighted Mean: ' ...
        num2str(pit_calibration_collection_weightedmean,4) ' +/- ' ...
        num2str(pit_calibration_collection_weightederr,3) ' [urad/ct]'])

subplot(212)
errorbar(1:4,yaw_calibration_collection_value.urad_per_ct,...
             yaw_calibration_collection_unc.urad_per_ct,'b*');
hold on
plot(0:5,ones(1,6)*yaw_calibration_collection_weightedmean,'r-',...
     0:5,ones(1,6)*yaw_calibration_collection_weightedmean+yaw_calibration_collection_weightederr,'r--',...
     0:5,ones(1,6)*yaw_calibration_collection_weightedmean-yaw_calibration_collection_weightederr,'r--')
hold off
set(gca,'XTick',1:4,'XTickLabel',yaw_calibration_collection_xticks,'FontSize',10)
xlim([0 5])
xlabel('Measurement')
ylabel('Calibration [urad/ct]')
title('Yaw')
legend('Calculations',...
       ['Weighted Mean: ' ...
        num2str(yaw_calibration_collection_weightedmean,4) ' +/- ' ...
        num2str(yaw_calibration_collection_weightederr,3) ' [urad/ct]'],...
        'Location','SouthWest')
    
%%
if printFigs
    for iFig = 1:length(figName)
        figure(iFig)
        FillPage('w')
        IDfig(', J. Kissel')
        saveas(gcf,[figName{iFig} '.pdf'])
    end
end
    
function [xbar,err] = weightedmean(x,sigma)
    w = 1./(sigma.^2);

    xbar = sum(x.*w) / sum(w);
   
    err = sqrt(1/sum(w));
end