%% Function List
%
%% ARMAX-GARCH Toolbox
%
% <matlab:web(fullfile('garch.m')) garch.m>, estimates the ARMAX-GARCH family of models
%
% Inputs: a data vector, the GARCH model to be estimated, the distribution, the order of AR, MA, ARCH and GARCH effects as well as a vector of factors for the mean and volatility process.
%
% Outputs: a vector of estimated parameters, standard errors estimated by the inverse Hessian, the Log-Likelihood value, a vector of conditional variances, a vector of residuals, and a summary of results which includes: model statistics, t-statistics, robust standard errors, scores among others.
%
% <matlab:web(fullfile('garchfind.m')) garchfind>, finds the combination of models and distributions that better fits the data based on a set of criteria (i.e. largest log likelihood value and the smallest AIC and BIC criteria).
%
% Inputs: a data vector, a model and distribution vectors, the order of AR, MA, ARCH and GARCH,
%
% Outputs: the best fit model
%
% % <matlab:web(fullfile('garchplot.m')) garchplot>, Plots the data,volatility and residuals
%
% <matlab:web(fullfile('garchfor.m')) garchfor> & <matlab:web(fullfile('garchfor2.m')) garchfor2>, estimates mean, volatility and kurtosis forecasts
%
% Inputs: a data vector, a vector of residuals, a vector of conditional variances, a set of parameters, the  model, the order of AR, MA, ARCH and GARCH and the number of forecasts
%
% Outputs: a vector of mean, and volatility forecasts as well as a vector of cumulative mean and volatility forecasts.
%
% <matlab:web(fullfile('garchsim.m')) garchsim>, simulates GARCH responses
%
% Inputs: estimated parameters, GARCH model, distribution, order of ARCH and GARCH, number of samples and number of paths. Additionally, a vector of time series of positive pre-sample conditional standard deviations may be provided, which the variance model will initialize.
%
% Outputs: a vector of simulated series and conditional standard deviations
%
% <matlab:web(fullfile('garchvar.m')) garchvar> & <matlab:web(fullfile('garchvar2.m')) garchvar2>, estimates  Value-at-Risk for a given confidence level and horizon period for both long and short positions
%
% Inputs: a data vector, a vector of residuals, a vector of conditional% variances, a set of parameters, the  model, the distribution, the order of AR, MA, ARCH and GARCH, the number of forecasts and confidence level
%
% Outputs: a vector of VaR forecasts
%
% <matlab:web(fullfile('garchvolfor.m')) garchvolfor>, an application in Volatility Forecasting & Value-at-Risk. It allows the comparison of volatility and Value-at-Risk estimates for a data vector and for a variety of GARCH models and distributions as specified in the model & distribution variables and at different forecast periods as well as sort the results according to only a sub-set of forecast periods.
%
% Inputs: a data vector, a vector of models and distributions, order of AR, MA, ARCH and GARCH, max forecasts, the forecasts of interest and a vector of a% VaR losses
%
% Outputs:  volatility and VaR back-testing results, vectors of forecasted estimated of volatility, VaR and returns
%
%% ARMAX-GARCH-K Toolbox
%
% <matlab:web(fullfile('garchk.m')) garchk>, estimates the ARMAX-GARCH-K family of models
%
% Inputs: a data vector, the GARCH model to be estimated, the order of AR, MA, ARCH and GARCH effects as well as a vector of factors for the mean and volatility process.
%
% Outputs: a vector of estimated parameters, standard errors estimated by the inverse Hessian, the Log-Likelihood value, a vector of conditional variances, a vector of conditional kurtosis, a vector of residuals, and a summary of results which includes: model statistics, t-statistics, robust standard errors, scores among others.
%
% <matlab:web(fullfile('garchkplot.m')) garchkplot>, Plots the data, conditional volatility, conditional kurtosis and residuals
%
% <matlab:web(fullfile('garchkfor.m')) garchkfor> & <matlab:web(fullfile('garchkfor2.m')) garchkfor2>, estimates mean, volatility and kurtosis forecasts
%
% Inputs: a data vector, a vector of residuals, a vector of conditional variances, a vector of conditional kurtosis, a set of parameters, the  model, the order of AR, MA, ARCH and GARCH and the number of forecasts
%
% Outputs: a vector of mean, volatility and kurtosis forecasts as well as a vector of cumulative mean and volatility forecasts.
%
% <matlab:web(fullfile('garchksim.m')) garchksim>, simulates volatility and kurtosis responses
%
% Inputs: estimated parameters, GARCH model, distribution, order of ARCH and GARCH, number of samples and number of paths. Additionally, a vector of time series of positive pre-sample conditional standard deviations may be provided, which the variance model will initialize.
%
% Outputs: a vector of simulated series, conditional standard deviations and conditional kurtosis
%
% <matlab:web(fullfile('garchkvar.m')) garchkvar> & <matlab:web(fullfile('garchkvar2.m')) garchkvar2>, estimates  Value-at-Risk for a given confidence level and horizon period for both long and short positions
%
% Inputs: a data vector, a vector of residuals, a vector of conditional variances, a set of parameters, the  model, the order of AR, MA, ARCH and GARCH, the number of forecasts and confidence level
%
% Outputs: a vector of VaR forecasts
%
% <..\readme\readme.html Return to Main>


function p = prior_sqinvgamma(varargin)
%PRIOR_SQINVGAMMA  Gamma prior structure for square inverse of the parameter
%
%  Description
%    P = PRIOR_SQINVGAMMA('PARAM1', VALUE1, 'PARAM2', VALUE2, ...)
%    creates Gamma prior structure for square inverse of the
%    parameter in which the named parameters have the specified
%    values. Any unspecified parameters are set to default values.
%
%    P = PRIOR_SQINVGAMMA(P, 'PARAM1', VALUE1, 'PARAM2', VALUE2, ...)
%    modify a prior structure with the named parameters altered
%    with the specified values.
%
%    Parametrisation is done by Bayesian Data Analysis,
%    second edition, Gelman et.al. 2004.
%
%    Parameters for Gamma prior [default]
%      sh       - shape [4]
%      is       - inverse scale [1]
%      sh_prior - prior for sh [prior_fixed]
%      is_prior - prior for is [prior_fixed]
%
%  See also
%    PRIOR_*

