%% Touch Probe Data: Post-Processing and Analysis
% This code processes the output file generated by Q-Server after touch probe measurements to produce plots and statistical summaries. Each line of the file must contain an XYZ coordinate, following the sequence shown in the figure below. This sequence is repeated for all poles.
%
%
%
% Table of Contents
% Reading touch probe output file and assigning information to variables
% Plots of top and bottom jaw measurements
% Statistics of top and bottom jaw measurements
% Plots of top and bottom jaw roll vs. z and gap vs. z
%     Notes on the roll angle
% Backup and testing codes
% List of improvements:
% Add a line in the touch probe output file that shows date, the nominal gap, gap reading, and tapper reading. Those pieces of information should be added to the statistics (end of the page)
% Use 'createFig' function to generate high resolution pdfs from the results
% Include coordinate system corrections (the current version is simply reading what Dover bench produced)
% Johann E. Baader, 10/01/2025
%
%% Reading touch probe output file and assigning information to variables

clear all

cd ('S:\magdata\LCLS-II-HE\Dover bench touch probe IO files')

%data=importdata("Kugler_out5_fullUndulator.out", ' ', 1);
data=importdata("Kugler.out", ' ', 1);
data = data.data; % x, y, and z in m

% Parameters
%sphereDiam = 5e-3; %m
poleNum1stZ = 1;
period = 19.5e-3; %m

indexArray=1:1:length(data);

index = zeros(1,length(data)); 
index(1:4:length(data))=1;
index = logical(index);
index = indexArray(index);
posUB = data(index, :); % Saves all the x, y, z positions, in m, for a specific pole location (see Pos 1 in the figure above)

index = zeros(1,length(data));
index(2:4:length(data))=1;
index = logical(index);
index = indexArray(index);
posUA = data(index, :); % Saves all the x, y, z positions, in m, for a specific pole location (see Pos 2 in the figure above)

index = zeros(1,length(data));
index(3:4:length(data))=1;
index = logical(index);
index = indexArray(index);
posLA = data(index, :); % Saves all the x, y, z positions, in m, for a specific pole location (see Pos 3 in the figure above)

index = zeros(1,length(data));
index(4:4:length(data))=1;
index = logical(index);
index = indexArray(index);
posLB = data(index, :); % Saves all the x, y, z positions, in m, for a specific pole location (see Pos 4 in the figure above)

Z = [posUB(:,3) posUA(:,3) posLB(:,3) posLA(:,3)]';
Z = mean(Z)';

poleNumbers = round((Z-Z(1))/period) + poleNum1stZ;

deltaX = posUA - posUB;
deltaX = deltaX(:,1);
rollU = atan((posUA(:,2) - posUB(:,2))./deltaX);
deltaX = posLA - posLB;
deltaX = deltaX(:,1);
rollL = atan((posLA(:,2) - posLB(:,2))./deltaX);

%
%% Plots of top and bottom jaw measurements

get(groot,'DefaultAxesXTickLabelRotation');   % likely returns 45
set(groot,'DefaultAxesXTickLabelRotation','remove')  % clears the default

fontsize = 14;
z=posUB(:,3); %m
y = 1e3*(posUB(:,2) + posUA(:,2))/2; % Average the two measurements on the same pole, in mm
meanY_U = mean(y); % mm
yUP_copy = y; % mm
y0 = y - meanY_U; % mm

p1 = polyfit(poleNumbers, y0, 1);
yfit = polyval(p1, poleNumbers); % mm

p2 = polyfit(z, y0, 1); % the slope p2(1) will be given in [dy0] / [dz] = mm / m = 1e-3. Therefore, a atan operation returns an angle in mrad
slope_U = atan(p2(1)) * 1e3; % urad

p3 = polyfit(poleNumbers, y, 1);
top1 = polyval(p3, 1);
top28 = polyval(p3, 28);
top90 = polyval(p3, 90);
topLast = polyval(p3, poleNumbers(end));

% Create taller figure
figure('Position',[100 100 1200 800]) 

