% ititialization of the FMM from given curves
% curve1 and curve2 assumed to be closed, i.e. wrap around formula can be applied

% closed curves - can compute curvature at any point
m = size(curve1, 1);
n = size(curve2, 1);

disp('Computing the curvatures...');

CURVATURE_THRESHOLD = 10;

% compute curvatures k1, k2 
% wrap around at endpoints
k1 = zeros(m,1);
for i=0:m-1,
    delX = curve1(mod(i+1,m)+1, 1) - curve1(mod(i-1,m)+1, 1);
    delY = curve1(mod(i+1,m)+1, 2) - curve1(mod(i-1,m)+1, 2);
    k1(i+1) = 4*( (curve1(mod(i+1,m)+1, 2) - 2*curve1(mod(i,m)+1, 2) + ...
        curve1(mod(i-1,m)+1, 2))*delX - (curve1(mod(i+1,m)+1, 1) - ...
        2*curve1(mod(i,m)+1, 1) + curve1(mod(i-1,m)+1, 1))*delY ) / ...
        (delX*delX + delY*delY)^(3/2);
    %   k1(i) = norm(curve1(i, 1:2) - 2*curve1(i+1, 1:2) + curve1(i+2, 1:2));
    if k1(i+1) > CURVATURE_THRESHOLD
        k1(i+1) = CURVATURE_THRESHOLD;
    elseif k1(i+1) < -CURVATURE_THRESHOLD
        k1(i+1) = -CURVATURE_THRESHOLD;
    end
end
k2 = zeros(n,1);
for i=0:n-1,
    delX = curve2(mod(i+1,n)+1, 1) - curve2(mod(i-1,n)+1, 1);
    delY = curve2(mod(i+1,n)+1, 2) - curve2(mod(i-1,n)+1, 2);
    k2(i+1) = 4*( (curve2(mod(i+1,n)+1, 2) - 2*curve2(mod(i,n)+1, 2) + ... 
        curve2(mod(i-1,n)+1, 2))*delX - (curve2(mod(i+1,n)+1, 1) - ...
        2*curve2(mod(i,n)+1, 1) + curve2(mod(i-1,n)+1, 1))*delY ) / ...
        (delX*delX + delY*delY)^(3/2);
    %   k2(i) = norm(curve2(i, 1:2) - 2*curve2(i+1, 1:2) + curve2(i+2, 1:2));
    if k2(i+1) > CURVATURE_THRESHOLD
        k2(i+1) = CURVATURE_THRESHOLD;
    elseif k2(i+1) < -CURVATURE_THRESHOLD
        k2(i+1) = -CURVATURE_THRESHOLD;
    end
end

% % smooth out the curvature vectors
% filt = fspecial('gaussian', [1 7], 1.5);
% %filt = [1 2 1]/4;
% k1 = conv(k1, filt);
% k2 = conv(k2, filt);
% k1 = k1(4:(end-3));
% k2 = k2(4:(end-3));
% %k1 = k1(2:(end-1));
% %k2 = k2(2:(end-1));

disp('Computing the cost function F...'); 

% compute the speed function F
F = zeros(m,n);
for i=1:m,
    for j=1:n,
        F(i,j) = abs(k2(j) - k1(i));
    end
end

if get(checkAutoLambda, 'Value') == 1
	% adaptive lambda
	sumDiffs = sum(sum(F));
	valLambda = sumDiffs/(m*n);
end
F = F + valLambda;

% assumes closed curves as input - duplicate curve2 points along x-axis
F = [F F; F F; F F];

disp('Computing the weighted distance function T...');

% compute grid spacing array for each curve
spacing1 = zeros(m, 1);
spacing2 = zeros(n, 1);
spacing1(1) = norm(curve1(1,:) - curve1(m,:));
for i = 2:m,
    spacing1(i) = norm(curve1(i,:) - curve1(i-1,:));
end
spacing1 = [spacing1; spacing1; spacing1];
spacing2(1) = norm(curve2(1,:) - curve2(n,:));
for j = 2:n,
    spacing2(j) = norm(curve2(j,:) - curve2(j-1,:));
end
spacing2 = [spacing2; spacing2];

initSet = [1 1];
% start points - a line of zeros along curve 2 axis
%for i = 1:n,
%    initSet = [initSet; 1 i];
%end

% begin
T = FMM(initSet, F);

