% LCLS-II Undualtor Quad Wire Fiducial analysis.
%  This script checks all of the data taken for multiple runs on LCLS quad 000709 with the refurbished 
%  wire fiducial system. 

%%  Load LCLS 000709 Tooling Ball data.
   
 % Load data from runs 4 & 5
  cd('C:\Magnet_Data\LCLS\Quad\000709\CMM data runs 4 and 5')
  [LCLS_Fid4.cmm] = Und_Quad_Center('CMM RUN 000709 PLUS.TXT', 4);
   [LCLS_Fid5.cmm] = Und_Quad_Center('CMM RUN 000709 MINUS.TXT', 5);
  Load_Und_Quad_TB('000709', 5, 4);
  
  % Load data from runs 6 & 7
  cd('C:\Magnet_Data\LCLS\Quad\000709\CMM data runs 6 and 7')
   [LCLS_Fid6.cmm] = Und_Quad_Center('CMM RUN 000709 PLUS.TXT', 6);
   [LCLS_Fid7.cmm] = Und_Quad_Center('CMM RUN 000709 MINUS.TXT', 57);
  Load_Und_Quad_TB('000709', 7, 6);
  
%  Load LCLS-II 000709 Tooling Ball data.
   
 % Load data from runs 1 & 2
  cd('C:\Magnet_Data\LCLS-II\Quad\000709\Fiducial Runs 1 and 2')
   [LCLS_II_Fid1.cmm] = Und_Quad_Center('CMM RUN 000709 PLUS.TXT', 1);
   [LCLS_II_Fid2.cmm] = Und_Quad_Center('CMM RUN 000709 MINUS.TXT', 2);
  Load_Und_Quad_TB('000709', 2, 1);
  
   % Load data from runs 1 & 2
  cd('C:\Magnet_Data\LCLS-II\Quad\000709\Fiducial 2nd Runs 1 & 2')
   [LCLS_II_Fid11.cmm] = Und_Quad_Center('CMM RUN 000709 PLUS.TXT', 11);
   [LCLS_II_Fid12.cmm] = Und_Quad_Center('CMM RUN 000709 MINUS.TXT', 12);
  Load_Und_Quad_TB('000709', 12, 11);
  
  % Load data from runs 3 & 4
  cd('C:\Magnet_Data\LCLS-II\Quad\000709\Fiducial Runs 3 and 4')
   [LCLS_II_Fid3.cmm] = Und_Quad_Center('CMM RUN 000709 PLUS.TXT', 3);
   [LCLS_II_Fid4.cmm] = Und_Quad_Center('CMM RUN 000709 MINUS.TXT', 4);
  Load_Und_Quad_TB('000709', 4, 3);
  
   % Load data from runs 5 & 6
  cd('C:\Magnet_Data\LCLS-II\Quad\000709\Fiducial Runs 5 and 6')
   [LCLS_II_Fid5.cmm] = Und_Quad_Center('CMM RUN 000709 PLUS.TXT', 5);
   [LCLS_II_Fid6.cmm] = Und_Quad_Center('CMM RUN 000709 MINUS.TXT', 6);
  Load_Und_Quad_TB('000709', 6, 5);
  
    % Load data from runs 7 & 8.  After run 6 the mirror plates were moved
    % around.  The were put on opposites sides and the top and bottom of
    % each side were also swapped. 
  cd('C:\Magnet_Data\LCLS-II\Quad\000709\Fiducial Runs 7 and 8')
   [LCLS_II_Fid7.cmm] = Und_Quad_Center('CMM RUN 000709 PLUS.TXT', 7);
   [LCLS_II_Fid8.cmm] = Und_Quad_Center('CMM RUN 000709 MINUS.TXT', 8);
  Load_Und_Quad_TB('000709', 8, 7);
  
      % Load data from runs 9 & 10.  After run 8 the quad was split and put
      % back together with the bolts torqued
  cd('C:\Magnet_Data\LCLS-II\Quad\000709\Fiducial Runs 9 and 10')
   [LCLS_II_Fid9.cmm] = Und_Quad_Center('CMM RUN 000709 PLUS.TXT', 9);
   [LCLS_II_Fid10.cmm] = Und_Quad_Center('CMM RUN 000709 MINUS.TXT', 10);
  Load_Und_Quad_TB('000709', 10, 9);

    
 %% Find mean and stdev LCLS data
 
  % Plus current run mean and stdev of runs  
  for i = 1:6
     LCLS_TB_Plus_mean(i,:) = mean([Fid_000709_4_5.final_tb(1).data(i,:); Fid_000709_6_7.final_tb(1).data(i,:)]);
     LCLS_TB_Plus_std(i,:) = std([Fid_000709_4_5.final_tb(1).data(i,:); Fid_000709_6_7.final_tb(1).data(i,:)]);
 
 % Minus current run mean and stdev of runs
  
     LCLS_TB_Minus_mean(i,:) = mean([Fid_000709_4_5.final_tb(2).data(i,:); Fid_000709_6_7.final_tb(2).data(i,:)]);
     LCLS_TB_Minus_std(i,:) = std([Fid_000709_4_5.final_tb(2).data(i,:); Fid_000709_6_7.final_tb(2).data(i,:)]);
  end
     
      %%  Find mean and stdev LCLS-II data
 
