%SNAKEMINIMIZE   Minimizes the energy function of a snake
%
% SYNOPSIS:
%  snake_out = snakeminimize(snake_in,Fext,alpha,beta,stepsz,kappa,iterations)
%
% PARAMETERS:
%  snake_in :   initial snake, Nx2 array with coordinates
%  Fext :       external force field, 2D-2D vector image
%               for example: GRADIENT, GVF, VFC
%  alpha :      elasticity parameter (membrane)
%  beta :       rigidity parameter (thin plate)
%  stepsz :     step size
%  kappa :      balloon force (negative for inwards force)
%  iterations : number of iterations performed
%
% DEFAULTS:
%  alpha = 0.2
%  beta = 0.4
%  stepsz = 1
%  kappa = 0
%  iterations = 20
%
% EXAMPLE:
%  a = noise(50+100*gaussf(rr>85,2),'gaussian',20)
%  f = gradient(gradmag(a,5));
%  f = f./max(norm(f));
%  x = 100+30*cos(0:0.1:2*pi); y = 150+40*sin(0:0.1:2*pi);
%  s = [x',y'];
%  snakedraw(s);
%  s = snakeminimize(s,f,0.01,100,3,0.3,20);
%  h = snakedraw(s);
%  set(h,'color',[0,0.7,0.7]);
%  s = snakeminimize(s,f,0.01,100,3,0.3,20);
%  snakedraw(s,h);
%  s = snakeminimize(s,f,0.01,100,3,0.3,20);
%  snakedraw(s,h);
%  s = snakeminimize(s,f,0.01,100,3,0.3,20);
%  snakedraw(s,h);
%  s = snakeminimize(s,f,0.01,100,3,0.3,20);
%  snakedraw(s,h);
%
% LITERATURE:
%  M. Kass, A. Witkin, D. Terzopoulos, "Snakes: Active Contour Models",
%     Int. J. of Computer Vision 4(1):321-331 (1988)
%  L.D. Cohen, I. Cohen, "Finite-element methods for active contour models and
%     balloons for 2-D and 3-D images", IEEE TPAMI 15(11):1131-1147 (1993)
%
% SEE ALSO: snakedraw, snake2im, vfc, gvf, dip_image/gradient

% (C) Copyright 2009, All rights reserved.
% Cris Luengo, Uppsala, 18-24 September 2009.

function s = snakeminimize(varargin)

d = struct('menu','Segmentation',...
           'display','Minimize snake energy',...
           'inparams',struct('name',       {'snake_in',   'Fext',                'alpha',     'beta',    'stepsz',   'kappa',        'iterations'},...
                             'description',{'Input snake','External force',      'Elasticity','Rigidity','Step size','Balloon force','Iterations'},...
                             'type',       {'array',      'image',               'array',     'array',   'array',    'array',        'array'},...
                             'dim_check',  {[-1,2],       2,                     0,           0,         0,          0,              0},...
                             'range_check',{'R+',         {'real','tensor'},     'R+',        'R+',      'R+',       'R',            'N'},...
                             'required',   {1,            1,                     0,           0,         0,          0,              0},...
                             'default',    {[],           'gradient(gradmag(a))',0.2,         0.4,       1,          0,              20}...
                            ),...
           'outparams',struct('name',{'snake_out'},...
                              'description',{'Output snake'},...
                              'type',{'array'}...
                              )...
          );
if nargin == 1
   s = varargin{1};
   if ischar(s) & strcmp(s,'DIP_GetParamList')
      s = d;
      return
   end
end
try
   [s,f,alpha,beta,stepsz,kappa,iterations] = getparams(d,varargin{:});
catch
   if ~isempty(paramerror)
      error(paramerror)
   else
      error(firsterr)
   end
end

if prod(imarsize(f))~=2
   error('The external force image must have 2D vectors ');
end
[maxx,maxy] = imsize(f);
maxx = maxx-1;
maxy = maxy-1;

x = s(:,1);
y = s(:,2);
if length(x)<3
   error('Snake has too few control points.')
end

% Constrict points to image
x(x<0) = 0;
x(x>maxx) = maxx;
y(y<0) = 0;
y(y>maxy) = maxy;

% Do the snake!
md = 1; % The average distance we want to keep between points.
[x,y] = resample(x,y,md);
%h = snakedraw([x,y]);
P = compute_matrix(length(x),alpha,beta,stepsz);
for ii = 1:iterations
   %disp(['iteration ii=',num2str(ii),', snake length = ',num2str(length(x))])

   % Do we need to resample the snake?
   d = sqrt( diff(x).^2 + diff(y).^2 );
   if any(d<md/3) || any(d>md*3)
      %disp('resampling snake')
      [x,y] = resample(x,y,md);
      P = compute_matrix(length(x),alpha,beta,stepsz);
   end
   
   % Calculate external force
   s = [x,y];
   fex = get_subpixel(f{1},s,'linear');
   fey = get_subpixel(f{2},s,'linear');

   % Calculate balloon force
   if kappa~=0
      b = [s(2:end,:);s(1,:)] - [s(end,:);s(1:end-1,:)];
      m = sqrt(sum(b.^2,2));
      bx =  b(:,2)./m;
      by = -b(:,1)./m;
      % Add balloon force to external force
      fex = fex+kappa*bx;
      fey = fey+kappa*by;
   end

   % Move control points
   x = P*(x+stepsz*fex);
   y = P*(y+stepsz*fey);

   % Constrict points to image
   x(x<0) = 0;
   x(x>maxx) = maxx;
   y(y<0) = 0;
   y(y>maxy) = maxy;
   
   %if mod(ii,10)
   %   snakedraw([x,y],h);
   %   drawnow;
   %   pause(0.5);
   %end

end
s = [x,y];



% The matrix P = (stepsz*A+I)^(-1)
function P = compute_matrix(N,alpha,beta,stepsz)
a = stepsz*(2*alpha+6*beta)+1;
b = stepsz*(-alpha-4*beta);
c = stepsz*beta;
P = diag(repmat(a,1,N));
P = P + diag(repmat(b,1,N-1), 1) + diag(   b, -N+1);
P = P + diag(repmat(b,1,N-1),-1) + diag(   b,  N-1);
P = P + diag(repmat(c,1,N-2), 2) + diag([c,c],-N+2);
P = P + diag(repmat(c,1,N-2),-2) + diag([c,c], N-2);
P = inv(P);



% Resamples the snake
function [x,y] = resample(x,y,d)
mode = 'linear'; r = 1;
% ( Alternative: mode = 'cubic'; r = 3; )
% replicate samples at either end (periodic boundary condition)
x = [x(end-r+1:end);x;x(1:r)];
y = [y(end-r+1:end);y;y(1:r)];
% p is the parameter along the snake
p = [0;cumsum(sqrt( diff(x).^2 + diff(y).^2 ))];
% the first control point should be at p=0
p = p-p(r+1);
% resample snake between first and last+1 control points
x = interp1(p,x,0:d:p(end-r+1),mode);
y = interp1(p,y,0:d:p(end-r+1),mode);
% if the last new point is too close to the first one, remove it
if norm([x(end),y(end)]-[x(1),y(1)]) < d/2
   x(end) = [];
   y(end) = [];
end
% ensure column vectors
x = x(:);
y = y(:);
if length(x)<3
   error('Snake has become too small!')
end
