function motionCoherenceDemo2D(varargin)
% function motionCoherenceDemo(id)
%
% Demonstration of AIH motion coherence code (analytical)
%
% Author: Johann Strasser
% Date: 081107

error(nargchk(0, inf, nargin));

if mod(length(varargin),2) ~= 0
    % Arguments have to come in parameter/vlaue pairs
    error(['Arguments to ', mfilename,' must come in parameter/value pairs.'])
end

mode = '1';

for i=1:2:length(varargin)
    switch lower(varargin{i})
        case 'mode'
            mode = varargin{i+1};            
            
            if isnumeric(mode)
                mode = num2str(mode);
            end
        otherwise
            error(['Unknown parameter name passed to ', mfilename, '.  Name was ' varargin{i}])
    end
end

disp([mfilename, ' executing.']);

% Initialise java for MCT
% mctRootdir = fileparts(which('mctj_demo.m'));
% mctj_initialize_java('mctroot', mctRootdir);

if strcmp(mode, '1')
   disp(['Mode ', mode, ' executing']); 
%    random2DPointsWithBiasSigmaDemo();
   random2DPointsWithBiasLambdaDemo();
elseif strcmp(mode, '2')
   disp(['Mode ', mode, ' executing']); 
end

function random2DPointsWithBiasSigmaDemo()

%% 2D Point set with bias

noOfPoints = 30;
scale = 100;
shift = 0;

% Generate random template points (scale and shift), use uniform
% distribution to get nicer spacing than with randn
templatePoints = rand(noOfPoints, 2) * scale + shift;

% Generate template points which implement random motion of the template 
% points with bias, use normal distribution
referencePoints = templatePoints + randn(noOfPoints, 2) * 5; % Scale random movement
referencePoints(:, 2) = referencePoints(:, 2) + 4;

load('motionCoherenceDemo2D_pointSetWithBias', 'templatePoints', 'referencePoints');
% load('twoDimensionalPointSetWithBias', 'templatePoints', 'referencePoints');

% Determine axis limits
xMaxT = [min(templatePoints(:, 1)), max(templatePoints(:, 1))];
yMaxT = [min(templatePoints(:, 2)), max(templatePoints(:, 2))];

xMaxR = [min(referencePoints(:, 1)), max(referencePoints(:, 1))];
yMaxR = [min(referencePoints(:, 2)), max(referencePoints(:, 2))];

xMax = [min(xMaxT(1), xMaxR(1)), max(xMaxT(2), xMaxR(2))];
yMax = [min(yMaxT(1), yMaxR(1)), max(yMaxT(2), yMaxR(2))];

border = 10;
xMax(1) = xMax(1) - border;
xMax(2) = xMax(2) + border;
yMax(1) = yMax(1) - border;
yMax(2) = yMax(2) + border;
 
% Generate grid for vector field
steps = 20;
x = linspace(xMax(1), xMax(2), steps);
y = linspace(yMax(1), yMax(2), steps);

[gridX, gridY] = meshgrid(x, y);
gridVector = [gridX(:), gridY(:)];

% Use analytical solution
x0 = templatePoints;
y0 = referencePoints;

% f = figure('Position', [0, 0, 1461, 1096]);
% f = figure('Position', [0, 0, 1096, 1096]);
f = figure('Position', [1000, 0, 670, 1096]);
set(f, 'Color', [1, 1, 1]);

% We want sigma to be large enough to have non-near-zero vector magnitudes
% in empty space (for the saved point set 8 is pretty good, 10 was best)
% sigmaStart = 0.5; % 0.1 lets the new points coincide with the reference 80 shows bias
lambda = 0.0001; % Was 0.1, .001 is good
sigmaStart = 0.2;

% Start with sigma small enough so vector field at measurement positions
% coincides with measurements. At the end we want sigma large enough to
% have the vector field be zero at the measurement positions. For the lower
% end sigma == 0.25 (lambda == 1). Doubling 6 times seems to cover the
% entire range. End of range then is 1.6

% However, using such low values of sigma, the magnitude of the vector
% field in empty areas always remains zero. So we add a second restriction:
% On top of the requirement of sigma low enough to satisfy the above
% constraint of, we require it to be high enough to generate non-zero
% magnitudes of the vector field in empty areas. Only lowering lambda to
% say 0.0000000000000000000001 does not do this as it causes the initial
% condition to persist over the range of sigmas. Only raising sigma does
% not help either as it does not qualify the initial condition
% lambda == 0.0001 and sigmaStart = 0.2 work, sigma 125 gives a good bias
% indicating vector field.

sigma = sigmaStart;
noSteps = 6;
stepSize = 2;

sigmaRange = linspace(0.1, 50, noSteps);

% figure;

