 function [psf, var, fwhm] = qpwls_psf_st(G, C, beta, mask, W, offset)
%function [psf, var, fwhm] = qpwls_psf_st(G, C, beta, mask, [W,] [offset])
%
% Compute FFT-approximate PSF
% for quadratically-penalized shift-invariant unweighted least-squares
% where G'WG and C'C are stacked (see qpwls_pcg_st)
% uses [G'WG + beta C'C]^{-1} G'WG e
%
% PSF is computed for pixel at position "offset" relative to
% the "center" pixel associated with an FFT psf(nx/2+1, ny/2+1)
% in
%       G                       system matrix
%       C                       can be C matrix, or C'Cej, or R object
%       beta    [1,2]           beta values, beta = [beta_R,beta_I]
%       mask                    logical support mask
%       W                       (opt) weight matrix (default: W = 1)
%       offset                  (opt) offset from center pixel (default: offset = (0,0))
%       
% out
%       psf     [nx*2,ny]       psf = [psf_R;psf_I]
%       var     [1,2]           approximate relative variance for the center pixel (always returns [0,0])
%       fwhm    [1,2]           FWHM of psf, fwmh = [fwhm_R,fwhm_I]
%
% Copyright 2002-1-30, Jeff Fessler, The University of Michigan
%  Changes: 2004-12-6, Valur Olafsson, The University of Michigan

% This line is different from qpwls_psf_st since I do
% not want to run the test
if (nargin < 4) | (nargin > 6), help(mfilename), error(mfilename), end

if ~isvar('W') | isempty(W)
	W = 1;	% cheap identity
end

if ~isvar('offset') | isempty(offset)
	offset = zeros(size(size(mask)));
end
if length(offset) ~= ndims(mask), error 'bad offset dim', end

if ~islogical(mask), warning 'not logical mask', mask = logical(mask); end

%
% unit vector at a pixel
% relative to 'center' pixel in FFT sense
%
dim = size(mask);
ndim = length(dim);
index = round(dim/2) + 1 + offset;
index = sum((index-1) .* cumprod([1 dim(1:end-1)])) + 1;
ej = zeros(dim);
ej(index) = 1;

% make sure power-of-2 dimensions
if 0
	dim = size(gtg);
	dpad = 2 .^ ceil(log2(dim-0.1))
	if any(dpad ~= dim)
		warning(['non power of 2, padding ', num2str(dpad)])
	end
end

% jth column of Fisher information: G'WG ej
gtg = embed(G' * (W * (G * ej(mask(:)))), mask);

% jth column of regularization matrix: R ej = C'C ej
% allow "C" to be the impulse response of R for the center pixel
if isnumeric(C) % sparse (or full) matrix
    if all(size(C) == dim * 2)
            ctc = C;
    else
            ctc = embed(C' * (C * ej(mask(:))), mask);
    end
elseif isstruct(C) % R object
        if isvar('C.cgrad')
                ctc = C.cgrad(C, 1e-2 * ej(mask(:))) / 1e-2; % trick
                ctc = embed(ctc, mask);
        else
                error 'bug: unknown C type'
        end
elseif isa(C, 'Fatrix')
        ctc = embed(C' * (C * ej(mask(:))), mask);
end

%	clf, im(121, gtg), im(122, ctc), error test

warn = 'disp';

%
% compensate for phase due to position offset
%
if ndim == 2
	k1 = [0:dim(1)-1] / dim(1);
	k2 = [0:dim(2)-1] / dim(2);
	[k1, k2] = ndgrid(k1, k2);
	phi = 2*pi * (k1*offset(1) + k2*offset(2));
elseif ndim == 3
	k1 = [0:dim(1)-1] / dim(1);
	k2 = [0:dim(2)-1] / dim(2);
	k3 = [0:dim(3)-1] / dim(3);
	[k1, k2, k3] = ndgrid(k1, k2, k3);
	phi = 2*pi * (k1*offset(1) + k2*offset(2) + k3*offset(3));
else
	if any(offset ~= 0)
		warning 'phase correction not implemented'
	end
	phi = 0;
end
f.phase = exp(i*phi);	clear k1 k2 k3 phi

% "spectrum" of Fisher information
f.gtg = fftn(fftshift(gtg));
f.gtg = f.gtg .* f.phase;
f.gtg = reale(f.gtg, warn);  % so that circ approx mtx is Hermitian
if max(f.gtg(:)) <= 0
        warning 'spectrum of Fisher information must have nonzero maximum!'
        keyboard
end
if min(f.gtg(:)) < 0
        printf('qpwls_psf: gtg, negative FT: %g%%, n=%d', ...
                min(f.gtg(:)) / max(f.gtg(:)) * 100, sum(f.gtg(:) < 0))
end

% make sure FT(G'G) is >= 0 for inverse filter !!! (PSD matrix)
% Had to force it to be eps, otherwise I sometimes get NaN
f.gtg = max(f.gtg,eps);

% Now to form the eigenvalues of the real and imaginary part
f.gtg_R = reshape((f.gtg(:)+f.gtg(mod([0:-1:-prod(dim)+1].',prod(dim))+1))./2,dim);
f.gtg_I = reshape((f.gtg(:)-f.gtg(mod([0:-1:-prod(dim)+1].',prod(dim))+1))./(2i),dim);

% Now for the penalty
f.ctc = fftn(fftshift(ctc));
f.ctc = f.ctc .* f.phase;
f.ctc = reale(f.ctc, warn);
if min(f.ctc(:)) < 0
	printf('qpwls_psf: ctc, negative FT: %g%%', ...
		min(f.ctc(:)) / max(f.ctc(:)) * 100)
end

% hessian "spectrum" 
f.hes = [ spdiag(f.gtg_R(:) + beta(1) * f.ctc(:)) -spdiag(f.gtg_I(:)) ; ...
          spdiag(f.gtg_I(:)) spdiag(f.gtg_R(:) + beta(2) * f.ctc(:))];

% hessian eigenvalues
f.hes_eig(1:2:size(f.hes,1)) = (f.gtg_R(:) + beta(1) * f.ctc(:)) + i*f.gtg_I(:);
f.hes_eig(2:2:size(f.hes,1)) = (f.gtg_R(:) + beta(1) * f.ctc(:)) - i*f.gtg_I(:);

printf('Approximate condition number of psf %g \n', ...
	max(abs(f.hes_eig)) / min(abs(f.hes_eig)))

% Find the psf's   NOTE: MUCH faster using backslash instead of inv() function
% The psf for e_{R,j} = [0 ... 0 1 0 ... 0] and e_{I,j} = zeros, i.e. the psf of the real part
f.psfR = f.hes \ [ f.gtg_R(:) ; f.gtg_I(:) ];
psfR = reale(fftshift(ifftn(reshape(f.psfR(1:end/2),dim))), warn);
% The psf for e_{R,j} = zeros and e_{I,j} = [0 ... 0 1 0 ... 0], i.e. the psf of the imag part
f.psfI = f.hes \ [ -f.gtg_I(:) ; f.gtg_R(:) ];
psfI = reale(fftshift(ifftn(reshape(f.psfI(end/2+1:end),dim))), warn);

% Setting psf output variables
psf = [psfR;psfI];

% Calculating fwhm
fwhm_psfR = fwhm2(psfR);
fwhm_psfI = fwhm2(psfI);
fwhm = [fwhm_psfR,fwhm_psfI];

% the variance calculations have not been implemented
var = [0,0];
%var = mean( abs(f.gtg(:)) ./ abs(f.hes(:)).^2 );

if 0
	psf = unpadn(psf, dim);
end
