function X = coeffs2vals( X )
%COEFFS2VALS Convert an array of Chebyshev--Fourier--Fourier of
%            coefficients to an array of values.
%
%   COEFFS2VALS( CFS ) computes the values on a tensor-product doubled-up
%   spherical grid of a function that is represented by a
%   Chebyshev-Fourier-Fourier expansion with expansion coefficients CFS.
%
% See also CHEBFUN.COEFFS2VALS, VALS2COEFFS.

% Copyright 2019 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

if isempty( X )
    return
end

[m, n, p] = size( X );

% Old slow approach:
% for k = 1:p
%     X(:,:,k) = chebtech2.coeffs2vals( X(:,:,k) );
%     X(:,:,k) = trigtech.coeffs2vals( X(:,:,k).' ).';
% end
% for j = 1:n
%     vj = reshape( X(:,j,:), m, p );
%     vj = trigtech.coeffs2vals( vj.' ).';
%     X(:,j,:) = reshape( vj, m, 1, p );
% end

% Faster approach, but code less readable:
if ( m > 1 )
    X(2:m-1, :, :) = X(2:m-1, :, :)/2;
    X = fft( vertcat(X, X(m-1:-1:2,:,:)), [], 1 );
    X = X(m:-1:1, :, :);
end

scl_p = (n*p)*even_odd_fix( p );
scl_n = even_odd_fix( n );
Enp = reshape(scl_n.'*scl_p, [1 n p]);

X = ifft(ifft(ifftshift(ifftshift(X.*repmat( Enp, m, 1, 1),2),3), [], 2),[],3);

end

function scl = even_odd_fix( n )

if ( mod(n, 2) )
    scl = (-1).^(-(n-1)/2:(n-1)/2);
else
    scl = (-1).^((-n/2):(n/2-1));
end

end


function y = logsumexp(x, dim)
% compute log(sum(exp(x),dim)) while avoiding numerical underflow
xmax = max(x, [], dim);
y    = xmax + log(sum(exp(bsxfun(@minus, x, xmax)), dim));
ind  = find(~isfinite(xmax));
if ~isempty(ind)
    y(ind) = xmax(ind);
end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% Script chap::2::script
% Density CEV Model

t = 10;                                      % maturity
f0 = 0.03;                                  % spot value

figname = 'Risk Neutral Density - CEV';

x = 0:0.001:2;                            % range

sigma_base = 0.2;                           % volatility base scenario
beta_base = 0.5;                            % CEV exponent base scenario

y_base = pcev(t,x,f0,sigma_base, beta_base);
legendname_base = 'Base';
%% Changing sigma_base
sigma_low = 0.15;
sigma_high = 0.25;

y_low = pcev(t,x,f0,sigma_low, beta_base);
y_high = pcev(t,x,f0,sigma_high, beta_base);

legendname_low = 'Changing \sigma low value';
legendname_high = 'Changing \sigma high value';

createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);

%% Changing beta_base
beta_low = 0.3;
beta_high = 0.7;

y_low = pcev(t,x,f0,sigma_base, beta_low);
y_high = pcev(t,x,f0,sigma_base, beta_high);

legendname_low = 'Changing \beta low value';
legendname_high = 'Changing \beta high value';

createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% Script chap::2::script
% Density Heston Model
%
%   uses the characteristic function of the Heston model
%
t = 10;                             % maturity
a = 600;                            % spot value
N = 512;                            % number of grid points
x = ( (0:N-1) - N/2 ) / a;          % range

figname = 'Risk Neutral Density - Heston-Hull-White';

vInst_base = 0.02;                  % instantanuous variance of base parameter set
vLong_base = 0.02;                  % long term variance of base parameter set
kappa_base = 0.1;                   % mean reversion speed of variance of base parameter set
omega_base = 0.2;                   % volatility of variance of base parameter set
rho_base = 0;                       % correlation of base parameter set

lambda_base = 0.1;
eta_base = 0.02;

icurveData = [0.999884333380315;0.996803132736937;0.993568709230647;0.990285301195274;0.986945903402709;0.983557350486521;0.980185549124449;0.976782934344041;0.973361992614499;0.969976793305220;0.966616749933289;0.962914317958160;0.959904777446077;0.920091903961326;0.882870065420196;0.847186544281939;0.812742515687365;0.779459552415061;0.747152463119429;0.715745016074346;0.685138723808460;0.655753392359115;0.627333845297308;0.599226698198774;0.572763319281569;0.547259133751455;0.523441996253080;0.499646068368557;0.477507905873099;0.456481811728753;0.436385788738282;0.417350253831050;0.399187111819286;0.381865611666566;0.365435617455498;0.349786183601181;0.334806921914717;0.320548897004994;0.306983265264429;0.294081800917050;0.282443547729164;0.269929224010243];
icurveDates = [734472;734501;734534;734562;734591;734622;734653;734683;734713;734744;734775;734807;734836;735202;735567;735931;736298;736663;737028;737393;737758;738125;738489;738854;739219;739585;739949;740316;740680;741046;741411;741776;742140;742507;742872;743237;743602;743967;744334;744698;745063;745429];
icurveInterpMethod = 'spline';
icurveType = 'Discount';
icurveSettle = 734471;
irdc = IRDataCurve(icurveType,icurveSettle,icurveDates,icurveData);
% cf (characteristic function) for the heston hull white model
f = @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0) .* cf_hullwhite(x+1i,t,0,lambda_base,eta_base,irdc);

legendname_base = 'Base parameter set';
y_base = fftdensity(f,a,N);         % density calculated from cf for base parameter set
%% Changing lambda_base
lambda_low = .005;                    % changing parameter (low value)
lambda_high = .5;                   % changing parameter (high value)

f_low = @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_low,eta_base,irdc);
f_high = @(x)  cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_high,eta_base,irdc);

y_low = fftdensity(f_low,a,N);
y_high = fftdensity(f_high,a,N);

legendname_low = 'Changing \lambda low value';
legendname_high = 'Changing \lambda high value';

% output density as figure
createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);
%% Changing eta_base
eta_low = .01;
eta_high = .03;

f_low =  @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_base,eta_low,irdc);
f_high =  @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_base,eta_high,irdc);

y_low = fftdensity(f_low,a,N);
y_high = fftdensity(f_high,a,N);

legendname_low = 'Changing \eta low value';
legendname_high = 'Changing \eta high value';

createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);

%% Changing curve_base
zero = -log(icurveData)./(icurveDates-icurveSettle)*360 - 0.01;
icurveData_low = exp(-zero .*(icurveDates-icurveSettle)/360);
zero = -log(icurveData)./(icurveDates-icurveSettle)*360 + 0.01;
icurveData_high = exp(-zero .*(icurveDates-icurveSettle)/360);

irdc_low = IRDataCurve(icurveType,icurveSettle,icurveDates,icurveData_low);
irdc_high = IRDataCurve(icurveType,icurveSettle,icurveDates,icurveData_high);

f_low =  @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_base,eta_base,irdc_low);
f_high =  @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_base,eta_base,irdc_high);

y_low = fftdensity(f_low,a,N);
y_high = fftdensity(f_high,a,N);

legendname_low = 'Changing curve parallel shift - low value';
legendname_high = 'Changing curve parallel shift - high value';

createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);


function mesh = genMesh3DType2(domain, nx, ny, nz)

%% Usage: mesh structure of a uniform cubic partition
%
% INPUTS:
% domain --- cubic domain = [xmin, xmax, ymin, ymax, zmin, zmax].
% nx --- the number of uniform partition in x direction.
% ny --- the number of uniform partition in y direction.
% nz --- the number of uniform partition in z direction.
%
% option.meshinfo --- basic (default): generate only p and t.
%                     all: enriched mesh information
%
% OUTPUTS:
% mesh --- a struct data contains mesh information.
%
% Last Modified: 08/07/2020 by Xu Zhang
%
%                A8-------------------A7        The Cube is divided into
%                /|                   /|        six congruent tetrahedrons
%               / |                  / |
%              /  |                 /  |
%             /   |                /   |        (1) A1-A2-A3-A7
%            /    |               /    |        (2) A1-A6-A2-A7
%          A5-----+-------------A6     |        (3) A1-A5-A6-A7
%           |     |             |      |        (4) A1-A8-A5-A7
%           |     |             |      |        (5) A1-A4-A8-A7
%           |     |             |      |        (6) A1-A3-A4-A7
%           |     A4------------+------A3
%           |     /             |      /
%           |    /              |     /
%           |   /               |    /
%           |  /                |   /
%           | /                 |  /
%           |/                  | /
%          A1-------------------A2
%
%% 1. Generate basic mesh info: p t
[p,T] = genMesh3DRectPT(domain, nx, ny, nz);
c1 = zeros(5,4);
c1(1,:) = [1,2,4,5];
c1(2,:) = [2,7,5,6];
c1(3,:) = [3,2,4,7];
c1(4,:) = [5,7,4,8];
c1(5,:) = [2,4,5,7];
c2 = zeros(5,4);
c2(1,:) = [1,6,8,5];
c2(2,:) = [1,3,6,2];
c2(3,:) = [8,6,3,7];
c2(4,:) = [1,8,3,4];
c2(5,:) = [1,3,8,6];

