%FINDLOCMAX   Finds iteratively the local maximum in a LSIZE cube
%
% SYNOPSIS:
%   [postion,value] = findlocmax(in, cood, lsize, subpixel, subpixelcube, submeth)
%
% PARAMETERS:
%   in:    input image
%   cood:  starting coordinates
%   lsize: cube size = 2*floor(lsize./2)+1
%   subpixel: 'yes','no' returns subpixel max position
%             via the center of intensities;
%             for negative intensities parabolic fit
%   subpixelcube: cube size = 2*floor(subpixelcube./2)+1
%   submeth: 'CenterOfMass','Parabolic','Gaussian'
%  
% DEFAULTS:
%   subpixel: 'no'
%   subpixelcube: 0 %same as lsize
%   submeth: 'CenterOfMass'
% 
% EXAMPLES:
%   out = findlocmax(readim,[20 160],[15 15])

% (C) Copyright 1999-2004               Pattern Recognition Group
%     All rights reserved               Faculty of Applied Physics
%                                       Delft University of Technology
%                                       Lorentzweg 1
%                                       2628 CJ Delft
%                                       The Netherlands
%
% Bernd Rieger, 2004
% July 2004, added max finding via center of mass of intensities
% September 2004, nD subpixel finding with own size 
% October 2004, added support for negative values, where center of mass does not work 
% April, 2005, return max value at maximum position 
% April, 2005, added several subpixel methods
% July 2006, remove local background for CM, still the best way is to go with an object
%            mask + Gravity, the square box is not optimal, espeically for skew intensity
 
function [out,out2] = findlocmax(varargin)


d = struct('menu','Analysis',...
        'display','Find local maximum',...
        'inparams',struct('name',{'in','cood','lsize','subp','subp_lsize','submeth'},...
   'description',{'Input image','Start coordinate','Size of the cube',...
               'Subpixel position','Size of the cube for subpixel','Subpixel method'},...
   'type',              {'image','array','array','boolean','array','option'},...
   'dim_check',  {0,1,1,0,1,0},...
   'range_check',{[],'R+','N',0,'N',{'CenterOfMass','Parabolic','Gaussian'}},...
   'required',  {1,1,1,0,0,0},...
   'default',   {'a',128,10,0,0,'CenterOfMass'}...
        ),...
   'outparams',struct('name',{'out','out2'},...
          'description',{'Position','Value'},...
          'type',{'image','array'}...
          )...
 );
if nargin == 1
   s = varargin{1};
   if ischar(s) & strcmp(s,'DIP_GetParamList')
      out = d;
      return
   end
end
try
   [in,cood,lsize,subp,subp_lsize,submeth] = getparams(d,varargin{:});
catch
   if ~isempty(paramerror)
      error(paramerror)
   else
      error(firsterr)
   end
end
%give possibility to supply not int cood, but round it here 
cood = round(cood);
in = squeeze(in);%just make sure
if any(cood > size(in))
   error('Coordinates not within the image.');
end


N=ndims(in);
s = floor(lsize./2);
%2s+1 neighborhood
co = zeros(N,2);%corner array
ss = 'roi=in(';%make roi coordinate array
ts = 'tmpv = in(';
for ii=1:N
   ss = [ss 'co(' num2str(ii) ',1):co(' num2str(ii) ',2),'];
   ts = [ts 'cood(' num2str(ii) '),']; 
end
ss(end)=[];ts(end)=[];
ss = [ ss ');'];
ts=[ts ');'];

%fprintf('cood: %d %d, s: %d %d\n',cood,s);
done =0;
while ~done
   for ii=1:N  
      co(ii,1) = max(min(cood(ii)-s(ii),size(in,ii)-1),0);
      co(ii,2) = max(min(cood(ii)+s(ii),size(in,ii)-1),0);        
   end  
   eval(ss);        %get roi
   v = max(roi);    %get max value in roi
   %fprintf('max_n: %f, cood(1) %d, cood(2) %d\n',v,cood(1),cood(2))
   eval(ts);        %get value of prior max
   if v == tmpv
      out2 = v; %intensity
      out = cood; %coordinate position
      done = 1;
   else
      nc = findcoord(roi==v);
      nc = nc(1,:);%just in case we find more than one max
      cood = cood - s;
      cood(cood<0)=0;      
      cood = cood + nc;
   end
end

if subp
   if ~subp_lsize
      subp_lsize = s;
   else
      subp_lsize = floor(subp_lsize./2);
   end
   %fprintf(' out:%d %d\n',out)
   mask = getcube(in,out,subp_lsize);
   if any(in(mask)<0)
      warning(['Negative values in the ROI of the image, center of mass estimation does not work ' ...
               'switching to parabolic subpixel fit.']);
         submeth = 'Parabolic';
   end
   switch submeth
      case 'CenterOfMass'
         [tmp,v] = dip_isodatathreshold(in,mask,1);
         mask2 = dip_image(tmp*mask,'sint32'); %this may give a unconnected mask
         if sum(mask2)==0
            mask2 = mask;
         end
         msr = measure(mask2,in-min(in(mask2>0)),{'Gravity'},[],1,0,0);
         out = msr.Gravity;
      case 'Parabolic'
         mask = newim(in,'bin');
         mask = setcoord(mask,out);
         out = dip_subpixmax(in,mask,'parabolic');
         out=out';
      case 'Gaussian'
         mask = newim(in,'bin');
         mask = setcoord(mask,out);
         out = dip_subpixmax(in,mask,'gaussian');
         out=out';
   end      
   if nargout ==2
     out2 = getcube_int(in,out,subp_lsize);
   end
end

%retrieves intensity image cube with boundary cutoff
function out = getcube_int(in,cood,s)
N=ndims(in);
co2 = cood;
cood=round(cood);
for ii=1:N  
      co(ii,1) = max(min(cood(ii)-s(ii)-2,size(in,ii)-1),0);
      co(ii,2) = max(min(cood(ii)+s(ii)+2,size(in,ii)-1),0);        
end
upleft = co(:,1); 
sz = diff(co,1,2);
out = dip_crop(in, upleft, sz);
%co2 = co2-cood;

co2 = co2- upleft;
%fprintf('\nco2: %f %f\n',co2);
%fprintf('sz : %d %d\n',sz);
%fprintf('upl: %d %d\n',upleft);
%out = get_subpixel(out,[s+1]'+co2,'cubic');
out = get_subpixel(out,co2,'cubic');


%retrieves binary image cube with boundary cutoff
function mask = getcube(in,cood,s)
mask = newim(in,'bin');
N=ndims(in);
for ii=1:N  
      co(ii,1) = max(min(cood(ii)-s(ii),size(in,ii)-1),0);
      co(ii,2) = max(min(cood(ii)+s(ii),size(in,ii)-1),0);        
end  
ss = 'mask(';
for jj=1:N
   ss = [ss 'co(' num2str(jj) ',1):co(' num2str(jj) ',2),'];
end
ss(end)=[];
ss = [ss ')=1;']; 
eval(ss);   
   
function out = setcoord(in,cood)
out = in;
s = 'out(';
for ii=1:length(cood)
   s = [s 'cood(' num2str(ii) '),'];
end
s(end)=[];
s = [s ')=1;'];
eval(s);
   
   
