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

%%% H-Matrix (recursion)
if (Mh.typ == 0)
    for i = 1:4
        Mh.chd{i} = hmxCopy(Mh.chd{i},tol);
        Mh.tol    = max(tol,Mh.tol);
    end
    Mh = hmxFusion(Mh);

%%% Compressed leaf
elseif (Mh.typ == 1)
    if (tol > Mh.tol)
        [A,B]  = hmxQRSVD(Mh.dat{1},Mh.dat{2},tol);
        Mh.dat = {A,B};
        Mh.tol = tol;
    end

%%% Full leaf
elseif (Mh.typ == 2)

%%% Unknown type
else
    error('hmxCopy.m : unavailable case')
end
end


function [ ap, det, inert ] = chpdi ( ap, n, ipvt, job )

%*****************************************************************************80
%
%% CHPDI: determinant, inertia and inverse of a complex hermitian matrix.
%
%  Discussion:
%
%    The routine uses the factors from CHPFA.
%
%    The matrix is stored in packed form.
%
%    A division by zero will occur if the inverse is requested and CHPCO has
%    set RCOND == 0.0 or CHPFA has set INFO ~= 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex AP(N*(N+1)/2); the factored matrix
%    from CHPFA.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer IPVT(N), the pivot vector from CHPFA.
%
%    Input, integer JOB, has the decimal expansion ABC where:
%    if C ~= 0, the inverse is computed,
%    if B ~= 0, the determinant is computed,
%    if A ~= 0, the inertia is computed.
%    For example, JOB = 111 gives all three.
%
%    Output, complex AP(N*(N+1)/2); if the inverse was requested, then
%    the upper triangle of the inverse of the original matrix, stored in packed
%    form.  The columns of the upper triangle are stored sequentially in a
%    one-dimensional array.
%
%    Output, real DET(2), if requested, the determinant of the original matrix.
%    Determinant = DET(1) * 10.0**DET(2) with 1.0 <= abs ( DET(1) ) < 10.0
%    or DET(1) = 0.0.
%
%    Output, integer INERT(3), if requested, the inertia of the original matrix.
%    INERT(1) = number of positive eigenvalues.
%    INERT(2) = number of negative eigenvalues.
%    INERT(3) = number of zero eigenvalues.
%
  det = [];
  inert = [];

  noinv = mod ( job, 10 ) == 0;
  nodet = floor ( mod ( job, 100 ) / 10 ) == 0;
  noert = floor ( mod ( job, 1000 ) / 100 ) == 0;

  if ( ~nodet | ~noert )

    if ( ~noert )
      inert(1:3) = 0;
    end

    if ( ~nodet )
      det(1) = 1.0;
      det(2) = 0.0;
    end

    t = 0.0;
    ik = 0;

    for k = 1 : n

      kk = ik + k;
      d = real ( ap(kk) );
%
%  Check if 1 by 1
%
      if ( ipvt(k) <= 0 )
%
%  2 by 2 block
%  Use DET (D  S; S  C)  =  ( D / T * C - T ) * T, T = abs ( S )
%  to avoid underflow/overflow troubles.
%  Take two passes through scaling.  Use T for flag.
%
        if ( t == 0.0 )
          ikp1 = ik + k;
          kkp1 = ikp1 + k;
          t = abs ( ap(kkp1) );
          d = ( d / t ) * real ( ap(kkp1+1) ) - t;
        else
          d = t;
          t = 0.0;
        end

      end

      if ( ~noert )

        if ( 0.0 < d )
          inert(1) = inert(1) + 1;
        elseif ( d < 0.0 )
          inert(2) = inert(2) + 1;
        elseif ( d == 0.0 )
          inert(3) = inert(3) + 1;
        end

      end

      if ( ~nodet )

        det(1) = det(1) * d;

        if ( det(1) ~= 0.0 )

          while ( abs ( det(1) ) < 1.0 )
            det(1) = det(1) * 10.0;
            det(2) = det(2) - 1.0;
          end

          while ( 10.0 <= abs ( det(1) ) )
            det(1) = det(1) / 10.0;
            det(2) = det(2) + 1.0;
          end

        end

      end

      ik = ik + k;

    end

  end
%
%  Compute inverse(A).
%
  if ( ~noinv )

    k = 1;
    ik = 0;

    while ( k <= n )

      km1 = k - 1;
      kk = ik + k;
      ikp1 = ik + k;
      kkp1 = ikp1 + k;
%
%  1 by 1
%
      if ( 0 <= ipvt(k) )

        ap(kk) = 1.0 / real ( ap(kk) );

        if ( 1 <= km1 )

          work(1:km1) = ap(ik+1:ik+km1);

          ij = 0;
          for j = 1 : km1
            jk = ik + j;
            ap(jk) = conj ( ap(ij+1:ij+j) ) * tranpose ( work(1:j) );
            ap(ik+1:ik+j-1) = ap(ik+1:ik+j-1) + work(j) * ap(ij+1:ij+j-1);
            ij = ij + j;
          end

          ap(kk) = ap(kk) + real ( conj ( work(1:km1) ) * transpose ( ap(ik+1:ik+km1) ) );

        end

        kstep = 1;
%
%  2 by 2
%
      else

        t = abs ( ap(kkp1) );
        ak = real ( ap(kk) ) / t;
        akp1 = real ( ap(kkp1+1) ) / t;
        akkp1 = ap(kkp1) / t;
        d = t * ( ak * akp1 - 1.0 );
        ap(kk) = akp1 / d;
        ap(kkp1+1) = ak / d;
        ap(kkp1) = -akkp1 / d;

        if ( 1 <= km1 )

          work(1:km1) = ap(ikp1+1:ikp1+km1);

          ij = 0;
          for j = 1 : km1
            jkp1 = ikp1 + j;
            ap(jkp1) = conj ( ap(ij+1:ij+j) ) * transpose ( work(1:j) );
            ap(ikp1+1:ikp1+j-1) = ap(ikp1+1:ikp1+j-1) ...
              + work(j) * ap(ij+1:ij+j-1);
            ij = ij + j;
          end

          ap(kkp1+1) = ap(kkp1+1) ...
            + real ( conj ( work(1:km1) ) * transpose ( ap(ikp1+1) ) );

          ap(kkp1) = ap(kkp1) ...
            + conj ( ap(ik+1:ik+km1) ) * transpose ( ap(ikp1+1:ikp1+km1) );

          work(1:km1) = ap(ik+1:ik+km1);

          ij = 0;

          for j = 1 : km1
            jk = ik + j;
            ap(jk) = conj ( ap(ij+1:ij+j) ) * transpose ( work(1:j) );
            ap(ik+1:ik+j-1) = ap(ik+1:ik+j-1) + work(j) * ap(ij+1:ij+j-1);
            ij = ij + j;
          end

          ap(kk) = ap(kk) ...
            + real ( conj ( work(1:km1) ) * transpose ( ap(ik+1:ik+km1) ) );

        end

        kstep = 2;

      end
%
%  Swap
%
      ks = abs ( ipvt(k) );

      if ( ks ~= k )

        iks = ( ks * ( ks - 1 ) ) / 2;

        temp             = ap(iks+1:iks+ks);
        ap(iks+1:iks+ks) = ap(ik+1:ik+ks);
        ap(ik+1:ik+ks)   = temp;

        ksj = ik + ks;

        for jb = ks : k

          j = k + ks - jb;
          jk = ik + j;

          temp    = conj ( ap(jk) );
          ap(jk)  = conj ( ap(ksj) );
          ap(ksj) = temp;

          ksj = ksj - ( j - 1 );

        end

        if ( kstep ~= 1 )

          kskp1 = ikp1 + ks;

          temp      = ap(kskp1);
          ap(kskp1) = ap(kkp1);
          ap(kkp1)  = temp;

        end

      end

      ik = ik + k;

      if ( kstep == 2 )
        ik = ik + k + 1;
      end

      k = k + kstep;

    end

  end

  return
end