% ========= TOP SUBPLOT =========
ax1 = subplot(2,1,1);
ax1.XTickLabelRotation = 0;
plot(ax1, poleNumbers, y0, '-b', 'LineWidth', 1); hold(ax1,'on');
plot(ax1, poleNumbers, yfit, '--k', 'LineWidth', 1);
xline(28, 'LineWidth',1, 'LineStyle', '-')
xline(90, 'LineWidth',1, 'LineStyle', '-')
xlabel(ax1, 'Pole number');
ylabel(ax1, 'Y dev. from mean (mm)');
set(ax1, 'FontSize', fontsize); grid(ax1,'on');

% Title for top subplot (shifted above top x-axis)
title(ax1, 'Top Jaw Pole Tips', 'FontSize', fontsize+2, ...
      'FontWeight','bold', 'Units','normalized', 'Position',[0.5 1.20 0]);

% Overlay axis for z scale
ax2 = axes('Position', ax1.Position, 'Color','none', 'XAxisLocation','top');
ax2.YAxis.Visible = 'off';
xlabel(ax2, 'z (m)');
ax1.XLim = [min(poleNumbers) max(poleNumbers)];
ax2.XLim = [min(z) max(z)];
text(0.8, 0.2, sprintf('Mean: %+.3f mm', meanY_U), 'Units','normalized', 'FontSize',fontsize)
text(0.8, 0.1, sprintf('Slope: %+.3f µrad', slope_U), 'Units','normalized', 'FontSize',fontsize)
%linkaxes([ax1 ax2],'x');

% Adjust subplot height for breathing room
pos = ax1.Position; pos(4) = pos(4) - 0.08; ax1.Position = pos; ax2.Position = pos;

if mean(diff(poleNumbers))>10
    ax1.XTick = poleNumbers;
else
    ax1.XTick = 0:10:poleNumbers(end);
end

% ========= BOTTOM SUBPLOT =========
z = posLB(:,3); %m

firstBtmPolePos = z(1);
y = 1e3*(posLB(:,2) + posLA(:,2))/2; %mm
meanY_L = mean(y);
yLO_copy = y;
y0 = y - meanY_L; %mm

p1 = polyfit(poleNumbers, y0, 1);
yfit = polyval(p1, poleNumbers);

p2 = polyfit(z, y0, 1); % the slope p2(1) will be given in [dy0] / [dz] = mm / m = 1e-3. Therefore, a atan operation returns an angle in mrad
slope_L = atan(p2(1)) * 1e3; % urad

p3 = polyfit(poleNumbers, y, 1);
down1 = polyval(p3, 1);
down28 = polyval(p3, 28);
down90 = polyval(p3, 90);
downLast = polyval(p3, poleNumbers(end));


ax3 = subplot(2,1,2);
ax3.XTickLabelRotation = 0;
plot(ax3, poleNumbers, y0, '-r', 'LineWidth', 1); hold(ax3,'on');
plot(ax3, poleNumbers, yfit, '--k', 'LineWidth', 1);
xline(28, 'LineWidth',1, 'LineStyle', '-')
xline(90, 'LineWidth',1, 'LineStyle', '-')
xlabel(ax3, 'Pole number');
ylabel(ax3, 'Y dev. from mean (mm)');
set(ax3, 'FontSize', fontsize); grid(ax3,'on');

% Title for bottom subplot (shifted above top x-axis)
title(ax3, 'Bottom Jaw Pole Tips', 'FontSize', fontsize+2, ...
      'FontWeight','bold', 'Units','normalized', 'Position',[0.5 1.20 0]);

% Overlay axis for z scale
ax4 = axes('Position', ax3.Position, 'Color','none', 'XAxisLocation','top');
ax4.YAxis.Visible = 'off';
xlabel(ax4, 'z (m)');
ax3.XLim = [min(poleNumbers) max(poleNumbers)];
ax4.XLim = [min(z) max(z)];
text(0.8, 0.2, sprintf('Mean: %+.3f mm', meanY_L), 'Units','normalized', 'FontSize',fontsize)
text(0.8, 0.1, sprintf('Slope: %+.3f µrad', slope_L), 'Units','normalized', 'FontSize',fontsize)
%linkaxes([ax3 ax4],'x');