CBxy = zeros(nx,ny);
CBxy(1:2:nx,2:2:ny)=1;
CBxy(2:2:nx,1:2:ny)=1;
CB = repmat(CBxy,1,1,nz);
CB(:,:,1:2:nz) = repmat(1-CBxy,1,1,length(1:2:nz));
CB = reshape(CB,[],1);
tid1 = find(CB==1); tid2 = find(CB==0);

t = T(:,c1(1,:));
t(tid2,:) = T(tid2,c2(1,:));
for i = 2:5
    tmp = T(:,c1(i,:));
    tmp(tid2,:) = T(tid2,c2(i,:));
    t = [t;tmp];
end

mesh = struct('p',p,'t',t,'T',T);


function [B0,BV] = spm_MDP_DP(MDP)
% dynamic programming using active inference
% FORMAT [B0,BV] = spm_MDP_DP(MDP)
%
% MDP.A(O,N)      - Likelihood of O outcomes given N hidden states
% MDP.B{M}(N,N)   - transition probabilities among hidden states (priors)
% MDP.C(N,1)      - prior preferences (prior over future states)
%
% MDP.V(T - 1,P)  - P allowable policies (control sequences)
%
% B0      - optimal state action policy or transition matrix
% BV      - corresponding policy using value iteration
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_MDP_DP.m 6598 2015-11-11 19:48:30Z karl $

% set up and preliminaries
%==========================================================================

% generative model and initial states
%--------------------------------------------------------------------------
T     = size(MDP.V,1) + 1;        % number of outcomes
Ns    = size(MDP.B{1},1);         % number of hidden states
Nu    = size(MDP.B,2);            % number of hidden controls
p0    = exp(-8);                  % smallest probability

% likelihood model (for a partially observed MDP implicit in G)
%--------------------------------------------------------------------------
try
    A = MDP.A + p0;
catch
    A = speye(Ns,Ns) + p0;
end
A     = A*diag(1./sum(A));        % normalise
lnA   = log(A);                   % log probabilities
H     = sum(A.*lnA)';             % negentropy of observations

