function [x,ERRobj,Reg] = fnlCg_susc_padding(x0,params)
%-----------------------------------------------------------------------
%
% res = fnlCg(x0,params)
%
% implementation of a L1 penalized non linear conjugate gradient reconstruction
%
% The function solves the following problem:
%
% given the field map b, a Fourier operator F and a kernel operator D, the function 
% finds the susceptibility map x that minimizes:
%
% Phi(x) = || F'*D*F *x - b||^2 + lambda*TV(x) 
%
%
% the optimization method used is non linear conjugate gradient with fast&cheap backtracking
% line-search.
% 
%-------------------------------------------------------------------------

x = x0;

params.D = fftn(params.kernel,params.padsize); % padded kernel


% line search parameters
maxlsiter = params.lineSearchItnlim ;
gradToll = params.gradToll ;
alpha = params.lineSearchAlpha;     beta = params.lineSearchBeta;
t0 = params.lineSearchT0;
k = 0;

% compute g0  = grad(Phi(x))
g0 = wGradient(x,params);

dx = -g0;


% iterations
while(1)

% backtracking line-search

	% pre-calculate values, such that it would be cheap to compute the objective
	% many times for efficient line-search
	[FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx] = preobjective(x, dx, params);
	f0 = objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx, 0, params);
	t = t0;
    [f1, ERRobj, Reg]  =  objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx, t, params);
	
       
	lsiter = 0;

    while (f1 > f0 - alpha*t* abs( dot(g0(:),dx(:)) ) )^2 & (lsiter<maxlsiter)
		lsiter = lsiter + 1;
		t = t * beta;
		[f1, ERRobj, Reg]  =  objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx, t, params);
	end

	if lsiter == maxlsiter
		disp('Reached max line search,.... not so good... might have a bug in operators. exiting... ');
		return;
	end

	% control the number of line searches by adapting the initial step search
	if lsiter > 2
		t0 = t0 * beta;
	end 
	
	if lsiter<1
		t0 = t0 / beta;
	end

	x = (x + t*dx);


    disp(['Iteration: ',num2str(k),' Objective: ',num2str(f1),' Consistency: ',num2str(ERRobj),' Regularization: ',num2str(Reg)])
	
    %conjugate gradient calculation    
	g1 = wGradient(x,params);
    bk = dot(g1(:),g1(:)) / (dot(g0(:),g0(:))+eps);
	g0 = g1;
	dx =  - g1 + bk* dx;
	k = k + 1;
	
	if (k > params.Itnlim) | (norm(dx(:)) < gradToll) 
		break;
	end

end


return;


% -------------------------------------------------------------------------
function [FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx] = preobjective(x, dx, params)
% precalculates transforms to make line search cheap

N = size(x);
FTXFMtx = ifftn(params.D .* fftn(x,params.padsize));
FTXFMtx = FTXFMtx(1+N(1)/2:1.5*N(1), 1+N(2)/2:1.5*N(2), 1+N(3)/2:1.5*N(3));

FTXFMtdx = ifftn(params.D .* fftn(dx,params.padsize));
FTXFMtdx = FTXFMtdx(1+N(1)/2:1.5*N(1), 1+N(2)/2:1.5*N(2), 1+N(3)/2:1.5*N(3));


if params.TVWeight
    DXFMtx = params.TV*x;
    DXFMtdx = params.TV*dx;
else
    DXFMtx = 0;
    DXFMtdx = 0;
end

end
% -------------------------------------------------------------------------

% -------------------------------------------------------------------------
function [res, obj, TV] = objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx, t, params)
%calculates the objective function

p = params.pNorm;

obj = FTXFMtx + t*FTXFMtdx - params.data;
obj = dot(obj(:),obj(:));

if params.TVWeight
    w = DXFMtx(:) + t*DXFMtdx(:);
    TV = (w.*conj(w)+params.l1Smooth).^(p/2); 
else
    TV = 0;
end

TV = sum(TV.*params.TVWeight(:));
%RMS = sqrt(obj/sum(abs(params.data(:))>0));

res = obj + TV;

end
% -------------------------------------------------------------------------

% -------------------------------------------------------------------------
function grad = wGradient(x,params)

gradTV = 0;
gradObj = gOBJ(x,params);

if params.TVWeight
    gradTV = gTV(x,params);
end

grad = (gradObj + params.TVWeight.*gradTV);
end
% -------------------------------------------------------------------------

% -------------------------------------------------------------------------
function gradObj = gOBJ(x,params)
% computes the gradient of the data consistency

N = size(x);
temp = ifftn(params.D .* fftn(x,params.padsize));
temp = temp(1+N(1)/2:1.5*N(1), 1+N(2)/2:1.5*N(2), 1+N(3)/2:1.5*N(3));

temp = ifftn(params.D .* fftn(temp-params.data,params.padsize));
gradObj = 2*temp(1+N(1)/2:1.5*N(1), 1+N(2)/2:1.5*N(2), 1+N(3)/2:1.5*N(3));

%gradObj = params.FT'*(params.D*(params.FT*(  params.FT'*(params.D*(params.FT*x)) - params.data  )));
%gradObj = 2*gradObj ;
end
% -------------------------------------------------------------------------

% -------------------------------------------------------------------------
function grad = gTV(x,params)
% computes gradient of TV operator

p = params.pNorm;

Dx = params.TV*x;
G = p*Dx.*(Dx.*conj(Dx) + params.l1Smooth).^(p/2-1);
grad = params.TV'*G;

end
% -------------------------------------------------------------------------

end