% AO Phase Reconstruction Problem in 2D

% Computational grid.
  n = 100;
  h = 1/(n+1);
  x = [h:h:1-h];
  [X,Y]=meshgrid(x,x);

% First and second derivative matrices
% Set up matrix A and vector f. Use MATLAB's sparse storage.
  N = n^2;
  Asubs = ones(N,1);
  Asubs1 = ones(N,1);
  Asuper1 = ones(N,1);
  % we must account for the zeroes in the sub and super diagonal
  for i=1:n
     Asubs1(i*n) = 0;
  end
  for i=0:n-1
     Asuper1(i*n+1)=0;
  end
  A  = (1/h^2)*spdiags([Asubs,Asubs1,-4*Asubs,Asuper1,Asubs],[-n -1 0 1 n],N,N);
  Dx = (1/h)*spdiags([-Asubs,Asubs],[0,n],N,N);
  Dy = (1/h)*spdiags([-Asubs,Asuper1],[0 1],N,N); 
  
% Generate true phase using biharmonic approximation of the Kolmogorov
% covariance matrix.
xx = 100*randn(n,n);
phase = A\xx(:);
sig = 0.1;
data = [Dx*phase,Dy*phase] + sig*rand(n^2,2);
SNR = norm(data(:))/sqrt(2*n^2*sig^2)

% Compute phase reconstruction
recon = A\([Dx,Dy]*data(:));

% Plote results
figure(1)
subplot(221)
 imagesc(reshape(data(:,1),n,n))
 title('x-derivative data plot')
subplot(222)
 imagesc(reshape(data(:,2),n,n))
 title('y-derivative data plot')
subplot(223)
 imagesc(reshape(phase,n,n))
 colorbar
 title('true phase')
subplot(224)
 imagesc(reshape(recon,n,n))
 colorbar
 title('reconstructed phase')