function [mst,tg] = frqtrackmc3(x, gm, tao, prs, t, nff, nodes, nfig, mthd, wshift)
% mthd:0 -> Quadrature rule
%      1 -> Importance sampling 
%      2 -> Importance sampling with Qr proposals
%      3-> Grid method

global f0 pi2 nf np na ns e bsz ubnd gmblk A C ; 
if nargin < 6, nff = 1; end
if nargin < 7, nodes = 200; end % numb. of nodes or samples
if nargin < 8, nfig = 0; end
if nargin < 9, mthd = 1; end 
if nargin < 10, wshift = floor(bsz/2); end 

td = t(2)-t(1);
nf = nff;
pi2 = 2*pi;
ind_upd = 0;

[n,d] = size(x);
mst0 = prs.m0;
vst0 = prs.v0;
Lt0 = inv(vst0);
gmblk = diag(repmat(diag(gm), bsz,1));

% ng = floor(n/bsz);
ng = 1 + floor( (n-bsz)/wshift );
mst = zeros(ns, ng);
vst = zeros(ns,ns,ng);
vst(:,:,1) = 1e6*eye(ns);
inda = [];  indp = []; indf = [];
inda = vec(1:na); 
indp = vec((na+1):(na+nf)); 
indf = vec(na+nf+1:na+2*nf);
indap = [inda;indp];
indpf = [indp;indf];

Atp = [1 0 0;0 1 2*pi*(t(2)-t(1)); 0 0 1];
A = zeros(nf*3);
Ctp = [1 0 0; 0 1 0];
% Ctp = eye(3);
C = zeros(nf*2, nf*3);
A = kron(Atp,eye(nf));
C = kron(Ctp,eye(nf));

e = eye(na); ee = eye(ns);
pree = 1e-7*ee;

m = 2;
muo0=zeros(nf*m,1); ho0=1e-6*eye(nf*m);
muot=zeros(nf*m,ng);
hot = 1e-2*ones(nf*m,nf*m,ng);
hot(:,:,1)=ho0;
f0 = zeros(nf*(m-1),1);

% 1:sample from t distri; 0: sample from normal
if mthd == 0
    [ndps, w] = quadrl(nodes); % ndps is row vec.
else
    ndps =[];
    w = ones(nodes,1);
end
mthd_ini = 1;
nodes_ini = 900;
w_ini = ones(nodes_ini,1); % for the initial runs

