%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
% This function manages the solution of H * dy = - dJ' for various settings
%
%   'backslash',         simply solve dy = H\rhs;
%   'mbCG',               matrixBased, plain CG
%   'mbPCG-SGS',         matrixBased, PCG with Symmetric Gauss Seidel preconditioner
%   'mbPCG-ICHOL',     matrixBased, PCG with incomplete Cholesky preconditioner
%   'mbPCG-Jacobi', matrixBased, PCG with Jacobi preconditioner
%
%   'MG-elastic',           matrixFree, PCG Multigrid for elastic regularizer
%   'CG-elastic',       plain CG with H = A as in (1) below
%   'CG-curvature',          plain CG with H = A as in (1) below
%   'PCG-elastic',      Jacobi preconditioned with H = A as in (1,) below
%   'PCG-curvature',      Jacobi preconditioned with H = A as in (1,) below
%   'PCG-hyperElastic', ???
%   'CG'                      plain CG with H full or as operator
%
% the operator H can be a
%    - matrix (matrixBased)
%    - function (coding the action ofH)
%    - struct where H describes the pieces of a complex H
%
%  (1) action of H: distance + regularizer,
%       H = P'*dr'*d2psi*dr*P + d2S
%
%      Hoperator = @(x) ...
%       H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x)) ...
%       + H.d2S.d2S(x,H.omega,H.m);
%
%  (2) Jacobi (diagonal) preconditing
%      Ddiag = diag(H.d2D.dr'*H.d2D.d2psi*H.d2D.dr);
%      D = H.d2D.P(full(Ddiag))  +  H.d2S.diag(H.omega,H.m);
%      PC = @(x) D.\x; % Jacobi preconditioner
%==============================================================================

function [dy,solver] = solveLinearSystem(rhs,H,solver,varargin)

if nargin == 0,
  testSolver
  help(mfilename);
  runMinimalExample;
  dy = 'endOfMinimalExample';
  return;
end;

maxIterCG = 500;
tolCG     = 1e-1;

for k=1:2:length(varargin), % overwrites default parameter
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

if isempty(solver) && isnumeric(H),
  warning('no solver specified, try backslash');
  solver = 'backslash';
end;

if isa(solver,'function_handle')
  dy     = solver(rhs,H,maxIterCG,tolCG);
  return;
end

if isstruct(H), % matrixFree mode, configure operator
  Hoperator = @(x) ...
    H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x)) ...
    + H.d2S.d2S(x,H.omega,H.m);
end;

%------------------------------------------------------------------------------
switch solver
  %------------------------------------------------------------------------------

  % ---------------------------------------------------------------------------
  % matrix based
  % ---------------------------------------------------------------------------

  case 'backslash',
    dy = H\rhs;

  case 'mbCG',
    [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG);

  case 'mbPCG-SGS',
    L   = tril(H); % Symmetric Gauss Seidel Preconditioning,
    D   = diag(H); % L is lower, D is diagonal, U = L'
    SGS = @(x) L\(D.*(L'\x));
    [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG,SGS);

  case 'mbPCG-ICHOL',
    %L1   = cholinc(sparse(H),'0');
    L = ichol(H);
    [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG,L,L');

  case 'mbPCG-Jacobi',
    D   = diag(H); % D is diagonal
    PC = @(x) D.\x;
    [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG,PC);

    % ---------------------------------------------------------------------------
    % matrix free
    % ---------------------------------------------------------------------------

    % In all matrix free solvers the approximates Hessian is supplied by function handle.
    % Note, that the functionals have the common form
    %
    %   Jc = D + S (+ P) ==> d_2 Jc = d2D + d2S (+d2P)
    %
    % regularization and penalization (S,P) supply a struct stored in H.d2S or H.d2P
    % which already has a function handle describing the operator.
    % The  distance term (d2D) needs more care and the approximation to the Hessian
    % depends on the solver as well. Therefore the objective function does NOT supply
    % the function handle, but all variables needed to built such.
    % For multi-grid (available for elastic and curvature), only the diagonal of the distance
    % term is used
    % whereas in conjugate gradient (CG, available for elastic, curvature and hyperelastic)
    % methods we can model also the off-diagonals d2D(x) = (P'*dr'*d2psi*dr*P) * x
    %
    % The default solver for a chosen regularizer is parameterized by d2S.solver.
    % However, one can overload this setting using
    %    >> MLIR(..., 'solverNPIR','myFavoriteSolver', ... );

  case {'MG-elastic','MG'}, % multigrid for elastic regularizer
    dy = MGsolver(rhs,H);

  case {'CG-elastic','CG-curvature'},
    if isstruct(H)
      %       A         = @(x) ...
      %         H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x)) ...
      %         + H.d2S.d2S(x,H.omega,H.m);
      [dy,flag,relres,iter] = pcg(Hoperator,rhs,tolCG,maxIterCG);
    else
      [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG);
    end

  case {'PCG-elastic','PCG-curvature'}

    % operator
    %     A         = @(x) ...
    %       H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x)) ...
    %       + H.d2S.d2S(x,H.omega,H.m);
    % preconditioner
    Ddiag = diag(H.d2D.dr'*H.d2D.d2psi*H.d2D.dr);
    D = H.d2D.P(full(Ddiag))  +  H.d2S.diag(H.omega,H.m);
    PC = @(x) D.\x; % Jacobi preconditioner
    [dy,flag,relres,iter] = pcg(Hoperator,rhs,tolCG,maxIterCG,PC);

  case 'CG'
    if isstruct(H)
      [dy,flag,relres,iter] = pcg(H.operator,rhs,tolCG,maxIterCG);
    else
      [dy,flag,relres,iter] = pcg(H,rhs,tolCG,maxIterCG);
    end

  case {'PCG-hyperElastic'}
    M         = @(x) H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x));
    Hoperator = @(x) M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
    Ddiag     = diag(H.d2D.dr'*H.d2D.d2psi*H.d2D.dr);
    D         = H.d2D.P(full(Ddiag))  +  H.d2S.diag(H.d2S.yc);
    Preconditioner = @(x) D.\x; % Jacobi preconditioner
    [dy,flag,relres,iter] = pcg(Hoperator,rhs,tolCG,maxIterCG,Preconditioner);

  otherwise,
    keyboard
    error(1)

    %------------------------------------------------------------------------------
end
%------------------------------------------------------------------------------

if exist('flag','var')
  flags = {
    sprintf('iter=%d>maxIter=%d but relres=%e>relResTol=%s',...
    iter,maxIterCG,relres,tolCG)
    'preconditioner is ill-conditioned'
    'stagnation: x_k=x_{k+1}'
    'scalars too small/large to continue computation'
    };
  switch flag
    case {1,2,3,4}
      fprintf('%s // PCG: %s\n',flags{flag});
    otherwise
      fprintf('%s // PCG :iter = %d of %d, relRes= %1.2e, tolCG= %1.2e\n',...
        mfilename,iter,maxIterCG,relres,tolCG);
  end
end

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

function runMinimalExample

%% prepare 3D data
setup3DbrainData
level = 4;
omega = ML{level}.omega;
m     = ML{level}.m;

viewImage('reset','viewImage','viewImage2D','colormap',bone(256),'axis','off');
imgModel('reset','imgModel','splineInterMex','regularizer','moments','theta',1e-2);
distance('reset','distance','SSD');
trafo('reset','trafo','affine3Dsparse');

[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega,'out',0);
xc = getCellCenteredGrid(omega,m);
Rc = imgModel(R,omega,xc);
w0   = trafo('w0');
beta = 0; M = []; wRef = [];
[Jc,para,dJPIRsparse,HPIRsparse] = PIRobjFctn(T,Rc,omega,m,beta,M,wRef,xc,w0);

%% prepare 2D data and approximations to Hessian
setup2DhandData

viewImage('reset','viewImage','viewImage2D','colormap',bone(256),'axis','off');
imgModel('reset','imgModel','splineInter','regularizer','moments','theta',1e-2);
distance('reset','distance','SSD');

level = 4;
omega = ML{level}.omega;
m     = ML{level}.m;
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega,'out',0);

% initialize the image model, distance measure and regularizer
xc    = getCellCenteredGrid(omega,m);
Rc    = imgModel(R,omega,xc);

% setup PIR objective
w0   = trafo('w0');
beta = 1e3;
M    = diag([4,4,4,4,1,1]);

trafo('reset','trafo','affine2D');
w0   = trafo('w0');
wRef = w0;
[Jc,para,dJPIR,HPIR] = PIRobjFctn(T,Rc,omega,m,beta,M,wRef,xc,w0);

% setup NPIR objective
yc   = getStaggeredGrid(omega,m);
yRef = 0*yc;

regularizer('reset','regularizer','mbElastic','alpha',1e4,'mu',1,'lambda',0);
[Jc,para,dJmbElas,HmbElas] = NPIRobjFctn(T,Rc,omega,m,yRef,yc);

regularizer('reset','regularizer','mfElastic','alpha',1e4,'mu',1,'lambda',0);
[Jc,para,dJmfElas,HmfElas] = NPIRobjFctn(T,Rc,omega,m,yRef,yc);

