function out = SY_SlidingWindow(y,windowStat,acrossWinStat,numSeg,incMove)
% SY_SlidingWindow  Sliding window measures of stationarity.
%
% This function is based on sliding a window along the time series, measuring
% some quantity in each window, and outputting some summary of this set of local
% estimates of that quantity.
%
% Another way of saying it: calculate 'windowStat' in each window, and computes
% 'acrossWinStat' for the set of statistics calculated in each window.
%
%---INPUTS:
%
% y, the input time series
%
% windowStat, the measure to calculate in each window:
%               (i) 'mean', mean
%               (ii) 'std', standard deviation
%               (iii) 'ent', distribution entropy
%               (iv) 'mom3', skewness
%               (v) 'mom4', kurtosis
%               (vi) 'mom5', the fifth moment of the distribution
%               (vii) 'lillie', the p-value for a Lilliefors Gaussianity test
%               (viii) 'AC1', the lag-1 autocorrelation
%               (ix) 'apen', Approximate Entropy, ApEn(1,0.2)
%               (ix) 'sampen', Sample Entropy, SampEn(2,0.1)
%
% acrossWinStat, controls how the obtained sequence of local estimates is
%                   compared (as a ratio to the full time series):
%                       (i) 'std': standard deviation
%                       (ii) 'ent' histogram entropy
%                       (iii) 'apen': Approximate Entropy, ApEn(1,0.2)
%                       (iii) 'sampen': Sample Entropy, SampEn(2,0.1)
%
% numSeg, the number of segments to divide the time series up into, thus
%       controlling the window length
%
% incMove, the increment to move the window at each iteration, as 1/fraction of the
%       window length (e.g., incMove = 2, means the window moves half the length of the
%       window at each increment)
%
% NOTE: SY_SlidingWindow(y,'mean','std',X,1) is the same as StatAvX, computed as
%                       SY_StatAv(y,'seg',X);
% cf. "Heart rate control in normal and aborted-SIDS infants", S. M. Pincus et al.
%           Am J. Physiol. Regul. Integr. Comp. Physiol. 264(3) R638 (1993)

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

doPlot = false; % plot outputs

% ------------------------------------------------------------------------------
% Check Inputs
% ------------------------------------------------------------------------------

if nargin < 2 || isempty(windowStat)
    windowStat = 'mean'; % measure within each window
end
if nargin < 3 || isempty(acrossWinStat)
    acrossWinStat = 'std'; % measure across all windows
end
if nargin < 4 || isempty(numSeg)
    numSeg = 5;
end
if nargin < 5 || isempty(incMove)
    incMove = 2;
end

% ------------------------------------------------------------------------------

winLength = floor(length(y)/numSeg); % length of window
if winLength==0
    warning('Time-series of length %u is too short for %u windows',length(y),numSeg);
    out = NaN;
    return
end
inc = floor(winLength/incMove); % increment to move at each step
% If increment rounded down to zero, prop it up:
if inc == 0
    inc = 1;
end

numSteps = (floor((length(y)-winLength)/inc)+1);
qs = zeros(numSteps,1);

% Convert a step index (stepInd) to a range of indices corresponding to that window:
getWindow = @(stepInd) ((stepInd-1)*inc + 1:(stepInd-1)*inc + winLength);

switch windowStat
    case 'mean' % Sliding window mean
        for i = 1:numSteps
            qs(i) = mean(y(getWindow(i)));
        end
    case 'std' % Sliding window std
        for i = 1:numSteps
            qs(i) = std(y(getWindow(i)));
        end
    case 'ent' % Sliding window distributional entropy
        for i = 1:numSteps
            qs(i) = EN_DistributionEntropy(y(getWindow(i)),'ks',[]);
        end
    case 'apen' % Sliding window ApEn
        for i = 1:numSteps
            qs(i) = EN_ApEn(y(getWindow(i)),1,0.2);
        end
    case 'sampen' % Sliding window SampEn
        for i = 1:numSteps
            sampEn_struct = EN_SampEn(y(getWindow(i)),1,0.1);
            qs(i) = sampEn_struct.sampen1;
        end
    case 'mom3' % Third moment
        for i = 1:numSteps
            qs(i) = DN_Moments(y(getWindow(i)),3);
        end
    case 'mom4' % Fourth moment
        for i = 1:numSteps
            qs(i) = DN_Moments(y(getWindow(i)),4);
        end
    case 'mom5' % Fifth moment
        for i = 1:numSteps
            qs(i) = DN_Moments(y(getWindow(i)),5);
        end
    case 'lillie' % Lilliefors test
        for i = 1:numSteps
            qs(i) = HT_DistributionTest(y(getWindow(i)),'lillie','norm');
        end
    case 'AC1' % Lag-1 autocorrelation
        for i = 1:numSteps
            qs(i) = CO_AutoCorr(y(getWindow(i)),1,'Fourier');
        end
    otherwise
        error('Unknown statistic ''%s''',windowStat)
end

% Check for all errors (e.g., short time series):
if all(isnan(qs))
    warning('These sliding window settings are not suitable for this time series');
    out = NaN;
    return
end

% ------------------------------------------------------------------------------
% Plot
% ------------------------------------------------------------------------------
if doPlot
    figure('color','w'); box('on');
    plot(round(winLength/2):inc:(numSteps-1)*inc+round(winLength/2),qs,'r');
end

% ------------------------------------------------------------------------------
% Compute the output statistic
% ------------------------------------------------------------------------------
switch acrossWinStat
    case 'std'
        % normalized by std of full time series
        out = std(qs)/std(y);
    case 'apen'
        out = EN_ApEn(qs,1,0.2); % ApEn of the sliding window measures
    case 'sampen'
        sampEn_struct = EN_SampEn(qs,2,0.15);
        out = sampEn_struct.quadSampEn1;
    case 'ent'
        % get a load of statistics from kernel-smoothed distribution (inefficient since only one is used)
        kssimpouts = DN_FitKernelSmooth(qs);
        out = kssimpouts.entropy; % distributional entropy
    otherwise
        error('Unknown statistic: ''%s''.',acrossWinStat)
end

end


% This function is designed to determine whether a sphere collides with a
% give triangle

function [haveCollided] = CheckSphereTriangleIntersection(p,r,pa,pb,pc)

% INpUT pcHEpcK
assert(numel(p) == 3,'pcentroid must be a column vector [3x1]');
assert(numel(r) == 1,'Radius must be a scalar');
assert(numel(pa) == 3 && numel(pb) == 3 && numel(pc) == 3,...
       'Vertex point must be a column vector [3x1]');

% pcHEpcK pLpaNpaR SEppaRpaTION
pa = pa - p;
pb = pb - p;
pc = pc - p;
rr = r^2;
V = cross(pb-pa,pc-pa);
d = dot(pa,V);
e = dot(V,V);
sep1 = d^2 > rr*e;
% pcHEpcK VERTIpcES
aa = dot(pa,pa);
ab = dot(pa,pb);
ac = dot(pa,pc);
bb = dot(pb,pb);
bc = dot(pb,pc);
cc = dot(pc,pc);
sep2 = (aa > rr) & (ab > aa) & (ac > aa);
sep3 = (bb > rr) & (ab > bb) & (bc > bb);
sep4 = (cc > rr) & (ac > cc) & (bc > cc);
% pcHEpcK TRIpaNGLE EDGES
papb = pb - pa;
pbpc = pc - pb;
pcpa = pa - pc;
d1 = ab - aa;
d2 = bc - bb;
d3 = ac - cc;
e1 = dot(papb,papb);
e2 = dot(pbpc,pbpc);
e3 = dot(pcpa,pcpa);
Q1 = pa*e1 - d1*papb;
Q2 = pb*e2 - d2*pbpc;
Q3 = pc*e3 - d3*pcpa;
Qpc = pc*e1 - Q1;
Qpa = pa*e2 - Q2;
Qpb = pb*e3 - Q3;
sep5 = dot(Q1,Q1) > (rr*e1*e1) & dot(Q1,Qpc) > 0;
sep6 = dot(Q2,Q2) > (rr*e2*e2) & dot(Q2,Qpa) > 0;
sep7 = dot(Q3,Q3) > (rr*e3*e3) & dot(Q3,Qpb) > 0;
isSeparated = sep1 || sep2 || sep3 || sep4 || sep5 || sep6 || sep7;
haveCollided = ~isSeparated;
end


close all; clear all;

%% Setting of the problem
global s
pde = fracLapdata6;
option.theta = 0.3;
option.estType = 'star';
option.maxIt = 18;
option.maxN = 5e4;
option.solver = 'mg';
option.tol = 1e-6;
[node,elem] = squaremesh([-1,1,-1,1],0.5);
bdFlag = setboundary(node,elem,'Dirichlet');

% %% s = 0.2
% s = 0.2;
% afemfracLap(node,elem,pde,bdFlag,option);
%
% %% s = 0.4
% s = 0.4;
% afemfracLap(node,elem,pde,bdFlag,option);

%% s = 0.6
s = 0.6;
afemfracLap(node,elem,pde,bdFlag,option);

%% s = 0.8
s = 0.8;
afemfracLap(node,elem,pde,bdFlag,option);


