%% DEMOLONG  Short demonstration of long numbers
%

%%
% The purpose of the long toolbox was to compute rigorous bounds for the
% value of certain standard functions. Those values are needed to initialize
% the INTLAB system. The long toolbox is slow, but fast enough to do the job.

setround(0)                 % set rounding to nearest
longprecision(0);           % default option

%% Definition of long numbers
%
% Long numbers are stored in midpoint radius representation.
% The midpoint is stored in an array of precision to be specified,
% the error is stored in one number. Long numbers or vectors are generated
% by the constructor long:

x = long(7)
V = long([-1;3])

%%
% Long vectors are always column vectors (or forced to column vectors).

%% Conversion
% Conversion of double to long by the constructor "long" is always into
% the current internal precision.
%
% The internal precision may be specified by "longprecision".
% The call without input parameter gives the current working precision
% in decimals, the call with input parameter sets working precision.

p = longprecision
longprecision(50)

%%
% Now, the statement "x = long(7)" generates a long number with
% approximately 50 decimal digits. This is only approximate because
% internal representation is to some base beta, a power of 2.

%% Output of long numbers
% Output is usually to a little more than double precision. If you want
% to see more digits, say k, use "display" with second parameter equal
% to k. To see all digits, use k=0.

longprecision
x = 1/long(7)
display(x,40)
display(x,0)

%%
% Output of long numbers is not rigorous. All but a few of the last
% digits are correct.

%% Arithmetic operations
% Long operations +,-,*,/ and ^ are supported. Note that operations on
% vectors are always performed elementwise.

x = [ long(3) ; -7 ]
x*x

%% Output of long intervals
% The display routine takes uncertainties into account. Only the
% correct digits plus some extra are displayed.

longprecision(50);
x = long(1)/37;
display(x,0)
for i=1:100
  x=x*x; x=x*37;
end
display(x,0)

%% Interval and non-interval operations
% Computing with uncertainties may be switched off by

longinit('WithoutErrorTerm')
longprecision(50);
x = long(1)/37;
display(x,0)
for i=1:100
  x=x*x; x=x*37;
end
display(x,0)

%%
% In this case all digits including incorrect ones are displayed.
% Computing without error term is a usual long precision arithmetic
% with specified precision. Note that scalar operations suffer from
% quite some interpretation overhead.

%% Conversion between long and double
% Conversion from long to double is approximately to nearest, conversion
% to interval is rigorous.
%
% For example, in the following the function "longpi" calculates
% "pi" to the specified longprecision, "IntPi" is a true inclusion of the
% transcendental number "pi".

longinit('WithErrorTerm');
longprecision(100);
Pi = longpi;
display(Pi,0)
flptPi = long2dble(Pi)
IntPi = long2intval(Pi)
format long
infsup(IntPi)

%% Long numbers with error term
% Long numbers may be specified with an explicit error term.
% For example,

longprecision(50);
x = long(-1.5)
display(x,0)
x = addlongerror(x,1e-40)
display(x,0)

%%
% defines x to be an interval with midpoint -1.5 and radius
% approximately 10^(-40). Only meaningful digits are stored and displayed.

%% Specifying extremely small errors
% For very small errors leaving the range double precision
% floating point numbers, the error may be specified by
% the mantissa and the exponent of the error:

longprecision(50);
x = long(2^-1000)^2;
x = addlongerror(x,1,-620)

%%
% The final x, which is 2^(-2000), is afflicted with an error
% of 10^(-620).

%% Taylor series: an example
% As an example, the following code computes the value of E = exp(x)
% by a Taylor series:

p = 100; longprecision(p);
x = -30;
t = 1; T = long(1); E = T; k = 0;
while abs(t)>10^(-p)
  k = k+1;
  t = t*x/k;
  T = T*x/k;
  E = E + T;
end
k
exp(x)
display(E,0)

%%
% Note that for large negative values of x there quite some
% cancellation. This can be seen by

x = 30;
t = 1; T = long(1); E = T; k = 0;
while abs(t)>10^(-p)
  k = k+1;
  t = t*x/k;
  T = T*x/k;
  E = E + T;
end
k
1/exp(x)
display(1/E,0)

%% Ill-conditioned polynomials
% Consider the following polynomial:

P = inline(' 4999*x.^6 - 200*x.^5 + 102*x.^4 - 2*x.^3 - 2500*x.^2 + 100*x - 1 ')

%%
% This is an example Bugeaud-Mignotte polynomial. The general form is
%
% ( X^n - aX + 1 )^k - 2X^(nk-k)(aX-1)^k
%
% where a>=10, n>=3 and k>=2.
%
% Those polynomials are constructed to have a pair of very close real roots near c=1/a+1/a^(n+1).
% A graph near c looks as follows:

e = 3e-8;
c = 1/50+1/50^4;
x = c*(1+linspace(-e,e));
close
plot(x,P(x),x,0*x)

%%
% From the graph it is not clear whether the polynomial has no, a double or two real roots
% in the interval c*[1-e,1+e]. An evaluation using the long package yields
% the following:

y = long2dble(P(long(x)));
close
plot(x,y,x,0*x)

%% Sample programs
% For sample programs using long numbers, see for example the
% source codes of long\longpi.m or long\@long\exp.m  .

%% Enjoy INTLAB
% INTLAB was designed and written by S.M. Rump, head of the Institute for Reliable Computing,
% Hamburg University of Technology. Suggestions are always welcome to rump (at) tuhh.de


function xdot=dpendulum(t,x)
%% Double Pendulum ODE
%
% ivp=[gamma0; dtgamma0; alpha0; dtalpha0; beta0; lambda; omega; psi; eta];

beta0=x(5); lambda=x(6); omega=x(7); psi=x(8); eta=x(9);
xdot=zeros(9,1); % a column vector

C = cos(beta0)*cos(x(3)-x(1))-sin(beta0)*sin(x(3)-x(1));
S = sin(beta0)*cos(x(3)-x(1))+cos(beta0)*sin(x(3)-x(1));

xdot(1) = x(2);
xdot(2) = ((omega^2)*sin(x(1))-psi*(S*(x(4)^2+C*x(2)^2*eta)+...
    C*(lambda^2)*sin(x(3))))/(-1+(C^2)*eta*psi);
xdot(3) = x(4);
xdot(4) = (S*eta*(x(2)^2+C*x(4)^2*psi)-C*eta*(omega^2)*sin(x(1))+...
    (lambda^2)*sin(x(3)))/(-1+(C^2)*eta*psi);


function G = gsp_stochastic_block_graph(N, k, params)
%GSP_STOCHASTIC_BLOCK_GRAPH  Create a stochastic block graph
%   Usage:  G = gsp_stochastic_block_graph( N );
%           G = gsp_stochastic_block_graph(N , k);
%
%   Input parameters:
%         N     : Number of nodes (default 1024)
%         k     : Number of clusters (default 5)
%         params: Structure of optional parameters
%   Output parameters:
%         G     : Graph structure.
%
%   *param* is an optional structure with the following fields
%
%   * *params.p*        : Intra-cluster edge probability (default 0.7)
%   * *params.q*        : Inter-cluster edge probability (default 0.3/k)
%   * *params.z*        : Assignment vector of nodes (default uniform random)
%   * *params.M*        : Link probability matrix between clusters (default uses p and q)
%   * *params.directed* : Flag the graph as directed or not (default false)
%
%   Use the stochastic block model to create a graph.

% Author: Pierre Vandergheynst, Nathanael Perraudin
% Date  : 2 novemeber 2015 (revision: 6 october 2016 -- Lionel Martin)

%% Stochastic Block Model generator
if nargin<1
    N = 1024; % number of nodes
end
if nargin<2
    k = 5; % number of clusters
end
if nargin < 3
    params = struct;
end

if ~isfield(params, 'p')
    params.p = 0.7;
end

if ~isfield(params, 'q')
    params.q = (1-params.p) / k;
end

if ~isfield(params, 'force_full')
    params.force_full = 0;
end

if ~isfield(params, 'auto_gen_M')
    params.auto_gen_M = 0;
end

if (~isfield(params, 'z') || length(params.z) ~= N || max(params.z) > k || min(params.z) < 1)
    params.z = randi(k, 1, N);
end

if (~isfield(params, 'M') || size(params.M) ~= [k, k])
    if params.force_full
        params.M = params.q * ones(k);
        params.M(1:k+1:end) = params.p;
    end
    params.auto_gen_M = 1;
end

if ~isfield(params, 'directed')
    params.directed = false;
end

%% partition with clusters of homogenous sizes
%
% THIS PART IS NOT NECESSARY BECAUSE SLOW AND REPLACED
% BY THE SIMPLE "UNIFORM" CLUSTER ASSIGNMENT ABOVE
%
% L = ceil(N/k);
% for i=1:k-1
%     z((i-1)*L + 1:i*L) = i;
% end
% z((k-1)*L + 1:end) = k;

%% Generate adjacency
z = params.z;

