function D = driving_function_mono_localwfs_sbl(x0,xs,src,f,conf)
%DRIVING_FUNCTION_MONO_LOCALWFS_SBL driving signal for local WFS using spatial
%bandwidth limitation
%
%   Usage: D = driving_function_mono_localwfs_sbl(x0,xs,src,f,conf)
%
%   Input parameters:
%       x0          - position and direction of the secondary source / m [nx7]
%       xs          - position of point source or direction of plane
%                     wave / m [1x3]
%       src         - source type of the virtual source
%                         'pw' - plane wave
%                         'ps' - point source
%       f           - frequency of the monochromatic source / Hz
%       conf        - configuration struct (see SFS_config)
%
%   Output parameters:
%       D           - driving function [nx1]
%
%   See also: sound_field_mono, sound_field_mono_localwfs_sbl

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 5;
nargmax = 5;
narginchk(nargmin,nargmax);
isargsecondarysource(x0);
isargxs(xs);
isargchar(src);
isargpositivescalar(f);
isargstruct(conf);

%% ===== Computation ==========================================================
switch src
case 'ps'
    D = driving_function_mono_localwfs_sbl_ps(x0,xs,f,conf);
case 'pw'
    xs = xs./norm(xs);
    D = driving_function_mono_localwfs_sbl_pw(x0,xs,f,conf);
otherwise
    error('%s: %s is not a known source type.',upper(mfilename),src);
end


gammaa=[];
deltaa=[];
alphaa=[];
for lc=1:25
    gamma=4*pi*(rand-0.5)
    delta=4*pi*(rand-0.5)
    alpha=4*pi*(rand-0.5)
    an=ax_an_bounding(gamma,delta,alpha)
    gamma=an(2);
    delta=an(3);
    alpha=an(4);
    gammaa=[gammaa gamma];
    deltaa=[deltaa delta];
    alphaa=[alphaa alpha];
end

figure;
plot(gammaa,zeros(1,length(gammaa)),'.b');
title('gamma');

figure;
plot(deltaa,zeros(1,length(deltaa)),'.r');
title('delta');

figure;
plot(alphaa,zeros(1,length(alphaa)),'.r');
title('alpha');


function  [slcstack,  interfstack] = ImgRead_isce(InSAR_path,nline,reference_date,slcslist)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   This file is part of TomoSAR.
%
%   TomoSAR is distributed in the hope that it will be useful,
%   but without warranty of any kind; without even the implied
%   warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
%   See the Apache License for more details.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Author : Dinh Ho Tong Minh (INRAE) and Yen Nhi Ngo, Mar. 2022
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

N_int = size(slcslist,1);

for ii=1:N_int
    interfstack.datastack(:,:,ii)=single(freadbkj([InSAR_path,'/',num2str(slcslist(ii)),'/isce_minrefdem.int'],nline,'cpxfloat32'));
    interfstack.filename(ii,2)=slcslist(ii);

    tempstack(:,:,ii)=single(freadbkj([InSAR_path,'/',num2str(slcslist(ii)),'/secondary.slc'],nline,'cpxfloat32'));

    fprintf('Reading slc/interferogram %d / %d \n',ii,N_int);
end
interfstack.filename(:,1) = reference_date;

% insert reference_date in to slc list
slcslist = [reference_date; slcslist];
slcslist_datenum = sort(datenum(num2str(slcslist),'yyyymmdd'));

slcslist = datestr(slcslist_datenum,'yyyymmdd');
N_slc = size(slcslist,1);

[~,reference_ind]=ismember(datenum(num2str(reference_date),'yyyymmdd'),slcslist_datenum);
if reference_ind > 1
    slcstack.datastack(:,:,[1:reference_ind-1,reference_ind+1:N_slc]) = tempstack;
    slcstack.datastack(:,:,reference_ind) = single(freadbkj([InSAR_path,'/',num2str(slcslist(1,:)),'/reference.slc'],nline,'cpxfloat32'));
else
    slcstack.datastack(:,:,1) = single(freadbkj([InSAR_path,'/',num2str(slcslist(2,:)),'/reference.slc'],nline,'cpxfloat32'));
    slcstack.datastack(:,:,[2:N_slc]) = tempstack;
end

for ii = 1:N_slc
    slcstack.filename(ii,1) = str2double(slcslist(ii,:));
end


function [result_features]=back_find_low_node_Nopara(result_dir,nSubj,k_times,nROI,w,cross_val,meth_FEX,meth_FS,varargin)
% This function aims to find features used in the classification, suitable
% for the PC, aHOFC, tHOFC brain network construction method. And it is also
% suitable for the LOOCV or the 10-fold cross validation. The found
% features can be applied on the visualization software to present the
% important brain regions or links.
% Input:
%         result_dir: the directory you want to store all the result files;
%         nSubj: number of subjects;
%         k_times: times of 10-fold cross validation;
%         nROI: number of ROIs
%         w: weight of each selected features;
%         cross_val: loocv or 10-fold;
%         meth_FEX: feature extraction method(connection coefficients or local clustering coefficients);
%         meth_FS: feature selection method (ttest,lasso,ttest+lasso);
%         varargin: the feature selection indexes;
%
% Output:
%        result_features: cell array, consisting of the averaged weight and occurrence of the features;
% Written by Zhen Zhou, zzstefan@email.unc.edu
% IDEA lab, https://www.med.unc.edu/bric/ideagroup
% Department of Radiology and BRIC, University of North Carolina at Chapel Hill

%clear all
% load SR_10_fold_middle.mat;
% load SR_loocv_middle.mat;
% cross_val='loocv';
% ttest_p=feature_index_ttest;
% midw_lasso=feature_index_lasso;
%load tHOFC_loocv_middle_clus.mat;
%load tHOFC_coef_LASSO_kfold.mat;
% load tHOFC_clus_LASSO_kfold.mat;
%load PC_loocv_coef_middle.mat;
% cross_val='10-fold';
%ttest_p=feature_index_ttest;
% midw_lasso=feature_index_lasso;
fprintf('Begin contributing feature identification\n');

switch meth_FS
    case 't-test + LASSO'
        ttest_p=varargin{1};
        midw_lasso=varargin{2};
        if strcmpi(cross_val,'10-fold')
            for i=1:size(ttest_p,1)
                for j=1:size(ttest_p,2)
                    tmp=find(ttest_p{i,j}<0.05);
                    tmp_b{i,j}=find(midw_lasso{i,j});
                    index=tmp(tmp_b{i,j});
                    if strcmpi(meth_FEX,'coef')
                        for k=1:length(index)
                            [first{i,j,k}(1),first{i,j,k}(2)]=find_elements(nROI,index(k));
                        end
                    elseif strcmpi(meth_FEX,'clus')
                        for k=1:length(index)
                            first{i,j,k}=index(k);
                        end
                    end
                end
            end
        elseif strcmpi(cross_val,'loocv')
            for i=1:length(ttest_p)
                tmp=find(ttest_p{i}<0.05);
                tmp_b{i}=find(midw_lasso{i});
                index=tmp(tmp_b{i});
                if strcmpi(meth_FEX,'coef')
                    for k=1:length(index)
                        [first{i,k}(1),first{i,k}(2)]=find_elements(nROI,index(k));
                    end
                elseif strcmpi(meth_FEX,'clus')
                    for k=1:length(index)
                        first{i,k}=index(k);
                    end
                end
            end
        end
    case 't-test'
        ttest_p=varargin{1};
        if strcmpi(cross_val,'10-fold')
            for i=1:size(ttest_p,1)
                for j=1:size(ttest_p,2)
                    tmp=find(ttest_p{i,j}<0.05);
                    if strcmpi(meth_FEX,'coef')
                        for k=1:length(tmp)
                            [first{i,j,k}(1),first{i,j,k}(2)]=find_elements(nROI,tmp(k));
                        end
                    elseif strcmpi(meth_FEX,'clus')
                        for k=1:length(tmp)
                            first{i,j,k}=tmp(k);
                        end
                    end
                end
            end
        elseif strcmpi(cross_val,'loocv')
            for i=1:length(ttest_p)
                tmp=find(ttest_p{i}<0.05);
                if strcmpi(meth_FEX,'coef')
                    for k=1:length(tmp)
                        [first{i,k}(1),first{i,k}(2)]=find_elements(nROI,tmp(k));
                    end
                elseif strcmpi(meth_FEX,'clus')
                    for k=1:length(tmp)
                        first{i,k}=tmp(k);
                    end
                end
            end
        end
    case 'LASSO'
        midw_lasso=varargin{1};
        if strcmpi(cross_val,'10-fold')
            for i=1:size(midw_lasso,1)
                for j=1:size(midw_lasso,2)
                    tmp=find(midw_lasso{i,j});
                    if strcmpi(meth_FEX,'coef')
                        for k=1:length(tmp)
                            [first{i,j,k}(1),first{i,j,k}(2)]=find_elements(nROI,tmp(k));
                        end
                    elseif strcmpi(meth_FEX,'clus')
                        for k=1:length(tmp)
                            first{i,j,k}=tmp(k);
                        end
                    end
                end
            end
        elseif strcmpi(cross_val,'loocv')
            for i=1:length(midw_lasso)
                tmp=find(midw_lasso{i});
                if strcmpi(meth_FEX,'coef')
                    for k=1:length(tmp)
                        [first{i,k}(1),first{i,k}(2)]=find_elements(nROI,tmp(k));
                    end
                elseif strcmpi(meth_FEX,'clus')
                    for k=1:length(tmp)
                        first{i,k}=tmp(k);
                    end
                end
            end
        end
end

%first=cellfun(@(x) x',first,'UniformOutput',false);
temp=first(:);

[a1,b,c]=unique(cellfun(@char,temp,'un',0));
lo=histc(c,1:max(c));
loo=lo(:)>1;
out=[temp(b(loo)),num2cell(lo(loo))];
out(1,:)=[];

All_link=out(find(cell2mat(out(:,2))>0),1);%find all links which exists in any one  LOOCV fold
if strcmpi(cross_val,'10-fold')
    for j=1:length(All_link)
        weight=[];
        temp_index=cell2mat(cellfun(@(x)isequal(x,All_link{j}),first,'un',0));
        ind=find(temp_index);
        [A,B,C]=ind2sub(size(temp_index),ind);
        for k=1:length(A)
            weight=[weight,w{A(k),B(k)}(C(k))];
        end
        all_weight(j)=mean(weight);
    end
elseif strcmpi(cross_val,'loocv')
    for j=1:length(All_link)
        weight=[];
        temp_index=cell2mat(cellfun(@(x)isequal(x,All_link{j}),first,'un',0));
        [A,B]=find(temp_index);
        for k=1:length(A)
            weight=[weight,w{A(k)}(B(k))];
        end
        all_weight(j)=mean(weight);
    end
end

if strcmpi(meth_FEX,'coef')
    matrix_1=zeros(nROI,nROI);
    matrix_2=zeros(nROI,nROI);
    for m=1:size(out,1)
        matrix_1(out{m,1}(1),out{m,1}(2))=all_weight(m);
        matrix_1(out{m,1}(2),out{m,1}(1))=all_weight(m);
        matrix_2(out{m,1}(1),out{m,1}(2))=out{m,2};
        matrix_2(out{m,1}(2),out{m,1}(1))=out{m,2};
    end
    result_features{1}=matrix_1;
    %result_features{2}=matrix_2;

elseif strcmpi(meth_FEX,'clus')
    matrix_1=zeros(1,nROI);
    matrix_2=zeros(1,nROI);
    for m=1:size(out,1)
        matrix_1(out{m,1})=all_weight(m);
        matrix_2(out{m,1})=out{m,2};
    end
    result_features{1}=matrix_1;
    %result_features{2}=matrix_2;
end
if strcmpi(cross_val,'loocv')
    matrix_2=matrix_2/nSubj;
elseif strcmpi(cross_val,'10-fold')
    matrix_2=matrix_2/(10*k_times);
end
result_features{2}=matrix_2;

if strcmpi(meth_FEX,'coef')
    figure('visible','off');
    subplot(1,2,1);
    imagesc(matrix_1);
    colormap jet
    colorbar
    axis square
    xlabel('ROI');
    ylabel('ROI');
    title('Averaged weight');

    subplot(1,2,2);
    imagesc(matrix_2);
    colormap jet
    colorbar
    axis square
    xlabel('ROI');
    ylabel('ROI');
    title('Normalized occurence');
    print(gcf,'-r1000','-dtiff',char(strcat(result_dir,'/result_features_weigthAndOccurence.tiff')));
end

fprintf('End contributing feature identification\n');
% node_matrix=eye(116,116);
% for i=1:length(NEW)
%     for j=1:length(NEW{i,3})
%         node_matrix(NEW{i,3}(j,1),NEW{i,3}(j,2))=1;
%         node_matrix(NEW{i,3}(j,2),NEW{i,3}(j,1))=1;
%     end
% end


function lambda = idem_random_eigenvalues ( n, rank, key )

%*****************************************************************************80
%
%% IDEM_RANDOM_EIGENVALUES returns the eigenvalues of the IDEM_RANDOM matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, integer RANK, the rank of A.
%
%    Input, integer KEY, a positive value that selects the data.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  lambda(1:rank,1) = 1.0;
  lambda(rank+1:n,1) = 0.0;

  return
end


% -------------------------------------------------------------------------- %
% hfssCoaxialCable(fid, Names, Axis, Center, Radii, Height, Units)
%
% Description :
% -------------
% This function creates the VBScript necessary to draw a Coaxial Cable in
% HFSS. The "Coaxial Cable" can have as many cylinders as you specify. This
% function only creates the geometric structure and does not set any material
% properties.
%
% Parameters :
% ------------
% fid - file identifier of the HFSS VBScript File.
% Name - a cell of strings that contains the names of each cylinder that is a
%        part of the co-axial cable (see example).
% Axis - axis of the Coaxial Cable (choose between 'X', 'Y' or 'Z').
% Center - Coordinates of the Center of the Coaxial Cable ([x, y, z]).
% Radii - an array of Radii of the Cylinders present in the Coaxial cable.
%         each radius corresponds to the respective name specfied in the
%         'Name' parameter.
% Height - length of the coaxial cable.
% Units - can be either 'in' or 'mm' or 'meter' or anything defined in HFSS.
%
% Example:
% ---------
% ...
% hfssCoaxialCable(fid, {'Cyl1_In', 'Cyl1_Er', 'Cyl1_Out'}, 'Z', [0, 0, 0],
%                  0.02, 0.03, 0.04], 10, 'in')
% ...
% -------------------------------------------------------------------------- %

