clear all;
close all;
%%
% measurement results
%M2x=1.04
%M2y=0.98
w0x=2.752e-3/2;
w0y=2.309e-3/2;
z0x=-4.314; % this is the distance away from MM head
z0y=-4.345;
%zrx=5.395;
%zry=4.023;

M6_L1_meas=12*25.4e-3;
M6_MM_meas = (31+3/8)*25.4e-3;
M5_M6_meas=35*25.4e-3;
TT2_M5_meas=sqrt(33.5^2+6.5^2)*25.4e-3;
TT1_TT2_meas=sqrt(32.25^2+1)*25.4e-3;

z_TT2_meas = TT1_TT2_meas; % TT1 is at z=0
z_M5_meas = z_TT2_meas+TT2_M5_meas;
z_M6_meas = z_M5_meas + M5_M6_meas;

lambda=1064e-9;
k=2*pi/lambda;
z_MM = z_M6_meas+M6_MM_meas; %TT1 is at z=0;
qx = beamq(-z0x+1i*w0x^2*k/2); % note the sign of z0x term. MM is at "past the waist".
qy = beamq(-z0y+1i*w0y^2*k/2); 

z_L1_meas = z_M6_meas + M6_L1_meas;
%% Input is the ideal case by Sam Waldman, but the mirror placement reflects the measurement.
qin=1.03+13.1i;

refl=beamPath();
refl.seedq=beamq(qin);
refl.seedz=0; % TT1 is here in his model. It doesn't matter that much as Rayleigh is 13m.
        
R_TT1=1.7;
z_TT1=1e-6;
refl.addComponent(component.curvedMirror(R_TT1, z_TT1, 'TT1'));
        
R_TT2=-0.6;
%z_TT2=831e-3;
z_TT2=z_TT2_meas;
refl.addComponent(component.curvedMirror(R_TT2, z_TT2, 'TT2'));
        
R_M5=1.7;
%z_M5=1686e-3;
z_M5=z_M5_meas;
refl.addComponent(component.curvedMirror(R_M5, z_M5, 'M5'));

%f_L1=333e-3;
% z_L1=2686e-3;
% refl.addComponent(component.lens(f_L1, z_L1, 'L1'));
%         
% f_L2=-167e-3;
% z_L2=2889e-3;
% refl.addComponent(component.lens(f_L2, z_L2, 'L1'));
% 
% zQPD1=3256e-3;
% refl.addComponent(component.flatMirror(zQPD1, 'QPD1')); % just a marker
% zQPD2=3625e-3;
% refl.addComponent(component.flatMirror(zQPD2, 'QPD2')); % just a marker

%% Let's first see how far off the measurement and the ideal case is.
% we want to back propagate the measurement.
measx=beamPath();
measx.seedq= qx; 
measx.seedq.q = conj(-qx.q);% real part is flipped because now we're "not past the waist yet".
measx.seedz = -z_MM;
measx.addComponent(component.curvedMirror(R_M5, -z_M5, 'M5'));
measx.addComponent(component.curvedMirror(R_TT2, -z_TT2, 'TT2'));
measx.addComponent(component.curvedMirror(R_TT1, -z_TT1, 'TT1'));
qx_src = measx.qPropagate(0);

%do the same for y
measy=measx.duplicate();
measy.seedq=qy;
measy.seedq.q = conj(-qy.q);
qy_src  = measy.qPropagate(0);

%throw away measx and measy object and propagate qx_source in the positive direction.
measx=refl.duplicate();
measx.seedq=qx_src;
measx.seedq.q = conj(-qx_src.q); % again flipping the direction
measx.seedz=0;
measx.targetq=refl.qPropagate(z_MM);
measx.targetz=z_MM;

measy=measx.duplicate();
measy.seedq=qy_src;
measy.seedq.q = conj(-qy_src.q);
measy.seedz=0;

%
z=0:0.01:z_MM;
rad_refl=zeros(size(z));
rad_x=rad_refl;
rad_y=rad_refl;
for ii=1:length(z)
    q=refl.qPropagate(z(ii));
    rad_refl(ii) = q.beamWidth;
    q=measx.qPropagate(z(ii));
    rad_x(ii) = q.beamWidth;
    q=measy.qPropagate(z(ii));
    rad_y(ii) = q.beamWidth;
end 

figure
plot(z, rad_refl, 'r', z, rad_x, 'b', z, rad_y, 'g');
hold on
refl.plotComponents();
ylabel('beam radius [m]')
xlabel('distance from TT1 [m]')
grid on
title('REFL beam on HAM1 WFS path')
legend('Expected beam from HAM1 propagated through the telescope with measured mirror spacing', ['measured x profile, overlap=', num2str(measx.targetOverlap())], ...
    ['measured y profile, overlap=', num2str(measy.targetOverlap())]);