% TTEST_CELL - compute paired t-test. Allow fast computation of
%                multiple t-test using matrix manipulation.
%
% Usage:
%    >> [F df] = ttest_cell( { a b } );
%    >> [F df] = ttest_cell(a, b);
%
% Inputs:
%   a,b       = data consisting of PAIRED arrays to be compared. The last
%               dimension of the data array is used to compute the t-test.
% Outputs:
%   T   - T-value
%   df  - degree of freedom (array)
%
% Example:
%   a = { rand(1,10) rand(1,10)+0.5 }
%   [T df] = ttest_cell(a)
%   signif = 1-tcdf(T, df(1))
%
%   % for comparison, the same using the Matlab t-test function
%   [h p ci stats] = ttest(a{1}', b{1}');
%   [ stats.tstat' p]
%
%   % fast computation (fMRI scanner volume 100x100x100 and 10 subjects in
%   % two conditions). The computation itself takes 0.5 seconds instead of
%   % half an hour using the standard approach (1000000 loops and Matlab
%   % t-test function)
%   a = rand(100,100,100,10); b = rand(100,100,100,10);
%   [F df] = ttest_cell({ a b });
%
% Author: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 2005
%
% Reference:
%   Schaum's outlines in statistics (3rd edition). 1999. Mc Graw-Hill.

% Copyright (C) Arnaud Delorme
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [tval, df] = ttest_cell(a,b)

    if nargin < 1
        help ttest_cell;
        return;
    end

    if iscell(a), b = a{2}; a = a{1}; end
    tmpdiff = a-b;
    diff = mymean(tmpdiff,    myndims(a));
    sd   = mystd( tmpdiff,[], myndims(a));
    tval = diff./sd*sqrt(size(a, myndims(a)));
    df   = size(a, myndims(a))-1;

    % check values againg Matlab statistics toolbox
    %[h p ci stats] = ttest(a', b');
    % [ tval stats.tstat' ]

function val = myndims(a)
    if ndims(a) > 2
        val = ndims(a);
    else
        if size(a,1) == 1,
            val = 2;
        elseif size(a,2) == 1,
            val = 1;
        else
            val = 2;
        end
    end;

function res = mymean( data, varargin) % deal with complex numbers
    res = mean( data, varargin{:});
    if ~isreal(data)
        res = abs( res );
    end

function res = mystd( data, varargin) % deal with complex numbers
    if ~isreal(data)
        res = std( abs(data), varargin{:});
    else
        res = sqrt(sum( bsxfun(@minus, data, mean( data, varargin{2})).^2, varargin{2})/(size(data,varargin{2})-1)); % 8 percent speedup
        %res = std( data, varargin{:});
    end


function ss=lpczz2ss(zz)
%LPCZZ2SS Convert z-place poles to s-plane poles SS=(ZZ)
%the s-plane is in units of Normalized Hz and so the imaginary part
% of each ss() value is in the range +-0.5
%
% If you multiply ss by the sample frequency, a formant with
% frequency f and bandwidth b will give an s-plane pole-pair
% of approximately -b/2 +- j f

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpczz2ss.m,v 1.4 2007/05/04 07:01:39 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ss=log(max(zz,1e-8))*0.5/pi;


function rhs_lucas_test ( )

%*****************************************************************************80
%
%% RHS_LUCAS_TEST samples the right hand side at the initial time.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/navier_stokes_2d_exact/rhs_lucas_test.m
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 March 2015
%
%  Author:
%
%    John Burkardt
%
  nu = 1.0;
  rho = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'RHS_LUCAS_TEST\n' );
  fprintf ( 1, '  Lucas Bystricky Flow\n' );
  fprintf ( 1, '  Sample the Navier-Stokes right hand sides\n' );
  fprintf ( 1, '  at the initial time T = 0, using the unit square.\n' );
  fprintf ( 1, '  Kinematic viscosity NU = %g\n', nu );
  fprintf ( 1, '  Fluid density RHO = %g\n', rho );

  n = 1000;
  xy_lo = 0.0;
  xy_hi = 1.0;
  seed = 123456789;
  [ x, seed ] = r8vec_uniform_ab ( n, xy_lo, xy_hi, seed );
  [ y, seed ] = r8vec_uniform_ab ( n, xy_lo, xy_hi, seed );
  t = 0.0;

  [ f, g, h ] = rhs_lucas ( nu, rho, n, x, y, t );

  fprintf ( 1, '\n' );
  fprintf ( 1, '           Minimum       Maximum\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F:  %14.6g  %14.6g\n', min ( f ), max ( f ) );
  fprintf ( 1, '  G:  %14.6g  %14.6g\n', min ( g ), max ( g ) );
  fprintf ( 1, '  H:  %14.6g  %14.6g\n', min ( h ), max ( h ) );

  return
end


function phiFaceAverage = upwindMean1D(phi, u)
% This function gets the value of the field variable phi defined
% over the MeshStructure and calculates the upwind average on
% the cell faces, based on the direction of the velocity vector for a uniform mesh.
%
% SYNOPSIS:
%   phiFaceAverage = upwindMean1D(phi, u)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Written by Ali A. Eftekhari
% See the license file

% extract the velocity data
% note: size(ux) = [1:m+1, 1:n] and size(uy) = [1:m, 1:n+1]
ux = u.xvalue;

% check the size of the variable and the mesh dimension
Nx = phi.domain.dims(1);

% assign to a temp variable for boundary corrections
phi_tmp = phi.value;

% correct the value of phi at the boundary (calculation trick)
% assign the value of the left boundary to the left ghost cell
phi_tmp(1) = (phi.value(1)+phi.value(2))/2;
% assign the value of the right boundary to the right ghost cell
phi_tmp(end) = (phi.value(end)+phi.value(end-1))/2;

% calculate the average value
xvalue = (ux>0).*phi_tmp(1:Nx+1)+ ...
                        (ux<0).*phi_tmp(2:Nx+2)+ ...
                        0.5*(ux==0).*(phi.value(1:Nx+1)+phi.value(2:Nx+2));
phiFaceAverage=FaceVariable(phi.domain, xvalue, [], []);


function [FV] = mesh_grow(FV,origin,dist),

% mesh_grow - explode vertices of mesh by specific distance
%
% FV = mesh_grow(FV,origin,dist)
%
% FV is a struct with fields:
%
% FV.vertices   - Nx3 matrix of Cartesian vertex coordindates (X,Y,Z)
% FV.faces      - Mx3 matrix of triangulation of FV.vertices
%
% origin        - 1x3 row vector, usually (0,0,0)
%
% dist          - how far to explode the mesh away from the origin;
%                 this distance is relative to current distance from
%                 the origin, not the total distance from the origin.
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:57 $

% Licence:  GNU GPL, no implied or express warranties
% History:  10/2002, Darren.Weber_at_radiology.ucsf.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    xo = origin(1); yo = origin(2); zo = origin(3);

    Nvert = size(FV.vertices,1);

    fprintf('...mesh explosion...'); tic;

    for v = 1:Nvert,

        x = FV.vertices(v,1);
        y = FV.vertices(v,2);
        z = FV.vertices(v,3);

        % Find direction cosines for line from centre to vertex
        d = sqrt( (x-xo)^2 + (y-yo)^2 + (z-zo)^2 );

        l = (x-xo)/d; % cos alpha
        m = (y-yo)/d; % cos beta
        n = (z-zo)/d; % cos gamma

        % now decrease d by dist
        d = d + dist;

        % locate vertex at this new distance
        x = (l * d) + xo;
        y = (m * d) + yo;
        z = (n * d) + zo;

        FV.vertices(v,:) = [ x y z ];
    end

    t = toc; fprintf('...done (%5.2f sec)\n',t);

return


% feldkamp_example.m
% example of how to use feldkamp.m for cone-beam CT reconstruction
% Copyright 2004-8-28, Nicole Caparanis, Patty Laskowsky, Taka Masuda,
% and Jeff Fessler, The University of Michigan
% modified version by Ajay Paidi

if ~isvar('proj')
	down = 1/2;
	nv = 240*down;%240*down;
	nh = 256*down;%256*down;
	na = 224*down;%224*down;
	ds = 1024/nh;
	dt = ds;
	dis_src_det = 949;
	dis_iso_det = 408;
	dis_src_iso = dis_src_det - dis_iso_det;
	dis_foc_src = inf; % flat detector panel
	offset_det_h = 0.25; % quarter detector
	offset_det_v = 0.0;
	horiz = ([-(nh-1)/2:(nh-1)/2]' - offset_det_h) * ds;
	verti = ([-(nv-1)/2:(nv-1)/2]' - offset_det_v) * dt;
	printf('rmax=%g', dis_src_iso*sin(atan(max(abs(horiz)) / dis_src_det)))

ell = [ ...
		[0 0 -72 150 200 15 0 5];
		[0 0 -32 150 200 20 0 10];
        [0 0 12 150 200 15 0 10];
		[0 0 52 150 200 20 0 7];
        [0 0 82 150 200 15 0 10];
        ];

	nx = 240*down;%256*down;
	ny = 256*down;%240*down;
	nz = 112*down;%184*down;
	dx = 2/down; dy = dx; dz = dx;
	x = ellipsoids(nx, ny, nz, ell, dx, dy, dz);

	% cone-beam system geometry, generalized from fan-beam geometry.
	% see ASPIRE users guide under tech. reports on web page for details.
	args = arg_pair('system', NaN, 'nx', nx, 'ny', ny, 'nz', nz, ...
		'nv', nv, 'nh', nh, 'na', na, 'support', 'all', ...
		'orbit', 360, 'orbit_start', 0, ...
		'pixel_size', dx, 'ray_spacing', ds, 'strip_width', 0, ...
		'dis_src_det', dis_src_det, ...
		'dis_iso_det', dis_iso_det, ...
		'dis_foc_src', dis_foc_src, ...
		'offset_source', 0, ...
		'offset_det_h', offset_det_h, ...
		'offset_det_v', offset_det_v);

	pl=230;
	im(pl+1, x, 'x'), cbar
	im(pl+2, proj, 'proj'), cbar
	drawnow
prompt
end

% cone-beam reconstruction
mask = true([nx ny nz]);
recon = feldkamp(proj, 'ramp', mask, args);

% show results (off-center slices worse than central slice)
im(pl+4, recon, 'recon'), cbar
im(pl+5, recon - x, 'error'), cbar
ix = 1:nx; iy = ny/2; iz = nz/2;
subplot(236)
plot(ix, x(ix,iy,iz), '-', ix, recon(ix,iy,iz), '--')
axis([1 nx -1 16]), legend('true', 'recon', 2)
title 'MIDDLE SLICE'
iz=12;
subplot(233)
plot(ix, x(ix,iy,iz), '-', ix, recon(ix,iy,iz), '--')
axis([1 nx -1 16]), legend('true', 'recon')
title(sprintf('SLICE %d', iz))
%figure
%subplot(2,1,1)
%ix1 = nx/2;
%iy1 = ny/2;
%iy = 1:ny;
%ix = 1:nx;
%iz = nx/2;
%plot(iy, x(ix1, iy, iz), '-', iy, recon(ix1,iy,iz), '--');
%title('plot of x vs. y on middle slice')


classdef DEAGNG < ALGORITHM
% <multi/many> <real/integer/label/binary/permutation>
% Decomposition based evolutionary algorithm guided by growing neural gas
% aph ---   0.1 --- Parameter alpha
% eps --- 0.314 --- Parameter epsilon

%------------------------------- Reference --------------------------------
% Y. Liu, H. Ishibuchi, N. Masuyama, and Y. Nojima, Adapting reference
% vectors and scalarizing functions by growing neural gas to handle
% irregular Pareto fronts. IEEE Transactions on Evolutionary Computation,
% 2020, 24(3): 439-453.
%--------------------------------------------------------------------------
% Copyright Yiping Liu
% Please contact {yiping0liu@gmail.com} if you have any problem.
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [aph,eps] = Algorithm.ParameterSet(0.1,0.314);

            %% DEA Initialization
            [Ru,Problem.N] = UniformPoint(Problem.N,Problem.M);	% Uniform reference vectors
            Population     = Problem.Initialization();          % Random population
            Zmin           = min(Population.objs,[],1);         % Ideal Point
            AS             = [];                                % Input Signal Archive
            Ruq            = Ru;                                % Refernce vectors in Ru for selection
            [FrontNo,~]    = NDSort(Population.objs,Problem.N); % Fitness for the first mating selection
            crd            = zeros(1,Problem.N);                % Fitness for the first mating selection
            MaxGen         = ceil(Problem.maxFE/Problem.N);    	% Maximum Generation

            %% GNG Initialization
            ArchiveSize = Problem.M*Problem.N;	% Size of Input Signal Archive
            NoG = aph*MaxGen;                   % Number of generations of Not Training GNG
            GNGnet.maxIter = 1;                 % Number of iterations to train GNG per Generation
            GNGnet.maxAge = Problem.N;          % Maximum cluster age
            GNGnet.maxNode = Problem.N;         % Max number of nodes
            GNGnet.lambda = 0.2*Problem.N;      % Cycle for topology reconstruction
            GNGnet.hp = [];                     % Hit point of node
            GNGnet.maxHP = 2*ArchiveSize;       % Max HP of node
            GNGnet.Node = [];                   % Node
            GNGnet.NodeS = [];                  % Expanded node
            GNGnet.NodeP = [];                  % Node mapped to hyperplane
            GNGnet.Err = [];                    % Error
            GNGnet.edge = zeros(2,2);           % Edge between nodes
            GNGnet.age = zeros(2,2);            % Age of edge
            GNGnet.epsilon_a = 0.2;             % Learning coefficient
            GNGnet.epsilon_nb = 0.01;           % Learning coefficient of neighbor
            GNGnet.alpha = 0.5;                 % Nodes r1max and r2max error reduction constant
            GNGnet.delta = 0.9;                 % Error reduction coefficient

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = TournamentSelection(2,Problem.N,FrontNo,crd);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                Zmin       = min([Zmin;Offspring.objs],[],1);

                %% GNG-based adaptation
                if ceil(Problem.FE/Problem.N) <= MaxGen - NoG
                    % Input Signal Archive Update
                    AS = ArchiveUpdate([AS;Offspring.objs],ArchiveSize,Ruq,GNGnet.NodeS,Zmin);
                    nAS = length(AS);
                    % GNG Update (and Algorithm 3)
                    GNGnet.maxNode = min(Problem.N,floor(nAS/2)); % paramter reset
                    GNGnet.maxHP = 2*nAS; % paramter reset
                    GNGnet = GNGUpdate(AS,GNGnet);
                    % Reference Vector Adaptation (Algorithm 4)
                    if size(GNGnet.NodeS,1)>2
                        [Ruq,GNGnet] = ReferenceCombination(Ru,GNGnet);
                    end
                    % Scalarizing Function Adaptation
                    theta = TunePBI(GNGnet,eps); % Tune theta in PBI function
                end

               %% Environmental Selection
               [Population,FrontNo,crd] = ESelection([Population,Offspring],Problem.N,Ruq,GNGnet.NodeS,theta,Zmin);
            end
        end
    end
end


function r = mpower(a,b)
%MPOWER       Hessian power  a ^ n
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

  % for scalars use .^ with improved diameter for even exponent
  if ( prod(size(a))==1 ) & ( prod(size(b))==1 )
    r = a .^ b ;
    return
  end

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

  if isa(b,'double') & isreal(b) & prod(size(b))==1 & b==round(b)
    if b<0
      error('negative exponent in mpower')
    end
    [m n] = size(a);
    if m~=n
      error('hessian mpower of non-square matrix')
    end
    if b==0
      if issparse(a)
        r = typeadj( hessian(speye(size(a))) , typeof(a) );
      else
        r = typeadj( hessian(eye(size(a))) , typeof(a) );
      end
    else                        % b is integer, at least 1
      b = b - 1;
      r = a;
      while b>0
        if mod(b,2)==1
          r = r*a;
        end
        b = floor(b/2);
        if b~=0
          a = a*a;
        end
      end
    end
  else
    error('invalid call of hessian mpower ^')
  end

  if rndold
    setround(rndold)
  end


function ls=v_lpcar2ls(ar)
%V_LPCAR2LS convert ar polynomial to line spectrum pair frequencies LS=(AR)
% output vector elements will be in range 0 to 0.5
% the returned vector will be of length p

% This routine is nowhere near as efficient as it might be

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: v_lpcar2ls.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p1]=size(ar);
p = p1-1;
p2 = fix(p/2);
d=0.5/pi;

if rem(p,2)		% odd order
  for k=1:nf
    aa=[ar(k,:) 0];
    r = aa + fliplr(aa);
    q = aa - fliplr(aa);
    fr = sort(angle(roots(r)));
    fq = [sort(angle(roots(deconv(q,[1 0 -1])))); 0];
    f = [fr(p2+2:p+1).' ; fq(p2+1:p).'];
    f(p+1) = [];
    ls(k,:) = d*f(:).';
  end
else
  for k=1:nf
    aa=[ar(k,:) 0];
    r = aa + fliplr(aa);
    q = aa - fliplr(aa);
    fr = sort(angle(roots(deconv(r,[1 1]))));
    fq = sort(angle(roots(deconv(q,[1 -1]))));
    f = [fr(p2+1:p).' ; fq(p2+1:p).'];
    ls(k,:) = d*f(:).';
  end
end


function sF = regularisation(nodes,y,lambda,varargin)
% computes a regularisation
% Syntax
%   sF = S2FunHarmonic.regularisation(S2Grid,f,lambda)
%   sF = S2FunHarmonic.regularisation(S2Grid,f,lambda,'bandwidth',
%        bandwidth,'node_weights',W,'fourier_weights',What)
%
% Input
%  S2Grid - grid on the sphere
%  f      - function values on the grid (may be multidimensional)
%  lambda - parameter for regularisation
%
% Options
%  bandwidth  - maximum harmonic degree
%  W          - weight w_n for the node nodes (default: Voronoi weights)
%  What       - weight what_{m,l} for the Fourier space (default Sobolev weights for s = 2)
%

[nodes,idx] = unique(nodes(:));
y = reshape(y(idx),length(nodes),[]);
N = length(nodes);
W = get_option(varargin,'node_weights');
if isempty(W)
  W = nodes.calcVoronoiArea;
end
bw = get_option(varargin,'bandwidth',floor(sqrt(2*pi/mean(W))));

What = get_option(varargin,'fourier_weights');
if isempty(What)
  s = 2;
  What = (2*(0:bw)+1).^(2*s);
  What = repelem(What,1:2:(2*bw+1))';
end

% initialize nfsft
nfsftmex('precompute',bw,1000,1,0);
plan = nfsftmex('init_advanced',bw,N,1);
nfsftmex('set_x',plan,[nodes.rho';nodes.theta']);
nfsftmex('precompute_x',plan);

y = W.*y;

% adjoint nfsft
nfsftmex('set_f',plan, y);
nfsftmex('adjoint',plan);
fhat = nfsftmex('get_f_hat_linear',plan);

[fhat,~] = pcg(@(x) afun(x,plan,lambda,W,What),fhat);

sF = S2FunHarmonic(fhat);

% finalize nfsft
nfsftmex('finalize',plan);

end

function y = afun(x,plan,lambda,W,What)
  nfsftmex('set_f_hat_linear',plan,x);
  nfsftmex('trafo',plan);
  y = nfsftmex('get_f',plan);

  y = W.*y;

  nfsftmex('set_f',plan,y);
  nfsftmex('adjoint',plan);
  y = nfsftmex('get_f_hat_linear',plan);

  y = y+lambda*What.*x;
end


function ans = calc_meanDose(doseBinsMidPtsV, volsHistV, volumeType)
%Calculate the mean dose for a given DVH
%  The last parameter 'volumeType' is a wash in this function again
%
%  MODIFICATION ALERT:  THIS FUNCTION IS UTILIZED BY THE DREXLER CODEBASE
%
%  LM: 6 Oct 06, JOD, corrected slight error in not taking middle of dose
%  bin.  Added warning if relative volume not close to one (0.5%
%  tolerance).
%      4 Apr 17, APA, passed the dose as min-points of bins. Hence, no need
%  to add half the binwidth to the passed dose. Call this function after
%  obtaining DVH from loadDVHMatrix.m, since it returns mid-points of dose bins.
%
% Usage: calc_meanDose(doseBinsMidPtsV, volsHistV)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

% doseBinsMidPtsV = (doseBinsLowerPtsV(1:end-1)+doseBinsLowerPtsV(2:end))/2;
% ans = (sum(doseBinsMidPtsV.*volsHistV(1:end-1))+doseBinsLowerPtsV(end)*volsHistV(end))/sum(volsHistV);

ans = sum(doseBinsMidPtsV.*volsHistV)/sum(volsHistV);

return;


% SP_EXTERIOR_DERIVAITVE: computes the exterior derivative as a matrix with size
%  given by the dimension of two consecutive spaces in the De Rham sequence.
%
%   diff_op = sp_exterior_derivative (space1, space2);
%
% INPUT:
%
%   space1:  domain space of the exterior derivative (number of columns)
%   space2:  image space of the exterior derivative (number of rows)
%
% OUTPUT:
%
%   diff_op: sparse matrix representation of the differential operator.
%
% Copyright (C) 2020-2023 Bernard Kapidani, Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function diff_op = sp_exterior_derivative (space1, space2)

  assert (~strcmpi(space1.transform, 'integral-preserving'), ...
    'The first space cannot be the one for integral-preserving splines (or n-forms)')

  ndim = numel (space1.scalar_spaces{1}.knots);
  if (ndim == 1)
    error ('Not implemented. For dimension 1, use the scalar spaces')
  elseif (ndim == 2)
    if (strcmpi (space1.transform, 'curl-preserving'))
      grad_curl = 'grad';
      output_der = 2;
      deg_shift = {[0 1], [1 0]};
      knt_shift = {[0 2], [2 0]};
      degree = space1.scalar_spaces{1}.degree + [1 0];
      knots = {space1.scalar_spaces{2}.knots{1}, space1.scalar_spaces{1}.knots{2}};
    elseif (strcmpi (space1.transform, 'div-preserving'))
      grad_curl = 'curl';
      output_der = 2;
      deg_shift = {[1 0], [0 1]};
      knt_shift = {[2 0], [0 2]};
      degree = space1.scalar_spaces{1}.degree + [0 1];
      knots = {space1.scalar_spaces{1}.knots{1}, space1.scalar_spaces{2}.knots{2}};
    else
      error ('The second space should be either curl-preserving or div-preserving')
    end
    assert (isa(space2, 'sp_scalar'), 'The two spaces are not compatible')
    for idim = 1:ndim
      assert (all(space1.scalar_spaces{idim}.degree == space2.degree+deg_shift{idim}), 'The degrees are not compatible')
      assert (all(cellfun(@numel,space1.scalar_spaces{idim}.knots) == (cellfun(@numel, space2.knots)+knt_shift{idim})), ...
        'The knot vectors are not compatible')
    end
  elseif (ndim == 3)
    grad_curl = 'grad';
    if (strcmpi (space1.transform, 'curl-preserving'))
      output_der = 2;
      deg_shift = {[-1 1 1], [1 -1 1], [1 1 -1]};
      knt_shift = {[-2 2 2], [2 -2 2], [2 2 -2]};
      degree = space1.scalar_spaces{1}.degree + [1 0 0];
      knots = {space1.scalar_spaces{2}.knots{1}, space1.scalar_spaces{1}.knots{2}, space1.scalar_spaces{1}.knots{3}};
      assert (isa(space2, 'sp_vector'), 'The two spaces are not compatible')
      for idim = 1:ndim
        assert (all(space1.scalar_spaces{idim}.degree == space2.scalar_spaces{idim}.degree+deg_shift{idim}), 'The degrees are not compatible')
        assert (all(cellfun(@numel,space1.scalar_spaces{idim}.knots) == (cellfun(@numel, space2.scalar_spaces{idim}.knots)+knt_shift{idim})), ...
          'The knot vectors are not compatible')
      end
    elseif (strcmpi (space1.transform, 'div-preserving'))
      output_der = 3;
      deg_shift = {[1 0 0], [0 1 0], [0 0 1]};
      knt_shift = {[2 0 0], [0 2 0], [0 0 2]};
      degree = space1.scalar_spaces{1}.degree + [0 1 1];
      knots = {space1.scalar_spaces{1}.knots{1}, space1.scalar_spaces{2}.knots{2}, space1.scalar_spaces{3}.knots{3}};
      assert (isa(space2, 'sp_scalar'), 'The two spaces are not compatible')
      for idim = 1:ndim
        assert (all(space1.scalar_spaces{idim}.degree == space2.degree+deg_shift{idim}), 'The degrees are not compatible')
        assert (all(cellfun(@numel,space1.scalar_spaces{idim}.knots) == (cellfun(@numel, space2.knots)+knt_shift{idim})), ...
          'The knot vectors are not compatible')
      end
    else
      error ('Only implemented for curl-preserving and div-preserving transforms')
    end
  end

  diff_ops = op_geom_exterior (knots, degree, grad_curl);
  diff_op = diff_ops{output_der};
end


classdef MW13 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP proposed by Ma and Wang

%------------------------------- Reference --------------------------------
% Z. Ma and Y. Wang, Evolutionary constrained multiobjective optimization:
% Test suite construction and performance comparisons. IEEE Transactions on
% Evolutionary Computation, 2019, 23(6): 972-986.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 15; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            z = 1 - exp(-10*(X(:,obj.M:end) - (repmat(obj.M:obj.D,size(X,1),1) - 1)/obj.D).^2);
            g = 1 + sum((1.5 + (0.1/obj.D)*z.^2 - 1.5*cos(2*pi*z)),2);
            PopObj(:,1) = g.*X(:,1)*1.5;
            PopObj(:,2) = g.*(5 - exp(PopObj(:,1)./g) - abs(0.5*sin(3*pi*PopObj(:,1)./g)));
            PopCon(:,1) = (5 - exp(PopObj(:,1)) - 0.5*sin(3*pi*PopObj(:,1)) - PopObj(:,2)).*(5 - (1 + 0.4*PopObj(:,1)) - 0.5*sin(3*pi*PopObj(:,1)) - PopObj(:,2));
            PopCon(:,2) = -(5 - (1 + PopObj(:,1) + 0.5*PopObj(:,1).^2) - 0.5*sin(3*pi*PopObj(:,1)) - PopObj(:,2)).*(5 - (1 + 0.7*PopObj(:,1)) - 0.5*sin(3*pi*PopObj(:,1)) - PopObj(:,2));
            Population  = SOLUTION(X,PopObj,PopCon,varargin{2:end});
            obj.FE      = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1)  = (0:1.5/(N-1):1.5)';
            R(:,2)  = 5 - exp(R(:,1)) - 0.5*abs(sin(3*pi*R(:,1)));
            c1      = (5-exp(R(:,1))-0.5*sin(3*pi*R(:,1))-R(:,2)).*(5-(1+0.4*R(:,1))-0.5*sin(3*pi*R(:,1))-R(:,2));
            invalid = c1>0;
            while any(invalid)
                R(invalid,:) = R(invalid,:).*1.001;
                c1      = (5-exp(R(:,1))-0.5*sin(3*pi*R(:,1))-R(:,2)).*(5-(1+0.4*R(:,1))-0.5*sin(3*pi*R(:,1))-R(:,2));
                invalid = c1>0;
            end
            R = R(NDSort(R,1)==1,:);
        end
        %% Generate the feasible region
        function R = GetPF(obj)
            [x,y] = meshgrid(linspace(0,2,400),linspace(0,4.5,400));
            z     = nan(size(x));
            fes1  = (5-exp(x)-0.5*sin(3*pi*x)-y).*(5-(1+0.4*x)-0.5*sin(3*pi*x)-y) <= 0;
            fes2  = -(5-(1+x+0.5*x.^2)-0.5*sin(3*pi*x)-y).*(5-(1+0.7*x)-0.5*sin(3*pi*x)-y) <= 0;
            z(fes1 & fes2 & exp(x)+abs(0.5*sin(3*pi*x))+y>=5) = 0;
            R = {x,y,z};
        end
    end
end


%% alternating minimization scheme for low-rank+sparse decomposition

Z_norm=norm(Z(:));
stat.ener=[]; stat.ener(1)=vm.f(A,B);
if input_sig==1
A_norm=norm(A_0(:));
B_norm=norm(B_0(:));
stat.err_a=[]; stat.err_a(1)=norm(A(:)-A_0(:))/A_norm;
stat.err_b=[]; stat.err_b(1)=norm(B(:)-B_0(:))/B_norm;
end
stat.f_a=[]; stat.f_a(1)=norm(proj_fr(A+B-Z,U,V),'fro');
% stat.f_b=[]; %stat.f_b(1)=norm(proj_l0(A+B-Z,B),'fro');
% stat.ts1=[]; stat.ts2=[];
% stat.vi1=[]; stat.vi2=[];
stat.ang1=[]; stat.ang2=[];
stat.ls_count=[];
stat.cg_count=[];
stat.n3=[]; stat.n3(1)=n3;
stat.n4=[]; stat.n4(1)=n4;
stat.sfg_a=[]; stat.sfg_b=[];
stat.cpu=[]; %stat.cpu=time_main;
if ~isempty(A_ref), stat.diff_a=[]; stat.diff_a=norm(A(:)-A_ref(:))/A_norm; end
if ~isempty(B_ref), stat.diff_b=[]; stat.diff_b=norm(B(:)-B_ref(:))/B_norm; end
A_old=A; B_old=B;

% I1=eye(n1); I2=eye(n2);
waiting=waitbar(0,'iterating...');
%time_main=tic;
for k=1:slv.kmax
    waiting=waitbar(k/slv.kmax);

    % A-subproblem
    if sg.sig~=1
    % first shot via partial SVD
    [U,S,V]=lansvd(Z-B,n3);
    A=U*S*V';

    % safeguard by projected dogleg method
    v1=A_old(:)-A(:);
    stat.ang1(end+1)=(vm.f(A_old,B)-vm.f(A,B))/norm(v1(:))^2;

    if sg.sig==2 && stat.ang1(end)<sg.thr
        A=A_old;
        stat.sfg_a(end+1)=1;
        disp(['A safeguarded by Riemannian optim at iter ',num2str(k)])
        riem_optim
    else
        stat.sfg_a(end+1)=0;
    end

    else    % directly via projected dogleg method
        riem_optim
    end

    % B-subproblem by sorting
    B=rtr_l0(Z-A,n4);

    % safeguard
    v1=B_old(:)-B(:);
    stat.ang2(end+1)=(vm.f(A,B_old)-vm.f(A,B))/norm(v1(:))^2;

    if sg.sig~=0 && stat.ang2(end)<sg.thr
        B=Z-A; B(B_old==0)=0;
        stat.ang2(end)=.5;
        stat.sfg_b(end+1)=1;
        disp(['B safeguarded at iter ',num2str(k)])
    else
        stat.sfg_b(end+1)=0;
    end

%     fprintf('|A^k-A^{k-1}|=%1.4e\n',norm(A(:)-A_old(:)))

    % trimming
    trimming

    % stats
    %stat.cpu(end+1)=stat.cpu(1)+toc(time_main);
    stat.ener(end+1)=vm.f(A,B);
    if input_sig==1
    stat.err_a(end+1)=norm(A(:)-A_0(:))/A_norm;
    stat.err_b(end+1)=norm(B(:)-B_0(:))/B_norm;
    end
    stat.f_a(end+1)=norm(proj_fr(vm.g(A,B_old),U,V),'fro');
%     stat.f_b(end+1)=norm(proj_l0(A+B-Z,B),'fro');
    if ~isempty(A_ref), stat.diff_a(end+1)=norm(A(:)-A_ref(:))/A_norm; end
    if ~isempty(B_ref), stat.diff_b(end+1)=norm(B(:)-B_ref(:))/B_norm; end
    stat.n3(end+1)=n3; stat.n4(end+1)=n4;

%     if sg.sig==1, cg.tol=1e-2*(stat.f_a(end)/stat.f_a(1))^.2; end

    % stopping rule
    if stat.f_a(end)<slv.ktol*stat.f_a(1)
%     if stat.err_a(end)<slv.ktol
% 	if stat.ener(end-1)-stat.ener(end)<slv.ktol*stat.ener(end-1)
        break
    end

    A_old=A;
    B_old=B;
end
%toc(time_main)
close(waiting)

% [sum(stat.ls_count),sum(stat.cg_count)]


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtDomND.m                                    |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Multi-dimensional quadrature                  |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Tetra mesh
Nvtx = 1e2;
L    = [5 4 3];
mesh = mshCube(Nvtx,L);

% Boundary mesh
bound = mesh.bnd;
ctr   = bound.ctr;
bound.col(ctr(:,3)==0.5*L(3)) = 1;

% Upperface
upperface = bound.sub(bound.col==1);
rectangle = upperface.bnd;

% Domain
omega = dom(mesh,5);
figure
plot(mesh)
hold on
plot(omega)
alpha(0.2)
axis equal
view(30,30)

% Boundary domain
sigma = dom(bound,3);
figure
plot(bound)
hold on
plot(sigma,'.y')
plotNrm(sigma,'y')
alpha(0.5)
axis equal
view(30,30)

% Rectangle
gamma = dom(rectangle,3);
figure
plot(rectangle)
hold on
plot(gamma)
alpha(0.5)
axis equal
view(30,30)

% Numerical integration on the cube
fct = @(X) ones(size(X,1),1);
V   = integral(omega,fct);
norm(V-prod(L),'inf')

% Numerical integration on the bundary of the cube
fct = @(X) ones(size(X,1),1);
S   = integral(sigma,fct);
norm(S-2*(L(1)*L(2)+L(2)*L(3)+L(1)*L(3)),'inf')

% Numerical integration on the rectangle
fct = @(X) ones(size(X,1),1);
P   = integral(gamma,fct);
norm(P-2*(L(1)+L(2)),'inf')

disp('~~> Michto gypsilab !')


function [T1, M0] = Compute_M0_T1_OnSPGR(data, flipAngles, TR, b1Map, roi, verbose)
%Perform a simple linear-least squares data fit on variable flip angle SPGR data
%
% function [M0, T1] = Compute_M0_T1_OnSPGR(data, flipAngles, TR [, b1Map, roi, verbose])
% -----------------------------------------------------------
% INPUTS:
%   data: width x length x slices x flipAngles matrix
%   flipAngles: vector of flip angles (in degrees) corresponding to last dimension of 'data'
%   TR: Repetition time in seconds
%   b1Map: width x length x slices matrix containing relative flip angle
%         (i.e. if nominal alpha is 60 and measured alpha is 61, then b1Map = 61/60
%   roi: width x length x slices binary mask
%   verbose: logical - for debugging

if ndims(data)<3, data = permute(data(:),[2 3 4 1]); end
[nX, nY, nZ, nFlip] = size(data);
nVox = nX*nY*nZ;

if ~exist('b1Map', 'var') || isempty(b1Map)
    b1Map = ones(nX, nY, nZ);
end

if ~exist('roi', 'var') || isempty(roi)
    roi = true(nX, nY, nZ);
end

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

if length(b1Map) ~= length(data(:,:,:,1)), error('B1 size is different from data size'); end
if ~islogical(roi), roi = logical(roi); end

% Reshape data into 2D array so that future steps are simpler
data = reshape(data,[nVox nFlip])'; % Transpose because MATLAB is column-major
data = data(:, roi(:));
nVox = sum(roi(:));

% Large matrix with redundant values will make computations faster,
% but will also consume more memory
alpha = deg2rad(flipAngles);
if isrow(alpha), alpha = alpha'; end
alpha = repmat(alpha,[1 nVox]) .* repmat(b1Map(roi(:))', [nFlip 1]);

% Do the linear least squares fit and estimate M0 & T1
y = data ./ sin(alpha);
x = data ./ tan(alpha);
[fittedSlope, fittedIntercept] = LinLeastSquares(x,y);
estM0 = fittedIntercept ./ (1-fittedSlope);
estT1 = -TR./log(fittedSlope);

% Assign arbitrary M0 & T1 value if fitted value is unphysical.
% Might be better to set this to NaN so users can identify voxels where fit fails
failedFit = isnan(fittedSlope)      | fittedSlope<0     | ...
            isnan(fittedIntercept)  | fittedIntercept<0 ;
failedFitValue = NaN;
estM0(failedFit) = failedFitValue;
estT1(failedFit) = failedFitValue;

% Assign estimated M0 and T1 values to correct voxel
[T1, M0] = deal(zeros(nX,nY,nZ));
M0(roi(:)) = estM0;
T1(roi(:)) = estT1;
end % END OF Compute_M0_T1_OnSPGR

function [fittedSlope, fittedIntercept] = LinLeastSquares(x,y)
% Simple linear least squares fit
% Inputs:
%   - x and y, arrays of equal sizes
% The first dimension should contain the measurements
% The second dimension could be different samples (e.g. voxels)

if size(x)~=size(y), error('X and Y must have same size for linear fitting'); end
% Use the fact that slope = cov(x,y) / cov(x,x)
lengthX = size(x,1);
numerator = sum(x.*y) - sum(x).*sum(y) / lengthX;
denominator = sum(x.^2) - sum(x).^2 / lengthX;
fittedSlope = numerator ./ denominator;
% Line of best fit has to pass through point (meanX, meanY)
% Use this fact to get intecept
fittedIntercept = mean(y) - fittedSlope .* mean(x);
end % END OF LinLeastSquares


function [ a, ipvt, info ] = zgefa ( a, lda, n )

%*****************************************************************************80
%
%% ZGEFA factors a complex matrix by Gaussian elimination.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%
%  Parameters:
%
%    Input, complex A(LDA,N), the matrix to be factored.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, complex A(LDA,N); an upper triangular matrix and the multipliers
%    which were used to obtain it.  The factorization can be written A = L*U
%    where L is a product of permutation and unit lower triangular matrices
%    and U is upper triangular.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, integer INFO,
%    0, normal value.
%    K, if U(K,K) == 0.0.  This is not an error condition for this
%    subroutine, but it does indicate that ZGESL or ZGEDI will divide by zero
%    if called.  Use RCOND in ZGECO for a reliable indication of singularity.
%

%
%  Gaussian elimination with partial pivoting.
%
  info = 0;

  for k = 1 : n - 1
%
%  Find L = pivot index.
%
    l = izamax ( n-k+1, a(k:n,k), 1 ) + k - 1;
    ipvt(k) = l;
%
%  Zero pivot implies this column already triangularized.
%
    if ( zabs1 ( a(l,k) ) == 0.0 )
      info = k;
      continue;
    end
%
%  Interchange if necessary.
%
    if ( l ~= k )
      temp = a(l,k);
      a(l,k) = a(k,k);
      a(k,k) = temp;
    end
%
%  Compute multipliers
%
    a(k+1:n,k) = - a(k+1:n,k) / a(k,k);
%
%  Row elimination with column indexing
%
    for j = k+1 : n
      t = a(l,j);
      if ( l ~= k )
        a(l,j) = a(k,j);
        a(k,j) = t;
      end
      a(k+1:n,j) = a(k+1:n,j) + t * a(k+1:n,k);
    end

  end

  ipvt(n) = n;

  if ( zabs1 ( a(n,n) ) == 0.0 )
    info = n;
  end

  return
end


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

function p = pnig(xnew, xold, t, r, alpha, beta, delta)
% probability distribution of the NIG model
d = delta * t;                          % time dependent parameter

gamma = sqrt(alpha^2 - beta^2);

% martingale correction removes the drift!
omega = delta * gamma - delta*sqrt(alpha^2-(1+beta)^2); % martingale correction

x = xnew -xold - (r - omega) * t;           %
arg = sqrt(1+(x./delta).^2);

p = real(alpha / pi * besselk(1,alpha .* d .* arg)./arg.* exp(d.*(gamma+beta.*(x./d))));
end


function CPad = directFilter(A,D,tensorOrder,lambda)
% bsarray\directFilter: compute B-spline coefficients from data array
% usage: C = directFilter(A,D,tensorOrder,lambda);
%
% arguments:
%   A - N-dimensional array (vector, image, volume, etc.)
%   D (1xN vector) - degree of each dimension of tensor product BSpline.
%   tensorOrder (scalar) - number of nonsingleton dimensions of A
%   lambda (1xN vector) - smoothing factor for each dimension
%
%   C - array of BSpline coefficients
%

% author: Nathan D. Cahill
% email: ndcahill@gmail.com
% date: 18 April 2008

% if D = 0 or 1, and lambda = 0, return A, with reflection padding
if all(D<2) && all(lambda==0)
    padNum = floor(D/2);
    idx   = cell(1,tensorOrder);
    for k = 1:tensorOrder
        M = size(A,k);
        dimNums = [1:M (M-1):-1:2];
        p = padNum(k);
        idx{k}   = dimNums(mod(-p:M+p, 2*M-2) + 1);
    end
    CPad = A(idx{:});
    return;
end

% get coefficients for BSpline filters
F = cell(tensorOrder,1);
F0 = cell(tensorOrder,1);
for i=1:tensorOrder
    % NEED TO UPDATE THIS TO SCALE LAMBDA FOR ELEMENT SPACING
    [F{i},F0{i}] = getBSplineFiltCoeffs(D(i),true,'direct',lambda(i));
end

% initialize output
C = double(A);

% choose tolerance for K0
K0Tol = eps;

switch tensorOrder
    case 1 % X is a vector - easy case

        CLen = numel(C);

        % loop through poles of direct BSpline filter
        for i = 1:length(F{1})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{1}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CLen (CLen-1):-1:2];
            numReps = ceil(K0/(2*CLen-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{1}(i)./(1-F{1}(i)^2);

            % apply symmetric exponential filter
            C(:) = symExpFilt(C(:),CLen,C0,F{1}(i),K0,KVec);

        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{1};

    case 2 % X is a matrix - a little more difficult

        % first perform direct filtering over each column
        [CRows,CCols] = size(C);

        % loop through poles of direct BSpline filter
        for i = 1:length(F{1})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{1}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CRows (CRows-1):-1:2];
            numReps = ceil(K0/(2*CRows-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{1}(i)./(1-F{1}(i)^2);

            % apply symmetric exponential filter for each column
            for k = 1:CCols
                C(:,k) = symExpFilt(C(:,k),CRows,C0,F{1}(i),K0,KVec);
            end
        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{1};

        % now perform direct filtering across each row
        % loop through poles of direct BSpline filter
        for i = 1:length(F{2})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{2}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CCols (CCols-1):-1:2];
            numReps = ceil(K0/(2*CCols-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{2}(i)./(1-F{2}(i)^2);

            % apply symmetric exponential filter for each column
            for k = 1:CRows
                C(k,:) = symExpFilt(C(k,:),CCols,C0,F{2}(i),K0,KVec);
            end
        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{2};

    case 3 % X is a volume - a bit more difficult

        % first perform direct filtering over each column
        [CRows,CCols,CSlices] = size(C);

        % loop through poles of direct BSpline filter
        for i = 1:length(F{1})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{1}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CRows (CRows-1):-1:2];
            numReps = ceil(K0/(2*CRows-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{1}(i)./(1-F{1}(i)^2);

            % apply symmetric exponential filter for each column
            for k = 1:CSlices
                for j = 1:CCols
                    C(:,j,k) = symExpFilt(C(:,j,k),CRows,C0,F{1}(i),K0,KVec);
                end
            end
        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{1};

        % now perform direct filtering across each row
        % loop through poles of direct BSpline filter
        for i = 1:length(F{2})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{2}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CCols (CCols-1):-1:2];
            numReps = ceil(K0/(2*CCols-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{2}(i)./(1-F{2}(i)^2);

            % apply symmetric exponential filter for each column
            for k = 1:CSlices
                for j = 1:CRows
                    C(j,:,k) = symExpFilt(C(j,:,k),CCols,C0,F{2}(i),K0,KVec);
                end
            end
        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{2};

        % now perform direct filtering across each slice
        % loop through poles of direct BSpline filter
        for i = 1:length(F{3})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{3}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CSlices (CSlices-1):-1:2];
            numReps = ceil(K0/(2*CSlices-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{3}(i)./(1-F{3}(i)^2);

            % apply symmetric exponential filter for each column
            for k = 1:CCols
                for j = 1:CRows
                    C(j,k,:) = symExpFilt(C(j,k,:),CSlices,C0,F{3}(i),K0,KVec);
                end
            end
        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{3};

    otherwise % X is multidimensional - the most difficult

        % get size of coefficients array
        Csz = size(C);

        % set up vector of indices into each dimension
        idx = cell(1,tensorOrder);
        for k = 1:tensorOrder
            idx{k} = 1:size(C,k);
        end

        % loop through each dimension
        for d = 1:tensorOrder

            % copy original version of idx
            idxCurrent = idx;

            % construct vector of indices which will be used in computing
            % the initial element of the filtered data
            indReflect = [1:Csz(d) (Csz(d)-1):-1:2];

            % construct list of dimensions without d
            dimList = setdiff(1:tensorOrder,d);

            % construct index into entries of remaining dimensions
            [idxCurrent{dimList}] = ndgrid(idx{dimList});
            dimListInd = sub2ind(Csz(dimList),idxCurrent{dimList});
            dimListInd = dimListInd(:);

            % loop through poles of BSpline filter
            for i = 1:length(F{d})

                % compute K0 for current pole
                K0 = ceil(log(K0Tol)./log(abs(F{d}(i))));

                % now compute vector of indices of length K0 that runs back and
                % forth through the length of the data vector (to mimic
                % reflection of the data at each end)
                numReps = ceil(K0/(2*Csz(d)-2));
                KVec = repmat(indReflect,[1 numReps]);
                KVec = KVec(1:K0);

                % compute scaling factor for current pole
                C0 = -F{d}(i)./(1-F{d}(i)^2);

                % now loop through remaining dimensions, applying symmetric
                % exponential filter
                for k=1:numel(dimListInd)
                    [idxCurrent{dimList}] = ind2sub(Csz(dimList),dimListInd(k));
                    C(idxCurrent{:}) = symExpFilt(C(idxCurrent{:}),Csz(d),C0,F{d}(i),K0,KVec);
                end
            end

            % multiply by numerator of direct BSpline filter
            C = real(C)*F0{d};

        end

end

% now pad dimensions by reflection
padNum = floor(D/2);

% Form index vectors to subsasgn input array into output array.
% Also compute the size of the output array.
idx   = cell(1,tensorOrder);
for k = 1:tensorOrder
  M = size(C,k);
  dimNums = [1:M (M-1):-1:2];
  p = padNum(k);
  idx{k}   = dimNums(mod(-p:M+p, 2*M-2) + 1);
end
CPad = C(idx{:});


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

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

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

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

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

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

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


function [model, time] = rbf_build(Xtr, Ytr, bf_type, bf_c, usePolyPart, verbose)
% RBFBUILD
% Builds a Radial Basis Function (RBF) interpolant using training data
%
% Call
%   [model, time] = rbf_build(Xtr, Ytr, bf_type, bf_c, usePolyPart, verbose)
%   [model, time] = rbf_build(Xtr, Ytr, bf_type, bf_c, usePolyPart)
%   [model, time] = rbf_build(Xtr, Ytr, bf_type, bf_c)
%   [model, time] = rbf_build(Xtr, Ytr, bf_type)
%   [model, time] = rbf_build(Xtr, Ytr)
%
% Input
% Xtr, Ytr    : Training data points (Xtr(i,:), Ytr(i)), i = 1,...,n
%               Note that the input variables must be scaled to e.g. [0,1]
%               or [-1,1] for better predictive performance.
% bf_type     : Type of the basis functions (default = 'MQ'):
%               'BH' = Biharmonic
%               'MQ' = Multiquadric
%               'IMQ' = Inverse Multiquadric
%               'TPS' = Thin plate spline
%               'G' = Gaussian
% bf_c        : Parameter c value (default = 1)
% usePolyPart : Use also the polynomial term P of the model Y = P + RBF
%               (default = 0, do not use)
% verbose     : Set to 0 for no verbose (default = 1)
%
% Output
% model     : RBF model - a struct with the following elements:
%    n      : Number of data points in the training data set
%    meanY  : Mean of Ytr
%    bf_type: Type of the basis functions
%    bf_c   : Parameter c value
%    poly   : Use also the polynomial term
%    coefs  : Coefficients of the model
% time      : Execution time
%
% Please give a reference to the software web page in any publication
% describing research performed using the software, e.g. like this:
% Jekabsons G. Radial Basis Function interpolation for Matlab, 2009,
% available at http://www.cs.rtu.lv/jekabsons/

% This source code is tested with Matlab version 7.1 (R14SP3).

% =========================================================================
% RBF interpolation
% Version: 1.1
% Date: August 12, 2009
% Author: Gints Jekabsons (gints.jekabsons@rtu.lv)
% URL: http://www.cs.rtu.lv/jekabsons/
%
% Copyright (C) 2009  Gints Jekabsons
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
% =========================================================================

if nargin < 2
    error('Too few input arguments.');
else

    [n, d] = size(Xtr);
    [ny, dy] = size(Ytr);
    if (n < 2) || (d < 1) || (ny ~= n) || (dy ~= 1)
        error('Wrong training data sizes.');
    end

    if nargin < 3
        bf_type = 'MQ';
    end
    if nargin < 4
        bf_c = 1;
    end
    if nargin < 5
        usePolyPart = 1;
    end
    if nargin < 6
        verbose = 0;
    end

    tic;

    model.n = n;
    model.meanY = mean(Ytr);
    model.bf_type = bf_type;
    model.bf_c = bf_c;
    model.poly = usePolyPart;

    %calculate and transform distances between all the points in the training data
    dist = zeros(n, n);
    switch upper(model.bf_type)
        case 'BH'
            if verbose
                fprintf('Building RBF (biharmonic) model...\n');
            end
            for i = 1 : n
                %for j = i : n
                %    dist(i, j) = norm(Xtr(i,:) - Xtr(j,:));
                %end
                dist(i, i:n) = sqrt(sum((repmat(Xtr(i,:),n-i+1,1) - Xtr(i:n,:)).^2,2));
                dist(i+1:n, i) = dist(i, i+1:n);
            end
        case 'IMQ'
            if verbose
                fprintf('Building RBF (inverse multiquadric) model...\n');
            end
            for i = 1 : n
                %for j = i : n
                %    dist(i, j) = 1 / sqrt(sum((Xtr(i,:) - Xtr(j,:)).^2) + bf_c^2);
                %end
                dist(i, i:n) = 1 ./ sqrt(sum((repmat(Xtr(i,:),n-i+1,1) - Xtr(i:n,:)).^2,2) + bf_c^2);
                dist(i+1:n, i) = dist(i, i+1:n);
            end
        case 'TPS'
            if verbose
                fprintf('Building RBF (thin plate spline) model...\n');
            end
            for i = 1 : n
                %for j = i : n
                %    dist(i, j) = sum((Xtr(i,:) - Xtr(j,:)).^2);
                %    dist(i, j) = (dist(i, j) + bf_c^2) * log(sqrt(dist(i, j) + bf_c^2));
                %end
                dist(i, i:n) = sum((repmat(Xtr(i,:),n-i+1,1) - Xtr(i:n,:)).^2,2);
                dist(i, i:n) = (dist(i, i:n) + bf_c^2) .* log(sqrt(dist(i, i:n) + bf_c^2));
                dist(i+1:n, i) = dist(i, i+1:n);
            end
        case 'G'
            if verbose
                fprintf('Building RBF (Gaussian) model...\n');
            end
            for i = 1 : n
                %for j = i : n
                %    dist(i, j) = exp(-sum((Xtr(i,:) - Xtr(j,:)).^2) / (2*bf_c^2));
                %end
                dist(i, i:n) = exp(-sum((repmat(Xtr(i,:),n-i+1,1) - Xtr(i:n,:)).^2,2) / (2*bf_c^2));
                dist(i+1:n, i) = dist(i, i+1:n);
            end
        otherwise %MQ
            if verbose
                fprintf('Building RBF (multiquadric) model...\n');
            end
            for i = 1 : n
                %for j = i : n
                %    dist(i, j) = sqrt(sum((Xtr(i,:) - Xtr(j,:)).^2) + bf_c^2);
                %end
                dist(i, i:n) = sqrt(sum((repmat(Xtr(i,:),n-i+1,1) - Xtr(i:n,:)).^2,2) + bf_c^2);
                dist(i+1:n, i) = dist(i, i+1:n);
            end
    end

    %calculate coefs
    if model.poly == 0
        model.coefs = dist \ (Ytr - model.meanY);
    else
        A = [dist, ones(n,1), Xtr; [ones(n,1), Xtr]', zeros(d+1,d+1)];
        model.coefs  = A \ [Ytr; zeros(d+1,1)];
    end

    time = toc;

    if verbose
        fprintf('Execution time: %0.2f seconds\n', time);
    end

end
return


function [tgrad,fgrad,c]=gabphasegrad(method,varargin)
%GABPHASEGRAD   Phase gradient of the DGT
%   Usage:  [tgrad,fgrad,c] = gabphasegrad('dgt',f,g,a,M);
%           [tgrad,fgrad]   = gabphasegrad('phase',cphase,a);
%           [tgrad,fgrad]   = gabphasegrad('abs',s,g,a);
%
%   `[tgrad,fgrad]=gabphasegrad(method,...)` computes the relative
%   time-frequency gradient of the phase of the |dgt| of a signal.
%   The derivative in time *tgrad* is the relative instantaneous
%   frequency while the frequency derivative *fgrad* is the negative
%   of the local group delay.
%
%   *tgrad* is a measure the deviation from the current channel frequency,
%   so a value of zero means that the instantaneous frequency is equal to
%   the center frequency of the considered channel, a positive value means
%   the true absolute intantaneous frequency is higher than the current
%   channel frequency and vice versa.
%   Similarly, *fgrad* is a measure of deviation from the current time
%   positions.
%
%   *fgrad* is scaled such that distances are measured in samples. Similarly,
%   *tgrad* is scaled such that the Nyquist frequency (the highest possible
%   frequency) corresponds to a value of L/2. The absolute time and
%   frequency positions can be obtained as
%
%      tgradabs = bsxfun(@plus,tgrad,fftindex(M)*L/M);
%      fgradabs = bsxfun(@plus,fgrad,(0:L/a-1)*a);
%
%   Please note that neither *tgrad* and *fgrad* nor *tgradabs* and
%   *fgradabs* are true derivatives of the |dgt| phase. To obtain the true
%   phase derivatives, one has to explicitly pass either 'freqinv' or
%   'timeinv' flags and scale both *tgrad* and *fgrad* by 2*pi/L.
%
%   The computation of *tgrad* and *fgrad* is inaccurate when the absolute
%   value of the Gabor coefficients is low. This is due to the fact the the
%   phase of complex numbers close to the machine precision is almost
%   random. Therefore, *tgrad* and *fgrad* may attain very large random values
%   when `abs(c)` is close to zero.
%
%   The computation can be done using four different methods.
%
%     'dgt'    Directly from the signal using algorithm by Auger and
%              Flandrin.
%
%     'phase'  From the phase of a DGT of the signal. This is the
%              classic method used in the phase vocoder.
%
%     'abs'    From the absolute value of the DGT. Currently this
%              method works only for Gaussian windows.
%
%     'cross'  Directly from the signal using algorithm by Nelson.
%
%   `[tgrad,fgrad]=gabphasegrad('dgt',f,g,a,M)` computes the time-frequency
%   gradient using a DGT of the signal *f*. The DGT is computed using the
%   window *g* on the lattice specified by the time shift *a* and the number
%   of channels *M*. The algorithm used to perform this calculation computes
%   several DGTs, and therefore this routine takes the exact same input
%   parameters as |dgt|.
%
%   The window *g* may be specified as in |dgt|. If the window used is
%   'gauss', the computation will be done by a faster algorithm.
%
%   `[tgrad,fgrad,c]=gabphasegrad('dgt',f,g,a,M)` additionally returns the
%   Gabor coefficients *c*, as they are always computed as a byproduct of the
%   algorithm.
%
%   `[tgrad,fgrad]=gabphasegrad('cross',f,g,a,M)` does the same as above
%   but this time using algorithm by Nelson which is based on computing
%   several DGTs.
%
%   `[tgrad,fgrad]=gabphasegrad('phase',cphase,a)` computes the phase
%   gradient from the phase *cphase* of a DGT of the signal. The original DGT
%   from which the phase is obtained must have been computed using a
%   time-shift of *a* using the default phase convention (`'freqinv'`) e.g.::
%
%        [tgrad,fgrad]=gabphasegrad('phase',angle(dgt(f,g,a,M)),a)
%
%   `[tgrad,fgrad]=gabphasegrad('abs',s,g,a)` computes the phase gradient
%   from the spectrogram *s*. The spectrogram must have been computed using
%   the window *g* and time-shift *a* e.g.::
%
%        [tgrad,fgrad]=gabphasegrad('abs',abs(dgt(f,g,a,M)),g,a)
%
%   `[tgrad,fgrad]=gabphasegrad('abs',s,g,a,difforder)` uses a centered finite
%   diffence scheme of order *difforder* to perform the needed numerical
%   differentiation. Default is to use a 4th order scheme.
%
%   Currently the `'abs'` method only works if the window *g* is a Gaussian
%   window specified as a string or cell array.
%
%   See also: resgram, gabreassign, dgt
%
%   References: aufl95 cmdaaufl97 fl65 ltfatnote042

% AUTHOR: Peter L. Søndergaard, 2008; Zdenek Průša 2015

%narginchk(4,6);

% If no phaseconv flag was passed, add 'relative'
definput = arg_gabphasederivconv;
if ~any(cellfun(@(el) any(strcmpi(el,varargin)),definput.flags.phaseconv))
    varargin{end+1} = 'relative';
end

if nargout<3
    phased = gabphasederiv({'t','f'},method,varargin{:});
else
    [phased,c] = gabphasederiv({'t','f'},method,varargin{:});
end

[tgrad,fgrad] = deal(phased{:});


classdef NMPCDCBF2 < handle
    % MPC with distance constraints
    properties
        system
        params
        x0
        x_curr
        time_curr = 0.0
        xlog = []
        ulog = []
        tt = 0
        distlog = []
        solvertime = []
        xopenloop = {}
        uopenloop = {}
        u_cost = 0
        obs
    end
    methods
        function self = NMPCDCBF2(x0, system, params)
            % Define MPC_CBF controller
            self.x0 = x0;
            self.x_curr = x0;
            self.system = system;
            self.params = params;
        end

        function sim(self, time)
            % Simulate the system until a given time
            tic;
            xk = self.x_curr;
            while self.time_curr < time
                [~, uk, tt] = self.solveNMPCDCBF1(self.x_curr);%if infeasiblility happens, stop
                if tt == -1
                    self.tt = tt;%record computing time for each time-step
                    return
                end
                xk = self.system.A * xk + [xk(4,1) * cos(xk(3,1))*self.system.dt;xk(4,1) * sin(xk(3,1))*self.system.dt;0;0] + self.system.C * uk;
                % update system
                self.x_curr = xk;
                self.time_curr = self.time_curr + self.system.dt;
                self.xlog = [self.xlog, xk];
                self.ulog = [self.ulog, uk];
                self.u_cost = self.u_cost + uk'*uk;
                tt = tt + toc;
                self.tt = tt;
                return
            end

        end

        function [xopt, uopt,tt] = solveNMPCDCBF1(self, xk)
            % Solve NMPC-DCBF
            [feas, x, u, J] = self.solve_cftoc1(xk);
            if ~feas
                xopt = [];
                uopt = [];
                tt = -1;
                return
            else
                xopt = x(:,2);
                uopt = u(:,1);
                tt = 0;
            end
        end

        function [feas, xopt, uopt, Jopt] = solve_cftoc1(self, xk)
            % Solve CFTOC
            % extract variables
            N = self.params.N;
            % define variables and cost
            x = sdpvar(4, N+1);%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            u = sdpvar(4, N);
            constraints = [];
            cost = 0;
            % initial constraint
            constraints = [constraints; x(:,1) == xk];
            % add constraints and costs
            AA=self.system.A;
            BB=self.system.B;
            CC=self.system.C;
            constraints = [constraints;
                    self.system.xl <= x(:,1) <= self.system.xu;
                    self.system.ul <= u(:,1) <= self.system.uu;
                    x(:,2) == AA * x(:,1) + [x(4,1)*cos(x(3,1))*self.system.dt;x(4,1)*sin(x(3,1))*self.system.dt;0; 0] + CC * u(:,1)];
                cost = cost + (x(:,1)-[3;0.01;0;0])'*self.params.Q*(x(:,1)-[3;0.01;0;0]) + (u(:,1)-[0;0;1;1])'*self.params.R*(u(:,1)-[0;0;1;1]); % self.params.S*(w(:,1)-1)^2;
            for i = 2:1:N
                constraints = [constraints;
                    self.system.xl <= x(:,i) <= self.system.xu;
                    self.system.ul <= u(:,i) <= self.system.uu;
                    x(:,i+1) == AA * x(:,i) + [x(4,i)*cos(x(3,i))*self.system.dt;x(4,i)*sin(x(3,i))*self.system.dt;0; 0] + CC * u(:,i)];
                cost = cost + (x(:,i)-[3;0.01;0;0])'*self.params.Q*(x(:,i)-[3;0.01;0;0]) + (u(:,i)-[0;0;1;1])'*self.params.R*(u(:,i)-[0;0;1;1]);% self.params.S*(w(:,i)-1)^2;
            end
            % add CBF constraints
            for i = 1:N-1
                pos = self.obs.pos1;
                r = self.obs.r1 ;
                b = (x([1:2],i)-pos)'*((x([1:2],i)-pos))-r^2;
                b_next = (x([1:2],i+1)-pos)'*((x([1:2],i+1)-pos))-r^2;
                b_next_next = (x([1:2],i+2)-pos)'*((x([1:2],i+2)-pos))-r^2;
                b1 = (b_next - b)/self.system.dt + self.params.gamma1/self.system.dt * (b);
                b1_next = (b_next_next - b_next)/self.system.dt + self.params.gamma1/self.system.dt * (b_next);
                constraints = [constraints; b1_next  >=u(4,i)*(1-self.params.gamma2)*b1;b_next  >= u(3,i)*(1-self.params.gamma1)*b];%Highest order mcbf=2
            end
            % add terminal cost
            cost = cost + (x(:,N+1)-[3;0.01;0;0])'*self.params.P*(x(:,N+1)-[3;0.01;0;0]);
            ops = sdpsettings('solver','ipopt','verbose',0);
            % solve optimization
            diagnostics = optimize(constraints, cost, ops);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt = [];
                uopt = [];
                Jopt = [];
            end
            pos = self.obs.pos1;
            r = self.obs.r1 ;
            self.distlog = [self.distlog, (r^2-(xk(1:2)-pos)'*(xk(1:2)-pos))];
            self.xopenloop{size(self.xopenloop,2)+1} = xopt;
            self.uopenloop{size(self.uopenloop,2)+1} = uopt;
            self.solvertime = [self.solvertime, diagnostics.solvertime];
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end
    end
end


%% FOURCURL3P2TEST
%
% Use the quadraticNedelec elements to approximate the velocity u and
% the stream function w.
%
% We solve the following equations:
%
%  -w + curl curl u = 0
%  curl curl w + u  = f
%
% with Dirichlet boundary condition
%
% u\times n = (curl u )\times n  = 0  on \partial \Omega
%
% Please check fourCurl3doc for details.
%
% Lin Zhong, May, 2013. Clean up. Long Chen, Oct 21, 2018
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

close all;
clear variables;

%% Problem Setting
pde = fourCurl3data;
[node,elem] = cubemesh([-1,1,-1,1,-1,1],2);
bdFlag = setboundary3(node,elem,'Dirichlet');

%% Parameters
maxIt = 3;
errwIwhL2 = zeros(maxIt,1);
errwIwhHcurl = zeros(maxIt,1);
errwL2 = zeros(maxIt,1);
errwwhHcurl = zeros(maxIt,1);
erruIuhL2 = zeros(maxIt,1);
erruIuhHcurl = zeros(maxIt,1);
erruL2 = zeros(maxIt,1);
erruuhHcurl = zeros(maxIt,1);
assembleTime = zeros(maxIt,1);
solverTime = zeros(maxIt,1);
N = zeros(maxIt,1);
h = zeros(maxIt,1);
option = [];

for k = 1:maxIt
    % refine grid
    [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    % solve the equation
    [w,u,eqn,info] = fourCurl3P2(node,elem,bdFlag,pde,option);
    % compute error
    uI = edgeinterpolate2(pde.exactu,node,eqn.edge,eqn.face,eqn.face2edge);
    wI = edgeinterpolate2(pde.curlcurlu,node,eqn.edge,eqn.face,eqn.face2edge);
    errwL2(k) = getL2error3ND2(node,elem,pde.curlcurlu,w);
    erruL2(k) = getL2error3ND2(node,elem,pde.exactu,u);
    errwwhHcurl(k) = getHcurlerror3ND2(node,elem,pde.curlcurlcurlu,w);
    erruuhHcurl(k) = getHcurlerror3ND2(node,elem,pde.curlu,u);
    errwIwhL2(k) = sqrt((w-wI)'*eqn.M*(w-wI));
    erruIuhL2(k) = sqrt((u-uI)'*eqn.M*(u-uI));
    errwIwhHcurl(k) = sqrt((w-wI)'*eqn.A*(w-wI));
    erruIuhHcurl(k) = sqrt((u-uI)'*eqn.A*(u-uI));
    % record information
    N(k) = length(u)+length(w);
    h(k) = 1./(size(node,1)^(1/3)-1);
    assembleTime(k) = info.assembleTime;
    solverTime(k) = info.solverTime;
end

%% Plot convergence rates
subplot(2,2,1)
showrateh2(h, errwIwhL2(1:maxIt), 1 , 'k-+', '||w_I-w_h||',...
           h, erruIuhL2(1:maxIt), 1 , 'r-*', '||u_I-u_h||' );
subplot(2,2,2)
showrateh2(h, errwL2(1:maxIt), 1 , 'k-+', '||w-w_h||',...
           h, erruL2(1:maxIt), 1 , 'r-*', '||u-u_h||' );
subplot(2,2,3)
showrateh2(h, errwIwhHcurl(1:maxIt), 1 , 'b-+', '||w_I-w_h||_1',...
           h, erruIuhHcurl(1:maxIt), 1 , 'g-*', '||u_I-u_h||_1' );
subplot(2,2,4)
showrateh2(h, errwwhHcurl(1:maxIt), 1 , 'b-+', '||w-w_h||_1',...
           h, erruuhHcurl(1:maxIt), 1 , 'g-*', '||u-u_h||_1' );

%% Output
err = struct('N', N, 'h', h, ...
             'wL2',errwL2(1:maxIt),'uL2',erruL2(1:maxIt),...
             'wIwhL2',errwIwhL2(1:maxIt), 'uIuhL2',erruIuhL2(1:maxIt),...
             'wIwhHcurl',errwIwhHcurl(1:maxIt),'uIuhHcurl',erruIuhHcurl(1:maxIt),...
             'wwhHcurl',errwwhHcurl(1:maxIt),'uuhHcurl',erruuhHcurl(1:maxIt));
time = struct('N',N,'assemble',assembleTime(1:maxIt),'solver',solverTime(1:maxIt));

%% Display error on screen
disp('Table: Error')
colname = {'#Dof','h','||w_I-w_h||','||w-w_h||','||w_I-w_h||_1','||w-w_h||_1'};
disptable(colname,err.N,[],err.h,'%0.3e',err.wIwhL2,'%0.5e',err.wL2,'%0.5e',...
              err.wIwhHcurl,'%0.5e',err.wwhHcurl,'%0.5e');

colname = {'#Dof','h','||u_I-u_h||','||u-u_h||','||u_I-u_h||_1','||u-u_h||_1'};
disptable(colname,err.N,[],err.h,'%0.3e',err.uIuhL2,'%0.5e',err.uL2,'%0.5e',...
              err.uIuhHcurl,'%0.5e',err.uuhHcurl,'%0.5e');

%     display('Table: CPU time')
colname = {'#Dof','Assemble','Solve'};
disptable(colname,time.N,[],time.assemble,'%0.2e',time.solver,'%0.2e');


% Version 1.000
%
% Code provided by Ruslan Salakhutdinov and Geoff Hinton
%
% Permission is granted for anyone to copy, use, modify, or distribute this
% program and accompanying programs and documents for any purpose, provided
% this copyright notice is retained and prominently displayed, along with
% a note saying that the original programs are available from our
% web page.
% The programs and documents are distributed without any warranty, express or
% implied.  As the programs were written for research purposes only, they have
% not been tested to the degree that would be advisable in any important
% application.  All use of these programs is entirely at the user's own risk.

function [f, df] = CG_CLASSIFY(VV,Dim,XX,target);

l1 = Dim(1);
l2 = Dim(2);
l3= Dim(3);
l4= Dim(4);
l5= Dim(5);
N = size(XX,1);

% Do decomversion.
 w1 = reshape(VV(1:(l1+1)*l2),l1+1,l2);
 xxx = (l1+1)*l2;
 w2 = reshape(VV(xxx+1:xxx+(l2+1)*l3),l2+1,l3);
 xxx = xxx+(l2+1)*l3;
 w3 = reshape(VV(xxx+1:xxx+(l3+1)*l4),l3+1,l4);
 xxx = xxx+(l3+1)*l4;
 w_class = reshape(VV(xxx+1:xxx+(l4+1)*l5),l4+1,l5);

  XX = [XX ones(N,1)];
  w1probs = 1./(1 + exp(-XX*w1)); w1probs = [w1probs  ones(N,1)];
  w2probs = 1./(1 + exp(-w1probs*w2)); w2probs = [w2probs ones(N,1)];
  w3probs = 1./(1 + exp(-w2probs*w3)); w3probs = [w3probs  ones(N,1)];

  targetout = exp(w3probs*w_class);
  targetout = targetout./repmat(sum(targetout,2),1,10);
  f = -sum(sum( target(:,1:end).*log(targetout))) ;

IO = (targetout-target(:,1:end));
Ix_class=IO;
dw_class =  w3probs'*Ix_class;

Ix3 = (Ix_class*w_class').*w3probs.*(1-w3probs);
Ix3 = Ix3(:,1:end-1);
dw3 =  w2probs'*Ix3;

Ix2 = (Ix3*w3').*w2probs.*(1-w2probs);
Ix2 = Ix2(:,1:end-1);
dw2 =  w1probs'*Ix2;

Ix1 = (Ix2*w2').*w1probs.*(1-w1probs);
Ix1 = Ix1(:,1:end-1);
dw1 =  XX'*Ix1;

df = [dw1(:)' dw2(:)' dw3(:)' dw_class(:)']';


function [err,time,solver,eqn] = femPoisson3(node,elem,pde,bdFlag,option,varargin)
%% FEMPOISSON3 solve Poisson equation by various finite element methods
%
%   FEMPOISSON3 computes approximations to the Poisson equation on a
%   sequence of meshes obtained by uniform refinement of a input mesh.
%
% See also Poisson, crack, Lshape
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

%% Check input arguments
if nargin >=1 && ischar(node)
    option.elemType = node;
    clear node
end
if ~exist('node','var') || ~exist('elem','var')
    [node,elem] = cubemesh([0,1,0,1,0,1],0.25); % default mesh is a cube
end
if ~exist('option','var'), option = []; end
if ~exist('pde','var')
    pde = sincosdata3;                          % default data
end
if ~exist('bdFlag','var')
    bdFlag = setboundary3(node,elem,'Dirichlet');
end

%% Parameters
option = femoption(option);
maxIt = option.maxIt;   maxN = option.maxN; L0 = option.L0;
elemType = option.elemType; refType = option.refType;

%% Generate an initial mesh
for k = 1:L0
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect3(node,elem,bdFlag);
    end
end

%% Initialize err
errL2 = zeros(maxIt,1);   errH1 = zeros(maxIt,1);
erruIuh = zeros(maxIt,1); errMax = zeros(maxIt,1);
errTime = zeros(maxIt,1); solverTime = zeros(maxIt,1);
assembleTime = zeros(maxIt,1); meshTime = zeros(maxIt,1);
itStep = zeros(maxIt,1);  stopErr = zeros(maxIt,1); flag = zeros(maxIt,1);
N = zeros(maxIt,1);

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    switch elemType
        case 'P1'     % piecewise linear function P1 element
            [u,Du,eqn,info] = Poisson3(node,elem,pde,bdFlag,option);
        case 'CR'     % piecewise linear function CR element
            [u,Du,eqn,info] = Poisson3CR(node,elem,pde,bdFlag,option);
        case 'P2'     % piecewise quadratic function
            [u,Du,eqn,info] = Poisson3P2(node,elem,pde,bdFlag,option);
        case 'WG'     % weak Galerkin element
            [u,Du,eqn,info] = Poisson3WG(node,elem,pde,bdFlag,option);
    end
    % compute error
    tic;
    if isfield(pde,'Du')
        if ~isempty(Du)
            errH1(k) = getH1error3(node,elem,pde.Du,Du);
        else
            errH1(k) = getH1error3(node,elem,pde.Du,u);
        end
    end
    if isfield(pde,'exactu')
        errL2(k) = getL2error3(node,elem,pde.exactu,u);
        % interpolation
        switch elemType
            case 'P1'
                uI = Lagrangeinterpolate(pde.exactu,node,elem);
            case 'CR'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'CR',eqn.face);
            case 'P2'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'P2',eqn.edge);
            case 'WG'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'WG',eqn.face);
        end
        erruIuh(k) = sqrt((u-uI)'*eqn.A*(u-uI));
        errMax(k) = max(abs(u-uI));
    end
    errTime(k) = toc;
    % record time
    solverTime(k) = info.solverTime;
    assembleTime(k) = info.assembleTime;
    if option.printlevel>1
        fprintf('Time to compute the error %4.2g s \n H1 err %4.2g    L2err %4.2g \n',...
            errTime(k),errH1(k), errL2(k));
    end
    % record solver information
    itStep(k) = info.itStep;
    stopErr(k) = info.stopErr;
    flag(k) = info.flag;
    % plot
    N(k) = length(u);
    if option.plotflag && N(k) < 2e3 % show mesh and solution for small size
       figure(1);  showresult3(node,elem,u);
    end
    if N(k) > maxN
        break;
    end
    % refine mesh
    tic;
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect3(node,elem,bdFlag);
    end
    meshTime(k) = toc;
end

%% Plot convergence rates
if option.rateflag
    figure;
    set(gcf,'Units','normal');
    set(gcf,'Position',[0.25,0.25,0.55,0.4]);
    subplot(1,2,1)
    showrate2(N(1:k),errH1(1:k),1,'-*','||Du-Du_h||',...
              N(1:k),errL2(1:k),1,'k-+','||u-u_h||');
    subplot(1,2,2)
    showrate2(N(1:k),erruIuh(1:k),1,'m-+','||Du_I-Du_h||',...
              N(1:k),errMax(1:k),1,'r-*','||u_I-u_h||_{\infty}');
end

%% Output
err = struct('N',N,'H1',errH1(1:k),'L2',errL2(1:k),...
             'uIuhH1',erruIuh(1:k),'uIuhMax',errMax(1:k));
time = struct('N',N,'err',errTime(1:k),'solver',solverTime(1:k), ...
              'assmble',assembleTime(1:k),'mesh',meshTime(1:k));
solver = struct('N',N(1:k),'itStep',itStep(1:k),'time',solverTime(1:k),...
                'stopErr',stopErr(1:k),'flag',flag(1:k));

%% Display error
ts = zeros(k,3); ts = char(ts);
display(' #Dof   ||u-u_h||     ||Du-Du_h||   ||DuI-Du_h||  ||uI-u_h||_{max}');
display([num2str(err.N) ts num2str(err.L2,'%0.5e') ts num2str(err.H1,'%0.5e')...
         ts num2str(err.uIuhH1,'%0.5e') ts num2str(err.uIuhMax,'%0.5e')]);


%FxNLMS
%Marko Stamenovic
%April 28, 2016

mus = [0.01 0.05 0.1 0.5 1 2 2.5 3];% 0.5 1];
for j = 1:length(mus)
    for i = 1:100
        %%INITIALIZE VALUES%%
        % generate input signal
        muOG=mus(j); %learning rate
        M=128; %buffer size (num filter weights)
        x=randn(10000,1); %input signal
        x=x/max(x); %sample rate
        fs=8000; %number of samples of the input signal
        N=length(x); %length of input signal

        % generate known filter coefficients
        Pz=0.5*[0:127];  %linear coefficients
        %Pz=randn(128,1); %random coefficients
        ylim = max(Pz)*1.20;
        ymin = min(Pz)-.2*max(Pz);
        % generate filtered input signal == desired signal
        d=conv(Pz,x); %input signal filtered by known filter Pz (primary path)

        %% ESTIMATE SECONDARY PATH SIGNAL USING LMS %%
        %generate dummy secondary path response Sz
        Sz = Pz/2;
        %run known signal through filter
        xp=conv(Sz,x);
        %initalize Sz hat values
        Szh=zeros(M,1);

        for n=M:N
            xpvec=x(n:-1:n-M+1); %input has to be in reverse orxer has to be
            %update mu
            mu(n) = 1/(xpvec'*xpvec);
            e(n)=xp(n)-Szh'*xpvec; %update error
            %plot(e)
            Szh=Szh+mu(n)*xpvec*(e(n)); %update filter coefficient
        end
        Szh = abs(ifft(1./abs(fft(Szh))));

        %% LMS FOR MAIN ANC %%
        emean=zeros(N,1);
        %filter input by learned filter to get x prime
        xph = conv(Szh,x);
        %initalize Wz filter values
        Wz=zeros(M,1);
        %Make sure that x and d are column vectors
        x=x(:);
        d=d(:);
        %LMS
        for n=M:N
            xvec=x(n:-1:n-M+1); %input has to be in reverse orxer
            xpvec=xph(n:-1:n-M+1);
            %update mu
            mu(n) = muOG/(xvec'*xvec);
            e(n)=d(n)-Wz'*xvec; %update error
            %plot(e)
            Wz=Wz+mu(n)*xpvec*(e(n)); %update filter coefficient

            %draw the learned filter in realtime
            %         plot(Pz)
            %         hold on
            %         plot(Wz)
            %         axis([0 inf ymin ylim])
            %         title(sprintf('n=%f time=%fs error = %f',n-M, (n-M)/fs, e(n)))
            %         hold off
            %         legend('Input coefficients','Learned Coefficients')
            %         drawnow;
        end
        e=e(:);

        emean = (emean(:)+e);
    end
    emean=(emean)/i;
    if max(emean)>1e4
        for l = 1:length(emean)
            if abs(emean(l)) > 1e3
                emean(l)=1e3;
            end
        end
        eall(j,:)=emean;
    else
        try
            [eall(j,:),q]=(envelope(abs(emean),500,'peaks'));
        catch
            eall(j,:) = 1e3;
        end
    end
end
figure
for i = 1:length(mus)
    plot(10*log10(abs(eall(i,:))))
    hold on
end
title('Convergence Time in Cycles')
ylabel('Error (dB)');
xlabel('Cycles');
hleg=legend('0.01','0.05','0.1','0.5','1.0', '2.0');
htitle=get(hleg,'Title');
set(htitle,'String','mu');
% %% PLOT RESULTS %%
% figure
% subplot(2,1,1)
% plot(e)
% title('Convergence Time in Cycles')
% ylabel('Amplitude');
% xlabel('Cycles');
% legend('Error');
% subplot(2,1,2)
% stem(Pz)
% hold on
% stem(Wz, 'r*')
% title('Input Coefficients vs Learned Coefficients')
% ylabel('Amplitude');
% xlabel('Numbering of filter tap');
% legend('Input Coefficients', 'learned coefficients')


%DEMO_LGCP  Demonstration for a log Gaussian Cox process
%           with inference via EP or Laplace approximation
%
%  Description
%    Log Gaussian Cox process (LGCP) is a model for non-homogeneous
%    point-process in which the log intensity is modelled using
%    Gaussian process. LGCP can be modelled using log GP and
%    Poisson observation model in a discretized space.
%
%    The model constructed is as follows:
%
%    The number of occurrences of the realised point pattern within cell w_i
%
%         y_i ~ Poisson(y_i| |w_i|exp(f_i))
%
%    where |w_i| is area of cell w_i and f_i is the log intensity.
%
%    We place a zero mean Gaussian process prior for f =
%    [f_1, f_2,...,f_n] ~ N(0, K),
%
%    where K is the covariance matrix, whose elements are given as
%    K_ij = k(x_i, x_j | th). The function k(x_i, x_j | th) is
%    covariance function and th its parameters. We place a
%    prior for parameters, p(th).
%
%    The inference is conducted via EP or Laplace, where we find
%    Gaussian approximation for p(f| th, data), where th is the
%    maximum a posterior (MAP) estimate for the parameters.
%
%  See also  LGCP, DEMO_SPATIAL2
%
% Copyright (c) 2010 Aki Vehtari

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

% =====================================
% 1D-example
% =====================================
fprintf(['Coal disaster data with EP integration over the latent values\n'])

% Coal disaster data
S = which('demo_lgcp');
L = strrep(S,'demo_lgcp.m','demodata/coal.txt');
x = load(L);
xt = (1850:1963)';
[p1,pq1] = lgcp(x,xt,'gpcf',@gpcf_exp);

figure()
hp=patch([xt; xt(end:-1:1)],[pq1(:,1); pq1(end:-1:1,2)],[.9 .9 .9]);
set(hp,'edgecolor',[.9 .9 .9])
xlim([min(x) max(x)])
line(xt,p1,'linewidth',2);
line([x x],[5 5.3],'color','k')
line(xlim,[5.15 5.15],'color','k')
xlim([1850 1963])
ylim([0 5.29])
title('The coal mine disaster data, estimated intensity, and 90% interval')
xlabel('Year')
ylabel('Intensity')

% =====================================
% 2D-example
% =====================================
fprintf(['Redwood data with Laplace integration over the latent\n' ...
         'values and MAP estimate for the parameters\n'])

% Redwood data
S = which('demo_lgcp');
L = strrep(S,'demo_lgcp.m','demodata/redwoodfull.txt');
x=load(L);
x1min=min(x(:,1));x1max=max(x(:,1));
x2min=min(x(:,2));x2max=max(x(:,2));
[xt1,xt2]=meshgrid(linspace(x1min,x1max,100),...
                   linspace(x2min,x2max,100));
xt=[xt1(:) xt2(:)];
p2 = lgcp(x,xt,'range',[0 1 0 1],'latent_method','Laplace','gridn',20);

figure()
G=zeros(size(xt1));
G(:)=p2;
pcolor(xt1,xt2,G);
shading flat
colormap('jet')
cx=caxis;
cx(1)=0;
caxis(cx);
colorbar
h=line(x(:,1),x(:,2),'marker','.','linestyle','none','color','k','markersize',10);
colorbar
axis square
set(gca,'xtick',[0:.2:1],'ytick',[0:.2:1])
title('Redwood data and intensity estimate')


% [R,Q] = vgg_rq(S)  Just like qr but the other way around.
%
% If [R,Q] = vgg_rq(X), then R is upper-triangular, Q is orthogonal, and X==R*Q.
% Moreover, if S is a real matrix, then det(Q)>0.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.

% By awf

function [U,Q] = rq(S)

S = S';
[Q,U] = qr(S(end:-1:1,end:-1:1));
Q = Q';
Q = Q(end:-1:1,end:-1:1);
U = U';
U = U(end:-1:1,end:-1:1);

if det(Q)<0
  U(:,1) = -U(:,1);
  Q(1,:) = -Q(1,:);
end

return