clear all;
close all;
%% initial parameters
% This is a green path
lambda=1064e-9;

d_rail_to_lens = 82e-3
d_lens_to_DM2 =190e-3
d2_rail_to_lens = 135e-3
d3_rail_to_lens = 245e-3

d_to_DM2 = d_lens_to_DM2 + d_rail_to_lens + ([0, 1, 2, 3] * 0.0254)
x1 = [1274, 1170, 1063, 955]/2 * 1e-6;
y1 = [1283, 1174, 1070, 954]/2 *1e-6;

d2_to_DM2 =  d_lens_to_DM2 + d2_rail_to_lens + ([2, 3]*0.0254)
x2 = [830, 712]/2 * 1e-6;
y2 = [845, 734]/2 * 1e-6;

d3_to_DM2 = d_lens_to_DM2 + d3_rail_to_lens + ([0, 1, 2, 3]*0.0254)
x3 = [591.1, 501.7, 432.8, 372.6]/2 * 1e-6;
y3 = [614.9, 528.9, 442.2, 400.7]/2 * 1e-6;

% ideal waist location 
zTargetWaist= d_lens_to_DM2 + d_rail_to_lens + 0.3;
wTargetWaist=200e-6;

%% Seed the beamPath using the fit
path=beamPath();
path.seedWaist(wTargetWaist, zTargetWaist)

pathx = path.fitBeamWidth([d_to_DM2 d2_to_DM2 d3_to_DM2], [x1 x2 x3])
pathy = path.fitBeamWidth([d_to_DM2 d2_to_DM2 d3_to_DM2], [y1 y2 y3])

z = linspace(0.2, 0.7)
%% Set the target waist
path.targetWaist(wTargetWaist, zTargetWaist, lambda);

figure
pathx.plotBeamWidth(z, 'b')
hold on
pathx.plotBeams(z, .7e-3, 'b')
plot(d_to_DM2, x1, 'bx');
plot(d2_to_DM2, x2, 'b*');
plot(d3_to_DM2, x3, 'bo');
plot(d_lens_to_DM2, 0, 'kx')
annotation('textarrow',[.18 .24],[0.4 0.51],'String','D:L2')
pathy.plotBeamWidth(z,'r')
pathy.plotBeams(z, .7e-3, 'r')
plot(d_to_DM2, y1, 'rx')
plot(d2_to_DM2, y2, 'r*')
plot(d3_to_DM2, y3, 'ro')
grid on
xlabel('distance from D:M2 [m]')
ylabel('beam size [m]')
title('Beam profile HAM7 D path, blue is x (yaw) red is y (pit)')