function indexOut = align4(t,x,templateTimes,templateWeights, alpha, beta, gamma, delta)

% Align4 - Jeff Lieberman 2oo5
%
% this file takes an input set of times with weights
% and aligns them intelligently to a template set
% of weights, using a dynamic programming/belief propagation network

% Revision History: 
% align = basic dynamic programming scheme, with trivial cost function
% align2 = updated cost function to include original t moved distances [to
%           minimize] and using real t instead of just template times 
% align3 = updated same as align2, but as a called function, that returns
%           the best choice indeces in order for given input
%           points/times/templatepoints/templatetimes/parameters
% align4 = now reads two levels of history so that it can minimize
%           acceleration and deceleration of the input data
% align5 = this revises the belief propagation routine so that it should
%           compute roughly an order of magnitude faster, by
%           careful/intelligent selection of which options to check, given
%           monotonicity of time in the samples [ie i < j < k]
% align6 = this adds the final cost function, of minimization of time
%           acc/decelerations

% cost is of the form cost(current point, current choice, previous choice)
% alpha:    deprecated, for pushing things away from each other
% beta:     denotes cost of moving a point to a template point [time differece]
% gamma:    denotes cost of different weights in template set
% delta:    denotes cost of changing accelerations [the new part]

n = length(t); % how many samples we need to fit
m = length(templateTimes); % how many template points there are

sprintf('%2g steps\n',n)

% initialize with NaN for things, to check for later when backtracking:
totalCostSoFarGivenBestChoice(1:n,1:m,1:m) = NaN;

% now let's do the rest.
for step = 1:n,                 % which step we are trying to fit
    step
    for i = step:m,        % current choice for this step in template, can't be earlier than it's number
        if(step == 1),
            cost = beta*(t(1) - templateTimes(i)).^2 + ...
                        gamma*(x(1) - templateWeights(i)).^2;
            min_k = 1; % trivial at this point, just to line up with other code below
            totalCostSoFarGivenBestChoice(step,i,1) = cost;
%            prevBestChoice(step,i,1) = min_k;
        else
            % for j and k, do not allow overlaps with i or each other:        
            for j = step-1:i-1,        % current choice for previous step in template
                if(step==2),
                    min_cost = beta*(t(2) - templateTimes(i)).^2 + ...
                        gamma*(x(2) - templateWeights(i)).^2 + ...
                        totalCostSoFarGivenBestChoice(step-1, j, 1);
                    min_k = 1; % also negligable
                else
                    % don't need to initialize cost, because we always write it to
                    % a bigger value, i think
                    for k = step-2:j-1,        % current choice for second previous step in template            
                        % generate costs: [do not need to save] for each choice of i,j. then
                        % choose best k given those
                        oldAcc = (templateTimes(j)-templateTimes(k))/(t(step-1)-t(step-2));
                        newAcc = (templateTimes(i)-templateTimes(j))/(t(step)-t(step-1));
                        cost(k) = beta*(t(step) - templateTimes(i)).^2 + ...
                                  gamma*(x(step) - templateWeights(i)).^2 + ...
                                  delta*(oldAcc - newAcc).^2 + ... % add in the acceleration term, once this compiles properly.
                                  totalCostSoFarGivenBestChoice(step-1, j, k); % total from previous step
                    end
                    % now we must choose the best k: only check valid k!
                    [min_cost min_k] = min(cost(1:j-1)); % returns both value and index minima
                end

                % total cost is minimum - this is total cost so far, for
                % this step, if i and j are the best choice:
                totalCostSoFarGivenBestChoice(step,i,j) = min_cost;
                
                % now the k that yielded the minimum total cost is the best previous choice:
                prevBestChoice(step,i,j) = min_k;
            end    
        end    
    end
end

% some of these things are not numbers now.. so check only ones that are:
for i=1:m,
    for j=1:m,
        if(isnan(totalCostSoFarGivenBestChoice(n,i,j)))
            % this only happens if it's a number
            totalCostSoFarGivenBestChoice(n,i,j) = 10000000; % make very high.
        end
    end
end

% now we can proceed with the normal thing:

% try to find best two final choices, then track back:
[a b] = min(min(squeeze(totalCostSoFarGivenBestChoice(n,:,:))));
[c d] = min(squeeze(totalCostSoFarGivenBestChoice(n,:,b)));

choice(n) = d;
choice(n-1) = b; % these come from the above minimal indeces

if (totalCostSoFarGivenBestChoice(n,d,b) ~= a)
    error('ba');
end

for step = (n-2):-1:1,
    % travel backwards, finding each previous best choice:
    choice(step) = prevBestChoice(step+2, choice(step+2), choice(step+1)); % pattern is prevBestChoice(step,j,i)
end    
choice; % this is the indeces

chosenTimes = templateTimes(choice);

sprintf('Maximum error: %5g\n',c)

% show old data with fixed points:
figure;
subplot(3,1,1)
bar(t,x,0.35,'k');
title('Original Segments');
hold on;
bar(templateTimes, 0.2*templateWeights,0.15,'r');
c = axis;
subplot(3,1,2);
title('Standard Quantization Routine');
hold on;
quantizedTimes = round(t*4)/4;
bar(quantizedTimes, x, 0.35,'b');
bar(templateTimes, 0.2*templateWeights,0.15,'r');
axis(c);
subplot(3,1,3);
hold on;
title('Improved Quantization Routine');
bar(chosenTimes,0.9*x,0.35,'b');
axis(c);
hold on;
bar(templateTimes, 0.2*templateWeights,0.15,'r');

indexOut = choice;