clear;set(0,'defaultaxesfontsize',20);format long
%%% p5.m MCMC pCN algorithm for sin map (Ex. 1.3) with noise

%% setup
J=10;% number of steps
alpha=2.5;% dynamics determined by alpha
gamma=1;% observational noise variance is gamma^2
sigma=1;% dynamics noise variance is sigma^2
C0=1;% prior initial condition variance
m0=0;% prior initial condition mean
sd=0;rng(sd);% Choose random number seed
 
%% truth

vt(1)=m0+sqrt(C0)*randn;% truth initial condition 
G=0;Phi=0;

for j=1:J
    vt(j+1)=alpha*sin(vt(j))+sigma*randn;% create truth
    y(j)=vt(j+1)+gamma*randn;% create data
    % calculate log density from (1.--)
    G=G+1/2/sigma^2*((alpha*sin(vt(j)))^2-2*vt(j+1)*alpha*sin(vt(j)));
    % calculate log likelihood phi(u;y) from (1.11)
    Phi=Phi+1/2/gamma^2*(y(j)-vt(j+1))^2;
end

%% solution
% Markov Chain Monte Carlo: N forward steps
N=1e5;% number of samples
beta=0.1;% step-size of pCN walker
v=vt;% truth initial condition (or update G + Phi)
V=zeros(N,J+1); n=1; bb=0; rat=0;
m=[m0,zeros(1,J)];
while n<=N
    iota=[sqrt(C0)*randn,sigma*randn(1,J)];% Gaussian prior sample
    w=m+sqrt(1-beta^2)*(v-m)+beta*iota;% propose sample from the pCN walker
    Gprop=0;Phiprop=0;
    for j=1:J
        Gprop=Gprop+1/2/sigma^2*((alpha*sin(w(j)))^2-2*w(j+1)*alpha*sin(w(j)));
        Phiprop=Phiprop+1/2/gamma^2*(y(j)-w(j+1))^2;
    end
    
    if rand<exp(Phi-Phiprop+G-Gprop)% accept or reject proposed sample
        v=w;Phi=Phiprop;G=Gprop;bb=bb+1;% update the Markov chain
    end
    rat(n)=bb/n;% running rate of acceptance
    V(n,:)=v;% store the chain
    n=n+1;
end
% plot acceptance ratio and cumulative sample mean
% plot acceptance ratio and cumulative sample mean
figure;plot(rat)
%figure;plot(cumsum(V(1:N,1))./[1:N]')
xlabel('samples N')
ylabel('avg accept rate')


% dx=0.0001; x0=[0.01:dx:0.99];
% Z=hist(V(:,1),x0);% construct the posterior histogram for x0
% figure(3), plot(x0,Z/trapz(x0,Z),'k','Linewidth',2)% visualize the posterior
% legend('MC approximation of posterior')


figure(3), plot(V(1:1000,1),'k','Linewidth',2)% visualize first component of MCMC
legend('first MCMC component  x_0')


figure(4), plot(cumsum(V(1:N,1))./[1:N]','k','Linewidth',2)
xlabel('samples N')
ylabel('(1/N) \Sigma_{n=1}^N x_0^{(n)}')



