function HamMC_testing;


% 3 bivariate densities
% 1 : 0.9 corr normal, 2: normal * (x^2+0.1), 3: mixture of 5 normals
% (banana)
%
%DensSel=1; % Gauss corr 0.95
DensSel=2; % Banana
%DensSel=3; % Volcano

x1=[-3:0.1:3]; s1=length(x1); 
x2=[-3:0.1:3]; s2=length(x2); 
x1mat=x1'*ones(1,s2);
x2mat=ones(s1,1)*x1;
xv1=reshape(x1mat,1,s1*s2);
xv2=reshape(x2mat,1,s1*s2);
xx=[xv1;xv2];

% plotting
f=fevalD(xx,DensSel);
figure(1);
clf;
imagesc(x1,x2,reshape(f,s1,s2)');
title('Gauss (RW MH)');
axis xy;
hold;

figure(11);
clf;
imagesc(x1,x2,reshape(f,s1,s2)');
title('Gauss (Langevin MH)');
axis xy; 
hold;

figure(21);
clf;
imagesc(x1,x2,reshape(f,s1,s2)');
title('Gauss (Hamiltonian MH)');
axis xy;
hold;

% RW MH
N=10000;
sigma=0.7;
x=zeros(2,N);
x(1:2,1)=[1;1];

ff=fevalD(x(1:2,1),DensSel);

mma=0;
it=1;
while (it<N)
 % Propose new value
 xprop=x(1:2,it)+sigma*randn(2,1);
 ffprop=fevalD(xprop,DensSel);
 
 alpha=min(1,ffprop/ff);
 mma=mma+alpha;
 
 if (alpha>rand)
     x(1:2,it+1)=xprop;
 else
     x(1:2,it+1)=x(1:2,it);
 end
 
 if (mod(it,N/5)==0)
     [it N]
 end
    
 it=it+1;
 
end
disp(sprintf('Mean acc rate is %0.3g',mma/N));
 
 figure(1);
axis([-3 3 -3 3]);
plot(x(1,1:it),x(2,1:it),'k-');

figure(2);
clf;
subplot(2,1,1), hist(x(1,:)); title('Random walk MH results');
subplot(2,1,2), hist(x(2,:));

xRWMH=x;

figure(3);
clf;
plot(xRWMH(1,:),xRWMH(2,:),'k.');
title('RW MH crossplot ');
axis([-3 3 -3 3]);


[acMH,iacMH]=xcorr(xRWMH(1,:));
figure(4); 
clf;
plot(acMH,'k');
hold;
%%------------- Langevin MH

sigma=0.8;
N=10000;
x=zeros(2,N);
x(1:2,1)=[1;1];
 
[ff,Gf]=fGeval(x(1:2,1)',DensSel);

mma=0;

it=1;
while (it<N)
 % Propose new value
 xprop=x(1:2,it)+0.5*sigma^2*Gf+sigma*randn(2,1);
 qprop=-0.5*(1/sigma^2)*(xprop-x(1:2,it)-0.5*sigma^2*Gf)'*(xprop-x(1:2,it)-0.5*sigma^2*Gf);
 
 [ffprop,Gffprop]=fGeval(xprop',DensSel);
 qback=-0.5*(1/sigma^2)*(x(1:2,it)-xprop-0.5*sigma^2*Gffprop)'*(x(1:2,it)-xprop-0.5*sigma^2*Gffprop);
 
 % Acceptance rate
 alpha=min(1,exp(ffprop-ff+qback-qprop));
 mma=mma+alpha;
 
 if (alpha>rand)
     x(1:2,it+1)=xprop;
     Gf=Gffprop;
     ff=ffprop;
 else
     x(1:2,it+1)=x(1:2,it);
 end
 
 if (mod(it,N/5)==0)
     [it N]
 end
 it=it+1;
 
end
disp(sprintf('Mean acc rate is %0.3g',mma/N));

 
figure(11);
axis([-3 3 -3 3]);
plot(x(1,1:it),x(2,1:it),'k-');

figure(12);
clf;
subplot(2,1,1), hist(x(1,:)); title('Langevin MH results');
subplot(2,1,2), hist(x(2,:));

xLMH=x;

figure(13);
clf;
plot(xLMH(1,:),xLMH(2,:),'k.');
title('L MH crossplot ');
axis([-3 3 -3 3]);

[acLH,iacLH]=xcorr(xLMH(1,:));
figure(4); 
plot(acLH,'b--');

%% ----------- Simple Hamiltonian sampler

N=2000;
eps=0.1;
x=zeros(2,N);
x(1:2,1)=[1;1];
[ff]=fevalD(x(1:2,1),DensSel);

it=1;
mma=0;
while (it<N)
 % Propose new value (Leapfrog)
 xprop=leapfrogH(x(1:2,it),eps,DensSel);
 
 % Evaluate
 [ffprop]=fevalD(xprop,DensSel);
 
 % Acceptance rate
 alpha=min(1,(ffprop/ff)); 
 mma=mma+alpha;
 
 if (alpha>rand)
     x(1:2,it+1)=xprop;
     ff=ffprop;
 else
     x(1:2,it+1)=x(1:2,it);
 end
 
 if (mod(it,N/5)==0)
     [it N]
 end
    

 it=it+1;
 
end
% 
disp(sprintf('Mean acc rate is %0.3g',mma/N));

figure(21);
axis([-3 3 -3 3]);
plot(x(1,1:it),x(2,1:it),'k-');

xHMH=x;

figure(22);
clf;
subplot(2,1,1), hist(x(1,:)); title('Hamiltonian MH results');
subplot(2,1,2), hist(x(2,:));

figure(23);
clf;
plot(xHMH(1,:),xHMH(2,:),'k.');
title('Ham MH crossplot ');
axis([-3 3 -3 3]);

[acH,iacH]=xcorr(xHMH(1,:));
figure(4); 
plot(acH,'r-x');

disp('Integrated AC is');
[iacMH iacLH iacH]

    
%% -------- Functions

function xprop=leapfrogH(x,eps,DensSel);

nstep=10;
xpde=zeros(2,nstep);

% Propose momentum
z=randn(2,1);
for i=1:nstep,
    [f,G]=fGeval(x',DensSel);
    z1=z+0.5*eps*G;
    xnew=x+eps*z1;
    [fn,Gn]=fGeval(xnew',DensSel);
    znew=z1+0.5*eps*Gn;
    
    xpde(1:2,i)=xnew;
     
    x=xnew;
    z=znew;
end;

figure(21);
plot(xpde(1,1:nstep),xpde(2,1:nstep),'b-');
plot(xpde(1,nstep),xpde(2,nstep),'ro');
pause(0.1);
xprop=x;

% ---

function ff=fevalD(x,DensSel);

if (DensSel==1)
    ff=gg(x');
end
if (DensSel==2)
    ff=banana(x');
end
if (DensSel==3)
    ff=droplet(x');
end

% ---

function [ff,G]=fGeval(x,DensSel);

if (DensSel==1)
    [ff,G]=ggG(x);
end;
if (DensSel==2)
    [ff,G]=bananaG(x);
end
if (DensSel==3)
    [ff,G]=dropletG(x);
end

% ---

function f=gg(x);

% bivar Gauss
S=[1 0.9;0.9 1];
f=mvnpdf(x,[0;0]',S);

% ---

function [f,fG]=ggG(x);

x=x';
% bivar Gauss
S=[1 0.9;0.9 1];
Si=inv(S);
f=-0.5*x'*Si*x;
fG=-Si*x;

% ----

function f=banana(x);

% banana

w1=0.125; mu1=[-2;1]'; Sigma1=0.5*[1 -0.9;-0.9 1];
f1=w1*mvnpdf(x,mu1,Sigma1);
w2=0.25; mu2=[-1;0]'; Sigma2=0.5*[1 -0.5;-0.5 1];
f2=w2*mvnpdf(x,mu2,Sigma2);
w3=0.25; mu3=[0;-1]'; Sigma3=0.5*[1 0;0 1];
f3=w3*mvnpdf(x,mu3,Sigma3);
w4=0.25; mu4=[1;0]'; Sigma4=0.5*[1 0.5;0.5 1];
f4=w4*mvnpdf(x,mu4,Sigma4);
w5=0.125; mu5=[2;1]'; Sigma5=0.5*[1 0.9;0.9 1];
f5=w5*mvnpdf(x,mu5,Sigma5);
f=f1+f2+f3+f4+f5;

% ----

function [f,fG]=bananaG(x);

% banana
w1=0.125; mu1=[-2;1]'; Sigma1=0.5*[1 -0.9;-0.9 1];
f1=w1*mvnpdf(x,mu1,Sigma1);
w2=0.25; mu2=[-1;0]'; Sigma2=0.5*[1 -0.5;-0.5 1];
f2=w2*mvnpdf(x,mu2,Sigma2);
w3=0.25; mu3=[0;-1]'; Sigma3=0.5*[1 0;0 1];
f3=w3*mvnpdf(x,mu3,Sigma3);
w4=0.25; mu4=[1;0]'; Sigma4=0.5*[1 0.5;0.5 1];
f4=w4*mvnpdf(x,mu4,Sigma4);
w5=0.125; mu5=[2;1]'; Sigma5=0.5*[1 0.9;0.9 1];
f5=w5*mvnpdf(x,mu5,Sigma5);
f0=f1+f2+f3+f4+f5;
f=log(f0);


fG=(1/f)*(f1*(-(Sigma1\(x-mu1)'))+f2*(-(Sigma2\(x-mu2)'))+f3*(-(Sigma3\(x-mu3)'))+...
    f4*(-(Sigma4\(x-mu4)'))+f5*(-(Sigma5\(x-mu5)')));


% -----

function f=droplet(x);

% droplet

fp=(x(:,1).*x(:,1)+x(:,2).*x(:,2)+0.25);
f=mvnpdf(x,[0;0]',eye(2)).*fp;

% ------

function [f,fG]=dropletG(x);

% droplet log density and gradient

fp=(x(:,1).*x(:,1)+x(:,2).*x(:,2)+0.25);
f0=mvnpdf(x,[0;0]',eye(2)).*fp;
f=log(f0);

fG=-x'+(1/fp)*2*[x(:,1);x(:,2)];




