clear
stacksize(90000000)
d=3;
E=[];
for e=1:d
E=[E,e];
end

A=diag(ones(1,d));
T=10;  
P=(1/d)*ones(d,T);
for t=2:d*T
P(:,1:t)=[P(:,1:t-1),P(:,t-1)+(A(:,floor(1+d*rand(1,1,'def')))-P(:,t-1))*1/2] ;
end




M=[];
for e=1:d
M=[M; P(:,T*e)'];
end


dy=4;
Ay=diag(ones(1,dy));
Ty=10;
Py=(1/dy)*ones(dy,Ty);
for t=2:d*Ty
Py(:,1:t)=[Py(:,1:t-1),Py(:,t-1)+(Ay(:,floor(1+dy*rand(1,1,'def')))-Py(:,t-1))*1/2] ;
end

G=[];
for e=1:d
G=[G; Py(:,Ty*e)'];
end


/////////////////////////////////////////////////////
// QQ exemples "non aléatoires"
//
//
//////////////////////////////////////////////////////
// Ex1: 
//  M=[.9 .1; 
//        .3 .7];
//      G=[.7 .1 .01 .19; 
//         .2 .7 .05 .05 ];
//Ex2:
// M=[.4 .6; 
//         .2 .8];
//      G=[.7 .1 .01 .19; 
//         .2 .7 .05 .05 ];
//
// Ex3:    
//     M=[.99 .01; 
//        .02 .98];  
//     G=[.5 .1 .15 .25; 
//       .2 .7 .05 .05 ];
//
// Ex4:    
//     M=[.99 .01; 
//        .02 .98];  
//     G=[.4 .2 .3 .1; 
//        .2 .4 .2 .2 ];
//
//
//////////////////////////////////////////////////////

TM=2000;
X=find(grand(1,'mul',1,P(1:d-1,1))==1);
Y=find(grand(1,'mul',1,G(X,1:dy-1)')==1);
for t=1:TM
X=[X, find(grand(1,'mul',1,M(X(t),1:d-1)')==1)];
Y=[Y, find(grand(1,'mul',1,G(X(t+1),1:dy-1)')==1)];
end


et(1,:)=(P(:,1)'*diag(G(:,Y(1)))/((P(:,1)'*diag(G(:,Y(1))))*ones(d,1)));
for t=2:TM+1
et(t,:)=et(t-1,:)*M*diag(G(:,Y(t)))/((et(t-1,:)*M*diag(G(:,Y(t))))*ones(d,1));
end



xset("window",1);
xbasc();
xselect();
plot2d(1:TM+1,X,5,rect=[0,0,TM,max(d,dy)+1])
plot2d(1:TM+1,Y,3,rect=[0,0,TM,max(d,dy)+1])

xset("window",2);
xbasc();
xselect();
plot2d(1:TM+1,X,5,rect=[0,.9,TM,d+.1])







MA=zeros(d,d);
tab_X=tabul(X(1:TM),'i');
PA=zeros(d,1);
PA(tab_X(:,1))=tab_X(:,2);



TX=[X(1:TM);X(2:TM+1)];
MA=zeros(d,d);
for e1=1:d
if PA(e1)<>0 then
                   for e2=1:d
                   MA(e1,e2)=length(vectorfind(TX,[e1;e2],'c'))/PA(e1);
                   end
end
end


ZA=[X;Y];
GA=zeros(d,dy);
for e=1:d
if PA(e)<>0 then
                   for ee=1:dy
                   GA(e,ee)=length(vectorfind(ZA,[e;ee],'c'))/PA(e);
                   end
end
end





M
G
MA



////////////////////////////////////////////////////////////////////////////////////
//  
//      ESTIMATION DES PROBABILITES DE TRANSITION "M" D'UN SIGNAL PARTIELLEMENT OBSERVE
//      PAR DES FILTRES PARTICULAIRES ET DES ARBRES GENEALOGIQUES
//      
//      Modèle :
//
//      P(X(n)=j / X(n-1)=i)=M(i,j) signal, M inconnu
//      P(Y(n)=k / X(n)=i)=G(i,k) observations ==> Y_0,...,Y_n
//
//      Modéle bayesien :
//
//      M_n chaîne de Markov dans l'espace des transitions markoviennes
//      M_n "=" M_(n-1) "+/- Delta "
//      P(X(n)=j / X(n-1)=i)=M_n(i,j) signal
//      P(Y(n)=k / X(n)=i)=G(i,k) observations
//
//      Une particule d'indice i, au temps n, représente une valeur possible 
//      M^i_n de M au temps n.La mesure d'occupation de l'arbre généalogique
//      est une estimation de la loi conditionnelle
// 
//      Loi(M_0,....,M_n/Y_0,....,Y_n)
//
//
//  Au moins 50 particules par inconnue
//Pour TS=500, N=100, TA=10  (Ex5) compter 2h de calculs.
//Pour des chaînes raisonnablement observées :
//=> TS=50, N=100, TA=20 ou 40 (min 10mn de calculs)
//
/////////////////////////////////////////////////////////////////////////////////

TA=40;       //Nb d'itérations (chaque itération=TS unités de temps)

N=200;       //Nb de particules, précision de l'algorithme

TS=50;  //100;   //50; 100; 200; 500;      //Interprétations de TS :
            //Fenêtre d'observation des transitions cachées du signal.
            //Pression de sélection "proportionnelle" à TS
            //Sur des plages/fenêtres d'horizon TS, les paramètres prédits restent inchangés
            //(Augmenter TS, et a fortiori TM, lorsque les observations sont peu "informatives".)

            

T=1;  
PI=(1/d)*ones(d,T);
for t=2:d*T*N
PI(:,1:t)=[PI(:,1:t-1),PI(:,t-1)+(A(:,floor(1+d*rand(1,1,'def')))-PI(:,t-1))*1/2] ;
end



for i=1:N
MM=[];
for e=1:d
MM=[MM; PI(:,i*T*e)'];
end
MI(:,:,1,i)=MM;
eta(1,:,i)=P(:,1)';
end









      

A=diag(ones(1,d));
U=.5*ones(d,1);
TU=10;

for s=1:TA

epsi=bool2s(rand(N,1,'uniform')<1/(2*log(s+1)));
//a=.9;
//epsi=bool2s(rand(N,1,'uniform')<1/(s+1)^a);
//epsilon=rand(N,1,'uniform')/(2*log(s+1));  

W=ones(1,N);
L=zeros(1,N);




for i=1:N

 if epsi(i)==1 then MI(:,:,s+1,i)=MI(:,:,s,i);                                        
                    J=floor(1+d*rand(1,1,'def'));
                    U(:,1)=MI(J,:,s+1,i)';
                               for r=2:TU+1
                               U(:,r)=.5*U(:,r-1)+ .5*A(:, floor(1+d*rand(1,1,'def')));
                               end
                    //MI(J,:,s+1,i)=(1-epsilon(i))*MI(J,:,s+1,i)+epsilon(i)*U(:,TU+1)';                               
                    MI(J,:,s+1,i)=U(:,TU+1)';
             else MI(:,:,s+1,i)=MI(:,:,s,i);
  end
 
 
 for t=1:TS
 eta((s-1)*TS+t+1,:,i)=(eta((s-1)*TS+t,:,i)*diag(G(:,Y((s-1)*TS+t)))*MI(:,:,s+1,i))/(eta((s-1)*TS+t,:,i)*diag(G(:,Y((s-1)*TS+t)))*ones(d,1));
 L(i)=L(i)+log( eta((s-1)*TS+t,:,i)*G(:,Y((s-1)*TS+t)) );
 end
 

end


W=exp((L-max(L)));
Wnorm=W/sum(W);
F=cumsum(Wnorm);



E=rand(1,N,'uniform');
Ac=find(E<W);
NAc=length(Ac);
Rej=find(E>=W);
NRej=length(Rej);



Expo=-log(grand(1,NRej+1,'def'));
Texpo=cumsum(Expo);  
Vexp=Texpo/Texpo(NRej+1);


        
        eta_multi=ones(s*TS+1,d,NRej);
        Mmulti=zeros(d,d,s+1,NRej);
        
        
        for i=1:NRej
        k=find(F>=Vexp(i));
        Mmulti(:,:,1:s+1,i)=MI(:,:,1:s+1,k(1));
        eta_multi(1:s*TS+1,:,i)=eta(1:s*TS+1,:,k(1));
        
        end


         
      if NRej>0 then
      MI(:,:,1:s+1,Rej)=Mmulti(:,:,1:s+1,1:NRej);
      eta(1:s*TS+1,:,Rej)=eta_multi(1:s*TS+1,:,1:NRej);
      
      end
 


xset("window",3);
     xbasc();
     plot2d(1:s+1,M(1,1)*ones(1,s+1),5,rect=[0,0,s+1,1])   
      
     for i=1:N
     plot2d(1:s+1,MI(1,1,s+1,i)*ones(1,s+1),i,rect=[0,0,s+1,1])   
     end
     xtitle(['Correction';''],'','')


xset("window",4);
     xbasc();
     plot2d(1:s+1,M(2,2)*ones(1,s+1),5,rect=[0,0,s+1,1])   
     for i=1:N
     plot2d(1:s+1,MI(2,2,s+1,i)*ones(1,s+1),i,rect=[0,0,s+1,1])   
     end
     xtitle(['Correction';''],'','')

xset("window",5);
     xbasc();
    plot2d(1:s+1,M(d,d)*ones(1,s+1),5,rect=[0,0,s+1,1])   
     for i=1:N
     plot2d(1:s+1,MI(d,d,s+1,i)*ones(1,s+1),i,rect=[0,0,s+1,1])   
     end
     xtitle(['Correction';''],'','')


end



M11=[];
M22=[];
Mdd=[];
for i=1:N
M11=[M11;MI(1,1,s+1,i)];
M22=[M22;MI(2,2,s+1,i)];
Mdd=[Mdd;MI(d,d,s+1,i)];
end


xset("window",6);
     xbasc();
     histplot(10,M11,2)
     histplot(10,M22,4) 
     histplot(10,Mdd,5) 
   

LF=zeros(1,N);


 
for i=1:N
 
 for t=1:TM
 eta(t+1,:,i)=(eta(t,:,i)*diag(G(:,Y(t)))*MI(:,:,TA+1,i))/(eta(t,:,i)*diag(G(:,Y(t)))*ones(d,1));
 LF(i)=LF(i)+log( eta(t,:,i)*G(:,Y(t)) );
 end

end

SLF=sort(LF);
SL=find(LF==SLF(1));
ML=MI(:,:,TA+1,SL(1))



LF_ref=0;
eta_ref(1,:)=P(:,1)';
for t=1:TM
 eta_ref(t+1,:)=(eta_ref(t,:)*diag(G(:,Y(t)))*M)/(eta_ref(t,:)*diag(G(:,Y(t)))*ones(d,1));
 LF_ref=LF_ref+log( eta_ref(t,:)*G(:,Y(t)) );
end
Log_Vmaxi=LF(1)/TM
Log_Vopt=LF_ref/TM
Log_Vmini=LF(N)/TM