function [x,info,Ai,Bi,BTi,Res,Pro,isFreeDof] = mg(A,b,elem,option,varargin)
%% MG multigrid-type solvers
%
% x = MG(A,b,elem) attempts to solve the system of linear equations A*x =
% b for x using geometric multigrid solvers. Inside mg, an coarsening algorithm
% is applied. See coarsen for the coarsening algorithm on bisection grids.
%
% The method is designed for the system from several finite element
% descritzations of elliptic equations on a grid whose topology
% is given by the array elem.
%
% - 2D: P0, P1, P2, P3, CR, WG
% - 3D: P0, P1, P2, CR, WG
%
% The algorithm is based on fast auxiliary space preconditioner (FASP).
% We first transfer a given element to P1 element and then build V-cycle
% for P1 element. The whole cycle is used as a preconditioner in CG to
% solve the linear system.
%
% Reference: J. Xu, The auxiliary space method and optimal multigrid
% preconditioning techniques for unstructured grids, Computing. 56 (1996)
% 215?235.
%
% The type of the finite element can be build into the size of the problem.
%
% - NT: number of elements         P0 element
% - N : number of vertices         P1 element
% - NE: number of edges            CR element in 2D
% - NF: number of faces            CR element in 3D
% - N + NE: sum of vertices and edges    P2 element
% - N + 2*NE + NT:                       P3 element in 2D
%
% For Dirichlet problems, usually the matrix equation can be restricted to
% free dofs and results a smaller matrix in which the link of the size of
% the matrix and the type of elements is missing. In this case, use
% option.freeDof to provide a logic array for free dofs and the length of
% option.freeDof can be used to determine the type.
%
% So for Dirichlet problems, use
% - mg(A,b,elem,option)      with option.freeDof is given
% - mg(AD,b,elem)       where AD is a larger matrix containing boundary dof
%
% For elements involving dof on edges and faces, additional mesh structure
% such as |edge|, |face| can be provided. Otherwise, the subroutine will
% generate them using |elem|.
%
% For 3-D *adaptive meshes*, HB information is needed, and should be
% listed as the first parameter in varargin.
%
%   - mg(A,b,elem,option,HB)            3-D linear P1 element
%   - mg(A,b,elem,option,HB,edge)       3-D quadratic P2 element
%   - mg(A,b,elem,option,HB,face)       3-D non-conforming CR P1 element
%
% For 3-D meshes obtained by uniformrefine3 or uniformbisect3, no HB is
% needed and simple mg(A,b,elem) is okay.
%
% In some applications, multigrid solvers are used as a build-in block of
% other larger systems. Then use
%
% [~,~,Ai,Bi,BBi,Res,Pro,isFreeDof] = mg(A,f,elem,setupOption);
%
% with setupOption.solver = 'NO' to get hierarichal structure and then use
%
%   e = mg(A,r,elem,option,Ai,Bi,BBi,Res,Pro,isFreeDof);
%
% This will save the setting time. See diapreStokes, tripremixPoisson.
%
% When testing the solvers, more options can be provided.
%
%   x = MG(A,b,elem,options) specifies options in the following list.
%   - option.x0: the initial guess. Default setting x0 = 0.
%   - option.tol: the tolerance of the convergence. Default setting 1e-8.
%   - option.maxIt: the maximum number of iterations. Default setting 200.
%   - option.N0: the size of the coarest grid. Default setting 500.
%   - option.mu: smoothing steps. Default setting 1
%   - option.coarsegridsolver: solver used in the coarest grid. Default
%     setting: direct solver.
%   - option.freeDof: free d.o.f
%   - option.solver: various cycles and Krylov space methods
%       * 'NO'     only setup the transfer matrix
%       * 'Vcycle'      V-cycle MultiGrid Method
%       * 'Wcycle'      W-cycle MultiGrid Method
%       * 'Fcycle'      Full cycle Multigrid Method
%       * 'cg'     MG-Preconditioned Conjugate Gradient
%       * 'minres' MG-Preconditioned Minimal Residual Method
%       * 'gmres'  MG-Preconditioned Generalized Minimal Residual Method
%       * 'bicg'   MG-Preconditioned BiConjugate Gradient Method
%       * 'bicgstable' MG-Preconditioned BiConjugate Gradient Stabilized Method
%       * 'bicgstable1' MG-Preconditioned BiConjugate Gradient Stabilized Method
%       The default setting is 'cg' which works well for SPD matrices. For
%       non-symmetric matrices, try 'gmres' and for symmetric but indefinite
%       matrices, try 'minres' or 'bicg' sequences.
%       The string option.solver is not case sensitive.
%   - option.preconditioner:  multilevel preconditioners including:
%       * 'V'   V-cycle MultiGrid used as a Preconditioner
%       * 'W'   W-cycle MultiGrid used as a Preconditioner
%       * 'F'   Full cycle Multigrid used as a Preconditioner
%       * 'bpx' BPX-Preconditioner
%   - option.printlevel: the level of screen print out
%       * 0: no output
%       * 1: name of solver and convergence information (step, err, time)
%       * 2: convergence history (err in each iteration step)
%
%   [x,info] = MG(A,b,elem) also returns information of the solver
%   - info.flag:
%       * 0: mg converged to the desired tolerance tol within maxIt iterations
%       * 1: mg iterated maxIt times but did not converge.
%       * 2: direct solver
%   - info.itStep: the iteration number at which x was computed.
%   - info.time: the cpu time to get x
%   - info.err: the approximate relative error in the energy norm in
%   err(:,1) and the relative residual norm(b-A*x)/norm(b) in err(:,2). If
%   flag is 0, then max(err(end,:)) <= tol.
%   - info.stopErr: the error when iteration stops
%
%   Example:
%
%   [node,elem] = squaremesh([0 1 0 1],0.5);
%   for k = 1:8
%     [node,elem] = uniformrefine(node,elem);
%   end
%   pde.f = inline('p(:,1).*p(:,2)','p');
%   pde.g_D = inline('zeros(size(p,1),1)','p');
%   option.solver = 'notsolve';
%   [soln,eqn] = Poisson(node,elem,[],pde,option);
%   fprintf('\n Number of unknowns: %8.0u\n',length(eqn.b))
%   tic; display('Direct solver'); u = eqn.A\eqn.b; toc;
%   tic; x = mg(eqn.A,eqn.b,elem); toc;
%   format shorte
%   fprintf('Difference between direct and mg solvers %0.2g \n',norm(u-x));
%
% See also mgMaxwell, mgstokes
%
% Documentation in Help browser ifem mgdoc
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

t = cputime;
%% Size of systems
Nborig = size(b,1);                % number of dof
Ndof = Nborig;
nb = size(b,2);                    % number of bs
N = max(elem(:));                  % number of nodes
NT = size(elem,1);                 % number of elements
dim = size(elem,2)-1;
if N > NT       % 2-D quad mesh
    dim = 2;
end

%% Options
% Assign default values to unspecified parameters
if ~exist('option','var')
    option = [];
end
option = mgoptions(option,Nborig);    % parameters
x0 = option.x0;
if size(x0,2) == 1
    x0 = repmat(x0,1,nb);
end
N0 = option.N0;
tol = option.tol;
maxIt = option.solvermaxit;
mu = option.smoothingstep;
smoothingRatio = option.smoothingratio; % for variable smoothing
solver = option.solver;
preconditioner = option.preconditioner;
coarsegridsolver = option.coarsegridsolver;
printlevel = option.printlevel;
setupflag = option.setupflag;
if nargin > 8      % with the multilevel structure in the input
    setupflag = 0;
end

%% Set up multilevel structure
if setupflag == true
%% eliminate isolated dof
if isfield(option,'freeDof') % freeDof is given
   if islogical(option.freeDof)
      Ndof = max(length(option.freeDof),Nborig);
   else
      error('Provide logic array for option.freeDof');
   end
   isFreeDof = false(Ndof,1);
   isFreeDof(option.freeDof) = true;
   isFixDof = true(Ndof,1);
   isFixDof(isFreeDof) = false;
else % find free dofs and eliminate isolated dofs
    Ndof = Nborig;
    degreeA = sum(spones(A));  % degree
    isFreeDof = false(size(A,1),1);
    isFreeDof(degreeA>1) = true;
    isFixDof = ~isFreeDof;
    isFixDof(degreeA == 0) = false;
end
if Nborig > sum(isFreeDof) % truncate the matrix
    xD = zeros(Nborig,nb);
    Nfix = sum(isFixDof);
    ADinv = spdiags(1./diag(A(isFixDof,isFixDof)),0,Nfix,Nfix);
    xD(isFixDof,:) = ADinv*b(isFixDof,:);
    A = A(isFreeDof,isFreeDof);
end
if Nborig > sum(isFreeDof) % truncate the rhs
    b = b(isFreeDof,:);
end
if size(x0,1) > sum(isFreeDof) % truncate the initial guess
    x0 = x0(isFreeDof,:);
    option.x0 = x0;
end
isFreeNode = isFreeDof(1:N); % the first N element is for P1

