function [Q2,Bg2,delta_X2,Pk2]=ESKF(Q1,Bg1,delta_X1,ImuData,t,Vm,Pk1)

% ESKF( Error State Kalman Filter)
% derivation <Quaternion Kinematics for the Error-state Kalman Filter>
% author Zhang xin

if isempty(Pk1)
    Para_delta_theta0=0.00001;
    Para_delta_Bg0=0.00001;
    Pk1=diag([[1,1,1]*Para_delta_theta0,[1,1,1]*Para_delta_Bg0]);
end

norm_a=norm(ImuData(1,2:4));
%norm_g=norm(ImuData(1,5:7)-Bg1);
wx=(ImuData(1,5)-Bg1(1))*t;
wy=(ImuData(1,6)-Bg1(2))*t;
wz=(ImuData(1,7)-Bg1(3))*t;

Qp=[ 1    , -wx/2 , -wy/2 , -wz/2  ;...
     wx/2 ,   1   ,  wz/2 , -wy/2  ;...
     wy/2 , -wz/2 ,   1   ,  wx/2  ;...
     wz/2 ,  wy/2 , -wx/2 ,   1   ]*Q1';
Qp=Qp/norm(Qp);

delta_theta=norm([wx,wy,wz]);

if delta_theta==0
    R_u_delta_theta=eye(3);
else
    u= [wx,wy,wz]'/delta_theta;
    R_u_delta_theta=eye(3)-Skew_symmetric(u)*sin(delta_theta)+Skew_symmetric(u)*Skew_symmetric(u)*(1-cos(delta_theta));
end

F_delta_X=[R_u_delta_theta, -eye(3)*t;...
           zeros(3,3),       eye(3) ];
Para_delta_theta=0.000001;
Para_delta_Bg=0.000001;
Q_delta_X=diag([[1,1,1]*Para_delta_theta,[1,1,1]*Para_delta_Bg]);

delta_Xp = F_delta_X*delta_X1';

P_k=F_delta_X*Pk1*F_delta_X'+Q_delta_X;

if abs(norm_a-9.8)<2 %&& norm_g< 2

   % R1=[2*Qp(1)^2+2*Qp(2)^2-1;...
   %     2*Qp(2)*Qp(3)-2*Qp(1)*Qp(4);...
   %     2*Qp(1)*Qp(3)+2*Qp(2)*Qp(4)];

    R2=[2*Qp(2)*Qp(3)+2*Qp(1)*Qp(4);...
        2*Qp(1)^2+2*Qp(3)^2-1;...
        2*Qp(3)*Qp(4)-2*Qp(1)*Qp(2)];

    R3=[2*Qp(2)*Qp(4)-2*Qp(1)*Qp(3);...
        2*Qp(3)*Qp(4)+2*Qp(1)*Qp(2);...
        2*Qp(1)^2+2*Qp(4)^2-1 ];

   % J1=2*[ Qp(1), Qp(2),-Qp(3),-Qp(4);...
   %       -Qp(4), Qp(3), Qp(2), Qp(1);...
   %        Qp(3), Qp(4), Qp(1), Qp(2)];

    J2=2*[ Qp(4), Qp(3), Qp(2) , Qp(1);...
           Qp(1),-Qp(2), Qp(3) ,-Qp(4);...
          -Qp(2),-Qp(1), Qp(4) , Qp(3)] ;

    J3=2*[-Qp(3), Qp(4),-Qp(1) , Qp(2);...
           Qp(2), Qp(1), Qp(4) , Qp(3);...
           Qp(1),-Qp(2),-Qp(3) , Qp(4)] ;

    h_acc= R3;
    h_mag = Vm(2)*R2+Vm(3)*R3;

    H_acc = [J3,zeros(3,3)];
    H_mag = [Vm(2)*J2+Vm(3)*J3,zeros(3,3)];

    Q_delta_theta= 1/2*[ -Qp(2), -Qp(3), -Qp(4) ;...
                          Qp(1), -Qp(4),  Qp(3) ;...
                          Qp(4),  Qp(1), -Qp(2) ;...
                         -Qp(3),  Qp(2),  Qp(1) ];

    X_delta_x=[Q_delta_theta, zeros(4,3);...
                zeros(3,3),      eye(3)];

    Hk= [H_acc;H_mag]* X_delta_x;

    para_Rk_acc=0.1;
    para_Rk_mag=0.2;
    Rk=diag([[1,1,1]*para_Rk_acc,[1,1,1]*para_Rk_mag]);

    Kk=P_k*Hk'*inv(Hk*P_k*Hk'+Rk);

    delta_X_hat=Kk*([ImuData(1,2:4)'/norm_a;ImuData(1,8:10)'/norm(ImuData(1,8:10))]-[h_acc;h_mag]);

    Q2=quaternProd(Qp, [1;delta_X_hat(1:3)/2])';
    Q2=Q2/norm(Q2);
    Bg2=Bg1+delta_X_hat(4:6)';

    Pk2_=(eye(6)-Kk*Hk)*P_k;

    delta_X2=delta_Xp'+delta_X_hat';

    G=[eye(3)-Skew_symmetric(-delta_X2(1:3)/2),zeros(3,3);...
        zeros(3,3)   ,                 eye(3)];

    Pk2=G*Pk2_*G';

else
    Q2=Qp';
    Bg2=Bg1;
    delta_X2=delta_Xp';
    Pk2=P_k;

end
    delta_X2(1:3)=[0,0,0];

    if Q2(1)<0
        Q2=-Q2;
    end

end

function S=Skew_symmetric(u)
%Skew_Operator

S=[  0  , -u(3) ,  u(2) ;...
    u(3),   0   , -u(1) ;...
   -u(2),  u(1),    0 ];

end

function ab = quaternProd(a, b)
    ab(1,1) = a(1)*b(1)-a(2)*b(2)-a(3)*b(3)-a(4)*b(4);
    ab(2,1) = a(1)*b(2)+a(2)*b(1)+a(3)*b(4)-a(4)*b(3);
    ab(3,1) = a(1)*b(3)-a(2)*b(4)+a(3)*b(1)+a(4)*b(2);
    ab(4,1) = a(1)*b(4)+a(2)*b(3)-a(3)*b(2)+a(4)*b(1);
    if ab(1)<0
        ab=-ab;
    end
end


%Let Cpn_est=(I-S(e))Cpn. Then e=[Egyro Eplat]*[gyro error;Platform error]
%platform err def:Cbp_est=(I-S(p))*Cbp
%gyro err def: gyro_meas=gyro+gyro_error
function [Cnp, Egyro, Eplat]=alingc_hd(gyro, Lat, Cbp)

WIE_E=7292115e-11;
nrot=WIE_E*cos(Lat);

%normalize gyro;
gyro=gyro/norm(gyro)*WIE_E;

%Compute the earth rotation in platform frame
if isempty(Cbp)
    Cbp=eye(3);
end
gyro_p=Cbp*gyro;

%Transformation
Cnp=zeros(3);
Cnp(1,1)=gyro_p(1)/nrot;
Cnp(2,2)=gyro_p(1)/nrot;
Cnp(2,1)=gyro_p(2)/nrot;
Cnp(1,2)=-Cnp(2,1);
Cnp(3,3)=1;

%Error matrices
Egyro=[-gyro_p(2)/nrot^2 gyro_p(1)/nrot^2 0]*Cbp;
Eplat=[-gyro_p(2)/nrot^2 gyro_p(1)/nrot^2 0]*skew(gyro_p);


% Replication of the trivariate VAR in Stock and Watson (2001, JEP).
% Figure 1 and Table 1.B.
%==========================================================================
% The VAR Toolbox 3.0 is required to run this code. To get the
% latest version of the toolboxes visit:
% https://github.com/ambropo/VAR-Toolbox
%==========================================================================
% Ambrogio Cesa Bianchi, November 2020
% ambrogio.cesabianchi@gmail.com

%% PRELIMINARIES
%==========================================================================
clear all; clear session; close all; clc
warning off all

% Load data
[xlsdata, xlstext] = xlsread('SW2001_Data.xlsx','Sheet1');
X = xlsdata;
dates = xlstext(3:end,1);
vnames_long = xlstext(1,2:end);
vnames = xlstext(2,2:end);
nvar = length(vnames);
data   = Num2NaN(xlsdata);
% Store variables in the structure DATA
for ii=1:length(vnames)
    DATA.(vnames{ii}) = data(:,ii);
end
% Convert the first date to numeric
year = str2double(xlstext{3,1}(1:4));
quarter = str2double(xlstext{3,1}(6));
% Observations
nobs = size(data,1);

%% VAR ESTIMATION
%==========================================================================
% Set deterministics for the VAR
det = 1;
% Set number of nlags
nlags = 4;
% Estimate VAR
[VAR, VARopt] = VARmodel(X,nlags,det);
% Print estimation on screen
VARopt.vnames = vnames;
[TABLE, beta] = VARprint(VAR,VARopt,2);

%% COMPUTE IR AND VD
%==========================================================================
% Set options some options for IRF calculation
VARopt.nsteps = 24;
VARopt.ident = 'short';
VARopt.vnames = vnames_long;
VARopt.FigSize = [26,12];
% Compute IRF
[IRF, VAR] = VARir(VAR,VARopt);
% Compute error bands
[IRinf,IRsup,IRmed,IRbar] = VARirband(VAR,VARopt);
% Plot
VARirplot(IRbar,VARopt,IRinf,IRsup);

% Compute VD
[VD, VAR] = VARvd(VAR,VARopt);
% Compute VD error bands
[VDinf,VDsup,VDmed,VDbar] = VARvdband(VAR,VARopt);
% Plot VD
VARvdplot(VDbar,VARopt);

%% Print Table 1.B on screen
%==========================================================================
% Retrieve Forecast Error Variance Decomposition
FEVD_Table(1, :) = VD(1,:,1);
FEVD_Table(2, :) = VD(4,:,1);
FEVD_Table(3, :) = VD(8,:,1);
FEVD_Table(4, :) = VD(12,:,1);

FEVD_Table(5, :) = VD(1,:,2);
FEVD_Table(6, :) = VD(4,:,2);
FEVD_Table(7, :) = VD(8,:,2);
FEVD_Table(8, :) = VD(12,:,2);

FEVD_Table(9, :) = VD(1,:,3);
FEVD_Table(10,:) = VD(4,:,3);
FEVD_Table(11,:) = VD(8,:,3);
FEVD_Table(12,:) = VD(12,:,3);

% Print on screen
disp(' ')
disp('Variance Decomposition of Inflation (t=1,4,8,12)')
disp('---------------------------------------------------')
mprint(FEVD_Table(1:4,:));
disp('Variance Decomposition of Unemployment (t=1,4,8,12)')
disp('---------------------------------------------------')
mprint(FEVD_Table(5:8,:));
disp('Variance Decomposition of Fed Funds (t=1,4,8,12)')
disp('---------------------------------------------------')
mprint(FEVD_Table(9:12,:));


function [nodeBel, edgeBel, L] = mrfMf(A, nodePot, edgePot, epoch)
% Mean field for MRF
% Assuming egdePot is symmetric
% Input:
%   A: n x n adjacent matrix of undirected graph, where value is edge index
%   nodePot: k x n node potential
%   edgePot: k x k x m edge potential
% Output:
%   nodeBel: k x n node belief
%   edgeBel: k x k x m edge belief
% Written by Mo Chen (sth4nth@gmail.com)
if nargin < 4
    epoch = 10;
end
L = -inf(1,epoch+1);
[nodeBel,lnZ] = softmax(nodePot,1);    % initialization
for iter = 1:epoch
    for i = 1:size(nodePot,2)
        [~,j,e] = find(A(i,:));             % neighbors
        [nodeBel(:,i),lnZ(i)] = softmax(nodePot(:,i)+reshape(edgePot(:,:,e),2,[])*reshape(nodeBel(:,j),[],1));
    end
%     E = dot(nodeBel,nodePot,1);
%     H = -dot(nodeBel,log(nodeBel),1);
%     L(iter+1) = sum(lnZ+E+H)/2;
    L(iter+1) = mrfGibbs(A,nodePot,edgePot,nodeBel);
%     if abs(L(iter+1)-L(iter))/abs(L(iter)) < tol; break; end
end
L = L(1,2:iter+1);

[s,t,e] = find(triu(A));
edgeBel = zeros(size(edgePot));
for l = 1:numel(e)
    edgeBel(:,:,e(l)) = nodeBel(:,s(l))*nodeBel(:,t(l))';
end


function  [h, compUp] =  lfmComputeH4(gamma1_p, gamma1_m, sigma2, t1, preFactor, preExp,...
    mode, term )
% LFMCOMPUTEH4 Helper function for computing part of the LFM kernel.
% FORMAT
% DESC computes a portion of the LFM kernel.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode: indicates in which way the vectors t1 and t2 must be transposed
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : David Luengo, 2007
%
% COPYRIGHT : Mauricio Alvarez, 2008
%
% MODIFICATIONS : Neil D. Lawrence, 2007
%
%
%
% SEEALSO : lfmKernParamInit, lfmXlfmKernCompute

% KERN

% Evaluation of h

if nargin<8
    term =[];
end

if ~mode
    if ~term
        if nargout>1
            compUp = lfmComputeUpsilonVector(gamma1_p,sigma2, t1);
            h = compUp*( preExp/preFactor(1) - conj(preExp)/preFactor(2)).';
        else
            h = lfmComputeUpsilonVector(gamma1_p,sigma2, t1)*( preExp/preFactor(1) - conj(preExp)/preFactor(2)).';
        end
    else
        if nargout>1
            compUp = lfmComputeUpsilonVector(gamma1_p,sigma2, t1);
            h = compUp*(preExp/preFactor(1)).' - conj(compUp)*(preExp/preFactor(2)).';
        else
            upsilon = lfmComputeUpsilonVector(gamma1_p,sigma2, t1);
            h = upsilon*(preExp/preFactor(1)).' - conj(upsilon)*(preExp/preFactor(2)).';
        end
    end
