% $$$ 
% $$$ %function plotgauss2D(xmu, ymu, xvar, yvar, rho)
% $$$ 
% $$$ %xmu, ymu, xvar, yvar
% $$$ xmu = 0;
% $$$ ymu = 0;
% $$$ xvar = 1;
% $$$ yvar = 2;
% $$$ rho = 0;
% $$$ 
% $$$ % make a contour plot and a surface plot of a 2D Gaussian
% $$$ % xmu, ymu - mean of x, y
% $$$ % xvar, yvar - variance of x, y
% $$$ % rho correlation coefficient between x and y
% $$$ xsd = sqrt(xvar); % std deviation on x axis
% $$$ ysd = sqrt(yvar); % std deviation on y axis
% $$$ if (abs(rho) >= 1.0)
% $$$     
% $$$     disp('error: rho must lie between -1 and 1');
% $$$     return
% $$$ end
% $$$ covxy = rho*xsd*ysd; % calculation of the covariance
% $$$ C = [xvar covxy; covxy yvar]; % the covariance matrix
% $$$ A = inv(C); % the inverse covariance matrix
% $$$ % plot between +-2SDs along each dimension
% $$$ maxsd = max(xsd,ysd);
% $$$ x = xmu-2*maxsd:0.1:xmu+2*maxsd; % location of points at which x is calculated
% $$$ y = ymu-2*maxsd:0.1:ymu+2*maxsd; % location of points at which y is calculated
% $$$ [X, Y] = meshgrid(x,y); % matrices used for plotting
% $$$ % Compute value of Gaussian pdf at each point in the grid
% $$$ z = 1/(2*pi*sqrt(det(C))) * exp(-0.5 * (A(1,1)*(X-xmu).^2 + 2*A(1,2)*(X-xmu).*(Y-ymu) + A(2,2)*(Y-ymu).^2));
% $$$ surf(x,y,z);
% $$$ figure;
% $$$ contour(x,y,z);
% $$$ 



mu = [0 0];
Sigma = [.25 .3; .3 1];
x1 = -3:.2:3; x2 = -3:.2:3;
[X1,X2] = meshgrid(x1,x2);
F = mvnpdf([X1(:) X2(:)],mu,Sigma);
F = reshape(F,length(x2),length(x1));
figure(1)
surf(x1,x2,F);
caxis([min(F(:))-.5*range(F(:)),max(F(:))]);
axis([-3 3 -3 3 0 .4])
xlabel('x1'); ylabel('x2'); zlabel('Probability Density');

figure(2)
contour(x1,x2,F);
xlabel('x1');
ylabel('x2');
title('Contour plot of 2D gaussian distrobution')


p1 = [0.4, 0.6, 0.28569];
idx1 = find(abs(x1 - 0.4) < 0.000001);
p2 = [0.6, 0.2, 0.15679];
idx2 = find(abs(x1 - 0.6) < 0.000001);

text(-0.4, 0.55, '(0.4, 0.6)')
hold on;
plot(0.4, 0.6, 'ko')
plot([0.4 0.4], [0 1.2], 'k')
hold off;

text(0.65, 0.35, '(0.4, 0.6)')
hold on;
plot(0.6, 0.4, 'ko')
plot([0.6 0.6], [-0.2 1], 'k')
hold off;

% Marginal Figure
x1 = -3:0.01:3;
mu2 = mu(2)
sig2 = Sigma(2,2) - Sigma(2,1)*inv(Sigma(1,1))*Sigma(1,2)
y1 = normpdf(x1,mu2,sig2);

mu1 = mu(1)
sig1 = Sigma(1,1) - Sigma(1,2)*inv(Sigma(2,2))*Sigma(2,1)
y2 = normpdf(x1,mu1,sig1);


figure(3);
%y1 = F(:,idx1);
%y2 = F(:,idx2);
plot(x1,y1,'b',x1,y2,'k--')
xlabel('x2');
ylabel('Probability');
title('Marginal probability of points in distrobution')
legend('P(x2|x1=0.4)','P(x2|x1=0.6)','FontSize',20)