% Adjust subplot height
pos = ax3.Position; pos(4) = pos(4) - 0.08; ax3.Position = pos; ax4.Position = pos;
if mean(diff(poleNumbers))>10
    ax3.XTick = poleNumbers;
else
    ax3.XTick = 0:10:poleNumbers(end);
end



createFig('Test')

%
%% Statistics of top and bottom jaw measurements

gap = yUP_copy - yLO_copy; % mm
meanGap = mean(gap); % mm
minGap = min(gap); % mm

ActualGapTaper = importdata('ActualGapTaper.txt');
actualGap = ActualGapTaper(1);
actualTaper = ActualGapTaper(2);


textFontSize = 13;
titleFontSize = 16;
figure('Position',[100 100 1200 800])
axis off   % blank canvas

% First set of data
nRows = 5;
nCols = 5;

% Title above table 
titleText = {'Top and Bottom Jaw: Regression Line Intersect Points (mm)'};
text(0.5, 1.0, titleText, ...
     'FontSize', titleFontSize, 'FontWeight','bold', ...
     'HorizontalAlignment','center', 'Color', 'b');

% Table content
tableData = { 'Jaw',   'First Pole',                 'US Actuator (#28)', 'DS Actuator (#90)', 'Last Pole';
              'Top',    sprintf('%.4f', top1),        sprintf('%.4f', top28),  sprintf('%.4f', top90),  sprintf('%.4f', topLast);
              'Bottom', sprintf('%.4f', down1),       sprintf('%.4f', down28), sprintf('%.4f', down90), sprintf('%.4f', downLast);
              'Gap',   sprintf('%.4f', top1 - down1), ' ',                   ' ',                   sprintf('%.4f', topLast - downLast);
              'Taper',' ',' ',' ',sprintf('%.4f', topLast - downLast - top1 + down1);};

% Vertical and horizontal positions (normalized units)
yPos = linspace(0.95,0.75,nRows);   % spread rows
xPos = linspace(0.1,0.9,nCols);   % spread columns

% Loop over cells
for r = 1:nRows
    for c = 1:nCols
        if r == 1 || c == 1
            % Bold first row OR first column
            text(xPos(c), yPos(r), tableData{r,c}, ...
                 'FontWeight','bold','HorizontalAlignment','center', "FontSize", textFontSize)
        else
            text(xPos(c), yPos(r), tableData{r,c}, ...
                 'HorizontalAlignment','center', "FontSize", textFontSize)
        end
    end
end

% Second set of data
nRows = 2;
nCols = 6;


% Title above table 
titleText = {'Summary of Mean Values (mm and mrad)'};
text(0.5, 0.65, titleText, ...
     'FontSize', titleFontSize, 'FontWeight','bold', ...
     'HorizontalAlignment','center', 'Color', 'b');

% Table content
tableData = { 'Top Jaw Poles',   'Bottom Jaw Poles',                 'Gap Values', 'Sym. Pt. Values', 'Top Jaw Roll', 'Bottom Jaw Roll';
              sprintf('%.4f', meanY_U),    sprintf('%.4f', meanY_L),        sprintf('%.4f', meanGap),  sprintf('%.4f', (meanY_U + meanY_L)/2), sprintf('%.4f', 1e3*mean(rollU)), sprintf('%.4f', 1e3*mean(rollL));};

% Vertical and horizontal positions (normalized units)
yPos = linspace(0.6,0.55,nRows);   % spread rows
xPos = linspace(0.1,0.9,nCols);   % spread columns

% Loop over cells
for r = 1:nRows
    for c = 1:nCols
        if r == 1
            % Bold first row OR first column
            text(xPos(c), yPos(r), tableData{r,c}, ...
                 'FontWeight','bold','HorizontalAlignment','center', "FontSize", textFontSize)
        else
            text(xPos(c), yPos(r), tableData{r,c}, ...
                 'HorizontalAlignment','center', "FontSize", textFontSize)
        end
    end