for i = 1:noSteps
%     sigma = sigmaRange(i);
    V = (y0-x0);
    beta = mctlib_CalcParam('x0', x0, 'f', V, 'sigma', sigma, 'lambda', lambda);
    ynew = x0 + mctlib_CalcVel('x0', x0, 'y0', x0, 'sigma', sigma, 'beta', beta);

    templatePointsTransformed = ynew;
    gridVectorTransformed = gridVector + mctlib_CalcVel('x0', x0, 'y0', gridVector, 'sigma', sigma, 'beta', beta);

    % Draw
    options.drawTemplatePoints = 1;
    options.drawReferencePoints = 1;
    options.drawTemplatePointsTransformed = 1;
    options.drawTempToRefVectors = 1;
    options.drawTempToTempTransVectors = 1;
    options.drawGridVectorToGridVectorTransformedVectors = 1;

    subplot(ceil(noSteps / 2), 2, i);

%     hold on;
    drawPointsAndVectors(gca, templatePoints, referencePoints, templatePointsTransformed, gridVector, gridVectorTransformed, xMax, yMax, options);
    title(texlabel([sprintf('%c)    ', i + 96), 'lambda = ', sprintf('%6.1e', lambda), '     sigma = ', sprintf('%6.4f', sigma)]));
%     hold off;

    sigma = sigma * 5; % % works well with lambda == 0.0001 and sigmaStart == 0.2
    %     analyticalMCTVaryingSigmaConstantLambda.png
%     f = getframe();
%     imres = frame2im(f);
%     format = 'png';
%     imFileName = 'analyticalMCTVaryingSigmaConstantLambda';
%     imwrite(imres, [imFileName, sprintf('%i', i), ['.'], format]);
%     saveas(gcf, [imFileName, sprintf('%i', i), '.png'], 'png');
%     cla;
end

% Note that the following does not yield the same result as manual save
% using the figure's File menu
% saveas(f, 'analyticalMCTVaryingSigmaConstantLambda.png', 'png');

function random2DPointsWithBiasLambdaDemo()

%% 2D Point set with bias

noOfPoints = 30;
scale = 100;
shift = 0;

% Generate random template points (scale and shift), use uniform
% distribution to get nicer spacing than with randn
templatePoints = rand(noOfPoints, 2) * scale + shift;

% Generate template points which implement random motion of the template 
% points with bias, use normal distribution
referencePoints = templatePoints + randn(noOfPoints, 2) * 5; % Scale random movement
referencePoints(:, 2) = referencePoints(:, 2) + 4;

load('motionCoherenceDemo2D_pointSetWithBias', 'templatePoints', 'referencePoints');
% load('twoDimensionalPointSetWithBias', 'templatePoints', 'referencePoints');

% Determine axis limits
xMaxT = [min(templatePoints(:, 1)), max(templatePoints(:, 1))];
yMaxT = [min(templatePoints(:, 2)), max(templatePoints(:, 2))];

xMaxR = [min(referencePoints(:, 1)), max(referencePoints(:, 1))];
yMaxR = [min(referencePoints(:, 2)), max(referencePoints(:, 2))];

xMax = [min(xMaxT(1), xMaxR(1)), max(xMaxT(2), xMaxR(2))];
yMax = [min(yMaxT(1), yMaxR(1)), max(yMaxT(2), yMaxR(2))];

border = 10;
xMax(1) = xMax(1) - border;
xMax(2) = xMax(2) + border;
yMax(1) = yMax(1) - border;
yMax(2) = yMax(2) + border;
 
% Generate grid for vector field
steps = 20;
x = linspace(xMax(1), xMax(2), steps);
y = linspace(yMax(1), yMax(2), steps);

[gridX, gridY] = meshgrid(x, y);
gridVector = [gridX(:), gridY(:)];

% Use analytical solution
x0 = templatePoints;
y0 = referencePoints;

% f = figure('Position', [0, 0, 1461, 1096]);
% f = figure('Position', [0, 0, 1096, 1096]);
f = figure('Position', [1000, 0, 670, 1096]);
set(f, 'Color', [1, 1, 1]);

% We want sigma to be large enough to have non-near-zero vector magnitudes
% in empty space (for the saved point set 8 is pretty good, 10 was best)
% sigmaStart = 0.5; % 0.1 lets the new points coincide with the reference 80 shows bias
% lambda = 0.0001; % Was 0.1, .001 is good
lambdaStart = 0.0000001;
sigmaStart = 0.2;

% Start with sigma small enough so vector field at measurement positions
% coincides with measurements. At the end we want sigma large enough to
% have the vector field be zero at the measurement positions. For the lower
% end sigma == 0.25 (lambda == 1). Doubling 6 times seems to cover the
% entire range. End of range then is 1.6

% However, using such low values of sigma, the magnitude of the vector
% field in empty areas always remains zero. So we add a second restriction:
% On top of the requirement of sigma low enough to satisfy the above
% constraint of, we require it to be high enough to generate non-zero
% magnitudes of the vector field in empty areas. Only lowering lambda to
% say 0.0000000000000000000001 does not do this as it causes the initial
% condition to persist over the range of sigmas. Only raising sigma does
% not help either as it does not qualify the initial condition
% lambda == 0.0001 and sigmaStart = 0.2 work, sigma 125 gives a good bias
% indicating vector field.