%% How much should we move the TT2 and M5 to optimize the telescope?

refl2=refl.duplicate();
refl2.targetz= z_MM;
refl2.targetq = qx;
reflxfit=refl2.optimizePath('TT2', [z_TT2-0.1, z_TT2+0.1], 'M5', [z_M5-0.1, z_M5+0.1]);
refl2.targetq = qy;
reflyfit=refl2.optimizePath('TT2', [z_TT2-0.1, z_TT2+0.1], 'M5', [z_M5-0.1, z_M5+0.1]);

rad_xfit=zeros(size(z));
rad_yfit=zeros(size(z));
for ii=1:length(z)
    q=reflxfit.qPropagate(z(ii));
    rad_xfit(ii) = q.beamWidth;
    q=reflyfit.qPropagate(z(ii));
    rad_yfit(ii) = q.beamWidth;
end 

figure
subplot 211
plot(z, rad_y, 'g', z, rad_yfit, 'r--')
grid on
hold on
reflyfit.plotComponents(z, 'r*');
measy.plotComponents(z, 'g*');
legend('measured y', ['optimized tele for y, delta1=', num2str(reflyfit.component('TT2').z - refl.component('TT2').z), ...
    ', delta2=',  num2str(reflyfit.component('M5').z -reflyfit.component('TT2').z - refl.component('M5').z + refl.component('TT2').z)]);
ylabel('beam radius [m]')
subplot 212
plot(z, rad_x, 'b', z, rad_xfit, 'r--')
grid on
hold on
reflxfit.plotComponents(z, 'r*');
measy.plotComponents(z, 'b*');
legend('measured x', ['optimized tele for x, delta1=', num2str(reflxfit.component('TT2').z - refl.component('TT2').z), ...
    ', delta2=',  num2str(reflxfit.component('M5').z -reflxfit.component('TT2').z - refl.component('M5').z + refl.component('TT2').z)]);
ylabel('beam radius [m]')
xlabel('distance from TT1 [m]')

%% Now look at Gouy
f_L1=333e-3;
z_L1=z_L1_meas;
refl.addComponent(component.lens(f_L1, z_L1, 'L1'));
measx.addComponent(component.lens(f_L1, z_L1, 'L1'));
measy.addComponent(component.lens(f_L1, z_L1, 'L1'));

f_L2=-167e-3;
z_L2=z_L1+(2889-2686)*1e-3;
refl.addComponent(component.lens(f_L2, z_L2, 'L2'));
measx.addComponent(component.lens(f_L2, z_L2, 'L2'));
measy.addComponent(component.lens(f_L2, z_L2, 'L2'));

zQPD1=z_L2+(3256-2889)*1e-3;
refl.addComponent(component.flatMirror(zQPD1, 'QPD1')); % just a marker
measx.addComponent(component.flatMirror(zQPD1, 'QPD1')); % just a marker
measy.addComponent(component.flatMirror(zQPD1, 'QPD1')); % just a marker
zQPD2=zQPD1+(3625-3256)*1e-3;
refl.addComponent(component.flatMirror(zQPD2, 'QPD2')); % just a marker
measx.addComponent(component.flatMirror(zQPD2, 'QPD2')); % just a marker
measy.addComponent(component.flatMirror(zQPD2, 'QPD2')); % just a marker

z=0:0.005:(zQPD2+0.1);
figure
refl.plotGouyPhase(z, 'nowrap', 'r');
hold on
measx.plotGouyPhase(z, 'nowrap', 'b');
measy.plotGouyPhase(z, 'nowrap', 'g');
measx.plotComponents(z);
grid on
xlabel('distance from TT1 [m]')
ylabel('Gouy phase')
legend(['Expected, Gouy(TTs)=' num2str(refl.gouySeperation('TT1', 'TT2')) 'deg, Gouy(QPDs)=' num2str(refl.gouySeperation('QPD1', 'QPD2')) 'deg'], ...
    ['Measured x, Gouy(TTs)=' num2str(measx.gouySeperation('TT1', 'TT2')) 'deg, Gouy(QPDs)=' num2str(measx.gouySeperation('QPD1', 'QPD2')) 'deg'], ...
    ['Measured y, Gouy(TTs)=' num2str(measy.gouySeperation('TT1', 'TT2')) 'deg, Gouy(QPDs)=' num2str(measy.gouySeperation('QPD1', 'QPD2')) 'deg'], ...
    'Location', 'NorthWest');