end


% Third set of data
nRows = 5;
nCols = 2;

% Title above table 
titleText = {'Additional Calculated Values'};
text(0.5, 0.45, titleText, ...
     'FontSize', titleFontSize, 'FontWeight','bold', ...
     'HorizontalAlignment','center', 'Color', 'b');

% Table content
tableData = { 'Bottom Pole #1 Z Value (m)',  sprintf('%.4f', firstBtmPolePos);
              'Top Jaw Pitch (mrad)',    sprintf('%.4f', slope_L);
              'Bottom Jaw Pitch (mrad)', sprintf('%.4f', slope_U);
              'Minimum Effective Gap (mm)',   sprintf('%.4f', minGap);
              'Reference Block Gap (mm)',     '---';};

% Vertical and horizontal positions (normalized units)
yPos = linspace(0.4,0.2,nRows);   % spread rows
xPos = linspace(0.35,0.65,nCols);   % spread columns

% Loop over cells
for r = 1:nRows
    for c = 1:nCols
        if c == 1
            % Bold first row OR first column
            text(xPos(c), yPos(r), tableData{r,c}, ...
                 'FontWeight','bold','HorizontalAlignment','center', "FontSize", textFontSize)
        else
            text(xPos(c), yPos(r), tableData{r,c}, ...
                 'HorizontalAlignment','center', "FontSize", textFontSize)
        end
    end
end

% Fourth set of data
nRows = 2;
nCols = 2;

% Title above table 
titleText = {'Gap and taper'};
text(0.5, 0.1, titleText, ...
     'FontSize', titleFontSize, 'FontWeight','bold', ...
     'HorizontalAlignment','center', 'Color', 'b');

% Table content
tableData = { sprintf('Actual gap: %.4f mm', actualGap),  sprintf('Actual taper: %.4f µm', 1e3*actualTaper); 
              sprintf('Nominal gap: %.4f mm', 10),  sprintf('Nominal taper: %.4f µm', 0); };

% Vertical and horizontal positions (normalized units)
yPos = linspace(0.05,0.00,nRows);   % spread rows
xPos = linspace(0.35,0.65,nCols);   % spread columns

% Loop over cells
for r = 1:nRows
    for c = 1:nCols
        text(xPos(c), yPos(r), tableData{r,c}, ...
                 'HorizontalAlignment','center', "FontSize", textFontSize)
    end
end
createFig('Test2')
system('gswin64c -dBATCH -dNOPAUSE -q -sDEVICE=pdfwrite -sOutputFile=merged.pdf Test.pdf Test2.pdf');

%
%% Plots of top and bottom jaw roll vs. z and gap vs. z

figure('Position',[100 100 1500 800]) 

hold on
pp3 = plot(posUB(:,3), 1e3*rollU, 'o-k');
yline(mean(1e3*rollU), 'Color','r', 'LineWidth',3)
xlim([0 4.3])
text(4, mean(1e3*rollU), sprintf('%.2f',mean(1e3*rollU)), 'VerticalAlignment','bottom', 'FontSize',18, 'Color','r')
ylabel('Roll (mrad)')
xlabel('z (m)')
title('Top Jaw Roll vs. z')
set(gca, 'FontSize', 18)
grid minor
figure('Position',[100 100 1500 800]) 

hold on
pp3 = plot(posLB(:,3), 1e3*rollL, 'o-k');
yline(mean(1e3*rollL), 'Color','r', 'LineWidth',3)
xlim([0 4.3])
text(4, mean(1e3*rollL), sprintf('%.2f',mean(1e3*rollL)), 'VerticalAlignment','bottom', 'FontSize',18, 'Color','r')
ylabel('Roll (mrad)')
xlabel('z (m)')
title('Bottom Jaw Roll vs. z')
set(gca, 'FontSize', 18)
grid minor
figure('Position',[100 100 1500 800]) 
hold on
plot(z, gap, 'o-k')
yline(mean(gap), 'Color','r', 'LineWidth',3)
xlim([0 4.3])
text(4, mean(gap), sprintf('%.4f',mean(gap)), 'VerticalAlignment','bottom', 'FontSize',18, 'Color','r')
xlabel('z (m)')
ylabel('Gap (mm)')
title('Gap vs. z')
set(gca, 'FontSize', fontsize)
grid

