function value = p23_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P23_F evaluates the integrand for problem 23.
%
%  Dimension:
%
%    DIM_NUM is arbitrary.
%
%  Region:
%
%    The interior of the unit simplex, for which all X's are nonnegative,
%    and sum ( X(1:N) ) <= 1.
%
%  Integral Parameters:
%
%    C defaults to 1.0.
%    Call P23_R8 to get or set this value.
%
%    E(1:DIM_NUM) defaults to (/ 2, 2, ..., 2 /).
%    Call P23_I4VEC to get or set this value.
%
%  Integrand:
%
%    F(X) = C * X1^E1 * X2^E2 * ... * Xn^En
%
%    C is real, all exponents E are nonnegative integers.
%
%  Exact Integral:
%
%    C * Gamma(E1+1) * Gamma(E2+1) * ... * Gamma(En+1) / Gamma(E1+E2+...+En+1)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  c = 0.0;
  c = p23_r8 ( 'G', 'C', c );

  e = [];
  e = p23_i4vec ( 'G', 'E', dim_num, e );

  value(1:point_num) = c;

  for point = 1 : point_num

    for dim = 1 : dim_num
      value(point) = value * x(dim,point)^e(dim);
    end

  end

  p23_i4 ( 'I', '#', point_num );

  return
end


function linplus_test22 ( )

%*****************************************************************************80
%
%% TEST22 tests R8GB_FA, R8GB_SL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = m;
  ml = 1;
  mu = 2;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST22\n' );
  fprintf ( 1, '  For a general banded matrix,\n' );
  fprintf ( 1, '  R8GB_FA computes the PLU factors.\n' );
  fprintf ( 1, '  R8GB_SL solves a factored linear system.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of matrix rows M    = %d\n', m );
  fprintf ( 1, '  Number of matrix columns N = %d\n', n );
  fprintf ( 1, '  Lower bandwidth ML         = %d\n', ml );
  fprintf ( 1, '  Upper bandwidth MU         = %d\n', mu );
%
%  Set the matrix.
%
  [ a, seed ] = r8gb_random ( m, n, ml, mu, seed );

  r8gb_print ( m, n, ml, mu, a, '  The banded matrix:' );
%
%  Set the desired solution.
%
  x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
  b = r8gb_mxv ( m, n, ml, mu, a, x );
%
%  Factor the matrix.
%
  [ alu, pivot, info ] = r8gb_fa ( n, ml, mu, a );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST022 - Fatal error!\n' );
    fprintf ( 1, '  R8GB_FA declares the matrix is singular!\n' );
    fprintf ( 1, '  The value of INFO is %d\n', info );
    return;
  end
%
%  Solve the linear system.
%
  job = 0;
  x = r8gb_sl ( n, ml, mu, alu, pivot, b, job );

  r8vec_print ( n, x, '  Solution to A*x=b:' );
%
%  Set the desired solution.
%
  x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
  job = 1;
  b = r8gb_ml ( n, ml, mu, alu, pivot, x, job );

  r8vec_print ( n, b, '  Right hand side of transposed system:' );
%
%  Solve the linear system.
%
  job = 1;
  x = r8gb_sl ( n, ml, mu, alu, pivot, b, job );

  r8vec_print ( n, x, '  Solution to A''x=b:' );

  return
end


function  [Z] =  MCWNNM_ADMM2( Y, NSig, Par )
% This routine solves the following weighted nuclear norm optimization problem with column weights,
%
% min_{X, Z} ||W(Y-X)||_F^2 + ||Z||_w,*  s.t.  X = Z
%
% Inputs:
%        Y      -- 3p^2 x M dimensional noisy matrix, D is the data dimension, and N is the number of image patches.
%        NSig -- 3p^2 x 1 dimensional vector of weights
%        Par   -- structure of parameters
% Output:
%        Z      -- 3p^2 x M dimensional denoised matrix

% tol = 1e-8;
if ~isfield(Par, 'maxIter')
    Par.maxIter = 10;
end
if ~isfield(Par, 'rho')
    Par.rho = 1;
end
if ~isfield(Par, 'mu')
    Par.mu = 1;
end
if ~isfield(Par, 'display')
    Par.display = true;
end
% Initializing optimization variables
% Intialize the weight matrix W
W = 1 ./ (NSig+eps);
% Initializing optimization variables
X = zeros(size(Y));
Z = zeros(size(Y));
A = zeros(size(Y));
%% Start main loop
iter = 0;
PatNum       = size(Y,2);
TempC  = Par.Constant * sqrt(PatNum);
while iter < Par.maxIter
    iter = iter + 1;

    % update X, fix Z and A
    % min_{X} ||W * Y - W * X||_F^2 + 0.5 * rho * ||X - Z + 1/rho * A||_F^2
    X = diag(1 ./ (W.^2 + 0.5 * Par.rho)) * (diag(W.^2) * Y + 0.5 * Par.rho * Z - 0.5 * A);

    % update Z, fix X and A
    % min_{Z} ||Z||_*,w + 0.5 * rho * ||Z - (X + 1/rho * A)||_F^2
    Temp = X + A/Par.rho;
    [U, SigmaTemp, V] =   svd(full(Temp), 'econ');
    [SigmaZ, svp] = ClosedWNNM(diag(SigmaTemp), 2/Par.rho*TempC, eps);
    Z =  U(:, 1:svp) * diag(SigmaZ) * V(:, 1:svp)';
    %     % check the convergence conditions
    %     stopC = max(max(abs(X - Z)));
    %     if Par.display && (iter==1 || mod(iter,10)==0 || stopC<tol)
    %         disp(['iter ' num2str(iter) ',mu=' num2str(Par.mu,'%2.1e') ...
    %             ',rank=' num2str(rank(Z,1e-4*norm(Z,2))) ',stopALM=' num2str(stopC,'%2.3e')]);
    %     end
    %     if stopC < tol
    %         break;
    %     else
    % update the multiplier A, fix Z and X
    A = A + Par.rho * (X - Z);
    Par.rho = min(1e4, Par.mu * Par.rho);
    %     end
end
return;


function [interpV] = finterp3(xInterpV, yInterpV, zInterpV, field3M, xFieldV, yFieldV, zFieldV, OOBV)
%"finterp3"
%   Fast 3D linear interpolation of the data field3M defined at
%   xFieldV,yFieldV,zFieldV, to the points listed in x/y/zInterpV.
%
%   xFieldV and yFieldV are 3 element vectors of the form [start, delta, end],
%   where start is the coordinate of the first column(x) or row(y) and end
%   is the coord of the last column/row, with delta being the spacing.  Delta
%   can be negative if start > end.
%
%   zFieldV must be specified as a list of coordinates for each slice in
%   field3M.
%
%   xInterpV, yInterpV, zInterpV are vectors of equal length indicating the
%   x,y,z, coordinates to interpolate from original data field3M.
%
%   Based on code by J.O.Deasy.
%
%JRA 1/13/05
%
%Usage:
%   function [interpV] = finterp3(xInterpV, yInterpV, zInterpV, field3M, xFieldV, yFieldV, zFieldV, outOfBoundsVal)

siz = size(field3M);

if ~exist('OOBV')
    OOBV = NaN;
end

xDelta = xFieldV(2);
yDelta = yFieldV(2);

%Get r,c,s indices.
% cols = interp1q(reshape(xFieldV, [], 1), reshape(1:length(xFieldV),[], 1), reshape(xInterpV, [], 1));
% rows = interp1q(reshape(yFieldV, [], 1), reshape(1:length(yFieldV),[], 1), reshape(yInterpV, [], 1));
cols = (xInterpV-(xFieldV(1)-xDelta))/xDelta;
rows = (yInterpV-(yFieldV(1)-yDelta))/yDelta;
if length(zFieldV) > 1;
    slcs = interp1(zFieldV,1:length(zFieldV),zInterpV);
else
    slcs = ones(size(cols));    %This effectively negates Z.  All values are in plane.  Bad idea?
end

clear xInterpV yInterpV zInterpV
clear xFieldV yFieldV zFieldV

%Find indices out of bounds.
colNaN = cols >= siz(2) | cols < 1;
rowNaN = rows >= siz(1) | rows < 1;
% slcNaN = slcs >= siz(3) | slcs < 1;

% colNaN = isnan(cols);
% rowNaN = isnan(rows);
slcNaN = isnan(slcs) | slcs < 1 | slcs >= siz(3);

%Set those to a proxy 1.
rows(rowNaN) = 1;
cols(colNaN) = 1;
slcs(slcNaN) = 1;

colFloor = floor(cols);
colMod   = cols - colFloor;
oneMinusColMod = (1-colMod);

rowFloor = floor(rows);
rowMod = rows - rowFloor;
oneMinusRowMod = (1-rowMod);

slcFloor = floor(slcs);
slcMod = slcs - slcFloor;
oneMinusSlcMod = (1-slcMod);

clear rows cols slcs

%Linear indices of lower bound contributing points.
INDEXLIST = rowFloor + (colFloor-1)*siz(1) + (slcFloor-1)*siz(1)*siz(2);

clear rowFloor colFloor slcFloor

%Linear offsets when moving in 3D matrix.
oneRow = 1;
oneCol = siz(1);
oneSlc = siz(1)*siz(2);

%Accumulate contribution from each voxel surrounding x,y,z point.
interpV = double(field3M(INDEXLIST)) .* oneMinusRowMod .* oneMinusColMod .* oneMinusSlcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneRow))) .* rowMod .* oneMinusColMod .* oneMinusSlcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneCol))) .* oneMinusRowMod .* colMod .* oneMinusSlcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneCol+oneRow))) .* rowMod .* colMod .* oneMinusSlcMod;;
interpV = interpV + double(field3M(INDEXLIST+(oneSlc))) .* oneMinusRowMod .* oneMinusColMod .* slcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneSlc+oneRow))) .* rowMod .* oneMinusColMod .* slcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneSlc+oneCol))) .* oneMinusRowMod .* colMod .* slcMod;
interpV = interpV + double(field3M(INDEXLIST+(oneSlc+oneCol+oneRow))) .* rowMod .* colMod .* slcMod;

%Replace proxy 1s with out of bounds vals.
interpV(rowNaN | colNaN | slcNaN) = OOBV;

return;


function r_size  = nwspgr_size ( type, dim, k, sym, compress )

%*****************************************************************************80
%
%% NWSPGR_SIZE determines the size of a sparse grid rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2012
%
%  Author:
%
%    Original MATLAB version by Florian Heiss, Viktor Winschel.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Florian Heiss, Viktor Winschel,
%    Likelihood approximation by numerical integration on sparse grids,
%    Journal of Econometrics,
%    Volume 144, 2008, pages 62-80.
%
%  Parameters:
%
%    Input, string TYPE, selects the 1D integration rule:
%    "kpu": Nested rule for unweighted integral over [0,1];
%    "kpn": Nested rule for integral with Gaussian weight;
%    "gqu": Gaussian quadrature for unweighted integral over [0,1]
%    (Gauss-Legendre);
%    "gqn": Gaussian quadrature for integral with Gaussian weight
%    (Gauss-Hermite);
%    func:  any function name. Function must accept level l and return nodes N
%    and weights W for univariate quadrature rule with polynomial exactness
%    2*L-1 as [n, w] = feval(func,level)
%
%    Input, integer DIM, the dimension of the integration problem.
%
%    Input, integer K, the level.  When using the built in 1D rules, the
%    resulting sparse grid will be exact for polynomials up to total order
%    2*K-1.  When using the built-in quadrature rules, the maximum value of K
%    that is available is 25.
%
%    Input, integer SYM.  This is only useful when the user has
%    chosen to supply the 1D quadrature rule.  In that case, if the rule is
%    symmetric, specifying SYM to be 1 will allow the code to run faster.
%    But this also requires that the user quadrature rule function only
%    return the nonnegative abscissas and their weights.
%
%    Input, integer COMPRESS,
%    0, do not compress the rule ( = do not merge duplicate points.)
%    1, compress the rule.
%
%    Output, integer R_SIZE, the "size" of the rule.
%

%
%  Interpret the inputs.  Use STRCMPI so that case is ignored.
%
  builtinfct = ( strcmpi ( type, 'gqu' ) || ...
                 strcmpi ( type, 'gqn' ) || ...
                 strcmpi ( type, 'kpu' ) || ...
                 strcmpi ( type, 'kpn' ) );

  sym = logical ( sym );
%
%  Retrieve the 1D rules of levels 1 to K.
%  Create cell arrays X1D and W1D containing the node and weight information.
%  The array N1D stores the length or order of each rule.
%
%  The built in functions already return only the positive orthant.
%
%  If the rule is being supplied by the user, and the user indicates that
%  it's a symmetric rule, then similarly keep only the positive orthant.
%
%  The result will be that if SYM is true, whether for built in or user-supplied
%  functions, we have only the positive orthant data stored.
%
  n1d = zeros ( 1, k );
  x1d = cell ( k, 1 );
  w1d = cell ( k, 1 );

  for level = 1 : k

    [ x, w ] = feval ( type, level );

    if ( ( ~ builtinfct ) && sym )
      [ numnew, dummy ] = size ( x );
      [ x, sortvec ] = sortrows ( x );
      w = w ( sortvec );
      x = x((floor(numnew/2)+1):numnew,:);
      w = w((floor(numnew/2)+1):numnew,:);
    end

    n1d(level) = length ( w );
    x1d{level} = x;
    w1d{level} = w;

  end
