%% beam fit

close all
clear classes
clear all

zScan = [3 6 9] * 0.0254 - 0.023 + 3.7e-3; % inches to m
xradiusN = [874 1021 1186] / 2 * 1e-6; % um
yradiusN = [838 1008 1178] / 2 * 1e-6; % um

xradiusS = [798 885 1034] / 2 * 1e-6; % um
yradiusS = [807 867 975] / 2 * 1e-6; % um

lam0 = 1550e-9; % 1550nm

launchN = beamPath;
launchS = beamPath;

launchN.seedWaist(0.87e-3 / 2, 4.21e-3, lam0); 
launchS.seedWaist(0.87e-3 / 2, 4.21e-3, lam0); 
% waist width, z position, wavelength
% Thor Labs F230APC-1550 (waist width=waist radius)

fitxN = launchN.fitBeamWidth(zScan,xradiusN); % z coordinates, radii
fityN = launchN.fitBeamWidth(zScan,yradiusN); % z coordinates, radii
fitxS = launchS.fitBeamWidth(zScan,xradiusS); % z coordinates, radii
fityS = launchS.fitBeamWidth(zScan,yradiusS); % z coordinates, radii

zdomain = -0.3:.001:0.7;%linspace(min(zScan),max(zScan),100);

%% define parameters
L_launch = (4.5 + 12 + 6) * 0.0254; % [m], length of path from fiber coupler to cavity mirror
L_cav    = 0.403;    % [m], length of PSOMA ring cavity
aoi_CM   = 7 * pi/180;  % [rad], angle of incidence on ring cavity curved mirror


[L_side, L_hyp, L_base, aoi_IC]= compute_aoi(L_cav, aoi_CM);

launch_z = (fitxN.getWaists + fityN.getWaists) / 2;
launch_w = ((329 + 348) / 2) * 1e-6; % waist in um from plot above



%target_w = 400e-6

z_1 = L_launch;
%z_2 = z_1 + L_base / 2;
z_2 = z_1 + L_side;
z_3 = z_2 + L_hyp;
z_4 = z_3 + L_base;

%target_z = L_launch + 3*L_cav + L_base / 2;
target_z = L_launch + L_cav / 2 + L_side;

% sanity check
if (z_4 == L_launch + L_cav)
    "paths good"
end

% data from beam profile
before_x = [2 3 4] * 0.0254 + z_1;
before_u = [980 1016 1009] / 2 * 1e-6;; % um
before_v = [923 965 1006] / 2 * 1e-6; % um

after_x = [5 6 7] * 0.0254 + z_2;
after_u = [1996 2274 2409] / 2 * 1e-6;
after_v = [1900 2038 2292] / 2 * 1e-6;




%% RoC 2

figure(3)
hold on


% create a new beam path object
psoma = beamPath;
% define seed waist
psoma.seedWaist(launch_w, launch_z, lam0); % define input beam
zdomain = -0.10:.001:1.2 ;%(L_launch + 2*L_cav + L_base);
%%%% add components to the beam path
% telescope lens 1 and 2
psoma.addComponent(component.lens(0.225, 2*0.0254, 'TL1'));
psoma.addComponent(component.lens(0.4129, 18*0.0254, 'TL2'));
% cavity mirrors
psoma.addComponent(component.flatMirror(z_1, "Ma_{flat}"))
% syntax: z position
roc = -2;
psoma.addComponent(component.curvedMirror(roc, z_2, "Mb1_{curved}"))
% syntax: radius of curvature, z position
orighandle=psoma.plotBeamWidth(zdomain, 'r');


% create a new beam path object
psoma = beamPath;
% define seed waist
psoma.seedWaist(launch_w, launch_z, lam0); % define input beam
zdomain = -0.10:.001:1.2 ;%(L_launch + 2*L_cav + L_base);
%%%% add components to the beam path
% telescope lens 1 and 2
psoma.addComponent(component.lens(0.225, 2*0.0254, 'TL1'));
psoma.addComponent(component.lens(0.4129, 18*0.0254, 'TL2'));
% cavity mirrors
psoma.addComponent(component.flatMirror(z_1, "Ma_{flat}"))
% syntax: z position
roc = -1;
psoma.addComponent(component.curvedMirror(roc, z_2, "Mb1_{curved}"))
% syntax: radius of curvature, z position
orighandle=psoma.plotBeamWidth(zdomain, 'm');


