function obj = constructObjectiveChiSquareZNorm(m, outputIndexes, times, sd, measurements)
%returns Z = (X2 - n) / sqrt(2n), chi-square normalized to Z

% sd: @(t,yInd,yVal) returns standard error given t, index of output, and
% value of output

% Clean up inputs
assert(nargin >= 4, 'KroneckerBio:constructObjectiveChiSquare:AtLeastFourInputs', 'ConstructObjectiveChiSquare requies at least 4 input arguments.')
if nargin < 5
    measurements = [];
end

% Check inputs
assert(isvector(outputIndexes), 'KroneckerBio:constructObjectiveChiSquare:outputIndexes', 'Input "outputIndexes" must be a vector.')
n = length(outputIndexes);
assert(isvector(times) && length(times) == n, 'KroneckerBio:constructObjectiveChiSquare:times', 'Input "times" must be a vector length of "outputIndexes".')
assert((isvector(measurements) && length(measurements) == n) || isempty(measurements), 'KroneckerBio:constructObjectiveChiSquare:measurements', 'Input "measurements" must be a vector length of "outputIndexes" or empty.')

% Gut m
temp = m;
clear m
m.nX = temp.nX;
m.nP = temp.nP;
m.c = temp.c;
clear temp

% Compute covariance matrix
if ~isempty(measurements)
    V = sparse(n,n);
    for j = 1:n
        V(j,j) = sd(times(j), outputIndexes(j), measurements(j))^2;
    end
end

obj.update = @update;
obj.G = @G;
obj.dGdx = @dGdx;
obj.dGdp = @dGdp;
obj.d2Gdx2 = @d2Gdx2;
obj.d2Gdp2 = @d2Gdp2;
obj.d2Gdpdx = @d2Gdpdx;
obj.d2Gdxdp = @d2Gdxdp;
obj.n = n;
obj.H = @H;
obj.Hn = @Hn;
obj.EH = @EH;
obj.EHn = @EHn;
obj.F = @F;
obj.Fn = @Fn;
obj.FnSimbio = @FnSimbio;
obj.EG = @EG;
obj.AddData = @AddData;
obj.AddSimulationData = @AddSimulationData;
obj.ModelPValue = @ModelPValue;

%% Update function
    function objNew = update(mNew)
        objNew = constructObjectiveChiSquare(mNew, outputIndexes, times, sd, measurements);
    end

%% AddData function
    function objNew = AddData(newMeasurements)
        objNew = constructObjectiveChiSquare(m, outputIndexes, times, sd, newMeasurements);
    end

%% AddPerfectData
    function objNew = AddSimulationData(m, sol)
        xs = deval(sol, times, 1:m.nX);
        newMeasurements = zeros(n,1);
        for i = 1:n
            newMeasurements(i) = m.c(outputIndexes(i),:) * xs(:,i);
        end
        objNew = constructObjectiveChiSquare(m, outputIndexes, times, sd, newMeasurements);
    end