% for i=1:N
%     for j=i+1:N
%         W(i, j) = ( rand <= M(z(i), z(j)) );
%         if params.directed
%             W(j, i) = ( rand <= M(z(j), z(i)) );
%         else
%             W(j, i) = W(i, j);
%         end
%     end
% end
if params.auto_gen_M && ~params.force_full
    [val_z, idx_z] = sort(z);
    counts = diff(find(diff([0, val_z, N])));

    if length(counts) ~= k
        error('There is at least one empty class. Check your z.');
    end

    W = sprandsym(counts(k), params.p);
    nb_cols_rect = 0;

    for i=k:-1:2
        nb_cols_rect = nb_cols_rect + counts(i);
        rect = sprand(counts(i-1), nb_cols_rect, params.q);
        top_left = sprandsym(counts(i-1), params.p);
        W = vertcat(horzcat(top_left, rect), horzcat(rect', W));
    end

    W(1:N+1:end) = 0;
    G.W(idx_z, idx_z) = abs(W) > 0;

else
    M = params.M;

    if params.directed
        W = rand(N) <= M(z, z);
    else
        A = rand(N);
        A(logical(triu(ones(N)))) = 1;
        W = A <= M(z, z);
        W = W + W';
    end

    G.W = sparse(W);
end

G = gsp_graph_default_parameters(G);
G.info.node_com = z;

G.coords = ones(N, 2);
com_coords = sqrt(N) * [-cos(2*pi*(1:k)/k)', sin(2*pi*(1:k)/k)'];

% create uniformly random points in the unit disc
for ii = 1:N
    % use rejection sampling to sample from a unit disc (probability = pi/4)
    while norm(G.coords(ii, :)) >= 1/2
        % sample from the square and reject anything outside the circle
        G.coords(ii, :) = [rand-.5, rand-.5];
    end
end

% add the offset for each node depending on which community it belongs to
for ii = 1:k
    idx_ii = find(z==ii);
    rad_com = sqrt(numel(idx_ii));
    G.coords(idx_ii, :) = bsxfun(@plus, rad_com * G.coords(idx_ii, :), com_coords(ii, :));
end

end


function [R,eff] = randmio_und_signed(W, ITER)
% RANDMIO_UND_SIGNED	Random graph with preserved signed degree distribution
%
%   R       = randmio_und_signed(W,ITER);
%   [R,eff] = randmio_und_signed(W,ITER);
%
%   This function randomizes an undirected network with positively and
%   negatively signed connections, while preserving the positively and
%   negatively signed degree distribution. The function does not preserve
%   the strength distribution in weighted networks.
%
%   Input:      W,      undirected (binary/weighted) connection matrix
%               ITER,   rewiring parameter
%                       (each edge is rewired approximately ITER times)
%
%   Output:     R,      randomized network
%               eff,    number of actual rewirings carried out
%
%   Reference:  Maslov and Sneppen (2002) Science 296:910
%
%
%   2011-2015
%   Dani Bassett, UCSB
%   Olaf Sporns,  Indiana U
%   Mika Rubinov, U Cambridge

%   Modification History:
%   Mar 2011: Original (Dani Bassett, based on randmio_und.m)
%   Mar 2012: Limit number of rewiring attempts,
%             count number of successful rewirings (Olaf Sporns)
%   Dec 2015: Rewritten the core of the rewiring algorithm to allow
%             unbiased exploration of all network configurations. The new
%             algorithm allows positive-positive/negative-negative
%             rewirings, in addition to the previous positive-positive/0-0
%             and negative-negative/0-0 rewirings (Mika Rubinov).

if nargin('randperm')==1
    warning('This function requires a recent (>2011) version of MATLAB.')
end

R     = double(W);              % sign function requires double input
n     = size(R,1);
ITER  = ITER*n*(n-1)/2;

% maximal number of rewiring attempts per 'iter'
maxAttempts = round(n/2);
% actual number of successful rewirings
eff = 0;

for iter=1:ITER
    att=0;
    while (att<=maxAttempts)    %while not rewired
        %select four distinct vertices
        nodes = randperm(n,4);
        a = nodes(1);
        b = nodes(2);
        c = nodes(3);
        d = nodes(4);

        r0_ab = R(a,b);
        r0_cd = R(c,d);
        r0_ad = R(a,d);
        r0_cb = R(c,b);

        %rewiring condition
        if      (sign(r0_ab)==sign(r0_cd)) && ...
                (sign(r0_ad)==sign(r0_cb)) && ...
                (sign(r0_ab)~=sign(r0_ad))

            R(a,d)=r0_ab; R(a,b)=r0_ad;
            R(d,a)=r0_ab; R(b,a)=r0_ad;
            R(c,b)=r0_cd; R(c,d)=r0_cb;
            R(b,c)=r0_cd; R(d,c)=r0_cb;

            eff = eff+1;
            break;
        end %rewiring condition
        att=att+1;
    end %while not rewired
end %iterations


function mt = SupIntensity_HP(t, History, para, options)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Compute the super bound of intensity function of Hawkes processes
%
% Parameters of Hawkes processes
% para.mu: base exogenous intensity
% para.A: coefficients of impact function
% para.kernel: 'exp', 'gauss'
% para.w: bandwith of kernel
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isempty(History)
    mt = sum(para.mu);
else
    Time = History(1, :);
    index = Time<=t;
    Time = Time(index);
    Event = History(2, index);

    MT = sum(para.mu)*ones(1, options.M);
    for m=1:options.M
        t_current = t+(m-1)*options.tstep/options.M;

        basis = Kernel(t_current-Time(:), para);
        A = para.A(Event, :, :);

        for c = 1:size(para.A, 3);
            MT(m) = MT(m) + sum(sum(basis.*A(:,:,c)));
        end
    end
    mt = max(MT);
end

mt = mt.*(mt>0);
end


% A = riemann_mean(B,epsilon,tol)
%
% Calcul du barycentre des matrice de covariances.
% A : baricentre des K matrices NxN
%
% B : Matrice NxNxK
% epsilon : Pas de la descente de gradient
% tol : arret de la descente si le crit�re < tol

function [A critere niter] = opttransp_mean(B,args)
Imat = size(B,3);
N_itermax = 200;
if (nargin<2)||(isempty(args))
    tol = 10^-3;
    %A = mean(B,3);
    A = eye(size(B,1));
else
    tol = args{1};
    A = args{2};
end

niter = 0;
fc = 0;
K = A^(0.5);

while (niter<N_itermax)
    niter = niter+1;

    Ktmp = 0;
    for i=1:Imat
       Ktmp = Ktmp + (K*B(:,:,i)*K)^(0.5);
    end
    Ktmp = (Ktmp)^(0.5);

    fcn = norm(Ktmp-K,'fro');
    K = Ktmp;
    % improvement
    conv = abs((fcn-fc)/fc);
    if conv<tol % break if the improvement is below the tolerance
       break;
    end
    fc = fcn;
end

A = ((1/Imat)*K)^2;

if niter==N_itermax
    disp('Warning : Nombre d''itérations maximum atteint');
end

critere = fc;


function x = bradford_cdf_inv ( cdf, a, b, c )

%*****************************************************************************80
%
%% BRADFORD_CDF_INV inverts the Bradford CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%    0.0 <= CDF <= 1.0.
%
%    Input, real A, B, C, the parameters of the PDF.
%    A < B,
%    0.0 < C.
%
%    Output, real X, the corresponding argument of the CDF.
%
  if ( cdf < 0.0 | 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BRADFORD_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'BRADFORD_CDF_INV - Fatal error!' );
  end

  if ( cdf <= 0.0 )
    x = a;
  elseif ( cdf < 1.0 )
    x = a + ( b - a ) * ( ( c + 1.0 )^cdf - 1.0 ) / c;
  elseif ( 1.0 <= cdf )
    x = b;
  end

  return
end


function [tfVMVariates] = vmrand(fMu, fKappa, varargin)

% vmrand - FUNCTION Draw random variates from the Von Mises circular distribution
%
% Usage: [tfVMVariates] = vmrand(fMu, fKappa)
%        [tfVMVariates] = vmrand(..., M)
%        [tfVMVariates] = vmrand(..., M, N, P, ...)
%        [tfVMVariates] = vmrand(..., [M N P ...])
%
% This function uses an envelope-rejection method based on a wrapped Cauchy
% distribution to draw random variates from an arbitrary Von Mises
% distribution, first proposed in [1].
%
% 'fMu' and 'fKappa' are the mean and variance parameter of the Von Mises
% distribution over [-pi, pi).  'tVMVariates' will be a tensor containing
% random variates drawn from the defined distribution.  If 'fMu' and
% 'fKappa' are non-scalar, then they must be the same size.  In this case
% 'tVMVariates' will be the same size.  If 'fMu' and 'fKappa' are scalar,
% then the number of variates returned can be specified as extra arguments.
%
% If only single dimension 'M' is provided, then the return argument
% 'tfVMVariates' will be M x M.
%
% This implementation is vectorised, and requires O(7.5*N) space.
%
% References:
% [1] D. J. Best and N. I. Fisher, 1979. "Efficient Simulation of the von
% Mises Distribution", Journal of the Royal Statistical Society. Series C
% (Applied Statistics), Vol. 28, No. 2, pp. 152-157.

% Author: Dylan Muir <muir@hifo.uzh.ch>
% Created: 19th June, 2012

% -- Check arguments

if (nargin < 2)
   help vmrand;
   error('VMRAND:Usage', '*** vmrand: Incorrect usage');
end

% -- Check sizes

vbScalarArgs = [isscalar(fMu) isscalar(fKappa)];

if (nnz(vbScalarArgs) == 1)
   if vbScalarArgs(1)
      % - fMu is scalar
      fMu = repmat(fMu, size(fKappa));
   else
      % - fKappa is scalar
      fKappa = repmat(fKappa, size(fMu));
   end

   % - Set return sizes
   vnTensorSize = size(fMu);

elseif (nnz(vbScalarArgs == 0))
   % - Two non-scalar arguments
   if (~isequal(size(fMu), size(fKappa)))
      error('VMRAND:UnequalSizes', ...
         '*** vmrand: ''fMu'' and ''fKappa must be the same size.');
   else
      vnTensorSize = size(fMu);
   end

elseif (~isempty(varargin))
   % - Get argument sizes from varargin (be forgiving)
   varargin = cellfun(@(c)(reshape(c, 1, [])), varargin, 'UniformOutput', false);
   vnTensorSize = [varargin{:}];

   % - Take Matlab semantics to make square matrices
   if (isscalar(vnTensorSize))
      vnTensorSize = vnTensorSize([1 1]);
   end

   fKappa = repmat(fKappa, vnTensorSize);

else
   % - Return a scalar variate
   vnTensorSize = [1 1];
end

% -- Check values

if (any(fKappa < 0))
   error('VMRAND:InvalidArguments', ...
      '*** vmrand: ''fKappa'' must be >= 0');
end

if (any(vnTensorSize < 1))
   error('VMRAND:InvalidArguments', ...
      '*** vmrand: Tensor size dimensions must be positive');
end

% -- Preallocate data

tfVMVariates = nan(vnTensorSize);
tfZ = nan(vnTensorSize);
tfF = nan(vnTensorSize);
tfC = nan(vnTensorSize);

% -- Short-cut fKappa == 0

tbUniform = fKappa == 0;
tfVMVariates(tbUniform) = rand(nnz(tbUniform), 1) * 2*pi - pi;
tbDraw = ~tbUniform;
tbAccept = tbUniform;

% -- Pre-compute what we can

if (all(vbScalarArgs))
   tfTau = sqrt(4 .* fKappa(1).^2 + 1) + 1;
   tfRho = (tfTau - sqrt(2 .* tfTau)) ./ (2 .* fKappa(1));
   tfR = repmat((1 + tfRho.^2) ./ (2 .* tfRho), vnTensorSize);

else
   tfTau = sqrt(4 .* fKappa.^2 + 1) + 1;
   tfRho = (tfTau - sqrt(2 .* tfTau)) ./ (2 .* fKappa);
   tfR = (1 + tfRho.^2) ./ (2 .* tfRho);
   clear tfTau tfRho;   % - To save some space
end

% -- Draw random variates

while (nnz(tbDraw > 0))
   % - Draw partial variates and estimate wrapped Cauchy distribution envelope
   nNumToDraw = nnz(tbDraw);
   tfZ(tbDraw) = cos(pi .* rand(nNumToDraw, 1));
   tfF(tbDraw) = (1 + tfR(tbDraw) .* tfZ(tbDraw)) ./ (tfR(tbDraw) + tfZ(tbDraw));
   tfC(tbDraw) = fKappa(tbDraw) .* (tfR(tbDraw) - tfF(tbDraw));

   % - Filter variates
   vfRand2 = rand(nNumToDraw, 1);
   tbAccept(tbDraw) = (tfC(tbDraw) .* (2 - tfC(tbDraw)) - vfRand2) > 0;
   vbRecheck = ~tbAccept(tbDraw);
   if (any(vbRecheck))
      tbAccept(tbDraw & ~tbAccept) = (log(tfC(tbDraw & ~tbAccept) ./ vfRand2(vbRecheck)) + 1 - tfC(tbDraw & ~tbAccept)) >= 0;
   end

   % - Construct final variates
   nNumToAccept = nnz(tbDraw & tbAccept);
   tfVMVariates(tbDraw & tbAccept) = sign(rand(nNumToAccept, 1) - 0.5) .* acos(tfF(tbDraw & tbAccept));

   % - Mark as being accepted (don't need to try again)
   tbDraw(tbAccept) = false;
end

% -- Shift variates to mean

tfVMVariates = tfVMVariates + fMu;
tfVMVariates(tfVMVariates > pi) = -2*pi + tfVMVariates(tfVMVariates > pi);
tfVMVariates(tfVMVariates < -pi) = 2*pi + tfVMVariates(tfVMVariates < -pi);

% --- END of vmrand.m ---


function sparse_count_test025 ( dim_min, dim_max, level_max_min, ...
  level_max_max )

%*****************************************************************************80
%
%% SPARSE_COUNT_TEST025 tests CFN_E_SIZE_TOTAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_MIN, the minimum spatial dimension to consider.
%
%    Input, integer DIM_MAX, the maximum spatial dimension to consider.
%
%    Input, integer LEVEL_MAX_MIN, the minimum value of LEVEL_MAX to consider.
%
%    Input, integer LEVEL_MAX_MAX, the maximum value of LEVEL_MAX to consider.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST025\n' );
  fprintf ( 1, '  CFN_E_SIZE_TOTAL returns the number of\n' );
  fprintf ( 1, '  points in a CFN_E sparse grid made from \n' );
  fprintf ( 1, '  any closed fully nested family of 1D quadrature\n' );
  fprintf ( 1, '  rules with exponential growth, including:\n' );
  fprintf ( 1, '  * CC_E, the Clenshaw Curtis Exponential Growth family;\n' );
  fprintf ( 1, '  * NCC_E, the Newton Cotes Closed Exponential Growth family.\n' );
  fprintf ( 1, '  No reduction is made because of repeated points.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   DIM: ' );

  for dim_num = dim_min : dim_max
    fprintf ( 1, '  %10d', dim_num );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   LEVEL_MAX\n' );
  fprintf ( 1, '\n' );

  for level_max = level_max_min : level_max_max
    fprintf ( 1, '    %4d', level_max );
    for dim_num = dim_min : dim_max
      point_num = cfn_e_size_total ( dim_num, level_max );
      fprintf ( 1, '  %10d', point_num );
    end
    fprintf ( 1, '\n' );
  end

  return
end


% vgg_vec  De-/vectorization of a matrix.
%
% For a matrix X, vgg_vec(X) = X(:).
% For a N^2-vector x, vgg_vec(x) = reshape(x,N,N).
%
% Classical matrix re-arrangement operator, see book Magnus-Neudecker.
% Trivial function, included mainly for consistency with notation in literature.
%
% Useful for rearranging matrix equations. Matrix from the middle of a product can be put to the right as
%
%    vgg_vec(A*B*C) = kron(C',A)*vgg_vec(B)
%
% See also vgg_vec_swap, vgg_commut_matrix, vgg_duplic_matrix.

function v = vgg_vec(A)

if any(size(A)==1)
  v = reshape(A,[1 1]*sqrt(prod(size(A))));
else
  v = A(:);
end

return


function program_05 ( )

%*****************************************************************************80
%
%% PROGRAM_05 displays the points that sample a triangle.
%
%  Discussion:
%
%    This program is similar to program_04, but displays the
%    sample points.
%
%    The program
%    * reads a triangle T (defined by three points),
%    * reads a random number seed;
%    * reads "1" for bad scheme, "2" for good scheme.
%    * reads N, the number of random values to generate;
%    * it then computes N random points in the triangle;
%    * it determines N1, N2, and N3, the number of points
%      that ended up in subtriangles mbc, amc, and abm,
%      where "m" is the centroid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 February 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROGRAM_05 - The Eyeball Test on Sampling\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Define a triangle T:\n' );

  t_v1 = input ( '  Enter [ T.v1.x, T.v1.y]: ' );
  t_v2 = input ( '  Enter [ T.v2.x, T.v2.y]: ' );
  t_v3 = input ( '  Enter [ T.v3.x, T.v3.y]: ' );
%
%  Get the random seed.
%
  seed = input ( 'Enter a random number seed:  ' );
  rand ( 'state', seed );
%
%  Get the scheme to use.
%
  scheme = input ( 'Enter 1 for "bad scheme", 2 for "good scheme":  ' );
%
%  Get the number of values to generate.
%
  n = input ( 'Enter the number of samples to generate:  ' );

  n1 = 0;
  n2 = 0;
  n3 = 0;

  p = zeros ( 2, n );

  for i = 1 : n

    if ( scheme == 1 )
      r = rand;
      s = rand;
      t = rand;
      xi1 = r / ( r + s + t );
      xi2 = s / ( r + s + t );
      xi3 = t / ( r + s + t );
    elseif ( scheme == 2 )
      r = rand;
      s = rand;

      xi1 =   1.0       - sqrt ( s );
      xi2 = ( 1.0 - r ) * sqrt ( s );
      xi3 =         r   * sqrt ( s );
    end

    if ( xi1 < xi2 & xi1 < xi3 )
      n1 = n1 + 1;
    elseif ( xi2 < xi1 & xi2 < xi3 )
      n2 = n2 + 1;
    elseif ( xi3 < xi1 & xi3 < xi2 )
      n3 = n3 + 1;
    end

    p(1:2,i) = xi1 * t_v1(1:2) ...
             + xi2 * t_v2(1:2) ...
             + xi3 * t_v3(1:2);

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  N1 = %8d  %% = %8.4f\n', n1, 100 * n1 / n );
  fprintf ( 1, '  N2 = %8d  %% = %8.4f\n', n2, 100 * n2 / n );
  fprintf ( 1, '  N3 = %8d  %% = %8.4f\n', n3, 100 * n3 / n );
  fprintf ( 1, '  N  = %8d  %% = %8.4f\n', n, 100 );

  scatter ( p(1,:), p(2,:), 'filled' );
  m = ( t_v1 + t_v2 + t_v3 ) / 3;

  line ( [ t_v1(1), t_v2(1) ], [ t_v1(2), t_v2(2) ], 'Color', 'r', 'LineWidth', 2.0 );
  line ( [ t_v2(1), t_v3(1) ], [ t_v2(2), t_v3(2) ], 'Color', 'r', 'LineWidth', 2.0 );
  line ( [ t_v3(1), t_v1(1) ], [ t_v3(2), t_v1(2) ], 'Color', 'r', 'LineWidth', 2.0 );

  line ( [ t_v1(1), m(1) ], [ t_v1(2), m(2) ], 'Color', 'r', 'LineWidth', 2.0 );
  line ( [ t_v2(1), m(1) ], [ t_v2(2), m(2) ], 'Color', 'r', 'LineWidth', 2.0 );
  line ( [ t_v3(1), m(1) ], [ t_v3(2), m(2) ], 'Color', 'r', 'LineWidth', 2.0 );

  p_min(1) = min ( p(1,:) );
  p_min(1) = min ( p_min(1), t_v1(1) );
  p_min(1) = min ( p_min(1), t_v2(1) );
  p_min(1) = min ( p_min(1), t_v3(1) );

  p_max(1) = max ( p(1,:) );
  p_max(1) = max ( p_max(1), t_v1(1) );
  p_max(1) = max ( p_max(1), t_v2(1) );
  p_max(1) = max ( p_max(1), t_v3(1) );

  p_min(2) = min ( p(2,:) );
  p_min(2) = min ( p_min(2), t_v1(2) );
  p_min(2) = min ( p_min(2), t_v2(2) );
  p_min(2) = min ( p_min(2), t_v3(2) );

  p_max(2) = max ( p(2,:) );
  p_max(2) = max ( p_max(2), t_v1(2) );
  p_max(2) = max ( p_max(2), t_v2(2) );
  p_max(2) = max ( p_max(2), t_v3(2) );

  p_range(1) = p_max(1) - p_min(1);
  p_range(2) = p_max(2) - p_min(2);

  margin = 0.025 * max ( p_range(1), p_range(2) );

  x_min = p_min(1) - margin;
  x_max = p_max(1) + margin;
  y_min = p_min(2) - margin;
  y_max = p_max(2) + margin;
%
%  The TITLE function will interpret underscores in the title.
%  We need to unescape such escape sequences!
%
  title_string = 'Sample points in triangle';
  title ( title_string )

  axis ( [ x_min, x_max, y_min, y_max ] );
  axis equal
%
%  Save data to file.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saving data to file "triangle_samples.txt".\n' );

  save 'triangle_samples.txt' p -ASCII
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROGRAM_05\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


function x = gen_form(L_p,x_s,A,b,K,M)
%GEN_FORM Transform a standard-form problem back to the general-form setting.
%
% x = gen_form(L_p,x_s,A,b,K,M)    (method 1)
% x = gen_form(L_p,x_s,x_0)        (method 2)
%
% Transforms the standard-form solution x_s back to the required
% solution to the general-form problem:
%    x = L_p*x_s + d ,
% where L_p and d depend on the method as follows:
%    method = 1: L_p = pseudoinverse of L, d  = K*(b - A*L_p*x_s)
%    method = 2: L_p = A-weighted pseudoinverse of L, d = x_0.
%
% Usually, the standard-form problem is generated by means of
% function std_form.
%
% Note that x_s may have more that one column.

% References: L. Elden, "Algorithms for regularization of ill-
% conditioned least-squares problems", BIT 17 (1977), 134-145.
% L. Elden, "A weighted pseudoinverse, generalized singular values,
% and constrained lest squares problems", BIT 22 (1982), 487-502.
% M. Hanke, "Regularization with differential operators.  An itera-
% tive approach", J. Numer. Funct. Anal. Optim. 13 (1992), 523-540.

% Per Christian Hansen, IMM, 06/12/93.

% Nargin determines which method.
if (nargin==6)
  [p,q] = size(x_s); Km = size(K,1);
  if (Km==0)
    x = L_p*x_s;
  else
    x = L_p*x_s + K*(M*(b*ones(1,q) - A*(L_p*x_s)));
  end
else
  x_0 = A; [p,q] = size(x_s);
  x = L_p*x_s + x_0*ones(1,q);
end


function clusterLabels = gacMerging(graphW, initClusters, groupNumber, strDescr, z)
%% Cluster merging for Graph Agglomerative Clustering
% Implements an agglomerative clustering algorithm based on maiximum graph
%   strcutural affinity of two groups
% Inputs:
%	- graphW: asymmetric weighted adjacency matrix
%   - initClusters: a cell array of clustered vertices
%   - groupNumber: the final number of clusters
%   - strDescr: structural descriptor, 'zeta' or 'path'
%   - z: (I - z*P), default: 0.01
% Outputs:
%   - clusterLabels: 1 x m list whose i-th entry is the group assignment of
%                   the i-th data vector w_i. Groups are indexed
%                   sequentially, starting from 1.
% by Wei Zhang (wzhang009 at gmail.com), June, 8, 2011

%%
numSample = size(graphW,1);
IminuszW = eye(numSample) - z*graphW;
clear graphW
myInf = 1e10;

%% initialization
VERBOSE = true;
switch lower(strDescr)
    case 'zeta'
        complexity_fun = @gacZetaEntropy;
        conditionalComplexity_fun = @gacZetaCondEntropy;
    case 'path'
        complexity_fun = @gacPathEntropy;
        conditionalComplexity_fun = @gacPathCondEntropy;
    otherwise
        error('GAC: Descriptor type is not supported!');
end

numClusters = length(initClusters);
if numClusters <= groupNumber
    error('GAC: too few initial clusters. Do not need merging!');
end

%% compute the structural complexity of each initial cluster
clusterComp = zeros(numClusters,1);
for i = 1 : numClusters
    clusterComp(i) = complexity_fun(IminuszW(initClusters{i}, initClusters{i}));
end

%% compute initial (negative) affinity table (upper trianglar matrix), very slow
if VERBOSE
    disp('   Computing initial table.' );
end
affinityTab = Inf(numClusters);
for j = 1 : numClusters
    for i = 1 : j-1
        affinityTab(i, j) = - conditionalComplexity_fun(IminuszW, initClusters{i}, initClusters{j});
    end
end
affinityTab = bsxfun(@plus, clusterComp, clusterComp') + affinityTab;

if VERBOSE
    disp('   Starting merging process');
end

curGroupNum = numClusters;
while true
    if mod( curGroupNum, 20 ) == 0 && VERBOSE
        disp(['   Group count: ' num2str(curGroupNum)]);
    end
    % Find two clusters with the best affinity
    [minAff, minIndex1] = min(affinityTab(1:curGroupNum, 1:curGroupNum), [], 1);
    [~, minIndex2] = min(minAff);
    minIndex1 = minIndex1(minIndex2);
    if minIndex2 < minIndex1,  [minIndex1, minIndex2] = swap(minIndex1, minIndex2); end

    % merge the two clusters
    new_cluster = unique([initClusters{minIndex1}; initClusters{minIndex2}]);
    % move the second cluster to be merged to the end of the cluster array
    % note that we only need to copy the end cluster's information to
    % the second cluster's position
    if (minIndex2 ~= curGroupNum)
        initClusters{minIndex2} = initClusters{end};
        clusterComp(minIndex2) = clusterComp(curGroupNum);
        % affinityTab is an upper triangular matrix
        affinityTab(1:minIndex2-1, minIndex2) = affinityTab(1:minIndex2-1, curGroupNum);
        affinityTab(minIndex2, minIndex2+1:curGroupNum-1) = affinityTab(minIndex2+1:curGroupNum-1, curGroupNum);
    end

    % update the first cluster and remove the second cluster
    initClusters{minIndex1} = new_cluster;
    initClusters(end) = [];
    clusterComp(minIndex1) = complexity_fun(IminuszW(new_cluster, new_cluster));
    clusterComp(curGroupNum) = myInf;
    affinityTab(:,curGroupNum) = myInf;
    affinityTab(curGroupNum,:) = myInf;
    curGroupNum = curGroupNum - 1;
    if curGroupNum <= groupNumber
        break;
    end

    % update the affinity table for the merged cluster
    for groupIndex1 = 1:minIndex1-1
        affinityTab(groupIndex1, minIndex1) = - conditionalComplexity_fun(IminuszW, initClusters{groupIndex1}, new_cluster);
    end
    for groupIndex1 = minIndex1+1:curGroupNum
        affinityTab(minIndex1, groupIndex1) = - conditionalComplexity_fun(IminuszW, initClusters{groupIndex1}, new_cluster);
    end
    affinityTab(1:minIndex1-1, minIndex1) = clusterComp(1:minIndex1-1) + clusterComp(minIndex1) + affinityTab(1:minIndex1-1, minIndex1);
    affinityTab(minIndex1, minIndex1+1:curGroupNum) = clusterComp(minIndex1+1:curGroupNum)' + clusterComp(minIndex1) + affinityTab(minIndex1, minIndex1+1:curGroupNum);
end

%% generate sample labels
clusterLabels = ones(numSample,1);
for i = 1:length(initClusters)
    clusterLabels(initClusters{i}) = i;
end
if VERBOSE
    disp(['   Final group count: ' num2str(curGroupNum)]);
end

end

function [y, x] = swap (x, y)
end


%%******************************************************************
%% geometric_mean: an example with rotated cone variables
%%
%%  max {prod(d+B*x) : d+B*x > 0, x <= 10}
%%
%%  where B = 4xn matrix,
%%        d = 4x1 vector
%%
%% [blk,At,C,b,xx] = geometric_mean(B,d,solve);
%%
%% E.g. p = 6; m = 4; B = [rand(2,p); -rand(2,p)]; d = rand(m,1);
%%
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function [blk,At,C,b,xx] = geometric_mean(B,d,solve);

   if (nargin == 2); solve = 0; end

   n = size(B,2);
   zz = zeros(1,n);
   r2 = sqrt(2);
   blk{1,1} = 'r'; blk{1,2} = [3,3,3];
   blk{2,1} = 'l'; blk{2,2} = [2];
   At{1,1} = -[B(1,:),0,0,0; B(2,:),0,0,0; zz,r2,0,0; ...
               B(3,:),0,0,0; B(4,:),0,0,0; zz,0,r2,0; ...
	       zz, 1,0,0; zz, 0,1,0; zz, 0,0,r2];
   At{2,1} = -[B(1,:),0,0,0; B(3,:),0,0,0];
   C{1,1} = [d(1:2); 0; d(3:4); 0; 0;0;0];
   C{2,1} = [d(1); d(3)];
   b = [zeros(n,1); 0;0;1];
   blk{3,1} = 'l'; blk{3,2} = n;
   At{3,1} = [eye(n), zeros(n,3)]; C{3,1} = 10*ones(n,1);
   if (solve)
      [bblk,AAt,CC,bb,T] = convertRcone(blk,At,C,b);
      [obj,X,y,Z] = sqlp(bblk,AAt,CC,bb);
      xx = y(1:n);
   end
%%******************************************************************


function [thetamat, dthetamat] ...
         = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, ...
                                     Ftipmat, Mlist, Glist, Slist, dt, ...
                                     intRes)
% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
% Takes thetalist: n-vector of initial joint variables,
%       dthetalist: n-vector of initial joint rates,
%       taumat: An N x n matrix of joint forces/torques, where each row is
%               the joint effort at any time step,
%       g: Gravity vector g,
%       Ftipmat: An N x 6 matrix of spatial forces applied by the
%                end-effector (If there are no tip forces, the user should
%                input a zero and a zero matrix will be used),
%       Mlist: List of link frames {i} relative to {i-1} at the home
%              position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame, in the format
%              of a matrix with the screw axes as the columns,
%       dt: The timestep between consecutive joint forces/torques,
%       intRes: Integration resolution is the number of times integration
%               (Euler) takes places between each time step. Must be an
%               integer value greater than or equal to 1.
% Returns thetamat: The N x n matrix of robot joint angles resulting from
%                   the specified joint forces/torques,
%         dthetamat: The N x n matrix of robot joint velocities.
% This function simulates the motion of a serial chain given an open-loop
% history of joint forces/torques. It calls a numerical integration
% procedure that uses ForwardDynamics.
% Example Inputs (3 Link Robot):
%
% clc; clear;
% thetalist = [0.1; 0.1; 0.1];
% dthetalist = [0.1; 0.2; 0.3];
% taumat = [[3.63, -6.58, -5.57]; [3.74, -5.55, -5.5]; ...
%         [4.31, -0.68, -5.19]; [5.18, 5.63, -4.31]; ...
%         [5.85, 8.17, -2.59]; [5.78, 2.79, -1.7]; ...
%         [4.99, -5.3, -1.19]; [4.08, -9.41, 0.07]; ...
%         [3.56, -10.1, 0.97]; [3.49, -9.41, 1.23]];
% %Initialise robot description (Example with 3 links)
% g = [0; 0; -9.8];
% Ftipmat = ones(size(taumat, 1), 6);
% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]];
% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]];
% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]];
% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]];
% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
% Glist = cat(3, G1, G2, G3);
% Mlist = cat(3, M01, M12, M23, M34);
% Slist = [[1; 0; 1;      0; 1;     0], ...
%        [0; 1; 0; -0.089; 0;     0], ...
%        [0; 1; 0; -0.089; 0; 0.425]];
% dt = 0.1;
% intRes = 8;
% [thetamat, dthetamat] ...
% = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, ...
%                           Ftipmat, Mlist, Glist, Slist, dt, intRes);
% %Output using matplotlib to plot the joint forces/torques
% Tf = size(taumat, 1);
% time=0: (Tf / size(thetamat, 1)): (Tf - (Tf / size(thetamat, 1)));
% plot(time,thetamat(:, 1),'b')
% hold on
% plot(time,thetamat(:, 2), 'g')
% plot(time,thetamat(:, 3), 'r')
% plot(time,dthetamat(:, 1), 'c')
% plot(time,dthetamat(:, 2), 'm')
% plot(time,dthetamat(:, 3), 'y')
% title('Plot of Joint Angles and Joint Velocities')
% xlabel('Time')
% ylabel('Joint Angles/Velocities')
% legend('Theta1', 'Theta2', 'Theta3', 'DTheta1', 'DTheta2', 'DTheta3')
%

