function out = hessiancheck(FUN,x0,varargin)
%HESSIANCHECK   Finite difference verification of analytic Hessian.
%   Modified from gradientcheck.m by Wei-Yen Day.
%
%   OUT = HESSIANCHECK(FUN,X0) computes the finite difference Hessian 
%   approximations to the gradient of FUN at X0. FUN is a handle for 
%   a function that takes a single vector input and returns two 
%   arguments --- the scalar function value and the matrix-valued 
%   Hessian. The output contains the following informationfields:
%
%     OUT.H                 : analytuc Hessian (H)
%     OUT.HFD               : FD approximation of Hessian (HFD)
%     OUT.MaxDiff           : maximum difference between H and HFD
%     OUT.NormHessianDiffs  : 2-norm of H - HFD
%     OUT.HessianDiffs      : H - HFD
%     OUT.Params            : paramters used to compute FD approximations
%
%   OUT = HESSIANCHECK(FUN,X0,'param,value) specifies a
%   parameters and its value. Available parameters are as follows:
%
%     DifferenceType - difference scheme to use {'forward'}
%       'forward'  : h_ij = (f(x+h*e_i+h*e_j) - f(x+h*e_i) - f(x+h*e_j) + f(x))/hj/hj
%       'backward' : h_ij = (f(x-h*e_i-h*e_j) - f(x-h*e_i) - f(x-h*e_j) + f(x))/hj/hj
%       'centered' : h_ij = (f(x+h*e_i+h*e_j) - f(x+h*e_i-h*e_j) - f(x-h*e_i+h*e_j) + f(x-h*e_i-h*e_j))/4/hj/hj
%
%     DifferenceStep - value of h in difference formulae {1e-6}
%
%
%   EXAMPLE
%
%   Suppose the function and gradient of the objective function are
%   specified in an mfile named mysin.m:
%
%     function [f,g,h]=example1(x,a)
%     if nargin < 2, a = 1; end
%     f = sum(sin(a*x));
%     g = a*cos(a*x);
%     h = -a^2sin(a*x);
%
%   We can call the gradient checker (using its default
%   parameters) using the command:
%
%     out = hessiancheck(@(x) example1(x,3), pi/4)
%
%   To change a parameter, we can specify a param/value input pair
%   as follows:
%
%     out = hessiancheck(@(x) example1(x,3), pi/4, 'DifferenceType', 'centered')
%
%   Alternatively, we can use a structure to define the parameters:
%  
%     params.DifferenceStep = 1e-6;
%     out = hessiancheck(@(x) example1(x,3), pi/4, params)
%
%MATLAB Poblano Toolbox.
%Copyright 2009, Sandia Corporation.

%% Parse parameters

% Create parser
params = inputParser;

% Set parameters for this method
params.addParamValue('DifferenceType','forward', @(x) ismember(x,{'forward','backward','centered'}));
params.addParamValue('DifferenceStep',1e-6, @(x) x >= 1e-16);

% Parse input
params.parse(varargin{:});

%% Compute finite difference approximation of gradient
fdtype = params.Results.DifferenceType;
h = params.Results.DifferenceStep;

% setup variables
N = length(x0);
ei = zeros(size(x0));
ej = zeros(size(x0)); 
g = zeros(N,1);
H = zeros(N,N);
if ~(strcmp(fdtype,'backward')), fxp = g; end
if ~(strcmp(fdtype,'forward')), fxm = g; end

% function value at x0
[f,g,H] = feval(FUN,x0);

Hfd = zeros(N,N);

for i = 1:N
    ei(i) = h;
    if i>1
        ei(i-1)=0; 
    end
    if ~(strcmp(fdtype,'backward')), fxp(i) = feval(FUN,x0+ei); end
    if ~(strcmp(fdtype,'forward')), fxm(i) = feval(FUN,x0-ei); end
end
ei(i) = 0;

%fxp_i = zeros(N,N);
fxp_j = zeros(N,N);
%fxm_i = zeros(N,N);
fxm_j = zeros(N,N);
for i = 1:N
    if ~(strcmp(fdtype,'backward')), fxp_j(:,i) = fxp; end
    if ~(strcmp(fdtype,'forward')), fxm_j(:,i) = fxm; end
end
fxp_i = fxp_j';
fxm_i = fxm_j';
fxp_ij = zeros(N,N);
fxm_ij = zeros(N,N);
fxpm_ij = zeros(N,N);

% check value on small perturbtion for Hessian
for i = 1:N
    ei(i) = h;
    if i>1
        ei(i-1)=0; 
    end
    for j = i:N
        ej(j) = h;
        if j>1
            ej(j-1)=0;
        end
        if ~(strcmp(fdtype,'backward'))
            fxp_ij(i, j) = feval(FUN,x0+ei+ej);
            fxp_ij(j, i) = fxp_ij(i, j);
        end
        if ~(strcmp(fdtype,'forward'))
            fxm_ij(i, j) = feval(FUN,x0-ei-ej);
            fxm_ij(j, i) = fxm_ij(i, j);
        end
        if strcmp(fdtype,'centered')
            fxpm_ij(i, j) = feval(FUN,x0+ei-ej);
            fxpm_ij(j, i) = feval(FUN,x0-ei+ej);
        end
    end
    ej(j) = 0;
end

% Compute the FD approximations
switch fdtype
    case 'forward'
        Hfd = (fxp_ij - fxp_i - fxp_j + f)/h/h;
    case 'backward'
        Hfd = (fxm_ij - fxm_i - fxm_j + f)/h/h;
    case 'centered'
        Hfd = (fxp_ij - fxpm_ij - fxpm_ij' + fxm_ij)/4/h/h;
end
H_Hfd = H-Hfd;

%% Output

% Analytic gradient and FD approximation
%out.G = g;
%out.GFD = gfd;

% Analytic Hessian and FD approximation
out.H = H;
out.HFD = Hfd;

% Maximum difference (in magnitude)
%[m,ind] = max(abs(g_gfd));
%out.MaxDiff = m; %g_gfd(ind);
%out.MaxDiffInd = ind;

[mH, indH] = max(abs(H_Hfd));
[mH2, indH2] = max(mH);
% Then A(indH(indH2), indH2) = mH2 is the maximum value
out.MaxDiff = mH2; %H_Hfd(indH(indH2), indH2);
out.MaxDiffInd = [indH(indH2) indH2];

% Differences between gradient and finite difference gradient
%out.NormGradientDiffs = norm(g_gfd);
%out.GradientDiffs = g_gfd;

out.NormHessianDiffs = norm(H_Hfd);
out.HessianDiffs = H_Hfd;

% Parameters used to compute the differences
out.Params = params.Results;