function [best,bestresult,bestfitresult] = decompose(times,vel,numsubmovements,xrng,yrng)
% DECOMPOSE - decompose two dimensional movement into submovements using the velocity profiles
%
% [best,bestresult,bestfitresult] = decompose(time,vel,numsubmovements,xrng,yrng)
%
% vel should be a N x 2 matrix, with the x and y velocities
%
% t should be a 1 x N matrix with the corresponding times (in seconds)
%
% numsubmovements is the number of submovements to look for, if it is
% empty or not specified, the function will try 1 to 4 submovements
%
% xrng is the valid range for the amplitude of x values (default = [-5 5])
%
% yrng is the valid range for the amplitude of y values (default = [0.1 5])
%
% best the best (lowest) value of the error function
% bestresult contains the function parameters corresponding to the best values
% (each four number is the 
% bestfitresult is the velocity profiles coresponding to the best values

% Jason Friedman, 2012
% www.curiousjason.com
% noisyaccumulation.blogspot.com

if nargin<3
    numsubmovements = [];
end

if nargin<4 || isempty(xrng)
    xrng = [-5 5];
end

if nargin<5 || isempty(yrng)
    yrng = [0.1 5];
end

if size(times,1)>1
    error('time must be a 1*N vector');
end

if isempty(numsubmovements) || length(numsubmovements)>1
    % If not specified, try 1 to 4 submovements
    if isempty(numsubmovements)
        numsubmovements = 1:4;
    end
    best = NaN * ones(1,max(numsubmovements)); bestresult = cell(1,max(numsubmovements)); bestfitresult = cell(1,max(numsubmovements));
    
    for k=numsubmovements
        [best(k),bestresult{k},bestfitresult{k}] = decompose(times,vel,k,xrng,yrng);
    end
    return;
end

best = inf;

% parameters are T0, D, Ax Ay
% ranges are
% 0 <= T0 <= finaltime - 0.167
% 0.167 <= D <= finaltime
% xrng(1) <= Ax <= xrng(2)
% yrng(1) <= Ay <= yrng(2)
lb_0 = [0                0.167     xrng(1) yrng(1)];
ub_0 = [times(end)-0.167 1.0       xrng(2) yrng(2)];
pps = 4; % parameters per submovement

if any(lb_0>ub_0)
    error('Lower bounds exceed upper bound - infeasible');
end

% As in Roher & Hogan 2006, select 10 random parameter selections
count=1;
while count<=10
    for i=1:numsubmovements
        % Randomly select 10 starting positions in the legal range
        initialparameters(1,i*pps-(pps-1):i*pps) = lb_0 + (ub_0-lb_0) .* rand(1,pps);
        lb(i*pps-(pps-1):i*pps) = lb_0;
        ub(i*pps-(pps-1):i*pps) = ub_0;
    end
    v = vel(:,1:2);
    tv = sqrt(vel(:,1).^2 + vel(:,2).^2);
    % Turn on GradObj and Hessian to use the gradient and Hessian
    % calculated in the functions
    options = optimset('GradObj','on','Hessian','on', 'LargeScale','on',...
        'MaxFunEvals',10^13,'MaxIter',5000,...
        'FunValCheck','on','DerivativeCheck','off',...
        'Algorithm','trust-region-reflective');
    % DerivativeCheck can be set to on when you want to make sure that your
    % analytically calculated derivative agree with the derivatives
    % calculated using finite differences
    
    % Run this part in a try loop because occasionally it crashes with some Matlab versions
    try
        result = fmincon(@(parameters) calculateerrorMJxy(parameters,times,v,tv),initialparameters,[],[],[],[],lb,ub,[],options);
        [epsilon,~,~,fitresult] = calculateerrorMJxy(result,times,v,tv);
        
        
        if ~isreal(result(1))
            error('Found an imaginary value');
        end
        
        if epsilon < best
            best = epsilon;
            bestresult = result;
            bestfitresult = fitresult;
        end
        
    catch exception
        % occasionally there are errors for reasons that are not clear, let
        % us just ignore them [seem to have gone away now]
        fprintf(['Got a strange error: ' exception.message ' in file ' exception.stack(1).name ' on line ' num2str(exception.stack(1).line) ', ignoring\n']);
        count = count-1;
    end
    count = count+1;
end
