Fixed-lag Sparse Viterbi

function d = flstViterbiUpdate(obsLik, d, isFinal)

if nargin < 3

    isFinal = 0;

end

% d.psi(:,1:(d.memory-1)) = d.psi(:,2:d.memory);

d.psi = circshift(d.psi, -1, 2);

% d.scale(1:(d.memory-1)) = d.scale(2:d.memory);

d.scale = circshift(d.scale, -1, 2);

if d.updateCount == 0

     % initialise first frame

    d.oldDelta = d.init .* obsLik;

    deltaSum = sum(d.oldDelta);

    d.oldDelta = d.oldDelta / deltaSum;

    d.scale(d.memory) = 1.0/deltaSum;

else

    tempPsi = ones(d.nState, 1);

    % calculate best previous state for every current state

    % this would be the "sparse" loop in C++

%     for jState = 1:d.nState

%         temp = d.oldDelta(d.from) .* d.prob';

%         [d.delta(jState), tempPsi(jState)] = max(temp(d.to==jState));

%     end

    for iTrans = 1:d.nTrans

        transProb = d.prob(iTrans);

        fromState = d.from(iTrans);

        toState = d.to(iTrans);

        currentValue = d.oldDelta(fromState) * transProb;

        if (currentValue > d.delta(toState))

            d.delta(toState) = currentValue; % will be multiplied by the right obs later!

            tempPsi(toState) = fromState;

        end

    end

%     d.delta = d.delta .* obsLik;

    d.delta = d.delta .* obsLik + eps;

    deltaSum = sum(d.delta);

    if deltaSum > 0

        d.oldDelta = d.delta / deltaSum; % normalise (scale)

        d.scale(d.memory) = 1.0/deltaSum;

        d.delta = zeros(size(d.delta));

    else

        warning('Viterbi has been fed some zero probabilities (update %d),\nat least they become zero in combination with the model.', d.updateCount);

        d.oldDelta = ones(d.nState,1)/d.nState;

        d.scale(d.memory) = 1.0;

        d.delta = zeros(size(d.delta));

    end

    d.psi(:,d.memory) = tempPsi;

end

d.updateCount = d.updateCount + 1;

if isFinal

    temp = flstDecode(d);

    d.path = [d.path, temp];

elseif d.updateCount > (d.memory-1)

    temp = flstDecode(d);

    d.path = [d.path, temp(1)];

end