%
%% Notes on the roll angle
% The top and bottom jaw roll angles average –1.03 mrad, indicating that either the undulator, the bench, or the X stage is rolled. A measurement was performed by aligning the kinematic base with the Dover bench surface and probing two different points along the X axis. The results are shown below.
%
% x (m)
% y (m)
% Point 1
% -0.039158
% -0.025449
% Point 2
% 0.000601
% -0.025488
%
% r = \arctan \left( \frac{y_2 - y_1}{x_2 - x_1} \right) = - 0.98 \ \text{mrad}
% The result confirms that the X stage should be shimmed. The same procedure will be done in the future for the Y-stage.
%
%% Backup and testing codes

% Working
fontsize = 14;
x1=poleNumbers;
x2=posUB(:,3); %m
y = 1e3*(posUB(:,2) + posUA(:,2))/2; %mm
meanY_U = mean(y);
yUP_copy = y;
y0 = y - meanY_U; % mm

p1 = polyfit(x1, y0, 1);
yfit = polyval(p1, x1);

p2 = polyfit(x2, y0, 1);
slope_U = atan(p2(1)) * 1e3; % mrad

p3 = polyfit(x1, y, 1);
top1 = polyval(p3, 1);
top28 = polyval(p3, 28);
top90 = polyval(p3, 90);
topLast = polyval(p3, x1(end));

% --- Create taller figure ---
figure('Position',[100 100 1200 800])  % make it tall enough

% ========= TOP SUBPLOT =========
ax1 = subplot(2,1,1);
plot(ax1, x1, y0, '-b', 'LineWidth', 1); hold(ax1,'on');
plot(ax1, x1, yfit, '--k', 'LineWidth', 1);
xlabel(ax1, 'Pole number');
ylabel(ax1, 'Y dev. from mean (mm)');
set(ax1, 'FontSize', fontsize); grid(ax1,'on');

% Title for top subplot (shifted above top x-axis)
title(ax1, 'Top Jaw Pole Tips', 'FontSize', fontsize+2, ...
      'FontWeight','bold', 'Units','normalized', 'Position',[0.5 1.20 0]);

% Overlay axis for z scale
ax2 = axes('Position', ax1.Position, 'Color','none', 'XAxisLocation','top');
ax2.YAxis.Visible = 'off';
xlabel(ax2, 'z (m)');
ax1.XLim = [min(x1) max(x1)];
ax2.XLim = [min(x2) max(x2)];
text(0.8, 0.2, sprintf('Mean: %+.3f mm', meanY_U), 'Units','normalized', 'FontSize',fontsize)
text(0.8, 0.1, sprintf('Slope: %+.3f µrad', slope_U), 'Units','normalized', 'FontSize',fontsize)
%linkaxes([ax1 ax2],'x');

% Adjust subplot height for breathing room
pos = ax1.Position; pos(4) = pos(4) - 0.08; ax1.Position = pos; ax2.Position = pos;

if mean(diff(x1))>10
    ax1.XTick = x1;
else
    ax1.XTick = 4:4:x1(end);
end

% ========= BOTTOM SUBPLOT =========
fontsize = 14;
x1=poleNumbers;
x2=posLB(:,3); %m
firstBtmPolePos = x2(1);
y = 1e3*(posLB(:,2) + posLA(:,2))/2; %mm
meanY_L = mean(y);
yLO_copy = y;
y0 = y - meanY_L; %mm

p1 = polyfit(x1, y0, 1);
yfit = polyval(p1, x1);

