function [ obj ] = mh_invert( obj )
% Metropolized Gibbs sampling on collapsed HUGE model.
%
% This is a protected method of the tapas_Huge class. It cannot be called
% from outside the class.
%
%

obj = obj.mh_init( );
% number of iterations and burn-in
nIt = obj.options.nvp.numberofiterations;
if isempty(nIt)
    nIt = 2e5;
end
nBi = obj.options.nvp.burnin;
if isempty(nBi) || nBi >= nIt
    nBi = fix(nIt/2);
end

% inverse chain temperature
invTemp = obj.options.nvp.inversetemperature;

% reserve memory
q_nk = zeros(obj.N, obj.K);
obj.trace = struct();
obj.trace.smp = repmat(obj.aux.sample, nIt, 1);
obj.trace.lpr = repmat(obj.aux.lpr, nIt, 1);
psrf = repmat(obj.aux.sample, fix(nIt/obj.const.nPsrf) + 1, 1);

%% ===== MAIN LOOP =====
for iIt = 1:nIt

    % subject level
    for iMhDcm = 1:obj.options.mh.nSteps.dcm
        % sample DCM (parameters)
        obj = mh_sample_dcm( obj, invTemp );
        % sample lambda (hyperparameters)
        obj = mh_sample_noise( obj, invTemp );
    end

    % group level
    if mod(iIt, obj.options.mh.nSteps.knKm) == 0
        k = mod(iIt/obj.options.mh.nSteps.knKm - 1,obj.K) + 1;
        obj = mh_sample_kmhop( obj, k );
    else
        % sample weights pi
        if obj.K > 1
            for iMh = 1:obj.options.mh.nSteps.weights
                obj = mh_sample_weights( obj );
            end
        end
        % sample cluster mu and Sigma
        for iMh = 1:obj.options.mh.nSteps.clusters
            obj = mh_sample_cluster( obj );
        end
    end

    % save sample
    obj.trace.smp(iIt) = obj.aux.sample;
    obj.trace.lpr(iIt) = obj.aux.lpr;

    % adapt proposal step size
    if (iIt <= nBi/3) && (mod(iIt, obj.const.mhAdapt(1)) == 0)
        obj = mh_adapt(obj, iIt);
    end

    % accumulate cluster assigment estimate
    if iIt > nBi
        tmp = bsxfun(@times, exp(obj.aux.rho), obj.aux.sample.pi);
        q_nk = q_nk + bsxfun(@rdivide, tmp, sum(tmp, 2));
    end
    % convergence monitoring
    if mod(iIt, obj.const.nPsrf) == 0
        iPsrf = iIt/obj.const.nPsrf;
        psrf(iPsrf) = mh_psrf(obj.trace.smp(1:iIt), 4);
    end
    if obj.options.nvp.verbose && mod(iIt, 100) == 1
        fprintf('Iteration %u\n', iIt);
    end

% -------------------------------------
end%         END MAIN LOOP
% -------------------------------------
%%          Post Processing
% estimates for assignment probability
q_nk = q_nk/(nIt - nBi);

% acceptance ratio
ratio = struct();
obj.aux.nProp.sp(1) = obj.aux.nProp.sp(1)*obj.N;
for parameter = {'pi', 'mu', 'kappa', 'theta', 'lambda', 'sp'}
    ratio.(parameter{1}) = obj.aux.nAccept.(parameter{1})./...
        obj.aux.nProp.(parameter{1});
end
% posterior mean and quantiles
postMean    = struct();
postVar     = struct();
postQuant   = struct();
for parameter = {'pi', 'mu', 'kappa', 'theta_c', 'theta_h', 'lambda'}
    tmp = reshape([obj.trace.smp(nBi+1:end).(parameter{1})], ...
        [size(obj.aux.sample.(parameter{1})), nIt - nBi]);
    postMean.(parameter{1})  = mean(tmp, 3);
    postVar.(parameter{1})   = var(tmp, 0, 3);
    postQuant.(parameter{1}) = quantile(tmp, obj.options.quantiles, 3);
end
% cumulative probability levels for quantiles
postQuant.levels = obj.options.quantiles;

% calculate PSRF post burn-in
psrf(end) = mh_psrf(obj.trace.smp(nBi + 1:end), 4);

% collect posterior summaries
obj.posterior = struct('nIt', nIt, 'nBi', nBi, 'q_nk', q_nk, ...
    'ratio', ratio, 'psrf', psrf, 'mean', postMean, 'variance', postVar, ...
    'quantile', postQuant, 'lvarBold', obj.aux.lvarBold, ...
    'nrv', exp(-postMean.lambda));

% thin MC chain
if ~isempty(obj.options.nTrace)
    % keep only nTrace samples from post-burn-in phase
    nTrace  = min(nIt - nBi, obj.options.nTrace);
    nThin   = fix((nIt - nBi)/nTrace);
    % select samples uniformly
    obj.trace.smp = obj.trace.smp(end-nThin*(nTrace - 1):nThin:end);
    obj.trace.lpr = obj.trace.lpr(end-nThin*(nTrace - 1):nThin:end);
end

end

%---------------------------------
%            SAMPLING
%---------------------------------
%% SAMPLING: weights (pi)
function [ obj ] = mh_sample_weights( obj )

% propose (in unconstrained space)
prop_piu = obj.aux.sample.pi_u + ...
    randn(1,obj.K-1)*obj.aux.step.pi;
% transform to unit simplex
prop_pis = 1./(1 + exp(log(obj.K-1:-1:1) - prop_piu));
prop_pic = cumprod(1-prop_pis);
prop_pi = [prop_pis(1),-diff(prop_pic),prop_pic(end)];

% evaluate joint (in unconstrained space)
% log-prior on pi
prop_lpr = log(prop_pi)*(obj.prior.alpha_0 - 1) + ...
   sum(log(prop_pis)) + sum(log(1-prop_pis)) + ...
   sum(log(prop_pic(1:end-1)));
