function [Q rect] = m2emit_lin_nonlin()

d = imread('face.png');
d = double(rgb2gray(d));
d = d/max(d(:));
d = abs(1-d); d = flipud(d);
d = d(1:2:end,1:2:end);
size(d)

%h = figure(1);
%imagesc(d); axis xy; caxis([0 1]); colormap(bluewhitered(256));
%truesize(h);

% inverse mapping function
%m = [1 -0.5; 0 1];
%m = [1 0; 1 1];
%m = [1 0; -1 1];

% setup beamline

z = linspace(0, 720, 721);
d1 = padarray(d, [60 130]);
dn = phasespacetrans(d1, genmat2(1.4, 0));
%pprof = zeros(dns(2),length(z));
%profscale = 165;
%profheight = 130;
d1 = dn;
e0 = smc(d1);
for ii = 1:length(z)

    %dn = phasespacerotnonlin(d1, z(ii));    

    dnl = phasespacerotlin(d1, z(ii));
    dnl = fliplr(dnl);
    dnnl = phasespacerotnonlin(d1, z(ii));
    dnnl = fliplr(dnnl);
    %sum(dn(:))
%    prof = sum(dn,1).';
%    pprof(:,ii) = -prof/profscale;
%    pp = resizem(pprof, [130, dns(2)]);
    dnl = padarray(dnl, [50 0], 'pre');
    dnnl = padarray(dnnl, [50 0], 'pre');
    %dn = padarray(dn, [50 0], 'pre');
%    pp = padarray(pp, [size(dn,1)-size(pp,1) 0], 'pre');

    f3 = figure(3); set(f3, 'Color', [1 1 1]);
%    imagesc(dn+pp); axis xy; caxis([-1 1]); colormap(graywhitered(256));
    subplot(1,2,1);
    imagesc(dnl); axis xy; caxis([-1 1]); colormap(graywhitered(256));
    hold on;
    plot(1:size(dnl,2),sum(dnl,1)*0.55,'k','Linewidth',2);
    plot([1 size(dnl,2)], [0 0], 'k');
    drawellipse(dnl);
    title(sprintf('ideal focusing channel, emittance = %.2f', smc(dnl)/e0), 'FontSize', 16);
    hold off;
    axis off; 

    subplot(1,2,2);
    imagesc(dnnl); axis xy; caxis([-1 1]); colormap(graywhitered(256));
    hold on;
    plot(1:size(dnnl,2),sum(dnnl,1)*0.55,'k','Linewidth',2);
    plot([1 size(dnnl,2)], [0 0], 'k');
    drawellipse(dnnl);
    title(sprintf('with aberration, emittance = %.2f', smc(dnnl)/e0), 'FontSize', 16);
    hold off;
    axis off; 
    truesize(f3);
    %imagesc(d); colormap(gray);
%    drawbeamline(c, size(dn), profheight);
%    hold off;
    drawnow; 
    rect = get(f3, 'Position');
    rect = [150 40 rect(3)-250 rect(4)-40];
    Q(:,ii) = getframe(f3, rect); clf;
    %Q(:,ii) = getframe(f3);
end

end

function drawellipse(z)
[y,x] = ndgrid(1:size(z,1),1:size(z,2));
[e a b x0 y0] = smc(z, x, y);
e = 4*e;
t = linspace(0,2*pi);
m11 = sqrt(b);
r = sqrt(e);
x = x0+m11*r.* cos(t);
m21 = -a * sqrt(1 / b);
m22 = 1 / sqrt(b);
y = y0+m21*r.*cos(t) + m22*r.*sin(t);
plot(x,y,'--','Color',[0.5 0.5 0.5]);
end

function m = genmat2(L, k)
m = [cos(k*L) sinc(k*L/pi)*L; -k*sin(k*L) cos(k*L)];
end

function out = phasespacetrans(in, m)

f = @(x,unused) [m(1,1)*x(:,1)+m(1,2)*x(:,2), m(2,1)*x(:,1)+m(2,2)*x(:,2)];
% maketform arguments
ndims_in = 2;
ndims_out = 2;
forward_mapping = [];
inverse_mapping = f;
tdata = [];
tform = maketform('custom', ndims_in, ndims_out, ...
    forward_mapping, inverse_mapping, tdata);
out = imtransform(in, tform, 'UData', [-1 1], 'VData', [-1 1], ...
    'XData', [-1 1], 'YData', [-1 1]);

%out = resizem(out,[size(out,1) fix(size(out,2)/1.5)-1]);
%out = resizem(out,[size(out,1) fix(size(out,2)/1.5)]);

end

function out = phasespacerotlin(in, phi)

r = @(x) sqrt(x(:,1).^2 + x(:,2).^2);
w = @(x) atan2(x(:,2), x(:,1));
f = @(x,unused) [r(x).*cos(w(x)-deg2rad(phi)), r(x).*sin(w(x)-deg2rad(phi))];
% maketform arguments
ndims_in = 2;
ndims_out = 2;
forward_mapping = [];
inverse_mapping = f;
tdata = [];
tform = maketform('custom', ndims_in, ndims_out, ...
    forward_mapping, inverse_mapping, tdata);
out = imtransform(in, tform, 'UData', [-1 1], 'VData', [-1 1], ...
    'XData', [-1 1], 'YData', [-1 1]);

%out = resizem(out,[size(out,1) fix(size(out,2)/1.5)-1]);
%out = resizem(out,[size(out,1) fix(size(out,2)/1.5)]);

end

function out = phasespacerotnonlin(in, phi)

r = @(x) sqrt(x(:,1).^2 + x(:,2).^2);
w = @(x) atan2(x(:,2), x(:,1));
f = @(x,unused) [r(x).*cos(w(x)-deg2rad(phi).*(1-r(x))), r(x).*sin(w(x)-deg2rad(phi).*(1-r(x)))];
% maketform arguments
ndims_in = 2;
ndims_out = 2;
forward_mapping = [];
inverse_mapping = f;
tdata = [];
tform = maketform('custom', ndims_in, ndims_out, ...
    forward_mapping, inverse_mapping, tdata);
out = imtransform(in, tform, 'UData', [-1 1], 'VData', [-1 1], ...
    'XData', [-1 1], 'YData', [-1 1]);

%out = resizem(out,[size(out,1) fix(size(out,2)/1.5)-1]);
%out = resizem(out,[size(out,1) fix(size(out,2)/1.5)]);

end