HxElas = @(x) HmfElas.d2D.P((HmfElas.d2D.dr'*HmfElas.d2D.d2psi*HmfElas.d2D.dr)*HmfElas.d2D.P(x)) ...
  + HmfElas.d2S.d2S(x,HmfElas.omega,HmfElas.m);

yc   = getCellCenteredGrid(omega,m);
yRef = 0*yc;

regularizer('reset','regularizer','mbCurvature','alpha',1e4);
[Jc,para,dJmbCurv,HmbCurv] = NPIRobjFctn(T,Rc,omega,m,yRef,yc);

regularizer('reset','regularizer','mfCurvature','alpha',1e4);
[Jc,para,dJmfCurv,HmfCurv] = NPIRobjFctn(T,Rc,omega,m,yRef,yc);
HxCurv = @(x) HmfCurv.d2D.P((HmfCurv.d2D.dr'*HmfCurv.d2D.d2psi*HmfCurv.d2D.dr)*HmfCurv.d2D.P(x)) ...
  + HmfCurv.d2S.d2S(x,HmfCurv.omega,HmfCurv.m);

problems = {
  'PIR-backslash'
  'PIR-sparse'
  'mbElastic-backslash'
  'mbElastic-PCG-SGS'
  'mbElastic-PCG-ICHOL'
  'mbElastic-PCG-Jacobi'
  'mbElastic-CG'
  'mfElastic-MG'
  'mfElastic-CG'
  'mfElastic-PCG'
  'mbCurvature-backslash'
  'mbCurvature-PCG-SGS'
  'mbCurvature-PCG-ICHOL'
  'mbCurvature-PCG-Jacobi'
  'mbCurvature-CG'
  'mfCurvature-PCG'
  'mfCurvature-CG'
  }

for j = 1:length(problems)

  para = {};

  switch problems{j},

    case 'PIR-backslash',
      solver = 'backslash';
      H = HPIR; b = -dJPIR'; Hx = @(y) H*y;

    case 'PIR-sparse',
      solver = 'backslash';
      H = HPIRsparse; b = -dJPIRsparse'; Hx = @(y) H*y;

      %     case 2, % PRIR with conensed matrix
      % % fprintf('test PIR with condensed matrix\n');
      % % trafo('reset','trafo','affine2Dsparse');
      % % [Jc,para,dJ,H] = PIRobjFctn(T,Rc,omega,m,beta,M,wRef,xc,w0)
      % % dy = solveLinearSystem(-dJ',H,'backslash');
      % % test2 = norm(H*dy + dJ')

    case 'mbElastic-backslash'
      solver = 'backslash';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;

    case 'mbElastic-PCG-SGS',
      solver = 'mbPCG-SGS';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbElastic-PCG-ICHOL',
      solver = 'mbPCG-ICHOL';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbElastic-PCG-Jacobi'
      solver = 'mbPCG-Jacobi';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbElastic-CG'
      solver = 'mbCG';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mfElastic-MG'
      solver = 'MG-elastic';
      H = HmfElas; b = -dJmfElas'; Hx = HxElas;

    case 'mfElastic-CG'
      solver = 'CG-elastic';
      H = HmfElas; b = -dJmfElas'; Hx = HxElas;

    case 'mfElastic-PCG'
      solver = 'PCG-elastic';
      H = HmfElas; b = -dJmfElas'; Hx = HxElas;

    case 'mbCurvature-backslash'
      solver = 'backslash';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;

    case 'mbCurvature-PCG-SGS',
      solver = 'mbPCG-SGS';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbCurvature-PCG-ICHOL',
      solver = 'mbPCG-ICHOL';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbCurvature-PCG-Jacobi'
      solver = 'mbPCG-Jacobi';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mbCurvature-CG'
      solver = 'mbCG';
      H = HmbElas; b = -dJmbElas'; Hx = @(y) H*y;
      para = {'tolCG',1e-5};

    case 'mfCurvature-PCG'
      solver = 'PCG-curvature';
      H = HmfElas; b = -dJmfElas'; Hx = HxElas;

    case 'mfCurvature-CG'
      solver = 'CG-curvature';
      H = HmfElas; b = -dJmfElas'; Hx = HxElas;

    case 'MG'

    otherwise,
      error('1');
  end;

  dy = solveLinearSystem(b,H,solver,para{:});
  test = norm(Hx(dy)-b)/norm(b);
  fprintf('%-2d of %2d: %-40s test = %s\n',j,length(problems),...
    sprintf('test <%s>:',problems{j}),num2str(test));
end;

return;
%==============================================================================

%   case 'mfCurvature',
%     Afun     = @(dy) mfAy(dy,H);
%     [dy,FLAG] = pcg(Afun,rhs,tolCG,maxIterCG);
%     %

%
%   if isnumeric(H),
%
%   elseif isstruct(H),
%     if isfield(H,'solver'),
%       solver = H.solver;
%     elseif isfield(H,'d2S') && isfield(H.d2S,'solver'),
%       solver = H.d2S.solver;
%     else
%       error('solver has not been defined')
%     end;
%   else
%     keyboard
%   end;
%   fprintf('[set solver to <%s> in %s]\n',solver,mfilename);
% end;
% if isempty(solver)
%   if isstruct(H),
%   else % no regularizer initialized, assuming PIR
%     dy = H\rhs;
%     return;
%   end;
% end;
%
% if isa(solver, 'function_handle')
%     dy = feval(solver, rhs, H, maxIterCG, tolCG);
%     return
% end

%
%     %     case {'Joint-CG-hyperElastic'}
%     %         Afctn = @(x) H.M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
%     %         [dy,flag,relres,iter] = pcg(Afctn,rhs,tolCG,maxIterCG);
%     %     case {'CG-hyperElastic'}
%     %         M =@(x) H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x));
%     %         Afctn = @(x) M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
%     %         [dy,flag,relres,iter] = pcg(Afctn,rhs,tolCG,maxIterCG);
%     %     otherwise
%     %         if isnumeric(H)
%     %             % if H is a matrix, solve the linear system using MATLAB's backslash
%     %             dy = H\rhs;
%     %         else
%     %             error(solver)
%     %         end
%   otherwise,
%     keyboard
%     error(1)
%     %
%   case {'PCG-hyperElastic'}
%     M =@(x) H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x));
%     Afctn = @(x) M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
%     Ddiag = diag(H.d2D.dr'*H.d2D.d2psi*H.d2D.dr);
%     D = H.d2D.P(full(Ddiag))  +  H.d2S.diag(H.d2S.yc);
%     PC = @(x) D.\x; % Jacobi preconditioner
%     [dy,flag,relres,iter] = pcg(Afctn,rhs,tolCG,maxIterCG,PC);
%
%     %   case {'PCG-elastic','PCG-curvature'}
%     %     % operator
%     %     A         = @(x) ...
%     %       H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x)) ...
%     %       + H.d2S.d2S(x,H.omega,H.m);
%     %     % preconditioner
%     %     Ddiag = diag(H.d2D.dr'*H.d2D.d2psi*H.d2D.dr);
%     %     D = H.d2D.P(full(Ddiag))  +  H.d2S.diag(H.omega,H.m);
%     %     PC = @(x) D.\x; % Jacobi preconditioner
%     %     [dy,flag,relres,iter] = pcg(A,rhs,tolCG,maxIterCG,PC);
%
%
%     %     case {'Joint-CG-hyperElastic'}
%     %         Afctn = @(x) H.M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
%     %         [dy,flag,relres,iter] = pcg(Afctn,rhs,tolCG,maxIterCG);
%     %     case {'CG-hyperElastic'}
%     %         M =@(x) H.d2D.P((H.d2D.dr'*H.d2D.d2psi*H.d2D.dr)*H.d2D.P(x));
%     %         Afctn = @(x) M(x) + H.d2S.d2S(x,H.omega,H.m,H.d2S.yc);
%     %         [dy,flag,relres,iter] = pcg(Afctn,rhs,tolCG,maxIterCG);
%     %     otherwise
%     %         if isnumeric(H)
%     %             % if H is a matrix, solve the linear system using MATLAB's backslash
%     %             dy = H\rhs;
%     %         else
%     %             error(solver)
%     %         end


% =========================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% Example illustrating the performance of different preconditioners for
% the Gauss-Newton system in LDDMM. The example is described in detail in
% Section 4.2 of the paper:
%
% @article{MangRuthotto2017,
%   Title = {A {L}agrangian {G}auss--{N}ewton--{K}rylov solver for mass- and intensity-preserving diffeomorphic image registration},
%   Year = {2017},
%   Journal = {SIAM Journal on Scientific Computing},
%   Author = {A. Mang, L. Ruthotto},
% }
%
% =========================================================================
clear all; clc;
setup2Ddisc2CData

%% run affine pre-registration
imgModel('reset','imgModel','splineInterMex','regularizer','moments','theta',.1);
        trafo('set','trafo','affine2D');
        distance('set','distance','SSD');

alpha = [6e2 0];
parametric = 0;
pad  = 0.5;
nt = 0;
N    = 3;
mV     = @(m) ceil(1*m);
minLevel = 5;
maxLevel = 5;

%% run multilevel LDDMM registration

% 1) setup grid for velocities (padded)
omegaV = omega; omegaV(1:2:end) = omegaV(1:2:end)-pad;  omegaV(2:2:end) = omega(2:2:end)+pad;

% 2) setup regularizer
regularizer('reset','regularizer','mfDiffusionCC','alpha',alpha,'nt',nt,'HessianShift',1e-2); % stationary velocity

NPIRpara    = optPara('NPIR-GN');
NPIRpara.maxIter = 40;
NPIRpara.scheme = @GaussNewtonLDDMM;
[vc,~,wc,his] = MLLDDMM(ML,'minLevel',minLevel,'maxLevel',maxLevel,...
    'omegaV',omegaV,'mV',mV,'N',N,'parametric',parametric,'NPIRpara',NPIRpara,'plots',1);

%% generate plots to explore conditioning / sparsity
xc = getNodalGrid(omega,ML{minLevel}.m);
[T,R] = imgModel('coefficients',ML{minLevel}.T,ML{minLevel}.R,omega);
Rc    = imgModel(R,omega,center(xc,ML{minLevel}.m));

mV = ML{minLevel}.m;
fctn = @(vc) LDDMMobjFctn(T,Rc,omega,ML{minLevel}.m,0*vc,center(xc,ML{minLevel}.m),omegaV,mV,N,vc);

close all
regularizer('set','regularizer','mbDiffusionCC')
[Sc,dS,d2Smb] = regularizer(vc,omegaV,mV);
% get objective function
[J0,p0,dJ0,Hmb0] = fctn(0*vc);
[Jc,pc,dJc,Hmbc] = fctn(vc);

regularizer('set','regularizer','mfDiffusionCC')
[J0,p0,dJ0,Hmf0] = fctn(0*vc);
[Jc,pc,dJc,Hmfc] = fctn(vc);

%%
fig = figure(); clf;
fig.Name =sprintf('%s: Sparsity of Hessian',mfilename);

subplot(1,2,1)
spy(Hmb0);
title('H(v0)');

subplot(1,2,2);
spy(Hmbc);
title('H(vOpt');
%% explore PCG convergence at first iteration
hd = prod((omega(2:2:end)-omega(1:2:end))./ML{minLevel}.m);
Hmb0 = Hmb0 + hd* regularizer('get','HessianShift')* speye(size(Hmb0));
D   = diag(Hmb0); % D is diagonal
L   = tril(Hmb0); % Symmetric Gauss Seidel Preconditioning,

PCjac = @(x) D.\x;
PCsgs = @(x) full(L\(D.*(L'\x)));
PCnone = @(x) x;

[~,iter0cg,relres0cg,resvec0cg] = spectralPrecondPCG(-dJ0(:), Hmf0, 250, 1e-10,'prec',PCnone);
[~,iter0jac,relres0jac,resvec0jac] = spectralPrecondPCG(-dJ0(:), Hmf0, 250, 1e-10,'prec',PCjac);
[~,iter0sgs,relres0sgs,resvec0sgs] = spectralPrecondPCG(-dJ0(:), Hmf0, 250, 1e-10,'prec',PCsgs);
[~,iter0sp,relres0sp,resvec0sp] = spectralPrecondPCG(-dJ0(:), Hmf0, 250, 1e-10);

%%
figConv = figure(); clf
figConv.Name = 'PCG convergence';

subplot(1,2,1);
semilogy(resvec0cg/resvec0cg(1));
hold on;
semilogy(resvec0jac/resvec0jac(1));
semilogy(resvec0sgs/resvec0sgs(1));
semilogy(resvec0sp/resvec0sp(1));
title('first GN iteration');
ax = [0 250 1e-10 10];
axis(ax)
legend('CG','PCG-Jac','PCG-SGS','PCG-Spec','Location','SouthWest')

%% explore PCG convergence at final iteration
hd = prod((omega(2:2:end)-omega(1:2:end))./ML{minLevel}.m);
Hmb0 = Hmb0 + hd* regularizer('get','HessianShift')* speye(size(Hmb0));
D   = diag(Hmbc); % D is diagonal
L   = tril(Hmbc); % Symmetric Gauss Seidel Preconditioning,

linSol = @spectralPrecondPCG;
PCjac = @(x) D.\x;
PCsgs = @(x) full(L\(D.*(L'\x)));
PCnone = @(x) x;

[~,itercg,relrescg,resveccg]    = linSol(-dJc(:), Hmfc, 250, 1e-10,'prec',PCnone);
[~,iterjac,relresjac,resvecjac] = linSol(-dJc(:), Hmfc, 250, 1e-10,'prec',PCjac);
[~,itersgs,relressgs,resvecsgs] = linSol(-dJc(:), Hmfc, 250, 1e-10,'prec',PCsgs);
[~,itersp,relressp,resvecsp]    = linSol(-dJc(:), Hmfc, 250, 1e-10);

%%
fig = figure(figConv);
subplot(1,2,2);
semilogy(resveccg/resveccg(1));
hold on;
semilogy(resvecjac/resvecjac(1));
semilogy(resvecsgs/resvecsgs(1));
semilogy(resvecsp/resvecsp(1));
title('final iteration');

legend('CG','PCG-Jac','PCG-SGS','PCG-Spec','Location','SouthWest')
axis(ax)


function temp_obj_eval = my_compute_precision_recall_fmeasure(u, u0_GT, u0_SKL_GT)
% 121227: Reza FARRAHI MOGHADDAM and Hossein ZIAEI NAFCHI
% 090519: Reza FARRAHI MOGHADDAM
% used in objective_evaluation_core.m

%
[xm ym] = size(u);
% figure, imshow(u)

%
if (numel(u0_GT) == 0)
    u0_GT = NaN * ones([xm ym]);
end

%
if (nargin == 2)
    % u0_SKL_GT = NaN * ones([xm ym]);
	u0_SKL_GT = ~bwmorph(~u0_GT, 'thin', 'inf');
end

% TP pixels
temp_tp = [u == 0] & [u0_GT == 0];

% FP pixels
temp_fp = [u == 0] & [u0_GT ~= 0];

% FN pixels
temp_fn = [u ~= 0] & [u0_GT == 0];

% TN pixels
temp_tn = [u ~= 0] & [u0_GT ~= 0];

% SKL TP / FN pixels
temp_skl_tp = [u == 0] & [u0_SKL_GT == 0];
temp_skl_fp = [u == 0] & [u0_SKL_GT ~= 0];
temp_skl_fn = [u ~= 0] & [u0_SKL_GT == 0];
temp_skl_tn = [u ~= 0] & [u0_SKL_GT ~= 0];

% counts
count_tp = sum(sum(temp_tp));
count_fp = sum(sum(temp_fp));
count_fn = sum(sum(temp_fn));
count_tn = sum(sum(temp_tn));
count_skl_tp = sum(sum(temp_skl_tp));
count_skl_fp = sum(sum(temp_skl_fp));
count_skl_fn = sum(sum(temp_skl_fn));
count_skl_tn = sum(sum(temp_skl_tn));

% precision
temp_p = count_tp / (count_fp + count_tp);

% recall
temp_r = count_tp / (count_fn + count_tp);
if (temp_r == 0)
    temp_r = NaN;
end

% p-recall
temp_pseudo_p = count_skl_tp / (count_skl_fp + count_skl_tp);
temp_pseudo_r = count_skl_tp / (count_skl_fn + count_skl_tp);
if (temp_pseudo_r == 0)
    temp_pseudo_r = NaN;
end

% f-measure
temp_f = 100 * 2 * (temp_p * temp_r) / (temp_p + temp_r);

% p-f-measure
temp_pseudo_f = 100 * 2 * (temp_p * temp_pseudo_r) / (temp_p + temp_pseudo_r);

% sensetivity
temp_sens = count_tp / (count_tp + count_fn);
if (temp_sens == 0)
    temp_sens = NaN;
end

% specificity
temp_spec = count_tn / (count_tn + count_fp);
if (temp_spec == 0)
    temp_spec = NaN;
end

% BCR: Balanced Classification Rate
temp_BCR = 0.5 * (temp_sens + temp_spec);

% AUC: Area Under the Curve
temp_AUC = 0.5 * (temp_sens + temp_spec);

% BER: Balanced Error Rate
temp_BER = 100 * (1 - temp_BCR);

% S-F-measure: harmonic mean of sensetivity and specificity
temp_s_f_measure = 100 * 2 * (temp_sens * temp_spec) / (temp_sens + temp_spec);

% Accuracy: mean of sensetivity and specificity
temp_accu = (count_tp + count_tn) / (count_tp + count_tn + count_fp + count_fn);

% gAccuracy: Geometric mean of sensetivity and specificity
temp_g_accu = sqrt(temp_sens * temp_spec);

% NRM (Negative Rate Metric) (*10^-2)
NR_FN = count_fn / (count_fn + count_tp);
NR_FP = count_fp / (count_fp + count_tn);
temp_NRM = (NR_FN + NR_FP) / 2;

% PSNR
err=sum(sum(temp_fp | temp_fn)) / (xm * ym);
temp_PSNR = 10 * log10( 1 / err);

% DRD: Distance Reciprocal Distortion Metric
blkSize=8; % even number
MaskSize=5; % odd number
u0_GT1 = false(xm + 2, ym + 2);
u0_GT1(2 : xm + 1, 2 : ym + 1) = u0_GT;
intim = cumsum(cumsum(u0_GT1, 1), 2);
NUBN = 0; blkSizeSQR = blkSize ^ 2;
for i= 2 : blkSize : (xm - blkSize + 1)
    for j = 2 : blkSize : (ym - blkSize + 1)
        blkSum=intim(i + blkSize - 1, j + blkSize - 1) - intim(i - 1, j + blkSize - 1) - intim(i + blkSize - 1, j - 1) + intim(i - 1, j - 1);
        if blkSum == 0 || blkSum == blkSizeSQR
        else
            NUBN = NUBN + 1;
        end
    end
end

wm = zeros(MaskSize, MaskSize);
ic = (MaskSize + 1) / 2; jc = ic; % center coordinate
for i = 1 : MaskSize
    for j = 1 : MaskSize
        wm(i, j) = 1 / (sqrt((i - ic) .^ 2 + (j - jc) .^ 2));
    end
end
wm(ic, jc) = 0;
wnm = wm ./ (sum(wm(:))); % Normalized weight matrix

u0_GT_Resized = zeros(xm + ic + 1, ym + jc + 1);
u0_GT_Resized(ic : xm + ic - 1, jc : ym + jc - 1) = u0_GT;
u_Resized = zeros(xm + ic + 1, ym + jc + 1);
u_Resized(ic : xm + ic - 1, jc : ym + jc - 1) = u;
temp_fp_Resized = [u_Resized == 0] & [u0_GT_Resized ~= 0];
temp_fn_Resized = [u_Resized ~= 0] & [u0_GT_Resized == 0];
Diff = temp_fp_Resized | temp_fn_Resized;
[xm2 ym2] = size(Diff);
SumDRDk = 0;
for i = ic : xm2 - ic + 1
    for j = jc : ym2 - jc + 1
        if Diff(i, j) == 1
            Local_Diff = my_xor_infile(u0_GT_Resized(i - ic + 1 : i + ic -1 , j - ic + 1 : j + ic - 1), u_Resized(i, j));
            DRDk = sum(sum(Local_Diff.* wnm));
            SumDRDk = SumDRDk + DRDk;
        end
    end
end
temp_DRD = SumDRDk / NUBN;

% MPM: Misclassification penalty metric
Contour = bwmorph(~u0_GT, 'remove');
Dist = bwdist(Contour, 'Chessboard');
D = sum(Dist(:));
MP_FN = sum(Dist(temp_fn)) / D;
MP_FP = sum(Dist(temp_fp)) / D;
temp_MPM = (MP_FN + MP_FP) / 2;

% output
%
temp_obj_eval.Precision = temp_p;
temp_obj_eval.Recall = temp_r;
temp_obj_eval.Fmeasure = temp_f;
temp_obj_eval.P_Precision = temp_pseudo_p;
temp_obj_eval.P_Recall = temp_pseudo_r;
temp_obj_eval.P_Fmeasure = temp_pseudo_f;
temp_obj_eval.Sensitivity = temp_sens;
temp_obj_eval.Specificity = temp_spec;
temp_obj_eval.BCR = temp_BCR;
temp_obj_eval.AUC = temp_AUC;
temp_obj_eval.BER = temp_BER;
temp_obj_eval.SFmeasure = temp_s_f_measure;
temp_obj_eval.Accuracy = temp_accu;
temp_obj_eval.GAccuracy = temp_g_accu;
temp_obj_eval.NRM = temp_NRM;
temp_obj_eval.PSNR = temp_PSNR;
temp_obj_eval.DRD = temp_DRD;
temp_obj_eval.MPM = temp_MPM;

end

function temp_xor_infile = my_xor_infile(u_infile, u0_GT_infile)
% Reza

temp_fp_infile = [u_infile == 0] & [u0_GT_infile ~= 0];
temp_fn_infile = [u_infile ~= 0] & [u0_GT_infile == 0];
temp_xor_infile = temp_fp_infile | temp_fn_infile;

end


function [x,g,xn,gg] = kmeanhar(d,k,l,e,x0)
%KMEANS Vector quantisation using K-harmonic means algorithm [X,G,XN,GG]=(D,K,L,E,X0)
%
%  Inputs:
%
%    D(N,P)  contains N data vectors of dimension P
%    K       is number of centres required
%    L       integer portion is max loop count, fractional portion
%            gives stopping threshold as fractional reduction in performance criterion
%    E       is exponent in the cost function. Significantly faster if this is an even integer. [default 4]
%    X0(K,P) are the initial centres (optional)
%            Alternatively, X0 can be a character determining the initialization method:
%                'f'    Initialize with K randomly selected data points [default]
%                'p'    Initialize with centroids and variances of random partitions
%
%  Outputs:
%
%    X(K,P)  is output row vectors
%    G       is the final performance criterion value (normalized by N)
%    XN      nearest centre for each input point
%    GG(L+1) value of performance criterion before each iteration and at end
%
% The k-harmonic means algorithm selects K cluster centres to minimize
%                           sum_n(K/sum_k((d_n-x_k)^-e))
% where sum_n is over the N inputs points d_n and sum_k is over the K cluster centres x_k.
%
% It is often a good idea to scale the input data so that it has equal variance in each
% dimension before calling KMEANHAR so that approximately equal weight is given
% to each dimension in the distance calculation.

%  [1] Bin Zhang, "Generalized K-Harmonic Means - Boosting in Unsupervised Learning",
%      Hewlett-Packartd Labs, Technical Report HPL-2000-137, 2000 [Zhang2000]
%      http://www.hpl.hp.com/techreports/2000/HPL-2000-137.pdf

%  Bugs:
%      (1) Could use nested blocking to allow very large data arrays
%      (2) Could then allow incremental calling with partial data arrays (but messy)

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: kmeanhar.m 713 2011-10-16 14:45:43Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% sort out the input arguments

if nargin<5
    x0='f';
    if nargin<4
        e=[];
        if nargin<3
            l=[];
        end
    end
end
if isempty(e)
    e=4;  % default value
end
if isempty(l)
    l=50+1e-3; % default value
end
sd=5;       % number of times we must be below threshold

% split into chunks if there are lots of data points

memsize=voicebox('memsize');
[n,p] = size(d);
nb=min(n,max(1,floor(memsize/(8*p*k))));    % block size for testing data points
nl=ceil(n/nb);                  % number of blocks

% initialize if X0 argument is not supplied

if ischar(x0)
    if k<n
        if any(x0=='p')                  % Initialize using a random partition
            ix=ceil(rand(1,n)*k);       % allocate to random clusters
            ix(rnsubset(k,n))=1:k;      % but force at least one point per cluster
            x=zeros(k,p);
            for i=1:k
                x(i,:)=mean(d(ix==i,:),1);
            end
        else                                % Forgy initialization: choose k random points [default]
            x=d(rnsubset(k,n),:);         % sample k centres without replacement
        end
    else
        x=d(mod((1:k)-1,n)+1,:);    % just include all points several times
    end
else
    x=x0;
end
eh=e/2;
th=l-floor(l);
l=floor(l)+(nargout>1);   % extra loop needed to calculate final performance value
if l<=0
    l=100;      % max number of iterations ever
end
if th==0
    th=-1;      % prevent any stopping if l has no fractional part
end
gg=zeros(l+1,1);
im=repmat(1:k,1,nb); im=im(:);

% index arrays for replication

wk=ones(k,1);
wp=ones(1,p);
% wn=ones(1,n);
%
% % Main calculation loop
%
% We have the following relationships to [1] where i and k index
% the data values and cluster centres respectively:
%
%   This program     [Zhang2000]                            Equation
%
%     d(i,:)            x_i                                 input data
%     x(k,:)            m_k                                 cluster centres
%     py(k,i)           (d_ik)^2
%     dm(i)'            d_i,min^2
%     pr(k,i)           (d_i,min/d_ik)^2
%     pe(k,i)           (d_i,min/d_ik)^p                    (7.6)
%     qik(k,i)          q_ik                                (7.2)
%     qk(k)             q_k                                 (7.3)
%     qik(k,i)./qk(k)   p_ik                                (7.4)
%     se(i)'            d_i,min^p * sumk(d_ik^-p)
%     xf(i)'            d_i,min^-2 / sumk(d_ik^-p)
%     xg(i)'            d_i,min^-(p+2) / sumk(d_ik^-p)^2

ss=sd+1;        % one extra loop at the start
g=0;                % dummy initial value of g
xn=zeros(n,1);
for j=1:l

    g1=g;                           % save old performance
    x1=x;                           % save old centres
    % first do partial chunk

    jx=n-(nl-1)*nb;
    ii=1:jx;
    kx=repmat(ii,k,1);
    km=repmat(1:k,1,jx);
    py=reshape(sum((d(kx(:),:)-x(km(:),:)).^2,2),k,jx);
    [dm,xn(ii)]=min(py,[],1);                 % min value in each column gives nearest centre
    dmk=dm(wk,:);                   % expand into a matrix
    dq=py>dmk;                      % update only these values
    pr=ones(k,jx);                   % leaving others at 1
    pr(dq)=dmk(dq)./py(dq);            % ratio of min(py)./py
    pe=pr.^eh;
    se=sum(pe,1);
    xf=dm.^(eh-1)./se;
    g=xf*dm.';                     % performance criterion (divided by k)
    xg=xf./se;
    qik=xg(wk,:).*pe.*pr;           % qik(k,i) is equal to q_ik in [Zhang2000]
    qk=sum(qik,2);
    xs=qik*d(ii,:);
    ix=jx+1;
    for il=2:nl
        jx=jx+nb;        % increment upper limit
        ii=ix:jx;
        kx=ii(wk,:);
        py=reshape(sum((d(kx(:),:)-x(im,:)).^2,2),k,nb);
        [dm,xn(ii)]=min(py,[],1);                 % min value in each column gives nearest centre
        dmk=dm(wk,:);                   % expand into a matrix
        dq=py>dmk;                      % update only these values
        pr=ones(k,nb);                   % leaving others at 1
        pr(dq)=dmk(dq)./py(dq);            % ratio of min(py)./py
        pe=pr.^eh;
        se=sum(pe,1);
        xf=dm.^(eh-1)./se;
        g=g+xf*dm.';                     % performance criterion (divided by k)
        xg=xf./se;
        qik=xg(wk,:).*pe.*pr;           % qik(k,i) is equal to q_ik in [Zhang2000]
        qk=qk+sum(qik,2);
        xs=xs+qik*d(ii,:);
        ix=jx+1;
    end
    gg(j)=g;
    x=xs./qk(:,wp);
    if g1-g<=th*g1
        ss=ss-1;
        if ~ss break; end  %  stop if improvement < threshold for sd consecutive iterations
    else
        ss=sd;
    end
end
gg=gg(1:j)*k/n;                       % scale and trim the performance criterion vector
g=g(end);
% gg' % *** DEBUIG ***
if nargout>1
    x=x1;                               % go back to the previous x values if G and/or XN value is output
end


function [F,V]=mesh2tri(X,Y,Z,tri_type)

% function [F,V]=mesh2tri(X,Y,Z,tri_type)
% ------------------------------------------------------------------------
%
% This function converts a regular mesh defined by X,Y and Z into a regular
% triangulation. The output is patch data (triangles) in the faces �F� and
% vertices �V� format. The quadrilateral mesh faces are converted to
% triangles by splitting the faces into triangles according to the setting
% tri_type:
%   tri_type ='f' -> forward slash division of quadrilateral
%   tri_type ='b' -> back slash division of quadrilateral
%   tri_type ='x' -> Cross division of quadrilateral
%
% The output coordinates "V" are in the form of V=[X(:),Y(:),Z(:)];
% For forward and back slash subdivision no extra coordinates are
% introduced and therefore the original meshgrid formatted coordinates can
% still be used for plotting, see examples below.
% For cross division extra points are created at the centre of each
% quadrilateral face using the mean of the input coordinates. The extra
% coordinates are the last prod(size(X)-1) points (e.g.
% V((numel(X)+1):end,:) )and can therefore be replaced by interpolated
% coordinates if desired, see example.
%
%
% %% EXAMPLE
% clear all; close all; clc;
%
% [X,Y] = meshgrid(linspace(-10,10,25));
% Z = sinc(sqrt((X/pi).^2+(Y/pi).^2));
%
% figure('units','normalized','Position',[0 0 1 1],'Color','w'); colordef('white');
% subplot(2,2,1);
% surf(X,Y,Z); hold on;
% axis tight; axis square; grid on; axis off; view(3); view(-30,70);
% title('Meshgrid','FontSize',20);
%
% [F,V]=mesh2tri(X,Y,Z,'f');
% C=V(:,3); C=mean(C(F),2);
% subplot(2,2,2);
% patch('Faces',F,'Vertices',V,'FaceColor','flat','CData',C); hold on;
% axis tight; axis square; grid on; axis off; view(3); view(-30,70);
% title('Forward slash','FontSize',20);
%
% [F,V]=mesh2tri(X,Y,Z,'b');
% C=V(:,3); C=mean(C(F),2);
% subplot(2,2,3);
% Example of using original meshgrid coordinates instead
% trisurf(F,X,Y,Z);
% axis tight; axis square; grid on; axis off; axis off; view(3); view(-30,70);
% title('Back slash','FontSize',20);
%
% [F,V]=mesh2tri(X,Y,Z,'x');
% Replace Z-coordinates of added points by interpolated values if desired
% IND=(numel(X)+1):size(V,1);
% ZI = interp2(X,Y,Z,V(IND,1),V(IND,2),'cubic');
% V(IND,3)=ZI;
%
% C=V(:,3); C=mean(C(F),2);
% subplot(2,2,4);
% patch('Faces',F,'Vertices',V,'FaceColor','flat','CData',C); hold on;
% axis tight; axis square; grid on; axis off; view(3); view(-30,70);
% title('Crossed','FontSize',20);
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 15/07/2010
%------------------------------------------------------------------------

[J,I]=meshgrid(1:1:size(X,2)-1,1:1:size(X,1)-1);

switch tri_type
    case 'f'%Forward slash
        TRI_I=[I(:),I(:)+1,I(:)+1;  I(:),I(:),I(:)+1];
        TRI_J=[J(:),J(:)+1,J(:);   J(:),J(:)+1,J(:)+1];
        F = sub2ind(size(X),TRI_I,TRI_J);
    case 'b'%Back slash
        TRI_I=[I(:),I(:)+1,I(:);  I(:)+1,I(:)+1,I(:)];
        TRI_J=[J(:)+1,J(:),J(:);   J(:)+1,J(:),J(:)+1];
        F = sub2ind(size(X),TRI_I,TRI_J);
    case 'x'%Cross
        TRI_I=[I(:)+1,I(:);  I(:)+1,I(:)+1;  I(:),I(:)+1;    I(:),I(:)];
        TRI_J=[J(:),J(:);    J(:)+1,J(:);    J(:)+1,J(:)+1;  J(:),J(:)+1];
        IND=((numel(X)+1):numel(X)+prod(size(X)-1))';
        F = sub2ind(size(X),TRI_I,TRI_J);
        F(:,3)=repmat(IND,[4,1]);
        Fe_I=[I(:),I(:)+1,I(:)+1,I(:)]; Fe_J=[J(:),J(:),J(:)+1,J(:)+1];
        Fe = sub2ind(size(X),Fe_I,Fe_J);
        Xe=mean(X(Fe),2); Ye=mean(Y(Fe),2);  Ze=mean(Z(Fe),2);
        X=[X(:);Xe(:)]; Y=[Y(:);Ye(:)]; Z=[Z(:);Ze(:)];
end

V=[X(:),Y(:),Z(:)];

end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.


% RSFIT - find p value for a given value in a given distribution
%             using Ramberg-Schmeiser distribution
%
% Usage: >> p = rsfit(x, val)
%        >> [p c l chi2] = rsfit(x, val, plot)
%
% Input:
%   x    - [float array] accumulation values
%   val  - [float] value to test
%   plot - [0|1|2] plot fit. Using 2, the function avoids creating
%          a new figure. Default: 0.
%
% Output:
%   p    - p value
%   c    - [mean var skewness kurtosis] distribution cumulants
%   l    - [4x float vector] Ramberg-Schmeiser distribution best fit
%          parameters.
%   chi2 - [float] chi2 for goodness of fit (based on 12 bins).
%          Fit is significantly different from data histogram if
%          chi2 > 19 (5%)
%
% Author: Arnaud Delorme, SCCN, 2003
%
% See also: RSADJUST, RSGET, RSPDFSOLV, RSPFUNC
%
% Reference: Ramberg, J.S., Tadikamalla, P.R., Dudewicz E.J., Mykkytka, E.F.
%            A probability distribution and its uses in fitting data.
%            Technimetrics, 1979, 21: 201-214.

% Copyright (C) 2003 Arnaud Delorme, SCCN, arno@salk.edu
%
% 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 [p, c, l, res] = rsfit(x, val, plotflag)

    if nargin < 2
        help rsfit;
        return;
    end
    if nargin < 3
        plotflag  = 0;
    end

    % moments
    % -------
    m1  = mean(x);
    m2  = sum((x-m1).^2)/length(x);
    m3  = sum((x-m1).^3)/length(x);
    m4  = sum((x-m1).^4)/length(x);

    xmean = m1;
    xvar  = m2;
    xskew = m3/(m2^1.5);
    xkurt = m4/(m2^2);
    c     = [ xmean xvar xskew xkurt ];

    if xkurt < 0
        disp('rsfit error: Can not fit negative kurtosis');
        save('/home/arno/temp/dattmp.mat', '-mat', 'x');
        disp('data saved to disk in /home/arno/temp/dattmp.mat');
    end

    % find fit
    % --------
    try,
        [sol tmp exitcode] = fminsearch('rspdfsolv', [0.1 0.1], optimset('TolX',1e-12, 'MaxFunEvals', 100000000), abs(xskew), xkurt);
    catch, exitcode = 0; % did not converge
    end
    if ~exitcode
        try, [sol tmp exitcode] = fminsearch('rspdfsolv', -[0.1 0.1], optimset('TolX',1e-12, 'MaxFunEvals', 100000000), abs(xskew), xkurt);
        catch, exitcode = 0; end
    end
    if ~exitcode,           error('No convergence'); end
    if sol(2)*sol(1) == -1, error('Wrong sign for convergence'); end
    %fprintf('          l-val:%f\n', sol);

    res = rspdfsolv(sol, abs(xskew), xkurt);
    l3 = sol(1);
    l4 = sol(2);

    %load res;
    %[tmp indalpha3] = min( abs(rangealpha3 - xskew) );
    %[tmp indalpha4] = min( abs(rangealpha4 - xkurt) );
    %l3  = res(indalpha3,indalpha4,1);
    %l4  = res(indalpha3,indalpha4,2);
    %res = res(indalpha3,indalpha4,3);

    % adjust fit
    % ----------
    [l1 l2 l3 l4] = rsadjust(l3, l4, xmean, xvar, xskew);
    l = [l1 l2 l3 l4];
    p = rsget(l, val);

    % compute goodness of fit
    % -----------------------
    if nargout > 3 || plotflag

        % histogram of value 12 bins
        % --------------------------
        [N X] = hist(x, 25);
        interval = X(2)-X(1);
        X = [X-interval/2 X(end)+interval/2]; % borders

        % regroup bin with less than 5 values
        % -----------------------------------
        indices2rm = [];
        for index = 1:length(N)-1
            if N(index) < 5
                N(index+1) = N(index+1) + N(index);
                indices2rm = [ indices2rm index];
            end
        end
        N(indices2rm)   = [];
        X(indices2rm+1) = [];
        indices2rm = [];
        for index = length(N):-1:2
            if N(index) < 5
                N(index-1) = N(index-1) + N(index);
                indices2rm = [ indices2rm index];
            end
        end
        N(indices2rm)   = [];
        X(indices2rm)   = [];

        % compute expected values
        % -----------------------
        for index = 1:length(X)-1
            p1 = rsget( l, X(index+1));
            p2 = rsget( l, X(index  ));
            expect(index) = length(x)*(p1-p2);
        end

        % value of X2
        % -----------
        res = sum(((expect - N).^2)./expect);

        % plot fit
        % --------
        if plotflag
            if plotflag ~= 2, figure('paperpositionmode', 'auto'); end
            hist(x, 10);

            % plot fit
            % --------
            xdiff = X(end)-X(1);
            abscisia   = linspace(X(1)-0.2*xdiff, X(end)+0.2*xdiff, 100);
            %abscisia  = (X(1:end-1)+X(2:end))/2;
            expectplot = zeros(1,length(abscisia)-1);
            for index = 2:length(abscisia);
                p1 = rsget( l, abscisia(index-1));
                p2 = rsget( l, abscisia(index  ));
                expectplot(index-1) = length(x)*(p2-p1);
                % have to do this subtraction since this a cumulate density distribution
            end
            abscisia = (abscisia(2:end)+abscisia(1:end-1))/2;
            hold on; plot(abscisia, expectplot, 'r');

            % plot PDF
            % ----------
            pval = linspace(0,1, 102); pval(1) = []; pval(end) = [];
            rp   = l(1) + (pval.^l(3) - (1-pval).^l(4))/l(2);
            fp   = l(2)*1./(l(3).*(pval.^(l(3)-1)) + l(4).*((1-pval).^(l(4)-1)));
            [maxval index]  = max(expect);
            [tmp closestind] = min(abs(rp - abscisia(index)));
            fp = fp./fp(closestind)*maxval;
            plot(rp, fp, 'g');
            legend('Chi2 fit (some bins have been grouped)', 'Pdf', 'Data histogram'  );
            xlabel('Bins');
            ylabel('# of data point per bin');
            title (sprintf('Fit of distribution using Ramberg-Schmeiser distribution (Chi2 = %2.4g)', res));
        end
    end
    return


function [Yl,Yh,Yscale] = dtwavexfm(X,nlevels,biort,qshift);

% Function to perform a n-level DTCWT decompostion on a 1-D column vector X
% (or on the columns of a matrix X).
%
% [Yl,Yh,Yscale] = dtwavexfm(X,nlevels,biort,qshift);
%
%     X -> real 1-D signal column vector (or matrix of vectors)
%
%     nlevels -> No. of levels of wavelet decomposition
%
%     biort ->  'antonini'   => Antonini 9,7 tap filters.
%               'legall'     => LeGall 5,3 tap filters.
%               'near_sym_a' => Near-Symmetric 5,7 tap filters.
%               'near_sym_b' => Near-Symmetric 13,19 tap filters.
%
%     qshift -> 'qshift_06' => Quarter Sample Shift Orthogonal (Q-Shift) 10,10 tap filters,
%                              (only 6,6 non-zero taps).
%               'qshift_a' =>  Q-shift 10,10 tap filters,
%                              (with 10,10 non-zero taps, unlike qshift_06).
%               'qshift_b' => Q-Shift 14,14 tap filters.
%               'qshift_c' => Q-Shift 16,16 tap filters.
%               'qshift_d' => Q-Shift 18,18 tap filters.
%
%
%     Yl     -> The real lowpass subband from the final level.
%     Yh     -> A cell array containing the complex highpass subband for each level.
%     Yscale -> This is an OPTIONAL output argument, that is a cell array containing
%               the real lowpass coefficients at every scale.
%
%
% Example: [Yl,Yh] = dtwavexfm(X,5,'near_sym_b','qshift_b');
% performs a 5-level transform on the real image X using the 13,19-tap filters
% for level 1 and the Q-shift 14-tap filters for levels >= 2.
%
% Nick Kingsbury and Cian Shaffrey
% Cambridge University, May 2002

if isstr(biort) & isstr(qshift)		%Check if the inputs are strings
   biort_exist = exist([biort '.mat']);
   qshift_exist = exist([qshift '.mat']);
   if biort_exist == 2 & qshift_exist == 2;  %Check to see if the filters exist as .mat files
      load (biort);
      load (qshift);
   else
      error('Please enter the correct names of the Biorthogonal or Q-Shift Filters, see help DTWAVEXFM for details.');
   end
else
   error('Please enter the names of the Biorthogonal or Q-Shift Filters as shown in help DTWAVEXFM.');
end

L = size(X);

if any(rem(L(1),2)),	 % ensure that X is an even length, thus enabling it to be extended if needs be.
   error('Size of X must be a multiple of 2');
end

if nlevels == 0, return; end

%initialise
Yh=cell(nlevels,1);
if nargout == 3
   Yscale=cell(nlevels,1);   % This is only required if the user specifies a third output component.
end

j = sqrt(-1);

% Level 1.
Hi = colfilter(X, h1o);
Lo = colfilter(X, h0o);
t = 1:2:size(Hi,1);
Yh{1} = Hi(t,:) + j*Hi(t+1,:); % Convert Hi to complex form.
if nargout == 3
   Yscale{1} = Lo;
end

if nlevels >= 2;  % Levels 2 and above.
   for level = 2:nlevels;
      if rem(size(Lo,1),4),	% Check to see if height of Lo is divisable by 4, if not extend.
         Lo = [Lo(1,:); Lo; Lo(end,:)];
      end
      Hi = coldfilt(Lo,h1b,h1a);
      Lo = coldfilt(Lo,h0b,h0a);
	   t = 1:2:size(Hi,1);
   	Yh{level} = Hi(t,:) + j*Hi(t+1,:); % Convert Hi to complex form.
   	if nargout == 3
      	Yscale{level} = Lo;
   	end
   end
end

Yl = Lo;

return


%% EXAMPLE 2: Plot SPOD spectrum and inspect SPOD modes.
%  The large-eddy simulation data provided along with this example is a
%  subset of the database of a Mach 0.9 turbulent jet described in [1] and
%  was calculated using the unstructured flow solver Charles developed at
%  Cascade Technologies. If you are using the database in your research or
%  teaching, please include explicit mention of Brès et al. [1]. The test
%  database consists of 5000 snapshots of the symmetric component (m=0) of
%  a round turbulent jet. A physical interpretaion of the SPOD results is
%  given in [2], and a comprehensive discussion and derivation of SPOD and
%  many of its properties can be found in [3].
%
%   References:
%     [1] G. A. Brès, P. Jordan, M. Le Rallic, V. Jaunet, A. V. G.
%         Cavalieri, A. Towne, S. K. Lele, T. Colonius, O. T. Schmidt,
%         Importance of the nozzle-exit boundary-layer state in subsonic
%         turbulent jets, J. of Fluid Mech. 851, 83-124, 2018
%     [2] Schmidt, O. T. and Towne, A. and Rigas, G. and Colonius, T. and
%         Bres, G. A., Spectral analysis of jet turbulence, J. of Fluid Mech. 855, 953–982, 2018
%     [3] Towne, A. and Schmidt, O. T. and Colonius, T., Spectral proper
%         orthogonal decomposition and its relationship to dynamic mode
%         decomposition and resolvent analysis, J. of Fluid Mech. 847, 821–867, 2018
%
% O. T. Schmidt (oschmidt@ucsd.edu), A. Towne, T. Colonius
% Last revision: 5-Sep-2022 (OTS)

clc, clear variables
addpath('utils')
load(fullfile('jet_data','jetLES.mat'),'p','x','r','dt');

%% SPOD of the test database.
%   Calculate the SPOD of the data matrix 'p' and use the timestep 'dt'
%   between snapshots to obtain the physical frequency 'f'. 'L' is the
%   matrix of modal energies, as before, and 'P' the data matrix of SPOD
%   modes. We leave all other options empty for now.
[L,P,f] = spod(p,[],[],[],dt);

%   First, we plot the SPOD spectrum.
figure
loglog(f,L)
xlabel('frequency'), ylabel('SPOD mode energy')

%   Second, we visualize the 1st and 2nd SPOD modes at three frequencies.
figure
count = 1;
for fi = [10 15 20]
    for mi = [1 2]
        subplot(3,2,count)
        contourf(x,r,real(squeeze(P(fi,:,:,mi))),11,'edgecolor','none'), axis equal tight, caxis(max(abs(caxis))*[-1 1])
        xlabel('x'), ylabel('r'), title(['f=' num2str(f(fi),'%.2f') ', mode ' num2str(mi) ', \lambda=' num2str(L(fi,mi),'%.2g')])
        xlim([0 10]); ylim([0 2])
        count = count + 1;
    end
end

%% Animate the same modes.
%   Note how all wavepackets travel at approximately the same phase
%   speed c_ph. The reason is that their streamwise wavenumber k_x changes
%   with frequency such that c_ph = omega/k_x is approximately constant.
figure
nt      = 30;
T       = 1/f(10);              % period of the 10th frequency
time    = linspace(0,T,nt);     % animate over one period
count = 1;
for ti = 1:nt
    for fi = [10 15 20]
        for mi = [1 2]
            subplot(3,2,count)
            pcolor(x,r,real(squeeze(P(fi,:,:,mi)*exp(2i*pi*f(fi)*time(ti))))), shading interp, axis equal tight, caxis(max(abs(caxis))*[-1 1])
            xlabel('x'), ylabel('r'), title(['f=' num2str(f(fi),'%.2f') ', mode ' num2str(mi) ', \lambda=' num2str(L(fi,mi),'%.2g')])
            xlim([0 10]); ylim([0 2])
            count = count + 1;
            hold on
        end
    end
    drawnow
    hold off
    count = 1;
end


function g = mlpderiv(net, x)
%MLPDERIV Evaluate derivatives of network outputs with respect to weights.
%
%	Description
%	G = MLPDERIV(NET, X) takes a network data structure NET and a matrix
%	of input vectors X and returns a three-index matrix G whose I, J, K
%	element contains the derivative of network output K with respect to
%	weight or bias parameter J for input pattern I. The ordering of the
%	weight and bias parameters is defined by MLPUNPAK.
%
%	See also
%	MLP, MLPPAK, MLPGRAD, MLPBKP
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Check arguments for consistency
errstring = consist(net, 'mlp', x);
if ~isempty(errstring);
  error(errstring);
end

[y, z] = mlpfwd(net, x);

ndata = size(x, 1);

if isfield(net, 'mask')
  nwts = size(find(net.mask), 1);
  temp = zeros(1, net.nwts);
else
  nwts = net.nwts;
end

g = zeros(ndata, nwts, net.nout);
for k = 1 : net.nout
  delta = zeros(1, net.nout);
  delta(1, k) = 1;
  for n = 1 : ndata
    if isfield(net, 'mask')
      temp = mlpbkp(net, x(n, :), z(n, :), delta);
      g(n, :, k) = temp(logical(net.mask));
    else
      g(n, :, k) = mlpbkp(net, x(n, :), z(n, :),...
	delta);
    end
  end
end


classdef CF8 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, S. Zhao, P. N. Suganthan, W. Liu, and S. Tiwari,
% Multiobjective optimization test instances for the CEC 2009 special
% session and competition, School of CS & EE, University of Essex, Working
% Report CES-487, 2009.
%------------------------------- 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 = 3;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = [0,0,zeros(1,obj.D-2)-4];
            obj.upper    = [1,1,zeros(1,obj.D-2)+4];
            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));
            D  = size(X,2);
            J1 = 4 : 3 : D;
            J2 = 5 : 3 : D;
            J3 = 3 : 3 : D;
            Y  = X - 2*repmat(X(:,2),1,D).*sin(2*pi*repmat(X(:,1),1,D)+repmat(1:D,size(X,1),1)*pi/D);
            PopObj(:,1) = cos(0.5*X(:,1)*pi).*cos(0.5*X(:,2)*pi) + 2*mean(Y(:,J1).^2,2);
            PopObj(:,2) = cos(0.5*X(:,1)*pi).*sin(0.5*X(:,2)*pi) + 2*mean(Y(:,J2).^2,2);
            PopObj(:,3) = sin(0.5*X(:,1)*pi)                     + 2*mean(Y(:,J3).^2,2);
            PopCon      = 1 - (PopObj(:,1).^2+PopObj(:,2).^2)./(1-PopObj(:,3).^2) + 4*abs(sin(2*pi*((PopObj(:,1).^2-PopObj(:,2).^2)./(1-PopObj(:,3).^2)+1)));
            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)
            N      = ceil(N/5)*5;
            R      = zeros(N,3);
            R(:,3) = repmat(sin((0:1/(N/5-1):1).*pi/2)',5,1);
            for i = 0 : 4
                R(i*N/5+1:(i+1)*N/5,1) = sqrt(i/4*(1-R(i*N/5+1:(i+1)*N/5,3).^2));
            end
            R(:,2) = sqrt(max(1-R(:,1).^2-R(:,3).^2,0));
        end
    end
end


function [network, mappedX, reconX] = train_autoencoder(X, layers, noise, max_iter)
%TRAIN_AUTOENCODER Trains an simple autoencoder
%
%   [network, mappedX, reconX] = train_encoder(X, layers, noise, max_iter)
%
% Trains up an autoencoder with the structure that is specified in layers.
% The low-dimensional data is returned in mappedX, and the network in
% network. The variable noise indicates how much of the input data should be
% blacked out (default = 0).
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    if nargin < 2
        error('Not enough inputs.');
    end
    if isempty(layers)
        error('There should be at least one hidden layer.');
    end
    if ~exist('noise', 'var') || isempty(noise)
        noise = 0;
    end
    if ~exist('max_iter', 'var') || isempty(max_iter)
        max_iter = 50;
    end

    % Initialize the network
    D = size(X, 2);
    no_layers = length(layers) + 1;
    network = cell(no_layers, 1);
    network{1}.W = randn(D, layers(1)) * .0001;
    network{1}.bias_upW = zeros(1, layers(1));
    for i=2:no_layers - 1
        network{i}.W = randn(layers(i - 1), layers(i)) * .0001;
        network{i}.bias_upW = zeros(1, layers(i));
    end
    network{no_layers}.W = randn(layers(end), D) * .0001;
    network{no_layers}.bias_upW = zeros(1, D);
    reconX = run_data_through_autoenc(network, X);
    disp(['Initial MSE of reconstructions: ' num2str(mean((X(:) - reconX(:)) .^ 2))]);

    % Perform backpropagation to minimize reconstruction error
    network = backprop(network, X, X, max_iter, noise);

    % Get representation from hidden layer
    [reconX, mappedX] = run_data_through_autoenc(network, X);
    disp(['Final MSE of reconstructions: ' num2str(mean((X(:) - reconX(:)) .^ 2))]);


function [k, sk, n2] = rbfperiodic2KernCompute(kern, x, x2)

% RBFPERIODIC2KERNCOMPUTE Compute the RBFPERIODIC2 kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the RBF periodic covariance with variying period
% kernel given inputs associated with rows and columns.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN k : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel matrix for the RBF periodic covariance with variying period
% kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : the kernel matrix computed at the given points.
%
% SEEALSO : rbfperiodic2KernParamInit, kernCompute, kernCreate, rbfperiodic2KernDiagCompute
%
% COPYRIGHT : Neil D. Lawrence, 2007, 2009
%
% MODIFICATIONS : Andreas C. Damianou, 2011
%
% MODIFICATIONS : Michalis K. Titsias, 2011

% KERN

factor = kern.factor; % Default (if period is fixed: 2*pi/kern.period)
if nargin < 3
  n2 = sin(0.5*factor*(repmat(x, 1, size(x, 1)) - repmat(x', size(x, 1), 1)));
  n2 = n2.*n2;
  wi2 = (2 .* kern.inverseWidth);
  sk = exp(-n2*wi2);
else
  n2 = sin(0.5*factor*(repmat(x, 1, size(x2, 1)) - repmat(x2', size(x, 1), 1)));
  n2 = n2.*n2;
  wi2 = (2 .* kern.inverseWidth);
  sk = exp(-n2*wi2);
end
k = kern.variance*sk;
% Test kernel with: kernTest('rbfperiodic2',1)


function [X, R, S] = snapshot_gen_sym(design, doas, wavelength, stype, t, ncov, sp)
%SNAPSHOT_GEN_SYM Generates snapshots from symbols.
%Syntax:
%   X = SNAPSHOT_GEN_SYM(design, doas, wavelength, stype[, t, ncov, sp]);
%   [X, R] = SNAPSHOT_GEN_SYM(design, doas, wavelength, stype[, t, ncov, sp]);
%   [X, R, S] = SNAPSHOT_GEN_SYM(design, doas, wavelength, stype[, t, ncov, sp]);
%Inputs:
%   design - Array design.
%   doas - DOA vector. For 2D DOAs, each column represents a DOA pair.
%   wavelength - Wavelength.
%   stype - Symbol type.
%   t - Number of snapshots.
%   ncov - Covariance matrix of the additive complex circular-symmetric
%          Gaussian noise. Can be a scalar, vector (for uncorrelated noise
%          with different powers), or a matrix.
%   sp - Sources powers.
%Outputs:
%   X - Snapshots, where each columns is a single snapshot.
%   R - Sample covariance matrix (averaged by the number of snapshots).
%   S - A source_count x snapshot_count matrix consists of source signal
%       vectors.
if nargin <= 6
    sp = 1;
end
if nargin <= 5
    ncov = 1;
end
if nargin <= 4
    t = 1;
end
A = steering_matrix(design, wavelength, doas);
[m, k] = size(A);
S_internal = gen_symbols(k, t, stype, sp);
X = A * S_internal + gen_ccsg(m, t, ncov);
if nargout >= 2
    R = (X*X')/t;
    if nargout == 3
        S = S_internal;
    end
end
end

function S = gen_symbols(m, n, stype, sp)
switch lower(stype)
    case 'bpsk'
        symbols = [-1 1];
    case 'qpsk'
        symbols = exp(1j*[1 3 5 7]/4*pi);
    otherwise
        error('Unsupported symbol type.')
end
S = symbols(randi([1 length(symbols)], m, n));
if isscalar(sp)
    S = sqrt(sp) * S;
else
    S = bsxfun(@times, sqrt(sp(:)), S);
end
end

function X = gen_ccsg(m, n, cov)
X0 = randn(m, n) + 1j * randn(m, n);
if isscalar(cov)
    X = sqrt(cov/2) * X0;
elseif isvector(cov)
    X = bsxfun(@times, X0, cov(:));
else
    C = sqrtm(cov/2);
    X = C*X0;
end
end


function [u, erriter, i, timet] = CMF_Cut
%
%   Performing the continuous max-flow algorithm to solve the
%   continuous min-cut problem in 2D
%
%   Usage: [u, erriter, i, timet] = CMF_Cut;
%
%   Inputs: there is no input since all data and parameters can be
%           adjusted within the program
%
%   Outputs:
%       - u: the final results u(x) in [0,1]. As the following paper,
%           the global binary result can be available by threshholding u
%           by any constant alpha in (0,1):
%
%           Nikolova, M.; Esedoglu, S.; Chan, T. F.
%           Algorithms for Finding Global Minimizers of Image Segmentation and Denoising Models
%           SIAM J. App. Math., 2006, 66, 1632-1648
%
%       - erriter: it returns the error evaluation of each iteration,
%           i.e. it shows the convergence rate. One can check the algorithm
%           performance.
%
%       - i: gives the total number of iterations, when the algorithm converges.
%
%       - timet: gives the total computation time.
%
%   Example:
%       >> [u, erriter, i, timet] = CMF_Cut;
%
%       >> us = max(u, beta);  % where beta in (0,1)
%
%       >> imagesc(us), colormap gray, axis image, axis off;figure(gcf)
%
%       >> figure, loglog(erriter,'DisplayName','erriterN');figure(gcf)
%
%
%
%   The original algorithm was proposed in the following papers:
%
%   [1] Yuan, J.; Bae, E.;  Tai, X.-C.
%       A Study on Continuous Max-Flow and Min-Cut Approaches
%       CVPR, 2010
%
%   [2] Yuan, J.; Bae, E.; Tai, X.-C.; Boycov, Y.
%       A study on continuous max-flow and min-cut approaches. Part I: Binary labeling
%       UCLA CAM, Technical Report 10-61, 2010
%
%   The mimetic finite-difference discretization method was proposed for
%   the total-variation function in the paper:
%
%   [1] Yuan, J.; Schn{\"o}rr, C.; Steidl, G.
%       Simultaneous Optical Flow Estimation and Decomposition
%       SIAM J.~Scientific Computing, 2007, vol. 29, page 2283-2304, number 6
%
%   This software can be used only for research purposes, you should cite ALL of
%   the aforementioned papers in any resulting publication.
%
%   Please email cn.yuanjing@gmail.com for any questions, suggestions and bug reports
%
%   The Software is provided "as is", without warranty of any kind.
%
%
%                       Version 1.0
%           https://sites.google.com/site/wwwjingyuan/
%
%           Copyright 2011 Jing Yuan (cn.yuanjing@gmail.com)
%

ur = double(imread('cameraman.jpg'))/255;

[rows, cols] = size(ur);
imgSize = rows*cols;

% define the required parameters:
%
%   - alpha: the penalty parameter to the total-variation term.
%       For the case without incorporating image-edge weights, alpha is given
%       by the constant everywhere. For the case with image-edge weights,
%       alpha is given by the pixelwise weight function:
%
%       For example, alpha(x) = b/(1 + a*| nabla f(x)|) where b and a are positive
%       constants and |nabla f(x)| gives the strength of the local gradient.
%
%   - cc: gives the step-size of the augmented Lagrangian method.
%       The optimal range of cc is [0.3, 3].
%
%   - errbound: the error bound for convergence.
%
%   - numIter: the maximum iteration number.
%
%   - steps: the step-size for the graident-projection step to the
%       total-variation function. The optimal range of steps is [0.1,
%       0.17].
%

alpha = 0.5*ones(rows,cols);
cc = 0.3;
errbound = 1e-4;
numIter = 300;
steps = 0.16;

% build up the data terms
ulab(1) = 0.15;
ulab(2) = 0.6;
Cs = abs(ur - ulab(1));
Ct = abs(ur - ulab(2));

% set the initial values
%   - the initial value of u is set to be an initial cut, see below.
%   - the initial values of two terminal flows ps and pt are set to be the
%     specified legal flows.
%   - the initial value of the spatial flow fiels p = (pp1, pp2) is set to
%   be zero.

u = double((Cs-Ct) >= 0);
ps = min(Cs, Ct);
pt = ps;

pp1 = zeros(rows, cols+1);
pp2 = zeros(rows+1, cols);
divp = pp1(:,2:cols+1)-pp1(:,1:cols)+pp2(2:rows+1,:)-pp2(1:rows,:);

erriter = zeros(numIter,1);

tic
for i = 1:numIter

	% update the spatial flow field p = (pp1, pp2):
    %   the following steps are the gradient descent step with steps as the
    %   step-size.

    pts = divp - (ps - pt  + u/cc);
    pp1(:,2:cols) = pp1(:,2:cols) + steps*(pts(:,2:cols) - pts(:,1:cols-1));
    pp2(2:rows,:) = pp2(2:rows,:) + steps*(pts(2:rows,:) - pts(1:rows-1,:));

    % the following steps give the projection to make |p(x)| <= alpha(x)

    gk = sqrt((pp1(:,1:cols).^2 + pp1(:,2:cols+1).^2 + pp2(1:rows,:).^2 + pp2(2:rows+1,:).^2)*0.5);
    gk = double(gk <= alpha) + double(~(gk <= alpha)).*(gk ./ alpha);
    gk = 1 ./ gk;

    pp1(:,2:cols) = (0.5*(gk(:,2:cols) + gk(:,1:cols-1))).*pp1(:,2:cols);
    pp2(2:rows,:) = (0.5*(gk(2:rows,:) + gk(1:rows-1,:))).*pp2(2:rows,:);

    divp = pp1(:,2:cols+1)-pp1(:,1:cols)+pp2(2:rows+1,:)-pp2(1:rows,:);

    % updata the source flow ps

    pts = divp + pt - u/cc + 1/cc;
    ps = min(pts, Cs);

    % update the sink flow pt

    pts = - divp + ps + u/cc;
    pt = min(pts, Ct);

	% update the multiplier u

	erru = cc*(divp + pt  - ps);
	u = u - erru;

    % evaluate the avarage error

    erriter(i) = sum(sum(abs(erru)))/imgSize;

    if (erriter(i) < errbound)
        break;
    end
end
toc
timet = toc

msg = sprintf('number of iterations = %u. \n', i);
disp(msg);


function uh = CurlCurl2dNd0(node,elem,bdFlag,pde)
%% CURLCURL2DND0 curlcurl equation: lowest order edge element on triangular mesh
% rot(\mu^{-1} curl u) + \kappa u = f in \Omega
% u\times n = 0 on \Gamma_D
% \mu^{-1} curl u = g_N on \Gamma_N

% Solve u_h in Nd0 \cap {u \times n = 0 on \Gamma_D} = V_h
% (\mu^{-1} curl u_h, curl v_h) + (\kappa u_h, v_h)
% = (f, v_h) + <v_h \cdot \tau, g_N>_{\Gamma_N} for any v_h \in V_h

% basis function associated with an edge |e|
% \phi_e = \frac{|e|}{2|K|} (\lamda_{i+1} n_{i-1} - \lambda_{i+1} n_{i-1})
% implented based on an older iFEM circa 2010.
%
% For 3D curl-curl problems
% See also Maxwell, cubeMaxwell
%
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.
%%
NT = size(elem,1);
T = auxstructure(elem);
elem2edge = T.elem2edge;
edge = T.edge;
elem2edgeSign = ones(NT,3);
totalEdge = uint32([elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])]);
idx = (totalEdge(:,1)>totalEdge(:,2));
elem2edgeSign(idx) = -1;

if ~isfield(pde,'mu'), pde.mu = 1; end
if ~isfield(pde,'kappa'), pde.kappa = 1; end
if ~isfield(pde,'g_D'), pde.g_D = []; end
if ~isfield(pde,'g_N'), pde.g_N = []; end

NE = size(edge,1);
uh = zeros(NE,1);
g_D = pde.g_D;
g_N = pde.g_N;
f_x = pde.f_x;
f_y = pde.f_y;
mu = pde.mu;
kappa = pde.kappa;

%% Boundary
idxD = (bdFlag(:) == 1);     % all Dirichlet edges in bdFlag
isFixedEdge = false(NE,1);
isFixedEdge(elem2edge(idxD)) = true;  % index of fixed boundary edges
freeEdge = ~isFixedEdge;

idxN = (bdFlag(:) == 2);     % all Neumann edges in bdFlag
Neumann = edge(elem2edge(idxN),:);

%% geometric quantities
%edge vector which follows elem2edge counterclockwisely
ve(:,:,1) = node(elem(:,3),:)-node(elem(:,2),:);
ve(:,:,2) = node(elem(:,1),:)-node(elem(:,3),:);
ve(:,:,3) = node(elem(:,2),:)-node(elem(:,1),:);
area = 0.5*abs(-ve(:,1,3).*ve(:,2,2)+ve(:,2,3).*ve(:,1,2));

%length_ve(:,:,i) is the length of i^th edge
length_ve = sqrt(sum(ve.^2,2));
length_ve = reshape(length_ve,NT,3);

%outer normal vector of each edge which follows the index of elem2edge
ne = [ve(:,2,:), -ve(:,1,:)];

%%
%M is the mass matrix M_{ij} = (\kappa \phi_i, \phi_j)
%B is the stiffness matrix B_{ij} = (\mu^{-1} curl \phi_i, curl \phi_j)
c = [3 1 2 3 1];
%cyclic group of {1,2,3}: c(i)=i-1, c(i+2)=i+1

M = sparse(NE,NE);
B = sparse(NE,NE);
muinv = 1/mu;

for i = 1:3
    for j = 1:3
        sij = elem2edgeSign(:,i).*elem2edgeSign(:,j);

        if i==j
            nini = length_ve(:,i).^2;
            njnk = dot(ne(:,:,c(i)),ne(:,:,c(i+2)),2);
            Mij = kappa.*length_ve(:,i).*(nini - 3*njnk)./(24*area);
        else
            ninj = dot(ne(:,:,i),ne(:,:,j),2);
            ni2 = length_ve(:,i).^2;
            nj2 = length_ve(:,j).^2;
            Mij = -sij.*kappa.*length_ve(:,i).*length_ve(:,j).*(ni2 + nj2 + 3*ninj)./(24*area);
        end

        Bij = muinv.*sij.*length_ve(:,i).*length_ve(:,j)./area;

        M = M + sparse(elem2edge(:,i),elem2edge(:,j),Mij,NE,NE);
        B = B + sparse(elem2edge(:,i),elem2edge(:,j),Bij,NE,NE);
    end
end

A = M+B;

%% rhs (old hard-coded implementation)
Fx = quadelem2node(node,elem,f_x);
Fy = quadelem2node(node,elem,f_y);

bt1x = -0.5*elem2edgeSign(:,1).*length_ve(:,1)...
    .*(Fx(:,2).*ne(:,1,3) - Fx(:,3).*ne(:,1,2))./area;
bt2x = -0.5*elem2edgeSign(:,2).*length_ve(:,2)...
    .*(Fx(:,3).*ne(:,1,1) - Fx(:,1).*ne(:,1,3))./area;
bt3x = -0.5*elem2edgeSign(:,3).*length_ve(:,3)...
    .*(Fx(:,1).*ne(:,1,2) - Fx(:,2).*ne(:,1,1))./area;

bt1y = -0.5*elem2edgeSign(:,1).*length_ve(:,1)...
    .*(Fy(:,2).*ne(:,2,3) - Fy(:,3).*ne(:,2,2))./area;
bt2y = -0.5*elem2edgeSign(:,2).*length_ve(:,2)...
    .*(Fy(:,3).*ne(:,2,1) - Fy(:,1).*ne(:,2,3))./area;
bt3y = -0.5*elem2edgeSign(:,3).*length_ve(:,3)...
    .*(Fy(:,1).*ne(:,2,2) - Fy(:,2).*ne(:,2,1))./area;

bt1 = bt1x + bt1y;
bt2 = bt2x + bt2y;
bt3 = bt3x + bt3y;

F = accumarray(elem2edge(:),[bt1;bt2;bt3],[NE 1]);

%% Dirichlet Boundary
if ~isempty(pde.g_D)
    fixedEdge = edge(isFixedEdge,:);
    signFixedEdge = elem2edgeSign(idxD);
    Nve = node(fixedEdge(:,1),:) - node(fixedEdge(:,2),:);
    NveLength = sqrt(sum(Nve.^2,2));
    GD = quadedge(node,fixedEdge,g_D).*signFixedEdge./NveLength;
    uh(isFixedEdge) = GD;
end

%% Neumann Boundary
if (~isempty(Neumann) && ~isempty(pde.g_N))
    signNeumannEdge = elem2edgeSign(idxN);
    GN = quadedge(node,Neumann,g_N).*signNeumannEdge;
    F = F + accumarray(idxN(:),GN(:),[NE,1]);
end

%direct solver
uh(freeEdge) = A(freeEdge,freeEdge)\F(freeEdge);


function solution = CS_GWO(UAV, SearchAgents, Max_iter)
%CS_GWO 灰狼-布谷鸟优化算法
%Gray Wolf Cuckoo Optimization

% 超参数
pa = 0.25;   % 布谷鸟搜索参数

% 算法初始化
[WolfPops, Tracks] = PopsInit(UAV, SearchAgents, false);   % 随机生成 初始狼群 和 轨迹们
dim = WolfPops.PosDim;                                                         % 状态变量维度

% 初始化解
Alpha_pos = zeros(1, dim);   % α解
Alpha_score = inf;                  % α解适应度
Alpha_no = 1;                         % α解编号

Beta_pos = zeros(1, dim);      % β解
Beta_score = inf;                     % β解适应度
Beta_no = 1;                             % β解编号

Delta_pos = zeros(1, dim);     % δ解
Delta_score = inf;                    % δ解适应度
Delta_no = 1;                           % δ解编号

Fitness_list = zeros(1, Max_iter);

% 迭代求解
tic
fprintf('>>CS_GWO 优化中    00.00%%')
for iter = 1 : Max_iter

    % ①  计算每只狼的适应度，更改其种群等级
    ProbPoints = cell(SearchAgents, 1);
    for i = 1 : SearchAgents
        % 计算目标函数
        [fitness, ~, Data] = ObjFun(Tracks{i}, UAV);    % 一个智能体的目标函数
        ProbPoints{i} = Data.ProbPoint;                       % 所有智能体不符合条件的状态

        % 更新 Alpha、Beta 和 Delta 解
        if fitness <= Alpha_score  % 适应能力最强（因为性能指标越小越好，因此为小于号）
            Alpha_score = fitness;
            Alpha_pos = WolfPops.Pos(i, :);
            Alpha_no = i;
        end
        if fitness > Alpha_score && fitness <= Beta_score
            Beta_score = fitness;
            Beta_pos = WolfPops.Pos(i, :);
            Beta_no = i;
        end
        if fitness > Alpha_score && fitness > Beta_score && fitness <= Delta_score
            Delta_score = fitness;
            Delta_pos = WolfPops.Pos(i, :);
            Delta_no = i;
        end
    end

    % ②  更新参数a
    a = 2 - iter * 2 / Max_iter;                  % 线性递减
    %a = 2 * cos((iter / Max_iter) * pi/2);   % 非线性递减

    % ③  更新位置（朝着前三只狼位置前进）
    for i = 1 : SearchAgents
        for j = 1 : dim

            r1 = rand();
            r2 = rand();
            A1 = 2*a*r1 - a;
            C1 = 2*r2;
            D_alpha = abs(C1*Alpha_pos(j) - WolfPops.Pos(i, j));
            X1(i, j) = Alpha_pos(j) - A1*D_alpha;

            r1 = rand();
            r2 = rand();
            A2 = 2*a*r1 - a;
            C2 = 2*r2;
            D_beta = abs(C2*Beta_pos(j) - WolfPops.Pos(i, j));
            X2(i, j) = Beta_pos(j) - A2*D_beta;

            r1 = rand();
            r2 = rand();
            A3 = 2*a*r1 - a;
            C3 = 2*r2;
            D_delta = abs(C3*Delta_pos(j) - WolfPops.Pos(i, j));
            X3(i, j) = Delta_pos(j) - A3*D_delta;

            %更新
            %WolfPops.Pos(i, j) = (X1(i, j) + X2(i, j) + X3(i, j)) / 3;

        end

    end

    % ④  Cuckoo 搜索
    fitness = nan(SearchAgents, 1);
    for i = 1 : SearchAgents
        [fitness(i), ~, ~] = ObjFun(Tracks{i}, UAV);
    end
    [~, index] = min(fitness);
    best = WolfPops.Pos(index, :);
    X1 = get_cuckoos(X1, best, WolfPops.lb, WolfPops.ub);
    X2 = get_cuckoos(X2, best, WolfPops.lb, WolfPops.ub);
    X3 = get_cuckoos(X3, best, WolfPops.lb, WolfPops.ub);
    X1 = empty_nests(X1, WolfPops.lb, WolfPops.ub, pa);
    X2 = empty_nests(X2, WolfPops.lb, WolfPops.ub, pa);
    X3 = empty_nests(X3, WolfPops.lb, WolfPops.ub, pa);
    WolfPops.Pos = (X1 + X2 + X3) / 3;

    % ⑤  调整不符合要求的状态变量
    [WolfPops, Tracks] = BoundAdjust(WolfPops, ProbPoints, UAV);

    % ⑥  存储适应度
    Fitness_list(iter) = Alpha_score;

    if iter/Max_iter*100 < 10
        fprintf('\b\b\b\b\b%.2f%%', iter/Max_iter*100)
    else
        fprintf('\b\b\b\b\b\b%.2f%%', iter/Max_iter*100)
    end
end
fprintf('\n\n>>计算完成！\n\n')
toc

% 输出值
solution.method = 'GWO';                % 算法
solution.WolfPops = WolfPops;       % 所有解种群信息
solution.Tracks = Tracks;                  % 所有解航迹信息
solution.Fitness_list = Fitness_list;   % α解适应度曲线
solution.Alpha_Data = Data;            % α解的威胁信息
solution.Alpha_no = Alpha_no;        % α解的位置
solution.Beta_no = Beta_no;             % β解的位置
solution.Delta_no = Delta_no;          % δ解的位置

end

%%%%% 布谷鸟搜索 %%%%%
function nest = get_cuckoos(nest, best, Lb, Ub)
    n = size(nest, 1);
    beta = 3/2;
    sigma = (gamma(1 + beta) * sin(pi*beta/2)/(gamma((1 + beta)/2)*beta*2^((beta - 1)/2)))^(1/beta);
    for j = 1:n
        s = nest(j, :);
        u = randn(size(s))*sigma;
        v = randn(size(s));
        step = u./abs(v).^(1/beta);
        stepsize = 0.01*step.*(s - best);
        s = s + stepsize.*randn(size(s));
        nest(j, :) = BoundClamp(s, Lb, Ub);
    end
end
function new_nest = empty_nests(nest, Lb, Ub, pa)
    n = size(nest, 1);
    K = rand(size(nest)) > pa;
    stepsize = rand*(nest(randperm(n), :) - nest(randperm(n), :));
    new_nest = nest + stepsize.*K;
    for j = 1:size(new_nest, 1)
        s = new_nest(j, :);
        new_nest(j, :) = BoundClamp(s, Lb, Ub);
    end
end
function x = BoundClamp(x, lb, ub)
    Flag4ub = x > ub;
    Flag4lb = x < lb;
    x = x .* ( ~(Flag4ub + Flag4lb) ) + ub .* Flag4ub + lb .* Flag4lb;
end


clear, clc;

% This is an example for running the function tree_LogisticR
%
%  Problem:
%
%  min  f(x,c) = - sum_i weight_i * log (p_i) + z * sum_j w_j ||x_{G_j}||
%
%  a_i denotes a training sample,
%      and a_i' corresponds to the i-th row of the data matrix A
%
%  y_i (either 1 or -1) is the response
%
%  p_i= 1/ (1+ exp(-y_i (x' * a_i + c) ) ) denotes the probability
%
%  G_j's are nodes with tree structure
%
%  The tree structured group information is contained in
%  opts.ind, which is a 3 x nodes matrix, where nodes denotes the number of
%  nodes of the tree.
%
%  opts.ind(1,:) contains the starting index
%  opts.ind(2,:) contains the ending index
%  opts.ind(3,:) contains the corresponding weight (w_j)
%
%  Note:
%  1) If each element of x is a leaf node of the tree and the weight for
%  this leaf node are the same, we provide an alternative "efficient" input
%  for this kind of node, by creating a "super node" with
%  opts.ind(1,1)=-1; opts.ind(2,1)=-1; and opts.ind(3,1)=the common weight.
%
%  2) If the features are well ordered in that, the features of the left
%  tree is always less than those of the right tree, opts.ind(1,:) and
%  opts.ind(2,:) contain the "real" starting and ending indices. That is to
%  say, x( opts.ind(1,j):opts.ind(2,j) ) denotes x_{G_j}. In this case,
%  the entries in opts.ind(1:2,:) are within 1 and n.
%
%
%  If the features are not well ordered, please use the input opts.G for
%  specifying the index so that
%   x( opts.G ( opts.ind(1,j):opts.ind(2,j) ) ) denotes x_{G_j}.
%  In this case, the entries of opts.G are within 1 and n, and the entries of
%  opts.ind(1:2,:) are within 1 and length(opts.G).
%
%% Related papers
%
% [1] Jun Liu and Jieping Ye, Moreau-Yosida Regularization for
%     Grouped Tree Structure Learning, NIPS 2010
%
%%

cd ..
cd ..

root=cd;
addpath(genpath([root '/SLEP']));
                     % add the functions in the folder SLEP to the path

% change to the original folder
cd Examples/tree;

m=50;  n=100;       % The data matrix is of size m x n

% ---------------------- generate random data ----------------------
%randn('state',(randNum-1)*3+1);
A=randn(m,n);        % the data matrix

y=[-ones(25,1); ones(25,1)];      % the response

%% In this example, the tree is set as:
%
% root, 1:100, with weight 0
% its children nodes, 1:50, and 51:100
%
% For 1:50, its children are 1:20, 21:40, and 41:50
%
% For 51:100, its children are 51:70, and 71:100
%
% These nodes in addition have each individual features (they contain) as
% children nodes.
%
%%

%% One efficient way
% We make use of the fact that the indices of the left nodes of the tree
% are smaller than the right nodes.

%----------------------- Set optional items -----------------------
opts=[];

% Starting point
opts.init=2;        % starting from a zero point

% Termination
opts.tFlag=5;       % run .maxIter iterations
opts.maxIter=100;   % maximum number of iterations

% regularization
opts.rFlag=1;       % use ratio

% Normalization
opts.nFlag=0;       % without normalization

% Group Property
opts.ind=[[-1, -1, 1]',... % leave nodes (each node contains one feature)
    [1, 20, sqrt(20)]', [21, 40, sqrt(20)]',... % the layer above the leaf
    [41, 50, sqrt(10)]', [51, 70, sqrt(20)]', [71,100, sqrt(30)]',...
    [1, 50, sqrt(50)]', [51, 100, sqrt(50)]']; % the higher layer

%----------------------- Run the code mc_cgLassoLeast -----------------------
z=0.1;
tic;
[x, c, funVal, ValueL]= tree_LogisticR(A, y, z, opts);
toc;

%% An alternative way
% We make use of the fact that the indices of the left nodes of the tree
% are smaller than the right nodes.
%%

%----------------------- Set optional items -----------------------
opts=[];

% Starting point
opts.init=2;        % starting from a zero point

% Termination
opts.tFlag=5;       % run .maxIter iterations
opts.maxIter=100;   % maximum number of iterations

% regularization
opts.rFlag=1;       % use ratio

% Normalization
opts.nFlag=0;       % without normalization

% Group Property
opts.ind=[[-1, -1, 1]',... % leave nodes (each node contains one feature)
    [1, 20, sqrt(20)]', [21, 40, sqrt(20)]',... % the layer above the leaf
    [41, 50, sqrt(10)]', [51, 70, sqrt(20)]', [71,100, sqrt(30)]',...
    [101, 150, sqrt(50)]', [151, 200, sqrt(50)]']; % the higher layer
opts.G=[1:100, 1:100];

%----------------------- Run the code mc_cgLassoLeast -----------------------
z=0.1;
tic;
[x2, c2, funVal2, Value2L]= tree_LogisticR(A, y, z, opts);
toc;


function [x, infos] = acc_online_mu_nmf(V, rank, in_options)
% Accelerated online non-negative matrix factorization (ONMF) algorithm.
%
% Inputs:
%       matrix      V
%       rank        rank
%       options     options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       S. S. Bucak, B. Gunsel,
%       "Incremental Subspace Learning via Non-negative Matrix Factorization,"
%       Pattern Recognition, 2009.
%
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai and H.Sakai on Feb. 12, 2017
%
% Change log:
%
%       Feb. 12, 2017 (Hiroyuki Kasai): Fixed algorithm.
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.rep_mode = 'fix';
    local_options.w_repeat = 1;
    local_options.h_repeat = 1;
    local_options.alpha    = 2;
    local_options.delta    = 0.1;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    Wt = init_factors.W;
    H = init_factors.H;

    % initialize
    method_name = 'ACC-Online-NMF';
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    %At = zeros(m, rank);
    %Bt = zeros(rank, rank);

    % initialize for this algorithm
    if strcmp(options.rep_mode, 'adaptive')
        K = m*n;
        rhoh = 1+(K+m*rank)/(n*(rank+1));
    end

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, Wt, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main outer loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % Reset sufficient statistic
        At = zeros(m, rank);
        Bt = zeros(rank, rank);

        % main inner loop
        for t = 1 : options.batch_size : n - 1

            % retrieve vt and ht
            vt = V(:, t:t+options.batch_size-1);
            ht = H(:, t:t+options.batch_size-1);

%             % uddate ht
%             Wtv = Wt.' * vt;
%             WtW = Wt.' * Wt;
%             for iii=1:h_repeat
%                 ht = ht .* (Wtv) ./ (WtW * ht);
%                 ht = ht + (ht<eps) .* eps;
%             end

            Wtv = Wt.' * vt;
            WtW = Wt.' * Wt;
            if strcmp(options.rep_mode, 'adaptive')
                gamma = 1;
                eps0 = 1;
                j = 1;
                rhoh_alpha = rhoh * options.alpha;

                %while j <= floor(1+rhoh*alpha) &&  gamma >= delta*eps0
                while j <= rhoh_alpha && gamma >= options.delta * eps0
                    ht0 = ht;
                    ht = ht .* (Wtv) ./ (WtW * ht);
                    ht = ht + (ht<eps) .* eps;
                    if j == 1
                        eps0 = norm(ht0 - ht);
                    end
                    gamma = norm(ht0 - ht);
                    j = j+1;
                end
            else
                for iii = 1 : options.h_repeat
                    ht = ht .* (Wtv) ./ (WtW * ht);
                    ht = ht + (ht<eps) .* eps;
                end
            end

            % update sufficient statistics
            At = At + vt *  ht';
            Bt = Bt + ht *  ht';

            % update W
            for iii = 1 : options.w_repeat
                Wt = Wt .* At ./ (Wt * Bt);
                Wt = Wt + (Wt<eps) .* eps;
            end

            % store new h
            H(:, t:t+options.batch_size-1) = ht;

            grad_calc_count = grad_calc_count + m * options.batch_size;
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, Wt, H, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = Wt;
    x.H = H;
    x.R = [];

end


function nn = nnff(nn, x, y)
%NNFF performs a feedforward pass
% nn = nnff(nn, x, y) returns an neural network structure with updated
% layer activations, error and loss (nn.a, nn.e and nn.L)

    n = nn.n;
    m = size(x, 1);

    nn.a{1} = x;

    %feedforward pass
    for i = 2 : n-1
        nn.a{i} = sigm(repmat(nn.b{i - 1}', m, 1) + nn.a{i - 1} * nn.W{i - 1}');
        if(nn.dropoutFraction > 0)
            if(nn.testing)
                nn.a{i} = nn.a{i}.*(1 - nn.dropoutFraction);
            else
                nn.a{i} = nn.a{i}.*(rand(size(nn.a{i}))>nn.dropoutFraction);
            end
        end
        %calculate running exponential activations for use with sparsity
        if(nn.nonSparsityPenalty>0)
            nn.p{i} = 0.99 * nn.p{i} + 0.01 * mean(nn.a{i}, 1);
        end
    end
    switch nn.output
        case 'sigm'
            nn.a{n} = sigm(repmat(nn.b{n - 1}', m, 1) + nn.a{n - 1} * nn.W{n - 1}');
        case 'linear'
            nn.a{n} = repmat(nn.b{n - 1}', m, 1) + nn.a{n - 1} * nn.W{n - 1}';
        case 'softmax'
            nn.a{n} = repmat(nn.b{n - 1}', m, 1) + nn.a{n - 1} * nn.W{n - 1}';
            nn.a{n} = exp(bsxfun(@minus, nn.a{n}, max(nn.a{n},[],2)));
            nn.a{n} = bsxfun(@rdivide, nn.a{n}, sum(nn.a{n}, 2));
    end

    %error and loss
    nn.e = y - nn.a{n};
    switch nn.output
        case {'sigm','linear'}
            nn.L = 1/2 * sum(sum(nn.e .^ 2)) / m;
        case 'softmax'
            nn.L = -sum(sum(y .* log(nn.a{n}))) / m;
    end
end


function [H,S,D]=sobi(X,n,p)
% SOBI - Second Order Blind Identification (SOBI) by joint diagonalization of
%          correlation  matrices. THIS CODE ASSUMES TEMPORALLY CORRELATED SIGNALS,
%          and uses correlations across times in performing the signal separation.
%          Thus, estimated time delayed covariance matrices must be nonsingular
%          for at least some time delays.
% Usage:
%         >> winv = sobi(data);
%         >> [winv,act] = sobi(data,n,p);
% Inputs:
%   data - data matrix of size [m,N] ELSE of size [m,N,t] where
%                m is the number of sensors,
%                N is the  number of samples,
%                t is the  number of trials (avoid epoch boundaries)
%         n - number of sources {Default: n=m}
%         p - number of correlation matrices to be diagonalized
%             {Default: min(100, N/3)} Note that for non-ideal data,
%             the authors strongly recommend using at least 100 time delays.
%
% Outputs:
%   winv - Matrix of size [m,n], an estimate of the *mixing* matrix. Its
%          columns are the component scalp maps. NOTE: This is the inverse
%          of the usual ICA unmixing weight matrix. Sphering (pre-whitening),
%          used in the algorithm, is incorporated into winv. i.e.,
%
%             >> icaweights = pinv(winv); icasphere = eye(m);
%
%   act  - matrix of dimension [n,N] an estimate of the source activities
%
%             >> data            = winv            * act;
%                [size m,N]        [size m,n]        [size n,N]
%             >> act = pinv(winv) * data;
%
% Authors:  A. Belouchrani and A. Cichocki (references: See function body)
% Note:     Adapted by Arnaud Delorme and Scott Makeig to process data epochs by
%           computing covariances while respecting epoch boundaries.

% REFERENCES:
% A. Belouchrani, K. Abed-Meraim, J.-F. Cardoso, and E. Moulines, ``Second-order
%  blind separation of temporally correlated sources,'' in Proc. Int. Conf. on
%  Digital Sig. Proc., (Cyprus), pp. 346--351, 1993.
%
%  A. Belouchrani and K. Abed-Meraim, ``Separation aveugle au second ordre de
%  sources correlees,'' in  Proc. Gretsi, (Juan-les-pins),
%  pp. 309--312, 1993.
%
%  A. Belouchrani, and A. Cichocki,
%  Robust whitening procedure in blind source separation context,
%  Electronics Letters, Vol. 36, No. 24, 2000, pp. 2050-2053.
%
%  A. Cichocki and S. Amari,
%  Adaptive Blind Signal and Image Processing, Wiley,  2003.

% Authors note: For non-ideal data, use at least p=100 the time-delayed covariance matrices.
DEFAULT_LAGS = 100;

[m,N,ntrials]=size(X);

if nargin<1 || nargin > 3

    help sobi

elseif nargin==1

    n=m; % Source detection (hum...)
    p=min(DEFAULT_LAGS,ceil(N/3)); % Number of time delayed correlation matrices to be diagonalized

elseif nargin==2

    p=min(DEFAULT_LAGS,ceil(N/3)); % Default number of correlation matrices to be diagonalized
    % Use < DEFAULT_LAGS delays if necessary for short data epochs
end

%
% Make the data zero mean
%
X(:,:)=X(:,:)-kron(mean(X(:,:)')',ones(1,N*ntrials));

%
% Pre-whiten the data based directly on SVD
%
[UU,S,VV]=svd(X(:,:)',0);
Q= pinv(S)*VV';
X(:,:)=Q*X(:,:);

% Alternate whitening code
% Rx=(X*X')/T;
% if m<n, % assumes white noise
%   [U,D]=eig(Rx);
%   [puiss,k]=sort(diag(D));
%   ibl= sqrt(puiss(n-m+1:n)-mean(puiss(1:n-m)));
%    bl = ones(m,1) ./ ibl ;
%   BL=diag(bl)*U(1:n,k(n-m+1:n))';
%   IBL=U(1:n,k(n-m+1:n))*diag(ibl);
% else    % assumes no noise
%    IBL=sqrtm(Rx);
%    Q=inv(IBL);
% end
% X=Q*X;

%
% Estimate the correlation matrices
%
k=1;
pm=p*m; % for convenience
for u=1:m:pm
    k=k+1;
    for t = 1:ntrials
        if t == 1
            Rxp=X(:,k:N,t)*X(:,1:N-k+1,t)'/(N-k+1)/ntrials;
        else
            Rxp=Rxp+X(:,k:N,t)*X(:,1:N-k+1,t)'/(N-k+1)/ntrials;
        end
    end
    M(:,u:u+m-1)=norm(Rxp,'fro')*Rxp;  % Frobenius norm =
end                                    % sqrt(sum(diag(Rxp'*Rxp)))

%
% Perform joint diagonalization
%
epsil=1/sqrt(N)/100;
encore=1;
V=eye(m);
step_n=0;
while encore
    encore=0;
    for p=1:m-1
        for q=p+1:m
            % Perform Givens rotation
            g=[   M(p,p:m:pm)-M(q,q:m:pm)  ;
                M(p,q:m:pm)+M(q,p:m:pm)  ;
                i*(M(q,p:m:pm)-M(p,q:m:pm)) ];
            [vcp,D] = eig(real(g*g'));
            [la,K]=sort(diag(D));
            angles=vcp(:,K(3));
            angles=sign(angles(1))*angles;
            c=sqrt(0.5+angles(1)/2);
            sr=0.5*(angles(2)-j*angles(3))/c;
            sc=conj(sr);
            oui = abs(sr)>epsil ;
            encore=encore | oui ;
            if oui  % Update the M and V matrices
                colp=M(:,p:m:pm);
                colq=M(:,q:m:pm);
                M(:,p:m:pm)=c*colp+sr*colq;
                M(:,q:m:pm)=c*colq-sc*colp;
                rowp=M(p,:);
                rowq=M(q,:);
                M(p,:)=c*rowp+sc*rowq;
                M(q,:)=c*rowq-sr*rowp;
                temp=V(:,p);
                V(:,p)=c*V(:,p)+sr*V(:,q);
                V(:,q)=c*V(:,q)-sc*temp;
            end %% if
        end %% q loop
    end %% p loop
    step_n=step_n+1;
    fprintf('%d step\n',step_n);
end %% while

%
% Estimate the mixing matrix
%
H = pinv(Q)*V;

%
% Estimate the source activities
%
if nargout>1
    S=V'*X(:,:); % estimated source activities
end


% StackExchange Signal Processing Q62024
% https://dsp.stackexchange.com/questions/62024
% Proximal Gradient Method (PGM) for a Function Model with More than 2 Functions (Sum of Functions)
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     25/11/2019
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0; %<! Continue from Question 1
figureCounterSpec   = '%04d';

generateFigures = OFF;

%% Simulation Parameters

numElements = 40;
paramLambda1 = 0.5; %<! L1 Norm
paramLambda2 = 0.75; %<! TV Norm

numIterations = 1000; %<! For the ADMM

%% Generate Data

vY = 10 * randn(numElements, 1);

% Generate the Diff Operator (1D Gradient) by Finite Differences
mD = spdiags([-ones(numElements, 1), ones(numElements, 1)], [0, 1], numElements - 1, numElements);

hSolveProxTv = @(vY, paramLambda) SolveProxTvAdmm(vY, mD, paramLambda, numIterations);

% Objective Function
hObjFun = @(vX) (0.5 * sum( (vX - vY) .^ 2)) + (paramLambda1 * sum(abs(vX))) + (paramLambda2 * sum(abs(mD * vX)));

%% Solution by CVX

cvx_begin('quiet')
    cvx_precision('best');
    variable vX(numElements);
    minimize( (0.5 * pow_pos(norm(vX - vY, 2), 2)) + (paramLambda1 * norm(vX, 1)) + (paramLambda2 * norm(mD * vX, 1)));
cvx_end

disp([' ']);
disp(['CVX Solution Summary']);
disp(['The CVX Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(cvx_optval)]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp([' ']);

%% Solution by Analytical Solution
%{
Solving $ \arg \min_x \frac{1}{2} {\left\| x - y \right\|}_{2}^{2} +
{\lambda}_{1} {\left\| x \right\|}_{1} + {\lambda}_{2} {\left\| D x \right\|}_{1} $
%}

vX = SolveProxL1(hSolveProxTv(vY, paramLambda2), paramLambda1);

disp([' ']);
disp(['Analytical Direct Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp([' ']);

%% Solution by Analytical Solution
%{
Solving $ \arg \min_x \frac{1}{2} {\left\| x - y \right\|}_{2}^{2} +
{\lambda}_{1} {\left\| x \right\|}_{1} + {\lambda}_{2} {\left\| D x \right\|}_{1} $
%}

vX = hSolveProxTv(SolveProxL1(vY, paramLambda1), paramLambda2);

disp([' ']);
disp(['Analytical Direct Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp([' ']);

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function [sol, infos] = gsp_ml_rls(xl, y, k, tau, A, At, param)
%GSP_ML_RLS Manifold Learning regularized least square
%   Usage: sol = gsp_ml_rls(xl, y, k, tau);
%          sol = gsp_ml_rls(xl, y, k, tau, A, At);
%          sol = gsp_ml_rls(xl, y, k, tau,  A, At, param);
%          [sol, infos] = gsp_ml_rls(...)
%
%   Input parameters:
%       xl      : labeled points
%       y       : labels
%       k       : kernel
%       tau     : regularization parameters
%       A       : Operator
%       At      : Adoint operator
%       param   : Optional parameters
%   Output parameters:
%       sol     : solution of the problem (kernel coefficients)
%       infos   : convergence info
%
%   *param* is a structure of optional argument given to the solver
%   gradient_descent. Please see the function gradient descent for more
%   information.
%
%   In *param*, you also have to set an upperbound for the operator A as
%   param.nu!
%
%   This function solves the following problem:
%
%   ..  argmin_alpha  || A (K alpha) - y ||_2^2 + tau *alpha^T K alpha
%
%   If tau is set to zero, then the following problem is solved
%
%   ..  argmin_alpha alpha^T K alpha s. t.  A (K alpha) = y
%

% Author: Nathanael Perraudin
% Date  : 8 decembre 2014

if nargin<5
    A = @(x) x;
end

if nargin<6
    At = A;
end

if nargin<7
    param = struct;
end

if ~isfield(param, 'tol'), param.tol = 1e-6; end
if ~isfield(param, 'nu'), param.nu = 1; end
if ~isfield(param, 'verbose'), param.verbose = 1; end

%N = size(xl,2);

% Evaluate the kernel on the data points
K = gsp_rkhs_evaluate(k,xl);
nu = norm(K);

[N,M] = size(y);
Nk = length(k);
alpha_in = zeros(N*Nk,M);

if tau >0

    fp.eval = @(x) tau * sum(norm_rkhs( K,x ));
    fp.grad = @(x) tau * grad_rkhs( K,x);
    fp.beta =   2*nu*tau;

    ffid.eval = @(x) norm(A(K*x)-y,'fro')^2;
    ffid.grad = @(x) 2*K'*At(A(K*x)-y);
    ffid.beta = 2*nu^2*param.nu^2;

%     paramfid.A = @(x) A(K*x);
%     paramfid.At = @(x) K'*At(x);
%     paramfid.nu = nu^2;
%     paramfid.tight = 0;
%     paramfid.y = y;
%     paramfid.verbose = param.verbose -1;
%     ffid.eval = @(x) norm(A(K*x)-y,'fro')^2;
%     ffid.prox = @(x,T) prox_l2(x,T,paramfid);

    [sol,infos] = solvep(alpha_in, {fp,ffid}, param);

%     param.gamma = 0.5/(tau*nu);
%     [sol,infos] = forward_backward(alpha_in, ffid, fp, param);
else

    fp.eval = @(x) sum(norm_rkhs( K,x ));
    fp.grad = @(x) grad_rkhs( K,x);
    fp.beta =   2*nu;

    paramproj.A = @(x) A(K*x);
    paramproj.At = @(x) K'*At(x);
    paramproj.nu = nu^2*param.nu^2;
    paramproj.tight = 0;
    paramproj.verbose = param.verbose-1;
    paramproj.y = y;
    paramproj.maxit = 50;
    ffid.eval = @(x) eps;
    ffid.prox = @(x,T) proj_b2(x,T,paramproj);

    [sol,infos] = forward_backward(alpha_in, ffid, fp, param);

end

end


function linpack_z_test14 ( )

%*****************************************************************************80
%
%% TEST14 tests ZHPCO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST14\n' );
  fprintf ( 1, '  For a double precision complex (C)\n' );
  fprintf ( 1, '  Hermitian matrix using packed storage (HP),\n' );
  fprintf ( 1, '  ZHPCO factors the matrix and estimates\n' );
  fprintf ( 1, '  the reciprocal condition number.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix order is N = %d\n', n );
%
%  Set the values of the matrix A.
%
  k = 0;
  seed = 123456789;

  for j = 1 : n

    for i = 1 : j-1
      k = k + 1;
      [ a(k), seed ] = c8_uniform_01 ( seed );
      a_save(i,j) = a(k);
      a_save(j,i) = conj ( a(k) );
    end

    k = k + 1;
    [ a(k), seed ] = r8_uniform_01 ( seed );
    a_save(j,j) = a(k);

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix A:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  (%8f  %8f)', real ( a_save(i,j) ), imag ( a_save(i,j) ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Factor the matrix A.
%
  [ a, ipvt, rcond ] = zhpco ( a, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimated reciprocal condition RCOND = %f\n', rcond );

  return
end