% ----------------------------------------------------------------------------
% This file is part of HFSS-MATLAB-API.
%
% HFSS-MATLAB-API is free software; you can redistribute it and/or modify it
% under the terms of the GNU General Public License as published by the Free
% Software Foundation; either version 2 of the License, or (at your option)
% any later version.
%
% HFSS-MATLAB-API 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
% Foobar; if not, write to the Free Software Foundation, Inc., 59 Temple
% Place, Suite 330, Boston, MA  02111-1307  USA
%
% Copyright 2004, Vijay Ramasami (rvc@ku.edu)
% ----------------------------------------------------------------------------
function hfssCoaxialCable(fid, Names, Axis, Center, Radii, Height, Units)

% Sort the Radii first in Ascending order.
[Radii, iR] = sort(Radii);
Names = cellstr(char(Names{iR}));

% Get the # of Cylinders.
nCylinders = length(Radii);

% First create the N-1 hollow cylinders.
for iR = nCylinders:-1:2
	hfssHollowCylinder(fid, Names{iR}, Axis, Center, Radii(iR-1), ...
	                   Radii(iR), Height, Units);
end

% Finally create the inner cylinder.
hfssCylinder(fid, Names{1}, Axis, Center, Radii(1), Height, Units);


function [x,testdata]=alstpz_solve(A,y,tol,varargin)
%Solution of linear systems in TT-format via ALS(t+z) iteration
%!Note! This method is provided mostly for test purposes.
%!Note! You may prefer to use amen_solve2 instead.
%   [X,somedata]=ALSTPZ_SOLVE(A,Y,TOL,OPTIONS) Attempts to solve the linear
%   system A*X = Y with accuracy/residual TOL using the global (t+z)-enriched ALS.
%   Matrix A has to be given in the TT-format, right-hand side Y should be
%   given in the TT-format also. Options are provided in form
%   'PropertyName1',PropertyValue1,'PropertyName2',PropertyValue2 and so
%   on. The parameters are set to default (in brackets in the following)
%   The list of option names and default values are:
%       o x0 - initial approximation [random rank-2 tensor]
%       o nswp - maximal number of sweeps [10]
%       o max_full_size - maximal size of the local matrix to full solver [50]
%       o local_prec - local preconditioner: '' (no prec.), 'ljacobi',
%         'cjacobi', 'rjacobi' ['']
%       o local_iters - number of local gmres restarts [2]
%       o local_restart - dimension of local gmres [40]
%       o kickrank - compression rank of the residual Z, i.e. expansion
%         size [4]
%       o kicktype - how to approximate the residual: via the SVD rounding ('svd'),
%         or using one approximation ALS sweep ('als'). ['svd']
%       o ismex - shall we use the MEX lib solve3d_2 for local solution [true]
%       o resid_damp - solve local problems with accuracy tol/resid_damp.
%         Larger value may reduce a spurious noise from inexact local
%         solutions, but increase CPU time [2]
%       o symm - shall we consider the symmetrized system (A'A)x=(A'y). [false]
%
%       Example:
%           d=8; f=8;
%           mat=tt_qlaplace_dd(d*ones(1,f)); % Laplace in the QTT-format
%           rhs=tt_ones(2,d*f); % Right-hand side of all ones
%           sol = alstpz_solve(mat, rhs, 1e-5); % solve the Poisson eqn.
%
%********
%   References:
%   S. Dolgov, D. Savostyanov.
%   http://arxiv.org/abs/1301.6068
%   http://arxiv.org/abs/1304.1222
%
%   Please send feedback to: {sergey.v.dolgov,dmitry.savostyanov}@gmail.com
%
%********
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
% Inner parameters
max_full_size=50;

resid_damp = 2; % Truncation error to true residual treshold

nswp=10;
local_restart=40;
local_iters=2;

local_prec = '';

ismex = true;

% kicktype = 'als';
kicktype = 'svd';

kickrank = 4;
x=[];
symm = false;
verb = 3;

for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'nswp'
            nswp=varargin{i+1};
        case 'x0'
            x=varargin{i+1};
        case 'local_prec'
            local_prec=varargin{i+1};
        case 'local_restart'
            local_restart=varargin{i+1};
        case 'ismex'
            ismex=varargin{i+1};
        case 'local_iters'
            local_iters=varargin{i+1};
        case 'kickrank'
            kickrank=varargin{i+1};
        case 'kicktype'
            kicktype=varargin{i+1};
        case  'max_full_size'
            max_full_size=varargin{i+1};
        case 'resid_damp'
            resid_damp = varargin{i+1};
        case 'symm'
            symm=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};

        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

% Disable MEX if it does not exist
if (ismex)&&(exist('solve3d_2', 'file')<2)
    warning('MEX local solver is not found, disabled');
    ismex = false;
end;

% Extract sizes and TT blocks
d = y.d;
n = A.n;
if (isempty(x))
    x = tt_rand(n, A.d, 2);
end;

if (symm)
    % Symmetrize the system if necessary
    A2 = A'*A;
    Ay = A'*y;
end;

if (strcmp(kicktype, 'als'))&&(kickrank>0)
    z = tt_rand(n, d, kickrank);
    rz = z.r;
    phi = cell(d+1,1); phi{1}=1; phi{d+1}=1;
end;

% Test output data -- e.g. for convergence tracking
testdata = cell(3,1);
testdata{2} = cell(d, nswp);
testdata{1} = zeros(d, nswp);
testdata{3} = zeros(1, nswp);
t_corr_amen = tic;