% Copyright (c) 2000-2001,2010,2012 Aki Vehtari
% Copyright (c) 2010 Jaakko Riihim�ki

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'PRIOR_SQINVGAMMA';
  ip.addOptional('p', [], @isstruct);
  ip.addParamValue('sh',4, @(x) isscalar(x) && x>0);
  ip.addParamValue('sh_prior',[], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('is',1, @(x) isscalar(x) && x>0);
  ip.addParamValue('is_prior',[], @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  p=ip.Results.p;

  if isempty(p)
    init=true;
    p.type = 'SqInv-Gamma';
  else
    if ~isfield(p,'type') && ~isequal(p.type,'SqInv-Gamma')
      error('First argument does not seem to be a valid prior structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('sh',ip.UsingDefaults)
    p.sh = ip.Results.sh;
  end
  if init || ~ismember('is',ip.UsingDefaults)
    p.is = ip.Results.is;
  end
  % Initialize prior structure
  if init
    p.p=[];
  end
  if init || ~ismember('sh_prior',ip.UsingDefaults)
    p.p.sh=ip.Results.sh_prior;
  end
  if init || ~ismember('is_prior',ip.UsingDefaults)
    p.p.is=ip.Results.is_prior;
  end

  if init
    % set functions
    p.fh.pak = @prior_sqinvgamma_pak;
    p.fh.unpak = @prior_sqinvgamma_unpak;
    p.fh.lp = @prior_sqinvgamma_lp;
    p.fh.lpg = @prior_sqinvgamma_lpg;
    p.fh.recappend = @prior_sqinvgamma_recappend;
  end

end

function [w, s] = prior_sqinvgamma_pak(p)

  w=[];
  s={};
  if ~isempty(p.p.sh)
    w = log(p.sh);
    s=[s; 'log(SqInv-Gamma.sh)'];
  end
  if ~isempty(p.p.is)
    w = [w log(p.is)];
    s=[s; 'log(SqInv-Gamma.is)'];
  end
end

function [p, w] = prior_sqinvgamma_unpak(p, w)

  if ~isempty(p.p.sh)
    i1=1;
    p.sh = exp(w(i1));
    w = w(i1+1:end);
  end
  if ~isempty(p.p.is)
    i1=1;
    p.is = exp(w(i1));
    w = w(i1+1:end);
  end
end

function lp = prior_sqinvgamma_lp(x, p)

  lJ = -log(x)*3 + log(2);  % log(-2/x^3) log(|J|) of transformation
  xt = x.^-2;               % transformation
  lp = sum(-p.is.*xt + (p.sh-1).*log(xt) +p.sh.*log(p.is)  -gammaln(p.sh) +lJ);

  if ~isempty(p.p.sh)
    lp = lp + p.p.sh.fh.lp(p.sh, p.p.sh) + log(p.sh);
  end
  if ~isempty(p.p.is)
    lp = lp + p.p.is.fh.lp(p.is, p.p.is) + log(p.is);
  end
end

function lpg = prior_sqinvgamma_lpg(x, p)

  lJg = -3./x;              % gradient of log(|J|) of transformation
  xt  = x.^-2;              % transformation
  xtg = -2/x.^3;            % derivative of transformation
  lpg = xtg.*((p.sh-1)./xt - p.is) + lJg;

  if ~isempty(p.p.sh)
    lpgsh = (sum(-digamma1(p.sh) + log(p.is) + log(x)) + p.p.sh.fh.lpg(p.sh, p.p.sh)).*p.sh + 1;
    lpg = [lpg lpgsh];
  end
  if ~isempty(p.p.is)
    lpgis = (sum(p.sh./p.is+x) + p.p.is.fh.lpg(p.is, p.p.is)).*p.is + 1;
    lpg = [lpg lpgis];
  end

end

function rec = prior_sqinvgamma_recappend(rec, ri, p)
% The parameters are not sampled in any case.
  rec = rec;
  if ~isempty(p.p.sh)
    rec.sh(ri,:) = p.sh;
  end
  if ~isempty(p.p.is)
    rec.is(ri,:) = p.is;
  end
end


classdef HolonomicConstraint < handle
    % HolonomicConstraint: represents a group of holonomic constraints of a
    % dynamical systems.
    %
    % A holonomic constraint is defined as a function of state variables
    % that equals to constants, i.e.,
    %   h(x) == hd
    %
    % @author ayonga @date 2017-04-20
    %
    % Copyright (c) 2016-2017, AMBER Lab
    % All right reserved.
    %
    % Redistribution and use in source and binary forms, with or without
    % modification, are permitted only in compliance with the BSD 3-Clause
    % license, see
    % http://www.opensource.org/licenses/bsd-license.php

    % properties determined internally
    properties (SetAccess=protected, GetAccess=public)
       % The dimension of the virtual constraints
        %
        % @type integer
        Dimension

        % The symbolic representation of constant parameter value 'hd' that
        % the holonomic constraints associated wtih.
        %
        % @type SymVariable
        Param

        % The symbolic representation of the input variables associated
        % with the holonomic constraints
        %
        % @type SymVariable
        Input
    end

    % properties must be determined by the users
    properties (SetAccess=protected, GetAccess=public)
        % The name of the virtual constraints
        %
        % @type char
        Name

        % The label of the holonomic constraint
        %
        % @type char
        ConstrLabel

        % The highest order of derivatives to enforce the holonomic
        % constraints. This is the same as the relative degree of a virtual
        % constraint.
        %
        % @type integer
        DerivativeOrder

    end

    properties (Dependent)
        % The holonomic constraint expression
        %
        % @type SymFunction
        ConstrExpr

        % The Jacobian matrix of the holonomic constraitns
        %
        % @type SymFunction
        ConstrJac

         % The Jacobian matrix of the holonomic constraitns
        %
        % @type SymFunction
        ConstrJacDot

        % The name of the parameter variable 'hd' associated with the
        % holonomic constraints
        %
        % @type char
        ParamName

        % The name othe input variables (external forces, etc.) associated
        % with the holonomic constraints
        %
        % @type char
        InputName
    end

    %% GET methods
    methods
        function cstr = get.ConstrExpr(obj)
            cstr = obj.h_;
        end
        function jac = get.ConstrJac(obj)
            jac = obj.Jh_;
        end
        function jacdot = get.ConstrJacDot(obj)
            jacdot = obj.dJh_;
        end
        function name = get.ParamName(obj)
            name = ['p' obj.Name];
        end
        function name = get.InputName(obj)
            name = ['f' obj.Name];
        end
        function name = get.h_name(obj)
            name = ['h_' obj.Name '_' obj.Model.Name];
        end

        function name = get.Jh_name(obj)
            name = ['Jh_' obj.Name '_' obj.Model.Name];
        end

        function name = get.dJh_name(obj)
            name = ['dJh_' obj.Name '_' obj.Model.Name];
        end

        function name = get.dh_name(obj)
            name = ['dh_' obj.Name '_' obj.Model.Name];
        end

        function name = get.ddh_name(obj)
            name = ['ddh_' obj.Name '_' obj.Model.Name];
        end
    end

    properties (Dependent)

        h_name

        Jh_name

        dh_name

        dJh_name

        ddh_name
    end

    properties (Access = protected)
        % The dynamical system model
        %
        % @type DynamicalSystem
        Model

        % The holonomic constraint expression
        %
        % @type SymFunction
        h_

        % The Jacobian matrix of the holonomic constraint expression
        %
        % @type SymFunction
        Jh_

        % The first order derivative of the holonomic constraint expression
        %
        % @type SymFunction
        dh_
        % The first order derivative of the Jacobian matrix of the
        % holonomic constraint expression
        %
        %
        % @type SymFunction
        dJh_

        % The second order derivatives of the holonomic constraints
        % expression
        %
        % @type SymFunction
        ddh_
    end

    methods

        function obj = HolonomicConstraint(model, h, name, varargin)
            % The class constructor function
            %
            % Parameters:
            % model: the dynamical system model in which the virtual
            % constraints are defined @type DynamicalSystem
            % h: the symbolic expression of the constraints
            % name: the name of the virtual constraints @type char
            % @type SymExpression
            % varargin: optional parameters. In details
            %  ConstrLabel: labels for constraints @type cellstr
            %  Jacobian: the custom Jacobian matrix @type SymExpression
            %  DerivativeOrder: the degree of holonomic constraints
            %  @type integer

            if nargin == 0
                return;
            end

            % validate (model) argument
            validateattributes(model, {'ContinuousDynamics'},...
                {'scalar'},...
                'HolonomicConstraint','model');
            obj.Model = model;
            x = model.States.x;

            % validates (name) argument
            validateName(obj, name);
            obj.Name = name;

            % parse the input options
            args = struct(varargin{:});
            assert(isscalar(args),...
                'The values of optional properties are must be scalar data.');

            if ~isempty(h)
                % validate (ya) argument
                validateattributes(h,{'SymExpression'},...
                    {'nonempty','vector'},...
                    'HolonomicConstraint','h');
                if isrow(h) % convert to column vector if it is a row vector
                    h = vertcat(h(:));
                end
            elseif isfield(args, 'LoadPath') && ~isempty(args.LoadPath)
                h = SymExpression([]);
                h = load(h, args.LoadPath, obj.h_name);
            else
                error(['Unable to create the HolonomicConstraint object. ',...
                    'Either the expression is empty or the load path is not specified.'],...
                    'HolonomicConstraint');
            end

            if isfield(args, 'LoadPath') && ~isempty(args.LoadPath)
                is_loaded = true;
            else
                is_loaded = false;
            end

            dim = length(h);
            obj.Dimension = dim;

            hd = SymVariable(obj.ParamName, [dim,1]);
            obj.Param = hd;
            obj.Input = SymVariable(obj.InputName,[dim,1]);

            if is_loaded % the loaded expression is h:= h_a - h_d
                obj.h_ = SymFunction(obj.h_name, h, {x, hd});
            else
                obj.h_ = SymFunction(obj.h_name, h-hd, {x, hd});
            end

            % validate and assign the desired outputs
            if isfield(args, 'ConstrLabel')
                obj.setConstrLabel(args.ConstrLabel);
            end

            if is_loaded
                Jh = SymFunction(obj.Jh_name, [], {model.States.x});
                obj.Jh_ = load(Jh,args.LoadPath);
            else
                if isfield(args, 'Jacobian')
                    obj.setJacobian(args.Jacobian);
                end
            end

            if isfield(args, 'DerivativeOrder')
                obj.setDerivativeOrder(args.DerivativeOrder);
            end

            if is_loaded
                obj.configure(args.LoadPath);
            else
                obj.configure();
            end

        end

    end

    methods

        % configure and compile the holonomic constraints
        obj = configure(obj, load_path);

        % enforce as NLP constraints
        nlp = imposeNLPConstraint(obj, nlp);

        % calculate the Jacobian matrix of the holonomic constraints
        [Jh, dJh] = calcJacobian(obj, x, dx);

        h = calcConstraint(obj, x);

        % save the symbolic expressions
        saveExpression(obj, export_path, varargin);

    end

    % set functions
    methods

        function export(obj, export_path, varargin)
            % export the symbolic expressions of the constraints matrices and
            % vectors and compile as MEX files.
            %
            % Parameters:
            %  export_path: the path to export the file @type char
            %  varargin: variable input parameters @type varargin
            %   Vars: a list of symbolic variables @type SymVariable
            %   File: the (full) file name of exported file @type char
            %   ForceExport: force the export @type logical
            %   BuildMex: flag whether to MEX the exported file @type logical
            %   Namespace: the namespace of the function @type char

            export(obj.h_,export_path, varargin{:});
            export(obj.Jh_,export_path, varargin{:});

            if ~isempty(obj.dJh_)
                export(obj.dJh_,export_path, varargin{:});
            end

        end

        % Jacobian matrix
        function obj = setJacobian(obj, jac)
            % sets the Jacobian matrix of the holonomic constraints if it
            % is provided directly by the users
            %
            % Parameters:
            % jac: the jacobian matrix @type SymExpression

            model = obj.Model;
            if nargin > 1
                validateattributes(jac,{'SymExpression'},...
                    {'2d','size',[obj.Dimension,model.numState]},...
                    'HolonomicConstraint','Jacobian');
            else
                jac = jacobian(obj.h_, model.States.x);
            end
            obj.Jh_ = SymFunction(obj.Jh_name, jac, {model.States.x});
        end

        % RelativeDegree
        function obj = setDerivativeOrder(obj, degree)
            % sets the highest derivative order of holonomic constraints in
            % order to enforce as bilateral constraints of the system
            %
            % Parameters:
            % degree: derivative order @type integer

            if degree == 1
                error('Currently we do not support J(x)dx = 0 type of holonomic constraitns.')
            end

            validateattributes(degree, {'double'},...
                {'nonempty','scalar','positive','integer','>=',1,'<=',2},...
                'HolonomicConstraint','DerivativeOrder');
            obj.DerivativeOrder = degree;

        end

        % OutputLabel
        function obj = setConstrLabel(obj, label)
            % sets the naming labels of outputs
            %
            % Parameters:
            % label: the cell array of labels @type cellstr

            validateattributes(label,{'cell'},...
                {'nonempty','numel',obj.Dimension,'row'},...
                'HolonomicConstraint','ConstrLabel');
            cellfun( @(x) validateattributes(...
                x, {'char'},{}), label);

            obj.ConstrLabel = label;
        end

        % Name
        function name = validateName(~, name)
            validateattributes(name, {'char'},...
                {'nonempty','scalartext'},...
                'HolonomicConstraint','Name');

            assert(isempty(regexp(name, '\W', 'once')) || ~isempty(regexp(name, '\$', 'once')),...
                'HolonomicConstraint:invalidSymbol', ...
                'Invalid symbol string, can NOT contain special characters.');

            assert(isempty(regexp(name, '_', 'once')),...
                'HolonomicConstraint:invalidSymbol', ...
                'Invalid symbol string, can NOT contain ''_''.');

        end
    end
end


function [dm] = directmethods(y,lags,options)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 'directmethods' generates local projeciton IRF and direct forecasts

% Core Inputs:
% - y, data columns variables
% - lags, lag order of the VAR

% Additonal Inputs collected options
% (...) see below
% See the Hitchhiker's guide for more details.
% https://github.com/naffe15/BVAR_/blob/master/HitchhikerGuide_.pdf

% Filippo Ferroni, 27/02/2020
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin < 2
    error('the BVAR toolbox needs at least two inputs: data and number of lags');
end
if lags < 1
    error('lags cannot be zero or negative');
end
% number of observable variables
[T,ny]       = size(y);

%********************************************************
%* DEFAULT SETTINGS
%********************************************************
% Control random number generator
if isOctave == 0
    isMatlab = 1;
    rng('default');
    rng(999);
else
    isMatlab = 0;
    % pkg load optim
     pkg load statistics
    randn('state',999);
    rand('state',999);
end

hor          = 24;
conf_sig     = 0.9;
controls_    = 0;
robust_se_   = 1; % by default robust SE
proxy_       = 0;
noconstant   = 0;
ns           = 0;
Q            = eye(ny);
dummy        = 0;
K            = 5000;
max_prior_tau_ = 0;
lb             = -1e10;
ub             = 1e10;
clags         = 0;

%********************************************************
%* CUSTOMIZED SETTINGS
%********************************************************
if nargin>2
    %======================================================================
    % Various options
    %======================================================================
    if isfield(options,'hor') ==1 % horizon of IRF and Forecasts
        hor = options.hor;
    end
    if isfield(options,'conf_sig') ==1 % CI of OLS estimation of LP and DF
        conf_sig = options.conf_sig;
    end
    if isfield(options,'Q') == 1 % orthonormal matrix
        Q = options.Q;
    end
    if isfield(options,'K') == 1 % # of draws for the BLP
        K = options.K;
    end
    %======================================================================
    % options: adding controls
    %======================================================================
    if isfield(options,'controls') ==1
        controls_  = 1;
        controls   = options.controls;
        if T~=size(controls,1)
            error('Control variables and observables must have the same time length')
        end
        if isfield(options,'clags') ==1
            clags = options.clags;
        end
    end
    %======================================================================
    % options: adding proxy variable for identification
    %======================================================================
    if isfield(options,'proxy') ==1
        proxy_  = 1;
        proxy   = options.proxy;
        ns      = size(proxy,2);
        if T~=size(proxy,1)
            error('Shocks proxies and observables must have the same time length')
        end
    end
    %======================================================================
    % options: Compuing Robust Standard Errors
    %======================================================================
    if isfield(options,'robust_se_') == 1
        robust_se_ = options.robust_se_;
        % robust_se_ = 0    unadjusted SE
        % robust_se_ = 1    NW Robust SE:  Hamilton (1994), Ch 10 pag 282, eq (10.5.20)
        % robust_se_ = 5    Matlab HAC function: need Matlab Econ Toolbox
    end
    %======================================================================
    % Conjugate/Hierachical prior options
    %======================================================================
    if (isfield(options,'priors')==1 && strcmp(options.priors.name,'Conjugate')==1) || (isfield(options,'priors')==1 && strcmp(options.priors.name,'conjugate')==1) || ...
            (isfield(options,'prior')==1 && strcmp(options.prior.name,'Conjugate')==1) || (isfield(options,'prior')==1 && strcmp(options.prior.name,'conjugate')==1)

        dummy = 2;
        prior.name= 'Conjugate';
        % Priors for the AR parameters
        if isfield(options.priors,'Phi') == 1
            % mean
            if isfield(options.priors.Phi,'mean') == 1
                prior.Phi.mean  = options.priors.Phi.mean;
                if size(prior.Phi.mean) ~= [ny*lags+(1-noconstant)    ny]
                    error('Size mismatch')
                end
            else
                warning(['You did not provide a prior mean for the AR coeff. ' ...
                    'Assume zeros everywhere.'])
                prior.Phi.mean  = zeros(ny*lags+(1-noconstant) , ny);
            end
            % variance
            if isfield(options.priors.Phi,'cov') == 1
                prior.Phi.cov   = options.priors.Phi.cov;
                if length(prior.Phi.cov) ~= (ny*lags+(1-noconstant) ) || size(prior.Phi.cov,1 )~=size(prior.Phi.cov,2)
                    error('Size mismatch: Covariance Phi should be square, e.g. size(Phi.mean,1)x size(Phi.mean,1)x')
                end
            else
                warning(['You did not provide a Covariance for the AR coeff. ' ...
                    'Assume 10 times Identity Matrix.'])
                prior.Phi.cov  = 10 * eye((ny*lags+(1-noconstant) ));
            end
        else
            warning(['You did not provide prior mean and covariance for the AR coeff ' ...
                'Assume zeros everywhere with covariance 10 times Identity Matrix.'])
            %             prior.Phi.cov   = 10 * eye((ny*lags+(1-noconstant) +timetrend ) * ny);
            prior.Phi.cov   = 10 * eye((ny*lags+(1-noconstant)  ));
            prior.Phi.mean  = zeros(ny*lags+(1-noconstant) ,  ny);
        end
        % Priors for the Residual Covariance
        if isfield(options.priors,'Sigma') == 1
            % scale
            if isfield(options.priors.Sigma,'scale') == 1
                prior.Sigma.scale = options.priors.Sigma.scale;
                if size(prior.Sigma.scale) ~= [ny ny]
                    error('Size mismatch')
                end
            else
                warning(['You did not provide a prior mean for the Residual Covariance. ' ...
                    'Assume identity matrix.'])
                prior.Sigma.scale = eye(ny);
            end
            % degrees of freedom
            if isfield(options.priors.Sigma,'df') == 1
                prior.Sigma.df = options.priors.Sigma.df;
                if length(prior.Sigma.df) ~= 1
                    error('Size mismatch')
                end
                if prior.Sigma.df < ny
                    error('Too few degrees of freedom - prior on variance residuals')
                end
            else
                warning(['You did not provide the degrees of freedom for the Residual Covariance. ' ...
                    'Assume ny+1.'])
                prior.Sigma.df = ny + 1;
            end
        else
            warning(['You did not provide prior mean and variance for the Residual Covariance. ' ...
                'Assume an identity matrix matrix with N+1 degrees of freedom.'])
            prior.Sigma.scale = eye(ny);
            prior.Sigma.df    = ny + 1;
        end
        if isfield(options.priors,'tau') == 1 && isnumeric(options.priors.tau) == 1
            prior.tau = options.priors.tau;
            if length(options.priors.tau) ~= hor
                error('tau must be a vector of size hor')
            end
        elseif isfield(options.priors,'max_tau') == 1 && options.priors.max_tau ==1
            max_prior_tau_ = 1;
            max_compute    = 3;
            prior.tau = ones(hor,1);
        else
            prior.tau = ones(hor,1);
        end
    end
    % options for the maximization
    if isfield(options,'max_compute') == 1
        max_compute    = options.max_compute;
        max_prior_tau_ = 1;
    end
    if isfield(options,'ub') == 1
        ub    = options.ub;
        if length(ub) ~=  length(prior.tau)
            error('Mismatch between the size of upper bounds and the param vector');
        end
    end
    if isfield(options,'lb') == 1
        lb    = options.lb;
        if length(lb) ~=  length(prior.tau)
            error('Mismatch between the size of lower bounds and the param vector');
        end
    end
end

% Confidence Interval Points and Index for confidence serts
alpha  = 1 - conf_sig;
talpha = abs(tinv(alpha/2,T-ny-1));
sort_idx   = round((0.5 + [-conf_sig, conf_sig, 0]/2) * K);

%**************************************************
% Construct the RHS matrix
%**************************************************

X1 = lagX(y,1:lags);
positions_nylags = 1  : size(X1,2);
nylags           = length(positions_nylags);

% forecast launching point
fdata_initval            = lagX(y(end-lags+1:end, :),0:lags-1);
fdata_initval(1:end-1,:) = [];
% add controls if any
if controls_ == 1
    a  = size(X1,2) + 1;
    X1 = [X1 lagX(controls,0:clags)];
    b  = size(X1,2);
    position_controls = a:b;
    % forecast launching point
    %lastvalc = lagX(controls(end-lags+1:end, :),0:lags-1);
    fdata_initval  = [fdata_initval, controls(end,:)];

else
    position_controls = [];
end
% add proxy shocks if any
if proxy_ == 1
    a     = size(X1,2) + 1;
    X1    = [X1 proxy];
    b     = size(X1,2);
    position_proxy = a:b ;
    % forecast launching point (assume shocks are zero)
    fdata_initval  = [fdata_initval,  zeros(1,ns)];
else
    position_proxy = [];
end
X_ = X1;
if noconstant == 0
    X_ = [X1 ones(T,1)];
    position_constant = size(X_,2);
    % forecast launching point
    fdata_initval  = [fdata_initval, 1];
else
    position_constant = [];
end
nx           = 1 - noconstant;
% other checks
if (lags + hor) >= size(X_,1)
    error('More parameters than observations: consider reducing ''hor'' or ''lags''')
end

%**************************************************
% pre allocation
%**************************************************
Omegaproxy  = eye(ny);
Omega       = eye(ny);
ir_lp       = nan(ny,hor+1,ny,3);  % variable, horizon, shock and mean upper lower
irproxy_lp  = nan(ny,hor+1,1,3);
forecasts   = nan(hor,ny,3);
if dummy == 2
    ir_blp                  = nan(ny,hor+1,ny,K);
    irproxy_blp             = nan(ny,hor+1,1,K);
    bforecasts_no_shocks    = nan(hor,ny,K);
    bforecasts_with_shocks  = nan(hor,ny,K);
    log_dnsty               = nan(hor,1);
end

%**************************************************
%* Computing the LP and DF
%**************************************************
wb = waitbar(0, 'Direct Methods');
for hh = 0 : hor % iteration over horizon
    ytmp = lagX(y, -hh);
    % Reduced Form estimations
    if robust_se_ ~= 0 % Robust SE
        options.robust_se_ = robust_se_;
        options.L     = lags + hh + 1;
        olsreg_(hh+1) = ols_reg(ytmp, X_, options);
    else
        olsreg_(hh+1) = ols_reg(ytmp, X_);
    end
    % Proxy IV identifications
    if proxy_
        irproxy_lp(:, hh+1, :, 2) = olsreg_(hh+1).beta(position_proxy, :)'; % mean
        irproxy_lp(:, hh+1, :, 3) = irproxy_lp(:, hh+1, :, 2) + talpha * olsreg_(hh+1).se(position_proxy, :)'; % upper
        irproxy_lp(:, hh+1, :, 1) = irproxy_lp(:, hh+1, :, 2) - talpha * olsreg_(hh+1).se(position_proxy, :)'; % lower
        if hh == 0
            Omegaproxy(:,1) = olsreg_(hh+1).beta(position_proxy, :)';
            Omegasproxy(:,1,:) =  repmat(Omegaproxy(:,1), 1, 1, K) + ...
                olsreg_(hh+1).se(position_proxy, :)'.* randn(ny,1,K);
        end
    end
    % Choleski/Rotated identification
    if hh == 0
        try
            Omega = chol(olsreg_(hh+1).Serror,'Lower') * Q;
        catch
            warning('Covariance of the error term is almost singular')
            Omega = chol(cov(olsreg_(hh+1).error),'Lower') * Q;
        end
        % generate uncertainty on S (flat prior)
        Sbar = olsreg_(hh+1).error'*olsreg_(hh+1).error;
        df   = olsreg_(hh+1).N - olsreg_(hh+1).K;
        if df > 0
        [~, Omegas] = generateOmegas(Sbar,df,K,Q);
            Omegasort = sort(Omegas,3);
        else
            warning('not enought degrees of freedom - too many regressors')
            Omegasort = repmat(Omega,1,1,K);
        end
        ir_lp(:, hh+1, :, 2) = Omega; % mean
        ir_lp(:, hh+1, :, 3) = Omegasort(:,:,sort_idx(2)); % UPPER
        ir_lp(:, hh+1, :, 1) = Omegasort(:,:,sort_idx(1)); % LOWER
    else
        [ir] =  iresponse(olsreg_(hh).beta(positions_nylags, :), eye(ny) , 2, Omega);
        ir_lp(:, hh+1, :, 2) = ir(:, 2, :);  % mean
        [ir3] =  iresponse(olsreg_(hh).beta(positions_nylags, :) + talpha * olsreg_(hh).se(positions_nylags, :), eye(ny) , 2, Omega);
        ir_lp(:, hh+1, :, 3) = ir3(:, 2, :); % upper
        [ir1] =  iresponse(olsreg_(hh).beta(positions_nylags, :) - talpha * olsreg_(hh).se(positions_nylags, :), eye(ny) , 2, Omega);
        ir_lp(:, hh+1, :, 1) = ir1(:, 2, :); % lower
    end

    % forecast part
    forecasts(hh+1, :, 2) = (fdata_initval * olsreg_(hh+1).beta);
    forecasts(hh+1, :, 3) = forecasts(hh+1, :, 2) + talpha * diag(olsreg_(hh+1).Serror)' ;
    forecasts(hh+1, :, 1) = forecasts(hh+1, :, 2) - talpha * diag(olsreg_(hh+1).Serror)' ;

    %======================================================================
    % Baysian DM
    if dummy == 2 % activating Bayesian Direct methods.
        if hh == 0
            % cholesky IRF on impact
            ir_blp(:, hh+1, :, :)  = Omegas;
            % proxy IRF on impact
            if proxy_
                irproxy_blp(:, hh+1, :, :) = Omegasproxy;
            end
            % one-step ahead forecast
            bforecasts_no_shocks(hh+1, :, :)   = repmat(forecasts(hh+1, :, 2),1,1,K);
            fnoise = Omega * randn(ny,K);
            bforecasts_with_shocks(hh+1, :, :) = bforecasts_no_shocks(hh+1, :, :) + reshape(fnoise,1,ny,K);

            % computing the companion matrix
            F       = [prior.Phi.mean(1 : ny * lags, :)'; eye(ny*(lags-1), ny*lags)];
            % constant
            Fo        = [prior.Phi.mean(end, :)'; zeros(ny * (lags-1), 1)];
            % Shocks Companion
            G       = eye(ny * lags, ny);

        else % hh > 0
            %********************************************************
            % Conjugate Prior: MN-IW
            %********************************************************
            if max_prior_tau_ == 1 % Maximize the shrinkage on the VAR coefficients
                try
                    disp(['***********************************************'])
                    disp(['***********************************************'])
                    disp(['Optimization at horizon ' num2str(hh)])
                    x0   = log(prior.tau(hh));
                    switch max_compute
                        case 1
                            optim_options = optimset('display','iter','MaxFunEvals',100000,'TolFun',1e-8,'TolX',1e-6);
                            [xh,fh,~,~,~,~] = ...
                                fminunc('blp_opt_hyperpara',x0,optim_options,...
                                hh,prior,olsreg_(hh),F,G,Fo,positions_nylags,position_constant);
                            %=====================================================================
                        case 2 % constraint
                            % Set default optimization options for fmincon.
                            optim_options = optimset('display','iter', 'LargeScale','off', 'MaxFunEvals',100000, 'TolFun',1e-8, 'TolX',1e-6);
                            [xh,fh,~,~,~,~,~] = ...
                                fmincon('blp_opt_hyperpara',x0,[],[],[],[],lb(hh),ub(hh),[],optim_options,y,lags,options);
                            %=====================================================================
                        case 3 % Sims
                            crit = 10e-5;
                            nit  = 10e-4;
                            [fh, xh, ~, ~, ~, ~, ~] = ...
                                csminwel('blp_opt_hyperpara',x0,.1*eye(length(x0)),[],crit,nit,hh,prior,olsreg_(hh),F,G,Fo,positions_nylags,position_constant);
                            %=====================================================================
                        case 7 % Matlab's simplex (Optimization toolbox needed).
                            optim_options = optimset('display','iter','MaxFunEvals',30000,'MaxIter',10000,'TolFun',1e-3,'TolX',1e-3);
                            [xh,fh,~,~] = fminsearch('blp_opt_hyperpara',x0,optim_options,y,lags,options);
                    end
                    fprintf('%s = %0.5g\n','Hyper-parameter Mode ',exp(xh))
                    fprintf('%s = %0.5g\n','Marginal Likelihood ',-fh)
                catch
                    warning('Maximization NOT Successful')
                    disp('Using hyper parameter default values')
                    xh = x0;
                end
                prior.tau(hh) = exp(xh);
                disp(['***********************************************'])
            end
            % constructing the posterior moments given the (optimal) shrinkage
            [posterior_, ~]   = p2p(hh,prior.tau(hh),prior,olsreg_(hh),F,G,Fo,positions_nylags,position_constant);
            % computing the marginal likelihood
            log_dnsty(hh)     = blp_ml(prior.tau(hh),hh,prior,olsreg_(hh),F,G,Fo,positions_nylags,position_constant);
            % Second moments
            S_inv_upper_chol  = chol(inv(posterior_.S));
            XXi_lower_chol    = chol(posterior_.XXi)';
            % number of regressors
            nk = nylags + nx;
            %**************************************************
            %* Generating draws form the Posterior Distribution
            %**************************************************
            for  d =  1 : K %  Gibbs Sampler
                %======================================================================
                % Inferece: Drawing from the posterior distribution
                % Step 1: draw from the Covariance
                Sigma = rand_inverse_wishart(ny, posterior_.df, S_inv_upper_chol);

                % Step 2: given the Covariance Matrix, draw from the AR parameters
                Sigma_lower_chol = chol(Sigma)';
                Phi1 = randn(nk * ny, 1);
                Phi2 = kron(Sigma_lower_chol , XXi_lower_chol) * Phi1;
                Phi3 = reshape(Phi2, nk, ny);
                Phi  = Phi3 + posterior_.PhiHat;

                % Step 3: compute IRF
                blp  =  iresponse(Phi, eye(ny) , 2, Omega);
                ir_blp(:, hh+1, :, d) = blp(:, 2, :);  % mean
                if proxy_
                    blpproxy  =  iresponse(Phi, eye(ny) , 2, Omegaproxy);
                    irproxy_blp(:, hh+1, :, d) = blpproxy(:,2,1);
                end
                % Step 4: compute Forecasts
                bforecasts_no_shocks(hh+1, :, d) = (fdata_initval(1, [positions_nylags position_constant]) * Phi);
                bforecasts_with_shocks(hh+1, :, d) = (fdata_initval(1, [positions_nylags position_constant]) * Phi) + (Sigma_lower_chol * randn(ny,1))';
            end
        end
    end
    waitbar(hh/hor, wb);
end
close(wb);

%********************************************************
%* Storing the resutls
%*******************************************************
% dm.olsreg_     = olsreg_;
dm.forecasts   = forecasts;
dm.ir_lp       = ir_lp;
dm.irproxy_lp  = irproxy_lp;
if dummy == 2
    dm.ir_blp                 = ir_blp;
    dm.irproxy_blp            = irproxy_blp;
    dm.bforecasts.no_shocks   = bforecasts_no_shocks;         % trajectories of forecasts without shocks
    dm.bforecasts.with_shocks = bforecasts_with_shocks;       % trajectories of forecasts with shocks
    dm.logmlike               = log_dnsty;
    dm.prior                  = prior;
else
    dm.ir_blp                 = [];
    dm.irproxy_blp            = [];
    dm.bforecasts.no_shocks   = [];
    dm.bforecasts.with_shocks = [];
    dm.logmlike               = [];
    dm.prior                  = [];
end

%**************************************************************************
%**************************************************************************
%**************************************************************************
%**************************************************************************
function [S,Omegas] = generateOmegas(Sbar,df,K,Q)
% generate uncertainty on S
ny      = size(Sbar,1);
S       = nan(ny,ny,K);
Omegas  = nan(ny,ny,K);

S_inv_upper_chol = chol(inv(Sbar));

for  d = 1:K
    S(:,:,d)      = rand_inverse_wishart(ny, df, S_inv_upper_chol);
    Omegas(:,:,d) = chol(S(:,:,d),'Lower') * Q;
end


function [tow,prn,P1,Phi1,P2,Phi2,eof] = readGrilR
%READGRIL Reading a binary GRIL files.
%         The following message was sent to receiver:
%             em,,/msg/jps/{GT,SI,R1,P1,R2,P2}
%             dm

%Kai Borre November 12, 2007
%Copyright (c) by Kai Borre
%$Revision: 1.1 $  $Date: 2007/11/12 $

global fidr

% Constants
c = 299792458;  % vacuum speed of light, m/s
f1 = 154*10.23E6;			% L1 frequency Hz
f2 = 120*10.23E6;			% L2 frequency Hz
lambda1 = c/f1;	     % wavelength on L1:  .19029367  m
lambda2 = c/f2;	     % wavelength on L2:  .244210213 m
Pi = 3.1415926535898; % GPS value for pi
eof = 0;

% struct GPSEphemeris
et = fread(fidr,5,'uint8');
if isempty(et) == 1
    eof = 1;
    tow = 0;
    prn = [];
    P1 = [];
    Phi1 = [];
    P2 = P1;
    Phi2 = Phi1;
    return;
end

while strcmp(char(et(1:2)'), 'GE')
    %et2 = hex2dec(et1(3:end));
    sv = fread(fidr,1,'uint8');
    i = 1;% sv;
    eph(1,i) = sv;
    tow = fread(fidr,1,'uint32');
    flags = fread(fidr,1,'uint8');
    iodc = fread(fidr,1,'int16');
    eph(21,i) = fread(fidr,1,'int32'); % toc
    ura = fread(fidr,1,'int8');
    healthS = fread(fidr,1,'uint8');
    wn = fread(fidr,1,'int16');
    tgd = fread(fidr,1,'single');
    eph(2,i) = fread(fidr,1,'single'); % af2
    eph(20,i) = fread(fidr,1,'single'); %af1
    eph(19,i) = fread(fidr,1,'single'); %af0
    eph(18,i) = fread(fidr,1,'int32'); %toe
    iode = fread(fidr,1,'int16');
    eph(4,i) = fread(fidr,1,'double'); %rootA
    eph(6,i) = fread(fidr,1,'double'); %ecc
    eph(3,i) = fread(fidr,1,'double')*Pi; %m0
    eph(16,i) = fread(fidr,1,'double')*Pi; %Omega0
    eph(12,i) = fread(fidr,1,'double')*Pi; %i0
    eph(7,i) = fread(fidr,1,'double')*Pi; %omega
    eph(5,i) = fread(fidr,1,'single')*Pi; %deltan
    eph(17,i) = fread(fidr,1,'single')*Pi; %Omegadot
    eph(13,i) = fread(fidr,1,'single')*Pi; %idot
    eph(10,i) = fread(fidr,1,'single'); %crc
    eph(11,i) = fread(fidr,1,'single'); %crs
    eph(8,i) = fread(fidr,1,'single'); %cuc
    eph(9,i) = fread(fidr,1,'single'); %cus
    eph(14,i) = fread(fidr,1,'single'); %cic
    eph(15,i) = fread(fidr,1,'single'); %cis
    cs = fread(fidr,1,'uint8');
    dummy = fread(fidr,1,'uint8');
    et = fread(fidr,5,'char');
    if ~strcmp(char(et(1:2)'), 'GE'), break; end
end

% struct GPSTime
if ~strcmp(char(et(1:2)'), 'GT')
    gt = fread(fidr,5,'uint8=>char');
else
    gt = et;
end
gt1 = char(gt');
%gt2 = str2num(gt1(3:end));
tow = fread(fidr,1,'uint32')/1000; %211 588 seconds
wn = fread(fidr,1,'uint16'); % 347 correct
cs = fread(fidr,1,'uint8');
dummy = fread(fidr,1,'uint8');

% struct SatIndex
si = fread(fidr,5,'char');
si1 = char(si');
si2 = hex2dec(si1(3:end));
for i = 1:si2-1
    prn(i) = fread(fidr,1,'uint8');
end
cs = fread(fidr,1,'uint8');
dummy = fread(fidr,1,'uint8');
PRN = prn;

% struct PR_P1
pr = fread(fidr,5,'char');
pr1 = char(pr');
pr2 = hex2dec(pr1(3:end));
for i = 1:si2-1
    prange1(i) = fread(fidr,1,'double')*c;
end
cs = fread(fidr,1,'uint8');
dummy = fread(fidr,1,'uint8');
P1 = prange1;

% struct PhaseP1
ph = fread(fidr,5,'char');
ph1 = char(ph');
ph2 = hex2dec(ph1(3:end));
for i = 1:si2-1
    phase1(i) = fread(fidr,1,'double')*lambda1;
end
cs = fread(fidr,1,'uint8');
dummy = fread(fidr,1,'uint8');
Phi1 = phase1;

% struct PR_P2
pr = fread(fidr,5,'char');
pr1 = char(pr');
pr2 = hex2dec(pr1(3:end));
for i = 1:si2-1
    prange2(i) = fread(fidr,1,'double')*c;
end
cs = fread(fidr,1,'uint8');
dummy = fread(fidr,1,'uint8');
P2 = prange2;

% struct PhaseP2
ph = fread(fidr,5,'char');
ph1 = char(ph');
ph2 = hex2dec(ph1(3:end));
for i = 1:si2-1
    phase2(i) = fread(fidr,1,'double')*lambda2;
end
cs = fread(fidr,1,'uint8');
dummy = fread(fidr,1,'uint8');
Phi2 = phase2;
%%%%%%%%% end readGrilR.m %%%%%%%%%%%%%%%%%%%%%%%%


% [INPUT]
% p = A vector of floats [0,Inf) of length t representing the prices.
% bw = An integer [21,252] representing the dimension of each rolling window.
% w = An integer [500,Inf) representing the number of sweeps (optional, default=1000).
% c = A float (0,Inf) representing the starting coefficient value (optional, default=0.01).
% s2 = A float (0,Inf) representing the starting variance of innovations (optional, default=0.0004).
%
% [OUTPUT]
% ris = A column vector of floats (0,Inf) of length t representing the Roll implicit spread.

function ris = roll_implicit_spread(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('p',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('bw',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 21 '<=' 252 'scalar'}));
        ip.addOptional('w',1000,@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 500}));
        ip.addOptional('c',0.01,@(x)validateattributes(x,{'double'},{'real' 'finite' 'positive'}));
        ip.addOptional('s2',0.0004,@(x)validateattributes(x,{'double'},{'real' 'finite' 'positive'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    p = validate_input(ipr.p);
    bw = ipr.bw;
    w = ipr.w;
    c = ipr.c;
    s2 = ipr.s2;

    nargoutchk(1,1);

    ris = roll_implicit_spread_internal(p,bw,w,c,s2);

end

function ris = roll_implicit_spread_internal(p,bw,w,c,s2)

    up = isempty(getCurrentTask());

    windows = extract_rolling_windows(log(max(1e-6,p)),bw);
    ris = zeros(numel(windows),1);

    if (up)
        parfor i = 1:numel(windows)
            ris(i) = gibbs_sampler(windows{i},w,c,s2);
        end
    else
        for i = 1:numel(windows)
            ris(i) = gibbs_sampler(windows{i},w,c,s2);
        end
    end

    alpha = 2 / (bw + 1);

    ris = [ris(1); filter(alpha,[1 (alpha - 1)],ris(2:end),(1 - alpha) * ris(1))];

end

function g = gibbs_sampler(p,w,c,s2)

    dp = diff(p);
    q = [1; sign(dp)];

    for i = 1:w
        dq = diff(q);

        d = 1 + ((1 / s2) * (dq.' * dq));
        mu = linsolve(d,(1 / s2) * (dq.' * dp));
        rho = inv(d);
        c = mvnrnd_truncated(mu,rho);

        u = dp - (c .* dq);
        alpha = 1e-12 + (numel(u) / 2);
        beta = 1e-12 + (sum(u.^2) / 2);
        s2 = 1 / ((1 / beta) * gamrnd(alpha,1));

        q = perform_draw(p,q,c,s2);
    end

    g = 2 * c;

end

function q = perform_draw(p,q,c,s2)

    t = numel(p);
    q_nnz = q ~= 0;
    s22 = s2 * 2;

    m = mod((1:t).',2);
    r = rand(t,1);

    q = [q q];
    p = [p p];

    for i = 1:2
        o = (m == (i - 1)) & q_nnz;

        if (~any(o))
            continue;
        end

        q(o,1) = 1;
        q(o,2) = -1;

        u = diff(p - (c * q));

        s = u.^2 ./ s22;
        s_sum = [s; [0 0]] + [[0 0]; s];
        s_sum = s_sum(o,:);

        odds_log = diff(s_sum,1,2);
        in_range = odds_log < 500;
        odds = exp(in_range .* odds_log);

        buy = odds ./ (1 + odds);
        buy = (in_range .* buy) + ~in_range;

        if (i == 1)
            q(o,:) = repmat(1 - (2 * (r(o) > buy)),1,2);
        else
            q(o,1) = 1 - (2 * (r(o) > buy));
        end
    end

    q = q(:,1);

end

function r = mvnrnd_truncated(mu,rho)

    f = sqrt(rho);
    low = -mu / f;

    if (low > 6)
        eta = low + (100 * eps());
    else
        plow = normcdf(low);

        p = plow + (rand() * (1 - plow));

        if (p == 1)
            eta = low + (100 * eps());
        else
            eta = norminv(p);
        end
    end

    r = mu + (f * eta);

end

function p = validate_input(p)

    p = p(:);

    if (numel(p) < 5)
        error('The value of ''p'' is invalid. Expected input to be a vector containing at least 5 elements.');
    end

end


function [xDwCentr,yDwCentr,zDwCentr] = calc_doseWeightedCentroid(structNum,doseNum,normalizeFlag,planC)
%function [xDwCentr,yDwCentr,zDwCentr] = calc_doseWeightedCentroid(structNum,doseNum,normalizeFlag,planC)
%
% This function returns the dose-weighted centroid for the input structure and dose.
%
% INPUTS:
% structNum: structure index within the planC object
% doseNum: dose index within the planC object
% normalizeFlag: Flag to normalize the centroid between 0 and 1.
% If normalizeFlag == 1, the structure coordinates are scaled between and 0
% and 1 based on min/max coordinates along that dimension. x,y,z are based
% on RTOG coordinate system: https://github.com/cerr/CERR/wiki/Coordinate-system
% planC: can be accessed from Viewer by using 'global planC' or by loading
% the CERR .mat file in memory.
%
% OUTPUTS:
% x,y,z: dose weighted centroid coordinates
%
% Example:
%
% structNum = 10;
% doseNum = 1;
% normFlg = 1;
% global planC
% [x,y,z] = calc_doseWeightedCentroid(structNum,doseNum,normFlg,planC)
%
%APA, 05/15/2018
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

%Check if plan passed, if not use global.
if ~exist('planC')
    global planC;
end
indexS = planC{end};

[iV,jV,kV] = getUniformStr(structNum, planC);
assocScanNum = getStructureAssociatedScan(structNum,planC);
[xV,yV,zV] = getUniformScanXYZVals(planC{indexS.scan}(assocScanNum));
xCalcV = xV(jV);
yCalcV = yV(iV);
zCalcV = zV(kV);

%Get scan transM
transM_scan = getTransM('scan',assocScanNum,planC);
if isempty(transM_scan)
    transM_scan = eye(4);
end

%Get dose transM
transM_dose = getTransM('dose',doseNum,planC);
if isempty(transM_dose)
    transM_dose = eye(4);
end

%Apply transM to dose calc pts
if ~isequal(transM_scan,transM_dose)
    [xCalcV, yCalcV, zCalcV] = applyTransM(transM_scan*inv(transM_dose), xCalcV, yCalcV, zCalcV);
    [jnk,jnk,zV] = applyTransM(transM_scan*inv(transM_dose), zV*0, zV*0 ,zV);
end

dosesV = getDoseAt(doseNum, xCalcV, yCalcV, zCalcV, planC);

%Normalize x,y,z coordinates
if normalizeFlag
    xCalcV = (xCalcV - min(xCalcV)) ./ (max(xCalcV) - min(xCalcV));
    yCalcV = (yCalcV - min(yCalcV)) ./ (max(yCalcV) - min(yCalcV));
    zCalcV = (zCalcV - min(zCalcV)) ./ (max(zCalcV) - min(zCalcV));
end

xDwCentr = sum(dosesV.*xCalcV)/sum(dosesV);
yDwCentr = sum(dosesV.*yCalcV)/sum(dosesV);
zDwCentr = sum(dosesV.*zCalcV)/sum(dosesV);


clear; close all; clc;
load '5jia1.mat'
load 'x1'
load 'x2'
load 'x3'
load 'x4'
load 'x5'
load 'uavW.mat'
i = uavW;
clear uavW;

P.gravity = 9.8;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Params for Aersonade UAV
%physical parameters of airframe
P.mass = 13.5;
P.Jx   = 0.8244;
P.Jy   = 1.135;
P.Jz   = 1.759;
P.Jxz  = .1204;
% aerodynamic coefficients
P.S_wing        = 0.55;
P.b             = 2.8956;
P.c             = 0.18994;
P.S_prop        = 0.2027;
P.rho           = 1.2682;
P.k_motor       = 80;
P.k_T_P         = 0;
P.k_Omega       = 0;
P.e             = 0.9;

P.C_L_0         = 0.28;
P.C_L_alpha     = 3.45;
P.C_L_q         = 0.0;
P.C_L_delta_e   = -0.36;
P.C_D_0         = 0.03;
P.C_D_alpha     = 0.30;
P.C_D_p         = 0.0437;
P.C_D_q         = 0.0;
P.C_D_delta_e   = 0.0;
P.C_m_0         = -0.02338;
P.C_m_alpha     = -0.38;
P.C_m_q         = -3.6;
P.C_m_delta_e   = -0.5;
P.C_Y_0         = 0.0;
P.C_Y_beta      = -0.98;
P.C_Y_p         = 0.0;
P.C_Y_r         = 0.0;
P.C_Y_delta_a   = 0.0;
P.C_Y_delta_r   = -0.17;
P.C_ell_0       = 0.0;
P.C_ell_beta    = -0.12;
P.C_ell_p       = -0.26;
P.C_ell_r       = 0.14;
P.C_ell_delta_a = 0.08;
P.C_ell_delta_r = 0.105;
P.C_n_0         = 0.0;
P.C_n_beta      = 0.25;
P.C_n_p         = 0.022;
P.C_n_r         = -0.35;
P.C_n_delta_a   = 0.06;
P.C_n_delta_r   = -0.032;
P.C_prop        = 1.0;
P.M             = 50;
P.epsilon       = 0.1592;
P.alpha0        = 0.4712;

% wind parameters
P.wind_n = 0;
P.wind_e = 0;
P.wind_d = 0;
P.L_u = 200;
P.L_v = 200;
P.L_w = 50;
P.sigma_u = 1.06;
P.sigma_v = 1.06;
P.sigma_w = .7;

% r1 - r8
P.r = P.Jx*P.Jz-P.Jxz^2;
P.r1 = P.Jxz*(P.Jx-P.Jy+P.Jz)/P.r;
P.r2 = P.Jz*(P.Jz-P.Jy)+P.Jxz^2;
P.r3 = P.Jz/P.r;
P.r4 = P.Jxz/P.r;
P.r5 = (P.Jz-P.Jx)/P.Jy;
P.r6 = P.Jxz/P.Jy;
P.r7 = ((P.Jx-P.Jy)*P.Jx+P.Jxz^2)/P.r;
P.r8 = P.Jx/P.r;

% C parameters on p.62
P.C_p_0 = P.r3 * P.C_ell_0 + P.r4 * P.C_n_0;
P.C_p_beta = P.r3 * P.C_ell_beta + P.r4 * P.C_n_beta;
P.C_p_p = P.r3 * P.C_ell_p + P.r4 * P.C_n_p;
P.C_p_r = P.r3 * P.C_ell_r + P.r4 * P.C_n_r;
P.C_p_delta_a = P.r3 * P.C_ell_delta_a + P.r4 * P.C_n_delta_a;
P.C_p_delta_r = P.r3 * P.C_ell_delta_r + P.r4 * P.C_n_delta_r;
P.C_r_0 = P.r4 * P.C_ell_0 + P.r8 * P.C_n_0;
P.C_r_beta = P.r4 * P.C_ell_beta + P.r8 * P.C_n_beta;
P.C_r_p = P.r4 * P.C_ell_p + P.r8 * P.C_n_p;
P.C_r_r = P.r4 * P.C_ell_r + P.r8 * P.C_n_r;
P.C_r_delta_a = P.r4 * P.C_ell_delta_a + P.r8 * P.C_n_delta_a;
P.C_r_delta_r = P.r4 * P.C_ell_delta_r + P.r8 * P.C_n_delta_r;

% compute trim conditions using 'mavsim_chap5_trim.slx'
% initial airspeed
P.Va0 = 35;
gamma = 0; %5*pi/180;  % desired flight path angle (radians)
R     = inf; %150;         % desired radius (m) - use (+) for right handed orbit, can't be 0

% autopilot sample rate
P.Ts = 0.01;
P.tau = 0.5;

% first cut at initial conditions

P.pn0    = 0;  % initial North position
P.pe0    = 0;  % initial East position
P.pd0    = 0;  % initial Down position (negative altitude)

P.u0     = P.Va0; % initial velocity along body x-axis
P.v0     = 0;  % initial velocity along body y-axis
P.w0     = 0;  % initial velocity along body z-axis
P.phi0   = 0;  % initial roll angle
P.theta0 = 0;  % initial pitch angle
P.psi0   = 0;  % initial yaw angle
P.p0     = 0;  % initial body frame roll rate
P.q0     = 0;  % initial body frame pitch rate
P.r0     = 0;  % initial body frame yaw rate

% run trim commands
[x_trim, u_trim]=compute_trim('mavsim_trim',P.Va0,gamma,R);
P.u_trim = u_trim;
P.x_trim = x_trim;

% set initial conditions to trim conditions
% initial conditions
P.pn0    = Xplot2(1,6*i-5);
P.pe0    = Xplot2(1,6*i-4);
P.pd0    = -Xplot2(1,6*i-3);

% P.pn0    = 0;  % initial North position
% P.pe0    = 0;  % initial East position
% P.pd0    = 0;  % initial Down position (negative altitude)

P.u0     = x_trim(4);  % initial velocity along body x-axis
P.v0     = x_trim(5);  % initial velocity along body y-axis
P.w0     = x_trim(6);  % initial velocity along body z-axis
P.phi0   = x_trim(7);  % initial roll angle
P.theta0 = x_trim(8);  % initial pitch angle
P.psi0   = x_trim(9);  % initial yaw angle
P.p0     = x_trim(10);  % initial body frame roll rate
P.q0     = x_trim(11);  % initial body frame pitch rate
P.r0     = x_trim(12);  % initial body frame yaw rate

% compute different transfer functions
[T_phi_delta_a,T_chi_phi,T_theta_delta_e,T_h_theta,T_h_Va,T_Va_delta_t,T_Va_theta,T_v_delta_r]...
    = compute_tf_model(x_trim,u_trim,P);

% linearize the equations of motion around trim conditions
[A_lon, B_lon, A_lat, B_lat] = compute_ss_model('mavsim_trim',x_trim,u_trim);

eig_lon = eig(A_lon);
eig_lat = eig(A_lat);

compute_gains;

% longitudinal state-machine parameters
P.altitude_take_off_zone = 50;
P.altitude_hold_zone = 10;

% sensor parameters(chapter 7)
P.sigma_gyro = 0.13;
P.sigma_accel = 0.0025;
P.beta_abs_pres = 0.125;
P.sigma_abs_pres = 0.01;
P.beta_diff_pres = 0.02;
P.sigma_diff_pres = 0.002;

P.Ts_gps = 1;
P.k_gps = 1/1000;
P.sigma_gps_n = 0.21;
P.sigma_gps_e = 0.21;
P.sigma_gps_altitude = 0.4;
P.sigma_gps_V_g = 0.05;

% filter parameters(chapter 8)
P.alpha_lpf_gyro = 0.1;
P.alpha_lpf_static_pres = 0.1;
P.alpha_lpf_diff_pres = 0.1;

P.bias_gyro_x = 0;             % x-gyro bias
P.bias_gyro_y = 0;             % y-gyro bias
P.bias_gyro_z = 0;             % z-gyro bias

% autopilot guidance model coefficients(chapter 9)
wn_h = 1.5;
zeta_h = 0.707;
P.b_hdot = 2 * zeta_h * wn_h;
P.b_h = wn_h^2;

wn_chi = 0.7;
zeta_chi = 0.707;
P.b_chidot = 2 * zeta_chi * wn_chi;
P.b_chi = wn_chi^2;

P.b_Va = 10;
P.gamma_max = 45*pi/180;

% chapter 10 parameters
P.k_path = 0.05;
P.chi_inf = 40*pi/180;
P.k_orbit = 0.01;

% chapter 11 - path manager
% number of waypoints in data structure
P.size_waypoint_array = 100;
P.R_min = P.Va0^2/P.gravity/tan(P.roll_max);

% create random city map
city_width      = 2000;  % the city is of size (width)x(width)
building_height = 300;   % maximum height of buildings
%building_height = 1;   % maximum height of buildings (for camera)
num_blocks      = 5;    % number of blocks in city
street_width    = .8;   % percent of block that is street.
P.h0 = 100;
P.pd0           = -P.h0;  % initial height of MAV
P.map = createWorld(city_width, building_height, num_blocks, street_width);


function [x,zo,xs]=estnoisem_noiseseg(yf,noise_seg,pv01,tz,pp)
%ESTNOISEM - estimate noise spectrum using minimum statistics
%
% Usage:    ninc=round(0.016*fs);   % frame increment [fs=sample frequency]
%           ovf=2;                  % overlap factor
%           f=rfft(enframe(s,hanning(ovf*ninc,'periodic'),ninc),ovf*ninc,2);
%           f=f.*conj(f);           % convert to power spectrum
%           x=estnoisem(f,ninc/fs); % estimate the noise power spectrum
%
% Inputs:
%   yf      input power spectra (one row per frame)
%   tz      frame increment in seconds
%           Alternatively, the input state from a previous call (see below)
%   pp      algorithm parameters [optional]
%
% Outputs:
%   x       estimated noise power spectra (one row per frame)
%   zo      output state
%   xs      estimated std error of x (one row per frame)
%           xs seems often to be an underestimate by a factor of 2 or 3
%
% The algorithm parameters are defined in reference [1] from which equation
% numbers are given in parentheses. They are as follows:
%
%        pp.taca      % (11): smoothing time constant for alpha_c [0.0449 seconds]
%        pp.tamax     % (3): max smoothing time constant [0.392 seconds]
%        pp.taminh    % (3): min smoothing time constant (upper limit) [0.0133 seconds]
%        pp.tpfall    % (12): time constant for P to fall [0.064 seconds]
%        pp.tbmax     % (20): max smoothing time constant [0.0717 seconds]
%        pp.qeqmin    % (23): minimum value of Qeq [2]
%        pp.qeqmax    % max value of Qeq per frame [14]
%        pp.av        % (23)+13 lines: fudge factor for bc calculation  [2.12]
%        pp.td        % time to take minimum over [1.536 seconds]
%        pp.nu        % number of subwindows to use [3]
%        pp.qith      % Q-inverse thresholds to select maximum noise slope [0.03 0.05 0.06 Inf ]
%        pp.nsmdb     % corresponding noise slope thresholds in dB/second   [47 31.4 15.7 4.1]
%
% Example use:      y=enframe(s,w,ni);                  % divide speech signal s(n) into
%                                                       % overlapping frames using window w(n)
%                   yf=rfft(y,nf,2);                    % take fourier transform
%                   dp=estnoisem(yf.*conj(yf),tinc);    % estimate the noise
%
% If convenient, you can call estnoisem in chunks of arbitrary size. Thus the following are equivalent:
%
%                   (a) dp=estnoisem(yp(1:300),tinc);
%
%                   (b) [dp(1:100),z]=estnoisem(yp(1:100),tinc);
%                       [dp(101:200),z]=estnoisem(yp(101:200),z);
%                       [dp(201:300),z]=estnoisem(yp(201:300),z);

% This is intended to be a precise implementation of [1] with Table III
% replaced by the updated table 5 from [2]. The only deliberate algorithm
% change is the introduction of a minimum value for 1/Qeq in equation (23).
% This change only affects the first few frames and improves the
% convergence of the algorithm. A minor improveemnt was reported in [3] but
% this has not yet been included.
%
% Refs:
%    [1] Rainer Martin.
%        Noise power spectral density estimation based on optimal smoothing and minimum statistics.
%        IEEE Trans. Speech and Audio Processing, 9(5):504-512, July 2001.
%    [2] Rainer Martin.
%        Bias compensation methods for minimum statistics noise power spectral density estimation
%        Signal Processing, 2006, 86, 1215-1229
%    [3] Dirk Mauler and Rainer Martin
%        Noise power spectral density estimation on highly correlated data
%        Proc IWAENC, 2006

%	   Copyright (C) Mike Brookes 2008
%      Version: $Id: estnoisem.m 1718 2012-03-31 16:40:41Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   Modified code, Zheng-Hua Tan, 2012
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nr,nrf]=size(yf);          % number of frames and freq bins

x=zeros(nr,nrf);            % initialize output arrays
xs=zeros(nr,nrf);           % will hold std error in the future
if isempty(yf) && isstruct(tz)             % no real data
    zo=tz;              % just keep the same state
else
    if isstruct(tz)       % take parameters from a previous call
        nrcum=tz.nrcum;
        p=tz.p;          % smoothed power spectrum
        ac=tz.ac;               % correction factor (9)
        sn2=tz.sn2;              % estimated noise power
        pb=tz.pb;               % smoothed noisy speech power (20)
        pb2=tz.pb2;
        pminu=tz.pminu;
        actmin=tz.actmin;   % Running minimum estimate
        actminsub=tz.actminsub;           % sub-window minimum estimate
        subwc=tz.subwc;                   % force a buffer switch on first loop
        actbuf=tz.actbuf;  % buffer to store subwindow minima
        ibuf=tz.ibuf;
        lminflag=tz.lminflag;      % flag to remember local minimum
        tinc=tz.tinc;     % frame increment
        qq=tz.qq;         % parameter structure
    else
        tinc = tz;          % second argument is frame increment
        nrcum=0;            % no frames so far
        % default algorithm constants

        qq.taca=0.0449;    % smoothing time constant for alpha_c = -tinc/log(0.7) in equ (11)
        qq.tamax=0.392;    % max smoothing time constant in (3) = -tinc/log(0.96)
        qq.taminh=0.0133;    % min smoothing time constant (upper limit) in (3) = -tinc/log(0.3)
        qq.tpfall=0.064;   % time constant for P to fall (12)
        qq.tbmax=0.0717;   % max smoothing time constant in (20) = -tinc/log(0.8)
        qq.qeqmin=2;       % minimum value of Qeq (23)
        qq.qeqmax=14;      % max value of Qeq per frame
        qq.av=2.12;             % fudge factor for bc calculation (23 + 13 lines)
        qq.td=1.536;       % time to take minimum over
        qq.nu=8;           % number of subwindows
        qq.qith=[0.03 0.05 0.06 Inf]; % noise slope thresholds in dB/s
        qq.nsmdb=[47 31.4 15.7 4.1];

        if nargin>=4 && ~isempty(pp)
            qqn=fieldnames(qq);
            for i=1:length(qqn)
                if isfield(pp,qqn{i})
                    qq.(qqn{i})=pp.(qqn{i});
                end
            end
        end
    end

    % unpack parameter structure

    taca=qq.taca;    % smoothing time constant for alpha_c = -tinc/log(0.7) in equ (11)
    tamax=qq.tamax;    % max smoothing time constant in (3) = -tinc/log(0.96)
    taminh=qq.taminh;    % min smoothing time constant (upper limit) in (3) = -tinc/log(0.3)
    tpfall=qq.tpfall;   % time constant for P to fall (12)
    tbmax=qq.tbmax;   % max smoothing time constant in (20) = -tinc/log(0.8)
    qeqmin=qq.qeqmin;       % minimum value of Qeq (23)
    qeqmax=qq.qeqmax;      % max value of Qeq per frame
    av=qq.av;             % fudge factor for bc calculation (23 + 13 lines)
    td=qq.td;       % time to take minimum over
    nu=qq.nu;           % number of subwindows
    qith=qq.qith; % noise slope thresholds in dB/s
    nsmdb=qq.nsmdb;   % maximum permitted +ve noise slope in dB/s

    % derived algorithm constants

    aca=exp(-tinc/taca); % smoothing constant for alpha_c in equ (11) = 0.7
    acmax=aca;          % min value of alpha_c = 0.7 in equ (11) also = 0.7
    amax=exp(-tinc/tamax); % max smoothing constant in (3) = 0.96
    aminh=exp(-tinc/taminh); % min smoothing constant (upper limit) in (3) = 0.3
    bmax=exp(-tinc/tbmax); % max smoothing constant in (20) = 0.8
    snrexp = -tinc/tpfall;
    nv=round(td/(tinc*nu));    % length of each subwindow in frames
    if nv<4            % algorithm doesn't work for miniscule frames
        nv=4;
        nu=max(round(td/(tinc*nv)),1);
    end
    nd=nu*nv;           % length of total window in frames
    [md,hd]=mhvals(nd); % calculate the constants M(D) and H(D) from Table III
    [mv,hv]=mhvals(nv); % calculate the constants M(D) and H(D) from Table III
    nsms=10.^(nsmdb*nv*tinc/10);  % [8 4 2 1.2] in paper
    qeqimax=1/qeqmin;  % maximum value of Qeq inverse (23)
    qeqimin=1/qeqmax; % minumum value of Qeq per frame inverse

    if isempty(yf)      % provide dummy initialization
        ac=1;               % correction factor (9)
        subwc=nv;                   % force a buffer switch on first loop
        ibuf=0;
        p=x;          % smoothed power spectrum
        sn2=p;              % estimated noise power
        pb=p;               % smoothed noisy speech power (20)
        pb2=pb.^2;
        pminu=p;
        actmin=repmat(Inf,1,nrf);   % Running minimum estimate
        actminsub=actmin;           % sub-window minimum estimate
        actbuf=repmat(Inf,nu,nrf);  % buffer to store subwindow minima
        lminflag=zeros(1,nrf);      % flag to remember local minimum
    else

        if ~nrcum

            %Initialization excluding 3 left, current (noisy frame), 3 right frames
            By=[]; Cx=[]; dC=[]; Ax=[];
            xNc=find(noise_seg==1); % current noise frame indexs
            L_3f=[ [xNc-3]' [xNc-2]' [xNc-1]' ]; %3 frames(indexes) 'left' of 'xNc'
            R_3f=[ [xNc+1]' [xNc+2]' [xNc+3]' ]; % 3 frames(indexes)  'right' of 'xNc'

            dC=[ L_3f xNc' R_3f ];   SxY=sort(unique(dC(:))); % (frame indexes which should not be considered)

            By=SxY(find(SxY>0)); % (discard 0 or negative frame indexes)
            Ax=1:1:length(noise_seg); Cx = setdiff(Ax,By); % 'By' all unsatisfied frames (ascending sorted order)

            if isempty(Cx) == 0 % satisfied frame indexes
               p=yf(Cx(1),:);   %first satisfied frame from begining (as 'Cx' asced. sorted order)

            else    % otherwise

                Cx=find(noise_seg==0); %speech frames
                if length (Cx) > 3
                   Cx(1:3)=[];  % avoid first 3 speech frames - insense of "only slight better" 'init of p'
                end
                By=[];       % removed 3L-current-3R indexes (else impose to discard all frames in file)- during 'p' update [ ~isempty(intersect(t,By)) ]

                if isempty(Cx) == 1 % not exist 4th speech frame/only contents noisy segment

                      Cx=1;   p=yf(1,:); %no hope (as usual)
                      warning('File seems have only noisy segments ..  noise-power-spectra-estimation may not good!');
                    else
                      p=yf(Cx(1),:); %i.e. 4th speech frame
                end

            end %

            ne_min=p;
            ac=1;               % correction factor (9)
            sn2=p;              % estimated noise power
            pb=p;               % smoothed noisy speech power (20)
            pb2=pb.^2;
            pminu=p;
            actmin=repmat(Inf,1,nrf);   % Running minimum estimate
            actminsub=actmin;           % sub-window minimum estimate
            subwc=nv;                   % force a buffer switch on first loop
            actbuf=repmat(Inf,nu,nrf);  % buffer to store subwindow minima
            ibuf=0;
            lminflag=zeros(1,nrf);      % flag to remember local minimum
        end

        % loop for each frame
        if sum(pv01(1:10))>=1
            p=min(yf(1:min(50,nr),:),[],1);
            ne_min=p; sn2=p; pb=p; pb2=pb.^2; pminu=p;
        end

        for t=Cx(1):nr % start from the "initialization frame for 'p'  %% we use t instead of lambda in the paper
            yft=yf(t,:);        % noise speech power spectrum
            acb=(1+(sum(p)./sum(yft)-1).^2).^(-1);  % alpha_c-bar(t)  (9)
            ac=aca*ac+(1-aca)*max(acb,acmax);       % alpha_c(t)  (10)
            ah=amax*ac.*(1+(p./sn2-1).^2).^(-1);    % alpha_hat: smoothing factor per frequency (11)
            snr=sum(p)/sum(sn2);
            ah=max(ah,min(aminh,snr^snrexp));       % lower limit for alpha_hat (12)

            if noise_seg(t) ||  ~isempty(intersect(t,By)) || (t<11 && sum(pv01(1:10))>=1) %avoid the frames in "By" (3Left, curr. noise,3Right)
            else
                p=ah.*p+(1-ah).*yft;            % smoothed noisy speech power (3)
                b=min(ah.^2,bmax);              % smoothing constant for estimating periodogram variance (22 + 2 lines)
                pb=b.*pb + (1-b).*p;            % smoothed periodogram (20)
                pb2=b.*pb2 + (1-b).*p.^2;     	% smoothed periodogram squared (21)
            end

            qeqi=max(min((pb2-pb.^2)./(2*sn2.^2),qeqimax),qeqimin/(t+nrcum));   % Qeq inverse (23)
            qiav=sum(qeqi)/nrf;             % Average over all frequencies (23+12 lines) (ignore non-duplication of DC and nyquist terms)
            bc=1+av*sqrt(qiav);             % bias correction factor (23+11 lines)
            bmind=1+2*(nd-1)*(1-md)./(qeqi.^(-1)-2*md);      % we use the simplified form (17) instead of (15)
            bminv=1+2*(nv-1)*(1-mv)./(qeqi.^(-1)-2*mv);      % same expression but for sub windows
            kmod=bc*p.*bmind<actmin;        % Frequency mask for new minimum

            if any(kmod)
                actmin(kmod)=bc*p(kmod).*bmind(kmod);
                actminsub(kmod)=bc*p(kmod).*bminv(kmod);
            end
            if subwc>1 && subwc<nv              % middle of buffer - allow a local minimum
                lminflag=lminflag | kmod;    	% potential local minimum frequency bins
                pminu=min(actminsub,pminu);
                sn2=pminu;
            else
                if subwc>=nv                    % end of buffer - do a buffer switch
                    ibuf=1+rem(ibuf,nu);     	% increment actbuf storage pointer
                    actbuf(ibuf,:)=actmin;    	% save sub-window minimum
                    pminu=min(actbuf,[],1);
                    i=find(qiav<qith);
                    nsm=nsms(i(1));          	% noise slope max
                    lmin=lminflag & ~kmod & actminsub<nsm*pminu & actminsub>pminu;
                    if any(lmin)
                        pminu(lmin)=actminsub(lmin);
                        actbuf(:,lmin)=repmat(pminu(lmin),nu,1);
                    end
                    lminflag(:)=0;
                    actmin(:)=Inf;
                    subwc=0;
                end
            end
            subwc=subwc+1;
            x(t,:)=sn2;
            %ne_min=min(ne_min, sn2);
            qisq=sqrt(qeqi);
            % empirical formula for standard error based on Fig 15 of [2]
            xs(t,:)=sn2.*sqrt(0.266*(nd+100*qisq).*qisq/(1+0.005*nd+6/nd)./(0.5*qeqi.^(-1)+nd-1));
        end
    end

    if nargout>1    % we need to store the state for next time
        zo.nrcum=nrcum+nr;      % number of frames so far
        zo.p=p;          % smoothed power spectrum
        zo.ac=ac;               % correction factor (9)
        zo.sn2=sn2;              % estimated noise power
        zo.pb=pb;               % smoothed noisy speech power (20)
        zo.pb2=pb2;
        zo.pminu=pminu;
        zo.actmin=actmin;   % Running minimum estimate
        zo.actminsub=actminsub;           % sub-window minimum estimate
        zo.subwc=subwc;                   % force a buffer switch on first loop
        zo.actbuf=actbuf;  % buffer to store subwindow minima
        zo.ibuf=ibuf;
        zo.lminflag=lminflag;      % flag to remember local minimum
        zo.tinc=tinc;     % must be the last one
        zo.qq=qq;
    end
    if ~nargout
        clf;
        subplot(212);
        plot((1:nr)*tinc,10*log10([sum(yf,2) sum(x,2)]))
        ylabel('Frame Energy (dB)');
        xlabel(sprintf('Time (s)   [%d ms frame incr]',round(tinc*1000)));
        axisenlarge([-1 -1.05]);
        legend('input','noise','Location','Best');
        subplot(211);
        plot(1:nrf,10*log10([sum(yf,1)'/nr sum(x,1)'/nr]))
        ylabel('Power (dB)');
        xlabel('Frequency bin');
        axisenlarge([-1 -1.05]);
        legend('input','noise','Location','Best');
    end
end

function [m,h,d]=mhvals(d)
% Values are taken from Table 5 in [2]
%[2] R. Martin,"Bias compensation methods for minimum statistics noise power
%               spectral density estimation", Signal Processing Vol 86, pp1215-1229, 2006.

% approx: plot(d.^(-0.5),[m 1-d.^(-0.5)],'x-'), plot(d.^0.5,h,'x-')
persistent dmh
if isempty(dmh)
    dmh=[
        1   0       0;
        2   0.26    0.15;
        5   0.48    0.48;
        8   0.58    0.78;
        10  0.61    0.98;
        15  0.668   1.55;
        20  0.705   2;
        30  0.762   2.3;
        40  0.8     2.52;
        60  0.841   3.1;
        80  0.865   3.38;
        120 0.89    4.15;
        140 0.9     4.35;
        160 0.91    4.25;
        180 0.92    3.9;
        220 0.93    4.1;
        260 0.935   4.7;
        300 0.94    5];
end

if nargin>=1
    i=find(d<=dmh(:,1));
    if isempty(i)
        i=size(dmh,1);
        j=i;
    else
        i=i(1);
        j=i-1;
    end
    if d==dmh(i,1)
        m=dmh(i,2);
        h=dmh(i,3);
    else
        qj=sqrt(dmh(i-1,1));    % interpolate using sqrt(d)
        qi=sqrt(dmh(i,1));
        q=sqrt(d);
        h=dmh(i,3)+(q-qi)*(dmh(j,3)-dmh(i,3))/(qj-qi);
        m=dmh(i,2)+(qi*qj/q-qj)*(dmh(j,2)-dmh(i,2))/(qi-qj);
    end
else
    d=dmh(:,1);
    m=dmh(:,2);
    h=dmh(:,3);
end


function [pdag, G] = learn_struct_pdag_pc(cond_indep, n, k, varargin)
% LEARN_STRUCT_PDAG_PC Learn a partially oriented DAG (pattern) using the PC algorithm
% P = learn_struct_pdag_pc(cond_indep, n, k, ...)
%
% n is the number of nodes.
% k is an optional upper bound on the fan-in (default: n)
% cond_indep is a boolean function that will be called as follows:
%   feval(cond_indep, x, y, S, ...)
% where x and y are nodes, and S is a set of nodes (positive integers),
% and ... are any optional parameters passed to this function.
%
% The output P is an adjacency matrix, in which
% P(i,j) = -1 if there is an i->j edge.
% P(i,j) = P(j,i) = 1 if there is an undirected edge i <-> j
%
% The PC algorithm does structure learning assuming all variables are observed.
% See Spirtes, Glymour and Scheines, "Causation, Prediction and Search", 1993, p117.
% This algorithm may take O(n^k) time if there are n variables and k is the max fan-in,
% but this is quicker than the Verma-Pearl IC algorithm, which is always O(n^n).

sep = cell(n,n);
ord = 0;
done = 0;
G = ones(n,n);
G=setdiag(G,0);
while ~done
  done = 1;
  [X,Y] = find(G);
  for i=1:length(X)
    x = X(i); y = Y(i);
    %nbrs = mysetdiff(myunion(neighbors(G, x), neighbors(G,y)), [x y]);
    nbrs = mysetdiff(neighbors(G, y), x);  % bug fix by Raanan Yehezkel <raanany@ee.bgu.ac.il> 6/27/04
    nbrs = unique(nbrs); % bug fix Immer Ebert ebert@tree.com 3/278/11
    if length(nbrs) >= ord & G(x,y) ~= 0
      done = 0;
      %SS = subsets(nbrs, ord, ord); % all subsets of size ord
      SS = subsets1(nbrs, ord);
      for si=1:length(SS)
	S = SS{si};
	if feval(cond_indep, x, y, S, varargin{:})
	  %if isempty(S)
 	  %  fprintf('%d indep of %d ', x, y);
	  %else
 	  %  fprintf('%d indep of %d given ', x, y); fprintf('%d ', S);
 	  %end
 	  %fprintf('\n');

	  % diagnostic
	  %[CI, r] = cond_indep_fisher_z(x, y, S, varargin{:});
	  %fprintf(': r = %6.4f\n', r);

	  G(x,y) = 0;
	  G(y,x) = 0;
	  sep{x,y} = myunion(sep{x,y}, S);
	  sep{y,x} = myunion(sep{y,x}, S);
	  break; % no need to check any more subsets
	end
      end
    end
  end
  ord = ord + 1;
end

% Create the minimal pattern,
% i.e., the only directed edges are V structures.
pdag = G;
[X, Y] = find(G);
% We want to generate all unique triples x,y,z
% This code generates x,y,z and z,y,x.
for i=1:length(X)
  x = X(i);
  y = Y(i);
  Z = find(G(y,:));
  Z = mysetdiff(Z, x);
  for z=Z(:)'
    if G(x,z)==0 & ~ismember(y, sep{x,z}) & ~ismember(y, sep{z,x})
      %fprintf('%d -> %d <- %d\n', x, y, z);
      pdag(x,y) = -1; pdag(y,x) = 0;
      pdag(z,y) = -1; pdag(y,z) = 0;
    end
  end
end

% Convert the minimal pattern to a complete one,
% i.e., every directed edge in P is compelled
% (must be directed in all Markov equivalent models),
% and every undirected edge in P is reversible.
% We use the rules of Pearl (2000) p51 (derived in Meek (1995))

old_pdag = zeros(n);
iter = 0;
while ~isequal(pdag, old_pdag)
  iter = iter + 1;
  old_pdag = pdag;
  % rule 1
  [A,B] = find(pdag==-1); % a -> b
  for i=1:length(A)
    a = A(i); b = B(i);
    C = find(pdag(b,:)==1 & G(a,:)==0); % all nodes adj to b but not a
    if ~isempty(C)
      pdag(b,C) = -1; pdag(C,b) = 0;
      %fprintf('rule 1: a=%d->b=%d and b=%d-c=%d implies %d->%d\n', a, b, b, C, b, C);
    end
  end
  % rule 2
  [A,B] = find(pdag==1); % unoriented a-b edge
  for i=1:length(A)
    a = A(i); b = B(i);
    if any( (pdag(a,:)==-1) & (pdag(:,b)==-1)' );
      pdag(a,b) = -1; pdag(b,a) = 0;
      %fprintf('rule 2: %d -> %d\n', a, b);
    end
  end
  % rule 3
  [A,B] = find(pdag==1); % a-b
  for i=1:length(A)
    a = A(i); b = B(i);
    C = find( (pdag(a,:)==1) & (pdag(:,b)==-1)' );
    % C contains nodes c s.t. a-c->ba
    G2 = setdiag(G(C, C), 1);
    if any(G2(:)==0) % there are 2 different non adjacent elements of C
      pdag(a,b) = -1; pdag(b,a) = 0;
      %fprintf('rule 3: %d -> %d\n', a, b);
    end
  end
end


function [ALLEEG cfg] = pop_est_validateMVAR(ALLEEG,typeproc,varargin)
%
% Validate a fitted VAR model. With two inputs, this function generates a
% GUI where the validation scheme can be specified. Validation consists of
% statistical tests for "whiteness" of fitted VAR model residuals [1,2],
% consistency of the fitted model [1,3] and stability of fitted model [1-3]
%
% Input:
%
%   ALLEEG:     Array of EEGLAB data structures containing fitted MODEL
%   typeproc:   reserved for future use. Use 0
%
% Optional:
%
%     'whitenessCriteria':    Cell array containing names of residual whiteness
%                             test criteria to evaluate. See [1, 2] for details.
%                             Possible Values: {'Ljung-Box','ACF','Box-Pierce','Li-McLeod'}
%                             Default Value  : all
%                             Data Input Type: cell array
%
%
%     'checkWhiteness':       Whether or not to check whiteness.
%                             Default Value  : true
%                             Data Input Type: boolean
%
%     'checkConsistency':     Whether or not to check consistency. See [1,3]
%                             for details.
%                             Default Value  : true
%                             Data Input Type: boolean
%
%     'checkStability':       Whether or not to check stability. See
%                             [1-3] for details.
%                             Default Value  : true
%                             Data Input Type: boolean
%
%     'alpha':                significance level for determining whiteness
%                             Data Input Range: [0 1]
%                             Default Value   : 0.05
%                             Data Input Type : real number (double)
%
%     'prctWinToSample':      percent of time windows to randomly select
%                             Data Input Range: [0 100]
%                             Default Value   : 100
%                             Data Input Type : real number (double)
%
%     'verb':                 verbosity level (0=no output, 1=text, 2=gui)
%
%
% Output:
%
%     whitestats:             Structure containing whiteness statistics.
%                             See est_checkMVARWhiteness() for details on
%                             structure format
%
%     PC:                     Vector of percent consistency estimates for
%                             each window.
%
%     stability:              Vector of stability estimates for each window
%     cfg:                    parameter configuration object
%
% See Also: est_checkMVARWhiteness(), est_checkMVARStability(),
%           est_checkMVARConsistency, pop_est_fitMVAR()
%
%
% References:
%
% [1] Mullen T (2010) The Source Information Flow Toolbox (SIFT):
%   Theoretical Handbook and User Manual. Chapter 3.6 and 6.
%   Available at: http://www.sccn.ucsd.edu/wiki/Sift
%
% [2] Lutkepohl, H. (2007) New Introduction to Time Series Analysis.
%   Springer.
%
% [3] Ding M, Bressler SL, Yang W, Liang H (2000) Short-window spectral
%   analysis of cortical event-related potentials by adaptive multivariate
%   autoregressive modeling: data preprocessing, model validation, and
%   variability assessment. Biol. Cybern. 83:35-45
%
% Author: Tim Mullen, 2010, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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 3 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

if nargin<2
    typeproc = 0;
end

fcnName     = strrep(mfilename,'pop_','');
fcnHandle   = str2func(fcnName);

% check the dataset
res = hlp_checkeegset(ALLEEG,{'model'});
if ~isempty(res)
    error(['SIFT:' fcnName],res{1});
end

if isfield(ALLEEG(1).CAT.configs,fcnName)
    % get default configuration (from prior use) and merge with varargin
    varargin = [hlp_struct2varargin(ALLEEG(1).CAT.configs.(fcnName)) varargin];
end

if strcmpi(typeproc,'nogui')
    % get the config from function
    cfg = arg_tovals(arg_report('rich',fcnHandle,[{'EEG',ALLEEG(1)},varargin]),false);
else
    % render the GUI
    [PGh figh] = feval(['gui_' fcnName],ALLEEG(1),varargin{:});

    if isempty(PGh)
        % user chose to cancel
        cfg = [];
        return;
    end

    % get the specification of the PropertyGrid
    ps = PGh.GetPropertySpecification;
    cfg = arg_tovals(ps,false);
end

drawnow;

if strcmpi(typeproc,'cfg_only')
    return;
end

% Apply model validation routines
for cnd=1:length(ALLEEG)

    [ALLEEG(cnd).CAT.VALIDATION.whitestats ...
     ALLEEG(cnd).CAT.VALIDATION.PCstats    ...
     ALLEEG(cnd).CAT.VALIDATION.stabilitystats ...
     ALLEEG(cnd).CAT.VALIDATION.residualstats] ...
        = est_validateMVAR('EEG',ALLEEG(cnd),cfg);

    if ~isempty(cfg)
        % store the configuration structure
        ALLEEG(cnd).CAT.configs.(fcnName) = cfg;
    end
end


function pout = polydern(p,diffvar)
% polydern: partial derivatives of a n-dimensional polynomial model created by polyfitn.
% usage: pout = polydern(p,diffvar)
%
% arguments: (input)
%  p - a structure containing a polynomial regression model.  See polyfitn
%      for details.
%
%  diffvar - integer, which variable to compute the derivative with respect
%      to. If diffvar is a character string, then it refers to an explicit
%      variable in the model that is found in the VarNames field. If so,
%      the variable name must be an exact match for one of the variables
%      listed in p.VarNames.
%
% arguments: (output)
% pout - a structure containing a polynomial regression model which
%           describes the partial derivative with respect to variable #i.
%
% Note:
% Polydern modifies the "ModelTerms" and "Coefficients" fields in creating
% the output polynomial. ParameterVar, parameterStd are also updated to
% have the proper shape, as well as scaling them to reflect the derivative.
%
% The other fields (R2, RMSE, VarNames) are copied without change, even
% though the error measures are no longer meaningful for the derivative
% polynomial.
%
% This function uses polynomial structure variables created by polyfitn,
% which is part of the PolyfitnTools package by John D'Errico:
%     http://www.mathworks.com/matlabcentral/fileexchange/10065
%
% See also: polyder, polyfitn, polyvaln
%
% Polydern author: Jason Goodman
% Error checks, ParameterStd/Var updates and allowance for the variable
% name itself instead of an index added by John D'Errico

% check the number of input args
if nargin ~= 2
  error('POLYDERN:argumentcount','Exactly two arguments are required')
end

% is p a struct, created by polyfitn?
if ~isstruct(p) || ~isfield(p,'ModelTerms') || ~isfield(p,'Coefficients')
  error('POLYDERN:invalidp','p must be a struct as created by polyfitn')
end

% check diffvar too. scalar, numeric, integer, real, in the proper range,
% or it must be character, an exact match for one of the variables listed
% in p.VarNames.
if ischar(diffvar)
  ind = ismember(p.VarNames,diffvar);
  if sum(ind) == 1
    % we have a hit
    diffvar = find(ind);
  else
    error('POLYDERN:invaliddiffvar','diffvar must be a scalar numeric real integer value, or a valid variable name')
  end
elseif ~isnumeric(diffvar) || (numel(diffvar) ~= 1) || (round(diffvar) ~= diffvar) || ~isreal(diffvar)
  % diffvar must be scalar, numeric, etc. Also verify that it is real to
  % be complete
  error('POLYDERN:invaliddiffvar','diffvar must be a scalar numeric real integer value, or a valid variable name')
elseif (diffvar < 1) || (diffvar > size(p.ModelTerms,2))
  % test that diffvar is at least 1, and is not larger than the
  % number of variables in the nmodel
  error('POLYDERN:invaliddiffvar', ...
    ['diffvar (=',num2str(diffvar), ...
    ') cannot represent the variable index to be differentiated, as it is out of range for this model'])
end

pout = p;
pout.ModelTerms = [];
pout.Coefficients = [];

remainingterms = true(1,size(p.ModelTerms,1));
jout = 1;
for jin=1:length(p.Coefficients)
    if (p.ModelTerms(jin,diffvar) ~= 0)
        pout.Coefficients(jout) = p.Coefficients(jin).*p.ModelTerms(jin,diffvar);
        pout.ModelTerms(jout,:) = p.ModelTerms(jin,:);
        pout.ModelTerms(jout,diffvar) = pout.ModelTerms(jout,diffvar)-1;
        jout = jout + 1;
    else
        % this term got dropped from the model
        remainingterms(jin) = false;
    end
end

if (jout == 1)   % Polynomial has no terms in this variable: deriv is zero
    [ncoeff, nvars] = size(p.ModelTerms);
    pout.ModelTerms(1,:) = zeros(1,nvars);
    pout.Coefficients = 0;

    pout.ParameterVar = 0;
    pout.ParameterStd = 0;
else
    % at least some terms remain, so update the parameter variances and
    % standard deviations. First, drop those terms that died.
    pout.ParameterVar = reshape(p.ParameterVar(remainingterms),1,[]);
    pout.ParameterStd = reshape(p.ParameterStd(remainingterms),1,[]);

    % scale them appropriately. Thus if std(A) is sigma, then std(k*A)
    % is k*sigma, but multiply the corresponding variances by k.^2.
    scale = p.ModelTerms(remainingterms,diffvar).';
    pout.ParameterStd = pout.ParameterStd.*scale;
    pout.ParameterVar = pout.ParameterVar.*scale.^2;
end