%% WrongModelPValue
    function pval = ModelPValue(xSol, obj, xSolUnk, objUnk)
        % Make combination objective list for ease of getting ntot
        comboObj = [obj;objUnk];
        
        % Compute normalization factor
        for i = 1:length(comboObj)
            ntot = ntot + comboObj(i).n;
        end
        
        Z = 0;
        
        % Add normalized chi-square for established data
        for i = 1:length(xSol)
            % TODO: fix once normalization factor is repaired
            Z = Z + obj(i).G([], xSol(i), [], 1) * sqrt(2*obj(i).n) / sqrt(2*ntot);
        end
        
        % Add expected normalized chi-square for unknown data
        for i = 1:length(xSolUnk)
            Z = Z + objUnk(i).EG(xSol, i, comboObj);
        end
        
        pval = chi2pvalue(Z * sqrt(2*ntot) + ntot);
    end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Parameter fitting functions %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    function [val stopTimes] = G(t,xSol,u,nSim)
        % assume all objectives have the same number of measuremments.
        % TODO: fix this assumption that ntot = n*length(xSol)
        ntot = n*length(xSol);
        
        % evaluate the ODE solver's xSol structure to get x(time)
        x = deval(xSol(nSim), times, 1:m.nX);
        
        % get e for every point evaluated
        e = zeros(n,1);
        for i = 1:n
            e(i,1) = m.c(outputIndexes(i),:)*x(:,i) - measurements(i);
        end
        
        %compute chi-square
        % assume all objectives have the same number of measuremments.
        % TODO: fix this assumption that ntot = n*length(xSol)
        val = (e'*(V\e) - n) / sqrt(2*ntot);
        if nargout >= 2
            stopTimes = unique(times);
        end
    end

    function val = dGdx(t,xSol,u,nSim)
        % assume all objectives have the same number of measuremments.
        % TODO: fix this assumption that ntot = n*length(xSol)
        ntot = n*length(xSol);

        %Find all data points that have a time that matches t
        currentIndexes = find(t == times);
        tlength = length(currentIndexes);
        if tlength > 0
            % evaluate the ODE solver's xSol structure to get x(time)
            x = deval(xSol(nSim), t, 1:m.nX);
            
            % compute e for each datapoint that has a matching time to t
            e = zeros(tlength,1);
            C = zeros(tlength,m.nX);
            for i = 1:tlength
                C(i,:) = m.c(outputIndexes(currentIndexes(i)),:);
                e(i,1) = C(i,:)*x - measurements(currentIndexes(i));
            end
            
            %compute dx2/dx = 2*e'*W*dy/dx = 2*e'*W*C
            % assume all objectives have the same number of measuremments.
            % TODO: fix this assumption that ntot = n*length(xSol)
            val = 2*e'*(V(currentIndexes,currentIndexes)\C) / sqrt(2*ntot);
        else
            val = zeros(1, m.nX);
        end
    end

    function val = dGdp(t,xSol,u,nSim)
        val = zeros(1, m.nP);
    end

    function val = d2Gdx2(t,xSol,u,nSim)
        % assume all objectives have the same number of measuremments.
        % TODO: fix this assumption that ntot = n*length(xSol)
        ntot = n*length(xSol);

        %Find all data points that have a time that matches t
        currentIndexes = find(t == times);
        tlength = length(currentIndexes);
        if tlength < 0
            %compute d2x2/dx2 = 2*C'*W*C
            C = zeros(tlength,m.nX);
            for i = 1:tlength
                C(i,:) = m.c(outputIndexes(currentIndexes(i)),:);
            end
            val = 2*C*(V(currentIndexes,currentIndexes)\C) / sqrt(2*ntot);
        else
            val = zeros(m.nX, m.nX);
        end
    end

    function val = d2Gdp2(t,xSol,u,nSim)
        val = zeros(m.nP, m.nP);
    end

    function val = d2Gdpdx(t,xSol,u,nSim)
        val = zeros(m.nX, m.nP);
    end

    function val = d2Gdxdp(t,xSol,u,nSim)
        val = zeros(m.nP, m.nX);
    end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Experimental design functions %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%Hessian of the objective function
    function val = H(xSol, dxdpSol, dx2dp2Sol)
        % assume all objectives have the same number of measuremments.
        % TODO: fix this assumption that ntot = n*length(xSol)
        ntot = n*length(xSol);

        nP = size(dxdpSol.y, 1) / m.nX;
        
        % Evaluate the ODE solver's xSol structure to get x(time)
        x = deval(xSol, times, 1:m.nX);
        dxdp = deval(dxdpSol, times, 1:m.nX*nP);
        d2xdp2 = deval(dx2dp2Sol, times, 1:m.nX*nP*nP);

        % Get values for every point
        e = zeros(n,1);
        dydp = zeros(n, nP);
        d2ydp2 = zeros(n, nP*nP);
        for i = 1:n
            iC = m.c(outputIndexes(i),:);
            % Compute e
            e(i) = iC*x(:,i) - measurements(i);
            
            % Compute dy/dp
            dydp(i,:) = iC * reshape(dxdp(:,i), m.nX, nP);
            
            % Compute d2y/dp2
            d2ydp2(i,:) = iC * reshape(d2xdp2(:,i), m.nX, nP*nP);
        end
        
        % Compute hessian
        val =  e.' * (V \ d2ydp2);
        val = reshape(val, nP, nP);
        val = val + dydp.' * (V \ dydp);
        val = 2 * symmat(val) / sqrt(2*ntot);
    end

%% Normalized hessian of the objective function
    function val = Hn(xSol, dxdpSol, dx2dp2Sol, p)
        % assume all objectives have the same number of measuremments.
        % TODO: fix this assumption that ntot = n*length(xSol)
        ntot = n*length(xSol);

        nP = size(dxdpSol.y, 1) / m.nX;
        
        % Evaluate the ODE solver's xSol structure to get x(time)
        x = deval(xSol, times, 1:m.nX);
        dxdp = deval(dxdpSol, times, 1:m.nX*nP);
        d2xdp2 = deval(dx2dp2Sol, times, 1:m.nX*nP*nP);
        
        % Get values for every point
        e = zeros(n,1);
        dydp = zeros(n, nP); % y_p
        d2ydp2 = zeros(n, nP*nP); % y_pp
        for i = 1:n
            iC = m.c(outputIndexes(i),:);
            % Compute e
            e(i) = iC*x(:,i) - measurements(i);
            
            % Compute dy/dp
            dydp(i,:) = iC * reshape(dxdp(:,i), m.nX, nP);
            
            % Compute d2y/dp2
            d2ydp2(i,:) = iC * reshape(d2xdp2(:,i), m.nX, nP*nP); % auto-reshape invoked
        end
        d2ydp2 = reshape(d2ydp2, n*nP, nP); % yp_p
        
        % Construct normalization matrices
        dpdlnp = spdiags(p,0,sparse(nP,nP)); % p along the diagonal
        linInd = (0:(nP-1)).' * (nP*nP+nP+1) + 1; % linear indexes for putting p into d2pdlnp2
        d2pdlnp2 = sparse(nP, nP*nP);
        d2pdlnp2(linInd) = p; % p along the 3D diagonal, or 2D along the linear indexes
        
        % Compute normalized hessian
        dydlnp = dydp*dpdlnp; % n_p
        val = dpdlnp*dpdlnp; % p_p
        val = d2ydp2*val; % np_p
        val = val + reshape( dydp*d2pdlnp2, n*nP,nP); % np_p
        val = reshape(val, n,nP*nP); % n_pp
        val = e.' * (V \ val); % _pp
        val = reshape(val, nP,nP); % p_p
        val = val + dydlnp.' * (V \ dydlnp); % p_p
        val = 2 * symmat(val) / sqrt(2*ntot); % p_p
    end

%% Expected hessian of the objective function
    function val = EH(xSol, dxdpSol)
        % assume all objectives have the same number of measuremments.
        % TODO: fix this assumption that ntot = n*length(xSol)
        ntot = n*length(xSol);

        nP = size(dxdpSol.y, 1) / m.nX;
        
        % Evaluate the ODE solver structure
        x = deval(xSol, times, 1:m.nX);
        dxdp = deval(dxdpSol, times, 1:m.nX*nP);
        
        % Get dydp for every point
        y = zeros(n,1);
        dydp = zeros(n, nP);
        EV = sparse(n,n);
        for i = 1:n
            iC = m.c(outputIndexes(i),:);
            % Compute y
            y(i) = iC*x(:,i);
            
            % Compute dy/dp
            dydp(i,:) = iC * reshape(dxdp(:,i), m.nX, nP);
            
            % Compute expected V matrix
            EV(i,i) = sd(times(i), outputIndexes(i), y(i))^2;
        end
        
        % Compute expected hessian
        val = dydp.'*(EV\dydp);
        val = 2 * symmat(val) / sqrt(2*ntot);
    end

%% Expected normalized hessian of the objective function
    function val = EHn(xSol, dxdpSol, p)
        % assume all objectives have the same number of measuremments.
        % TODO: fix this assumption that ntot = n*length(xSol)
        ntot = n*length(xSol);

        nP = size(dxdpSol.y, 1) / m.nX;
        
        % Evaluate the ODE solver structure
        x = deval(xSol, times, 1:m.nX);
        dxdp = deval(dxdpSol, times, 1:m.nX*nP);
        
        % Get dydp for every point
        y = zeros(n,1);
        dydp = zeros(n, nP);
        EV = sparse(n,n);
        for i = 1:n
            iC = m.c(outputIndexes(i),:);
            % Compute y
            y(i) = iC*x(:,i);
            
            % Compute dy/dp
            dydp(i,:) = iC * reshape(dxdp(:,i), m.nX, nP);
            
            % Compute expected V matrix
            EV(i,i) = sd(times(i), outputIndexes(i), y(i))^2;
        end

        % Construct normalization matrix
        val = spdiags(p,0,sparse(nP,nP)); % p along the diagonal
        
        % Compute expected normalized hessian
        val = dydp*val;
        val = val.' * (EV \ val);
        val = 2 * symmat(val) / sqrt(2*ntot);
    end

%% Fisher information matrix
    function val = F(xSol, dxdpSol)
        nP = size(dxdpSol.y, 1) / m.nX;
        
        % Evaluate the ODE solver structure
        x = deval(xSol, times, 1:m.nX);
        dxdp = deval(dxdpSol, times, 1:m.nX*nP);
        
        % Get dydp for every point
        y = zeros(n,1);
        dydp = zeros(n, nP);
        EV = sparse(n,n);
        for i = 1:n
            iC = m.c(outputIndexes(i),:);
            % Compute y
            y(i) = iC*x(:,i);
            
            % Compute dy/dp
            dydp(i,:) = iC * reshape(dxdp(:,i), m.nX, nP);
            
            % Compute expected V matrix
            EV(i,i) = sd(times(i), outputIndexes(i), y(i))^2;
        end

        % Compute expected normalized hessian
        val = dydp.' * (EV \ dydp);
        val = symmat(val);
    end

%% Fisher information matrix
    function val = Fn(xSol, dxdpSol, p)
        nP = size(dxdpSol.y, 1) / m.nX;
        
        % Evaluate the ODE solver structure
        x = deval(xSol, times, 1:m.nX);
        dxdp = deval(dxdpSol, times, 1:m.nX*nP);
        
        % Get dydp for every point
        y = zeros(n,1);
        dydp = zeros(n, nP);
        EV = sparse(n,n);
        for i = 1:n
            iC = m.c(outputIndexes(i),:);
            % Compute y
            y(i) = iC*x(:,i);
            
            % Compute dy/dp
            dydp(i,:) = iC * reshape(dxdp(:,i), m.nX, nP);
            
            % Compute expected V matrix
            EV(i,i) = sd(times(i), outputIndexes(i), y(i))^2;
        end

        % Construct normalization matrix
        val = spdiags(p,0,sparse(nP,nP)); % p along the diagonal
        
        % Compute expected normalized hessian
        val = dydp*val;
        val = val.' * (EV \ val);
        val = symmat(val);
    end

%% Fisher information matrix - SimBiology version
    function val = FnSimbio(simData, p)
        % simData is stack of species and sensitivities
        nP = (size(simData.Data, 2) - m.nX) / m.nX;
        
        % Use linear interpolation to evaluate the SimData
        x = piecewiselinear(times, simData.Time, simData.Data(:,1:m.nX)).';
        dxdp = piecewiselinear(times, simData.Time, simData.Data(:,(m.nX+1):end)).';
        
        % Get dydp for every point
        y = zeros(n,1);
        dydp = zeros(n, nP);
        EV = sparse(n,n);
        for i = 1:n
            iC = m.c(outputIndexes(i),:);
            % Compute y
            y(i) = iC*x(:,i);
            
            % Compute dy/dp
            dydp(i,:) = iC * reshape(dxdp(:,i), m.nX, nP);
            
            % Compute expected V matrix
            EV(i,i) = sd(times(i), outputIndexes(i), y(i))^2;
        end

        % Construct normalization matrix
        val = spdiags(p,0,sparse(nP,nP)); % p along the diagonal
        
        % Compute expected normalized hessian
        val = dydp*val;
        val = val.' * (EV \ val);
        val = symmat(val);
    end

%% Expected goal function
% The expected value of normalized chi-square given the current
% measurements as the bias. If measurements is unset, there is assumed to
% be no bias.
    function val = EG(xSol,nSim,obj)
        % Clean up inputs
        if nargin < 4
            obj = [];
        end
        
        % Compute normalization factor
        if isempty(obj)
            ntot = n;
        else
            ntot = 0;
        end
        for i = 1:length(obj)
            ntot = ntot + obj(i).n;
        end
        
        % evaluate the ODE solver's xSol structure to get x(time)
        x = deval(xSol(nSim), times, 1:m.nX);
        
        % get bias d for every point evaluated
        d = zeros(n,1);
        if ~isempty(measurements)
            for i = 1:n
                d(i,1) = m.c(outputIndexes(i),:)*x(:,i) - measurements(i);
            end
        end
        
        % compute expected chi-square
        val = (d.'*(V\d)) / sqrt(2*ntot);
        val = symmat(val);
    end

end