close all; clear; clc;

%% Show beam in the cavity for many possible geometrical factors
K = exp(1i*linspace(0,2*pi,1e2+1))*1.5; K(end) = [];
Nmax = 50;
options = odeset('RelTol',1e-6,'AbsTol',1e-6,'MaxStep',0.01);

g1s = linspace(0,1,20); g2s = -g1s;
ps = linspace(-1/2,1/2,10+2); ps([1 end]) = [];
Ns = zeros(numel(g1s),numel(g2s),numel(ps));
for g1i = 1:numel(g1s)
    for g2i = 1:numel(g2s)
        M1 = resonator_alignment_make_mirror(g1s(g1i),1);
        M2 = resonator_alignment_make_mirror(g2s(g2i),2);
        options.Events = @(t,y) resonator_alignment_eventfun(t,y,M1,M2);
        N = [];
        fprintf('g1 = %.2f, g2 = %.2f\n',g1s(g1i),g2s(g2i));
        for pi = 1:numel(ps)
            yc = [0+ps(pi)*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)]; %#ok<AGROW>
                if abs(yc(1))>=1.5, break, end % Beam escaped the cavity
            end
            N(end+1) = n;
            
            % Plot
            figure(1); clf;
            plot(0,ps(pi),'ko'); hold on;
            plot(M1,'k-','linewidth',2); plot(M2,'k-','linewidth',2);
            plot(K,'k--');
            axis equal; grid on;
            xlim([-1 1]*2); ylim([-1 1]*2);
            
            cm = viridis(numel(T));
            for ti = 2:numel(T)
                plot(Y(ti-1:ti),'color',cm(ti,:),'linewidth',1);
            end
            fprintf('  p = %+.2f | n = %d\n',ps(pi),n);
        end
        fprintf('---\n');
        Ns(g1i,g2i,:) = N;
        figure(2); imagesc(g1s,g2s,mean(Ns,3)); hold on;
        plot(g1s(g1s<0),1./g1s(g1s<0),'k--','linewidth',1);
        plot(g1s(g1s>0),1./g1s(g1s>0),'k--','linewidth',1);
        plot([0 0],[-2 2],'k--','linewidth',1); plot([-2 2],[0 0],'k--','linewidth',1);
        a = gca; a.YDir = 'normal';
        grid on; box on; axis equal; xlim([min(g1s) max(g1s)]); ylim([min(g2s) max(g2s)]);
        drawnow;
    end
end