%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%     Simple objective analysis
%
%     Square grid with Nx*Ny points.
%
%     Toy model of 500mb temperature.
%
%     Small number of observed radiances.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; clf

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Main parameters to change
%% Grid size
Nx = 2; Ny = 2; NxNy = Nx*Ny;
Nx = 20; Ny = 20; NxNy = Nx*Ny;

%%  Number of observations
Nobs = 3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Domain size
Lx = 1E7; Ly = 1E7;
dx = Lx/(Nx-1); dy = Ly/(Ny-1); dxdy = dx*dy;
x = linspace(0,Lx,Nx);
y = linspace(0,Ly,Ny);
[xx,yy] = meshgrid(x,y);
xx = xx'; yy = yy';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Background field
T_b = zeros(Nx,Ny);

%% Define corner values.
T_b( 1, 1) = -10;
T_b(Nx, 1) = -15;
T_b( 1,Ny) = -40;
T_b(Nx,Ny) = -45;

%% Fill field by bi-linear interpolation.
for nx = 1:Nx
for ny = 1:Ny
  Tnx01 = (nx-1)*T_b(Nx, 1) + (Nx-nx)*T_b( 1, 1);
  TnxNy = (nx-1)*T_b(Nx,Ny) + (Nx-nx)*T_b( 1,Ny);
  Tnxny = ((ny-1)*TnxNy + (Ny-ny)*Tnx01) ...
        / ((Nx-1)*(Ny-1));
  T_b(nx,ny) = Tnxny;
end
end
contours=-50:5:0;
[cs,h]=contourf(xx,yy,T_b,contours);
clabel(cs,h,'fontsize',15)
title('BACKGROUND FIELD')
axis square; colorbar
hold on; plot(xx,yy,'k+'); hold off
pause

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Background field (absolute temperature)