%% Plus current run mean and stdev of runs
  for i = 1:6
     LCLS_II_TB_Plus_mean(i,:) = mean([Fid_000709_1_2.final_tb(1).data(i,:); Fid_000709_11_12.final_tb(1).data(i,:); Fid_000709_3_4.final_tb(1).data(i,:); Fid_000709_5_6.final_tb(1).data(i,:); Fid_000709_7_8.final_tb(1).data(i,:); Fid_000709_9_10.final_tb(1).data(i,:)]);
     LCLS_II_TB_Plus_std(i,:) = std(  [Fid_000709_1_2.final_tb(1).data(i,:); Fid_000709_11_12.final_tb(1).data(i,:); Fid_000709_3_4.final_tb(1).data(i,:); Fid_000709_5_6.final_tb(1).data(i,:); Fid_000709_7_8.final_tb(1).data(i,:); Fid_000709_9_10.final_tb(1).data(i,:)]);
 
     % Minus current run mean and stdev of runs
  
     LCLS_II_TB_Minus_mean(i,:) = mean([Fid_000709_1_2.final_tb(2).data(i,:); Fid_000709_11_12.final_tb(2).data(i,:); Fid_000709_3_4.final_tb(2).data(i,:); Fid_000709_5_6.final_tb(2).data(i,:); Fid_000709_7_8.final_tb(2).data(i,:); Fid_000709_9_10.final_tb(2).data(i,:)]);
     LCLS_II_TB_Minus_std(i,:) = std(  [Fid_000709_1_2.final_tb(2).data(i,:); Fid_000709_11_12.final_tb(2).data(i,:); Fid_000709_3_4.final_tb(2).data(i,:); Fid_000709_5_6.final_tb(2).data(i,:); Fid_000709_7_8.final_tb(2).data(i,:); Fid_000709_9_10.final_tb(2).data(i,:)]);
  end
     
   cd('C:\Magnet_Data\LCLS-II\Quad\000709')  
   
   % Make file with final tooling ball numbers for LCLS measurements and LCLS-II measurements. 
for i = 1:6  % # of tooling balls.  TB 6 is a virtual sum tooling ball of the first 4. 
 
% Put + current data into individual tooling ball files     
fidB = fopen(['Quady 709 Fiducial Tooling Ball ', num2str(i), ' + Current Comparison.txt'],'w');

