clear all
addpath(genpath('alm'));

% position of curved mirrors (double checked) after move on October 5th,
% 2013
RM2_pos=sqrt((17*2)^2+1)*25.4e-3;
M5_pos=RM2_pos+sqrt((18*2)^2+6.5^2)*25.4e-3;
f1 = 333e-3;      % r=150mm fused silica lens
f2 = -167e-3;     % r=-75mm fused silica lens
L101_dist= 3.0111;
L102_dist=3.2096;
WFSA_dist=3.5920;
WFSB_dist=3.9600;

%mode master before Tip tilts

%mode master was at 40+sqrt(16^2+2^2) inches after M2
M2toModeMasterB4=(40+sqrt(16^2+2^2))*25.4e-3;
distM2toRM1=(26*2+1)*25.4e-3; %from counting holes on D1000313
MMposB4=M2toModeMasterB4-distM2toRM1;
Meas_wB4_pos=[-2.387,-3.578];
Meas_wB4=[3.588e-3, 3.532e-3]./2;

%mode master measurement after moving Tip tilts, 

%mode master 31.5 inches from BS
MMpos1 =(31+31.75)*25.4e-3+M5_pos;
Moved_w1=[2.891e-3,3.321e-3]./2;%X, Y
Moved_w_pos1=[4.494, 2.855]+MMpos1;

%with mode master 22 inches from BS
MMpos2=(31+22)*25.4e-3+M5_pos; %MMpos for last measurement
Moved_w2=[2.89e-3,3.348e-3]./2;%X, Y % last emsaurement
Moved_w_pos2=[4.777, 2.993]+MMpos2;

%pauls calculated waist
Pauls_w=[2.123e-3, 2.123e-3];
Pauls_w_pos=([-2.469, -2.150]+0.0794);

%summarize all waists and positions in single vectors

Summary_w=[Pauls_w; Meas_wB4; Moved_w1; Moved_w2];
Summary_w_pos=[Pauls_w_pos; Meas_wB4_pos; Moved_w_pos1; Moved_w_pos2];
 %vector of approximate positions at which beam parameter was measured (or
 %calculated) (to avoid progaging through opitcs in wrong direction)
meas_positions=[-1,-1,  2.5, 2.5];

%NanoscanData
Nano_Pos=[MMposB4, M5_pos+(31+23)*25.4e-3, M5_pos+(31+31)*25.4e-3];
Nano_radius_X=[4126, 3964, 3780].*1e-6/2;
Nano_radius_Y=[4025,  3828, 3670 ].*1e-6/2;

