
function jsmctj_demo(varargin)
% function mctj_demo(id)
%
% A method that demonstrates the use of the motion coherent velocity field
% estimation methods. Here we generate 
%
% Inputs:
%  id - 1, 2, 3 (various demos)
%
% Example 1:
% mct_demo;
%
% Example 2:
% mct_demo('id', 1);
%
% Example 3:
% mct_demo('id', 2);
%
% Example 4:
% mct_demo('id', 3);
%
% Dr. A. I. Hanna, CMP, UEA, 2008.

id = parseargs(varargin);
rootdir = fileparts(which(mfilename));

% mctj_initialize_java('mctroot', rootdir);

% Generate some synthetic data where X is the reference point set and Y is the
% template point set.
N = 20;
switch id
    case 1
        X = randn(N,2)*5+30;
        Y = 3*(X-30) + 30;
        Y = Y + randn(size(Y))/10;
    case 2
        X = randn(N,3)*5;
        Y = X - 10;
        Y = Y + randn(size(Y))/10;
    case 3
        X = randn(N,3)*5;
        Y = 2*X + 0;
        Y = Y + randn(size(Y))/10;
    case 4
        X = randn(N,2)*5+30;
%         Y = 3*(X-30) + 30;
        Y = randn(20, 2) * 5 * 3 + 30;
        Y = Y + randn(size(Y))/10;
end
ind = randperm(size(Y,1));
Y = Y(ind,:);

%%%%%%
%
% BEGIN CORE
%
%%%%%%
% initialize some parameters
sigma = 1;
beta = 1;
lambda = 1;
anneal = 0.97;
max_it = 100;
tol = 1e-3;

% calculate the coefficients that define the velocity field
W = mctlibj.MCTReg.calculateVectorField(X, Y, beta, sigma, lambda, anneal, tol, max_it);

% apply those coefficients to the template point set.
Y2 = mctlibj.MCTReg.applyVectorField(Y, Y, X, W, beta);

%%%%%%
%
% END CORE
%
%%%%%%


% For each point in the warped template point set, find the closest point
% in the reference set and make the correspondance.
m1 = [];
m2 = [];
for i=1:size(X,1)
   d = matdistance(Y2, X(i,:)); 
   [val, ind] = min(d);
    m2 = cat(1, m2, X(i,:));
    m1 = cat(1, m1, Y(ind(1),:));
end

%%%
% the art corner
%%%

% Show the initial point sets
figure(1); clf;

%%% plots of reference and template point sets
subplot(2,2,1);
ph = plot_point_sets(X, Y);
title('Reference and Template Point Set');

% subplot(2,2,2);
% ph = plot_point_sets(Y2, X);
% title('Reference Point Set and Template Point Set plus coordinates');

%%% assignment of reference and template point sets
subplot(2,2,2);
ph = plot_point_sets(X, Y);
assign_h = plot_assignment(X, Y);
title('Initial Registration');

%%% registered assignments of reference and template point sets
subplot(2,2,3);
ph = plot_point_sets(X, Y);
assign_h = plot_assignment(m1, m2);
title('Final Registration');

%%% reference and template point sets together with the continuous velocity
%%% field.
subplot(2,2,4);
ph = plot_point_sets(X, Y);
plot_vector_field(X, Y, W, beta);
title('Computed Velocity Field');

return;


%%%
%
%%%
function ph = plot_point_sets(X, Y)
gridcol = [1 1 1];
%set(gcf, 'Color', [0 0 0]);
%set(gca, 'Color', [0 0 0], 'XColor', gridcol, 'YColor', gridcol, 'ZColor', gridcol);
%grid on; 
box on;
hold on;
ph = [];
ms = 7;
switch size(X,2)
    case 2
        ph = cat(1, ph, plot(X(:,1), X(:,2), 'o', 'MarkerFaceColor', 'c', 'MarkerEdgeColor', 'm', 'MarkerSize', ms));
        ph = cat(1, ph, plot(Y(:,1), Y(:,2),  's', 'MarkerFaceColor', 'y', 'MarkerEdgeColor', 'r', 'MarkerSize', ms));
        axis image;
    case 3
        ph = cat(1, ph, plot3(X(:,1), X(:,2), X(:,3), 'co', 'MarkerFaceColor', 'c', 'MarkerEdgeColor', 'm', 'MarkerSize', ms));
        ph = cat(1, ph, plot3(Y(:,1), Y(:,2), Y(:,3),  'rs', 'MarkerFaceColor', 'y', 'MarkerEdgeColor', 'r', 'MarkerSize', ms));
        view(3); 
        axis tight vis3d;
        
end
return
%%%
%
%%%
function ph = plot_assignment(X, Y)
linecol = [0 0 0];
axis vis3d;
hold on;
ph = [];
lw = 1;
switch size(X,2)
    case 2
        for i=1:size(X,1)
            ph = cat(1, ph, plot([X(i,1) Y(i,1)],[X(i,2) Y(i,2)], ':', 'Color', linecol, 'LineWidth', lw));
        end
    case 3
        for i=1:size(X,1)
            ph = cat(1, ph, plot3([X(i,1) Y(i,1)],[X(i,2) Y(i,2)],[X(i,3) Y(i,3)], ':', 'Color', linecol, 'LineWidth', lw));
        end
end
return;

%%%
%
%%%
% function plot_vector_field(X, Y, W, beta)
% switch size(X,2)
%     case 2
%         xlim = get(gca, 'XLim');
%         ylim = get(gca, 'YLim');
%         xi = linspace(xlim(1), xlim(2), 10);
%         yi = linspace(ylim(1), ylim(2), 10);
%         [Fx, Fy] = meshgrid(xi, yi);
%         F = [Fx(:), Fy(:)];
%         F2 = mctlibj.MCTReg.applyVectorField(F, Y, X, W, beta);
%         quiver(F(:,1), F(:,2), F(:,1) - F2(:,1), F(:,2) - F2(:,2), 'k');
% 
%     case 3
%         xlim = get(gca, 'XLim');
%         ylim = get(gca, 'YLim');
%         zlim = get(gca, 'ZLim');
%         %
%         xi = linspace(xlim(1), xlim(2), 5);
%         yi = linspace(ylim(1), ylim(2), 5);
%         zi = linspace(zlim(1), zlim(2), 5);
% 
%         [Fx, Fy, Fz] = meshgrid(xi, yi, zi);
%         F = [Fx(:), Fy(:), Fz(:)];
%         F2 = mctlibj.MCTReg.applyVectorField(F, Y, X, W, beta);
%         quiver3(F(:,1), F(:,2), F(:,3), F(:,1) - F2(:,1), F(:,2) - F2(:,2), F(:,3) - F2(:,3), 'k');
% end
%%%
%
%%%
function id = parseargs(varargin)
varargin = varargin{1};
% Default params
if mod(length(varargin),2) ~= 0
    % input args have not com in pairs, woe is me
    error(['Arguments to ', mfilename,' must come param/value in pairs.'])
end
id = 1;
for i=1:2:length(varargin)
    switch lower(varargin{i})
        case 'id'
            id = varargin{i+1};
        otherwise
            error(['Unknown parameter name passed to ', mfilename, '.  Name was ' varargin{i}])
    end
end