fprintf(fidB,'Final Tooling Ball %2i Comparison LCLS vs LCLS-II for Quad 000709 + Current\n', i);
fprintf(fidB,' Run        X            Y           Z  \n');
fprintf(fidB,'           (mm)         (mm)        (mm) \n\n');
fprintf(fidB,'                        LCLS              \n'); 
fprintf(fidB,'  4      %8.4f    %8.4f    %8.4f   \n',Fid_000709_4_5.final_tb(1).data(i,1), Fid_000709_4_5.final_tb(1).data(i,2),Fid_000709_4_5.final_tb(1).data(i,3));
fprintf(fidB,'  6      %8.4f    %8.4f    %8.4f   \n',Fid_000709_6_7.final_tb(1).data(i,1), Fid_000709_6_7.final_tb(1).data(i,2),Fid_000709_6_7.final_tb(1).data(i,3));
fprintf(fidB,' Mean    %8.4f    %8.4f    %8.4f   \n',LCLS_TB_Plus_mean(i,1), LCLS_TB_Plus_mean(i,2),LCLS_TB_Plus_mean(i,3));
fprintf(fidB,' STD     %8.4f    %8.4f    %8.4f   \n',LCLS_TB_Plus_std(i,1), LCLS_TB_Plus_std(i,2),LCLS_TB_Plus_std(i,3));

fprintf(fidB,'\n');
fprintf(fidB,'                      LCLS-II              \n'); 
fprintf(fidB,'  1      %8.4f    %8.4f    %8.4f   \n',Fid_000709_1_2.final_tb(1).data(i,1), Fid_000709_1_2.final_tb(1).data(i,2),Fid_000709_1_2.final_tb(1).data(i,3));
fprintf(fidB,'  1B     %8.4f    %8.4f    %8.4f   \n',Fid_000709_11_12.final_tb(1).data(i,1), Fid_000709_11_12.final_tb(1).data(i,2),Fid_000709_11_12.final_tb(1).data(i,3));
fprintf(fidB,'  3      %8.4f    %8.4f    %8.4f   \n',Fid_000709_3_4.final_tb(1).data(i,1), Fid_000709_3_4.final_tb(1).data(i,2),Fid_000709_3_4.final_tb(1).data(i,3));
fprintf(fidB,'  5      %8.4f    %8.4f    %8.4f   \n',Fid_000709_5_6.final_tb(1).data(i,1), Fid_000709_5_6.final_tb(1).data(i,2),Fid_000709_5_6.final_tb(1).data(i,3));
fprintf(fidB,'  7      %8.4f    %8.4f    %8.4f   \n',Fid_000709_7_8.final_tb(1).data(i,1), Fid_000709_7_8.final_tb(1).data(i,2),Fid_000709_7_8.final_tb(1).data(i,3));
fprintf(fidB,'  9      %8.4f    %8.4f    %8.4f   \n',Fid_000709_9_10.final_tb(1).data(i,1), Fid_000709_9_10.final_tb(1).data(i,2),Fid_000709_9_10.final_tb(1).data(i,3));
fprintf(fidB,' Mean    %8.4f    %8.4f    %8.4f   \n',LCLS_II_TB_Plus_mean(i,1), LCLS_II_TB_Plus_mean(i,2),LCLS_II_TB_Plus_mean(i,3));
fprintf(fidB,' STD     %8.4f    %8.4f    %8.4f   \n',LCLS_II_TB_Plus_std(i,1), LCLS_II_TB_Plus_std(i,2),LCLS_II_TB_Plus_std(i,3));
fprintf(fidB,'\n');
fprintf(fidB,' Diff    %8.4f    %8.4f    %8.4f   \n',LCLS_TB_Plus_mean(i,1) - LCLS_II_TB_Plus_mean(i,1), LCLS_TB_Plus_mean(i,2) - LCLS_II_TB_Plus_mean(i,2),LCLS_TB_Plus_mean(i,3) - LCLS_II_TB_Plus_mean(i,3));
fclose(fidB);

% Put - current data into individual tooling ball files     
fidC = fopen(['Quady 709 Fiducial Tooling Ball ', num2str(i), ' - Current Comparison.txt'],'w');