else
    if nargout > 1
        compUp{1} = lfmComputeUpsilonVector(gamma1_p,sigma2, t1);
        compUp{2} = lfmComputeUpsilonVector(gamma1_m,sigma2, t1);
        h =  compUp{1}*( preExp(:,1)/preFactor(1) - preExp(:,2)/preFactor(2)).' ...
            + compUp{2}*( preExp(:,2)/preFactor(3) - preExp(:,1)/preFactor(4)).';
    else
        h =  lfmComputeUpsilonVector(gamma1_p,sigma2, t1)*( preExp(:,1)/preFactor(1) - preExp(:,2)/preFactor(2)).' ...
            + lfmComputeUpsilonVector(gamma1_m,sigma2, t1)*( preExp(:,2)/preFactor(3) - preExp(:,1)/preFactor(4)).';
    end
end


function beta_vector = scattering_coefficient_random(number_of_images,...
    parameters)
%SCATTERING_COEFFICIENT_RANDOM  Generate scattering coefficient values for a set
%of images uniformly at random.
%   Inputs:
%       -|number_of_images|: number of images for which scattering coefficient
%       is simulated.
%       -|parameters|: structure containing miscellaneous parameters, such as
%       range of values for scattering coefficient and type of random number
%       generator. Guarantees uniformity with other functions that implement
%       generation of scattering coefficient values.
%
%   Outputs:
%       -|beta_vector|: 1-by-|number_of_images| vector containing the random
%       values of scattering coefficient for every image in the set.

% Determine the range of random values for scattering coefficient.
maximum_value = parameters.maximum_value;
minimum_value = parameters.minimum_value;

% Get type of random number generator, e.g. 'default'.
random_generator = parameters.random_generator;

% Get binary flag that indicates whether the random number generator should be
% configured or not.
configure_random_generator = parameters.configure_random_generator;

% Optionally configure random number generation for repeatability.
if configure_random_generator
    rng(random_generator);
end

% Generate random scattering coefficient for each image following a uniform
% distribution inside the specified range.
beta_vector = minimum_value + (maximum_value - minimum_value) *...
    rand(1, number_of_images);

end


classdef PoolingNode < GraphNode
    properties
        poolingDimension = 3;      % poolinig happens along this dimension
        poolingType = 'max';       % [max|min|mean|median|sum]
        selectedIdx = [];          % remember the index of data selected by pooling in [max|min|median]
    end

    methods
        function obj = PoolingNode(dimOut, poolingType, poolingDimension)
            obj = obj@GraphNode('Pooling',dimOut);
            if nargin>=2
                obj.poolingType = poolingType;
            end
            if nargin>=3
                obj.poolingDimension = poolingDimension;
            end
        end

        function obj = forward(obj,prev_layers)
            obj = obj.preprocessingForward(prev_layers);
            input = prev_layers{1}.a;

            switch lower(obj.poolingType)
                case 'max'
                    [obj.a, obj.selectedIdx] = max(input, [], obj.poolingDimension);
                case 'min'
                    [obj.a, obj.selectedIdx] = min(input, [], obj.poolingDimension);
                case 'mean'
                    obj.a = mean(input, obj.poolingDimension);
                case 'median'
                    obj.a = median(input, obj.poolingDimension);
                    % need to implement selectedIdx by myself in the future
                case 'sum'
                    obj.a = sum(input, obj.poolingDimension);
            end

            obj = forward@GraphNode(obj, prev_layers);
        end

        function obj = backward(obj,prev_layers, future_layers)
            if obj.skipGrad || obj.skipBP
                return;
            end

            future_grad = obj.GetFutureGrad(future_layers);
            input = prev_layers{1}.a;
            [D(1), D(2), D(3), D(4)] = size(input);

            switch lower(obj.poolingType)
                case {'max', 'min', 'median'}
                    obj.grad{1} = obj.AllocateMemoryLike(D, future_grad);
                    obj.grad{1}(obj.selectedIdx) = future_grad;
                case {'mean','sum'}
                    if strcmpi(obj.poolingType, 'mean')
                        future_grad = future_grad / D(obj.poolingDimension);
                    end
                    shape = ones(1,4);
                    shape(obj.poolingDimension) = D(obj.poolingDimension);
                    obj.grad{1} = repmat(future_grad, shape);
            end

            obj = backward@GraphNode(obj, prev_layers, future_layers);
        end

    end

end


function [dat,beta,x] = ft_preproc_detrend(dat, begsample, endsample)

% FT_PREPROC_DETREND removes mean and linear trend from the
% data using using a General Linear Modeling approach.
%
% Use as
%   [dat] = ft_preproc_detrend(dat, begin, end)
% where
%   dat        = data matrix (Nchans X Ntime)
%   begsample  = index of the begin sample for the trend estimate
%   endsample  = index of the end sample for the trend estimate
%
% If no begin and end sample are specified for the trend estimate, it
% will be estimated on the complete data.
%
% See also FT_PREPROC_BASELINECORRECT, FT_PREPROC_POLYREMOVAL

% Copyright (C) 2008-2014, 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$

% take the whole segment if begsample and endsample are not specified
if nargin<2 || isempty(begsample)
  begsample = 1;
end
if nargin<3|| isempty(endsample)
  endsample = size(dat,2);
end

[dat,beta,x] = ft_preproc_polyremoval(dat, 1, begsample, endsample);


% @author: Maziar Raissi

function KDV()
clc; close all;

plt = 1;
plt_pred = 0;
save_plt = 1;

addpath ..
addpath ../Utilities
addpath ../Kernels/KDV
addpath ../Utilities/export_fig

function CleanupFun()
    rmpath ..
    rmpath ../Utilities
    rmpath ../Kernels/KDV
    rmpath ../Utilities/export_fig
end

finishup = onCleanup(@() CleanupFun());

set(0,'defaulttextinterpreter','latex')

%% Load Data
load('../Data/kdv.mat', 'usol', 't', 'x')
u_star = real(usol); % 512x201
t_star = t; % 201x1
x_star = x';   % 512x1
N_star = size(x_star,1);
nsteps = size(t_star,1)-1;

%% Setup
N0 = 111;
N1 = 109;
%% Clean Data
rng('default')
i = randi(nsteps);
dt = t_star(i+1) - t_star(i);

idx0 = randsample(N_star, N0);
x0 = x_star(idx0,:);
u0 = u_star(idx0,i);

idx1 = randsample(N_star,N1);
x1 = x_star(idx1,:);
u1 = u_star(idx1,i+1);

hyp = [log([1.0 1.0]) 0.0 0.0 -4.0];
model = HPM(x1, u1, x0, u0, dt, hyp);
model = model.train(5000);

hyp = model.hyp;
params = hyp(3:4);

[pred_n_star, var_n_star] = model.predict(x_star);
var_n_star = abs(diag(var_n_star));

error = norm(pred_n_star - u_star(:,i+1))/norm(u_star(:,i+1));

fprintf(1,'=========================\n');
fprintf(1,'Step: %d, Time = %.2f\n\nNLML = %.2f, Error = %.2e\n\n', i, ...
    t_star(i+1), model.NLML, error);

str = sprintf('%.4f  ', params);
fprintf('Parameters: %s\n\n', str)
fprintf(1,'=========================\n\n');

if plt_pred == 1
    figure();
    plot_prediction_1D(x_star, u_star(:,i+1), pred_n_star, var_n_star, ...
        '$x$', '$u(t,x)$', 'Prediction (clean data)');

    drawnow;
end

%% Noisy Data
noise = 0.01;
u0 = u0 + noise*std(u0)*randn(size(u0));
u1 = u1 + noise*std(u1)*randn(size(u1));

hyp = [log([1.0 1.0]) 0.0 0.0 -4.0];
model = HPM(x1, u1, x0, u0, dt, hyp);
model = model.train(5000);

hyp = model.hyp;
params_noise = hyp(3:4);

[pred_n_star, var_n_star] = model.predict(x_star);
var_n_star = abs(diag(var_n_star));

error = norm(pred_n_star - u_star(:,i+1))/norm(u_star(:,i+1));

fprintf(1,'=========================\n');
fprintf(1,'Step: %d, Time = %.2f\n\nNLML = %.2f, Error = %.2e\n\n', i, ...
    t_star(i+1), model.NLML, error);

str = sprintf('%.4f  ', params_noise);
fprintf('Parameters: %s\n\n', str)
fprintf(1,'=========================\n\n');

if plt_pred == 1
    figure();
    plot_prediction_1D(x_star, u_star(:,i+1), pred_n_star, var_n_star, ...
        '$x$', '$u(t,x)$', 'Prediction (noisy data)');

    drawnow;
end

%% Plot Results

if plt == 1
    fig = figure();
    set(fig,'units','normalized','outerposition',[0 0 1 .5])
    subplot(3,2,1:2)
    plot_surface(t_star, x_star, u_star, '$t$', '$x$', '$u(t,x)$');

    hold on
    plot3([t_star(i) t_star(i)],get(gca,'ylim'),[10 10],'w','LineWidth',2)
    plot3([t_star(i+1) t_star(i+1)],get(gca,'ylim'),[10 10],'w','LineWidth',2)

    subplot(3,2,3);
    tit = sprintf('$t = $ %.2f\n%d training data\n', t_star(i), N0);
    plot_data_1D(x_star, u_star(:,i), x0, u0, '$x$', '$u(t,x)$', tit);

    subplot(3,2,4);
    tit = sprintf('$t = $ %.2f\n%d training data\n', t_star(i+1), N1);
    plot_data_1D(x_star, u_star(:,i+1), x1, u1, '$x$', '$u(t,x)$', tit);

    subplot(3,2,5:6);
    s1 = '$\begin{tabular}{ |c|c| }  \hline Correct PDE & $u_t + 6 u u_x + u_{xxx} = 0$ \\  \hline Identified PDE (clean data) &';
    s2 = sprintf('$u_t + %.3f u u_x + %.3f u_{xxx} = 0$', params(1), params(2));
    s3 = ' \\  \hline Identified PDE (1\% noise) &';
    s4 = sprintf('$u_t + %.3f u u_x + %.3f u_{xxx} = 0$', params_noise(1), params_noise(2));
    s5 = ' \\  \hline \end{tabular}$';
    s = strcat(s1,s2,s3,s4,s5);
    text(0.1,0.8,s,'interpreter','latex','FontSize',18)
    axis off

    if save_plt == 1
        export_fig ../Figures/KDV.png -r300
    end

    drawnow();
end

end


function matrix = genMatEllPPIFE3D(pde,mesh,fem,femI,femIF,PPtype,sig)
%% Generate global matrices and load vector of PPIFEM for 3D ellipic eq
%     -div(A grad u)  = f,    x\in \Omega
%      where A is a piecewise constant on Omega^+ and Omega^-.
% INPUTS:
% pde --- given data function from equation, e.g.
%         pde.A --- diffusion coefficient
%         pde.f --- right hand side function
%         pde.gD --- Dirichlet boundary value function
%         pde.one --- constant function 1.
% mesh --- mesh structure.
% fem --- global degree of freedom of FEM
% femI --- quadrature info on interface cells
% femIF --- quadrature info on interface faces, required in PPIFE.
% PPtype --- partial penalty type possible value
%            'N' : Nonsymmetric PPIFE (e = 1)
%            'S' : Symmetric PPIFE (e = -1)
%            'I' : Incomplete PPIFE (e = 0)
% OUTPUTS:
% matrix.S --- stiffness matrix (w/o boundary condition)
% matrix.E --- consistence matrix (w/o boundary condition)
% matrix.P --- penalty matrix (w/o boundary condition)
% matrix.A --- final FEM matrix (after boundary condition)
% matrix.rhsF --- load vector (w/o boundary condition)
% matrix.rhsE --- consistence vector (=0 if no interface face on boundary)
% matrix.rhsP --- penalty vector (=0 if no interface face on boundary)
% matrix.f --- final RHS matrix (after boundary condition)

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

%% 1. Stiffness Matrix
S = globMatrixIFE3DStiff(pde.A,mesh,femI,fem,fem);
% This matrix generator is a little faster than doing it separately.
% Sx = globMatrixIFE3D(pde.A,mesh,femI,fem,[1,0,0],fem,[1,0,0]);
% Sy = globMatrixIFE3D(pde.A,mesh,femI,fem,[0,1,0],fem,[0,1,0]);
% Sz = globMatrixIFE3D(pde.A,mesh,femI,fem,[0,0,1],fem,[0,0,1]);
% S = Sx+Sy+Sz;

%% 2. Face Matrices
d1 = 0; j1 = 1; d2 = 1; j2 = 0;
E = globMatrixIFE3DFace(pde.one, pde.A, fem, fem, femIF, d1,j1, d2,j2);

d1 = 0; j1 = 1; d2 = 0; j2 = 1;
P = globMatrixIFE3DFace(pde.one, pde.one,fem,fem, femIF, d1,j1, d2,j2);

%% 3. Generate the Right Hand Side Vector
rhsF = globRHSIFE3D(pde.f, mesh, fem, femI, [0,0,0]);
rhsE = globRHSIFE3DFace(pde.A, pde.exactu, fem, femIF, 1);
rhsP = globRHSIFE3DFace(pde.one, pde.exactu, fem, femIF, 0);

%% 4. Dirichlet Boundary Conditions
if strcmp(PPtype,'N')
    e = 1;
elseif strcmp(PPtype,'S')
    e = -1;
elseif strcmp(PPtype,'I')
    e = 0;
end
h = mesh.p(2,1) - mesh.p(1,1);
Atotal = S - E + e*E' + (sig/h)*P;
rhs = rhsF + e*rhsE + (sig/h)*rhsP;
tu = feval(pde.gD,fem.p(:,1),fem.p(:,2),fem.p(:,3));
ub = tu;
ub(fem.mapper) = 0;
rhsB = Atotal*ub;
A = Atotal(fem.mapper,fem.mapper);
f = rhs(fem.mapper) - rhsB(fem.mapper);