% sigma = sigmaStart;
lambda = lambdaStart;
sigma = 25;
noSteps = 6;
stepSize = 2;

sigmaRange = linspace(0.1, 50, noSteps);
lambdaRange = linspace(0.000000001, 0.001, noSteps);

for i = 1:noSteps
%     sigma = sigmaRange(i);
%     lambda = lambdaRange(i);
    V = (y0-x0);
    beta = mctlib_CalcParam('x0', x0, 'f', V, 'sigma', sigma, 'lambda', lambda);
    ynew = x0 + mctlib_CalcVel('x0', x0, 'y0', x0, 'sigma', sigma, 'beta', beta);

    templatePointsTransformed = ynew;
    gridVectorTransformed = gridVector + mctlib_CalcVel('x0', x0, 'y0', gridVector, 'sigma', sigma, 'beta', beta);

    % Draw
    options.drawTemplatePoints = 1;
    options.drawReferencePoints = 1;
    options.drawTemplatePointsTransformed = 1;
    options.drawTempToRefVectors = 1;
    options.drawTempToTempTransVectors = 1;
    options.drawGridVectorToGridVectorTransformedVectors = 1;

    subplot(ceil(noSteps / 2), 2, i);
%     subplot(1, 2, 1);
    % hold on;
    drawPointsAndVectors(gca, templatePoints, referencePoints, templatePointsTransformed, gridVector, gridVectorTransformed, xMax, yMax, options);
    
    % ASCII a is 97, we use that to number the figures
    title(texlabel([sprintf('%c)    ', i + 96), 'lambda = ', sprintf('%6.1e', lambda), '     sigma = ', sprintf('%6.4f', sigma)]));
    % hold off;

%     sigma = sigma * 5; % % works well with lambda == 0.0001 and sigmaStart == 0.2
    lambda = lambda * 10;  
end

% Note that the following does not yield the same result as manual save
% using the figure's File menu
% saveas(f, 'analyticalMCTVaryingSigmaConstantLambda.png', 'png');

function drawPointsAndVectors(axisHandle, templatePoints, referencePoints, templatePointsTransformed, gridVector, gridVectorTransformed, xMax, yMax, options)
%
%

velocities = referencePoints - templatePoints;
toTemplatePointsTransformedVelocities = templatePointsTransformed - templatePoints; 
vField = gridVectorTransformed - gridVector;

templateColor = [0, 0.85, 0];
referenceColor = [0.85, 0, 0];

if options.drawTemplatePoints == 1
    ht = plot(axisHandle, templatePoints(:, 1), templatePoints(:, 2), 'o', ...
        'MarkerEdgeColor', 'k', 'MarkerFaceColor', templateColor);
end

hold on;

if options.drawReferencePoints == 1
    hr = plot(axisHandle, referencePoints(:, 1), referencePoints(:, 2), 'o', ...
        'MarkerEdgeColor', 'k', 'MarkerFaceColor', referenceColor);
end

% Use the two 
axis tight;
axis equal;

if options.drawTemplatePointsTransformed == 1
templateTranformedColor = [0, 0, 1];
htt = plot(axisHandle, templatePointsTransformed(:, 1), templatePointsTransformed(:, 2), 'o', ...
    'MarkerEdgeColor', 'k', 'MarkerFaceColor', templateTranformedColor);
end

% % Set axis limits
% xMaxT = [min(templatePoints(:, 1)), max(templatePoints(:, 1))];
% yMaxT = [min(templatePoints(:, 2)), max(templatePoints(:, 2))];
% 
% xMaxR = [min(referencePoints(:, 1)), max(referencePoints(:, 1))];
% yMaxR = [min(referencePoints(:, 2)), max(referencePoints(:, 2))];
% 
% xMax = [min(xMaxT(1), xMaxR(1)), max(xMaxT(2), xMaxR(2))];
% yMax = [min(yMaxT(1), yMaxR(1)), max(yMaxT(2), yMaxR(2))];
% 
% border = 10;
% xMax(1) = xMax(1) - border;
% xMax(2) = xMax(2) + border;
% yMax(1) = yMax(1) - border;
% yMax(2) = yMax(2) + border;
% 
xlim(xMax);
ylim(yMax);


% Plot vectors from template points to reference points
if options.drawTempToRefVectors == 1
    hv = quiver(axisHandle, templatePoints(:, 1), templatePoints(:, 2), ...
        velocities(:, 1), velocities(:, 2), 0, 'r');
end

% Plot vectors from template points to transformed template points
if options.drawTempToTempTransVectors == 1
    htv = quiver(templatePoints(:, 1), templatePoints(:, 2), ...
        toTemplatePointsTransformedVelocities(:, 1), toTemplatePointsTransformedVelocities(:, 2), 0, 'b');
end

% Plot grid vectors
if options.drawGridVectorToGridVectorTransformedVectors == 1
    hvf = quiver(axisHandle, gridVector(:, 1), gridVector(:, 2), ...
        vField(:, 1), vField(:, 2), 0, 'k');
end

% Delete all open figures
% delete(findobj('Type', 'figure'));