fprintf(fidC,'Final Tooling Ball %2i Comparison LCLS vs LCLS-II for Quad 000709 - Current\n', i);
fprintf(fidC,' Run        X            Y           Z  \n');
fprintf(fidC,'           (mm)         (mm)        (mm) \n\n');
fprintf(fidC,'                        LCLS              \n'); 
fprintf(fidC,'  5      %8.4f    %8.4f    %8.4f   \n',Fid_000709_4_5.final_tb(2).data(i,1), Fid_000709_4_5.final_tb(2).data(i,2),Fid_000709_4_5.final_tb(2).data(i,3));
fprintf(fidC,'  7      %8.4f    %8.4f    %8.4f   \n',Fid_000709_6_7.final_tb(2).data(i,1), Fid_000709_6_7.final_tb(2).data(i,2),Fid_000709_6_7.final_tb(2).data(i,3));
fprintf(fidC,' Mean    %8.4f    %8.4f    %8.4f   \n',LCLS_TB_Plus_mean(i,1), LCLS_TB_Plus_mean(i,2),LCLS_TB_Plus_mean(i,3));
fprintf(fidC,' STD     %8.4f    %8.4f    %8.4f   \n',LCLS_TB_Plus_std(i,1), LCLS_TB_Plus_std(i,2),LCLS_TB_Plus_std(i,3));

fprintf(fidC,'\n');
fprintf(fidC,'                      LCLS-II              \n'); 
fprintf(fidC,'  2      %8.4f    %8.4f    %8.4f   \n',Fid_000709_1_2.final_tb(2).data(i,1), Fid_000709_1_2.final_tb(2).data(i,2),Fid_000709_1_2.final_tb(2).data(i,3));
fprintf(fidC,'  2B     %8.4f    %8.4f    %8.4f   \n',Fid_000709_11_12.final_tb(2).data(i,1), Fid_000709_11_12.final_tb(2).data(i,2),Fid_000709_11_12.final_tb(2).data(i,3));
fprintf(fidC,'  4      %8.4f    %8.4f    %8.4f   \n',Fid_000709_3_4.final_tb(2).data(i,1), Fid_000709_3_4.final_tb(2).data(i,2),Fid_000709_3_4.final_tb(2).data(i,3));
fprintf(fidC,'  6      %8.4f    %8.4f    %8.4f   \n',Fid_000709_5_6.final_tb(2).data(i,1), Fid_000709_5_6.final_tb(2).data(i,2),Fid_000709_5_6.final_tb(2).data(i,3));
fprintf(fidC,'  8      %8.4f    %8.4f    %8.4f   \n',Fid_000709_7_8.final_tb(2).data(i,1), Fid_000709_7_8.final_tb(2).data(i,2),Fid_000709_7_8.final_tb(2).data(i,3));
fprintf(fidC,'  10     %8.4f    %8.4f    %8.4f   \n',Fid_000709_9_10.final_tb(2).data(i,1), Fid_000709_9_10.final_tb(2).data(i,2),Fid_000709_9_10.final_tb(2).data(i,3));
fprintf(fidC,' Mean    %8.4f    %8.4f    %8.4f   \n',LCLS_II_TB_Minus_mean(i,1), LCLS_II_TB_Minus_mean(i,2),LCLS_II_TB_Minus_mean(i,3));
fprintf(fidC,' STD     %8.4f    %8.4f    %8.4f   \n',LCLS_II_TB_Minus_std(i,1), LCLS_II_TB_Minus_std(i,2),LCLS_II_TB_Minus_std(i,3));
fprintf(fidC,'\n');
fprintf(fidC,' Diff    %8.4f    %8.4f    %8.4f   \n',LCLS_TB_Minus_mean(i,1) - LCLS_II_TB_Minus_mean(i,1), LCLS_TB_Minus_mean(i,2) - LCLS_II_TB_Minus_mean(i,2),LCLS_TB_Minus_mean(i,3) - LCLS_II_TB_Minus_mean(i,3));
fclose(fidC);

%% Compute Angles From QR CS to Mech CS
for j = 1:5
Quad_TB_check_roll_angle(j) = vector_angle(Quad_TB_MC_TR(j,1:2), Quad_TB_Mech(j,1:2));
% Quad_TB_check_pitch_angle(j) = vector_angle(Quad_TB_MC_TR(j,2:3), Quad_TB_Mech(j,2:3));
% Quad_TB_check_yaw_angle(j) = vector_angle([Quad_TB_MC_TR(j,1),Quad_TB_TR(j,3)], [Quad_TB_Mech(j,1),Quad_TB_Mech(j,3)]);
end

end
  