#######################################################
#Simulation
L<-5000
h.vec<-rep(0,L)
nH<-2
e.mat<-matrix(0,nrow=nH,ncol=4)

a.mat<-matrix(0,nrow=nH,ncol=nH)
for(h in 1:nH){
	e.mat[h,]<-rdirich(1,rep(1,4),3)
	a.mat[h,]<-rdirich(1,rep(1,nH),nH-1)
}

a.eq<-a.mat
for(i in 1:50){
	a.eq<-a.eq %*% a.mat
}
a.eq<-a.eq[1,]

h.vec[1]<-sample(c(1:nH),size=1,prob=a.eq)
for(i in 2:L){
	h.vec[i]<-sample(c(1:nH),size=1,prob=a.mat[h.vec[i-1],])
}
x.mat<-e.mat[h.vec,]
x.vec<-apply(x.mat,1,function(x){sample(c(1:length(x)),size=1,prob=x)})

#######################################################
#Viterbi

index<-c(1:nH)
lv.mat<-matrix(0,nrow=nH,ncol=L)
point.mat<-t(lv.mat)
for(l in 1:nH){
	lv.mat[l,1]<-log(e.mat[l,x.vec[1]])+log(a.eq[l])
}
for(i in 2:L){
	for(l in 1:nH){
		tvec<-lv.mat[,(i-1)]+log(a.mat[,l])
		lv.mat[l,i]<-log(e.mat[l,x.vec[i]])+max(tvec)
		point.mat[i,l]<-sample(index[tvec == max(tvec)],size=1)
	}
}
P.max<-max(lv.mat[,L]+log(a.eq))
hstar.vec<-h.vec
tvec<-lv.mat[,L]+log(a.eq)
hstar.vec[L]<-sample(index[tvec == max(tvec)],size=1)
for(i in 2:L){
	hstar.vec[L-i+1]<-point.mat[i,hstar.vec[L-i+2]]
}
plot(hstar.vec[1:100])
table(hstar.vec)

#######################################################
#Forward
index<-c(1:nH)
lf.mat<-matrix(0,nrow=nH,ncol=L)
lfnew.mat<-matrix(0,nrow=nH,ncol=L)

for(l in 1:nH){
	lf.mat[l,1]<-log(e.mat[l,x.vec[1]])+log(a.eq[l])
}

lscale<-rep(0,L)
lscale[1]<-log(sum(e.mat[,x.vec[1]]*a.eq))
for(l in 1:nH){
	lfnew.mat[l,1]<-log(e.mat[l,x.vec[1]])+log(a.eq[l])-lscale[1]
}

for(i in 2:L){
	
	tvec<-sum(lfnew.mat[,(i-1)]*a.mat[,1])
	lscale[i]<-log(sum(e.mat[,x.vec[i]]*a.eq))
	
	for(l in 1:nH){
		tvec<-lf.mat[,(i-1)]+log(a.mat[,l])
		lf.mat[l,i]<-log(e.mat[l,x.vec[i]])+log(sum(exp(tvec)))

		lfnew.mat[l,i]<-log(e.mat[l,x.vec[i]])+log(sum(exp(tvec)))

	}
}
P.max<-max(lv.mat[,L]+log(a.eq))
hstar.vec<-h.vec
tvec<-lv.mat[,L]+log(a.eq)
hstar.vec[L]<-sample(index[tvec == max(tvec)],size=1)
for(i in 2:L){
	hstar.vec[L-i+1]<-point.mat[i,hstar.vec[L-i+2]]
}
plot(hstar.vec[1:100])
table(hstar.vec)