# A simple implementation of the Viterbi algorithm, 
# for comparison with Forward Backward

# The starting probabilities for x_1: 
startprobs <- c(1, 0, 0, 0, 0, 0, 0, 0, 0, 0)
M <- length(startprobs)

# A hidden Markov model, with each x_i having possible values 1,...,M, 
# and with transition probabilities 
# 1/3   increase by 1 (if possible)
# 1/3   decrease by 1 (if possible)
# remaining probability: stay at same value. 
# Matrix of transition probailities: 
transprobs <- diag(M)/3 + 
  rbind(cbind(0,diag(M-1)/3),0) + 
  rbind(0,cbind(diag(M-1)/3,0))
transprobs[1,1] <- transprobs[M,M] <- 2/3

# For the data y_i we assume
# y_i | x_i ~ Poisson(x_i)
y <- c(3, 5, 8, 8, 8 ,8, 4, 3, 4, 2, 1, 1, 1, 0, 1, 
       3, 2, 1, 3, 8, 12, 14, 11, 10)
N <- length(y)

maxprob <- matrix(startprobs, N, M, byrow=TRUE)
prev <- matrix(0, N, M)

for (i in 2:N) {
  for (j in 1:M) {
    rr <- maxprob[i-1,]*transprobs[,j]*dpois(y[i],1:M)
    prev[i,j] <- which.max(rr)
    maxprob[i,j] <- rr[prev[i,j]]
  }
}
result <- rep(which.max(maxprob[N,]), N)
for (i in (N-1):1) 
  result[i] <- prev[i+1,result[i+1]]

plot(y)
lines(result, col="red")