% FUNCTION PROGRAM phi = BG(S,C,g,gamma2)
%---------------------------------------------------------------------------
% The program computes the Backus-Gilbert reconstruction vector from the
% moment matrix S, noise covariance matrix C and the linear constraint
% vector g.
% INPUT :  S   -  (n,n) - matrix containing the weighted moments 
%          C   -  (n,n) - matrix, the covariance of the noise
%          g   -  n - vector containing the integrals of the kernel functions
%          gamma2 - variance of the reconstruction
% OUTPUT : phi -  (n,1) - reconstruction vector
%--------------------------------------------------------------------------
% CALLS TO : None
% 8/2/95 Erkki Somersalo 
%------------------------------------------------------------------------- 
function phi = BG(S,C,g,gamma2)

% The Householder projection P

[n,m] = size(g);
if n<m
  g = g'; n = m;
end

en = zeros(n,1); en(n) = 1;
ss = sign(g(n));
if ss == 0
  ss = 1;
end
nu = g + ss*norm(g)*en;
P = eye(n) - 2/(nu'*nu)*nu*nu';

S = P*S*P';
C = P*C*P';
phin = -ss/norm(g);

A = S(1:n-1,1:n-1); w = S(1:n-1,n);
B = C(1:n-1,1:n-1); v = C(1:n-1,n);
tau = C(n,n);

% Upper and lower bound for active gamma2:

xhigh = -phin*(A\w);
xlow = -phin*(B\v);
ghigh = [xhigh',phin]*C*[xhigh;phin];
glow = [xlow',phin]*C*[xlow;phin];

if gamma2 < glow
  S = sprintf('Variance below the allowed minimum = %5e', glow);
  disp(S);
  return
end

if gamma2 > ghigh
 disp('Variance condition not active');
 phi = P*[xhigh;phin];
 return
end
 

% The Lagrange parameter mu by Newton iteration

mu = 1e-15;  % Initial value
count = 0;
tiny = 1e-12;
gap = 100;
while (gap > tiny & count < 100)
  x = -phin*((A + mu*B)\(w + mu*v));
  dx = -(A + mu*B)\(phin*v + B*x);
  psi = x'*B*x + 2*phin*x'*v + phin^2*tau;
  gap = abs(gamma2 - psi);
  dpsi = 2*dx'*(B*x + phin*v);
  mu = mu + (gamma2 - psi)/dpsi;
  count = count + 1;
end

% Rotation back

phi = P*[x;phin];