taumat = taumat';
Ftipmat = Ftipmat';
thetamat = taumat;
thetamat(:, 1) = thetalist;
dthetamat = taumat;
dthetamat(:, 1) = dthetalist;
for i = 1: size(taumat, 2) - 1
    for j = 1: intRes
       ddthetalist ...
       = ForwardDynamics(thetalist, dthetalist, taumat(:,i), g, ...
                         Ftipmat(:, i), Mlist, Glist, Slist);
       [thetalist, dthetalist] = EulerStep(thetalist, dthetalist, ...
                                           ddthetalist, dt / intRes);
    end
    thetamat(:, i + 1) = thetalist;
    dthetamat(:, i + 1) = dthetalist;
end
thetamat = thetamat';
dthetamat = dthetamat';
end


function [mm]=misssum(X,def)
%MISSSUM sum of a matrix X with NaN's
%
%[mm]=misssum(X,def)
%
%This function calculates the sum of a matrix X.
%X may hold missing elements denoted by NaN's which
%are ignored.
%
%The result is standardized, that is, corrected for the lower
%number of contributing terms.
%
%Check that for no column of X, all values are missing

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% This program is free software; you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation; either version 2 of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more details.
% You should have received a copy of the GNU General Public License along with
% this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
% Street, Fifth Floor, Boston, MA  02110-1301, USA.

