%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% 	TEST THE DYNAMICS OF A 2DOF robot under different circumstances.
%
%   Do exercises 1, 2 and 3
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2016, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function exercise_inv_dynamics_2dofplanar()
close all

%Just load the robot once
robot=load_robot('example', '2dofplanar');

%TODO: uncommment as you solve the exercises
exercise1(robot)
%exercise2(robot)
%exercise3(robot)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Given the following specifications, find the maximum torques if:
%
%   g = [0 -9.81 0]'
%   amax = [2 2] rad/s^2, max angular acceleration
%   wmax = [3 3], max angular speed
%
%   OBTAIN tau for different cases and find the maximum value.
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exercise1(robot)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%TODO: CHANGE q to find the "worst" pose
<<<<<<< HEAD
q=[pi/2 0]';
=======
q=[0 pi/2]';
>>>>>>> develop_new_simulations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
qd=[0 0]'; %speed --> does it make any difference?

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TODO: CHANGE qdd [-3 3] to find whether higher torques are achieved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
<<<<<<< HEAD
qdd = [0 0]'; %--> max acceleration
=======
qdd = [1 1]'; %--> max acceleration
>>>>>>> develop_new_simulations

g=[0 -9.81 0]'; % acting on the Y0 axis.
fext = [0 0 0 0 0 0]'; %[fx fy fz nx ny nz]
figure, drawrobot3d(robot, q)

%Compute the torques
%TODO: store the results and find the maximum torque
tau = inversedynamic(robot, q, qd, qdd, g, fext)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Given the following specifications, find the maximum torques if:
%
%   g = [0 -9.81 0]'
%   amax = [2 2] rad/s^2, max angular acceleration
%   wmax = [3 3], max angular speed
%
%   The robot carries a payload with m=2kg at the end effector.
%
%   OBTAIN tau for different cases and find the maximum value.
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exercise2(robot)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%TODO: CHANGE q to find the "worst" pose
q=[0 0]';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
qd=[0 0]'; %speed --> does it make any difference?
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TODO: CHANGE qdd [-3 3] to find whether higher torques are achieved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
qdd = [0 0]'; %--> max acceleration

m = 2;          %kg
g=[0 -9.81 0]'; % acting on the Y0 axis.

%forces and moments are expressed in the base reference frame
% TODO express the external forces and moments
fext = [  ]';

figure, drawrobot3d(robot, q)

%Compute the torques
tau = inversedynamic(robot, q, qd, qdd, g, fext)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Given the following specifications, find the maximum torques if the robot
%   follows a trajectory from points in cartesian coordinates:
%
%   initial = (1.5, 0.2)
%   final = (0.2,1.3)
%
%   assume that acceleration stays constant at:
%
%       amax = [2 2] rad/s^2, max angular acceleration
%       wmax = [3 3], max angular speed
%   The robot carries a piece at the end effector with mass=2kg.
%   g = [0 -9.81 0]'
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exercise3(robot)
robot.q = [0 0]
p0 = [1.5, 0.2]; %initial point
p1 = [0.2  1.3]
speed = 0.5; %m/s, linear speed

distance = norm(p1-p0);
%unit vector in the direction of the trajectory normalized
u = (p1-p0)/norm(p1-p0);
dist= linspace(0,distance,100);

%initialize T
T=eye(3);
T(1:2,4) = p0';
q_line = []
%Generate a number of
for j = 1:length(dist),
    %find all the possible solutions to reach T
    qinv = inversekinematic(robot, T);

    %Select the closest coordinates from the two possible solutions of the
    %inverse kinematics.
    q=select_closest_joint_coordinates(qinv, robot.q);

    %store the joint coordinates in a vector for ulterior animation
    q_line = [q_line q];
    robot.q=q;

    pj = p0 + u*dist(j);
    T(1:2,4) = pj';
end

%
% At this point, the joint coordinates needed are stored at q_line
%

figure, plot(q_line(1,:)), hold on
plot(q_line(2,:)), xlabel('time step'), title('Joint coordinates for a line in space')
legend('q_1', 'q_2')

q=[0 0]';
qd=[3 3]'; %speed --> does it make any difference?
qdd=[2 2]'; %--> max acceleration
m = 2;          %kg
g=[0 -9.81 0]'; % acting on the Y0 axis.

%forces and moments are expressed in the last reference frame
% TODO: express the external force.
fext = [  ]';

figure, drawrobot3d(robot, q)

%
%   TODO:   Compute tau for each q_line().
%           Store tau.
%

figure, plot(taus(1,:)), hold on
plot(taus(2,:)), xlabel('time step'), title('Torques when the robot follows a line in space')
legend('\tau_1', '\tau_2')


function [ a, seed ] = r8cc_random ( m, n, nz_num, colptr, rowind, seed )

%*****************************************************************************80
%
%% R8CC_RANDOM randomizes a R8CC matrix.
%
%  Discussion:
%
%    The R8CC format is the double precision sparse compressed column
%    format.  Associated with this format, we have an M by N matrix
%    with NZ_NUM nonzero entries.  We construct the column pointer
%    vector COL of length N+1, such that entries of column J will be
%    stored in positions COL(J) through COL(J+1)-1.  This indexing
%    refers to both the ROW and A vectors, which store the row indices
%    and the values of the nonzero entries.  The entries of the
%    ROW vector corresponding to each column are assumed to be
%    ascending sorted.
%
%    The R8CC format is equivalent to the MATLAB "sparse" format,
%    and the Harwell Boeing "real unsymmetric assembled" (RUA) format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Iain Duff, Roger Grimes, John Lewis,
%    User's Guide for the Harwell-Boeing Sparse Matrix Collection,
%    October 1992
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix.
%
%    Input, integer N, the number of columns of the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero elements in A.
%
%    Input, integer COLPTR(N+1), points to the first element of each column.
%
%    Input, integer ROWIND(NZ_NUM), contains the row indices of the elements.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(NZ_NUM), the matrix.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  for j = 1 : n
    for k = colptr(j) : colptr(j+1) - 1
      [ a(k), seed ] = r8_uniform_01 ( seed );
    end
  end

  return
end


