function HMM_demo3(N_state,tau,scale_mu,scale_cov,trans_info)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%           HMM      
%%
%% function HMM_demo3(N_state,tau,scale_mu,scale_cov,trans_info)
%%
%% N_state = number of state
%% tau = lentgh of the sequence
%% scale_mu = scale of the mean [5]
%% scale_cov = scale of the cov [1]
%% trans_info = amount of info in transition matrix [1]
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% X = observation vector
%% Y = state vector

colordef black;
%clear;
%close all;

figure(1), clf;
figure(2), clf;

figure(2),
title ('green=state 1; red=state 2; magenta=state 3; yell=state 4; green=state 5; ...')

%%%%%%%% initialization
color = ['g' 'r' 'm' 'y' 'c' 'b' 'w' ];
%N_state = 4; %% number of states
%tau = 30;    %% length of the state vector
dim_Y = 2;   %% dimension of the state  
dim_X = 2;   %% dimension of observation

X = zeros(dim_X,tau); %%observation
Y =  zeros(1,tau);   %% state
Y_est =  zeros(1,tau);   %% state estimate
Y_mu  = zeros(dim_Y,N_state);
Y_cov = zeros(dim_Y,dim_Y,N_state);
Y_axis1 = 1:1:tau;

for i=1:N_state,
    Y_mu(:,i)=randMean(dim_Y,scale_mu);
    Y_cov(:,:,i)=randCovariance(dim_Y,scale_cov);
end;

gamma_i  = zeros(N_state,tau);
omega_ij = zeros(N_state,N_state,tau);
A_ij	 = zeros(N_state,N_state,dim_Y);
B_i	 = zeros(N_state,tau);
alpha_i  = zeros(N_state,tau);
K        = zeros(tau);
beta_i   = zeros(N_state,tau);
mu_i     = zeros(N_state,tau);
delta_i	 = zeros(N_state,tau);

%%%%%% generate transition probability matrix

A1_ij	 = zeros(N_state,N_state);
A1_ij(:,:) = abs(randn(N_state,N_state)).^trans_info;
A2=sum(A1_ij,1);
A3=ones(N_state,1)*A2(1,:);
A_ij=A1_ij./A3;
    
%%%%%%%% generate P(Yo=i) vector

P2_Yo	    = rand(N_state,1);
P3_Yo =sum(P2_Yo,1)*ones(N_state,1);
P_Yo = P2_Yo./P3_Yo;


%%%%%%%%% compute the initial state

Yo = new_state(P_Yo,1); %% label of the state

Y(1)=Yo;
    

%%%%%% generate the sequence of states

for i=2:tau,
    Y(i) = new_state(A_ij,Y(i-1));
end;

%%%%%%% plot the sequence of state

figure(1);

axis([0 tau+1 0 N_state+1]);
grid on;
title('yellow square = actual state; white cross = estimated state');
drawnow;

%%%%% generate the sequence of observations

for i=1:tau,
    
    X(:,i)= randvec(1,Y_mu(:,Y(i)),Y_cov(:,:,Y(i)));
    figure(2);
    set(gcf,'doublebuffer','on');
    hold on;
    plot(X(1,i),X(2,i),[color(Y(i)) 'x'],'linewidth',2);
    figure(1);
    set(gcf,'doublebuffer','on');
    hold on;
    plot(Y_axis1(i),Y(i),[color(Y(i)) 's'],'linewidth',2);
    
    if (i<10) ; end;
    drawnow;
end;

input('press return to continue');

%%%%%%%%% init paramters  for the Viterbi algorithm

for i=1:N_state,
    for t=1:tau,
	B_i(i,t)  = gauss2(Y_mu(:,i),Y_cov(:,:,i),X(:,t));
    end;
end;

mu_i(:,1) = P_Yo;

delta_i(:,1) = B_i(:,1).*mu_i(:,1);
delta_i(:,1)=delta_i(:,1)/norm(delta_i(:,1));

%%%%%%%%%%  Viterbi Algorithm
t1=zeros(N_state,1);
psi=zeros(N_state,tau);


for t=1:tau-1,
    for i=1:N_state,
	t1=A_ij(i,:).*delta_i(:,t)';
        [max_t1,index] = max(t1);
	delta_i(i,t+1) = B_i(i,t+1)*max_t1;
	psi(i,t+1)=index;
    end;

    delta_i(:,t+1)=delta_i(:,t+1)/norm(delta_i(:,t+1));
end;

%%%%%%%%%%  back tracking

list=((-tau+1):-1);
list=list*(-1);

[max_t1 index] = max(delta_i(:,tau));
Y_est(tau) = index;

for t=list,
    delta_i(:,t);
    Y_est(t) = psi(Y_est(t+1),t+1);
end;

%%%%%%% plot the sequence of estimated state

figure(1);
hold on;
plot(Y_axis1,Y_est,'wx','linewidth',2);
axis([0 tau+1 0 N_state+1]);
grid on;

%%%%%%%%% compute error

error = not((Y_est==Y));
index2 = find(error);

total_error_Viterbi = length(index2)

figure(2);
hold on;
plot(X(1,index2),X(2,index2),'wo');


 input('press return to continue');
%%%%%%% clusterize with MoG


mu_MoG= Y_mu;
cov_MoG= Y_cov;
[SS,XX] = eig(A_ij);
pi_MoG = abs(SS(:,1))
pause

for i=1:N_state
    plotGauss(mu_MoG(1,i),mu_MoG(2,i),cov_MoG(1,1,i),cov_MoG(2,2,i),cov_MoG(2,1,i));
end;


Rit= zeros(N_state,tau);
den=zeros(1,N_state);
for t=1:tau,
    for i=1:N_state,
	den(i)=gauss2(mu_MoG(:,i),cov_MoG(:,:,i),X(:,t))*pi_MoG(i);
    end;
	den2=sum(den);
    for i=1:N_state,
	Rit(i,t)=gauss2(mu_MoG(:,i),cov_MoG(:,:,i),X(:,t))*pi_MoG(i)/den2;
    end;
end;

[max_R Y_resp] = max(Rit,[],1);

y_i3=zeros(1,N_state);
Y_resp2=zeros(1,tau);

for i=1:N_state,
    for j=1:N_state,
	[index1] = find(Y==i);
	[y_i1] = (Y==i);
        [index2] = find(Y_resp==j);
	[y_i2] = (Y_resp==j);
	y_i3(j) = sum(y_i1.* y_i2);
   end;
   [maxi index3] = max(y_i3);
   j=index3;
   [index2] = find(Y_resp==j);
   Y_resp2(index2)=i;
end;

%%%%%%%%% compute error

error = not((Y_resp2==Y));
index2 = find(error);
total_error_MoG=length(index2)

figure(2);
hold on;
plot(X(1,index2),X(2,index2),'wd');

%figure(1), hold off;
%figure(2), hold off;

%%%%%%% plot the sequence of estimated state with MoG
figure(1);
hold on;
plot(Y_axis1,Y_resp2,'yo','linewidth',4,'markersize',4);
axis([0 tau+1 0 N_state+1]);
grid on;