function motionCoherenceDemo(varargin)
% function motionCoherenceDemo(id)
%
% Demonstration of AIH motion coherence code (analytical solution).
%
% Author: Johann Strasser
% Date: 080902

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']); 
   random2DPointsWithBias();
elseif strcmp(mode, '2')
   disp(['Mode ', mode, ' executing']); 
end

function random2DPointsWithBias()

%% 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('twoDimensionalPointSetWithBias', 'templatePoints', 'referencePoints');

velocities = referencePoints - templatePoints;

% find a nice colour
% newColour = uisetcolor;

 f = figure('Position', [0, 0, 1461, 1096]);

templateColor = [0, 0.85, 0];
ht = plot(templatePoints(:, 1), templatePoints(:, 2), 'o', ...
    'MarkerEdgeColor', 'k', 'MarkerFaceColor', templateColor);
hold on;

referenceColor = [0.85, 0, 0];
hr = plot(referencePoints(:, 1), referencePoints(:, 2), 'o', ...
    'MarkerEdgeColor', 'k', 'MarkerFaceColor', referenceColor);
axis tight;
axis equal;

% 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);

% axis tight;
% delete(hr);

%% Plot velocities
hv = quiver(templatePoints(:, 1), templatePoints(:, 2), ...
    velocities(:, 1), velocities(:, 2), 0, 'g');

% %% 
% % initialize some parameters2
% % sigma = 10000;
% sigma = 1;
% mctBeta = 1; % Avoid nameclash with MATLAB beta function
% lambda = 1;
% anneal = 0.97;
% max_it = 100;
% % max_it = 1000;
% tol = 1e-3;
% % tol = 1e-6;
% 
% % calculate the coefficients that define the velocity field
% W = mctlibj.MCTReg.calculateVectorField(...
%     referencePoints, templatePoints, mctBeta, sigma, lambda, anneal, tol, max_it);
% 
% % apply those coefficients to the template point set.
% templatePointsTransformed = mctlibj.MCTReg.applyVectorField(...
%     templatePoints, templatePoints, referencePoints, W, mctBeta);
% 
% % figure;
% 
% % Generate grid for vector field
% % xMax = get(gca, 'XLim');
% % yMax = get(gca, 'YLim');
% 
% xMax = [min(templatePoints(:, 1)), max(templatePoints(:, 1))];
% yMax = [min(templatePoints(:, 2)), max(templatePoints(:, 2))];
% 
% 
% steps = 50;
% x = linspace(xMax(1), xMax(2), steps);
% y = linspace(yMax(1), yMax(2), steps);
% 
% % Or 
% % x = xMax(1):1:xMax(2);
% % y = yMax(1):1:yMax(2);
% 
% [gridX, gridY] = meshgrid(x, y);
% gridVector = [gridX(:), gridY(:)];
% 
% gridVectorTransformed = mctlibj.MCTReg.applyVectorField(...
%     gridVector, templatePoints, referencePoints, W, mctBeta);

%% Use analytical solution
% 
% [x0, datax] = mctlib_normalise('pts', referencePoints);
% [y0, datay] = mctlib_normalise('pts', templatePoints);

x0 = templatePoints;
y0 = referencePoints;

% Generate grid for vector field
xMax = get(gca, 'XLim');
yMax = get(gca, 'YLim');
% xMax = [min(referencePoints(:, 1)), max(referencePoints(:, 1))];
% yMax = [min(referencePoints(:, 2)), max(referencePoints(:, 2))];

steps = 20;
x = linspace(xMax(1), xMax(2), steps);
y = linspace(yMax(1), yMax(2), steps);

% Or 
% x = xMax(1):1:xMax(2);
% y = yMax(1):1:yMax(2);

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

% 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)
sigma = 10; % 0.1 lets the new points coincide with the reference 80 shows bias
lambda = 0.001; % Was 0.1, .001 is good
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 

templateTranformedColor = [0, 0, 1];
% htt = plot(templatePointsTransformed(:, 1), templatePointsTransformed(:, 2), 'o', ...
%     'MarkerEdgeColor', 'k', 'MarkerFaceColor', templateTranformedColor);

vField = gridVectorTransformed - gridVector;

hvf = quiver(gridVector(:, 1), gridVector(:, 2), ...
    vField(:, 1), vField(:, 2), 0, 'k');

% axis tight

save('twoDimensionalPointSetWithBias', 'templatePoints', 'referencePoints');