p2 = polyfit(x2, y0, 1);
slope_L = atan(p2(1)) * 1e3; % mrad

p3 = polyfit(x1, y, 1);
down1 = polyval(p3, 1);
down28 = polyval(p3, 28);
down90 = polyval(p3, 90);
downLast = polyval(p3, x1(end));


ax3 = subplot(2,1,2);
plot(ax3, x1, y0, '-r', 'LineWidth', 1); hold(ax3,'on');
plot(ax3, x1, yfit, '--k', 'LineWidth', 1);
xlabel(ax3, 'Pole number');
ylabel(ax3, 'Y dev. from mean (mm)');
set(ax3, 'FontSize', fontsize); grid(ax3,'on');

% Title for bottom subplot (shifted above top x-axis)
title(ax3, 'Bottom Jaw Pole Tips', 'FontSize', fontsize+2, ...
      'FontWeight','bold', 'Units','normalized', 'Position',[0.5 1.20 0]);

% Overlay axis for z scale
ax4 = axes('Position', ax3.Position, 'Color','none', 'XAxisLocation','top');
ax4.YAxis.Visible = 'off';
xlabel(ax4, 'z (m)');
ax3.XLim = [min(x1) max(x1)];
ax4.XLim = [min(x2) max(x2)];
text(0.8, 0.2, sprintf('Mean: %+.3f mm', meanY_L), 'Units','normalized', 'FontSize',fontsize)
text(0.8, 0.1, sprintf('Slope: %+.3f µrad', slope_L), 'Units','normalized', 'FontSize',fontsize)
%linkaxes([ax3 ax4],'x');

% Adjust subplot height
pos = ax3.Position; pos(4) = pos(4) - 0.08; ax3.Position = pos; ax4.Position = pos;
if mean(diff(x1))>10
    ax3.XTick = x1;
else
    ax3.XTick = 4:4:x1(end);
end

%

function createFig(fileName)
% createFig saves the current figure in PNG, PDF, and EPS formats.
%
% Usage:
%   createFig(fileName)
%
% Description:
%   The createFig function saves the current figure in three formats: PNG, PDF,
%   and EPS, with high resolution (300 dpi). The function prompts the user to 
%   choose a directory to save the figure. If the user does not select a 
%   directory, the function displays a message and does not save the figure.
%
% Input:
%   fileName (string) - Name of the file (without extension) under which 
%   the figure will be saved in the chosen directory.
%
% Example:
%   % Assuming a figure is already open in MATLAB
%   createFig('MyFigure')
%
% Note:
%   Ensure that the figure you wish to save is the active figure when calling
%   createFig. This function uses a 300 dpi resolution for high-quality output.
    
    
    % Johann E. Baader (12.11.2024)

    % % Prompt the user to select a directory for saving the figure
    % figDir = uigetdir(pwd, 'Select a directory to save the figure');
    figDir = 'S:\magdata\LCLS-II-HE\Dover bench touch probe IO files';
    % 
    % % Check if the user selected a valid directory, if not, return
    % if figDir == 0
    %     disp('No directory selected. Figure not saved.');
    %     return;
    % end

    % Get the handle of the current figure
    h = gcf;

    % Set figure properties for saving
    set(h, 'PaperOrientation', 'portrait');
    set(h, 'Units', 'Inches');
    pos = get(h, 'Position');
    set(h, 'PaperPositionMode', 'Auto', 'PaperUnits', 'Inches', 'PaperSize', [pos(3), pos(4)]);

    % Save the figure as PNG
    pngFile = fullfile(figDir, [fileName, '.png']);
    print(h, pngFile, '-dpng', '-r300');

    % Save the figure as PDF
    pdfFile = fullfile(figDir, [fileName, '.pdf']);
    print(h, pdfFile, '-dpdf', '-r300');

    % Save the figure as EPS
    epsFile = fullfile(figDir, [fileName, '.eps']);
    print(h, epsFile, '-depsc', '-r300');

    %disp(['Figure saved to ', figDir]);
end