function [ unique_num, seed ] = point_radial_tol_unique_count ( m, n, a, ...
  tol, seed )

%*****************************************************************************80
%
%% POINT_RADIAL_TOL_UNIQUE_COUNT counts the tolerably unique points.
%
%  Discussion:
%
%    The input data is an M x N array A, representing the M-dimensional
%    coordinates of N points.
%
%    The output is the number of tolerably unique points in the list.
%
%    This program performs the same task as POINT_TOL_UNIQUE_COUNT.
%    But that program is guaranteed to use N^2 comparisons.
%
%    It is hoped that this function, on the other hand, will tend
%    to use O(N) comparisons after an O(NLog(N)) sort.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 July 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows.
%
%    Input, integer N, the number of columns.
%
%    Input, real A(M,N), the array of N columns of data.
%
%    Input, real TOL, a tolerance for equality.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, integer UNIQUE_NUM, the number of tolerably
%    unique points.
%
  if ( n <= 0 )
    unique_num = 0;
    return
  end
%
%  Assign a base point Z randomly in the convex hull.
%
  [ w, seed ] = r8vec_uniform_01 ( n, seed );

  z = zeros ( m, 1 );
  for i = 1 : m
    z(i) = a(i,1:n) * w(1:n,1) / sum ( w(1:n) );
  end
%
%  Compute the radial distance R of each point to Z.
%
  r = zeros ( n, 1 );
  for j = 1 : n
    r(j) = sqrt ( sum ( ( a(1:m,j) - z(1:m) ).^2 ) );
  end
%
%  Implicitly sort the R array.
%
  indx = r8vec_sort_heap_index_a ( n, r );
%
%  To determine if a point I is tolerably unique, we only have to check
%  whether it is distinct from all points J such that R(I) <= R(J) <= R(J)+TOL.
%
  unique_num = 0;
  unique(1:n) = 1;

  for i = 1 : n

    if ( unique(indx(i)) )
%
%  Point INDX(I) is unique, in that no earlier point is near it.
%
      unique_num = unique_num + 1;
%
%  Look for later points which are close to point INDX(I)
%  in terms of R.
%
      hi = i;

      while ( hi < n )
        if ( r(indx(i)) + tol < r(indx(hi+1)) )
          break
        end
        hi = hi + 1;
      end
%
%  Points INDX(I+1) through INDX(HI) have an R value close to
%  point INDX(I).  Are they truly close to point INDEX(I)?
%
      for j = i + 1 : hi
        if ( unique(indx(j)) )
          dist = sqrt ( sum ( ( a(1:m,indx(i)) - a(1:m,indx(j)) ).^2 ) );
          if ( dist <= tol )
            unique(indx(j)) = 0;
          end
        end
      end

    end

  end

  return
end


%
% n=erlangbinv(p,rho)
%
% This function finds the smallest n such the Erlang B probability that
% a system with n servers, no waiting line, Poisson arrival rate lambda,
% service rate (per server) mu, and intensity rho=lambda/mu will have
% a probability <=p of having all servers busy.
%
% The Erlang B probability is given by
%
%  B=(rho^m/m!)/(sum(rho^k/k!),k=0..m)
%
% We use a recurrence relation which is more accurate than direct evaluation
% of the formula.  This recurrence relation is a "folk theorem".  The author
% would appreciate a reference to its first publication.  The recurrence is
%
%  B(0,rho)=1
%
%  B(n,rho)=(rho*B(n-1,rho)/n)/(1+rho*B(n-1,rho)/n)
%
% This routine simply loops through the recursion until B is <= p, and
% then returns n.
%
function n=erlangbinv(p,rho)
%
% Start the recursion with B=1.
%
B=1;
%
% Loop, iterating the recursion until the probability is <= p.
%
n=1;
while (1 == 1),
  B=((rho*B)/n)/(1+rho*B/n);
  if (B <= p),
    return;
  end;
  n=n+1;
end;


function out = nrlremap(x, a, c)
% NRLREMAP Lin-log style remap
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

if nargin<3
    c = 220; % Output "knee" in lin-log curve
end
if nargin<2
    a = 1; % Scale factor of 99th percentile for input "knee"
end

A = abs(single(x));
Amin = min(A(:));
P99 = pctl(A(:),99);
b = (255-c)/log10((max(A(:))-Amin)/((a*P99)-Amin));

out = uint8(zeros(size(x)));
linear_region = (A<=a*P99);
out(linear_region) = (A(linear_region) - Amin)*c/((a*P99)-Amin);
out(~linear_region) = b*log10((A(~linear_region)-Amin)/((a*P99)-Amin))+c;

end