%% Additional mesh structure for different elements
% list of elements: P0,P2,P3,CR,HB,WG
if Ndof > N % other than P1 element
    NE = N + NT -1; % estimate of NE by Euler formula
    if dim == 3
        NF = 2*NT;  % estimate of NF
    end
    % get additional data structure
    if Ndof ~= NT   % not piecewise constant element
        if dim == 2
            if nargin > 4
                edge = varargin{1};
                NE = size(edge,1);
            else
                [tempvar,edge] = dofedge(elem);
            end
        elseif dim == 3
            if nargin > 5 % additional data structure is in the input
                if size(varargin{1},2) == 2 % edge
                    edge = varargin{1};
                    NE = size(edge,1);
                elseif size(varargin{1},2) == 3 % face
                    face = varargin{1};
                    NF = size(face,1);
                end
            else % if no additional edge/face is input, generate it
                 [tempvar,edge] = dof3edge(elem); %#ok<*ASGLU>
                 [tempvar,face] = dof3face(elem);
                 NE = size(edge,1);
                 NF = size(face,1);
            end
        end
    end
    % transfer operators from P1 to the current element
    isFreeNode = true(N,1);
    if Ndof == NT % piecewise constant element
       if dim == 2
          P1toP0 = sparse([1:NT;1:NT;1:NT]',elem,ones(3*NT,1)/3,NT,N);
       elseif dim == 3
          P1toP0 = sparse([1:NT;1:NT;1:NT;1:NT]',elem,ones(4*NT,1)/4,NT,N);
       end
       auxPro = P1toP0(isFreeDof,1:N);
       isFreeNode = [];
    end
    if Ndof == N + NE % quadratic element
        isFreeNode = isFreeDof(1:N);
        P1toP2 = sparse([(1:N)'; N+(1:NE)'; N+(1:NE)'], ...
                        [(1:N)'; double(edge(:))],...
                        [ones(N,1); 0.5*ones(2*NE,1)]',Ndof,N);
        auxPro = P1toP2(isFreeDof,isFreeNode);
    end
    if (dim == 2) && (Ndof == N + 2*NE +NT) % cubic element in 2-D
        isFreeNode = isFreeDof(1:N);
        P1toP3 = sparse([(1:N)'; N+2*(1:NE)'-1; N+2*(1:NE)'-1; N+2*(1:NE)';...
                          N+2*(1:NE)';N+2*NE+(1:NT)';N+2*NE+(1:NT)';N+2*NE+(1:NT)'], ...
                        [(1:N)'; double(edge(:));double(edge(:));elem(:);],...
                        [ones(N,1); 2/3*ones(NE,1);1/3*ones(NE,1);...
                         1/3*ones(NE,1);2/3*ones(NE,1);1/3*ones(3*NT,1)]',Ndof,N);
         auxPro = P1toP3(isFreeDof,isFreeNode);
    end
    if (dim == 2) && (Ndof == NE)  % 2-D CR nonconforming element
        isFreeNode(edge(isFixDof,:)) = false;
        P1toCR = sparse([1:NE;1:NE]',double(edge(:)),0.5*ones(2*NE,1),NE,N);
        auxPro = P1toCR(isFreeDof,isFreeNode);
    end
    if (dim == 3) && (Ndof == NF) % 3-D CR nonconforming element
        isFreeNode(face(isFixDof,:)) = false;
        P1toCR = sparse([1:NF;1:NF;1:NF]',double(face(:)),ones(3*NF,1)/3,NF,N);
        auxPro = P1toCR(isFreeDof,isFreeNode);
    end
    if (dim == 2) && (Ndof == NT + NE) % 2-D weak Galerkin element (P0,P0,RT0) element
        fixEdgeDof = find(isFixDof) - NT;
        isFreeNode(edge(fixEdgeDof,:)) = false;
        P1toWG = sparse([repmat((1:NT)',3,1); repmat((NT+1:Ndof)',2,1)], ... % i
                        [elem(:);             double(edge(:))], ...   % j
                        [ones(3*NT,1)/3;      ones(2*NE,1)/2], NT+NE, N);
        auxPro = P1toWG(isFreeDof,isFreeNode);
    end
    if (dim == 3) && (Ndof == NT + NF) % 3-D weak Galerkin element (P0,P0,RT0) element
        fixFaceDof = find(isFixDof) - NT;
        isFreeNode(face(fixFaceDof,:)) = false;
        P1toWG = sparse([repmat((1:NT)',4,1); repmat((NT+1:Ndof)',3,1)], ... % i
                        [elem(:);           double(face(:))], ...   % j
                        [ones(4*NT,1)/4;    ones(3*NF,1)/3], NT+NF, N);
        auxPro = P1toWG(isFreeDof,isFreeNode);
    end
    if (nargin > 4) && (dim == 2) && ischar(varargin{1}) && strcmp(varargin{1},'HB') % HB basis element
        isFreeNode = isFreeDof(1:N);
        P1toPk = speye(Ndof,N);
        auxPro = P1toPk(isFreeDof,isFreeNode);
    end
end

%% Hierarchical Structure of Mesh
if dim == 2  % 2-D
    [HB, NL, level] = HBstructure(elem,N0);
end
if dim == 3  % 3-D
    if nargin > 4
        HBmesh = varargin{end}; % need HB for bisection refinement
        [HB, NL, level] = HBstructure3(elem,N0,HBmesh);
    else % no HBmesh is given. only work for the red uniform refinement
        [HB, NL, level] = HBstructure3(elem,N0);
    end
end

%% Transfer operators between multilevel meshes for P1 element
% standard prolongation and restriction operator for P1 element
[Pro,Res] = transferoperator(HB,NL,isFreeNode);
% add one more level from P1 to the current element
if Ndof > N
    if ~exist('auxPro','var')
        disp('The current element is not supported by mg');
    else
        Pro{level} = auxPro;
        Res{level+1} = Pro{level}';
        level = level + 1;
    end
end
clear HB auxPro

%% Matrices in each level
Ai = cell(level,1);
Ai{level} = A;
if level == 1
    Bi{1} = tril(Ai{1});
    BTi{1} = transpose(Bi{1});
    Res = []; Pro = [];
end
for j = level:-1:2
    Ai{j-1} = Res{j}*Ai{j}*Pro{j-1};           % Ac = Res*Af*Pro
    switch option.smoother
        case 'GS'
            Bi{j} = tril(Ai{j});        % Forward Gauss-Seidel   B = D+L
            BTi{j} = triu(Ai{j});       % Backward Gauss-Seidel BT = D+U
        case 'JAC'                      % Jacobi iteration  B = BT = D;
            Bi{j} = spdiags(diag(Ai{j}),0,size(Ai{j},1),size(Ai{j},1));
            BTi{j} = Bi{j};
    end
    if option.smoothingparameter~=1     % scaling of the smoother
        Bi{j} = Bi{j}/option.smoothingparameter;
        BTi{j} = BTi{j}/option.smoothingparameter;
    end
end
end % end for setup

%% Only set up the transfer operators
if strcmp(solver,'NO')
    x = x0; flag = 0; itStep = 0; err = 0; time = cputime-t;
    info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));
    return
end

%% No need of set up
if setupflag == false
    Ai = varargin{1};
    Bi = varargin{2};
   BTi = varargin{3};
   Res = varargin{4};
   Pro = varargin{5};
   if length(varargin)>=6
      isFreeDof = varargin{6};
   else
      isFreeDof = true(Ndof,1);
   end
   level = length(Ai);
   isFixDof = ~isFreeDof;
   xD = zeros(Ndof,nb);
   if any(isFixDof) && Ndof == length(isFreeDof) % larger system
        Nfix = sum(isFixDof);
        ADinv = spdiags(1./diag(A(isFixDof,isFixDof)),0,Nfix,Nfix);
        xD(isFixDof,:) = ADinv*b(isFixDof,:);
        b = b(isFreeDof,:);
        A = A(isFreeDof,isFreeDof);
        x0 = x0(isFreeDof,:);
   end
end
if condest(Ai{1}) > 1e16 % Ai{1} is singular
    Ai{1} = Ai{1} + 1e-12*speye(size(Ai{1}));
%     coarsegridsolver = 'pcg';
end

%% No coarsening or coarsened nodes is small
if Ndof <= N    % linear element
    if level == 1
        if Ndof < N0 % small size
            solver = 'DIRECT';
        else
            [x,info] = amg(A,b,option);
            if any(isFixDof) && Nborig > size(x,1) % a larger system
                xD(isFreeDof,:) = x;
                x = xD;
            end
            Ai{1} = A; Bi{1} = tril(A); BTi{1} = Bi'; Res = []; Pro = [];
            fprintf('No hierarchical structure of the mesh is found. AMG is used. \n');
            return
        end
    end
end

%% Direct solver
if strcmp(solver,'DIRECT')
    x = A\b;                       % use direct solver
    flag = 2; itStep = 0; err = norm(b-A*x)/norm(b); time = cputime - t;
    %% Modify x to include fix dof
    if any(isFixDof) && Nborig > size(x,1)
        xD(isFreeDof,:) = x;
        x = xD;
    end
    if printlevel >= 1
        fprintf('Direct solver \n')
        fprintf('#dof: %8.0u,  #nnz: %8.0u,  iter: %2.0u,  err = %8.4e,  time = %4.2g s\n',...
                 Ndof, nnz(A), itStep, max(err(end,:)), time)
        if level == 1
            fprintf('No coarsening of the grid. \n')
        end
    end
    info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));
    Ai{1} = A; Bi{1} = tril(A); BTi{1} = Bi'; Res = []; Pro = [];
    return
end

%% Krylov iterative methods use Multigrid-type Preconditioners
% set up preconditioner
if ~strcmp(solver(2:end),'CYCLE')
    switch preconditioner
        case 'V'
            prefunc = @vcycle;
            if printlevel >= 1
                fprintf('\n Multigrid V-cycle Preconditioner with ')
            end
        case 'W'
            prefunc = @wcycle;
            if printlevel >= 1
                fprintf('\n Multigrid W-cycle Preconditioner with ')
            end
        case 'F'
            prefunc = @fcycle;
            if printlevel >= 1
                fprintf('\n Multigrid Full Cycle Preconditioner with ')
            end
        case 'BPX'
            % modify smoother
            for j = level:-1:2
                Di{j} = diag(Ai{j});
            end
            prefunc = @bpx;
            if printlevel >= 1
                fprintf('BPX Preconditioner with ')
            end
    end
end
% initial set up
k = 1;
x = x0;
r = b - A*x;
nb = max(sqrt(sum(b.^2,1)));
err = zeros(maxIt,2);
if nb > eps  % nb is non-zero
    err(1,:) = max(sqrt(sum(r.^2,1)))/nb;
else
    err(1,:) = max(sqrt(sum(r.^2,1)));
end
% solvers
switch solver
    case 'CG'
        if printlevel >= 1
            fprintf('Conjugate Gradient Method\n')
        end
        while (max(err(k,:)) > tol) && (k <= maxIt)
            % compute Br by MG
            Br = prefunc(r);
            % update tau, beta, and p
            rho = dot(Br,r);  % e'*ABA*e approximates e'*A*e
            if k == 1
                p = Br;
            else
                beta = rho./rho_old;
                p = Br + beta.*p;
            end
            % update alpha, x, and r
            Ap = A*p;
            alpha = rho./dot(Ap,p);
            r = r - alpha.*Ap;
            x = x + alpha.*p;
            rho_old = rho;
            k = k + 1;
            % compute err for the stopping criterion
        %     err(k,1) = alpha*sqrt(p'*Ap/(x'*A*x)); % increamental error in energy norm
            err(k,1) = max(sqrt(abs(rho./dot(x,b)))); % approximate relative error in energy norm
            if nb > eps
                err(k,2) = max(sqrt(sum(r.^2,1)))/nb; % relative error of the residual in L2-norm
            else
                err(k,2) = max(sqrt(sum(r.^2,1)));
            end
            if printlevel >= 2
                fprintf('#dof: %8.0u,  #nnz: %8.0u, MGCG iter: %2.0u, err = %8.4e\n',...
                         Ndof, nnz(A), k-1, max(err(k,:)));
            end
        end
        err = err(1:k,:);
        itStep = k-1;
    case 'VCYCLE'
        if printlevel >= 1
            fprintf('\n Multigrid Vcycle Iteration \n')
        end
        while (max(err(k,:)) > tol) && (k <= maxIt)
            k = k + 1;
            % Step 2: Compute Br by one Vcylce MG
            Br = vcycle(r);
            % Step 3: Correct the solution
            x = x + Br;
            % Step 1: Form residual r
            r = r - A*Br;
            err(k,1) = max(sqrt(abs(dot(Br,r)/dot(x,b)))); % approximate relative error in energy norm
            if nb > eps
                err(k,2) = max(sqrt(sum(r.^2,1)))/nb; % relative error of the residual in L2-norm
            else
                err(k,2) = max(sqrt(sum(r.^2,1)));
            end
            if printlevel >= 2
                fprintf('#dof: %8.0u,  #nnz: %8.0u, MG Vcycle iter: %2.0u, err = %8.4e\n',...
                         Ndof, nnz(A), k-1, max(err(k,:)));
            end
        end
        err = err(1:k,:);
        itStep = k-1;
    case 'WCYCLE'
        if printlevel >= 1
            fprintf('\n Multigrid Wcycle Iteration \n')
        end
        while (max(err(k,:)) > tol) && (k <= maxIt)
            k = k + 1;
            % Step 2: Compute Br by one Vcylce MG
            Br = wcycle(r);
            % Step 3: Correct the solution
            x = x + Br;
            err(k,1) = max(sqrt(abs(dot(Br,r)/dot(x,b)))); % approximate relative error in energy norm
            % Step 1: Form residual r
            r = r - A*Br;
            if nb > eps
                err(k,2) = max(sqrt(sum(r.^2,1)))/nb; % relative error of the residual in L2-norm
            else
                err(k,2) = max(sqrt(sum(r.^2,1)));
            end
            if printlevel >= 2
                fprintf('#dof: %8.0u,  #nnz: %8.0u, MG Wcycle iter: %2.0u, err = %8.4e\n',...
                         Ndof, nnz(A), k-1, max(err(k,:)));
            end
        end
        err = err(1:k,:);
        itStep = k-1;
    case 'FCYCLE'
        if printlevel >= 1
            fprintf('\n Multigrid Full Cycle Iteration \n')
        end
        while (max(err(k,:)) > tol) && (k <= maxIt)
            k = k + 1;
            % Step 2: Compute Br by one Full cycle MG
            Br = fcycle(r);
            % Step 3: Correct the solution
            x = x + Br;
            err(k,1) = max(sqrt(abs(dot(Br,r)/dot(x,b)))); % approximate relative error in energy norm
            % Step 1: Form residual r
            r = r - A*Br;
            if nb > eps
                err(k,2) = max(sqrt(sum(r.^2,1)))/nb; % relative error of the residual in L2-norm
            else
                err(k,2) = max(sqrt(sum(r.^2,1)));
            end
            if printlevel >= 2
                fprintf('#dof: %8.0u, #nnz: %8.0u, MG Fcycle iter: %2.0u, err = %8.4e\n',...
                         Ndof, nnz(A), k-1, max(err(k,:)));
            end
        end
        err = err(1:k,:);
        itStep = k-1;
    case 'MINRES'
        if printlevel >= 1
            fprintf('Minimum Residual Method \n')
        end
        [x,flag,err,itStep] = minres(A,b,tol,maxIt,prefunc,[],x0);
    case 'GMRES'
        if printlevel >= 1
            fprintf('General Minimum Residual Method\n')
        end
        if isfield(option,'restart')
            restart = option.restart;
        else
            restart = min(N,10);
        end
        [x,flag,err,itStep] = gmres(A,b,restart,tol,maxIt,prefunc,[],x0);
        itStep = (itStep(1)-1)*restart + itStep(2);
    case 'BICG'
        if printlevel >= 1
            fprintf('BiConjugate Gradient Method\n')
        end
        [x,flag,err,itStep] = bicg(A,b,tol,maxIt,prefunc,[],x0);
    case 'BICGSTAB'
        if printlevel >= 1
            fprintf('Stablilized BiConjugate Gradient Method\n')
        end
        [x,flag,err,itStep] = bicgstab(A,b,tol,maxIt,prefunc,[],x0);
    case 'BICGSTAB1'
        if printlevel >= 1
            fprintf('Stablilized BiConjugate Gradient Method\n')
        end
        [x,flag,err,itStep] = bicgstab1(A,b,tol,maxIt,prefunc,[],x0);
end

%% Modify x to include fix dof
if Nborig > size(x,1) % a larger system
    xD(isFreeDof,:) = x;
    x = xD;
end

%% Output
if k > maxIt
    flag = 1;
else
    flag = 0;
end
time = cputime - t;
if printlevel >= 2
    fprintf('#dof: %8.0u, level: %2.0u,   coarse grid %2.0u, #nnz: %8.0u\n',...
              size(b,1), level, size(Ai{1},1), nnz(Ai{1}))
end
if printlevel >= 1
    fprintf('#dof: %8.0u,  #nnz: %8.0u, smoothing: (%1.0u,%1.0u), iter: %2.0u,   err = %8.2e,   time = %4.2g s\n',...
                 size(b,1), nnz(A), mu, mu, itStep, max(err(end,:)), time)
end
if (flag == 1) && (printlevel>0)
   fprintf('NOTE: the iterative method does not converge! \n');
end
info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions vcycle, wcycle, fcycle, bpx
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Vcycle MG
    function Br = vcycle(r,J)  % solve equations Ae = r in each level
    if nargin<=1
        J = level;
    end
    ri = cell(J,1);            % residual in each level
    ei = cell(J,1);            % correction in each level
    mui = cell(J,1);           % variable smoothing steps
    ri{J} = r;
    mui{J} = mu;
    for i = J:-1:2
        ei{i} = Bi{i}\ri{i};   % pre-smoothing
        for s = 1:mui{i}-1     % extra mu-1 steps smoothing
            if mod(s,2)        % switch between B and BT to symmetrize smoother
                ei{i} = ei{i} + BTi{i}\(ri{i}-Ai{i}*ei{i});
            else
                ei{i} = ei{i} + Bi{i}\(ri{i}-Ai{i}*ei{i});
            end
        end
        ri{i-1} = Res{i}*(ri{i} - Ai{i}*ei{i});
        mui{i-1} = ceil(mui{i}*smoothingRatio);
    end
    switch coarsegridsolver
        case 'direct'
            ei{1} = Ai{1}\ri{1};   % direct solver in the coarest level
        case 'pcg'
            D = spdiags(diag(Ai{1}),0,size(Ai{1},1),size(Ai{1},1));
            [ei{1},flag] = pcg(Ai{1},ri{1},1/size(Ai{1},1),1000,D);
        case 'amg'
            amgoption.printlevel = 0;
            ei{1} = amg(Ai{1},ri{1},amgoption);
    end
    for i = 2:J
        ei{i} = ei{i} + Pro{i-1}*ei{i-1};
        for s = 1:mui{i}       % post-smoothing
            if mod(s,2)        % switch between B and BT to symmetrize smoother
                ei{i} = ei{i} + BTi{i}\(ri{i}-Ai{i}*ei{i});
            else
                ei{i} = ei{i} + Bi{i}\(ri{i}-Ai{i}*ei{i});
            end
        end
    end
    Br = ei{J};
    end

%% Wcycle MG
    function e = wcycle(r,J)    % solve equations Ae = r in level J
    if nargin<=1
        J = level;
    end
    if J == 1
        switch coarsegridsolver
            case 'direct'
                e = Ai{J}\r;   % direct solver in the coarest level
            case 'pcg'
                D = spdiags(diag(Ai{J}),0,size(Ai{J},1),size(Ai{J},1));
                [e,flag] = pcg(Ai{J},r,1/size(Ai{J},1),1000,D);
            case 'amg'
                amgoption.printlevel = 0;
                e = amg(Ai{J},r,amgoption.printlevel);
        end
        return
    end
    % fine grid pre-smoothing
    e = Bi{J}\r;        % pre-smoothing
    for s = 1:mu-1      % extra mu-1 steps smoothing
        if mod(s,2)
            e = e + Bi{J}\(r-Ai{J}*e);
        else
            e = e + BTi{J}\(r-Ai{J}*e);
        end
    end
    % restriction
    rc = Res{J}*(r - Ai{J}*e);
    % coarse grid correction twice
    ec = wcycle(rc,J-1);
    ec = ec + wcycle(rc - Ai{J-1}*ec,J-1);
    % prolongation
    e = e + Pro{J-1}*ec;
    % fine grid post-smoothing
    for s = 1:mu
        if mod(s,2)
            e = e + Bi{J}\(r-Ai{J}*e);
        else
            e = e + BTi{J}\(r-Ai{J}*e);
        end
    end
    end

%% Fcycle MG
    function Br = fcycle(r)
    ri = cell(level,1);            % residual in each level
    ei = cell(level,1);            % correction in each level
    ri{level} = r;
    for i = level:-1:2
        ei{i} = vcycle(ri{i},i);   % pre-smoothing
        for s = 1:mu-1             % extra smoothing steps
            ei{i} = ei{i} + vcycle(ri{i}-Ai{i}*ei{i},i); % pre-smoothing
        end
        ri{i-1} = Res{i}*(ri{i} - Ai{i}*ei{i});
    end
    switch coarsegridsolver
        case 'direct'
            ei{1} = Ai{1}\ri{1};   % direct solver in the coarest level
        case 'pcg'
            D = spdiags(diag(Ai{1}),0,size(Ai{1},1),size(Ai{1},1));
            [ei{1},flag] = pcg(Ai{1},ri{1},1/size(Ai{1},1),1000,D);
        case 'amg'
            amgoption.printlevel = 0;
            ei{1} = amg(Ai{1},ri{1},amgoption);
    end
    for i = 2:level
        ei{i} = ei{i} + Pro{i-1}*ei{i-1};
        for s = 1:mu   % post-smoothing
            ei{i} = ei{i} + vcycle(ri{i}-Ai{i}*ei{i},i); % post-smoothing
        end
    end
    Br = ei{level};
    end

%% BPX preconditioner
    function Br = bpx(r)
    ri = cell(level,1);            % record residual in each level
    ei = cell(level,1);            % record err in each level
    % compute Br by BPX
    ri{level} = r;
    for i = level:-1:2
        ei{i} = ri{i}./Di{i};       % Jacobi smoothing
        ri{i-1} = Res{i}*ri{i};     % restriction of the residual
    end
    switch coarsegridsolver
        case 'direct'
            ei{1} = Ai{1}\ri{1};   % direct solver in the coarest level
        case 'pcg'
            D = spdiags(diag(Ai{1}),0,size(Ai{1},1),size(Ai{1},1));
            [ei{1},flag] = pcg(Ai{1},ri{1},1/size(Ai{1},1),1000,D);
        case 'amg'
            amgoption.printlevel = 0;
            ei{1} = amg(Ai{1},ri{1},amgoption);
    end
    for i=2:level
        ei{i} = ei{i} + Pro{i-1}*ei{i-1};  % prolongation of the correction
    end
    Br = ei{level};
    end
end %% end of mg


function[varargout]=ridgeinterp(varargin)
% RIDGEINTERP  Interpolate quantity values onto ridge locations.
%
%   RIDGEINTERP is a low-level function called by RIDGEWALK.
%
%   XI=RIDGEINTERP(FS,RQ,IR,JR,X) where RQ is a "ridge quantity" at
%   *radian* frequencies FS, and IR and JR are time and scale indices of
%   ridges, interpolates quantity X along the ridge locations to give XI.
%
%   IR and JR give indices into the first two dimensions of X.  The output
%   XI has the the same number of rows and columns as IR and JR, that is,
%   SIZE(XI,1)=SIZE(IR,1) and SIZE(XI,2)=SIZE(IR,2).  The locations of
%   NANs in IR and JR are duplicated in XI.
%
%   X may have more than one 'page' along its third dimension, in which
%   case each page is interpolated separately, and SIZE(XI,3)=SIZE(X,3).
%
%   RQ is output by ISRIDGEPOINT based on a wavelet transform output by
%   WAVETRANS, and IR and JR are output by RIDGEWALK.
%
%   RIDGEINTERP interpolates transform values between discrete frequency
%   levels to find a more precise value of the transform along the ridges
%   than simply looking up the values of X at rows IR and columns JR.
%
%   XI=RIDGEINTERP(FS,RQ,IR,JR,MU,X) interprets the ridges as belonging
%   to the MU-th derivative of the signal to which the quantity X belongs
%   and applies an appropriate correction factor.
%
%   [XI1,XI2,...,XIN]=RIDGEINTERP(FS,RQ,IR,JR,X1,X2,...,XN) also
%   interpolates the quantities X1,X2,...,XN, all having the same number of
%   rows and columns as IR and JR.
%
%   RIDGEINTERP uses fast quadratic interpolation via QUADINTERP.  In rare
%   cases, quadratic interpolation fails for an individual point and
%   therefore linear interpolation is used instead.
%   __________________________________________________________________
%
%   RIDGEINTERP is a low-level function called by RIDGEWALK.
%
%   See also RIDGEWALK, ISRIDGEPOINT, QUADINTERP.
%
%   Usage:  xi=ridgeinterp(rq,ir,jr,x);
%           [xi1,xi2,xi3]=ridgeinterp(rq,ir,jr,x1,x2,x3);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2005--2019 J.M. Lilly --- type 'help jlab_license' for details

rq=varargin{1};
ir=varargin{2};
jr=varargin{3};
varargin=varargin(4:end);

if iscell(varargin{end})||isempty(varargin{end})
    derivparams=varargin{end};
    varargin=varargin(1:end-1);
else
    derivparams=[];
end

index=find(~isnan(ir));
if ~isempty(index)
    varargout=ridgeinterp1_quadratic(index,ir,jr,derivparams,rq,varargin);
else
    for i=1:length(varargin)
        varargout{i}=[];
    end
end

function[outargs]=ridgeinterp1_quadratic(index,ir,jr,derivparams,rq,args)
sizeir=size(ir);
vcolon(ir,jr);
vindex(ir,jr,index,1);

indexr=nonnan(sub2ind(size(rq),ir,jr));

%Ridge quantity along the ridges, and at one scale up and down

di=size(rq,1);
dr=rq(indexr);
drp=rq(indexr+di);
drn=rq(indexr-di);

[~,jre]=quadinterp(jr-1,jr,jr+1,abs(drn).^2,abs(dr).^2,abs(drp).^2);

%Rare complete failure of quadratic interpolation associated
%with the ridge quantity changing sign between jrp and jrn, yet
%not having the ridge quantity at jr being between these two values

bool=~( (jr+1>jre) & (jre> jr-1));
jre(bool)=lininterp(jr(bool)-1,jr(bool)+1,drn(bool),drp(bool));

if ~isempty(derivparams)
    ga=derivparams{1};
    be=derivparams{2};
    mu=derivparams{3};

    fact=frac(morsefreq(ga,be-mu),morsefreq(ga,be));

    jro=frac(jr,fact);
    jrp=frac(jr+1,fact);
    jrn=frac(jr-1,fact);

    btop=(ceil(jro)>=(size(rq,2)-1));
    jro(btop)=size(rq,2)-1;
    jrp(btop)=size(rq,2);
    jrn(btop)=size(rq,2)-2;

    bbottom=(floor(jro)<=2);
    jro(bbottom)=2;
    jrp(bbottom)=3;
    jrn(bbottom)=1;

    %The problem here is that I also have to interpolate to find the
    %values of the quantities to be interpolated at the rescaled scale
    %levels, so you have to do another linear interpolation between
    %the floors and the ceilings.  You can't just do the above any
    %more because the rescaled scale levels are no longer jr+/-1.

%     figure,plot(jro)
%     maxmax(jro)
%     minmin(jro)
    indexrf=nonnan(sub2ind(size(rq),ir,floor(jro)));
    indexrc=nonnan(sub2ind(size(rq),ir,ceil(jro)));

    indexrpf=nonnan(sub2ind(size(rq),ir,floor(jrp)));
    indexrpc=nonnan(sub2ind(size(rq),ir,ceil(jrp)));

    indexrnf=nonnan(sub2ind(size(rq),ir,floor(jrn)));
    indexrnc=nonnan(sub2ind(size(rq),ir,ceil(jrn)));
end

for i=1:length(args)
   x=args{i};
   if isreal(x)
        xr=nan*ones(sizeir(1),size(x,3));
   else
        xr=(nan+sqrt(-1)*nan)*ones(sizeir(1),size(x,3));
   end
   for k=1:size(x,3)
        xk=x(:,:,k);
        if isempty(derivparams)
            xro=xk(indexr);
            xrp=xk(indexr+di);
            xrn=xk(indexr-di);
        else
            xro=lininterp(floor(jro),ceil(jro),xk(indexrf),xk(indexrc),jro);
            xrp=lininterp(floor(jrp),ceil(jrp),xk(indexrpf),xk(indexrpc),jrp);
            xrn=lininterp(floor(jrn),ceil(jrn),xk(indexrnf),xk(indexrnc),jrn);
        end

        xrk=quadinterp(jr-1,jr,jr+1,xrn,xro,xrp,jre);
        %Use linear interpolation where quadratic fails
        xrk(bool)=lininterp(jr(bool)-1,jr(bool)+1,xrn(bool),xrp(bool),jre(bool));
        xr(index,k)=xrk;
   end
   %Linearly interpolate between the approximate ridge and the bracketing curve
   outargs{i}=xr;
end


function clebsch_gordan_values_test ( )

%*****************************************************************************80
%
%% CLEBSCH_GORDAN_VALUES_TEST demonstrates the use of CLEBSCH_GORDAN_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CLEBSCH_GORDAN_VALUES_TEST:\n' );
  fprintf ( 1, '  CLEBSCH_GORDAN_VALUES returns values of \n' );
  fprintf ( 1, '  the Clebsch-Gordan coefficient.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '      J1      J2      J3      M1      M2      M3        CG\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, j1, j2, j3, m1, m2, m3, fx ] = clebsch_gordan_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %6.2f  %6.2f  %6.2f  %6.2f  %6.2f  %6.2f  %24.16f\n', ...
      j1, j2, j3, m1, m2, m3, fx );

  end

  return
end


function [BAS1,BAS2] = basIFE3DNed1coef(vert,intpt,coef)

%% USAGE: Coefficients of P1 IFE Shape Function on a Tetrahedron
%
% INPUTS:
% vert --- 4-by-3 matrix stores the
%          vertices of the tetrahedron
%          [A1;A2;A3;A4] = [x1 y1 z1; x2 y2 z2; x3 y3 z3; x4 y4 z4]
% intpt --- 3-by-3 or 4-by-3 matrix stores three intersection points
%          [D;E;F] = [xd yd zd; xe ye ze; xf yf zf];
%           the order is 1,2,3 or 2,3,4,5 of the index of edges
% coef --- [bt1,bt2] jump coefficient where bt1 is associated with A1.
%                                    bt2 is associated with A2 A3 A4
%
%     Type 1 Interface Element (3 intersection points), it means
%         A1 is on piece #1 whose coefficient is coef(1)
%         A2 A3 A4 are on piece #2 whose coefficient is coef(2)
%
%     Type 2 Interface Element (4 intersection points), it means
%         A1 A2 are on piece #1 whose coefficient is coef(1)
%         A3 A4 are on piece #2 whose coefficient is coef(2)
%
% OUTPUTS:
% BAS1 --- 6-by-6 matrix stores the coefficients of shape fun on piece #1
% BAS2 --- 6-by-6 matrix stores the coefficients of shape fun on piece #2
%          The ith basis: a * x + b on piece #j = 1 or 2
%              1st basis [a11,a12,a13,b11,b12,b13]
%              2nd basis [a21,a22,a23,b21,b22,b23]
%              3rd basis [a31,a32,a33,b31,b32,b33]
%              4th basis [a41,a42,a43,b41,b42,b43]
%              5th basis [a51,a52,a53,b51,b52,b53]
%              6th basis [a61,a62,a63,b61,b62,b63]
%
% Last Modified by Ruchi Guo on 19/11/20

%% 0. Initialization
bt1 = coef(1);  bt2 = coef(2); rb = bt2/bt1;
at1 = coef(3);  at2 = coef(4); ra = at2/at1;
 M = zeros(6,6); l = zeros(6,2); % the 2nd component is only associated with the shape fun on the piece2
if size(intpt,1) == 3
    D = intpt(1,:); E = intpt(2,:); F = intpt(3,:); C = 1/3*(D+E+F);
    M(1,1:3) = 1/2*(vert(1,:)+D); l(1,1) = norm(D-vert(1,:));
    M(1,4:6) = 1/2*(vert(2,:)+D); l(1,2) = norm(D-vert(2,:));
    M(2,1:3) = 1/2*(vert(1,:)+E); l(2,1) = norm(E-vert(1,:));
    M(2,4:6) = 1/2*(vert(3,:)+E); l(2,2) = norm(E-vert(3,:));
    M(3,1:3) = 1/2*(vert(1,:)+F); l(3,1) = norm(F-vert(1,:));
    M(3,4:6) = 1/2*(vert(4,:)+F); l(3,2) = norm(F-vert(4,:));
    M(4,4:6) = 1/2*(vert(2,:)+vert(3,:)); l(4,2) = norm(vert(2,:)-vert(3,:));
    M(5,4:6) = 1/2*(vert(2,:)+vert(4,:)); l(5,2) = norm(vert(2,:)-vert(4,:));
    M(6,4:6) = 1/2*(vert(3,:)+vert(4,:)); l(6,2) = norm(vert(3,:)-vert(4,:));
elseif size(intpt,1) == 4
    [intptU,~,~] = vert4to3(intpt);
    D = intptU(1,:); E = intptU(2,:); F = intptU(3,:); C = 1/3*(D+E+F);
    M(1,1:3) = 1/2*(vert(1,:)+vert(2,:)); l(1,1) = norm(vert(1,:)-vert(2,:));
    M(2,1:3) = 1/2*(vert(1,:)+intpt(1,:)); l(2,1) = norm(vert(1,:)-intpt(1,:));
    M(2,4:6) = 1/2*(vert(3,:)+intpt(1,:)); l(2,2) = norm(vert(3,:)-intpt(1,:));
    M(3,1:3) = 1/2*(vert(1,:)+intpt(2,:)); l(3,1) = norm(vert(1,:)-intpt(2,:));
    M(3,4:6) = 1/2*(vert(4,:)+intpt(2,:)); l(3,2) = norm(vert(4,:)-intpt(2,:));
    M(4,1:3) = 1/2*(vert(2,:)+intpt(3,:)); l(4,1) = norm(vert(2,:)-intpt(3,:));
    M(4,4:6) = 1/2*(vert(3,:)+intpt(3,:)); l(4,2) = norm(vert(3,:)-intpt(3,:));
    M(5,1:3) = 1/2*(vert(2,:)+intpt(4,:)); l(5,1) = norm(vert(2,:)-intpt(4,:));
    M(5,4:6) = 1/2*(vert(4,:)+intpt(4,:)); l(5,2) = norm(vert(4,:)-intpt(4,:));
    M(6,4:6) = 1/2*(vert(3,:)+vert(4,:)); l(6,2) = norm(vert(3,:)-vert(4,:));
end

t = zeros(6,6);
t(1,1:3) = vert(2,:)-vert(1,:); t(1,1:3) = t(1,1:3)/norm(t(1,1:3));
t(2,1:3) = vert(3,:)-vert(1,:); t(2,1:3) = t(2,1:3)/norm(t(2,1:3));
t(3,1:3) = vert(4,:)-vert(1,:); t(3,1:3) = t(3,1:3)/norm(t(3,1:3));
t(4,1:3) = vert(3,:)-vert(2,:); t(4,1:3) = t(4,1:3)/norm(t(4,1:3));
t(5,1:3) = vert(4,:)-vert(2,:); t(5,1:3) = t(5,1:3)/norm(t(5,1:3));
t(6,1:3) = vert(4,:)-vert(3,:); t(6,1:3) = t(6,1:3)/norm(t(6,1:3));

n = cross(E-D,F-D);
n = n/norm(n); % unit normal of plane DEF
% L = @(x,y,z) dot([x-C(1),y-C(2),z-C(3)],n);

%% for the piece2
bas0 = zeros(6,6);
bas0(:,1) = (M(:,2).*t(:,3) - M(:,3).*t(:,2)).*l(:,1) + (M(:,5).*t(:,3) - M(:,6).*t(:,2)).*l(:,2);
bas0(:,2) = (M(:,3).*t(:,1) - M(:,1).*t(:,3)).*l(:,1) + (M(:,6).*t(:,1) - M(:,4).*t(:,3)).*l(:,2);
bas0(:,3) = (M(:,1).*t(:,2) - M(:,2).*t(:,1)).*l(:,1) + (M(:,4).*t(:,2) - M(:,5).*t(:,1)).*l(:,2);
bas0(:,4:6) = t(:,1:3).*((l(:,1)+l(:,2))*ones(1,3));

%% for the piece1
MM = M - [ones(6,1)*C, ones(6,1)*C];
bas1 = zeros(6,6);
bas1(:,1) = ((ra-1)*(MM(:,2).*t(:,3) - MM(:,3).*t(:,2)) +...
    (rb-1)*(t(:,1:3)*n')*(C(2)*n(3)-C(3)*n(2))).*l(:,1);
bas1(:,2) = ((ra-1)*(MM(:,3).*t(:,1) - MM(:,1).*t(:,3)) +...
    (rb-1)*(t(:,1:3)*n')*(C(3)*n(1)-C(1)*n(3))).*l(:,1);
bas1(:,3) = ((ra-1)*(MM(:,1).*t(:,2) - MM(:,2).*t(:,1)) +...
    (rb-1)*(t(:,1:3)*n')*(C(1)*n(2)-C(2)*n(1))).*l(:,1);
bas1(:,1:3) = bas1(:,1:3) -...
    (ra-1)*(sum(cross(ones(6,1)*n,MM(:,1:3)).*t(:,1:3),2).*l(:,1))*n;
bas1(:,4:6) = (rb-1)*(l(:,1).*(t(:,1:3)*n'))*n;

A = bas0 + bas1;

ss = A\eye(6);
ss = ss';
BAS2 = ss;
BAS1 = ss;
BAS1(:,1:3) = ss(:,1:3) + (ra-1)*(ss(:,1:3)-(ss(:,1:3)*n')*n);
BAS1(:,4:6) = ss(:,4:6) - (ra-1)*cross(ss(:,1:3)-(ss(:,1:3)*n')*n,ones(6,1)*C) +...
    (rb-1)*((cross(ss(:,1:3),ones(6,1)*C)+ss(:,4:6))*n')*n;
%BAS1 = BAS1'; BAS2 = BAS2';

return;


classdef MMF6 < PROBLEM
% <multi> <real> <multimodal>
% Multi-modal multi-objective test function

%------------------------------- Reference --------------------------------
% C. Yue, B. Qu, and J. Liang, A multi-objective particle swarm optimizer
% using ring topology for solving multimodal multiobjective Problems, IEEE
% Transactions on Evolutionary Computation, 2018, 22(5): 805-817.
%------------------------------- 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".
%--------------------------------------------------------------------------

    properties
        POS;    % Pareto optimal set for IGDX calculation
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            obj.D = 2;
            obj.lower    = [1,-1];
            obj.upper    = [3,2];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            temp = X(:,2)<=0 | X(:,2)<=1 & (X(:,1)<=7/6|8/6<X(:,1)&X(:,1)<=9/6|10/6<X(:,1)&X(:,1)<=11/6|13/6<X(:,1)&X(:,1)<=14/6|15/6<X(:,1)&X(:,1)<=16/6|17/6<X(:,1));
            y    = zeros(size(X,1),1);
            y(temp)  = X(temp,2) - sin(6*pi*abs(X(temp,1)-2)+pi);
            y(~temp) = X(~temp,2) - 1 - sin(6*pi*abs(X(~temp,1)-2)+pi);
            PopObj(:,1) = abs(X(:,1)-2);
            PopObj(:,2) = 1 - sqrt(PopObj(:,1)) + 2*y.^2;
        end
        %% Generate Pareto optimal solutions
        function R = GetOptimum(obj,N)
            % Generate points in Pareto optimal set
            obj.POS(:,1) = linspace(1,3,N/2)';
            obj.POS(:,2) = sin(6*pi*abs(obj.POS(:,1)-2)+pi);
            obj.POS = [obj.POS;obj.POS(:,1),obj.POS(:,2)+1];
            % Generate points on Pareto front
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R(:,1) = linspace(0,1,100)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case 'IGDX'
                    score = feval(metName,Population,obj.POS);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Display a population in the objective space
        function DrawObj(obj,Population)
            PopDec = Population.decs;
            temp1  = PopDec(:,2)<=0 | PopDec(:,2)<=1 & (PopDec(:,1)<=7/6|8/6<PopDec(:,1)&PopDec(:,1)<=9/6|10/6<PopDec(:,1)&PopDec(:,1)<=11/6|13/6<PopDec(:,1)&PopDec(:,1)<=14/6|15/6<PopDec(:,1)&PopDec(:,1)<=16/6|17/6<PopDec(:,1));
            temp2  = PopDec(:,1)<=2;
            Draw(Population(temp1&temp2).objs,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[1 .5 .5],'Markeredgecolor',[1 .2 .2],{'\it f\rm_1','\it f\rm_2',[]});
            Draw(Population(temp1&~temp2).objs+0.05,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[.5 .5 1],'Markeredgecolor',[.2 .2 1]);
            Draw(Population(~temp1&temp2).objs+0.1,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[.5 1 .5],'Markeredgecolor',[.2 1 .2]);
            Draw(Population(~temp1&~temp2).objs+0.15,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[1 .5 1],'Markeredgecolor',[1 .2 1]);
            Draw(obj.PF,'-','LineWidth',1,'Color',[1 .2 .2]);
            Draw(obj.PF+0.05,'-','LineWidth',1,'Color',[.2 .2 1]);
            Draw(obj.PF+0.1,'-','LineWidth',1,'Color',[.2 1 .2]);
            Draw(obj.PF+0.15,'-','LineWidth',1,'Color',[1 .2 1]);
        end
    end
end


% [INPUT]
% r = A vector of floats (-Inf,Inf) of length t representing the logarithmic returns.
% v = A vector of floats [0,Inf) of length t representing the trading volumes.
% sv = A float t-by-k matrix (-Inf,Inf) representing the state variables.
% bw = An integer [21,252] representing the dimension of each rolling window.
% mem = A string representing the MEM type:
%   - 'B' for Baseline MEM;
%   - 'A' for Asymmetric MEM;
%   - 'P' for Asymmetric Power MEM;
%   - 'S' for Spline MEM.
% mag = An integer [1,Inf) obtained as 10^x representing the magnitude of logarithmic returns and trading volumes (optional, default=[]).
%
% [OUTPUT]
% illiq = A column vector of floats [0,Inf) of length t representing the ILLIQ indicator.
% illiqc = A column vector of floats [0,Inf) of length t representing the ILLIQ indicator with covariates if state variables are provided, an empty array otherwise.
% knots = A row vector of floats [0,Inf) containing the optimal number of knots if Spline MEM is used, an empty array otherwise.

function [illiq,illiqc,knots] = illiq_indicator(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('r',@(x)validateattributes(x,{'double'},{'real' 'finite' 'vector' 'nonempty'}));
        ip.addRequired('v',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('sv',@(x)validateattributes(x,{'double'},{'real' 'finite'}));
        ip.addRequired('bw',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 21 '<=' 252 'scalar'}));
        ip.addRequired('mem',@(x)any(validatestring(x,{'A' 'B' 'P' 'S'})));
        ip.addOptional('mag',[],@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    [r,v,sv,mag] = validate_input(ipr.r,ipr.v,ipr.sv,ipr.mag);
    bw = ipr.bw;
    mem = ipr.mem;

    nargoutchk(2,3);

    [illiq,illiqc,knots] = illiq_indicator_internal(r,v,sv,bw,mem,mag);

end

function [illiq,illiqc,knots] = illiq_indicator_internal(r,v,sv,bw,mem,mag)

    alpha = 2 / (bw + 1);

    input = mag .* (abs(r) ./ v);
    input(~isfinite(input) | (input == 0)) = NaN;
    input(isnan(input)) = mean(input,'omitnan');

    if (any(strcmp(mem,{'A' 'P'})))
        input = [input r];
    end

    knots = [];

    [illiq,~,mem_params] = multiplicative_error(input,mem);
    illiq = [illiq(1); filter(alpha,[1 (alpha - 1)],illiq(2:end),(1 - alpha) * illiq(1))];
    illiq = (illiq - min(illiq)) ./ (max(illiq) - min(illiq));

    if (strcmp(mem,'S'))
        knots(1) = mem_params(1);
    end

    if (isempty(sv))
        illiqc = [];
    else
        [illiqc,~,mem_params] = multiplicative_error([input sv],mem);
        illiqc = [illiqc(1); filter(alpha,[1 (alpha - 1)],illiqc(2:end),(1 - alpha) * illiqc(1))];
        illiqc = (illiqc - min(illiqc)) ./ (max(illiqc) - min(illiqc));

        if (strcmp(mem,'S'))
            knots(2) = mem_params(1);
        end
    end

end

function [r,v,sv,mag] = validate_input(r,v,sv,mag)

    data = {r(:) v(:)};

    l = unique(cellfun(@numel,data));

    if (numel(l) ~= 1)
        error('The number of elements of ''r'' and ''v'' must be equal.');
    end

    if (l < 5)
        error('The value of ''r'' and ''v'' is invalid. Expected inputs to be vectors containing at least 5 elements.');
    end

    [r,v] = deal(data{:});

    if (~isempty(sv))
        if (size(sv,1) ~= l)
            error(['The value of ''sv'' is invalid. Expected input to be a matrix with ' num2str(l) ' rows.']);
        end
    end

    if (isempty(mag))
        mag_r = floor(round((log(abs(r)) ./ log(10)),15));
        mag_r(~isfinite(mag_r)) = [];
        mag_r = round(abs(mean(mag_r)),0);

        mag_v = floor(round((log(abs(v)) ./ log(10)),15));
        mag_v(~isfinite(mag_v)) = [];
        mag_v = round(mean(mag_v),0);

        mag = 10^(mag_r + mag_v);
    else
        if ((mag ~= 1) && (rem(mag,10) ~= 0))
            error('The value of ''mag'' is invalid. Expected input to be an integer obtained as 10^x.');
        end
    end

end


function recon_im = sense_recon(smap, reduced_fft, reduced_dim, np, regularizer, mask, figs_on)
% function recon_im = sense_recon(smap, reduced_fft, reduced_dim, np,
%       regularizer, figs_on)
% in:
%   smap          [N M nc]                complex sensitivity maps
%   reduced_fft   [N/np M] or [N M/np]    undersampled fft
%   reduced_dim   1 or 2                  dimension of undersampling
%   np            degree of undersampling
%   regularizer   string, 'none' or 'tikhonov'
%   mask          [N M] boolean
%   figs_on       boolean that shows intermediate images for debugging
% out:
%   recon_im      [N M]                   SENSE reconstructed image
% This implementation assumes that the fft will be reduced in such a way
% that the DC term is still included (i.e. even DFT coefficients retained)
% 2012-06-07 Mai Le, University of Michigan

assert(isequal(size(mask), size(squeeze(smap(:,:,1)))), 'mask size does not match image size');

% dimensions of original image
dims = [size(smap,1) size(smap,2)];

% number of coils
nc = size(smap,3);

% create aliased intermediate images
aliased_im = zeros(size(reduced_fft,1), size(reduced_fft,2), nc);
for ii = 1:nc
     aliased_im(:,:,ii) = ifft2(reduced_fft(:,:,ii));
end

% plot aliased images
if (figs_on)
    figure;
    for ii = 1:nc
        subplot(2,2,ii); imshow(abs(aliased_im(:,:,ii)),[]); colorbar;
    end
end

% calculate regularization parameter
% I found empirically that somewhere between 0.5% and 1% of the max SVD
% value works well for beta
C = Cdiff1(np);
[U, SIG, V] = svd(C'*C);
% beta = 0.01*max(SIG(:));
beta = 0.005*max(SIG(:));

% reconstruct image by finding (regularized) LS solution for each set of pixels
recon_im = zeros(dims);
indeces = (0:np-1)*dims(reduced_dim)/np;
if (reduced_dim == 1)
    for ii = 1:dims(1)/np
        for jj = 1:dims(2)
            if any(mask(ii+indeces,jj))
%                 S = constructS(reduced_dim, smap, ii+indeces, jj, np);
                ivect = ii+indeces;
                S = squeeze([smap(ivect(mask(ii+indeces,jj)),jj,:)]).';
                if ~all(mask(ii+indeces,jj))
                    S = S.';
                end
                a = squeeze([aliased_im(ii,jj,1:nc)]);
                v = recon_pixels(S,a,regularizer, beta);
                [recon_im] = place_pixels(recon_im, v, reduced_dim, dims, ii, jj, np, mask(ii+indeces,jj));
            end
        end
    end
else
    for ii = 1:dims(1)
        for jj = 1:dims(2)/np
            if any(mask(ii,jj+indeces))
%                 S = constructS(reduced_dim, smap, ii, jj+indeces, np, mask(ii,jj+indeces));
                jvect = jj+indeces;
                S = squeeze([smap(ii,jvect(mask(ii,jj+indeces)),:)]).';
                if ~all(mask(ii,jj+indeces))
                    S = S.';
                end
                a = squeeze([aliased_im(ii,jj,1:nc)]);
                v = recon_pixels(S,a,regularizer, beta);
                [recon_im] = place_pixels(recon_im, v, reduced_dim, dims, ii, jj, np, mask(ii,jj+indeces));
            end
        end
    end
end
% figure; subplot(121); imshow(test_fill,[]); colorbar;
% subplot(122); imshow(real(recon_im),[]); colorbar;
% keyboard;

% selects values from sensitivity map relevant for specific pixels
% depending on reduced_dim, either ivect or jvect is a 1x2 vector
% function S = constructS(reduced_dim, smap, ivect, jvect, np, mask_vector)
%
%
% S = squeeze([smap(ivect,jvect(mask_vector),:)]).';

% places each of the reconstructed pixels in the reconstructed image
function [recon_im] = place_pixels(recon_im, v, reduced_dim, dims, ii, jj, np, mask)
indeces = (0:np-1)*dims(reduced_dim)/np;
if (reduced_dim == 1)
    mk = 1;
    vk = 1;
    while (mk <= np)
        if (mask(mk))
            recon_im(ii+indeces(mk),jj) = v(vk);
            vk = vk + 1;
        end
        mk = mk + 1;
    end
else
    vk = 1;
    mk = 1;
    while (mk <= np)
        if (mask(mk))
            recon_im(ii,jj+indeces(mk)) = v(vk);
            vk = vk + 1;
        end
        mk = mk + 1;
    end
end

% reconstruct pixels with or without Tikhonov regularization
function pixels = recon_pixels(S,a,regularizer, beta)
switch regularizer
    case 'none'
        pixels = S\a;
    case 'tikhonov'
        M = min(size(S));
        pixels = inv(S'*S+(beta^2)*eye(M))*S'*a;
    otherwise
        pixels = S\a;
end


%This Matlab script can be used to reproduce Figure 5.8 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%% Propagation and hardware parameters

%Communication bandwidth
B = 0.1*10^(6); %100 kHz

%PA efficiency
mu = 0.4;

%Range of antenna-UE ratios
cRange = [1 2 4 8];

%Number of UEs
K = 10;

%Fixed circuit power per BS (in Watt)
P_FIX = 10;

%Circuit power per BS antenna (in Watt)
P_BS = 1;

%Circuit power UE (in Watt)
P_UE = 0.5;

%Range of SE values
sumSE = (0:0.00002:40)';

%Select ratio between noise power and beta_0^0
sigma2_beta = 10^(-6*0.1);

%Compute nu_0 in (5.14)
nu_0 = sigma2_beta/mu;

%Strength of inter-cell interference  = -15 dB
barbeta = 10^(-15/10);

%Prepare to save simulation results
EE = zeros(length(sumSE),length(cRange));
maxEE = zeros(length(cRange),1);
maxSE = zeros(length(cRange),1);

%% Go through range of antenna-UE ratios
for index1 = 1:length(cRange)

    %Compute number of BS antennas
    M = cRange(index1)*K;

    %Go through range of SE values
    for index2 = 1:length(sumSE)

        p = sigma2_beta*((M-1)/(2^sumSE(index2) - 1) - K*barbeta +1 - K)^-1;

        if p < 0

            break;

        end

        EE(index2,index1) = B*K*sumSE(index2)/(K*p/mu + P_FIX + M*P_BS + K*P_UE);

    end

    [max_value, index_max ] = max(EE(:,index1));

    maxEE(index1) = max_value;

    maxSE(index1) = K*sumSE(index_max);

end

%% Plot the simulation results
figure(1);
hold on; box on;

plot(K*sumSE,EE(:,1),'k','LineWidth',1);
plot(K*sumSE,EE(:,2),'k-.','LineWidth',1);
plot(K*sumSE,EE(:,3),'k--','LineWidth',1);
plot(K*sumSE,EE(:,4),'k:','LineWidth',1);

set(gca,'YScale','log');
xlabel('Sum SE [bit/s/Hz/cell]');
ylabel('EE [bit/Joule]');
legend('M/K = 1','M/K = 2','M/K = 4','M/K = 8','Location','NorthEast');

axis([0 max(sumSE) 10^2 10^6])

semilogy(maxSE,maxEE,'ro','LineWidth',1,'MarkerFaceColor','r');


function c = tapas_hgf_ar1_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Hierarchical Gaussian Filter (HGF) for AR(1)
% processes and continuous inputs.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% This file refers to CONTINUOUS inputs (Eqs 48ff in Mathys et al., (2011));
% for binary inputs, refer to tapas_hgf_ar1_binary_config.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% The phis are estimated in 'logit space' because they are confined to the interval from 0 to 1.
% 'Logit-space' is a logistic sigmoid transformation of native space with a variable upper bound
% a>0:
%
% tapas_logit(x) = ln(x/(a-x)); x = a/(1+exp(-tapas_logit(x)))
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_hgf_ar1_plotTraj(est)
%
% where est is the stucture returned by tapas_fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu_0       row vector of initial values of mu (in ascending order of levels)
%         est.p_prc.sa_0       row vector of initial values of sigma (in ascending order of levels)
%         est.p_prc.phi        row vector of phis
%         est.p_prc.m          row vector of ms
%         est.p_prc.ka         row vector of kappas (in ascending order of levels)
%         est.p_prc.om         row vector of omegas (in ascending order of levels)
%         est.p_prc.al         alpha
%
%         est.traj.mu          mu (rows: trials, columns: levels)
%         est.traj.sa          sigma (rows: trials, columns: levels)
%         est.traj.muhat       prediction of mu (rows: trials, columns: levels)
%         est.traj.sahat       precisions of predictions (rows: trials, columns: levels)
%         est.traj.v           inferred variance of random walk (rows: trials, columns: levels)
%         est.traj.w           weighting factors (rows: trials, columns: levels)
%         est.traj.da          volatility prediction errors  (rows: trials, columns: levels)
%         est.traj.dau         input prediction error
%         est.traj.ud          updates with respect to prediction  (rows: trials, columns: levels)
%         est.traj.psi         precision weights on prediction errors  (rows: trials, columns: levels)
%         est.traj.epsi        precision-weighted prediction errors  (rows: trials, columns: levels)
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate) (rows: trials, columns: levels)
%
% Tips:
% - When analyzing a new dataset, take your inputs u and use
%
%   >> est = tapas_fitModel([], u, 'tapas_hgf_ar1_config', 'tapas_bayes_optimal_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
% - If the log-model evidence cannot be calculated because the Hessian poses problems, look at
%   est.optim.H and fix the parameters that lead to NaNs.
%
% - Your guide to all these adjustments is the log-model evidence (LME). Whenever the LME increases
%   by at least 3 across datasets, the adjustment was a good idea and can be justified by just this:
%   the LME increased, so you had a better model.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'hgf_ar1';

% Number of levels (minimum: 2)
c.n_levels = 2;

% Input intervals
% If input intervals are irregular, the last column of the input
% matrix u has to contain the interval between inputs k-1 and k
% in the k-th row, and this flag has to be set to true
c.irregular_intervals = false;

% Sufficient statistics of Gaussian parameter priors

% PLACEHOLDER VALUES
% It is often convenient to set some priors to values
% derived from the inputs. This can be achieved by
% using placeholder values. The available placeholders
% are:
%
% 99991   Value of the first input
%         Usually a good choice for mu_0mu(1)
% 99992   Variance of the first 20 inputs
%         Usually a good choice for mu_0sa(1)
% 99993   Log-variance of the first 20 inputs
%         Usually a good choice for logsa_0mu(1)
%         and logalmu
% 99994   Log-variance of the first 20 inputs minus two
%         Usually a good choice for ommu(1)

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first level, this is usually best
% kept fixed to 1 (determines origin on x_i-scale).
c.mu_0mu = [99991, 1];
c.mu_0sa = [99992, 0];

c.logsa_0mu = [99993, log(0.1)];
c.logsa_0sa = [    1,        1];

% Phis
% Format: row vector of length n_levels.
% Phi is estimated in logit-space because it is
% bounded between 0 and 1
% Fix this to zero (leading to a Gaussian random walk) by
% setting logitphimu = -Inf; logitphisa = 0;
c.logitphimu = [tapas_logit(0.1,1), -Inf];
c.logitphisa = [        10^2,    0];

% ms
% Format: row vector of length n_levels.
% This should be fixed for all levels where the omega of
% the next lowest level is not fixed because that offers
% an alternative parametrization of the same model.
c.mmu = [99991, c.mu_0mu(2)];
c.msa = [99992,           0];

% Kappas
% Format: row vector of length n_levels-1.
% This should be fixed (preferably to 1) if the observation model
% does not use mu_i+1 (kappa then determines the scaling of x_i+1).
c.logkamu = [log(1)];
c.logkasa = [     0];

% Omegas
% Format: row vector of length n_levels
c.ommu = [99994,   -6];
c.omsa = [ 10^2, 10^2];

% Alpha
% Format: scalar
% Fix this to zero (no percpeptual uncertainty) by setting
% logalmu = -Inf; logalsa = 0;
c.logalmu = 99993;
c.logalsa = 2^2;

% Gather prior settings in vectors
c.priormus = [
    c.mu_0mu,...
    c.logsa_0mu,...
    c.logitphimu,...
    c.mmu,...
    c.logkamu,...
    c.ommu,...
    c.logalmu,...
         ];

c.priorsas = [
    c.mu_0sa,...
    c.logsa_0sa,...
    c.logitphisa,...
    c.msa,...
    c.logkasa,...
    c.omsa,...
    c.logalsa,...
         ];

% Check whether we have the right number of priors
expectedLength = 4*c.n_levels+2*(c.n_levels-1)+2;
if length([c.priormus, c.priorsas]) ~= 2*expectedLength;
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_hgf_ar1;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_hgf_ar1_transp;

return;


% SP_EVALUATE_ELEMENT_LIST_PARAM: compute the basis functions, in the parametric domain, in a given list of elements.
%
%     sp = sp_evaluate_element_list_param (space, msh_elems, 'option1', value1, ...)
%
% INPUTS:
%
%    space:   object defining the space of discrete functions (see sp_scalar)
%    msh_elems: msh structure containing the information of quadrature or
%               visualization points, for a given list of elements
%               (see msh_cartesian/msh_evaluate_element_list)
%   'option', value: additional optional parameters, currently available options are:
%
%              Name     |   Default value |  Meaning
%           ------------+-----------------+----------------------------------
%            value      |      true       |  compute shape_functions
%            gradient   |      false      |  compute shape_function_gradients
%            hessian    |      false      |  compute shape_function_hessians
%
% OUTPUT:
%
%    sp: struct representing the discrete function space, with the following fields:
%              (see the article for a detailed description)
%
%    FIELD_NAME      (SIZE)                                        DESCRIPTION
%    ncomp           (scalar)                                      number of components of the functions of the space (actually, 1)
%    ndof            (scalar)                                      total number of degrees of freedom
%    ndof_dir        (1 x ndim vector)                             degrees of freedom along each direction
%    nsh_max         (scalar)                                      maximum number of shape functions per element
%    nsh             (1 x msh_elems.nel vector)                    actual number of shape functions per each element
%    connectivity    (nsh_max x msh_elems.nel vector)              indices of basis functions that do not vanish in each element
%    shape_functions (msh_elems.nqn x nsh_max x msh_elems.nel)     basis functions evaluated at each quadrature node in each element
%    shape_function_gradients
%         (ndim x msh_elems.nqn x nsh_max x msh_elems.nel)         basis function gradients evaluated at each quadrature node in each element
%    shape_function_hessians
%         (ndim x ndim x msh_elems.nqn x nsh_max x msh_elems.nel)  basis function hessians evaluated at each quadrature node in each element
%
% Copyright (C) 2009, 2010, 2011 Carlo de Falco
% Copyright (C) 2011, 2015, 2019 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

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

function sp = sp_evaluate_element_list_param (space, msh, varargin)

value = true;
gradient = false;
hessian = false;
if (~isempty (varargin))
  if (~rem (length (varargin), 2) == 0)
    error ('sp_evaluate_element_list_param: options must be passed in the [option, value] format');
  end
  for ii=1:2:length(varargin)-1
    if (strcmpi (varargin {ii}, 'value'))
      value = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'gradient'))
      gradient = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'hessian'))
      hessian = varargin {ii+1};
    else
      warning ('Ignoring unknown option %s', varargin {ii});
    end
  end
end

sp_univ = space.sp_univ;

elem_list = msh.elem_list;

elem_ind = cell (msh.ndim, 1);
[elem_ind{:}] = ind2sub (msh.nel_dir, elem_list);
elem_ind = cell2mat (elem_ind);

nsh = 1;
for idim = 1:msh.ndim
  nsh = nsh .* sp_univ(idim).nsh(elem_ind(idim,:));
end

for idim = 1:msh.ndim
  csize = ones (1, msh.ndim);
  csize(idim) = sp_univ(idim).nsh_max;
  crep = [sp_univ.nsh_max];
  crep(idim) = 1;
  conn{idim} = reshape (sp_univ(idim).connectivity(:,elem_ind(idim,:)), [csize, msh.nel]);
  conn{idim} = repmat (conn{idim}, [crep, 1]);
end

connectivity = zeros (space.nsh_max, msh.nel);
indices = ones (size (conn{1}));
for idim = 1:msh.ndim
  indices = indices & conn{idim} ~= 0;
end
% for idim = 1:msh.ndim
%   conn{idim} = conn{idim}(indices);
% end
connectivity(indices) = sub2ind ([space.ndof_dir, 1], conn{:}); % The extra 1 makes things work in any dimension
connectivity = reshape (connectivity, space.nsh_max, msh.nel);

clear conn csize crep indices

sp = struct('nsh_max', space.nsh_max, 'nsh', nsh, 'ndof', space.ndof,  ...
            'ndof_dir', space.ndof_dir, 'connectivity', connectivity, ...
            'ncomp', 1, 'degree', space.degree);

if (value || gradient || hessian)
shp = cell(1,msh.ndim); shg = cell(1,msh.ndim); shh = cell(1,msh.ndim);
  for idim = 1:msh.ndim
    ssize = ones (1, 2*msh.ndim);
    ssize([idim, msh.ndim+idim]) = [msh.nqn_dir(idim), sp_univ(idim).nsh_max];
    srep = [msh.nqn_dir, sp_univ.nsh_max];
    srep([idim, msh.ndim+idim]) = 1;
    shp{idim} = reshape (sp_univ(idim).shape_functions(:,:,elem_ind(idim,:)), [ssize, msh.nel]);
    shp{idim} = repmat (shp{idim}, [srep, 1]);
    shp{idim} = reshape (shp{idim}, msh.nqn, space.nsh_max, msh.nel);
    shg{idim} = reshape (sp_univ(idim).shape_function_gradients(:,:,elem_ind(idim,:)), [ssize, msh.nel]);
    shg{idim} = repmat (shg{idim}, [srep, 1]);
    shg{idim} = reshape (shg{idim}, msh.nqn, space.nsh_max, msh.nel);
    shh{idim} = reshape (sp_univ(idim).shape_function_hessians(:,:,elem_ind(idim,:)), [ssize, msh.nel]);
    shh{idim} = repmat (shh{idim}, [srep, 1]);
    shh{idim} = reshape (shh{idim}, msh.nqn, space.nsh_max, msh.nel);
  end

  if (value)
    sp.shape_functions = 1;
    for idim = 1:msh.ndim
      sp.shape_functions = sp.shape_functions .* shp{idim};
    end
  end

  if (gradient)
    for idim = 1:msh.ndim
      shape_fun_grad = shg{idim};
      for jdim = setdiff (1:msh.ndim, idim)
        shape_fun_grad = shape_fun_grad .* shp{jdim};
      end
      sp.shape_function_gradients(idim,:,:,:) = shape_fun_grad;
    end
    sp.shape_function_gradients = reshape (sp.shape_function_gradients, ...
                                  msh.ndim, msh.nqn, sp.nsh_max, msh.nel);
  end

  if (hessian && (isfield (msh, 'geo_map_der2') || msh.nel == 0))
    for idim = 1:msh.ndim
      shape_fun_hess = shh{idim};
      for jdim = setdiff (1:msh.ndim, idim)
        shape_fun_hess = shape_fun_hess .* shp{jdim};
      end
      sp.shape_function_hessians(idim,idim,:,:,:) = shape_fun_hess;

      for jdim = setdiff (1:msh.ndim, idim)
        shape_fun_hess = shg{idim} .* shg{jdim};
        for kdim = setdiff (1:msh.ndim, [idim, jdim])
          shape_fun_hess = shape_fun_hess .* shp{kdim};
        end
        sp.shape_function_hessians(idim,jdim,:,:,:) = shape_fun_hess;
      end
    end
  end

  clear shp shg shh

  if (strcmpi (space.space_type, 'NURBS'))
    sp = bsp_2_nrb__ (sp, msh, space.weights);
  end
end

end