%Insert zeros for missing, correct afterwards
missidx = isnan(X);
i = find(missidx);
if ~isempty(i),
    X(i) = zeros(size(i));
end;

%Find the number of real(non-missing objects)
if min(size(X))==1,
   n_real=length(X)-sum(missidx);
   weight=length(X);
else
   n_real=size(X,1)-sum(missidx);
   weight=size(X,1);
end

i=find(n_real==0);
if isempty(i) %All values are real and can be corrected
   mm=weight*sum(X)./n_real;
else %There are columns with all missing, insert missing
   n_real(i)=1;
   mm=weight*sum(X)./n_real;
   mm(i)=i + NaN;
end


function FMatrix=kannumfcc(num,s,Fs)
%Author:        Olutope Foluso Omogbenigun
%Email:         olutopeomogbenigun at hotmail.com
%University:    London Metropolitan University
%Date:          11/09/07
%Syntax:        M=mfccf(num,s, Fs);
%Computes and returns the mfcc coefficients for a speech signal s
%where num is the required number of MFCC coefficients. It utilises the
%function 'melbankm' from the toolbox Voicebox by Mike Brooks copyright(c)
%1997 (GNU General Public License), freely available on the internet,
%to implement the triangular mel filter bank

n=512;              %Number of FFT points
Tf=0.025;           %Frame duration in seconds
N=Fs*Tf;            %Number of samples per frame
fn=24;              %Number of mel filters
l=length(s);        %total number of samples in speech
Ts=0.01;            %Frame step in seconds
FrameStep=Fs*Ts;    %Frame step in samples
a=1;
b=[1, -0.97];       %a and b are high pass filter coefficients

noFrames=floor(l/FrameStep);    %Maximum no of frames in speech sample
FMatrix=zeros(noFrames-2, num); %Matrix to hold cepstral coefficients
lifter=1:num;                   %Lifter vector index
lifter=1+floor((num)/2)*(sin(lifter*pi/num));%raised sine lifter version

if mean(abs(s)) > 0.01
    s=s/max(s);                     %Normalises to compensate for mic vol differences
end

%Segment the signal into overlapping frames and compute MFCC coefficients
for i=1:noFrames-2
    frame=s((i-1)*FrameStep+1:(i-1)*FrameStep+N);  %Holds individual frames
    Ce1=sum(frame.^2);          %Frame energy
    Ce2=max(Ce1,2e-22);         %floors to 2 X 10 raised to power -22
    Ce=log(Ce2);
    framef=filter(b,a,frame);   %High pass pre-emphasis filter
    F=framef.*hamming(N);       %multiplies each frame with hamming window
    FFTo=fft(F,N);              %computes the fft
    melf=melbankm(fn,n,Fs);     %creates 24 filter, mel filter bank
    halfn=1+floor(n/2);
    spectr1=log10(melf*abs(FFTo(1:halfn)).^2);%result is mel-scale filtered
    spectr=max(spectr1(:),1e-22);
    c=dct(spectr);              %obtains DCT, changes to cepstral domain
    c(1)=Ce;                    %replaces first coefficient
    coeffs=c(1:num);            %retains first num coefficients
    ncoeffs=coeffs.*lifter';    %Multiplies coefficients by lifter value
    FMatrix(i, :)=ncoeffs';     %assigns mfcc coeffs to succesive rows i
end