function ptile = pctl(v, p)
ptile = interp1(linspace(0.5/numel(v), 1-0.5/numel(v), numel(v))', sort(v(:)), p*0.01, 'spline');
end
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [ypred, ll, mse] = eval_AR_perf(coef, C, y, model)
% Evaluate the performance of an AR model.
%
% Inputs
% coef(:,:,k,m) - coef. matrix to use for k steps back, model m
% C(:,:,m)      - cov. matrix for model m
% y(:,t)        - observation at time t
% model(t)      - which model to use at time t (defaults to 1 if not specified)
%
% Outputs
% ypred(:,t)    - the predicted value of y at t based on the evidence thru t-1.
% ll            - log likelihood
% mse           - mean squared error = sum_t d_t . d_t, where d_t = pred(y_t) - y(t)

[s T] = size(y);
k = size(coef, 3);
M = size(coef, 4);

if nargin<4, model = ones(1, T); end

ypred = zeros(s, T);
ypred(:, 1:k) = y(:, 1:k);
mse = 0;
ll = 0;
for j=1:M
  c(j) = log(normal_coef(C(:,:,j)));
  invC(:,:,j) = inv(C(:,:,j));
end
coef = reshape(coef, [s s*k M]);

for t=k+1:T
  m = model(t-k);
  past = y(:,t-1:-1:t-k);
  ypred(:,t) = coef(:, :, m) * past(:);
  d = ypred(:,t) - y(:,t);
  mse = mse + d' * d;
  ll = ll + c(m) - 0.5*(d' * invC(:,:,m) * d);
end
mse = mse / (T-k+1);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  In this script, we perform phase transition analysis
%  of Orthogonal matching pursuit.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear all;
clc;
rng('default');
% Create the directory for storing results
[status_code,message,message_id] = mkdir('bin');
target_file_path = 'bin/ar_omp_phase_transition_gaussian_dict_gaussian_data.mat';
N = 1024;
pta = spx.pursuit.PhaseTransitionAnalysis(N);
pta.NumTrials = 200;
dict_model = @(M, N) spx.dict.simple.gaussian_dict(M, N);
data_model = @(N, K) spx.data.synthetic.SparseSignalGenerator(N, K).gaussian;
%recovery_solver = @(Phi, K, y) atom_ranked_omp(Phi, y, K).z;
pta.run(dict_model, data_model, @ar_solver);
pta.save_results(target_file_path);


function result = tetra_07 ( func, x, y, z )

%*****************************************************************************80
%
%% TETRA_07 approximates an integral inside a tetrahedron in 3D.
%
%  Integration region:
%
%    Points inside a tetrahedron whose four corners are given.
%
%  Discussion:
%
%    A 64 point 7-th degree conical product Gauss formula is used,
%    Stroud number T3:7-1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2004
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%    Stroud and Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966, pages 42-43.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function of three variables which is to be integrated,
%    of the form:
%      function value = func ( x, y, z )
%
%    Input, real X(4), Y(4), Z(4), the X, Y and Z coordinates of
%    the vertices.
%
%    Output, real RESULT, the approximate integral of the function.
%
  norder = 4;
  weight2 = [ ...
    0.1355069134, 0.2034645680, 0.1298475476, 0.0311809709 ];
  weight3 = [ ...
    0.1108884156, 0.1434587898, 0.0686338872, 0.0103522407 ];
  xtab2 = [ ...
    0.0571041961E+00, 0.2768430136E+00, 0.5835904324E+00, 0.8602401357E+00 ];
  xtab3 = [ ...
    0.0485005495E+00, 0.2386007376E+00, 0.5170472951E+00, 0.7958514179E+00 ];
%
%  Get the Gauss-Legendre weights and abscissas for [-1,1].
%
  [ xtab1, weight1 ] = legendre_set ( norder );
%
%  Adjust the rule for the interval [0,1].
%
  a = -1.0E+00;
  b = +1.0E+00;

  c =  0.0E+00;
  d =  1.0E+00;

  [ xtab1, weight1 ] = rule_adjust ( a, b, c, d, norder, xtab1, weight1 );
%
%  Carry out the quadrature.
%
  quad = 0.0;

  for i = 1 : norder
    for j = 1 : norder
      for k = 1 : norder
%
%  Compute the barycentric coordinates of the point in the unit triangle.
%
        t =                                                 xtab3(k);
        u =                        xtab2(j)   * ( 1.0E+00 - xtab3(k) );
        v = xtab1(i) * ( 1.0E+00 - xtab2(j) ) * ( 1.0E+00 - xtab3(k) );
        w = 1.0E+00 - t - u - v;
%
%  Compute the corresponding point in the triangle.
%
        xval = t * x(1) + u * x(2) + v * x(3) + w * x(4);
        yval = t * y(1) + u * y(2) + v * y(3) + w * y(4);
        zval = t * z(1) + u * z(2) + v * z(3) + w * z(4);
        quad = quad + 6.0 * weight1(i) * weight2(j) * weight3(k) ...
          * feval ( func, xval, yval, zval );

      end
    end
  end

  volume = tetra_volume ( x, y, z );
  result = quad * volume;

  return
end


function [x,t]=meanLinTrajConstEnds(x0,x2,deltaT,q0,order,N,FFun,QFun)
%%MEANLINTRAJCONSTENDS Generate a the mean trajectory based on a linear
%               dynamic model with additive noise, as is typically used in
%               tracking systems, given known locations of the start and
%               end points. A polynomial dynamic model is used by default,
%               but one can specify other models.
%
%INPUTS: x0, x2 The xDimX1 target states at the beginning and end of the
%               region over which paths are to be simulated. These are
%               treated as known values.
%        deltaT The time between when state x0 occurred and when state x2
%               occurred. This is a positive value.
%            q0 The power spectral density of the process noise. This is
%               passed to QFun, as described below and by default would
%               corresponds to the same-named input in QPolyKal. With the
%               default polynomial dynamic model, FPolyKal and QPolyKal, q0
%               has no effect on the result and is just set to 1 if an
%               empty matrix is passed.
%         order The order >=0 of the filter. If order=1, then it is
%               constant velocity, 2 means constant acceleration, 3 means
%               constant jerk, etc. This is passed to FFun and QFun. If
%               custom FFun and QFun functions are used and this is not
%               needed, then an empty matrix can be passed. The default if
%               omitted or an empty matrix is passed is 1.
%             N The number of points in the random path to generate between
%               x0 and x2. The default if omitted or an empty matrix is
%               passed is 100; N>=0.
%    FFun, QFun Optional function handles to functions that respectively
%               provide the state transition matrix and the process noise
%               covariance matrix as a function of the time duration over
%               which the prediction is taken. The defaults if omitted or
%               empty matrices are passed are handles to FPolyKal and
%               QPolyKal. The functions are called as FFun(T,xDim,order)
%               and QFun(T,xDim,order,q0), where T is the duration over
%               which the prediction is taken.
%
%OUTPUTS: x The xDimX(N+2) set of mean trajectory according to the
%           specified model. x(:,1) is always x0 and x(:,N+2) is always x2.
%         t The (N+2)X1 set of time offsets (starting at 0) where the
%           values in x are generated.
%
%Consider a process at points 0, 1, and 2, where we are given the value at
%time x0. The transition models are:
%x1=F0*x0+w1
%x2=F1*x1+w2
%Where x0 is given (deterministic) and w1 and w2 are independent zero-mean
%Gaussian random variables with covariance matrices Q1 and Q1.
%x1 and x2 are jointly normal with mean mu and covariance matrix Sigma:
%mu=[x1Pred; = [F0*x0
%    x2Pred]    F1*F0*x0];
%Sigma=[   Q1,        Q1*F1'; =[Pxx, Pxz;
%       F1*Q1,  F1*Q1*F1'+Q2]   Pxz',Pzz];
%When given the value of x2, the conditional distribution of x1|x0,x2 is
%the same as the Kalman filter update taking the measurement covariance
%matrix to be Q2, the measurement matrix H to be F1, the prior state to be
%F0*x0, and the covariance of the prior state to be Q1. Thus, as derived,
%for example, in [Ch. 5,1], [2], the posterior distribution of x1 is
%Gaussian with mean and covariance matrix
%x1Bar=x1Pred+Pxz/Pzz*(x2-x2Pred)
%This function is similar to randLinTrajConstEnds.
%
%EXAMPLE:
%This plots a trajectory that eventually has to turn around, since the
%endpoint is not close to the direction of the initial velocity vector.
% x0=[1e3;0;30;20];
% x2=[709;
%     527;
%     -39;
%       4];
% deltaT=100;
% x=meanLinTrajConstEnds(x0,x2,deltaT);
% figure(1)
% clf
% plot(x(1,:),x(2,:))
% h1=xlabel('x');
% h2=ylabel('y');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<6||isempty(N))
    N=100;
end

if(nargin<5||isempty(order))
    order=1;
end

if(nargin<4||isempty(q0))
    q0=1;
end

xDim=size(x0,1);

if(nargin<8||isempty(QFun))
    Q=@(T)QPolyKal(T,xDim,order,q0);
else
    Q=@(T)QFun(T,xDim,order,q);
end

if(nargin<7||isempty(FFun))
    F=@(T)FPolyKal(T,xDim,order);
else
    F=@(T)FFun(T,xDim,order);
end

x=zeros(xDim,N+2);

%The increment between the sample points.
deltaTInc=deltaT/(N+1);

x(:,1,:)=x0;
x(:,N+2,:)=x2;

F0=F(deltaTInc);
Q1=Q(deltaTInc);
for curPoint=1:N
    deltaT0=deltaTInc*(curPoint-1);
    deltaT2=deltaT-deltaT0-deltaTInc;

    F1=F(deltaT2);
    Q2=Q(deltaT2);

    Pxz=Q1*F1';
    Pzz=F1*Q1*F1'+Q2;
    x1Pred=F0*x(:,curPoint);
    x2Pred=F1*F0*x(:,curPoint);
    x1Mean=x1Pred+Pxz*inv(Pzz)*(x2-x2Pred);

    x(:,curPoint+1)=x1Mean;
end

if(nargout>1)
    t=[0;deltaTInc*(1:(N+1)).'];
    t(end)=deltaT;%Make it exact despite finite precision limitations.
end
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.


function [soln,eqn,info] = Poisson(node,elem,bdFlag,pde,option)
%% POISSON Poisson equation: P1 linear element.
%
%   u = Poisson(node,elem,bdFlag,pde) produces the linear finite element
%   approximation of the Poisson equation
%
%       -div(d*grad(u))=f  in \Omega, with
%       Dirichlet boundary condition u=g_D on \Gamma_D,
%       Neumann boundary condition   d*grad(u)*n=g_N on \Gamma_N,
%       Robin boundary condition     g_R*u + d*grad(u)*n=g_N on \Gamma _R
%
%   The mesh is given by node and elem and the boundary edge is by
%   bdFlag. See meshdoc, bddoc for details. The data is given by the
%   structure pde which contains function handles f, g_D, g_N, g_R, or d.
%   For general elliptic equations with convection and reaction
%   coefficients, see ellipticpde.
%
%   soln = Poisson(node,elem,bdFlag,pde,option) specifies the options.
%
%   In the output, soln structure contains
%     - soln.u: solution u
%     - soln.Du: gradient of u
%
%   In the input, option structures contains
%    - option.dquadorder: quadrature order for diffusion coefficients
%    - option.fquadorder: quadrature order for computing right hand side f
%    - option.solver
%      'direct': the built in direct solver \ (mldivide)
%      'mg':     multigrid-type solvers mg is used.
%      'amg':    algebraic multigrid method is used.
%      'none':   only assemble the matrix equation but not solve.
%   The default setting is to use the direct solver for small size problems
%   and multigrid solvers for large size problems. For more options on the
%   multigrid solver mg, type help mg.
%
%   The function Poisson assembes the matrix equation AD*u = b and solves
%   it by the direct solver (small size <= 2e3) or the multigrid solver
%   (large size > 2e3). The Dirichlet boundary condition is built into the
%   matrix AD and the Neumann boundary condition is build into b.
%
%   The diffusion coefficient d is a scalar function or a column array with
%   the same length as the elem array.
%
%   When only one type of boundary condition is imposed, the input argument
%   bdFlag can be skipped. The boundary condition is implicitly given in
%   the pde structure by specifying g_D or g_N only. See examples below.
%
%   [soln,eqn] = Poisson(node,elem,bdFlag,pde) returns also the equation
%   structure eqn, which includes:
%     - eqn.AD:  modified stiffness matrix AD;
%     - eqn.b:   the right hand side.
%     - eqn.Lap: non-modified stiffness matrix
%
%   The solution u = AD\b. The matrix eqn.Lap can be used to evulate the
%   bilinear form a(u,v) = u'*eqn.Lap*v, especially the enery norm of a finite
%   element function u is given by by sqrt(u'*eqn.Lap*u).
%
%   [soln,eqn,info] = Poisson(node,elem,bdFlag,pde) returns also the
%   information on the assembeling and solver, which includes:
%     - info.assembleTime: time to assemble the matrix equation
%     - info.solverTime:   time to solve the matrix equation
%     - info.itStep:       number of iteration steps for the mg solver
%     - info.error:        l2 norm of the residual b - A*u
%     - info.flag:         flag for the mg solver.
%       flag = 0: converge within max iteration
%       flag = 1: iterated maxIt times but did not converge
%       flag = 2: direct solver
%       flag = 3: no solve
%
%   Example
%     squarePoisson; Poissonfemrate;
%
%   Example
%     clear all
%     node = [0,0; 1,0; 1,1; 0,1];
%     elem = [2,3,1; 4,1,3];
%     for k = 1:4
%       [node,elem] = uniformrefine(node,elem);
%     end
%     % Homogenous Dirichlet boundary condition
%     pde.f = inline('ones(size(p,1),1)','p');
%     pde.g_D = 0;
%     u = Poisson(node,elem,[],pde);
%     figure(1);
%     showresult(node,elem,u);
%
%   See also Poisson3, femPoisson, squarePoisson, Poissonfemrate
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Preprocess
if ~exist('bdFlag','var'), bdFlag = []; end
if ~exist('option','var'), option = []; end
% important constants
N = size(node,1);
NT = size(elem,1);
Ndof = N;

%% Diffusion coefficient
time = cputime;  % record assembling time
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder'), option.dquadorder = 1; end
if ~isempty(pde.d) && isnumeric(pde.d)
   K = pde.d;                                 % d is an array
end
if ~isempty(pde.d) && ~isnumeric(pde.d)       % d is a function
    [lambda,weight] = quadpts(option.dquadorder);
    nQuad = size(lambda,1);
    K = zeros(NT,1);
    for p = 1:nQuad
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:);
        K = K + weight(p)*pde.d(pxy);
   end
end

%% Compute geometric quantities and gradient of local basis
[Dphi,area] = gradbasis(node,elem);

%% Assemble stiffness matrix
A = sparse(Ndof,Ndof);
for i = 1:3
    for j = i:3
        % $A_{ij}|_{\tau} = \int_{\tau}K\nabla \phi_i\cdot \nabla \phi_j dxdy$
        Aij = (Dphi(:,1,i).*Dphi(:,1,j) + Dphi(:,2,i).*Dphi(:,2,j)).*area;
        if ~isempty(pde.d)
            Aij = K.*Aij;
        end
        if (j==i)
            A = A + sparse(elem(:,i),elem(:,j),Aij,Ndof,Ndof);
        else
            A = A + sparse([elem(:,i);elem(:,j)],[elem(:,j);elem(:,i)],...
                           [Aij; Aij],Ndof,Ndof);
        end
    end
end
clear K Aij

%% Assemble the right hand side
b = zeros(Ndof,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if isreal(pde.f) % f is a real number or vector and not a function
   switch length(pde.f)
       case NT  % f is piecewise constant
         bt = pde.f.*area/3;
         b = accumarray(elem(:),[bt; bt; bt],[Ndof 1]);
       case N   % f is piecewise linear
         bt = zeros(NT,3);
         bt(:,1) = area.*(2*pde.f(elem(:,1)) + pde.f(elem(:,2)) + pde.f(elem(:,3)))/12;
         bt(:,2) = area.*(2*pde.f(elem(:,2)) + pde.f(elem(:,3)) + pde.f(elem(:,1)))/12;
         bt(:,3) = area.*(2*pde.f(elem(:,3)) + pde.f(elem(:,1)) + pde.f(elem(:,2)))/12;
         b = accumarray(elem(:),bt(:),[Ndof 1]);
       case 1   % f is a scalar e.g. f = 1
         bt = pde.f*area/3;
         b = accumarray(elem(:),[bt; bt; bt],[Ndof 1]);
   end
end
if ~isempty(pde.f) && ~isreal(pde.f)  % f is a function
    [lambda,weight] = quadpts(option.fquadorder);
    phi = lambda;                 % linear bases
	nQuad = size(lambda,1);
    bt = zeros(NT,3);
    for p = 1:nQuad
		% quadrature points in the x-y coordinate
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:);
		fp = pde.f(pxy);
        for i = 1:3
            bt(:,i) = bt(:,i) + weight(p)*phi(p,i)*fp;
        end
    end
    bt = bt.*repmat(area,1,3);
    b = accumarray(elem(:),bt(:),[Ndof 1]);
end
clear pxy bt

%% Set up boundary conditions
[AD,b,u,freeNode,isPureNeumann] = getbd(b);

%% Record assembling time
assembleTime = cputime - time;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(freeNode), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')  || isfield(option,'mgoption')   % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
if isPureNeumann
    option.solver = 'mg';
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        t = cputime;
        u(freeNode) = AD(freeNode,freeNode)\b(freeNode);
        residual = norm(b - AD*u);
        info = struct('solverTime',cputime - t,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
        if ~isfield(option,'mgoption')   % no option.mgoption
            option.mgoption.x0 = u;
            option.mgoption.solver = 'CG';
        end
        [u,info] = mg(AD,b,elem,option.mgoption);
    case 'amg'
        if ~isfield(option,'amgoption')  % no option.amgoption
            option.amgoption.x0 = u;
            option.amgoption.solver = 'CG';
        end
        [u(freeNode),info] = amg(AD(freeNode,freeNode),b(freeNode),option.amgoption);
end
% post-process for pure Neumann problem
if isPureNeumann
    patchArea = accumarray(elem(:),[area;area;area]/3, [N 1]);
    uc = sum(u.*patchArea)/sum(area);
    u = u - uc;   % int u = 0
end

%% Compute Du
dudx =  u(elem(:,1)).*Dphi(:,1,1) + u(elem(:,2)).*Dphi(:,1,2) ...
      + u(elem(:,3)).*Dphi(:,1,3);
dudy =  u(elem(:,1)).*Dphi(:,2,1) + u(elem(:,2)).*Dphi(:,2,2) ...
      + u(elem(:,3)).*Dphi(:,2,3);
Du = [dudx, dudy];

%% Output
if nargout == 1
    soln = u;
else
    soln = struct('u',u,'Du',Du);
    eqn = struct('A',AD,'b',b,'freeNode',freeNode,'Lap',A);
    info.assembleTime = assembleTime;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbd
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,b,u,freeNode,isPureNeumann] = getbd(b)
    %% Set up of boundary conditions.
    %
    % 1) Modify the matrix for Dirichlet boundary nodes, which are not degree
    % of freedom. Values at these nodes are evaluatation of pde.g_D. The
    % original stiffness matrix A is turn into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    %
    % 2) Modify the right hand side b. The Neumann boundary integral is added
    % to b. For Dirichlet boundary ndoes, b(fixedNode) is the evaluation of
    % pde.g_D.
    %
    % Special attentation should be given for the pure Neumann boundary
    % condition. To enforce the compatible condition, the vector b should have
    % mean value zero. To avoid a singular matrix, the 1st node is chosen as
    % fixedNode.
    %
    % The order of assigning Neumann and Dirichlet boundary condition is
    % important to get the right setting at the intersection nodes of Dirichlet
    % and Neumann boundary edges.
    %
    % Reference: Long Chen. Finite Element Methods and its Programming. Lecture
    % Notes.

    u = zeros(Ndof,1);
    %% Initial check
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Modify the matrix for Dirichlet and Robin condition
    % Robin boundary condition
    Robin = [];
    isRobin = (bdFlag(:) == 3);
    if any(isRobin)
        allEdge = [elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])];
        Robin = allEdge(isRobin,:);
    end
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        ve = node(Robin(:,1),:) - node(Robin(:,2),:);
        edgeLength = sqrt(sum(ve.^2,2));
        mid = (node(Robin(:,1),:) + node(Robin(:,2),:))/2;
        % use Simplson rule to compute int g_R phi_iphi_j ds
        ii = [Robin(:,1),Robin(:,1),Robin(:,2),Robin(:,2)];
        jj = [Robin(:,1),Robin(:,2),Robin(:,1),Robin(:,2)];
        temp = pde.g_R(mid).*edgeLength;
        ss = [1/3*temp, 1/6*temp, 1/6*temp, 1/3*temp];
        A = A + sparse(ii,jj,ss,Ndof,Ndof);
    end

    % Find Dirichlet boundary nodes: fixedNode
    fixedNode = []; freeNode = [];
    if ~isempty(bdFlag) % find boundary edges and boundary nodes
        [fixedNode,bdEdge,isBdNode] = findboundary(elem,bdFlag);
        freeNode = ~isBdNode;
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        % no bdFlag, only pde.g_D is given
        [fixedNode,bdEdge,isBdNode] = findboundary(elem);
        freeNode = ~isBdNode;
    end

    % Modify the matrix for different boundary conditions
    % Dirichlet boundary condition
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    if ~isempty(fixedNode)
        bdidx = zeros(Ndof,1);
        bdidx(fixedNode) = 1;
        Tbd = spdiags(bdidx,0,Ndof,Ndof);
        T = spdiags(1-bdidx,0,Ndof,Ndof);
        AD = T*A*T + Tbd;
    end
    % Neumann boundary condition
    isPureNeumann = false;
    if isempty(fixedNode) && isempty(Robin) % pure Neumann boundary condition
        isPureNeumann = true;
        AD = A;
        AD(1,1) = AD(1,1) + 1e-6;