% create a new beam path object
psoma = beamPath;
% define seed waist
psoma.seedWaist(launch_w, launch_z, lam0); % define input beam
zdomain = -0.10:.001:1.2 ;%(L_launch + 2*L_cav + L_base);
%%%% add components to the beam path
% telescope lens 1 and 2
psoma.addComponent(component.lens(0.225, 2*0.0254, 'TL1'));
psoma.addComponent(component.lens(0.4129, 18*0.0254, 'TL2'));
% cavity mirrors
psoma.addComponent(component.flatMirror(z_1, "Ma_{flat}"))
% syntax: z position
roc = -.4
psoma.addComponent(component.curvedMirror(roc, z_2, "Mb1_{curved}"))
% syntax: radius of curvature, z position
orighandle=psoma.plotBeamWidth(zdomain, 'b');

% create a new beam path object
psoma = beamPath;
% define seed waist
psoma.seedWaist(launch_w, launch_z, lam0); % define input beam
zdomain = -0.10:.001:1.2 ;%(L_launch + 2*L_cav + L_base);
%%%% add components to the beam path
% telescope lens 1 and 2
psoma.addComponent(component.lens(0.225, 2*0.0254, 'TL1'));
psoma.addComponent(component.lens(0.4129, 18*0.0254, 'TL2'));
% cavity mirrors
psoma.addComponent(component.flatMirror(z_1, "Ma_{flat}"))
% syntax: z position
roc = 1
psoma.addComponent(component.curvedMirror(roc, z_2, "Mb1_{curved}"))
% syntax: radius of curvature, z position
orighandle=psoma.plotBeamWidth(zdomain, 'c');

% create a new beam path object
psoma = beamPath;
% define seed waist
psoma.seedWaist(launch_w, launch_z, lam0); % define input beam
zdomain = -0.10:.001:1.2 ;%(L_launch + 2*L_cav + L_base);
%%%% add components to the beam path
% telescope lens 1 and 2
psoma.addComponent(component.lens(0.225, 2*0.0254, 'TL1'));
psoma.addComponent(component.lens(0.4129, 18*0.0254, 'TL2'));
% cavity mirrors
psoma.addComponent(component.flatMirror(z_1, "Ma_{flat}"))
% syntax: z position
roc = 2
psoma.addComponent(component.curvedMirror(roc, z_2, "Mb1_{curved}"))
% syntax: radius of curvature, z position
orighandle=psoma.plotBeamWidth(zdomain, 'black');

psoma.plotComponents(zdomain, 'b*')


%% 
plot(before_x, before_u, 'ro')
plot(before_x, before_v, 'bo')
plot(after_x, after_u, 'ro')
plot(after_x, after_v, 'bo')


legend("RoC: -2 m", '', "RoC: -1 m", '',"RoC: -.4 m",'', "RoC: 1 m",'', "RoC: 2 m", '','location', 'northwest')

lines = findobj(gcf,'Type','Line');
for i = 1:numel(lines)
  lines(i).LineWidth = 2;
end
ax = gca;
ax.XAxis.FontSize = 15;
ax.YAxis.FontSize = 15;
set(findall(gcf,'-property','FontSize'),'FontSize',12)
%legend([orighandle newhandle],'Original Beam Path','Optimized Beam Path')
ylabel('Beam Width (m)')
xlabel('Propagation axis (m)')

set(gca, 'Box', 'off', 'TickDir', 'out', 'TickLength', [.02 .02], ...
    'XMinorTick', 'on', 'YMinorTick', 'on', 'YGrid', 'on', 'XGrid', 'on',...
    'XColor', [.3 .3 .3], 'YColor', [.3 .3 .3],  ...
    'LineWidth', 1)


hold off

%% 

function [L_side, L_hyp, L_base, aoi_IC] = compute_aoi(L_cav, aoi_CM)
    theta = 2 * aoi_CM;
    aoi_IC = pi / 4;
    L_hyp = L_cav / (1 + sin(theta) + cos(theta));
    L_side = L_hyp * cos(theta);
    L_base = L_hyp * sin(theta);
end