prop_lpr = max(prop_lpr, -realmax);
% log-conditional of theta_c given pi
prop_lcd = log(exp(obj.aux.rho)*prop_pi') + obj.aux.rho_max;

% accept/reject
obj.aux.nProp.pi = obj.aux.nProp.pi + 1;
a = exp(prop_lpr - obj.aux.lpr.pi...
        + sum(prop_lcd - obj.aux.lpr.theta_c));
%         a = exp(prop_lpr - obj.aux.lpr.pi);
if ~isnan(a) && ~isinf(a) && rand()<a
    obj.aux.sample.pi_u = prop_piu;
    obj.aux.sample.pi = prop_pi;
    obj.aux.lpr.pi = prop_lpr;
    obj.aux.lpr.theta_c = prop_lcd;
    obj.aux.nAccept.pi = obj.aux.nAccept.pi + 1;
end

end

%% SAMPLING: cluster parameters (mu, kappa = - log(sigma^2))
function [ obj ] = mh_sample_cluster( obj )

obj.aux.nProp.mu = obj.aux.nProp.mu + 1;
obj.aux.nProp.kappa = obj.aux.nProp.mu;

for k = 1:obj.K

    prop_kappa = obj.aux.sample.kappa(k,:);
    % mean
    tmp = randn(1,obj.idx.P_c)*obj.aux.step.mu(k);
    prop_mu = obj.aux.sample.mu(k,:) + tmp*...
        obj.aux.transform.mu(:,:,k);
    dlq = 0; % delta log-proposal density

    % evaluate log-conditional
    % prior
    prop_dmu_k = prop_mu - obj.prior.m_0;
    prop_lpr_mu = -prop_dmu_k*obj.prior.T_0*prop_dmu_k'/2;

    % log-conditional theta_c given mu and kappa
    prop_dtheta_c = bsxfun(@minus, obj.aux.sample.theta_c, prop_mu);
    tmp = obj.aux.l2pi - .5*prop_dtheta_c.^2*exp(prop_kappa') ...
        + .5*sum(prop_kappa);
    if obj.K > 1
        prop_rho = bsxfun(@plus, obj.aux.rho, obj.aux.rho_max);
        prop_rho(:,k) = tmp;
        prop_rho_max = max(prop_rho, [], 2);
        prop_rho = bsxfun(@minus, prop_rho, prop_rho_max);
        prop_lcd = log(exp(prop_rho)*obj.aux.sample.pi') ...
            + prop_rho_max; % log-sum-exp
    else
        prop_rho_max = tmp;
        prop_lcd = prop_rho_max;
        prop_rho = zeros(obj.N, 1);
    end

    % accept/reject
    a = exp(sum(prop_lcd - obj.aux.lpr.theta_c) ...
          + prop_lpr_mu - obj.aux.lpr.mu(k) + dlq);
%             a = exp(prop_lpr_mu - obj.aux.lpr.mu(k));
    if ~isnan(a) && ~isinf(a) && rand()<a
        obj.aux.nAccept.mu(k) = obj.aux.nAccept.mu(k) + 1;
        obj.aux.sample.mu(k,:) = prop_mu;
        obj.aux.rho = prop_rho;
        obj.aux.rho_max = prop_rho_max;
        obj.aux.lpr.theta_c = prop_lcd;
        obj.aux.lpr.mu(k) = prop_lpr_mu;
    end

    % precision
    prop_kappa = prop_kappa + randn(1,obj.idx.P_c)*obj.aux.step.kappa(k);

    % evaluate log-conditional
    % prior
    prop_dkappa = prop_kappa - obj.prior.s_0;
    prop_lpr_kappa = -.5*prop_dkappa.^2*obj.prior.nu_0;
    % log-conditional theta_c given mu and kappa
    prop_dtheta_c = bsxfun(@minus, obj.aux.sample.theta_c, ...
        obj.aux.sample.mu(k,:));
    tmp = obj.aux.l2pi - .5*prop_dtheta_c.^2*exp(prop_kappa') ...
        + .5*sum(prop_kappa);
    if obj.K > 1
        prop_rho = bsxfun(@plus, obj.aux.rho, obj.aux.rho_max);
        prop_rho(:,k) = tmp;
        prop_rho_max = max(prop_rho, [], 2);
        prop_rho = bsxfun(@minus, prop_rho, prop_rho_max);
        prop_lcd = log(exp(prop_rho)*obj.aux.sample.pi') ...
            + prop_rho_max;
    else
        prop_rho_max = tmp;
        prop_lcd = prop_rho_max;
        prop_rho = zeros(obj.N, 1);
    end

    % accept/reject
    a = exp(sum(prop_lcd - obj.aux.lpr.theta_c) ...
          + prop_lpr_kappa - obj.aux.lpr.kappa(k));
%             a = exp(prop_lpr_kappa - obj.aux.lpr.kappa(k));
    if ~isnan(a) && ~isinf(a) && rand()<a
        obj.aux.nAccept.kappa(k) = obj.aux.nAccept.kappa(k) + 1;
        obj.aux.sample.kappa(k,:) = prop_kappa;
        obj.aux.rho = prop_rho;
        obj.aux.rho_max = prop_rho_max;
        obj.aux.lpr.theta_c = prop_lcd;
        obj.aux.lpr.kappa(k) = prop_lpr_kappa;
    end

end

end

%% SAMPLING: DCM parameter (theta)
function [ obj ] = mh_sample_dcm( obj, invTemp )
nProp = obj.aux.nProp.theta + obj.aux.nProp.sp(1) + 1;
bGmm = mod(nProp, obj.options.mh.nSteps.knGmm) == 0;
if bGmm
    obj.aux.nProp.sp(1) = obj.aux.nProp.sp(1) + 1;
else
    obj.aux.nProp.theta = obj.aux.nProp.theta + 1;
end
prec = -.5*exp(obj.aux.sample.kappa);

for n = 1:obj.N

    % propose
    if bGmm % sample from GMM
        k = randsample(obj.K, 1, true, obj.aux.sample.pi);
        prop_theta_c = obj.aux.sample.mu(k,:) + ...
            randn(1, obj.idx.P_c).*exp(-.5*obj.aux.sample.kappa(k,:));
        prop_theta_h = obj.prior.mu_h + randn(1,obj.idx.P_h)./...
            sqrt(diag(obj.prior.Pi_h))'; % assume diagonal precision
    else % sample from Gaussian kernel
%             prop_theta_c = obj.aux.sample.theta_c(n,:) + ...
%                 randn(1, obj.idx.P_c).*obj.aux.step.theta_c(n,:);
        tmp = (randn(1, obj.idx.P_c + obj.idx.P_h).*obj.aux.step.theta(n,:))* ...
            obj.aux.transform.theta;
        prop_theta_c = obj.aux.sample.theta_c(n,:) + tmp(1:obj.idx.P_c);
        prop_theta_h = obj.aux.sample.theta_h(n,:) + tmp(obj.idx.P_c+1:end);
    end

    % evaluate joint
    % log-prior
    prop_dtheta_c = bsxfun(@minus, prop_theta_c, obj.aux.sample.mu);
    prop_rho = obj.aux.l2pi + sum(prec.*prop_dtheta_c.^2, 2)' ...
        + 0.5*sum(obj.aux.sample.kappa, 2)';
    prop_rho_max = max(prop_rho);
    prop_rho = prop_rho - prop_rho_max;
    prop_lpr_c = log(exp(prop_rho)*obj.aux.sample.pi(:)) + prop_rho_max;
    prop_dtheta_h = prop_theta_h - obj.prior.mu_h;
    prop_lpr_h = -.5*prop_dtheta_h*obj.prior.Pi_h*prop_dtheta_h';
    % log-likelihood
    if invTemp
        prop_epsilon = obj.bold_gen( [prop_theta_c, prop_theta_h], ...
            obj.data(n), obj.inputs(n), obj.options.hemo, obj.R, ...
            obj.L, obj.idx );
        tmp = obj.aux.sample.lambda(n,:) - obj.aux.lvarBold(n,:);
        prop_llh = -.5*sum(prop_epsilon.^2*exp(tmp)') ...
           +.5*obj.aux.q_r(n)*sum(tmp);
    else % skip calculation of BOLD signal if inverse temp is zero
        prop_llh = 0;
        prop_epsilon = 0;
    end

    % accept/reject
    a = exp(~bGmm*(prop_lpr_c - obj.aux.lpr.theta_c(n) ...
        + prop_lpr_h - obj.aux.lpr.theta_h(n)) ...
        + invTemp*(prop_llh - obj.aux.lpr.llh(n)));

    if ~isnan(a) && ~isinf(a) && rand()<a
        if bGmm
            obj.aux.nAccept.sp(1) = obj.aux.nAccept.sp(1) + 1;
        else
            obj.aux.nAccept.theta(n) = obj.aux.nAccept.theta(n) + 1;
        end
        obj.aux.sample.theta_c(n,:) = prop_theta_c;
        obj.aux.sample.theta_h(n,:) = prop_theta_h;
        obj.aux.lpr.theta_c(n) = prop_lpr_c;
        obj.aux.lpr.theta_h(n) = prop_lpr_h;
        obj.aux.rho(n,:) = prop_rho;
        obj.aux.rho_max(n) = prop_rho_max;
        obj.aux.epsilon{n} = prop_epsilon;
        obj.aux.lpr.llh(n) = prop_llh;
    end

end
end

%% SAMPLING: Signal-to-noise (lambda)
function [ obj ] = mh_sample_noise( obj, invTemp )

obj.aux.nProp.lambda = obj.aux.nProp.lambda + 1;
for n = 1:obj.N

    % propose in unconstrained space
    prop_lambda = obj.aux.sample.lambda(n,:) + ...
        randn(1, obj.R).*obj.aux.step.lambda(n);

    % evaluate log-conditional
    % prior
    prop_dlambda = prop_lambda - obj.prior.lambda_0;
    prop_lpr = -.5*prop_dlambda.^2*obj.prior.omega_0;
    % likelihood
    tmp = prop_lambda - obj.aux.lvarBold(n,:);
    prop_llh = -.5*sum(obj.aux.epsilon{n}.^2*exp(tmp)') ...
        +.5*obj.aux.q_r(n)*sum(tmp);

    % accept/reject
    a = exp(prop_lpr - obj.aux.lpr.lambda(n) ...
            + invTemp*(prop_llh - obj.aux.lpr.llh(n)));

    if ~isnan(a) && ~isinf(a) && rand()<a
        obj.aux.nAccept.lambda(n) = obj.aux.nAccept.lambda(n) + 1;
        obj.aux.sample.lambda(n,:) = prop_lambda;
        obj.aux.lpr.lambda(n) = prop_lpr;
        obj.aux.lpr.llh(n) = prop_llh;
    end
end
end

%% SAMPLING: k-means-based mode hopping
% A special proposal for clustering part of HUGE model (pi, mu and kappa)
function [ obj ] = mh_sample_kmhop(obj, k)
% track acceptance rate
obj.aux.nProp.sp(2) = obj.aux.nProp.sp(2) + 1;

% --- do kmeans for k = 1,...,K, and define q(...|k) ---
% set up prior
tmpm = exp(obj.prior.s_0+1./2./obj.prior.nu_0');
tmpv = (exp(1./obj.prior.nu_0')-1).*exp(2*obj.prior.s_0+1./obj.prior.nu_0');
b0 = tmpm./tmpv;
a0 = tmpm.^2./tmpv;

% get data
X = obj.aux.sample.theta_c;

% reserve memory
qmm = repmat(obj.prior.m_0,obj.K,1,obj.K);
qms = repmat(diag(obj.prior.T_0)',obj.K,1,obj.K);
qkm = repmat(obj.prior.s_0,obj.K,1,obj.K);
qks = repmat(obj.prior.nu_0',obj.K,1,obj.K);
qpm = zeros(obj.K,obj.K);
for k1 = 1:obj.K
    % do kmeans
    idx = kmeans(X,k1,'Replicates',100);
    % assemble parameters for q
    for k2 = 1:obj.K
        nk = nnz(idx(:)==k2);
        if nk > 0
            mk = mean(X(idx(:)==k2,:),1);
            ak = a0 + (nk + 1)/2;
            bk = b0 + 0.5.*sum(bsxfun(@minus,X(idx(:)==k2,:),mk).^2,1);
            tmpm = ak./bk;
            tmpv = ak./bk.^2;
            % q(kappa)
            qks(k2,:,k1) = 1./log(tmpv./tmpm.^2 + 1);
            qkm(k2,:,k1) = log(tmpm) - 1./2./qks(k2,:,k1);
            % q(mu)
            qms(k2,:,k1) = diag(obj.prior.T_0)' + nk.*exp(obj.prior.s_0);
            qmm(k2,:,k1) = (diag(obj.prior.T_0)'.*obj.prior.m_0 + ...
                nk.*exp(obj.prior.s_0).*mk)./qms(k2,:,k1);
        end
        qpm(k1,k2) = nk + obj.prior.alpha_0(k2);
    end
end

% --- generate proposal for k in 1,...,K, and eval p ---
k1 = k;

prop_lpr_mu = zeros(1,obj.K);
prop_lpr_kappa = zeros(1,obj.K);
prop_rho = obj.aux.rho;

prop_pi = zeros(1,obj.K);
prop_kappa = zeros(obj.K,obj.idx.P_c);
prop_mu = zeros(obj.K,obj.idx.P_c);
for k2 = 1:obj.K
    % draw kappa*
    prop_kappa(k2,:) = qkm(k2,:,k1) + randn(1,obj.idx.P_c)./sqrt(qks(k2,:,k1));
    % draw mu*
    prop_mu(k2,:) = qmm(k2,:,k1) + randn(1,obj.idx.P_c)./sqrt(qms(k2,:,k1));
    % draw pi*
    prop_pi(1,k2) = gamrnd(qpm(k1,k2),1);

    % eval log p(mu*)
    prop_dmu_k = prop_mu(k2,:) - obj.prior.m_0;
    prop_lpr_mu(k2) = -prop_dmu_k*obj.prior.T_0*prop_dmu_k'/2;
    % eval log p(kappa*)
    prop_dkappa = prop_kappa(k2,:) - obj.prior.s_0;
    prop_lpr_kappa(k2) = -.5*prop_dkappa.^2*obj.prior.nu_0;

    % log-conditional theta_c given mu and kappa
    prop_dtheta_c = bsxfun(@minus, obj.aux.sample.theta_c, prop_mu(k2,:));
    prop_rho(:,k2) = obj.aux.l2pi - .5*prop_dtheta_c.^2*exp(prop_kappa(k2,:)') ...
        + .5*sum(prop_kappa(k2,:));

end
prop_pi = prop_pi./sum(prop_pi);

% eval log p(pi*)
prop_pis = [prop_pi(1),prop_pi(2:end-1)./(1 - cumsum(prop_pi(1:end-2)))];
prop_pic = cumprod(1-prop_pis);
prop_piu = tapas_huge_logit(prop_pis) - log(1./(obj.K-1:-1:1));
% log-prior on pi
prop_lpr_pi = log(prop_pi)*(obj.prior.alpha_0 - 1) + ...
   sum(log(prop_pis)) + sum(log(1-prop_pis)) + ...
   sum(log(prop_pic(1:end-1)));
prop_lpr_pi = max(prop_lpr_pi, -realmax);

% eval log p(theta|pi*,mu*,kappa*)
prop_rho_max = max(prop_rho, [], 2);
prop_rho = bsxfun(@minus, prop_rho, prop_rho_max);
prop_lcd = log(exp(prop_rho)*prop_pi') + prop_rho_max; % log-sum-exp

% --- iterate over all permutations and eval q ---
% eval log q(pi*,kappa*,mu*)
[prop_lq_pi,prop_lq_kappa,prop_lq_mu] = eval_lq_perm(prop_pi,...
    prop_kappa,prop_mu,qpm,qkm,qks,qmm,qms,obj.K);

% eval log q(pi,kappa,mu)
[smp_lq_pi,smp_lq_kappa,smp_lq_mu] = eval_lq_perm(obj.aux.sample.pi,...
    obj.aux.sample.kappa,obj.aux.sample.mu,qpm,qkm,qks,qmm,qms,obj.K);

m_prop_lq_mu = logmeanexp(prop_lq_mu);
m_prop_lq_kappa = logmeanexp(prop_lq_kappa);
m_prop_lq_pi = logmeanexp(prop_lq_pi);

m_smp_lq_mu = logmeanexp(smp_lq_mu);
m_smp_lq_kappa = logmeanexp(smp_lq_kappa);
m_smp_lq_pi = logmeanexp(smp_lq_pi);

% --- eval MH acceptance ratio ---
a = [sum(prop_lcd - obj.aux.lpr.theta_c) ...
    ;sum(prop_lpr_kappa - obj.aux.lpr.kappa) ...
    ;sum(prop_lpr_mu - obj.aux.lpr.mu) ...
    ;prop_lpr_pi - obj.aux.lpr.pi ...
    ;sum(m_smp_lq_kappa - m_prop_lq_kappa) ...
    ;sum(m_smp_lq_mu - m_prop_lq_mu) ...
    ;m_smp_lq_pi - m_prop_lq_pi ...
];
% figure(1);clf;stem(a);
a = min(1,exp(sum(a)));

% accept/reject
if ~isnan(a) && ~isinf(a) && rand()<a
    obj.aux.nAccept.sp(2) = obj.aux.nAccept.sp(2) + 1;
    obj.aux.sample.pi_u = prop_piu;
    obj.aux.sample.pi = prop_pi;
    obj.aux.sample.mu = prop_mu;
    obj.aux.sample.kappa = prop_kappa;
    obj.aux.lpr.pi = prop_lpr_pi;
    obj.aux.lpr.mu = prop_lpr_mu;
    obj.aux.lpr.kappa = prop_lpr_kappa;
    obj.aux.lpr.theta_c = prop_lcd;
    obj.aux.rho = prop_rho;
    obj.aux.rho_max = prop_rho_max;
end

end

% evaluate log q(pi*,kappa*,mu*) for all permutations
function [lq_pi,lq_kappa,lq_mu] = eval_lq_perm(p_pi,p_kappa,p_mu,qpm,qkm,qks,qmm,qms,K)
pidx = perms(1:K);

lq_mu = zeros(size(pidx));
lq_kappa = zeros(size(pidx));
lq_pi = zeros(size(pidx));

x = p_pi;
z = [x(1),x(2:end-1)./(1 - cumsum(x(1:end-2)))];
c = cumprod(1-z);

for k1 = 1:K
    m_pi = zeros(K,K);
    m_mu = zeros(size(m_pi));
    m_kappa = zeros(size(m_pi));
    % mesh log q(pi)
    tmp = gammaln(sum(qpm(k1,:))) - sum(gammaln(qpm(k1,:))) + ... % const
    sum(log(z)) + sum(log(1-z)) + ... % jacobian
    sum(log(c(1:end-1))); % jacobian
    m_pi(:,:) = (qpm(k1,:)' - 1)*log(x(:)') + tmp/K; % x

    % mesh log q(mu,kappa)
    for k2 = 1:K
        for k3 = 1:K
            % log p(mu*)
            m_mu(k2,k3) = -.5*(p_mu(k3,:) - qmm(k2,:,k1)).^2*qms(k2,:,k1)' -.5*log(2*pi) +.5*sum(log(qms(k2,:,k1)));
            % log p(kappa*)
            m_kappa(k2,k3) = -.5*(p_kappa(k3,:) - qkm(k2,:,k1)).^2*qks(k2,:,k1)' -.5*log(2*pi) +.5*sum(log(qks(k2,:,k1)));
        end
    end

    linInd = sub2ind([K,K], repmat(1:K,size(pidx,1),1), pidx);
    lq_mu(:,k1) = sum(m_mu(linInd),2);
    lq_kappa(:,k1) = sum(m_kappa(linInd),2);
    lq_pi(:,k1) = sum(m_pi(linInd),2);

end
end

% log(mean(exp(log_probability)))
function lmep = logmeanexp(lpr)
tmp = max(lpr(:));
lmep = log(mean(exp(lpr(:) - tmp))) + tmp;
end

%---------------------------------
%              MISC
%---------------------------------
%% adapt proposal distribution
function [ obj ] = mh_adapt(obj, iIt)

% adapt step size for:
for parameter = {'pi', 'mu', 'kappa', 'theta', 'lambda'}
    % current acceptance rate
    ratio = obj.aux.nAccept.(parameter{1})./obj.aux.nProp.(parameter{1});
    % correction factor
    tmp = ratio - obj.const.mhRate;
    tmp = exp(sign(tmp).*abs(tmp).^3.*obj.const.mhReg);
    % adapt step size
    obj.aux.step.(parameter{1}) = obj.aux.step.(parameter{1}).*tmp;
    % reset counter
    obj.aux.nProp.(parameter{1}) = 0;
    obj.aux.nAccept.(parameter{1}) = ...
        zeros(size(obj.aux.nAccept.(parameter{1})));

end

step = fix(iIt/obj.const.mhTrans);
idx = iIt:-step:1;

% match proposal distribution to posterior covariance for ...
% ... cluster means
mu = reshape([obj.trace.smp(idx).mu], obj.K, obj.idx.P_c, []);
for k = 1:obj.K
    % calculate SVD on empirical covariance of posterior samples
    tmp = cov(permute(mu(k,:,:), [3 2 1]));
    [rotation, scales] = svd(tmp);
    scales = sqrt(diag(scales));
    % limit smallest proposal step size to 5% of maximum step size
    scales(scales < max(scales)*.05) = max(scales)*.05;
    if max(scales) > 0
        obj.aux.transform.mu(:,:,k) = bsxfun(@times, rotation, scales')';
        tmp = sum(log(scales));
        obj.aux.step.mu(k) = obj.aux.step.mu(k)*exp(...
            (obj.aux.logdet.mu(k) - tmp)/obj.idx.P_c);
        obj.aux.logdet.mu(k) = tmp;
    end
end

% ... subject means
theta = [reshape([obj.trace.smp(idx).theta_c], obj.N, obj.idx.P_c, []), ...
         reshape([obj.trace.smp(idx).theta_h], obj.N, obj.idx.P_h, [])];
pooled = zeros(obj.N, size(theta, 3), obj.idx.P_c + obj.idx.P_h);
for n = 1:obj.N
    tmp = permute(theta(n,:,:), [1 3 2]);
    pooled(n, :, :) = bsxfun(@minus, tmp, mean(tmp, 2));
end
pooled = reshape(pooled, [], obj.idx.P_c + obj.idx.P_h);
% calculate SVD on empirical covariance of posterior samples
tmp = cov(pooled);

[rotation, scales] = svd(tmp);
scales = sqrt(diag(scales));
% limit smallest proposal step size to 5% of maximum step size
scales(scales < max(scales)*.05) = max(scales)*.05;
if max(scales) > 0
    obj.aux.transform.theta = bsxfun(@times, rotation, scales')';
    tmp = sum(log(scales));
    tmp = exp((obj.aux.logdet.theta - tmp)/(obj.idx.P_c + obj.idx.P_h));
    for n = 1:obj.N
        obj.aux.step.theta(n) = obj.aux.step.theta(n)*tmp;
    end
    obj.aux.logdet.theta = sum(log(scales));
end

end

%% potential scale reduction factor
function [ psrf ] = mh_psrf( trace, nChains )

nIt = length(trace);
psrf = struct();
for parameter = fieldnames(trace)'
    smpSize = [size(trace(1).(parameter{1})), nIt];
    try
        tmp = permute(reshape([trace(:).(parameter{1})], smpSize), [3 1 2]);

        psrf.(parameter{1}) = tapas_huge_psrf( tmp, nChains );
    catch
        warning('TAPAS:HUGE:convergence', [ 'Potential scale reduction ' ...
            'factor could not be calculated for %s.'], parameter{1});
        psrf.(parameter{1}) = NaN(smpSize);
    end
end

end


function [result, M] = warp_pnt(input, target, method);

% WARP_PNT determine intermediate positions using warping (deformation)
% the input cloud of points is warped to match the target.
% The strategy is to start with simpelest linear warp, followed by a more
% elaborate linear warp, which then is followed by the nonlinear warps up
% to the desired order.
%
% [result, M] = warp_pnt(input, target, method)
%     input          contains the Nx3 measured 3D positions
%     target         contains the Nx3 template 3D positions
%     method         should be empty or any of 'nonlin1', 'nonlin2' ... 'nonlin5'
%
% The default is a traditional linear warp with rescaling in each
% dimension. Optionally you can select a nonlinear warp of the 1st (affine)
% up to the 5th order.
%
% This function depends on the OPTIM and WARPING toolboxes.

% Copyright (C) 2000-2005, Robert Oostenveld
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% $Log: warp_optim.m,v $
% Revision 1.1  2009/01/30 04:02:13  arno
% *** empty log message ***
%
% Revision 1.7  2006/11/23 11:34:51  roboos
% use optimization toolbox if possible, othewise use the standard fminsearch function
%
% Revision 1.6  2006/09/13 09:55:58  roboos
% fixed bug (typo) in rigidbody
%
% Revision 1.5  2006/04/13 12:55:45  roboos
% added a str2func to solve a problem with feval and private
%
% Revision 1.4  2006/04/13 10:50:34  roboos
% renamed calls to warp3d into warp_apply
%
% Revision 1.3  2006/04/13 10:47:39  roboos
% renamed all calls to warpfun into warp_error
%
% Revision 1.2  2006/04/13 10:38:24  roboos
% fixed a problem due to find/strmatch
%
% Revision 1.1  2005/08/15 08:12:40  roboos
% Renamed warp_pnt into warp_optim for consistency with other functions.
% Also changed the code, the subsequent ordering of the simple to
% more complex warps is handled more clean.
%
% Revision 1.4  2005/03/21 15:43:42  roboos
% fixed bug in output for nonlinear warping
% added support for rigidbody or globalrescale warp
%
% Revision 1.3  2004/05/19 09:57:08  roberto
% added GPL copyright statement, added CVS log item
%

global fb;

if nargin<3
  method='traditional';
end

pos1 = input;
pos2 = target;

% The warp_error function might be located in the private subdirectory fo
% fieldtrip, i.e. only available to functions in the fieldtrip toolbox.
% The following line ensures that the function can also be found by the
% feval that is executed by the optimalization toolbox.
warp_error = str2func('warp_error');

% set the options for the minimalisation routine
if exist('fminunc')
  % use the optimization toolbox
  optimfun = @fminunc;
  options  = optimset('fminunc');
  options  = optimset(options, 'Display', 'off');
  options  = optimset(options, 'MaxIter', 1500);
  % options  = optimset(options, 'MaxFunEvals', '1000*numberOfVariables');
  options  = optimset(options, 'TolFun', 1e-4);
  options  = optimset(options, 'LargeScale', 'off');
else
  % use a standard matlab function, this function converges slower
  optimfun = @fminsearch;
  options  = optimset('fminsearch');
  options  = optimset(options, 'Display', 'off');
  options  = optimset(options, 'MaxIter', 4500);
end

if fb; fprintf('distance = %f\n', warp_error([0 0 0 0 0 0], pos1, pos2, 'rigidbody')); end

% the warp is done in steps, starting simple and progressively getting more complex
level = find(strcmp(method, {
  'rigidbody'          % 1
  'globalrescale'     % 2
  'traditional'       % 3
  'nonlin1'           % 4
  'nonlin2'           % 5
  'nonlin3'           % 6
  'nonlin4'           % 7
  'nonlin5'           % 8
  }));

if isempty(method)
  error('incorrect warping method specified');
end

if level>=1
  % do a rigid-body transformation (6 parameters)
  if fb; disp('rigidbody...'); end
  ri = [0 0 0 0 0 0];
  rf = optimfun(warp_error, ri, options, pos1, pos2, 'rigidbody');
  if fb; fprintf('distance = %f\n', warp_error(rf, pos1, pos2, 'rigidbody')); end
end

if level>=2
  % do a rigid-body + global rescaling transformation (7 parameters)
  if fb; disp('rigidbody + global rescaling...'); end
  gi = [rf 1];
  gf = optimfun(warp_error, gi, options, pos1, pos2, 'globalrescale');
  if fb; fprintf('distance = %f\n', warp_error(gf, pos1, pos2, 'globalrescale')); end
end

if level>=3
  % do a rigid-body + individual rescaling transformation (9 parameters)
  if fb; disp('rigidbody + individual rescaling...'); end
  ti = [gf gf(7) gf(7)];
  tf = optimfun(warp_error, ti, options, pos1, pos2, 'traditional');
  if fb; fprintf('distance = %f\n', warp_error(tf, pos1, pos2, 'traditional')); end
end

if level>=4
  % do a first order nonlinear transformation,
  if fb; disp('1st order nonlinear...'); end
  e1i = traditional(tf);
  e1i = [e1i(1:3,4) e1i(1:3,1:3)];	% reshuffle from homogenous into nonlinear
  e1f = optimfun(warp_error, e1i, options, pos1, pos2);
  if fb; fprintf('distance = %f\n', warp_error(e1f, pos1, pos2, 'nonlinear')); end
end

if level>=5
  % do a second order nonlinear transformation,
  if fb; disp('2nd order nonlinear...'); end
  e2i = [e1f zeros(3,6)];
  e2f = optimfun(warp_error, e2i, options, pos1, pos2);
  if fb; fprintf('distance = %f\n', warp_error(e2f, pos1, pos2, 'nonlinear')); end
end

if level>=6
  % do a third order nonlinear transformation,
  if fb; disp('3rd order nonlinear...'); end
  e3i = [e2f zeros(3,10)];
  e3f = optimfun(warp_error, e3i, options, pos1, pos2);
  if fb; fprintf('distance = %f\n', warp_error(e3f, pos1, pos2, 'nonlinear')); end
end

if level>=7
  % do a fourth order nonlinear transformation,
  if fb; disp('4th order nonlinear...'); end
  e4i = [e3f zeros(3,10)];
  e4f = optimfun(warp_error, e4i, options, pos1, pos2);
  if fb; fprintf('distance = %f\n', warp_error(e4f, pos1, pos2, 'nonlinear')); end
end

if level>=8
  % do a fifth order nonlinear transformation,
  if fb; disp('5th order nonlinear...'); end
  e5i = [e4f zeros(3,10)];
  e5f = optimfun(warp_error, e5i, options, pos1, pos2);
  if fb; fprintf('distance = %f\n', warp_error(e5f, pos1, pos2, 'nonlinear')); end
end

% return the estimated parameters of the highest level warp
% and compute the warped points
switch level
  case 1
    M = rf;
  case 2
    M = gf;
  case 3
    M = tf;
  case 4
    M = e1f;
  case 5
    M = e2f;
  case 6
    M = e3f;
  case 7
    M = e4f;
  case 8
    M = e5f;
end
result = warp_apply(M, input, method);


function [newDietModel,pointsModel,roiFlux,pointsModelSln,menuChanges] = nutritionAlgorithmWBM(model,obj,objMinMax,rois,roisMinMax,options)
% Identifies the minimal changes to a diet necessary to get a desired
% change in one or more reactions of interest. One may enter a metabolite
% of the pointsModel instead of a reaction and the algorithm will optimize the
% diet with a sink or demand reaction for the corresponding metabolite of
% interest.
%
% USAGE:
%
%    [newDietModel,pointsModel,slnMin,slnMax,pointsModelSln,itemsRemoved,itemsAdded] = nutritionAlgorithmWBM(pointsModel,obj,objMinMax,rois,roisMinMax,options)
%
%     Example: [newDietModel,pointsModel,roiFlux,pointsModelSln,itemsRemoved,itemsAdded] = nutritionAlgorithmWBM(WBmodel,'Whole_body_objective_rxn','max',{},{})
%
% INPUTS:
%    pointsModel:          COBRA pointsModel structure with the fields:
%                      * .S
%                      * .b
%                      * .ub
%                      * .ub
%                      * .mets  (required if pointsModel.SIntRxnBool absent)
%                      * .rxns  (required if pointsModel.SIntRxnBool absent)
%
%   obj:           organism's objective function
%
%   objMinMax:     minimize ('min') or maximize ('max') objective function
%
%   rois:          cell array of all reactions of interest
%
%   roisMinMax:    cell array of 'min'/'max' entries for rois
%
% OPTIONAL INPUTS:
%   options:  Structure containing the optional specifications:
%
%       * .foodOrMets:  dictates if the algorithm adds individual
%       metabolites to the diet or food items. Default is food items.
%       "Food Cat" adjust algorithm to identify categories of food rather
%       than specific items."AllMets" allows any dietary metabolite into
%       the solution and "FoodMets" only allows metabolites that are in
%       the fdTable spreadsheet into the solution.
%       Possible inputs are: "Food Items", "Food Cat", "AllMets", "FoodMets".
%
%       * .roiWeights:   a vector of weights for each reaction of interest
%       default is equal to 1
%
%       * .weightedFoodItems: A cell vector that specifies any food items
%       or metabolites that should be weighted and the corresponding weight.
%
%       * .initObjSln: provide an initial solution for the objective
%       function. Output from optimizeWBmodel.
%
%       * .caloricRange: 1x2 vector defining boundries for diet calories
%
%       * .slnType: Specify if solution should be 'Detailed' or 'Quick'.
%                   Default setting is 'Detailed'
%
%       * .roiBound: 'Unbounded' or 'Bounded'. Default is 'Bounded'.
%
%       * .foodAddedLimit: Specify a limit for the units of food that can
%                          be added to the diet
%
%       * .foodRemovedLimit: Specify a limit for the units of food that can
%                          be removed from the diet
%
%       * .freeMets: Specifies any metabolites that should be freely
%       available to the model.
%
%       * .calorieWeight: set to 'True' to weight by caloric content rather
%       than servings. Default is 'False'
%
%       * .graphicalAnalysis: set to 'True' include graphical analysis and
%       'False' to not include. Default is 'False' if .slnType is set to
%       'Quick but is 'True; if .slnType is 'Detailed'.
%
% OUTPUT:
%    solution:       Structure containing the following fields:
%
% relaxedModel       pointsModel structure that admits a flux balance solution
%
% .. Authors: - Bronson R. Weston   2021-2022

disp('_____________________________________________________')
%is foodOrMets variable established in the options struct?
if exist('options','var')
    if isfield(options,'foodOrMets')
        foodOrMets=options.foodOrMets;
    else
        foodOrMets='Food Items';
    end
end

%Identify which tables should be loaded
if strcmp(foodOrMets,'Food Items')
    load('fdTable.mat')
elseif strcmp(foodOrMets,'Food Cat')
    try
        load('FoodCategories/fdCategoriesTable.mat')
    catch
        load('fdCategoriesTable.mat')
    end
    fdTable=fdCategoriesTable;
elseif ~strcmp(foodOrMets,'FoodMets') && ~strcmp(foodOrMets,'AllMets')
    error('foodOrMets invalid. Possible inputs are: "Food Items", "Food Cat", "AllMets", "FoodMets".')
else
    options.graphicalAnalysis
    try
        strcmp(options.graphicalAnalysis,'True')
        warning('graphicalAnalysis not available for metabolite based solutions at this time. Only for food items or categories')
        options.graphicalAnalysis='False';
    catch
    end
    load('fdTable.mat')
end

model = changeObjective(model,obj);
model.osenseStr = objMinMax;

% Determine if any rois are metabolites
metRois=[];
for i=1:length(rois)
    if any(strcmp(model.mets,rois{i}))
        metRois=[metRois,i];
        %         if strcmp(slnType,'Detailed')
        %             slnType='Quick';
        %             disp('slnType changed to Quick because one or more rois defined as a metabolite')
        %         end
        if strcmp(roisMinMax{i},'max')
            model=addDemandReaction(model,rois{i}); %adds demand reaction as 'DM_metabolite'
            rois{i}=['DM_',rois{i}];
            model=changeRxnBounds(model,rois{i},100000,'u');
        else
            model=addSinkReactions(model,rois(i),-100000,0);
            rois{i}=['sink_',rois{i}];
        end
    end
end

%initialize optional variables
roiWeights=10*ones(1,length(rois));
initObjSln=[];
weightedFoodItems={};
caloricRange=[0 1e6];
slnType='Detailed';
roiBound='Bounded';
foodAddedLimit=1000000;
foodRemovedLimit=1000000;
calorieWeight='False';
freeMets={};
try
    if strcmp(options.slnType,'Detailed')
        graphicalAnalysis='True';
    else
        graphicalAnalysis='False';
    end
catch
    graphicalAnalysis='True';
end

if exist('options','var')
    fn = fieldnames(options);
    for k=1:numel(fn)
        %         if( isnumeric(options.(fn{k})) )
        %             % do stuff
        %         end
        if strcmp(fn{k},'roiWeights')
            roiWeights=options.roiWeights;
            if length(roiWeights)~=length(rois)
                error('length of roiWeights vector must be the same as items rois')
            end
        elseif strcmp(fn{k},'foodOrMets')
            foodOrMets=options.foodOrMets;
        elseif strcmp(fn{k},'calorieWeight')
            try
                foodOrMets=options.foodOrMets;
            catch
            end
            if strcmp(foodOrMets,'Food Items') || strcmp(foodOrMets,'Food Cat')
                calorieWeight=options.calorieWeight;
            else
                if strcmp(options.calorieWeight,'True') || strcmp(options.calorieWeight,'true')
                    error('"calorieWeight" cannot be True for metabolite solutions')
                end
            end
        elseif strcmp(fn{k},'roiBound')
            roiBound=options.roiBound;
            if ~strcmp(roiBound,'Unbounded') && ~strcmp(roiBound,'Bounded')
                error('Invalid roiBound input. Must be "Unbounded" or "Bounded"')
            end
        elseif strcmp(fn{k},'graphicalAnalysis')
            graphicalAnalysis=options.graphicalAnalysis;
            if ~strcmp(graphicalAnalysis,'True') && ~strcmp(graphicalAnalysis,'False')
                error('Invalid graphicalAnalysis input. Must be "True" or "False"')
            end
        elseif strcmp(fn{k},'foodAddedLimit')
            foodAddedLimit=options.foodAddedLimit;
        elseif strcmp(fn{k},'foodRemovedLimit')
            foodRemovedLimit=options.foodRemovedLimit;
        elseif strcmp(fn{k},'initObjSln')
            initObjSln=options.initObjSln;
        elseif strcmp(fn{k},'freeMets')
            freeMets=options.freeMets;
        elseif strcmp(fn{k},'caloricRange')
            caloricRange=options.caloricRange;
        elseif strcmp(fn{k},'slnType')
            slnType=options.slnType;
            if ~strcmp(slnType,'Detailed') && ~strcmp(slnType,'Quick')
                error('Invalid slnType input. Must be "Detailed" or "Quick"')
            end
        elseif strcmp(fn{k},'weightedFoodItems')
            weightedFoodItems=options.weightedFoodItems;
            if any(contains(options.weightedFoodItems(:,1),'Any_'))
                for i=length(options.weightedFoodItems(:,1)):-1:1
                    word=strsplit(options.weightedFoodItems{i,1},'Any_');
                    if isempty(word{1}) %If the word starts with 'Any_'
                        tmp=fdTable.Properties.VariableNames(contains(fdTable.Properties.VariableNames,word{2})).';
                        tmp=[tmp,num2cell(weightedFoodItems{i,2}*ones(length(tmp),1))];
                        weightedFoodItems(i,:)=[];
                        weightedFoodItems=[weightedFoodItems;tmp];
                    end
                end
            end
        else
            error(['Invalid "options" field entered: ', fn{k}])
        end
    end
end

if any(roiWeights<=0)
    error('"roiWeights" variable must be greater than zero')
end

%adjust ub and lb if roiBound specifies 'Unbound'
if strcmp(roiBound, 'Unbounded')
    for i=1:length(rois)
        f=find(strcmp(model.rxns,rois{i}));
        if strcmp(roisMinMax{i},'max')
            if model.ub(f)~=0
                model.ub(f)=100000;
            end
        else
            if model.lb(f)~=0
                model.lb(f)=-100000;
            end
        end
    end
end

for i=1:length(freeMets)
    str=['Diet_EX_',freeMets{i},'[d]'];
    try
        if strcmp(freeMets{i},'h2o')
            model = changeRxnBounds(model, str, -1e7, 'l');
        else
            model = changeRxnBounds(model, str, -1e5, 'l');
        end
    catch
        error(['Invalid metabolite specified in freeMets: ', freeMets{i}])
    end
end

newDietModel=model; %Copy original instance of model for new diet pointsModel
pointsModel=model; %Copy original instance of model for points pointsModel

%Calculate newDietModel objective function and restrict obj in main pointsModel
objIndex=find(contains(model.rxns,obj));
%
% roiIndex=find(strcmp(newDietModel.rxns,roi));
% disp(['Reaction of Interest = ', newDietModel.rxns{roiIndex}])
roiIndexO=zeros(1,length(rois));
for i=1:length(roiIndexO)
    roiIndexO(i)=find(strcmp(newDietModel.rxns,rois{i}));
    disp(['Reaction of Interest ', num2str(i),' = ', newDietModel.rxns{roiIndexO(i)}])
end

% get flux of objective function
if ~isempty(initObjSln)
    model_Obj=initObjSln;
    f1=model_Obj.f;
    initRoiFlux=model_Obj.v(roiIndexO);
elseif model.ub(objIndex)~=model.lb(objIndex)
    model_Obj = optimizeWBModel(newDietModel);
    f1=model_Obj.f;
    initRoiFlux=model_Obj.v(roiIndexO);
else
    f1=model.ub(objIndex);
    initRoiFlux=NaN(1,length(rois));
end

% If sln type is detailed, check if roi is already min or maxed out and
% if not, define min max range for roi

if strcmp(slnType,'Detailed')
    for i=1:length(rois)
        if initRoiFlux(i)==newDietModel.lb(roiIndexO)
            OroiFluxMin(i)=newDietModel.lb(roiIndexO(i));
        else
            pointsModel = changeObjective(pointsModel,rois{i});
            pointsModel.osenseStr = 'min';
            sln = optimizeWBModel(pointsModel);
            OroiFluxMin(i)=sln.v(roiIndexO(i));
        end
        if initRoiFlux(i)==newDietModel.ub(roiIndexO)
            OroiFluxMax(i)=newDietModel.ub(roiIndexO(i));
        else
            pointsModel = changeObjective(pointsModel,rois{i});
            pointsModel.osenseStr = 'max';
            sln = optimizeWBModel(pointsModel);
            OroiFluxMax(i)=sln.v(roiIndexO(i));
        end
    end
end
pointsModel=addMetabolite(pointsModel, 'unitOfFoodAdded[dP]');
pointsModel=addMetabolite(pointsModel, 'unitOfFoodRemoved[dP]');
pointsModel=addMetabolite(pointsModel, 'unitOfFoodChange[dP]');
pointsModel=addMetabolite(pointsModel, 'roiPoint[roiP]');
pointsModel=addMetabolite(pointsModel, 'point[P]');

fdTableMod=fdTable;
pro_Dindex=find(contains(fdTableMod.Var1,'pro_D')); %for now, remove pro_D from table.
fdTableMod(pro_Dindex,:)=[];
% eIndex=find(contains(fdTableMod.Var1,'Energy in Kcal'));  %remove Energy row from table.
% fdTableMod(eIndex,:)=[];

%Now add row for unitOfFoodAdded variable
tableChange=num2cell(ones(1,length(fdTableMod.Properties.VariableNames)-1));
tableChange=['unitOfFoodAdded',tableChange];
tableChange=cell2table(tableChange,'VariableNames',fdTableMod.Properties.VariableNames);
fdTableMod=[fdTableMod;tableChange];

%Modify unitOfFoodAdded points based on weight from weightedFoodItems
if ~isempty(weightedFoodItems)
    fdTableMod(end, fdTableMod.Properties.VariableNames(weightedFoodItems(:,1)))=(weightedFoodItems(:,2).');
end

if strcmp(foodOrMets,'Food Items') || strcmp(foodOrMets,'Food Cat')
    %Specify all food reactions and food metabolites
    if strcmp(calorieWeight,'True')
        fdTableMod{end,2:end}=fdTableMod{end-1,2:end}/206.28;
    end
    foodRxns=fdTableMod.Properties.VariableNames(2:end);
    foodRxns=strcat('Food_Added_EX_',foodRxns);
    foodRxns=strcat(foodRxns,'[d]');
    foodMetabolites= fdTableMod.Var1;
    foodMetabolites= strcat(foodMetabolites.','[d]');
    uof=find(contains(foodMetabolites,'unitOfFoodAdded'));
    foodMetabolites{uof}='unitOfFoodAdded[dP]';
    %Include food added reaction to pointsModel
    sMatrix=-1*table2array(fdTableMod(1:length(foodMetabolites),2:end));
    pointsModel = addMultipleReactions(pointsModel, foodRxns, foodMetabolites, sMatrix, 'lb', -100000*ones(1,length(foodRxns)), 'ub', zeros(1,length(foodRxns)));
    pointsModel = addMultipleReactions(pointsModel, {'Point_EX_unitOfFoodRemoved2Change[dp]','Point_EX_unitOfFoodAdded2Change[dp]','Point_EX_unitOfFoodChange[dP]_[P]','Point_EX_Point[P]'}, {'unitOfFoodRemoved[dP]','unitOfFoodAdded[dP]','unitOfFoodChange[dP]','point[P]'}, [-1 0 0 0;0 -1 0 0;1 1 -1 0;0 0 1 -1], 'lb', [-1000000,-1000000, -1000000,-1000000], 'ub', [foodRemovedLimit,foodAddedLimit,1000000,1000000]);
    % pointsModel = addMultipleReactions(pointsModel, {'Point_EX_unitOfFoodChange[dP]_[P]','Point_EX_Point[P]','Excretion_EX_Energy'}, {'unitOfFoodChange[dP]','point[P]','Energy in Kcal[d]'}, [-1 0 0;1 -1 0; 0 0 -1], 'lb', [-1000000,-1000000,caloricRange(1)], 'ub', [1000000,1000000,caloricRange(2)]);
elseif strcmp(foodOrMets,'AllMets') || strcmp(foodOrMets,'allMets')
    foodRxns= find(contains(pointsModel.rxns,'Diet_EX_'));
    foodMetabolites= foodRxns;
    foodMetabolites=regexprep(pointsModel.rxns(foodMetabolites),'Diet_EX_','');
    foodRxns=pointsModel.rxns(foodRxns);
    foodRxns=regexprep(foodRxns,'Diet_EX_','Food_Added_EX_');
    foodMetabolites{end+1}='unitOfFoodAdded[dP]';

    %Include food added reaction to pointsModel
    sMatrix=-1*eye(length(foodRxns));
    sMatrix=[sMatrix;-1*ones(1,length(foodRxns))];
    pointsModel = addMultipleReactions(pointsModel, foodRxns, foodMetabolites, sMatrix, 'lb', -100000*ones(1,length(foodRxns)), 'ub', zeros(1,length(foodRxns)));
    pointsModel = addMultipleReactions(pointsModel, {'Point_EX_unitOfFoodAdded2Change[dp]','Point_EX_unitOfFoodChange[dP]_[P]','Point_EX_Point[P]'}, {'unitOfFoodAdded[dP]','unitOfFoodChange[dP]','point[P]'}, [-1 0 0;1 -1 0;0 1 -1], 'lb', [-1000000, -1000000,-1000000], 'ub', [foodAddedLimit,1000000,1000000]);
elseif strcmp(foodOrMets,'FoodMets') || strcmp(foodOrMets,'foodMets')
    foodMetabolites= fdTableMod.Var1;
    foodMetabolites= strcat(foodMetabolites.','[d]');
    uof=find(contains(foodMetabolites,'unitOfFoodAdded'));
    foodMetabolites{uof}='unitOfFoodAdded[dP]';
    foodRxns=foodMetabolites;
    foodRxns(uof)=[];
    f=find(contains(foodMetabolites,'Energy_in_Kcal'));
    foodRxns(f)=[];
%     foodRxns=regexprep(foodRxns,'Diet_EX_','Food_Added_EX_');
    foodRxns=strcat('Food_Added_EX_',foodRxns);
    foodMetabolites(f)=[];
    %     foodRxns= strcat('Food_Added_EX_',foodRxns);
    %Include food added reaction to pointsModel
    sMatrix=-1*eye(length(foodRxns));
    sMatrix=[sMatrix;-1*ones(1,length(foodRxns))];
    pointsModel = addMultipleReactions(pointsModel, foodRxns, foodMetabolites, sMatrix, 'lb', -100000*ones(1,length(foodRxns)), 'ub', zeros(1,length(foodRxns)));
    pointsModel = addMultipleReactions(pointsModel, {'Point_EX_unitOfFoodAdded2Change[dp]','Point_EX_unitOfFoodChange[dP]_[P]','Point_EX_Point[P]'}, {'unitOfFoodAdded[dP]','unitOfFoodChange[dP]','point[P]'}, [-1 0 0;1 -1 0;0 1 -1], 'lb', [-1000000, -1000000,-1000000], 'ub', [foodAddedLimit,1000000,1000000]);
else
    error('Invalid foodOrMets specification')
end

uof=find(contains(foodMetabolites,'unitOfFoodAdded[dP]'));
foodMetabolites{uof}='unitOfFoodRemoved[dP]';
if strcmp(foodOrMets,'Food Items') || strcmp(foodOrMets,'Food Cat')
    %Identify food items already in the diet
    foodDietIndex=find(contains(pointsModel.rxns,'Food_EX_'));
    foodInDietIndex=foodDietIndex(pointsModel.lb(foodDietIndex)<0);
    %Build reaction matrix for food to be removed from diet
    sMatrix=zeros(length(foodMetabolites),length(foodInDietIndex));
    for i=1:length(foodInDietIndex)
        foodItem = regexprep(pointsModel.rxns{foodInDietIndex(i)},'Food_EX_','');
        foodRxn=strcat('Food_Removed_EX_',foodItem);
%         foodItem = regexprep(foodItem,'\[d\]','');
%         sMatrix(:,i)=fdTableMod.(foodItem);
%         sMatrix(end,i)=-1/sMatrix(end,i);
%         sMatrix(:,i)
        RxnFormula=printRxnFormula(pointsModel,pointsModel.rxns{foodInDietIndex(i)},0);
        [metaboliteList, stoichCoeffList, ~]=parseRxnFormula(RxnFormula{1});
        metaboliteList=[metaboliteList,{'unitOfFoodRemoved[dP]'}];
        stoichCoeffList=-1*stoichCoeffList;
        stoichCoeffList=[stoichCoeffList,-1];
        pointsModel = addMultipleReactions(pointsModel, {foodRxn}, metaboliteList, stoichCoeffList, 'lb', pointsModel.ub(foodInDietIndex(i)), 'ub', 0);
    end

%     pointsModel = addMultipleReactions(pointsModel, foodRxns, foodMetabolites, sMatrix, 'lb', pointsModel.ub(foodInDietIndex), 'ub', zeros(1,length(foodRxns)));
else

    %Build reaction matrix for food to be removed from diet
    sMatrix=-1*sMatrix;
    sMatrix(end,:)=-1./sMatrix(end,:);
    foodRxns = regexprep(foodRxns,'Food_Added_EX_','Food_Removed_EX_');
    pointsModel = addMultipleReactions(pointsModel, foodRxns, foodMetabolites, sMatrix, 'lb', -1000000*ones(1,length(foodRxns)), 'ub', zeros(1,length(foodRxns)));

end

%note that previous line makes food removal lb equal to food consumption ub
%as more food should not be able to be removed than the food consumption
%capabilities. If removing all of a food item yeilds the optimum solution,
%the diet consumption of said food item should hug the ub and food removal
%will then be equivalent to said ub yeilding 0.

% Introduce any sink or demand reactions if necessary
% if ~isempty(metRois)
%     for i=length(metRois):-1:1
%         if strcmp(roisMinMax{metRois(i)},'max')
%             pointsModel = addMultipleReactions(pointsModel, {strcmp(rois{metRois(i)},'_Demand')}, {rois{metRois(i)},'roiPoint[roiP]'}, [-1; -1*roiWeights(metRois(i))], 'lb', 0, 'ub', 1000000);
%             rois{metRois(i)}=[];
%             roiIndexO(metRois(i))=[];
%         else
%             pointsModel = addMultipleReactions(pointsModel, {strcmp(rois{metRois(i)},'_Sink')}, {rois{metRois(i)},'roiPoint[roiP]'}, [-1; roiWeights(metRois(i))], 'lb', -1000000, 'ub', 0);
%             rois{metRois(i)}=[];
%             roiIndexO(metRois(i))=[];
%         end
%     end
% end

%Get roi Indexes
for i=1:length(rois)
    roiIndexP(i)=find(strcmp(pointsModel.rxns,rois{i}));
end
roiUB=pointsModel.ub(roiIndexP);
roiLB=pointsModel.lb(roiIndexP);

%replace roi function
stoich=pointsModel.S(:,roiIndexP);
if length(roiIndexP)>1
    metInd=find(any(stoich.'~=0));
    metsRoi=pointsModel.mets(any(stoich.'~=0)).';
    metsStoich=full(stoich(any(stoich.'~=0),:));
else
    metsRoi=pointsModel.mets(find(stoich~=0)).';
    metsStoich=full(stoich(find(stoich~=0)));
end

weightVector=zeros(1,length(roiIndexP));
weightVector(contains(roisMinMax,'max'))=-1;
weightVector(contains(roisMinMax,'min'))=1;
metsStoich=[metsStoich;weightVector.*roiWeights;zeros(1,length(roiIndexP))];

for i=1:length(rois)
    evalc('[pointsModel,~,~]= removeRxns(pointsModel, rois{i})');
% [pointsModel,~,~]= removeRxns(pointsModel, rois{i});
end

pointsModel = addMultipleReactions(pointsModel, [rois,'Point_EX_roiPoints[roiP]_[P]'], [metsRoi,'roiPoint[roiP]','point[P]'], [metsStoich,[zeros(length(metsStoich(:,1))-2,1);-1;1]], 'lb', [roiLB.',-1000000], 'ub', [roiUB.',1000000]);
caloriesRxn=find(contains(pointsModel.rxns,'EX_DietEnergy'));
pointsModel.lb(caloriesRxn)=caloricRange(1);
pointsModel.ub(caloriesRxn)=caloricRange(2);

%Find solution
pointsModel = changeObjective(pointsModel,'Point_EX_Point[P]');
pointsModel.osenseStr = 'min';
pointsModelSln = optimizeWBModel(pointsModel);
% pointsModelSln.v(roiIndex)

disp(['Solution points =',num2str(pointsModelSln.f)])
disp([num2str(pointsModelSln.v(find(strcmp(pointsModel.rxns,'Point_EX_unitOfFoodChange[dP]_[P]')))),' come from diet']);
disp([num2str(pointsModelSln.v(find(strcmp(pointsModel.rxns,'Point_EX_roiPoints[roiP]_[P]')))),' come from roi']);
foodAddedIndexes=find(contains(pointsModel.rxns,'Food_Added_EX_'));
foodRemovedIndexes=find(contains(pointsModel.rxns,'Food_Removed_EX_'));
slnIndexes1=foodAddedIndexes(pointsModelSln.v(foodAddedIndexes)<0);
slnIndexes2=foodRemovedIndexes(pointsModelSln.v(foodRemovedIndexes)<0);

disp('Food items of interest are:')
T=table([pointsModel.rxns(slnIndexes1);pointsModel.rxns(slnIndexes2)],pointsModelSln.v([slnIndexes1;slnIndexes2]),'VariableNames',{'Food Rxn', 'Flux'})
if ~strcmp(foodOrMets,'AllMets') && ~strcmp(foodOrMets,'FoodMets')
    disp(['Diet Energy = ',num2str(pointsModelSln.v(contains(pointsModel.rxns,'EX_DietEnergy')))])
end

%Add and remove relevant food items from diet in newDietModel
if strcmp(foodOrMets,'Food Items') || strcmp(foodOrMets,'Food Cat')
    foodItemsAdd= regexprep(pointsModel.rxns(slnIndexes1),'Food_Added_EX_','Food_EX_');
    foodItemsRemove= regexprep(pointsModel.rxns(slnIndexes2),'Food_Removed_EX_','Food_EX_');
else
    foodItemsAdd= regexprep(pointsModel.rxns(slnIndexes1),'Food_Added_EX_','Diet_EX_');
    foodItemsRemove= regexprep(pointsModel.rxns(slnIndexes2),'Food_Removed_EX_','Diet_EX_');
end
modelOindexAdd=zeros(1,length(foodItemsAdd));
sl2IndexAdd=zeros(1,length(foodItemsAdd));
modelOindexRemove=zeros(1,length(foodItemsRemove));
sl2IndexRemove=zeros(1,length(foodItemsRemove));
for i=1:length(foodItemsAdd)
    modelOindexAdd(i)=find(contains(newDietModel.rxns,foodItemsAdd(i)));
    sl2IndexAdd(i)=find(contains(pointsModel.rxns,foodItemsAdd(i)));
end
for i=1:length(foodItemsRemove)
    modelOindexRemove(i)=find(contains(newDietModel.rxns,foodItemsRemove(i)));
    sl2IndexRemove(i)=find(contains(pointsModel.rxns,foodItemsRemove(i)));
end
newDietModel.lb(modelOindexAdd)=(pointsModelSln.v(sl2IndexAdd)+pointsModelSln.v(slnIndexes1))*1.01;
newDietModel.ub(modelOindexAdd)=(pointsModelSln.v(sl2IndexAdd)+pointsModelSln.v(slnIndexes1))*0.99;
newDietModel.lb(modelOindexRemove)=(pointsModelSln.v(sl2IndexRemove)-pointsModelSln.v(slnIndexes2))*1.01;
newDietModel.ub(modelOindexRemove)=(pointsModelSln.v(sl2IndexRemove)-pointsModelSln.v(slnIndexes2))*0.99;

if strcmp(graphicalAnalysis,'True')
    [ogCatTable] = getDietComposition(model,'Off');
    [newCatTable] = getDietComposition(newDietModel,'Off');
    OgMacros=ogCatTable.('Mass (g)');
    NewCategories=newCatTable.('Category');
    NewMacros=newCatTable.('Mass (g)');
    figure()
    t = tiledlayout(1,2,'TileSpacing','compact');
    ax1 = nexttile;
    pie(ax1,OgMacros(1:4))
    title('Original Diet')
    ax2 = nexttile;
    pie(ax2,NewMacros(1:4))
    title('New Diet')
    legend(NewCategories(1:4),'Location','South')
end

menuChanges=T;
if strcmp(slnType,'Quick')
    for i=1:length(rois)
        ind=find(strcmp(pointsModel.rxns,rois{i}));
        disp([rois{i},' flux = ', num2str(pointsModelSln.v(ind))])
    end
    roiFlux(i)=pointsModelSln.v(ind);
    slnRanges=pointsModelSln.v(roiIndexP);
    %     menuChanges.itemsRemoved=[];
    %     menuChanges.itemsAdded=[];
    %     menuChanges.fullMenu=[];
    newDietModel = changeObjective(newDietModel,obj);
    newDietModel.osenseStr = objMinMax;
    return
end

%Find new obj flux with new diet

if model.ub(objIndex)~=model.lb(objIndex)
    model_Obj = optimizeWBModel(newDietModel);
    f2=model_Obj.f;
    newDietModel=changeRxnBounds(newDietModel,obj,f2,'b'); %constrain pointsModel obj flux
else
    f2=model.ub(objIndex);
end

disp(['f1 =',num2str(f1), ' & f2=', num2str(f2)])

%Compute new min max ranges for roi with new diet
%%
for i=1:length(rois)
    disp(rois{i})
    newDietModel = changeObjective(newDietModel,rois{i});
    newDietModel.osenseStr = 'min';
    tmp=optimizeWBModel(newDietModel);
    slnMin.(['Rxn',num2str(i)]) = tmp;
    NroiFluxMin(i)=slnMin.(['Rxn',num2str(i)]).v(roiIndexO(i));
    newDietModel.osenseStr = 'max';
    tmp=optimizeWBModel(newDietModel);
    slnMax.(['Rxn',num2str(i)]) = tmp;
    NroiFluxMax(i)=slnMax.(['Rxn',num2str(i)]).v(roiIndexO(i));
    disp(['Original Diet RoI range = ', num2str(OroiFluxMin(i)), ':', num2str(OroiFluxMax(i))])
    disp(['New Diet RoI range = ', num2str(NroiFluxMin(i)), ':', num2str(NroiFluxMax(i))])
end

% slnMin
roiFlux=[slnMin.',slnMax.'];
newDietModel.ub(objIndex)=model.ub(objIndex);
newDietModel.lb(objIndex)=model.lb(objIndex);
newDietModel = changeObjective(newDietModel,obj);
newDietModel.osenseStr = objMinMax;

disp('___________________________________________________________________')

end

%TO DO:
% -Impliment itemsRemoved and itemsAdded
% -Introduce option for metabolite adjustments
% -Include intelligent default weighting for roi
% -slnMin & slnMax turn to structs or convert to flux values

%Finished:
%-introduced Quick/Detailed functionality
%-renamed variables to be more obvious
%-commented code

% figure()
% t = tiledlayout(2,2,'TileSpacing','compact');
% ax1 = nexttile;
% pie(ax1,OgMacros(1:4))
% title('Original Diet')
% ax2 = nexttile;
% x=pie(ax2,NewMacros(1:4))
% title('New Diet')
% legend(NewCategories(1:4),'Location','South')
% ax3 = nexttile;
% bar(ax3,(NewMacros(1:4)-OgMacros(1:4))./OgMacros(1:4))
% ylim([-1 14])
% ax4= nexttile;
% legend(x,NewCategories(1:4))


% This file is part of TREEQSM.
%
% TREEQSM is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% TREEQSM is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with TREEQSM.  If not, see <http://www.gnu.org/licenses/>.

function Pass = filtering(P,inputs)

% ---------------------------------------------------------------------
% FILTERING.M       Filters noise from point clouds.
%
% Version 3.0.0
% Latest update     3 May 2022
%
% Copyright (C) 2013-2022 Pasi Raumonen
% ---------------------------------------------------------------------

% Filters the point cloud as follows:
%
% 1) the possible NaNs are removed.
%
% 2) (optional, done if filter.k > 0) Statistical kth-nearest neighbor
% distance outlier filtering based on user defined "k" (filter.k) and
% multiplier for standard deviation (filter.nsigma): Determines the
% kth-nearest neighbor distance for all points and then removes the points
% whose distances are over average_distance + nsigma*std. Computes the
% statistics for each meter layer in vertical direction so that the
% average distances and SDs can change as the point density decreases.
%
% 3) (optional, done if filter.radius > 0) Statistical point density
% filtering based on user defined ball radius (filter.radius) and multiplier
% for standard deviation (filter.nsigma): Balls of radius "filter.radius"
% centered at each point are defined for all points and the number of
% points included ("point density") are computed and then removes the points
% whose density is smaller than average_density - nsigma*std. Computes the
% statistics for each meter layer in vertical direction so that the
% average densities and SDs can change as the point density decreases.
%
% 4) (optional, done if filter.ncomp > 0) Small component filtering based
% on user defined cover (filter.PatchDiam1, filter.BallRad1) and threshold
% (filter.ncomp): Covers the point cloud and determines the connected
% components of the cover and removes the points from the small components
% that have less than filter.ncomp cover sets.
%
% 5) (optional, done if filter.EdgeLength > 0) cubical downsampling of the
% point cloud based on user defined cube size (filter.EdgeLength):
% selects randomly one point from each cube
%
% Does the filtering in the above order and thus always applies the next
% fitering to the point cloud already filtered by the previous methods.
% Statistical kth-nearest neighbor distance outlier filtering and the
% statistical point density filtering are meant to be exlusive to each
% other.
%
% Inputs:
% P         Point cloud
% inputs    Inputs structure with the following subfields:
%   filter.EdgeLength   Edge length of the cubes in the cubical downsampling
%   filter.k            k of knn method
%   filter.radius       Radius of the balls in the density filtering
%   filter.nsigma       Multiplier for standard deviation, determines how
%                         far from the mean the threshold is in terms of SD.
%                         Used in both the knn and the density filtering
%   filter.ncomp        Threshold number of components in the small
%                         component filtering
%   filter.PatchDiam1   Defines the patch/cover set size for the component
%                         filtering
%   filter.BallRad1     Defines the neighbors for the component filtering
%   filter.plot         If true, plots the filtered point cloud
% Outputs:
% Pass      Logical vector indicating points passing the filtering
% ---------------------------------------------------------------------

% Changes from version 2.0.0 to 3.0.0, 3 May 2022:
% Major changes and additions.
% 1) Added two new filtering options: statistical kth-nearest neighbor
%    distance outlier filtering and cubical downsampling.
% 2) Changed the old point density filtering, which was based on given
%    threshold, into statistical point density filtering, where the
%    threshold is based on user defined statistical measure
% 3) All the input parameters are given by "inputs"-structure that can be
%    defined by "create_input" script
% 4) Streamlined the coding and what is displayed

%% Initial data processing
% Only double precision data
if ~isa(P,'double')
  P = double(P);
end
% Only x,y,z-data
if size(P,2) > 3
  P = P(:,1:3);
end
np = size(P,1);
np0 = np;
ind = (1:1:np)';
Pass = false(np,1);

disp('----------------------')
disp(' Filtering...')
disp(['  Points before filtering:  ',num2str(np)])

%% Remove possible NaNs
F = ~any(isnan(P),2);
if nnz(F) < np
  disp(['  Points with NaN removed:  ',num2str(np-nnz(Pass))])
  ind = ind(F);
end

%% Statistical kth-nearest neighbor distance outlier filtering
if inputs.filter.k > 0
  % Compute the knn distances
  Q = P(ind,:);
  np = size(Q,1);
  [~, kNNdist] = knnsearch(Q,Q,'dist','euclidean','k',inputs.filter.k);
  kNNdist = kNNdist(:,end);

  % Change the threshold kNNdistance according the average and standard
  % deviation for every vertical layer of 1 meter in height
  hmin = min(Q(:,3));
  hmax = max(Q(:,3));
  H = ceil(hmax-hmin);
  F = false(np,1);
  ind = (1:1:np)';
  for i = 1:H
    I = Q(:,3) < hmin+i & Q(:,3) >= hmin+i-1;
    points = ind(I);
    d = kNNdist(points);
    J = d < mean(d)+inputs.filter.nsigma*std(d);
    points = points(J);
    F(points) = 1;
  end
  ind = ind(F);
  disp(['  Points removed as statistical outliers:  ',num2str(np-length(ind))])
end

%% Statistical point density filtering
if inputs.filter.radius > 0
  Q = P(ind,:);
  np = size(Q,1);

  % Partition the point cloud into cubes
  [partition,CC] = cubical_partition(Q,inputs.filter.radius);

  % Determine the number of points inside a ball for each point
  NumOfPoints = zeros(np,1);
  r1 = inputs.filter.radius^2;
  for i = 1:np
    if NumOfPoints(i) == 0
      points = partition(CC(i,1)-1:CC(i,1)+1,CC(i,2)-1:CC(i,2)+1,CC(i,3)-1:CC(i,3)+1);
      points = vertcat(points{:,:});
      cube = Q(points,:);
      p = partition{CC(i,1),CC(i,2),CC(i,3)};
      for j = 1:length(p)
        dist = (Q(p(j),1)-cube(:,1)).^2+(Q(p(j),2)-cube(:,2)).^2+(Q(p(j),3)-cube(:,3)).^2;
        J = dist < r1;
        NumOfPoints(p(j)) = nnz(J);
      end
    end
  end

  % Change the threshold point density according the average and standard
  % deviation for every vertical layer of 1 meter in height
  hmin = min(Q(:,3));
  hmax = max(Q(:,3));
  H = ceil(hmax-hmin);
  F = false(np,1);
  ind = (1:1:np)';
  for i = 1:H
    I = Q(:,3) < hmin+i & Q(:,3) >= hmin+i-1;
    points = ind(I);
    N = NumOfPoints(points);
    J = N > mean(N)-inputs.filter.nsigma*std(N);
    points = points(J);
    F(points) = 1;
  end
  ind = ind(F);
  disp(['  Points removed as statistical outliers:  ',num2str(np-length(ind))])
end

%% Small component filtering
if inputs.filter.ncomp > 0
  % Cover the point cloud with patches
  input.BallRad1 = inputs.filter.BallRad1;
  input.PatchDiam1 = inputs.filter.PatchDiam1;
  input.nmin1 = 0;
  Q = P(ind,:);
  np = size(Q,1);
  cover = cover_sets(Q,input);

  % Determine the separate components
  Components = connected_components(cover.neighbor,0,inputs.filter.ncomp);

  % The filtering
  B = vertcat(Components{:}); % patches in the components
  points = vertcat(cover.ball{B}); % points in the components
  F = false(np,1);
  F(points) = true;
  ind = ind(F);
  disp(['  Points with small components removed:  ',num2str(np-length(ind))])
end

%% Cubical downsampling
if inputs.filter.EdgeLength > 0
  Q = P(ind,:);
  np = size(Q,1);
  F = cubical_downsampling(Q,inputs.filter.EdgeLength);
  ind = ind(F);
  disp(['  Points removed with downsampling:  ',num2str(np-length(ind))])
end

%% Define the output and display summary results
Pass(ind) = true;
np = nnz(Pass);
disp(['  Points removed in total: ',num2str(np0-np)])
disp(['  Points removed in total (%): ',num2str(round((1-np/np0)*1000)/10)])
disp(['  Points left: ',num2str(np)])

%% Plot the filtered and unfiltered point clouds
if inputs.filter.plot
  plot_comparison(P(Pass,:),P(~Pass,:),1,1,1)
  plot_point_cloud(P(Pass,:),2,1)
end


function [gamma,phase,dta,alpha,xmesh_lv,ymesh_lv,zmesh_lv]=dicomrt_GAMMAcal2D(evalm,refm,dose_xmesh,dose_ymesh,dose_zmesh,slice,resf,range,dta_criteria,dd_criteria,voi,voiselect,pbopt)
% dicomrt_GAMMAcal2D(evalm,refm,dose_xmesh,dose_ymesh,dose_zmesh,slice,resf,range,dta_criteria,dd_criteria,voi,voiselect,pbopt);
%
% Calculates 2D gamma distribution for a 2D dataset using a 2D algorithm.
%
% evalm is the 2D matrix to evaluate.
% refm is the reference 2D matrix.
%   Both eval and ref can be a TPS generated dataset or a MC generated dataset.
%   Doses are normalised to the target dose whenever is possible.
%   If no normalization dose is provided within the data it is assumed that matrices are already normalised.
% dose_xmesh, dose_ymesh, are the coordinates of the center of the pixels for eval and ref.
% slice is the number of the slice on which gamma should be calculated
% resf is the "resolution factor". Each voxel is divided resf times to allow dose to be interpolated
%   and quantities calculated. The higher resf the more accurate the calculation is, the slower the
%   function will be.
% range is the "search range". Range is the number of pixel about (j,i) that is considered for calculation.
%   If a dta or a dd match is not found within the search range gamma and all the other quantities in that point
%   will be set to infinite value.
% dta_criteria is the Distance-to-agreement criteria in cm (e.g. 0.3).
% dd_criteria:
%   1) IF matrices ARE NOT NORMALIZED before calling the function
%      dta_criteria is the percentage dose difference (e.g. =0.03);
%   2) IF matrices ARE already NORMALIZED before calling the function
%      dd_criteria must be given accordingly to the normalization applyed
%      (e.g. =0.03 for 3% over dose norm =1Gy, or =1.98 for 3% over dose norm=66Gy).
% voi and voiselect are the vois' cell array and the # of the voi to be used for the gamma calculation respectively.
%   They have to be specified together. Both matrices will be masked and reduced in size accordingly with the selected
%   voi's dimensions. This reduce calculation time especially for the 3D algorithm.
% pbopt progress bar option (OPTIONAL): =0 (default) no progress bar is displayed, ~0 progress bar is displayed
%
% Example:
%
% [gamma1_2,phase1_2,dta1_2,alpha1_2,xmesh,ymesh]=dicomrt_GAMMAcal2D(image_eval,image_ref, ...
%                                                             xmesh_red,ymesh_red,40,2,4,0.3,0.03);
%
% returns the calculated gamma function for image_eval vs image_ref at slice # 40 in gamma1_2.
% Gamma is calculated with 3%-3mm DD-DTA criteria.
% The phase is returned in phase1_2, the dta matrix in dta1_2.
% The function return also the direction cosines in alpha1_2. These are the
% direction cosines on the gamma vector in the two dimensional space xy. The direction
% cosines may provide useful information in detecting systematic spatial displacements between eval and ref.
% Coordinates of the xy location where gamma is defined are returned in xmesh and ymesh.
%
% The concept of gamma function was developed by Low et al Med. Phys. 25 656-661.
%
% See also dicomrt_gvhcal, dicomrt_gahcal
%
% Copyright (C) 2002 Emiliano Spezi (emiliano.spezi@physics.org)

% Check number of argument and set-up some parameters and variables
error(nargchk(10,13,nargin))

if nargin >10 & (exist('voi')==1 & exist('voiselect')~=1) | (exist('voi')~=1 & exist('voiselect')==1)
    error('dicomrt_GAMMAcal2D: Check VOI and voi2plot: they must be both present or both absent. Exit now!');
end

if exist('pbopt')~=1
    pbopt=0;
end

% Suppress warnings
warning off MATLAB:divideByZero

% Check case and set-up some parameters and variables
[eval_temp,type_dose_one,label1,PatientPosition]=dicomrt_checkinput(evalm);
[ref_temp,type_dose_two,label2,PatientPosition]=dicomrt_checkinput(refm);
local_eval=dicomrt_varfilter(eval_temp);
ref=dicomrt_varfilter(ref_temp);

% Get DICOM-RT toolbox dataset info
local_eval_pointer=eval_temp{1,1};
local_eval_header=local_eval_pointer{1};
ref_pointer=ref_temp{1,1};
ref_header=ref_pointer{1};

[voi_temp]=dicomrt_checkinput(voi);
voi=dicomrt_varfilter(voi_temp);

if strcmpi(type_dose_one,'rtplan')==1 & (strcmpi(type_dose_two,'mc')==1 | strcmpi(type_dose_two,'unknown')==1)
    targetdose_one=getfield(local_eval_header.DoseReferenceSequence,'Item_1','TargetPrescriptionDose'); % normalisation factor
    % Mask arrays
    temp=ref;
    % temp(find(local_eval==0))=0;
    % local_eval(find(temp==0))=0;
    % Normalise dose and calculate dose difference
    eval_norm_temp=local_eval(:,:,slice)/targetdose_one*100;
    ref_norm_temp=temp(:,:,slice)/targetdose_one*100;
elseif strcmpi(type_dose_two,'rtplan')==1 & (strcmpi(type_dose_one,'mc')==1 | strcmpi(type_dose_one,'unknown')==1)
    targetdose_two=getfield(ref{1,1}.DoseReferenceSequence,'Item_1','TargetPrescriptionDose'); % normalisation factor
    % Mask arrays
    temp=local_eval;
    % temp(find(ref==0))=0;
    % ref(find(temp==0))=0;
    % Normalise dose and calculate dose difference
    eval_norm_temp=temp(:,:,slice)/targetdose_two*100;
    ref_norm_temp=ref(:,:,slice)/targetdose_two*100;
elseif strcmpi(type_dose_one,'rtplan')==1 & strcmpi(type_dose_two,'rtplan')==1
    targetdose_one=getfield(local_eval_header.DoseReferenceSequence,'Item_1','TargetPrescriptionDose'); % normalisation factor
    targetdose_two=getfield(ref_header.DoseReferenceSequence,'Item_1','TargetPrescriptionDose'); % normalisation factor
    % no mask is performed. We assume TPS dose are calculated on the same patient using the same patient outline outline
    % Normalise dose and calculate dose difference
    eval_norm_temp=local_eval(:,:,slice)/targetdose_one*100;
    ref_norm_temp=ref(:,:,slice)/targetdose_two*100;
elseif (strcmpi(type_dose_one,'mc')==1 | strcmpi(type_dose_one,'unknown')==1) & ...
        (strcmpi(type_dose_one,'mc')==1 | strcmpi(type_dose_two,'unknown')==1)
    eval_norm_temp=local_eval(:,:,slice);
    ref_norm_temp=ref(:,:,slice);
else
    error('dicomrt_GAMMAcal2D: Cannot determine dose arrays format. Exit now!');
end

% Mask matrices as appropriate
if exist('voi')==1 & exist('voiselect')==1
    [locate_voi_min_x,locate_voi_max_x,locate_voi_min_y,locate_voi_max_y] = dicomrt_voiboundaries(...
        dose_xmesh,dose_ymesh,dose_zmesh,voi_temp,voiselect,PatientPosition);
    eval_norm=eval_norm_temp(locate_voi_min_y:locate_voi_max_y,locate_voi_min_x:locate_voi_max_x);
    ref_norm=ref_norm_temp(locate_voi_min_y:locate_voi_max_y,locate_voi_min_x:locate_voi_max_x);
else
    locate_voi_min_x=1;
    locate_voi_max_x=length(dose_xmesh);
    locate_voi_min_y=1;
    locate_voi_max_y=length(dose_ymesh);
    eval_norm=eval_norm_temp;
    ref_norm=ref_norm_temp;
end

%
% Define parameters
% radians to degrees conversion factor
r2d=360/(2*pi);
d2r=2*pi/360;

% Retrieve basic information
pixel_spacing_x=dicomrt_mmdigit(dose_xmesh(2)-dose_xmesh(1),7);
pixel_spacing_y=dicomrt_mmdigit(dose_ymesh(2)-dose_ymesh(1),7);

% 2D algorithm
%
% create grid for matrix interpolation
% this is done using the resolution factor resf
%[xmesh,ymesh]=dicomrt_build2dgrid(dose_xmesh-pixel_spacing_x/resf,dose_ymesh-pixel_spacing_y/resf);

% Start calculating elapsed time
% tic

disp('(+) Initializing ...');

[xmesh,ymesh]=dicomrt_build2dgrid(dose_xmesh(locate_voi_min_x:locate_voi_max_x),...
    dose_ymesh(locate_voi_min_y:locate_voi_max_y));
xmesh_res=imresize(xmesh,resf,'bilinear');
ymesh_res=imresize(ymesh,resf,'bilinear');

% These variables are exported to use with other functions
xmesh_lv=xmesh(1,:);
ymesh_lv=ymesh(:,1);
zmesh_lv=dose_zmesh(slice);

% Normalize quantities
eval_norm=eval_norm./dd_criteria;
ref_norm=ref_norm./dd_criteria;
xmesh=xmesh./dta_criteria;
ymesh=ymesh./dta_criteria;
xmesh_res=xmesh_res./dta_criteria;
ymesh_res=ymesh_res./dta_criteria;

% interpolate reference matrix
eval_norm_interp(:,:)=interp2(xmesh,ymesh,eval_norm(:,:),xmesh_res,ymesh_res);

% Define output size
gamma=zeros(size(ref_norm));
phase=zeros(size(ref_norm));
dta=zeros(size(ref_norm));
alpha=zeros(size(ref_norm));

% logical steps being taken:
%
% 0) So far the reference matrix was modified to comply with the resolution parameter resf input by the user.
%    Two new matrices which incorporate the dose difference criteria (dd_criteria) were also built.
% 1) The search_range parameter will be used now to define an area around each voxel of the new reference matrices.
%    This area is called Search Area (SA) and will be temporary stored onto a matrix called Transit Area (TA).
%    Distance to agreemenet (DTA) will be then searched within TA for each point of the evaluation matrix.
% 2) If one or more DTA matches are found the dose difference will be calculated for the minimum of the DTA matches.
%
% 1a) Calculate voxels that will define the search volume in 3D

disp('(+) Calculating ...');

if pbopt~=0
    h = waitbar(0,'Calculation progress');
    set(h,'Name','dicomrt_GAMMAcal2D: calculates gamma function');
end

for j=1:size(ref_norm,1)       % loop over y
    for i=1:size(ref_norm,2)   % loop over x
        % all voxels in the new matrices are used for the DTA search. A portion of the new matrices will be copied
        % in the temporary matrix called volume. In order to do this we need to calculate the index that refer to
        % the portion of the matrix to copy.
        if i==1
            iSAmax=range*resf+resf;
            iSAmin=1;
        elseif i>1 & i< range +1
            iSAmax=resf*i+range*resf;
            iSAmin=1;
        elseif i>=range +1 & i<size(ref_norm,2)-range
            iSAmax=resf*i+range*resf;
            iSAmin=i*resf-(resf-1)-range*resf;
        elseif i>=size(ref_norm,2)-range
            iSAmax=size(eval_norm_interp,2);
            iSAmin=i*resf-(resf-1)-range*resf;
        end

        if j==1
            jSAmax=range*resf+resf;
            jSAmin=1;
        elseif j>1 & j< range +1
            jSAmax=resf*j+range*resf;
            jSAmin=1;
        elseif j>=range +1 & j<size(ref_norm,1)-range
            jSAmax=resf*j+range*resf;
            jSAmin=j*resf-(resf-1)-range*resf;
        elseif j>=size(ref_norm,1)-range
            jSAmax=size(eval_norm_interp,1);
            jSAmin=j*resf-(resf-1)-range*resf;
        end

        % debug start
        %display(['i is: ',num2str(i),' - j is: ', num2str(j)]);
        %display(['iSAmin is: ',num2str(iSAmin),' - iSAmax is: ', num2str(iSAmax)]);
        %display(['jSAmin is: ',num2str(jSAmin),' - jSAmax is: ', num2str(jSAmax)]);
        %if j==64 & i==22
        %    disp('debug');
        %end

        temp_xmesh_res=xmesh_res(jSAmin:jSAmax,iSAmin:iSAmax);
        temp_ymesh_res=ymesh_res(jSAmin:jSAmax,iSAmin:iSAmax);
        temp_eval_norm=eval_norm_interp(jSAmin:jSAmax,iSAmin:iSAmax);

        temp_dta=zeros(size(temp_eval_norm));
        %temp_dta(:,:)=nan;
        temp_dta(:,:)=inf;

        temp_dd=zeros(size(temp_eval_norm));
        %temp_dd(:,:)=nan;
        temp_dd(:,:)=inf;

        temp_gamma=zeros(size(temp_eval_norm));
        %temp_gamma(:,:)=nan;
        temp_gamma(:,:)=inf;

        dd_spot=eval_norm(j,i)-ref_norm(j,i);

        [lo]=find(temp_eval_norm<=ref_norm(j,i)+1 & temp_eval_norm>ref_norm(j,i)-1);

        % initialize variable: lo will contain the location (number) of the pixel
        % where the match was found. Pixel numbering is done following the example
        % below for a 2d matrix:
        %
        % A=
        %
        % 1 1 1   1|  /|  /|
        % 2 2 2    | / | / |
        % 1 3 1    |/  |/  |9
        %
        % 6 is the number of the pixel which contains the value 3 and it is the
        % result of the following command:
        %
        % find(A(:,:)==3)

        if isempty(lo)~=1
            temp_dta(lo)=sqrt((temp_xmesh_res(lo)-xmesh(j,i)).^2+(temp_ymesh_res(lo)-ymesh(j,i)).^2);
            temp_dd(lo)=temp_eval_norm(lo)-ref_norm(j,i);
            temp_gamma(lo)=sqrt(temp_dta(lo).^2+temp_dd(lo).^2);
            [temp_gamma_min,temp_gamma_min_index]=min(temp_gamma(lo));
            [gamma(j,i),index]=min([temp_gamma_min sqrt(dd_spot.^2)]);
            % the following if is due because dicomrt_mask, which is used to calculate DVHs GVHs and GAHs,
            % mask matrices to zero. This makes impossible to disguish between a point outside the VOI from
            % a point with dose or gamma equal to 0.
            % In practice this is not a problem for DVHs but can represent a problem for gamma
            % calculation. A value 0 or 0.001 do not change the meaning of gamma.
            %
            if gamma(j,i)==0
                gamma(j,i)=0.01;
            end
            %
            if index==2
                phase(j,i)=0;
                dta(j,i)=0;
                alpha(j,i)=0;
            else
                phase(j,i)=asin(temp_dd(lo(temp_gamma_min_index))./gamma(j,i))*r2d;
                dta(j,i)=temp_dta(lo(temp_gamma_min_index));
                alpha(j,i)=acos(((temp_xmesh_res(lo(temp_gamma_min_index))-xmesh(j,i))./dta(j,i)))*r2d;
                %alpha(j,i)=acos((temp_xmesh_res(lo(temp_gamma_min_index))-xmesh(j,i))/...
                %    (gamma(j,i)*cos(d2r*phase(j,i))))*r2d;
                %alpha(j,i)=acos((temp_xmesh_res(lo(temp_gamma_min_index))-xmesh(j,i))/...
                %    temp_dta(lo(temp_gamma_min_index)))*r2d;
                %alpha(j,i)=acos(temp_dta(lo(temp_gamma_min_index))/(gamma(j,i)*cos(d2r*phase(j,i))))*r2d;
                %alpha(j,i)=acos((temp_xmesh_res(lo(temp_gamma_min_index))-xmesh(j,i))/...
                %    (gamma(j,i)*cos(phase(j,i))))*r2d;
                %alpha(j,i)=asin(temp_dd(lo(temp_gamma_min_index))*sin(phase(j,i)*d2r))*r2d;
            end
        else
            %gamma(j,i)=nan;
            gamma(j,i)=inf;
            %phase(j,i)=nan;
            phase(j,i)=inf;
            %dta(j,i)=nan;
            dta(j,i)=inf;
            %alpha(j,i)=nan;
            alpha(j,i)=inf;
        end
        if pbopt~=0
            waitbar(j/size(ref_norm,1),h);
        end
    end
end
disp('(=) Calculation completed.');
if pbopt~=0
    close(h)
end

% Returns elapsed time
% toc

% Plot
%figure;
%set(gcf,'Name',['dicomrt_GAMMAcal2D: local_eval= ',inputname(1),', ref= ',inputname(2)]);
%surf(gamma,phase,'XData',dose_xmesh(locate_voi_min_x:locate_voi_max_x),'Ydata',dose_ymesh(locate_voi_min_y:locate_voi_max_y));
%colormap jet;
%shading interp;
%title(['gamma/phase',' Z= ',num2str(dose_zmesh(slice))],'FontSize',16);
%xlabel('X axis (cm)','FontSize',12);
%ylabel('Y axis (cm)','FontSize',12);
%grid on;
%colorbar;
%set(gca,'XLim',[min(dose_xmesh(locate_voi_min_x:locate_voi_max_x)) max(dose_xmesh(locate_voi_min_x:locate_voi_max_x))]);
%set(gca,'YLim',[min(dose_ymesh(locate_voi_min_y:locate_voi_max_y)) max(dose_ymesh(locate_voi_min_y:locate_voi_max_y))]);
%set(gca,'ZLim',[min(min(gamma)) max(max(gamma))]);
%set(gca,'ZLim',[min(min(gamma)) 1]);

%figure;
%set(gcf,'Name',['dicomrt_GAMMAcal2D: local_eval= ',inputname(1),', ref= ',inputname(2)]);
%surf(gamma,alpha,'XData',dose_xmesh(locate_voi_min_x:locate_voi_max_x),'Ydata',dose_ymesh(locate_voi_min_y:locate_voi_max_y));
%colormap jet;
%shading interp;
%title(['gamma/alpha',' Z= ',num2str(dose_zmesh(slice))],'FontSize',16);
%xlabel('X axis (cm)','FontSize',12);
%ylabel('Y axis (cm)','FontSize',12);
%grid on;
%colorbar;
%set(gca,'XLim',[min(dose_xmesh(locate_voi_min_x:locate_voi_max_x)) max(dose_xmesh(locate_voi_min_x:locate_voi_max_x))]);
%set(gca,'YLim',[min(dose_ymesh(locate_voi_min_y:locate_voi_max_y)) max(dose_ymesh(locate_voi_min_y:locate_voi_max_y))]);
%set(gca,'ZLim',[min(min(gamma)) max(max(gamma))]);
%set(gca,'ZLim',[min(min(gamma)) 1]);


function [noiseVariance, debiasedNoiseVariance, IQEstimate, noiseEstimateOomen] = realized_noise_estimate(price, time, timeType, options)
% Estimation of the optimal bandwidth to use when estimating the quadratic variation using a Realized Kernel
%
% USAGE:
%   [NOISEVARIANCE,DEBIASEDNOISEVARIANCE,IQESIQESTIMATETIMATE,NOISEVARIANCEOOMEN]
%                        = realized_kernel_select_bandwidth(PRICE,TIME,TIMETYPE,OPTIONS)
%
% INPUTS:
%   PRICE       - m by 1 vector of prices
%   TIME        - m by 1 vector of times where TIME(i) corresponds to PRICE(i)
%   TIMETYPE    - String describing the way times are measured
%               'wall'    24-hour clock of the form HHMMSS, e.g. 101543 or 153217
%               'seconds' Time measured in seconds past midnight on the first day
%               'unit'  Unit normalized date format, e.g. .1, .234, .9. Unit normalized times are
%                 more general than the other types and can be applied to data from more than one
%                 calendar day
%   OPTIONS     - Realized kernel options structure.  See help realized_kernel_options
%
% OUTPUTS:
%   NOISEVARIANCE         - Bandi-Russel noise variance estimate
%   DEBIASEDNOISEVARIANCE - Debiased Bandi-Russel noise variance estimate using the adjustment of BNHLS
%   IQESTIMATE            - An estimate of the lower bound of the IQ based on low-freuqency returns
%   NOISEVARIANCEOOMEN    - Estimate using Oomen (2006) alternative AC(1) estimator
%
% COMMENTS:
%   This is a helper function for REALIZED_KERNEL.  See Barndorf-Nielsen, Hansen, Lunde and Shephard
%   (2008) for details about the optimal selection of bandwidth
%
%  See also REALIZED_KERNEL, REALIZED_PRICE_FILTER, REALIZED_KERNEL_WEIGHTS
%  REALIZED_KERNEL_SELECT_LAG_LENGTH, REALIZED_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 5/1/2008

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin~=4
    error('Seven inputs required.')
end
if size(price,2)>size(price,1)
    price=price';
end
if size(price,2)>1
    error('PRICE must be a m by 1 vector.')
end
if size(time,2)>size(time,1)
    time=time';
end
if any(diff(time)<0)
    error('TIME must be sorted and increasing')
end
if size(time,2)>1 || length(time)~=length(price)
    error('TIME must be a m by 1 vector.')
end
time = double(time);
timeType=lower(timeType);
if ~ismember(timeType,{'wall','seconds','matlab'})
    error('TIMETYPE must be one of ''wall'', ''seconds'' or ''Matlab''.');
end

% List of flat top kernels
flatTopKernelList = {'bartlett','twoscale','2ndorder','epanechnikov',...
    'cubic','multiscale','5thorder','6thorder','7thorder','8thorder','parzen',...
    'th1','th2','th5','th16'};

% List of non flat top kernels
nonFlatTopKernelList = {'nonflatparzen','qs','fejer','thinf','bnhls'};

% Combined kernel list
kernelList = [flatTopKernelList nonFlatTopKernelList];

if ~isfield(options,'medFrequencyKernel') || ~ismember(options.medFrequencyKernel,kernelList)
    error('KERNEL must be a field of OPTIONS and one of the listed types.')
end

medFrequencySamplingType = options.medFrequencySamplingType;
medFrequencySamplingInterval = options.medFrequencySamplingInterval;
medFrequencyKernel = options.medFrequencyKernel;
medFrequencyBandwidth = options.medFrequencyBandwidth;
if ~isscalar(medFrequencySamplingInterval) || floor(medFrequencySamplingInterval)~=medFrequencySamplingInterval || medFrequencySamplingInterval<=0
    error('MEDIUMFREQUENCYTIME must be a postive integer scalar value.');
end

IQEstimationSamplingType = options.IQEstimationSamplingType;
IQEstimationSamplingInterval = options.IQEstimationSamplingInterval;
if ~isscalar(IQEstimationSamplingInterval) || floor(IQEstimationSamplingInterval)~=IQEstimationSamplingInterval || IQEstimationSamplingInterval<=0
    error('LOWFREQUENCYTIME must be a postive integer scalar value.');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% To compute the "optimal" bandwidth an estimate of the noise and the QV using low frequency data
lowFrequencyRealizedVariance = realized_variance(price,time, timeType , IQEstimationSamplingType, IQEstimationSamplingInterval);

noiseVarianceSamplingType = options.noiseVarianceSamplingType;
noiseVarianceSamplingInterval = options.noiseVarianceSamplingInterval;

% Compute the noiseVariance using the debiased version of Bandi-Russell, using all prices
noiseVariance = realized_variance(price, time, timeType , noiseVarianceSamplingType , noiseVarianceSamplingInterval);
% Compute the effective 'n' to use in the Bandi-Russel estimator
noiseFilteredPrice = realized_price_filter(price, time, timeType , noiseVarianceSamplingType , noiseVarianceSamplingInterval);
if options.useAdjustedNoiseCount
    % Only count the number of non-zero returns
    n = sum(diff(noiseFilteredPrice)~=0);
else
    n = length(noiseFilteredPrice) - 1;
end
noiseVariance = noiseVariance/(2*n);

noiseReturns = diff(log(noiseFilteredPrice));
n = length(noiseReturns);
noiseEstimateOomen = -1/(n-1) * noiseReturns(1:end-1)'*noiseReturns(2:end);

% Require a realized kernel estimate to adjust the variance
medFrequencyOptions = realized_options('kernel');
medFrequencyOptions.kernel = medFrequencyKernel;
medFrequencyOptions.bandwidth = medFrequencyBandwidth;
medFrequencyOptions.endTreatment = 'stagger';

medFreuqencyRealizedKernel = realized_kernel(price, time, timeType, medFrequencySamplingType, medFrequencySamplingInterval, medFrequencyOptions);
medFrequencyRealizedVariance = realized_variance(price,time, timeType , medFrequencySamplingType, medFrequencySamplingInterval);
% Use the BNHLS corrected noise estimate
debiasedNoiseVariance = exp( log(noiseVariance) - medFreuqencyRealizedKernel/medFrequencyRealizedVariance);
IQEstimate = lowFrequencyRealizedVariance^2;


function [muX,SigmaX,suffStat] = VBA_EKF(y,u,posterior,dim,options,flag)
% standard EKF & computation of the predictive density
% function [muX,SigmaX,suffStat] = VBA_EKF(y,u,posterior,dim,options,flag)
%
% This function inverts any nonlinear state-space model of the form:
%   y_t = g( x_t,u_t,phi ) + e_t
%   x_t+1 = f( x_t,u_t,theta ) + f_t
% using a standard extended Kalman filter (EKF).
%
% IN :          [ see VBA_NLStateSpaceModel.m ]
%   - y: pxn_t mesurements matrix
%   - u: mxn_t known input matrix (which is required as an argument in
%   the obvservation/evolution functions)
%
%   - posterior: posterior pdf structure (see VBA_NLStateSpaceModel.m).
%   This is used to extract first and second order parameters of the
%   generative model (theta, phi, alpha, sigma), as well as the initial
%   conditions of the hidden states (X0).
%   - dim: a structure variable containing the dimensions of the 3 sets of
%   model unknown variables (see VBA_NLStateSpaceModel.m)
%   - options: user-defined structure containing specific informations
%   regarding the model, ie (see VBA_check.m):
%       .f_fname (resp. g_fname): name/handle of the function that outputs
%       the evolution (resp. observation) of the hidden states.
%       .u0: the mx1 initial value of the input {0}
%       .inF: a (possibly structure) variable containing the additional
%       (internal) fixed parameters which may have to be sent to the
%       evolution function (eg pointing to different variants) {[]}
%       .inG: idem for the observation function {[]}
%   -  flag: a switch for computing just the mode of the deterministic
%   predictive density (flag=0), the 1st and 2d order statistics of the EKF
%   ({flag=1}), or the 1st and 2d order statistics of the predictive
%   density (flag = 2).
%
% OUT:
%   - muX: posterior mean of the hidden states X (nxn_t matrix)
%   - SigmaX: covariance matrices of the variational posterior pdf of
%       the dynamic hidden-states.

if numel(options.sources) > 1
    error('*** EKF is not yet compatible with multisource observations');
end

if options.sources.type > 1
    error('*** EKF is not yet compatible with multinomial observations');
end

% By default, this function implements an EKF:
if ~exist('flag','var') || isempty(flag)
    flag = 1;
end

% This checks and fills in required dummy variables
if isempty(u)
    u = zeros(1,dim.n_t);
end
[dim.p,dim.n_t] = size(y);
try
    dim.u;
catch
    dim.u = size(u,1);
end
if isfield(options,'microU') && options.microU
    u = VBA_getU(u,options,dim,'2macro');
end
if ~isfield(options,'nout_f')
    options.nout_f = nargout(options.f_fname);
end
if ~isfield(options,'nout_g')
    options.nout_g = nargout(options.g_fname);
end
if ~isfield(options,'OnLine')
    options.OnLine = 0;
end
try
    X0 = posterior.muX0;
    SigmaX0 = posterior.SigmaX0;
catch
    X0 = zeros(dim.n,1);
    SigmaX0 = zeros(dim.n,dim.n);
end
try
    theta = posterior.muTheta;
catch
    theta = [];
end
try
    phi = posterior.muPhi;
catch
    phi = [];
end
try
    iQx = options.priors.iQx;
    iQy = options.priors.iQy;
catch
    iQx = cell(dim.n_t,1);
    iQy = cell(dim.n_t,1);
    for t= 1:dim.n_t
        iQx{t} = eye(dim.n);
        iQy{t} = eye(dim.p);
    end
end

switch flag
    case 0
        suffStat = [];
        str = 'deterministic time series';
    case {1,2}
        try
            alpha = posterior.a_alpha(end)./posterior.b_alpha(end);
            if options.sources.type == 0
                sigma = posterior.a_sigma(end)./posterior.b_sigma(end);
            end
        catch
            error('Not enough info in posterior structure!')
        end
        mStar = zeros(dim.n,dim.n_t);
        SigmaX = cell(dim.n_t,1);
        %--- Initialize sufficient statistics time-series ---%
        suffStat = VBA_getSuffStat(options);
        if isequal(flag,1)
            str = 'standard EKF';
        else
            str = 'predictive density';
        end
end
muX = zeros(dim.n,dim.n_t);
gx = zeros(dim.p,dim.n_t);

% First time iteration (from initial conditions)
if ~options.OnLine && options.verbose
    fprintf(1,['Deriving ',str,' ...'])
end
if flag>=1
    %--- Prediction
    [fx0,dF_dX0] = VBA_evalFun('f',X0,theta,u(:,1),options,dim,1);
    mStar(:,1) = fx0;
    Rp = dF_dX0'*SigmaX0*dF_dX0 + 1./alpha.*VBA_inv(iQx{1});
    if flag == 1 % EKF update
        [gx(:,1),dG_dX] = VBA_evalFun('g',mStar(:,1),phi,u(:,1),options,dim,1);
        iRp = pinv(Rp);
        C =  dG_dX*iQy{1}*dG_dX';
        iSX = iRp + sigma*C;
        SigmaX{1} = pinv( iSX );
        muX(:,1) = mStar(:,1) + sigma.*SigmaX{1}*dG_dX*iQy{1}* (y(:,1)-gx(:,1));
    else % Predictive density
        muX(:,1) = mStar(:,1);
        SigmaX{1} = Rp;
    end
    % get predicted observation at the mode
    [gx(:,1),dG_dX] = VBA_evalFun('g',muX(:,1),phi,u(:,1),options,dim,1);
    suffStat.dy(:,1) = y(:,1) - gx(:,1);
    if options.sources.type == 0
        suffStat.vy(:,1) = diag( sigma.^-1.*pinv(iQy{1}) + dG_dX'*SigmaX{1}*dG_dX );
        suffStat.dy2 = suffStat.dy2 + suffStat.dy(:,1)'*iQy{1}*suffStat.dy(:,1);
    else
        suffStat.vy(:,1) = gx(:,1).*(1-gx(:,1));
        suffStat.logL = y(:,1)'*log(gx(:,1)) + (1-y(:,1))'*log(1-gx(:,1));
    end
    suffStat.dx(:,1) = muX(:,1) - fx0;
    suffStat.dx2 = suffStat.dx2 + suffStat.dx(:,1)'*iQx{1}*suffStat.dx(:,1);
else
    muX(:,1) = VBA_evalFun('f',X0,theta,u(:,1),options,dim);
end

% Loop over time samples
if ~options.OnLine && options.verbose
    fprintf(1,'%6.2f %%',0)
end
for t = 1:dim.n_t-1
    if flag >= 1
        %-- Prediction
        [fx,dF_dX] = VBA_evalFun('f',muX(:,t),theta,u(:,t+1),options,dim,t+1);
        mStar(:,t+1) = fx;
        Rp = dF_dX'*SigmaX{t}*dF_dX + 1./alpha.*VBA_inv(iQx{t+1});
        if flag == 1    % EKF update
            [gx(:,t+1),dG_dX] = VBA_evalFun('g',mStar(:,t+1),phi,u(:,t+1),options,dim,t+1);
            C =  dG_dX*iQy{t+1}*dG_dX';
            iRp = pinv(Rp);
            iSX = iRp + sigma*C;
            SigmaX{t+1} = pinv( iSX );
            muX(:,t+1) = mStar(:,t+1) + sigma.*SigmaX{t+1}*dG_dX*iQy{t+1}* (y(:,t+1)-gx(:,t+1));
        else
            muX(:,t+1) = mStar(:,t+1);
            SigmaX{t+1} = Rp;
        end
        % get predicted observation at the mode
        [gx(:,t+1),dG_dX] = VBA_evalFun('g',muX(:,t+1),phi,u(:,t+1),options,dim,t+1);
        suffStat.dy(:,t+1) = y(:,t+1) - gx(:,t+1);
        if options.sources.type == 0
            suffStat.vy(:,t+1) = diag( sigma.^-1.*pinv(iQy{t+1}) + dG_dX'*SigmaX{t+1}*dG_dX );
            suffStat.dy2 = suffStat.dy2 + suffStat.dy(:,t+1)'*iQy{t+1}*suffStat.dy(:,t+1);
        else
            suffStat.vy(:,t+1) = gx(:,t+1).*(1-gx(:,t+1));
            suffStat.logL = suffStat.logL + y(:,t+1)'*log(gx(:,t+1)) + (1-y(:,t+1))'*log(1-gx(:,t+1));
        end
        suffStat.dx(:,t+1) = muX(:,t+1) - fx;
        suffStat.dx2 = suffStat.dx2 + suffStat.dx(:,t+1)'*iQx{t+1}*suffStat.dx(:,t+1);
    else
        muX(:,t+1) = VBA_evalFun('f',muX(:,t),theta,u(:,t+1),options,dim);
    end
    if ~options.OnLine && isequal(mod(t,32),0) && options.verbose
        fprintf(1,repmat('\b',1,8))
        fprintf(1,'%6.2f %%',100*t/dim.n_t)
    end
end
if ~options.OnLine  && options.verbose
    fprintf(1,repmat('\b',1,8))
    fprintf(' OK.')
    fprintf('\n')
end
if flag >= 1
    suffStat.gx = gx;
end