%         fixedNode = 1;
%         freeNode = 2:Ndof;    % eliminate the kernel by enforcing u(1) = 0;
    end
    % Robin boundary condition
    if isempty(fixedNode) && ~isempty(Robin)
        AD = A;
    end

    %% Part 2: Find boundary edges and modify the right hand side b
    % Find boundary edges: Neumann
    Neumann = [];
    if ~isempty(bdFlag)  % bdFlag specifies different bd conditions
        Neumann = bdEdge;
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        [tempvar,Neumann] = findboundary(elem); %#ok<ASGLU>
    end

    % Neumann boundary condition
    if  isnumeric(pde.g_N) && all(pde.g_N == 0)
        pde.g_N = [];
    end
    if ~isempty(Neumann) && ~isempty(pde.g_N)
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 2;   % default order exact for linear gN
        end
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        phigN = lambdagN;                 % linear bases
        nQuadgN = size(lambdagN,1);
        ge = zeros(size(Neumann,1),2);
        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNp = pde.g_N(ppxy);
            for igN = 1:2
                ge(:,igN) = ge(:,igN) + weightgN(pp)*phigN(pp,igN)*gNp;
            end
        end
        ge = ge.*repmat(el,1,2);
        b = b + accumarray(Neumann(:), ge(:),[Ndof,1]);
    end
    % The case with non-empty Neumann edges but g_N=0 or g_N=[] corresponds to
    % the zero flux boundary condition on Neumann edges and no modification of
    % A,u,b is needed.

    % Dirichlet boundary condition
    if isnumeric(pde.g_D) && all(pde.g_D == 0)   % zero g_D
        pde.g_D = [];
    end
    if ~isPureNeumann && ~isempty(fixedNode) && ~isempty(pde.g_D)
        if isnumeric(pde.g_D)  % pde.g_D could be a numerical array
            u(fixedNode) = pde.g_D(fixedNode);
        else % pde.g_D is a function handle
            u(fixedNode) = pde.g_D(node(fixedNode,:));
        end
        b = b - A*u;
    end
    if ~isempty(fixedNode) % non-empty Dirichlet boundary condition
        b(fixedNode) = u(fixedNode);
    end
    % The case with non-empty Dirichlet nodes but g_D=0 or g_D=[] corresponds
    % to the zero Dirichlet boundary condition and no modification of u,b is
    % needed.

    % Pure Neumann boundary condition
    if isPureNeumann
        b = b - mean(b); % compatiable condition (f,1) + <gN,1> = 0
%         b(1) = 0;
    end
    end % end of getbd
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end % end of Poisson


function [ n_data, x, fx ] = arctan_values ( n_data )