% transition probabilities (priors)
%--------------------------------------------------------------------------
for j = 1:Nu
    B{j}   = MDP.B{j} + p0;
    B{j}   = B{j}*diag(1./sum(B{j}));
    sB{j}  = B{j};
    rB{j}  = spm_softmax(log(B{j})');
    lnB{j} = log(B{j});
end

% terminal probabilities over outcomes (priors)
%--------------------------------------------------------------------------
try
    C = MDP.C;
catch
    C = zeros(No,1);
end

% asume constant preferences over states
%--------------------------------------------------------------------------
if size(C,2) ~= T
    C = C(:,end)*ones(1,T);
end
C = A'*diag(1./sum(A,2))*spm_softmax(C);
C = log(C);

% policies, states and their expectations
%--------------------------------------------------------------------------
V     = MDP.V;
Np    = size(V,2);                % number of allowable policies

% policy iteration
%==========================================================================
for s = 1:Ns

    % Variational iterations (hidden states)
    %======================================================================
    x     = zeros(Ns,T,Np) + 1/Ns;
    for k = 1:Np

        % gradient descent on free energy
        %------------------------------------------------------------------
        for i = 1:16

            % hiddens states (x)
            %--------------------------------------------------------------
            x(:,1,k) = 0;
            x(s,1,k) = 1;

            for j = 2:T

                % current state
                %----------------------------------------------------------
                xj   = x(:,j,k);
                qx   = log(xj);
                v    = 0;

                % evaluate free energy and gradients (v = dFdx)
                %----------------------------------------------------------
                if j > 1, v = v + qx - log(sB{V(j - 1,k)}*x(:,j - 1,k)); end
                if j < T, v = v      - log(rB{V(j    ,k)}*x(:,j + 1,k)); end

                % update
                %----------------------------------------------------------
                x(:,j,k) = spm_softmax(qx - v/4);
                F(j,k)   = xj'*v;

            end

            % convergence
            %--------------------------------------------------------------
            if i > 1
                dF = F0 - sum(F(:,k));
                if dF > 1/128, F0 = F0 - dF; else, break, end
            else
                F0 = sum(F(:,k));
            end

        end
    end

    % value of policies (Q)
    %======================================================================
    Q     = zeros(Np,1);
    for k = 1:Np

        % path integral of expected free energy
        %------------------------------------------------------------------
        for j = 2:T
            v    = C(:,j) - log(x(:,j,k)) + H;
            Q(k) = Q(k) + v'*x(:,j,k);

        end
    end

    % optimal transition from this state
    %======================================================================
    [u,k]   = max(Q);
    B0(:,s) = lnB{V(1,k)}(:,s);

end

if nargout < 2, return, end

% value iteration
%==========================================================================
V     = zeros(Ns,1);
C     = C(:,end);
g     = 1 - 1/T;
for i = 1:32
    for s  = 1:Ns

        % value of actions (Q)
        %------------------------------------------------------------------
        Q     = zeros(Nu,1);
        for k = 1:Nu
            Q(k)   = B{1,k}(:,s)'*(C + g*V);
        end

        % optimal transition from this state
        %------------------------------------------------------------------
        [u,k]   = max(Q);
        BV(:,s) = B{1,k}(:,s);

    end

    % optimal transition from this state
    %----------------------------------------------------------------------
    dV   = BV'*(C + g*V) - V;
    V    = V + dV;

    % convergence
    %----------------------------------------------------------------------
    if norm(dV) < 1e-2, break, end

end


%% Read shape

[X,T] = readOff('../data/meshes/sphere_102.off');
M = getMeshData(X,T,10); % compute 10 LB eigenfunctions for fun

%% Set up Gaussian blur function

blurTime = .005; % if this gets too small, distances get noisy
blurSteps = 50;

% blur = @(x) blurOnMesh(x,M,blurTime,blurSteps)+1e-300; % faster than pre-factored?
% blurTranspose = @(x) blurOnMesh(x,M,blurTime,blurSteps,1)+1e-300;

% structure = prefactorMeshBlur(M,blurTime,blurSteps,5000);
% blur = @(x) prefactoredBlur(x,structure,0)+1e-300;
% blurTranspose = @(x) prefactoredBlur(x,structure,1)+1e-300;

h = blurTime/blurSteps;
nv = M.numVertices;

% Sphere is small enough that we can just write down the heat kernel explicitly
blurInverse = spdiags(M.areaWeights,0,nv,nv) - h*M.cotLaplacian;
mtx = full(blurInverse) \ diag(M.areaWeights);
mtx = mtx^blurSteps;

mtx(mtx<1e-50) = 1e-50;

blur = @(x) mtx*x;
blurTranspose = @(x) mtx'*x;

%% Make boundary

[~,fixedVerts(1)] = min(X(:,1));
[~,fixedVerts(2)] = min(X(:,2));
[~,fixedVerts(3)] = min(X(:,3));
[~,fixedVerts(4)] = max(X(:,1));
[~,fixedVerts(5)] = max(X(:,2));
[~,fixedVerts(6)] = max(X(:,3));

nFixed = length(fixedVerts);

fixedDistributions = zeros(M.numVertices,nFixed);

close all;
for i=1:nFixed
    fixedDistributions(fixedVerts(i),i) = 1/M.areaWeights(fixedVerts(i));
end

fixedDistributions = fixedDistributions+1e-10;
% fixedDistributions = blur(fixedDistributions);
showDescriptor(M,fixedDistributions(:,1));

%% Compute average entropy of data

averageEntropy = -mean(sum(bsxfun(@times,M.areaWeights,(fixedDistributions.*log(fixedDistributions))),1));

%% Solve barycenter problem using this machinery

edges = [2 1; 3 1; 4 1];
edgeWeights = [1 1 1 1 1 1];

targetEntropy = averageEntropy + 1;

result = convolutionalPropagation(edges, edgeWeights, [2 3 4], ...
    fixedDistributions(:,1:3),M.areaWeights,blur,blurTranspose,targetEntropy);

barycenter = convolutionalBarycenter(fixedDistributions(:,1:3),[1 1 1],...
    M.areaWeights,blur,blurTranspose,targetEntropy);

close all

f = subplot(1,3,1);
showDescriptor(M,sum(fixedDistributions(:,1:3),2),[],[],[],f);
title('Boundary distributions');
colorbar off;

f = subplot(1,3,2);
showDescriptor(M,result(:,1),[],[],[],f);
title('Barycenter computed using graph algorithm');
colorbar off;

f = subplot(1,3,3);
showDescriptor(M,barycenter,[],[],[],f);
title('Barycenter computed using specialized barycenter algorithm')
colorbar off;

%% Solve displacement problem using this machinery

k = 9;

edges = [(1:(k-1))' (2:k)'];
edges = [edges ; edges(:,2) edges(:,1)];
edgeWeights = ones(1,size(edges,1));

targetEntropy = averageEntropy + 1.5;

result = convolutionalPropagation(edges, edgeWeights, [1 k], ...
    fixedDistributions(:,[1 6]),M.areaWeights,blur,blurTranspose,targetEntropy);

figure

for i=1:k
    f = subplot(1,k,i);
    showDescriptor(M,result(:,i),[],[],[],f);
    colorbar off;
end

%% Solve soft mapping problem

edges = [T(:,1) T(:,2); T(:,2) T(:,3); T(:,3) T(:,1)];
edges = sort(edges,2);
edges = unique(edges,'rows');
edges = [edges; edges(:,2) edges(:,1)];

edgeWeights = ones(size(edges,1),1);

targetEntropy = averageEntropy+1.5;

result = convolutionalPropagation(edges, edgeWeights, fixedVerts, ...
                    fixedDistributions,M.areaWeights,blur,blurTranspose,targetEntropy);

%% Show result

testVerts = randperm(M.numVertices);
nTests = 5;
testVerts = testVerts(1:nTests);

figure;
for i=1:length(testVerts)
    f = subplot(2,length(testVerts),i);
    ind = zeros(M.numVertices,1);
    ind(testVerts(i)) = 1;
    showDescriptor(M,ind,[],[],[],f);
    colorbar off;
    colormap hot;

    f = subplot(2,length(testVerts),i+length(testVerts));
    showDescriptor(M,result(:,testVerts(i)),[],[],[],f);
    colorbar off;
    colormap hot;
end


function [eMat,f2eMat,f2eIsDirMat] = mapface2edge(vMat,fMat)
% MAPFACE2EDGE creates a mapping from faces to edges
%
% Input:
%   regular:
%       vMat: double[nVerts,3] - coordinates of vertices
%       fMat: double[nFace,3] - indices of face vertices in vMat
%
% Output:
%   eMat: double[nEdges,2] - contains indices of vertices corresponding
%       to each edge
%
%   f2eMat: double[nFaces,3] - contains indices of edges for
%       each face in this order (1-2, 2-3, 1-3)
%
%   f2eIsDirMat: logical[nFaces,3] - contains true if face
%       references edge in a direct order (i.e. 1-2 for instance)
%       and false if reference is in an opposite order
%
% $Author: Peter Gagarinov, PhD  <pgagarinov@gmail.com> $
% $Copyright: Peter Gagarinov, PhD,
%            Moscow State University,
%            Faculty of Computational Mathematics and Computer Science,
%            System Analysis Department 2011-2016 $
%
trObj = triangulation(fMat,vMat);
eMat=trObj.edges;
nEdges=size(eMat,1);
nFaces=size(fMat,1);
%% Build Face to Edges map and edge orientation map for each face
fMidCVec=trObj.edgeAttachments(eMat);
indShiftVec=cellfun('length',fMidCVec);
indF2EVec=zeros(sum(indShiftVec),1);
indF2EVec(1)=1;
indF2EVec(1+cumsum(indShiftVec(1:end-1)))=ones(nEdges-1,1);
indF2EVec=cumsum(indF2EVec);
indFVec=[fMidCVec{:}].';
%edges for each face are expected to be oriented as
%1-2, 2-3 , 3-1
indEdgeNumVec=...
    all(fMat(indFVec,[1,2])==eMat(indF2EVec,:),2)...
    -all(fMat(indFVec,[2,1])==eMat(indF2EVec,:),2)...
    +2*all(fMat(indFVec,[2,3])==eMat(indF2EVec,:),2)...
    -2*all(fMat(indFVec,[3,2])==eMat(indF2EVec,:),2)...
    +3*all(fMat(indFVec,[1,3])==eMat(indF2EVec,:),2)...
    -3*all(fMat(indFVec,[3,1])==eMat(indF2EVec,:),2);
%
[~,indSortVec]=sortrows([indFVec,abs(indEdgeNumVec)]);
indF2EVec=indF2EVec(indSortVec);
f2eMat=reshape(indF2EVec,3,nFaces).';
f2eIsDirMat=reshape(indEdgeNumVec(indSortVec)>0,3,nFaces).';


function [xhk, pf] = particle_filter(sys, yk, pf, resampling_strategy)
%% Generic particle filter
%
% Note: when resampling is performed on each step this algorithm is called
% the Bootstrap particle filter
%
% Usage:
% [xhk, pf] = particle_filter(sys, yk, pf, resamping_strategy)
%
% Inputs:
% sys  = function handle to process equation
% yk   = observation vector at time k (column vector)
% pf   = structure with the following fields
%   .k                = iteration number
%   .Ns               = number of particles
%   .w                = weights   (Ns x T)
%   .particles        = particles (nx x Ns x T)
%   .gen_x0           = function handle of a procedure that samples from the initial pdf p_x0
%   .p_yk_given_xk    = function handle of the observation likelihood PDF p(y[k] | x[k])
%   .gen_sys_noise    = function handle of a procedure that generates system noise
% resampling_strategy = resampling strategy. Set it either to
%                       'multinomial_resampling' or 'systematic_resampling'
%
% Outputs:
% xhk   = estimated state
% pf    = the same structure as in the input but updated at iteration k
%
% Reference:
% [1] Arulampalam et. al. (2002).  A tutorial on particle filters for
%     online nonlinear/non-gaussian bayesian tracking. IEEE Transactions on
%     Signal Processing. 50 (2). p 174--188

%% Programmed by:
% Diego Andres Alvarez Marin (diegotorquemada@gmail.com)
% Universidad Nacional de Colombia at Manizales, February 29, 2012

%%
k = pf.k;
if k == 1
   error('error: k must be an integer greater or equal than 2');
end

%% Initialize variables
Ns = pf.Ns;                              % number of particles
nx = size(pf.particles,1);               % number of states

wkm1 = pf.w(:, k-1);                     % weights of last iteration
if k == 2
   for i = 1:Ns                          % simulate initial particles
      pf.particles(:,i,1) = pf.gen_x0(); % at time k=1
   end
   wkm1 = repmat(1/Ns, Ns, 1);           % all particles have the same weight
end

%%
% The importance sampling function:
% PRIOR: (this method is sensitive to outliers)   THIS IS THE ONE USED HERE
% q_xk_given_xkm1_yk = pf.p_xk_given_xkm1;

% OPTIMAL:
% q_xk_given_xkm1_yk = q_xk_given_xkm1^i_yk;
% Note this PDF can be approximated by MCMC methods: they are expensive but
% they may be useful when non-iterative schemes fail

%% Separate memory
xkm1 = pf.particles(:,:,k-1); % extract particles from last iteration;
xk   = zeros(size(xkm1));     % = zeros(nx,Ns);
wk   = zeros(size(wkm1));     % = zeros(Ns,1);

%% Algorithm 3 of Ref [1]
for i = 1:Ns
   % xk(:,i) = sample_vector_from q_xk_given_xkm1_yk given xkm1(:,i) and yk
   % Using the PRIOR PDF: pf.p_xk_given_xkm1: eq 62, Ref 1.
   xk(:,i) = sys(k, xkm1(:,i), pf.gen_sys_noise());

   % Equation 48, Ref 1.
   % wk(i) = wkm1(i) * p_yk_given_xk(yk, xk(:,i))*p_xk_given_xkm1(xk(:,i), xkm1(:,i))/q_xk_given_xkm1_yk(xk(:,i), xkm1(:,i), yk);

   % weights (when using the PRIOR pdf): eq 63, Ref 1
   wk(i) = wkm1(i) * pf.p_yk_given_xk(k, yk, xk(:,i));

   % weights (when using the OPTIMAL pdf): eq 53, Ref 1
   % wk(i) = wkm1(i) * p_yk_given_xkm1(yk, xkm1(:,i)); % we do not know this PDF
end;

%% Normalize weight vector
wk = wk./sum(wk);

%% Calculate effective sample size: eq 48, Ref 1
Neff = 1/sum(wk.^2);

%% Resampling
% remove this condition and sample on each iteration:
% [xk, wk] = resample(xk, wk, resampling_strategy);
%if you want to implement the bootstrap particle filter
resample_percentaje = 0.50;
Nt = resample_percentaje*Ns;
if Neff < Nt
   disp('Resampling ...')
   [xk, wk] = resample(xk, wk, resampling_strategy);
   % {xk, wk} is an approximate discrete representation of p(x_k | y_{1:k})
end

%% Compute estimated state
xhk = zeros(nx,1);
for i = 1:Ns;
   xhk = xhk + wk(i)*xk(:,i);
end

%% Store new weights and particles
pf.w(:,k) = wk;
pf.particles(:,:,k) = xk;

return; % bye, bye!!!

%% Resampling function
function [xk, wk, idx] = resample(xk, wk, resampling_strategy)

Ns = length(wk);  % Ns = number of particles

% wk = wk./sum(wk); % normalize weight vector (already done)

switch resampling_strategy
   case 'multinomial_resampling'
      with_replacement = true;
      idx = randsample(1:Ns, Ns, with_replacement, wk);
%{
      THIS IS EQUIVALENT TO:
      edges = min([0 cumsum(wk)'],1); % protect against accumulated round-off
      edges(end) = 1;                 % get the upper edge exact
      % this works like the inverse of the empirical distribution and returns
      % the interval where the sample is to be found
      [~, idx] = histc(sort(rand(Ns,1)), edges);
%}
   case 'systematic_resampling'
      % this is performing latin hypercube sampling on wk
      edges = min([0 cumsum(wk)'],1); % protect against accumulated round-off
      edges(end) = 1;                 % get the upper edge exact
      u1 = rand/Ns;
      % this works like the inverse of the empirical distribution and returns
      % the interval where the sample is to be found
      [~, idx] = histc(u1:1/Ns:1, edges);
   % case 'regularized_pf'      TO BE IMPLEMENTED
   % case 'stratified_sampling' TO BE IMPLEMENTED
   % case 'residual_sampling'   TO BE IMPLEMENTED
   otherwise
      error('Resampling strategy not implemented')
end;

xk = xk(:,idx);                    % extract new particles
wk = repmat(1/Ns, 1, Ns);          % now all particles have the same weight

return;  % bye, bye!!!


function J = lotkaObjectiveFCN_models(u,x,N,xref,u0,p,Q,R,Ru, select_model)
%% Cost function of nonlinear MPC for Lotka-Volterra system
%
% Inputs:
%   u:      optimization variable, from time k to time k+N-1
%   x:      current state at time k
%   Ts:     controller sample time
%   N:      prediction horizon
%   xref:   state references, constant from time k+1 to k+N
%   u0:     previous controller output at time k-1
%
% Output:
%   J:      objective function cost
%

%% Nonlinear MPC design parameters
% Q = diag([1,1,1]);
% R = 0.01;

%% Integrate system
if strcmp(select_model,'DelayDMDc')
    [xk,~] = lsim(p.sys,[p.udelay(1:N);u'],[0:N-1].*p.dt,[p.xdelay(:,1); x]-[p.xmean; p.xmean]);
    xk = xk(:,3:4);
    xk = xk + repmat(p.xmean',[N 1]); xk = xk';
elseif strcmp(select_model,'DMDc')
    [xk,~] = lsim(p.sys,[u' 0],[0:N].*p.dt,x-p.xmean);
    xk = xk(2:end,:) + repmat(p.xmean',[N 1]); xk = xk';
elseif strcmp(select_model,'SINDYc')
    Ns = size(x,1);
    xk = zeros(Ns,N+1); xk(:,1) = x;
    for ct=1:N
        % Obtain plant state at next prediction step.
        xk(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xk(:,ct),u(ct),p.dt,1,[],p);
    end
    xk = xk(:,2:N+1);
elseif strcmp(select_model,'NARX')
    Hu = [u',0];
    Hx = zeros(2,length(Hu)); Hx(:,1) = x;
    [Us,Ui,Si] = preparets(p.net,con2seq(Hu),{},con2seq(Hx));
    xk = p.net(Us,Ui,Si);
    xk = cell2mat(xk);
end

%% Cost Calculation
% Set initial plant states, controller output and cost
uk = u(1);
J = 0;
% Loop through each prediction step
for ct=1:N
    % Obtain plant state at next prediction step
    xk1 = xk(:,ct);

    % Accumulate state tracking cost from x(k+1) to x(k+N)
    J = J + (xk1-xref)'*Q*(xk1-xref);
    % Accumulate MV rate of change cost from u(k) to u(k+N-1)
    if ct==1
        J = J + (uk-u0)'*R*(uk-u0) + uk'*Ru*uk;
    else
        J = J + (uk-u(ct-1))'*R*(uk-u(ct-1)) + uk'*Ru*uk;
    end
    % Update uk for the next prediction step
    if ct<N
        uk = u(ct+1);
    end
end


%% BVAR tutorial: Minnesota prior and heteroskedasticity weights
% Author:   Filippo Ferroni and  Fabio Canova
% Date:     09/14/2020, revised 16/12/2020

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 1) baseline  forecasting
% 2) forecasting  with  heteroskedastic  weights
% 3) forecasting  with  optimal  heteroskedastic  weights
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all; clc; clear;

addpath ../../cmintools/
addpath ../../bvartools/

%% read the data
if exist('DataCovid.mat','file') == 2
    load DataCovid
else
    url = 'https://fred.stlouisfed.org/';
    c = fred(url);
    series    = {'PAYEMS','UNRATE','PCE','INDPRO','CPIAUCSL','PCEPILFE'};
    startdate = datenum('12/01/1988', 'mm/dd/yyyy');
    enddate   = datenum('07/01/2020', 'mm/dd/yyyy');
    time      = 1988+11/12 : 1/12 : 2020+6/12;

    y=zeros(380,size(series,2)); y0=y; y1=y;
    for kk=1:size(series,2)
        tmp        = fetch(c,series{kk},startdate,enddate);
        if strcmp('UNRATE',series{kk})==1
            y(:,kk)    = tmp.Data(:,2);
            y0(:,kk)    = tmp.Data(:,2);
            y1(:,kk)  =(y0(:,kk));
        else
            y0(:,kk)    = tmp.Data(:,2);
            y1(:,kk)    = log(y0(:,kk));
            % rebase the log variable, transform to a index (2020m1=100)
            y(:,kk)= y1(:,kk) * 100 / y1(time==2019,kk);
        end
    end
    save DataCovid y y1 y0 time
end

% y, y0, y1 are  in DataCovid.

%% Forecast post COVID-19 (July 2020 - last insample data). Minnesota
lags                 = 13;
options.priors.name  = 'Minnesota';
options.fhor         = 24;
options.K            = 1000;
bvar0                = bvar_(y,lags,options);

options.nplots       = [2 3];
% start of the forecast plot - default first date in-sample data
options.time_start   = 2019;
% Titles for subplot
options.varnames = {'PAYMS','UNRATE','PCE','INDPRO','CPIAUCSL','PCEPILFE'};
% multiple credible set - default .68
options.conf_sig_2   = 0.9;
plot_frcst_(bvar0.forecasts.with_shocks,y,time,options)
pause;

% Forecast post COVID-19 (July 2020 - last insample data).
% Minnesota + Heteroskedasticity weights

lags = 13;
tstar  = find(time==2020) + 2; % pick march 2020
% scale the variables by factor <1 in the periods that
% characterize the COVID-19 induced recession
st                   = ones(size(y,1),1);
st(tstar:tstar+2 ,:) = [10 10 10]; % March, April, May
st(1:lags)           = [];

options.heterosked_weights = st;
options.priors.name  = 'Minnesota';
options.fhor         = 24;
bvar1                = bvar_(y,lags,options);

% adding  forecast  without  weights
options.add_frcst = [y; mean(bvar0.forecasts.no_shocks,3)];
plot_frcst_(bvar1.forecasts.with_shocks,y,time,options)
pause;

% Forecast  with  optimal  weights

hyperpara(1)    = 3;		  % tau
hyperpara(2)    = 0.5;		  % decay
hyperpara(3)    = 1;		  % lambda
hyperpara(4)    = 1;		  % mu
hyperpara(5)    = 2;		  % omega
hyperpara(6)    = 2; % s0: scale march 2020
hyperpara(7)    = 2; % s1: scale april 2020
hyperpara(8)    = 2; % s2: scale may   2020
% setting the options
options.index_est          = [1 6:8]; % hyper-parameter over which maximize
options.objective_function = 'bvar_opt_heterosked';
options.tstar              = find(time==2020) + 2; %march 2020
[postmode,logmlike,HH] = bvar_max_hyper(hyperpara,y,lags,options);

disp('weights for 2020:3, 2020:4, 2020:5')
disp(postmode(2:4))
heterosked_esse             = postmode(2:end); % s0, s1, s2
esse                        = ones(size(y,1),1);
esse(options.tstar : options.tstar+2 ,:)   = heterosked_esse;
esse(1:lags)                = [];

options.heterosked_weights = esse;
options.minn_prior_tau = postmode(1);
bvar2                 = bvar_(y,lags,options);
plot_frcst_(bvar2.forecasts.with_shocks,y,time,options)


function y = idualtree3D(w, J, Fsf, sf)

% Inverse 3D Dual-Tree Discrete Wavelet Transform
%
% USAGE:
%   y = idualtree3D(w, J, Fsf, sf)
% INPUT:
%   w - wavelet coefficients
%   J - number of stages
%   Fsf - synthesis filter for the last stage
%   sf - synthesis filters for the preceeding stages
% OUTPUT:
%   y - output arry
% See dualtree3D
%
% WAVELET SOFTWARE AT POLYTECHNIC UNIVERSITY, BROOKLYN, NY
% http://taco.poly.edu/WaveletSoftware/

for k = 1:J
    for m = 1:7
        [w{k}{1}{m} w{k}{2}{m} w{k}{3}{m} w{k}{4}{m}] = ...
            pm4inv(w{k}{1}{m}, w{k}{2}{m}, w{k}{3}{m}, w{k}{4}{m});
    end
end

M = [
    1 1 1
    2 2 1
    2 1 2
    1 2 2
];

% initialize output array
y = zeros(2^J * size(w{J}{1}{1}));

for i = 1:4
    f1 = M(i,1);
    f2 = M(i,2);
    f3 = M(i,3);
    yi = w{J+1}{i};
    for k = J:-1:2
        yi = sfb3D(yi, w{k}{i}, sf{f1}, sf{f2}, sf{f3});
    end
    yi = sfb3D(yi, w{1}{i}, Fsf{f1}, Fsf{f2}, Fsf{f3});
    y = y + yi;
end

% normalization
y = y/2;


%DEMO_MULTICLASS  Classification problem demonstration for 3 classes
%                 using Gaussian process prior
%
%  Description
%    The data used in the demonstration program is the same used by
%    Radford M. Neal in his three-way classification example in
%    Software for Flexible Bayesian Modeling
%    (http://www.cs.toronto.edu/~radford/fbm.software.html) The
%    data consists of 1000 4-D vectors which are classified into
%    three classes. The data is generated by drawing the components
%    of vector, x1, x2, x3 and x4, uniformly form (0,1). The class
%    of each vector is selected according to the first two
%    components of the vector, x_1 and x_2. After this a Gaussian
%    noise with standard deviation of 0.1 has been added to every
%    component of the vector. Because there are two irrelevant
%    components in the input vector a prior with ARD should be of
%    help.
%
%    The data is divided into two parts, trainig set of 400 units
%    and test set of 600 units.
%
%    The latent values for N training points and C classes are
%    f=(f1_1,f2_1,...,fN_1,f1_2,f2_2,...,fN_2,...,f1_C,f2_C,...,fN_C)^T,
%    and are given a zero mean Gaussian process prior
%
%      f ~ N(0, K),
%
%    where K is a block diagonal covariance matrix with blocks
%    K_1,...,K_C whose elements are given by K_ij = k(x_i, x_j |
%    th). The function k(x_i, x_j | th) is covariance function and
%    th its parameters.
%
%    In this demo we approximate the posterior distribution with
%    Laplace approximation.
%

% Copyright (c) 2010 Jaakko Riihim�ki, Jarno Vanhatalo, Aki Vehtari

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

% Load the data
S = which('demo_multiclass');
L = strrep(S,'demo_multiclass.m','demodata/cdata.txt');
x=load(L);
y=repmat(0,size(x,1),3);
y(x(:,5)==0,1) = 1;
y(x(:,5)==1,2) = 1;
y(x(:,5)==2,3) = 1;
x(:,end)=[];

% Divide the data into training and test parts.
xt = x(401:end,:);
x=x(1:400,:);
yt=y(401:end,:);
y=y(1:400,:);

[n, nin] = size(x);

% Create covariance functions
gpcf1 = gpcf_sexp('lengthScale', ones(1,nin), 'magnSigma2', 1);
% Set the prior for the parameters of covariance functions
pl = prior_t('s2',10,'nu',10);
pm = prior_sqrtt('s2',10,'nu',10);
gpcf1 = gpcf_sexp(gpcf1, 'lengthScale_prior', pl,'magnSigma2_prior', pm);

% Create the GP structure
gp = gp_set('lik', lik_softmax, 'cf', gpcf1, 'jitterSigma2', 1e-2);

% ------- Laplace approximation --------
fprintf(['Softmax model with Laplace integration over the latent\n' ...
         'values and MAP estimate for the parameters\n'])

% Set the approximate inference method
gp = gp_set(gp, 'latent_method', 'Laplace');

% We could also use own covariance function for each output the following
% way
% gp2 = gp_set('lik', lik_softmax, 'cf', {gpcf1 gpcf1 gpcf1}, 'jitterSigma2', 1e-2);
% gp2 = gp_set(gp2, 'latent_method', 'Laplace');
% gp2.comp_cf = {1 2 3};
% [Eft2, Varft2, ~, ~, pyt2] = gp_pred(gp2, x, y, xt, 'yt', ones(size(yt)));

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3,'Display','iter');
% Optimize with the BFGS quasi-Newton method
gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

% make the prediction for test points
[Eft, Varft, lpyt] = gp_pred(gp, x, y, xt, 'yt', ones(size(yt)));
Eft = reshape(Eft, size(xt,1), size(yt,2));
lpyt = reshape(lpyt, size(xt,1), size(yt,2));

% calculate the percentage of misclassified points
tt = exp(lpyt)==repmat(max(exp(lpyt),[],2),1,size(exp(lpyt),2));
missed = (sum(sum(abs(tt-yt)))/2)/size(yt,1)

% grid for making prediction
xtg1 = meshgrid(linspace(min(x(:,1))-.1, max(x(:,1))+.1, 30));
xtg2 = meshgrid(linspace(min(x(:,2))-.1, max(x(:,2))+.1, 30))';
xtg=[xtg1(:) xtg2(:) repmat(mean(x(:,3:4)), size(xtg1(:),1),1)];

[Eft, Covft, pg] = gp_pred(gp, x, y, xtg, 'yt', ones(size(xtg,1),3));
Eft = reshape(Eft, size(xtg,1), 3);
pg=reshape(pg,size(xtg,1),3);

% plot the train data o=0, x=1
figure, set(gcf, 'color', 'w'), hold on
plot(x(y(:,1)==1,1),x(y(:,1)==1,2),'ro', 'linewidth', 2);
plot(x(y(:,2)==1,1),x(y(:,2)==1,2),'x', 'linewidth', 2);
plot(x(y(:,3)==1,1),x(y(:,3)==1,2),'kd', 'linewidth', 2);
axis([-0.4 1.4 -0.4 1.4])
contour(xtg1, xtg2, reshape(exp(pg(:,1)),30,30),'r', 'linewidth', 2)
contour(xtg1, xtg2, reshape(exp(pg(:,2)),30,30),'b', 'linewidth', 2)
contour(xtg1, xtg2, reshape(exp(pg(:,3)),30,30),'k', 'linewidth', 2)

% MCMC approach

% Set the approximate inference method
% Note that MCMC for latent values requires often more jitter
lat = gp_pred(gp, x, y, x);
gp = gp_set(gp, 'latent_method', 'MCMC', 'jitterSigma2', 1e-4);
gp = gp_set(gp, 'latent_opt', struct('method',@scaled_mh));
gp.latentValues = lat(:);

gp_e(gp_pak(gp), gp, x,y)
gp_g(gp_pak(gp), gp, x,y)
gradcheck(randn(size(gp_pak(gp))), @gp_e, @gp_g, gp, x, y);

% Set the parameters for MCMC...
hmc_opt.steps=10;
hmc_opt.stepadj=0.001;
hmc_opt.nsamples=1;
latent_opt.display=0;
latent_opt.repeat = 20;
latent_opt.sample_latent_scale = 0.05;
hmc2('state', sum(100*clock))

% Sample
[r,g,opt]=gp_mc(gp, x, y, 'hmc_opt', hmc_opt, 'latent_opt', latent_opt, 'nsamples', 1, 'repeat', 15);

% re-set some of the sampling options
hmc_opt.repeat=1;
hmc_opt.steps=4;
hmc_opt.stepadj=0.02;
latent_opt.repeat = 5;
hmc2('state', sum(100*clock));

% Sample
[rgp,g,opt]=gp_mc(gp, x, y, 'nsamples', 400, 'hmc_opt', hmc_opt, 'latent_opt', latent_opt, 'record', r);
% Remove burn-in
rgp=thin(rgp,102);

% Make predictions
[Efs_mc, Varfs_mc, pgs_mc] = gpmc_preds(rgp, x, y, xtg, 'yt', ones(size(xtg,1),3));

Ef_mc = reshape(mean(Efs_mc,2),900,3);
pg_mc = reshape(mean(exp(pgs_mc),2),900,3);

figure, set(gcf, 'color', 'w'), hold on
plot(x(y(:,1)==1,1),x(y(:,1)==1,2),'ro', 'linewidth', 2);
plot(x(y(:,2)==1,1),x(y(:,2)==1,2),'x', 'linewidth', 2);
plot(x(y(:,3)==1,1),x(y(:,3)==1,2),'kd', 'linewidth', 2);
axis([-0.4 1.4 -0.4 1.4])
contour(xtg1, xtg2, reshape(pg_mc(:,1),30,30),'r', 'linewidth', 2)
contour(xtg1, xtg2, reshape(pg_mc(:,2),30,30),'b', 'linewidth', 2)
contour(xtg1, xtg2, reshape(pg_mc(:,3),30,30),'k', 'linewidth', 2)

% With scaled HMCS

gp2 = gp_set(gp, 'latent_opt', struct('method',@scaled_hmc));
gp2.latentValues = lat(:);

% Set the parameters for MCMC...
hmc_opt.steps=10;
hmc_opt.stepadj=0.001;
hmc_opt.nsamples=1;

% latent opt
latent_opt.nsamples=1;
latent_opt.nomit=0;
latent_opt.persistence=0;
latent_opt.repeat=20;
latent_opt.steps=20;
latent_opt.stepadj=0.15;
latent_opt.window=5;
hmc2('state', sum(100*clock))

% Here we make an initialization with
% slow sampling parameters

[rgp2,gp2,opt]=gp_mc(gp2, x, y, 'hmc_opt', hmc_opt, 'latent_opt', latent_opt, 'nsamples', 1, 'repeat', 15);

% re-set some of the sampling options
hmc_opt.repeat=1;
hmc_opt.steps=4;
hmc_opt.stepadj=0.02;
latent_opt.repeat = 20;
hmc2('state', sum(100*clock));

% Sample
[rgp2,g2,opt2]=gp_mc(gp2, x, y, 'nsamples', 200, 'hmc_opt', hmc_opt, 'latent_opt', latent_opt, 'record', rgp2);
% Remove burn-in
rgp2=thin(rgp2,102);

% Make predictions
[Efs_mc, Varfs_mc, pgs_mc] = gpmc_preds(rgp2, x, y, xtg, 'yt', ones(size(xtg,1),3));

Ef_mc2 = reshape(mean(Efs_mc,2),900,3);
pg_mc2 = reshape(mean(exp(pgs_mc),2),900,3);

figure, set(gcf, 'color', 'w'), hold on
plot(x(y(:,1)==1,1),x(y(:,1)==1,2),'ro', 'linewidth', 2);
plot(x(y(:,2)==1,1),x(y(:,2)==1,2),'x', 'linewidth', 2);
plot(x(y(:,3)==1,1),x(y(:,3)==1,2),'kd', 'linewidth', 2);
axis([-0.4 1.4 -0.4 1.4])
contour(xtg1, xtg2, reshape(pg_mc2(:,1),30,30),'r', 'linewidth', 2)
contour(xtg1, xtg2, reshape(pg_mc2(:,2),30,30),'b', 'linewidth', 2)
contour(xtg1, xtg2, reshape(pg_mc2(:,3),30,30),'k', 'linewidth', 2)


function S = globMatrixNed3DMass(fun,mesh,fem1,fem2)

%% USAGE: generate stiffness global matrix on a 3D mesh
%
% INPUTS:
% fun --- coefficient function
% mesh --- a struct data contains very rich mesh information.
% fem1 --- global DoF for test function space
% fem2 --- global DoF for trial function space
%
% OUTPUTS:
% [IN JN XN] --- triplets of the sparse matrix from regular elements.
% [II JI XI] --- triplets of the sparse matrix from interface elements.

% Last Modified: 08/07/2020 by Xu Zhang

%% 0. Initializaiton
feEvalBas1 = @EvalNed1Bas3D;
feEvalBas2 = @EvalNed1Bas3D;

dof1 = fem1.ldof; dof2 = fem2.ldof; nloc = dof1*dof2;
nt = length(mesh.t);
A = fem1.area; gx = fem1.gx; gy = fem1.gy; gz = fem1.gz; gw = fem1.gw;
X = zeros(nloc*nt, 1);

coef = feval(fun,gx,gy,gz);
Ibasx = cell(dof1,1); Ibasy = cell(dof1,1); Ibasz = cell(dof1,1);
Jbasx = cell(dof2,1); Jbasy = cell(dof2,1); Jbasz = cell(dof2,1);
for i = 1:dof1
    Ibasx{i} = feEvalBas1(fem1.bas, ':', gx, gy, gz, i, 0, 1).*fem1.t_e_orit(:,i);
    Ibasy{i} = feEvalBas1(fem1.bas, ':', gx, gy, gz, i, 0, 2).*fem1.t_e_orit(:,i);
    Ibasz{i} = feEvalBas1(fem1.bas, ':', gx, gy, gz, i, 0, 3).*fem1.t_e_orit(:,i);
end
for j = 1:dof2
    Jbasx{j} = feEvalBas2(fem2.bas, ':', gx, gy, gz, j, 0, 1).*fem2.t_e_orit(:,j);
    Jbasy{j} = feEvalBas2(fem2.bas, ':', gx, gy, gz, j, 0, 2).*fem2.t_e_orit(:,j);
    Jbasz{j} = feEvalBas2(fem2.bas, ':', gx, gy, gz, j, 0, 3).*fem2.t_e_orit(:,j);
end

I = reshape(repmat(fem1.g2ldof,6,1),nloc*nt,1);
J = repmat(reshape(fem2.g2ldof,dof2*nt,1),6,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        X(ind+1:ind+nt) = A.*(sum(((Ibasx{i}.*(coef.*Jbasx{j})).*gw'),2) + ...
            sum(((Ibasy{i}.*(coef.*Jbasy{j})).*gw'),2) + ...
            sum(((Ibasz{i}.*(coef.*Jbasz{j})).*gw'),2));
        ind = ind + nt;
    end
end
ID = find(X~=0);
S = sparse(I(ID),J(ID),X(ID),size(fem1.gdof,1),size(fem2.gdof,1));


function p06_limit_test ( option )

%*****************************************************************************80
%
%% P06_LIMIT_TEST seeks limit points for problem 6.
%
%  Discussion:
%
%    We want to find points X such that TAN(7) = 0.
%
%    The number of limit points that may be expected depends on the option:
%
%    There are five options, which vary in the value they fix the
%    elevator value X(6):
%
%      Option   Elevator Value    Limit Points
%
%       1        -0.050              1
%       2        -0.008              3
%       3         0.0                2
%       4         0.05               1
%       5         0.1                1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Raman Mehra, William Kessel, James Carroll,
%    Global stability and contral analysis of aircraft at high angles of attack,
%    Technical Report CR-215-248-1, -2, -3,
%    Office of Naval Research, June 1977.
%
%    Rami Melhem, Werner Rheinboldt,
%    A Comparison of Methods for Determining Turning Points of Nonlinear Equations,
%    Computing,
%    Volume 29, Number 3, September 1982, pages 201-226.
%
%    Albert Schy, Margery Hannah,
%    Prediction of Jump Phenomena in Roll-coupled Maneuvers of Airplanes,
%    Journal of Aircraft,
%    Volume 14, Number 4, 1977,  pages 375-382.
%
%    John Young, Albert Schy, Katherine Johnson,,
%    Prediction of Jump Phenomena in Aircraft Maneuvers, Including
%    Nonlinear Aerodynamic Effects,
%    Journal of Guidance and Control,
%    Volume 1, Number 1, 1978, pages 26-31.
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%    1, X(6) fixed at -0.050, X(7) free, X(8) fixed at 0.0;
%    2, X(6) fixed at -0.008, X(7) free, X(8) fixed at 0.0;
%    3, X(6) fixed at  0.000, X(7) free, X(8) fixed at 0.0;
%    4, X(6) fixed at  0.050, X(7) free, X(8) fixed at 0.0;
%    5, X(6) fixed at  0.100, X(7) free, X(8) fixed at 0.0.
%
  problem = 6;
  lim = 7;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'P06_LIMIT_TEST\n' );
  fprintf ( 1, '  Compute a series of solutions for problem 6.\n' );
  fprintf ( 1, '  We are trying to find limit points X such that\n' );
  fprintf ( 1, '  TAN(%d) = 0.\n', lim );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The option chosen is %d\n', option );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   #   Tan(7)     X1       X2       X3       X4       X5       X6       X7      X8\n' );
  fprintf ( 1, '                Roll     Pitch    Yaw      Attack   Sideslip Elevator Aileron Rudder\n' );
  fprintf ( 1, '\n' );
%
%  Get problem size.
%
  nvar = p00_nvar ( problem, option );

  lim_num = 0;
%
%  Get starting point.
%
  x2 = p00_start ( problem, option, nvar );
%
%  Get the tangent vector.
%
  tan2 = p00_tan ( problem, option, nvar, x2 );
%
%  For correction of initial point, use variable index 7.
%
  par_index = 7;
%
%  Force F(X) = 0.
%
  step = -1;
  fprintf ( 1, '  %2d %8.1e %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n', ...
    step, tan2(lim), x2(1:nvar) );

  [ x2, status ] = p00_newton ( problem, option, nvar, x2, par_index );

  if ( status < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Newton iteration failed on starting point.\n' );
    return
  end

  tan2 = p00_tan ( problem, option, nvar, x2 );

  step = 0;
  fprintf ( 1, '  %2d %8.1e %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n', ...
    step, tan2(lim), x2(1:nvar) );
%
%  Get stepsize.
%
  [ h, hmin, hmax ] = p00_stepsize ( problem, option );
%
%  LOOP:
%
  step_max = 30;

  for step = 1 : step_max
%
%  Save old data as X1, TAN1.
%
    x1 = x2;
    tan1 = tan2;

    h_reduction = 0;
%
%  Use X1 + H * TAN1 as a starting estimate for Newton iteration.
%
    while ( 1 )

      if ( hmax < abs ( h ) )
        h = hmax * r8_sign ( h );
      end

      if ( abs ( h ) < hmin )
        h = hmin * r8_sign ( h );
      end

      x2 = x1 + h * tan1;

      par_index = 0;
      [ x2, status ] = p00_newton ( problem, option, nvar, x2, par_index );
%
%  If we didn't get it, can we try again?
%
      if ( status < 0 )

        if ( abs ( h ) <= hmin )
          fprintf ( 1, '\n' );
          fprintf ( 1, '  Cannot decrease stepsize any more.\n' );
          fprintf ( 1, '  Cannot complete the computation.\n' );
          return
        else
          h = h / 4.0;
          h_reduction = h_reduction + 1;
        end
%
%  We computed the point.
%  Should we change the stepsize?
%
      else

        if ( h_reduction == 0 )

          if ( status <= 1 )
            h = h * 4.0;
          elseif ( status <= 3 )
            h = h * 2.0;
          elseif ( 12 <= status )
            h = h / 4.0;
          elseif ( 8 <= status )
            h = h / 2.0;
          end

        end

        break;

      end

    end
%
%  Compute the tangent vector.
%
    tan2 = p00_tan ( problem, option, nvar, x2 );
%
%  Check for a limit point.
%
    if ( tan1(lim) * tan2(lim) <= 0.0 )
      [ x, tan, status ] = p00_limit ( problem, option, nvar, x1, tan1, x2, tan2, lim );
      fprintf ( 1, '   L %8.1e %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n', ...
        tan(lim), x(1:nvar) );
      lim_num = lim_num + 1;
    end

    fprintf ( 1, '  %2d %8.1e %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n', ...
      step, tan2(lim), x2(1:nvar) );

    if ( step == step_max )
      break
    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of limit points found was %d\n', lim_num );

  return
end


function [ n_data, x, fx ] = tran06_values ( n_data )

%*****************************************************************************80
%
%% TRAN06_VALUES returns some values of the order 6 transportation function.
%
%  Discussion:
%
%    The function is defined by:
%
%      TRAN06(x) = Integral ( 0 <= t <= x ) t^6 * exp(t) / ( exp(t) - 1 )^2 dt
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Allan McLeod,
%    Algorithm 757, MISCFUN: A software package to compute uncommon
%    special functions,
%    ACM Transactions on Mathematical Software,
%    Volume 22, Number 3, September 1996, pages 288-301.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 20;

  fx_vec = [ ...
     0.56843405953641209574E-14, ...
     0.59601180165247401484E-08, ...
     0.60978424397580572815E-05, ...
     0.61578909866319494394E-02, ...
     0.18854360275680840514E+00, ...
     0.13319251347921659134E+01, ...
     0.50857202271697616755E+01, ...
     0.13729222365466557122E+02, ...
     0.29579592481641441292E+02, ...
     0.88600835706899853768E+02, ...
     0.10916037113373004909E+03, ...
     0.18224323749575359518E+03, ...
     0.23765383125586756031E+03, ...
     0.29543246745959381136E+03, ...
     0.50681244381280455592E+03, ...
     0.63878231134946125623E+03, ...
     0.72699203556994876111E+03, ...
     0.73230331643146851717E+03, ...
     0.73248692015882096369E+03, ...
     0.73248700462879996604E+03 ];

  x_vec = [ ...
       0.0019531250E+00, ...
       0.0312500000E+00, ...
       0.1250000000E+00, ...
       0.5000000000E+00, ...
       1.0000000000E+00, ...
       1.5000000000E+00, ...
       2.0000000000E+00, ...
       2.5000000000E+00, ...
       3.0000000000E+00, ...
       4.0000000000E+00, ...
       4.2500000000E+00, ...
       5.0000000000E+00, ...
       5.5000000000E+00, ...
       6.0000000000E+00, ...
       8.0000000000E+00, ...
      10.0000000000E+00, ...
      15.0000000000E+00, ...
      20.0000000000E+00, ...
      30.0000000000E+00, ...
      50.0000000000E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function y = tan(x)
%TAN          Implements  tan(x)  for intervals
%
%   y = tan(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 09/13/99     S.M. Rump  complex allowed, sparse input, NaN input,
%                                  major revision, improved accuracy
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 12/04/05     S.M. Rump  extreme values for approximate part
% modified 09/06/07     S.M. Rump  approximate std fcts removed
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

  if issparse(x)
    [ix,jx,sx] = find(x);
    [m,n] = size(x);
    y = sparse(ix,jx,tan(full(sx)),m,n);
    return
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if x.complex
    y = sin(x)./cos(x);
    setround(rndold)
    return
  end

  INTLAB_STDFCTS_PI = getappdata(0,'INTLAB_STDFCTS_PI');

  y = x;

  % transform x.inf and x.sup mod pi/2
  [ xinfinf , xinfsup , Sinf ] = modpi2(x.inf);
  [ xsupinf , xsupsup , Ssup ] = modpi2(x.sup);

  % indices with result +/- inf
  setround(1)
  delta = x.sup-x.inf;
  indexinf = ( delta >= INTLAB_STDFCTS_PI.PIINF ) | ...
    ( floor(Sinf/4) ~= floor(Ssup/4) );
  Sinf(Sinf>3) = Sinf(Sinf>3) - 4;
  Ssup(Ssup>3) = Ssup(Ssup>3) - 4;

  % transformation of input arguments by modpi2:
  %   [ xinf,xsup,s ] = modpi2(y)  ==>  0 <= s <= 7, 0 <= x <= pi/4 and
  %   x = -pi/2 + y + s*pi/4 + 2k*pi        for s even
  %   x =       - y + (s-1)*pi/4 + 2k*pi    for s odd

  y.inf(:) = -inf;
  y.sup(:) = inf;

  % treat non-infinity intervals
  Sinf(indexinf) = -1;
  Ssup(indexinf) = -1;

  % save warning status
  wng = warning;
  warning off

  % treat infimum
  index = ( Sinf==0 );
  if any(index(:))
    y.inf(index) = 1 ./ ( - tan_pos(xinfinf(index),-1) );
  end
  index = ( Sinf==1 );
  if any(index(:))
    y.inf(index) = - tan_pos(xinfsup(index),1);
  end
  index = ( Sinf==2 );
  if any(index(:))
    y.inf(index) = tan_pos(xinfinf(index),-1);
  end
  index = ( Sinf==3 );
  if any(index(:))
    res = tan_pos(xinfsup(index),1);
    setround(-1)
    y.inf(index) = 1 ./ res;
  end

  % treat supremum
  index = ( Ssup==0 );
  if any(index(:))
    y.sup(index) = 1 ./ ( - tan_pos(xsupsup(index),1) );
  end
  index = ( Ssup==1 );
  if any(index(:))
    y.sup(index) = - tan_pos(xsupinf(index),-1);
  end
  index = ( Ssup==2 );
  if any(index(:))
    y.sup(index) = tan_pos(xsupsup(index),1);
  end
  index = ( Ssup==3 );
  if any(index(:))
    res = tan_pos(xsupinf(index),-1);
    setround(1)
    y.sup(index) = 1 ./ res;
  end

  % restore warning status
  warning(wng);

  index = isnan(x.inf);
  if any(index(:))
    y.inf(index) = NaN;
    y.sup(index) = NaN;
  end

  setround(rndold)


classdef Polyhedron

  properties
    A;
    b;
    Aeq = [];
    beq = [];
    vertices;
    has_vertices = false;
  end

  methods
    function obj = Polyhedron(A, b, Aeq, beq)
      if nargin < 3
        Aeq = [];
      end
      if nargin < 4
        beq = [];
      end
      obj.A = A;
      obj.b = b;
      obj.Aeq = Aeq;
      obj.beq = beq;
    end

    function vertices = getVertices(obj)
      obj = obj.reduce();
      if ~obj.has_vertices
        if exist('cddmex', 'file')
          H = struct('A', [obj.Aeq; obj.A], 'B', [obj.beq; obj.b], 'lin', (1:size(obj.Aeq, 1))');
          V = cddmex('extreme', H);
          obj.vertices = V.V';
        else
          obj.vertices = iris.thirdParty.polytopes.lcon2vert(obj.A, obj.b, obj.Aeq, obj.beq)';
        end
        obj.has_vertices = true;
      end
      vertices = obj.vertices;
    end

    function reduced_poly = reduce(obj)
      % Find a minimal representation of the polyhedron
      if ~exist('cddmex', 'file')
        error('IRIS:MissingDependency', 'This function requires the cddmex tool. The easiest way to get it is using tbxmanager: http://www.tbxmanager.com/');
      end
      H = struct('A', [obj.Aeq; obj.A], 'B', [obj.beq; obj.b], 'lin', (1:size(obj.Aeq, 1))');
      Hred = cddmex('reduce_h', H);
      assert(isempty(Hred.lin), 'as far as I know, Hred.lin should always be empty. That is, the reduced polyhedron should not contain equality constraints. -rdeits');
      reduced_poly = iris.Polyhedron(Hred.A, Hred.B);
    end

    function plotVertices(obj, varargin)
      vertices = obj.getVertices();
      if ~isempty(vertices)
        if size(vertices, 2) > 2
          k = convhull(vertices(1,:), vertices(2,:));
        else
          k = [1:size(vertices, 2), 1];
        end
        plot(vertices(1,k), vertices(2,k), varargin{:});
      end
    end

    function drawLCMGL(obj, lcmgl)
      lcmgl.glBegin(lcmgl.LCMGL_LINES);
      vertices = obj.getVertices();
      if size(vertices, 2) > 2
        k = convhull(vertices(1,:), vertices(2,:));
      else
        k = [1:size(vertices, 2), 1];
      end
      for j = 1:length(k)-1
        lcmgl.glVertex3d(vertices(1,k(j)), vertices(2,k(j)), vertices(3,k(j)));
        lcmgl.glVertex3d(vertices(1,k(j+1)), vertices(2,k(j+1)), vertices(3,k(j+1)));
      end
      lcmgl.glEnd();
    end

    function obj = normalize(obj)
      n = zeros(size(obj.A, 1), 1);
      for j = 1:size(obj.A, 1)
        n(j) = norm(obj.A(j,:));
        obj.A(j,:) = obj.A(j,:) / n(j);
        obj.b(j) = obj.b(j) / n(j);
      end
      obj.A = obj.A(n > 0, :);
      obj.b = obj.b(n > 0);

      n = zeros(size(obj.Aeq, 1), 1);
      for j = 1:size(obj.Aeq, 1)
        n(j) = norm(obj.Aeq(j,:));
        obj.Aeq(j,:) = obj.Aeq(j,:) / n(j);
        obj.beq(j) = obj.beq(j) / n(j);
      end
      obj.Aeq = obj.Aeq(n > 0, :);
      obj.beq = obj.beq(n > 0);
    end

  end

  methods(Static)
    function obj = fromVertices(vertices)
      [A, b] = iris.thirdParty.polytopes.vert2lcon(vertices');
      obj = iris.Polyhedron(A, b);
    end

    function obj = from2DVertices(vertices)
      assert(size(vertices, 1) == 2);
      x = vertices(1,:);
      y = vertices(2,:);
      k = convhull(x,y, 'simplify', true);
      A = [(y(k(2:end)) - y(k(1:end-1)))', (x(k(1:end-1)) - x(k(2:end)))'];
      b = sum(A' .* [x(k(1:end-1)); y(k(1:end-1))], 1)';
      obj = iris.Polyhedron(A, b);
    end

    function obj = from2DVerticesAndPlane(vertices, normal, v)
      assert(size(vertices, 1) == 2);
      % normal' * [x;y;z] = v;
      obj = iris.Polyhedron.from2DVertices(vertices);
      obj.Aeq = reshape(normal, 1, []);
      obj.beq = v;
      obj.A = [obj.A, zeros(size(obj.A, 1), 1)];
    end

    function obj = fromBounds(lb, ub)
      % create a polyhedron representing a bounding box in n dimensions
      dim = length(lb);
      assert(length(lb) == length(ub));
      A = [eye(dim); -eye(dim)];
      b = [reshape(ub,[],1); reshape(-lb,[],1)];
      obj = iris.Polyhedron(A, b);
    end
  end
end


function [ r, seed ] = r8mat_uniform_01 ( m, n, seed )

%*****************************************************************************80
%
%% R8MAT_UNIFORM_01 returns a unit pseudorandom R8MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the array.
%
%    Input, integer SEED, the integer "seed" used to generate
%    the output random number.
%
%    Output, real R(M,N), an array of random values between 0 and 1.
%
%    Output, integer SEED, the updated seed.  This would
%    normally be used as the input seed on the next call.
%
  i4_huge = 2147483647;
  r = zeros ( m, n );

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8MAT_UNIFORM_01 - Fatal error!' );
  end

  for j = 1 : n
    for i = 1 : m

      seed = floor ( seed );

      seed = mod ( seed, i4_huge );

      if ( seed < 0 )
        seed = seed + i4_huge;
      end

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + i4_huge;
      end

      r(i,j) = seed * 4.656612875E-10;

    end
  end

  return
end