%% 5. Outputs
matrix = struct('A',A,'f',f,'S',S,'E',E,'P',P,'rhsF',rhsF,'rhsE',rhsE,...
    'rhsP',rhsP);


function centroids = faceCentroids(nodes, faces)
%FACECENTROIDS Compute centroids of a mesh faces.
%
%   NORMALS = faceCentroids(VERTICES, FACES)
%   VERTICES is a set of 3D points  (as a N-by-3 array), and FACES is
%   either a N-by-3 index array or a cell array of indices. The function
%   computes the centroid of each face, and returns a Nf-by-3 array
%   containing their coordinates.
%
%   Example
%     [v e f] = createIcosahedron;
%     normals1 = faceNormal(v, f);
%     centros1 = faceCentroids(v, f);
%     figure; drawMesh(v, f);
%     hold on; axis equal; view(3);
%     drawVector3d(centros1, normals1);
%
%
%   See also:
%   meshes3d, drawMesh, faceNormal, convhull, convhulln
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2006-07-05
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas)

% deprecation warning
warning('geom3d:deprecated', ...
    [mfilename ' is deprecated, use ''meshFaceCentroids'' instead']);

if isnumeric(faces)
    % trimesh or quadmesh
    nf = size(faces, 1);
    centroids = zeros(nf, size(nodes, 2));
    if size(nodes, 2) == 2
        % planar case
        for f = 1:nf
            centroids(f,:) = polygonCentroid(nodes(faces(f,:), :));
        end
    else
        % 3D case
        for f = 1:nf
            centroids(f,:) = polygonCentroid3d(nodes(faces(f,:), :));
        end
    end
else
    % mesh with faces stored as cell array
    nf = length(faces);
    centroids = zeros(nf, size(nodes, 2));
    if size(nodes, 2) == 2
        % planar case
        for f = 1:nf
            centroids(f,:) = polygonCentroid(nodes(faces{f}, :));
        end
    else
        % 3D case
        for f = 1:nf
            centroids(f,:) = polygonCentroid3d(nodes(faces{f}, :));
        end
    end
end


function [post nlZ dnlZ] = infExactWarp(hyp, mean, cov, lik, x, y)

% Exact inference for a GP with Gaussian likelihood. Compute a parametrization
% of the posterior, the negative log marginal likelihood and its derivatives
% w.r.t. the hyperparameters. See also "help infMethods".
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2015-07-13.
%                                      File automatically generated using noweb.
%
% See also INFMETHODS.M.

if iscell(lik), likstr = lik{1}; else likstr = lik; end
if ~ischar(likstr), likstr = func2str(likstr); end
if ~strcmp(likstr,'likGaussWarpExact')               % NOTE: no explicit call to likGaussWarpExact
  error('Exact inference only possible with warped exact Gaussian likelihood');
end
warp = lik{2}; if ischar(warp); warp = str2func(warp); end

ng = feval(warp{:});               % number of hyperparameters for the warping function
nhyp = feval(lik{:});              % number of hyperparameters
% if nargin<4, varargout = {nhyp}; return, end       % report number of parameters
nhyp = eval(nhyp);
if nhyp>length(hyp.lik), error('not enough hyperparameters'), end

[gy,lgpy] = feval(warp{:},y,hyp.lik(1:ng));                     % evaluate warping function

% figure(100); scatter(y,gy); drawnow;

[n, D] = size(x);
K = feval(cov{:}, hyp.cov, x);                      % evaluate covariance matrix
m = feval(mean{:}, hyp.mean, x);                    % evaluate mean vector
[gm,lgpm] = feval(warp{:},m,hyp.lik(1:ng));         % evaluate warped mean

sn2 = exp(2*hyp.lik(end));                          % noise variance of likGauss
if sn2<1e-6                        % very tiny sn2 can lead to numerical trouble
  L = chol(K+sn2*eye(n)); sl =   1;   % Cholesky factor of covariance with noise
  pL = -solve_chol(L,eye(n));                            % L = -inv(K+inv(sW^2))