% ALS(t+z) sweeps
for swp=1:nswp
    % Truncate the solution first
    x = round(x,tol);
    if (strcmp(kicktype, 'als'))&&(kickrank>0)
        % Update the residual approximation via one ALS sweep for |z-ZZ|->min
        % Exact residual
        if (symm)
            ZZ = Ay - A2*x;
        else
            ZZ = y - A*x;
        end;
        rZZ = ZZ.r;
        ZZ = core2cell(ZZ);
        z = core2cell(z); % Approximate one
        for i=1:d-1
            % Go backward and prepare projections
            cr = reshape(z{i}, rz(i)*n(i), rz(i+1));
            [cr,rv]=qr(cr,0);
            cr2 = reshape(z{i+1}, rz(i+1), n(i+1)*rz(i+2));
            cr2 = rv*cr2;
            rz(i+1) = size(cr, 2);
            z{i} = reshape(cr, rz(i), n(i), rz(i+1));
            z{i+1} = reshape(cr2, rz(i+1), n(i+1), rz(i+2));

            phi{i+1} = compute_next_Phi(phi{i}, z{i}, [], ZZ{i}, 'lr');
        end;
        for i=d:-1:1
            % Go forward and project exact ZZ
            cr = reshape(ZZ{i}, rZZ(i)*n(i), rZZ(i+1));
            cr = cr*phi{i+1};
            cr = reshape(cr, rZZ(i), n(i)*rz(i+1));
            cr = phi{i}*cr;
            if (i>1)
                [cr,rv]=qr(cr.', 0);
                cr2 = reshape(z{i-1}, rz(i-1)*n(i-1), rz(i));
                cr2 = cr2*rv.';
                rz(i) = size(cr, 2);
                z{i} = reshape(cr.', rz(i), n(i), rz(i+1));
                z{i-1} = reshape(cr2, rz(i-1), n(i-1), rz(i));

                phi{i} = compute_next_Phi(phi{i+1}, z{i}, [], ZZ{i}, 'rl');
            else
                z{i} = reshape(cr, rz(i), n(i), rz(i+1));
            end;
        end;
        z = cell2core(tt_tensor, z);
    else
        % SVD rounding of the exact residual up to kickrank
        if (kickrank>0)
            if (symm)
                z = round(Ay - A2*x, tol, kickrank);
            else
                z = round(y - A*x, tol, kickrank);
            end;
        end;
    end;

    % Augment the solution with Z basis
    if (kickrank>0)
        x = x+0*z;
    end;
    t_old = toc(t_corr_amen);
    % Update via the ALS iteration
    if (symm)
        [x,td_als] = als_solve(A2, Ay, tol,  'max_full_size', max_full_size, 'x0', x, 'verb', verb, 'local_prec', local_prec, 'local_iters', local_iters, 'local_restart', local_restart, 'resid_damp', resid_damp, 'ismex', ismex);
    else
        [x,td_als] = als_solve(A, y, tol,  'max_full_size', max_full_size, 'x0', x, 'verb', verb,  'local_prec', local_prec, 'local_iters', local_iters, 'local_restart', local_restart, 'resid_damp', resid_damp, 'ismex', ismex);
    end;
    testdata{1}(:, swp) = t_old+td_als{1}(:,1);
    testdata{2}(:, swp) = td_als{2};
    testdata{3}(swp) = max(td_als{3});
end;

end

function [x,td]=als_solve(A, y, tol, varargin)
% One sweep of the one-site DMRG solution scheme.
% Uses the same parameters as in the main function

local_prec_char = 0;
verb = 1;
ismex = true;

x=[];

for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'x0'
            x=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};
        case 'local_prec'
            local_prec=varargin{i+1};
        case 'local_restart'
            local_restart=varargin{i+1};
        case 'local_iters'
            local_iters=varargin{i+1};
        case 'ismex'
            ismex=varargin{i+1};
        case  'max_full_size'
            max_full_size=varargin{i+1};
        case 'resid_damp'
            resid_damp = varargin{i+1};

        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

if (strcmp(local_prec, 'cjacobi')); local_prec_char = 1;  end;
if (strcmp(local_prec, 'ljacobi')); local_prec_char = 2;  end;
if (strcmp(local_prec, 'rjacobi')); local_prec_char = 3;  end;

% Extract sizes and TT cores
d = y.d;
n = A.n;
if (isempty(x))
    x = tt_rand(n, A.d, 2);
end;

ry = y.r;
ra = A.r;
rx = x.r;

cry = core2cell(y);
crA = core2cell(A);
crx = core2cell(x);

% Interfaces
phia = cell(d+1,1); phia{1}=1; phia{d+1}=1;
phiy = cell(d+1,1); phiy{1}=1; phiy{d+1}=1;

% This is some convergence output for test purposes
td = cell(3,1);
td{1} = zeros(d, 1);
td{2} = cell(d, 1);
td{3} = zeros(d, 1);

t_amr_solve = tic;

% Orthogonalization
for i=d:-1:2
    cr = crx{i};
    cr = reshape(cr, rx(i), n(i)*rx(i+1));
    [cr, rv]=qr(cr.', 0);
    cr2 = crx{i-1};
    cr2 = reshape(cr2, rx(i-1)*n(i-1), rx(i));
    cr2 = cr2*(rv.');
    rx(i) = size(cr, 2);
    cr = reshape(cr.', rx(i), n(i), rx(i+1));
    crx{i-1} = reshape(cr2, rx(i-1), n(i-1), rx(i));
    crx{i} = cr;

    phia{i} = compute_next_Phi(phia{i+1}, cr, crA{i}, cr, 'rl');
    phiy{i} = compute_next_Phi(phiy{i+1}, cr, [], cry{i}, 'rl');
end;

max_res = 0;
max_dx = 0;

% DMRG sweep
for i=1:d
    % Extract elements - matrix
    Phi1 = phia{i}; Phi2 = phia{i+1};
    % Phi1: rx'1, rx1, ra1, or rx'1, ry1
    % Phi2: rx2, ra2, rx'2, or ry'2, rx2
    A1 = crA{i}; y1 = cry{i};
    % RHS - rewrite it in accordance with new index ordering
    rhs = phiy{i}; % rx'1, ry1
    y1 = reshape(y1, ry(i), n(i)*ry(i+1));
    rhs = rhs*y1;
    rhs = reshape(rhs, rx(i)*n(i), ry(i+1));
    rhs = rhs*phiy{i+1};
    rhs = reshape(rhs, rx(i)*n(i)*rx(i+1),1);
    norm_rhs = norm(rhs);
    % sol_prev
    sol_prev = reshape(crx{i}, rx(i)*n(i)*rx(i+1), 1);

    real_tol = (tol/sqrt(d))/resid_damp;

    if (rx(i)*n(i)*rx(i+1)<max_full_size) % Full solution
        %      |     |    |
        % B = Phi1 - A1 - Phi2
        %      |     |    |
        B = reshape(Phi1, rx(i)*rx(i), ra(i));
        B = B*reshape(A1, ra(i), n(i)*n(i)*ra(i+1));
        B = reshape(B, rx(i), rx(i), n(i), n(i)*ra(i+1));
        B = permute(B, [1, 3, 2, 4]);
        B = reshape(B, rx(i)*n(i)*rx(i)*n(i), ra(i+1));
        B = B*reshape(permute(Phi2, [2, 3, 1]), ra(i+1), rx(i+1)*rx(i+1));
        B = reshape(B, rx(i)*n(i), rx(i)*n(i), rx(i+1), rx(i+1));
        B = permute(B, [1, 3, 2, 4]);
        B = reshape(B, rx(i)*n(i)*rx(i+1), rx(i)*n(i)*rx(i+1));

        res_prev = norm(B*sol_prev - rhs)/norm_rhs;

        if (res_prev>real_tol)
            sol = B \ rhs;
            res_new = norm(B*sol-rhs)/norm_rhs;
        else
            sol = sol_prev;
            res_new = res_prev;
        end;

    else % Structured solution.

        res_prev = norm(bfun3(Phi1, A1, Phi2, sol_prev) - rhs)/norm_rhs;

        if (res_prev>real_tol)
            if (~ismex)
                sol = solve3d_2ml(Phi1, A1, Phi2, rhs, real_tol, sol_prev, local_prec_char, local_restart, local_iters);
            else % use MEX
                sol = solve3d_2(Phi1, A1, Phi2, rhs, real_tol, 1, sol_prev, local_prec_char, local_restart, local_iters, 0);
            end;

            res_new = norm(bfun3(Phi1, A1, Phi2, sol) - rhs)/norm_rhs;
        else
            sol = sol_prev;
            res_new = res_prev;
        end;

    end;

    if (res_prev/res_new<resid_damp)&&(res_new>real_tol)
        fprintf('--warn-- the residual damp was smaller than in the truncation\n');
    end;

    dx = norm(sol-sol_prev)/norm(sol);
    max_dx = max(max_dx, dx);
    max_res = max(max_res, res_prev);

    td{3}(i) = dx;

    % QR
    sol = reshape(sol, rx(i)*n(i), rx(i+1));
    [u,v]=qr(sol, 0);
    r = size(u,2);

    if (verb==2)
        fprintf('=als_solve=   block %d, dx: %3.3e, res: %3.3e, r: %d\n', i, dx, res_prev, r);
    end;

    if (i<d) % left-to-right, kickrank, etc
        cr2 = crx{i+1};
        cr2 = reshape(cr2, rx(i+1), n(i+1)*rx(i+2));
        v = v*cr2; % size r+radd, n2, r3

        u = reshape(u, rx(i), n(i), r);
        v = reshape(v, r, n(i+1), rx(i+2));

        % Recompute phi. Left ones, so permute them appropriately
        phia{i+1} = compute_next_Phi(phia{i}, u, crA{i}, u, 'lr');
        phiy{i+1} = compute_next_Phi(phiy{i}, u, [], cry{i}, 'lr');

        % Stuff back
        rx(i+1) = r;
        crx{i} = u;
        crx{i+1} = v;
    else
        % Just stuff back the last core
        sol = reshape(sol, rx(i), n(i), rx(i+1));
        crx{i} = sol;
    end;

    if (verb>2)
        td{1}(i) = toc(t_amr_solve);
        if (verb>3)||(i==d) % each microstep is returned only if really asked for
            % Otherwise the memory will blow up
            x = cell2core(x, crx); % for test
            td{2}{i} = x;
        end;
    end;

end;

if (verb>0)
     fprintf('=als_solve= max_dx: %3.3e, max_res: %3.3e, erank: %g\n', max_dx, max_res, sqrt(rx(1:d)'*(n.*rx(2:d+1))/sum(n)));
end;

x = cell2core(x, crx);

end

function [Phi] = compute_next_Phi(Phi_prev, x, A, y, direction)
% Performs the recurrent Phi (or Psi) matrix computation
% Phi = Phi_prev * (x'Ay).
% If direction is 'lr', computes Psi
% if direction is 'rl', computes Phi
% A can be empty, then only x'y is computed.

% Phi1: rx1, ry1, ra1, or rx1, ry1
% Phi2: ry2, ra2, rx2, or ry2, rx2

rx1 = size(x,1); n = size(x,2); rx2 = size(x,3);
ry1 = size(y,1); m = size(y,2); ry2 = size(y,3);
if (~isempty(A))
    ra1 = size(A,1); ra2 = size(A,4);
else
    ra1 = 1; ra2 = 1;
end;

if (strcmp(direction, 'lr'))
    %lr: Phi1
    x = reshape(x, rx1, n*rx2);
    Phi = reshape(Phi_prev, rx1, ry1*ra1);
    Phi = x'*Phi;
    if (~isempty(A))
        Phi = reshape(Phi, n*rx2*ry1, ra1);
        Phi = Phi.';
        Phi = reshape(Phi, ra1*n, rx2*ry1);
        A = reshape(A, ra1*n, m*ra2);
        Phi = A.'*Phi;
        Phi = reshape(Phi, m, ra2*rx2*ry1);
    else
        Phi = reshape(Phi, n, rx2*ry1);
    end;
    Phi = Phi.';
    Phi = reshape(Phi, ra2*rx2, ry1*m);

    y = reshape(y, ry1*m, ry2);
    Phi = Phi*y;
    if (~isempty(A))
        Phi = reshape(Phi, ra2, rx2*ry2);
        Phi = Phi.';
    end;
    Phi = reshape(Phi, rx2, ry2, ra2);
else
    %rl: Phi2
    y = reshape(y, ry1*m, ry2);
    Phi = reshape(Phi_prev, ry2, ra2*rx2);
    Phi = y*Phi;
    if (~isempty(A))
        Phi = reshape(Phi, ry1, m*ra2*rx2);
        Phi = Phi.';
        Phi = reshape(Phi, m*ra2, rx2*ry1);
        A = reshape(A, ra1*n, m*ra2);
        Phi = A*Phi;
        Phi = reshape(Phi, ra1*n*rx2, ry1);
        Phi = Phi.';
    end;

    Phi = reshape(Phi, ry1*ra1, n*rx2);
    x = reshape(x, rx1, n*rx2);
    Phi = Phi*x';
    if (~isempty(A))
        Phi = reshape(Phi, ry1, ra1, rx1);
    else
        Phi = reshape(Phi, ry1, rx1);
    end;
end;

end

% new
function [y]=bfun3(Phi1, A, Phi2, x)
% Phi1: ry1, rx1, ra1
ry1 = size(Phi1,1);
rx1 = size(Phi1,2);
ra1 = size(Phi1,3);
% Phi2: rx2, ra2, ry2
ry2 = size(Phi2,3);
rx2 = size(Phi2,1);
ra2 = size(Phi2,2);

n = size(A,2);
m = size(A,3);

y = reshape(x, rx1*m, rx2);
Phi2 = reshape(Phi2, rx2, ra2*ry2);
y = y*Phi2;
y = reshape(y, rx1, m*ra2*ry2);
y = y.';
y = reshape(y, m*ra2, ry2*rx1);
A = reshape(A, ra1*n, m*ra2);
y = A*y;
y = reshape(y, ra1*n*ry2, rx1);
y = y.';
y = reshape(y, rx1*ra1, n*ry2);
Phi1 = reshape(Phi1, ry1, rx1*ra1);
y = Phi1*y;
y = reshape(y, ry1*n*ry2, 1);
end


function grid = getGrid(~, N, dom)
%GETGRID   Returns a grid correspoding to a SPINOP3 object.
%
% See also SPINOP3.

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

xx = trigpts(N, dom(1:2));
yy = trigpts(N, dom(3:4));
zz = trigpts(N, dom(5:6));
[xx, yy, zz] = meshgrid(xx, yy, zz);
grid = {xx; yy; zz};

end


function test_ChallengeDB_SVR(testDatabase, testMethod, Trainingproportion)

    %% set up database
    if ~exist(fullfile('result', testDatabase, testMethod), 'dir')
        mkdir(fullfile('result', testDatabase, testMethod));
    end

    fprintf('processing LIVE ChallengeDB images.....');
    Path = fullfile('databases', testDatabase);
    imageList = load(fullfile(Path,'Data','AllImages_release.mat'));
    mos = load(fullfile(Path,'Data','AllMOS_release.mat'));
    imageList = fullfile(Path,'Images',imageList.AllImages_release(8:end));
    mos = mos.AllMOS_release(8:end);

    %% feature extraction
    if ~exist(fullfile('result', testDatabase, testMethod, 'features.mat'), 'file')
        for i = 1:length(mos)
            fprintf('processing image %d / %d \n',i,length(mos));
            features{i} = CallingTheSourceCodeForFeatureExtraction(imageList{i},testMethod);
        end
        features = cat(2,features{:});
        save(fullfile('result', testDatabase, testMethod, 'features.mat'),'features');
    else
        load(fullfile('result', testDatabase, testMethod, 'features.mat'),'features');
    end

    %% train SVR and test
    tic;
    trainingNum = ceil(length(mos)*Trainingproportion);
    for seed = 1:10
        rng(seed-1);
        randomsplit = randperm(length(mos),length(mos));
        trainingSet = randomsplit(1:trainingNum);
        testingSet = randomsplit(trainingNum+1:end);
        trainX = features(:,trainingSet)';
        testX = features(:,testingSet)';
        [trainX, testX] = featNormalize(trainX, testX);
        switch testMethod
            case 'BRISQUE'
                svm_model = svmtrain(mos(trainingSet)', double(trainX), '-s 4 -t 2 -c 10000 -g 0.05');
            case 'CORNIA'
                svm_model = svmtrain(mos(trainingSet)', double(trainX), '-s 4 -t 0');
            case 'HOSA'
                svm_model = train(mos(trainingSet)', sparse(trainX), '-s 11 -c 128');
            case 'DIIVINE'
                svm_model = svmtrain(mos(trainingSet)', double(trainX), '-s 4 -t 2 -c 4096 -g 0.02');
            case 'FRIQUEE'
                svm_model = svmtrain(mos(trainingSet)', double(trainX), '-s 4 -t 2 -c 256 -g 0.02');
            otherwise
                error('Unsupported Method!');
        end
        if strcmp(testMethod,'HOSA')
            [scores, ~,~] = predict(mos(testingSet)', sparse(testX), svm_model,'-q');
        else
            [scores, ~,~] = svmpredict(mos(testingSet)', double(testX), svm_model,'-q');
        end

        SRCC(seed) = corr(scores, mos(testingSet)', 'type', 'Spearman');
        PLCC(seed) = RegressionLIVE(scores, mos(testingSet)');
    end
    toc;

    fprintf('SRCC = %.4f, std = %.4f;  PLCC = %.4f, std = %.4f \n', abs(median(SRCC)), std(SRCC), abs(median((PLCC))), std((PLCC)));

end


function [x y pos trials map mapping] = getBehavEvents_linearTrack(pos,bins)
dbstop if error
scatter(pos(:,8),pos(:,10),'.')
% axis([-1 1 -1 1])
[x y]=ginput();
cx =  x(end-1:end);
y = y(end-1:end);
dist(:,1)=fastrms(abs(pos(:,8)-x(1))+abs(pos(:,10)-y(1)),120);
dist(:,2)=fastrms(abs(pos(:,8)-x(2))+abs(pos(:,10)-y(2)),120);

plot(dist,'.')
% axis([0 length(dist) 0 3])
[xx yy] = ginput();

[pks_one locs_one]=findpeaks(-dist(:,1),'MINPEAKHEIGHT',-yy);
[pks_two locs_two]=findpeaks(-dist(:,2),'MINPEAKHEIGHT',-yy);
c=1;
lastTrial = 0;
for i=1:length(locs_one)
    f = locs_one(i)-locs_two;
    ff = find(f>0);
    [a b]=min(locs_one(i)-locs_two(ff));
    if a < 500 & a > 60 & locs_two(ff(b)) > lastTrial + 60
       trials{c} = pos(locs_two(ff(b)):locs_one(i),:);
       c=1+c;
       lastTrial = locs_two(ff(b));
    end
    f = locs_two-locs_one(i);
    ff = find(f>0);
    [a b]=min(locs_two(ff)-locs_one(i));
    if a < 500 & a > 60 & locs_one(i) > lastTrial + 60
       trials{c} = pos(locs_one(i):locs_two(ff(b)),:);
       c=1+c;
       lastTrial = locs_one(i);
    end
end

% for i=1:length(locs_one)
%     two = locs_two(locs_two-locs_one(i)>0);
%     if ~isempty(two)
%     [a b]=min(abs(two-locs_one(i)));
%     if two(b)-locs_one(i) < 24000 && two(b)-locs_one(i) > 120 && locs_one(i) > lastTrial-60
%         trials{c} = pos(locs_one(i):two(b),:);
%         c=1+c;
%         lastTrial = two(b);
%     end
%     end
%     two = locs_two(locs_one(i)-locs_two>0);
%     if ~isempty(two)
%     [a b]=min(abs(two-locs_one(i)));
%     if locs_one(i)-two(b) < 24000 && locs_one(i)-two(b) > 120 && two(b) > lastTrial-60
%         trials{c} = pos(two(b):locs_one(i),:);
%         c=1+c;
%         lastTrial = locs_one(i);
%     end
%     end
% end

%% fine tune trials here
% for i=1:length(trials)
% clear t tt ttt tttt; t = trials{i};
% tt=t(:,8:10);
% d = diff(trials{i}(:,8:10));
% tt(find(d==0)+1)=nan;
% for j=1:3
% tttt(:,j) = smooth(fillmissing(tt(:,j),'pchip'),15);
% end
% subplot(3,1,1)
% plot((tttt))
% axis([0 length(tttt) -1 1])
% % hold on
% % plot(tttt(:,2));
% % hold off
% subplot(3,1,2)
% plot(diff(tttt))
% hold on
% plot(sum(abs(diff(tttt(:,[1 3])))'))
% hold off
% axis([0 length(tttt) -.015 .015])
% subplot(3,1,3)
% plot(abs(tttt(:,1)-tttt(:,3)))
% axis([0 length(tttt) 0 1.4])
% % scatter3(tttt(:,1),tttt(:,3),tttt(:,2),'.k')
% [xxx yyy]=ginput();
% if xxx(2) > length(trials{i})
%     xxx(2)=length(trials{i});
% end
% trials_new{i}=trials{i}(round(xxx(1)):round(xxx(2)),:)
% end

%% merge trials to the same length
trials_unsorted = trials;
for i=1:length(trials_unsorted)
       d = pdist(trials_unsorted{i}(:,[8 9 10]),'euclidean');
       dd = squareform(d);
       dd(dd==0)=nan;
       [mins ind] = min(dd(:));
           [row col] = ind2sub(size(dd),ind);
%            f = find(abs(row-col)==1);  % this worked but it was sloowww,
%            row = row(f);               % removed and replaced w ind2sub
%            col = col(f);
    while length(trials_unsorted{i})>bins
         [mins ind] = min(dd(:));
         [row col] = ind2sub(size(dd),ind);
       % check that row and col are adjacent
       if ((row(1)-col(1)))== -1
           trials_unsorted{i} = [trials_unsorted{i}(1:row(1)-1,:);...
                                nanmean(trials_unsorted{i}(row(1):col(1),:));...
                                trials_unsorted{i}(col(1)+1:end,:)];
           dd(row,:)=[];
           dd(:,col)=[];
       elseif ((row(1)-col(1)))== 1
           trials_unsorted{i} = [trials_unsorted{i}(1:col(1)-1,:);...
                                nanmean(trials_unsorted{i}(col(1):row(1),:));...
                                trials_unsorted{i}(row(1)+1:end,:)];
           dd(row,:)=[];
           dd(:,col)=[];
       elseif abs((row(1)-col(1)))>1 | abs((row(1)-col(1)))<1
           dd(row,col)=nan;
       end
    end
    i
end

% cluster similarity
for i=1:length(trials_unsorted)
    for j =1:length(trials_unsorted)
        for ax = 8:10
        temp(ax-7,:)=crosscorr(trials_unsorted{i}(:,[ax]),trials_unsorted{j}(:,[ax]),bins-1);
        end
        if sum(~isreal(temp(:)))>0
            disp([i j]);
        end
        a = trials_unsorted{i}(:,[8 9 10]);
        b = trials_unsorted{j}(:,[8 9 10]);
% a = [trials_unsorted{i}(:,[9]) trials_unsorted{i}(:,[10])-trials_unsorted{i}(:,[9])];
% b = [trials_unsorted{j}(:,[9]) trials_unsorted{j}(:,[10])-trials_unsorted{j}(:,[9])];
%         temp = corrcoef(reshape(a,bins*3,1),reshape(b,bins*2,1));
%           temp = crosscorr(reshape(a,bins*3,1),reshape(b,bins*,1),120);
%         cc(i,j)=temp(2);
          [cc(i,j) locs]=max(nanmean(temp));
    end
end
% remove NaN's?
n = isnan(cc(:,1));

%%
numConditions = str2num(input('How many conditions were there: ','s'));
while ~isnumeric(numConditions) || isempty(numConditions)
    numConditions = str2num(input('How many conditions were there: ','s'));
end
exitC=0;
while exitC == 0
    clust = kmeans(cc,numConditions);
    clear trials
    for i=1:numConditions
        f = find(clust==i);
        for j=1:length(f)
            trials{i}{j}=trials_unsorted{f(j)};
        end
    end
    for i=1:numConditions
        subplot(4,5,i)
        hold off
        for t = 1:length(trials{i})
            scatter3(trials{i}{t}(:,8),trials{i}{t}(:,10),trials{i}{t}(:,9),'.k')
            hold on
            scatter3(trials{i}{t}(1,8),trials{i}{t}(1,10),trials{i}{t}(1,9),'.g')
            scatter3(trials{i}{t}(end,8),trials{i}{t}(end,10),trials{i}{t}(end,9),'.r')
        end
    end
    pause(.01);
    adjust = input('merge[m]/split[s] ','s');
    if strcmp(adjust,'s')
        numConditions = numConditions+1;
    end
    if strcmp(adjust,'m')
        on = str2num(input('condition #1:','s'));
        tw = str2num(input('condition #2','s'));
        for t = 1:length(trials{tw})
           trials{on}{length(trials{on})+1}=trials{tw}{t};
        end
        trials{tw}=[];
    end
    if ~strcmp(adjust,'s') && ~strcmp(adjust,'m')
       exitC = 1;
       clf();
    end
end
% check that clustering was correct

c=1;
for tt = 1:length(trials)
%             if d == 5  %% only for rec 21st
%                 bins = 100;
%             elseif d == 4
%                 bins = 100;
%             else
%                 bins = 80;
%             end

    hold off
    map{tt}=[];
    t_conc=[];
    for t = 1:length(trials{tt})
        t_conc = [trials{tt}{t}(:,:),20*(trials{tt}{t}(:,1)-trials{tt}{t}(1,1))];
    if length(t_conc)>=bins
        while length(t_conc)>bins+1
        di = pdist(t_conc);
        s = squareform(di);
        s(find(eye(size(s))))=nan;
        [a b] = min(s(:));

        [coords blah] = find(s==a);
        t_conc(coords(1),:) = (t_conc(coords(1),:)+t_conc(coords(2),:))./2;
        t_conc(coords(2),:) = [];
        % debug
%         scatter(t_conc(:,1),t_conc(:,2))
%         pause(.01)
        end
    t_conc_all(t,:,:) = t_conc;
    end
    end
    if length(trials{tt})>0
    map{tt} = squeeze(median(t_conc_all(:,:,:),1));

    if ~isempty(trials{tt})
    subplot(8,4,c)
    c=1+c
    for t = 1:length(trials{tt})
    scatter(trials{tt}{t}(:,1),trials{tt}{t}(:,2),'.k')
    hold on
    scatter(trials{tt}{t}(1,1),trials{tt}{t}(1,2),'.g')
    scatter(trials{tt}{t}(end,1),trials{tt}{t}(end,2),'.r')
    end
    end
    end
    clear t_conc_all
    for t =1:length(trials{tt})  % all trial types (rotations)
        for p = 1:length(trials{tt}{t})
            [a b] = min(nansum(abs([trials{tt}{t}(p,1)-map{tt}(:,1),...
                trials{tt}{t}(p,8)-map{tt}(:,8),...
                trials{tt}{t}(p,9)-map{tt}(:,9),...
                trials{tt}{t}(p,10)-map{tt}(:,10),...
                (trials{tt}{t}(p,1)-trials{tt}{t}(1,1))*50-map{tt}(:,1),...  % penalty for time differences
                40*(p./length(trials{tt}{t})*length(map{tt}) - (1:length(map{tt})))'])'));     % penalty for order differences
            mapping{tt}{t}(p,:) = [map{tt}(b,1:end) b trials{tt}{t}(p,1)];
%             plot(nansum(abs([trials{tt}{t}(p,1)-map{tt}(:,1),...
%                 trials{tt}{t}(p,2)-map{tt}(:,2),...
%                 trials{tt}{t}(p,3)-map{tt}(:,3),...
%                 trials{tt}{t}(p,4)-map{tt}(:,4),...
%                 (trials{tt}{t}(p,5)-trials{tt}{t}(1,5))*20-map{tt}(:,5),...  % penalty for time differences
%                 50*(p./length(trials{tt}{t})*length(map{tt}) - (1:length(map{tt})))'])'))
%             pause
        end
    end
end

%% incorporate this below to reformat things and double check trial assignments...
% load behav
%
c=1
clear m mm tt
for i=1:length(trials)
    if ~isempty(trials{i})
    tt{c}=trials{i};
    m{c}=map{i};
    mm{c}=mapping{i};

    tt{c}=tt{c}(~cellfun('isempty',tt{c}));
    mm{c}=mm{c}(~cellfun('isempty',mm{c}));
    c=1+c;
    end
end
dbmap=m;
mapping=mm;
trials= tt;

for tt = 1:length(trials)
subplot(5,4,tt)
for t = 1:length(trials{tt})
%     scatter(map{tt}(:,1),map{tt}(:,2),'.')
scatter(trials{tt}{t}(:,8),trials{tt}{t}(:,10),'.k')
hold on

%     axis([0 550 0 550])
end
for t = 1:length(trials{tt})
%     scatter(map{tt}(:,1),map{tt}(:,2),'.')
scatter(trials{tt}{t}(1,8),trials{tt}{t}(1,10),'.g')
scatter(trials{tt}{t}(end,8),trials{tt}{t}(end,10),'.r')
hold on

%     axis([0 550 0 550])
end
title(tt);
end

end


function [opts]=test_rnn(net,opts)

    opts.training=0;

    opts.MiniBatchError=[];
    opts.MiniBatchLoss=[];

    for mini_b=1:opts.n_test_batch
        idx=1+(mini_b-1)*opts.parameters.batch_size:mini_b*opts.parameters.batch_size;
        opts.input_data=opts.test(:,idx,:);
        opts.input_labels=opts.test_labels(idx,:);

        %forward
        if strcmp(opts.network_name,'lstm')
            [ net,res,opts ] = lstm_ff( net,opts );
        end
        if strcmp(opts.network_name,'gru')
            [ net,res,opts ] = gru_ff( net,opts );
        end
        if strcmp(opts.network_name,'rnn')
            [ net,res,opts ] = rnn_ff( net,opts );
        end
        if strcmp(opts.network_name,'qrnn')
            [ net,res,opts ] = qrnn_ff( net,opts );
        end

        if isfield(opts,'err')
            opts.MiniBatchError=[opts.MiniBatchError;gather( opts.err(1))];
        end
        opts.MiniBatchLoss=[opts.MiniBatchLoss;gather( opts.loss)];

    end

    opts.results.TestEpochError=[opts.results.TestEpochError;mean(opts.MiniBatchError(:))];
    opts.results.TestEpochLoss=[opts.results.TestEpochLoss;mean(opts.MiniBatchLoss(:))];

end


classdef prtClassRvmForContext < prtClass
    % prtClassRvmForContext  Relevance vector machine classifier for
    % context-dependent applications

    properties (SetAccess=private)
        name = 'Relevance Vector Machine'  % Relevance Vector Machine
        nameAbbreviation = 'RVM'           % RVM
        isNativeMary = false;  % False
        useForContext = true;
    end

    properties
        kernels = prtKernelDc & prtKernelDirect;  % The kernels to be used

        verboseText = true;  % Whether or not to display text during training
        verbosePlot = false;  % Whether or not to plot during training

        maxIterations = 1000;       % The maximum number of iterations
        convergenceThreshold = .01;  % Learning tolerance;

        prior = struct('a',1e-6,'b',1e-6','c',1e-6,'d',1e-6);
        weights = [];

    end

    methods

        function Obj = prtClassRvm(varargin)
            Obj = prtUtilAssignStringValuePairs(Obj,varargin{:});
        end

        function Obj = set.maxIterations(Obj,val)
            if ~prtUtilIsPositiveInteger(val)
                error('prt:prtClassRvm:maxIterations','maxIterations must be a positive integer');
            end
            Obj.learningMaxIterations = val;
        end

        function Obj = set.convergenceThreshold(Obj,val)
            if ~prtUtilIsPositiveScalar(val)
                error('prt:prtClassRvm:convergenceThreshold','convergenceThreshold must be a positive scalar');
            end
            Obj.learningConvergedTolerance = val;
        end

        function Obj = set.kernels(Obj,val)
            assert(numel(val)==1 &&  isa(val,'prtKernel'),'prt:prtClassRvm:kernels','kernels must be a prtKernel');

            Obj.kernels = val;
        end

        function Obj = set.verbosePlot(Obj,val)
            assert(isscalar(val) && (islogical(val) || prtUtilIsPositiveInteger(val)),'prt:prtClassRvm:verbosePlot','verbosePlot must be a logical value or a positive integer');
            Obj.verbosePlot = val;
        end

        function Obj = set.verboseText(Obj,val)
            assert(isscalar(val) && islogical(val),'prt:prtClassRvm:verboseText','verboseText must be a logical value, but value provided is a %s',class(val));
            Obj.verboseText = val;
        end

        function varargout = plot(Obj)
            % plot - Plot output confidence of the prtClassRvm object
            %
            %   CLASS.plot plots the output confidence of the prtClassRvm
            %   object. The dimensionality of the dataset must be 3 or
            %   less, and verboseStorage must be true.

            HandleStructure = plot@prtClass(Obj);

            holdState = get(gca,'nextPlot');
            hold on;
            Obj.sparseKernels.plot;
            set(gca, 'nextPlot', holdState);

            varargout = {};
            if nargout > 0
                varargout = {HandleStructure};
            end
        end

    end

    methods (Access=protected, Hidden = true)

        function Obj = trainAction(Obj,DataSet)
            %Note: do not assume that getTargets returns a double array or
            %values "0" and "1", instead use this:
            Y = Obj.getMinusOneOneTargets(DataSet);
            Y(Y==-1) = 0;

            Obj.kernels = Obj.kernels.train(DataSet);
            GRAMM = Obj.kernels.run_OutputDoubleArray(DataSet);

            prior = Obj.prior;
            prior.A = diag(repmat(prior.a,1,size(GRAMM,2)));
            prior.S = eye(size(GRAMM,2));
            prior.m = zeros(size(GRAMM,2),1);

            if isfield(DataSet.observationInfo,'contextPosterior')
                for i = 1:DataSet.nObservations
                    pCgivenX(i,:) = DataSet.observationInfo(i).contextPosterior;
                end
            else
                pCgivenX = ones(DataSet.nObservations,1);
            end
            pCgivenXmat = repmat(pCgivenX',size(GRAMM,2),1);

            % Run VB
            iter = 0;
            finished = false;
            while ~finished
                iter = iter + 1;

                phi = GRAMM';
                N = size(phi,2);              % Number of samples
                D = size(phi,1);              % Dimensionality
                t = Y;

                if iter == 1 %% Initialization
                    a = repmat(prior.a,1,D);
                    b = repmat(prior.b,1,D);
                    S = prior.S;
                    m = prior.m;
                end

                xi = sqrt(diag(phi'*(S+m*m')*phi));
                lambda = (1./(4*xi)).*tanh(xi./2);

                A = diag(a./b);
                S = inv(A + 2*pCgivenXmat.*phi*diag(lambda)*phi');

                m = .5*S*sum(pCgivenXmat.*repmat(2*t'-1,D,1).*phi,2);

                a = repmat(prior.a,D,1) + .5;
                b = diag(diag(repmat(prior.b,1,D)) + .5*(diag(m.^2) + diag(diag(S))));

                % Calculate Negative Free Energy (Convergence Criterion)
                sig = (1 + exp(-xi)).^-1;
                logF = sum(pCgivenX.*(log(sig) + .5*(2*t-1).*(phi'*m) - .5*xi - lambda.*((diag(phi'*(S+m*m')*phi)) - xi.^2)));

                KLDalpha = 0;
                for d = 1:D
                    KLDalpha = KLDalpha + prtRvUtilGammaKld(a(d),b(d),prior.a,prior.b);
                end
                if det(S) ~= 0
                    KLDw  = .5*(-sum(log(diag(A))) -log(det(S)) + trace(A.*S)' + sum(diag(A).*m.^2) - N);
                else
                    KLDw  = .5*(-sum(log(diag(A))) -log(realmin) + trace(A.*S)' + sum(diag(A).*m.^2) - N);
                end

                NFE(iter) = logF - KLDalpha - KLDw;
                if Obj.verbosePlot
                    figure(666),plot(NFE)
                end

                if iter > 1
                    if NFE(iter) > 0
                        Lpct = 100*(NFE(iter) - NFE(iter-1))/NFE(iter-1);
                    else
                        Lpct = 100*(NFE(iter-1) - NFE(iter))/NFE(iter-1);
                    end
                else
                    Lpct = nan;
                end

                %% Progress mssages, check convergence
                if Obj.verboseText
                    fprintf(['Iteration #',num2str(iter),': Negative Free Energy = ',num2str(NFE(iter)),' (',num2str(Lpct),'%%)\n'])
                end

                if iter > 1
                    if abs(Lpct) <= Obj.convergenceThreshold;
                        finished = true;
                        Obj.weights = m;
                        if Obj.verboseText
                            fprintf('NFE Converged! Congratulation.\n')
                        end
                    elseif iter == Obj.maxIterations;
                        finished = true;
                        Obj.weights = m;
                        if Obj.verboseText
                            fprintf('Max iterations reached. Get out of here!\n')
                        end
                    elseif isinf(Lpct)
                        finished = true;
                        Obj.weights = m;
                        if Obj.verboseText
                            fprintf('Inf found...exiting\n')
                        end
                    end
                end
            end

        end

        function DataSetOut = runAction(Obj,DataSet)

            GRAMM = Obj.kernels.run_OutputDoubleArray(DataSet);
            x = DataSet.X;
            w = Obj.weights;
            for n = 1:size(x,1);
                y(n,:) = w'*GRAMM(n,:)';
            end
            Yout = (1 + exp(-y)).^(-1);

            DataSetOut = prtDataSetClass(Yout,DataSet.Y);
            if isa(DataSet,'prtDataSetClassContext')
                DataSetOut = prtDataSetClassContext(DataSetOut,prtDataSetClass);
            end
        end
    end

    methods (Access=protected, Hidden = true)

        function y = getMinusOneOneTargets(Obj, DataSet) %#ok<MANU>
            yMat = double(DataSet.getTargetsAsBinaryMatrix());
            y = nan(size(yMat,1),1);
            y(yMat(:,1) == 1) = -1;
            y(yMat(:,2) == 1) = 1;
        end

        function G = regularizeGramInnerProduct(Obj, gram)
            nBasis = size(gram,2);

            sigmaSquared = 1e-6;

            %Check to make sure the problem is well-posed.  This can be fixed either
            %with changes to kernels, or by regularization
            G = gram'*gram;
            while rcond(G) < 1e-6
                if sigmaSquared == eps && Obj.verboseText
                    %warning('prt:prtClassRvm:illConditionedG','RVM initial G matrix ill-conditioned; regularizing diagonal of G to resolve; this can be modified by changing kernel parameters\n');
                    fprintf('\n\tRegularizing Gram matrix...\n');
                end
                G = (sigmaSquared*eye(nBasis) + gram'*gram);
                sigmaSquared = sigmaSquared*2;
            end

        end

        function verboseIterationPlot(Obj,DataSet,relevantIndices)
            DsSummary = DataSet.summarize;

            [linGrid, gridSize,xx,yy] = prtPlotUtilGenerateGrid(DsSummary.lowerBounds, DsSummary.upperBounds, Obj.plotOptions); %#ok<ASGLU>

            localKernels = Obj.kernels.train(DataSet);
            cKernels = localKernels.retainKernelDimensions(relevantIndices);
            cPhiDataSet = cKernels.run(prtDataSetClass([xx(:),yy(:)]));
            cPhi = cPhiDataSet.getObservations;

            confMap = reshape(prtRvUtilNormCdf(cPhi*Obj.beta(relevantIndices)),gridSize);
            imagesc(xx(1,:),yy(:,1),confMap,[0,1])
            colormap(Obj.plotOptions.twoClassColorMapFunction());
            axis xy
            hold on
            plot(DataSet);
            cKernels.plot();
            hold off;

            set(gcf,'color',[1 1 1]);
            drawnow;
        end
    end
end


function [v,H,var,n,vsat,idx]=robust_spp(v0,H0,var0,nv,vsat,idx,flag)

[nr,nl]=size(H0);
ave_distance=zeros(nv,1); exc=zeros(nv,1); n=0; %#ok
v=zeros(nr,1); H=zeros(nr,nl); var=zeros(nr,nr);

if flag==0
    MAX_DISTANCE=3; %for pseudorange
else
    MAX_DISTANCE=0.2; %for doppler
end

for i=1:nv
    vi=abs(v0(i));
    if vi<MAX_DISTANCE,continue;end
    v_other=abs(v0);v_other(i)=[];v_other(nv:end,:)=[];

    if vi>3*sum(v_other)/(nv-1)
        exc(i)=1;
        vsat(idx(i))=0;
    end
end

for i=1:nv
    if exc(i)==1,continue;end
    v(n+1)=v0(i);
    H(n+1,:)=H0(i,:);
    var(n+1,n+1)=var0(i,i);
    n=n+1;
end

return


clear
clc

% mysql default configure
mdl_data = get_default_mysql_data('iris');
class_index = 1;
node_sizes = get_node_sizes(mdl_data);
% make a dag
[mdl_data_row,mdl_data_col] = size(mdl_data);

% make a naive struct
dag = mk_naive_struct(length(node_sizes),class_index);

%
% order = catty_order_algorithm(mdl_data,'class_node',1,'subtract_type','sum');
% dag = learn_struct_K2(mdl_data,node_size,order);
% draw_graph(dag);
%
% % make a bnet
bnet = get_bnet(mdl_data,dag,'tabular');
% % set classified times
times = 10;
class_test_algorithm = 'CV-5';
[correct_rate, confusion_matrix] = times_classified_test(bnet,mdl_data,...
    class_index,class_test_algorithm,times);
%
[avg_correct_rate,avg_confusion_matrix] ...
    = computer_avg_classified(correct_rate,confusion_matrix);


classdef TVPsettings < bear.settings.BASEsettings
    %TVPSETTINGS Panel VAR settings class
    %   The bear.settings.TVPsettings class is a class that creates a settings
    %   object to run a time-varying VAR. It can be created directly by
    %   running:
    %
    %   bear.settings.TVPsettings(ExcelFile, varargin)
    %
    %   or in its more convenient form:
    %
    %   BEARsettings('TVP', ExcelFile = 'path/To/file.xlsx')
    %
    % TVPsettings Properties:
    %    tvbvar          - choice of time-varying BVAR model
    %    It              - Gibbs sampler iterations
    %    Bu              - Gibbs sampler burn-in iterations
    %    pick            - retain only one post burn iteration
    %    pickf           - frequency of iteration picking
    %    ar              - auto-regressive coefficients
    %    PriorExcel      - Select individual priors
    %    priorsexogenous - Gibbs sampler burn-in iterations
    %    lambda4         - hyperparameter
    %    gamma           - hyperparameter
    %    alpha0          - hyperparameter
    %    delta0          - hyperparameter
    %    alltirf         - calculate IRFs for every sample period
    %    favar           - FAVAR Options

    properties
        % choice of time-varying BVAR model
        % 1 = time-varying coefficients (TVP)
        % 2 = general time-varying (TVP_SV)
        tvbvar (1,1) bear.TVPtype = 2;
        % switch to Excel interface
        PriorExcel (1,1) logical = false; % set to 1 if you want individual priors, 0 for default
        % switch to Excel interface for exogenous variables
        priorsexogenous (1,1) logical = true; % set to 1 if you want individual priors, 0 for default
        % hyperparameter: lambda4
        lambda4=100;
    end

    properties %Hyperparameters
        % hyperparameter: gamma
        gamma (1,1) double = 0.85;
        % hyperparameter: alpha0
        alpha0 (1,1) double = 0.001;
        % hyperparameter: delta0
        delta0 (1,1) double = 0.001;
        % calculate IRFs for every sample period (1=yes, 0=no)
        alltirf (1,1) logical = true;
    end

    properties
        % total number of iterations for the Gibbs sampler
        It (1,1) double {mustBeGreaterThanOrEqual(It,1)} = 2000;
        % number of burn-in iterations for the Gibbs sampler
        Bu (1,1) double = 1000;
        % choice of retaining only one post burn iteration over 'pickf' iterations (1=yes, 0=no)
        pick (1,1) logical = false;
        % frequency of iteration picking (e.g. pickf=20 implies that only 1 out of 20 iterations will be retained)
        pickf=20;
        % just for the code to run (do not touch)
        ar=0;
    end

    properties % FAVAR
        % FAVAR options
        favar (1,1) bear.settings.favar.FAVARsettings = bear.settings.favar.VARtypeSpecificFAVARsettings; % augment VAR model with factors (1=yes, 0=no)
    end

    methods

        function obj = TVPsettings(excelPath, varargin)

            obj@bear.settings.BASEsettings(6, excelPath)

            obj = parseBEARSettings(obj, varargin{:});

        end

        function obj = set.Bu(obj,value)
            if (value <= obj.It-1) %#ok<MCSUP>
                obj.Bu = value;
            else
                error('bear:settings:TVPsettings',"The maximum value of Bu is It-1: " + (obj.It-1)) %#ok<MCSUP>
            end
        end

        function obj = set.It(obj,value)
            if (value > obj.Bu-1) %#ok<MCSUP>
                obj.It = value;
            else
                error('bear:settings:TVPsettings',"The minimum value of It is Bu+1: " + (obj.Bu+1)) %#ok<MCSUP>
            end
        end

    end

end


function value = r8vec_dif_f ( x )

%*****************************************************************************80
%
%% R8VEC_DIF_F evaluates the function used in R8VEC_DIF_TEST.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  value = exp ( x );

  return
end


clc
close all
clear
% This file tests safety performance for various approaches

%% Setup and simulations
timestep = 0.1;
system_param.A = [[1 ,timestep, 0]; [0, 1, timestep]; [0, 0, 1]];
system_param.B = [0; 0; timestep];
system_param.ul = -1;
system_param.uu = 1;
system_param.timestep = timestep;
x0 = [-2.0;0.0;1.0];
t0 = 0.0;

gammalist = [0.05, 0.10, 0.15, 0.2];

%% MPC-DCBF simulator
mpcdcbf_simulators = {};
for index = 1:length(gammalist)
    mpcdcbf_simulators{index} = CBFDT(system_param, x0, t0);
    param_mpcdcbf = ParamMPCDCBF(8, gammalist(index), 10.0*eye(3), 10.0*eye(3), 1.0);
    mpcdcbf_simulators{index}.setOpt('mpcdcbf', param_mpcdcbf);
    mpcdcbf_simulators{index}.sim(10.0);
end

%% MPC-GCBF simulator
mpcgcbf_simulators = {};
for index = 1:length(gammalist)
    mpcgcbf_simulators{index} = CBFDT(system_param, x0, t0);
    param_mpcgcbf = ParamMPCGCBF(8, gammalist(index), 10.0*eye(3), 10.0*eye(3), 1.0);
    mpcgcbf_simulators{index}.setOpt('mpcgcbf', param_mpcgcbf);
    mpcgcbf_simulators{index}.sim(10.0);
end

%% NMPC-DCBF simulator
nmpcdcbf_simulators = {};
for index = 1:length(gammalist)
    nmpcdcbf_simulators{index} = CBFDT(system_param, x0, t0);
    param_nmpcdcbf = ParamNMPCDCBF(8, 8, gammalist(index), 10.0*eye(3), 10.0*eye(3), 1.0, 10.0);
    nmpcdcbf_simulators{index}.setOpt('nmpcdcbf', param_nmpcdcbf);
    nmpcdcbf_simulators{index}.sim(10.0);
end

%% Plotting
figure('Renderer', 'painters', 'Position', [0 0 500 400]);
color1 = '[0, 0.4470, 0.7410]';
color2 = '[0.8500, 0.3250, 0.0980]';
color3 = '[0.9290, 0.6940, 0.1250]';
color4 = '[0.4940, 0.1840, 0.5560]';
set(gca,'LineWidth', 0.2, 'FontSize', 20);
hold on;
grid on;
set(gca, 'YScale', 'log');
plot(mpcdcbf_simulators{2}.tlog, -mpcdcbf_simulators{2}.xlog(1, :), 'Color', color2, 'LineWidth', 1.0);
plot(mpcdcbf_simulators{3}.tlog, -mpcdcbf_simulators{3}.xlog(1, :), 'Color', color3, 'LineWidth', 1.0);
plot(mpcdcbf_simulators{4}.tlog, -mpcdcbf_simulators{4}.xlog(1, :), 'Color', color4, 'LineWidth', 1.0);
h_legend = legend('MPC-DCBF ($\gamma=0.10$)', 'MPC-DCBF ($\gamma=0.15$)', 'MPC-DCBF ($\gamma=0.20$)');
set(h_legend, 'Interpreter','latex', 'Location', 'SouthWest');
xlim([0, 10]);
% save data and generate figures
print(gcf, 'figures/safety-mpcdcbf.eps', '-depsc');
print(gcf, 'figures/safety-mpcdcbf.png', '-dpng', '-r800');
save('data/safety-mpcdcbf.mat');

figure('Renderer', 'painters', 'Position', [0 0 500 400]);
set(gca,'LineWidth', 0.2, 'FontSize', 20);
hold on;
grid on;
set(gca, 'YScale', 'log');
plot(mpcgcbf_simulators{2}.tlog, -mpcgcbf_simulators{2}.xlog(1, :), 'Color', color2, 'LineWidth', 1.0);
plot(mpcgcbf_simulators{3}.tlog, -mpcgcbf_simulators{3}.xlog(1, :), 'Color', color3, 'LineWidth', 1.0);
plot(mpcgcbf_simulators{4}.tlog, -mpcgcbf_simulators{4}.xlog(1, :), 'Color', color4, 'LineWidth', 1.0);
h_legend = legend('MPC-GCBF ($\gamma=0.10$)', 'MPC-GCBF ($\gamma=0.15$)', 'MPC-GCBF ($\gamma=0.20$)');
set(h_legend, 'Interpreter','latex', 'Location', 'SouthWest');
xlim([0, 10]);
ylim([0.01, 2]);
% save data and generate figures
print(gcf, 'figures/safety-mpcgcbf.eps', '-depsc');
print(gcf, 'figures/safety-mpcgcbf.png', '-dpng', '-r800');
save('data/safety-mpcgcbf.mat');

figure('Renderer', 'painters', 'Position', [0 0 500 400]);
set(gca,'LineWidth', 0.2, 'FontSize', 20);
hold on;
grid on;
set(gca, 'YScale', 'log');
plot(nmpcdcbf_simulators{1}.tlog, -nmpcdcbf_simulators{1}.xlog(1, :), 'Color', color1, 'LineWidth', 1.0);
plot(nmpcdcbf_simulators{2}.tlog, -nmpcdcbf_simulators{2}.xlog(1, :), 'Color', color2, 'LineWidth', 1.0);
plot(nmpcdcbf_simulators{3}.tlog, -nmpcdcbf_simulators{3}.xlog(1, :), 'Color', color3, 'LineWidth', 1.0);
plot(nmpcdcbf_simulators{4}.tlog, -nmpcdcbf_simulators{4}.xlog(1, :), 'Color', color4, 'LineWidth', 1.0);
h_legend = legend('NMPC-DCBF ($\gamma=0.05$)', 'NMPC-DCBF ($\gamma=0.10$)', 'NMPC-DCBF ($\gamma=0.15$)', 'NMPC-DCBF ($\gamma=0.20$)');
set(h_legend, 'Interpreter','latex', 'Location', 'SouthWest');
xlim([0, 10]);
% save data and generate figures
print(gcf, 'figures/safety-nmpcdcbf.eps', '-depsc');
print(gcf, 'figures/safety-nmpcdcbf.png', '-dpng', '-r800');
save('data/safety-nmpccbf.mat');


function p = abss(p);
%ABSS         Polynomial with absolute values of coefficients
%
%   r = abss(p);
%
% Obsolete, replaced by mag (thanks to Arnold for better naming).
%

% written  08/28/00     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 10/18/08     S.M. Rump  obsolete
%

  p = mag(p);


function A=storageclass(lwl,hwl,sto_class)
%STORAGECLASS berfungsi untuk membagi kapasitas tampungan dalam beberapa
%kelas
%lwl adalah low water level operation (m3)
%hwl adalah high water level operation (m3)
%sto_class adalah jumlah storage class
%berbeda dengan inflowclass, nilai kelas disini adalah nilai batas
incr=(hwl-lwl)/(sto_class-1);
%penentuan nilai maksimum per kelas
for a=0:(sto_class-1)
    A(a+1,:)=lwl+incr*a;
end
A;


%**************************************************************
%* mex interface to Andy Liaw et al.'s C code (used in R package randomForest)
%* Added by Abhishek Jaiantilal ( abhishek.jaiantilal@colorado.edu )
%* License: GPLv2
%* Version: 0.02
%
% Calls Regression Random Forest
% A wrapper matlab file that calls the mex file
% This does prediction given the data and the model file
%**************************************************************

function [Y_hat,prediction_per_tree, nodes] = regRF_predict(X,model,extra_options)
    %function Y_hat = regRF_predict(X,model)
    %requires 2 arguments
    %X: data matrix
    %model: generated via regRF_train function
	if nargin<2
		error('need atleast 2 parameters,X matrix and model');
    end
    if exist('extra_options','var')
        if isfield(extra_options,'predict_all')
            predict_all = extra_options.predict_all;
        end
        if isfield(extra_options,'nodes')
            nodes = extra_options.nodes;
        end
    end

    if ~exist('predict_all','var'); predict_all=0;end

    if ~exist('nodes','var'); nodes=0;end

    if isfield(model, 'categorical_feature')
        % have to map prediction array to the correct categories
        for i=1:size(X,2)
            if model.categorical_feature(i)
                tmp_uniques_in_feature = model.orig_uniques_in_feature{i};
                tmp_mapped_uniques_in_feature = model.mapped_uniques_in_feature{i};
                X_loc = X(:,i); %cannot change the original array which may cause chained change of categories to something totally wrong
                for j=1:length(tmp_uniques_in_feature)
                    indices_to_change = find( X(:,i) == tmp_uniques_in_feature(j) );
                    X_loc(indices_to_change) = tmp_mapped_uniques_in_feature(j);
                end
                X(:,i) = X_loc;
            end
        end
        ncat = model.ncat;
    else
        ncat = ones(1,size(X,2));
    end

    maxcat = max(ncat);
	[Y_hat,prediction_per_tree, nodes] = mexRF_predict(X',model.lDau,model.rDau,model.nodestatus,model.nrnodes,model.upper,model.avnode,model.mbest,model.ndtree,model.ntree,predict_all, nodes, int32(ncat), maxcat );

    if ~isempty(find(model.coef)) %for bias corr
        Y_hat = model.coef(1) + model.coef(2)*Y_hat;
    end
	clear mexRF_predict


function r = GetRadar(dt)
%
%
persistent posp

if isempty(posp)
  posp = 0;
end

vel = 100  +  5*randn;
alt = 1000 + 10*randn;

pos = posp + vel*dt;

v = 0 + pos*0.05*randn;

r = sqrt(pos^2 + alt^2) + v;

posp = pos;


function [ dose ] = matRad_interpDicomDoseCube( ct, currDose )
% matRad function to interpolate a given Dicom Dose Cube dicom RTDOSE data
%
% call
%   [ dose ] = matRad_interpDicomDoseCube( ct, currDose )
%
% input
%   ct:             ct imported by the matRad_importDicomCt function
%   currDose:       one (of several) dose cubes which should be interpolated
%
% output
%   dose:           struct with different actual current dose cube and several
%                   meta data
%
% References
%   -
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% read information out of the RT file
dosefile = currDose{1};
if verLessThan('matlab','9')
    doseInfo = dicominfo(dosefile);
else
    doseInfo = dicominfo(dosefile,'UseDictionaryVR',true);
end

% read the dosefile itself
dosedata = dicomread(dosefile);
dose.cube = double(dosedata);
% dose.cube = single(dosedata);

% give it an internal name
dose.internalName = currDose{12};

% read out the resolution
dose.resolution.x = doseInfo.PixelSpacing(1);
dose.resolution.y = doseInfo.PixelSpacing(2);
dose.resolution.z = doseInfo.SliceThickness;

% target resolution is ct.resolution
target_resolution = ct.resolution;

% convert dosedata to 3-D cube
dose.cube = squeeze(dose.cube(:,:,1,:));

% ct resolution is target resolution, now convert to new cube;

% generating grid vectors
x = doseInfo.ImagePositionPatient(1) + doseInfo.ImageOrientationPatient(1) * ...
                                       doseInfo.PixelSpacing(1) * double([0:doseInfo.Columns - 1]);
y = doseInfo.ImagePositionPatient(2) + doseInfo.ImageOrientationPatient(5) * ...
                                       doseInfo.PixelSpacing(2) * double([0:doseInfo.Rows - 1]);
z = [doseInfo.ImagePositionPatient(3) + doseInfo.GridFrameOffsetVector];

% set up grid matrices - implicit dimension permuation (X Y Z-> Y X Z)
% Matlab represents internally in the first matrix dimension the
% ordinate axis and in the second matrix dimension the abscissas axis
[ X,  Y,  Z] = meshgrid(x,y,z);
[Xq, Yq, Zq] = meshgrid(ct.x,ct.y,ct.z);

% get GridScalingFactor
gridScale = double(doseInfo.DoseGridScaling);
% rescale dose.cube
dose.cube = gridScale * dose.cube;

% interpolation to ct grid - cube is now stored in Y X Z
dose.cube = interp3(X,Y,Z,dose.cube,Xq,Yq,Zq,'linear',0);

% write new parameters
dose.resolution = ct.resolution;
dose.x = ct.x;
dose.y = ct.y;
dose.z = ct.z;

% write Dicom-Tags
dose.dicomInfo.PixelSpacing            = [target_resolution.x; ...
                                                target_resolution.y];
dose.dicomInfo.ImagePositionPatient    = [min(dose.x); min(dose.y); min(dose.z)];
dose.dicomInfo.SliceThickness          = target_resolution.z;
dose.dicomInfo.ImageOrientationPatient = doseInfo.ImageOrientationPatient;
dose.dicomInfo.DoseType                = doseInfo.DoseType;
dose.dicomInfo.DoseSummationType       = doseInfo.DoseSummationType;
%dose.dicomInfo.InstanceNumber          = doseInfo.InstanceNumber; %Not
%always given
dose.dicomInfo.SOPClassUID             = doseInfo.SOPClassUID;
dose.dicomInfo.SOPInstanceUID          = doseInfo.SOPInstanceUID;
dose.dicomInfo.ReferencedRTPlanSequence = doseInfo.ReferencedRTPlanSequence;

end


node = [0,0,0; 1,0,0; 0,1,0; 0,0,1];
elem = [1 2 3 4];


function SO3VF = plus(SO3VF1, SO3VF2)
% overloads |SO3VF1 + SO3VF2|
%
% Syntax
%   SO3VF = SO3VF1 + SO3VF2
%   SO3VF = a + SO3VF1
%   SO3VF = SO3VF1 + a
%   SO3VF = SO3VF1 + SO3F;
%   SO3VF = SO3F + SO3VF2;
%
% Input
%  SO3VF1, SO3VF2 - @SO3VectorField
%  a - double, @vector3d
%  SO3F - @SO3Fun
%
% Output
%  SO3VF - @SO3VectorField
%

if isa(SO3VF2,'vector3d')
  SO3VF2 = SO3VF2.xyz.';
end

if isnumeric(SO3VF1) || isa(SO3VF1,'SO3Fun')
  SO3VF = SO3VF2;
  SO3VF.SO3F = SO3VF1 + SO3VF2.SO3F;
  return
end
if isnumeric(SO3VF2) || isa(SO3VF2,'SO3Fun')
  SO3VF = SO3VF2 + SO3VF1;
  return
end

ensureCompatibleSymmetries(SO3VF1,SO3VF2)
SO3VF1 = SO3VectorFieldHarmonic(SO3VF1);
SO3VF2 = SO3VectorFieldHarmonic(SO3VF2);
SO3VF = SO3VF1;
SO3VF.SO3F = SO3VF1.SO3F+SO3VF2.SO3F;

end


 % Left Stance Domain
 %
 % Contact: Left Toe
function domain = LeftStance(model, load_path)
    % construct the right stance domain of Cassie
    %
    % Parameters:
    % model: the right body model of Cassie robot

    %% first make a copy of the robot model
    %| @note Do not directly assign with the model variable, since it is a
    %handle object.
    domain = copy(model);
    % set the name of the new copy
    domain.setName('LeftStance');

    % Extract state variables
    q = domain.States.x;

    if nargin < 2
        load_path = [];
    end

    %% Add contact
    % left foot point contact
    [left_foot, fric_coef] = sys.frames.LeftFoot(model);

    p_left_foot = getCartesianPosition(domain, left_foot);
    r_left_foot  = getRelativeEulerAngles(domain, left_foot );

    constr = [p_left_foot(1);p_left_foot(2);p_left_foot(3);r_left_foot(3)];
    hol = HolonomicConstraint(domain, constr, 'LeftFoot',...
        'ConstrLabel',{{'LeftFootX','LeftFootY','LeftFootZ','LeftFootYaw'}},...
        'DerivativeOrder',2);
    domain = addHolonomicConstraint(domain,hol);
    %     domain = addContact(domain,left_foot,fric_coef, geom, load_path);
    if ~isempty(fric_coef)

        f = domain.Inputs.ConstraintWrench.fLeftFoot;
        % get the friction cone constraint
        %         [friction_cone, fc_label, auxdata] = getFrictionCone(left_foot, f, fric_coef);
        mu = SymVariable('mu');
        gamma = SymVariable('gamma');
        % x, y, z, roll, yaw
        constr = [f(3) - 300; % fz >= 0
            f(1) + (mu/sqrt(2))*f(3);  % -mu/sqrt(2) * fz < fx
            -f(1) + (mu/sqrt(2))*f(3); % fx < mu/sqrt(2) * fz
            f(2) + (mu/sqrt(2))*f(3);  % -mu/sqrt(2) * fz < fu
            -f(2) + (mu/sqrt(2))*f(3); % fy < mu/sqrt(2) * fz
            f(4) + gamma * f(3);       % -gamma * fz < wy
            -f(4) + gamma * f(3)];     % wy < gamma * fz

        % create a symbolic function object
        friction_cone = SymFunction(['u_friction_cone_', left_foot.Name],...
            constr,{f},{[mu;gamma]});

        % create the label text
        fc_label = {'normal_force';
            'friction_x_pos';
            'friction_x_neg';
            'friction_y_pos';
            'friction_y_neg';
            'tor_firction_neg';
            'tor_firction_pos';
            };

        auxdata = [fric_coef.mu; fric_coef.gamma];
        % create an unilateral constraint object
        fc_cstr = UnilateralConstraint(domain, friction_cone,...
            ['fc' left_foot.Name], 'fLeftFoot', ...
            'ConstrLabel',{fc_label(:)'},...
            'AuxData',auxdata);
        % add as a set of unilateral constraitns
        domain = addUnilateralConstraint(domain, fc_cstr);
    end

    %% Add event
    % height of non-stance foot (left toe)
    [right_foot_frame] = sys.frames.RightFoot(model);
    p_swingFoot = getCartesianPosition(domain, right_foot_frame);
    h_nsf = UnilateralConstraint(domain,p_swingFoot(3),'rightFootHeight','x');
    domain = addEvent(domain, h_nsf);

    % phase variable: time
    t = SymVariable('t');
    p = SymVariable('p',[2,1]);
    tau = (t-p(1))/(p(2)-p(1));

    % relative degree two outputs:
    ya_2 = [ q('qHLeft');
        (q('qALeft')+q('qBLeft'))./2;
        -q('qALeft')+q('qBLeft');
        q('qHRight');
        (q('qARight')+q('qBRight'))./2;
        -q('qARight')+q('qBRight');];

    y2 = VirtualConstraint(domain,ya_2,'output','DesiredType','Bezier','PolyDegree',5,...
        'RelativeDegree',2,'PhaseType','TimeBased',...
        'PhaseVariable',tau,'PhaseParams',p,'Holonomic',true, 'LoadPath', load_path);

    domain = addVirtualConstraint(domain,y2);

    domain.PreProcess = @sim.LeftStancePreProcess;
end


function out = dfun(in)

global X

out = L2_distance(X,X(:,in));
out = out';


function a = dtiGetAlgoHeaderSizeWordAligned()
% Assume a word is 4 bytes

a = dtiGetAlgoHeaderSize();
a = ceil(a/4)*4;


function i4_abs_test ( )

%*****************************************************************************80
%
%% I4_ABS_TEST tests I4_ABS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2015
%
%  Author:
%
%    John Burkardt
%
  i4_lo = -100;
  i4_hi = +100;
  test_num = 10;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4_ABS_TEST\n' );
  fprintf ( 1, '  I4_ABS returns the absolute value of an I4.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      A         B=I4_ABS(A)\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num
    [ a, seed ] = i4_uniform_ab ( i4_lo, i4_hi, seed );
    b = i4_abs ( a );
    fprintf ( 1, '  %10d  %10d\n', a, b );
  end

  return
end


function job = calcParent2Child(job, varargin)
% compute optimal parent to child orientation relationship
%
% The function |calcParent2Child| uses the parent to child orientation
% relationship stored in |job.p2c| as a starting point for an iterative
% process to find a parent to child orientation
% relationship that best possible fits to the child to child
% misorientations in the measured grain data |job.grains|.
%
% Syntax
%
%   % find optimal parent to child orientation relationship
%   job.calcParent2Child
%
%   % display distribtion of the misfit
%   histogram(job.calcGBFit ./ degree)
%
% Input
%  job - @parentGrainReconstructor
%
% Output
%  job.p2c - fitted parent to child orientation relationship
%
% Options
%  c2c - consider only child to child misorientations
%  p2c - consider only parent to child misorientations
%  peakFitting - use peak fitting algorithm
%  quantile  - consider only misorientation within this quantile to the current p2c guess (default 0.9)
%  threshold - only consider misorientations that are within this threshold of the current parent to child OR guess
%
% References
%
% * Tuomo Nyyssönen, <https://www.researchgate.net/deref/http%3A%2F%2Fdx.doi.org%2F10.1007%2Fs11661-018-4904-9?_sg%5B0%5D=gRJGzFvY4PyFk-FFoOIj2jDqqumCsy3e8TU6qDnJoVtZaeUoXjzpsGmpe3TDKsNukQYQX9AtKGniFzbdpymYvzYwhg.5jfOl5Ohgg7pW_6yACRXN3QiR-oTn8UsxZjTbJoS_XqwSaaB7r8NgifJyjSES2iXP6iOVx57sy8HC4q2XyZZaA
% Crystallography, Morphology, and Martensite Transformation of Prior
% Austenite in Intercritically Annealed High-Aluminum Steel>
%
% See also
% calcParent2Child
%

noOpt = ~check_option(varargin,{'p2c','c2c'});
threshold = get_option(varargin,'threshold',5*degree);
quant = get_option(varargin,'quantile',0.5);

if ~isempty(job.p2c), p2c0 = job.p2c; end
p2c0 = getClass(varargin,'orientation',p2c0);

% get p2c from parent 2 child OR
if nnz(job.ebsdPrior.phaseId==job.parentPhaseId) > 0.01 * length(job.ebsdPrior) ...
    && (noOpt || check_option(varargin,'p2c'))

  p2cPairs = neighbors(job.grains(job.csParent),job.grains(job.csChild));

  p2c = inv(job.grains(p2cPairs(:,2)).meanOrientation) .* ...
    job.grains(p2cPairs(:,1)).meanOrientation;

  for k = 1:3
    omega = angle(p2c,p2c0);
    ind = omega < min(threshold, quantile(omega, quant));
    p2c0 = mean(p2c(ind));
  end

  if check_option(varargin,'peakFitting')
    mdf = calcDensity(p2c(ind),'halfwidth',1*degree);
    p2c0 = steepestDescent(mdf,p2c0);
  end

  p2cData = p2c0;

else % some default parent2child orientation relationship

  p2cData = [];

end

% consider also child to child
if (noOpt && angle(p2c0)>5*degree) || check_option(varargin,'c2c')

  % get neighbouring grain pairs
  [c2cPairs, oriChild] = getC2CPairs(job, varargin{:});

  % compute c2c misorientation
  mori = inv(oriChild(:,1)) .* oriChild(:,2);

  % ignore pairs with misorientation angle smaller then 5 degree
  mori(mori.angle < 5 * degree) = [];

  if ~isempty(job.p2c), p2c0 = job.p2c; end
  p2c0 = getClass(varargin,'orientation',p2c0);

  % compute an optimal parent to child orientation relationship
  if check_option(varargin,'v3')

   p2c = fitP2C(mori,p2c0);

  elseif check_option(varargin,'v2')

    p2c = calcParent2Child2(mori,p2c0);

  else

    p2c = calcParent2Child(mori,p2c0,varargin{:});

  end

  % combine p2c and c2c orientation relationships
  if ~isempty(p2cData) && ~check_option(varargin,'noP2C')

    p2c = mean([p2c,p2cData],'weights',[length(c2cPairs),length(p2cPairs)]);

  end

else

  p2c = p2c0;

end

% compute new variantMap
if ~isempty(job.p2c) && length(p2c.variants) == length(job.p2c.variants)
  [~,new2old] = min(angle_outer(p2c.variants,job.p2c.variants,'noSym1'),[],2);
  p2c.opt.variantMap = new2old;
end

% update p2c
job.p2c = p2c;

end


function trsoutput = trs_tCG_cached(problem, trsinput, options, storedb, key)
% Truncated (Steihaug-Toint) Conjugate-Gradient method with caching.
%
% minimize <eta,grad> + .5*<eta,Hess(eta)>
% subject to <eta,eta>_[inverse precon] <= Delta^2
%
% function trsoutput = trs_tCG_cached(problem, trsinput, options, storedb, key)
%
% trs_tCG_cached stores information (when options.trscache == true)
% which can help avoid redundant computations (using tCG_rejectedstep)
% upon step rejection by trustregions compared to trs_tCG at the cost of
% using extra memory.
%
% Inputs:
%   problem: Manopt optimization problem structure
%   trsinput: structure with the following fields:
%       x: point on the manifold problem.M
%       fgradx: gradient of the cost function of the problem at x
%       Delta = trust-region radius
%   options: structure containing options for the subproblem solver
%   storedb, key: manopt's caching system for the point x
%
% Options specific to this subproblem solver:
%   kappa (0.1)
%       kappa convergence tolerance.
%       kappa > 0 is the linear convergence target rate: trs_tCG_cached
%       terminates early if the residual was reduced by a factor of kappa.
%   theta (1.0)
%       theta convergence tolerance.
%       1+theta (theta between 0 and 1) is the superlinear convergence
%       target rate. trs_tCG_cached terminates early if the residual
%       was reduced by a power of 1+theta.
%   mininner (1)
%       Minimum number of inner iterations.
%   maxinner (problem.M.dim())
%       Maximum number of inner iterations.
%   trscache (true)
%       Set to false if no caching for the trs_tCG_cached is desired. It is
%       default true to improve computation time if there are many step
%       rejections in trustregions. Setting trscache to false can reduce
%       memory usage.
%   memorytCG_warningtol (1000)
%       Tolerance memory value in MB before issuing warning when
%       trscache = true.
%       The default is 1GB but this value can be increased depending on the
%       user's machine. To disable the warning completely use:
%       warning('off', 'manopt:trs_tCG_cached:memory')
%
% Output: the structure trsoutput contains the following fields:
%   eta: approximate solution to the trust-region subproblem at x
%   Heta: Hess f(x)[eta] -- this is necessary in the outer loop, and it
%       is often naturally available to the subproblem solver at the
%       end of execution, so that it may be cheaper to return it here.
%   limitedbyTR: true if a boundary solution is returned
%   printstr: logged information to be printed by trustregions.
%   stats: structure with the following statistics:
%           numinner: number of inner loops before returning
%           hessvecevals: number of Hessian calls issued
%           memorytCG_MB: memory of store_iters and store_last in MB
%
% Stored Information:
%   store_iters: a struct array with enough information to compute the next
%       step upon step rejection when the algorithm exits due to negative
%       curvature or trust-region radius violation.
%   store_last: an additional struct to store_iters to compute the next
%       step upon step rejection when the algorithm exits but not due to
%       negative curvature or trust-region radius violation
%
%
% trs_tCG_cached can also be called in the following way (by trustregions)
% to obtain part of the header to print and an initial stats structure:
%
% function trsoutput = trs_tCG_cached([], [], options)
%
% In this case trsoutput contains the following fields:
%   printheader: subproblem header to be printed before the first pass of
%       trustregions
%   initstats: struct with initial values for stored stats in subsequent
%       calls to trs_tCG_cached. Used in the first call to savestats
%       in trustregions to initialize the info struct properly.
%
% See also: trustregions trs_tCG tCG_rejectedstep

% This file is part of Manopt: www.manopt.org.
% This code is an adaptation to Manopt of the original GenRTR code:
% RTR - Riemannian Trust-Region
% (c) 2004-2007, P.-A. Absil, C. G. Baker, K. A. Gallivan
% Florida State University
% School of Computational Science
% (http://www.math.fsu.edu/~cbaker/GenRTR/?page=download)
% See accompanying license file.
% The adaptation was executed by Nicolas Boumal.
%
% Change log:
%
%   VL June 24, 2022:
%       trs_tCG_cached by default stores information at each iteration
%       compared to trs_tCG.
%       This can be useful for the next call to trs_tCG_cached and the work
%       is passed to tCG_rejectedstep rather than the normal tCG loop.

% See trs_tCG for references to relevant equations in
% [CGT2000] Conn, Gould and Toint: Trust-region methods, 2000.

% trustregions only wants header and default values for stats.
if nargin == 3 && isempty(problem) && isempty(trsinput)
    trsoutput.printheader = '';
    if options.verbosity == 2
        trsoutput.printheader = sprintf('%9s   %9s   %9s   %s', ...
                            'numinner', 'hessvec', 'numstored', ...
                            'stopreason');
    elseif options.verbosity > 2
        trsoutput.printheader = sprintf('%9s   %9s   %9s   %9s   %s', ...
                            'numinner', 'hessvec', 'numstored', ...
                            'memtCG_MB', 'stopreason');
    end
    trsoutput.initstats = struct('numinner', 0, 'hessvecevals', 0, ...
                   'memorytCG_MB', 0);
    return;
end

if isfield(options, 'useRand') && options.useRand
    warning('manopt:trs_tCG_cached:rand', ...
            ['options.useRand = true but @trs_tCG_cached ignores it.\n' ...
             'You may set options.subproblemsolver = @trs_tCG;\n', ...
             'Alternatively, set options.useRand = false;']);
end

x = trsinput.x;
Delta = trsinput.Delta;
grad = trsinput.fgradx;

inner   = @(u, v) problem.M.inner(x, u, v);
lincomb = @(a, u, b, v) problem.M.lincomb(x, a, u, b, v);
tangent = @(u) problem.M.tangent(x, u);

% Set local defaults here
localdefaults.kappa = 0.1;
localdefaults.theta = 1.0;
localdefaults.mininner = 1;
localdefaults.maxinner = problem.M.dim();
localdefaults.trscache = true;
localdefaults.memorytCG_warningtol = 1000;

% Merge local defaults with user options, if any
if ~exist('options', 'var') || isempty(options)
    options = struct();
end
options = mergeOptions(localdefaults, options);

% If the previous step was rejected and we want to use caching,
if ~trsinput.accept && options.trscache
    % Then check if there is cached information for the current point.
    store = storedb.get(key);
    if isfield(store, 'store_iters')
        % If so, use that cache to produce the same output as would have
        % been produced by running the code below (after the 'return'), but
        % without issuing Hessian-vector calls that were already issued.
        trsoutput = tCG_rejectedstep(problem, trsinput, options, store);
        return;
    end
end

% returned boolean to trustregions. true if we are limited by the TR
% boundary (returns boundary solution). Otherwise false.
limitedbyTR = false;

theta = options.theta;
kappa = options.kappa;

eta = problem.M.zerovec(x);
Heta = problem.M.zerovec(x);
r = grad;
e_Pe = 0;

r_r = inner(r, r);
norm_r = sqrt(r_r);
norm_r0 = norm_r;

% Precondition the residual.
z = getPrecon(problem, x, r, storedb, key);

% Compute z'*r.
z_r = inner(z, r);
d_Pd = z_r;

% Initial search direction (we maintain -delta in memory, called mdelta, to
% avoid a change of sign of the tangent vector.)
mdelta = z;
e_Pd = 0;

% If the Hessian or a linear Hessian approximation is in use, it is
% theoretically guaranteed that the model value decreases strictly
% with each iteration of trs_tCG. Hence, there is no need to monitor the model
% value. But, when a nonlinear Hessian approximation is used (such as the
% built-in finite-difference approximation for example), the model may
% increase. It is then important to terminate the trs_tCG iterations and return
% the previous (the best-so-far) iterate. The variable below will hold the
% model value.
%
% This computation could be further improved based on Section 17.4.1 in
% Conn, Gould, Toint, Trust Region Methods, 2000.
% If we make this change, then also modify trustregions to gather this
% value from trs_tCG rather than recomputing it itself.
model_fun = @(eta, Heta) inner(eta, grad) + .5*inner(eta, Heta);
model_value = 0;

% Pre-assume termination because j == end.
stopreason_str = 'maximum inner iterations';

% Track certain iterations in case step is rejected.
% store_iters tracks candidate etas with increasing squared
% norm relevant when limitedbyTR = true, or when <eta, Heta> <= 0
store_iters = struct('normsq', [], 'numinner', [], 'e_Pe', [], ...
    'd_Pd', [], 'e_Pd', [], 'd_Hd', [], 'eta', [], 'Heta', [], ...
    'mdelta', [], 'Hmdelta', []);

max_normsq = 0;

% only need to compute memory for one item in store_iters in Megabytes(MB)
peritermemory_MB = 0;

% total cached memory stored in MB
memorytCG_MB = 0;

% number of iterations where trs_tCG_cached stores information. This value
% will be length(store_iters) plus 1 if store_last is used.
numstored = 0;

% string that is printed by trustregions. For printing
% per-iteration information
printstr = '';

% Begin inner/trs_tCG loop.
for j = 1 : options.maxinner

    % This call is the computationally expensive step.
    Hmdelta = getHessian(problem, x, mdelta, storedb, key);

    % Compute curvature (often called kappa).
    d_Hd = inner(mdelta, Hmdelta);

    % Note that if d_Hd == 0, we will exit at the next "if" anyway.
    alpha = z_r/d_Hd;
    % <neweta,neweta>_P =
    % <eta,eta>_P + 2*alpha*<eta,delta>_P + alpha*alpha*<delta,delta>_P
    e_Pe_new = e_Pe + 2.0*alpha*e_Pd + alpha*alpha*d_Pd;

    if options.debug > 2
        fprintf('DBG:   (r,r)  : %e\n', r_r);
        fprintf('DBG:   (d,Hd) : %e\n', d_Hd);
        fprintf('DBG:   alpha  : %e\n', alpha);
    end

    if options.trscache
        % Selectively store info in store_iter.
        % next_smallest = (1/4^n Delta)^2 with n the smallest integer such
        % that max_normsq <= next_smallest.
        % We use this condition to only store relevant iterations in case
        % of rejection in trustregions.
        if max_normsq > 0
            next_smallest = (1/16)^floor(-(1/4)*(log2(max_normsq) - ...
                                                 log2(Delta^2))) * Delta^2;
        else
            next_smallest = 0;
        end

        if d_Hd <= 0 || e_Pe_new >= next_smallest
            numstored = numstored + 1;

            store_iters(numstored) = struct('normsq', e_Pe_new, 'numinner', ...
                             j, 'e_Pe', e_Pe, 'd_Pd', d_Pd, 'e_Pd', e_Pd,...
                             'd_Hd', d_Hd, 'eta', eta, 'Heta', Heta, ...
                             'mdelta', mdelta, 'Hmdelta', Hmdelta);
            max_normsq = e_Pe_new;

            % getSize for one entry in store_iters which will be the same
            % for all others.
            if peritermemory_MB == 0
                peritermemory_MB = getsize(store_iters(numstored))/1024^2;
            end

            memorytCG_MB = memorytCG_MB + peritermemory_MB;

            if memorytCG_MB > options.memorytCG_warningtol
                warning('manopt:trs_tCG_cached:memory', ...
                [sprintf('trs_tCG_cached will cache %.2f [MB] for at least one iteration of trustregions until a step is accepted.', memorytCG_MB) ...
                'If memory is limited turn off caching by options.trscache = false.\n' ...
                'To disable this warning: warning(''off'', ''manopt:trs_tCG_cached:memory'')']);
             end

        end
    end

    % Check against negative curvature and trust-region radius violation.
    % If either condition triggers, we bail out.
    if d_Hd <= 0 || e_Pe_new >= Delta^2
        % want
        %  ee = <eta,eta>_prec,x
        %  ed = <eta,delta>_prec,x
        %  dd = <delta,delta>_prec,x
        % Note (Nov. 26, 2021, NB): numerically, it might be better to call
        %   tau = max(real(roots([d_Pd, 2*e_Pd, e_Pe-Delta^2])));
        % This should be checked.
        % Also, we should safe-guard against 0/0: could happen if grad = 0.

        % store new struct containing all the required info in store_iter
        tau = (-e_Pd + sqrt(e_Pd*e_Pd + d_Pd*(Delta^2-e_Pe))) / d_Pd;
        if options.debug > 2
            fprintf('DBG:     tau  : %e\n', tau);
        end
        eta = lincomb(1,  eta, -tau,  mdelta);

        % If only a nonlinear Hessian approximation is available, this is
        % only approximately correct, but saves an additional Hessian call.
        Heta = lincomb(1, Heta, -tau, Hmdelta);

        % Technically, we may want to verify that this new eta is indeed
        % better than the previous eta before returning it (this is always
        % the case if the Hessian approximation is linear, but I am unsure
        % whether it is the case or not for nonlinear approximations.)
        % At any rate, the impact should be limited, so in the interest of
        % code conciseness (if we can still hope for that), we omit this.

        limitedbyTR = true;

        if d_Hd <= 0
            stopreason_str = 'negative curvature';
        else
            stopreason_str = 'exceeded trust region';
        end
        break;
    end

    % No negative curvature and eta_prop inside TR: accept it.
    e_Pe = e_Pe_new;
    new_eta  = lincomb(1,  eta, -alpha,  mdelta);

    % If only a nonlinear Hessian approximation is available, this is
    % only approximately correct, but saves an additional Hessian call.
    % TODO: this computation is redundant with that of r, L241. Clean up.
    new_Heta = lincomb(1, Heta, -alpha, Hmdelta);

    % Verify that the model cost decreased in going from eta to new_eta. If
    % it did not (which can only occur if the Hessian approximation is
    % nonlinear or because of numerical errors), then we return the
    % previous eta (which necessarily is the best reached so far, according
    % to the model cost). Otherwise, we accept the new eta and go on.
    new_model_value = model_fun(new_eta, new_Heta);
    if new_model_value >= model_value
        stopreason_str = 'model increased';
        break;
    end

    eta = new_eta;
    Heta = new_Heta;
    model_value = new_model_value; %% added Feb. 17, 2015

    % Update the residual.
    r = lincomb(1, r, -alpha, Hmdelta);

    % Compute new norm of r.
    r_r = inner(r, r);
    norm_r = sqrt(r_r);

    % Check kappa/theta stopping criterion.
    % Note that it is somewhat arbitrary whether to check this stopping
    % criterion on the r's (the gradients) or on the z's (the
    % preconditioned gradients). [CGT2000], page 206, mentions both as
    % acceptable criteria.
    if j >= options.mininner && norm_r <= norm_r0*min(norm_r0^theta, kappa)
        % Residual is small enough to quit
        if kappa < norm_r0^theta
            stopreason_str = 'reached target residual-kappa (linear)';
        else
            stopreason_str = 'reached target residual-theta (superlinear)';
        end
        break;
    end

    % Precondition the residual.
    z = getPrecon(problem, x, r, storedb, key);

    % Save the old z'*r.
    zold_rold = z_r;
    % Compute new z'*r.
    z_r = inner(z, r);

    % Compute new search direction.
    beta = z_r/zold_rold;
    mdelta = lincomb(1, z, beta, mdelta);

    % Since mdelta is passed to getHessian, which is the part of the code
    % we have least control over from here, we want to make sure mdelta is
    % a tangent vector up to numerical errors that should remain small.
    % For this reason, we re-project mdelta to the tangent space.
    % In limited tests, it was observed that it is a good idea to project
    % at every iteration rather than only every k iterations, the reason
    % being that loss of tangency can lead to more inner iterations being
    % run, which leads to an overall higher computational cost.
    mdelta = tangent(mdelta);

    % Update new P-norms and P-dots [CGT2000, eq. 7.5.6 & 7.5.7].
    e_Pd = beta*(e_Pd + alpha*d_Pd);
    d_Pd = z_r + beta*beta*d_Pd;

end  % of trs_tCG loop

if options.trscache
    store = storedb.get(key);
    store.store_iters = store_iters;
    if ~limitedbyTR
        % Store extra information since we did not exit because we were
        % limited by TR (model value increased or kappa/theta stopping
        % criterion satisfied)
        store_last = struct('numinner', j, 'stopreason_str', ...
            stopreason_str, 'eta', eta, 'Heta', Heta);
        memorytCG_MB = memorytCG_MB + getsize(store_last)/1024^2;

        if memorytCG_MB > options.memorytCG_warningtol
            warning('manopt:trs_tCG_cached:memory', ...
            [sprintf('trs_tCG_cached will cache %.2f [MB] for at least one iteration of trustregions until a step is accepted.', memorytCG_MB) ...
            'If memory is limited turn off caching by options.trscache = false.\n' ...
            'If more memory can be used without problem increase options.memorytCG_warningtol accordingly.\n' ...
            'To disable this warning: warning(''off'', ''manopt:trs_tCG_cached:memory'')']);
        end
        store.store_last = store_last;

        numstored = numstored + 1;
    end

    storedb.set(store, key);
end
stats = struct('numinner', j, 'hessvecevals', j, ...
                    'memorytCG_MB', memorytCG_MB);

if options.verbosity == 2
    printstr = sprintf('%9d   %9d   %9d   %s', j, j, numstored, ...
                stopreason_str);
elseif options.verbosity > 2
    printstr = sprintf('%9d   %9d   %9d   %9.2f   %s', j, j, numstored, ...
                memorytCG_MB, stopreason_str);
end

trsoutput.eta = eta;
trsoutput.Heta = Heta;
trsoutput.limitedbyTR = limitedbyTR;
trsoutput.printstr = printstr;
trsoutput.stats = stats;
end