clear
L = 100000;
lambda = .2;
x(1) = 1;
x(2) = 1;
x(3) = 1;
y(1) = 1;
y(2) = 1;
y(3) = 1;
v(1) = .5;      % a direction vector for testing the distribution of x.
v(2) = .5;
v(3) = sqrt(.5);

Vsum     = 0; % The data to take.
W1sum    = 0;
W2sum    = 0;
VVsum    = 0;
VW1sum   = 0;
VW2sum   = 0;
W1W1sum  = 0;
W1W2sum  = 0;
W2W2sum  = 0;

nbins = 50;
for bin = 1:nbins
   BinCount(bin) = 0;
  end
  
for sample=1:L

  R     = rand(1)^(1/3);
  Zeta  = 2*rand(1) - 1;    % z component of a point on the unit sphere
  Theta = 2*pi*rand(1);     % random angle 
  Rho   = sqrt(1 - Zeta^2); % length of the (x,y) part of a point on the sphere
  S     = sin(Theta);      
  C     = cos(Theta);
  Xi    = Rho*C;            % x and y coordinates of the sphere point
  Eta   = Rho*S;
  X(1)  = R*Xi;     % Three coordinates of a point uniformly distributed ... 
  X(2)  = R*Eta;    % in the unit sphere.
  X(3)  = R*Zeta;
                          % cut and paste this block of code to sample Y
  R     = rand(1)^(1/3);
  Zeta  = 2*rand(1) - 1;    % z component of a point on the unit sphere
  Theta = 2*pi*rand(1);     % random angle 
  Rho   = sqrt(1 - Zeta^2); % length of the (x,y) part of a point on the sphere
  S     = sin(Theta);      
  C     = cos(Theta);
  Xi    = Rho*C;            % x and y coordinates of the sphere point
  Eta   = Rho*S;
  Y(1)  = R*Xi;     % Three coordinates of a point uniformly distributed ... 
  Y(2)  = R*Eta;    % in the unit sphere.
  Y(3)  = R*Zeta;
  
  Rsq     = ( X(1)- Y(1) )^2 + ( X(2)- Y(2) )^2 + ( X(3)- Y(3) )^2;
  R       = sqrt(Rsq);
  absX    = sqrt( X(1)^2 + X(2)^2 + X(3)^2 );
  omaX    = 1 - absX;
  if ( R > omaX )
     V = exp(-lambda*R)/R;
  else
     V = (3/lambda^2)*( 1 - exp(-lambda*omaX)*( 1 + lambda*omaX ) )/(omaX^3);
    end
  W1      = Rsq;
  W2      = Rsq^2;
  Vsum    = V     + Vsum;
  W1sum   = W1    + W1sum;
  W2sum   = W2    + W2sum;
  VVsum   = V*V   + VVsum ;
  VW1sum  = V*W1  + VW1sum ;
  VW2sum  = V*W2  + VW2sum ;
  W1W1sum = W1*W1 + W1W1sum ;
  W1W2sum = W1*W2 + W1W2sum ;
  W2W2sum = W2*W2 + W2W2sum ;

  U   = v(1)*X(1) + v(2)*X(2) + v(3)*X(3);
  bin = 1 + fix( nbins*( U + 1 ) / 2);
  BinCount(bin) = BinCount(bin) + 1;
 end
 
dx = 2 / nbins;  % bin size
xc = -1 + dx/2;  % center of the last bin
for bin = 1:nbins
   EBC(bin) = (3/4)*(1 - xc^2)* dx * L;   % expected bin count 
   xc = xc + dx; % move to the center of the next bin.
 end

plot(BinCount)

hold
plot(EBC);
hold

Ahat0 = Vsum / L;
B1    = 6/5;         % expected value of the W1
B2    = 6/7 + 6/5;   % expected value of the W1
muHat = zeros(2,1);  % Stupid Matlab doesn't know vectors are columns.
muHat(1)  = ( VW1sum - L*Ahat0*B1)/L;
muHat(2)  = ( VW2sum - L*Ahat0*B2)/L;
Chat(1,1) = ( W1W1sum - L*B1^2 )/L;
Chat(1,2) = ( W1W2sum - L*B1*B2 )/L;
Chat(2,2) = ( W2W2sum - L*B2^2 )/L;
Chat(2,1) = Chat(1,2);

alpha1 = muHat(1)/Chat(1,1);
H     = inv( Chat );
alpha = H*muHat;
VVar  = ( VVsum - L*Ahat0^2 )/L;
Z1Var = VVar - muHat(1)^2/Chat(1,1);
Z2Var = VVar - ( muHat.' )*H*muHat;

Ahat1 = Ahat0 - alpha1*( W1sum - L*B1 )/L;
Ahat2 = Ahat0 - alpha(1)*( W1sum - L*B1 )/L - alpha(2)*( W1sum - L*B1 )/L;

A0ErrorBar = sqrt(VVar/L);
A1ErrorBar = sqrt(Z1Var/L);
A2ErrorBar = sqrt(Z2Var/L);

sprintf('Rao Blackwellization and one or two control variates\n lambda = %10.4e, L = %8d\n Ahat0  = %10.4e\n V var  = %10.4e\n Z1 var = %10.4e\n Z2 var = %10.4e\n',...
                                                                 lambda,         L,        Ahat0,            VVar,                     Z1Var,    Z2Var)
sprintf('  answers: A = %10.4e +- %10.4e (no control)\n           A = %10.4e +- %10.4e (one control)\n           A = %10.4e +- %10.4e (two controls)\n', ...
                         Ahat0,    A0ErrorBar,                  Ahat1,    A1ErrorBar,                 Ahat2,    A2ErrorBar)