% ititialization of the FMM from given curves
% lambda is the stretch parameter
%function T = process(curve1, curve2, lambda)

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

m = size(curve1, 1)-2;
n = size(curve2, 1)-2;
CURVATURE_THRESHOLD = 10;

% compute curvatures k1, k2 (disregard endpoints)
k1 = zeros(m,1);
k2 = zeros(n,1);
for i=1:m,
    delX = curve1(i+2, 1) - curve1(i, 1);
    delY = curve1(i+2, 2) - curve1(i, 2);
    k1(i) = 4*( (curve1(i+2, 2) - 2*curve1(i+1, 2) + curve1(i, 2))*delX - ...
        (curve1(i+2, 1) - 2*curve1(i+1, 1) + curve1(i, 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) > CURVATURE_THRESHOLD
        k1(i) = CURVATURE_THRESHOLD;
    elseif k1(i) < -CURVATURE_THRESHOLD
        k1(i) = -CURVATURE_THRESHOLD;
    end
end

for i=1:n,
    delX = curve2(i+2, 1) - curve2(i, 1);
    delY = curve2(i+2, 2) - curve2(i, 2);
    k2(i) = 4*( (curve2(i+2, 2) - 2*curve2(i+1, 2) + curve2(i, 2))*delX - ...
        (curve2(i+2, 1) - 2*curve2(i+1, 1) + curve2(i, 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) > CURVATURE_THRESHOLD
        k2(i) = CURVATURE_THRESHOLD;
    elseif k2(i) < -CURVATURE_THRESHOLD
        k2(i) = -CURVATURE_THRESHOLD;
    end
end

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

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

F = [];
SMALL = 0.0001;
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;

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

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

% start point
initSet = [1 1];
% begin 
T = FMM(initSet, F);