%% Rearrange in one-dimensional array.
X_b = reshape(T_b+273,NxNy,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Background error covariance matrix

Terror = 1.0;  
Tdelta = Lx/6;  
for ny1 = 1:Ny
for nx1 = 1:Nx
  n1 = (ny1-1)*Nx+nx1;
  for ny2 = 1:Ny
  for nx2 = 1:Nx
    n2 = (ny2-1)*Nx+nx2;
    dx12 = x(nx1)-x(nx2);
    dy12 = y(ny1)-y(ny2);
    r12 = sqrt(dx12^2+dy12^2);
    B(n1,n2) = Terror*exp(-r12^2/(2*Tdelta^2));
  end
  end
end
end

%% Plot some covariance functions
if ( NxNy > 4 )
  BB = reshape(B,Nx,Ny,Nx,Ny);
  B4(:,:) = BB(fix(Nx/2),fix(Ny/2),:,:);
  contourf(xx,yy,B4); 
  hold on; plot(xx,yy,'k+'); hold off
  title('BACKGROUND ERROR COVARIANCE')
  axis square; colorbar
  pause
  B4(:,:) = BB(1,fix(Ny/2),:,:);
  contourf(xx,yy,B4); 
  hold on; plot(xx,yy,'k+'); hold off
  title('BACKGROUND ERROR COVARIANCE')
  axis square; colorbar
  pause
  B4(:,:) = BB(1,Ny,:,:);
  contourf(xx,yy,B4); 
  hold on; plot(xx,yy,'k+'); hold off
  title('BACKGROUND ERROR COVARIANCE')
  axis square; colorbar
  pause
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Radiance Observations

Tobs = zeros(Nobs,1);
Y_o  = zeros(Nobs,1);

nxobs(1) = fix(  (Nx-1)/4)+1;
nyobs(1) = fix(  (Ny-1)/4)+1;
dxobs(1) = 0.3;
dyobs(1) = 0.3; 
Tobs (1) = -20;

nxobs(2) = fix(  (Nx-1)/4)+1;
nyobs(2) = fix(3*(Ny-1)/4)+1;
dxobs(2) = 0.5;
dyobs(2) = 0.5; 
Tobs (2)  = -30;

nxobs(3) = fix(3*(Nx-1)/4)+1;
nyobs(3) = fix(3*(Ny-1)/4)+1;
dxobs(3) = 0.7;
dyobs(3) = 0.7; 
Tobs (3)  = -40;

nxobs(4) = fix(Nx/2)+1;
nyobs(4) = Ny-1;
dxobs(4) = 0.0;
dyobs(4) = 0.99; 
Tobs (4)  = -50;

%%  Convert to radiances.
sigma = 4.567E-8;
for nobs=1:Nobs
  xobs(nobs) = (nxobs(nobs)-1+dxobs(nobs))*dx;
  yobs(nobs) = (nyobs(nobs)-1+dyobs(nobs))*dy;
  Y_o (nobs) = sigma*(Tobs(nobs) + 273)^4;
end

% Y_o(1) = sigma*(-30 + 273)^4;
% Y_o(2) = sigma*(-25 + 273)^4;
% Y_o(3) = sigma*(-30 + 273)^4;
% Y_o(1) = 1.8E3;
% Y_o(2) = 1.7E3;
% Y_o(3) = 1.6E3;
% Y_o(1) = 200;
% Y_o(2) = 150;
% Y_o(3) = 120;
%
%% Invert the radiances (check).
%% Tobsinv = zeros(Nobs,1);
%% for nobs=1:Nobs
%%   Tobsinv(nobs) = (Y_o(nobs)/sigma)^(0.25) - 273;
%% end
%% 
%% Tobsinv - Tobs;
%% pause

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Observation error covariance matrix
%% Raderr = Teps at 220K
Teps = 1.5;
Ramp = sigma*((220+Teps)^4 - (220)^4);

%%  We allow for covariance between obs
R = Ramp * ones(Nobs,Nobs);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Forward Operator or Observation Operator

%% First, the linear interpolation matrix
Hinterp = zeros(Nobs,NxNy);
for nobs=1:Nobs
  for ny=1:Ny
  for nx=1:Nx
    nn = (ny-1)*Nx+nx;
    nxyobs = (nyobs(nobs)-1)*Nx+nxobs(nobs);
    if ( nn==nxyobs ) % then
      wx1 = (x(nx+1)-xobs(nobs))*(y(ny+1)-yobs(nobs))/dxdy;
      wx2 = (xobs(nobs)-x(nx  ))*(y(ny+1)-yobs(nobs))/dxdy;
      wx3 = (x(nx+1)-xobs(nobs))*(yobs(nobs)-y(ny  ))/dxdy;
      wx4 = (xobs(nobs)-x(nx  ))*(yobs(nobs)-y(ny  ))/dxdy;
      %  fprintf('Checksum %g \n',wx1+wx2+wx3+wx4)
      Hinterp(nobs,nn) = wx1;
      Hinterp(nobs,nn+1) = wx2;
      Hinterp(nobs,nn+Nx) = wx3;
      Hinterp(nobs,nn+Nx+1) = wx4;
    end
  end
  end
end

T_interp = Hinterp*X_b;   

%% Linearize the Stephan-Boltzmann Law.
H_convert = diag(4*sigma*T_interp.^3);

%% Combine interpolation and conversion in H.
H = H_convert * Hinterp;

%%%%%%%%
%% Convert temperatures to radiances
Y_b = sigma*T_interp.^4;

%%  Check: Difference (Tobs - Tinterp)
T_diff = Tobs(1:Nobs) - (T_interp -273);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%             The Analysis

%%  Analysis  Xa  = Xb  +  W * [ Y - H(Xb) ]
%%  Gain matrix W = BH'[R+HBH']^(-1)
%%  Analysis error covariance A = (1-WH)B

%% Innovation (obs minus background field)
d = Y_o - Y_b;

%%  Construct the gain matrix W = BH'[R+HBH']^(-1)
W  =  B*H'*inv(R+H*B*H');

%%  Compute the analysed fields.
X_a = X_b  +  W * [ d ];

%%  Analysis error covariance matrix A = (1-WH)B
A = (1-W*H)*B;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Plot analysed temperature
T_a = reshape(X_a-273,Nx,Ny);
contours=-50:5:0;
[cs,h]=contourf(xx,yy,T_a,contours);
clabel(cs,h,'fontsize',15)
hold on; plot(xx,yy,'k+');
plot(xobs,yobs,'ko'); hold off
title('ANALYSIS FIELD')
axis square; colorbar
pause

%% Plot analysed temperature difference
[cs,h]=contourf(xx,yy,T_a-T_b);
clabel(cs,h,'fontsize',15)
hold on; plot(xx,yy,'k+'); hold off
title('DIFFERENCE FIELD')
axis square; colorbar; hold on
plot(xobs,yobs,'ko'); hold off
pause
contours=-50:5:0;
[cs,h]=contour(xx,yy,T_b,contours);
clabel(cs,h,'fontsize',15)
hold on
[cs,h]=contour(xx,yy,T_a,contours);
clabel(cs,h,'fontsize',15)
title('BACKGROUND & ANALYSIS FIELDS')
axis square; colorbar;
plot(xobs,yobs,'ko')
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