function [knn_dist, nn_dist] = smooth_knn_dist(distances, k, varargin)
%SMOOTH_KNN_DIST Compute a continuous version of the distance to the kth
% nearest neighbor. That is, this is similar to knn-distance but allows
% continuous k values rather than requiring an integral k. In esscence we
% are simply computing the distance such that the cardinality of fuzzy set
% we generate is k.
%
% [knn_dist, nn_dist] = SMOOTH_KNN_DIST(distances, k, local_connectivity,
% n_iter, bandwidth)
%
% Parameters
% ----------
% distances: array of size (n_samples, n_neighbors)
%     Distances to nearest neighbors for each samples. Each row should be a
%     sorted list of distances to a given sample's nearest neighbors.
%
% k: double
%     The number of nearest neighbors to approximate for.
%
% n_iter: double (optional, default 64)
%     We need to binary search for the correct distance value. This is the
%     max number of iterations to use in such a search.
%
% local_connectivity: double (optional, default 1)
%     The local connectivity required -- i.e. the number of nearest
%     neighbors that should be assumed to be connected at a local level.
%     The higher this value the more connected the manifold becomes
%     locally. In practice this should be not more than the local intrinsic
%     dimension of the manifold.
%
% bandwidth: double (optional, default 1)
%     The target bandwidth of the kernel, larger values will produce
%     larger return values.
%
% Returns
% -------
% knn_dist: array of size (n_samples, 1)
%     The distance to kth nearest neighbor, as suitably approximated.
%
% nn_dist: array of size (n_samples, 1)
%     The distance to the 1st nearest neighbor for each point.
%
%   AUTHORSHIP
%   Math Lead & Primary Developer:  Connor Meehan <connor.gw.meehan@gmail.com>
%   Secondary Developer: Stephen Meehan <swmeehan@stanford.edu>
%   Bioinformatics Lead:  Wayne Moore <wmoore@stanford.edu>
%   Provided by the Herzenberg Lab at Stanford University
%   License: BSD 3 clause
%
    p=parseArguments();
    parse(p,varargin{:});
    args=p.Results;
    local_connectivity = args.local_connectivity;
    n_iter = args.n_iter;
    bandwidth = args.bandwidth;
    same_set = args.same_set;

    SMOOTH_K_TOLERANCE = 1e-5;
    MIN_K_DIST_SCALE = 1e-3;

    height = size(distances,1);

    target = log2(k)*bandwidth;

    lo = zeros(height,1);
    hi = Inf(height,1);
    mid = ones(height,1);

    zero_dists = sum(distances == 0, 2);

    if any(zero_dists==size(distances,2))
        warning('There are at least n_neighbors identical data points in the raw data. Results may be inaccurate.');
    end

    index = floor(local_connectivity);
    interpolation = local_connectivity - index;
    aug_dists = [lo distances repmat(max(distances, [], 2), [1 index+1])];

    idx = sub2ind(size(aug_dists), (1:height)', zero_dists + index + 1);
    rho = aug_dists(idx) + interpolation*(aug_dists(idx) - aug_dists(idx+height));

    if same_set
        d = distances(:,2:end) - rho;
    else
        d = distances(:,1:end) - rho;
    end

    for n = 1:n_iter

        summands = exp(-max(0, d./repmat(mid, [1 size(d, 2)])));

        psum = sum(summands, 2);

        if all(abs(psum - target) < SMOOTH_K_TOLERANCE)
            break
        end

        b = ~(psum > target).*hi;
        b(isnan(b)) = 0;

        hi = (psum > target).*mid + b;
        lo = (psum > target).*lo + ~(psum > target).*mid;

        c = (psum > target).*((lo + hi)/2);
        c(isnan(c)) = 0;

        mid = c + ~(psum > target).*min(2*lo, (lo + hi)/2);

    end

    result = mid;

    result = max(result, MIN_K_DIST_SCALE * ((rho > 0).*mean(distances,2) + (rho == 0).*mean(mean(distances))));

    knn_dist = result;
    nn_dist = rho;
end

    function p=parseArguments(varargin)
        p = inputParser;
        addParameter(p,'local_connectivity', 1);
        addParameter(p,'n_iter', 64);
        addParameter(p,'bandwidth', 1);
        addParameter(p,'same_set','true');
    end


function M = lbs_matrix(V,W)
  % LBS_MATRIX  construct a matrix that when multiplied against a column of
  % affine transformation entries computes new coordinates of the vertices
  %
  % M = lbs_matrix(V,W)
  %
  % Input:
  %   V  #V by dim list of vertex rest positions
  %   W  #W by #handles list of correspondence weights
  % Output:
  %   M  #V * dim by #handles * dim * (dim+1) matrix such that
  %     new_V(:) = LBS(V,W,A) = reshape(M * A,size(V)), where A is a column
  %     vectors formed by the entries in each handle's dim by dim+1
  %     transformation matrix. Specifcally, A =
  %       reshape(permute(Astack,[3 1 2]),n*dim*(dim+1),1)
  %     or A = [Lxx;Lyx;Lxy;Lyy;tx;ty], and likewise for other dim
  %     if Astack(:,:,i) is the dim by (dim+1) transformation at handle i
  %
  % Example:
  %  MLBS = lbs_matrix(V,W);
  %  n = size(V,1);
  %  dim = size(V,2);
  %  m = size(W,2);
  %  % stack of identity transformations
  %  Astack = repmat([eye(dim,dim) zeros(dim,1)],[1 1 m]);
  %  % collect transformations into column
  %  A = reshape(permute(Astack,[3 1 2]),m*dim*(dim+1),1);
  %  % apply transformations
  %  new_V = MLBS*A;
  %  new_V = reshape(new_V,[n dim]);
  %
  %  % Alternative:
  %  % Q is #W by 4 list of quats, T is #W by 3 list of translations
  %  % A is #W*4 by 3 stack of transposed affine transformations
  %  A = reshape(cat(2,permute(quat2mat(Q),[2 1 3]),permute(T,[2 3 1])),3,[])';
  %  % M is #V by #W*4 skinning matrix
  %  % M = (𝟙ᵀ⊗ [V 𝟙]) ⊙ (W ⊗ 𝟙ᵀ)
  %  %M = kron(ones(1,size(W,2)),[V ones(size(V,1),1)]).* ...
  %  %    kron(W,ones(1,size(V,2)+1));
  %  M = reshape([V ones(size(V,1),1)].*permute(W,[1 3 2]),size(V,1),[]);
  %  U = M*A;
  %

  % number of mesh (domain) vertices
  n = size(V,1);
  assert(n == size(W,1));
  % dimension of mesh
  dim = size(V,2);
  % number of handles
  m = size(W,2);

  % M = zeros(V*dim,m*dim*(dim+1));

  % repeat vertex positions so that VV(:,:,i) gives #V by #handles matrix where
  % each column is ith coordinates of vertices
  VV = permute(repmat(V,[1 1 m]),[1 3 2]);
  % multiply each column in VV by respective weights for that handle
  VVW = VV.*repmat(W,[1 1 dim]);
  % matrix of zeros
  Z = zeros(n,m);
  switch dim
  case 2
    M = [ ...
      VVW(:,:,1) Z          VVW(:,:,2) Z          W Z; ...
      Z          VVW(:,:,1) Z          VVW(:,:,2) Z W];
  case 3
    M = [ ...
      VVW(:,:,1) Z          Z          VVW(:,:,2) Z          Z          VVW(:,:,3) Z          Z          W Z Z; ...
      Z          VVW(:,:,1) Z          Z          VVW(:,:,2) Z          Z          VVW(:,:,3) Z          Z W Z; ...
      Z          Z          VVW(:,:,1) Z          Z          VVW(:,:,2) Z          Z          VVW(:,:,3) Z Z W];
  otherwise
    error('Only dim=2 or dim=3 supported');
  end

end


function [ radius ] = atomic_radius( Z )
%ATOMIC_RADIUS Get atomic radius of element.
%   r = ATOMIC_RADIUS(Z) returns the atomic radius of element number Z in
%   Angstroms, as given in J. Chem. Phys. 47, 1300 (1967) doi:
%   10.1063/1.1712084. Z can also be a string the the element symbol. For
%   unrecognized symbols/element numbers, a a value of 1.0 is returned.
%
%   See also CHEMSYM2NUMBER, NUMBER2CHEMSYM

    if ischar(Z)
        Z = chemsym2number(Z);
    end

    default_r = 1.0;

    r = [ ...
     0.53 ... H
     0.31 ... He
     1.67 ... Li
     1.12 ... Be
     0.87 ... B
     0.67 ... C
     0.56 ... N
     0.48 ... O
     0.42 ... F
     0.38 ... Ne
     1.90 ... Na
     1.45 ... Mg
     1.18 ... Al
     1.11 ... Si
     0.98 ... P
     0.88 ... S
     0.79 ... Cl
     0.71 ... Ar
     2.43 ... K
     1.94 ... Ca
     1.84 ... Sc
     1.76 ... Ti
     1.71 ... V
     1.66 ... Cr
     1.61 ... Mn
     1.56 ... Fe
     1.52 ... Co
     1.49 ... Ni
     1.45 ... Cu
     1.42 ... Zn
     1.36 ... Ga
     1.25 ... Ge
     1.14 ... As
     1.03 ... Se
     0.94 ... Br
     0.88 ... Kr
     2.65 ... Rb
     2.19 ... Sr
     2.12 ... Y
     2.06 ... Zr
     1.98 ... Nb
     1.90 ... Mo
     1.83 ... Tc
     1.78 ... Ru
     1.73 ... Rh
     1.69 ... Pd
     1.65 ... Ag
     1.61 ... Cd
     1.56 ... In
     1.45 ... Sn
     1.33 ... Sb
     1.23 ... Te
     1.15 ... I
     1.08 ... Xe
     2.98 ... Cs
     2.53 ... Ba
     default_r ... La (missing value)
     default_r ... Ce (missing value)
     2.47 ... Pr
     2.06 ... Nd
     2.05 ... Pm
     2.38 ... Sm
     2.31 ... Eu
     2.33 ... Gd
     2.25 ... Tb
     2.28 ... Dy
     2.26 ... Ho
     2.26 ... Er
     2.22 ... Tm
     2.22 ... Yb
     2.17 ... Lu
%{
     ... Hf
      ... Ta
      ... W
      ... Re
      ... Os
      ... Ir
      ... Pt
      ... Au
      ... Hg
      ... Tl
      ... Pb
      ... Bi
      ... Po
      ... At
      ... Rn
      ... Fr
      ... Ra
      ... Ac
      ... Th
      ... Pa
      ... U
      ... Np
      ... Pu
      ... Am
      ... Cm
      ... Bk
      ... Cf
      ... Es
      ... Fm
      ... Md
      ... No
      ... Lr
      ... Rf
      ... Db
      ... Sg
      ... Bh
      ... Hs
      ... Mt
%}
     ];
     if Z <= numel(r)
        radius = r(Z);
     else
         radius = default_r; % default radius
     end
end


function [v_INTERP,ind_Start,ind_End] = FitzHugh_Nagumo_1d(dt_IBM,T_final)
%function FitzHugh_Nagumo_1d(dt_IBM,T_final)

%
% This script solves the FitzHugh-Nagumo Equations in 1d, which are
% a reduced order model of the more complicated Hodgkin-Huxley Equations.
%
% Author:  Nick Battista
% Created: 09/11/2015
% University: UNC-CH
%
% Equations:
% dv/dt = D*Laplacian(v) + v*(v-a)*(v-1) - w - I(t)
% dw/dt = eps*(v-gamma*w)
%
% Inputs:
% dt_IBM: time-step from immersed boundary code
% T_final: final time for immersed boundary simulation
%
% Variables & Parameters:
% v(x,t): membrane potential
% w(x,t): blocking mechanism
% D:      diffusion rate of potential
% a:      threshold potential
% gamma:  resetting rate
% eps:    strength of blocking
% I(t):   initial condition for applied activation
%

%USER-INPUT FROM MODEL:
numPtsAlongTube = 78;    % # of Lag. Muscle Pts Along Tube
ind_Start = 155;         % first index for muscle between tube in spring count
ind_End = 232;           % last index for muscle between tube in spring count
dt_IBM = dt_IBM*2500;    % scale time-info
T_final_Ca = T_final;    % Stays on time-scale of IBM
T_final = T_final * 2500;% scale time-info

% Save Movie?
PLAY_MOVIE = 0; % (1 for YES, 0 for NO)

% Parameters in model %
D = 10.0;       % Diffusion coefficient
a = 0.3;        % Threshold potential (Note: a=0.3 is traveling wave value, a=0.335 is interesting)
gamma = 1.0;    % Resetting rate (Note: large values give 'funky thick' traveling wave, gamma = 1.0 is desired)
eps = 0.001;    % Blocking strength (Note: eps = 0.001 is desired)
I_mag = 0.05;   % Activation strength

% Discretization/Simulation Parameters %
factor = 4;     % 4x the resolution of the FHN model than IBM for tube
N = factor*numPtsAlongTube-1; % # of discretized points for FHN model (finer resolution than IBM mesh)
L = 500;        % Length of domain, [0,L]
dx = L/N;       % Spatial Step
x = 0:dx:L;     % Computational Domain

% Temporal  Parameters %
Np = 10;              % Set the number of pulses
pulse = T_final/Np;   % determines the length of time between pulses.
numFHN = 10;          % # that relates # of time-steps of FHN to IBM
dt = dt_IBM / numFHN; % Time-step for FitzHugh-Nagumo (fraction of IBM time-step)
NT = T_final / dt;    % Total # of time-steps to be taken for FHN
NT_IBM = T_final / dt_IBM; % Total # of time-steps for IBM
i1 = 0.25;%0.475;     % fraction of total length where current starts
i2 = 0.3;%0.525;      % fraction of total length where current ends
dp = pulse/50;        % Set the duration of the current pulse
pulse_time = 0;       % pulse time is used to store the time that the next pulse of current will happen
IIapp=zeros(1,N+1);   % this vector holds the values of the applied current along the length of the neuron
dptime = T_final/100; % This sets the length of time frames that are saved to make a movie.

% Initialization %
v = zeros(1,N+1);
w = v;
t=0;
ptime = 0;
store = 1;       % counter for storing data to match time-steps from IBM
tVec = 0:dt:T_final;
Nsteps = length(tVec);
vStore = zeros(NT_IBM,N+1); vStore(store,:) = v;
wStore = zeros(NT_IBM,N+1); wStore(store,:) = w;
IIappStore=vStore;
store = store+1; % update storage counter

% Compute Calcium-Dynamics a-priori for activation wave!
% (returns Ca: free calcium ions, Caf: bound to filaments Ca-ions)
fprintf('     --> Solving Calcium Dynamics Model\n');
[Ca,Caf] = Calcium_Dynamics(Nsteps,T_final_Ca);
fprintf('     --> Finished calculating Calcium Dynamics\n');

%
% **** % **** BEGIN SIMULATION! **** % **** %
%
for i=2:Nsteps;

     % Update the time
    t = t+dt;

    % Give Laplacian
    DD_v_p = give_Me_Laplacian(v,dx);

    % Gives activation wave (either from prescribed or Calcium-Dynamics model)
    %[IIapp,pulse_time] = Iapp(pulse_time,i1,i2,I_mag,N,pulse,dp,t,IIapp);
    IIapp = Iapp_from_Calcium_Dynamics(i,i1,i2,N,Caf);

    % Update potential and blocking mechanism, using Forward Euler
    vN = v + dt * ( D*DD_v_p - v.*(v-a).*(v-1) - w + IIapp );
    wN = w + dt * ( eps*( v - gamma*w ) );

    % Update time-steps
    v = vN;
    w = wN;

    % Store time-step values
    if mod(i-1,numFHN) == 0
        vStore(store,:) = v;
        wStore(store,:) = w;
        IIappStore(store,:) = IIapp;
        store = store + 1;
    end

    % PLAY MOVIE?
    if PLAY_MOVIE == 1
        %This is used to determine if the current time step will be a frame in the movie
        if t > ptime,
            figure(1)
            plot(x, v);
            axis([0 L -0.5 1.5]);
            xlabel('Distance (x)');
            ylabel('Electropotenital (v)');
            ptime = ptime+dptime;
            fprintf('Time(s): %d\n',t);
            pause(0.01);
        end
    end

end %END TIME-STEPPING LOOP

% Compute electro-potential at associated Lagrangian Pts.
v_INTERP = compute_IBM_Potential_At_IBM_Lag_Pts(factor,numPtsAlongTube,vStore);
%IIapp_INTERP = compute_IBM_Potential_At_IBM_Lag_Pts(factor,numPtsAlongTube,IIappStore);

% TEST SOLUTION
%  pause();
%  x=1:1:78;
%  for i=1:10:length(v_INTERP(:,1))
%     plot(x,v_INTERP(i,:),'-');
%     %plot(x,IIapp_INTERP(i,:),'-');
%     axis([0 79 -0.5 1.5]);
%     pause(0.01);
%  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes the electro-potential, v(x,t), at the correct points
% along the IBM's Lagrangian Structure
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [v_INTERP] = compute_IBM_Potential_At_IBM_Lag_Pts(factor,numPtsAlongTube,vStore)

% v: row: electro-potential
% factor: resolution of FHN:IBM
% vStore: membrane potential along tube

v_INTERP = zeros(length(vStore(:,1)),numPtsAlongTube);

ct = 1;
for j=1:length(vStore(1,:))
   if ( mod(j-1,factor) == 0 )
       v_INTERP(:,ct) = vStore(:,j);
       ct = ct+1;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: the CALCIUM inspired injection function, Iapp = activation wave
% for system, and returns the activation signal between i1 and i2 in
% geometry along tube
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function IIapp = Iapp_from_Calcium_Dynamics(ith_step,i1,i2,N,Caf)

    % Resets activation to zero
    IIapp = zeros(1,N+1);

    % Activates the proper region based on Calcium-Dynamics
    for j=(floor(i1*N):floor(i2*N))
        coeff = 0.5;                 % Coefficient to scale activation wave accordingly
        IIapp(j) = coeff*Caf(ith_step); % Activation of bound Calcium to filaments
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: the injection function, Iapp = activation wave for system, and
% returns both the activation as well as updated pulse_time
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [app,pulse_time] = Iapp(pulse_time,i1,i2,I_mag,N,pulse,dp,t,app)

    %Check to see if there should be a pulse
    if t > (pulse_time),

        % Sets pulsing region to current amplitude of I_mag x\in[i1*N,i2*N]
        for j=(floor(i1*N):floor(i2*N)),
            app(j) = I_mag;
        end

        % Checks if the pulse is over & then resets pulse_time to the next pulse time.
        if t > (pulse_time+dp),
            pulse_time = pulse_time+pulse;
        end

    else

        % Resets to no activation
        app = zeros(1,N+1);

    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: gives Laplacian of the membrane potential, note: assumes
% periodicity and uses the 2nd order central differencing operator.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function DD_v = give_Me_Laplacian(v,dx)

Npts = length(v);
DD_v = zeros(1,Npts);

for i=1:Npts
   if i==1
       %DD_v(i) = ( v(i+1) - 2*v(i) + v(end) ) / dx^2;
       DD_v(i) = ( v(i+1) - 2*v(i) + 0 ) / dx^2;
   elseif i == Npts
       %DD_v(i) = ( v(1) - 2*v(i) + v(i-1) ) / dx^2;
       DD_v(i) = ( 0 - 2*v(i) + v(i-1) ) / dx^2;
   else
       DD_v(i) = ( v(i+1) - 2*v(i) +  v(i-1) ) /dx^2;
   end

end


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

%Empty workspace and close figures
close all;
clear;

%Number of BS antennas
M = 100;

%Angular standard deviation in the local scattering model (in degrees)
ASDs = [10 30];

%Set the nominal angle of the desired UE
varphiDesired = pi/6;

%Set range of nominal angles of the interfering UE
varphiInterfererDegrees = -180:1:180;
varphiInterfererRadians = varphiInterfererDegrees*(pi/180);

%Define the antenna spacing (in number of wavelengths)
antennaSpacing = 1/2; %Half wavelength distance

%Preallocate matrix for storing the simulation results
variance = zeros(length(varphiInterfererRadians),length(ASDs));

%% Go through the range of ASDs
for n = 1:length(ASDs)

    %Output simulation progress
    disp([num2str(n) ' ASDs out of ' num2str(length(ASDs))]);

    %Compute spatial correlation matrix of the desired UE
    R1 = functionRlocalscattering(M,varphiDesired,ASDs(n),antennaSpacing);

    %Go through all angles of the interfering UE
    for r = 1:length(varphiInterfererRadians)

        %Compute spatial correlation matrix of the interfering UE
        R2 = functionRlocalscattering(M,varphiInterfererRadians(r),ASDs(n),antennaSpacing);

        %Compute variance of favorable propagation according to (2.19)
        variance(r,n) = real(trace(R1*R2)/(trace(R1)*trace(R2)));

    end

end

%% Plot the simulation results
figure;
hold on; box on;

plot(varphiInterfererDegrees,variance(:,1),'r--','LineWidth',1);
plot(varphiInterfererDegrees,variance(:,2),'b-.','LineWidth',1);
plot(varphiInterfererDegrees,1/M*ones(length(varphiInterfererDegrees),1),'k-','LineWidth',1);

xlabel('Angle of interfering UE [degree]');
ylabel('Variance in (2.19)');
xlim([-180 180]);

legend('Gaussian, ASD 10^o','Gaussian, ASD 30^o','Uncorrelated','Location','NorthWest');


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

% Author: Nathanael Perraudin
% Date  : 8 decembre 2014

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

if nargin<8
    At = A;
end

if nargin<9
    param = struct;
end

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

N = size(xl,2);

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

alpha_in = zeros(N,size(y,2));

if ~isfield(G,'D');
    G = gsp_adj2vec(G);
end
if ~isfield(G,'lmax');
    G = gsp_estimate_lmax(G);
end

paramtv.verbose = param.verbose - 1;
ftv.eval = @(x) sum(lambda*gsp_norm_tv(G,K*x));
ftv.prox = @(x,T) gsp_prox_tv(x,lambda*T,G,paramtv);

if tau >0

    fp.eval = @(x) tau * sum(gsp_norm_tik(K,x));
    fp.grad = @(x) 2*tau*K*x;

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

    ftot.grad = @(x) ffid.grad(x) + fp.grad(x);
    ftot.eval = @(x) ffid.eval(x) + fp.eval(x);

    param.gamma = 0.5/(tau*nu+nu^2*param.nu^2+lambda*nu*G.lmax);

    [sol,infos] = forward_backward(alpha_in, ftv, ftot, param);
else

    fp.eval = @(x)  x'*K*x;
    fp.grad = @(x) 2*K*x;

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

    param.gamma = 0.5/(nu+lambda*nu*G.lmax);

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

end

end


function [] = ControlPanel()

% initialize control variables
Lx = 200*10^-3; % billet size
Ly = 200*10^-3;
dx = 01*10^-3;
dy = 01*10^-3;
dt = 0.001;

% Temperature definitions
initial_Temperature = 510+273;
left_Temperature = 65+273;
upper_Temperature = 65+273;
right_Temperature = 65+273;
bottom_Temperature = 65+273;
BC_temperature = left_Temperature;
new_Temperature = BC_temperature;
reference_Temperature = 200+273;

% Secondary variables
boolean = 0;
timeRequired = 0;
time = 26;

% Material properties
alpha = 6.584*10^-5; % Steel
rho = 2700;
Cp = 900;

courant_Number = alpha*dt/(dx^2);

% Define computational domain (Geometry)

% storage parameters
x_intervals = Lx/dx + 1;
y_intervals = Ly/dy + 1;

% Matrices
T_old =  zeros(x_intervals,y_intervals); % 2-D storage for previous time step
T_new = zeros(x_intervals,y_intervals); % 2-D storage for current time step
%T = zeros((time/dt),x_intervals,y_intervals); % Mega Temperature matrix to stroe in 2-D for each time step, hence 3-D

% Initialize domain
T_old = InitializeSolution(T_old,initial_Temperature,left_Temperature,upper_Temperature,right_Temperature,bottom_Temperature,x_intervals,y_intervals);
T_boundary = zeros(1,time/dt);

% formulation : EXPLICIT scheme
for time_index = 1:1:(time/dt)

    T_new = ExplicitScheme(T_old,alpha,dx,dy,dt,x_intervals,y_intervals);
    % Introduce the water bath dynaic BC factor
    new_Temperature = CalculatedBathAdjustedBoundaryTemperature(T_new,T_old,x_intervals,y_intervals,dt,rho,Cp,new_Temperature);
    boolean = ExitConditionEvaluation(T_new,reference_Temperature,x_intervals,y_intervals);

    if(boolean == 1 && timeRequired == 0)
        timeRequired = time_index;
    end

    T_boundary(time_index) = new_Temperature;

    T_old = ReInitializeSolution(T_new,new_Temperature,x_intervals,y_intervals);

end

T_plot = T_new';
T_plotting = CalculatePlottingMatrix(T_plot,x_intervals,y_intervals); % Plotting matrix

%Contour plot on the 2-D spatial domain
T_constant = initial_Temperature:.001:max([initial_Temperature,left_Temperature,upper_Temperature,right_Temperature,bottom_Temperature]);
[C,h] = contour(T_plotting,T_constant);

end


% THE DEMO PRESENTS AN ERROR PROPAGATION USING A MONTE-CARLO METHOD
% The error in the positioning of the joints is propagated to an error
% in the position of the end effector

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.

close all

M=1500; %number of particles

%load arm parameters
robot=load_robot('ABB', 'IRB140');

%standard deviation AT EACH JOINT
sigmaq=0.017;%rad

%find errors around this pose
q=[pi/2 -pi/2 0 0 0 0]';

puntos=[];
for i=1:M,
    qi = q + [normrnd(0, sigmaq, robot.DOF, 1)];
    T=directkinematic(robot, qi);
    puntos=[puntos; T(1,4) T(2,4) T(3,4)];
end

adjust_view(robot)
drawrobot3d(robot,q), hold on
plot3(puntos(:,1),puntos(:,2), puntos(:,3),'r.')


function [A,h] = build_A_h_3d(dphi,cert,phiGrad,transformatioinModel)
% BUILD_A_H_LINEAR3D Builds the equation system A*p = h
%
% INPUT ARGUMENTS
% dphi                  - Phase-difference
% cert                  - Certainty
% phiGrad               - Phase gradient
% transformationModel   - Transformation model (translation or affine)
%
% OPTIONAL INPUT ARGUMENTS
% N/A
%
% OUTPUT ARGUMENTS
% A 					- A matrix
% h 					- h vector
%
% See "Phase-Based Multidimensional Volume Registration" by Hemmendorf et al
% or "PHASE BASED VOLUME REGISTRATION USING CUDA" by Eklund et al for a
% detailed description of how the equation system is set up. Note that here
% we use a differenct S(x)p then the one used in the papers. This is done
% in order to be consistent with functions build_G_h_linear2d and
% build_G_h_linear3d.

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

sz = size(dphi(:,:,:,1));

dphiX = vec(dphi(:,:,:,1));
dphiY = vec(dphi(:,:,:,2));
dphiZ = vec(dphi(:,:,:,3));
certX = vec(cert(:,:,:,1));
certY = vec(cert(:,:,:,2));
certZ = vec(cert(:,:,:,3));
phiGradX = vec(phiGrad(:,:,:,1));
phiGradY = vec(phiGrad(:,:,:,2));
phiGradZ = vec(phiGrad(:,:,:,3));

switch transformatioinModel
    case 'translation'
        A = [   sum(certX.*phiGradX.^2),    0,                          0; ...
                0,                          sum(certY.*phiGradY.^2),    0; ...
                0,                          0,                          sum(certZ.*phiGradZ.^2)];
        h = [   sum(certX.*dphiX.*phiGradX); ...
                sum(certY.*dphiY.*phiGradY); ...
                sum(certZ.*dphiZ.*phiGradZ)];
    case {'rigid','affine'}
        [x,y,z] = meshgrid(1:sz(2),1:sz(1),1:sz(3));
        x = x - sz(2)/2 - 0.5;
        y = y - sz(1)/2 - 0.5;
        z = z - sz(3)/2 - 0.5;
        x = x(:);
        y = y(:);
        z = z(:);

        A = [   sum(certX.*phiGradX.^2.*x.^2),  sum(certX.*phiGradX.^2.*x.*y),  sum(certX.*phiGradX.^2.*x.*z)       0,                                  0,                              0,                              0,                              0,                              0,                              sum(certX.*phiGradX.^2.*x), 0,                          0; ...
                0,                              sum(certX.*phiGradX.^2.*y.^2),  sum(certX.*phiGradX.^2.*y.*z)       0,                                  0,                              0,                              0,                              0,                              0,                              sum(certX.*phiGradX.^2.*y), 0,                          0; ...
                0,                              0,                              sum(certX.*phiGradX.^2.*z.^2)       0,                                  0,                              0,                              0,                              0,                              0,                              sum(certX.*phiGradX.^2.*z), 0,                          0; ...
                0,                              0,                              0,                                  sum(certY.*phiGradY.^2.*x.^2),      sum(certY.*phiGradY.^2.*x.*y),  sum(certY.*phiGradY.^2.*x.*z),  0,                              0,                              0,                              0,                          sum(certY.*phiGradY.^2.*x), 0; ...
                0,                              0,                              0,                                  0,                                  sum(certY.*phiGradY.^2.*y.^2),  sum(certY.*phiGradY.^2.*y.*z),  0,                              0,                              0,                              0,                          sum(certY.*phiGradY.^2.*y), 0; ...
                0,                              0,                              0,                                  0,                                  0,                              sum(certY.*phiGradY.^2.*z.^2),  0,                              0,                              0,                              0,                          sum(certY.*phiGradY.^2.*z), 0; ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              sum(certZ.*phiGradZ.^2.*x.^2),  sum(certZ.*phiGradZ.^2.*x.*y),  sum(certZ.*phiGradZ.^2.*x.*z),  0,                          0,                          sum(certZ.*phiGradZ.^2.*x); ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              0,                              sum(certZ.*phiGradZ.^2.*y.^2),  sum(certZ.*phiGradZ.^2.*y.*z),  0,                          0,                          sum(certZ.*phiGradZ.^2.*y); ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              0,                              0,                              sum(certZ.*phiGradZ.^2.*z.^2),  0,                          0,                          sum(certZ.*phiGradZ.^2.*z); ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              0,                              0,                              0,                              sum(certX.*phiGradX.^2),    0,                          0; ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              0,                              0,                              0,                              0,                          sum(certY.*phiGradY.^2),    0; ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              0,                              0,                              0,                              0,                          0,                          sum(certZ.*phiGradZ.^2)];

        h = [   sum(certX.*dphiX.*phiGradX.*x); ...
                sum(certX.*dphiX.*phiGradX.*y); ...
                sum(certX.*dphiX.*phiGradX.*z); ...
                sum(certY.*dphiY.*phiGradY.*x); ...
                sum(certY.*dphiY.*phiGradY.*y); ...
                sum(certY.*dphiY.*phiGradY.*z); ...
                sum(certZ.*dphiZ.*phiGradZ.*x); ...
                sum(certZ.*dphiZ.*phiGradZ.*y); ...
                sum(certZ.*dphiZ.*phiGradZ.*z); ...
                sum(certX.*dphiX.*phiGradX); ...
                sum(certY.*dphiY.*phiGradY); ...
                sum(certZ.*dphiZ.*phiGradZ)];

        A = A + transpose(A) - diag(diag(A));
end


function [num, den, gamma] = designIIR(phi, D, m0, h, M)

% designIIR: design filters F_i(z) (as in Section III.B.)
%
% Usage:    [num, den, gamma] = designIIR(phi, D, m0, h, M)
%
% INPUTS:
%   phi: input rational transfer functions
%   D:   vector of fractional delays
%   m0:  system delay tolerance
%   h:   fast sampling interval
%   M:   superresolution factor (integer)
%
% OUTPUTS:
%   gamma:  the H infinity norm of the induced error system K
%   num: numerator vectors
%   den: denominator vectors
%
% filter F_i(z) will be an IIR filter with num{i} is the coefficients of
% the numerator and den{i} is the coefficients of the denominator.
%
% See also: getF, demo

% Get the ingeter and residual of the delays
m = floor(D(:)/h);
d = D(:) - m * h;

% Get the digital system as in Prop. 1 and 2
Ad = getAd(phi, h, d);
Bd = getBd(phi, h, d);
Cd = getCd(phi, h, d);
Dd = zeros(size(Cd,1), size(Bd,2));

% The Integer Delay Operator
sysd = IntDelayOp([m0; m]);

% Get the digital system as in Prop. 3 by taking into account the integer
% delay operators
sys = sminreal( sysd * ss(Ad, Bd, Cd, Dd, -1) );    % sminreal to reduce the system's dimension on the fly
% sys = sysd * ss(Ad, Bd, Cd, Dd, -1);

% Get the system P (see Fig. 6)
[Ap, Bp, Cp, Dp, p, q] = getP(sys, M);

% Convert P into analog, since hinfsyn works in analog domain
[Ac, Bc, Cc, Dc] = Digital2Analog(Ap, Bp, Cp, Dp);

% Design in continuous time
P = pck(Ac, Bc, Cc, Dc);

ubd = 1;
lbd = 0;
inc = .0001;

[F, g, gamma] = hinfsyn(P,p,q,lbd,ubd,inc);

% Get the synthesis system F
[Af, Bf, Cf, Df] = unpck(F);

% Convert F back to digital domain
[Af, Bf, Cf, Df] = Analog2Digital(Af, Bf, Cf, Df);

% Convert to filter coefficients
[num, den] = getF(Af, Bf, Cf, Df, M);


%KNNR Trainable Nearest Neighbor Regression
%
%    Y = KNNR(X,K)
%    Y = X*KNNR([],K)
%    Y = X*KNNR(K)
%
% INPUT
%   X    Regression dataset, used for training
%   K    number of neighbors (default K=3)
%
% OUTPUT
%   Y    k-nearest neighbor regression
%
% DESCRIPTION
% Define a k-Nearest neighbor regression on dataset X.
%
% SEE ALSO (PRTools Guide)
% LINEARR, TESTR, PLOTR

% Copyright: D.M.J. Tax, D.M.J.Tax@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function y = knnr(varargin)

  mapname = 'KNN regression';
  argin = shiftargin(varargin,'scalar');
  argin = setdefaults(argin,[],3);

  if mapping_task(argin,'definition')

    y = define_mapping(argin,'untrained',mapname);

	elseif mapping_task(argin,'training')			% Train a mapping.

		[x,k] = deal(argin{:});
    [n,d] = size(x);
    W.x = +x;
    W.y = gettargets(x);
    W.k = k;
    y = prmapping(mfilename,'trained',W,1,d,1);
    y = setname(y,'k-nearest neighbor regression');

  else                                      % Evaluation

    [x,v] = deal(argin{1:2});
    w = getdata(v);
    [n,d] = size(x);
    D = distm(+x,w.x);
    [sD,I] = sort(D,2);
    if n==1
      out = mean(w.y(I(:,1:w.k)));
    else
      out = mean(w.y(I(:,1:w.k)),2);
    end
    y = setdat(x,out);

  end


function value = r4_besk1 ( x )

%*****************************************************************************80
%
%% R4_BESK1 evaluates the Bessel function K of order 1 of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 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 Bessel function K of order 1 of X.
%
  persistent bk1cs
  persistent ntk1
  persistent xmax
  persistent xmin
  persistent xsml

  if ( isempty ( ntk1 ) )

    bk1cs = [ ...
       0.0253002273389477705, ...
      -0.353155960776544876, ...
      -0.122611180822657148, ...
      -0.0069757238596398643, ...
      -0.0001730288957513052, ...
      -0.0000024334061415659, ...
      -0.0000000221338763073, ...
      -0.0000000001411488392, ...
      -0.0000000000006666901, ...
      -0.0000000000000024274, ...
      -0.0000000000000000070 ]';

    ntk1 = r4_inits ( bk1cs, 11, 0.1 * r4_mach ( 3 ) );
    xmin = exp ( max ( log ( r4_mach ( 1 ) ), ...
      - log ( r4_mach ( 2 ) ) ) + 0.01 );
    xsml = sqrt ( 4.0 * r4_mach ( 3 ) );
    xmax = - log ( r4_mach ( 1 ) );
    xmax = xmax - 0.5 * xmax * log ( xmax ) ...
      / ( xmax + 0.5 ) - 0.01;

  end

  if ( x <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_BESK1 = Fatal error!\n' );
    fprintf ( 1, '  X <= 0.\n' );
    error ( 'R4_BESK1 = Fatal error!' )
  elseif ( x <= xsml )
    y = 0.0;
    value = log ( 0.5 * x ) * r4_besi1 ( x ) + ( 0.75 ...
      + r4_csevl ( 0.5 * y - 1.0, bk1cs, ntk1 ) ) / x;
  elseif ( x <= 2.0 )
    y = x * x;
    value = log ( 0.5 * x ) * r4_besi1 ( x ) + ( 0.75 ...
      + r4_csevl ( 0.5 * y - 1.0, bk1cs, ntk1 ) ) / x;
  elseif ( x <= xmax )
    value = exp ( - x ) * r4_besk1e ( x );
  else
    value = 0.0;
  end

  return
end


function y = MVL(Ai, Bi, Aj, Bj, t)

% MVL: compute the integral Mij(t) of equation (51)
%
% Usage:    y = MVL(Ai, Bi, Aj, Bj, t)
%
% INPUTS:
%       Ai, Aj, Bi, Bj: matrices of the operators Phi_i(s) and Phi_j(s)
%       t: upper limit of the integral
%
% OUTPUT:
%       y = MVL(t) = int_0^t expm(tau*Ai)*Bi*Bj'*expm(tau*Aj')dtau
%
% SEE ALSO: reference [19]

F = expm([-Ai                                   Bi*Bj';
          zeros( size(Aj',1), size(Ai,2) )      Aj'] * t );

y = expm(Ai*t) * F( 1:size(Ai,1), (1+size(Ai,2)):end );


function [Y1 h11]=derotated_dtcwt(I1,n,biot,Qshift)
%     X -> 2D real matrix/Image
%
%     nlevels -> No. of levels of wavelet decomposition
%
%     biort ->  'antonini'   => Antonini 9,7 tap filters.
%               'legall'     => LeGall 5,3 tap filters.
%               'near_sym_a' => Near-Symmetric 5,7 tap filters.
%               'near_sym_b' => Near-Symmetric 13,19 tap filters.
%
%     qshift -> 'qshift_06' => Quarter Sample Shift Orthogonal (Q-Shift) 10,10 tap filters,
%                              (only 6,6 non-zero taps).
%               'qshift_a' =>  Q-shift 10,10 tap filters,
%                              (with 10,10 non-zero taps, unlike qshift_06).
%               'qshift_b' => Q-Shift 14,14 tap filters.
%               'qshift_c' => Q-Shift 16,16 tap filters.
%               'qshift_d' => Q-Shift 18,18 tap filters.
%
%
%     Y1     -> The real lowpass image from the final level
%     h11     -> A cell array containing the 6 complex highpass subimages
%     for each level.
%clear all;
%clc;
[Y1,h1] = dtwavexfm2(I1,n,biot,Qshift);
%[Y2,h2] = dtwavexfm2(I2,n,biot,Qshift);

h11{n}=h1{n};
for k=n:-1:2
    for m=1:6
        xp=imresize(h1{k}(:,:,m),2);%
        argxp=angle(xp);
        argx=angle(h1{k-1}(:,:,m));
        argx=argx-2.*argxp;
        absx=abs(h1{k-1}(:,:,m));
        xa=absx.*cos(argx);
        xb=absx.*sin(argx);
        h11{k-1}(:,:,m)=complex(xa,xb);
    end
end
%figure;
%cimage5(h11{1}(:,:,4));
%figure;
%cimage5(h1{1}(:,:,4));
%figure;
%cimage5(h11{2}(:,:,4));
%figure;
%cimage5(h1{2}(:,:,4));


%DEMO_REGRESSION_SPARSE2  Regression demo comparing different sparse
%                         approximations with optimization of inducing
%                         variables
%
%  Description
%    A regression problem with one input variable and one output
%    variable with Gaussian noise. The output is assumed to be
%    realization of additive functions and Gaussian noise.
%
%    For standard full GP demonstration, see for example
%    DEMO_REGRESSION1, and for detailed discussion, Rasmussen and
%    Williams (2006). For more sparse demonstrations including use
%    of compact support covariance functions see DEMO_REGRESSION2,
%    DEMO_MODELASSESMENT2, and DEMO_SPARSEAPPROX.
%
%    In this demo, sparse approximations for the full GP model are
%    compared. We use
%      - FIC, fully independent conditional
%      - DTC, deterministic training conditional
%      - VAR, variational approach
%    For illustration purposes the hyperparameters from the full GP
%    are used for the sparse models and only the inducing variables
%    are optimised.
%
%    For technical details, see Quinonero-Candela and Rasmussen
%    (2005) for the FIC and DTC models and Titsias (2009) for the
%    VAR model.
%
%    We use a simple one dimensional data set to present the three
%    methods.
%
%  See also DEMO_REGRESSION1, DEMO_REGRESSION2, DEMO_REGRESSION_SPARSE1
%
%
%  References:
%
%    Quinonero-Candela, J. and Rasmussen, C. E. (2005). A Unifying
%    View of Sparse Approximate Gaussian Process Regression. Journal
%    of Machine Learning Research.
%
%    Rasmussen, C. E. and Williams, C. K. I. (2006). Gaussian
%    Processes for Machine Learning. The MIT Press.
%
%    Titsias, M. K. (2009). Variational Model Selection for Sparse
%    Gaussian Process Regression. Technical Report, University of
%    Manchester.

% Copyright (c) 2010 Heikki Peura, Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% Set randomstream for reproducing same results
prevstream=setrandstream();

% Start by creating 1D data
xx=linspace(1,10,901);

% Choose a subset of data so that the data are less dense in the right end.
% xt are the inputs and yt are the outputs, xstar are the values we want to
% predict.
x1=logspace(0,1,100);
x1=round(x1*100)-99;
x=xx(x1)';
y=2*sin(4*x)+0.2*randn(size(x));
xt=[1:0.01:14]';
[n,nin] = size(x);

fprintf('Full GP\n')
% Initialize full GP with a squared exponential component and set
% priors for their parameters.
pl = prior_t('s2', 1);
pm = prior_logunif();
pn = prior_logunif();

gpcfse = gpcf_sexp('lengthScale',0.5,'magnSigma2',1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
lik = lik_gaussian('sigma2', 0.1, 'sigma2_prior', pn);

gp = gp_set('lik', lik, 'cf', gpcfse, 'jitterSigma2', 1e-6);

% Set the options for the optimization
opt=optimset('TolFun',1e-4,'TolX',1e-4);
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

[Eft_full, Varft_full] = gp_pred(gp, x, y, xt);
Varft_full = Varft_full + gp.lik.sigma2;

figure
% Blue crosses are the initial inducing input locations, red ones are
% the optimised ones. Black circles represent the distance to the next
% optimized location, with a dashed trendline.');

subplot(2,2,1);hold on;
plot(xt,Eft_full,'k', 'LineWidth', 2)
plot(xt,Eft_full-2.*sqrt(Varft_full),'--','Color',[0 0.5 0])
plot(xt,Eft_full+2.*sqrt(Varft_full),'--','Color',[0 0.5 0])
plot(x,y,'.', 'MarkerSize',7)
ylim([-3 3])
title('FULL GP')

fprintf('FIC GP\n')
% Run FIC approximation for the same data: choose the inducing
% inputs Xu, then proceed with the inference with the optimized
% parameters from the full GP: here, we optimize only the locations
% of the inducing inputs for the FIC model.
Xu=round(10+90*rand(18,1))/10; % Random placement

% Change type to FIC, add inducing inputs, and optimize only inducing inputs
gp_fic = gp_set(gp, 'type','FIC','X_u',Xu,'infer_params','inducing');

% Set the options for the optimization
opt=optimset('TolFun',1e-4,'TolX',1e-4);
% Optimize with the scaled conjugate gradient method
gp_fic=gp_optim(gp_fic,x,y,'opt',opt);

[Eft_fic, Varft_fic] = gp_pred(gp_fic, x, y, xt);
Varft_fic = Varft_fic + gp_fic.lik.sigma2;

subplot(2,2,2);hold on;
h1=plot(xt,Eft_fic,'k', 'LineWidth', 2);
h2=plot(xt,Eft_fic-2.*sqrt(Varft_fic),'--','Color',[0 0.5 0]);
plot(xt,Eft_fic+2.*sqrt(Varft_fic),'--','Color',[0 0.5 0])
h3=plot(x,y,'.', 'MarkerSize',7);
h4=plot(Xu, -2.8, 'bx', 'MarkerSize', 5, 'LineWidth', 2);
h5=plot(gp_fic.X_u, -3, 'rx', 'MarkerSize', 5, 'LineWidth', 2);
legend([h1  h2 h3 h4(1) h5(1)],'Ef','95% CI','Data','Initial X_u','Optimized X_u')
% plot diff of sorted X_u and regress line for that
%XuSorted=sort(gp_fic.X_u);
%dXuSorted=diff(XuSorted);
%bb=regress(dXuSorted,[ones(size(dXuSorted)) XuSorted(1:end-1)]);
%plotbb=bb(1)+(min(XuSorted):0.1:max(XuSorted))*bb(2);
%plot(XuSorted(1:end-1),dXuSorted,'ko');
%plot(min(XuSorted):0.1:max(XuSorted),plotbb,'k--')
ylim([-3 3])
title('FIC')

fprintf('VAR GP\n')
% Run the VAR model similarly to the FIC model with the same
% starting inducing inputs. The difference in the optimized results
% is notable. The VAR model places the inducing inputs quite evenly
% (slightly increasing as the data becomes more sparse), with
% predictions closely matching the full GP model. The other two
% sparse approximations yield less reliable results.
gp_var = gp_set(gp,'type','VAR','X_u',Xu,'infer_params','inducing');

% Set the options for the optimization
opt=optimset('TolFun',1e-4,'TolX',1e-4);
% Optimize with the scaled conjugate gradient method
gp_var=gp_optim(gp_var,x,y,'opt',opt);

[Eft_var, Varft_var] = gp_pred(gp_var, x, y, xt);
Varft_var = Varft_var + gp_var.lik.sigma2;

subplot(2,2,4);hold on
plot(xt,Eft_var,'k', 'LineWidth', 2)
plot(xt,Eft_var-2.*sqrt(Varft_var),'--','Color',[0 0.5 0])
plot(xt,Eft_var+2.*sqrt(Varft_var),'--','Color',[0 0.5 0])
plot(x,y,'.', 'MarkerSize',7)
plot(gp_var.X_u, -3, 'rx', 'MarkerSize', 5, 'LineWidth', 2)
plot(Xu, -2.8, 'bx', 'MarkerSize', 5, 'LineWidth', 2)
% plot diff of sorted X_u and regress line for that
%XuSorted=sort(gp_var.X_u);
%dXuSorted=diff(XuSorted);
%bb=regress(dXuSorted,[ones(size(dXuSorted)) XuSorted(1:end-1)]);
%plotbb=bb(1)+(min(XuSorted):0.1:max(XuSorted))*bb(2);
%plot(XuSorted(1:end-1),dXuSorted,'ko');
%plot(min(XuSorted):0.1:max(XuSorted),plotbb,'k--')
ylim([-3 3])
title('VAR')

fprintf('DTC GP\n')
% Run the DTC model similarly to the FIC model with the same starting
% inducing inputs. The difference in the optimized results is notable.
gp_dtc = gp_set(gp,'type','DTC','X_u',Xu,'infer_params','inducing');

% Set the options for the optimization
opt=optimset('TolFun',1e-4,'TolX',1e-4);
% Optimize with the scaled conjugate gradient method
gp_dtc=gp_optim(gp_dtc,x,y,'opt',opt);

[Eft_dtc, Varft_dtc] = gp_pred(gp_dtc, x, y, xt);
Varft_dtc = Varft_dtc + gp_dtc.lik.sigma2;

subplot(2,2,3);hold on
plot(xt,Eft_dtc,'k', 'LineWidth', 2)
plot(xt,Eft_dtc-2.*sqrt(Varft_dtc),'--','Color',[0 0.5 0])
plot(xt,Eft_dtc+2.*sqrt(Varft_dtc),'--','Color',[0 0.5 0])
plot(x,y,'.', 'MarkerSize',7)
plot(gp_dtc.X_u, -3, 'rx', 'MarkerSize', 5, 'LineWidth', 2)
plot(Xu, -2.8, 'bx', 'MarkerSize', 5, 'LineWidth', 2)
% plot diff of sorted X_u and regress line for that
%XuSorted=sort(gp_dtc.X_u);
%dXuSorted=diff(XuSorted);
%bb=regress(dXuSorted,[ones(size(dXuSorted)) XuSorted(1:end-1)]);
%plotbb=bb(1)+(min(XuSorted):0.1:max(XuSorted))*bb(2);
%plot(XuSorted(1:end-1),dXuSorted,'ko');
%plot(min(XuSorted):0.1:max(XuSorted),plotbb,'k--')
ylim([-3 3])
title('DTC')

% Set back initial random stream
setrandstream([],prevstream);


function [ value_min, value_mean, value_max, value_var ] = ...
 tet_mesh_quality1 ( node_num, node_xyz, tetra_order, tetra_num, tetra_node )

%*****************************************************************************80
%
%% TET_MESH_QUALITY1 returns a tet mesh quality factor.
%
%  Discussion:
%
%    The tet mesh quality measure is the minimum of the
%    corresponding tetrahedron quality measure, over all tetrahedrons in the
%    tet mesh.
%
%    This routine is designed for an order-4 tet mesh.  Order 10 tet meshes
%    may be input, but the extra nodes are ignored.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, double NODE_XYZ(3,NODE_NUM), the nodes.
%
%    Input, integer TETRA_ORDER, the order of the tetrahedrons.
%
%    Input, integer TETRA_NUM, the number of tetrahedrons.
%
%    Input, integer TETRA_NODE(TETRA_ORDER,TETRA_NUM), the indices of the nodes
%    that make up the tetrahedrons.
%
%    Output, double VALUE_MIN, VALUE_MEAN, VALUE_MAX, VALUE_VAR,
%    the minimum, mean, maximum and variance of the quality measure.
%
  dim_num = 3;

  for tetra = 1 : tetra_num

    tetrahedron(1:dim_num,1:4) = node_xyz(1:dim_num,tetra_node(1:4,tetra));

    tetrahedron_quality(tetra) = tetrahedron_quality1_3d ( tetrahedron );

  end

  value_max  = max  ( tetrahedron_quality(1:tetra_num) );
  value_min  = min  ( tetrahedron_quality(1:tetra_num) );
  value_mean = mean ( tetrahedron_quality(1:tetra_num) );
  value_var  = var  ( tetrahedron_quality(1:tetra_num) );

  return
end


function [vertices, faces] = curveToMesh(curve, varargin)
%CURVETOMESH  Create a mesh surrounding a 3D curve
%
%   [V, F] = curveToMesh(CURVE)
%   Computes the vertices and the faces of the mesh surrounding the
%   specified 3D curve.
%
%   [V, F] = curveToMesh(CURVE, THICKNESS)
%   Specifies the thickness of the mesh (distance between mesh vertices and
%   curve vertices). Default is 0.5.
%
%   [V, F] = curveToMesh(CURVE, THICKNESS, NCORNERS)
%   Also specifies the number of mesh vertices around each curve vertex.
%   Default is 8.
%
%
%   Example
%     % Creates a tubular mesh around a trefoil knot curve
%     t = linspace(0, 2*pi, 200)';
%     x = sin(t) + 2 * sin(2 * t);
%     y = cos(t) - 2 * cos(2 * t);
%     z = -sin(3 * t);
%     curve = [x, y, z];
%     [v2, f2] = curveToMesh(curve, .5, 16);
%     figure;
%     drawMesh(v2, f2);
%     axis equal; view(3);
%     axis([-4 4 -4 4 -2 2]);
%
%   See also
%     meshes3d, torusMesh, surfToMesh

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2015-01-07,    using Matlab 8.4.0.150421 (R2014b)
% Copyright 2015 INRA - Cepia Software Platform.

radius = .1;
if nargin > 1
    radius = varargin{1};
end

nCorners = 8;
if nargin > 2
    nCorners = varargin{2};
end

nNodes = size(curve, 1);
nVerts = nNodes * nCorners;

vertices = zeros(nVerts, 3);

% create reference corners, that will be rotated and translated
t = linspace(0, 2*pi, nCorners + 1)';
t(end) = [];
baseCorners = radius * [cos(t) sin(t) zeros(size(t))];

for iNode = 1:nNodes
    % coordinate of current node
    node = curve(iNode, :);

    % compute local tangent vector
    iNext = mod(iNode, nNodes) + 1;
    tangentVector = normalizeVector3d(curve(iNext, :) - node);

    % convert to spherical coordinates
    [theta, phi, rho] = cart2sph2(tangentVector); %#ok<ASGLU>

    % apply transformation to place corners around current node
    rotY = createRotationOy(theta);
    rotZ = createRotationOz(phi);
    trans = createTranslation3d(node);
    transformMatrix = trans * rotZ * rotY;
    corners = transformPoint3d(baseCorners, transformMatrix);

    % concatenate with other corners
    vertices( (1:nCorners) + (iNode - 1) * nCorners, :) = corners;
end

% indices of vertices
inds = (1:nVerts)';
add1 = repmat([ones(nCorners-1, 1) ; 1-nCorners], nNodes, 1);

% generate faces
faces = [inds ...
    mod(inds + add1 - 1, nVerts) + 1 ...
    mod(inds + nCorners + add1 - 1, nVerts) + 1 ...
    mod(inds + nCorners - 1, nVerts) + 1];