%
%  Initialization.
%
  minq = max ( 0, k - dim );
  maxq = k - 1;
  nodes = [];
  weights = [];
%
%  Loop for max ( 0, K - DIM ) <= Q <= K - 1.
%
  for q = minq : maxq

    r = length ( weights );
%
%  BQ is the combinatorial coefficient applied to the component
%  product rules which have level Q.
%
    bq = ( -1 )^( maxq - q ) * nchoosek ( dim - 1, dim + q - k );
%
%  Compute the D-dimensional row vectors that sum to DIM+Q.
%
    is = get_seq ( dim, dim + q );
%
%  Preallocate new rows for nodes and weights.
%
    Rq = prod ( n1d(is), 2 );
    sRq = sum ( Rq );
    nodes   = [ nodes;   zeros(sRq,dim) ];
    weights = [ weights; zeros(sRq,1) ];
%
%  Generate each of the product rules indicated by IS, and
%  insert them into NODES and WEIGHTS.
%
    for j = 1 : size(is,1)
      midx = is(j,:);
      [ newn, neww ] = tensor_product ( x1d(midx), w1d(midx) );
      nodes((r+1):(r+Rq(j)),:) = newn;
      weights((r+1):(r+Rq(j))) = bq .* neww;
      r = r + Rq(j);
    end
%
%  Sort the nodes and merge repeated values.
%
    [ nodes, sortvec ] = sortrows ( nodes );
    weights = weights(sortvec);
    keep = 1;
    lastkeep = 1;

    for j = 2 : size(nodes,1)
      if ( compress )

        if ( nodes(j,:) == nodes(j-1,:) )
          weights(lastkeep) = weights(lastkeep) + weights(j);
        else
          lastkeep = j;
          keep = [ keep ; j ];
        end
      else
        lastkeep = j;
        keep = [ keep ; j ];
      end
    end

    nodes = nodes(keep,:);
    weights = weights(keep);

  end
%
%  The rule has been computed.
%  If we used symmetry, we now have to extend the rule.
%
  if ( sym )

    nr = length ( weights );
    m = x1d{1};

    for j = 1 : dim

      keep = zeros(nr,1);
      numnew = 0;

      for r = 1 : nr
        if ( nodes(r,j) ~= m )
          numnew = numnew + 1;
          keep(numnew) = r;
        end
      end

      if ( 0 < numnew )
        nodes = [nodes ; nodes(keep(1:numnew),:)];
        nodes(nr+1:nr+numnew,j) = 2*m - nodes(nr+1:nr+numnew,j);
        weights = [weights ; weights(keep(1:numnew))];
        nr = nr + numnew;
      end

    end

    [ nodes, sortvec ] = sortrows ( nodes );
    weights = weights(sortvec);

  end

  r_size = length ( weights );

  return
end


function fem1d_pack_test01 ( )