function nh=cosmo_singleton_neighborhood(ds)
% return neighborhood where each feature is only neighbor of itself
%
% nh=cosmo_singleton_neighborhood(ds)
%
% Input:
%   ds                      dataset strucut
%
% Output:
%   nh                      neighborhood structure where each feature is
%                           only neighbor of itself.
%
%
% Example:
%     % This example shows how to run group analysis with multiple
%     % comparison correction using a clustering neighborhood defined
%     % by cosmo_singleton_neighborhood with cosmo_montecarlo_cluster_stat
%     %
%     nsubjects=10;
%     nrois=4;
%     ds=struct();
%     ds.a=struct();
%     ds.fa=struct();
%     % set random data for 10 participants and 4 ROIs
%     ds.samples=randn(nsubjects,nrois);
%     %
%     % set chunks and targets for one-sample t-test
%     ds.sa.chunks=(1:nsubjects)';
%     ds.sa.targets=ones(nsubjects,1);
%     %
%     nh=cosmo_singleton_neighborhood(ds);
%     cosmo_disp(nh);
%     %|| .origin
%     %||   .a
%     %||     struct (empty)
%     %||   .fa
%     %||     struct (empty)
%     %|| .fa
%     %||   .sizes
%     %||     [ 1         1         1         1 ]
%     %|| .a
%     %||   struct (empty)
%     %|| .neighbors
%     %||   { [ 1 ]
%     %||     [ 2 ]
%     %||     [ 3 ]
%     %||     [ 4 ] }
%     %||
%     %
%     opt=struct();
%     opt.progress=false;
%     %
%     % t-test against mean=0
%     opt.h0_mean=0;
%     %
%     % make this a fast example.
%     % usually one uses opt.niter=1000; even better is opt.niter=10000
%     opt.niter=10; % use 10000 for publication quality
%     %
%     tfce_z_scores=cosmo_montecarlo_cluster_stat(ds,nh,opt);
%     %
%     % output contains one z-score per ROI
%     cosmo_disp(size(tfce_z_scores.samples))
%     %|| [1, 4]
%
%
% Notes:
% - this function can be used for ROI group analysis with multiple
%   comparison correction using cosmo_montecarlo_cluster_stat.
%   To do so, each column in
%     ds.samples
%   should correspond to values in one ROI.
%

    cosmo_check_dataset(ds);

    nh=struct();
    nh.origin=struct();
    nh.fa=struct();
    nh.a=struct();

    if isfield(ds,'a')
        a=ds.a;

        nh.a=a;
        nh.origin.a=a;
    end

    if isfield(ds,'fa')
        fa=ds.fa;

        nh.fa=fa;
        nh.origin.fa=fa;
    end

    nfeatures=size(ds.samples,2);
    nh.fa.sizes=ones(1,nfeatures);

    nh.neighbors=num2cell((1:nfeatures)');


function [ vX, vV, mX ] = ProjectedGdMomentum( vX, vV, hG, hP, numIterations, stepSizeGd, stepSizeMomentum )
% ----------------------------------------------------------------------------------------------- %
% [ vX, vV, mX ] = ProjectedGdMomentum( vX, vV, hG, hP, numIterations, stepSizeGd, stepSizeMomentum )
% Solves an objective function by the Projected Gradient Descent /
% Projected Gradient Method (PGM) with momentum.
% Input:
%   - vX                -   Input Vector.
%                           The starting point for the gradient iterations.
%                           Structure: Vector (numElements x 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vV                -   Momentum Vector.
%                           The buffer to hold the momentum direction
%                           vector.
%                           Structure: Vector (numElements x 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - hG                -   Gradient Function Handler.
%                           A function handler which accepts a vector of
%                           size (numElements x 1) and returns the gradient
%                           in the form of a vector (numElements x 1).
%                           Structure: Function Handler.
%                           Type: NA.
%                           Range: NA.
%   - hP                -   Gradient Function Handler.
%                           A function handler which accepts a vector of
%                           size (numElements x 1) and returns the gradient
%                           in the form of a vector (numElements x 1).
%                           Structure: Function Handler.
%                           Type: NA.
%                           Range: NA.
%   - numIterations     -   Number of Iterations.
%                           Sets the number of iterations of the gradient
%                           descent.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3, ...}.
%   - stepSizeGd        -   Step Size.
%                           Sets the step size of the Gradient step.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - stepSizeMomentum  -   Momentum Step Size.
%                           The step size of the momentum step.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: [0, 1).
% Output:
%   - vX                -   Output Vector.
%                           The end point for the gradient iterations.
%                           Structure: Vector (numElements x 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vV                -   Acceleration Vector.
%                           The buffer to hold the momentum direction
%                           vector of last iteration. May used by a wrapper
%                           for warm start.
%                           Structure: Vector (numElements x 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - mX                -   The Path Matrix.
%                           The points of the gradient iterations.
%                           Structure: Matrix (numElements x numIterations).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References:
%   1.  Why Momentum Really Works (https://distill.pub/2017/momentum/).
% Remarks:
%   1.  B
% TODO:
%   1.  C
% Release Notes:
%   -   1.0.000     26/12/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

%{
V0 = 0
V1 = beta * v0 + alpha * (1 - beta) * vG1
V2 = beta * beta * v0 + beta * alpha * (1 - beta) * vG1 + alpha * (1 -
beta) * vG2 = alpha * beta * (1 - beta) * vG1 + alpha * (1 - beta) * vG2 =
alpha * (beta * (1 - beta) * vG1 + (1 - beta) * vG2)

Then:

vW = vW - Vt = vW - alpha * Vt`

Where Vt = beta * Vt-1 + (1 - beta) * vGt

%}
stepSize    = stepSizeGd * (1 - stepSizeMomentum);
vG          = zeros(size(vX, 1), 1);

mX(:, 1) = vX;

for ii = 2:numIterations
    vG(:) = hG(vX); %<! The gradient

    % My variant
%     vV(:) = (stepSizeMoment * vV) - (stepSizeGd * vG); %<! Momentum step
%     vX(:) = hP(vX + vV); %<! Projection step

    % https://towardsdatascience.com/a84097641a5d
    % Seems to have very insignificant improvement over SGD
%     vV(:) = (stepSizeMoment * vV) + ((1 - stepSizeMoment) * vG); %<! Momentum step
%     vX(:) = hP(vX - (stepSizeGd *  vV)); %<! Projection step

    % https://ruder.io/optimizing-gradient-descent/
    vV(:) = (stepSizeMomentum * vV) + (stepSizeGd * vG); %<! Momentum step
    vX(:) = hP(vX - vV); %<! Projection step

    mX(:, ii) = vX;
end

end


function pass = test_constructor(pref)
% Test the Chebfun3v constructor when performing simple arithmetic
% operations.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 100*pref.cheb3Prefs.chebfun3eps;

doms = [-1 1 -1 1 -1 1;
       -2 3 0 1 2 4];

for j = 1:size(doms, 1)
    dom = doms(j, :);
    % Check the constructor works with lots of different syntax:
    f1 = @(x,y,z) cos(x.*y.*z);
    f2 = @(x,y,z) sin(x+z) + cos(y);
    f3 = @(x,y,z) exp(x) - exp(-y) + z;
    g1 = chebfun3(f1, dom);
    g2 = chebfun3(f2, dom);
    g3 = chebfun3(f3, dom);

    % Two components:
    H1 = chebfun3v(f1, f2, dom);
    H2 = chebfun3v(f1, g2, dom);
    H3 = chebfun3v(g1, f2, dom);
    H4 = chebfun3v(g1, g2, dom);

    pass(j, 1) = norm(H1 - H2) < tol;
    pass(j, 2) = norm(H1 - H3) < tol;
    pass(j, 3) = norm(H1 - H4) < tol;

    % Three components:
    H1 = chebfun3v(f1, f2, f3, dom);
    H2 = chebfun3v(f1, f2, g3, dom);
    H3 = chebfun3v(f1, g2, f3, dom);
    H4 = chebfun3v(g1, f2, f3, dom);
    H5 = chebfun3v(g1, g2, f3, dom);
    H6 = chebfun3v(g1, f2, g3, dom);
    H7 = chebfun3v(f1, g2, g3, dom);
    H8 = chebfun3v(g1, g2, g3, dom);

    pass(j, 4) = norm(H1 - H2) < tol;
    pass(j, 5) = norm(H1 - H3) < tol;
    pass(j, 6) = norm(H1 - H4) < tol;
    pass(j, 7) = norm(H1 - H5) < tol;
    pass(j, 8) = norm(H1 - H6) < tol;
    pass(j, 9) = norm(H1 - H7) < tol;
    pass(j, 10) = norm(H1 - H8) < tol;

end

end


function rnd = getround
%GETROUND     Get rounding mode
%
%   rnd = getround
%
%On return,
%
%   rnd = -1   for rounding downwards
%   rnd =  0   for rounding to nearest
%   rnd =  1   for rounding upwards
%   rnd =  2   for rounding towards zero (chop)
%

% written  11/23/98     S.M. Rump
% modified 12/04/05     S.M. Rump  improved performance
% modified 12/15/07     T. Ogita   modified for Intel-based Mac
%

e = [];
e = 1e-30;
x = 1 + e;
y = 1 - e;
if x == y                      % fast check for rounding to nearest
    rnd = 0;
else
    e = [];
    e = 1e-30;
    z = (-1) + e;
    if ( x==1 ) & ( z==-1 )    % round downwards
        rnd = -1;
    elseif ( y==1 )            % round upwards
        rnd = +1;
    else
        rnd = 2;
    end
end


function [out] = recharge_7(p1,fin)
%recharge_7

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Constant recharge limited by incoming flux
% Constraints:  -
% @(Inputs):    p1   - maximum recharge rate [mm/d]
%               fin  - incoming flux [mm/d]

out = min(p1,fin);

end


function runSimSCFDE()

SP.FFTsize = 512;
SP.CPsize = 20;

SP.SNR = [0:2:30];
SP.numRun = 10^4;

% TS 25.104
pedAchannel = [1 10^(-9.7/20) 10^(-22.8/20)];
pedAchannel = pedAchannel/sqrt(sum(pedAchannel.^2));
vehAchannel = [1 0 10^(-1/20) 0 10^(-9/20) 10^(-10/20) 0 0 0 10^(-15/20) 0 0 0 10^(-20/20)];
vehAchannel = vehAchannel/sqrt(sum(vehAchannel.^2));
idenChannel = 1;

SP.channel = idenChannel;
%SP.channel = pedAchannel;
%SP.channel = vehAchannel;

SP.equalizerType ='ZERO';
%SP.equalizerType ='MMSE';

SER_scfde = scfde(SP);
SER_ofdm = ofdm(SP);
save scfde_awgn

SP.channel = pedAchannel;
SP.equalizerType ='ZERO';
SER_scfde = scfde(SP);
SER_ofdm = ofdm(SP);
save scfde_pedA_zero

SP.channel = pedAchannel;
SP.equalizerType ='MMSE';
SER_scfde = scfde(SP);
SER_ofdm = ofdm(SP);
save scfde_pedA_mmse

SP.channel = vehAchannel;
SP.equalizerType ='ZERO';
SER_scfde = scfde(SP);
SER_ofdm = ofdm(SP);
save scfde_vehA_zero

SP.channel = vehAchannel;
SP.equalizerType ='MMSE';
SER_scfde = scfde(SP);
SER_ofdm = ofdm(SP);
save scfde_vehA_mmse

SP.CPsize = 0;

SP.channel = pedAchannel;
SP.equalizerType ='MMSE';
SER_scfde = scfde(SP);
SER_ofdm = ofdm(SP);
save scfde_pedA_noCP_mmse

SP.channel = vehAchannel;
SP.equalizerType ='MMSE';
SER_scfde = scfde(SP);
SER_ofdm = ofdm(SP);
save scfde_vehA_noCP_mmse


function [CVAR] = cvar_(y,lags,options)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 'cvar_' estimates the paramters of VAR model with classical methods
% Impulse Response Function CI are computed using bootstrap methods

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

% Additonal Inputs collected options:
% - options are not mandatory.
% See the Hitchhiker's guide for more details.
% https://github.com/naffe15/BVAR_/blob/master/HitchhikerGuide_.pdf

% Output:

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin < 2
    error('the cvar_ funtion 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
ny                  = size(y, 2);

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

% Default Settings (they can all be changed in 'options' see below)
K                   = 5000;         % number of draws from the posterior
hor                 = 24;           % horizon for the IRF
fhor                = 12;           % horizon for the forecasts
nethor              = 12;           % network horizon/ splioover and connectedness
firstobs            = lags+1;       % first observation
presample           = 0;            % using a presample for setting the hyper-parameter of the Minnesosta prior
noconstant          = 0;            % when 0, includes a constatn in the VAR
timetrend           = 0;            % when 1, includes a time trend in the VAR
long_run_irf        = 0;            % when 0, it does not compute long run IRF
irf_1STD            = 1;            % when 1, IRF are computed as 1SD increase. Else, IRF are compued as unitary increase in the shock
cfrcst_yes          = 0;            % no conditional forecast unless defined in options
% non_explosive_      = 0;            %
heterosked          = 0;
replacement         = 1;           % with replacement when bootstrapping
bootstrap           = 1;

signs_irf           = 0;
narrative_signs_irf = 0;
zeros_signs_irf     = 0;
proxy_irf           = 0;
heterosked_irf      = 0;
hmoments_signs_irf  = 0;
nexogenous          = 0;
exogenous           = [];
cnnctdnss_          = 0;
Ridge_              = 0;
Lasso_              = 0;
ElasticNet_         = 0;
set_irf             = 0;

if any(any(isnan(y))) %== 1
    error('cvar_ cannot handle missing observation; use bvar_ instead');

end

% declaring the names for the observable variables
for v = 1 : ny
    eval(['varnames{'   num2str(v) '} =  ''Var' num2str(v) ''';'])
end

%********************************************************
%* CUSTOMIZED SETTINGS
%********************************************************
if nargin > 2
    if isfield(options,'vnames')==1
        varnames = options.vnames;
    end
    %======================================================================
    % Inference options
    %======================================================================
    if isfield(options,'K')==1
        K = options.K;
    end
    if isfield(options,'firstobs')==1
        firstobs = options.firstobs;
        if firstobs < lags + 1
            error('firstobs need to be larger than lags +1')
        end
    end
    if isfield(options,'presample')==1
        presample = options.presample;
    end
    if isfield(options,'noconstant')==1
        noconstant = options.noconstant;
    end
    if isfield(options,'timetrend')==1
        timetrend  = options.timetrend;
        noconstant = 0;
    end
%     if isfield(options,'non_explosive_')==1
%         non_explosive_  = options.non_explosive_;
%     end
    if isfield(options,'heterosked_weights')==1
        ww  = options.heterosked_weights;
        heterosked = 1;
    end
    %======================================================================
    % Bootstrap options
    %======================================================================
    if isfield(options,'bootstrap')==1
        bootstrap = options.bootstrap;
    end
    if isfield(options,'replacement')==1
        replacement = options.replacement;
    end
    %======================================================================
    % Exogenous Variables options
    %======================================================================
    if isfield(options,'exogenous')==1 || isfield(options,'controls')==1
        if isfield(options,'controls')==1
            exogenous = options.controls;
        else
            exogenous = options.exogenous;
        end
        nexogenous = size(exogenous,2);
        if size(exogenous,1) ~= size(y,1)+ fhor && size(exogenous,1) ~= size(y,1)
            error('Size Mismatch between endogenous and exogenos variables; exo must be either T or T+fhor');
        end
        if any(isnan(exogenous(lags+1:end,:)))
            error('Exogenous variables cannnot be ''nan'' from lags+1 onward.');
        end
    end
    %======================================================================
    % IRF options
    %======================================================================
    if isfield(options,'hor') ==1
        hor = options.hor;
    end
    if isfield(options,'set_irf') ==1
        % set_irf is the # of rotations for set identified systems in wihch
        % Phi and Sigma are point estimates (e.g. OLS, Ridge, Lasso or
        % Elastic Nets); default 0
        set_irf = options.set_irf;
    end
    if isfield(options,'long_run_irf')==1
        % Activating Long run IRF
        long_run_irf = options.long_run_irf;
    end
    if isfield(options,'heterosked_regimes')==1
        % Activating identification via heteroskedasticity
        if length(options.heterosked_regimes) ~= size(y,1)
            error('heterosked_regimes must have the same time dimension of y')
        end
        heterosked_irf     = 1;
        heterosked_regimes = options.heterosked_regimes(lags+1:end);
        if any(heterosked_regimes>1) || any(heterosked_regimes<0)
            error('heterosked_regimes must contain zero (first regime) and one (second regime)')
        end
    end
    if isfield(options,'signs')==1
        % Activating IRF with sign restrictions (mulitple horizons allowed)
        signs_irf       = 1;
        signs           = options.signs;
        if iscellstr(signs) == 0
            error(['options.signs should be a cell array. Each cell must contain a string with the format'...
                '\n''y(a,b,c)<0'' or ''y(a,b,c)>0'' where a, b and c are integers.',...
                '\na = index of the variable',...
                '\nb = horizon',...
                '\nc = index of the shock'],class(zeros))
        end
    end
    if isfield(options,'narrative')==1
        if signs_irf  == 0
            warning('You did not provide any sign restrictions.')
            signs{1} = 'isempty(y(1,1,1))==0';
        end
        if signs_irf  == 1
            signs_irf       = 0;  % disactivating signs
        end
        narrative_signs_irf = 1;
        narrative           = options.narrative ;
    end
    if isfield(options,'hmoments')==1
        if signs_irf  == 0
            warning('You did not provide any sign restrictions.')
            signs{1} = 'isempty(y(1,1,1))==0';
        end
        if signs_irf  == 1
            signs_irf       = 0;  % disactivating signs
        end
        hmoments_signs_irf = 1;
        hmoments           = options.hmoments;
        [f]                = hmoments2matrix(hmoments,ny);
    end
    if isfield(options,'zeros_signs')==1
        if signs_irf  == 1
            signs_irf       = 0;  % disactivating signs
        end
        if narrative_signs_irf  == 1
            narrative_signs_irf       = 0;  % disactivating narrative
        end
        % Activating IRF with zeros and sign restrictions (mulitple horizons NOT allowed)
        zeros_signs_irf  = 1;
        zeros_signs      = options.zeros_signs;
        if iscellstr(zeros_signs) == 0
            error(['options.zeros_signs should be a cell array.'...
                '\nEach cell must contain a string with the following format'...
                '\nFor sign restrictions ''y(a,b)=1'' or ''y(a,b)=-1'',',...
                '\nFor short run zero restriction ''ys(a,b)=0'',',...
                '\nFor long run restriction ''yr(a,1,b)=0'' where a and b are integers.',...
                '\na = index of the variable',...
                '\nb = index of the shock'],class(zeros_signs))
        end
        [f,sr] = sign2matrix(zeros_signs,ny);
        if isfield(options,'var_pos')==0
            var_pos = ones(1,ny);
        else
            var_pos = options.var_pos;
        end
    end
    if isfield(options,'proxy')==1
        % Activating IRF with provy
        proxy_irf   = 1;
        in.proxies  = options.proxy;
        in.vars     = y;
        in.p        = lags;
        in.compute_F_stat = 0;
        if isfield(options,'proxy_end') == 1
            in.T_m_end  = options.proxy_end;
        else
            in.T_m_end  = 0;  %if the times series of the instrument ends when VAR data ends
        end
        in.irhor    = hor;
        if isnumeric(in.proxies) == 0
            error(['options.proxy should be a numeric array (nans or inf not allowed)'],class(in.proxies))
        end
        inols =in;
    end
    if isfield(options,'irf_1STD')==1
        % Activating of unitary IRF, i.e. a unitary increase in the shocks
        % (instead of 1 STD)
        irf_1STD = options.irf_1STD;
    end
    %======================================================================
    % (Un)Conditional Forecasts options
    %======================================================================
    if isfield(options,'fhor')==1
        fhor = options.fhor;
        if fhor < 1
            error('Forecast horizon must be positive')
        end
    end
    if isfield(options,'endo_index')==1
        % Forecast conditional on the path of an endogenous var
        cfrcst_yes      = 1;
        if isfield(options,'endo_path')== 0
            error('You need to provide the p[ath for the endogenou variable')
        end
        % rows forecasts, column variables
        endo_path       = options.endo_path;
        endo_path_index = options.endo_index;
        if length(endo_path_index) ~= size(endo_path)
            error(['Mismatch beween the number of endogenous paths and the number of conditioned variables'...
                '\nE.g. the # of conditioned variables must coincide with the # of column in ''options.endo_path'''],class(endo_path_index));
        end
        if isfield(options,'exo_index')==1
            % Forecast conditional on the path of an endo var using only a
            % subset of shocks. notice that the # of endo and # exo must coincide
            cfrcst_yes  = 2;
            exo_index   = options.exo_index;
            Omega       = eye(ny);
            %             if isfield(options,'Omegaf')==1
            %                 Omegaf = options.Omegaf;
            %             end
            if length(exo_index) ~= length(endo_path_index)
                error('the # of conditioned endogenous and # exogenous shocks used must coincide');
            end
        end

    end
    %======================================================================
    % Regularization options
    %======================================================================
    if isfield(options,'Ridge')==1
        Ridge_     = 1;%options.Ridge;
        if isfield(options.Ridge,'est') == 1
            Ridge_ = options.Ridge.est;
        end
        %cnnctdnss_ = 1;
        if isfield(options.Ridge,'lambda') == 1
            Ridge_lambda = options.Ridge.lambda;
        else
            warning('You did not specify a value for the penalization parameter (options.Ridge.lambda)')
            warning('I am using lambda = 0.02')
            Ridge_lambda = 0.02;
        end
    end
    if isfield(options,'Lasso')==1
        % (matlab stat toolbox needed)
        if exist('lasso') ~= 2
            error('Cannot estimate VAR with Lasso: matlab stat toolbox needed')
        end
        Lasso_     = 1;%options.Lasso;
        if isfield(options.Lasso,'est') == 1
            Lasso_ = options.Lasso.est;
        end
        %cnnctdnss_ = 1;
        if isfield(options.Lasso,'lambda') == 1
            Lasso_lambda = options.Lasso.lambda;
        else
            warning('You did not specify a value for the penalization parameter (options.Lasso.lambda)')
            %warning('Use the largest value of Lambda that gives a nonnull model')
            warning('I am using lambda = 0.05')
            Lasso_lambda = 0.05;
        end
    end
    if isfield(options,'ElasticNet')==1
        % (matlab stat toolbox needed)
        if  exist('lasso') ~= 2
            error('Cannot estimate VAR with ElasticNet: matlab stat toolbox needed')
        end
        ElasticNet_ = 1;%options.ElasticNet;
        if isfield(options.ElasticNet,'est') == 1
            ElasticNet_ = options.ElasticNet.est;
        end
        %cnnctdnss_  = 1;
        if isfield(options.ElasticNet,'lambda') == 1
            ElasticNet_lambda = options.ElasticNet.lambda;
        else
            warning('You did not specify a value for the penalization parameter (options.ElasticNet.lambda)')
            warning('I am using lambda = 0.05')
            ElasticNet_lambda = 0.05;
        end
        if isfield(options.ElasticNet,'alpha') == 1
            ElasticNet_alpha = options.ElasticNet.alpha;
        else
            warning('You did not specify a value for the relative penalization parameter (options.ElasticNet.alpha)')
            warning('I am using 0.5')
            ElasticNet_alpha = 0.5;
        end
    end
	%======================================================================
    % Network-Spillover-Connectedness options
    %======================================================================
    if isfield(options,'nethor')==1
        nethor = options.nethor;
    end
    if isfield(options,'connectedness')==1
        cnnctdnss_ = options.connectedness;
        if cnnctdnss_ == 2 && set_irf == 0
           set_irf = 100;
        end
    end
end

%********************************************************
%* Consistency Checks
%********************************************************
nobs = size(y,1)-firstobs+1;
if (firstobs+ nobs-1)> size(y,1)
    fprintf('Incorrect or missing specification of the number of observations. nobs can be at most %4u\n',size(y,1)-firstobs+1);
    error('Inconsistent number of observations.')
end
if firstobs + presample + lags >= nobs
    error('presample too large')
end
if firstobs + presample  <= lags
    error('firstobs+presample should be > # lags (for initializating the VAR)')
end
if cfrcst_yes ~= 0 && nexogenous > 0
    warning('I will not use exogenous variables with conditional forecasts');
    nexogenous = 0;
end
if size(exogenous,1) == size(y,1) && nexogenous > 0
    warning('For forecast purposes, I will assume that exo are zero out-of sample.')
    warning('To change this, include the exogenous forecasts in options.exogenous.')
    %fprintf('To change this, include the exogenous forecasts in options.exogenous.\n')
    exogenous = [exogenous; zeros(fhor,nexogenous)];
end
if (ElasticNet_==1 && Lasso_==1 && Ridge_==1) || ...
        (ElasticNet_==1 && Lasso_==1) || ...
        (Lasso_==1 && Ridge_==1) || ...
        (ElasticNet_==1 && Ridge_==1)
%    fprintf('Incorrect or missing specification of the number of observations. nobs can be at most %4u\n',size(y,1)-firstobs+1);
    error('You chose more than one regularization estimator, Ridge Lasso or ElasticNet. Please chose only one.')
end

%********************************************************
%* Estimate VAR
%********************************************************

idx = firstobs+presample-lags:firstobs+nobs-1;
nx  = 1;
if noconstant
    nx = 0;
end

% organize data as  yy = XX B + E
[yy,XX] = YXB_(y(idx, :),lags,[nx timetrend]);

% preparing the data for LS estimation
ydata   = y(idx, :);
T       = size(ydata, 1);
if T-lags < lags*ny + nx %+ flat*(ny+1)
    error('Less observations than regressors: increase the # of obs or decrease the # of lags.')
end
xdata   = ones(T,nx);
if timetrend == 1
    % xdata = [xdata [1:T]'];
    xdata = [xdata [1-lags : T-lags]'];
end
if nexogenous > 0
    xdata = [xdata exogenous(idx,:)];
    XX    = [XX exogenous(idx(1)+lags : idx(end),:)];
end

% OLS estimate:
if heterosked == 0
    varols  = rfvar3(ydata, lags, xdata, [T; T], 0, 0);
else
    varols  = rfvar3(ydata, lags, xdata, [T; T], 0, 0, ww);
end

if Ridge_ == 1
    varRidge   = varols;
    varRidge.B = (eye(size(varRidge.B,1)) + Ridge_lambda*varols.xxi)\varRidge.B;
    % varRidge.B = inv(eye(size(varRidge.B,1)) + Ridge_lambda*varols.xxi)*varRidge.B;
    varRidge.u = varRidge.y - varRidge.X*varRidge.B;
end
if Lasso_ == 1
    varLasso   = varols;
    for vv = 1 : ny
        varLasso.B(:,vv) = lasso(varLasso.X,varLasso.y(:,vv),'Lambda',Lasso_lambda);
    end
    varLasso.u = varLasso.y - varLasso.X*varLasso.B;
end
if ElasticNet_ == 1
     varElasticNet   = varols;
     for vv = 1 : ny
         varElasticNet.B(:,vv) = ...
             lasso(varElasticNet.X,varElasticNet.y(:,vv),'Lambda',ElasticNet_lambda,'Alpha',ElasticNet_alpha);
     end
     varElasticNet.u = varElasticNet.y - varElasticNet.X*varElasticNet.B;
end

%**************************************************
%* Generating draws form the Posterior Distribution
%**************************************************

% Preallocation of memory
% Matrices for collecting draws from Posterior Density
% Last dimension corresponds to a specific draw
Phi_boots     = zeros(ny*lags+nx+timetrend + nexogenous, ny, K);   % Autoregressive Parameters
Sigma_boots   = zeros(ny,ny,K);                     % Shocks Covariance
ir_boots      = zeros(ny,hor,ny,K);                 % variable, horizon, shock and draws - Cholesky IRF
irlr_boots    = zeros(ny,hor,ny,K);                 % variable, horizon, shock and draws - Long Run IRF
Qlr_boots     = zeros(ny,ny,K);                     % long run impact matrix
e_boots       = zeros(size(yy,1), ny,K);                  % residuals
yhatfut_no_shocks         = NaN(fhor, ny, K);   % forecasts with shocks
yhatfut_with_shocks       = NaN(fhor, ny, K);   % forecast without the shocks
yhatfut_cfrcst            = NaN(fhor, ny, K);   % forecast conditional on endogenous path
if signs_irf == 1
    irsign_boots = ir_boots;
    Omega_boots    = Sigma_boots;
end
if nexogenous > 0
    irx_boots = zeros(ny,hor,nexogenous,K);
    % Ox_boots  = Sigma_boots;
end
if narrative_signs_irf == 1
    irnarrsign_boots = ir_boots;
    Omegan_boots     = Sigma_boots;
end
if hmoments_signs_irf == 1
    irhmomsign_boots = ir_boots;
    Omegam_boots     = Sigma_boots;
end
if zeros_signs_irf == 1
    irzerosign_boots   = ir_boots;
    Omegaz_boots       = Sigma_boots;
end
if proxy_irf == 1
    irproxy_boots = ir_boots;
end
if heterosked_irf == 1
   irheterosked_boots = ir_boots;
   Omegah_boots       = Sigma_boots;
end
if cnnctdnss_ == 1
    CnndtnssIndex         = nan(K,1);
    CnndtnssFromAlltoUnit = nan(ny,K);
    CnndtnssFromUnitToAll = nan(ny,K);
    Ctheta                = nan(ny,ny,K);
end

% Settings for the forecasts
forecast_data.xdata       = ones(fhor, nx);
if timetrend
    forecast_data.xdata = [forecast_data.xdata (T-lags+1 : T-lags+fhor)'];
end
if nexogenous>0
    forecast_data.xdata = [forecast_data.xdata exogenous(T-lags+1 : T-lags+fhor,:)];
end
forecast_data.initval     = ydata(end-lags+1:end, :);

% # of RHS variables
nk               = ny*lags+nx+timetrend + nexogenous;

waitbar_yes = 0;
if K > 99
    waitbar_yes = 1;
    wb = waitbar(0, 'Bootstrapping');
end

% LS quantities for bootstrapping
Phi_    = varols.B;
e_      = varols.u;
Sigma_  = 1/(nobs-nk) * (e_' * e_);

% use penalized LS if activated in options
if Ridge_ == 1
    Phi_    = varRidge.B;
    e_      = varRidge.u;
    Sigma_  = 1/(nobs-nk) * (e_' * e_);
end
if Lasso_ == 1
    Phi_    = varLasso.B;
    e_      = varLasso.u;
    Sigma_  = 1/(nobs-nk) * (e_' * e_);
end
if ElasticNet_ == 1
    Phi_    = varElasticNet.B;
    e_      = varElasticNet.u;
    Sigma_  = 1/(nobs-nk) * (e_' * e_);
end

% setting of the forecast
deterministic_data.xdata = ones(nobs, nx);
if timetrend
    deterministic_data.xdata = [deterministic_data.xdata [1-lags : T-lags]'];
end
if nexogenous>0
    deterministic_data.xdata = [deterministic_data.xdata exogenous(idx,:)];
end

% Start bootstrap procedure
for  d =  1 : K
    % Bootstrap residuals
    switch bootstrap
        case 1
            % sampling with replacement
            % generating K new samples by bootstrapping the VAR reduced form errors
            e_boots(:,:,d) = bootstrap_(e_, 1, replacement);
            % generating K new initial conditions
            initval     =  bootstrap_(ydata(1 : firstobs-1,:), 1, replacement);
        case 2
            % Wild bootstrap based on simple distribution (~Rademacher)
            rr = 1-2*(rand(nobs,1)>0.5);
            %T x n randomly choose the sign of the time T shocks (all)
            e_boots(:,:,d) = (e_ .* (rr*ones(1,ny)));
            initval        = ydata(1 : firstobs-1,:);
    end
    % With bootstrapped errors / intial conditions construct new data
    deterministic_data.initval = initval;
    [~,ystar0]                 = forecasts(deterministic_data,Phi_,Sigma_,nobs,lags,e_boots(:,:,d),1);
    ystar                      = [initval; ystar0];
    % Construct left and right ahnd side variables for LS methods
    [yys,XXs] = YXB_(ystar(idx, :),lags,[nx timetrend]);
    % correct for heteroskedasticity of known form (ww) if any
    if heterosked == 1
        wws = [ww; ones(length(yys)-length(ww),1) ];
        yys = yys ./ repmat(wws,1,size(y,2)) ;
        XXs = XXs ./ repmat(wws,1,size(X,2));
    end
    % Compute LS estimator
    [vl,d_,vr] = svd(XXs,0);
    di         = 1./diag(d_);
    Phi        = (vr .* repmat(di', nk, 1)) * vl' * yys;
    % Penalized the LS estimator if activated in options
    if Ridge_ == 1
        xxi        = vr .* repmat(di',nk,1);
        iXX        = xxi * xxi';
        Phi = (eye(nk) + Ridge_lambda*iXX)\Phi;
    end
    if Lasso_ == 1
        LassoPhi = nan(size(Phi));
        for vv = 1 : ny
            LassoPhi(:,vv) = lasso(XXs,yys(:,vv),'Lambda',Lasso_lambda);
        end
        Phi = LassoPhi;
    end
    if ElasticNet_ == 1
        ElasticNetPhi = nan(size(Phi));
        for vv = 1 : ny
            ElasticNetPhi(:,vv) = ...
                lasso(XXs,yys(:,vv),'Lambda',ElasticNet_lambda,'Alpha',ElasticNet_alpha);
        end
        Phi = ElasticNetPhi;
    end
    % compute residual and covariance matrix
    errors  = yys - XXs*Phi;
    Sigma   = 1/(nobs-nk)*(errors' * errors);
    % store the draw
    Phi_boots(:,:,d)   = Phi;
    Sigma_boots(:,:,d) = Sigma;

    %======================================================================
    % IRF
    % Compute the impulse response functions
    % with cholesky
    if irf_1STD == 1
        % one STD increase
        ir_boots(:,:,:,d)      = iresponse(Phi(1 : ny*lags, 1 : ny),Sigma,hor,eye(ny));
    else
        % one percent increase
        ir_boots(:,:,:,d)      = iresponse(Phi(1 : ny*lags, 1 : ny),Sigma,hor,eye(ny),0);
    end
    if nexogenous > 0
        Phi1         = Phi(1 : ny*lags + nx + timetrend, 1 : ny);
        Exo1         = zeros(ny,nexogenous);
        Exo1(:,1:nexogenous) = Phi(ny*lags + nx + timetrend +1 ....
                            : ny*lags + nx + timetrend + nexogenous, 1 : ny)';
        Sig1         = eye(ny);
        % unitary increase
        irx_boots(:,:,:,d) = iresponse(Phi1,Sig1,hor,Exo1);
    end
    % define the identity rotation
    Omega = eye(ny);

    % with long run restrictions
    if long_run_irf == 1
        [irlr,Qlr]             = iresponse_longrun(Phi(1 : ny*lags, 1 : ny),Sigma,hor,lags);
        irlr_boots(:,:,:,d)    = irlr;
        Qlr_boots(:,:,d)       = Qlr;
        Omega                  = Qlr;
    end
%     % with sign restrictions
%     if signs_irf == 1
%         [irsign,Omega]         = iresponse_sign(Phi(1 : ny*lags, 1 : ny),Sigma,hor,signs);
%         irsign_boots(:,:,:,d)  = irsign;
%         Omega_boots(:,:,d)     = Omega;
%     end
%     % with narrative and sign restrictions
%     if narrative_signs_irf == 1
%         [irnarrsign,Omega]         = iresponse_sign_narrative(errors,Phi(1 : ny*lags, 1 : ny),Sigma,hor,signs,narrative);
%         irnarrsign_boots(:,:,:,d)  = irnarrsign;
%         Omegan_boots(:,:,d)        = Omega;
%     end
%     % with higher-moments and sign restrictions
%     if hmoments_signs_irf == 1
%         [irhmomsign,Omega]         = iresponse_sign_hmoments(errors,Phi(1 : ny*lags, 1 : ny),Sigma,hor,signs,hmoments,f);
%         irhmomsign_boots(:,:,:,d)  = irhmomsign;
%         Omegam_boots(:,:,d)        = Omega;
%     end
    % with zeros and sign restrictions
    if zeros_signs_irf == 1         %= iresponse_zeros_signs( Phi,Sigma,bvar1.hor,lags,var_pos,f,sr);
        [irzerosign,Omega]          = iresponse_zeros_signs(Phi,Sigma,hor,lags,var_pos,f,sr);
        irzerosign_boots(:,:,:,d)   = irzerosign;
        Omegaz_boots(:,:,d)         = Omega;
    end
    % with proxy
    if proxy_irf == 1
        in.res                  = e_boots(:,:,d);
        in.Phi                  = Phi_boots(:,:,d)  ;
        in.Sigma                = Sigma;
        tmp_                    = iresponse_proxy(in);
        irproxy_boots(:,:,1,d)  = tmp_.irs';
        clear tmp_
    end
    % with heteroskedasticity
    if heterosked_irf == 1
        [irheterosked,Omegah]       = iresponse_heterosked(Phi(1 : ny*lags, 1 : ny),errors,hor,heterosked_regimes);
        irheterosked_boots(:,:,:,d) = irheterosked;
        Omegah_boots(:,:,d)         = Omegah;
    end

    %======================================================================
    % Forecasts
    % compute the out of sample forecast (unconditional)
    [frcst_no_shock,frcsts_with_shocks] = forecasts(forecast_data,Phi,Sigma,fhor,lags);
    yhatfut_no_shocks(:,:,d)            = frcst_no_shock;
    yhatfut_with_shocks(:,:,d)          = frcsts_with_shocks;

    if cfrcst_yes == 1
        % Forecast conditional on the path of an endo var using all shocks
        [sims_with_endopath,EPS(:,:,d)] = ...
            cforecasts(endo_path,endo_path_index,forecast_data,Phi,Sigma);
        yhatfut_cfrcst(:,:,d) = sims_with_endopath;

    elseif cfrcst_yes == 2
        % Forecast conditional on the path of an endo var using only a
        % subset of shocks. notice that the # of endo and # exo must coincide
        % Omega is the structural orthonormal matrix
        [sims_with_endopath,EPS(:,:,d)] = ...
            cforecasts2(endo_path,endo_path_index,exo_index,forecast_data,Phi,Sigma,Omega);
        yhatfut_cfrcst(:,:,d) = sims_with_endopath;
    end

    %======================================================================
    % Connectedness
    if cnnctdnss_ > 0
%         if use_omega == 1
%             [C] = connectedness(Phi,Sigma,nethor,Omega);
%         else
%         end
        if cnnctdnss_ == 2
            Sigma_lower_chol = chol(Sigma)';
            [C] = connectedness(Phi(1 : ny*lags, 1 : ny),Sigma,nethor, Sigma_lower_chol * Omega );
        elseif cnnctdnss_ == 1
            [C] = connectedness(Phi(1 : ny*lags, 1 : ny),Sigma,nethor);
        end
        CnndtnssIndex(d,1)         = C.Index;
        CnndtnssFromAlltoUnit(:,d) = C.FromAllToUnit;
        CnndtnssFromUnitToAll(:,d) = C.FromUnitToAll;
        Ctheta(:,:,d)              = C.theta;
    end

    if waitbar_yes, waitbar(d/K, wb); end
%     dd = 0; % reset
end
if waitbar_yes, close(wb); end

%********************************************************
%* Storing the resutls
%*******************************************************

%==========================================================================
% classical inference: LS estimator
CVAR.Phi_ols    = varols.B;
CVAR.e_ols      = varols.u;
CVAR.Sigma_ols  = 1/(nobs-nk)*varols.u'*varols.u;
[CVAR.InfoCrit.AIC, CVAR.InfoCrit.HQIC, CVAR.InfoCrit.BIC] = IC(CVAR.Sigma_ols, CVAR.e_ols, nobs, nk);
% the model with the lowest IC is preferred

% OLS irf
% with cholesky
CVAR.ir_ols      = iresponse(CVAR.Phi_ols(1 : ny*lags, 1 : ny),CVAR.Sigma_ols,hor,eye(ny));
% with long run
if long_run_irf == 1
    [irlr,Qlr]              = iresponse_longrun(CVAR.Phi_ols(1 : ny*lags, 1 : ny),CVAR.Sigma_ols,hor,lags);
    CVAR.irlr_ols           = irlr;
    CVAR.Qlr_ols(:,:)       = Qlr;
end
% set identified IRF
if set_irf > 0
    wb = waitbar(0, ['Generating rotations for set-identification - LS Estimator']);
    % with sign restrictions
    if signs_irf == 1
        for d1 = 1 : set_irf
            [CVAR.irsign_ols(:,:,:,d1),CVAR.Omega_ols(:,:,d1)] = ...
                iresponse_sign(CVAR.Phi_ols(1 : ny*lags, 1 : ny),CVAR.Sigma_ols,hor,signs);
            waitbar(d1/set_irf, wb);
        end
    end
    % with narrative and sign restrictions
    if narrative_signs_irf == 1
        for d1 = 1 : set_irf
            [CVAR.irnarrsign_ols(:,:,:,d1),CVAR.Omegan_ols(:,:,d1)] = ...
                iresponse_sign_narrative(CVAR.e_ols,CVAR.Phi_ols(1 : ny*lags, 1 : ny),CVAR.Sigma_ols,hor,signs,narrative);
            waitbar(d1/set_irf, wb);
        end
    end
    % with zeros and sign restrictions
    if zeros_signs_irf == 1
        for d1 = 1 : set_irf
            [CVAR.irzerosign_ols(:,:,:,d1),CVAR.Omegaz_ols(:,:,d1)] = ...
                iresponse_zeros_signs(CVAR.Phi_ols,CVAR.Sigma_ols,hor,lags,var_pos,f,sr);
            waitbar(d1/set_irf, wb);
        end
    end
    close(wb)
end
% proxy
if proxy_irf == 1
    inols.res               = CVAR.e_ols;
    inols.Phi               = CVAR.Phi_ols;
    inols.Sigma             = CVAR.Sigma_ols;
    inols.compute_F_stat    = 1;
    tmp_                    = iresponse_proxy(inols);
    CVAR.irproxy_ols(:,:,1) = tmp_.irs';
    CVAR.proxy.F_m          = tmp_.F_m;
    CVAR.proxy.F_m_rob      = tmp_.F_m_rob;
    CVAR.proxy.R2adj_m      = tmp_.R2adj_m;
    CVAR.proxy.data         = options.proxy;
    clear tmp_
end
% with heteroskedasticity
if heterosked_irf == 1
    [CVAR.irheterosked_ols,CVAR.Omegah_ols] = ...
        iresponse_heterosked(CVAR.Phi_ols(1 : ny*lags, 1 : ny),CVAR.e_ols,hor,heterosked_regimes);
end
% test the normality of the ols VAR residuals (matlab stat toolbox needed)
if  exist('kstest') ==2
    for gg = 1 : ny
        [H,Pv] = kstest(CVAR.e_ols(:,gg)/sqrt(CVAR.Sigma_ols(gg,gg)));
        CVAR.HP(gg,:) = [H,Pv];
        %      H = 0 => Do not reject the null hypothesis at the 5% significance
        %      level.
    end
else
    CVAR.HP = [];
end

%==========================================================================
% Penalized Approaches (Regularization)
penalizationstrn = {'Ridge','Lasso','ElasticNet'};
% loop across penalization approaches
for pp = 1 : 3
    Phi_ = []; Sigma_ = []; u_ = []; Omega_(:,:,1) = eye(ny);  InfoCrit_ = [];
    if eval([ penalizationstrn{pp} '_ == 1'])
        % AR param
        eval(['CVAR.' penalizationstrn{pp} '.Phi    = var' penalizationstrn{pp} '.B;']);
        eval(['Phi_   = CVAR.' penalizationstrn{pp} '.Phi;'])
        % Error term
        eval(['CVAR.' penalizationstrn{pp} '.e      = var' penalizationstrn{pp} '.u;']);
        eval(['u_ = CVAR.' penalizationstrn{pp} '.e;'])
        % Covariance Matrix
        eval(['CVAR.' penalizationstrn{pp} '.Sigma  = 1/(nobs-nk) * var' penalizationstrn{pp} '.u'' * var' penalizationstrn{pp} '.u;'])
        eval(['Sigma_ = CVAR.' penalizationstrn{pp} '.Sigma;'])
        % Recursive IRFs
        eval(['CVAR.' penalizationstrn{pp} '.ir      = iresponse(Phi_(1 : ny*lags, 1 : ny),Sigma_,hor,eye(ny));']);
        % info crit
        [InfoCrit_.AIC, InfoCrit_.HQIC, InfoCrit_.BIC] ...
            = IC(Sigma_, u_, nobs, nk);
        eval(['CVAR.' penalizationstrn{pp} '.InfoCrit      = InfoCrit_;']);
        % IRFs with different identification schemes
        % point identification: LR
        if long_run_irf == 1
            eval(['[CVAR.' penalizationstrn{pp} '.irlr,Omega_] = iresponse_longrun(Phi_(1 : ny*lags, 1 : ny),Sigma_,hor,lags);'])
        end
        % point identification: with heteroskedasticity
        if heterosked_irf == 1
            eval(['[CVAR.' penalizationstrn{pp} '.irheterosked,Omega_] = iresponse_heterosked(Phi_(1 : ny*lags, 1 : ny),u_,hor,heterosked_regimes);'])
        end
        % set identification: signs
        if set_irf > 0
            wb = waitbar(0, ['Generating rotations for set-identification - ' penalizationstrn{pp} ' Estimator']);
        end
        if signs_irf == 1
            for d1 = 1 : set_irf
                eval(['[CVAR.' penalizationstrn{pp} '.irsign_boots(:,:,:,' num2str(d1) ...
                    '),Omega_(:,:,' num2str(d1) ')] = iresponse_sign(Phi_(1 : ny*lags, 1 : ny),Sigma_,hor,signs);'])
                waitbar(d1/set_irf, wb);
            end
        end
        % set identification: narrative and sign restrictions
        if narrative_signs_irf == 1
            for d1 = 1 : set_irf
                eval(['[CVAR.' penalizationstrn{pp} '.irnarrsign_boots(:,:,:,' num2str(d1) ...
                    '),Omega_(:,:,' num2str(d1) ')] = iresponse_sign_narrative(Phi_(1 : ny*lags, 1 : ny),Sigma_,hor,signs,narrative);'])
                waitbar(d1/set_irf, wb);
            end
        end
        % set identification: zeros and sign restrictions
        if zeros_signs_irf == 1
            for d1 = 1 : set_irf
                eval(['[CVAR.' penalizationstrn{pp} 'irzerosign_boots.(:,:,:,' num2str(d1) ...
                    '),Omega_(:,:,' num2str(d1) ')] = iresponse_zeros_signs(Phi_(1 : ny*lags, 1 : ny),Sigma_,hor,lags,var_pos,f,sr);'])
                waitbar(d1/set_irf, wb);
            end
        end
        if set_irf>0, close(wb); end
    end
    if cnnctdnss_ == 1 % default identification (Pesaran and Shin)
        eval(['CVAR.' penalizationstrn{pp} '.Connectedness = connectedness(Phi_(1 : ny*lags, 1 : ny),Sigma_,nethor);']);
    elseif cnnctdnss_ == 2 % customized identification
        Sigma_lower_chol = chol(Sigma_)';
        % this part is not correct as the median orthogonal rotation is not
        % orthogonal
        Omegam           = median(Omega_,3);
        eval(['CVAR.' penalizationstrn{pp} '.Connectedness = connectedness(Phi_(1 : ny*lags, 1 : ny), Sigma_, nethor, Sigma_lower_chol * Omegam);']);
    end

end

%==========================================================================
% Store the output
% the last dimension of these objects corresponds to a bootstrapped sample

% inference and IRFs
CVAR.Phi_boots    = Phi_boots;          % draws from the autoregressive part
CVAR.Sigma_boots  = Sigma_boots;        % draws from the covarance matrix
CVAR.ir_boots     = ir_boots;           % draws from the IRF with cholesky
CVAR.irlr_boots   = irlr_boots;         % draws from the IRF with Long Run
CVAR.Qlr_boots    = Qlr_boots;          % Long Run Rotation matrix
CVAR.lags         = lags;               % lags
CVAR.N            = ny;                 % number of variables
CVAR.e_boots      = e_boots;            % residuals
CVAR.XX           = XX;                 % regressors (no dummy)
CVAR.yy           = yy;                 % dependent  (no dummy)

% prediction
CVAR.fhor         = fhor;               % forecast horizon
CVAR.hor          = hor;                % IRF horizon
CVAR.forecasts.no_shocks      = yhatfut_no_shocks;         % trajectories of forecasts without shocks
CVAR.forecasts.with_shocks    = yhatfut_with_shocks;       % trajectories of forecasts with shocks
CVAR.forecasts.conditional    = [];            % trajectories of conditional forecasts
CVAR.forecasts.EPScond        = [];            % shocks of conditional forecasts
if cfrcst_yes ~= 0
    CVAR.forecasts.conditional    = yhatfut_cfrcst;            % trajectories of forecasts
    CVAR.forecasts.EPScond        = EPS;                       % shocks of forecasts
end
CVAR.forecast_data            = forecast_data;

%
CVAR.varnames     = varnames;
CVAR.ndraws       = K;

if signs_irf == 1 && narrative_signs_irf == 0
    CVAR.irsign_boots = irsign_boots;
    CVAR.Omegas       = Omega_boots;
else
    CVAR.irsign_boots = [];
    CVAR.Omegas       = [];
end
if narrative_signs_irf == 1
    CVAR.irnarrsign_boots = irnarrsign_boots;
    CVAR.Omegan           = Omegan_boots;
else
    CVAR.irnarrsign_boots = [];
    CVAR.Omegan           = [];
end
if hmoments_signs_irf == 1
    CVAR.irhmomsign_boots = irhmomsign_boots;
    CVAR.Omegam           = Omegam_boots;
else
    CVAR.irhmomsign_boots = [];
    CVAR.Omegam           = [];
end
if zeros_signs_irf == 1
    CVAR.irzerosign_boots   = irzerosign_boots;
    CVAR.Omegaz             = Omegaz_boots;
else
    CVAR.irzerosign_boots = [];
    CVAR.Omegaz           = [];
end
if heterosked_irf == 1
    CVAR.irheterosked_boots   = irheterosked_boots;
    CVAR.Omegah_boots         = Omegah_boots;
    CVAR.Omegah               = CVAR.Omegah_boots;
else
    CVAR.irheterosked_boots   = [];
    CVAR.Omegah_boots         = [];
    CVAR.Omegah               = [];
end
if proxy_irf == 1
    CVAR.irproxy_boots = irproxy_boots;
else
    CVAR.irproxy_boots= [];
end
if nexogenous > 0
    CVAR.irx_boots = irx_boots;
else
    CVAR.irx_boots = [];
end

% connetedness computed using LS (or penalized LS if activated in options)
if cnnctdnss_
    CVAR.Connectedness.Index         = CnndtnssIndex;
    CVAR.Connectedness.FromAlltoUnit = CnndtnssFromAlltoUnit;
    CVAR.Connectedness.FromUnitToAll = CnndtnssFromUnitToAll;
    CVAR.Connectedness.theta         = Ctheta;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% end of cvar_.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Johann Diep (johann.diep@esa.int) - November 2021
%
% This script builds the structure for the Katwijk GNSS dataset.

warning off;

%% Conversion Constants

G =  9.80665;       % Gravity constant, m/s^2
G2MSS = G;          % g to m/s^2
MSS2G = (1/G);      % m/s^2 to g
D2R = (pi/180);     % degrees to radians
R2D = (180/pi);     % radians to degrees
KT2MS = 0.514444;   % knot to m/s
MS2KMH = 3.6;       % m/s to km/h

%% GNSS ERROR PROFILE

gnss_data_planetary = readtable('./external-data/katwijk/gnss/gps-latlong.txt');
load('./katwijk-planetary-data/data/imu_planetary.mat');

% GNSS data structure:
%         t: Mx1 time vector (seconds).
%       lat: Mx1 latitude (radians).
%       lon: Mx1 longitude (radians).
%         h: Mx1 altitude (m).
%       vel: Mx3 NED velocities (m/s).
%       std: 1x3 position standard deviations, [lat lon h] (rad, rad, m).
%      stdm: 1x3 position standard deviations, [lat lon h] (m, m, m).
%      stdv: 1x3 velocity standard deviations, [Vn Ve Vd] (m/s).
%      larm: 3x1 lever arm from IMU to GNSS antenna (x-fwd, y-right, z-down) (m).
%      freq: 1x1 sampling frequency (Hz).
%   zupt_th: 1x1 ZUPT threshold (m/s).
%  zupt_win: 1x1 ZUPT time window (seconds).
%       eps: 1x1 time interval to compare IMU time vector to GNSS time vector (seconds).

gnss_planetary.stdm = 0.01 * [1 1 1]; % degradation

% GNSS positions
gnss_planetary.lat = table2array(gnss_data_planetary(:,3)).*D2R;
gnss_planetary.lon = table2array(gnss_data_planetary(:,4)).*D2R;
gnss_planetary.h = table2array(gnss_data_planetary(:,5));

gnss_planetary = gnss_m2r(gnss_planetary.lat(1),gnss_planetary.h(1),gnss_planetary); % convert error from meters to radians

% parsing the timestamps of the measurements
Date = cell2mat(table2array(gnss_data_planetary(:,1)));
Year = str2num(Date(:,1:4));
Month = str2num(Date(:,6:7));
Day = str2num(Date(:,9:10));
Hour = str2num(Date(:,12:13))-1;
Minutes = str2num(Date(:,15:16));
Seconds = str2num(Date(:,18:19));
MilliSeconds = str2num(Date(:,21:23));
DateTime = datetime(Year, Month, Day, Hour, Minutes, Seconds, MilliSeconds);
Timestamp = posixtime(DateTime);
gnss_planetary.t = Timestamp - Timestamp(1);

gnss_planetary.freq = 1/mean(diff(gnss_planetary.t)); % estimating the frequency
[gnss_planetary.vel,~] = pllh2vned(gnss_planetary); % estimating the velocity

gnss_planetary.larm = [0,0,0]'; % GNSS lever arm from IMU to GNSS antenna
gnss_planetary.eps = mean(diff(imu_planetary.t))/3; % rule of thumb for choosing eps
gnss_planetary.stdv = gnss_planetary.stdm/100; % educated guess

% Parameters for ZUPT detection algorithm  copied from above
gnss_planetary.zupt_th = 0; % ZUPT threshold (m/s).
gnss_planetary.zupt_win = 4; % ZUPT time window (seconds).

% Increase frequency by interpolation
t_q = 0:0.05:gnss_planetary.t(end);
lon_q = interp1(gnss_planetary.t,gnss_planetary.lon,t_q);
lat_q = interp1(gnss_planetary.t,gnss_planetary.lat,t_q);
h_q = interp1(gnss_planetary.t,gnss_planetary.h,t_q);
v_q = interp1(gnss_planetary.t,gnss_planetary.vel,t_q);
freq_q = 1/mean(diff(t_q));

gnss_planetary.t = t_q';
gnss_planetary.lon = lon_q';
gnss_planetary.lat = lat_q';
gnss_planetary.h = h_q';
gnss_planetary.vel = v_q;
gnss_planetary.freq = freq_q;

gnss_planetary_r = gnss_planetary; % reference

% remove datapoints
gnss_planetary.t(2000:4000) = [];
gnss_planetary.lon(2000:4000) = [];
gnss_planetary.lat(2000:4000) = [];
gnss_planetary.h(2000:4000) = [];
gnss_planetary.vel(2000:4000,:) = [];

gnss_planetary_sparse_r = gnss_planetary; % reference
[gnss_planetary,~] = gnss_gen(gnss_planetary_sparse_r, gnss_planetary); % degredation

%% Saving

save('./katwijk-planetary-data/data/gnss_planetary_r.mat','gnss_planetary_r');
save('./katwijk-planetary-data/data/gnss_planetary_sparse_r.mat','gnss_planetary_sparse_r');
save('./katwijk-planetary-data/data/gnss_sparse_planetary.mat','gnss_planetary');


function [el, prj] = project_elec(elc, pnt, tri)

% PROJECT_ELEC projects electrodes on a triangulated surface
% and returns triangle index, la/mu parameters and distance
%
% Use as
%   [el, prj] = project_elec(elc, pnt, tri)
% which returns
%   el    = Nx4 matrix with [tri, la, mu, dist] for each electrode
%   prj   = Nx3 matrix with the projected electrode position
%
% See also TRANSFER_ELEC

% Copyright (C) 1999-2013, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

Nelc = size(elc,1);
el   = zeros(Nelc, 4);

% this is a work-around for http://bugzilla.fieldtriptoolbox.org/show_bug.cgi?id=2369
elc = double(elc);
pnt = double(pnt);
tri = double(tri);

for i=1:Nelc
  [proj,dist] = ptriprojn(pnt(tri(:,1),:), pnt(tri(:,2),:), pnt(tri(:,3),:), elc(i,:), 1);

  [mindist, minindx] = min(abs(dist));
  [la, mu] = lmoutr(pnt(tri(minindx,1),:), pnt(tri(minindx,2),:), pnt(tri(minindx,3),:), proj(minindx,:));
  smallest_dist = dist(minindx);
  smallest_tri  = minindx;
  smallest_la   = la;
  smallest_mu   = mu;

  % the following can be done faster, because the smallest_dist can be
  % directly selected
%   Ntri = size(tri,1);
%   for j=1:Ntri
%     %[proj, dist] = ptriproj(pnt(tri(j,1),:), pnt(tri(j,2),:), pnt(tri(j,3),:), elc(i,:), 1);
%     if dist(j)<smallest_dist
%       % remember the triangle index, distance and la/mu
%       [la, mu] = lmoutr(pnt(tri(j,1),:), pnt(tri(j,2),:), pnt(tri(j,3),:), proj(j,:));
%       smallest_dist = dist(j);
%       smallest_tri  = j;
%       smallest_la   = la;
%       smallest_mu   = mu;
%     end
%   end

  % store the projection for this electrode
  el(i,:) = [smallest_tri smallest_la smallest_mu smallest_dist];
end

if nargout>1
  prj = zeros(size(elc));
  for i=1:Nelc
    v1 = pnt(tri(el(i,1),1),:);
    v2 = pnt(tri(el(i,1),2),:);
    v3 = pnt(tri(el(i,1),3),:);
    la = el(i,2);
    mu = el(i,3);
    prj(i,:) = routlm(v1, v2, v3, la, mu);
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION this is an alternative implementation that will also work for
% polygons
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function prj = polyproj(elc,pnt)
% projects a point on a plane, e.g. an electrode on a polygon
% pnt is a Nx3 matrix with multiple vertices that span the plane
% these vertices can be slightly off the plane
center = mean(pnt,1);
% shift the vertices to have zero mean
pnt(:,1) = pnt(:,1) - center(1);
pnt(:,2) = pnt(:,2) - center(2);
pnt(:,3) = pnt(:,3) - center(3);
elc(:,1) = elc(:,1) - center(1);
elc(:,2) = elc(:,2) - center(2);
elc(:,3) = elc(:,3) - center(3);
pnt = pnt';
elc = elc';
[u, s, v] = svd(pnt);
% The vertices are assumed to ly in plane, at least reasonably. That means
% that from the three eigenvectors there is one which is very small, i.e.
% the one orthogonal to the plane. Project the electrodes along that
% direction.
u(:,3) = 0;
prj = u * u' * elc;
prj = prj';
prj(:,1) = prj(:,1) + center(1);
prj(:,2) = prj(:,2) + center(2);
prj(:,3) = prj(:,3) + center(3);


DSP_MIN_SCALE = 1 / 6;
DSP_MAX_SCALE = 3;
DSP_NUM_SCALES = 10;

pool = gcp('nocreate');
if isempty(pool)
    pool = parpool(maxNumCompThreads());
end

parfor i = 1:num_images
    fprintf('Computing features for %s [%d/%d]', ...
            image_names{i}, i, num_images);

    if exist(keypoint_paths{i}, 'file') ...
            && exist(descriptor_paths{i}, 'file')
        fprintf(' -> skipping, already exist\n');
        continue;
    end

    tic;

    % Read the image for keypoint detection, patch extraction and
    % descriptor computation.
    image = imread(image_paths{i});
    if ismatrix(image)
        image = single(image);
    else
        image = single(rgb2gray(image));
    end

    % Read the pre-computed SIFT keypoints.
    keypoints = read_keypoints(keypoint_paths{i});

    % Compute the descriptors for the detected keypoints.
    if size(keypoints, 1) == 0
        descriptors = zeros(0, 128);
    else
        % Create DSP keypoints, sampled around the standard SIFT detections.
        num_keypoints = size(keypoints, 1);
        dsp_keypoints = zeros(4, num_keypoints, DSP_NUM_SCALES);
        dsp_scale_idx = 1;
        for dsp_scale = linspace(DSP_MIN_SCALE, DSP_MAX_SCALE, DSP_NUM_SCALES)
            dsp_keypoints([1 2 4],:,dsp_scale_idx) = keypoints(:,[1 2 4])';
            dsp_keypoints(3,:,dsp_scale_idx) = dsp_scale * keypoints(:,3)';
            dsp_scale_idx = dsp_scale_idx + 1;
        end

        % Transpose to VLFeat format.
        dsp_keypoints = reshape(dsp_keypoints, ...
                                [4, num_keypoints * DSP_NUM_SCALES]);

        % Extract the descriptors from the DSP keypoints.
        [~, descriptors] = vl_covdet(image, 'Frames', dsp_keypoints, ...
                                     'Descriptor', 'SIFT');

        % Aggregate the descriptors across all scales.
        descriptors = reshape(descriptors, ...
                              [128, num_keypoints, DSP_NUM_SCALES]);
        descriptors = mean(double(descriptors), 3)';
    end

    % Make sure that each keypoint has one descriptor.
    assert(size(keypoints, 1) == size(descriptors, 1));

    % Write the descriptors to disk for matching.
    write_descriptors(descriptor_paths{i}, descriptors);

    fprintf(' in %.3fs\n', toc);
end


function sliceThicknessV = deduceSliceWidths(planC)
%function sliceThicknessV = deduceSliceWidths(planC)
%
%This function deduces slice thicknesses when the treatment planning system
%provides only z Values.
%
%
%Created:  30 Apr 03, JOD.
%
% 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/>.

indexS = planC{end};

zValuesV = [planC{indexS.scan}.scanInfo(:).zValue];

sliceThicknessV = ones(size(zValuesV)) * nan;

for i = 2 : length(zValuesV) - 1

  nextDelta = abs(zValuesV(i+1) - zValuesV(i));

  sliceThicknessV(i) = nextDelta;

end

sliceThicknessV(1) = 2 * (abs(zValuesV(2) - zValuesV(1)) - 0.5 * sliceThicknessV(2));

sliceThicknessV(end) = 2 * (abs(zValuesV(end) - zValuesV(end - 1)) - 0.5 * sliceThicknessV(end - 1));


% DEMSWISSROLLLLE1 Demonstrate LLE on the oil data.

% MLTOOLS

[Y, lbls] = lvmLoadData('swissRoll');

options = lleOptions(4);
model = lleCreate(2, size(Y, 2), Y, options);
model = lleOptimise(model, 2);

lvmScatterPlotColor(model, model.Y(:, 2));

if exist('printDiagram') & printDiagram
  lvmPrintPlot(model, model.Y(:, 2), 'SwissRoll', 1, true);
end


function [x,fval,exitflag,info] = opti_mkltrnls(fun,grad,x0,ydata,lb,ub,opts)
%OPTI_MKLTRNLS Solve a NLS using MKLTRNLS (Intel MKL Trust Region Solver)
%
%   min sum[ (F(x) - ydata)^2 ]       subject to:   lb <= x <= ub
%    x
%
%   x = opti_mkltrnls(fun,grad,x0,ydata) solves a NLS where fun is the
%   fitting function. grad is an optional gradient of the fitting function
%   and x0 is a starting guess. ydata is the data to fit the function to.
%
%   x = opti_mkltrnls(fun,grad,x0,ydata,lb,ub) solves subject to decision
%   variables bounds lb <= x <= ub. Avoid Infinite bounds.
%
%   x = opti_mkltrnls(fun,...,ub,opts) uses opts to pass optiset options to
%   the solver.
%
%   [x,fval,exitflag,info] = opti_mkltrnls(...) returns the objective value
%   at the solution, together with the solver exitflag, and an information
%   structure.
%
%   THIS IS A WRAPPER FOR DTRNLS, DTRNLSBC and DJACOBI

%   Copyright (C) 2011 Jonathan Currie (IPL)

if(nargin < 7), opts = optiset; end
if(nargin < 6), ub = []; end
if(nargin < 5), lb = []; end
if(nargin < 4), error('MKLTRNLS requires at least 4 arguments'); end

%Setup display level
opts.display = dispLevel(opts.display);
opts.optiver = optiver;

%Check we have a valid x0
if(isempty(x0) || any(isnan(x0)))
    error('MKLTRNLS requires an initial guess, x0!');
end

%Addin mkltrnls settings if specified
if(isfield(opts,'solverOpts') && ~isempty(opts.solverOpts))
    mopts = mkltrnlsset(opts.solverOpts);
else
    mopts = [];
end
%Add OPTI Options
mopts.maxiter = opts.maxiter;
mopts.maxtime = opts.maxtime;
mopts.display = dispLevel(opts.display);
mopts.tolrfun = opts.tolrfun;
mopts.tolafun = opts.tolafun;
mopts.iterfun = opts.iterfun;

t = tic;
%MKLTRNLS requires x0 within bounds
x0 = movex0(lb,ub,x0);
% Run MKLTRNLS
[x, fval, exitflag, iter, feval, geval] = mkltrnls(fun,grad,x0,ydata,lb,ub,mopts);

%Collect Results
info.Iterations = iter;
info.FuncEvals = feval;
info.GradEvals = geval;
info.Time = toc(t);
info.Algorithm = 'MKLTRNLS: Intel MKL Trust Region NLS';

switch(exitflag)
    case 1
        info.Status = 'Optimal';
    case 0
        info.Status = 'Exceeded Iterations';
    case -1
        info.Status = 'Infeasible / Could not Converge';
    case -2
        info.Status = 'Singular / Error';
    case -5
        info.Status = 'User Exit';
    otherwise
        info.Status = 'MKLTRNLS Error';
end


%-------------------------------------------------------------------------------------------------------------
% This is an implementation of the TWSC algorithm for real-world image denoising
% by processing the YUV channels insead of RGB channels, it would improve
% TWSC by 0.1dB on PSNR.
% Author:  Jun Xu, csjunxu@comp.polyu.edu.hk / nankaimathxujun@gmail.com
%          The Hong Kong Polytechnic University
%
% Please refer to the following paper if you find this code helps:
%
% @article{TWSC_ECCV2018,
% 	author = {Jun Xu and Lei Zhang and David Zhang},
% 	title = {A Trilateral Weighted Sparse Coding Scheme for Real-World Image Denoising},
% 	journal = {ECCV},
%   year = {2018}
% }
% Please see the file License.txt for the license governing this code.
%-------------------------------------------------------------------------------------------------------------
clear,clc;
load '/home/csjunxu/Github/data/NTIRE2019/ValidationNoisyBlocksSrgb.mat';

method = 'TWSCrgb2yuv';
dataset = 'SIDD_2018';
% write image directory
write_MAT_dir = ['/home/csjunxu/Github/data/NTIRE2019/'];
% Parameters
Par.ps = 6;        % patch size
Par.step = 3;      % the step of two neighbor patches
Par.win = 20;      % size of window around the patch
Par.Outerloop = 4;
Par.Innerloop = 2;
Par.nlspini = 70;
Par.display = 0;
Par.delta = 0;
Par.nlspgap = 0; %10
Par.lambda1 = 0;
% Par.lambda2 = 3;
for lambda2 = [4]
    results = zeros(size(ValidationNoisyBlocksSrgb));
    Par.lambda2 =lambda2;
    save([write_MAT_dir 'TWSC' num2str(lambda2) '.mat'], 'results');
    for i = 1:size(ValidationNoisyBlocksSrgb,1)
        for j = 1:size(ValidationNoisyBlocksSrgb,2)
            Par.nlsp = Par.nlspini;  % number of non-local patches
            % iterate over bounding boxes
            Par.nim = im2double(squeeze(ValidationNoisyBlocksSrgb(i,j,:,:,:)));
            Par.I = Par.nim;
            Par.nim = rgb2ycbcr(Par.nim);
            [h,w,ch] = size(Par.nim);
            % noise estimation
            for c = 1:ch
                Par.nSig(c) = NoiseEstimation(Par.nim(:, :, c)*255, Par.ps)/255;
            end
            [IMout, Par]  =  TWSC_RW(Par);
            IMout = ycbcr2rgb(IMout);
            fprintf('%s/%s: \n', num2str(i), num2str(j));
            %% output
            results(i,j,:,:,:) = uint8(IMout*255);
        end
        fprintf('Image %d/%d done\n', i,j);
    end
end


function Cd = som_cldist(D,clinds1,clinds2,cldist,q,mask)

% SOM_CLDIST Distances between two clusters.
%
%   Cd = som_cldist(Md,c1,c2,'single')
%   Cd = som_cldist(Md,c1,c2,'average')
%   Cd = som_cldist(Md,c1,c2,'complete')
%   Cd = som_cldist(Md,c1,c2,'neighf',H)
%   Cd = som_cldist(Md,c1,[],...)
%   Cd = som_cldist(D,c1,c2,'centroid',q,mask)
%   Cd = som_cldist(D,c1,c2,'ward',q,mask)
%   Cd = som_cldist(D,c1,[],...)
%
%  Input and output arguments ([]'s are optional):
%   D        (matrix) size dlen x dim, the data set
%            (struct) map or data struct
%   Md       (matrix) size dlen x dlen, mutual distance matrix, see SOM_MDIST
%   c1       (cell array) size n1 x 1, indices of clusters from which
%                     the distances should be calculated, each cell
%                     contains indices of vectors that belong to that
%                     cluster (indices are between 1...dlen)
%   c2       (cell array) size n2 x 1, same as c1 but have the clusters
%                     to which the distances should be calculated
%            (empty)  c1 is used in place of c2
%   [q]      (scalar) distance norm, default = 2
%   [mask]   (vector) size dim x 1, the weighting mask, a vector of ones
%                     by default
%   H        (matrix) size dlen x dlen, neighborhood function values
%
%   Cd       (matrix) size n1 x n2, distances between the clusters
%
% See also SOM_MDIST.

% Copyright (c) 2000 by Juha Vesanto
% Contributed to SOM Toolbox on XXX by Juha Vesanto
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 2.0beta juuso 250800

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[dlen dim] = size(D);
if nargin<5, q = 2; end
if nargin<6, mask = ones(dim,1); end
if ~iscell(clinds1), clinds1 = {clinds1}; end
if ~isempty(clinds2) && ~iscell(clinds2), clinds2 = {clinds2}; end

n1 = length(clinds1);
n2 = length(clinds2);
if n2>0, Cd = zeros(n1,n2); else Cd = zeros(n1); end
if n1==0, return; end

switch cldist,

 % centroid distance %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 case 'centroid',

  C1 = zeros(n1,dim); for i=1:n1, C1(i,:) = mean(D(clinds1{i},:),1); end
  C2 = zeros(n2,dim); for i=1:n2, C2(i,:) = mean(D(clinds2{i},:),1); end
  if n2==0,
    for i=1:n1-1,
      for j=i+1:n1,
	diff = C1(i,:)-C1(j,:);
	switch q,
	 case 1,    Cd(i,j)=abs(diff)*mask;
	 case 2,    Cd(i,j)=sqrt((diff.^2)*mask);
	 case Inf,  Cd(i,j)=max(diag(mask)*abs(diff),[],2);
	 otherwise, Cd(i,j)=((abs(diff).^q)*mask).^(1/q);
	end
      end
      Cd([(i+1):n1],i) = Cd(i,[(i+1):n1])';
    end
  else
    for i=1:n1,
      for j=1:n2,
	diff = C1(i,:)-C2(j,:);
	switch q,
	 case 1,    Cd(i,j)=abs(diff)*mask;
	 case 2,    Cd(i,j)=sqrt((diff.^2)*mask);
	 case Inf,  Cd(i,j)=max(diag(mask)*abs(diff),[],2);
	 otherwise, Cd(i,j)=((abs(diff).^q)*mask).^(1/q);
	end
      end
    end
  end

 % ward distance %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 case 'ward',

  C1 = zeros(n1,dim); nn1 = zeros(n1,dim);
  for i=1:n1, C1(i,:) = mean(D(clinds1{i},:),1); nn1(i) = length(clinds1{i}); end
  C2 = zeros(n2,dim); nn2 = zeros(n2,dim);
  for i=1:n2, C2(i,:) = mean(D(clinds2{i},:),1); nn2(i) = length(clinds2{i}); end
  if n2==0,
    for i=1:n1-1,
      for j=i+1:n1,
	diff = C1(i,:) - C1(j,:);
	f = 2*nn1(i)*nn1(j) / (nn1(i)+nn1(j));
	switch q,
	 case 1,    Cd(i,j)=f*abs(diff)*mask;
	 case 2,    Cd(i,j)=f*sqrt((diff.^2)*mask);
	 case Inf,  Cd(i,j)=f*max(diag(mask)*abs(diff),[],2);
	 otherwise, Cd(i,j)=f*((abs(diff).^q)*mask).^(1/q);
	end
      end
      Cd([(i+1):n1],i) = Cd(i,[(i+1):n1])';
    end
  else
    for i=1:n1,
      for j=1:n2,
	diff = C1(i,:) - C2(j,:);
	f = 2*nn1(i)*nn2(j) / (nn1(i)+nn2(j));
	switch q,
	 case 1,    Cd(i,j)=f*abs(diff)*mask;
	 case 2,    Cd(i,j)=f*sqrt((diff.^2)*mask);
	 case Inf,  Cd(i,j)=f*max(diag(mask)*abs(diff),[],2);
	 otherwise, Cd(i,j)=f*((abs(diff).^q)*mask).^(1/q);
	end
      end
    end
  end

 % single linkage distance %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 case 'single',

  if n2==0,
    for i=1:n1-1,
      for j=i+1:n1,
	vd = D(clinds1{i},clinds1{j});
	fi = isfinite(vd(:));
	if any(fi), Cd(i,j) = min(vd(fi)); else Cd(i,j) = Inf; end
      end
      Cd([(i+1):n1],i) = Cd(i,[(i+1):n1])';
    end
  else
    for i=1:n1,
      for j=1:n2,
	vd = D(clinds1{i},clinds2{j});
	fi = isfinite(vd(:));
	if any(fi), Cd(i,j) = min(vd(fi)); else Cd(i,j) = Inf; end
      end
    end
  end

 % average linkage distance %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 case 'average',

  if n2==0,
    for i=1:n1-1,
      for j=i+1:n1,
	vd = D(clinds1{i},clinds1{j});
	fi = isfinite(vd(:));
	if any(fi), Cd(i,j) = mean(vd(fi)); else Cd(i,j) = Inf; end
      end
      Cd([(i+1):n1],i) = Cd(i,[(i+1):n1])';
    end
  else
    for i=1:n1,
      for j=1:n2,
	vd = D(clinds1{i},clinds2{j});
	fi = isfinite(vd(:));
	if any(fi), Cd(i,j) = mean(vd(fi)); else Cd(i,j) = Inf; end
      end
    end
  end

 % complete linkage distance %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 case 'complete',

   if n2==0,
    for i=1:n1-1,
      for j=i+1:n1,
	vd = D(clinds1{i},clinds1{j});
	fi = isfinite(vd(:));
	if any(fi), Cd(i,j) = max(vd(fi)); else Cd(i,j) = Inf; end
      end
      Cd([(i+1):n1],i) = Cd(i,[(i+1):n1])';
    end
  else
    for i=1:n1,
      for j=1:n2,
	vd = D(clinds1{i},clinds2{j});
	fi = isfinite(vd(:));
	if any(fi), Cd(i,j) = max(vd(fi)); else Cd(i,j) = Inf; end
      end
    end
  end

 % neighborhood function linkage distance %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 case 'neighf',

  if n2==0,
    for i=1:n1-1,
      for j=i+1:n1,
	vd = D(clinds1{i},clinds1{j});
	fi = isfinite(vd(:));
	if any(fi),
	  hd = q(clinds1{i},clinds1{j});
	  hd = hd(fi);
	  Cd(i,j) = sum(hd.*vd(fi))/sum(hd);
	else Cd(i,j) = Inf;
	end
      end
      Cd([(i+1):n1],i) = Cd(i,[(i+1):n1])';
    end
  else
    for i=1:n1,
      for j=1:n2,
	vd = D(clinds1{i},clinds2{j});
	fi = isfinite(vd(:));
	if any(fi),
	  hd = q(clinds1{i},clinds2{j});
	  hd = hd(fi);
	  Cd(i,j) = sum(hd.*vd(fi))/sum(hd);
	else Cd(i,j) = Inf;
	end
      end
    end
  end

 otherwise, error(['Unknown cluster distance metric: ' cldist]);
end

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [x,fval,exitflag,info] = opti_cplex(H,f,A,rl,ru,lb,ub,xint,sos,qc,x0,opts)
%OPTI_CPLEX Solve a LP/MILP/QP/MIQP/QCQP/MIQCQP using CPLEX
%
%   min 0.5*x'*H*x + f'*x      subject to:     rl <= A*x <= ru
%    x                                         qrl <= x'Q'x + l'x <= qru
%                                              lb <= x <= ub
%                                              for i = 1..n: xi in Z
%                                              for j = 1..m: xj in {0,1}
%
%   x = opti_cplex([],f,A,rl,ru,lb,ub,xint) solves a LP/MILP where f is the
%   objective vector, A,rl,ru are the linear constraints, lb,ub are the
%   bounds and xint is a string of integer variables ('C', 'I', 'B').
%
%   x = opti_cplex(H,f,A,rl,ru,lb,ub,xint) solves a QP/MIQP where H is the
%   objective matrix, and the remainder of the arguments are as above.
%
%   x = opti_cplex(H,...,xint,sos) sos is a structure with fields type,
%   index, weight for SOS.
%
%   x = opti_cplex(H,...,sos,qc) qc is structure with fields Q, l, qrl and
%   qru for quadratic constraints.
%
%   x = opti_cplex(H,...,qc,x0) supples an initial guess of the solution.
%
%   x = opti_cplex(H,f,...,x0,opts) uses opts to pass CPLEX parameters to
%   the solver. For example to set the node limit:
%       opts.mip.limits.nodes.Cur = 1000;
%
%   [x,fval,exitflag,info] = opti_cplex(...) returns the objective value at
%   the solution, together with the solver exitflag, and an information
%   structure.

%   Copyright (C) 2013 Jonathan Currie (I2C2)

t = tic;

% Handle missing arguments
if nargin < 12, opts = []; end
if nargin < 11, x0 = []; end
if nargin < 10, qc = []; end
if nargin < 9, sos = []; end
if nargin < 8, xint = []; end
if nargin < 7, ub = []; end
if nargin < 6, lb = []; end
if nargin < 5, error('You must supply at least 5 arguments to opti_cplex'); end

% Set Defaults (not returned from Cplex if infeasible)
x = []; fval = [];

%Check cplex exists
if(isempty(which('Cplex.p')))
    error('Cannot find Cplex the class! Please ensure the MATLAB Cplex interface is installed on your computer!');
end

%Build CPLEX Model
cp = Cplex('OPTI');
%Fill in common properties
cp.Model.obj = f;
if(isempty(lb))
    cp.Model.lb = -Inf(size(f));
else
    cp.Model.lb = lb;
end
if(isempty(ub))
    cp.Model.ub = Inf(size(f));
else
    cp.Model.ub = ub;
end
if(isempty(A))
    cp.Model.A = zeros(0,length(f));
    cp.Model.lhs = zeros(0,0);
    cp.Model.rhs = zeros(0,0);
else
    cp.Model.A = A;
    if(isempty(rl))
        cp.Model.lhs = -Inf(size(A,1),1);
    else
        cp.Model.lhs = rl;
    end
    if(isempty(ru))
        cp.Model.rhs = Inf(size(A,1),1);
    else
        cp.Model.rhs = ru;
    end
end
%Check and fill in remainder of optional args
if(~isempty(x0)),   cp.Start.x = x0; end
if(~isempty(H)),    cp.Model.Q = H; end
if(~isempty(xint)), cp.Model.ctype = upper(xint); end
if(~isempty(qc))
    %Check we have all fields
    if(~isstruct(qc) || ~isfield(qc,'Q') || ~isfield(qc,'l') || ~isfield(qc,'qrl') || ~isfield(qc,'qru'))
        error('Quadratic constraints must be a structure with fields ''Q'', ''l'', ''qrl'' and ''qru''');
    end
    if(iscell(qc.Q)) %multiple constraints
        if(length(qc.Q) ~= size(qc.l,2) || length(qc.qrl) ~= length(qc.Q) || length(qc.qrl) ~= length(qc.qru))
            error('Error with quadratic constraint dimensions');
        end
        for i = 1:length(qc.Q)
            addQC(cp,qc.Q{i},qc.l(:,i),qc.qrl(i),qc.qru(i));
        end
    else %single row constraint
        addQC(cp,qc.Q,qc.l,qc.qrl,qc.qru);
    end
end
if(~isempty(sos))
    %Check we have all fields
    if(~isstruct(sos) || ~isfield(sos,'type') || ~isfield(sos,'index') || ~isfield(sos,'weight'))
        error('SOS constraints must be a structure with fields ''type'', ''index'' and ''weight''');
    end
    %Ensure rows for Cplex
    if(iscell(sos.index) && size(sos.index,1) > size(sos.index,2))
        sos.index = sos.index';
    end
    if(iscell(sos.weight) && size(sos.weight,1) > size(sos.weight,2))
        sos.weight = sos.weight';
    end
    cp.addSOSs(sos.type,sos.index,sos.weight);
end

%Get objective constant term
if(~isempty(opts) && isfield(opts,'objbias') && ~isempty(opts.objbias))
    objbias = opts.objbias;
    opts = rmfield(opts,'objbias');
else
    objbias = 0;
end

%Assign User supplied options to Cplex
if(~isempty(opts))
    on = fieldnames(opts);
    if(length(on) == 1 && strcmpi('Param',on{1})) %remove Param if present
        opts = opts.Param;
    end
    %Bit of a hack to be able to set Cplex parameters, must be an easier way!
    fn = nestedfieldnames(opts);
    %Get cell arrays of each field
    cn = regexp(fn,'\.','split');
    %For each field, assign to param struct
    param = cp.Param;
    for i = 1:length(cn)
        try
            param = setfield(param,cn{i}{:},getfield(opts,cn{i}{:}));
        catch ME
            error('Error setting a Cplex parameter [%s]. Ensure you are setting the .Cur field of a valid parameter!\n\nError: %s',fn{i},ME.message);
        end
    end
    %Reassign parameters
    cp.Param = param;
end

%OPTI Default Parameters
cp.Param.output.clonelog.Cur = 0;

%Solve problem
sol = cp.solve();

%Assign Outputs
if(isfield(sol,'x')), x = sol.x; end
if(isfield(sol,'objval')), fval = sol.objval; end
if(isfield(sol,'itcnt')), info.Iterations = sol.itcnt; end
if(isfield(sol,'mipitcnt')), info.Nodes = sol.mipitcnt; end
if(isfield(sol,'miprelgap')), info.RelGap = sol.miprelgap; end
info.Time = toc(t); ex = '';
if(isfield(sol,'method'))
    switch(sol.method)
        case 0, ex = 'Automatic';
        case 1, ex = 'Primal Simplex';
        case 2, ex = 'Dual Simplex';
        case 3, ex = 'Network Simplex';
        case 4, ex = 'Barrier';
        case 5, ex = 'Sifting';
        case 6, ex = 'Concurrent Dual and Barrier';
        case 11, ex = 'Feasible Optimal';
        case 12, ex = 'MIP Solver';
        case 13, ex = 'Robust Algorithm';
    end
end
info.Algorithm = ['Cplex: ' ex];
if(isfield(sol,'statusstring'))
    info.Status = sol.statusstring;
    info.Status(1) = upper(info.Status(1));
else
    info.Status = 'Unknown';
end
if(isfield(sol,'status'))
    switch(sol.status)
        case {1,101,102,24}
            exitflag = 1; %ok
        case {25,10,11,12,105,107,111,112}
            exitflag = 0; %time/iterations/nodes
        case {3,5,103,115,106}
            exitflag = -1; %infeasible
        case {4,118,119}
            exitflag = -2; %unbounded
        case {13}
            exitflag = -5;
        otherwise
            exitflag = -6;
    end
else
    exitflag = -6;
end

%Add constant objective term
if(~isempty(fval))
    fval = fval + objbias;
end

%Assign Lambda
if(isfield(cp.Solution,'dual') && ~isempty(cp.Solution.dual))
    info.Lambda.lin = cp.Solution.dual; %not splitting up into ineq and eq as we are using row constraints
    rc = cp.Solution.reducedcost;
    zu = rc; zu(zu > 0) = 0;
    zl = rc; zl(zl < 0) = 0;
    info.Lambda.upper = zu;
    info.Lambda.lower = zl;
end

function addQC(cp,Q,l,rl,ru)
if(isinf(rl))
    cp.addQCs(l,Q,'L',ru);
elseif(isinf(ru))
    cp.addQCs(l,Q,'G',rl);
elseif(rl == ru)
    error('Cplex does not support quadratic equalities!');
else
    cp.addQCs(l,Q,'L',ru);
    cp.addQCs(l,Q,'G',rl);
end


function TessMat = in_tess_off(TessFile)
% IN_TESS_OFF: Read Geomview .off mesh files
%
% USAGE:  TessMat = in_tess_off(TessFile);
%
% INPUT:
%     - TessFile : full path to a tesselation file
% OUTPUT:
%     - TessMat:  Brainstorm tesselation structure
% FORMAT:
%     ASCII file with four blocks:
%     - Header line 1: "OFF"
%     - Header line 2: "nVertices nFaces ?"
%     - Vertices     : nVertices lines ("x y z"), values in miliimeters
%     - Faces        : nFaces lines ("nvert vertex1 vertex2 vertex3")
%
% SEE ALSO: in_tess

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2012

% Open tesselation file
fid = fopen(TessFile, 'r');
if fid < 0
    error(['Cannot open file ', TessFile])
end

% ===== READ HEADER =====
% Read format name
h1 = fgetl(fid);
if ~strcmpi(h1, 'OFF')
    error('Not a valid Geomview .off file.');
end
% Read number of vertices and faces
h2 = fgetl(fid);
dim = sscanf(h2, '%d', 2);
nVertices = dim(1);
nFaces    = dim(2);

% ===== READ MESH =====
% Read vertices
Vertices = double(fscanf(fid, '%f', [3 nVertices]));
% Go to next line
fgetl(fid);
% Read faces
Faces = double(fscanf(fid, '%f',[4 nFaces]) + 1);
Faces = Faces(2:4,:);
% Close file
fclose(fid);

%% ===== CONVERT IN BRAINSTORM FORMAT =====
TessMat.Vertices = Vertices';
TessMat.Faces    = Faces';


function legendre_polynomial_plot01 ( )

%*****************************************************************************80
%
%% LEGENDRE_POLYNOMIAL_PLOT01 tests P_POLYNOMIAL_PLOT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_POLYNOMIAL_PLOT01:\n' );
  fprintf ( 1, '  P_POLYNOMIAL_PLOT creates a plot of one or more Legendre functions.\n' );

  index = [ 0, 1, 2, 3, 4, 5 ];
  filename = 'p_polynomial_plot.png';

  p_polynomial_plot ( index, filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Legendre polynomial plot stored in file "%s".\n', filename );

  return
end


function p = createsdpcut(p,x)
if any(p.K.s)
    % Cuts are added from violated relaxations
    % Might improve bound propagation.
    top = startofSDPCone(p.K);
    newcuts = 1;
    newF = [];
    newSOCP = [];
    for i = 1:length(p.K.s)
        n = p.K.s(i);
        X = p.F_struc(top:top+n^2-1,:)*[1;x];
        X = full(reshape(X,n,n));
        [d,v] = eig(X);
        if any(diag(v)<0)
             for m = 1:length(v)
                if v(m,m)<0
                    for j = 1:length(x)+1
                        newF(newcuts,j)= d(:,m)'*reshape(p.F_struc(top:top+n^2-1,j),n,n)*d(:,m);
                    end
                    newF(newcuts,1)=newF(newcuts,1)+1e-6;
                    newcuts = newcuts + 1;
                    if size(p.lpcuts,1)>0
                        dist = p.lpcuts*newF(newcuts-1,:)'/(newF(newcuts-1,:)*newF(newcuts-1,:)');
                        if any(abs(dist-1)<1e-3)
                            newF = newF(1:end-1,:);
                            newcuts = newcuts - 1;
                        end
                    end
                end
            end
        end
        top = top+n^2;
    end

    if ~isempty(newF)
        m = size(newF,2);
        p.lpcuts = [newF;p.lpcuts];
        p.cutState = [ones(size(newF,1),1);p.cutState];
    end
    if ~isempty(newSOCP)
        p.socpcuts.F_struc = [p.socpcuts.F_struc;newSOCP];
        p.socpcuts.K.q = [p.socpcuts.K.q 3];
    end
end


function [geom_data, valid_ids] = Read_geom_files(users, model_param_data_dir)

    geom_data = [];
    valid_ids = [];

    load('../../pca_generation/pdm_68_aligned_wild.mat');

    for i=1:numel(users)

        geom_file = [model_param_data_dir, '/' users{i} '.csv'];

        if(i == 1)
            tab = readtable(geom_file);
            column_names = tab.Properties.VariableNames;
            valid_ind = cellfun(@(x) ~isempty(x) && x==1, strfind(column_names, 'confidence'));
            shape_inds = cellfun(@(x) ~isempty(x) && x==1, strfind(column_names, 'p_'));
        end

        res = dlmread(geom_file, ',', 1, 0);

        % Check the confidence of detection
        valid = res(:, valid_ind) > 0.7;
        res = res(:, shape_inds);

        % Do not consider global parameters
        res = res(:, 7:end);

        actual_locs = res * V';
        res = cat(2, actual_locs, res);

        valid_ids = cat(1, valid_ids, valid);

        geom_data = cat(1, geom_data, res);

    end
end


function varargout = max(varargin)
% global, local and pointwise maxima of spherical functions
%
% Syntax
%   [v,pos] = max(sF) % the position where the maximum is atained
%
%   [v,pos] = max(sF,'numLocal',5) % the 5 largest local maxima
%
%   sF = max(sF, c) % maximum of a spherical functions and a constant
%   sF = max(sF1, sF2) % maximum of two spherical functions
%   sF = max(sF1, sF2, 'bandwidth', bw) % specify the new bandwidth
%
%   % compute the maximum of a multivariate function along dim
%   sF = max(sFmulti,[],dim)
%
% Input
%  sF, sF1, sF2 - @S2Fun
%  sFmulti - a multivariate @S2Fun
%  c       - double
%
% Output
%  v - double
%  pos - @vector3d
%
% Options
%  kmax - number of iterations
%  numLocal      - number of peaks to return
%  startingNodes - @vector3d
%  tolerance     - minimum distance between two peaks
%  resolution    - minimum step size
%  maxStepSize   - maximm step size
%

varargin{1} = -varargin{1};
if nargin>1 && ( ~isempty(varargin{2}) && isa(varargin{2},'double') || isa(varargin{2},'S2Fun' ))
  varargin{2} = -varargin{2};
end

[varargout{1:nargout}] = min(varargin{:});

varargout{1} = - varargout{1};

end


%function [meanmap,pmap]=randperm(srp_data,compmat,numperm,corp)

function [realcomp]=randperm(srp_data,compmat,corp)

if nargin <2
    error('need to input raw data, comparison matrix,and p level');
end

if nargin <3
    corp=0.05;
end

a=size(srp_data);
if length(a)>2;
    error('for 2d only. use anovacont() or permtest()');
end

numsub=size(srp_data,1);
numcond=size(srp_data,2);

%disp('NOT correcting across the third dimension');

srp_data=srp_data(:,:,:,:);
%disp([(numsub*factorial(numcond)),' of your permutations will be the same as point estimate']);

%%calculate degrees of freedom
df_tot=(numsub*numcond)-1;
df_group=numcond-1;
df_subs=numsub-1;
df_error=df_tot-df_group-df_subs;

%    disp('calculating critical F....');

    smtot=squeeze(sum(srp_data,1));               %sum of each condition
    smtot1=squeeze(sum(srp_data,2));
    smmean=squeeze(mean(srp_data,1));          %mean of each condition
    ss_srp_data=srp_data.^2;                     %matrix of squared entries
    g_srp_data=squeeze(sum(smtot,2));        % this is G, the sum of all entries
    sum_ss_srp_data=sum(ss_srp_data(:)); %sum of squares
    sstot=sum_ss_srp_data-((g_srp_data.^2)/((numcond*numsub)));  %ss total
    ssgroup=((squeeze(sum(smtot.^2)))/(numsub))-((g_srp_data.^2)/(numcond*numsub));   %ss group
    sssubs=((squeeze(sum(smtot1.^2)))/numcond)-((g_srp_data.^2)/(numcond*numsub));   %ss group
    sserror=sstot-ssgroup-sssubs;
    msgroup=ssgroup/df_group;
    mserror=sserror/df_error;  %df error=36: 56(dftot)-2(dfgroup)-18(dfsubjects)
    realF=msgroup./(mserror+0.00001);         %this is the matrix of critical F values for frequency*time
    for c=1:size(compmat,1);
        for w=1:numcond;
        smtot2(w)=smmean(w)*compmat(c,w);
        end;
        comp1=squeeze(sum(smtot2));
        estvar1=(mserror./numsub)*(sum(compmat(c,:).^2));
        realcomp=comp1./sqrt(estvar1+0.000001);    %critical F values for frequency*time for each planned comparison
    end


% TFSA PLOTTING FACILITIES
%
% Plotting routines used to visualise data. The following types of
% graphs can be employed:
%
%  (i)   Plot
%  (ii)  Image
%  (iii) Pseudo-Colour
%  (iv)  Waterfall (1 and 2)
%  (v)   Mesh
%  (vi)  Surf
%  (vii) Contour
%  (viii)TFSAPL
%
% All plots except (i) can be used to plot time-frequency
% distributions. The time domain and frequency spectra one
% dimensional plots can be added to the figure along the two axes.
% The plots can be configured by specifying various options in the
% figure. The 'Expand' button can be used to reveal more options. On
% the actual plot figure the appearance of the plots can be altered
% from a popup menu which is activated by right clicking.
%
% EXPLANATION OF VARIOUS PLOTTING ROUTINES
%
%  (i) Plot
%
%      Used for one dimensional plots of signals, IF estimation,
%      wavelet coefficients, frequency spectra etc.
%
%  (ii) Image
%
%      Used to plot two dimensional matrices such as time-frequency
%      distributions and scalograms. Displays the matrix as an image.
%
%  (iii) Pseudo-Colour
%
%      Pseudo-colour plots a colour grid for each element of the input
%      matrix. Also used to plot two dimensional matrices such as
%      time-frequency distributions and scalograms.
%
%  (iv) Waterfall 1 and 2
%
%      Again also used to plot two dimensional matrices such as
%      time-frequency distributions and scalograms. This uses a
%      waterfall style plot of the matrix, using different viewing
%      angles which distinguish Waterfall 1 and Waterfall 2.
%
%  (v) Mesh
%
%      Same as the waterfall plots except that column lines are
%      drawn to produce a 'mesh' surface.
%
%  (vi) Surf
%
%      Equivalent to the mesh plot except that a 'surface' is now
%      defined over the mesh.
%
%  (vii) Contour
%
%      Contour plot of the two dimensional matrix where the
%      different coloured lines represent varying degrees of magnitudes.
%
%  (viii) TFSAL
%
%      TFSA graphing function specifically optimised for viewing
%      time-frequency distributions.
%
%
%
%
%  See Also:  tfsapl


%% Copyright (C) 2016 Lagu
%% Copyright (C) 2016 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym frac (@var{x})
%% Return the fractional part of a symbolic expression.
%%
%% Examples:
%% @example
%% @group
%% y = frac(sym(3)/2)
%%   @result{} y = (sym) 1/2
%%
%% syms x
%% rewrite(frac(x), 'floor')
%%   @result{} ans = (sym) x - ⌊x⌋
%% @end group
%% @end example
%%
%% @seealso{@@sym/ceil, @@sym/floor, @@sym/fix, @@sym/round}
%% @end defmethod

function y = frac(x)
  if (nargin ~= 1)
    print_usage ();
  end
  y = elementwise_op ('frac', x);
end

%!test
%! f1 = frac(sym(11)/10);
%! f2 = sym(1)/10;
%! assert (isequal (f1, f2))

%!test
%! d = sym(-11)/10;
%! c = sym(9)/10;
%! assert (isequal (frac (d), c))

%!test
%! d = sym(-19)/10;
%! c = sym(1)/10;
%! assert (isequal (frac (d), c))