else
  L = chol(K/sn2+eye(n)); sl = sn2;                       % Cholesky factor of B
  pL = L;                                           % L = chol(eye(n)+sW*sW'.*K)
end
alpha = solve_chol(L,gy-gm)/sl;

post.alpha = alpha;                            % return the posterior parameters
post.sW = ones(n,1)/sqrt(sn2);                  % sqrt of noise precision vector
post.L = pL;

if nargout>1                               % do we want the marginal likelihood?
  nlZ = (gy-gm)'*alpha/2 + sum(log(diag(L))) + n*log(2*pi*sl)/2 - sum(lgpy);   % -log marg lik
  if nargout>2                                         % do we want derivatives?
    dnlZ = hyp;                                 % allocate space for derivatives
    Q = solve_chol(L,eye(n))/sl - alpha*alpha';     % precompute for convenience
    for i = 1:numel(hyp.cov)
      dnlZ.cov(i) = sum(sum(Q.*feval(cov{:}, hyp.cov, x, [], i)))/2;
    end
    for i = 1:ng
        [dgy,dlgpy] = feval(warp{:},y,hyp.lik(1:ng),i);
        dgm = feval(warp{:},m,hyp.lik(1:ng),i);
        dnlZ.lik(i) = -sum(dlgpy) + (dgy - dgm)'*alpha;
    end
    dnlZ.lik(ng+1) = sn2*trace(Q);
    for i = 1:numel(hyp.mean)
      dnlZ.mean(i) = -(exp(lgpm)'.*feval(mean{:}, hyp.mean, x, i)')*alpha;
    end
  end

end


function mtrPlotCorr(ccFilenameBase,threshVec,paramNames,midP,whichParams,strLineProp)
%
% paramNames = {'kLength','kSmooth','kMidSD'};
% midP = [0 18 0.175];

for pp = 1:length(whichParams)
    strThreshVec = {};
    parVecs = [];
    corrVecs = [];
    ovrVecs = [];
    for tt = 1:length(threshVec)
        cc = load([ccFilenameBase '_thresh_' num2str(threshVec(tt)) '.mat']);
        ccGrid = mtrCCMatrix2Grid(cc.ccMatrix,cc.paramData,paramNames);
        indGrid = ones(size(ccGrid,1),1);
        for ss = 1:length(paramNames)
            if ss ~= whichParams(pp)
                indGrid = indGrid & ccGrid(:,ss) == midP(ss);
            end
        end
        subGrid = ccGrid(indGrid,:);
        [foo, sortI] = sort(subGrid(:,whichParams(pp)));
        parVecs(:,tt) = subGrid(sortI(:),whichParams(pp));
        corrVecs(:,tt) = subGrid(sortI(:),4);
        strThreshVec{tt} = ['Top ' num2str(threshVec(tt))];
    end
    if( pp>1 ) figure; end
    if strcmp(paramNames{whichParams(pp)},'kSmooth')
        parVecs = asin(sqrt(1./parVecs)).*180./pi;
        plot(parVecs,corrVecs,strLineProp);
    else
        plot(parVecs,corrVecs,strLineProp);
    end
    %legend(strThreshVec);
    xlabel([paramNames{whichParams(pp)} ' parameter']);
    ylabel('Corr. Coef.');
end


x = [ 1 2 3 4 5 6];
y = [ 2 6 8 7 8 5];
stairs(x,y);
title('\bfExample of a Stair Plot');
xlabel('\bf\itx');
ylabel('\bf\ity');
axis([0 7 0 10]);


function h = p00_h ( problem, n, x )

%*****************************************************************************80
%
%% P00_H evaluates the Hessian for any problem.
%
%  Discussion:
%
%    H(I,J) = d2 F(X) / dX(I)dX(J)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer PROBLEM, the problem number.
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the Hessian matrix.
%
  if ( problem == 1 )
    h = p01_h ( n, x );
  elseif ( problem == 2 )
    h = p02_h ( n, x );
  elseif ( problem == 3 )
    h = p03_h ( n, x );
  elseif ( problem == 4 )
    h = p04_h ( n, x );
  elseif ( problem == 5 )
    h = p05_h ( n, x );
  elseif ( problem == 6 )
    h = p06_h ( n, x );
  elseif ( problem == 7 )
    h = p07_h ( n, x );
  elseif ( problem == 8 )
    h = p08_h ( n, x );
  elseif ( problem == 9 )
    h = p09_h ( n, x );
  elseif ( problem == 10 )
    h = p10_h ( n, x );
  elseif ( problem == 11 )
    h = p11_h ( n, x );
  elseif ( problem == 12 )
    h = p12_h ( n, x );
  elseif ( problem == 13 )
    h = p13_h ( n, x );
  elseif ( problem == 14 )
    h = p14_h ( n, x );
  elseif ( problem == 15 )
    h = p15_h ( n, x );
  elseif ( problem == 16 )
    h = p16_h ( n, x );
  elseif ( problem == 17 )
    h = p17_h ( n, x );
  elseif ( problem == 18 )
    h = p18_h ( n, x );
  elseif ( problem == 19 )
    h = p19_h ( n, x );
  elseif ( problem == 20 )
    h = p20_h ( n, x );
  elseif ( problem == 21 )
    h = p21_h ( n, x );
  elseif ( problem == 22 )
    h = p22_h ( n, x );
  elseif ( problem == 23 )
    h = p23_h ( n, x );
  elseif ( problem == 24 )
    h = p24_h ( n, x );
  elseif ( problem == 25 )
    h = p25_h ( n, x );
  elseif ( problem == 26 )
    h = p26_h ( n, x );
  elseif ( problem == 27 )
    h = p27_h ( n, x );
  elseif ( problem == 28 )
    h = p28_h ( n, x );
  elseif ( problem == 29 )
    h = p29_h ( n, x );
  elseif ( problem == 30 )
    h = p30_h ( n, x );
  elseif ( problem == 31 )
    h = p31_h ( n, x );
  elseif ( problem == 32 )
    h = p32_h ( n, x );
  elseif ( problem == 33 )
    h = p33_h ( n, x );
  elseif ( problem == 34 )
    h = p34_h ( n, x );
  elseif ( problem == 35 )
    h = p35_h ( n, x );
  elseif ( problem == 36 )
    h = p36_h ( n, x );
  elseif ( problem == 37 )
    h = p37_h ( n, x );
  elseif ( problem == 38 )
    h = p38_h ( n, x );
  elseif ( problem == 39 )
    h = p39_h ( n, x );
  elseif ( problem == 40 )
    h = p40_h ( n, x );
  elseif ( problem == 41 )
    h = p41_h ( n, x );
  elseif ( problem == 42 )
    h = p42_h ( n, x );
  elseif ( problem == 43 )
    h = p43_h ( n, x );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P00_H - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of PROBLEM = %d\n', problem );
    error ( ' - Fatal error!' );
  end

  return
end


function [pnt] = elec1020_fraction(cnt1, cnt2, fraction)

% ELEC1020_FRACTION

% Copyright (C) 2003, 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/>.

ncnt = size(cnt1,1);

% determine the total length of the contour
tot_l = 0;
for i=1:ncnt
  tot_l = tot_l + pntdist(cnt1(i,:), cnt2(i,:));
end

frac_l = fraction * tot_l;

% propagate along the contour untill we get at the desired fraction
sum_l = 0;
for i=1:ncnt
  seg_l = pntdist(cnt1(i,:), cnt2(i,:));
  if (sum_l+seg_l)>=frac_l
    % the desired point lies on this segment
    la = frac_l - sum_l;
    vec = cnt2(i,:)-cnt1(i,:);
    pnt = cnt1(i,:) + la * vec/norm(vec);
    return
  else
    sum_l = sum_l + seg_l;
    sum_f = sum_l/tot_l;
  end
end

pnt = [nan nan nan];


%
% Copyright (c) 2015, Yarpiz (www.yarpiz.com)
% All rights reserved. Please read the "license.txt" for license terms.
%
% Project Code: YPML110
% Project Title: Implementation of DBSCAN Clustering in MATLAB
% Publisher: Yarpiz (www.yarpiz.com)
%
% Developer: S. Mostapha Kalami Heris (Member of Yarpiz Team)
%
% Contact Info: sm.kalami@gmail.com, info@yarpiz.com
%

function PlotClusterinResult(X, IDX)
    nDim = size(X, 2);
    k=max(IDX);

    Colors=hsv(k);

    Legends = {};
    for i=0:k
        Xi=X(IDX==i,:);
        if i~=0
            Style = 'x';
            MarkerSize = 8;
            Color = Colors(i,:);
            Legends{end+1} = ['Cluster #' num2str(i)];
        else
            Style = 'o';
            MarkerSize = 6;
            Color = [0 0 0];
            if ~isempty(Xi)
                Legends{end+1} = 'Noise';
            end
        end
        if ~isempty(Xi)
            str = sprintf('Id=%02d, Len=%02d', i-1, length(Xi(:, 1)));
            pt = Xi(1, :);
            if nDim == 2
                text(pt(1), pt(2), str, 'FontSize', 12 );
                plot(Xi(:,1),Xi(:,2),Style,'MarkerSize',MarkerSize,'Color',Color);
            else
                text(pt(1), pt(2), pt(3), str, 'FontSize', 12 );
                plot3(Xi(:,1),Xi(:,2),Xi(:, 3), Style,'MarkerSize',MarkerSize,'Color',Color);
            end
        end
        hold on;
    end
    hold off;
    axis equal;
    grid on;
    % legend(Legends);
    % legend('Location', 'NorthEastOutside');

end


function [chirpF, Freqs] = bst_chirplet(sRate, nTime, chirpCenterFreqs)
% BST_CHIRPLET: Compute the Phase-Amplitude Coupling in one of several time series (directPAC)
%
% INPUTS:
%    - sRate  : Signal sampling rate (in Hz)
%    - nTime  : Number of time points of the signal to filter
%    - chirpCenterFreqs: Center frequencies of the chirplets to calculate
%
% DOCUMENTATION:
%    - The current code is inspired from Ryan Canolty's code provided originally with the article:
%         Canolty RT, Edwards E, Dalal SS, Soltani M, Nagarajan SS, Kirsch HE, Berger MS, Barbaro NM, Knight RT,
%         "High gamma power is phase-locked to theta oscillations in human neocortex",
%         Science, 2006 Sep 15;313(5793):1626-8.

% @=============================================================================
% 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: Ryan Canolty, 2006
%          Sylvain Baillet, 2011-2013
%          Francois Tadel, 2013

% ===== PARSE INPUTS =====
% To avoid out-of-memeory issues: Check this on your machine, machine-specific threshold
% ***** TODO: EVALUALATE THOSE LINES *****
if (nTime > 2^23)
    nFreq = nTime;
else
    % Fixed parameter for computational ease
    nFreq = 2^ceil(log2(nTime));
end
% Raw time_support
Freqs = (sRate/nFreq) * (0:nFreq-1);
inds = Freqs > (sRate/2);
Freqs(inds) = Freqs(inds) - sRate;
% Reduce storage space
% % ***** TODO: EVALUALATE THIS LINES *****
% Freqs = single(Freqs);

% ===== CALCULATE CHIRPLETS =====
% Initialize returned matrix
chirpF = zeros(1, nFreq, length(chirpCenterFreqs));
% Make set of chirplets
fbw = 0.15;
for iif = 1:length(chirpCenterFreqs)
    % Assign or compute duration parameter
    v0 = chirpCenterFreqs(iif);  % center_frequency
    c0 = 0;  % chirp_rate
    s0 = log((2*log(2)) / (fbw^2*pi*v0^2));
    % Frequency support
    std_multiple = 6;
    vstd = sqrt((exp(-s0) + c0^2*exp(s0)) / (4*pi));
    v = Freqs; % in Hz
    iFreq = find(...
        (v0 - std_multiple * vstd <= v) & ...
        (v <= v0 + std_multiple * vstd));
    % Shorten to include only chirplet support
    v = v(iFreq);
    % Chirplet in frequency domain:
    Gk = 2^(1/4)*sqrt(-1i*c0+exp(-s0))^-1 * exp(-s0/4 + (exp(s0)*pi*(v-v0).^2)/(-1+1i*c0*exp(s0)));
    n1 = sqrt(length(Freqs)) / norm(Gk);
    % Because of discrete sampling and different time/freq sample numbers
    Gk = n1 * Gk;  % filter
    % Report in returned structures
    chirpF(1, iFreq, iif) = Gk;
end


function [D,C]=JDDLDR_UDC(X,trls,lambda_a,lambda_b,D,C,Max_iteration)

nClass = max(trls);
Jstep_T = 1e-3;

disp_cycle = floor(nClass/10);
if disp_cycle < 1
    disp_cycle = 1;
end

fprintf('Update of D and C: class ');
for ci = 1:nClass
    cdat = X(:,trls==ci);
    temD = D(ci).M;
    iteration = 1;
    while  iteration < Max_iteration
    % update C
    afa = inv(temD'*temD+lambda_a*eye(size(temD,2)))*(temD'*cdat);
    PinvD = inv(temD'*temD+(lambda_a+lambda_b)*eye(size(temD,2)));
    for tj = 1:10
%         plot(afa(:,1));title(num2str(tj));pause(1);
        avg_afa = repmat(mean(afa,2),[1 size(afa,2)]);
        afa = PinvD*(temD'*cdat+lambda_b*avg_afa);
    end
    C(ci).M = afa;

    % update D
    for i=1:size(temD,2)
        ai        =    afa(i,:);
        Y         =    cdat-temD*afa+temD(:,i)*ai;
        di        =    Y*ai';
        di        =    di./norm(di,2);
        temD(:,i)    =    di;
    end
    D(ci).M  = temD;

    zz            =    cdat-temD*afa;
    zalpha        =    afa(:);
    avg_afa       =    repmat(mean(C(ci).M,2),[1 size(C(ci).M,2)]);
    z_afa         =    afa - avg_afa;
    Jnow          =    zz(:)'*zz(:)+lambda_a*sum(zalpha(:).*zalpha(:))+lambda_b*sum(z_afa(:).*z_afa(:));
    iteration     =    iteration+1;
    end

    if mod(ci, disp_cycle) == 0
        fprintf('%03d ', ci);
    end
end
fprintf('\n');


function [Knots_n] = refine_cubic_grid_2d(Knots, old_spacing, old_vsz, new_spacing, new_vsz, varargin)
    %[Knots_n] = refine_linear_grid_3d(Knots, old_spacing, ds, volsz,
    %  {upsampling_type}
    Nd = size(Knots, 4);

    upsampling_type = 'sample_std';
    if nargin >= 6
        upsampling_type = varargin{1};
    end

    ksz_new = ceil(new_vsz(1:Nd) ./ new_spacing(1:Nd)) + 3;
    k = (old_vsz(1:Nd)) ./ (new_vsz(1:Nd));
    d = 2 * (1-k);
    interp_type = 1;

    tmp = cat(3, volresize_kd(squeeze(Knots(:,:,1)), k,d, interp_type), ...
                 volresize_kd(squeeze(Knots(:,:,2)), k,d, interp_type));
    if any(size(tmp) < [ksz_new,2])
        warning('Knots upsampling unexpected behavior');
        tmp2 = zeros([ksz_new, 2]);
        szs = min([size(tmp,1), size(tmp,2)], ksz_new);
        tmp2(1:szs(1), 1:szs(2),  :) = tmp(1:szs(1), 1:szs(2),  :);
        tmp = tmp2;
    end
    Kn = tmp(1:ksz_new(1), 1:ksz_new(2), :);

    Knots_n = Kn;
    if strcmp(upsampling_type, 'variation')
        Tmin = cubic_disp_2d(squeeze(Knots), old_vsz, old_spacing);
        Tmin_u = cat(3, imresize_my(Tmin(:,:, 1), new_vsz, 1), ...
                        imresize_my(Tmin(:,:, 2), new_vsz, 1));
        objf = @(x) align_knots(x, Tmin_u, new_vsz, [ksz_new, 2], new_spacing);
        uopt = []; uopt.method = 'cg';
        uopt.MaxIter = 10;
        uopt.Corr = 15;
        uopt.Display = 'off';
        uopt.DerivativeCheck = 'off';
        uopt.LS_type = 0; uopt.LS_init = 8;
        K2 = minFunc(objf, Kn(:), uopt);
        Knots_n = reshape(K2, [ksz_new, 2]);
    end
end

function [f, gr] = align_knots(K, T, szv, szk, grid_spacing)
    K = reshape(K, szk);
    Kx = cubic_disp_2d(K, szv, grid_spacing);
    df = Kx - T;
    f = sum(df(:).^2)/2;
    [gr1, gr2] = cubic_partial_conv_2d(df(:,:, 1), df(:,:, 2), size(K), grid_spacing);
    gr = [gr1(:); gr2(:)];
end


function [res] = runsvm(Ks,lk)
% Copyright 2012 Nino Shervashidze, Karsten Borgwardt
% runsvm(Ks,lk)
% K = 1 x h cell array of kernelmatrices (n*n)
% lk = vector of class labels (n*1)
% cv = number of folds in cross-validation

% independent scheme
% best c

addpath('~/code/libsvm');
n=length(lk) % size of the dataset
% randomly permute labels: r will be also used for permuting the kernel matrices
r = randperm(n);
lk = lk(r);

% specify range of c-values
cvalues = (10 .^ [-7:2:7]) / size(lk,1);

cv = 10;
p80 = ceil(n * (1-2/cv));
p90 = ceil(n * (1-1/cv));
fs = n - p90; % fold size

% output variables
res.optkernel=zeros(cv,1);
res.optc=zeros(cv,1);
res.accuracy=zeros(cv,1);

% cross-validation loop
opth=zeros(1,cv);
for k = 1:cv
  imresult=[];

  height = length(Ks);
  for h=1:height
    K = Ks{h}(r,r);
    K_current = K([k*fs+1:size(K,2),1:(k-1)*fs,(k-1)*fs+1:k*fs],[k*fs+1:size(K,2),1:(k-1)*fs,(k-1)*fs+1:k*fs]);
    lk_current = lk([k*fs+1:size(K,2),1:(k-1)*fs,(k-1)*fs+1:k*fs]);
    K_current = makepos(K_current);
    K1 = [(1:size(K_current,1))', normalizekm(K_current)];

    for i = 1:size(cvalues,2)
      % train on 80%, predict on 10% (from 81% to 90%)
      size(lk_current(1:p80));
      size(K1(1:p80,1:p80+1));
      model = svmtrain(lk_current(1:p80,1), K1(1:p80,1:p80+1), strcat(['-t 4  -c ' num2str(cvalues(i))]));
      [predict_label, accuracy, dec_values] = svmpredict(lk_current(p80+1:p90,1),K1(p80+1:p90,1:p80+1), model);
      imresult(h,i)= accuracy(1);
    end
  end

  % determine optimal h and c
  [junk,position]= max(imresult(:));
  [optimalh, indoptimalc]=ind2sub(size(imresult),position);

  opth(k)=optimalh;
  res.optc(k)= cvalues(indoptimalc);
  res.optkernel(k)=optimalh;
  % train on 90% with optimal c, predict on 10% (from 91% to 100%)
  K = Ks{optimalh}(r,r);
  K_current = K([k*fs+1:size(K,2),1:(k-1)*fs,(k-1)*fs+1:k*fs],[k*fs+1:size(K,2),1:(k-1)*fs,(k-1)*fs+1:k*fs]);
  lk_current = lk([k*fs+1:size(K,2),1:(k-1)*fs,(k-1)*fs+1:k*fs]);
  K_current = makepos(K_current);
  K1 = [(1:size(K_current,1))', normalizekm(K_current)];

  model = svmtrain(lk_current(1:p90,1), K1(1:p90,1:p90+1),strcat(['-t 4  -c ' num2str(cvalues(indoptimalc))]) );
  [predict_label, accuracy, dec_values] = svmpredict(lk_current(p90+1:size(K,1),1), K1(p90+1:size(K,1),1:p90+1), model);
  res.accuracy(k)=accuracy(1)
end
res.mean_acc =  mean(res.accuracy)
res.std_acc = std(res.accuracy)
end

function result = makepos(K)
pd = 0;
addc = 10e-7;
while (pd ==  0)

addc = addc * 10
try
if (isinf(addc) == 1)
pd = 1;
else
chol(normalizekm(K + eye(size(K,1),size(K,1)) * addc));
pd = 1;
end
catch

end

end
if (isinf(addc)==0)
result = K + eye(size(K,1),size(K,1)) * addc;
else
result = eye(size(K,1));
end
end


function analyticalSolution = analyticalSolutionQP(HessianMatrixQP,gradientQP)

    % ANALYTICALSOLUTIONQP provides the unconstrained solution of a QP
    %                      problem. To be used as possible alternative when
    %                      the WBToolbox "QP block" fails to find a solution.
    %
    % FORMAT: analyticalSolution = analyticalSolutionQP(HessianMatrixQP,gradientQP)
    %
    % INPUT:   - HessianMatrixQP = hessian matrix of the QP problem;
    %          - gradientQP      = gradient of the QP problem.
    %
    % OUTPUT:  - analyticalSolution = the analytical solution of the QP problem.
    %
    % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava
    %
    %          all authors are with the Italian Istitute of Technology (IIT)
    %          email: name.surname@iit.it
    %
    % Genoa, Dec 2017
    %

    %% --- Initialization ---

    analyticalSolution = -inv(HessianMatrixQP)*gradientQP;
end


function [mu, sigma, sigma_points] = prediction_step(mu, sigma, u)
% Updates the belief concerning the robot pose according to the motion model.
% mu: state vector containing robot pose and poses of landmarks obeserved so far
% Current robot pose = mu(1:3)
% Note that the landmark poses in mu are stacked in the order by which they were observed
% sigma: the covariance matrix of the system.
% u: odometry reading (r1, t, r2)
% Use u.r1, u.t, and u.r2 to access the rotation and translation values

% For computing lambda.
global scale;

% Compute sigma points
sigma_points = compute_sigma_points(mu, sigma);

% Dimensionality
n = length(mu);
% lambda
lambda = scale - n;

% TODO: Transform all sigma points according to the odometry command
% Remember to vectorize your operations and normalize angles
% Tip: the function normalize_angle also works on a vector (row) of angles
for i=1:2*n+1
    sigma_points(1:3,i) = sigma_points(1:3,i) + [u.t*cos(sigma_points(3,i) + u.r1); u.t*sin(sigma_points(3,i) + u.r1); u.r1 + u.r2];
    sigma_points(3,i) = normalize_angle(sigma_points(3,i));
endfor
% Computing the weights for recovering the mean
wm = [lambda/scale, repmat(1/(2*scale),1,2*n)];
wc = wm;

% -------- My initialization ----------- %
xbar = 0;
ybar = 0;
mu = zeros(n,1);
sigma = zeros(n);
% TODO: recover mu.
% Be careful when computing the robot's orientation (sum up the sines and
% cosines and recover the 'average' angle via atan2)
for i=1:2*n+1
    mu = mu + wm(i)*sigma_points(:,i);
    xbar = xbar + wm(i)*cos(sigma_points(3,i));
    ybar = ybar + wm(i)*sin(sigma_points(3,i));
endfor
mu(3) = atan2(ybar,xbar);
mu(3) = normalize_angle(mu(3));

% TODO: Recover sigma. Again, normalize the angular difference
for i=1:2*n+1
    tmp = sigma_points(:,i) - mu;
    tmp(3) = normalize_angle(tmp(3));
    sigma = sigma + wc(i)*tmp*tmp';
endfor

% Motion noise
motionNoise = 0.1;
R3 = [motionNoise, 0, 0;
     0, motionNoise, 0;
     0, 0, motionNoise/10];
R = zeros(size(sigma,1));
R(1:3,1:3) = R3;

% TODO: Add motion noise to sigma
sigma = sigma + R;

end


function varargout = weights_int(mode, n, t0, Hdyn, Zdyn, Tdyn, Rdyn, Qdyn, Hmat, Zmat, Tmat, Rmat, Qmat, P, tol)
% mode:
%   0 - all primitive outputs.
%   1 - calculate Kalman filter weights.
%   2 - calculate Kalman smoother weights.

%% Initialization %%
m       = size(P, 1);
D       = (P == Inf);
init    = any(any(D)); % use exact initialization if init = true.
if init, d = n+1; P(D) = 0; P_inf = double(D);
else d = 0; end
stationary  = ~Hdyn && ~Zdyn && ~Tdyn && ~Rdyn && ~Qdyn;
converged   = false;
RQdyn       = Rdyn || Qdyn;
if ~Hdyn, H = Hmat; end
if ~Zdyn, Z = Zmat; end
if ~Tdyn, T = Tmat; end
if ~Rdyn, R = Rmat; end
if ~Qdyn, Q = Qmat; end
if ~RQdyn, RQRt = R*Q*R'; end

%% Preallocate Output Results %%
switch mode
    case 0 % all primitive outputs
        Result_P    = cell(1, n);
        Result_P{1} = P;
        Result_invF = cell(1, n);
        Result_K    = cell(1, n);
        Result_L    = cell(1, n);
        Result_N    = cell(1, n);
        Result_W    = cell(1, n);
    case 1 % calculate Kalman filter weights
        Result_K        = cell(1, t0-1);
        Result_L        = cell(1, t0-1);
        Result_omega    = cell(1, t0-1);
    case 2 % calculate Kalman smoother weights
        Result_invF         = cell(1, n);
        Result_K            = cell(1, n);
        Result_L            = cell(1, n);
        Result_LLasc        = cell(1, n);
        if t0 > 1, Result_LLasc{t0-1} = eye(m);
        else Result_P = P; end % t0 == 1
        Result_omega        = cell(1, t0-1);
        Result_omegaalpha   = cell(1, n);
end
Fns     = true(1, n); % Is F_inf nonsingular for each iteration

%% Kalman filter loop %%
for t = 1 : n
    if Hdyn, H = Hmat{t}; end
    if Zdyn, Z = Zmat{t}; end
    if Tdyn, T = Tmat{t}; end
    if Rdyn, R = Rmat{t}; end
    if Qdyn, Q = Qmat{t}; end
    if RQdyn, RQRt = R*Q*R'; end
    if ~converged
        if init
            %% Exact initial Kalman filter %%
            M       = P*Z';
            M_inf   = P_inf*Z';
            A_inf   = T*P_inf;
            if abs(M_inf) < tol % F_inf is zero
                Fns(t)  = false;
                invF    = inv(Z*M + H);
                K       = T*M*invF;
                L       = T - K*Z;
                P       = T*P*L' + RQRt;
                P_inf   = A_inf*T';
            else % F_inf is assumed to be nonsingular
                invF    = inv(Z*M_inf); % This is actually invF1
                F2      = -invF*(Z*M + H)*invF;
                K       = T*M_inf*invF;
                K1      = T*(M*invF + M_inf*F2);
                L       = T - K*Z;
                L1      = -K1*Z;
                P       = A_inf*L1' + T*P*L' + RQRt;
                P_inf   = A_inf*L';
            end
            if abs(P_inf) < tol, d=t; init=false; end
        else
            %% Normal Kalman filter %%
            M       = P*Z';
            invF    = inv(Z*M + H);
            K       = T*M*invF;
            L       = T - K*Z;
            prevP   = P;
            P       = T*P*L' + RQRt;
            if stationary, if abs(P-prevP) < tol, converged = true; end, end
        end
    end
    %% Store results for this time point %%
    switch mode
        case 0 % all primitive outputs
            Result_P{t+1}   = P;
            Result_invF{t}  = invF;
            Result_K{t}     = K;
            Result_L{t}     = L;
        case 1 % calculate Kalman filter weights
            Result_K{t}     = K;
            Result_L{t}     = L;
            if t >= t0 - 1, break; end
        case 2 % calculate Kalman smoother weights
            if t == t0 - 1, Result_P    = P; end
            if t == t0, Result_LLasc{t} = L'; end
            Result_invF{t}  = invF;
            Result_K{t}     = K;
            Result_L{t}     = L;
            if t > t0
                Result_LLasc{t} = Result_LLasc{t-1}*L';
            end
    end
end

%% Backwards recursion %%
switch mode
    case 0 % all primitive outputs
        N       = zeros(m);
        for t = n : -1 : 1
            if Hdyn, H = Hmat{t}; end
            if Zdyn, Z = Zmat{t}; end
            invF        = Result_invF{t};
            K           = Result_K{t};
            L           = Result_L{t};
            Result_W{t} = H*(invF*Z - K'*N*L);
            Result_N{t} = N;
            if t <= d && Fns(t), N = L'*N*L; else N = Z'*invF*Z + L'*N*L; end
        end
    case 1 % calculate Kalman filter weights
        LLdsc   = eye(m);
        for t = t0-1 : -1 : 1
            Result_omega{t} = LLdsc*Result_K{t};
            LLdsc           = LLdsc*Result_L{t};
        end
    case 2 % calculate Kalman smoother weights
        N       = zeros(m);
        LLdsc   = eye(m);
        for t = n : -1 : 1
            if t >= t0
                if t <= d && Fns(t)
                    K       = Result_K{t};
                    L       = Result_L{t};
                    W2      = -K'*N*L;
                    N       = L'*N*L;
                else
                    if Zdyn, Z = Zmat{t}; end
                    invF    = Result_invF{t};
                    K       = Result_K{t};
                    L       = Result_L{t};
                    W2      = invF*Z - K'*N*L;
                    N       = Z'*invF*Z + L'*N*L;
                end
                if t == t0
                    Result_omegaalpha{t} = Result_P*W2';
                    IPN = eye(m) - Result_P*N;
                else Result_omegaalpha{t} = Result_P*Result_LLasc{t-1}*W2'; end
            else
                Result_omega{t}         = LLdsc*Result_K{t};
                LLdsc                   = LLdsc*Result_L{t};
                Result_omegaalpha{t}    = IPN*Result_omega{t};
            end
        end
end

%% Output Results %%
switch mode
    case 0 % all primitive outputs
        varargout = {Result_P Result_invF Result_K Result_L Result_W Result_N};
    case 1 % calculate Kalman filter weights
        varargout = {Result_omega};
    case 2 % calculate Kalman smoother weights
        varargout = {Result_omega Result_omegaalpha};
end


function value = r8_gamma ( x )

%*****************************************************************************80
%
%% R8_GAMMA evaluates Gamma(X) for a real argument.
%
%  Discussion:
%
%    This routine calculates the gamma function for a real argument X.
%
%    Computation is based on an algorithm outlined in reference 1.
%    The program uses rational functions that approximate the gamma
%    function to at least 20 significant decimal digits.  Coefficients
%    for the approximation over the interval (1,2) are unpublished.
%    Those for the approximation for 12 <= X are from reference 2.
%
%    MATLAB provides a GAMMA function, which is likely to be faster, more
%    accurate, and which vectorizes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by William Cody, Laura Stoltz.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    William Cody,
%    An Overview of Software Development for Special Functions,
%    in Numerical Analysis Dundee, 1975,
%    edited by GA Watson,
%    Lecture Notes in Mathematics 506,
%    Springer, 1976.
%
%    John Hart, Ward Cheney, Charles Lawson, Hans Maehly,
%    Charles Mesztenyi, John Rice, Henry Thatcher,
%    Christoph Witzgall,
%    Computer Approximations,
%    Wiley, 1968,
%    LC: QA297.C64.
%
%  Parameters:
%
%    Input, real X, the argument of the function.
%
%    Output, real VALUE, the value of the function.
%

%
%  Coefficients for minimax approximation over (12, INF).
%
  c = [ ...
   -1.910444077728E-03, ...
    8.4171387781295E-04, ...
   -5.952379913043012E-04, ...
    7.93650793500350248E-04, ...
   -2.777777777777681622553E-03, ...
    8.333333333333333331554247E-02, ...
    5.7083835261E-03 ];
%
%  Mathematical constants
%
  sqrtpi = 0.9189385332046727417803297;
%
%  Machine dependent parameters
%
  xbig = 171.624E+00;
  xminin = 2.23E-308;
  eps = 2.22E-16;
  xinf = 1.79E+308;
%
%  Numerator and denominator coefficients for rational minimax
%  approximation over (1,2).
%
  p = [ ...
   -1.71618513886549492533811E+00, ...
    2.47656508055759199108314E+01, ...
   -3.79804256470945635097577E+02, ...
    6.29331155312818442661052E+02, ...
    8.66966202790413211295064E+02, ...
   -3.14512729688483675254357E+04, ...
   -3.61444134186911729807069E+04, ...
    6.64561438202405440627855E+04 ];

  q = [ ...
   -3.08402300119738975254353E+01, ...
    3.15350626979604161529144E+02, ...
   -1.01515636749021914166146E+03, ...
   -3.10777167157231109440444E+03, ...
    2.25381184209801510330112E+04, ...
    4.75584627752788110767815E+03, ...
   -1.34659959864969306392456E+05, ...
   -1.15132259675553483497211E+05 ];

  parity = 0;
  fact = 1.0;
  n = 0;
  y = x;
%
%  Argument is negative.
%
  if ( y <= 0.0 )

    y = - x;
    y1 = floor ( y );
    res = y - y1;

    if ( res ~= 0.0 )

      if ( y1 ~= floor ( y1 * 0.5 ) * 2.0 )
        parity = 1;
      end

      fact = - pi / sin ( pi * res );
      y = y + 1.0;

    else

      res = xinf;
      value = res;
      return

    end

  end
%
%  Argument is positive.
%
  if ( y < eps )
%
%  Argument < EPS.
%
    if ( xminin <= y )
      res = 1.0 / y;
    else
      res = xinf;
      value = res;
      return
    end

  elseif ( y < 12.0 )

    y1 = y;
%
%  0.0 < argument < 1.0.
%
    if ( y < 1.0 )

      z = y;
      y = y + 1.0;
%
%  1.0 < argument < 12.0.
%  Reduce argument if necessary.
%
    else

      n = floor ( y ) - 1;
      y = y - n;
      z = y - 1.0;

    end
%
%  Evaluate approximation for 1.0 < argument < 2.0.
%
    xnum = 0.0;
    xden = 1.0;
    for i = 1 : 8
      xnum = ( xnum + p(i) ) * z;
      xden = xden * z + q(i);
    end

    res = xnum / xden + 1.0;
%
%  Adjust result for case  0.0 < argument < 1.0.
%
    if ( y1 < y )

      res = res / y1;
%
%  Adjust result for case 2.0 < argument < 12.0.
%
    elseif ( y < y1 )

      for i = 1 : n
        res = res * y;
        y = y + 1.0;
      end

    end

  else
%
%  Evaluate for 12.0 <= argument.
%
    if ( y <= xbig )

      ysq = y * y;
      sum = c(7);
      for i = 1 : 6
        sum = sum / ysq + c(i);
      end
      sum = sum / y - y + sqrtpi;
      sum = sum + ( y - 0.5 ) * log ( y );
      res = exp ( sum );

    else

      res = xinf;
      value = res;
      return

    end

  end
%
%  Final adjustments and return.
%
  if ( parity )
    res = - res;
  end

  if ( fact ~= 1.0 )
    res = fact / res;
  end

  value = res;

  return
end


function [y,mask] = vl_nndropout(x,varargin)
%VL_NNDROPOUT CNN dropout.
%   [Y,MASK] = VL_NNDROPOUT(X) applies dropout to the data X. MASK
%   is the randomly sampled dropout mask. Both Y and MASK have the
%   same size as X.
%
%   VL_NNDROPOUT(X, 'rate', R) sets the dropout rate to R.
%
%   [DZDX] = VL_NNDROPOUT(X, DZDY, 'mask', MASK) computes the
%   derivatives of the blocks projected onto DZDY. Note that MASK must
%   be specified in order to compute the derivative consistently with
%   the MASK randomly sampled in the forward pass. DZDX and DZDY have
%   the same dimesnions as X and Y respectivey.
%
%   Note that in the original paper on dropout, at test time the
%   network weights for the dropout layers are scaled down to
%   compensate for having all the neurons active. In this
%   implementation the dropout function itself already does this
%   compensation during training. So at test time no alterations are
%   required.

% Copyright (C) 2014-15 Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

opts.rate = 0.5 ;
opts.mask = [] ;

backMode = numel(varargin) > 0 && ~isstr(varargin{1}) ;
if backMode
  dzdy = varargin{1} ;
  opts = vl_argparse(opts, varargin(2:end)) ;
else
  opts = vl_argparse(opts, varargin) ;
end

% determine mask
mask = opts.mask ;
scale = single(1 / (1 - opts.rate)) ;
if backMode && isempty(mask)
  warning('vl_nndropout: when using in backward mode, the mask should be specified') ;
end
if isempty(mask)
  if isa(x,'gpuArray')
    mask = scale * single(gpuArray.rand(size(x)) >= opts.rate) ;
  else
    mask = scale * single(rand(size(x)) >= opts.rate) ;
  end
end

% do job
if ~backMode
  y = mask .* x ;
else
  y = mask .* dzdy ;
end


%% This file is the main file of camparing the noise sensitivity of iSINDy
% and SINDy-PI method. This file takes a long time to run.
%
% Last Update: 2019/07/17
% Coded By: K

%% Close all, clear all, clc
close all;clear all; clc;
addpath('./Functions')
addpath('Datas')
set(0,'defaulttextInterpreter','latex')
%% Simulate the budworm population growth and gather the simulation data

% Define the system parameters
jx=0.6;Vmax=1.5;Km=0.3;

% Determine the simulation time step and time span
dt=0.1; T=5; tspan=0:dt:T;

% Define noise level and add gaussian noise to the data
noise=0;

% Define whehter you have control, if you have it, please define it
Control=0;u=0;

%Define whether you want to shuffel the final data
Shuffle=0;

%% Set up some parameters
% Get the number of states we have
n_state=1;

% Define the control input(Should be zero in our example)
n_control=0;

% Choose whether you want to display actual ODE or not
disp_actual_ode=1;

% If the ODEs you want to display is the actual underlyting dynamics of the
% system, please set actual as 1
actual=1;

% Print the actual ODE we try to discover
Print_ODEs(@(t,y)MMK_ODE(t,y,jx,Vmax,Km),n_state,n_control,disp_actual_ode,actual);

% Create symbolic states
dz=sym('dz',[n_state,1]);

% Now we first create the parameters of the function right hand side
Highest_Poly_Order_Guess=1;
Highest_Trig_Order_Guess=0;
Highest_U_Order_Guess=0;

% Then create the right hand side library parameters
Highest_Poly_Order=4;
Highest_Trig_Order=0;
Highest_U_Order=0;
Highest_dPoly_Order=1;

% Determine whether you want to normalize your libary or not. 1 is yes and
% 0 is no.
NormalizeLib=0;

% Determine the model selection method
Model_Selection_Method=1;
% Determine the prediction step
Prediction_Steps=0;

% The follwoing parameters will determine the for loop and loop through the
% different noise level
d_percent=1;
percent=0;
percent_start=1;
% Must be smaller or equal to 24
percent_end=24;

% Determine a vector to store whether SINDy-PI identification is correct
is_Right=zeros(percent_end-percent_start+1,1);

% Define a matrix to store the null space vector
Xi_ns=cell(percent_end-percent_start+1,1);

% Turn off the rank deficient warning
MSGID='MATLAB:rankDeficientMatrix';
warning('off',MSGID);

% Determine how many iteration you need
Final_pin=30;

% Choose which denoise method you want:
% 1 is adding Guassian noise to actual derivative, 2 is direct
% finite difference, 3 is TVRegDiff.
Noisy_Data_Generation_Method=3;

% Determine how many initial conditions you need
init_num=2400;

% Create the new directory to save the function files
FolderName=strcat('Result_i_SINDy_Derivative_Method',num2str(Noisy_Data_Generation_Method),'_PridictionStep_',num2str(Prediction_Steps),'_IniNum_',num2str(init_num));
[fld_status, fld_msg, fld_msgID]=mkdir('TempFunctions_iSINDy');
[fld_status, fld_msg, fld_msgID]=mkdir(FolderName);
addpath('TempFunctions_iSINDy')

%% Now calculate the sensitivity of the iSINDy and SINDy-PI to the noise.

% Print the start process
fprintf('Start calculating ....\n\n\n')

for total=1:Final_pin
    percent=0;

    for percent_iter=percent_start:d_percent:percent_end
        % Set the pin one step forward
        percent=percent+1;

        % Set the noise level
        noise_level(percent,1)=Determine_Noise_Level(percent_iter);

        % Print the current noise level
        fprintf('Using the noise level as %i on implicit-SINDy.\n',noise_level(percent,1))

        % Read files, load the training data and testing data
        File_Name=strcat('Data_Iter_',num2str(total),'_NoiseLevel_',num2str(noise_level(percent,1)),'_NoiseMethod_',num2str(Noisy_Data_Generation_Method),'_GoodLuck.mat');
        load(File_Name)
        [size1,size2]=size(xt_test);

        %% Run implicit-SINDy (some function in this section is obtained from the github code iSINDy )
        fprintf('\n\n\n\n USing implicit-SINDy now...\n')

        % Sweep through the states
        %tic
        for iter=1:n_state
            fprintf('\n \n Calculating the %i expression...\n',iter)

            % Build library data
            [iSINDy_Data,iSINDy_Struct]=SINDyLib(Data,dData(:,iter),u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order,Highest_dPoly_Order);

            % Use Null-Space method and set the tolerance
            tol = 1e-5;

            % Plotting option
            pflag=0;

            % Print process
            fprintf('\n \n \t Calculating the null space...\n')
            try
                % Sparse Regression
                [Xi_ns{percent,1}, indTheta, lambdavec, numterms, errorv] = ADMpareto(iSINDy_Data, tol, pflag);
                Xi_dum=zeros(10,28);
                Xi_dum=Xi_ns{percent,1};
                %Define the variable to store the ODE expression and score of each
                % expression. This is necessary to perform the parallel for loop.
                ODEs_iS=cell(n_state,size(Xi_dum,2)-1);
                if percent==1 && iter==1
                    Score_iS=NaN*ones(percent_end-percent_start+1,size(Xi_dum,2)-1);
                end
                dz_dum=dz(iter);

                % Sweep the null space and store the score
                parfor j=1:(size(Xi_dum,2)-1)

                    % Print which vector we are working on
                    fprintf('\t\t Sweeping the %i null sapce vector...\n',j)

                    % Validate each sparse vector
                    try
                        % Solve for the ODE expression
                        ODEs_iS{iter,j}=solve(vpa(cell2sym(iSINDy_Struct)*Xi_dum(:,j))==0,dz_dum);

                        % Store the previous result into a matrix
                        Generate_ODE_RHS(ODEs_iS{iter,j},n_state,n_control,strcat('TempFunctions_iSINDy/ParForiS',num2str(j)));

                        % Calculate the accuracy of the file
                        Test_Score=0;
                        Test_Score=Get_Score(dxt_test,xt_test,u,Control,tspan,Shuffle,strcat('ParForiS',num2str(j)),Prediction_Steps,dt,size1,size2,Model_Selection_Method);
                        Score_iS(percent,j)=Test_Score;
                    catch
                        ODEs_iS{iter,j}=NaN;
                        Score_iS(percent,j)=NaN;
                    end
                end
            catch
                fprintf('\n\t\t\tSomething is wrong....\n')
            end

        end

    end

    % Store the result for this iteration

    % Store the minimum of implicit SINDy
    for pin1=1:size(Score_iS,1)
        [min_iS_Val(total,pin1),Index1(total,pin1)]=min(Score_iS(pin1,:));

    end

    % Test whether the correct structure is identified
    for pin1=1:size(Score_iS,1)
        Xi_dum=Xi_ns{pin1,1};
        Coffs=Xi_dum(:,Index1(total,pin1));
        Coffs_dum=Coffs~=0;
        if Coffs_dum==[1;1;0;0;0;1;1;0;0;0]
            is_Right(pin1,1)=is_Right(pin1,1)+1;
            Coffs_norm=Coffs/Coffs(7);
            Coffs_error(total,pin1)=norm(Coffs_norm-[-0.18;0.9;0;0;0;0.3;1;0;0;0]);
        elseif Coffs_dum==[0;1;1;0;0;0;1;1;0;0]
            is_Right(pin1,1)=is_Right(pin1,1)+1;
            Coffs_norm=Coffs/Coffs(8);
            Coffs_error(total,pin1)=norm(Coffs_norm-[0;-0.18;0.9;0;0;0;0.3;1;0;0]);
        elseif Coffs_dum==[0;0;1;1;0;0;0;1;1;0]
            is_Right(pin1,1)=is_Right(pin1,1)+1;
            Coffs_norm=Coffs/Coffs(9);
            Coffs_error(total,pin1)=norm(Coffs_norm-[0;0;-0.18;0.9;0;0;0;0.3;1;0]);
        elseif Coffs_dum==[0;0;0;1;1;0;0;0;1;1]
            is_Right(pin1,1)=is_Right(pin1,1)+1;
            Coffs_norm=Coffs/Coffs(10);
            Coffs_error(total,pin1)=norm(Coffs_norm-[0;0;0;-0.18;0.9;0;0;0;0.3;1]);
        end
    end

    for pin1=1:size(Score_iS,1)
        try
            Num=sum(Coffs_error(:,pin1)~=0);
            Ave_Coff_Error(pin1,1)=sum(Coffs_error(:,pin1))/Num;
        catch
            Ave_Coff_Error(pin1,1)=NaN;
        end
    end

    %% Plot the result
    figure(1)
    plot(noise_level,log(1+mean(min_iS_Val,1)),'o','linewidth',2,'color','blue')
    drawnow
    grid on
    title('Peformance Comparison','FontSize',18)
    xlabel('Noise Level $\sigma$','FontSize', 18)
    ylabel('Average L2 Norm','FontSize', 18)
    set(gca,'FontSize',18);
    set(gcf,'Position',[100 100 600 400]);
    set(gcf,'PaperPositionMode','auto');

    figure(2)
    plot(noise_level,is_Right/total,'o')
    drawnow
    grid on
    title('Success Rate','FontSize',18)
    xlabel('Noise Level $\sigma$','FontSize', 18)
    ylabel('Success Rate','FontSize', 18)
    ylim([0 1])
    set(gca,'FontSize',18);
    set(gcf,'Position',[100 100 600 400]);
    set(gcf,'PaperPositionMode','auto');
    set(gca, 'XScale', 'log')

    figure(3)
    plot(noise_level,Ave_Coff_Error,'o')
    drawnow
    grid on
    title('Average Parameter Error','FontSize',18)
    xlabel('Noise Level $\sigma$','FontSize', 18)
    ylabel('L2 norm','FontSize', 18)
    ylim([0 total])
    set(gca,'FontSize',18);
    set(gcf,'Position',[100 100 600 400]);
    set(gcf,'PaperPositionMode','auto');
    set(gca, 'XScale', 'log')

    % Save the calculation result of current iteration
    cc=clock;
    ResultName=strcat(FolderName,'/implicit_SINDY_Result',num2str(total),'__',num2str(cc(3)),'_',num2str(cc(4)),'_',num2str(cc(5)),'_',num2str(round(cc(6))),'.mat');
    save(ResultName,'Score_iS','Xi_ns')

end


% OP_SU_EV_TP: assemble the matrix A = [a(i,j)], a(i,j) = 1/2 (sigma (u_j), epsilon (v_i)), exploiting the tensor product structure.
%
%   mat = op_su_ev_tp (spu, spv, msh, lambda, mu);
%   [rows, cols, values] = op_su_ev_tp (spu, spv, msh, lambda, mu);
%
% INPUT:
%
%   spu:     object representing the space of trial functions (see sp_vector)
%   spv:     object representing the space of test functions (see sp_vector)
%   msh:     object that defines the domain partition and the quadrature rule (see msh_cartesian)
%   lambda, mu: function handles to compute the Lame' coefficients
%
% OUTPUT:
%
%   mat:    assembled matrix
%   rows:   row indices of the nonzero entries
%   cols:   column indices of the nonzero entries
%   values: values of the nonzero entries
%
% Copyright (C) 2011, 2017 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function varargout = op_su_ev_tp (space1, space2, msh, lambda, mu)

  for icomp = 1:space1.ncomp_param
    for idim = 1:msh.ndim
      size1 = size (space1.scalar_spaces{icomp}.sp_univ(idim).connectivity);
      size2 = size (space2.scalar_spaces{icomp}.sp_univ(idim).connectivity);
      if (size1(2) ~= size2(2) || size1(2) ~= msh.nel_dir(idim))
        error ('One of the discrete spaces is not associated to the mesh')
      end
    end
  end

  A = spalloc (space2.ndof, space1.ndof, 5*space1.ndof);

  for iel = 1:msh.nel_dir(1)
    msh_col = msh_evaluate_col (msh, iel);
    sp1_col = sp_evaluate_col (space1, msh_col, 'value', false, ...
                               'gradient', true, 'divergence', true);
    sp2_col = sp_evaluate_col (space2, msh_col, 'value', false, ...
                               'gradient', true, 'divergence', true);

    for idim = 1:msh.rdim
      x{idim} = reshape (msh_col.geo_map(idim,:,:), msh_col.nqn, msh_col.nel);
    end

    A = A + op_su_ev (sp1_col, sp2_col, msh_col, lambda (x{:}), mu (x{:}));
  end

  if (nargout == 1)
    varargout{1} = A;
  elseif (nargout == 3)
    [rows, cols, vals] = find (A);
    varargout{1} = rows;
    varargout{2} = cols;
    varargout{3} = vals;
  end

end


%--- help for arima/simulate ---
%
% SIMULATE Simulate ARIMA model responses and conditional variances
%
%  Syntax:
%
%    [Y,E,V] = simulate(Mdl,numObs)
%    [Y,E,V] = simulate(Mdl,numObs,param1,val1,param2,val2,...)
%
%  Description:
%
%    Simulate sample paths of responses, innovations, and conditional
%    variances of a univariate ARIMA process.
%
%  Input Arguments:
%
%    Mdl - ARIMA model specification object, as produced by the ARIMA
%      constructor or ARIMA/ESTIMATE method.
%
%    numObs - Positive integer indicating the number of observations (rows)
%      generated for each path of the outputs Y, E, and V.
%
%  Optional Input Parameter Name/Value Pairs:
%
%    'NumPaths'   Positive integer indicating the number of sample paths
%                 (columns) generated for all simulated outputs. The default
%                 is 1.
%
%    'Y0'         Presample response data, providing initial values for the
%                 model. Y0 is a column vector or a matrix. If Y0 is a
%                 column vector, then it is applied to each simulated path.
%                 If Y0 is a matrix, then it must have at least NumPaths
%                 columns. Y0 may have any number of rows, provided at least
%                 Mdl.P observations exist to initialize the model. If the
%                 number of rows exceeds Mdl.P, then only the most recent
%                 Mdl.P observations are used. If the number of columns
%                 exceeds NumPaths, then only the first NumPaths columns are
%                 used. If Y0 is unspecified, any necessary presample
%                 observations are set to the unconditional mean for stationary
%                 AR processes, and to zero if the process is non-stationary
%                 or contains a regression component. The last row contains
%                 the most recent observation.
%
%    'E0'         Mean-zero presample innovations, providing initial values
%                 for the model. E0 is a column vector or a matrix. If E0 is
%                 a column vector, then it is applied to each simulated path.
%                 If E0 is a matrix, then it must have at least NumPaths
%                 columns. E0 may have any number of rows, provided sufficient
%                 observations exist to initialize the ARIMA model as well
%                 as any conditional variance model (the number of observations
%                 required is at least Mdl.Q, but may be more if a conditional
%                 variance model is included). If the number of rows
%                 exceeds the number necessary, then only the most recent
%                 observations are used. If the number of columns exceeds
%                 NumPaths, then only the first NumPaths columns are used. If
%                 no presample data is specified, any necessary observations
%                 are set to zero. The last row contains the most recent
%                 observation.
%
%    'V0'         Positive presample conditional variances, providing initial
%                 values for any conditional variance model; if the variance
%                 of the model is constant, then V0 is unnecessary. V0 is a
%                 column vector or a matrix. If V0 is a column vector, then
%                 it is applied to each simulated path. If V0 is a matrix,
%                 then it must have at least NumPaths columns. V0 may have
%                 any number of rows, provided sufficient observations exist
%                 to initialize any conditional variance model. If the number
%                 of rows exceeds the number necessary, then only the most
%                 recent observations are used. If the number of columns
%                 exceeds NumPaths, then only the first NumPaths columns are
%                 used. If no presample variance data is specified, any
%                 necessary observations are set to the unconditional variance
%                 of the conditional variance process. The last row contains
%                 the most recent observation.
%
%    'X'          Matrix of predictor data used to include a regression
%                 component in the conditional mean. Each column of X is a
%                 separate time series, and the last row of each contains
%                 the most recent observation of each series. The number of
%                 observations in X must equal or exceed numObs. When the
%                 number of observations in X exceeds numObs, only the most
%                 recent observations are used. If missing, the conditional
%                 mean will have no regression component regardless of the
%                 presence of any regression coefficients found in the model.
%
%  Output Arguments:
%
%    Y - numObs-by-NumPaths matrix of simulated response data.
%
%    E - numObs-by-NumPaths matrix of simulated mean-zero innovations.
%
%    V - numObs-by-NumPaths matrix of conditional variances of the
%      innovations in E.
%
%  Notes:
%
%    o Missing data values, indicated by NaNs, are removed from X by listwise
%      deletion (i.e., any row in X with at least one NaN is removed), reducing
%      the effective sample size. The presample data Y0, E0, and V0 are merged
%      into a composite series, and any row of the combined series with at
%      least one NaN is also removed by listwise deletion. The presample data
%      is also synchronized such that the last (most recent) observation of
%      each series occurs at the same time.
%
%   o  Regression models included in the conditional mean are based on the
%      presence of the predictor matrix X. Although each column of the output
%      time series represents a different path of the corresponding univariate
%      stochastic process, the regression matrix X represents as a single
%      path of a (possibly) multivariate time series matrix in which each
%      column is a different time series. When the conditional mean has a
%      regression component, the entire predictor matrix X is applied to
%      every column of the output time series.
%
%  References:
%
%    [1] Box, G. E. P., G. M. Jenkins, and G. C. Reinsel. Time Series
%        Analysis: Forecasting and Control. 3rd edition. Upper Saddle River,
%        NJ: Prentice-Hall, 1994.
%
%    [2] Enders, W. Applied Econometric Time Series. Hoboken, NJ: John Wiley
%        & Sons, 1995.
%
%    [3] Hamilton, J. D. Time Series Analysis. Princeton, NJ: Princeton
%        University Press, 1994.
%
%  See also ARIMA, FORECAST, ESTIMATE, INFER, FILTER.
%
%    Reference page in Doc Center
%       doc arima/simulate
%
%    Other functions named simulate
%
%       conjugateblm/simulate    generic/simulate
%       customblm/simulate       gjr/simulate
%       diffuseblm/simulate      regARIMA/simulate
%       dsge/simulate            sde/simulate
%       dtmc/simulate            semiconjugateblm/simulate
%       egarch/simulate          ssm/simulate
%       empiricalblm/simulate    varm/simulate
%       garch/simulate           vecm/simulate
%


function seed = latin_center_test01 ( seed )

%*****************************************************************************80
%
%% LATIN_CENTER_TEST01 tests LATIN_CENTER.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 April 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer SEED, an initial seed for the random number generator.
%
%    Output, integer SEED, the updated random number seed.
%
  dim_num = 2;
  point_num = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  LATIN_CENTER chooses a Latin cell arrangement,\n' );
  fprintf ( 1, '  and returns the centers of those cells.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension = %d\n', dim_num );
  fprintf ( 1, '  Number of points =  %d\n', point_num );
  fprintf ( 1, '  Using seed = %d\n', seed );

  [ x, seed ] = latin_center ( dim_num, point_num, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The Latin center points:\n' );
  fprintf ( 1, '\n' );

  for j = 1: point_num
    for i = 1: dim_num
      fprintf ( 1, '%10f  ', x(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


%% Copyright (C) 2016 Lagu
%% Copyright (C) 2016, 2018-2019, 2022 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
%% @deftypemethod  @@sym {[@var{A}, @var{b}] =} equationsToMatrix (@var{eqns}, @var{vars})
%% @deftypemethodx @@sym {[@var{A}, @var{b}] =} equationsToMatrix (@var{eqns})
%% @deftypemethodx @@sym {[@var{A}, @var{b}] =} equationsToMatrix (@var{eq1}, @var{eq2}, @dots{})
%% @deftypemethodx @@sym {[@var{A}, @var{b}] =} equationsToMatrix (@var{eq1}, @dots{}, @var{v1}, @var{v2}, @dots{})
%% Convert set of linear equations to matrix form.
%%
%% In its simplest form, equations @var{eq1}, @var{eq2}, etc can be
%% passed as inputs:
%% @example
%% @group
%% syms x y z
%% [A, b] = equationsToMatrix (x + y == 1, x - y + 1 == 0)
%%   @result{} A = (sym 2×2 matrix)
%%
%%       ⎡1  1 ⎤
%%       ⎢     ⎥
%%       ⎣1  -1⎦
%%
%%   @result{} b = (sym 2×1 matrix)
%%
%%       ⎡1 ⎤
%%       ⎢  ⎥
%%       ⎣-1⎦
%% @end group
%% @end example
%% In this case, appropriate variables @emph{and their ordering} will be
%% determined automatically using @code{symvar} (@pxref{@@sym/symvar}).
%%
%% In some cases it is important to specify the variables as additional
%% inputs @var{v1}, @var{v2}, etc:
%% @example
%% @group
%% syms a
%% [A, b] = equationsToMatrix (a*x + y == 1, y - x == a)
%%   @print{} ??? ... nonlinear...
%%
%% [A, b] = equationsToMatrix (a*x + y == 1, y - x == a, x, y)
%%   @result{} A = (sym 2×2 matrix)
%%
%%       ⎡a   1⎤
%%       ⎢     ⎥
%%       ⎣-1  1⎦
%%
%%   @result{} b = (sym 2×1 matrix)
%%
%%       ⎡1⎤
%%       ⎢ ⎥
%%       ⎣a⎦
%% @end group
%% @end example
%%
%% The equations and variables can also be passed as vectors @var{eqns}
%% and @var{vars}:
%% @example
%% @group
%% eqns = [x + y - 2*z == 0, x + y + z == 1, 2*y - z + 5 == 0];
%% [A, B] = equationsToMatrix (eqns, [x y])
%%   @result{} A = (sym 3×2 matrix)
%%
%%       ⎡1  1⎤
%%       ⎢    ⎥
%%       ⎢1  1⎥
%%       ⎢    ⎥
%%       ⎣0  2⎦
%%
%%   B = (sym 3×1 matrix)
%%
%%       ⎡ 2⋅z ⎤
%%       ⎢     ⎥
%%       ⎢1 - z⎥
%%       ⎢     ⎥
%%       ⎣z - 5⎦
%% @end group
%% @end example
%% @seealso{@@sym/solve}
%% @end deftypemethod

function [A, b] = equationsToMatrix(varargin)

  % when Symbols are specified, this won't be used
  s = findsymbols (varargin);

  cmd = {'L, symvars = _ins'
         'if not isinstance(L[-1], MatrixBase):'
         '    if isinstance(L[-1], Symbol):'  % Symbol given, fill vars...
         '        vars = list()'
         '        for i in reversed(range(len(L))):'
         '            if isinstance(L[i], Symbol):'
         '                vars = [L.pop(i)] + vars'
         '            else:'  % ... until we find a non-Symbol
         '                break'
         '    else:'
         '        vars = symvars'
         'else:'
         '    if len(L) == 1:'  % we have only a list of equations
         '        vars = symvars'
         '    else:'
         '        vars = L.pop(-1)'
         'if len(L) == 1:'  % might be matrix of eqns, don't want [Matrix]
         '    L = L[0]'
         'vars = list(vars)'
         'A, B = linear_eq_to_matrix(L, vars)'
         'return True, A, B' };

  for i = 1:length(varargin)
    varargin{i} = sym (varargin{i});
  end

  [s, A, b] = pycall_sympy__ (cmd, varargin, s);

  if ~s
    error('Cannot convert to matrix; system may be nonlinear.');
  end

end

%!test
%! syms x y z
%! [A, B] = equationsToMatrix ([x + y - z == 1, 3*x - 2*y + z == 3, 4*x - 2*y + z + 9 == 0], [x, y, z]);
%! a = sym ([1 1 -1; 3 -2 1; 4 -2 1]);
%! b = sym ([1; 3; -9]);
%! assert (isequal (A, a))
%! assert (isequal (B, b))

%!test
%! syms x y z
%! A = equationsToMatrix ([3*x + -3*y - 5*z == 9, 4*x - 7*y + -3*z == -1, 4*x - 9*y - 3*z + 2 == 0], [x, y, z]);
%! a = sym ([3 -3 -5; 4 -7 -3; 4 -9 -3]);
%! assert (isequal (A, a))

%!test
%! syms x y
%! [A, B] = equationsToMatrix ([3*x + 9*y - 5 == 0, -8*x - 3*y == -2]);
%! a = sym ([3 9; -8 -3]);
%! b = sym ([5; -2]);
%! assert (isequal (A, a))
%! assert (isequal (B, b))

%!test
%! % override symvar order
%! syms x y
%! [A, B] = equationsToMatrix ([3*x + 9*y - 5 == 0, -8*x - 3*y == -2], [y x]);
%! a = sym ([9 3; -3 -8]);
%! b = sym ([5; -2]);
%! assert (isequal (A, a))
%! assert (isequal (B, b))

%!test
%! syms x y z
%! [A, B] = equationsToMatrix ([x - 9*y + z == -5, -9*y*z == -5], [y, x]);
%! a = sym ([[-9 1]; -9*z 0]);
%! b = sym ([-5 - z; -5]);
%! assert (isequal (A, a))
%! assert (isequal (B, b))

%!test
%! syms x y
%! [A, B] = equationsToMatrix (-6*x + 4*y == 5, 4*x - 4*y - 5, x, y);
%! a = sym ([-6 4; 4 -4]);
%! b = sym ([5; 5]);
%! assert (isequal (A, a))
%! assert (isequal (B, b))

%!test
%! % vertical list of equations
%! syms x y
%! [A, B] = equationsToMatrix ([-6*x + 4*y == 5; 4*x - 4*y - 5], [x y]);
%! a = sym ([-6 4; 4 -4]);
%! b = sym ([5; 5]);
%! assert (isequal (A, a))
%! assert (isequal (B, b))

%!test
%! syms x y
%! [A, B] = equationsToMatrix (5*x == 1, y, x - 6*y - 7, y);
%! a = sym ([0; 1; -6]);
%! b = sym ([1 - 5*x; 0; -x + 7]);
%! assert (isequal (A, a))
%! assert (isequal (B, b))

%!error <nonlinear>
%! syms x y
%! [A, B] = equationsToMatrix (x^2 + y^2 == 1, x - y + 1, x, y);

%!test
%! % single equation
%! syms x
%! [A, B] = equationsToMatrix (3*x == 2, x);
%! a = sym (3);
%! b = sym (2);
%! assert (isequal (A, a))
%! assert (isequal (B, b))

%!test
%! % single equation w/ symvar
%! syms x
%! [A, B] = equationsToMatrix (3*x == 2);
%! a = sym (3);
%! b = sym (2);
%! assert (isequal (A, a))
%! assert (isequal (B, b))

%!error <unique>
%! syms x
%! equationsToMatrix (3*x == 2, [x x])


function p = nodes_brick27 ( )

%*****************************************************************************80
%
%% NODES_BRICK27 returns the natural coordinates of the BRICK27 element.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real P(3,27), the coordinates.
%
  p = [ ...
    -1.0, -1.0, -1.0; ...
    +1.0, -1.0, -1.0; ...
    +1.0, +1.0, -1.0; ...
    -1.0, +1.0, -1.0; ...
    -1.0, -1.0, +1.0; ...
    +1.0, -1.0, +1.0; ...
    +1.0, +1.0, +1.0; ...
    -1.0, +1.0, +1.0; ...
     0.0, -1.0, -1.0; ...
    +1.0,  0.0, -1.0; ...
     0.0, +1.0, -1.0; ...
    -1.0,  0.0, -1.0; ...
    -1.0, -1.0,  0.0; ...
    +1.0, -1.0,  0.0; ...
    +1.0, +1.0,  0.0; ...
    -1.0, +1.0,  0.0; ...
     0.0, -1.0, +1.0; ...
    +1.0,  0.0, +1.0; ...
     0.0, +1.0, +1.0; ...
    -1.0,  0.0, +1.0; ...
     0.0,  0.0, -1.0; ...
     0.0, -1.0,  0.0; ...
    +1.0,  0.0,  0.0; ...
     0.0, +1.0,  0.0; ...
    -1.0,  0.0,  0.0; ...
     0.0,  0.0, +1.0; ...
     0.0,  0.0,  0.0 ]';

  return
end


function [p1Clip, p2Clip]=clipLineSegment2Rect(rectMaxMin,p1,p2)
%%CLIPLINESEGMENT2RECT Given a 2D rectangle (axis-aligned) and given the
%           two endpoints of a line segment clip the line segment to the
%           rectangle. If the segment and the rectangle do not overlap,
%           then an empty matrix is returned.
%
%INPUTS: rectMaxMin A length-4 array specifying the maximum and minimum
%                   bounds in x and y. rectMaxMin=[xMin;xMax;yMin;yMax].
%                   The minimum values must be <=the maximum values.
%                p1 A length-2 array specifying the start of the line
%                   segment.
%                p2 A length-2 array specifying the end of the line
%                   segment.
%
%OUTPUTS: p1Clip, p2Clip These are the same p1 and p2 but have been clipped to the
%                start and end of the line segment. If nothing clips, the
%                empty matrices are returned.
%
%The algorithm of [1] is used, without speed improvements that are possible
%from direct substitution, which is mentioned in Section IX of [1]. The
%performance of the algorithm is discussed in [2].
%
%EXAMPLE:
% rect=[-1;1;-1;1];
% numLines=6;
% p1=zeros(2,numLines);
% %Lines entirely inside the box.
% p1(:,1)=[0;0]; p2(:,1)=[0.8;0.8];
% p1(:,2)=[0.6;0]; p2(:,2)=[-0.8;0.8];
% %Lines leaving via each edge of the box.
% p1(:,3)=[0.6;-0.1]; p2(:,3)=[1.6;-0.2];
% p1(:,4)=[-0.6;0.1]; p2(:,4)=[-1.6;0.2];
% p1(:,5)=[-0.1;0.5]; p2(:,5)=[-0.2;2.8];
% p1(:,6)=[0.1;-0.5]; p2(:,6)=[0.2;-1.8];
%
% figure(1)
% clf
% hold on
% %Draw the bounding box lines, extending them.
% extDist=3;
% plot([-extDist,extDist],[-1,-1],'-k','linewidth',2)
% plot([-extDist,extDist],[1,1],'-k','linewidth',2)
% plot([-1,-1],[-extDist,extDist],'-k','linewidth',2)
% plot([1,1],[-extDist,extDist],'-k','linewidth',2)
% for k=1:numLines
%     %Plot the line segments
%     plot([p1(1,k),p2(1,k)],[p1(2,k),p2(2,k)],'-b','linewidth',4)
%     %Clip the segments
%     [p1Clip,p2Clip]=clipLineSegment2Rect(rect,p1(:,k),p2(:,k));
%     %Plot the clipped segments.
%     if(~isempty(p1Clip))
%         plot([p1Clip(1),p2Clip(1)],[p1Clip(2),p2Clip(2)],'-r','linewidth',2)
%     end
% end
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.
%[2] F. Dévai, "Analysis of the Nichioll-Lee-Nicholl Algorithm," in
%    Proceedings of the International Conference on Computational Science
%    and its Applications, Signapore, pp. 726-736, 9-12 May, 2005.
%
%August 2020 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xMin=rectMaxMin(1);
xMax=rectMaxMin(2);
yMin=rectMaxMin(3);
yMax=rectMaxMin(4);

x1=p1(1);
y1=p1(2);
x2=p2(1);
y2=p2(2);

if(x1<xMin)
    [x1,y1,x2,y2,isVisible]=leftColumn(xMin,yMax,xMax,yMin,x1,y1,x2,y2);
elseif(x1>xMax)
    [x1,y1]=rotate180c(x1,y1);
    [x2,y2]=rotate180c(x2,y2);

    [x1,y1,x2,y2,isVisible]=leftColumn(-xMax,-yMin,-xMin,-yMax,x1,y1,x2,y2);

    [x1,y1]=rotate180c(x1,y1);
    [x2,y2]=rotate180c(x2,y2);
else
    [x1,y1,x2,y2,isVisible]=centerColumn(xMin,yMax,xMax,yMin,x1,y1,x2,y2);
end

if(isVisible==false)
    %If the line is not in the box at all.
    p1Clip=[];
    p2Clip=[];
else
    p1Clip=[x1;y1];
    p2Clip=[x2;y2];
end
end

function [x1,y1,x2,y2,isVisible]=leftColumn(xL,yT,xR,yB,x1,y1,x2,y2)
%%LEFTCOLUMN The case where (x1,y1) is to the left of the vertical line
%            x=xL.

if(x2<xL)
    isVisible=false;
elseif(y1>yT)
    [x1,y1,x2,y2,isVisible]=topLeftCorner(xL,yT,xR,yB,x1,y1,x2,y2);
elseif(y1<yB)
    y1=reflectxAxis(y1);
    y2=reflectxAxis(y2);
    [x1,y1,x2,y2,isVisible]=topLeftCorner(xL,-yB,xR,-yT,x1,y1,x2,y2);

    y1=reflectxAxis(y1);
    y2=reflectxAxis(y2);
else
    [x1,y1,x2,y2,isVisible]=leftEdge(xL,yT,xR,yB,x1,y1,x2,y2);
end
end

function [x1,y1,x2,y2,isVisible]=topLeftCorner(xL,yT,xR,yB,x1,y1,x2,y2)
%TOPLEFTCORNER The case where (x1,y1) is in the top-left corner.

    if(y2>yT)
        isVisible=false;
    else
        relx2=x2-x1;
        rely2=y2-y1;

        topProd=(yT-y1)*relx2;
        leftProd=(xL-x1)*rely2;
        %If (x2,y2) is below the line connecting (x1,y1) and (xL,yT).
        if(topProd>leftProd)
            [x1,y1,x2,y2,isVisible]=leftBottomRegion(xL,xR,yB,x1,y1,x2,y2,relx2,rely2,leftProd);
        else
            [x1,y1]=reflectxMinusy(x1,y1);
            [x2,y2]=reflectxMinusy(x2,y2);

            [x1,y1,x2,y2,isVisible]=leftBottomRegion(-yT,-yB,-xR,x1,y1,x2,y2,-rely2,-relx2,topProd);

            [x1,y1]=reflectxMinusy(x1,y1);
            [x2,y2]=reflectxMinusy(x2,y2);
        end
    end
end

function [x1,y1,x2,y2,isVisible]=leftBottomRegion(xL,xR,yB,x1,y1,x2,y2,relx2,rely2,leftProd)
%%LEFTBOTTOMREGION The case where (x1,y1) is in the left bottom corner.

if(y2>=yB)
    if(x2>xR)
        %The right-edge intersection.
        y2=y1+(xR-x1)*rely2/relx2;
        x2=xR;
    end

    %The left-edge intersection.
    y1=y1+leftProd/relx2;
    x1=xL;
    isVisible=true;
else
    bottomProd=(yB-y1)*relx2;

    %If (x2,y2) is below the (x1,y1) to (xR,yB) line.
    if(bottomProd>leftProd)
        isVisible=false;
    else
        if(x2>xR)
            rightProd=(xR-x1)*rely2;

            %If (x2,y2) is below the (x1,y1) to (xR,yB) line .
            if(bottomProd>rightProd)
                %The bottom-edge intersection.
                x2=x1+bottomProd/rely2;
                y2=yB;
            else
                %The right-edge intersection.
                y2=y1+rightProd/relx2;
                x2=xR;
            end
        else
            %The bottom-edge intersection.
            x2=x1+bottomProd/rely2;
            y2=yB;
        end

        %The left-edge intersection.
        y1=y1+leftProd/relx2;
        x1=xL;
        isVisible=true;
    end
end
end

function [x1,y1,x2,y2,isVisible]=leftEdge(xL,yT,xR,yB,x1,y1,x2,y2)
%%LEFTEDGE (x1,y1) is in the left edge.

    if(x2<xL)
        isVisible=false;
    elseif(y2<yB)
        [x1,y1,x2,y2,isVisible]=p2Bottom(xL,xR,yB,x1,y1,x2,y2);
    elseif(y2>yT)
        y1=reflectxAxis(y1);
        y2=reflectxAxis(y2);

        [x1,y1,x2,y2,isVisible]=p2Bottom(xL,xR,-yT,x1,y1,x2,y2);

        y1=reflectxAxis(y1);
        y2=reflectxAxis(y2);
    else
        relx2=x2-x1;
        rely2=y2-y1;

        if(x2>xR)%The right-edge intersection.
            y2=y1+(xR-x1)*rely2/relx2;
            x2=xR;
        end

        %The left-edge intersection.
        y1=y1+(xL-x1)*rely2/relx2;
        x1=xL;
        isVisible=true;
    end
end

function [x1,y1,x2,y2,isVisible]=centerColumn(xL,yT,xR,yB,x1,y1,x2,y2)
%CENTERCOLUMN The case where (x1,y1) is between the left and right
%             boundaries.

    if(y1>yT)
        [x1,y1]=rotate270c(x1,y1);
        [x2,y2]=rotate270c(x2,y2);
        [x1,y1,x2,y2,isVisible]=leftEdge(-yT,xR,-yB,xL,x1,y1,x2,y2);
        [x1,y1]=rotate90c(x1,y1);
        [x2,y2]=rotate90c(x2,y2);
    elseif(y1<yB)
        [x1,y1]=rotate90c(x1,y1);
        [x2,y2]=rotate90c(x2,y2);
        [x1,y1,x2,y2,isVisible]=leftEdge(yB,-xL,yT,-xR,x1,y1,x2,y2);
        [x1,y1]=rotate270c(x1,y1);
        [x2,y2]=rotate270c(x2,y2);
    else
        [x1,y1,x2,y2,isVisible]=inside(xL,yT,xR,yB,x1,y1,x2,y2);
    end
end

function [x1,y1,x2,y2,isVisible]=inside(xL,yT,xR,yB,x1,y1,x2,y2)
%%INSIDE The case where (x1,y1) is inside the rectangle.

if(x2<xL)
    [x1,y1,x2,y2]=p2Left(xL,yT,yB,x1,y1,x2,y2);
elseif(x2>xR)
	[x1,y1]=rotate180c(x1,y1);
	[x2,y2]=rotate180c(x2,y2);
    [x1,y1,x2,y2]=p2Left(-xR,-yB,-yT,x1,y1,x2,y2);
    [x1,y1]=rotate180c(x1,y1);
	[x2,y2]=rotate180c(x2,y2);
elseif(y2>yT)
    x2=x1+(x2-x1)*(yT-y1)/(y2-y1);
    y2=yT;
elseif(y2<yB)
    x2=x1+(x2-x1)*(yB-y1)/(y2-y1);
    y2=yB;
end

isVisible=true;
end

function [x1,y1,x2,y2]=p2Left(xL,yT,yB,x1,y1,x2,y2)
%%P2LEFT

    if(y2>yT)
        [x1,y1,x2,y2]=p2LeftTop(xL,yT,x1,y1,x2,y2);
    elseif(y2<yB)
        [x1,y1]=rotate90c(x1,y1);
        [x2,y2]=rotate90c(x2,y2);
        [x1,y1,x2,y2]=p2LeftTop(yB,-xL,x1,y1,x2,y2);
        [x1,y1]=rotate270c(x1,y1);
        [x2,y2]=rotate270c(x2,y2);
    else
        y2=y1+(y2-y1)*(xL-x1)/(x2-x1);
        x2=xL;
    end
end

function [x1,y1,x2,y2]=p2LeftTop(xL,yT,x1,y1,x2,y2)
%%P2LEFTTOP

    relx2=x2-x1;
    rely2=y2-y1;
    leftProduct=rely2*(xL-x1);
    topProduct=relx2*(yT-y1);

    if(topProduct>leftProduct)
        x2=x1+topProduct/rely2;
        y2=yT;
    else
        y2=y1+leftProduct/relx2;
        x2=xL;
    end
end

function [x1,y1,x2,y2,isVisible]=p2Bottom(xL,xR,yB,x1,y1,x2,y2)
%%P2BOTTOM (x1,y1) is in the left edge; (x2,y2) is not beyond the left
%          edge,  and (x2,y2) is beyond the bottom edge.

    relx2=x2-x1;
    rely2=y2-y1;
    leftProd=(xL-x1)*rely2;
    bottomProd=(yB-y1)*relx2;
    if(bottomProd>leftProd)
        isVisible=false;
    else
       if(x2<=xR)
           x2=x1+bottomProd/rely2;
           y2=yB;
       else
           rightProd=(xR-x1)*rely2;
           if(bottomProd>rightProd)
               x2=x1+bottomProd/rely2;
               y2=yB;
           else
               y2=y1+rightProd/relx2;
               x2=xR;
           end
       end
       y1=y1+leftProd/relx2;
       x1=xL;
       isVisible=true;
    end
end

function [x,y]=reflectxMinusy(x,y)
%%REFLECTXMINUSY This function is from Section VI-2 of [1].
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.

t=x;
x=-y;
y=-t;

end

function y=reflectxAxis(y)
%%REFLECTAXIS This function is from Section VI-2 of [1].
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.

y=-y;

end

function [x,y]=rotate90c(x,y)
%%ROTATE90C This function is from Section VI-2 of [1].
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.

t=x;
x=y;
y=-t;

end

function [x,y]=rotate180c(x,y)
%%ROTATE180C This function is from Section VI-2 of [1].
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.

x=-x;
y=-y;
end

function [x,y]=rotate270c(x,y)
%%ROTATE270C This function is from Section VI-2 of [1].
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.

t=x;
x=-y;
y=t;

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.