%*****************************************************************************80
%
%% FEM1D_PACK_TEST01 tests LOCAL_BASIS_1D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  node_num = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_PACK_TEST01:\n' );
  fprintf ( 1, '  LOCAL_BASIS_1D evaluates the local basis functions\n' );
  fprintf ( 1, '  for a 1D element.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Test that the basis functions, evaluated at the nodes,\n' );
  fprintf ( 1, '  form the identity matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', node_num );

  node_x(1:node_num) = [ 1.0, 2.0, 4.0, 4.5 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Node coordinates:\n' );
  fprintf ( 1, '\n' );
  for j = 1 : node_num
    fprintf ( 1, '  %8d  %7.3f\n', j, node_x(j) );
  end

  for j = 1 : node_num
    x = node_x(j);
    phi_matrix(1:node_num,j) = local_basis_1d ( node_num, node_x, x );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(I,J) = PHI(I) at node (J):\n' );
  fprintf ( 1, '\n' );
  for i = 1 : node_num
    for j = 1 : node_num
      fprintf ( 1, '  %7.3f', phi_matrix(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The PHI functions should sum to 1 at random X values:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       X        Sum ( PHI(:)(X) )\n' );
  fprintf ( 1, '\n' );

  for j = 1 : 5
    [ x, seed ] = r8_uniform_ab ( 1.0, 4.5, seed );
    phi = local_basis_1d ( node_num, node_x, x );
    fprintf ( 1, '  %14.6g  %14.6g\n', x, sum ( phi(1:node_num) ) );
  end

  return
end


function [fobj, fresidue, fsparsity, fregs] = getObjective_RegSc(X, B, S, Sigma, beta, gamma)

Err = X - B*S;

fresidue = 0.5*sum(sum(Err.^2));

fsparsity = gamma*sum(sum(abs(S)));

fregs = 0;
for ii = size(S, 1),
    fregs = fregs + beta*S(:, ii)'*Sigma*S(:, ii);
end

fobj = fresidue + fsparsity + fregs;


function [x1,lun,err] = perform_douglas_rachford(A,y,options)

% perform_douglas_rachford -
%
%   [x1,lun,err] = perform_douglas_rachford(A,y,options);
%
%   Solve noiseless Basis Pursuit
%       x1 = argmin_x \sum |x[k]|   subj.to A*x=y
%
%   A can be a matrix or an operator A(x,dir,options)
%   with dir=1 for A*x and dir=-2 for A^{+}*x (pseudo inverse).
%
%   options.x can be an initial guess
%
%   options.niter is the number of iterations
%
%   Special thanks to Jalal Fadili for useful helps on this algorithm.
%
%   Copyright (c) 2008 Gabirel Peyre

niter = getoptions(options, 'niter', 100);
mu = getoptions(options, 'mu', 3);

if isnumeric(A)
    pA = getoptions(options, 'pseudo_inv', []);
    if isempty(pA)
        pA = pinv(A);
    end
end

% initial guess
x = getoptions(options, 'x', [] );
if isempty(x)
    if isnumeric(A)
        x = pA*y;
    else
        x = feval(A, y, -2, options);
    end
end

% target result (error monitoring
x0 = getoptions(options, 'x0', [] );

drawiter = getoptions(options, 'drawiter',0);
verb = getoptions(options, 'verb', 1);
isreal = getoptions(options, 'isreal', 0);

solve_tv = getoptions(options, 'solve_tv', 0);

lun = [];
err = [];
if isnumeric(A)
    x1 = x + pA*(y-A*x);
else
    x1 = y - feval(A,x, +1, options);
    x1 = x + feval( A, x1, -2, options);
end
if drawiter
    clf;
end

nrefresh = 100;
ndisp = max(2, ceil(niter/nrefresh) );

for i=1:niter
    if verb
        progressbar(i,niter);
    end
    % compute the intermediary point
    %   x <- x-x1 + thresh( 2*x1-x )

    u = 2*x1-x;

%    sparse_weight = getoptions(options, 'sparse_weight', 0,1);
%    u = u./sparse_weight;
    if not(solve_tv)
        % L1 regularization
        u = perform_thresholding( u, mu, 'soft' );
    else
        % TV regularization
        opts.method = 'gradient';
        opts.lambda = mu;
        opts.niter = getoptions(options, 'niter_tv', 50);
        opts.niter_inner = 1;
        opts.display = 0;
        opts.verb = 0;
        [u1,err,tv,lalist,Err] = perform_tv_denoising(u,opts);
        % u1 = u - mu*div( xi, options )
%        opts.xi = grad( (u1-u)/mu, options );
        u = u1;
    end
%    u = u.*sparse_weight;

    x = x-x1 + u;

    % perform projection step on the constraints
    if isnumeric(A)
        x1 = x + pA*(y-A*x);
    else
        x1 = y - feval(A,x,+1, options);
        x1 = x + feval( A, x1, -2, options);
    end

    if isreal
 %       x1 = real(x1); % force real signal
    end
    % check for error decay
    if nargout>=3 && not(isempty(x0))
        err(end+1) = norm(x0-x1, 'fro')^2;
    end
    if nargout>=2
        if not(solve_tv)
            lun(end+1) = sum(abs(x1(:))); % mean(sum(abs(x1)));
        else
            lun(end+1) = compute_total_variation(x1,options);
        end
    end
    if drawiter && mod(i,ndisp)==1  % && ntest==1
        if nb_dims(x1)==1
            n = size(x1,1);
            plot( real(x1(:,1:min(end,3))) );
            axis([1,n,-1,1]);
            axis tight;
            drawnow;
        else
            clf; imageplot(x1); drawnow;
        end
    end
end
% x1 = real(x1);


function [ result, seed ] = sphere01_triangle_quad_00 ( n, v1, v2, v3, f, ...
  seed )

%*****************************************************************************80
%
%% SPHERE01_TRIANGLE_QUAD_00: quadrature over a triangle on the unit sphere.
%
%  Discussion:
%
%    This is a Monte Carlo approach.
%
%    The integral is approximated by averaging the values at N random points,
%    multiplied by the area.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sample points.
%
%    Input, real V1(3), V2(3), V3(3), the XYZ coordinates of
%    the vertices of the triangle.
%
%    Input, function v = f ( x ), evaluates the integrand at X.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real RESULT, the approximate integral.
%
  area = sphere01_triangle_vertices_to_area ( v1, v2, v3  );

  [ vc, seed ] = sphere01_triangle_sample ( n, v1, v2, v3, seed );

  quad = 0.0;
  for j = 1 : n
    quad = quad + f ( vc(1:3,j) );
  end

  result = quad * area / n;

  return
end


function [Rz] = R_z(psi)

% Input sanity check
if nargin < 1
    psi = sym('psi_t','real');
end
assert(numel(psi) == 1,'Angle must be a scalar [1x1].');
assert(isa(psi,'sym') || isnumeric(psi),'Angle must be a symbolic or numeric value.');

% Rotate about the z-axis (yaw)
Rz = [cos(psi) -sin(psi) 0;
      sin(psi)  cos(psi) 0;
             0         0 1];
end


%% ALS optimization for CP and Tucker tensor decompositions

%% Alternating least squares for PARAFAC/CANDECOMP
% The function |parafac_als| computes an estimate of the best rank-R
% PARAFAC model of a tensor X using an alternating least-squares
% algorithm.  The input X can be a tensor, sptensor, ktensor, or
% ttensor. The result P is a ktensor.
rand('state',0);
X = sptenrand([5 4 3], 10)
%%
P = parafac_als(X,2)
%%
P = parafac_als(X,2,struct('dimorder',[3 2 1]))
%%
P = parafac_als(X,2,struct('dimorder',[3 2 1],'init','nvecs'))
%%
U0 = {rand(5,2),rand(4,2),[]}; %<-- Initial guess for factors of P
P = parafac_als(X,2,struct('dimorder',[3 2 1],'init',{U0}))
%% Alternating least squares for Tucker model
% The function |tucker_als| computes the best rank(R1,R2,..,Rn)
% approximation of tensor X, according to the specified dimensions in
% vector R.  The input X can be a tensor, sptensor, ktensor, or
% ttensor.  The result returned in T is a ttensor.
X = sptenrand([5 4 3], 10)
%%
T = tucker_als(X,2)        %<-- best rank(2,2,2) approximation
%%
T = tucker_als(X,[2 2 1])  %<-- best rank(2,2,1) approximation
%%
T = tucker_als(X,2,struct('dimorder',[3 2 1]))
%%
T = tucker_als(X,2,struct('dimorder',[3 2 1],'init','eigs'))
%%
U0 = {rand(5,2),rand(4,2),[]}; %<-- Initial guess for factors of T
T = tucker_als(X,2,struct('dimorder',[3 2 1],'init',{U0}))


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  thresholds calculation from ideal and mismatched    %
%         (both random and linear gradient)            %
%               resistive binary string                %
% code by Fabrizio Conso, university of pavia, student %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                      %
  function [v,v_err]=trs(nbit,vref)
  vref_err=vref*0.1*randn(1);
  for i=1:nbit      %calculation of resistance values
      R(1)=1;
      R1(1)=1+0.1*0.5*randn(1)+0.01*(i); % ideal DAC
      R(i+1) =2^(i-1);
      R1(i+1)=2^(i-1)+0.1*0.5*randn(1)+0.01*(i); % mismatched DAC
  end

  for j=1:nbit        %calculation of tension values
      v(j)    =vref*sum(R(1:j))/sum(R);
      v_err(j)=vref_err*sum(R1(1:j))/sum(R1);
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%             Output             %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                %
 v=v;
 if nargout > 1
	v_err=v_err;
 end
%                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [A] = PoissonCstab1D();

% function [A] = PoissonCstab1D();
% Purpose: Set up symmetric Poisson matrix with estabilized central fluxes

Globals1D;
A = zeros(K*Np); g = zeros(K*Np,1);

% Build matrix -- one column at a time
for i=1:K*Np
    g(i) = 1.0;
    gmat = reshape(g,Np,K);
    Avec = PoissonCstabRHS1D(gmat);
    A(:,i) = reshape(Avec,K*Np,1);
    g(i)=0.0;
end
return


function value = orth_symm_condition ( n )

%*****************************************************************************80
%
%% ORTH_SYMM_DETERMINANT returns the L1 condition of the ORTH_SYMM matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 February 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real VALUE, the L1 condition.
%
  a_norm = 0.0;
  j = 1;
  for i = 1 : n
    angle = 2.0 * i * j * pi / ( 2 * n + 1 );
    a_norm = a_norm + 2.0 * abs ( sin ( angle ) ) / sqrt ( 2 * n + 1 );
  end
  b_norm = a_norm;
  value = a_norm * b_norm;

  return
end


function y = Ackley(x)
  n = 2;
  sum1 = 0;
  sum2 = 0;

  for i=1:n
   sum1 = s1+x(i)^2;
   sum2 = s2+cos((2*pi)*x(i));
  end

  y = 20 + exp(1)-20*exp(-0.2*sqrt(1/n*sum1))-exp(1/n*sum2);

end


function ddata=mydiff(data,h)

L=length(data);
ddata=NaN(1,L);
for i=1:L;
    if i==1;
        ddata(1)=(-3*data(1)+4*data(2)-data(3))/(2*h);
    else
        if i==L;
            ddata(L)=(-3*data(L-2)+4*data(L-1)-data(L))/(2*h);
        else
            ddata(i)=(-data(i-1)+data(i+1))/(2*h);
        end
    end
end


function [coeff,variance]=OLSPriorTheta(y,timetrend,f)
% OLSPriorTheta(y,timetrend,f) computes the prior for coefficients of the
% deterministic trends in the TVE-VAR approach. The prior is normally
% distributed with mean "coeff" and variance "variance". The prior
% parameters are computed regressing the data y on timetrend. Then, coeff is
% the OLS estimate, and variance is the asymptotic variance of the OLS,
% scaled up of a factor f.

%% Prepare data: make sure the dimensions of the inputs are correct
if size(y,1)>size(y,2)
    y=y';
end
if size(timetrend,1)>size(timetrend,2)
    timetrend=timetrend';
end

%% Run OLS
T=size(y,2); % number of data available
k=size(timetrend,1); % number of regressors

P=y*timetrend'*inv(timetrend*timetrend'); % the OLS coefficients
resid=y-P*timetrend; % the residuals from the regression
O=1/(T-k)*resid*resid';
Q=timetrend*timetrend';
varpar=kron(O,Q^-1); % the asymptotic variance

%% The output
variance=varpar*f;
coeff=P';

end


% [PYR, INDICES, STEERMTX, HARMONICS] = buildFullSFpyr2(IM, HEIGHT, ORDER, TWIDTH)
%
% Construct a steerable pyramid on matrix IM, in the Fourier domain.
% Unlike the standard transform, subdivides the highpass band into
% orientations.

function [pyr,pind,steermtx,harmonics] = buildFullSFpyr2(im, ht, order, twidth)

%-----------------------------------------------------------------
%% DEFAULTS:

max_ht = floor(log2(min(size(im)))+1);

if (exist('ht') ~= 1)
  ht = max_ht;
else
  if (ht > max_ht)
    error(sprintf('Cannot build pyramid higher than %d levels.',max_ht));
  end
end

if (exist('order') ~= 1)
  order = 3;
elseif ((order > 15)  | (order < 0))
  fprintf(1,'Warning: ORDER must be an integer in the range [0,15]. Truncating.\n');
  order = min(max(order,0),15);
else
  order = round(order);
end
nbands = order+1;

if (exist('twidth') ~= 1)
  twidth = 1;
elseif (twidth <= 0)
  fprintf(1,'Warning: TWIDTH must be positive.  Setting to 1.\n');
  twidth = 1;
end

%-----------------------------------------------------------------
%% Steering stuff:

if (mod((nbands),2) == 0)
  harmonics = [0:(nbands/2)-1]'*2 + 1;
else
  harmonics = [0:(nbands-1)/2]'*2;
end

steermtx = steer2HarmMtx(harmonics, pi*[0:nbands-1]/nbands, 'even');

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

dims = size(im);
ctr = ceil((dims+0.5)/2);

[xramp,yramp] = meshgrid( ([1:dims(2)]-ctr(2))./(dims(2)/2), ...
    ([1:dims(1)]-ctr(1))./(dims(1)/2) );
angle = atan2(yramp,xramp);
log_rad = sqrt(xramp.^2 + yramp.^2);
log_rad(ctr(1),ctr(2)) =  log_rad(ctr(1),ctr(2)-1);
log_rad  = log2(log_rad);

%% Radial transition function (a raised cosine in log-frequency):
[Xrcos,Yrcos] = rcosFn(twidth,(-twidth/2),[0 1]);
Yrcos = sqrt(Yrcos);

YIrcos = sqrt(1.0 - Yrcos.^2);
lo0mask = pointOp(log_rad, YIrcos, Xrcos(1), Xrcos(2)-Xrcos(1), 0);
imdft = fftshift(fft2(im));
lo0dft =  imdft .* lo0mask;

[pyr,pind] = buildSFpyrLevs(lo0dft, log_rad, Xrcos, Yrcos, angle, ht, nbands);

%% Split the highpass band into orientations

hi0mask = pointOp(log_rad, Yrcos, Xrcos(1), Xrcos(2)-Xrcos(1), 0);

lutsize = 1024;
Xcosn = pi*[-(2*lutsize+1):(lutsize+1)]/lutsize;  % [-2*pi:pi]
order = nbands-1;
const = (2^(2*order))*(factorial(order)^2)/(nbands*factorial(2*order));
Ycosn = sqrt(const) * (cos(Xcosn)).^order;

bands = zeros(prod(size(imdft)), nbands);
bind = zeros(nbands,2);

for b = 1:nbands
    anglemask = pointOp(angle, Ycosn, Xcosn(1)+pi*(b-1)/nbands, Xcosn(2)-Xcosn(1));
    Mask = ((-sqrt(-1))^(nbands-1))*anglemask.*hi0mask;
    % make real the contents in the HF cross (to avoid information loss in these freqs.)
    % It distributes evenly these contents among the nbands orientations
    Mask(1,:) = ones(1,size(im,2))/sqrt(nbands);
    Mask(2:size(im,1),1) = ones(size(im,1)-1,1)/sqrt(nbands);

    banddft =  imdft .* Mask;
    band = real(ifft2(fftshift(banddft)));
    bands(:,b) = real(band(:));
    bind(b,:)  = size(band);
end

pyr = [bands(:); pyr];
pind = [bind; pind];

pind = [ [0 0]; pind];  %% Dummy highpass


function [pos vel alt] = RadarUKF(z, dt)
%
%
persistent Q R
persistent x P
persistent n m
persistent firstRun

if isempty(firstRun)
  Q = [ 0.01  0      0;
        0  0.01  0;
        0  0      0.01 ];

  R = 100;

  x = [0 90 1100]';
  P = 100*eye(3);

  n = 3;
  m = 1;

  firstRun = 1;
end

[Xi W] = SigmaPoints(x, P, 0);

fXi = zeros(n, 2*n+1);
for k = 1:2*n+1
  fXi(:, k) = fx(Xi(:,k), dt);
end

[xp Pp] = UT(fXi, W, Q);
%norm(xp - fx(x, dt))

hXi = zeros(m, 2*n+1);
for k = 1:2*n+1
  hXi(:, k) = hx(fXi(:,k));
end

[zp Pz] = UT(hXi, W, R);
%norm(zp - hx(xp))

Pxz = zeros(n, m);
for k = 1:2*n+1
  Pxz = Pxz + W(k)*(fXi(:, k) - xp)*(hXi(:, k) - zp)';
end

K = Pxz*inv(Pz);

x = xp + K*(z - zp);
P = Pp - K*Pz*K';

pos = x(1);
vel = x(2);
alt = x(3);

%------------------------------
function xp = fx(x, dt)
%
%
A = eye(3) + dt*[ 0 1 0;
                  0 0 0;
                  0 0 0 ];

xp = A*x;

%------------------------------
function yp = hx(x)
%
%
yp = sqrt(x(1)^2 + x(3)^2);


% Build a spectrum-like harmonic model using a given sinusoids set
%
% Input
%  sins     : Nx3 (or Nx4), frequency[Hz], amplitude, phase, (harmnb)
%             Only positive frequencies, with DC.
%  opt      : Options (see below)
%
% Output
%  M        : Spectrum built from sins.
%             (With same format as a DFT result)
%
% Copyright (c) 2012 University of Crete - Computer Science Department
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%
% TODO
%  Manage holes in the harmonic structure
%

function [M, opt] = sin2shm(sins, opt)

    if nargin<2
        opt.nbh             = Inf; % Number of harmonics to be use in the model
                                   % (excluding DC).
        opt.max_freq        = 8000;% Consider harmonics up to this limit.

        opt.prevpow2        = false;% if true, reduce the harmonic model to the
                                  % biggest possible power of 2 length
        opt.dc_extrap       = 1;  % 0: Use the current value
                                  % 1; Copy the amplitude of the first harm
                                  % 2: Use linear extrapolation on log scale
                                  % both 1 and 2 assume angle(DC)=0
    end
    if nargin==0; M=opt; return; end

    % TODO manage holes in the harmonic structure

    if ~isempty(opt.max_freq) && opt.max_freq>0
        inds = sins(1,:)<=opt.max_freq;
        sins = sins(:,inds);
    end
    sins = sins(:,1:min(size(sins,2), 1+opt.nbh));
    if opt.prevpow2
        % in order to speed up the computation of the min phase
        harmsize = 2^(nextpow2(size(sins,2))-1) + 1;
        sins = sins(:,1:min(size(sins,2), harmsize));
    end

    M = [sins(2,:).'.*exp(1i*sins(3,:).')];

    M(1) = sign(real(M(1)))*abs(M(1));        % set DC phase to 0 or pi

    M(end) = sign(real(M(end)))*abs(M(end));  % set the Nyquist phase to 0 or pi

    M = hspec2spec(M);

    % Extrapolate the model DC
    % Doesn't keep the DC phase !
    % Angle(DC) never makes sens in acoustic measurement
    % For technical reasons, we can therefore assume any constant (here zero).
    if opt.dc_extrap==1;     M(1) = abs(M(2));
    elseif opt.dc_extrap==2; M(1) = db2lin(lin2db(M(2))+(lin2db(M(2))-lin2db(M(3))));   end

return


function a = r8pp_indicator ( n )

%*****************************************************************************80
%
%% R8PP_INDICATOR sets up a R8PP indicator matrix.
%
%  Discussion:
%
%    The R8PP storage format is appropriate for a symmetric positive
%    definite matrix.  Only the upper triangle of the matrix is stored,
%    by successive partial columns, in an array of length (N*(N+1))/2,
%    which contains (A11,A12,A22,A13,A23,A33,A14,...,ANN)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Output, real A((N*(N+1))/2), the R8PP matrix.
%
  fac = 10^( i4_log_10 ( n ) + 1 );

  k = 0;
  for j = 1 : n
    for i = 1 : j
      k = k + 1;
      a(k) = fac * i + j;
    end
  end

  return
end


function writeDoublePendulumDynamics(f,M)

%This function writes the dynamics file for the double pendulum.

filename = 'doublePendulumDynamics';

comments{1} = ['DZ = ' upper(filename) '(T,Z,P)'];
comments{2} = ' ';
comments{3} = 'FUNCTION:  This function computes the dynamics of a double';
comments{4} = '    pendulum, and is designed to be called from ode45. The';
comments{5} = '    model allows for arbitrary mass and inertia for each';
comments{6} = '    link, but no friction or actuation';
comments{7} = ' ';
comments{8}  = 'INPUTS: ';
comments{9}  = '    t = time. Dummy input for ode45. Not used.';
comments{10} = '    z = [4xn] matrix of states.';
comments{12} = '    P = struct of parameters';
comments{13} = 'OUTPUTS: ';
comments{14} = '    dz = [4xn] matrix of state derivatives';
comments{15} = ' ';
comments{16} = 'NOTES:';
comments{17} = ['    This file was automatically generated by ' mfilename];

params{1} = {'m1','link one mass'};
params{2} = {'m2','link two mass'};
params{3} = {'g ','gravity'};
params{4} = {'l1','link one length'};
params{5} = {'l2','link two length'};
params{6} = {'I1','link one moment of inertia about its center of mass'};
params{7} = {'I2','link two moment of inertia about its center of mass'};
params{8} = {'d1','distance between link one center of mass and parent joint'};
params{9} = {'d2','distance between link two center of mass and parent joint'};

states{1} = {'th1','link one absolute angle'};
states{2} = {'dth1','link one angular rate'};
states{3} = {'th2','link two absolute angle'};
states{4} = {'dth2','link two angular rate'};

dstates{1} = {'dth1','derivative of link one absolute angle'};
dstates{2} = {'ddth1','derivative of link one angular rate'};
dstates{3} = {'dth2','derivative of link two absolute angle'};
dstates{4} = {'ddth2','derivative of link two angular rate'};

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                               write file                                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
fid = fopen([filename '.m'],'w');

fprintf(fid, ['function dz = ' filename '(~,z,P) \n']);

for i=1:length(comments)
    fprintf(fid,['%%' comments{i} '\n']);
end
fprintf(fid,'\n');

for i=1:length(params)
    fprintf(fid,[params{i}{1} ' = P.' params{i}{1} '; %%' params{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:length(states)
    fprintf(fid,[states{i}{1} ' = z(' num2str(i) ',:); %%' states{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:2;
    fprintf(fid,['f' num2str(i) ' = ' vectorize(char(f(i))) ';\n']);
end
fprintf(fid,'\n');

for i=1:2
    for j=1:2
        fprintf(fid,['M' num2str(i) num2str(j) ' = ' vectorize(char(M(i,j))) ';\n']);
    end
end
fprintf(fid,'\n');

fprintf(fid,'D = M11.*M22 - M12.*M21;\n\n');

fprintf(fid,'ddth1 = (f2.*M12 - f1.*M22)./D;\n');
fprintf(fid,'ddth2 = -(f2.*M11 - f1.*M21)./D;\n');
fprintf(fid,'\n');

fprintf(fid,'dz = [...\n');
for i=1:length(dstates)
    fprintf(fid,['    ' dstates{i}{1} '; %%' dstates{i}{2} '\n']);
end
fprintf(fid,'];\n');

fprintf(fid,'end \n');

fclose(fid);
end


function result = cube_unit_3d ( func )

%*****************************************************************************80
%
%% CUBE_UNIT_3D approximates an integral inside the unit cube in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that:
%
%      -1 <= X <= 1,
%      -1 <= Y <= 1,
%      -1 <= Z <= 1.
%
%  Discussion:
%
%    An 8 point third degree formula is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F(X,Y,Z), of the form
%      function value = func ( x, y, z )
%
%    Output, real RESULT, the approximate integral of the function.
%
  s = 1.0E+00 / sqrt ( 3.0E+00 );
  w = 1.0E+00 / 8.0E+00;

  x = s;
  y = s;
  z = s;

  quad = w * ( ...
      feval ( func,  x,  y,  z ) ...
    + feval ( func,  x,  y, -z ) ...
    + feval ( func,  x, -y,  z ) ...
    + feval ( func,  x, -y, -z ) ...
    + feval ( func, -x,  y,  z ) ...
    + feval ( func, -x,  y, -z ) ...
    + feval ( func, -x, -y,  z ) ...
    + feval ( func, -x, -y, -z ) );

  volume = cube_unit_volume_nd ( 3 );
  result = quad * volume;

  return
end


% TESTFITLINE - demonstrates RANSAC line fitting
%
% Usage: testfit2Dline(outliers, sigma, t, feedback)
%
% Arguments:
%               outliers - Fraction specifying how many points are to be
%                          outliers.
%               sigma    - Standard deviation of inlying points from the
%                          true line.
%               t        - Distance threshold to be used by the RANSAC
%                          algorithm for deciding whether a point is an
%                          inlier.
%               feedback - Optional flag 0 or 1 to turn on RANSAC feedback
%                          information.
%
%  Try using:  testfit2Dline(0.3, 0.05, 0.05)
%
% See also: RANSACFITPLANE, FITPLANE

% Copyright (c) 2003-2006 Peter Kovesi and Felix Duvallet (CMU)
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% August 2006  testfitline created from testfitplane
%              author: Felix Duvallet

function testfit3Dline(outliers, sigma, t, feedback)

    close all;
    if nargin == 0
        outliers = 0.3;
        sigma = 0.05;
        t = 0.05;
        feedback = 1;
    end
    if nargin == 3
        feedback = 0;
    end

    % Hard wire some constants - vary these as you wish

    npts = 100;  % Number of 3D data points

    % Define a line:
    %    Y = m*X
    %    Z = n*X + Y + b
    % This definition needs fixing, but it works for now

    m = 6;
    n = -3;
    b = -4;

    outsigma = 30*sigma;  % outlying points have a distribution that is
                          % 30 times as spread as the inlying points

    vpts = round((1-outliers)*npts);  % No of valid points
    opts = npts - vpts;               % No of outlying points

    % Generate npts points in the line
    X = rand(1,npts);

    Y = m*X + b;

    XY =  [X; Y];

    % Add uniform noise of +/-sigma
    XY = XY + (2*rand(size(XY))-1)*sigma;

    % Generate opts random outliers

    n = length(XY);
    ind = randperm(n);  % get a random set of point indices
    ind = ind(1:opts);  % ... of length opts

    % Add uniform noise of outsigma to the points chosen to be outliers.
    XY(:,ind) = XY(:,ind)  +   sign(rand(2,opts)-.5).*(rand(2,opts)+1)*outsigma;

    % Perform RANSAC fitting of the line
    [V, P, inliers] = ransacfit2Dline(XY, t, feedback);

    if(feedback)
        disp(['Number of Inliers: ' num2str(length(inliers)) ]);
    end

    % We want to plot the inlier points blue, with the outlier points in
    % red.  In order to do that, we must find the outliers.
    % Use setxor on all the points, and the inliers to find outliers
    %  (plotting all the points in red and then plotting over them in blue
    %  does not work well)
    oulier_points = setxor(transpose(XY), transpose(XY(:, inliers)), 'rows');
    oulier_points = oulier_points';

    % Display the cloud of outlier points
    figure(1); clf
    hold on;
    plot(oulier_points(1,:),oulier_points(2,:) , 'r*');

    % Plot the inliers as blue points
    plot(XY(1,inliers), XY(2, inliers), 'b*');

    % Display the line formed by the 2 points that gave the
    % line of maximum consensus as a green line
    line(P(1,:), P(2,:), 'Color', 'green', 'LineWidth', 4);

    %Display the line formed by the covariance fitting in magenta
    line(V(1,:), V(2, :), 'Color', 'magenta', 'LineWidth', 5);
    grid('on');
    % box('on');


%  fix_point_iterator solves the fix point of a function
%
%  ::
%
%    [T,itercode,retcode]=fix_point_iterator(iterate_func,T0,options,varargin)
%
%  Args:
%
%     - iterate_func : [func_handle]: a function handle which returns 2
%       elements: The improved value of T and F0, the value of the function
%       whose discrepancy is to be minmized
%
%     - T0 : is the initial guess for the solution
%
%     - options : [struct] with fields
%       - fix_point_explosion_limit : [positive scalar |{1e+6}] : maximum
%         divergence of F0
%       - fix_point_TolFun : [positive scalar |{sqrt(eps)}] : tolerance
%         criterion
%       - fix_point_maxiter : [positive scalar |{1000}] : maximum number of
%         iterations
%       - fix_point_verbose : [true|{false}] : show iterations or not
%       - fix_point_valid_func : [function_handle|{@utils.error.valid}] :
%         function that checks the validity (nans, complex, inf, etc.) of
%         the calculations. The default function checks for complex numbers.
%         If you want to allow for complex numbers, use instead
%         @utils.error.validComplex
%
%     - varargin
%
%  Returns:
%     :
%     - T : final solution
%     - itercode : final number of iterations
%     - retcode : return code
%       - 0 : successful
%       - 21 : maximum number of iterations reached
%       - 22 : nan or inf in F0 or T
%       - 23 : divergence
%
%  Note:
%
%  Example:
%
%     See also:
%


function y = inverseGammaPdf(x,alpha,beta)
% The inverse Gamma PDF.
% INPUT
% x  (>0)
% alpha - shape parameter
% beta  - scale parameter
% OUTPUT
% y = the pdf [nrX nrBeta]
% BK - 2018

[nrX,nrBetaInX] = size(x);
[nrBeta,nrXInBeta] = size(beta);
if nrXInBeta<nrBetaInX
    beta =repmat(beta,[1 nrBetaInX]);
end

if nrX==1 && nrBeta>1
    x = repmat(x,[1 nrBeta]);
end

z = x<0;
x(z) =NaN;
y = (beta.^alpha)./gamma(alpha).*(1./x).^(alpha+1).*exp(-beta./x);
y(z) = 0;
end


%% RPCA demo using TFOCS
% This is a demo of Robust PCA (RPCA) using <http://tfocs.stanford.edu TFOCS>. Since 2009,
% there has been much interest in this specific RPCA formulation
% (RPCA can refer to many different formulations; we will state
% our formulation precisely below), but most solvers
% assume equality constraints. One great feature of TFOCS
% is how easy it is to prototype new formulations, so we show
% here a formulation using inequality constraints.
%
% The basic (equality constraint) RPCA formulation is:
% ( see <http://perception.csl.uiuc.edu/matrix-rank/references.html here>
% for references )
%
% $$ \min_{S,L} \|L\|_* + \lambda \|S\|_{\ell_1} \quad\textrm{subject to}\;L+S = X $$
%
% The idea is that some object $X$ can be broken into two components $X=L+S$,
% where $L$ is low-rank and $S$ is sparse.  To tease out this separation,
% RPCA penalizes $\|L\|_*$ (the nuclear norm of $L$), which is a proxy for
% the rank of $L$.  We also penalizes $\|S\|_{\ell_1}$ which is just the sum
% of the absolute value of all the entries of the matrix $S$; this is a proxy
% for the number of non-zero entries of $S$.
%
% The parameter $\lambda$ is important, as it controls how much weight
% to put on $S$ relative to $L$.  This parameter is not overly sensitive,
% and we picked the value used in this demo by a few quick rounds of trial-and-error.
%
% _Code by Stephen Becker, May 2011_

%% Inequality constraints
% In this demo, $X$ is a video. In particular, each column of $X$ is the set
% of $m n$ pixels from a $m \times n$ pixel frame of the video, reshaped
% so that it is now a vector instead of a matrix.
%
% The particular video we use is not compressed using a video codec, so
% each frame of the video is stored. This leads to huge files, so to save
% a bit of space, the value of each pixel is stored as an 8 bit number,
% which means we can think of it as a value from $0,1,2,\ldots,255$.  Now,
% we may imagine that there is some "true" video where each pixel
% is a _real_ number between $[0,255]$, and that we see a quantized version of it.
% It would be nice to apply RPCA to this true version instead of applying
% it to the quantized version.  In particular, it is unlikely that the quantized
% version can be split nicely into a low-rank and sparse component.
% To account for this quantization, we will ask that $S+L$ is not exactly equal
% to $X$, but rather that $S+L$ agrees with $X$ up to the precision
% of the quantization.  The quantization can induce at most an error of .5 in
% the pixel value (e.g. true value is 5.6, which is rounded to 6.0, so an error of .4;
% or a true value of 6.4, which is also rounded to 6.0, so also an error of 0.4).
% A nice way to capture this is via the $\ell_\infty$ norm.
%
% So, the inequality constrained version of RPCA uses the same objective
% function, but instead of the constraints $L+S=X$, the constraints are
%
% $$ \|L+S-X\|_{\ell_\infty} \le 0.5. $$

%% Numerical demo
% We demonstrate this on a video clip taken from a surveillance
% camera in a subway station.  Not only is there a background
% and a foreground (i.e. people), but there is an escalator
% with periodic motion.  Conventional background subtraction
% algorithms would have difficulty with the escalator, but
% this RPCA formulation easily captures it as part of the low-rank
%   structure.
% The clip is taken from the data at this website (after
% a bit of processing to convert it to grayscale):
% http://perception.i2r.a-star.edu.sg/bk_model/bk_index.html

% Load the data:
load escalator_data % contains X (data), m and n (height and width)
X = double(X);

% addpath ~/Dropbox/TFOCS/   % add TFOCS to your path, so change this to suit your computer

%%
nFrames     = size(X,2);

lambda  = 1e-2;

opts = [];
opts.stopCrit       = 4;
opts.printEvery     = 1;
opts.tol            = 1e-4;

opts.maxIts         = 25;

opts.errFcn{1}      = @(f,d,p) norm(p{1}+p{2}-X,'fro')/norm(X,'fro');

largescale      = false;

for inequality_constraints = 0:1

    if inequality_constraints
        % if we already have equality constraint solution,
        %   it would make sense to "warm-start":
%         x0      = { LL_0, SS_0 };
        % but it's more fair to start all over:
        x0      = { X, zeros(size(X))   };
        z0      = [];
    else
        x0      = { X, zeros(size(X))   };
        z0      = [];
    end

    obj    = { prox_nuclear(1,largescale), prox_l1(lambda) };
    affine = { 1, 1, -X };

    mu = 1e-4;
    if inequality_constraints
        epsilon  = 0.5;
        dualProx = prox_l1(epsilon);
    else
        dualProx = proj_Rn;
    end

    tic
    % call the TFOCS solver:
    [x,out,optsOut] = tfocs_SCD( obj, affine, dualProx, mu, x0, z0, opts);
    toc

    % save the variables
    LL = x{1};
    SS = x{2};
    if ~inequality_constraints
        z0      = out.dual;
        LL_0    = LL;
        SS_0    = SS;
    end

end % end loop over "inequality_constriants" variable

%% show all together in movie format
% If you run this in your own computer, you can see the movie. On the webpage,
%   we have a youtube version of the video.
% The top row is using equality constraints, and the bottom row
% is using inequality constraints.
%  The first column of both rows is the same (i.e. it is the original image).
mat  = @(x) reshape( x, m, n );
figure();
colormap( 'Gray' );
k = 1;
for k = 1:nFrames

    imagesc( [mat(X(:,k)), mat(LL_0(:,k)),  mat(SS_0(:,k)); ...
              mat(X(:,k)), mat(LL(:,k)),    mat(SS(:,k))  ] );

    axis off
    axis image

    drawnow;
    pause(.05);

    if k == round(nFrames/2)
        snapnow; % Take a single still snapshot for publishing the m file to html format
    end
end

%% Is there a difference between the two versions?
% Compare the equality constrained version and the inequality
%   constrained version.  To do this, we can check whether
%   the "L" components are really low rank
%   and whether the "S" components are really sparse.
% Even if the two versions appear visually similar, the variables
%   may behave quite differently with respect to low-rankness
%   and sparsity.

fprintf('"S" from equality constrained version has  \t%.1f%% nonzero entries\n',...
    100*nnz(SS_0)/numel(SS_0) );
fprintf('"S" from inequality constrained version has  \t%.1f%% nonzero entries\n',...
    100*nnz(SS)/numel(SS) );

s  = svd(LL);    % inequality constraints
s0 = svd(LL_0);  % equality constraints

fprintf('"L" from equality constrained version has numerical rank\t %d (of %d possible)\n',...
    sum( s0>1e-6), min( m*n, nFrames)  );
fprintf('"L" from inequality constrained version has numerical rank\t %d (of %d possible)\n',...
    sum( s > 1e-6), min( m*n, nFrames)  );

figure();
semilogy( s0 ,'o-')
hold all;
semilogy( s ,'o-')
legend('Equality constraints','Inequality constraints');
xlabel('sorted singular value location');
ylabel('value of singular value');
title('Comparison of RPCA using equality and inequality constraints');

%%


function q = p03_q ( m, c, w )

%*****************************************************************************80
%
%% P03_Q evaluates the integral for problem p03.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Genz,
%    A Package for Testing Multiple Integration Subroutines,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast and Graeme Fairweather,
%    Reidel, 1987, pages 337-340,
%    ISBN: 9027725144,
%    LC: QA299.3.N38.
%
%  Parameters:
%
%    Input, integer M, the dimension of the argument.
%
%    Input, real C(M), W(M), the problem parameters.
%
%    Output, real Q, the integral.
%
  c = c(:);
%
%  Here, we need to generate all possible DIM_NUM tuples with
%  values of 0 or 1.
%
  q = 0.0;
  rank = 0;
  ivec = zeros ( m, 1 );

  while ( 1 )

    [ rank, ivec ] = tuple_next ( 0, 1, m, rank, ivec );

    if ( rank == 0 )
      break
    end

    s = sum ( ivec(1:m) );

    q = q + r8_mop ( s ) / ( 1.0 + c(1:m,1)' * ivec(1:m,1) );

  end

  q = q / ( r8_factorial ( m ) * prod ( c(1:m) ) );

  return
end


function hpp_test03 ( )

%*****************************************************************************80
%
%% HPP_TEST03 tests HEPP_VALUE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HPP_TEST03:\n' );
  fprintf ( 1, '  HePP_VALUE evaluates a Hermite product polynomial.\n' );
  fprintf ( 1, '  POLYNOMIAL_VALUE evaluates a polynomial.\n' );

  m = 3;
  n = 1;
  seed = 123456789;
  [ x, seed ] = r8vec_uniform_ab ( m, -1.0, +1.0, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Evaluate at X = ( %g, %g, %g )\n', x(1:3) )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Rank  I1  I2  I3:  He(I1,X1)*He(I2,X2)*He(I3,X3)  P(X1,X2,X3)\n' );
  fprintf ( 1, '\n' );

  for rank = 1 : 20

    l = comp_unrank_grlex ( m, rank );
%
%  Evaluate the HePP directly.
%
    v1 = hepp_value ( m, n, l, x );
%
%  Convert the HePP to a polynomial, and reevaluate.
%
    o_max = prod ( floor ( ( l(1:m) + 2 ) / 2 ) );

    [ o, c, e ] = hepp_to_polynomial ( m, l, o_max );

    v2 = polynomial_value ( m, o, c, e, n, x );
%
%  Compare results.
%
    fprintf ( 1, '  %4d  %2d  %2d  %2d  %12g  %12g\n', rank, l(1:m), v1, v2 );

  end

  return
end


function v_cheb = legcoeffs2chebvals(c_leg, varargin)
%LEGCOEFFS2CHEBVALS  Convert Legendre coefficients to Chebyshev values.
%   V_CHEB = LEGCOEFFS2CHEBVALS(C_LEG) converts the vector C_LEG of Legendre
%   coefficients to a vector V_CHEB of values at second-kind Chebyshev points.
%
%   V_CHEB = LEGCOEFFS2CHEBVALS(C_LEG, 1) is similar, but evaluates at
%   first-kind Chebyshev points.

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

% Parse inputs:
v1 = {}; v2 = {};
for k = 1:numel(varargin)
    if ( isnumeric(varargin{k}) )
        v1{1} = varargin{k};
    elseif ( ischar(varargin{k}) )
        v2{1} = varargin{k};
    end
end

c_cheb = leg2cheb(c_leg, v2{:}); % Convert to Chebyshev coefficients.
v_cheb = chebcoeffs2chebvals(c_cheb, v1{:});  % Convert to Chebyshev values.

end


function point_num = levels_index_size_own ( dim_num, level_max )

%*****************************************************************************80
%
%% LEVELS_INDEX_SIZE_OWN sizes a sparse grid made from OWN 1D rules.
%
%  Discussion:
%
%    The sparse grid is presumed to have been created from products
%    of OPEN WEAKLY NESTED 1D quadrature rules.
%
%    OWN rules include Gauss Hermite and Gauss Legendre.
%
%    The sparse grid is the logical sum of product grids with total LEVEL
%    between LEVEL_MIN and LEVEL_MAX.
%
%    Oddly enough, in order to count the number of points, we will
%    behave as though LEVEL_MIN was zero.  This is because our computation
%    concentrates on throwing away all points generated at lower levels,
%    but, in fact, if we start at a nonzero level, we need to include
%    on that level all the points that would have been generated on lower
%    levels.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Output, integer POINT_NUM, the number of points in the grid.
%

%  Special case.
%
  if ( level_max == 0 )
    point_num = 1;
    return
  end
%
%  The outer loop generates LEVELs from LEVEL_MIN to LEVEL_MAX.
%
%  The normal definition of LEVEL_MIN:
%
%   level_min = max ( 0, level_max + 1 - dim_num )
%
%  Our somewhat artificial temporary local definition of LEVEL_MIN:
%
  if ( dim_num == 1 )
    level_min = level_max;
    point_num = 1;
  else
    level_min = 0;
    point_num = 0;
  end

  for level = level_min : level_max
%
%  The middle loop generates the next partition that adds up to LEVEL.
%
    level_1d = [];
    more = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ level_1d, more, h, t ] = comp_next ( level, dim_num, level_1d, more, h, t );
%
%  Transform each 1D level to a corresponding 1D order.
%
      order_1d = level_to_order_open ( dim_num, level_1d );

      for dim = 1 : dim_num
%
%  Account for the repetition of the center point.
%
        if ( 1 < order_1d(dim) )
          order_1d(dim) = order_1d(dim) - 1;
        end

      end

      point_num = point_num + prod ( order_1d(1:dim_num) );

      if ( ~more )
        break
      end

    end
  end

  return
end


function test04 ( m, n )

%*****************************************************************************80
%
%% TEST04 estimates integrals in M dimensions, using N points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of evaluation points.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04\n' );
  fprintf ( 1, '  P00_Q returns the integral of F over [0,1]^m.\n' );
  fprintf ( 1, '  Here, we use spatial dimension M = %d\n', m );
  fprintf ( 1, '  The number of sample points is N = %d\n', n );

  seed = 123456789;
  [ x, seed ] = r8mat_uniform_01 ( m, n, seed );

  prob_num = p00_prob_num ( );

  for prob = 1 : prob_num

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Problem %d\n', prob );

    [ c, seed ] = p00_c ( prob, m, seed );
    r8vec_print ( m, c, '  C parameters:' );

    [ w, seed ] = p00_w ( prob, m, seed );
    r8vec_print ( m, w, '  W parameters:' );

    fprintf ( 1, '\n' );
    fprintf ( 1, '      Exact Integral     Q\n' );
    fprintf ( 1, '\n' );

    q1 = p00_q ( prob, m, c, w );

    f = p00_f ( prob, m, c, w, n, x );
    q2 = sum ( f(1:n) ) / n;

    fprintf ( 1, '  %14g  %14g\n', q1, q2 );

  end

  return
end


function basis_brick8_test ( )

%*****************************************************************************80
%
%% BASIS_BRICK8_TEST verifies BASIS_BRICK8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  node_num = 8;
  test_num = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BASIS_BRICK8_TEST:\n' );
  fprintf ( 1, '  Verify basis functions for element BRICK8.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', node_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The basis function values at basis nodes\n' );
  fprintf ( 1, '  should form the identity matrix.\n' );
  fprintf ( 1, '\n' );

  n = node_num;

  p = nodes_brick8 ( );

  phi = basis_brick8 ( n, p );

  for j = 1 : n
    fprintf ( 1, '  ' );
    for i = 1 : node_num
      fprintf ( 1, '%7.3f', phi(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The basis function values at ANY point P\n' );
  fprintf ( 1, '  should sum to 1:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    ------------P-------------     PHI_SUM\n' );
  fprintf ( 1, '\n' );

  n = test_num;

  seed = 123456789;

  [ p, seed ] = r8mat_uniform_01 ( 3, n, seed );

  phi = basis_brick8 ( n, p );

  for j = 1 : n
    phi_sum = sum ( phi(1:node_num,j) );
    fprintf ( 1, '  %8.4f  %8.4f  %8.4f  %8.4f\n',  p(1:3,j), phi_sum );
  end

  return
end


function a = chow_inverse ( alpha, beta, n )

%*****************************************************************************80
%
%% CHOW_INVERSE returns the inverse of the CHOW matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the ALPHA value.  A typical value is 1.0.
%
%    Input, real BETA, the BETA value.  A typical value is 0.0.
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  if ( 0.0 == alpha & beta == 0.0 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'CHOW_INVERSE - Fatal error!\n' );
    fprintf ( 1, '  The Chow matrix is not invertible, because\n' );
    fprintf ( 1, '  ALPHA = 0 and BETA = 0.\n' );
    error ( 'CHOW_INVERSE - Fatal error!' );

  elseif ( 0.0 == alpha & beta ~= 0.0 )

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

        if ( i <= j )
          a(i,j) = (-1)^(j-i) / beta^(j+1-i);
        else
          a(i,j) = 0.0;
        end

      end
    end

    return

  elseif ( 0.0 ~= alpha & beta == 0.0 )

    if ( 1 < n )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CHOW_INVERSE - Fatal error!\n' );
      fprintf ( 1, '  The Chow matrix is not invertible, because\n' );
      fprintf ( 1, '  BETA = 0 and 1 < N.\n' );
      error ( 'CHOW_INVERSE - Fatal error!' );
    end

    a(1,1) = 1.0 / alpha;

    return

  end

  d(0+1) = 1.0;
  d(1+1) = beta;
  for i = 2 : n
    d(i+1) = beta * d(i-1+1) + alpha * beta * d(i-2+1);
  end

  dp(-1+2) = 1.0 / beta;
  dp(0+2) = 1.0;
  dp(1+2) = alpha + beta;
  for i = 2 : n
    dp(i+2) = d(i+1) + alpha * d(i-1+1);
  end

  for i = 1 : n
    for j = 1 : i-1
      a(i,j) = - alpha * ( alpha * beta )^(i-j) * dp(j-2+2) * d(n-i+1) ...
        / dp(n+2);
    end
    for j = i : n
      a(i,j) = (-1)^(i+j) * dp(i-1+2) * d(n+1-j+1) / ( beta * dp(n+2) );
    end
  end

  return
end


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

% Script chap::2::script
% Density Bachelier Model

t = 5;                                     % maturity
x0 = 100;                                  % spot value
r= 0;                                       % riskless rate
q=0;                                        % dividend yield

figname = 'Risk Neutral Density - Bachelier';

x = 95:.1:105;                            % range

sigma_base = 0.2;                           % volatility base scenario

y_base = pg(x, x0, t, r, q, sigma_base);
legendname_base = 'Base';
%% Changing sigma_base
sigma_low = 0.15;
sigma_high = 0.25;

y_low = pg(x, x0, t, r, q, sigma_low);
y_high = pg(x, x0, t, r, q, sigma_high);

legendname_low = 'Changing \sigma low value';
legendname_high = 'Changing \sigma high value';

createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);

x = 95:.1:105;                            % range

sigma_base = 0.2;                           % volatility base scenario

y_base = pg(x, x0, t, r, q, sigma_base);
legendname_base = 'Base';


function pass = test_promote_functional(~)

% Test operator + functional and operator*functional.

tol = 1e-10;

%%
N = chebop(@(u) diff(u,2) + sum(u));
N.lbc = 0;
N.rbc = 0;
u = N\1;
pass(1) = norm(N(u) - 1, inf) < tol;

%%
N = chebop(@(u) diff(u,2) + u.*sum(u));
N.lbc = 0;
N.rbc = 0;
u = N\1;
pass(2) = norm(N(u) - 1, inf) < tol;

%%
N = chebop(@(x,u,v) [diff(u) ; diff(v)]);
N.bc = @(x,u, v) [u(0) - 1; u(0).*v(.5)];
N.init = [1 ; 0];
pass(3) = size(matrix(linearize(N), 3), 1) == 8;
u = N\1;
pass(4) = norm(N(u) - 1, inf) < tol;

%%

N = chebop(@(x,u,v) [diff(u) ; diff(v) + v.*sum(u) + u(.3)]);
N.bc = @(x,u, v) [u(0)-1; u(0).*v(.5)];
pass(5) = size(matrix(linearize(N), 3), 1) == 8;
u = N\1;
pass(6) = norm(N(u) - 1, inf) < tol;

%% Periodic (with values):

N = chebop(@(u) diff(u,2) + sum(u));
N.bc = 'periodic';
rhs = chebfun(@(x) cos(pi*x));
u = N\rhs;
pass(7) = norm(N(u) - rhs, inf) < tol;
pass(8) = isequal(get(u.funs{1}, 'tech'), @trigtech);

%% Periodic (with coeffs):

N = chebop(@(u) diff(u,2) + sum(u));
N.bc = 'periodic';
rhs = chebfun(@(x) cos(pi*x));
pref = cheboppref();
pref.discretization = 'coeffs';
u = solvebvp(N, rhs, pref);
pass(9) = norm(N(u) - rhs, inf) < tol;
pass(10) = isequal(get(u.funs{1}, 'tech'), @trigtech);

end


classdef DifferentialDrive < simiam.robot.dynamics.Dynamics

% Copyright (C) 2013, Georgia Tech Research Corporation
% see the LICENSE file included with this software

    properties
        wheel_radius
        wheel_base_length
    end

    methods
        function obj = DifferentialDrive(wheel_radius, wheel_base_length)
            obj = obj@simiam.robot.dynamics.Dynamics();
            obj.wheel_radius = wheel_radius;
            obj.wheel_base_length = wheel_base_length;
        end

        function pose_t_1 = apply_dynamics(obj, pose_t, dt, vel_r, vel_l)
            R = obj.wheel_radius;
            L = obj.wheel_base_length;

%             fprintf('(vel_r,vel_l) = (%0.6g,%0.6g)\n', vel_r,vel_l);

            v = R/2*(vel_r+vel_l);
            w = R/L*(vel_r-vel_l);

%             fprintf('Calculated velocities (v,w): (%0.3g,%0.3g)\n', v, w);

            [x_k, y_k, theta_k] = pose_t.unpack();

            options = odeset('RelTol',1e-8,'AbsTol',1e-8);
            [t,z] = ode23(@obj.dynamics, [0 dt], [x_k, y_k, theta_k, v, w], options);

%             x_k_1 = x_k + dt*(v*cos(theta_k));
%             y_k_1 = y_k + dt*(v*sin(theta_k));
%             theta_k_1 = theta_k + dt*w;

%             pose_t_1 = simiam.ui.Pose2D(x_k_1, y_k_1, theta_k_1);
            pose_t_1 = simiam.ui.Pose2D(z(end,1),z(end,2),z(end,3));
        end

        function dz = dynamics(obj, t, z)
            dz = zeros(5,1);
            dz(1:2) = z(4)*[cos(z(3));sin(z(3))];
            dz(3) = z(5);
        end

        function [vel_r,vel_l] = uni_to_diff(obj,v,w)
            R = obj.wheel_radius;
            L = obj.wheel_base_length;

            %% START CODE BLOCK %%
            vel_r = (2*v+w*L)/(2*R);
            vel_l = (2*v-w*L)/(2*R);
            %% END CODE BLOCK %%
        end

        function [v,w] = diff_to_uni(obj,r,l)
            R = obj.wheel_radius;
            L = obj.wheel_base_length;

            v = R/2*(r+l);
            w = R/L*(r-l);
        end
    end

end


function det = r8plu_det ( n, pivot, lu )

%*****************************************************************************80
%
%% R8PLU_DET computes the determinant of an R8PLU matrix.
%
%  Discussion:
%
%    The matrix should have been factored by R8MAT_TO_R8PLU.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer PIVOT(N), the pivot vector computed by R8MAT_TO_R8PLU.
%
%    Input, real LU(N,N), the LU factors computed by R8MAT_TO_R8PLU.
%
%    Output, real DET, the determinant of the matrix.
%
  det = 1.0;

  for i = 1 : n
    det = det * lu(i,i);
    if ( pivot(i) ~= i )
      det = -det;
    end
  end

  return
end


function q = Qinv(p)

% QINV  quaternion inverse
%
%   Q = QINV(P) returns a quaternion Q which is the quaternion inverse of
%     quaternion P.
%     - P is a quaternion. It is a 4-vector or a 4*N array (column i
%        represents quaternion i) where N is the number of quaternions.
%     - Q is the quaternion inverse of quaternion P. It is a 4*N array.
%
% See also DQINV, QCONJ, DQCONJ, QNORM

sp = size(p);
if sp == [1 4], p = p'; sp = size(p); end

% wrong format
if sp(1) ~= 4
    error('DualQuaternion:Qinv:wrongsize',...
        '%d rows in the P array. It should be 4.',sp(1));
end

normp = Qnorm(p);
if min(normp) == 0
     error('DualQuaternion:Qinv:noInverse',...
        'One of the quaternions has 0 norm. Therefore it has no inverse');
end
q = Qconj(p)./repmat(normp.^2,4,1);


% this function solves the odometry calibration problem
% given a measurement matrix Z.
% We assume that the information matrix is the identity
% for each of the measurements
% Every row of the matrix contains
% z_i = [u'x, u'y, u'theta, ux, uy, ytheta]
% Z:	The measurement matrix
% X:	the calibration matrix
% returns the correction matrix X
function X = ls_calibrate_odometry(Z)
  % initial solution (the identity transformation)
  X = eye(3);

  % TODO: initialize H and b of the linear system
  H = zeros(9);
  b = zeros(9,1);
  omega = eye(3);

  % TODO: loop through the measurements and update H and b
  % You may call the functions error_function and jacobian, see below
  % We assume that the information matrix is the identity.
  for i=1:size(Z,1)
	H = H + jacobian(i,Z)'*omega*jacobian(i,Z);
	b = b + jacobian(i,Z)'*omega'*error_function(i,X,Z);
  endfor
  % TODO: solve and update the solution
  delX = -H\b;
  X = X + [delX(1:3)';delX(4:6)';delX(7:9)'];
end

% this function computes the error of the i^th measurement in Z
% given the calibration parameters
% i:	the number of the measurement
% X:	the actual calibration parameters
% Z:	the measurement matrix, each row contains first the scan-match result
%       and then the motion reported by odometry
% e:	the error of the ith measurement
function e = error_function(i, X, Z)
  % TODO compute the error of each measurement
  e = Z(i,1:3)' - X*Z(i,4:6)';
end

% derivative of the error function for the ith measurement in Z
% i:	the measurement number
% Z:	the measurement matrix
% J:	the jacobian of the ith measurement
function J = jacobian(i, Z)
  % TODO compute the Jacobian
  J = -[Z(i,4) Z(i,5) Z(i,6) zeros(1,6); zeros(1,3) Z(i,4) Z(i,5) Z(i,6) zeros(1,3); zeros(1,6) Z(i,4) Z(i,5) Z(i,6)];
end


function [ aj, bj, zemu ] = class_matrix ( kind, m, alpha, beta )

%*****************************************************************************80
%
%% CLASS_MATRIX computes the Jacobi matrix for a quadrature rule.
%
%  Discussion:
%
%    This routine computes the diagonal AJ and subdiagonal BJ
%    elements of the order M tridiagonal symmetric Jacobi matrix
%    associated with the polynomials orthogonal with respect to
%    the weight function specified by KIND.
%
%    For weight functions 1-7, M elements are defined in BJ even
%    though only M-1 are needed.  For weight function 8, BJ(M) is
%    set to zero.
%
%    The zero-th moment of the weight function is returned in ZEMU.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev Type 1,     (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,+oo)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-oo,+oo)   |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,+oo)     (x-a)^alpha*(x+b)^beta
%    9, Chebyshev Type 2,     (a,b)       ((b-x)*(x-a))^(+0.5)
%
%    Input, integer M, the order of the Jacobi matrix.
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Output, real AJ(M), BJ(M), the diagonal and subdiagonal
%    of the Jacobi matrix.
%
%    Output, real ZEMU, the zero-th moment.
%
  temp = eps;

  parchk ( kind, 2 * m - 1, alpha, beta );

  temp2 = 0.5;

  if ( 500.0 * temp < abs ( ( gamma ( temp2 ) )^2 - pi ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CLASS - Fatal error!\n' );
    fprintf ( 1, '  Gamma function does not match machine parameters.\n' );
    error ( 'CLASS - Fatal error!' );
  end

  bj = zeros(m,1);
  aj = zeros(m,1);

  if ( kind == 1 )

    ab = 0.0;

    zemu = 2.0 / ( ab + 1.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      abi = i + ab * mod ( i, 2 );
      abj = 2 * i + ab;
      bj(i) = abi * abi / ( abj * abj - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 2 )

    zemu = pi;

    aj(1:m) = 0.0;

    bj(1) =  sqrt ( 0.5 );
    bj(2:m) = 0.5;

  elseif ( kind == 3 )

    ab = alpha * 2.0;
    zemu = 2.0^( ab + 1.0 ) * gamma ( alpha + 1.0 )^2 ...
      / gamma ( ab + 2.0 );

    aj(1:m) = 0.0;
    bj(1) = 1.0 / ( 2.0 * alpha + 3.0 );
    for i = 2 : m
      bj(i) = i * ( i + ab ) / ( 4.0 * ( i + alpha )^2 - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 4 )

    ab = alpha + beta;
    abi = 2.0 + ab;
    zemu = 2.0^( ab + 1.0 ) * gamma ( alpha + 1.0 ) ...
      * gamma ( beta + 1.0 ) / gamma ( abi );
    aj(1) = ( beta - alpha ) / abi;
    bj(1) = 4.0 * ( 1.0 + alpha ) * ( 1.0 + beta ) ...
      / ( ( abi + 1.0 ) * abi * abi );
    a2b2 = beta * beta - alpha * alpha;

    for i = 2 : m
      abi = 2.0 * i + ab;
      aj(i) = a2b2 / ( ( abi - 2.0 ) * abi );
      abi = abi^2;
      bj(i) = 4.0 * i * ( i + alpha ) * ( i + beta ) * ( i + ab ) ...
        / ( ( abi - 1.0 ) * abi );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 5 )

    zemu = gamma ( alpha + 1.0 );

    for i = 1 : m
      aj(i) = 2.0 * i - 1.0 + alpha;
      bj(i) = i * ( i + alpha );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 6 )

    zemu = gamma ( ( alpha + 1.0 ) / 2.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      bj(i) = ( i + alpha * mod ( i, 2 ) ) / 2.0;
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 7 )

    ab = alpha;
    zemu = 2.0 / ( ab + 1.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      abi = i + ab * mod(i,2);
      abj = 2 * i + ab;
      bj(i) = abi * abi / ( abj * abj - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 8 )

    ab = alpha + beta;
    zemu = gamma ( alpha + 1.0 ) * gamma ( - ( ab + 1.0 ) ) ...
      / gamma ( - beta );
    apone = alpha + 1.0;
    aba = ab * apone;
    aj(1) = - apone / ( ab + 2.0 );
    bj(1) = - aj(1) * ( beta + 1.0 ) / ( ab + 2.0 ) / ( ab + 3.0 );
    for i = 2 : m
      abti = ab + 2.0 * i;
      aj(i) = aba + 2.0 * ( ab + i ) * ( i - 1 );
      aj(i) = - aj(i) / abti / ( abti - 2.0 );
    end

    for i = 2 : m - 1
      abti = ab + 2.0 * i;
      bj(i) = i * ( alpha + i ) / ( abti - 1.0 ) * ( beta + i ) ...
        / ( abti^2 ) * ( ab + i ) / ( abti + 1.0 );
    end

    bj(m) = 0.0;
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 9 )

    zemu = pi /  2.0;

    aj(1:m) = 0.0;

    bj(1:m) = 0.5;

  end

  return
end


function pass = test_interp1(pref)

%% Linear interpolation:
% Test a scalar function:
seedRNG(6178);
x = (0:10).';
y = sin(x);
f = chebfun.interp1(x, y, 'linear');
tol = 10*eps;
pass(1) = norm(feval(f, x) - y) < tol;
pass(2) = numel(f.funs) == 10;
pass(3) = length(f) == 20;

% Test an array-valued function:
x = (0:10)';
y = [sin(x), cos(x)];
f = chebfun.interp1(x, y, 'linear');
tol = 10*eps;
pass(4) = norm(feval(f, x) - y) < tol;
pass(5) = numel(f.funs) == 10;
pass(6) = length(f) == 20;

% Test a different domain:
dom = [.01, 10.01];
x = (0:10)';
y = [sin(x), cos(x)];
f = chebfun.interp1(x, y, 'linear', dom);
err = norm(feval(f, x(2:end-1)) - y(2:end-1,:));
tol = 10*eps;
pass(7) = all(f.domain == [dom(1), 1:10, dom(2)]);
pass(8) = err < tol;
pass(9) = numel(f.funs) == 11;
pass(10) = length(f) == 22;

%% Polynomial interpolation:
% Test an scalar-valued function:
x = (0:10)';
y = sin(x);
f = chebfun.interp1(x, y);
tol = 100*eps;
pass(11) = norm(feval(f, x) - y) < tol;
pass(12) = numel(f.funs) == 1;
pass(13) = length(f) == length(x);

% Test an array-valued function:
x = (0:10)';
y = [sin(x), cos(x)];
f = chebfun.interp1(x, y);
tol = 100*eps;
pass(14) = norm(feval(f, x) - y) < tol;
pass(15) = numel(f.funs) == 1;
pass(16) = length(f) == length(x);

% Test a different domain:
dom = [.01, 10.01];
f = chebfun.interp1(x, y, dom);
tol = 100*eps;
pass(17) = norm(feval(f, x) - y) < tol;
pass(18) = numel(f.funs) == 1;
pass(19) = length(f) == length(x);
pass(20) = all(f.domain == dom);

%% Check that things work for row vector input.
x = (0:10).';
y = sin(x);
f1 = chebfun.interp1(x.', y, 'linear');
f2 = chebfun.interp1(x, y.', 'linear');
f3 = chebfun.interp1(x.', y.', 'linear');
pass(21) = (norm(feval(f1, x) - y) < 10*vscale(f1)*eps) && ...
           (norm(feval(f2, x) - y) < 10*vscale(f2)*eps) && ...
           (norm(feval(f3, x) - y) < 10*vscale(f3)*eps);

% Test random points:
x = rand(11,1);
y = sin(x);
f = chebfun.interp1(x, y, 'linear');
tol = 10*eps;
pass(22) = norm(feval(f, x) - y) < tol;
pass(23) = numel(f.funs) == 10;
pass(24) = length(f) == 20;

% Test a chebfun
x = chebfun('x', [0, 1]);
y = exp(x);
x = rand(11, 1);
f = chebfun.interp1(x, y, 'poly', [0, 1]);
tol = 100*eps;
pass(25) = norm(f(x)-y(x), inf) < tol;

%% Test trigonometric interpolation:
x = (0:10)';
y = cos(pi*x);
f = chebfun.interp1(x, y, 'trig');
tol = 100*eps;
pass(26) = norm(feval(f, x(2:end-1)) - y(2:end-1)) < tol && ...
    norm(feval(f, x(1))-(y(1)+y(end))/2) < tol ;
pass(27) = numel(f.funs) == 1;
pass(28) = length(f) == length(x)-1;

%%
% Test an array-valued function:
x = (0:10)';
y = [exp(sin(x)), cos(pi*x)];
f = chebfun.interp1(x, y, 'trig', [0, 11]);
tol = 100*eps;
pass(29) = norm(feval(f, x) - y) < tol;
pass(30) = numel(f.funs) == 1;
pass(31) = length(f) == length(x);
%%
% Test a different domain:
dom = [-.01, 10.01];
f = chebfun.interp1(x, y, dom);
tol = 1e3*eps;
pass(32) = norm(feval(f, x) - y) < tol;
pass(33) = numel(f.funs) == 1;
pass(34) = length(f) == length(x);
pass(35) = all(f.domain == dom);

end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% The  command invfreqs

%verification of the result of the command freqs
num=[ -2/j^2 7 0];
den=[3 0 2];
w=0:.1:10;
H=freqs(num,den,w);
freqs(num,den,w)
[num1,den1]=invfreqs(H,w,2,2)

%Equivalent frequency responses
[num2,den2]=invfreqs(H,w,3,3)
freqs(num2,den2,w)

figure
[num3,den3]=invfreqs(H,w,10,7);
freqs(num3,den3,w)


%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

%%Transformation of the end effector frame following the RPY angles in
%%closed form
%%input: rotation angles along y,z,x axis expressed in degrees

function [RPYmat]=RPY(fia,fio,fin)

RPYmat=[cosd(fia)*cosd(fio),cosd(fia)*sind(fin)*sind(fio)-sind(fia)*cosd(fin),cosd(fia)*sind(fio)*cosd(fin)+sind(fia)*sind(fin),0;
        sind(fia)*cosd(fio),sind(fia)*sind(fio)*sind(fin)+cosd(fia)*cosd(fin),sind(fia)*sind(fio)*cosd(fin)-cosd(fia)*sind(fin),0;
        -sind(fio),cosd(fio)*sind(fin),cosd(fio)*cosd(fin),0;
        0,0,0,1];

end


function imResult = blendMode_SoftLight(A, B, offsetW, offsetH)
%% Soft Light blending mode: combines Multiply and Screen blend modes.
%   This is a softer version of Hard Light.
%
% Input:
%       A       -       Base Image
%       B       -       Top Image
%   offsetW     -   move picture B horizontally in respect to the top-left
%                   corner of picture A. Default value = 1.
%   offsetH     -   move picture B vertically in respect to the top-left
%                   corner of picture A. Default value = 1.
%
% Output:
%       imResult    -   Result of the blending, having the same size of the
%                       Base Image A.
%

%% Check Input
a = size(A);
b = size(B);
blendMode_checkInput(nargin, a, b, func2str(@blendMode_SoftLight));

if nargin < 3
    offsetW = 1;
    offsetH = 1;
end

if nargin < 4
    offsetH = 1;
end

%% Implementation
imResult = A;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    [A, B] = blendMode_ResizeImages(A, B, a, b, offsetW, offsetH);
end

ind = B < 0.5;
indCompl = abs(ind - 1);

tmp1 = 2 * A .* B + A.^2 .* (1 - 2 * B);
tmp2 = 2 * A .* (1 - B) + sqrt(A) .* (2 * B - 1);
C = ind .* tmp1 + indCompl .* tmp2;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    imResult = blendMode_CreateResult(imResult, C, offsetW, offsetH);
else
    imResult = C;
end

end


% Test file for bndfun/cumsum.m

function pass = test_cumsum(pref)

% Get preferences.
if ( nargin < 1 )
    pref = chebfunpref();
end

% Set a domain
dom = [-2 7];
a = dom(1);

% Generate a few random points to use as test values.
seedRNG(6178);
x = diff(dom) * rand(100, 1) + dom(1);

%%
% Spot-check antiderivatives for a couple of functions. We also check that
% feval(cumsum(f), a) == 0 each time.

f = bndfun(@(x) exp(x/10) - 1, struct('domain', dom), pref);
F = cumsum(f);
F_ex = @(x) 10*exp(x/10) - x;
err = feval(F, x) - F_ex(x);
pass(1) = (norm(diff(err), inf) < 100*get(f, 'vscale')*eps) ...
    && (abs(feval(F, a)) <= get(f, 'vscale')*eps);

f = bndfun(@(x) 1./(1 + x.^2), struct('domain', dom), pref);
F = cumsum(f);
F_ex = @(x) atan(x);
err = feval(F, x) - F_ex(x);
pass(2) = (norm(diff(err), inf) < 1e3*get(f, 'vscale')*eps) ...
    && (abs(feval(F, a)) <= 1e1*get(f, 'vscale')*eps);

f = bndfun(@(x) cos(1e4*x), struct('domain', dom), pref);
F = cumsum(f);
F_ex = @(x) sin(1e4*x)/1e4;
err = feval(F, x) - F_ex(x);
pass(3) = (norm(diff(err), inf) < 1e3*get(f, 'vscale')*eps) ...
    && (abs(feval(F, a)) <= 1e1*get(f, 'vscale')*eps);

z = exp(2*pi*1i/6);
f = bndfun(@(t) sinh(t*z), struct('domain', dom), pref);
F = cumsum(f);
F_ex = @(t) cosh(t*z)/z;
err = feval(F, x) - F_ex(x);
pass(4) = (norm(diff(err), inf) < 100*get(f, 'vscale')*eps) ...
    && (abs(feval(F, a)) <= get(f, 'vscale')*eps);

%%
% Check that applying cumsum() and direct construction of the antiderivative
% give the same results (up to a constant).

f = bndfun(@(x) sin(4*x).^2, struct('domain', dom), pref);
F = bndfun(@(x) 0.5*x - 0.0625*sin(8*x), struct('domain', dom), pref);
G = cumsum(f);
err = feval(G - F, x);
pass(5) = (norm(diff(err), inf) < 1e2*get(f, 'vscale')*eps) && ...
    (abs(feval(G, a)) < 1e2*get(f, 'vscale')*eps);

%%
% Check that diff(cumsum(f)) == f and that cumsum(diff(f)) == f up to a
% constant.

f = bndfun(@(x) x.*(x - 1).*sin(x), struct('domain', dom), pref);
g = diff(cumsum(f));
tol_f = 10*get(f, 'vscale')*eps;
tol_g = 10*get(g, 'vscale')*eps;

err = feval(f, x) - feval(g, x);
pass(6) = (norm(diff(err), inf) < 1e2*max(tol_f, tol_g));
h = cumsum(diff(f));
err = feval(f, x) - feval(h, x);
pass(7) = (norm(diff(err), inf) < 10*max(tol_f, tol_g) && ...
    (abs(feval(h, a)) < 10*max(tol_f, tol_g)));

%%
% Check operation for array-valued bndfun objects.

f = bndfun(@(x) [sin(x) x.^2 exp(1i*x)], struct('domain', dom), pref);
F_exact = bndfun(@(x) [-cos(x) x.^3/3 exp(1i*x)/1i], struct('domain', dom), ...
    pref);
F = cumsum(f);
err = feval(F, x) - feval(F_exact, x);
pass(8) = (norm(diff(err), inf) < ...
    10*max(get(f, 'vscale')*eps)) && ...
    all(abs(feval(F, a)) < max(get(f, 'vscale')*eps));

%% Test on singular function:
pref.blowup = true;

% Singularity at one endpoint:
dom = [-2 7];
pow = -0.64;
op = @(x) (x-dom(1)).^pow;
data.domain = dom;
data.exponents = [pow 0];
f = bndfun(op, data, pref);
g = cumsum(f);
vals_g = feval(g, x);
g_exact = @(x) (x-a).^(pow+1)./(pow+1);
vals_exact = feval(g_exact, x);
err = vals_g - vals_exact;
pass(9) = ( norm(err, inf) < 1e2*eps*norm(vals_exact, inf) );

% TODO: Singularities at both endpoints. (Note, NH has an idea for an improved
% algorithm for this case. Will add a test once that is implemented.)

end


function out = binsumImage(in, bin)
% Rebins image by summing bin(1)xbin(2) pixels together along x and y
% dimension, respectively. Starts at upper left corner. Rebinned image will
% have floor(size(in)./bin) pixels.
%
% out = binsum(in, bin)
di=0;
if strcmp(class(in),'dip_image')
    di=1;
    in=double(in);
end

npix=ceil(size(in)./bin);
out=zeros(npix);
for xi=1:npix(1)
    startx=(xi-1)*bin(1)+1;
    endx=min(startx+bin(1)-1,size(in,1));
    for yi=1:npix(2)
        starty=(yi-1)*bin(2)+1;
        endy=min(starty+bin(2)-1,size(in,2));
        binsum=sum(sum(in(startx:endx,starty:endy)));
        out(xi,yi)=binsum;
    end
end

if di
    out=dip_image(out);
end


function y = nanvar(x,w,dim)
%Replacement for Matlab NANVAR Variance function, ignoring NaNs.
%
%
%
%

if nargin < 2 || isempty(w), w = 0; end

sz = size(x);
if nargin < 3 || isempty(dim)
    % The output size for [] is a special case when DIM is not given.
    if isequal(x,[]), y = NaN(class(x)); return; end

    % Figure out which dimension sum will work along.
    dim = find(sz ~= 1, 1);
    if isempty(dim), dim = 1; end
elseif dim > length(sz)
    sz(end+1:dim) = 1;
end

% Need to tile the mean of X to center it.
tile = ones(size(sz));
tile(dim) = sz(dim);

if isequal(w,0) || isequal(w,1)
    % Count up non-NaNs.
    n = sum(~isnan(x),dim);

    if w == 0
        % The unbiased estimator: divide by (n-1).  Can't do this when
        % n == 0 or 1, so n==1 => we'll return zeros
        denom = max(n-1, 1);
    else
        % The biased estimator: divide by n.
        denom = n; % n==1 => we'll return zeros
    end
    denom(n==0) = NaN; % Make all NaNs return NaN, without a divideByZero warning

    x0 = x - repmat(nanmean(x, dim), tile);
    y = nansum(abs(x0).^2, dim) ./ denom; % abs guarantees a real result

% Weighted variance
elseif numel(w) ~= sz(dim)
    error('MATLAB:nanvar:InvalidSizeWgts','The length of W must be compatible with X.');
elseif ~(isvector(w) && all(w(~isnan(w)) >= 0))
    error('MATLAB:nanvar:InvalidWgts','W must be a vector of nonnegative weights, or a scalar 0 or 1.');
else
    % Embed W in the right number of dims.  Then replicate it out along the
    % non-working dims to match X's size.
    wresize = ones(size(sz)); wresize(dim) = sz(dim);
    wtile = sz; wtile(dim) = 1;
    w = repmat(reshape(w, wresize), wtile);

    % Count up non-NaNs.
    n = nansum(~isnan(x).*w,dim);

    x0 = x - repmat(nansum(w.*x, dim) ./ n, tile);
    y = nansum(w .* abs(x0).^2, dim) ./ n; % abs guarantees a real result
end


function [mlfcoeffs, R2] = mlffit2(XNodes, YDataPoints, mlfcoeffs0, Precision)
%
% Fitting data [XNodes, YDataPoints] by Mittag-Leffler function
% y(t) = C*t^(beta-1)*E_{alpha, beta}(a*t^alpha)
%
% Output:   mlfcoeffs(1) = alpha
%           mlfcoeffs(2) = beta
%           mlfcoeffs(3) = C
%           mlfcoeffs(4) = a
%
%           R2 = sum of squares of vertical offsets
%                of the fitting curve from the data points
%
% (C) Igor Podlubny, 2011

% The following is just one-time installation
% of the necessary packages 8738 and 31069
% from Matlab Central FIle Exchange (FEX)

% Check if the function 'mlf' from the FEX package 8738
% (for evaluation of the Mittag-Leffler function)
% is on your MATLAB path, and if it is not there,
% require the FEX packages 31069 ("requireFEXpackage")
% and 8738 ("Mittag-Leffler function"):
if ~(exist('mlf', 'file') == 2)
    P = requireFEXpackage(31069); % "requireFEXpackage"
    P = requireFEXpackage(8738);  % "Mittag-Leffler function"
end

% Also, the function 'fminsearchbnd' (FEX package 8277) is nedeed:
if ~(exist('fminsearchbnd', 'file') == 2)
    P = requireFEXpackage(8277);  % fminsearchbnd is part of 8277
end

% Now, we have MLF.M installed and can do the fitting

c = fminsearchbnd(@(Params) mlfparam(Params, XNodes, Precision, YDataPoints), mlfcoeffs0, ...
            [eps; eps; -Inf; -Inf]);

R2 = sum(c(3)*XNodes.^(c(2)-1).*(mlf(c(1), c(2), c(4)*XNodes.^c(1), Precision) - YDataPoints).^2);
mlfcoeffs = c;
end

function f = mlfparam(Params, XNodes, Precision, YDataPoints)
R2 =  sum((Params(3)* XNodes.^(Params(2)-1).*mlf(Params(1), Params(2), ...
        Params(4)*XNodes.^Params(1), Precision) - YDataPoints).^2);
f = R2;
end


function [filt] = bandstopfilter(dat,Fs,Fbp,N,type,dir)

% BANDSTOPFILTER filters EEG/MEG data in a specified band
%
% Use as
%   [filt] = bandstopfilter(dat, Fsample, Fbp, N, type, dir)
% where
%   dat        data matrix (Nchans X Ntime)
%   Fsample    sampling frequency in Hz
%   Fbp        frequency band, specified as [Fhp Flp]
%   N          optional filter order, default is 4 (but) or 25 (fir)
%   type       optional filter type, can be
%                'but' Butterworth IIR filter (default)
%                'fir' FIR filter using MATLAB fir1 function
%   dir        optional filter direction, can be
%                'onepass'         forward filter only
%                'onepass-reverse' reverse filter only, i.e. backward in time
%                'twopass'         zero-phase forward and reverse filter (default)
%
% Note that a one- or two-pass filter has consequences for the
% strength of the filter, i.e. a two-pass filter with the same filter
% order will attenuate the signal twice as strong.
%
% See also LOWPASSFILTER, HIGHPASSFILTER, BANDPASSFILTER

% Copyright (c) 2007, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% set the default filter order later
if nargin<4
    N = [];
end

% set the default filter type
if nargin<5
  type = 'but';
end

% set the default filter direction
if nargin<6
  dir = 'twopass';
end

% Nyquist frequency
Fn = Fs/2;

% compute filter coefficients
switch type
  case 'but'
    if isempty(N)
      N = 4;
    end
    [B, A] = butter(N, [min(Fbp)/Fn max(Fbp)/Fn], 'stop');
  case 'fir'
    if isempty(N)
      N = 25;
    end
    B = fir1(N, [min(Fbp)/Fn max(Fbp)/Fn], 'stop');
    A = 1;
end

% apply filter to the data
switch dir
  case 'onepass'
    filt = filter(B, A, dat')';
  case 'onepass-reverse'
    dat  = fliplr(dat);
    filt = filter(B, A, dat')';
    filt = fliplr(filt);
  case 'twopass'
    filt = filtfilt(B, A, dat')';
end


function out = poly(f)
%POLY   Polynomial coefficients of a CHEBTECH.
%   C = POLY(F) returns the polynomial coefficients of F so that
%           F(x) = C(1)*x^N + C(2)*x^(N-1) + ... + C(N)*x + C(N+1)
%
%   Note that unlike the MATLAB POLY command, CHEBTECH/POLY can operate on
%   array-valued CHEBTECH objects, and hence produce a matrix output. In such
%   instances, the rows of C correspond to the columns of F = [F1, F2, ...].
%   That is,
%           F1(x) = C(1,1)*x^N + C(1,2)*x^(N-1) + ... + C(1,N)*x + C(1,N+1)
%           F2(x) = C(2,1)*x^N + C(2,2)*x^(N-1) + ... + C(2,N)*x + C(2,N+1).
%   This strange behaviour is a result of MATLAB's decision to return a row
%   vector from the POLY command, even for column vector input.
%
% See also CHEBCOEFFS, TRIGCOEFFS, LEGCOEFFS.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [Mathematical reference]: Section 3.3 Mason & Handscomb, "Chebyshev
% Polynomials". Chapman & Hall/CRC (2003).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Deal with empty case:
if ( isempty(f) )
    out = [];
    return
end

% Flip the Chebyshev coefficients to match Matlab ordering:
coeffs = flipud(f.coeffs);
[n, m] = size(coeffs);

% Coefficients on the unit interval:
if ( n == 1 )
    % Constant case:
    out = coeffs;

elseif ( n == 2 )
    % Linear case:
    out = coeffs.';

else
    % General case:

    % Initialise working vectors:
    tn = zeros(m, n);
    tnold1 = tn;
    tnold1(:,2) = 1;
    tnold2 = tn;
    tnold2(:,1) = 1;
    zer = zeros(m, 1);

    % Transpose coeffs (as output is in this form):
    coeffs = coeffs.';

    % Initialise output:
    out = zeros(m, n);

    % Initial step:
    out(:,[1,2]) = [ zer coeffs(:,end).*tnold2(:,1) ] + ...
        bsxfun(@times, coeffs(:,end-1), tnold1(:,[2,1]));

    % Recurrence:
    for k = 3:n
        tn(:,1:k)  = [ zer 2*tnold1(:,1:k-1) ] - [ tnold2(:,1:k-2), zer, zer ];
        out(:,1:k) = bsxfun(@times, coeffs(:,end-k+1), tn(:,k:-1:1)) + ...
                        [ zer, out(:,1:k-1) ];
        tnold2 = tnold1;
        tnold1 = tn;
    end

end

end


function y_line=runline(y,n,dn)
% Running line fit (local linear regression)
%
% Usage: y_line=runline(y,n,dn);
%
% Inputs:
% y: input 1-d time series (real)
% n: length of running window in samples
% dn: stepsize of window in samples
%
% Outputs:
% y_line: local line fit to data

y=y(:);
nt=length(y);
y_line=zeros(nt,1);
norm=y_line;
nwin=ceil((nt-n)/dn);
yfit=zeros(nwin,n);
xwt=((1:n)-n/2)/(n/2);
wt=(1-abs(xwt).^3).^3;
for j=1:nwin,
	tseg=y(dn*(j-1)+1:dn*(j-1)+n);
	y1=mean(tseg);
	y2=mean((1:n)'.*tseg)*2/(n+1);
	a=(y2-y1)*6/(n-1); b=y1-a*(n+1)/2;
	yfit(j,:)=(1:n)*a+b;
	y_line((j-1)*dn+(1:n))=y_line((j-1)*dn+(1:n))+(yfit(j,:).*wt)';
	norm((j-1)*dn+(1:n))=norm((j-1)*dn+(1:n))+wt';
end
mask=find(norm>0); y_line(mask)=y_line(mask)./norm(mask);
indx=(nwin-1)*dn+n-1;
npts=length(y)-indx+1;
y_line(indx:end)=(n+1:n+npts)'*a+b;


function value = r4_gamr ( x )

%*****************************************************************************80
%
%% R4_GAMR evaluates the reciprocal gamma function of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the value of the reciprocal gamma
%    function at X.
%
  if ( x <= 0.0 && r4_aint ( x ) == x )

    value = 0.0;

  elseif ( abs ( x ) <= 10.0 )

    value = 1.0 / r4_gamma ( x );

  else

    [ alngx, sgngx ] = r4_lgams ( x );
    value = sgngx * exp ( - alngx );

  end

  return
end


function [ Ycap ] = lraNTF( Y,opts )
%% Low-rank approximation based Non-Negative Tensor(CP) Factorization
%   Usage: Ycap=lraNCP(Y,opts);
%   Input: Y can be ktensor/ttensor/tensor/double
%  Output: Ycap is a nonnegative ktensor
%
%  Parameters:
%   opts.NumOfComp:  rank of Ycap
%   opts.lra_rank:   rank of low-rank approximation of Y
%   opts.CPalg:      a -mat file including the configuration of
%                           unconstrained CPD
%   opts.maxiter:    max. of global iterations    [default:50]
%   opts.maxiniter: max. of inner iterations. [default:20]
%   opts.nlssolver: [HALS|APG|MU] the algorithm used for solving NLS
%                      sub-problem
%   opts.initmode:  [cp|random]
%   opts.tol:        stop threshold. [default:1e-6]
%   opts.sparsity  an N-by-1 vector in [0,1) to specify the sparsity of
%                   results (l1-norm based sparsity).
%
%
%
% Cite this work currently:
%  Guoxu Zhou; Cichocki, A.; Shengli Xie; , "Fast Nonnegative Matrix/Tensor Factorization Based on Low-Rank Approximation,"
%  IEEE Transactions on Signal Processing, vol.60, no.6, pp.2928-2940, June 2012
%  doi: 10.1109/TSP.2012.2190410
%  URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6166354&isnumber=6198804
%
% By Guoxu Zhou
% E-Mail: zhouguoxu@gmail.com
% Last updated: 2013-09-09


function lagrange2D
%% STARTVARIABLES
[X,Y]=meshgrid(-1:1:5,-1:1:5);
Z = sin(Y)*sin(X);

step = 0.2;
%% INTERPOLATION
% erst kurven in x richtung ausrechnen
xCurves={};
for i=1:size(X,1)
    % x vector must be a column vector for the lagrange function
    x = X(i,:)';
    % z vector must be a column vector for the lagrange function
    z = Z(i,:)';
    p=[];
    for j = x(1):step:x(end)
        % interpolate for every parameter value j and add it to p
        p = [p,lagrange(x,z,j)];
    end
    % save curves in x direction
    xCurves{i} = p;
end

y = Y(:,1);
% matrix for the graphical outpu
A=[];
% interpolate in y-direction
for i=1:length(xCurves{1})
    p=[];
    z=[];
    for l=1:length(y)
        z = [z;xCurves{l}(i)];
    end
    for j = y(1):step:y(end)
         % interpolate for every parameter value j and add it to p
        p = [p;lagrange(y,z,j)];
    end
    A = [A,p];
end

%% GRAPHICAL OUTPUT
% plot surface
surf(x(1):(x(end)-x(1))/(size(A,1)-1):x(end),y(1):(y(end)-y(1))/(size(A,1)-1):y(end),A);
hold on;
% plot points
for i=1:size(X,1)
    for j=1:size(Y,1)
        p = plot3(X(i,j),Y(i,j),Z(i,j));
        set(p,'Marker','.');
        set(p,'MarkerSize',30);
    end
end

end
%% nested functions
function res = lagrange(x,z,t)
% lagrange interpolation in 1d
% input: x...vektor, welcher die x (bzw. y) Werte beinhaltet

res = 0;
for i=1:length(x)
    % delete the component not needed
    temp = [x(1:i-1);x(i+1:end)];

    denominator = x(i)-temp;

    numerator = t-temp;
    res = res + z(i)*(prod(numerator))/(prod(denominator));
end
end