%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (c) 2012, Andreas Karrenbauer (andreas.karrenbauer@uni-konstanz.de)
% Copyright (c) 2012, Christoph Kölbl (christoph.koelbl@uni-konstanz.de)
% Copyright (c) 2012, Dominik Wöll (dominik.woell@uni-konstanz.de)
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in the
%       documentation and/or other materials provided with the distribution.
%     * The name of the author may not be used to endorse or promote products
%       derived from this software without specific prior written permission.
%
% THIS SOFTWARE IS PROVIDED BY THE AUTHOR ''AS IS'' AND ANY EXPRESS OR IMPLIED
% WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
% EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
% SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
% OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
% WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
% OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
% ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [track]=tracking(pos,R,T,minlen,costfcn,joinradius,useQF,weightfcn)

%% important information:
%
% Procedure requires the IBM ILOG CPLEX Optimizer as linear solver which can 
% be obtained free of charge from the IBM homepage.
%
%
%% input variables:
%
% pos: Matrix with spot positioning data in the following structure
%     [frame number, x-position, y-position, spot quality (optional)]
%     example:
%     1 234.1 367.4 0.80
%     1 345.2 122.5 0.91
%     2 237.3 365.2 0.75
%     2 340.2 121.9 0.97
%
% R: Maximum allowed step length for automatic tracking (in units of x- and
%    y-position)
%
% T: Maximum number of frames between position belonging to the same track
%
% minlen: Minimum length of a track (shorter tracks will be deleted directly)
%
% costfcn: cost function with spatial and temporal coordinates,
%          e.g. costfcn='x.^2+y.^2+t.^2'.
%
% joinradius: join radius; set to 0 if not used
%
% useQF: if weight function for spot quality is used 1, else 0
%
% weightfnc: function with quality variable 'q', e.g. 'q.^(1/2)'
%
%% example:
% [track]=tracking(pos,5,5,3,'x.^2+y.^2+t.^2',0,0,'');
%
%% Matlab code:

costs = inline(costfcn,'x','y','t');
pos = pos(pos(:,1)>0,:);
n=size(pos,1);

Quality_function = inline(weightfcn,'q');
if size(pos,2)<4 || useQF==0
	p=costs(R,R,T)*ones(n,1);
else
	p=costs(R,R,T)*Quality_function(pos(:,4));
end
[arcs,m] = generate_arcs( pos, R, T, joinradius );
disp('Arcs constructed');

E1=speye(n);
c = costs( pos(arcs(:,2),2)-pos(arcs(:,1),2),...
           pos(arcs(:,2),3)-pos(arcs(:,1),3),...
           pos(arcs(:,2),1)-pos(arcs(:,1),1) );
links = pos(arcs(:,2),1)==pos(arcs(:,1),1);
c(links) = c(links) + 0.5*(p(arcs(links,1)) + p(arcs(links,2)));
disp('Matrix constructed');

cplex = Cplex('Tracking');
cplex.Model.sense = 'minimize';
Z=spalloc(n,n,n);
disp('Initializing CPLEX');

cplex.addCols([c ; p ; p], [], [], [] );
cplex.addRows(ones(n,1),[E1(:,arcs(:,2)) E1 Z],ones(n,1));
cplex.addRows(ones(n,1),[E1(:,arcs(:,1)) Z E1],ones(n,1));
disp('Solving LP');

cplex.Param.lpmethod.Cur=4;
cplex.solve();
cplex.Param.lpmethod.Cur=2;
disp('Processing solution');

% POSTPROCESSING

successor=zeros(n,1);
first=zeros(n,2);
len=zeros(n,1);
roots=[];
usedarcs=arcs(cplex.Solution.x(1:m)==1,:);
for a = 1:length(usedarcs)
	successor(usedarcs(a,1))=usedarcs(a,2);
	if first(usedarcs(a,1),1)==0
		roots=[roots; usedarcs(a,1)];
		first(usedarcs(a,1),1) = usedarcs(a,1);
	end
	f = first(usedarcs(a,1),1);
	first(usedarcs(a,2),1) = f;
	len(f) = len(f) + 1;