%*****************************************************************************80
%
%% ARCTAN_VALUES returns some values of the arc tangent function.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      ArcTan[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 11;

  fx_vec = [ ...
    0.00000000000000000000, ...
    0.24497866312686415417, ...
    0.32175055439664219340, ...
    0.46364760900080611621, ...
    0.78539816339744830962, ...
    1.1071487177940905030, ...
    1.2490457723982544258, ...
    1.3258176636680324651, ...
    1.3734007669450158609, ...
    1.4711276743037345919, ...
    1.5208379310729538578 ];

  x_vec = [ ...
    0.00000000000000000000, ...
    0.25000000000000000000, ...
    0.33333333333333333333, ...
    0.50000000000000000000, ...
    1.0000000000000000000, ...
    2.0000000000000000000, ...
    3.0000000000000000000, ...
    4.0000000000000000000, ...
    5.0000000000000000000, ...
    10.000000000000000000, ...
    20.000000000000000000 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function [ fea, out ] = ex_convreact1( varargin )
%EX_CONVREACT1 1D Time dependent convection-reaction equation example.
%
%   [ FEA, OUT ] = EX_CONVREACT1( VARARGIN ) 1D time dependent convection equation with reaction
%   on a line with exact solutions. Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       k           scalar {1e-1}          Reaction coefficient
%       a           scalar {1}             Convection velocity
%       hmax        scalar {1/25}          Max grid cell size
%       dt          scalar {0.1}           Time step size
%       ischeme     scalar {3}             Time stepping scheme
%       sfun        string {sflag1}        Shape function
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'k',       -1; ...
  'a',        1; ...
  'hmax',     1/25; ...
  'dt'        0.1; ...
  'ischeme'   3; ...
  'sfun',     'sflag1'; ...
  'iplot',    1; ...
  'tol',      1e-2; ...
  'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

tmax   = 1;
refsol = ['exp(',num2str(opt.k),'*t)*sin(x-',num2str(opt.a),'*t)'];

% Grid generation.
fea.grid = linegrid( 1/opt.hmax, 0, 1 );

% Problem definition.
fea.sdim  = { 'x' };
fea = addphys( fea, @convectiondiffusion );
fea.phys.cd.sfun = { opt.sfun };
fea.phys.cd.eqn.coef{2,4} = { 0 };
fea.phys.cd.eqn.coef{3,4} = { opt.a };
fea.phys.cd.eqn.coef{4,4} = { [num2str(opt.k),'*c'] };
fea = parsephys(fea);

% Parse and solve problem.
x = fea.grid.p';
n = length(x);
if( strcmp( opt.sfun,'sflag2' ) )
  x = [ x; (x(2:end)+x(1:end-1))/2 ];
end
t   = 0;
u0  = eval( refsol );
fea = parseprob( fea );
if( opt.ischeme>0 )

  fea.bdr.d{1} = refsol;
  fea.bdr.d{2} = refsol;
  fea.bdr.n    = cell(1,2);
  [fea.sol.u,tlist] = solvetime( fea, 'fid', fid, 'init', u0, 'ischeme', opt.ischeme, 'tstep', opt.dt, 'tmax', tmax, 'nstbwe', 0, 'tstop', 0, 'imass', 2 );

else

  fea.sol.u = u0;
  [M,A] = assembleprob( fea, 'f_m', 1, 'imass', 2, 'f_a', 1, 'f_sparse', 1 );
  M = spdiags( full(sum(M')'), 0, size(M,1), size(M,1) );
  dt = opt.dt;
  it = 0;
  tlist = 0;
  if( opt.ischeme==-1 )       % Crank-Nicolson.
    C = [ M + dt/2*A ];
  elseif( opt.ischeme==-2 )   % Backward Euler.
    C = [ M + dt*A ];
  end
  C(1,:) = 0;
  C(1,1) = 1;
  C(n,:) = 0;
  C(n,n) = 1;
  fea_it = fea;
  while 1
    t  = t  + dt;
    it = it + 1;

    fea_it.sol.u = u0;
    [~,~,f0] = assembleprob( fea_it, 'f_f', 1 );

    u_r = eval( refsol );
    dnm = inf;
    while( dnm>1e-6 )
      [~,~,f1] = assembleprob( fea_it, 'f_f', 1 );

      if( opt.ischeme==-1 )       % Crank-Nicolson.
        b = [ [ M - dt/2*A ]*u0 + dt/2*f1 + dt/2*f0 ];
      elseif( opt.ischeme==-2 )   % Backward Euler.
        b = [ M*u0 + dt*f1 ];
      end
      b(1) = u_r(1);
      b(n) = u_r(n);

      dnm = fea_it.sol.u;
      u1  = C\b;
      dnm = norm( dnm - u1 );
      fea_it.sol.u = u1;

    end

    if( t>=tmax )
      break
    end

    err = norm( u1 - u_r )/norm( u_r );
    errnm(it) = err;
    if( ~isempty(fid) )
      fprintf( fid, 'Time = %f, error norm = %d\n', t, err );
    end

    fea.sol.u = [ fea.sol.u u1 ];
    tlist = [ tlist t ];
    u0 = u1;
  end

end

% Postprocessing.
if( opt.iplot>0 )
  figure;
  if( opt.iplot>1 )
    i_sol_list = 1:numel(tlist);
  else
    i_sol_list = numel(tlist);
  end
  [~,ix] = sort( x );
  for i_sol=i_sol_list
    t = tlist(i_sol);
    clf
    postplot( fea, 'surfexpr', 'c', 'solnum', i_sol );
    hold on
    u_r = eval( refsol );
    plot( sort(x), u_r(ix), 'r--' );
    title( ['Solution at time ',num2str(t)])
    xlabel( 'x' )
  end
end

% Error checking.
for i_sol=1:numel(tlist)
  u_i = fea.sol.u(:,i_sol);
  t   = tlist(i_sol);
  u_r = eval( refsol );
  errnm(i_sol) = norm( u_i - u_r )/norm( u_r );
end
out.err  = errnm;
out.pass = all( errnm<opt.tol );

if ( nargout==0 )
  clear fea out
end


function gray_3x3 = image_denoise_gray_3x3 ( gray )

%*****************************************************************************80
%
%% IMAGE_DENOISE_GRAY_3X3 filters out noise from a gray scale image.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, uint8 GRAY(:,:), the noisy grayscale data.
%
%    Output, uint8 GRAY_3X3(:,:), the grayscale data for the filtered image.
%
  [ m, n ] = size ( gray );
  gray3d = zeros ( 9, m, n );
%
%  For pixel (I,J), GRAY3D(*,I,J) contains the current pixel value
%  and the 8 neighboring values.
%
  gray3d(1,:,:) = circshift ( gray, [ -1, -1 ] );
  gray3d(2,:,:) = circshift ( gray, [ -1,  0 ] );
  gray3d(3,:,:) = circshift ( gray, [ -1,  1 ] );
  gray3d(4,:,:) = circshift ( gray, [  0, -1 ] );
  gray3d(5,:,:) = circshift ( gray, [  0,  0 ] );
  gray3d(6,:,:) = circshift ( gray, [  0,  1 ] );
  gray3d(7,:,:) = circshift ( gray, [  1, -1 ] );
  gray3d(8,:,:) = circshift ( gray, [  1,  0 ] );
  gray3d(9,:,:) = circshift ( gray, [  1,  1 ] );
%
%  By taking the median of the 9 values, we hope to drop any noisy pixels.
%
  gray3d = median ( gray3d );
%
%  The MEDIAN command returned a 3D array with a first dimension of 1.
%  For convenience, we suppress the first dimension.
%
  gray_3x3(:,:) = gray3d(1,:,:);
%
%  GRAY is a UINT8 array.  The MEDIAN operation returned a DOUBLE value.
%  So we have to convert the array back to UINT8.
%
  gray_3x3 = uint8 ( gray_3x3 );

  return
end


% MAIN Illustrates how to use the EPnP algorithm described in:
%
%       Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua.
%       Accurate Non-Iterative O(n) Solution to the PnP Problem.
%       In Proceedings of ICCV, 2007.
%
% Copyright (C) <2007>  <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
%
% 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/>.
%
% Francesc Moreno-Noguer, CVLab-EPFL, September 2007.
% fmorenoguer@gmail.com, http://cvlab.epfl.ch/~fmoreno/

clear all; close all;

addpath data;
addpath error;
addpath EPnP;

fprintf('\n---------EPnP--------------\n');
%1.-Generate simulated input data------------------------------------------
load_points=0;
if ~load_points
    n=50; %number of points
    std_noise=10; %noise in the measurements (in pixels)
    [A,point,Rt]=generate_noisy_input_data(n,std_noise);
    save('data\input_data_noise.mat','A','point','Rt');
else
    load('data\input_data_noise.mat','A','point','Rt');
    n=size(point,2);
    draw_noisy_input_data(point);
end

%2.-Inputs format--------------------------------
x3d=zeros(n,4);
x2d=zeros(n,3);
A=A(:,1:3);
for i=1:n
    x3d_h(i,:)=[point(i).Xworld',1];
    x2d_h(i,:)=[point(i).Ximg(1:2)',1];

    %world and camera coordinates
    X3d_world(i,:)=point(i).Xworld';
    X3d_cam(i,:)=point(i).Xcam';
end

%3.-EPnP----------------------------------------------------
Xw=x3d_h(:,1:3);
U=x2d_h(:,1:2);

[Rp,Tp,Xc,sol]=efficient_pnp(x3d_h,x2d_h,A);

%draw Results
for i=1:n
    point(i).Xcam_est=Xc(i,:)';
end
figure; h=gcf;
plot_3d_reconstruction(point,'EPnP (Old)',h);
xlim([-2 2]); ylim([-2 2]);

%compute error
error=reprojection_error_usingRT(Xw,U,Rp,Tp,A);
fprintf('error EPnP: %.3f\n',error);

%3.-EPnP_GAUSS_NEWTON----------------------------------------------------
Xw=x3d_h(:,1:3);
U=x2d_h(:,1:2);

[Rp,Tp,Xc,sol]=efficient_pnp_gauss(x3d_h,x2d_h,A);

%draw Results
for i=1:n
    point(i).Xcam_est=Xc(i,:)';
end
figure; h=gcf;
plot_3d_reconstruction(point,'EPnP Gauss Newton',h);

%compute error
error=reprojection_error_usingRT(Xw,U,Rp,Tp,A);
fprintf('error EPnP_Gauss_Newton: %.3f\n',error);
xlim([-2 2]); ylim([-2 2]);


function varargout = sigmoid(varargin)
% VL_SIGMOID Sigmoid function
%   Y = VL_SIGMOID(X) returns
%
%    Y = 1 ./ (1 + EXP(X)) ;
%
%   Remark::
%     Useful properties of the sigmoid function are:
%
%     -  1 - VL_SIGMOID(X) = VL_SIGMOID(-X)
%     -  Centered sigmoid: 2 * VL_SIGMOID(X) - 1 ;
%     -  VL_SIGMOID(X) = (EXP(X/2) - EXP(X/2)) / (EXP(X/2) + EXP(X/2))
%
%   See also: VL_DSIGMOID(), VL_HELP().
[varargout{1:nargout}] = vl_sigmoid(varargin{:});


% Plot nodes radially out from a given center. Equidistant nodes
% have the same radius, but different angles. Works best as a quick
% visualization for trees, or very sparse graphs.
%
% Note 1: If a center node is not specified, the nodes are ordered by
%   sum of neighbor degrees, and the node with highest sum is plotted
%   in the center.
% Note 2: The graph has to be connected.
%
% Inputs: adjacency matrix (nxn), and center node (optional)
% Outputs: plot
%
% Other routines used: sortNodesBySumNeighborDegrees.m,
%                      adj2adjL.m, diameter.m, kminNeighbors.m
% GB: last updated, December 6 2012

function [] = radialPlot(adj,i0)

d=diameter(adj);
if d==Inf; printf('The graph is not connected.\n'); return; end

if nargin==1; I=sortNodesBySumNeighborDegrees(adj); i0=I(1); end;

L=adj2adjL(adj);

circles={};   % per circle
circle{i0}=0; % per node

for k=1:d
  kneigh = kminNeighbors(adj,i0,k);
  if not(isempty(kneigh));
    circles{k}=kneigh;
    for nei=1:length(kneigh); circle{kneigh(nei)}=k; end
  end;
end

for i=1:size(adj,1); descendants{i} = 0; end;

for k=length(circles)-1:-1:1

  for i=1:length(circles{k})
    node = circles{k}(i);
    neighs=L{node};

    for nei=1:length(L{node})
      nei = L{node}(nei);
      if sum(find(circles{k+1}==nei))>0; descendants{node}=descendants{node}+descendants{nei}+1; end
    end
  end

end

descendants{i0}=descendants{i0} + length(L{i0});
for i=1:length(L{i0}); descendants{i0}=descendants{i0} + descendants{L{i0}(i)}; end;

start_angle{i0}=0;
range{i0}=2*pi;

queue=[i0]; visited=[];

while not(isempty(queue))

  node=queue(1); queue=queue(2:length(queue)); visited=[visited node];

  % select neighbours that have not been visited
  neigh=[];
  for ll=1:length(L{node})
    if sum(find(visited==L{node}(ll)))==0 && sum(find(queue==L{node}(ll)))==0;
      neigh=[neigh L{node}(ll)]; end;
  end

  if isempty(neigh); continue; end;

  range{neigh(1)} = range{node}*(descendants{neigh(1)}+1)/descendants{node};
  start_angle{neigh(1)}=start_angle{node};
  if sum(find(queue==neigh(1)))==0; queue=[queue neigh(1)]; end;

  for j=2:length(neigh)
    range{neigh(j)} = range{node}*(descendants{neigh(j)}+1)/descendants{node};
    start_angle{neigh(j)} = start_angle{neigh(j-1)} + range{neigh(j-1)};
    if sum(find(queue==neigh(j)))==0; queue=[queue neigh(j)]; end;
  end

end

set(gcf,'Color',[1 1 1],'Colormap',hot);   % Modify color scheme here
map=colormap('hot');

% plot with the angle and radius (circle) information
for i=1:size(adj,1)
  x = circle{i}*cos(start_angle{i}+range{i}/2);
  y = circle{i}*sin(start_angle{i}+range{i}/2);
  plot(x,y,'.','Color',map(mod(5*circle{i},length(map))+1,:)); hold off; hold on;
end

xs = cos([0:1:30]*2*pi/30);
ys = sin([0:1:30]*2*pi/30);
for c=1:length(circles)
  plot(c*xs,c*ys,':','Color',[244/255,244/255,244/255]);
  hold off; hold on;
end

% add the links between nodes
for i=1:size(adj,1)
  for j=i+1:size(adj,1)
    if adj(i,j)>0
      xi=circle{i}*cos(start_angle{i}+range{i}/2);
      yi=circle{i}*sin(start_angle{i}+range{i}/2);

      xj=circle{j}*cos(start_angle{j}+range{j}/2);
      yj=circle{j}*sin(start_angle{j}+range{j}/2);

      line([xi xj],[yi yj],'Color',map(mod(5*circle{i},length(map))+1,:))
    end
  end
end

axis tight
axis off


function y = poly_env( p, x )

%POLY_ENV Evaluate the convex or concave envelope of a polynomial.
%   POLY_ENV( P, X ) uses a semidefinite program to compute the value of the
%   convex or concave envelope of the polynomial represented by the vector
%   P. The format of the vector P is identical to that required by POLYVAL,
%   with two additional restrictions. First, the elements of P must be real
%   and finite. Second, the length of P must 0, 2, or odd (1,3,5,...)
%
%   If the polynomial described by P is convex or concave, then a call to
%   POLY_ENV( P, X ) produces the same result as POLYVAL( P, X ).
%
%   POLY_ENV looks at the first nonzero element of P (presumably, but not
%   necessarily, P(1)) to determine if a convex or concave envelope is to
%   be selected. If P(1)>0, then a convex envelope is produced---i.e., the
%   function that is the tightest convex lower bound for POLYVAL(P,X).
%   Otherwise, a concave envelope is produced---the function that is the
%   tightest concave upper bound for POLYVAL(P,X).
%
%   If the degree N of P is odd---except for the special case N==1---no
%   proper convex/concave envelope exists. Therefore, POLY_ENV returns an
%   error in such cases.
%
%   If X is an array, the result will be computed elementwise.
%
%   Disciplined convex programming information:
%       POLY_ENV(P,X) is convex or concave and nonmotonic in X; therefore,
%       in CVX expressions, X must be affine, unless the polynomial
%       described by P has a degree of 0 or 1. P must be constant.

%
% Check the polynomial
%

sp = size( p );
if isempty( p ),
    p = zeros( 1, 0 );
elseif ~isa( p, 'double' ) || ~isreal( p ) || length( sp ) > 2 || ~any( sp == 1 ),
    cvx_throw( 'First argument must be a non-empty real vector.' );
elseif any( isnan( p ) | isinf( p ) ),
    cvx_throw( 'First argument must not contain Inf or NaN.' );
end
n = prod( sp );
if n > 2 && rem( n, 2 ) == 0,
    cvx_throw( 'The length of the vector p must be odd.' );
end

%
% Check the second argument
%

if n > 1 && ( ~cvx_isaffine( x ) || ~isreal( x ) ),
    cvx_throw( 'The second argument must be real and affine.' );
end

%
% Handle the special cases
%

sx = size( x );
ndxs = find( p );
if isempty( ndxs ),
    y = zeros( sx );
    return
else
    for k = ndxs(:)',
        pt = p( k : end );
        if ~any( isinf( pt ./ pt(1) ) ),
            p = pt;
            break;
        end
    end
end
n = length( p );
switch n,
    case 0,
        y = zeros(sx);
        return
    case 1,
        y = p(1) * ones(sx);
        return
    case 2,
        y = p(1) * x + p(2);
        return
    case 3,
        % Quadratic
        b2a = p(2) ./ ( 2 * p(1) );
        y = p(1) * ( square( x + b2a ) - b2a * b2a ) + p(3);
        return
    otherwise,
        if all( p(2:end) == 0 ),
            y = p(1) * ( x .^ (n-1) ) + p(end);
            return
        end
end

%
% Build and solve the SDP
%

degr  = n - 1;
deg2  = 0.5 * degr + 1;
nv    = prod( sx );
psign = sign(p(1));
p     = psign * reshape( p, 1, n );
cvx_begin sdp
    variable y(sx);
    variable P(deg2,deg2,sx) hankel;
    if cvx_isconstant( x )
        minimize(sum(y)); %#ok
    else
        minimize(y); %#ok
    end
    P >= 0; %#ok
    1 == P(1,1,:); %#ok
    x == reshape( P(2,1,:), sx ); %#ok
    y == reshape( p(end:-1:1) * [ reshape( P(1,:,:), deg2, nv ) ; reshape( P(2:end,end,:), deg2-1, nv ) ], sx ); %#ok
cvx_end

y = cvx_optval * psign;

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


%KCENTRES Finds K center objects from a distance matrix
%
%  [LAB,J,DM] = KCENTRES(D,K,N)
%
% INPUT
%   D    Distance matrix between, e.g. M objects (may be a dataset)
%   K    Number of center objects to be found (optional; default: 1)
%   N    Number of trials starting from a random initialization
%        (optional; default: 1)
%
% OUTPUT
%   LAB  Integer labels: each object is assigned to its nearest center
%   J    Indices of the center objects
%   DM   A list of distances corresponding to J: for each center in J
%        the maximum distance of the objects assigned to this center.
%
% DESCRIPTION
% Finds K center objects from a symmetric distance matrix D. The center
% objects are chosen from all M objects such that the maximum of the
% distances over all objects to the nearest center is minimized. For K > 1,
% the results depend on a random initialization. The procedure is repeated
% N times and the best result is returned.
%
% If N = 0, initialisation is not random, but done by a systematic
% selection based on a greedy approach.
%
% SEE ALSO (PRTools Guide)
% HCLUST, PRKMEANS, EMCLUST, MODESEEK

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: kcentres.m,v 1.5 2008/07/03 09:08:43 duin Exp $

function [labels,Jopt,dm] = kcentres(d,k,n)

	if (nargin < 3) | isempty(n)
      n = 1;
      prwarning(4,'Number of trials not supplied, assuming one.');
   end

	if (nargin < 2) | isempty(k)
      k = 1;
      prwarning(4,'Number of centers not supplied, assuming one.');
   end

	if(isdataset(d))
		d = +d;
		prwarning(4,'Distance matrix is convert to double.');
	end

	[m,m2] = size(d);
	if ( ~issym(d,1e-12) | any(diag(d) > 1e-14) )
		error('Distance matrix should be symmetric and have zero diagonal')
	end

% checking for a zero diagonal
	t = eye(m) == 1;
	if(~all(d(t)==0))
		error('Distance matrix should have a zero diagonal.')
	end

	if (k == 1)
		dmax = max(d);
		[dm,Jopt] = min(dmax);
		labels = repmat(1,m,1);
		return;
	end

	if k > m
		error('Number of centres should not exceed number of objects')
	end

	% We are here only if K (> 1) centers are to be found.
	% Loop over number of trials.
	dmax = max(max(d));
	dopt = inf;
	s = sprintf('k-centres, %i attempts: ',n);
	prwaitbar(n,s,n>1);
	if n == 0
		nrep = 1;
	else
		nrep = n;
	end
	for tri = 1:nrep
		prwaitbar(n,tri,[s int2str(tri)]);
		if n == 0
			M = kcentresort(d,k);           % systematic initialisation
		else
			M = randperm(m); M = M(1:k);		% Random initializations
		end
 		J = zeros(1,k); 					    	  % Center objects to be found.

		% Iterate until J == M. See below.
		while 1,
			[dm,I] = min(d(M,:));

			% Find K centers.
			for i = 1:k
				JJ = find(I==i);
				if (isempty(JJ))
					%JJ can be empty if two or more objects are in the same position of
					% feature space in dataset
					J(i) = 0;
				else
						% Find objects closest to the object M(i)
					[dummy,j,dm] = kcentres(d(JJ,JJ),1,1);
					J(i) = JJ(j);
				end
			end

			J(find(J==0)) = [];
			k = length(J);
			if k == 0
				error('kcentres fails as some objects are identical: add some noise')
			end
			if (length(M) == k) & (all(M == J))
				% K centers are found.
				[dmin,labs] = min(d(J,:));
				dmin = max(dmin);
				break;
			end
			M = J;
		end

		% Store the optimal centers in JOPT.

		if (dmin <= dopt)
			dopt = dmin;
			labels = labs';
			Jopt = J;
		end

	end
	prwaitbar(0)

	% Determine the best centers over the N trials.
	dm = zeros(1,k);
	for i=1:k
		L = find(labels==i);
		dm(i) = max(d(Jopt(i),L));
	end

return;

%KCENTRESORT Sort objects given by dissimilarity matrix
%
%               N = KCENTRESORT(D,P,CRIT)
%
% INPUT
%   D     Square dissimilarity matrix, zeros on diagonal
%   P     Number of prototypes to be selected
%   CRIT  'dist' or 'centre'
%
% OUTPUT
%   N     Indices of selected prototypes
%
% DESCRIPTION
% Sort objects given by square dissim matrix D using a greedy approach
% such that the maximum NN distance from all objects (prototypes)
% to the first K: max(min(D(:,N(1:K),[],2)) is minimized.
%
% This routines tries to sample the objects such that they are evenly
% spaced judged from their dissimilarities. This may be used as
% initialisation in KCENTRES. It works reasonably, but not very good.
%
% SEE ALSO (PRTools Guide)
% KCENTRES

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function N = kcentresort(d,p,crit);

d = +d;
m = size(d,1);
if nargin < 3, crit = 'dist'; end
if nargin < 2 | isempty(p), p = m; end
L = [1:m];
N = zeros(1,p);
[dd,n] = min(max(d,[],2));  % this is the first (central) prototype
e = d(:,n);                 % store here the distances to the nearest prototype (dNNP)
f = min(d,repmat(e,1,m));   % replace distances that are larger than dNNP by dNNP
N(1) = n;                   % ranking of selected prototypes
L(n) = [];                  % candidate prototypes (all not yet selected objects)

for k=2:p                   % extend prototype set
        if strcmp(crit,'centre')
        [dd,n] = min(max(f(L,L),[],1)); % This selects the next prototype out of candidates in L
        e = min([d(:,L(n)) e],[],2);    % update dNNP
        f = min(d,repmat(e,1,m));       % update replacement of distances that are larger
                                              % than dNNP by dNNP
        elseif strcmp(crit,'dist')
                [dd,n] = max(mean(d(N(find(N > 0)),L)));
        else
                error('Illegal crit')
        end
  N(k) = L(n);                    % update list of selected prototypes
  L(n) = [];                      % update list of candidate prototypes
end


function b = r8mat_l1_inverse ( n, a )

%*****************************************************************************80
%
%% R8MAT_L1_INVERSE inverts a unit lower triangular R8MAT.
%
%  Discussion:
%
%    A unit lower triangular matrix is a matrix with only 1's on the main
%    diagonal, and only 0's above the main diagonal.
%
%    The inverse of a unit lower triangular matrix is also
%    a unit lower triangular matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 October 2005
%
%  Author:
%
%    Original FORTRAN77 version by Albert Nijenhuis, Herbert Wilf.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    A Nijenhuis and H Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, number of rows and columns in the matrix.
%
%    Input, real A(N,N), the unit lower triangular matrix.
%
%    Output, real B(N,N), the inverse matrix.
%
  for i = 1 : n

    for j = 1 : n

      if ( i < j )
        b(i,j) = 0.0;
      elseif ( j == i )
        b(i,j) = 1.0;
      else
        b(i,j) = -( a(i,1:i-1) * b(1:i-1,j) );
      end

    end
  end

  return
end


function y = modefilt1( x, s )
% One-dimensional mode filtering.
%
% Applies a width s one-dimensional mode filter to vector x. That is each
% element of the output y(j) corresponds to the mode of x(j-r:j+r), where
% r~s/2. At boundary regions, y is calculated on smaller windows, for
% example y(1) is calculated over x(1:1+r).  Note that for this function to
% make sense x should take on only a small number of discrete values
% (running time is actually proportional to number of unique values of x).
% This function is modeled after medfilt1, which is part of Matlab's
% 'Signal Processing Toolbox'.
%
% USAGE
%  y = modefilt1( x, s )
%
% INPUTS
%  x   - length n vector
%  s   - filter size
%
% OUTPUTS
%  y   - filtered vector x
%
% EXAMPLE
%  x=[0 1 0 0 0 3 0 1 3 1 2 2 0 1]; s=3;
%  xmedian = medfilt1( x, s ); % may not be available
%  xmode = modefilt1( x, s );
%  [x; xmedian; xmode]
%
% See also MEDFILT1
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.35
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

% get unique values in x
[vals,~,inds]=unique(x(:)'); m=length(vals); n=length(x);
if(m>256), warning('x takes on large number of diff vals'); end %#ok<WNTAG>

% create quantized representation [H(i,j)==1 iff x(j)==vals(i)]
H=zeros(m,n); H(sub2ind2([m,n],[inds; 1:n]'))=1;

% create histogram [H(i,j) is count of x(j-r:j+r)==vals(i)]
H=localSum(H,[0 s],'same');

% compute mode for each j and map inds back to original vals
[~,inds]=max(H,[],1); y=vals(inds);

end


% Chapter 7: Statistical estimation
%
%  counting_problem_poisson.m - Section 7.1.1: Counting problems with Poisson distribution
%  ML_covariance_est.m        - Section 7.1.1: Covariance estimation for Gaussian variables
%  probbounds.m               - Section 7.4.3: Probability bounds example with Voronoi diagram
%  expdesign.m                - Section 7.5.2: Experiment design
%  logistics.m                - Figure 7.1: Logistic regression
%  logistics_gp.m             - Figure 7.1: Logistic regression (GP version)
%  maxent.m                   - Example 7.2: Maximum entropy distribution
%  detector2.m                - Example 7.4: Binary hypothesis testing
%  cheb.m                     - Computes Chebyshev lower bounds on probability vectors
%  cher.m                     - Computes Chernoff upper bounds on probability
%  montecarlo.m               - Probability estimate using Monte Carlo methods
help Contents


function [x fx] = newtonArmijo(func,init,arguments,varargin)
%function [x fx] = newton(func,init,arguments,varargin)
% do a newton minimization
% func must return the likelihood and the value of the update -H^{-1}g

[max_its init_alpha thresh debug min_alpha max_inner_its] = process_options(varargin,'max-its',1000,'init-alpha',1,'thresh',1e-8,'debug',0,'min-alpha',1e-15,'max-inner-its',1000);
x = init;
beta = 1e-4; tau = 0.25;

iterator = newIterator(max_its,'thresh',thresh,'debug',debug);
alpha = init_alpha;
while ~iterator.done
    [old_score gradient step] = feval(func,x,arguments{:});

    %do armijo linear-searchto find step size
    new_score = feval(func,x+alpha*step,arguments{:});
    inner_its = 0;
    while inner_its < max_inner_its && alpha > min_alpha && (isnan(new_score) || isinf(new_score) || new_score > old_score + beta * gradient' * (alpha * step))
        %if debug, fprintf('alpha: %.2e -> %.2e\n',alpha,alpha*tau); end
        alpha = alpha * tau;
        new_score = feval(func,x+alpha*step,arguments{:});
        inner_its = inner_its + 1;
    end
    if alpha < min_alpha, alpha == 0; end
    try %try to take a step, if you can
        x = x + alpha * step;
        alpha = alpha ./ tau; %walk back one step
        fx(iterator.its+1) = new_score;
        iterator = updateIterator(iterator,-new_score);
    catch
        iterator.done = true;
    end
end


function [ v, d ] = pts_pca( x, kernel )
% PTS_PCA  Linear and Kernel Principal Component Analysis (PCA and KPCA)
%
% [ V, D ] = PTS_PCA(X)
%
%   Center X and compute eigenvectors V and eigenvalues D using linear PCA.
%
%   X is a matrix where each column is a vector. Each row is a sample of a
%   variable.
%
%   The eigenproblem solved is
%
%     S * v = d * v
%
%   where v is each eigenvector, and d each corresponding eigenvalue. S is
%   the biased covariance matrix
%
%     S = 1/M * Xc * Xc'
%
%   where Xc is the matrix of centered vectors X.
%
%   NOTE: If there are less vectors than variables, or kernel PCA is
%   selected, MDS is used to speed up computation of eigenvectors.
%
%   NOTE: Tiny eigenvalues can give numerical errors, so eigenvalues <
%   1e-13 and the corresponding eigenvectors are removed from the output.
%   Negative eigenvalues are assumed to arise from numerical errors, and
%   removed too.
%
%   NOTE: Matlab's function cov() computes the _unbiased_ covariance.
%
% [ A, DA ] = PTS_PCA(X, KERNEL)
%
%   Generalization to any type of kernel. KERNEL is a struct with the
%   kernel description
%
%     Linear PCA, as above
%
%       KERNEL.type = 'linear'
%
%     Kernel PCA, Polynomial homogeneous order q
%
%       KERNEL.type = 'polyh'
%       KERNEL.params = q
%
%     Kernel PCA, Gaussian with standard deviation sigma
%
%       KERNEL.type = 'gauss'
%       KERNEL.params = sigma
%
%   Because in general we want to avoid any explicit computations in
%   feature space, we solve the eigenproblem [1]
%
%     Kc * a = da * a
%
%   where a is each "coefficient eigenvector", and da = m * d, where m is
%   the number of vectors X. Kc is the kernel matrix of centered feature
%   vectors phi(X).
%
%   If you want to obtain the eigenvectors and eigenvalues in feature space
%   (for those cases where feature space is finite dimensional), you need
%   to compute the feature space training vectors phi(X), center them to
%   phic, and do
%
%     v = phic * a * diag(1./sqrt(da))
%     d = da / m
%
%   where m is the number of training vectors. (Note that v computed as
%   above is equal to v computed from the feature vectors save possibly the
%   sign).
%
% [1] B. Schölkopf, A.J. Smola and K.-R. Müller. "Kernel Principal
% Component Analysis". In "Advances in Kernel Methods - SV Learning", B.
% Schölkopf, C.J.C. Burges and A.J. Smola, eds. pp. 327--352, MIT Press,
% 1999.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2009-2011 University of Oxford
% Version: 1.1.0
% $Rev: 434 $
% $Date: 2011-06-03 17:16:33 +0100 (Fri, 03 Jun 2011) $
%
% 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/>.

% check arguments
error( nargchk( 1, 2, nargin, 'struct' ) );
error( nargoutchk( 0, 2, nargout, 'struct' ) );

if ndims( x ) > 2
    error( 'X must be a matrix, not a volume' )
end

% get sizes
%   N: vector length
%   M: number of training vectors
[ N, M ] = size( x );

% defaults
if ( nargin < 2 || isempty( kernel ) )
    kernel.type = 'linear';
end

switch kernel.type

    case 'linear'

        % center training vectors
        xmean = mean( x, 2 );
        x = x - repmat( xmean, 1, M );

        % compute matrix for eigenproblem
        if ( M < N ) % "kernel matrix" trick to speed up computation
            x_cov = x' * x;
        else % normal case, covariance matrix
            x_cov = x * x' / M;
        end

        % compute eigenvalues and eigenvectors
        [ v, d ] = eig( x_cov );

        % save memory by reducing eigenvalues to a vector
        d = diag( d );

        % reoder in decreasing order of signed modulus value
        [~, idx] = sort(abs(d).*sign(d), 1, 'descend');
        d = d(idx);
        v = v(:, idx);

        % if "kernel matrix" trick was used, it is necessary to convert the
        % coefficient eigenvectors and eigenvalues to feature space
        % eigenvectors and eigenvalues
        if ( M < N )

            % scale eigenvectors
            v = x * ( v * diag( 1 ./ sqrt( d ) ) );

            % scale eigenvalues
            d = d / M;

        end

    otherwise

        % for kernel PCA, we use a generalization of the speed up trick
        % above
        k = pts_kmat( kernel, x );

        % center kernel matrix (this is the same as centering the feature
        % space training vectors and then computing the kernel matrix)
        onesm = ones( M ) / M;
        k = k - onesm * k - k * onesm + onesm * k * onesm;

        % k must be symmetric in order to avoid complex eigenvalues, and in
        % theory it should be, but there are small errors due to numeric
        % precision when the centered kernel matrix is computed
        k = ( k + k' ) / 2;

        % compute eigenvalues and eigenvectors
        [ v, d ] = eig( k );

        % save memory by reducing eigenvalues to a vector
        d = diag( d );

        % reoder in decreasing order of signed modulus value
        [~, idx] = sort(abs(d).*sign(d), 1, 'descend');
        d = d(idx);
        v = v(:, idx);

end


function value = c1_geg_monomial_integral ( alpha, expon )

%*****************************************************************************80
%
%% C1_GEG_MONOMIAL_INTEGRAL: integral of monomial with Gegenbauer weight on C1.
%
%  Discussion:
%
%    C1_GEG is the interval [-1,+1] with the Gegenbauer weight function
%
%      w(alpha;x) = (1-x^2)^alpha
%
%    with -1.0 < alpha.
%
%    value = integral ( -1 <= x <= +1 ) x^expon (1-x^2)^alpha dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the exponent of (1-X^2).
%    - 1.0 < ALPHA.
%
%    Input, integer EXPON, the exponent.
%    0 <= EXPON.
%
%    Output, real VALUE, the value of the integral.
%
  if ( alpha <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'C1_GEG_MONOMIAL_INTEGRAL - Fatal error!\n' );
    fprintf ( 1, '  ALPHA <= -1.0\n' );
    error ( 'C1_GEG_MONOMIAL_INTEGRAL - Fatal error!' );
  end

  if ( mod ( expon, 2 ) == 1 )
    value = 0.0;
    return
  end

  c = expon;

  arg1 = - alpha;
  arg2 =   1.0 + c;
  arg3 =   2.0 + alpha + c;
  arg4 = - 1.0;

  value1 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );

  value = 2.0 * gamma ( 1.0 + c ) * gamma ( 1.0 + alpha ) ...
    * value1 / gamma ( 2.0 + alpha  + c );

  return
end


function [M1] = spm_eeg_inv_rigidreg(data1, data2)
% Computes homogenious transformation matrix based on two sets
% of points from two coordinate systems
%
% FORMAT [M1] = spm_eeg_inv_rigidreg(data1, data2)
% Input:
% data1      - locations of the first set of points corresponding to the
%            3D surface to register onto
% data2      - locations of the second set of points corresponding to the
%            second 3D surface to be registered
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak
% $Id: spm_eeg_inv_rigidreg.m 5219 2013-01-29 17:07:07Z spm $

M       = spm_detrend(data1');
S       = spm_detrend(data2');
[U,A,V] = svd(S'*M);
R1      = V*U';
if det(R1) < 0
    B      = eye(3);
    B(3,3) = det(V*U');
    R1     = V*B*U';
end
t1 = mean(data1,2) - R1*mean(data2,2);
M1 = [R1 t1; 0 0 0 1];


% Stability domain of second order extrapolated BDF scheme

% Copyright 2001 P. Wesseling
% This program and its subprograms may be freely used, modified and distributed
% under the GNU General Public License: http://www.gnu.org/copyleft/gpl.html

% Theory is given in Section 5.8 of
% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program generates Fig. 5.7  in the book

figure(1), clf, hold on

		% Plot of stability domain
theta = 0:0.01:0.5; theta = theta*pi;
a = (6*cos(theta)-9)./(2*cos(2*theta) - 4*cos(theta)) - 3/2;
b = (sin(theta).*(2-cos(theta)))./(cos(2*theta) - 2*cos(theta));
z = - a - i*b; plot(z)

		% Plot of oval
a = 1; b =(2*a/4)^(1/4); theta = 0:0.002:1; theta = theta*pi;
z =  - a*(1-cos(theta)) + i*b*(sin(theta)).^0.5; plot(z,'--')

		% Plot of parabola
yy = 0:0.02:1.8; b = 1; par = -(yy/b).^2; plot(par,yy,'.')


function [ scimat_out ] = scimat_tformarray( Sci_HR, new_rotmat, new_size, new_spacing, new_min)
%UNTITLED4 transforms a 3D scimat image using tformarray

% new_rotmat = old_rotmat;
% new_size = old_size;
% new_spacing = old_spacing;
% new_min = old_min;

old_rotmat = Sci_HR.rotmat;

old_spacing_x = Sci_HR.axis(1).spacing;
old_spacing_y = Sci_HR.axis(2).spacing;
old_spacing_z = Sci_HR.axis(3).spacing;
old_spacing = [old_spacing_x, old_spacing_y, old_spacing_z];

old_min_x = Sci_HR.axis(1).min;
old_min_y = Sci_HR.axis(2).min;
old_min_z = Sci_HR.axis(3).min;
old_min = [old_min_x, old_min_y, old_min_z];

%%

% tformarray indexes from 0, not 1
T0 = eye(4);
T0(1:3,4) = -1;

% convert to mm:
S1 = diag([old_spacing([2 1 3]) 1]);

% convert the min field, which is the top left coordinate, into voxel
% centers (offset)
% (y, x, z) -> (x, y, z)
old_offset = old_min([2 1 3]) + old_spacing([2 1 3])/2;

% translate to origin:
T1 = eye(4);
T1(1:3, 4) = old_offset;

% rotate input:
R1 = pinv(old_rotmat);
R1(4,4) = 1;

T_old = T1 * R1 * S1 * T0;

%%
% scale of new image:
S2 = diag([new_spacing([2 1 3]) 1]);

% rotate to new plane:
R2 = pinv(new_rotmat);
R2(4,4) = 1;

% new transalation
new_offset = new_min([2 1 3]) + new_spacing([2 1 3])/2;
T2 = eye(4);
T2(1:3, 4) = new_offset;

T_new = T2 * R2 * S2 * T0;

%%
% this assumes multiplying by the left - TF * [i j k 1]'
TF = pinv(T_new) * T_old;

if max(abs(TF(4, 1:3))) > 1E-10
    disp('Transformation function might be broken, T = ')
    disp(TF)
end
TF(end, :) = [0 0 0 1];

% maketform assumes multiplying from the right, so we need to transpose the
% transformation
T = maketform('affine', TF');
Res = makeresampler('nearest','bound');

% the images are stored permuted as YXZ, reverse this
data = permute(Sci_HR.data, [2 1 3]);

rotated_image = tformarray(data, T, Res, [1 2 3], [1 2 3], new_size([2 1 3]), [], []);

% and re-permute when saving again
scimat_out.data = permute(rotated_image, [2 1 3]);
scimat_out.rotmat = new_rotmat;

for i = 1:3
    scimat_out.axis(i).size = new_size(i);
    scimat_out.axis(i).spacing = new_spacing(i);
    scimat_out.axis(i).min = new_min(i);
end


function [tout,yout] = pendulumDE(tVals,omega0,startAngle)

%   Copyright 2008-2009 The MathWorks, Inc.

% Equations of motion
eqnsOfMotion = @(t,x) [x(2); -omega0^2*sin(x(1))];

[tout,yout] = ode45(eqnsOfMotion,tVals,[startAngle; 0]);


function V=latticetype2matrix(L,a,M,lt);
%LATTICETYPE2MATRIX  Convert lattice description to matrix form
%   Usage: V=latticetype2matrix(L,a,M,lt);
%
%   `V=latticetype2matrix(L,a,M,lt)` converts a standard description of a
%   lattice using the *a*, *M* and *lt* parameters into a $2\times 2$
%   integer matrix description. The conversion is *only* valid for the
%   specified transform length *L*.
%
%   The output will be in lower triangular Hemite normal form.
%
%   For more information, see
%   `<http://en.wikipedia.org/wiki/Hermite_normal_form>`_.
%
%   An example:::
%
%     V = latticetype2matrix(120,10,12,[1 2])
%
%   See also: matrix2latticetype

L2=dgtlength(L,a,M,lt);

if L~=L2
    error('%s: Invalid transform length.',upper(mfilename));
end;

b=L/M;
s=b/lt(2)*lt(1);
V=[a 0;...
   s b];


function A = sdt_A(H, F)
% Calculate signal detection theory A statistic:
% a non-parametric estimate of sensitivity in ROC analysis
% per Zhang and Mueller (2005)
%
% :Usage:
% ::
%
%     A = sdt_A(H, F)
%
% :Inputs:
%
%   **H:**
%        hit rate
%
%   **F:**
%        false alarm rate
%
% :Outputs:
%
%   **A:**
%        A measure of sensitivity of a detector, essentially the area under
%        the ROC curve for either "normal" (H>=F) or "reverse skill" (F>H)
%        case (ranges from 0.5 to 1).
%
% Meaning of values where F > H ("reverse skill"):
%
% The meaningful measure of sensitivity in this case is A(F,H)
% as opposed to A(H,F).  Subject is assumed to still be sensitive,
% but to have inverted their interpretation (or "reverse skill").
%
% To distinguish from the normal case, such results are returned
% as (1 - sdt_A(F,H)), so the full range from 0 to 1 is used.
% For comparisons of sensitivity use abs(sdt_A(H,F) - 0.5).
%
% :Example:
%
% sdt_A(0.6,0.8) = 0.325. This indicates the same sensitivity as
% sdt_A(0.8,0.6) = 0.675, but with reversed skill.
%
% ..
%    Author: Joe Wielgosz 8/13/2009
% ..

if F <= H % Normal case

    if (F <= 0.5) && (0.5 <= H)
        A = 0.75 + (H-F)/4 - F*(1-H);
    elseif (F <= H) && (H <= 0.5)
        A = 0.75 + (H-F)/4 - F/(4*H);
    else % (0.5 < F) && (F <= H)
        A = 0.75 + (H-F)/4 - (1-H)/(4*(1-F));
    end

else % "Reverse skill" - more false positives than hits
    % Using Jason Buhle's approach
    A = 1 - sdt_A(F, H);
%    A = NaN;
end

end


function [fevd_estimates]=olsfevd(irf_estimates,IRFperiods,gamma,n)

% function [fevd_estimates]=olsfevd(irf_estimates,IRFperiods,gamma,n,endo,datapath)
% computes and displays fevd values for the OLS VAR model
% inputs:  - cell 'irf_estimates': lower bound, point estimates, and upper bound for the IRFs
%          - integer 'IRFperiods': number of periods for IRFs
%          - matrix 'gamma': structural disturbance variance-covariance matrix (defined p 48 of technical guide)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - cell 'endo': list of endogenous variables of the model
%          - string 'datapath': user-supplied path to excel data spreadsheet
% outputs: - cell 'fevd_estimates': lower bound, point estimates, and upper bound for the FEVD

% this function implements the procedure described p55-56

% preliminary tasks
% create the first cell
temp=cell(n,n+1);

% start by filling the first column of every Tij matrix in the cell
% loop over rows of temp
for jj=1:n
   % loop over columns of temp
   for ii=1:n
   % square each element
   temp{jj,ii}(:,1)=irf_estimates{jj,ii}(:,1).^2;
   end
end
% fill all the other entries of the Tij matrices
% loop over rows of temp
for jj=1:n
   % loop over columns of temp
   for ii=1:n
      % loop over remaining columns
      for kk=2:IRFperiods
      % define the column as the square of the corresponding column in orthogonalised_irf_record
      % additioned to the value of the preceeding columns, which creates the cumulation
      temp{jj,ii}(:,kk)=irf_estimates{jj,ii}(:,kk).^2+temp{jj,ii}(:,kk-1);
      end
   end
end
% multiply each matrix in the cell by the variance of the structural shocks
% loop over rows of temp
for ii=1:n
% loop over columns of temp
   for jj=1:n
   % multiply column jj of the matrix by the variance of the structural shock
   temp{ii,jj}(1,:)=temp{ii,jj}(1,:)*gamma(jj,jj);
   end
end

% obtain now the values for Ti, the (n+1)th matrix of each row
% loop over rows of temp
for ii=1:n
% start the summation over Tij matrices
temp{ii,n+1}=temp{ii,1};
   % sum over remaining columns
   for jj=2:n
   temp{ii,n+1}=temp{ii,n+1}+temp{ii,jj};
   end
end

% create the output cell fevd_record
fevd_estimates=cell(n,n);
% fill the cell
% loop over rows of fevd_estimates
for ii=1:n
   % loop over columns of fevd_estimates
   for jj=1:n
   % define the matrix Vfij as the division (pairwise entry) of Tfij by Tfj
   fevd_estimates{ii,jj}=temp{ii,jj}./temp{ii,n+1};
   end
end


% GRADIENT DESCENT function.
% Iteratively optimizes theta model parameters.
function [theta, J, exit_flag] = gradient_descent(X, y, theta, lambda)
    % X - training set.
    % y - training output values.
    % theta - model parameters.
    % lambda - regularization parameter.

    % Set Options
    options = optimset('GradObj', 'on', 'MaxIter', 400);

    % Optimize
    [theta, J, exit_flag] = fminunc(@(t)(gradient_callback(X, y, t, lambda)), theta, options);
end


function x = complex_lorentz( sx, dim )

%COMPLEX_LORENTZ   Complex second-order cone.
%   COMPLEX_LORENTZ(N), where N is a positive integer, creates a column
%   variable of length N and a scalar variable, and constrains them
%   to lie in a second-order cone. That is, given the declaration
%       variable x(n) complex
%       variable y
%   the constraint
%       {x,y} == complex_lorentz(n)
%   is equivalent to
%       norm(x,2) <= y
%   The inequality form is more natural, and preferred in most cases. But
%   in fact, the COMPLEX_LORENTZ set form is used by CVX itself to convert
%   complex NORM()-based constraints to solvable form.
%
%   COMPLEX_LORENTZ(SX,DIM), where SX is a valid size vector and DIM is a
%   positive integer, creates an array variable of size SX and an array
%   variable of size SY (see below) and applies the second-order cone
%   constraint along dimension DIM. That is, given the declarations
%       sy = sx; sy(min(dim,length(sx)+1))=1;
%       variable x(sx) complex
%       variable y(sy)
%   the constraint
%       {x,y} == complex_lorentz(sx,dim)
%   is equivalent to
%       norms(x,2,dim) <= y
%   Again, the inequality form is preferred, but CVX uses the set form
%   internally. DIM is optional; if it is omitted, the first non-singleton
%   dimension is used.
%
%   LORENTZ(SX,DIM,CPLX) creates real second-order cones if CPLX is FALSE,
%   and complex second-order cones if CPLX is TRUE. The latter case is
%   equivalent to COMPLEX_LORENTZ(SX,DIM).
%
%   Disciplined convex programming information:
%       LORENTZ is a cvx set specification. See the user guide for
%       details on how to use sets.

if nargin == 1,
    x = lorentz( sx, [], true );
else
    x = lorentz( sx, dim, true );
end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [x,y] = snakedeform(x,y,alpha,beta,gamma,kappa,fx,fy,ITER)
% -------In this function, the initial contour of Active Coutour Model(Snake)
%  will be deformed in the given external force field.
%
%     alpha:   elasticity parameter
%     beta:    rigidity parameter
%     gamma:   viscosity parameter
%     kappa:   external force weight
%     fx,fy:   external force field

N = length(x);

alpha = alpha* ones(1,N);
beta = beta*ones(1,N);

% produce the five diagnal vectors
alpham1 = [alpha(2:N) alpha(1)];
alphap1 = [alpha(N) alpha(1:N-1)];
betam1 = [beta(2:N) beta(1)];
betap1 = [beta(N) beta(1:N-1)];

a = betam1;
b = -alpha - 2*beta - 2*betam1;
c = alpha + alphap1 +betam1 + 4*beta + betap1;
d = -alphap1 - 2*beta - 2*betap1;
e = betap1;

% generate the parameters matrix
A = diag(a(1:N-2),-2) + diag(a(N-1:N),N-2);
A = A + diag(b(1:N-1),-1) + diag(b(N), N-1);
A = A + diag(c);
A = A + diag(d(1:N-1),1) + diag(d(N),-(N-1));
A = A + diag(e(1:N-2),2) + diag(e(N-1:N),-(N-2));

invAI = inv(A + gamma * diag(ones(1,N)));

for count = 1:ITER,
   vfx = interp2(fx,x,y,'*linear');
   vfy = interp2(fy,x,y,'*linear');

   % deform snake
   x = invAI * (gamma* x + kappa*vfx);
   y = invAI * (gamma* y + kappa*vfy);
end


function y = simple_fitness(x)
y = 100*(x(1)^2 - x(2))^2 + (1 - x(3))^2 + abs(0.4 - x(2));


% Isoparametric Formulation Implementation
% clear memory
clear all;

figure(5)

% Displacements
subplot(1,2,1); hold on;
%plot(xx,displacements_exact,'-r');

% Stress
subplot(1,2,2); hold on;
%plot(xx,stress_exact,'-r');

%% Physical Parameters
% Initial Area
A0 = 2; % m^2

% Modulus of elasticity
E = 8; % N/m^2

% Length of bar
Ltotal = 4; % m

%% Build Elements
% number of elements,
%numberElements = 4;
test = [1 2 4 8];
for numberElements = test

% number of nodes,
numberNodes = 2*numberElements+1;

% node coordinates,
x = linspace(2,6,numberNodes)';

% Generation of coordinates and connectivities
L = zeros(numberElements,1); elementNodes = zeros(numberElements,3);
L(1) = Ltotal/numberElements; elementNodes(1,:) = [1,2,3];
for i = 2:numberElements
    L(i) = L(1); elementNodes(i,:) = elementNodes(i-1,:) + 2;
end
nodeCoordinates = x;

% element center coordinate
x_mid = 0.5*(x(elementNodes(:,3))-x(elementNodes(:,1)))+x(elementNodes(:,1));

% Evaluate area the center of each element.
% A: area of cross section
A = A0*x_mid;

% for structure:
   % displacements: displacement vector
   % force : force vector
   % stiffness: stiffness matrix
force=zeros(numberNodes,1);
stiffness=zeros(numberNodes,numberNodes);
% computation of the system stiffness matrix and force vector
for e=1:numberElements;
  % elementDof: element degrees of freedom (Dof)
  elementDof=elementNodes(e,:) ;
  detJacobian=L(e)/2;
  invJacobian=1/detJacobian;
  ngp = 3;
  [w,xi]=gauss1d(ngp);
  xc=0.5*(nodeCoordinates(elementDof(1))+nodeCoordinates(elementDof(3)));
  for ip=1:ngp;
      [shape,naturalDerivatives]=shapeFunctionL3(xi(ip));
      B=naturalDerivatives*invJacobian;
      stiffness(elementDof,elementDof)=...
      stiffness(elementDof,elementDof)+ B'*B*w(ip)*detJacobian*E*A(e);
      force(elementDof) = force(elementDof)+...
          8*(xc+detJacobian*xi(ip))*shape'*detJacobian*w(ip);
  end
end

%% BCs and solution
% prescribed dofs
prescribedDof = find(nodeCoordinates == 2); % fixed at node x = 2 m
% solution
GDof=numberNodes;
displacements=solution(GDof,prescribedDof,stiffness,force);
% output displacements/reactions
outputDisplacementsReactions(displacements,stiffness, ...
    numberNodes,prescribedDof,force)

%% Stress
elementCoords=zeros(numberElements*ngp,1);
stress=zeros(numberElements*ngp,1);
for e=1:numberElements;
  % elementDof: element degrees of freedom (Dof)
  elementDof=elementNodes(e,:);
  detJacobian=L(e)/2;
  invJacobian=1/detJacobian;
  [w,xi]=gauss1d(ngp);
  xc=0.5*(nodeCoordinates(elementDof(1))+nodeCoordinates(elementDof(3)));
  for ip=1:ngp;
      [shape,naturalDerivatives]=shapeFunctionL3(xi(ip));
      B=naturalDerivatives*invJacobian;
      elementCoords(ip+(e-1)*3,1)=xc+xi(ip)*detJacobian;
      stress(ip+(e-1)*3,1)=E*B*displacements(elementDof,1);
  end
end

%% plot figures

switch numberElements
    case 1
        line='r*--';
    case 2
        line='g*--';
    case 4
        line='k*--';
    case 8
        line='m*--';
end

% Displacements
subplot(1,2,1); plot(nodeCoordinates,displacements,line)

% Stress
subplot(1,2,2); axis([2,6,0,26]); plot(elementCoords,stress,line)

end %end for
subplot(1,2,1); legend('Exact','NEL=1','NEL=2','NEL=4','NEL=8',4); hold off
subplot(1,2,2); legend('Exact','NEL=1','NEL=2','NEL=4','NEL=8',1); hold off


% Local Linear Embedding
% This function implements the weight computation defined in
% Sam T. Roweis, Lawrence K. Saul, "Nonlinear Dimensionality
% Reduction by Local Linear Embedding", Science, 2000.
% 'w' is the weights for representing the row-vector 'pt' in terms
% of the dimensions x neighborCount matrix 'neighbors'.
% 'conditionerMult' is the multiplier of the identity matrix added
% to the neighborhood correlation matrix before inversion.

function w = localLinearEmbedding(pt, neighbors, conditionerMult)
    % each column of neighbors represent a neighbor, each row a dimension
    % pt is a row vector
    corr = neighbors' * neighbors + conditionerMult * eye(size(neighbors, 2));
    ptDotN = neighbors' * pt;
    alpha = 1 - sum(corr \ ptDotN);
    beta = sum(corr \ ones(size(corr, 1), 1)); % sum of elements of inv(corr)
    lagrangeMult = alpha / beta;
    w = corr \ (ptDotN + lagrangeMult);
end


function quadrule_test27 ( )

%*****************************************************************************80
%
%% TEST27 tests LEGENDRE_SET_LOG and SUM_SUB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2009
%
%  Author:
%
%    John Burkardt
%
  order_max = 20;
  test_num = 9;

  norder_test = [ 1, 2, 3, 4, 5, 6, 7, 8, 16 ];

  a = 0.0;
  b = 1.0;

  nsub = 1;

  xlo = 0.0;
  xhi = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST27\n' );
  fprintf ( 1, '  LEGENDRE_SET_LOG sets up Gauss-Legendre quadrature\n' );
  fprintf ( 1, '    over [0,1] with weight function -LOG(X);\n' );
  fprintf ( 1, '  SUM_SUB carries it out.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The integration interval is [%f, %f]\n', a, b );
  fprintf ( 1, '  Number of subintervals is %d\n', nsub );
  fprintf ( 1, '  Quadrature order will vary.\n' );
  fprintf ( 1, '  Integrand will vary.\n' );

  nfunc = func_set ( 'COUNT', 'DUMMY' );

  for ilo = 1 : 5 : nfunc

    ihi = min ( ilo + 4, nfunc );

    fprintf ( 1, '\n' );
    fprintf ( 1, '    ' );
    for i = ilo : ihi
      fprintf ( '%14s', fname(i) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );

    for test = 1 : test_num

      norder = norder_test(test);

      fprintf ( 1, '  %2d', norder );

      for i = ilo : ihi

        func_set ( 'SET', i );

        [ xtab, weight ] = legendre_set_log ( norder );

        result(i) = sum_sub ( @func, a, b, nsub, norder,  xlo, xhi, xtab, ...
          weight );

        fprintf ( 1, '  %12f', result(i) );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


function pols = llegepols1 ( degree, x )

%*****************************************************************************80
%
%% LLEGEPOLS1 evaluates orthogonal polynomials on the symmetric interval.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the maximum degree.
%
%    Input, real X, the evaluation point.
%
%    Output, real POLS(DEGREE+1)), the orthogonal
%    polynomials evaluated at X.
%
  pols = zeros ( degree + 1, 1 );

  pkp1 = 1.0;
  pols(1) = pkp1;

  if ( degree == 0 )
    return
  end

  pk = pkp1;
  pkp1 = x;
  pols(2) = pkp1;

  if ( degree == 1 )
    return
  end

  for k = 1 : degree - 1

    pkm1 = pk;
    pk = pkp1;
    pkp1 = ( ( 2 * k + 1 ) * x * pk ...
           - (     k     ) * pkm1 ) ...
           / (     k + 1 );

    pols(k+2) = pkp1;

  end

  return
end


function [derivativeMatrix,r8fdx]=firstOrderDerivativeMatrix(xl,xu,n)
% firstOrderDerivativeMatrix precomputes the matrices used to implement numerical differentiation using 9 points.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

%  Compute the spatial increment
dx=(xu-xl)/(n-1);
r8fdx=1./(40320.*dx);
nm4=n-4;

mid_block = zeros(n-8,n);

first_row 	= [-109584 	+322560 	-564480 	+752640 	-705600 	+451584 	-188160 	+46080 	-5040];
second_row 	= [-5040 	-64224 		+141120 	-141120 	+117600 	-70560 		+28224 		-6720 	+720];
third_row 	= [+720 	-11520 		-38304 		+80640 		-50400 		+26880 		-10080 		+2304 	-240];
fourth_row 	= [-240 	+2880 		-20160 		-18144 		+50400 		-20160 		+6720 		-1440 	+144];

i_th_row 	= [+144 	-1536 		+8064 		-32256 		+0 			+32256 		-8064 		+1536 	-144];

n_min_3_row 	= [-144 	+1440 		-6720 		+20160 		-50400 		+18144 		+20160 		-2880 	+240];
n_min_2_row 	= [+240 	-2304 		+10080 		-26880 		+50400 		-80640 		+38304 		+11520 	-720];
n_min_1_row 	= [-720 	+6720 		-28224 		+70560 		-117600 	+141120 	-141120 	+64224 	+5040];
n_min_0_row 	= [+5040 	-46080 		+188160  	-451584 	+705600 	-752640 	+564480 	-322560 +109584];

first_block = [first_row;second_row;third_row;fourth_row];
first_block = [first_block zeros(4,n-9)];

row_index = 1;
for i=5:nm4
    mid_block(row_index,row_index:row_index+8)=i_th_row;
    row_index = row_index+1;
end

last_block = [n_min_3_row;n_min_2_row;n_min_1_row;n_min_0_row];
last_block=[zeros(4,n-9) last_block];

derivativeMatrix = [first_block;mid_block;last_block];

end


clear all

disp('Data loading and generation...');
ima = double(imread('lena.png'));
L = 3;
s = zeros(size(ima));
for k = 1:L
    s = s + abs(randn(size(ima)) + i * randn(size(ima))).^2 / 2;
end
ima_nse = ima .* sqrt(s / L);

disp('Non-it PPB filter...');
hW = 10;
hD = 3;
alpha = 0.88;
T = inf;
nbit = 1;
ima_fil_1 = ppb_nakagami(ima_nse, L, ...
                         hW, hD, ...
                         alpha, T, ...
                         nbit);

disp('It PPB filter...');
hW = [1 3 5 10];
hD = [0 1 2 3];
alpha = 0.92;
T = 0.2;
nbit = [1 2 3 4];
ima_fil_2 = ppb_nakagami(ima_nse, L, ...
                         hW, hD, ...
                         alpha, T, ...
                         nbit);

figure
subplot(2,2,1), plotimage_sar(ima);
title('Noise-free image');
subplot(2,2,2), plotimage_sar(ima_nse, ima);
title(sprintf('Noisy image (PSNR %.3f)', snr(ima_nse, ima)));
subplot(2,2,3), plotimage_sar(ima_fil_1, ima);
title(sprintf('Non-it PPB filter (PSNR %.3f)', snr(ima_fil_1, ima)));
subplot(2,2,4), plotimage_sar(ima_fil_2, ima);
title(sprintf('It PPB filter (PSNR %.3f)', snr(ima_fil_2, ima)));


function pass = test_pcg( pref )
% Test operator PCG method.

% Tolerance:
if ( nargin == 0 )
    pref = cheboppref;
end
tol = 1e2*pref.bvpTol;

%% Example 1: (-u_xx=f, bc=0, mean(f)==0)
a = @(x) 1;
c = @(x) 1;
L = @(x,u) -diff(a(x).*diff(u)) + c(x).*u;

% Chebop solve:
N = chebop( L );
N.bc = 0;
f = chebfun(@(x) 1-3*x.^2);

u = N\f;
% Chebfun-ready pcg:

v = pcg(N, f);

% Errors:
pass(1) = ( norm( u - v ) < tol );

%% Example 2: (-(a(x)*u_x)_x=f, bc=0, mean(f)==0)
a = @(x) 2+cos(pi*x);
c = @(x) 0*x;
L = @(x,u) -diff(a(x).*diff(u)) + c(x).*u;

% Chebop solve:
N = chebop( L );
N.bc = 0;
f = chebfun(@(x) 1-3*x.^2);
u = N \ f;

% Chebfun-ready pcg:
v = pcg(N, f);

% Errors:
pass(2) = ( norm( u - v ) < tol );

%% Example 3: (-(a(x)*u_x)_x + c(x)*u=f, bc=0, mean(f)==0)

a = @(x) 2+cos(pi*x);
c = @(x) 1+10*x.^2;
L = @(x,u) -diff(a(x).*diff(u)) + c(x).*u;

% Chebop solve:
N = chebop( L );
N.bc = 0;
f = chebfun(@(x) 1-3*x.^2);
u = N \ f;
% Chebfun-ready pcg:
v = pcg(N, f);

% Errors:
pass(3) = ( norm( u - v ) < tol );

%% Example 4: (-(a(x)*u_x)_x + c(x)*u=f, bc=0, sum(f)==0, piecewise)
a = @(x) 2+cos(pi*abs((x-0.5)));
c = @(x) 1+10*x.^2;
L = @(x,u) -diff(a(x).*diff(u)) + c(x).*u;

% Chebop solve:
N = chebop( L );
N.bc = 0;
%f = chebfun(@(x) 1-2*x.^2);
f = chebfun(@(x) abs(x) - 0.5 ,'splitting','on');
u = N \ f;
% Chebfun-ready pcg:
v = pcg(N, f);
pass(4) = ( norm(u - v) < tol );

%% Example 5: (-(a(x)*u_x)_x + c(x)*u=f, bc~=0, mean(f)~=0)
a = @(x) 2+cos(pi*x);
c = @(x) 1+10*x.^2;
L = @(x,u) -diff(a(x).*diff(u)) + c(x).*u;

% Chebop solve:
N = chebop( L );
N.lbc = 1;
N.rbc = -1;
f = chebfun(@(x) 1-2*x.^2);
u = N \ f;

% Chebfun-ready pcg:
v = pcg(N, f);

pass(5) = ( norm(u - v) < tol );

%% Example 6: (-(a(x)*u_x)_x + c(x)*u=f, bc~=0, mean(f)~=0, piecewise)
a = @(x) 2+cos(pi*x);
c = @(x) abs(cos(2*pi*x));
L = @(x,u) -diff(a(x).*diff(u)) + c(x).*u;

% Chebop solve:
N = chebop( L );
N.lbc = 1;
N.rbc = -1;
f = chebfun(@(x) 1-2*x.^2);
u = N \ f;

% Chebfun-ready pcg:
v = pcg(N, f);

pass(6) = ( norm(u - v) < tol );

%% Example 7: (-(a(x)*u_x)_x + c(x)*u=f, bc~=0, mean(f)~=0, nonstandard domain)
a = @(x) 2+cos(pi*x);
c = @(x) abs(cos(2*pi*x));
L = @(x,u) -diff(a(x).*diff(u)) + c(x).*u;

% Chebop solve:
dom = [-3/2,2];
N = chebop( L, dom );
N.lbc = 3;
N.rbc = -1;
f = chebfun(@(x) 1-2*x.^2, dom);
u = N \ f;

% Chebfun-ready pcg:
v = pcg(N, f);

pass(7) = ( norm(u - v) < tol );

if ( all(pass) == 1 )
    pass = 1;
end

end


% StackExchange Mathematics Q3674241
% https://math.stackexchange.com/questions/3674241
% Solve argmina∥ax−y∥1 - Minimizer of the L1 Norm of the Difference of a Vectors
% References:
%   1.
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     14/05/2020
%   *   First release.

%% General Parameters

subStreamNumberDefault = 42; %<! Set to 0 for Random

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Parameters

numElements = 5; %<! Symmetric Matrix
numPts      = numElements * 1000;

%% Load / Generate Data

vX = randn(numElements, 1);
vY = randn(numElements, 1);

hObjFun = @(valA) sum(abs(valA * vX - vY)); %<! Scaled L1 Norm
hS = @(valA) sign(valA * vX - vY); %<! The 'vS' vector

%% Solution by CVX

solverString = 'CVX';

cvx_solver('SDPT3'); %<! Default, Keep numRows low
% cvx_solver('SeDuMi');
% cvx_solver('Mosek'); %<! Can handle numRows > 500, Very Good!
% cvx_solver('Gurobi');

hRunTime = tic();

cvx_begin('quiet')
% cvx_begin()
    % cvx_precision('best');
    variable valA(1, 1);
    minimize( norm(valA * vX - vY, 1) );
cvx_end

runTime = toc(hRunTime);

% vX = mX(:);

disp([' ']);
disp([solverString, ' Solution Summary']);
disp(['The ', solverString, ' Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(valA))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(valA), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

%% Solution by Minimum Search

solverString = 'Solution by 1D Search of the Minimum';

hRunTime = tic();

valA = fminsearch(hObjFun, mean(vY ./ vX));

runTime = toc(hRunTime);

disp([' ']);
disp([solverString, ' Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(valA))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(valA), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

%% Solution by Root Search

solverString = 'Solution by 1D Search of the Roto of the Gradient';

hF = @(valA) sign(valA * vX - vY).' * vX;

hRunTime = tic();

valA = fzero(hF, mean(vY ./ vX));

runTime = toc(hRunTime);

disp([' ']);
disp([solverString, ' Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(valA))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(valA), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

%% Solution by Analytic Form

solverString = 'Solution by Analytic Form';

hRunTime = tic();

valA = SolveScaledL1(vX, vY);

runTime = toc(hRunTime);

disp([' ']);
disp([solverString, ' Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(valA))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(valA), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

%% Display Results

vA = sort(vY ./ vX, 'ascend');
vB = zeros(numElements, 1);
vC = zeros(numElements, 1);

for ii = 1:numElements
    vB(ii) = hS(vA(ii)).' * vX;
    vC(ii) = hObjFun(vA(ii));
end

vD = linspace(vA(1) - 1, vA(numElements) + 1, numPts);
vD = sort([vD(:); vA], 'ascend');
vE = zeros(numPts + numElements, 1);
vF = zeros(numPts + numElements, 1);

for ii = 1:(numPts + numElements)
    vE(ii) = hS(vD(ii)).' * vX;
    vF(ii) = hObjFun(vD(ii));
end

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);

hAxes       = axes();
set(hAxes, 'NextPlot', 'add');
hLineSeries = plot(vD, vE);
set(hLineSeries, 'LineWidth', lineWidthNormal);
hLineSeries = plot(vD, zeros(numPts + numElements, 1));
set(hLineSeries, 'LineWidth', lineWidthNormal);
hLineSeries = plot(vA, vB);
set(hLineSeries, 'LineStyle', 'none', 'Marker', '*', 'MarkerSize', markerSizeLarge);
set(get(hAxes, 'Title'), 'String', ['Value of f''(a)'], 'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', 'a', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', '$ {f}^{''} \left( a \right) $', ...
    'FontSize', fontSizeAxis, 'Interpreter', 'latex');
hLegend = ClickableLegend({['Values of All Points'], ['Zero Range'], ['Junction Points']});

if(generateFigures == ON)
    % saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
    print(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
end

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);

hAxes       = axes();
set(hAxes, 'NextPlot', 'add');
hLineSeries = plot(vD, vF);
set(hLineSeries, 'LineWidth', lineWidthNormal);
hLineSeries = plot(vA, vC);
set(hLineSeries, 'LineStyle', 'none', 'Marker', '*', 'MarkerSize', markerSizeLarge);
set(get(hAxes, 'Title'), 'String', ['Value of f(a)'], 'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', 'a', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', '$ f \left( a \right) $', ...
    'FontSize', fontSizeAxis, 'Interpreter', 'latex');
hLegend = ClickableLegend({['Values of All Points'], ['Junction Points']});

if(generateFigures == ON)
    % saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
    print(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
end

%% Restore Defaults

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