function [normF, normLoc] = norm(f, n)
%NORM   Norm of a CHEBFUN object.
%   For scalar-valued column CHEBFUN objects:
%       NORM(F) = sqrt(integral of abs(F)^2).
%       NORM(F, 2) is the same as NORM(F).
%       NORM(F, 'fro') is also the same as NORM(F).
%       NORM(F, 1) = integral of abs(F).
%       NORM(F, P) = (integral of abs(F)^P)^(1/P).
%       NORM(F, inf) = max(abs(F)).
%       NORM(F, -inf) = min(abs(F)).
%
%   For array-valued column CHEBFUN objects:
%       NORM(F) is the Frobenius norm, sqrt(sum(svd(F).^2)).
%       NORM(F, 'fro') is the same as NORM(F).
%       NORM(F, 1) is the maximum of the 1-norms of the columns of F.
%       NORM(F, 2) is the largest singular value of F.
%       NORM(F, inf) is the maximum of the 1-norms of the rows of F.
%       NORM(F, -inf) is the minimum of the 1-norms of the rows of F.
%       NORM(F, P) is the P-th root of the maximum of the sum of the P-th
%                  powers of the magnitudes of the columns of F.
%
% Furthermore, the +\-inf norms for scalar-valued CHEBFUN objects may also
% return a second output, giving the position where the max/min occurs. For
% array-valued CHEBFUN objects, the 1 norm can return as its 2nd output the
% index of the column with the largest norm, while the inf and -inf norms
% can return as their 2nd output the point in the domain of the CHEBFUN at
% which the norm is attained.
%
% If F is a row CHEBFUN, NORM(F, TYPE) is equal to NORM(F.', TYPE).

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

% Empty CHEBFUN has norm 0:
if ( isempty(f) )
    normF = 0;
    return
end

if ( nargin == 1 )
    n = 'fro'; 	% Frobenius norm is the default (2 norm would be much slower).
end

% Initialise:
normLoc = [];
if ( numel(f) > 1 )
    numCols = numel(f);
else
    numCols = size(f.funs{1}, 2);
end

%% %%%%%%%%%%%%%%%%%%%%%%%%% SCALAR-VALUED CHEBFUNS %%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ( numCols == 1 )
    switch n
        case 1
            if ( nargout == 2 )
                error('CHEBFUN:CHEBFUN:norm:argout', ...
                        'Cannot return two outputs for 1-norms');
            end
            normF = sum(abs(f));

        case {2, 'fro'}
            if ( nargout == 2 )
                error('CHEBFUN:CHEBFUN:norm:argout', ...
                        'Cannot return two outputs for ''fro''-norms');
            end
            f.isTransposed = 0;
            normF = sqrt(abs(innerProduct(f, f)));

        case {inf, 'inf'}
            if ( isreal(f) )
                [normF, normLoc] = minandmax(f);
                [normF, idx] = max(abs(normF));
                normLoc = normLoc(idx);
            else
                [normF, normLoc] = max(conj(f).*f);
                normF = sqrt(normF);
            end

        case {-inf, '-inf'}
            [normF, normLoc] = min(conj(f).*f);
            normF = sqrt(normF);

        otherwise
            if ( isnumeric(n) && isreal(n) )
                if ( nargout == 2 )
                    error('CHEBFUN:CHEBFUN:norm:argout', ...
                            'Cannot return two outputs for p-norms.');
                end
                if ( mod(n, 2) == 0 )
                    normF = sum((conj(f).*f).^(n/2))^(1/n);
                else
                    normF = sum(abs(f).^n)^(1/n);
                end
            else
                error('CHEBFUN:CHEBFUN:norm:unknownNorm', ...
                 'The only matrix norms available are 1, 2, inf, and ''fro''.');
            end
    end

%% %%%%%%%%%%%%%%%%%%%%%%%%%% ARRAY-VALUED CHEBFUNS %%%%%%%%%%%%%%%%%%%%%%%%%%%%
else
    if ( f(1).isTransposed )
        f = f.';
    end

    switch n
        case 1
            f = mat2cell(f);
            normF = zeros(1, numel(f));
            for k = 1:numel(f)
                normF(k) = norm(f{k}, 1);
            end
            [normF, normLoc] = max(normF);

        case 2
            if (nargout == 2 )
                error('CHEBFUN:CHEBFUN:norm:argout', ...
                    ['Cannot return two outputs for 2-norms of ' ...
                     'array-valued CHEBFUNs.']);
            end
            s = svd(f, 0);
            normF = s(1);

        case 'fro'
            if ( nargout == 2 )
                error('CHEBFUN:CHEBFUN:norm:argout', ...
                    ['Cannot return two outputs for ''fro''-norms of ' ...
                     'array-valued CHEBFUNs.']);
            end
            normF = sqrt(sum(sum(f.*conj(f))));

        case {'inf', inf}
            [normF, normLoc] = max(sum(abs(f), 2));

        case {'-inf', -inf}
            [normF, normLoc] = min(sum(abs(f), 2));

        otherwise
            if ( isnumeric(n) && isreal(n) )
                [normF, normLoc] = max(sum(abs(f).^n, 2));
                normF = normF^(1/n);
            else
                error('CHEBFUN:CHEBFUN:norm:unknownNorm', ...
                 'The only matrix norms available are 1, 2, inf, and ''fro''.');
            end

    end

end

% Discard possible imaginary rounding errors:
normF = real(normF);

end


function [boxedX,Bx,By]=boxpdf(X)
% Forces the pdf of data to have a boxed distribution using a data adaptive lookup table.
%
% [boxedX,Bx,By]=boxpdf(X)
%
% boxedX=N(X) where N is an data adaptive monotonically increasing function.
% boxedX vary between zero and one.
%
% Bx,By describes the lookup table
%
% (c) Aslak Grinsted 2002-2004

%tested with: [prctile(x,boxpdf(x)*100)-x]

%home made fast unique:
[Bx,Js]=sort(X(:));
n=size(Bx,1);
d=(diff(Bx)~=0);

I=(1:n)';
I=I([d;logical(1)]);
%if (nargout>1)
    Bx=Bx(I);
%end

J=cumsum([1;d]);

n=length(X);
I=[0;I];

By=(.5/n)*(I(1:(end-1))+(I(2:end))); %percentile

%boxedX=By(J(Js([2:end 1])));
boxedX=interp1q(Bx,By,X);
%
% %old boxpdf:
%
% [sx i]=sort(X(:));
% [n, m] = size(sx);
% minx  = min(sx);
% maxx  = max(sx);
% range = maxx-minx;
%
%
% % Use the same Y vector if all columns have the same count
% eprob = [0.5./n:1./n:(n - 0.5)./n]';
%
%
% %group x's with same value
% I=[1; find(diff(sx)~=0)+1]; %unique indices
% Bx=sx(I); %unique values
% nI=diff([I;length(sx)+1])-1;
% By=zeros(length(Bx),1);
% for i=length(I):-1:1
%
%     By(i,1)=mean(eprob(I(i)+(0:nI(i))));
% end
% %plot(sx,y);
%
%
%
% boxedX=interp1q(Bx,By,X);


function f = p39_f ( n, x )

%*****************************************************************************80
%
%% P39_F evaluates the objective function for problem 39.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  f = x(1) * x(1) + 2.0 * x(2) * x(2) ...
    - 0.3 * cos ( 3.0 * pi * x(1) ) ...
    * cos ( 4.0 * pi * x(2) ) + 0.3;

  return
end


function R = RPYtoRot_ZXY(phi,theta,psi)
%RPYtoRot_ZXY Converts roll, pitch, yaw to a body-to-world Rotation matrix
%   The rotation matrix in this function is world to body [bRw] you will
%   need to transpose this matrix to get the body to world [wRb] such that
%   [wP] = [wRb] * [bP], where [bP] is a point in the body frame and [wP]
%   is a point in the world frame
%   written by Daniel Mellinger
%

R = [cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), ...
     cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), ...
     -cos(phi)*sin(theta); ...
     -cos(phi)*sin(psi),...
     cos(phi)*cos(psi), ...
     sin(phi);...
     cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi),...
     sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi),...
     cos(phi)*cos(theta)];

end