end
disp('Storing tracks');

lr = len(roots(:)) > minlen;
longroots = sortrows([len(roots(lr)) roots(lr)],-1);
first(longroots(:,2),2)=1:size(longroots,1);
usedarcs(:,4) = first(first(usedarcs(:,1),1),2);
usedspots = cplex.Solution.x(m+1:m+n)+cplex.Solution.x(n+m+1:2*n+m) <= 1;
usedspots(usedspots) = first(first(usedspots,1),2)>0;
track = [ pos(usedspots(:),2) pos(usedspots(:),3) ...
          pos(usedspots(:),1) first(first(usedspots(:),1),2)];
track = track(track(:,4)>0,:);
track = sortrows(track,4);
disp('Tracking finished');


function [arcs,m]=generate_arcs( spots, R, T, joinradius)
arcs = zeros(10,3);
m=0;
n = size(spots,1);
minx = min( spots(:,2) );
miny = min( spots(:,3) );
maxx = max( spots(:,2) );
maxy = max( spots(:,3) );
width = maxx - minx;
height = maxy - miny;
fieldwidth = R;
xfields = floor(width/fieldwidth)+1;
yfields = floor(height/fieldwidth)+1;
mg=max(spots(:,1));
candidate = zeros( mg, xfields, yfields );
data = zeros( mg, xfields, yfields, 1 );
coord = zeros(n,4);

for i = 1:n
	t=spots(i,1);
	binx = floor((spots(i,2)-minx)/fieldwidth)+1;
	biny = floor((spots(i,3)-miny)/fieldwidth)+1;
	candidate(t,binx,biny)=candidate(t,binx,biny)+1;
	if candidate(t,binx,biny) > size(data,4)
		data = cat(4, data, zeros(mg, xfields, yfields, size(data,4)));
	end
	data(spots(i,1), binx, biny, candidate(t,binx,biny))=i;
	coord(i,1)=spots(i,1);
	coord(i,2)=binx;
	coord(i,3)=biny;
	coord(i,4)=candidate(t,binx,biny);
end
data=data(:,:,:,1:max(candidate(:)));

for i=1:n
	list = data( coord(i,1),max([1 coord(i,2)-1]):min([coord(i,2)+1 xfields]),...
	             max([1 coord(i,3)-1]):min([coord(i,3)+1 yfields]),:);
	list = list( list > 0 & list ~= i );
	list = list( (spots(i,2)-spots(list(:),2)).^2 +...
	             (spots(i,3)-spots(list(:),3)).^2 < joinradius^2 );
	l = length(list);
	if l > 0
		if m+l > size(arcs,1)
			arcs = [arcs; zeros(m+l,3)];
		end
		arcs(m+(1:l)',1) = list;
		arcs(m+(1:l)',2) = i*ones(l,1);
		arcs(m+(1:l)',3) = m+(1:l)';
		m = m + l;
	end
	list = data( coord(i,1)-1:-1:max([1 coord(i,1)-T]),...
	             max([1 coord(i,2)-1]):min([coord(i,2)+1 xfields]),...
	             max([1 coord(i,3)-1]):min([coord(i,3)+1 yfields]),:);
	list = list(list(:)>0);
	list = list( (spots(i,2)-spots(list(:),2)).^2 +...
	             (spots(i,3)-spots(list(:),3)).^2 < R^2 );
	l = length(list);
	if l > 0
		if m+l > size(arcs,1)
			arcs = [arcs; zeros(m+l,3)];
		end
		arcs(m+(1:l)',1) = list;
		arcs(m+(1:l)',2) = i*ones(l,1);
		arcs(m+(1:l)',3) = m+(1:l)';
		m = m + l;
	end
end
arcs = arcs(1:m,:);