tg = 0;
g = 0;
tg(1) = t(1);
for i = 1:wshift:(ng-1)*wshift +1
    g = g+1;
    tg(g) = t(i+floor(bsz/2));
  
    if mod(i,200)== 0, fprintf('\n process %d data point', i); end 
     % step A: compute forward message
     if g > 1 
         mst(:,g) =  A*mst(:,g-1);
         vst(:,:,g) = A*vst(:,:,g-1)*A' + tao;
     else
         mst(:,g) =  mst0;
         vst(:,:,g) = vst0;  
     end

    mstold = mst(:,g);
    vstold = vst(:,:,g);
    % Step B.C.D: compute the moments based on new observation
    % Only good for bsz =1's case    
    if g < 15
        [mstn, vstn] = upd_marg_norm_moments(mstold, vstold, mst(:,g),vst(:,:,g),...
             inda, indp, x(g), td, gm, nodes_ini, [], w_ini, mthd_ini);
    else
        [mstn, vstn] = upd_marg_norm_moments(mstold, vstold, mst(:,g),vst(:,:,g),...
             inda, indp, x(g), td, gm, nodes, ndps, w, mthd);
    end
    
    % step E and F: compute the observation message and new belief
    [muot(:,g),hot(:,:,g),mapt, vapt]=upd_mssg_belief( mstold(indap), ...
        vstold(indap,indap), mstn, vstn, muo0, ho0,1);      

    F = vst(indf,indap,g)*inv(vst(indap,indap,g));
    Fmd = F*(mapt - mst(indap,g));
    mst(indf,g) = mst(indf,g) + Fmd;
    vst(indf,indf,g) = F*(vapt*F' - vst(indf,indap,g)')...
            + vst(indf,indf,g);
    vst(indf,indap,g) = F*vapt;
    vst(indap,indf,g) = vst(indf,indap,g)';
    mst(indap,g) = mapt;
    vst(indap,indap,g) = vapt;
    
    Lt(:,:,g) = inv(A*vst(:,:,g)*A' + tao);

   if min(eig(vst(:,:,g)) ) <= 0
           wstr = ['vstnew:not positive definite at forward iter.', num2str(k), 'and ', num2str(g) 'th dt point'];
           return; %  error(wstr);
   end   
end

g_old = g;
nhd =1;
if nfig == 1
    plotmst(mst,tg, 1,nhd,':')
end

niter = 3;

for k = 2:niter
    
    mdfall = 0;
    % step G and H: kalman smoothing and obs. message updating
    g = g_old - 1;
    for i = ((ng-2)*wshift+1):-wshift:1  
       if mod(i,200)== 0, fprintf('\n k:%d  i:%d', k,i); end 
       
       mstki =  mst(:,g);
       % compute the difference 
       mdfall = max( abs(mst(:,g) - mstki)) + mdfall; 
     
      % G step: Kalman smoothing
       vsAt = vst(:,:,g)*A';
       Jt = vsAt * Lt(:,:,g);    
       mst(:,g) = mst(:,g) + Jt*(mst(:,g+1) - A*mst(:,g));
       % vst(:,:,g) = vst(:,:,g)*(ee+ (Lt(:,:,g)*vst(:,:,i+1) - ee)*Jt');
       vst(:,:,g) = vst(:,:,g) + Jt*( vst(:,:,g+1)*Jt' ...
           - vsAt');

      %     g = g - 1;
      %  end
      %  g = g_old-1;
      %  for i = ((ng-2)*wshift+1):-wshift:1        
        
        %  H step 
        % first remove the term from q to avoid double counts
        ivst = inv(vst(:,:, g));
        tpvst = ivst;
        tpvst(indap,indap) = tpvst(indap,indap)-hot(:,:,g);
        vapt = inv(tpvst);
        mapt = vapt * ( ivst*mst(:,g) - C'*muot(:,g) );
        
%       if isposdef( vstold ) == 0,  
        if  min(eig(vapt)) <= 0
           wstr = ['vstold: not positive definite at backward iter.', num2str(k), 'and ', num2str(i) 'th dt point'];
           warning(wstr);
           g = g-1;
           continue;
        end 
        mstold = mapt;
        vstold = vapt;
    
       % ADF update: compute the moments based on observation message
       [mstn, vstn] = upd_marg_norm_moments(mstold, vstold, mst(:,g),vst(:,:,g),...
       inda, indp, x(g), td, gm, nodes, ndps, w, mthd);
    
       % Update the obs message and the new belief       
        muot_old = muot(:,g);
        hot_old = hot(:,:,g);
        [muot(:,g), hot(:,:,g), mapt,vapt]= ...
        upd_mssg_belief( mstold(indap), vstold(indap,indap), ...
        mstn, vstn,muot_old,hot_old);  
        
        F = vstold(indf,indap)*inv(vstold(indap,indap));
        Fmd = F*(mapt - mstold(indap));
        mst(indf,g) = mstold(indf) + Fmd;
        vst(indf,indf,g) = F*(vapt*F' - vstold(indf,indap)')...
            + vstold(indf,indf);
        vst(indf,indap,g) = F*vapt;
        vst(indap,indf,g) = vst(indf,indap,g)';
        mst(indap,g) = mapt;
        vst(indap,indap,g) = vapt;        
        g = g-1;
   end % end of F and G steps: smoothing and message updating
   
   if nfig == 1    
       plotmst(mst,tg,1,nhd);
       % pause;
   end     
   
%   if mdfall/n < 1e-3, 
%       fprintf('\nep converge at iteration %d', k); 
%       break; 
%  end
   if k == niter, break; end
   
    % Step I: Kalman filtering
%   vst(:,:,1) = inv( Lt0 + C'*hot(:,:,1)*C);
%   mst(:,1) =  vst(:,:,1)*( C'*muot(:,1) + Lt0*A*mst0);           
    for g = 2:g_old  
       Lt(:,:,g-1) = inv(A*vst(:,:,g-1)*A' + tao);
       mstold = mst(:,g);
       vstold = vst(:,:,g);
       vst(:,:,g) = inv( Lt(:,:,g-1) + C'*hot(:,:,g)*C);
       %       K = ee - vst(:,:,g)*Lt(:,:,i-1);
       mst(:,g) = vst(:,:,g)*(  C'*muot(:,g) + Lt(:,:,g-1)*A*mst(:,g-1) );
       if min(eig(vst(:,:,g)) ) <= 0
           mst(:,g)= mstold;
           vst(:,:,g) = vstold;                   
           wstr = ['vstnew:not pos-def at forward iter.', num2str(k), 'and ', num2str(g) 'th dt point'];
           warning(wstr);
       end
    end       
end    