function [V,S] = alphavol(X,R,fig)
% ALPHAVOL Alpha shape of 2D or 3D point set.
%
%   V = ALPHAVOL(X,R) gives the area or volume V of the basic alpha shape
%   for a 2D or 3D point set. X is a coordinate matrix of size Nx2 or Nx3.
%
%   R is the probe radius with default value R = Inf. In the default case
%   the basic alpha shape (or alpha hull) is the convex hull.
%
%   [V,S] = ALPHAVOL(X,R) outputs a structure S with fields:
%    S.tri - Triangulation of the alpha shape (Mx3 or Mx4)
%    S.vol - Area or volume of simplices in triangulation (Mx1)
%    S.rcc - Circumradius of simplices in triangulation (Mx1)
%    S.bnd - Boundary facets (Px2 or Px3)
%
%   ALPHAVOL(X,R,1) plots the alpha shape.
%
%   % 2D Example - C shape
%   t = linspace(0.6,5.7,500)';
%   X = 2*[cos(t),sin(t)] + rand(500,2);
%   subplot(221), alphavol(X,inf,1);
%   subplot(222), alphavol(X,1,1);
%   subplot(223), alphavol(X,0.5,1);
%   subplot(224), alphavol(X,0.2,1);
%
%   % 3D Example - Sphere
%   [x,y,z] = sphere;
%   [V,S] = alphavol([x(:),y(:),z(:)]);
%   trisurf(S.bnd,x,y,z,'FaceColor','blue','FaceAlpha',1)
%   axis equal
%
%   % 3D Example - Ring
%   [x,y,z] = sphere;
%   ii = abs(z) < 0.4;
%   X = [x(ii),y(ii),z(ii)];
%   X = [X; 0.8*X];
%   subplot(211), alphavol(X,inf,1);
%   subplot(212), alphavol(X,0.5,1);
%
%   See also DELAUNAY, TRIREP, TRISURF

%   Author: Jonas Lundgren <splinefit@gmail.com> 2010

%   2010-09-27  First version of ALPHAVOL.
%   2010-10-05  DelaunayTri replaced by DELAUNAYN. 3D plots added.
%   2012-03-08  More output added. DELAUNAYN replaced by DELAUNAY.

if nargin < 2 || isempty(R), R = inf; end
if nargin < 3, fig = 0; end

% Check coordinates
dim = size(X,2);
if dim < 2 || dim > 3
    error('alphavol:dimension','X must have 2 or 3 columns.')
end

% Check probe radius
if ~isscalar(R) || ~isreal(R) || isnan(R)
    error('alphavol:radius','R must be a real number.')
end

% Unique points
[X,imap] = unique(X,'rows');

% Delaunay triangulation
T = delaunay(X);

% Remove zero volume tetrahedra since
% these can be of arbitrary large circumradius
if dim == 3
    n = size(T,1);
    vol = volumes(T,X);
    epsvol = 1e-12*sum(vol)/n;
    T = T(vol > epsvol,:);
    holes = size(T,1) < n;
end

% Limit circumradius of simplices
[~,rcc] = circumcenters(TriRep(T,X));
T = T(rcc < R,:);
rcc = rcc(rcc < R);

% Volume/Area of alpha shape
vol = volumes(T,X);
V = sum(vol);

% Return?
if nargout < 2 && ~fig
    return
end

% Turn off TriRep warning
warning('off','MATLAB:TriRep:PtsNotInTriWarnId')

% Alpha shape boundary
if ~isempty(T)
    % Facets referenced by only one simplex
    B = freeBoundary(TriRep(T,X));
    if dim == 3 && holes
        % The removal of zero volume tetrahedra causes false boundary
        % faces in the interior of the volume. Take care of these.
        B = trueboundary(B,X);
    end
else
    B = zeros(0,dim);
end

% Plot alpha shape
if fig
    if dim == 2
        % Plot boundary edges and point set
        x = X(:,1);
        y = X(:,2);
        plot(x(B)',y(B)','r','linewidth',2), hold on
        plot(x,y,'k.'), hold off
        str = 'Area';
    elseif ~isempty(B)
        % Plot boundary faces
        trisurf(TriRep(B,X),'FaceColor','red','FaceAlpha',1/3);
        str = 'Volume';
    else
        cla
        str = 'Volume';
    end
    axis equal
    str = sprintf('Radius = %g,   %s = %g',R,str,V);
    title(str,'fontsize',12)
end

% Turn on TriRep warning
warning('on','MATLAB:TriRep:PtsNotInTriWarnId')

% Return structure
if nargout == 2
    S = struct('tri',imap(T),'vol',vol,'rcc',rcc,'bnd',imap(B));
end

%--------------------------------------------------------------------------
function vol = volumes(T,X)
%VOLUMES Volumes/areas of tetrahedra/triangles

% Empty case
if isempty(T)
    vol = zeros(0,1);
    return
end

% Local coordinates
A = X(T(:,1),:);
B = X(T(:,2),:) - A;
C = X(T(:,3),:) - A;

if size(X,2) == 3
    % 3D Volume
    D = X(T(:,4),:) - A;
    BxC = cross(B,C,2);
    vol = dot(BxC,D,2);
    vol = abs(vol)/6;
else
    % 2D Area
    vol = B(:,1).*C(:,2) - B(:,2).*C(:,1);
    vol = abs(vol)/2;
end

%--------------------------------------------------------------------------
function B = trueboundary(B,X)
%TRUEBOUNDARY True boundary faces
%   Remove false boundary caused by the removal of zero volume
%   tetrahedra. The input B is the output of TriRep/freeBoundary.

% Surface triangulation
facerep = TriRep(B,X);

% Find edges attached to two coplanar faces
E0 = edges(facerep);
E1 = featureEdges(facerep, 1e-6);
E2 = setdiff(E0,E1,'rows');

% Nothing found
if isempty(E2)
    return
end

% Get face pairs attached to these edges
% The edges connects faces into planar patches
facelist = edgeAttachments(facerep,E2);
pairs = cell2mat(facelist);

% Compute planar patches (connected regions of faces)
n = size(B,1);
C = sparse(pairs(:,1),pairs(:,2),1,n,n);
C = C + C' + speye(n);
[~,p,r] = dmperm(C);

% Count planar patches
iface = diff(r);
num = numel(iface);

% List faces and vertices in patches
facelist = cell(num,1);
vertlist = cell(num,1);
for k = 1:num

    % Neglect singel face patches, they are true boundary
    if iface(k) > 1

        % List of faces in patch k
        facelist{k} = p(r(k):r(k+1)-1);

        % List of unique vertices in patch k
        vk = B(facelist{k},:);
        vk = sort(vk(:))';
        ik = [true,diff(vk)>0];
        vertlist{k} = vk(ik);

    end
end

% Sort patches by number of vertices
ivert = cellfun(@numel,vertlist);
[ivert,isort] = sort(ivert);
facelist = facelist(isort);
vertlist = vertlist(isort);

% Group patches by number of vertices
p = [0;find(diff(ivert));num] + 1;
ipatch = diff(p);

% Initiate true boundary list
ibound = true(n,1);