% create Generic beamPath with the optics at their as built positions (Oct
% 5th)


        %dummy paths to place measured waists at their location
    Paths_Start=[beamPath, beamPath;beamPath, beamPath;...
        beamPath, beamPath;beamPath, beamPath;]; 


    
    %add waists
    for ii=1:length(Paths_Start)
        for jj=1:2;
            Paths_Start(ii, jj).seedWaist(Summary_w(ii, jj), Summary_w_pos(ii, jj));
            Paths(ii, jj)=Paths_Start(ii, jj).branchPath(meas_positions(ii));
            Paths(ii, jj).addComponent(...
                component.curvedMirror(1.7, 0+1e-6, 'RM1'));
            Paths(ii, jj).addComponent(...
                component.curvedMirror(-0.6, RM2_pos, 'RM2'));
            Paths(ii, jj).addComponent(...
                component.curvedMirror(1.7, M5_pos, 'M5'));
            Paths(ii, jj).addComponent(...
                component.lens(f1, L101_dist, 'L101'));
            Paths(ii, jj).addComponent(...
                component.lens(f2, L102_dist, 'L102'));
            Paths(ii, jj).addComponent(...
                component.flatMirror(WFSA_dist, 'ASC-REFL\_A'));
            Paths(ii, jj).addComponent(...
                component.flatMirror(WFSB_dist, 'ASC-REFL\_B'));
          Gouy_Seperation_TT(ii, jj)=Paths(ii, jj).gouySeperation('RM1', 'RM2');
          Gouy_Seperation_WFS(ii, jj)=Paths(ii, jj).gouySeperation('ASC-REFL\_A', 'ASC-REFL\_B');
        end
    end
    
    % to compare nanoscan to other measurements, take out some optics

    for ii=1:length(Paths)
        for jj=1:2;
            Paths_compare_A(ii, jj)=Paths(ii, jj).duplicate;
            Paths_compare_B4(ii, jj)=Paths(ii, jj).branchPath(-1);
            Paths_compare_A(ii,jj).deleteComponent('L101');
            Paths_compare_A(ii, jj).deleteComponent('L102');
            Paths_compare_B4(ii, jj).deleteComponent('RM1');
        end
    end
    
    zz=-1:0.01:5;
    colors=['r', 'b', 'g', 'm'];
    figure(1)
 
    for ii=1:length(Paths_Start)
        
        subplot(2, 1, 1)
            Paths(ii, 1).plotBeamWidth(zz, colors(ii));
            title('Horizontal')
            grid on
            
            hold on
            subplot(2,1,2)
            Paths(ii, 1).plotGouyPhase(zz, 'wrap', colors(ii));
            grid on
            hold on
    end
    subplot(2, 1, 1)
   % plot(Nano_Pos, Nano_radius_X, 'b*')
    ylim([0, 3e-3]);
    ylabel('beam radius')
    Paths(1, 1).plotComponents(zz,1e-3, 'k.')
    hold off
    subplot(2, 1, 2)
    ylabel('Gouy Phase [degrees]')
    xlabel('Position [m]')
        legend('Pauls Calculation', 'Mode master before TTs',...
                'Mode Master after TTs 1', 'Mode Master after TTs 2',...
                'NanoScan measurements',...
                'Location', 'SouthEast');
    hold off
            
    figure(2)

    for ii=1:length(Paths_Start)
        
        subplot(2, 1, 1)
            Paths(ii, 2).plotBeamWidth(zz, colors(ii));
              title('Vertical direction')
            grid on
            
            hold on
            subplot(2,1,2)
            Paths(ii, 2).plotGouyPhase(zz, 'wrap', colors(ii));
            grid on
            hold on
    end
    subplot(2, 1, 1)
    %plot(Nano_Pos, Nano_radius_Y, 'b*')
    ylim([0, 3e-3]);
    ylabel('beam radius')
    Paths(1, 1).plotComponents(zz,1e-3, 'k.')
    hold off
    subplot(2, 1, 2)
    ylabel('Gouy Phase [degrees]')
    xlabel('Position [m]')
        legend('Pauls Calculation', 'Mode master before TTs',...
                'Mode Master after TTs 1', 'Mode Master after TTs 2',...
                'Location', 'SouthEast');
    hold off

    zz1=0:0.01:0.5;
    figure(3)
    for ii=1:length(Paths)
            subplot(2, 1, 1)
            Paths_compare_B4(ii, 1).plotBeamWidth(zz1, colors(ii));
            hold on
            subplot(2,1,2)
            Paths_compare_B4(ii, 2).plotBeamWidth(zz1, colors(ii));
            hold on
    end
    subplot(2,1,1)
            plot(Nano_Pos(1), Nano_radius_X(1), 'k*',Nano_Pos(1), -1*Nano_radius_X(1), 'k*')
            %ylim([1.5e-3, 2.5e-3])
            ylabel('hoizontal beam radius [m]')
                subplot(2,1,2)
                %ylim([1.5e-3, 2.5e-3])
            plot(Nano_Pos(1), Nano_radius_Y(1), 'k*', Nano_Pos(1), -1*Nano_radius_Y(1), 'k*')
            ylabel('vertical beam radius [m]')
            xlabel('position [m]') 
                    legend('Pauls Calculation','Pauls Calculation',...
                        'Mode master before TTs', 'Mode master before TTs',...
                'Mode Master after TTs 1','Mode Master after TTs 1',...
                'Mode Master after TTs 2','Mode Master after TTs 2',...
                'NanoScan measurements',...
                'Location', 'SouthEast');
            
            zz2=2.5:0.001:3.5;
            
    figure(4)
    for ii=1:length(Paths)
            subplot(2, 1, 1)
            Paths_compare_A(ii, 1).plotBeamWidth(zz2, colors(ii));
            hold on
            subplot(2,1,2)
            Paths_compare_A(ii, 2).plotBeamWidth(zz2, colors(ii));
            hold on
    end
    subplot(2,1,1)
            plot(Nano_Pos(2:3), Nano_radius_X(2:3), 'k*',Nano_Pos(2:3), -1*Nano_radius_X(2:3), 'k*')
            ylim([1.5e-3, 2.5e-3])
            ylabel('hoizontal beam radius [m]')
                subplot(2,1,2)
                ylim([1.5e-3, 2.5e-3])
            plot(Nano_Pos(2:3), Nano_radius_Y(2:3), 'k*', Nano_Pos(2:3), -1*Nano_radius_Y(2:3), 'k*')
            ylabel('vertical beam radius [m]')
            xlabel('position [m]') 
                    legend('Pauls Calculation','Pauls Calculation',...
                        'Mode master before TTs', 'Mode master before TTs',...
                'Mode Master after TTs 1','Mode Master after TTs 1',...
                'Mode Master after TTs 2','Mode Master after TTs 2',...
                'NanoScan measurements',...
                'Location', 'SouthWest');
