close all; clear; clc;

%%
th = linspace(-pi,pi,1e2+1); th(end) = [];
K = 1.5*exp(-1i*th);
Nmax = 100;

options = odeset('RelTol',1e-6,'AbsTol',1e-6,'MaxStep',0.01);

ps = linspace(-1/2,0,10); ps([1 end]) = [];
cm = plasma(numel(ps)+1);

figure(1); clf;
gs = {-[0.6,1.2],[0.9,0.2]};
for jj = 1:2
a = subplot(2,2,2*jj); hold on;

M1 = resonator_alignment_make_mirror(gs{jj}(1),1);
M2 = resonator_alignment_make_mirror(gs{jj}(2),2);
options.Events = @(t,y) resonator_alignment_eventfun(t,y,M1,M2);

hm = plot(M1,'k-','linewidth',2); plot(M2,'k-','linewidth',2);
plot(K,'k--');
axis equal; grid on; box on;
xlim([-1 1]*1.75); ylim([-1 1]*0.75);
drawnow;

N = [];
for j = 1:numel(ps)
    yc = [0+ps(j)*1i 1+0i]; % Starting position
    Y = yc(1); T = 0;
    for n = 1:Nmax
        f = @(t,y) [y(2);0];
        [t,y] = ode23(f,[0 100],yc,options);
        yc = resonator_alignment_impactmap(y(end,:),M1,M2);
        Y = [Y; yc(1)]; T = [T; t(end)+T(end)];
        if abs(yc(1))>=1.5, break, end % Beam escaped the cavity
    end
    N(end+1) = n;
    if true
        hp(j) = plot(0,ps(j),'o','color',cm(j,:));
        for ti = 2:numel(T)
            alpha = (1-(ti-2)./numel(T));
            if ti > 20, break, end
            color = [cm(j,:) alpha];
            plot(Y(ti-1:ti),'color',color,'linewidth',1);
            if ~mod(ti,1); end
        end
        drawnow
    end
end
uistack(hp,'top');
for j = 1:numel(ps)
    hp(j).Marker = '.';
    hp(j).MarkerSize = 10;
end
uistack(hm,'top');
a.XTickLabel = ''; a.YTickLabel = '';
end

%% Add the 
load('resonator_alignment_Ns_60x60.mat')
a = subplot(2,2,[1 3]); hold on;
imagesc(g1s,g2s,mean(Ns,3)');
plot(g1s(g1s<0),1./g1s(g1s<0),'w--','linewidth',1);
plot(g1s(g1s>0),1./g1s(g1s>0),'w--','linewidth',1);
plot([0 0],[-2 2],'w--','linewidth',1); plot([-2 2],[0 0],'w--','linewidth',1);
plot(gs{1}(1),gs{1}(2),'wo'); plot(gs{2}(1),gs{2}(2),'wo');
plot(0.8,0.8,'rx');
xlim([-1 1]*2); ylim([-1 1]*2);
a.YDir = 'normal';
axis square;
colormap(viridis);
xlabel('g1'); ylabel('g2');