% Loop over groups
for k = 1:numel(ipatch)

    % Treat groups with at least two patches and four vertices
    if ipatch(k) > 1 && ivert(p(k)) > 3

        % Find double patches (identical vertex rows)
        V = cell2mat(vertlist(p(k):p(k+1)-1));
        [V,isort] = sortrows(V);
        id = ~any(diff(V),2);
        id = [id;0] | [0;id];
        id(isort) = id;

        % Deactivate faces in boundary list
        for j = find(id')
            ibound(facelist{j-1+p(k)}) = 0;
        end

    end
end

% Remove false faces
B = B(ibound,:);


function [ rexp, sexp ] = poly_t6 ( )

%*****************************************************************************80
%
%% POLY_T6 returns the monomials associated with a 6 node triangle.
%
%  Reference Element T6:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  6  5
%    |  |   \
%    |  |    \
%    0  1--4--2
%    |
%    +--0--R--1-->
%
%  Formula:
%
%    Given coefficients A(I), the polynomial interpolant at (R,S) is
%
%      P(R,S) = sum ( 1 <= I <= N ) A(I) * R**REXP(I) * S**SEXP(I)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer REXP(6), SEXP(6), the powers of R and S associated
%    with each monomial.
%
  rexp(1:6) = [ 0, 0, 1, 0, 1, 2 ];
  sexp(1:6) = [ 0, 1, 0, 2, 1, 0 ];

  return
end


function a = companion_inverse ( n, x )

%*****************************************************************************80
%
%% COMPANION_INVERSE returns the inverse of the COMPANION matrix.
%
%  Example:
%
%    N = 5, X = ( 1, 2, 3, 4, 5 )
%
%    0    1    0    0    0
%    0    0    1    0    0
%    0    0    0    1    0
%    0    0    0    0    1
%   1/1 -5/1 -4/1 -3/1 -2/1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 July 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub, Charles Van Loan,
%    Matrix Computations, second edition,
%    Johns Hopkins University Press, Baltimore, Maryland, 1989,
%    section 7.4.6.
%
%    Charles Kenney, Alan Laub,
%    Controllability and stability radii for companion form systems,
%    Math. Control Signals Systems,
%    Volume 1, 1988, pages 239-256.
%
%    James Wilkinson,
%    The Algebraic Eigenvalue Problem,
%    Oxford University Press,
%    1965, page 12.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real X(N), the coefficients of the polynomial
%    which define the matrix.  X(1) must not be zero.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for j = 1 : n
    for i = 1 : n

      if ( i == n )

        if ( j == 1 )
          a(i,j) = 1.0 / x(1);
        else
          a(i,j) = - x(n+2-j) / x(1);
        end

      elseif ( i == j - 1 )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


%% SQUAREPOISSONWG Poisson equation in a square domain solved by Weak Galerkin method.
%
%   squarePoissoWGn computes the lowest order Weak Galerkin approximations
%   of the Poisson equation in the unit square on a sequence of meshes
%   obtained by uniform refinement.
%
% See also PoissonWG,
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all; clear variables;

%% Parameters
maxIt = 4;
N = zeros(maxIt,1);
h = zeros(maxIt,1);
erruIuh = zeros(maxIt,1);

%% Generate an initial mesh
[node,elem] = squaremesh([0 1 0 1], 0.25);
%bdFlag = setboundary(node,elem,'Dirichlet','~(x==0)','Neumann','x==0');
bdFlag = setboundary(node,elem,'Dirichlet','all','Neumann','y==1');
%bdFlag = setboundary(node,elem,'Dirichlet');
for k = 1:3
    [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%     [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
end

%% Get the data of the pde
pde = sincosdata;
% pde = mixBCdata;
% option.solver = 'mg';

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    [soln,eqn,info] = PoissonWG(node,elem,bdFlag,pde);
    N(k) = size(elem,1);
    h(k) = 1./(sqrt(size(node,1))-1);
    % compute error
    uI = zeros(N(k)+size(eqn.edge,1),1);
    uI(1:N(k)) = pde.exactu((node(elem(:,1),:)+node(elem(:,2),:)+node(elem(:,3),:))/3);
    uI(N(k)+1:end) = pde.exactu((node(eqn.edge(:,1),:)+node(eqn.edge(:,2),:))/2);
    erruIuh(k) = sqrt((soln.u-uI)'*eqn.A*(soln.u-uI));
    % refine mesh
   [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%     [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
end

%% Plot convergence rates
figure(2);
showrateh(h,erruIuh);


function moment = truncated_normal_b_moment ( order, mu, sigma, b )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_B_MOMENT returns a moment of the upper truncated Normal PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Phoebus Dhrymes,
%    Moments of Truncated Normal Distributions,
%    May 2005.
%
%  Parameters:
%
%    Input, integer ORDER, the order of the moment.
%    0 <= ORDER.
%
%    Input, real MU, SIGMA, the mean and standard deviation of the
%    parent Normal distribution.
%
%    Input, real B, the upper truncation limit.
%
%    Output, real MOMENT, the moment of the PDF.
%
  if ( order < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRUNCATED_NORMAL_B_MOMENT - Fatal error!\n' );
    fprintf ( 1, '  ORDER < 0.\n' );
    error ( 'TRUNCATED_NORMAL_B_MOMENT - Fatal error!\n' );
  end

  h = ( b - mu ) / sigma;
  pdf = normal_01_pdf ( h );
  cdf = normal_01_cdf ( h );

  if ( cdf == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRUNCATED_NORMAL_B_MOMENT - Fatal error!\n' );
    fprintf ( 1, '  PDF/CDF ratio fails, because CDF too small.\n', cdf );
    fprintf ( 1, '  PDF = %g\n', pdf );
    fprintf ( 1, '  CDF = %g\n', cdf );
    error ( 'TRUNCATED_NORMAL_B_MOMENT - Fatal error!\n' );
  end

  f = pdf / cdf;

  moment = 0.0;
  irm2 = 0.0;
  irm1 = 0.0;

  for r = 0 : order

    if ( r == 0 )
      ir = 1.0;
    elseif ( r == 1 )
      ir = - f;
    else
      ir = - h ^ ( r - 1 ) * f + ( r - 1 ) * irm2;
    end

    moment = moment + r8_choose ( order, r ) ...
      * mu ^ ( order - r ) ...
      * sigma ^ r * ir;

    irm2 = irm1;
    irm1 = ir;

  end

  return
end


function q = p04_q ( m, c, w )

%*****************************************************************************80
%
%% P04_Q evaluates the integral for problem p04.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Genz,
%    A Package for Testing Multiple Integration Subroutines,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast and Graeme Fairweather,
%    Reidel, 1987, pages 337-340,
%    ISBN: 9027725144,
%    LC: QA299.3.N38.
%
%  Parameters:
%
%    Input, integer M, the dimension of the argument.
%
%    Input, real C(M), W(M), the problem parameters.
%
%    Output, real Q, the integral.
%
  q = 1.0;

  for i = 1 : m

    q = q * sqrt ( pi ) ...
      * ( erf ( c(i) * ( 1.0 - w(i) ) ) ...
        + erf ( c(i) *         w(i) ) ) ...
      / ( 2.0 * c(i) );

  end

  return
end


function varargout = kullbackleibler(varargin)
% KULLBACKLEIBLER
%
% y = KULLBACKLEIBLER(x,y)
%
% Computes/declares the convex Kullback-Leibler divergence sum(x.*log(x./y))
% Alternatively -sum(x.*log(y/x)), i.e., negated perspectives of log(y)
%
% See also ENTROPY, CROSSENTROPY

switch class(varargin{1})

    case 'double'
        if nargin == 1
            z = varargin{1};
            z = reshape(z,[],2);
            x = z(:,1);
            y = z(:,2);
        else
            x = varargin{1}(:);
            y = varargin{2}(:);
        end
        l = log(x./y);
        l(x<=0) = 0;
        l = real(l);
        varargout{1} = sum(x.*l);

    case {'sdpvar','ndsdpvar'}

        varargin{1} = reshape(varargin{1},[],1);
        varargin{2} = reshape(varargin{2},[],1);

        if length(varargin{1})~=length(varargin{2})
            if length(varargin{1})==1
                varargin{1} = repmat(varargin{1},length(varargin{2}),1);
            elseif  length(varargin{2})==1
                varargin{2} = repmat(varargin{2},length(varargin{1}),1);
            else
                error('Dimension mismatch in kullbackleibler')
            end
        end

        varargout{1} = yalmip('define',mfilename,[varargin{1};varargin{2}]);

    case 'char'

        X = varargin{3};
        F = [X >= 0];

        operator = struct('convexity','convex','monotonicity','none','definiteness','none','model','callback');
        operator.range = [-inf inf];
        operator.domain = [0 inf];
        operator.derivative = @derivative;

        varargout{1} = F;
        varargout{2} = operator;
        varargout{3} = X;

    otherwise
        error('SDPVAR/KULLBACKLEIBLER called with CHAR argument?');
end

function df = derivative(x)
z = abs(reshape(x,[],2));
x = z(:,1);
y = z(:,2);
% Use KL = -Entropy + Cross Entropy
df = [1+log(x);0*y] + [-log(y);-x./y];


%% Transient diffusion equation
%% PDE and boundary conditions
% The transient diffusion equation reads
%
% $$\alpha\frac{\partial c}{\partial t}+\nabla.\left(-D\nabla c\right)=0,$$
%
% where $c$ is the independent variable (concentration, temperature, etc)
% , $D$ is the diffusion coefficient, and $\alpha$ is a constant.
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc

%% Define the domain and create a mesh structure
L = 50;  % domain length
Nx = 20; % number of cells
m = createMesh3D(Nx,Nx,Nx, L,L,L);
%% Create the boundary condition structure
BC = createBC(m); % all Neumann boundary condition structure
BC.left.a(:) = 0; BC.left.b(:)=1; BC.left.c(:)=0; % left boundary
BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=0; % right boundary
BC.top.a(:) = 0; BC.top.b(:)=1; BC.top.c(:)=0; % top boundary
BC.bottom.a(:) = 0; BC.bottom.b(:)=1; BC.bottom.c(:)=0; % bottom boundary
%% define the transfer coeffs
D_val = 1;
D = createCellVariable(m, D_val);
alfa = createCellVariable(m, 1);
%% define initial values
c_init = 1;
c_old = createCellVariable(m, c_init,BC); % initial values
c = c_old; % assign the old value of the cells to the current values
%% loop
dt = 1; % time step
final_t = 50;
for t=dt:dt:final_t
    [M_trans, RHS_trans] = transientTerm(c_old, dt, alfa);
    Dave = harmonicMean(D);
    Mdiff = diffusionTerm(Dave);
    [Mbc, RHSbc] = boundaryCondition(BC);
    M = M_trans-Mdiff+Mbc;
    RHS = RHS_trans+RHSbc;
    c = solvePDE(m,M, RHS);
    c_old = c;
end
%% visualization
visualizeCells(c)


function D = diffmat(n, m)
%DIFFMAT   Differentiation matrices for ultraspherical spectral method.
%   D = DIFFMAT(N, M) returns the differentiation matrix that takes N Chebyshev
%   coefficients and returns N C^{(M)} coefficients that represent the derivative
%   of the Chebyshev series. Here, C^{(K)} is the ultraspherical polynomial basis
%   with parameter K.

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

if ( nargin == 1 )
    m = 1;
end

% Create the differentation matrix.
if ( m > 0 )
    D = spdiags((0 : n - 1)', 1, n, n);
    for s = 1:m-1
        D = spdiags(2*s*ones(n, 1), 1, n, n) * D;
    end
else
    D = speye(n);
end

end


function M = spm_mesh_polyhedron(name)
% Return one of the Platonic solids with triangle faces
% FORMAT M = spm_mesh_polyhedron(name)
% name     - polyhedron name
%            (one of {'tetrahedron','octahedron','icosahedron'})
%
% M        - patch structure
%__________________________________________________________________________
%
% See https://www.wikipedia.org/wiki/Platonic_solid
%__________________________________________________________________________
% Copyright (C) 2012-2018 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin
% $Id: spm_mesh_polyhedron.m 7383 2018-07-31 10:53:37Z guillaume $

switch lower(name)

    case 'tetrahedron'
        M.vertices = [  1   0  -1/sqrt(2) ;
                       -1   0  -1/sqrt(2) ;
                        0   1   1/sqrt(2) ;
                        0  -1   1/sqrt(2) ];
        M.faces    = [  1   2   3 ;
                        1   2   4 ;
                        2   3   4 ;
                        3   1   4];

    case 'octahedron'
        M.vertices = [  0   0   1 ;
                        1   0   0 ;
                        0   1   0 ;
                       -1   0   0 ;
                        0  -1   0 ;
                        0   0  -1 ];
        M.faces    = [  1   2   3 ;
                        1   3   4 ;
                        1   4   5 ;
                        1   5   2 ;
                        6   3   2 ;
                        6   4   3 ;
                        6   5   4 ;
                        6   2   5 ];

    case 'icosahedron'
        r = (1 + sqrt(5)) / 2;
        M.vertices = [  1   0  -r ;
                        0   r  -1 ;
                        r   1   0 ;
                        r  -1   0 ;
                        0  -r  -1 ;
                       -1   0  -r ;
                       -r   1   0 ;
                        0   r   1 ;
                        1   0   r ;
                        0  -r   1 ;
                       -r  -1   0 ;
                       -1   0   r ];
        M.faces    = [  1   2   3
                        3   4   1
                        1   4   5
                        5   6   1
                        1   6   2
                        2   6   7
                        7   8   2
                        2   8   3
                        3   8   9
                        9   4   3
                        4   9  10
                        4  10   5
                       10  11   5
                        5  11   6
                        6  11   7
                        7  12   8
                        8  12   9
                        9  12  10
                       10  12  11
                       11  12   7 ];

    otherwise
        error('''%s'' is not a Platonic solid with triangle faces.',name);
end


%--------------------------------------------------------------------------
%   [dataout] = steeringvector(array_pos,sig_pos)
%--------------------------------------------------------------------------
%   功能:
%   空间传播方法计算导向矢量
%--------------------------------------------------------------------------
%   输入:
%           array_pos               接收信号阵列坐标，按照列排列
%           sig_pos                 发射信号阵列坐标 按照列排列
%           lambda                  载频波长
%   输出:
%           A                       导向矢量
%--------------------------------------------------------------------------
%  例子:
% c = 3e8;
% fc = 20e9;
% lambda = c/fc;
% tgt_angle = [50 90 140];
% R = [1000 5600 9000];
%
% tgt_pos = [R.*cosd(tgt_angle);R.*sind(tgt_angle);zeros(1,3)];
% radar_pos = [zeros(1,8);lambda/2*(1:8)-4*lambda/2 - lambda/4;zeros(1,8)];
% steering_vector(radar_pos,tgt_pos(:,1),lambda)
%--------------------------------------------------------------------------
function A = steering_vector(array_pos,sig_pos,lambda)
for idx = 1:size(array_pos,2)
    R(idx,:) = norm(array_pos(:,idx)-sig_pos);
end
A = exp(1j.*2*pi*R./lambda);
A = conj(A);
end


% FITELLIPSE  Least-squares fit of ellipse to 2D points.
%        A = FITELLIPSE(X, Y) returns the parameters of the best-fit
%        ellipse to 2D points (X, Y).
%        The returned vector A contains the center, radii, and orientation
%        of the ellipse, stored as (Cx, Cy, Rx, Ry, theta_radians)
%

% Authors: Andrew Fitzgibbon, Maurizio Pilu, Bob Fisher
% Reference: "Direct Least Squares Fitting of Ellipses", IEEE T-PAMI, 1999
%
%  @Article{Fitzgibbon99,
%   author = "Fitzgibbon, A.~W.and Pilu, M. and Fisher, R.~B.",
%   title = "Direct least-squares fitting of ellipses",
%   journal = pami,
%   year = 1999,
%   volume = 21,
%   number = 5,
%   month = may,
%   pages = "476--480"
%  }
%
% This is a more bulletproof version than that in the paper, incorporating
% scaling to reduce roundoff error, correction of behaviour when the input
% data are on a perfect hyperbola, and returns the geometric parameters
% of the ellipse, rather than the coefficients of the quadratic form.
%
%  See also http://research.microsoft.com/en-us/um/people/awf/ellipse/
%
function a = fitEllipse(X, Y)
    % normalize data
    mx = mean(X);
    my = mean(Y);
    sx = (max(X)-min(X))/2;
    sy = (max(Y)-min(Y))/2;

    x = (X-mx)/sx;
    y = (Y-my)/sy;

    % Force to column vectors
    x = x(:);
    y = y(:);

    % Build design matrix
    D = [ x.*x  x.*y  y.*y  x  y  ones(size(x)) ];

    % Build scatter matrix
    S = D'*D;

    % Build 6x6 constraint matrix
    C(6,6) = 0; C(1,3) = -2; C(2,2) = 1; C(3,1) = -2;

    % Solve eigensystem
    if 0
        % Old way, numerically unstable if not implemented in matlab
        [gevec, geval] = eig(S,C);

        % Find the negative eigenvalue
        I = find(real(diag(geval)) < 1e-8 & ~isinf(diag(geval)));

        % Extract eigenvector corresponding to negative eigenvalue
        A = real(gevec(:,I));
    else
        % New way, numerically stabler in C [gevec, geval] = eig(S,C);

        % Break into blocks
        tmpA = S(1:3,1:3);
        tmpB = S(1:3,4:6);
        tmpC = S(4:6,4:6);
        tmpD = C(1:3,1:3);
        tmpE = inv(tmpC)*tmpB';
        [evec_x, eval_x] = eig(inv(tmpD) * (tmpA - tmpB*tmpE));

        % Find the positive (as det(tmpD) < 0) eigenvalue
        I = find(real(diag(eval_x)) < 1e-8 & ~isinf(diag(eval_x)));

        % Extract eigenvector corresponding to negative eigenvalue
        A = real(evec_x(:,I));

        % Recover the bottom half...
        evec_y = -tmpE * A;
        A = [A; evec_y];
    end

    % unnormalize
    par = [
    A(1)*sy*sy,   ...
        A(2)*sx*sy,   ...
        A(3)*sx*sx,   ...
        -2*A(1)*sy*sy*mx - A(2)*sx*sy*my + A(4)*sx*sy*sy,   ...
        -A(2)*sx*sy*mx - 2*A(3)*sx*sx*my + A(5)*sx*sx*sy,   ...
        A(1)*sy*sy*mx*mx + A(2)*sx*sy*mx*my + A(3)*sx*sx*my*my   ...
        - A(4)*sx*sy*sy*mx - A(5)*sx*sx*sy*my   ...
        + A(6)*sx*sx*sy*sy   ...
        ]';

    % Convert to geometric radii, and centers

    thetarad = 0.5*atan2(par(2),par(1) - par(3));
    cost = cos(thetarad);
    sint = sin(thetarad);
    sin_squared = sint.*sint;
    cos_squared = cost.*cost;
    cos_sin = sint .* cost;

    Ao = par(6);
    Au =   par(4) .* cost + par(5) .* sint;
    Av = - par(4) .* sint + par(5) .* cost;
    Auu = par(1) .* cos_squared + par(3) .* sin_squared + par(2) .* cos_sin;
    Avv = par(1) .* sin_squared + par(3) .* cos_squared - par(2) .* cos_sin;

    % ROTATED = [Ao Au Av Auu Avv]

    tuCentre = - Au./(2.*Auu);
    tvCentre = - Av./(2.*Avv);
    wCentre = Ao - Auu.*tuCentre.*tuCentre - Avv.*tvCentre.*tvCentre;

    uCentre = tuCentre .* cost - tvCentre .* sint;
    vCentre = tuCentre .* sint + tvCentre .* cost;

    Ru = -wCentre./Auu;
    Rv = -wCentre./Avv;

    Ru = sqrt(abs(Ru)).*sign(Ru);
    Rv = sqrt(abs(Rv)).*sign(Rv);

    a = [uCentre, vCentre, Ru, Rv, thetarad];
end


function xfat = r8mat_expand_linear ( m, n, x, mfat, nfat )

%*****************************************************************************80
%
%% R8MAT_EXPAND_LINEAR linearly interpolates new data into an R8MAT.
%
%  Discussion:
%
%    In this routine, the expansion is specified by giving the number
%    of intermediate values to generate between each pair of original
%    data rows and columns.
%
%    The interpolation is not actually linear.  It uses the functions
%
%      1, x, y, and xy.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of input data.
%
%    Input, real X(M,N), the original data.
%
%    Input, integer MFAT, NFAT, the number of data values to interpolate
%    between each row, and each column, of original data values.
%
%    Output, real XFAT(M2,N2), the fattened data, where
%    M2 = (M-1)*(MFAT+1)+1,
%    N2 = (N-1)*(NFAT+1)+1.
%
  for i = 1 : m

    if ( i < m )
      ihi = mfat;
    else
      ihi = 0;
    end

    for j = 1 : n

      if ( j < n )
        jhi = nfat;
      else
        jhi = 0;
      end

      if ( i < m )
        ip1 = i+1;
      else
        ip1 = i;
      end

      if ( j < n )
        jp1 = j+1;
      else
        jp1 = j;
      end

      x00 = x(i,j);
      x10 = x(ip1,j);
      x01 = x(i,jp1);
      x11 = x(ip1,jp1);

      for ii = 0 : ihi

        s = ii / ( ihi + 1 );

        for jj = 0 : jhi

          t = jj / ( jhi + 1 );

          iii = 1 + ( i - 1 ) * ( mfat + 1 ) + ii;
          jjj = 1 + ( j - 1 ) * ( nfat + 1 ) + jj;

          xfat(iii,jjj) = ...
                                            x00   ...
              + s     * (       x10       - x00 ) ...
              + t     * (             x01 - x00 ) ...
              + s * t * ( x11 - x10 - x01 + x00 );

        end

      end

    end

  end

  return
end


function [x,y,s,exitCode]=secondOrderConeInteriorPoint(A,b,c,blockLengths,params)
%%SECONDORDERCONEINTERIORPOINT Perform second order cone programming
%           (SOCP) using an interior point algorithm. This solves the
%           primal optimization problem:
%             minimize sum_{i=1}^n c_i'*x_i
%           subject to sum_{i=1}^n A_i*x_i=b
%             and x_i(1)-norm(x_i(2:end))>=0 (second order cone constraint)
%           which is equivalent to the dual optimization problem
%            maximize b'*y
%           subject to sum_{i=1}^n A_i'*y_i+s_i=c_i
%            and s_i(1)-norm(s_i(2:end))>=0
%           Many types of optimization problems can be formulated as SOCP
%           problems. This function only supports optimization in the real
%           domain.
%
%INPUTS: A A real mXsum(blockLengths) matrix containing coefficients for
%          all of the constraints on the primal. A must have full row rank.
%          With regard to the A_i in the problem formulation,
%          A=[A_1,a_2,...A_n]
%        b An mX1 real vector the coefficients of the dual objective
%          function to maximize.
%        c A sum(blockLengths)X1 vector containing the coefficients of the
%          primal objective function to minimize. With regard to the c_i in
%          the problem formulation, c=[c_1;c_2;...;c_n].
% blockLengths An nX1 vector containing the length of each of the cone
%          constraints. That is the first primal cone constraint is
%          x(1)-norm(x(2:blockLengths(1)))>=0. The second is
%          x(blockLengths(1)+1)-norm(x((blockLengths(1)+1):(blockLengths(1)+blockLengths(2)-1)))
%          and so on when considering the full stacked x vector (all of the
%          x_i stacked).
%   params An optional structure of parameters controlling the behaviour of
%          the algorithm. Possible members of the structure are:
%          gammaScal
%          'delta' A onstant parameter affecting the scale of a line search
%                  in the algorithm. 0<delta<1. The default if omitted is
%                  3/4.
%          'mu0'   The initial value of a positive real valued function
%                  affecting the smoothing function in the algorithm and
%                  thus the convergence. The default value if omitted is
%                  1/100.
%          'sigma' A constant parameter affecting the comvergence of the
%                  algorithm with delta. 0<sigma<1. The default if omitted
%                  is 1/4.
%          'maxIter' The maximum possible number of iterations to perform.
%                  The default if omitted is
%          'epsNormH' A tolerance value for declaring the convergence of
%                  the algorithm. This is the maximum value of norm(H) (H
%                  is defined in Equation 6 of [1]) below which convergence
%                  is declared. The default if omitted is 1e-6. Setting to
%                  zero will mean convergence is declared only when
%                  the minProgress contraint ocurs or the maximum number of
%                  iterations elapses.
%          'minProgress' A value such that the algorithm terminates if the
%                  change in normH is less than this value during multiple
%                  iterations. This allows termination when further
%                  iterations make little change. The default if omitted or
%                  an empty matrix is passed is epsNormH/(2*maxIter).
%                  Setting this to zero will make termination depend only
%                  on epsNormH and maxIter.
%
%OUTPUTS: x The sum(blockLengths)X1 set of stacked vectors solving the
%           primal optimization problem. x=[x_1;...;x_n] or an empty matrix
%           if an error occurred.
%         y The mX1 vector solving the dual optimization problem or an
%           empty matrix if an error occurred.
%         s The sum(blockLengths)X1 vector in the dual optimization
%           equality constraint or an empty matrix if an error occurred.
%  exitCode A value indicating how the algorithm terminated. Possible
%           values are
%           0 The accuracy constraint on norm(H) is satisfied (the
%             algorithm converged).
%           1 The maximum number of iterations elapsed without convergence.
%           2 Termination occurred because insufficient progress was made
%             (this depends on the params.minProgress value).
%           3 Termination occurred due to numerical problems.
%
%This function implements the algorithm described in [1]. The
%implementation slightly differs from the paper in that it can handle
%multiple cone constraints. The implementation does not take advantage of
%any sparsity in A.
%
%REFERENCES:
%[1] X. Chi and S. Liu, "A one-step smoothing Newton method for second-
%    order cone programming," Journal of Computational and Applied
%    Mathematics, vol. 223, no. 1, pp. 114-123, 1 Jan. 2009.
%
%February 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%ni(i) is the size of the ith block.
%Implementing algorithm 3.1 in the paper....

numBlocks=length(blockLengths);%The number of constraints.
m=size(A,1);
k=size(A,2);%The length of x and b.

%Get a cell array where blockIndices{i} holds the indices for the elements
%of the ith block.
blockIndices=makeIndexList(blockLengths);

%Default parameters and initialization before step 1.
gammaScal=0.95;
delta=3/4;%A constant.
sigma=1/4;%A constant
mu0=1/100;%A positive value.
maxIter=100;
epsNormH=1e-6;%Convergence criterion on the norm of H.
%Termination criterion for taking a minimum stepsize
minProgress=epsNormH/(2*maxIter);

%If the user provided any parameters
if(nargin>4&&~isempty(params))
    if(isfield(params,'gammaScal'))
        gammaScal=params.gammaScal;
    end

    if(isfield(params,'delta'))
        delta=params.delta;
    end

    if(isfield(params,'sigma'))
        sigma=params.sigma;
    end

    if(isfield(params,'mu0'))
        mu0=params.mu0;
    end

    if(isfield(params,'maxIter'))
        maxIter=params.maxIter;
    end

    if(isfield(params,'epsNormH'))
        epsNormH=params.epsNormH;
    end

    if(isfield(params,'minProgress'))
        minProgress=params.minProgress;
    end
end

%Initialization before step 1
mu=mu0*ones(numBlocks,1);
zBar=[zeros(k+m,1);mu];%A constant

%We want initial x values that satisfy the cone constraints. We could set
%then all zero, but that would put it right on the boundary, so we will
%make all of the first elements of the cone constraints 1 and the rest
%zero, which puts it a bit away from the boundary.
x=zeros(k,1);
curIdx=1;
for curBlock=1:numBlocks
   x(curIdx)=1;
   curIdx=curIdx+blockLengths(curBlock);
end

%Initial dual variables.
y=zeros(m,1);%Dual variables initially just zero.
s=c-A'*y;%s must satisfy A'*y+s=c.
H=HFun(x,s,mu,A,b,blockIndices);
normH=norm(H);
normHPrev=Inf;

lkPrev=0;

%gammaVal satisfies both of the constraints for gamma: It is less than 1
%(as gammaScal is less than 1) and it is less than 1/normH.
gammaVal=gammaScal*min(1,1/normH);

if(any(~isreal(H)|~isfinite(H)))
    x=[];
    y=[];
    s=[];
    exitCode=3;%Numerical errors.
    return;
end

for curIter=1:maxIter
%%Step 1
    if(normH<epsNormH)%If it converged.
        exitCode=0;
        return;
    elseif(normHPrev-normH<minProgress)
        %If normH did not sufficiently change between iterations, then
        %stop.
        exitCode=2;
        return;
    end

    %Rho is defined between equations 7 and 8.
    rho=gammaVal*normH*min(1,normH);

%%Step 2
    gradH=HFunGrad(x,s,mu,A,blockIndices);
    [HGradSqrt,didFail]=chol(gradH,'lower');
    if(didFail)%Using pinv for stability if poorly conditioned.
        deltaZ=lsqminnorm(gradH,(rho*zBar-H));
    else%Otherwise, solve using forward and backward substitution.
        opts.LT=true;
        opts.UT=false;
        %Solve using forward substitution.
        temp=linsolve(HGradSqrt,rho*zBar-H,opts);
        opts.UT=true;
        opts.LT=false;
        %Solve using backward substitution.
        deltaZ=linsolve(HGradSqrt',temp,opts);
    end
    deltaX=deltaZ(1:k);
    deltaY=deltaZ((k+1):(k+m));
    deltaMu=deltaZ((k+m+1):end);

%Steps 3 and 4
    %Search for the smallest allowable integer lk >=0. In this instance,
    %rather than just searching from 0 and going upwards, we start at the
    %last found value. In large problems, this can make a difference as lk
    %does not necessarily change much and is often not just 0.

    lk=lkPrev;
    lambda=delta^lk;
    xNew=x+lambda*deltaX;
    yNew=y+lambda*deltaY;
    muNew=mu+lambda*deltaMu;
    sNew=c-A'*yNew;
    HNew=HFun(xNew,sNew,muNew,A,b,blockIndices);
    normHNew=norm(HNew);
    boolValPrev=normHNew<=(1-sigma*(1-gammaVal*mu0)*lambda)*normH;

    while(1)
        if(boolValPrev==true)
            if(lk==0)
                break;
            end
            lk=lk-1;
        else
            lk=lk+1;
        end
        lambda=delta^lk;
        xNew=x+lambda*deltaX;
        yNew=y+lambda*deltaY;
        muNew=mu+lambda*deltaMu;
        sNew=c-A'*yNew;
        HNew=HFun(xNew,sNew,muNew,A,b,blockIndices);
        normHNew=norm(HNew);
        boolVal=normHNew<=(1-sigma*(1-gammaVal*mu0)*lambda)*normH;

        if(boolVal==true && boolValPrev==false)
            break;
        end
        boolValPrev=boolVal;
    end

    %We subtract one, because the loop above wants to try to bound the
    %correct lk value.
    lkPrev=max(lk-1,0);

    x=xNew;
    y=yNew;
    mu=muNew;
    s=sNew;
    H=HNew;
    normHPrev=normH;
    normH=normHNew;
end

%Maximum number of iterations completed.
exitCode=1;

end

function blockIndices=makeIndexList(blockLengths)
%%MAKEINDEXLIST This takes the vector of block lentghs and returns a cell
%               array where the ith element contains the indices of the
%               components in the ith block.

numBlocks=length(blockLengths);

blockIndices=cell(numBlocks,1);
curIdx=1;
for curBlock=1:numBlocks
    blockIndices{curBlock}=curIdx:(curIdx+blockLengths(curBlock)-1);
    curIdx=curIdx+blockLengths(curBlock);
end
end

function H=HFun(x,s,mu,A,b,blockIndices)
%%HFUN This implements the function H(z) from Equation 6. Note that s is
%%used instead of y, because c-A'*y is supposed to be passed as the second
%%argument to phiFun and that is equal to s.

H=[b-A*x;
   phiFun(x,s,mu,blockIndices);
   mu];
end

function gradG=HFunGrad(x,s,mu,A,blockIndices)
%%GRADH This function implements the gradient of H(z) in Equation 10. It
%       has been modified to handle more than one constraint.

m=size(A,1);
k=size(A,2);
numBlocks=length(blockIndices);

M=zeros(k,k);
N=zeros(k,k);
P=zeros(k,numBlocks);

%Allocate space and zero.
gradG=zeros(m+k+numBlocks,m+k+numBlocks);

gradG(1:m,1:k)=-A;%The first m rows.

for curBlock=1:numBlocks
    sel=blockIndices{curBlock};
    xCur=x(sel);
    sCur=s(sel);
    muCur=mu(curBlock);

    %e is defined toward the beginning of section 2.
    blockLength=length(sel);
    e=zeros(blockLength,1);
    e(1)=1;

    w1Bar=xCur+muCur*sCur;%s=c-A'*y
    w2Bar=muCur*xCur+sCur;
    wBar=vectorSqrt(vectorSquared(w1Bar)+vectorSquared(w2Bar)+2*muCur^2*e);

    %Lw is defined at the beginning of Section 2. Lw should be positive
    %definite, but is often pooly conditioned, so we use a pseudoinverse.
    Lw=Lx(wBar);
    [LwChol,cholDidFail]=chol(Lw,'lower');
    if(cholDidFail)
        LwInv=pinv(Lx(wBar));
    else
        opts.LT=true;
        opts.UT=false;
        %Solve using forward substitution.
        temp=linsolve(LwChol,eye(size(Lw)),opts);
        opts.UT=true;
        opts.LT=false;
        %Solve using backward substitution.
        LwInv=linsolve(LwChol',temp,opts);
    end

    Lw1=Lx(w1Bar);
    Lw2=Lx(w2Bar);

    M(sel,sel)=(1+muCur)*eye(blockLength,blockLength)-LwInv*(Lw1+muCur*Lw2);
    N(sel,sel)=(1+muCur)*eye(blockLength,blockLength)-LwInv*(muCur*Lw1+Lw2);
    P(sel,curBlock)=xCur+sCur-LwInv*(Lw1*sCur+Lw2*xCur+2*muCur*e);
end

gradG((m+1):(m+k),1:k)=M;
gradG((m+1):(m+k),(k+1):(k+m))=-N*A';
gradG((m+1):(m+k),(k+m+1):(k+m+numBlocks))=P;
gradG((m+k+1):(m+k+numBlocks),(m+k+1):(m+k+numBlocks))=eye(numBlocks,numBlocks);

end

function val=Lx(x)
%%LX Create the arrow-shaped matrix Lx that is given toward the beginning
%    of section 2. x must be a single set of cone variables.

    x0=x(1);
    xBar=x(2:end);
    xBarLen=length(xBar);

    val=[x0,xBar';
         xBar,x0*eye(xBarLen,xBarLen)];
end

function phi=phiFun(x,s,mu,blockIndices)
%%PHIBAR This implements phi(x,s,mu) from Equation 4. It has been modified
%%to deal with multiple blocks. in this instance, the value for each block
%%is stacked.

numBlocks=length(blockIndices);

phi=zeros(length(x),1);

%jordan_sqrt(jordan_square
for curBlock=1:numBlocks
    sel=blockIndices{curBlock};

    %e is defined toward the beginning of section 2.
    numIdx=length(sel);
    e=zeros(numIdx,1);
    e(1)=1;

    muCur=mu(curBlock);
    xCur=x(sel);
    sCur=s(sel);

    phi(sel)=(1+muCur)*(xCur+sCur)-vectorSqrt(vectorSquared(xCur+muCur*sCur)+vectorSquared(muCur*xCur+sCur)+2*muCur^2*e);
end

end

function xSquared=vectorSquared(x)
%%VECTORSQUARED In Section 2, a definition of the square a second order
%cone vector in terms of a spectral factorization is given. It is
%%implemented in this function.

[lambda,u]=vectorSpectralFact(x);

xSquared=lambda(1)^2*u(:,1)+lambda(2)^2*u(:,2);

end

function sqrtX=vectorSqrt(x)
%%VECTORSQRT In Section 2, a definition of the square root of a second
%%order cone vector in terms of a spectral factorization is given. It is
%%implemented in this function.

[lambda,u]=vectorSpectralFact(x);

%sqrtX should be real.
sqrtX=sqrt(lambda(1))*u(:,1)+sqrt(lambda(2))*u(:,2);

end

function [lambda,u]=vectorSpectralFact(x)
%%SPECTRALFACT In section 2, a spectral factorization of a second order
%       cone vector is defined. The vector is factored into spectral values
%       lambda and spectral vectors u. This function computes the
%       factorization.

x0=x(1);
x1=x(2:end);
x1Mag=norm(x1);

%lambda should be positive. However, finite precision errors might push it
%negative.
lambda=abs([x0-x1Mag;x0+x1Mag]);

x1Norm=x1/x1Mag;
if(~all(isfinite(x1Norm)))
    x1Norm=zeros(size(x1));
    x1Norm(1)=1;
end

u=(1/2)*[[1;-x1Norm],[1;x1Norm]];
end

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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 	WRITE A FUNCTION THAT COMPUTES THE JACOBIAN OF A ROBOT
%
%   Jn = compute_jacobian(robot, q)
%
%   The method exposed here as a solution is translated from the method in:
%
%   "Robot Modeling and Control". Mark W. Spong, Seth Hutchinson, M. Vidyasagar
%   Ed. Wiley
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2016, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function Jn = compute_jacobian(robot, q)
Jn = []; %Initialize J
for i=1:robot.DOF,
   Ji = jacobian_submatrix(robot, q, i);
   Jn = [Jn Ji]; % Add submatrix to J
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Computes the value of the submatrix Ji which accounts for the
%   contribution of joint i (either rotational or prismatic) in the
%   rotational and translational speed of the end effector.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Ji = jacobian_submatrix(robot, q, i)
%CAution, the following functions are evaluated according to the values of q
%compute direct kinematics for serial robotics
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alfa = eval(robot.DH.alpha);

%load the position/orientation of the robot's base
T = robot.T0;
%Compute Matrix until joint i-1 (note that no computation is performed for i=1)
for j=1:i-1,
    T=T*dh(theta(j), d(j), a(j), alfa(j));
end

% obtain z_{i-1}
zim = T(1:3,3);
% obtain o_{i-1}
oim = T(1:3,4);
%a zero vector
zero = zeros(3,1);

%Compute the end effector's position
T = directkinematic(robot, q);
on = T(1:3,4);

%rotational joint
if robot.kind(i) == 'R'
    Ji = [cross(zim,(on-oim)); zim];
else %prismatic joint
    Ji = [zim; zero];
end


function [ MRI_signal_noisy ] = add_Rician_noise_to_simulated_dMRI_data( MRI_signal, SNR )
%ADD_RICIAN_NOISE_TO_SIMULATED_DMRI_DATA adds Rician noise to simulated
%MRI_signals.
%
% inputs: noisefree MRI_signal and SNR
% outputs: noisy MRI_signal
%
% noisy signal, S = |S* + N1 + iN2|
% where S* = noise-free signal
% N1 and N2 are random numbers, drawn from normal distribution, with mean=0, and std = 1/SNR.

% Author: Joanne Bates <joanne.bates@eng.ox.ac.uk>
% Copyright (c) 2015 University of Oxford
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

no_of_directions =  size(MRI_signal);
N1 = (1/SNR).*randn(no_of_directions);
N2 = (1/SNR).*randn(no_of_directions);

MRI_signal_noisy = abs(MRI_signal + N1 + 1i.*N2);
end


function yhat=spence21(y)
% function for Spencer's 21-point graduation rule.
% set out following Spencer's hand-calculation method,
% which isn't the shortest computer program!

oten = ones(10,1);
n = length(y);
y = [ oten*y(1); y; oten*y(n) ];

n = length(y);
k = 4:(n-3);
y1 = -y(k-3) + y(k-1) + 2*y(k) + y(k+1) -y(k+3);

n = length(y1);
k = 4:(n-3);
y2 = y1(k-3)+y1(k-2)+y1(k-1)+y1(k)+y1(k+1)+y1(k+2)+y1(k+3);

n = length(y2);
k = 3:(n-2);
y3 = y2(k-2)+y2(k-1)+y2(k)+y2(k+1)+y2(k+2);

n = length(y3);
k = 3:(n-2);
y4 = y3(k-2)+y3(k-1)+y3(k)+y3(k+1)+y3(k+2);

yhat = y4/350;
return;


function [ a, seed ] = wathen_ge ( nx, ny, n, seed )

%*****************************************************************************80
%
%% WATHEN_GE returns the Wathen matrix, using general (GE) storage.
%
%  Discussion:
%
%    The Wathen matrix is a finite element matrix which is sparse.
%
%    The entries of the matrix depend in part on a physical quantity
%    related to density.  That density is here assigned random values between
%    0 and 100.
%
%    The matrix order N is determined by the input quantities NX and NY,
%    which would usually be the number of elements in the X and Y directions.
%    The value of N is
%
%      N = 3*NX*NY + 2*NX + 2*NY + 1,
%
%    The matrix is the consistent mass matrix for a regular NX by NY grid
%    of 8 node serendipity elements.
%
%    Here is an illustration for NX = 3, NY = 2:
%
%     23-24-25-26-27-28-29
%      |     |     |     |
%     19    20    21    22
%      |     |     |     |
%     12-13-14-15-16-17-18
%      |     |     |     |
%      8     9    10    11
%      |     |     |     |
%      1--2--3--4--5--6--7
%
%    For this example, the total number of nodes is, as expected,
%
%      N = 3 * 3 * 2 + 2 * 2 + 2 * 3 + 1 = 29
%
%    The matrix is symmetric positive definite for any positive values of the
%    density RHO(X,Y).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
%    Andrew Wathen,
%    Realistic eigenvalue bounds for the Galerkin mass matrix,
%    IMA Journal of Numerical Analysis,
%    Volume 7, Number 4, October 1987, pages 449-457.
%
%  Parameters:
%
%    Input, integer NX, NY, values which determine the size of the matrix.
%
%    Input, integer N, the number of variables.
%
%    Input/output, integer SEED, the random number seed.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  em = [
     6.0, -6.0,  2.0, -8.0,  3.0, -8.0,  2.0, -6.0;
    -6.0, 32.0, -6.0, 20.0, -8.0, 16.0, -8.0, 20.0;
     2.0, -6.0,  6.0, -6.0,  2.0, -8.0,  3.0, -8.0;
    -8.0, 20.0, -6.0, 32.0, -6.0, 20.0, -8.0, 16.0;
     3.0, -8.0,  2.0, -6.0,  6.0, -6.0,  2.0, -8.0;
    -8.0, 16.0, -8.0, 20.0, -6.0, 32.0, -6.0, 20.0;
     2.0, -8.0,  3.0, -8.0,  2.0, -6.0,  6.0, -6.0;
    -6.0, 20.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0 ]';

  [ rho, seed ] = r8mat_uniform_01 ( nx, ny, seed );
  rho = 100.0 * rho;

  node = zeros(8,1);

  for j = 1 : ny

    for i = 1 : nx
%
%  For the element (I,J), determine the indices of the 8 nodes.
%
      node(1) = 3 * j * nx + 2 * j + 2 * i + 1;
      node(2) = node(1) - 1;
      node(3) = node(1) - 2;

      node(4) = ( 3 * j - 1 ) * nx + 2 * j + i - 1;
      node(8) = node(4) + 1;

      node(5) = ( 3 * j - 3 ) * nx + 2 * j + 2 * i - 3;
      node(6) = node(5) + 1;
      node(7) = node(5) + 2;

      for krow = 1 : 8
        for kcol = 1 : 8
          a(node(krow),node(kcol)) = a(node(krow),node(kcol)) ...
            + rho(i,j) * em(krow,kcol);
        end
      end

    end
  end

  return
end


function H = hessian_2sided_nrows(f,x,k,varargin)
% Computes the last K rows of a 2-sided finite difference Hessian
%
% USAGE:
%   H=hessian_2sided_nrows(FUNC,X,K,VARARGIN)
%
% INPUTS:
%   FUNC         - Function name, fval = func(x,varargin)
%   X            - Vector of parameters (N x 1)
%   K            - Number of rows to compute
%   VARARGIN     - Optional arguments passed to the function
%
% OUTPUTS:
%   H            - Finite difference, 2-sided hessian: N x K
%
% COMMENTS:
%   Modification of hessian_2sided
%   See also HESSIAN_2SIDED

% Code originally from COMPECON toolbox [www4.ncsu.edu/~pfackler]
% documentation modified to fit the format of the Econometrics Toolbox
% by James P. LeSage, Dept of Economics
% University of Toledo
% 2801 W. Bancroft St,
% Toledo, OH 43606
% jlesage@spatial-econometrics.com
%
% Further modified (to do 2-sided numerical derivs, rather than 1) by:
% Modifications Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

if size(x,2)>size(x,1)
    x=x';
end
if size(x,2)~=1
    error('X must be a column vector.')
end
n = size(x,1);

try
    feval(f,x,varargin{:});
catch FE
    errtxt=['There was an error evaluating the function.  Please check the arguements.  The specific error was:' FE.message];
    error(errtxt);
end

fx = feval(f,x,varargin{:});

% Compute the stepsize (h)
h = eps.^(1/3)*max(abs(x),1e-2);
xh = x+h;
h = xh-x;

ee = sparse(1:n,1:n,h,n,n);

% Compute forward and backward steps
gp = zeros(n,1);
gm = zeros(n,1);
for i=1:n
    gp(i) = feval(f,x+ee(:,i),varargin{:});
    gm(i) = feval(f,x-ee(:,i),varargin{:});
end

hh=h*h';
Hm=NaN*ones(n);
Hp=NaN*ones(n);
% Compute "double" forward and backward steps
for i=n-k+1:n
    for j=1:n
        Hp(i,j) = feval(f,x+ee(:,i)+ee(:,j),varargin{:});
        Hp(j,i)=Hp(i,j);
        Hm(i,j) = feval(f,x-ee(:,i)-ee(:,j),varargin{:});
        Hm(j,i)=Hm(i,j);
    end
end
%Compute the hessian
H = zeros(n);
for i=(n-k+1):n
    for j=1:n
        H(i,j) = (Hp(i,j)-gp(i)-gp(j)+fx+fx-gm(i)-gm(j)+Hm(i,j))/hh(i,j)/2;
        H(j,i) = H(i,j);
    end
end

%Only the final K rows
H=H((n-k+1):n,:);


function [ value, ifault ] = betain ( x, p, q, beta )

%*****************************************************************************80
%
%% BETAIN computes the incomplete Beta function ratio.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by KL Majumder, GP Bhattacharjee.
%    FORTRAN90 version by John Burkardt.
%
%  Reference:
%
%    KL Majumder, GP Bhattacharjee,
%    Algorithm AS 63:
%    The incomplete Beta Integral,
%    Applied Statistics,
%    Volume 22, Number 3, 1973, pages 409-411.
%
%  Parameters:
%
%    Input, real X, the argument, between 0 and 1.
%
%    Input, real P, Q, the parameters, which
%    must be positive.
%
%    Input, real BETA, the logarithm of the complete
%    beta function.
%
%    Output, real BETAIN, the value of the incomplete
%    Beta function ratio.
%
%    Output, integer IFAULT, error flag.
%    0, no error.
%    nonzero, an error occurred.
%
  acu = 0.1D-14;

  value = x;
  ifault = 0;
%
%  Check the input arguments.
%
  if ( p <= 0.0 | q <= 0.0 )
    ifault = 1;
    return
  end

  if ( x < 0.0 | 1.0 < x )
    ifault = 2;
    return
  end
%
%  Special cases.
%
  if ( x == 0.0 | x == 1.0 )
    return
  end
%
%  Change tail if necessary and determine S.
%
  psq = p + q;
  cx = 1.0 - x;

  if ( p < psq * x )
    xx = cx;
    cx = x;
    pp = q;
    qq = p;
    indx = 1;
  else
    xx = x;
    pp = p;
    qq = q;
    indx = 0;
  end

  term = 1.0;
  ai = 1.0;
  value = 1.0;
  ns = floor ( qq + cx * psq );
%
%  Use the Soper reduction formula.
%
  rx = xx / cx;
  temp = qq - ai;
  if ( ns == 0 )
    rx = xx;
  end

  while ( 1 )

    term = term * temp * rx / ( pp + ai );
    value = value + term;
    temp = abs ( term );

    if ( temp <= acu & temp <= acu * value )

      value = value * exp ( pp * log ( xx ) ...
      + ( qq - 1.0 ) * log ( cx ) - beta ) / pp;

      if ( indx )
        value = 1.0 - value;
      end

      break

    end

    ai = ai + 1.0;
    ns = ns - 1;

    if ( 0 <= ns )
      temp = qq - ai;
      if ( ns == 0 )
        rx = xx;
      end
    else
      temp = psq;
      psq = psq + 1.0;
    end

  end

  return
end


function value = f_02 ( x )

%*****************************************************************************80
%
%% F_02 evaluates 2*x-exp(-x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the point at which F is to be evaluated.
%
%    Output, real VALUE, the value of the function at X.
%
  value = 2.0 * x - exp ( - x );

  return
end


function grid_index = multigrid_scale_closed ( dim_num, order_nd, level_max, ...
  level_1d, grid_index )

%*****************************************************************************80
%
%% MULTIGRID_SCALE_CLOSED renumbers a grid as a subgrid on a higher level.
%
%  Discussion:
%
%    This routine takes a grid associated with a given value of
%    LEVEL, and multiplies all the indices by a power of 2, so that
%    the indices reflect the position of the same points, but in
%    a grid of level LEVEL_MAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer ORDER_ND, the number of points in the grid.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Input, integer LEVEL_1D(DIM_NUM), the level in each dimension.
%
%    Input, integer GRID_INDEX(DIM_NUM,POINT_NUM), the index
%    values for each grid point, based in the level for which the grid
%    was generated.
%
%    Output, integer GRID_INDEX(DIM_NUM,POINT_NUM), the index
%    values for each grid point, appropriate for the grid as a subgrid
%    of a grid of level LEVEL_MAX.
%
  for dim = 1 : dim_num

    if ( level_1d(dim) == 0 )

      if ( 0 == level_max )
        order_max = 1;
      else
        order_max = 2^level_max + 1;
      end

      grid_index(dim,1:order_nd) = floor ( ( order_max - 1 ) / 2 );

    else

      factor = 2^( level_max - level_1d(dim) );

      grid_index(dim,1:order_nd) = grid_index(dim,1:order_nd) * factor;

    end

  end

  return
end


function spline_test07 ( )

%*****************************************************************************80
%
%% TEST07 tests BASIS_MATRIX_OVERHAUSER_UNI, BASIS_MATRIX_TMP.
%
%  Discussion:
%
%   YDATA(1:NDATA) = ( TDATA(1:NDATA) + 2 )**2 + 3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author
%
%    John Burkardt
%
  n = 4;
  ndata = 4;
  nsample = 4;
  tdata = [ -1.0E+00, 0.0E+00, 1.0E+00, 2.0E+00 ];
  ydata = [ 4.0E+00, 7.0E+00, 12.0E+00, 19.0E+00 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST07\n' );
  fprintf ( 1, '  BASIS_MATRIX_OVERHAUSER_UNI sets up the basis\n' );
  fprintf ( 1, '    matrix for the uniform Overhauser spline.\n' );

  mbasis = basis_matrix_overhauser_uni ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '    TDATA, YDATA\n' );
  fprintf ( 1, '\n' );
  for i = 1 : ndata
    fprintf ( 1, '%14f  %14f\n', tdata(i), ydata(i) );
  end

  left = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, '    T, Spline(T)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : ndata

    if ( i == 0 )
      tlo = tdata(1) - 0.5E+00 * ( tdata(2) - tdata(1) );
      thi = tdata(1);
    elseif ( i < ndata )
      tlo = tdata(i);
      thi = tdata(i+1);
    elseif ( ndata <= i )
      tlo = tdata(ndata);
      thi = tdata(ndata) + 0.5E+00 * ( tdata(ndata) - tdata(ndata-1) );
    end

    if ( i < ndata )
      jhi = nsample - 1;
    else
      jhi = nsample;
    end

    for j = 0 : jhi

      tval = ( ( nsample - j ) * tlo   ...
             + (           j ) * thi ) ...
             /   nsample;

      yval = basis_matrix_tmp ( left, n, mbasis, ndata, tdata, ydata, tval );

      if ( 0 < i & j == 0 )
        mark = '*';
      else
        mark = ' ';
      end

      fprintf ( 1, '%c  %14f  %14f\n', mark, tval, yval );

    end

  end

  return
end


clear;
clc;
format short;

%% 线性代数及其应用第5版 5.8 Iterative Estimates For EienValues

%% EXAMPLE 1
A = [1.8 0.8; 0.2 1.2];
v1 = [4 1]';
x = [-0.5 1]';

for i = 1:8
    x = A*x;
end

x = x / norm(x);
v1= v1 / norm(v1);
%
% fprintf("可以看到 x 逼近v1的方向\r\n");
% x
% v1

%% EXAMPLE 2
A  = [6 5; 1 2];
x = [0 1]';

for i = 1:8
    x = A*x;
    lanbda = abs(max(x));
    x  = x / lanbda;

end

x
lanbda


% Studies the recovery probability of different algorithms
% at a fixed sparsity level and varying number of signals.

close all;
clear all;
clc;
rng('default');
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');
% Signal space
N = 256;
% Number of measurements
M = 48;
% Number of signals
Ss = 1:32;
% Sparsity level
K = 16;
num_trials = 100;
num_ss = length(Ss);
success_with_s.somp = zeros(num_ss, 1);
success_with_s.ra_omp = zeros(num_ss, 1);
success_with_s.ra_ormp = zeros(num_ss, 1);
success_with_s.gomp_2_mmv = zeros(num_ss, 1);
success_with_s.gomp_4_mmv = zeros(num_ss, 1);

snr_threshold = 100;

for ns=1:num_ss
    % Current number of signals
    S = Ss(ns);
    num_successes.somp = 0;
    num_successes.ra_omp = 0;
    num_successes.ra_ormp = 0;
    num_successes.gomp_2_mmv = 0;
    num_successes.gomp_4_mmv = 0;
    for nt=1:num_trials
        % Sensing matrix
        Phi = spx.dict.simple.gaussian_dict(M, N);
        % Sparse signal generator
        gen  = spx.data.synthetic.SparseSignalGenerator(N, K, S);
        % Gaussian distributed non-zero samples
        X = gen.gaussian;
        % Measurement vectors
        Y = Phi * X;

        % Create the solver for simultaneous orthogonal matching pursuit
        solver = spx.pursuit.joint.OrthogonalMatchingPursuit(Phi, K, 2);
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.somp = num_successes.somp + success;

        % Create the solver for rank aware orthogonal matching pursuit
        solver = spx.pursuit.joint.RankAwareOMP(Phi, K);
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.ra_omp = num_successes.ra_omp + success;

        % Create the solver for rank aware order recursive matching pursuit
        solver = spx.pursuit.joint.RankAwareORMP(Phi, K);
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.ra_ormp = num_successes.ra_ormp + success;

        % Create the solver for Generalized OMP MMV with L=2
        solver = spx.pursuit.joint.GOMP(Phi, K, 2);
        solver.L = 2;
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.gomp_2_mmv = num_successes.gomp_2_mmv + success;

        % Create the solver for Generalized OMP MMV with L=4
        solver = spx.pursuit.joint.GOMP(Phi, K, 2);
        solver.L = 4;
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.gomp_4_mmv = num_successes.gomp_4_mmv + success;

        fprintf('S: %d, K=%d, trial=%d\n', S, K, nt);
    end
    success_with_s.somp(ns) = num_successes.somp / num_trials;
    success_with_s.ra_omp(ns) = num_successes.ra_omp / num_trials;
    success_with_s.ra_ormp(ns) = num_successes.ra_ormp / num_trials;
    success_with_s.gomp_2_mmv(ns) = num_successes.gomp_2_mmv / num_trials;
    success_with_s.gomp_4_mmv(ns) = num_successes.gomp_4_mmv / num_trials;
end

save ('bin/success_with_s_comparison.mat');