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

%*****************************************************************************80
%
%% BASIS_MATRIX_TMP computes Q = T * MBASIS * P
%
%  Discussion:
%
%    YDATA is a vector of data values, most frequently the values of some
%    function sampled at uniformly spaced points.  MBASIS is the basis
%    matrix for a particular kind of spline.  T is a vector of the
%    powers of the normalized difference between TVAL and the left
%    endpoint of the interval.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LEFT, indicats that TVAL is in the interval
%    [ TDATA(LEFT), TDATA(LEFT+1) ], or that this is the "nearest"
%    interval to TVAL.
%    For TVAL < TDATA(1), use LEFT = 1.
%    For TDATA(NDATA) < TVAL, use LEFT = NDATA - 1.
%
%    Input, integer N, the order of the basis matrix.
%
%    Input, real MBASIS(N,N), the basis matrix.
%
%    Input, integer NDATA, the dimension of the vectors TDATA and YDATA.
%
%    Input, real TDATA(NDATA), the abscissa values.  This routine
%    assumes that the TDATA values are uniformly spaced, with an
%    increment of 1.0.
%
%    Input, real YDATA(NDATA), the data values to be interpolated or
%    approximated.
%
%    Input, real TVAL, the value of T at which the spline is to be
%    evaluated.
%
%    Output, real YVAL, the value of the spline at TVAL.
%
  if ( left == 1 )
    arg = 0.5E+00 * ( tval - tdata(left) );
    first = left;
  elseif ( left < ndata - 1 )
    arg = tval - tdata(left);
    first = left - 1;
  elseif ( left == ndata - 1 )
    arg = 0.5E+00 * ( 1.0E+00 + tval - tdata(left) );
    first = left - 1;
  end
%
%  TVEC(I) = ARG**(N-I).
%
  tvec(n,1) = 1.0E+00;
  for i = n-1 : -1 : 1
    tvec(i,1) = arg * tvec(i+1,1);
  end

  yval = 0.0E+00;
  for j = 1 : n
    yval = yval + ( tvec(1:n,1)' * mbasis(1:n,j) ) * ydata(first - 1 + j);
  end

  return
end


function V = construct_V_matrix(measurements)
%function V = construct_V_matrix(measurements)
%
% This function constructs and returns the translational data matrix V
% defined in equation (16) of the paper

% Copyright (C) 2016 by David M. Rosen

D = length(measurements.t{1}); % D = dimension of SE(d)
N = max(max(measurements.edges));  % N = number of nodes in the pose graph
M = size(measurements.edges,1); % M = number of edges in the pose graph

% The number of nonzero elements in V; there are D nonzero elements for
% each translational measurement, plus D*N slots to store the sum of the
% observations emanating from each node

NNZ = D*M + D*N;

rows = zeros(1, NNZ);
cols = zeros(1, NNZ);
vals = zeros(1, NNZ);

for e = 1:M

    %Extract measurement data
    i = measurements.edges(e, 1);  %The node that this edge *leaves*
    j = measurements.edges(e, 2);  %The node that this edge *enters*

    tij = measurements.t{e};
    tau_ij = measurements.tau{e};

    %Set V_ji = -tau_ij * t_ij'

    cmin = D*(e-1) + 1;
    cmax = D*(e-1) + D;

    rows(cmin:cmax) = j*ones(1, D);
    cols(cmin:cmax) = D*(i-1) + 1 : D*(i-1) + D;
    vals(cmin:cmax) = -tau_ij * tij';

    %Add this observation to the weighted sum of measurements emanating
    %from node i

    cmin = D*M + D*(i - 1) + 1;
    cmax = D*M + D*(i - 1) + D;
    vals(cmin : cmax) = vals(cmin : cmax) + tau_ij*tij';
end

% Fill in the indices for the running sums

for i = 1:N
    cmin = D*M + D*(i-1) + 1;
    cmax = D*M + D*(i-1) + D;

    rows(cmin:cmax) = i*ones(1,D);
    cols(cmin:cmax) = [D*(i-1) + 1 : D*(i - 1) + D];
end

V = sparse(rows, cols, vals, N, D*N);


function [ n_data, a, x, fx ] = poisson_cdf_values ( n_data )

%*****************************************************************************80
%
%% POISSON_CDF_VALUES returns some values of the Poisson CDF.
%
%  Discussion:
%
%    CDF(X)(A) is the probability of at most X successes in unit time,
%    given that the expected mean number of successes is A.
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`DiscreteDistributions`]
%      dist = PoissonDistribution [ a ]
%      CDF [ dist, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%    Daniel Zwillinger,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition, CRC Press, 1996, pages 653-658.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real A, the parameter of the function.
%
%    Output, integer X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 21;

  a_vec = [ ...
     0.02E+00, ...
     0.10E+00, ...
     0.10E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     1.00E+00, ...
     1.00E+00, ...
     1.00E+00, ...
     1.00E+00, ...
     2.00E+00, ...
     2.00E+00, ...
     2.00E+00, ...
     2.00E+00, ...
     5.00E+00, ...
     5.00E+00, ...
     5.00E+00, ...
     5.00E+00, ...
     5.00E+00, ...
     5.00E+00, ...
     5.00E+00 ];

  fx_vec = [ ...
     0.9801986733067553E+00, ...
     0.9048374180359596E+00, ...
     0.9953211598395555E+00, ...
     0.6065306597126334E+00, ...
     0.9097959895689501E+00, ...
     0.9856123220330293E+00, ...
     0.3678794411714423E+00, ...
     0.7357588823428846E+00, ...
     0.9196986029286058E+00, ...
     0.9810118431238462E+00, ...
     0.1353352832366127E+00, ...
     0.4060058497098381E+00, ...
     0.6766764161830635E+00, ...
     0.8571234604985470E+00, ...
     0.6737946999085467E-02, ...
     0.4042768199451280E-01, ...
     0.1246520194830811E+00, ...
     0.2650259152973617E+00, ...
     0.4404932850652124E+00, ...
     0.6159606548330631E+00, ...
     0.7621834629729387E+00 ];

  x_vec = [ ...
     0, 0, 1, 0, ...
     1, 2, 0, 1, ...
     2, 3, 0, 1, ...
     2, 3, 0, 1, ...
     2, 3, 4, 5, ...
     6 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function uI = faceinterpolate(u,node,elem,elemType)
%% FACEINTERPOLATE interpolate to face elements (RT0 or BDM1).
%
% uI = faceinterpolate(u,node,elem,elemType) interpolates a given function u
% into the lowesr order RT0 or BDM1 finite element spaces. The coefficient
% is given by the line integral int_e u*n ds. The input elemType can be 'RT0'
% or 'BDM1'.
%
% Example
%
%    maxIt = 5;
%    node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%    elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%    bdEdge = setboundary(node,elem,'Dirichlet');
%    pde = mixBCdata;
%    err = zeros(maxIt,2); N = zeros(maxIt,1);
%    for i =1:maxIt
%      [node,elem,bdEdge] = uniformrefine(node,elem,bdEdge);
%      [u,sigma,M] = PoissonRT0(node,elem,pde,bdEdge);
%      sigmaI = faceinterpolate(pde.Du,node,elem,'RT0');
%      err(i,1) = getL2errorRT0(node,elem,pde.Du,sigmaI,pde.d);
%      err(i,2)=sqrt((sigma-sigmaI)'*M*(sigma-sigmaI));
%      N(i) = size(u,1);
%    end
%     figure;
%     showrate2(N,err(:,1),2,'r-+','||Du - \sigma_I||',...
%               N,err(:,2),2,'b-+','||\sigma^{RT_0} - \sigma_I||');
%
% See also edgeinterpolate, edgeinterpolate1, edgeinterpolate2
%
% Created by Ming Wang at Mar 29, 2011, M-lint modified at May 14, 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('elemType','var'), elemType = 'RT0'; end
%% edge
if size(elem,2) == 2 % the input elem is edge
    edge = elem;
else
    [tempvar,edge] = dofedge(elem);
end
NE = size(edge,1);
edgeVec = node(edge(:,2),:) - node(edge(:,1),:);
nVec = zeros(NE,2);
nVec(:,1) = edgeVec(:,2);
nVec(:,2) = -edgeVec(:,1);

%% dof for RT0
[lambda,weight] = quadpts1(4);
nQuad = size(lambda,1);
uI = zeros(NE,1);
for i = 1:nQuad
    pxy = lambda(i,1)*node(edge(:,1),:)+lambda(i,2)*node(edge(:,2),:);
    flux = u(pxy);
    uI = uI + weight(i)*dot(flux,nVec,2);
end

%% dof for BDM1
if strcmp(elemType,'BDM1')
   uI(NE+1:2*NE) = zeros(NE,1);
   for i = 1:nQuad
        pxy = lambda(i,1)*node(edge(:,1),:)+lambda(i,2)*node(edge(:,2),:);
        flux = u(pxy);
        uI(NE+1:2*NE) = uI(NE+1:2*NE)+ ...
                     weight(i)*3*(lambda(i,1)-lambda(i,2))*dot(flux,nVec,2);
   end
end


function [fL] = bruteloglike(vValues,time_as)
    % bruteloglike calculates the log likelihood function for the modeled aftershock sequence and the maximum likelihood estimate for k, c and p
    %
    % [fL] = bruteloglike(vValues,time_as);
    % -------------------------------------------------------------
    % Reference: Ogata, Estimation of the parameters in the modified Omori formula
    % for aftershock sequences by  the maximum likelihood procedure, J. Phys. Earth, 1983
    % (Formula 6)
    %
    % J. Woessner
    % updated: 29.07.03

    p = vValues(1);
    c = vValues(2);
    k = vValues(3);

    % Setting start end end time
    fTstart = min(time_as);
    fTend = max(time_as);

    if p ~= 1
        fAcp = ((fTend+c).^(1-p)-(fTstart+c).^(1-p))./(1-p);
        %cumnr_model = k/(p-1)*(c^(1-p)-(c+time_as(i)).^(1-p)); % integrated form of MOL
    else
        fAcp = log(fTend+c)-log(fTstart+c);
        %cumnr_model = k*log(time_as(i)/c+1); % integrated form of MOL
    end
    % rms = (sum((i-cumnr_model).^2)/length(i))^0.5; % RMS between observed data and MOL
    % Log likelihood
    nNumEvents = length(time_as);
    fL = -(nNumEvents*log(k)-p*sum(log(time_as+c))-k*fAcp);


function [ r, s, area ] = node_reference_t10 ( )

%*****************************************************************************80
%
%% NODE_REFERENCE_T10 returns the basis nodes for a 10 node triangle.
%
%  Reference Element T10:
%
%    |
%    1  10
%    |  |\
%    |  | \
%    |  8  9
%    |  |   \
%    S  |    \
%    |  5  6  7
%    |  |      \
%    |  |       \
%    0  1--2--3--4
%    |
%    +--0----R---1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real R(10), S(10), the coordinates of the basis nodes.
%
%    Output, real AREA, the area of the element.
%
  r(1) = 0.0;
  s(1) = 0.0;

  r(2) = 1.0 / 3.0;
  s(2) = 0.0;

  r(3) = 2.0 / 3.0;
  s(3) = 0.0;

  r(4) = 1.0;
  s(4) = 0.0;

  r(5) = 0.0;
  s(5) = 1.0 / 3.0;

  r(6) = 1.0 / 3.0;
  s(6) = 1.0 / 3.0;

  r(7) = 2.0 / 3.0;
  s(7) = 1.0 / 3.0;

  r(8) = 0.0;
  s(8) = 2.0 / 3.0;

  r(9) = 1.0 / 3.0;
  s(9) = 2.0 / 3.0;

  r(10) = 0.0;
  s(10) = 1.0;

  area = 0.5;

  return
end


function cout=comp_col2diag(cin);
%COMP_COL2DIAG  transforms columns to diagonals (in a special way)
%
%  This function transforms the first column to the main diagonal. The
%  second column to the first side-diagonal below the main diagonal and so
%  on.
%
%  This way fits well the connection of matrix and spreading function, see
%  spreadfun.
%
%  This function is its own inverse.

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: OK
%   REFERENCE: OK

L=size(cin,1);
cout=zeros(L,assert_classname(cin));

jj=(0:L-1).';
for ii=0:L-1
  cout(ii+1,:)=cin(ii+1,mod(ii-jj,L)+1);
end;


function taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, ...
                                   g, Ftip, Mlist, Glist, Slist)
% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
% Takes thetalist: n-vector of joint variables,
%       dthetalist: n-vector of joint rates,
%       ddthetalist: n-vector of joint accelerations,
%       g: Gravity vector g,
%       Ftip: Spatial force applied by the end-effector expressed in frame
%             {n+1},
%       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.
% Returns taulist: The n-vector of required joint forces/torques.
% This function uses forward-backward Newton-Euler iterations to solve the
% equation:
% taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ...
%           + g(thetalist) + Jtr(thetalist) * Ftip
% Example Input (3 Link Robot):
%
% clear; clc;
% thetalist = [0.1; 0.1; 0.1];
% dthetalist = [0.1; 0.2; 0.3];
% ddthetalist = [2; 1.5; 1];
% g = [0; 0; -9.8];
% Ftip = [1; 1; 1; 1; 1; 1];
% 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]];
% taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, g, ...
%                         Ftip, Mlist, Glist, Slist)
%
% Output:
% taulist =
%   74.6962
%  -33.0677
%   -3.2306

n = size(thetalist, 1);
Mi = eye(4);
Ai = zeros(6, n);
AdTi = zeros(6, 6, n + 1);
Vi = zeros(6, n + 1);
Vdi = zeros(6, n + 1);
Vdi(4: 6, 1) = -g;
AdTi(:, :, n + 1) = Adjoint(TransInv(Mlist(:, :, n + 1)));
Fi = Ftip;
taulist = zeros(n, 1);
for i=1: n
    Mi = Mi * Mlist(:, :, i);
    Ai(:, i) = Adjoint(TransInv(Mi)) * Slist(:, i);
    AdTi(:, :, i) = Adjoint(MatrixExp6(VecTose3(Ai(:, i) ...
                    * -thetalist(i))) * TransInv(Mlist(:, :, i)));
    Vi(:, i + 1) = AdTi(:, :, i) * Vi(:, i) + Ai(:, i) * dthetalist(i);
    Vdi(:, i + 1) = AdTi(:, :, i) * Vdi(:, i) ...
                    + Ai(:, i) * ddthetalist(i) ...
                    + ad(Vi(:, i + 1)) * Ai(:, i) * dthetalist(i);
end
for i = n: -1: 1
    Fi = AdTi(:, :, i + 1)' * Fi + Glist(:, :, i) * Vdi(:, i + 1) ...
         - ad(Vi(:, i + 1))' * (Glist(:, :, i) * Vi(:, i + 1));
    taulist(i) = Fi' * Ai(:, i);
end
end


% "example_nufft1_reverse.m"
% This m-file is an example of applying the NUFFT method "in reverse,"
% meaning that you have a nonuniformly-sampled signal spectrum in hand
% and you want to try to "compute its inverse Fourier transform"
% to get uniform set of signal samples.  In the nomenclature of the 1999
% SIAM J Sci. Comput. paper by Nguyen and Liu, this is "Problem 1."
% (It is somewhat related to MRI reconstruction by gridding.)
% Before doing this, you should ask yourself if that is *really*
% what you want to do.  In most inverse problems, I think it is NOT
% what we should do, as argued in the T-SP paper on this method.
% Instead, we should solve the inverse problem *iteratively*, using
% the NUFFT method (and its adjoint) each iteration.
% Furthermore, the min-max optimality of the NUFFT method was only
% established for the "forward direction:" going from uniform signal
% samples to nonuniform frequency samples.
% (You can replace time and frequency in the above discussion.)
% Anyway, let's try it here and see how it works...

%
% synthesize some spectral data that is nonuniformly spaced
%
No = 2^7;				% number of frequency samples
rng(0)
om = 2*pi*sort(rand(No,1)-0.5);		% random frequency samples on [-pi,pi ]
%om = 2*pi*[-No/2:No/2-1]'/No;		% test with uniform samples

% a spectrum with periodic components, hopefully visible in other domain
X = inline('2 + 2*sin(om*10) + 4*cos(15*om) + 4*cos(20*om)', 'om');
Xm = X(om);

%
% go to the time domain the "exact" (slow) DTFT way.
% this implements essentially equation (1) in Nguyen and Liu (1999)
%
N1 = 2^6;		% # of time samples
n = [0:N1-1]'-N1/2;	% time sample locations
xd = dtft2_adj(Xm, [om 0*om], N1, 1, [N1/2 0]);

%
% now do it the fast NUFFT way
%
if 1 || ~isvar('st')	% create NUFFT structure
	J = 5;		% interpolation neighborhood
	K1 = N1*2;	% two-times oversampling
	st = nufft_init(om, N1, J, K1, N1/2, 'minmax:kb');
end

xn = nufft_adj(Xm, st);	% call ADJOINT to go "in reverse"

%
% compare slow exact to fast NUFFT
%
figure(1), clf, pl=220;
subplot(pl+1)
oo = [-200:200]'/400*2*pi;
plot(om, Xm, '.', oo, X(oo), '-')
xlabel \omega, ylabel X(\omega), title 'Spectrum and samples'

subplot(pl+2)
plot(n, real(xd), 'c.-', n, imag(xd), 'y.-')
xlabel n, ylabel x[n], title 'Exact "nonuniform FT"'

subplot(pl+3)
plot(n, real(xn), 'c.-', n, imag(xn), 'y.-')
xlabel n, ylabel x[n], title 'Fast NUFFT adjoint'

subplot(pl+4)
plot(n, real(xn-xd), 'c.-', n, imag(xn-xd), 'y.-')
xlabel n, ylabel x[n], title 'Approximation Error (very small!)'

%
% ok fine, so if you look at figure 1 you see that
% the NUFFT gives a great approximation to the "exact" formula
% but is the result *really* what you wanted?
% It seems that ideally the result should be 4 spikes with
% no background junk.  If we solved "Problem 5" in Nguyen and Liu,
% then we should get that!  This is how we have approached the
% MRI reconstruction problem, as described in ../mri.
%
% For iterative 2D version, see ../example/mri_example.m

return

%
% here is a different strategy: interpolate the nonuniform data
% onto a uniform grid, then simply take the inverse FFT.
% this didn't work so well because it is a lousy gridding method.
% todo: need to put a better gridding method here!
%
ok = [-N1/2:(N1/2-1)]'/N1*2*pi;
Xg = interp1(om, Xm, ok, 'linear', 'extrap');
xg = fftshift(ifft(fftshift(Xg)));

%xg = xg ./ nufft_sinc(n/N1).^2; % post-compensate?

figure(2), clf, pl=220;
subplot(pl+1)
plot(n, real(xg), 'c.-', n, imag(xg), 'y.-')
xlabel n, ylabel x[n], title 'Gridding "reconstruction"'

Xg = dtft1(xg, om, N1/2);
Xd = dtft1(xd/No, om, N1/2);

subplot(pl+2)
plot(om, real(Xg), '.', om, real(Xm), '-')
xlabel \omega, ylabel X(\omega), title ''

subplot(pl+3)
plot(om, real(Xd), '.', om, real(Xm), '-')
xlabel \omega, ylabel X(\omega), title ''


function K = covNNone(hyp, x, z, i)

% Neural network covariance function with a single parameter for the distance
% measure. The covariance function is parameterized as:
%
% k(x^p,x^q) = sf2 * asin(x^p'*P*x^q / sqrt[(1+x^p'*P*x^p)*(1+x^q'*P*x^q)])
%
% where the x^p and x^q vectors on the right hand side have an added extra bias
% entry with unit value. P is ell^-2 times the unit matrix and sf2 controls the
% signal variance. The hyperparameters are:
%
% hyp = [ log(ell)
%         log(sqrt(sf2) ]
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-09-10.
%
% See also COVFUNCTIONS.M.

if nargin<2, K = '2'; return; end                  % report number of parameters
if nargin<3, z = []; end                                   % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

n = size(x,1);
ell2 = exp(2*hyp(1));
sf2 = exp(2*hyp(2));

sx = 1 + sum(x.*x,2);
if dg                                                               % vector kxx
  K = sx./(sx+ell2);
else
  if xeqz                                                 % symmetric matrix Kxx
    S = 1 + x*x';
    K = S./(sqrt(ell2+sx)*sqrt(ell2+sx)');
  else                                                   % cross covariances Kxz
    S = 1 + x*z'; sz = 1 + sum(z.*z,2);
    K = S./(sqrt(ell2+sx)*sqrt(ell2+sz)');
  end
end

if nargin<4                                                        % covariances
  K = sf2*asin(K);
else                                                               % derivatives
  if i==1                                                          % lengthscale
    if dg
      V = K;
    else
      vx = sx./(ell2+sx);
      if xeqz
        V = repmat(vx/2,1,n) + repmat(vx'/2,n,1);
      else
        vz = sz./(ell2+sz); nz = size(z,1);
        V = repmat(vx/2,1,nz) + repmat(vz'/2,n,1);
      end
    end
    K = -2*sf2*(K-K.*V)./sqrt(1-K.*K);
  elseif i==2                                                        % magnitude
    K = 2*sf2*asin(K);
  else
    error('Unknown hyperparameter')
  end
end


function b = r8cc_vxm ( m, n, nz_num, colptr, rowind, a, x )

%*****************************************************************************80
%
%% R8CC_VXM multiplies a vector times a R8CC matrix.
%
%  Discussion:
%
%    The R8CC format is the double precision sparse compressed column
%    format.  Associated with this format, we have an M by N matrix
%    with NZ_NUM nonzero entries.  We construct the column pointer
%    vector COL of length N+1, such that entries of column J will be
%    stored in positions COL(J) through COL(J+1)-1.  This indexing
%    refers to both the ROW and A vectors, which store the row indices
%    and the values of the nonzero entries.  The entries of the
%    ROW vector corresponding to each column are assumed to be
%    ascending sorted.
%
%    The R8CC format is equivalent to the MATLAB "sparse" format,
%    and the Harwell Boeing "real unsymmetric assembled" (RUA) format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Iain Duff, Roger Grimes, John Lewis,
%    User's Guide for the Harwell-Boeing Sparse Matrix Collection,
%    October 1992
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix.
%
%    Input, integer N, the number of columns of the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero elements in A.
%
%    Input, integer COLPTR(N+1), points to the first element of each column.
%
%    Input, integer ROWIND(NZ_NUM), contains the row indices of the elements.
%
%    Input, real A(NZ_NUM), the matrix.
%
%    Input, real X(M), the vector to be multiplied.
%
%    Output, real B(N), the product A'*X.
%
  b(1:n) = 0.0;

  for j = 1 : n
    for k = colptr(j) : colptr(j+1) - 1
      i = rowind(k);
      b(j) = b(j) + a(k) * x(i);
    end
  end

  return
end


% Fig. 6.12   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

num1=[10 10];
den1=[1 10];
w=logspace(-2,3,100);
[m1,p1]=bode(num1,den1,w);
num2=[10 -10];
[m2,p2]=bode(num2,den1,w);
figure(1)
loglog(w,m1,w,m2);
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.12 Bode Plot for a NMP System (a) magnitude');
grid;
pause;
figure(2)
semilogx(w,p1,w,p2);
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
title('Fig 6.12 (b) phase');


function determ = cheby_van2_determinant ( n )

%*****************************************************************************80
%
%% CHEBY_VAN2_DETERMINANT returns the determinant of the CHEBY_VAN2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real DETERM, the determinant.
%
  if ( n <= 0 )
    determ = 0.0;
  elseif ( n == 1 )
    determ = 1.0;
  else
    determ = r8_mop ( floor ( n / 2 ) ) * sqrt ( 2.0 )^( 4 - n );
  end

  return
end


% CSparse: a Concise Sparse matrix Package.
%
%   Matrices used in CSparse must in general be either sparse and real,
%   or dense vectors.  Ordering methods can accept any sparse matrix.
%
%   cs_add       - sparse matrix addition.
%   cs_amd       - approximate minimum degree ordering.
%   cs_chol      - sparse Cholesky factorization.
%   cs_cholsol   - solve A*x=b using a sparse Cholesky factorization.
%   cs_counts    - column counts for sparse Cholesky factor L.
%   cs_dmperm    - maximum matching or Dulmage-Mendelsohn permutation.
%   cs_dmsol     - x=A\b using the coarse Dulmage-Mendelsohn decomposition.
%   cs_dmspy     - plot the Dulmage-Mendelsohn decomposition of a matrix.
%   cs_droptol   - remove small entries from a sparse matrix.
%   cs_esep      - find an edge separator of a symmetric matrix A
%   cs_etree     - elimination tree of A or A'*A.
%   cs_gaxpy     - sparse matrix times vector.
%   cs_lsolve    - solve a sparse lower triangular system L*x=b.
%   cs_ltsolve   - solve a sparse upper triangular system L'*x=b.
%   cs_lu        - sparse LU factorization, with fill-reducing ordering.
%   cs_lusol     - solve Ax=b using LU factorization.
%   cs_make      - compiles CSparse for use in MATLAB.
%   cs_multiply  - sparse matrix multiply.
%   cs_nd        - generalized nested dissection ordering.
%   cs_nsep      - find a node separator of a symmetric matrix A.
%   cs_permute   - permute a sparse matrix.
%   cs_print     - print the contents of a sparse matrix.
%   cs_qr        - sparse QR factorization.
%   cs_qleft     - apply Householder vectors on the left.
%   cs_qright    - apply Householder vectors on the right.
%   cs_qrsol     - solve a sparse least-squares problem.
%   cs_randperm  - random permutation.
%   cs_sep       - convert an edge separator into a node separator.
%   cs_scc       - strongly-connected components of a square sparse matrix.
%   cs_scc2      - cs_scc, or connected components of a bipartite graph.
%   cs_sparse    - convert a triplet form into a sparse matrix.
%   cs_sqr       - symbolic sparse QR factorization.
%   cs_symperm   - symmetric permutation of a symmetric matrix.
%   cs_transpose - transpose a real sparse matrix.
%   cs_updown    - rank-1 update/downdate of a sparse Cholesky factorization.
%   cs_usolve    - solve a sparse upper triangular system U*x=b.
%   cs_utsolve   - solve a sparse lower triangular system U'*x=b.
%   cspy         - plot a matrix in color.
%   ccspy        - plot the connected components of a matrix.

% Example:
%   help cs_add

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

% helper function:
%   cs_must_compile - return 1 if source code f must be compiled, 0 otherwise


function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R)
% UKF   Unscented Kalman Filter for nonlinear dynamic systems
% [x, P] = ukf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P
% for nonlinear dynamic system (for simplicity, noises are assumed as additive):
%           x_k+1 = f(x_k) + w_k
%           z_k   = h(x_k) + v_k
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
%       v ~ N(0,R) meaning v is gaussian noise with covariance R
% Inputs:   f: function handle for f(x)
%           x: "a priori" state estimate
%           P: "a priori" estimated state covariance
%           h: fanction handle for h(x)
%           z: current measurement
%           Q: process noise covariance
%           R: measurement noise covariance
% Output:   x: "a posteriori" state estimate
%           P: "a posteriori" state covariance
%
% Example:
%{
n=3;      %number of state
q=0.1;    %std of process
r=0.1;    %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2;        % covariance of measurement
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  % nonlinear state equations
h=@(x)x(1);                               % measurement equation
s=[0;0;1];                                % initial state
x=s+q*randn(3,1); %initial state          % initial state with noise
P = eye(n);                               % initial state covraiance
N=20;                                     % total dynamic steps
xV = zeros(n,N);          %estmate        % allocate memory
sV = zeros(n,N);          %actual
zV = zeros(1,N);
for k=1:N
  z = h(s) + r*randn;                     % measurments
  sV(:,k)= s;                             % save actual state
  zV(k)  = z;                             % save measurment
  [x, P] = ukf(f,x,P,h,z,Q,R);            % ekf
  xV(:,k) = x;                            % save estimate
  s = f(s) + q*randn(3,1);                % update process
end
for k=1:3                                 % plot results
  subplot(3,1,k)
  plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')
end
%}
% Reference: Julier, SJ. and Uhlmann, J.K., Unscented Filtering and
% Nonlinear Estimation, Proceedings of the IEEE, Vol. 92, No. 3,
% pp.401-422, 2004.
%
% By Yi Cao at Cranfield University, 04/01/2008
%
L=numel(x);                                 %numer of states
m=numel(z);                                 %numer of measurements
alpha=1e-3;                                 %default, tunable
ki=0;                                       %default, tunable
beta=2;                                     %default, tunable
lambda=alpha^2*(L+ki)-L;                    %scaling factor
c=L+lambda;                                 %scaling factor
Wm=[lambda/c 0.5/c+zeros(1,2*L)];           %weights for means
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta);               %weights for covariance
c=sqrt(c);
X=sigmas(x,P,c);                            %sigma points around x
[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q);          %unscented transformation of process
% X1=sigmas(x1,P1,c);                         %sigma points around x1
% X2=X1-x1(:,ones(1,size(X1,2)));             %deviation of X1
[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R);       %unscented transformation of measurments
P12=X2*diag(Wc)*Z2';                        %transformed cross-covariance
K=P12*inv(P2);
x=x1+K*(z-z1);                              %state update
P=P1-K*P12';                                %covariance update

function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
%Unscented Transformation
%Input:
%        f: nonlinear map
%        X: sigma points
%       Wm: weights for mean
%       Wc: weights for covraiance
%        n: numer of outputs of f
%        R: additive covariance
%Output:
%        y: transformed mean
%        Y: transformed smapling points
%        P: transformed covariance
%       Y1: transformed deviations

L=size(X,2);
y=zeros(n,1);
Y=zeros(n,L);
for k=1:L
    Y(:,k)=f(X(:,k));
    y=y+Wm(k)*Y(:,k);
end
Y1=Y-y(:,ones(1,L));
P=Y1*diag(Wc)*Y1'+R;

function X=sigmas(x,P,c)
%Sigma points around reference point
%Inputs:
%       x: reference point
%       P: covariance
%       c: coefficient
%Output:
%       X: Sigma points

A = c*chol(P)';
Y = x(:,ones(1,numel(x)));
X = [x Y+A Y-A];


%WAVE_BASES  1D Wavelet functions Morlet, Paul, or DOG
%
%  [DAUGHTER,FOURIER_FACTOR,COI,DOFMIN] = ...
%      wave_bases(MOTHER,K,SCALE,PARAM);
%
%   Computes the wavelet function as a function of Fourier frequency,
%   used for the wavelet transform in Fourier space.
%   (This program is called automatically by WAVELET)
%
% INPUTS:
%
%    MOTHER = a string, equal to 'MORLET' or 'PAUL' or 'DOG'
%    K = a vector, the Fourier frequencies at which to calculate the wavelet
%    SCALE = a number, the wavelet scale
%    PARAM = the nondimensional parameter for the wavelet function
%
% OUTPUTS:
%
%    DAUGHTER = a vector, the wavelet function
%    FOURIER_FACTOR = the ratio of Fourier period to scale
%    COI = a number, the cone-of-influence size at the scale
%    DOFMIN = a number, degrees of freedom for each point in the wavelet power
%             (either 2 for Morlet and Paul, or 1 for the DOG)
%
%----------------------------------------------------------------------------
%   Copyright (C) 1995-1998, Christopher Torrence and Gilbert P. Compo
%   University of Colorado, Program in Atmospheric and Oceanic Sciences.
%   This software may be used, copied, or redistributed as long as it is not
%   sold and this copyright notice is reproduced on each copy made.  This
%   routine is provided as is without any express or implied warranties
%   whatsoever.
%----------------------------------------------------------------------------
function [daughter,fourier_factor,coi,dofmin] = ...
	wave_bases(mother,k,scale,param);

mother = upper(mother);
n = length(k);

if (strcmp(mother,'MORLET'))  %-----------------------------------  Morlet
	if (param == -1), param = 6.;, end
	k0 = param;
	expnt = -(scale.*k - k0).^2/2.*(k > 0.);
	norm = sqrt(scale*k(2))*(pi^(-0.25))*sqrt(n);    % total energy=N   [Eqn(7)]
	daughter = norm*exp(expnt);
	daughter = daughter.*(k > 0.);     % Heaviside step function
	fourier_factor = (4*pi)/(k0 + sqrt(2 + k0^2)); % Scale-->Fourier [Sec.3h]
	coi = fourier_factor/sqrt(2);                  % Cone-of-influence [Sec.3g]
	dofmin = 2;                                    % Degrees of freedom
elseif (strcmp(mother,'PAUL'))  %--------------------------------  Paul
	if (param == -1), param = 4.;, end
	m = param;
	expnt = -(scale.*k).*(k > 0.);
	norm = sqrt(scale*k(2))*(2^m/sqrt(m*prod(2:(2*m-1))))*sqrt(n);
	daughter = norm*((scale.*k).^m).*exp(expnt);
	daughter = daughter.*(k > 0.);     % Heaviside step function
	fourier_factor = 4*pi/(2*m+1);
	coi = fourier_factor*sqrt(2);
	dofmin = 2;
elseif (strcmp(mother,'DOG'))  %--------------------------------  DOG
	if (param == -1), param = 2.;, end
	m = param;
	expnt = -(scale.*k).^2 ./ 2.0;
	norm = sqrt(scale*k(2)/gamma(m+0.5))*sqrt(n);
	daughter = -norm*(i^m)*((scale.*k).^m).*exp(expnt);
	fourier_factor = 2*pi*sqrt(2./(2*m+1));
	coi = fourier_factor/sqrt(2);
	dofmin = 1;
else
	error('Mother must be one of MORLET,PAUL,DOG')
end

return


function result = SimulateSSCOMP_3Spaces(D, K, Ng, theta, SNR, shuffle)
% Simulates SSC-OMP algorithm
% D - Ambient space dimension
% K - Subspace dimension
% Ng - Number of vectors per subspace
% theta - angle between disjoint subspaces
% shuffle - whether to shuffle the vectors or not.

    if nargin < 6
        shuffle = false;
    end
    if nargin < 5
        SNR = Inf;
    end
    % number of subspaces = number of clusters
    ns = 3;
    % Let us form the subspaces
    [A, B, C] = spx.la.spaces.three_disjoint_spaces_at_angle(K, deg2rad(theta));
    % Put them together
    X = [A B C];
    % Put them to bigger dimension
    X = spx.la.spaces.k_dim_to_n_dim(X, D);
    % Perform a random orthonormal transformation
    O = orth(randn(D));
    X = O * X;
    % Split them again
    A = X(:, 1:K);
    B = X(:, K + (1:K));
    C = X(:, 2*K + (1:K));
    % coefficients for Ng vectors chosen randomly in subspace A
    coeffs_a = randn(K,Ng);
    % avoid small values
    coeffs_a = 2*sign(coeffs_a) + coeffs_a;
    % coefficients for Ng vectors chosen randomly in subspace B
    coeffs_b = randn(K,Ng);
    % coefficients for Ng vectors chosen randomly in subspace C
    coeffs_c = randn(K,Ng);
    % avoid small values
    coeffs_c = 2*sign(coeffs_c) + coeffs_c;
    % Actual vectors from the three subspaces
    XA = A * coeffs_a;
    XB = B * coeffs_b;
    XC = C * coeffs_c;
    % Prepare the overall set of signals
    X = [XA XB XC];
    % ground through clustering data
    true_labels = [1*ones(Ng,1) ; 2*ones(Ng,1) ; 3*ones(Ng,1)];
    % All signals are expected to  have a K-sparse representation
    if shuffle
        % We also shuffle the signals
        shuffled_indices = randperm(3*Ng);
        X = X(:, shuffled_indices);
        true_labels = true_labels(shuffled_indices);
    end
    if ~isinf(SNR)
        % We need to add some noise in the signals.
        noises = spx.data.noise.Basic.createNoise(X, SNR);
        % Preserve original data
        X0 = X;
        % Add noise
        X = X0 + noises;
    end
    % Keep data for reference.
    result.num_subspaces = 3;
    result.theta = theta;
    result.subspace_dimension = K;
    result.ambient_dimension = D;
    result.num_signals_per_subspace = Ng;
    result.num_total_signals = size(X, 2);
    result.true_labels = true_labels;
    result.X = X;

    tstart = tic;
    % Application of Sparse subspace clustering
    solver = spx.cluster.ssc.SSC_OMP(X, K, ns);
    solver.Quiet = true;
    ssc_result = solver.solve();
    elapsed_time = toc(tstart);
    result.elapsed_time = elapsed_time;
    result.C = solver.Representation;
    % fprintf('Sparse subspace clustering time spent: %.2f seconds\n', elapsed_time);

    cluster_labels = ssc_result.Labels;
    %combined_labels = [true_labels cluster_labels]';
    result.cluster_labels = cluster_labels;

    % Time to compare the clustering
    comparer = spx.cluster.ClusterComparison(true_labels, cluster_labels);
    result.comparison = comparer.fMeasure();
    % fprintf('Sparse subspace clustering results:\n');
    % comparer.printF1MeasureResult(result.comparison);
end


function [node,elem,HB] = cubemesh(box,h)
%% CUBEMESH a uniform mesh of a cube
%
% [node,elem,HB] = cubemesh([x0,x1,y0,y1,z0,z1],h) enerates a uniform mesh
% of the cube [x0,x1]*[y0,y1]*[z0,z1] with mesh size h.
%
% Example
%
%  [node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.5);
%  showmesh3(node,elem);
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

x0 = box(1); x1 = box(2);
y0 = box(3); y1 = box(4);
z0 = box(5); z1 = box(6);
node = [x0,y0,z0; x1,y0,z0; x1,y1,z0; x0,y1,z0; ...
        x0,y0,z1; x1,y0,z1; x1,y1,z1; x0,y1,z1];
elem = [1 2 3 7; 1 4 3 7; 1 5 6 7; 1 5 8 7; 1 2 6 7; 1 4 8 7];
n = ceil(log2(abs(x1-x0)/h));
for k = 1:n
    [node,elem] = uniformrefine3(node,elem);
end
% Set this as an initial grid
N0 = size(node,1);
HB(1:N0,1:3) = repmat((1:N0)',1,3);
HB(1:N0,4) = 0;


function [hess, storedb] = getHessian(problem, x, d, storedb)
% Computes the Hessian of the cost function at x along d.
%
% function [hess, storedb] = getHessian(problem, x, d, storedb)
%
% Returns the Hessian at x along d of the cost function described in the
% problem structure. The cache database storedb is passed along, possibly
% modified and returned in the process.
%
% If an exact Hessian is not provided, an approximate Hessian is returned
% if possible, without warning. If not possible, an exception will be
% thrown. To check whether an exact Hessian is available or not (typically
% to issue a warning if not), use canGetHessian.
%
% See also: getPrecon getApproxHessian canGetHessian

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:

    if isfield(problem, 'hess')
    %% Compute the Hessian using hess.

		is_octave = exist('OCTAVE_VERSION', 'builtin');
		if ~is_octave
			narg = nargin(problem.hess);
		else
			narg = 3;
		end

        % Check whether the hess function wants to deal with the store
        % structure or not.
        switch narg
            case 2
                hess = problem.hess(x, d);
            case 3
                % Obtain, pass along, and save the store structure
                % associated to this point.
                store = getStore(problem, x, storedb);
                [hess store] = problem.hess(x, d, store);
                storedb = setStore(problem, x, storedb, store);
            otherwise
                up = MException('manopt:getHessian:badhess', ...
                    'hess should accept 2 or 3 inputs.');
                throw(up);
        end

    elseif isfield(problem, 'ehess') && canGetEuclideanGradient(problem)
    %% Compute the Hessian using ehess.

        % We will need the Euclidean gradient for the conversion from the
        % Euclidean Hessian to the Riemannian Hessian.
        [egrad, storedb] = getEuclideanGradient(problem, x, storedb);

		is_octave = exist('OCTAVE_VERSION', 'builtin');
		if ~is_octave
			narg = nargin(problem.ehess);
		else
			narg = 3;
		end

        % Check whether the ehess function wants to deal with the store
        % structure or not.
        switch narg
            case 2
                ehess = problem.ehess(x, d);
            case 3
                % Obtain, pass along, and save the store structure
                % associated to this point.
                store = getStore(problem, x, storedb);
                [ehess store] = problem.ehess(x, d, store);
                storedb = setStore(problem, x, storedb, store);
            otherwise
                up = MException('manopt:getHessian:badehess', ...
                    'ehess should accept 2 or 3 inputs.');
                throw(up);
        end

        % Convert to the Riemannian Hessian
        hess = problem.M.ehess2rhess(x, egrad, ehess, d);

    else
    %% Attempt the computation of an approximation of the Hessian.

        [hess, storedb] = getApproxHessian(problem, x, d, storedb);

    end

end


function [ fea, out ] = ex_axistressstrain1( varargin )
%EX_AXISTRESSSTRAIN1 Example for hollow cylider axisymmetric stress-strain.
%
%   [ FEA, OUT ] = EX_AXISTRESSSTRAIN1( VARARGIN ) Example to calculate displacements and stresses
%   in a hollow cylinder in axisymmetric/cylindrical coordinates.
%
%   Ref. 4.1.9 Long (generalized plane strain) cylinder subjected to internal and external pressure.
%   [1] Applied Mechanics of Solids, Allan F. Bower, 2012 (http://solidmechanics.org/).
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       a           scalar {1.5}           Cylinder inner radius
%       b           scalar {2}             Cylinder outer radius
%       l           scalar {3}             Cylinder length
%       pa          scalar {5e3}           Inner load force
%       pb          scalar {20e4}          Outer load force
%       E           scalar {200e9}         Modulus of elasticity
%       nu          scalar {0.3}           Poissons ratio
%       igrid       scalar 0/{1}           Cell type (0=quadrilaterals, 1=triangles)
%       hmax        scalar {0.1}           Max grid cell size
%       sfun        string {sflag2}        Shape function for displacements
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { 'a',        0.9;
            'b',        2;
            'l',        3;
            'pa',       5e3;
            'pb',       20e4;
            'E',        200e9;
            'nu',       0.3;
            'igrid',    0;
            'hmax',     0.01;
            'sfun',     'sflag2';
            'iplot',    1;
            'tol',      5e-2;
            'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

% Geometry and grid.
a = opt.a;
b = opt.b;
l = opt.l;
fea.geom.objects = { gobj_rectangle( a, b, 0, l, 'R1' ) };
if ( opt.igrid==1 )
  fea.grid = gridgen( fea, 'hmax', opt.hmax, 'fid', fid );
else
  fea.grid = rectgrid( ceil((b-a)/opt.hmax), ceil(l/opt.hmax), [a b;0 l] );
  if( opt.igrid<0 )
    fea.grid = quad2tri( fea.grid );
  end
end
n_bdr = max(fea.grid.b(3,:));   % Number of boundaries.

% Axisymmetric stress-strain equation definitions.
fea.sdim = { 'r', 'z' };
fea = addphys( fea, @axistressstrain );
fea.phys.css.eqn.coef{1,end} = { opt.nu };
fea.phys.css.eqn.coef{2,end} = { opt.E  };
fea.phys.css.sfun            = { opt.sfun opt.sfun };   % Set shape functions.

% Boundary conditions.
bctype = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
[bctype{2,:}] = deal( 1 );
fea.phys.css.bdr.coef{1,5} = bctype;

bccoef = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
bccoef{1,2} = opt.pb*b;
bccoef{1,4} = opt.pa*a;
fea.phys.css.bdr.coef{1,end} = bccoef;

% Solve.
fea       = parsephys( fea );
fea       = parseprob( fea );
fea.sol.u = solvestat( fea, 'icub', 1+str2num(strrep(opt.sfun,'sflag','')), 'fid', fid );

% Postprocessing.
n = 20;
r = linspace(a,b,n);
z = l/2*ones(1,n);
pa = opt.pa; pb = -opt.pb; E = opt.E; nu = opt.nu;
% 4.1.9 http://solidmechanics.org/Text/Chapter4_1/Chapter4_1.php#Sect4_1_9
u_ref  = (1+nu)*a^2*b^2/E/(b^2-a^2)*( (pa-pb)./r' + (1-2*nu)*(pa*a^2 - pb*b^2)/a^2/b^2*r' );
sr_ref = (pa*a^2-pb*b^2)/(b^2-a^2) - a^2*b^2/(b^2-a^2)./(r').^2*(pa-pb);
st_ref = (pa*a^2-pb*b^2)/(b^2-a^2) + a^2*b^2/(b^2-a^2)./(r').^2*(pa-pb);
sz_ref = 2*nu*(pa*a^2-pb*b^2)/(b^2-a^2);
u  = evalexpr( fea.phys.css.eqn.vars{3,2}, [r;z], fea );
w  = evalexpr( fea.phys.css.eqn.vars{4,2}, [r;z], fea );
sr = evalexpr( fea.phys.css.eqn.vars{5,2}, [r;z], fea );
st = evalexpr( fea.phys.css.eqn.vars{6,2}, [r;z], fea );
sz = evalexpr( fea.phys.css.eqn.vars{7,2}, [r;z], fea );
if( opt.iplot>0 )
  subplot(1,2,1)
  postplot( fea, 'surfexpr', fea.phys.css.eqn.vars{3,2} )
  title('r-displacement')
  subplot(1,2,2), hold on
  plot(u_ref,r,'r-')
  plot(u,r,'b.')
  legend('exact solution','computed solution')
  xlabel('r')
  grid on
end

% Error checking.
out.erru  = norm( u_ref - u )/norm( u_ref );
out.errw  = norm( w );
out.errsr = norm( sr_ref - sr )/norm( sr_ref );
out.errst = norm( st_ref - st )/norm( st_ref );
out.errsz = norm( sz_ref - sz )/norm( sz_ref );
out.err   = [ out.erru, out.errw, out.errsr, out.errst, out.errsz ];
out.pass  = all(out.err < opt.tol);

if( nargout==0 )
  clear fea out
end


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

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

snr_threshold = 100;

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

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

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

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

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

        % Create the solver for Rank Aware CoSaMP MMV
        solver = spx.pursuit.joint.CoSaMP(Phi, K);
        solver.RankAwareResidual = true;
        result = solver.solve(Y);
        % Solution vectors
        X_Rec = result.Z;
        % Comparison
        cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
        % Reconstruction SNR
        snr = cs.cum_signal_to_noise_ratio;
        success = snr > snr_threshold;
        num_successes.ra_cosamp = num_successes.ra_cosamp + success;

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

save ('bin/success_with_s_comparison.mat');


function y = lnCumGaussian(x)

% LNCUMGAUSSIAN log cumulative distribution for the normalised Gaussian.
% FORMAT
% DESC computes the logarithm of the cumulative Gaussian
% distribution.
% ARG X : input position.
% RETURN y : log probability of the value under the cumulative
% Gaussian.
%
% SEEALSO : erf, erfcx, cumGaussian, lnDiffCumGaussian, gaussOverDiffCumGaussian
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% NDLUTIL

index = find(x< 0);
if length(index)
  y(index) = -.5*x(index).*x(index) + log(.5) + log(erfcx(-sqrt(2)/2* ...
                                                    x(index)));
end
index = find(x>=0);
if length(index)
  y(index) = log(cumGaussian(x(index)));
end
y=reshape(y, size(x));


function value = hexagon_contains_point_2d ( v, p )

%*****************************************************************************80
%
%% HEXAGON_CONTAINS_POINT_2D finds if a point is inside a hexagon in 2D.
%
%  Discussion:
%
%    This test is only valid if the hexagon is convex.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real V(2,6), the vertics, in counterclockwise order.
%
%    Input, real P(2), the point to be tested.
%
%    Output, logical VALUE, is TRUE if X is in the hexagon.
%
  n = 6;
%
%  A point is inside a convex hexagon if and only if it is "inside"
%  each of the 6 halfplanes defined by lines through consecutive
%  vertices.
%
  for i = 1 : n

    j = mod ( i, n ) + 1;

    if (  v(1,i) * ( v(2,j) - p(2  ) ) ...
        + v(1,j) * ( p(2  ) - v(2,i) ) ...
        + p(1  ) * ( v(2,i) - v(2,j) ) < 0.0 )

      value = 0;
      return

    end

  end

  value = 1;

  return
end


classdef ContourFitting < handle
    %CONTOURFITTING  Contour Fitting algorithm using Fourier descriptors
    %
    % Contour fitting matches two contours `z_a` and `z_b` minimizing distance
    % `d(z_a, z_b) = sum_n (a_n - s * b_n * exp(j * (n * alpha + phi)))^2`
    % where `a_n` and `b_n` are Fourier descriptors of `z_a` and `z_b` and `s`
    % is a scaling factor and `phi` is angle rotation and `alpha` is starting
    % point factor adjustement.
    %
    % ## References:
    % [PersoonFu1977]:
    % > E Persoon and King-Sun Fu. "Shape discrimination using fourier
    % > descriptors". IEEE Transactions on Pattern Analysis and Machine
    % > Intelligence, 7(3):170-179, 1977.
    %
    % [BergerRaghunathan1998]:
    % > L Berger, V A Raghunathan, C Launay, D Ausserre, and Y Gallot.
    % > "Coalescence in 2 dimensions: experiments on thin copolymer films and
    % > numerical simulations". The European Physical Journal B - Condensed
    % > Matter and Complex Systems, 2(1):93-99, 1998.
    %
    % See also: cv.ContourFitting.ContourFitting, cv.matchShapes,
    %  cv.ShapeContextDistanceExtractor
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    properties (Dependent)
        % number of Fourier descriptors used in
        % cv.ContourFitting.estimateTransformation equal to number of contour
        % points after resampling.
        CtrSize
        % number of Fourier descriptors used for optimal curve matching in
        % cv.ContourFitting.estimateTransformation when using vector of points
        FDSize
    end

    %% ContourFitting
    methods
        function this = ContourFitting(varargin)
            %CONTOURFITTING  Create ContourFitting object
            %
            %     obj = cv.ContourFitting()
            %     obj = cv.ContourFitting('OptionName',optionValue, ...)
            %
            % ## Options
            % * __CtrSize__ number of contour points after resampling.
            %   default 1024
            % * __FDSize__ number of Fourier descriptors. default 16
            %
            % See also: cv.ContourFitting.estimateTransformation
            %
            this.id = ContourFitting_(0, 'new', varargin{:});
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     obj.delete()
            %
            % See also: cv.ContourFitting
            %
            if isempty(this.id), return; end
            ContourFitting_(this.id, 'delete');
        end

        function [alphaPhiST, d] = estimateTransformation(this, src, ref, varargin)
            %ESTIMATETRANSFORMATION  Fits two closed curves using Fourier descriptors
            %
            %     [alphaPhiST, d] = obj.estimateTransformation(src, ref)
            %     [...] = obj.estimateTransformation(..., 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __src__ Contour defining first shape (source), or Fourier
            %   descriptors if `FD` is true.
            % * __ref__ Contour defining second shape (target), or Fourier
            %   descriptors if `FD` is true.
            %
            % ## Output
            % * __alphaPhiST__ transformation as a 5-elements vector
            %   `[alpha, phi, s, Tx, Ty]`, where:
            %   * __alpha__ starting point factor adjustement
            %   * __phi__ angle rotation in radian
            %   * __s__ scaling factor
            %   * __Tx__, __Ty__ the translation
            % * __d__ distance between `src` and `ref` after matching.
            %
            % ## Options
            % * __FD__ If false then `src` and `ref` are contours, and
            %   if true `src` and `ref` are Fourier descriptors. default false
            %
            % When `FD` is false, it applies cv.ContourFitting.contourSampling
            % and cv.ContourFitting.fourierDescriptor to compute Fourier
            % descriptors.
            %
            % More details in [PersoonFu1977] and [BergerRaghunathan1998].
            %
            % See also: cv.ContourFitting.transformFD,
            %  cv.ContourFitting.contourSampling,
            %  cv.ContourFitting.fourierDescriptor
            %
            [alphaPhiST, d] = ContourFitting_(this.id, 'estimateTransformation', src, ref, varargin{:});
        end
    end

    %% Algorithm
    methods (Hidden)
        function clear(this)
            %CLEAR  Clears the algorithm state
            %
            %     obj.clear()
            %
            % See also: cv.ContourFitting.empty, cv.ContourFitting.load
            %
            ContourFitting_(this.id, 'clear');
        end

        function b = empty(this)
            %EMPTY  Checks if algorithm object is empty
            %
            %     b = obj.empty()
            %
            % ## Output
            % * __b__ Returns true if the algorithm object is empty
            %   (e.g. in the very beginning or after unsuccessful read).
            %
            % See also: cv.ContourFitting.clear, cv.ContourFitting.load
            %
            b = ContourFitting_(this.id, 'empty');
        end

        function save(this, filename)
            %SAVE  Saves the algorithm parameters to a file
            %
            %     obj.save(filename)
            %
            % ## Input
            % * __filename__ Name of the file to save to.
            %
            % This method stores the algorithm parameters in the specified
            % XML or YAML file.
            %
            % See also: cv.ContourFitting.load
            %
            ContourFitting_(this.id, 'save', filename);
        end

        function load(this, fname_or_str, varargin)
            %LOAD  Loads algorithm from a file or a string
            %
            %     obj.load(fname)
            %     obj.load(str, 'FromString',true)
            %     obj.load(..., 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __fname__ Name of the file to read.
            % * __str__ String containing the serialized model you want to
            %   load.
            %
            % ## Options
            % * __ObjName__ The optional name of the node to read (if empty,
            %   the first top-level node will be used). default empty
            % * __FromString__ Logical flag to indicate whether the input is a
            %   filename or a string containing the serialized model.
            %   default false
            %
            % This method reads algorithm parameters from the specified XML or
            % YAML file (either from disk or serialized string). The previous
            % algorithm state is discarded.
            %
            % See also: cv.ContourFitting.save
            %
            ContourFitting_(this.id, 'load', fname_or_str, varargin{:});
        end

        function name = getDefaultName(this)
            %GETDEFAULTNAME  Returns the algorithm string identifier
            %
            %     name = obj.getDefaultName()
            %
            % ## Output
            % * __name__ This string is used as top level XML/YML node tag
            %   when the object is saved to a file or string.
            %
            % See also: cv.ContourFitting.save, cv.ContourFitting.load
            %
            name = ContourFitting_(this.id, 'getDefaultName');
        end
    end

    %% Getters/Setters
    methods
        function value = get.CtrSize(this)
            value = ContourFitting_(this.id, 'get', 'CtrSize');
        end
        function set.CtrSize(this, value)
            ContourFitting_(this.id, 'set', 'CtrSize', value);
        end

        function value = get.FDSize(this)
            value = ContourFitting_(this.id, 'get', 'FDSize');
        end
        function set.FDSize(this, value)
            ContourFitting_(this.id, 'set', 'FDSize', value);
        end
    end

    %% Static functions
    methods (Static)
        function out = contourSampling(src, numElt)
            %CONTOURSAMPLING  Contour sampling
            %
            %     out = cv.ContourFitting.contourSampling(src, numElt)
            %
            % ## Input
            % * __src__ input contour, vector of 2D points stored in numeric
            %   array Nx2/Nx1x2/1xNx2 or cell array of 2-element vectors
            %   `{[x,y], ...}`.
            % * __NumElt__ number of points in `out` contour.
            %
            % ## Output
            % * __out__ output contour with `numElt` points.
            %
            % See also: cv.findContours, cv.approxPolyDP
            %
            out = ContourFitting_(0, 'contourSampling', src, numElt);
        end

        function dst = fourierDescriptor(src, varargin)
            %FOURIERDESCRIPTOR  Fourier descriptors for planed closed curves
            %
            %     dst = cv.ContourFitting.fourierDescriptor(src)
            %     dst = cv.ContourFitting.fourierDescriptor(src, 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __src__ input contour, vector of 2D points stored in numeric
            %   array Nx2/Nx1x2/1xNx2 or cell array of 2-element vectors
            %   `{[x,y], ...}`.
            %
            % ## Output
            % * __dst__ 2-channel array of type `single` and length `NumElt`.
            %
            % ## Options
            % * __NumElt__ number of rows in `dst` or cv.getOptimalDFTSize
            %   rows if `NumElt=-1`. default -1
            % * __NumFD__ number of FD to return in `dst`,
            %   `dst = [FD(1...NumFD/2) FD(NumFD/2-NumElt+1...:NumElt)]`.
            %   default -1 (return all of FD as is).
            %
            % For more details about this implementation, please see
            % [PersoonFu1977].
            %
            % See also: cv.dft
            %
            dst = ContourFitting_(0, 'fourierDescriptor', src, varargin{:});
        end

        function dst = transformFD(src, t, varargin)
            %TRANSFORMFD  Transform a contour
            %
            %     dst = cv.ContourFitting.transformFD(src, t)
            %     dst = cv.ContourFitting.transformFD(src, t, 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __src__ contour, or Fourier descriptors if `FD` is true.
            % * __t__ 1x5 transform matrix given by
            %   cv.ContourFitting.estimateTransformation method.
            %
            % ## Output
            % * __dst__ 2-channel matrix of type `double` and `NumElt` rows.
            %
            % ## Options
            % * __FD__ if true `src` are Fourier descriptors, if false `src`
            %   is a contour. default true
            %
            % See also: cv.ContourFitting.estimateTransformation,
            %  cv.ContourFitting.contourSampling,
            %  cv.ContourFitting.fourierDescriptor
            %
            dst = ContourFitting_(0, 'transformFD', src, t, varargin{:});
        end
    end

end


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

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

  if ( nargin < 2 )
    ny = nx;
    fprintf ( 1, '\n' );
    fprintf ( 1, '  NY was not supplied.  Setting NY = NX = %d.\n', ny );
  end

  if ( nargin < 3 )
    n = wathen_order ( nx, ny );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  N was not supplied.  N = %d\n', n );
  end

  if ( nargin < 4 )
    seed = 123456789;
  end

  ml = 3 * nx + 4;
  mu = 3 * nx + 4;

  a = zeros ( 2 * ml + mu + 1, n );

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

  node = zeros(8,1);

  for j = 1 : ny

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

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

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

      [ rho, seed ] = r8_uniform_01 ( seed );
      rho = 100.0 * rho;

      for krow = 1 : 8
        for kcol = 1 : 8
          ii = node(krow);
          jj = node(kcol);
          a(ii-jj+ml+mu+1,jj) = a(ii-jj+ml+mu+1,jj) + rho * em(krow,kcol);
        end
      end

    end
  end

  return
end


%DEMEV1	Demonstrate Bayesian regression for the MLP.
%
%	Description
%	The problem consists an input variable X which sampled from a
%	Gaussian distribution, and a target variable T generated by computing
%	SIN(2*PI*X) and adding Gaussian noise. A 2-layer network with linear
%	outputs is trained by minimizing a sum-of-squares error function with
%	isotropic Gaussian regularizer, using the scaled conjugate gradient
%	optimizer. The hyperparameters ALPHA and BETA are re-estimated using
%	the function EVIDENCE. A graph  is plotted of the original function,
%	the training data, the trained network function, and the error bars.
%
%	See also
%	EVIDENCE, MLP, SCG, DEMARD, DEMMLP1
%

%	Copyright (c) Ian T Nabney (1996-2001)

clc;
disp('This demonstration illustrates the application of Bayesian')
disp('re-estimation to determine the hyperparameters in a simple regression')
disp('problem. It is based on a local quadratic approximation to a mode of')
disp('the posterior distribution and the evidence maximization framework of')
disp('MacKay.')
disp(' ')
disp('First, we generate a synthetic data set consisting of a single input')
disp('variable x sampled from a Gaussian distribution, and a target variable')
disp('t obtained by evaluating sin(2*pi*x) and adding Gaussian noise.')
disp(' ')
disp('Press any key to see a plot of the data together with the sine function.')
pause;

% Generate the matrix of inputs x and targets t.

ndata = 16;			% Number of data points.
noise = 0.1;			% Standard deviation of noise distribution.
randn('state', 0);
x = 0.25 + 0.07*randn(ndata, 1);
t = sin(2*pi*x) + noise*randn(size(x));

% Plot the data and the original sine function.
h = figure;
nplot = 200;
plotvals = linspace(0, 1, nplot)';
plot(x, t, 'ok')
xlabel('Input')
ylabel('Target')
hold on
axis([0 1 -1.5 1.5])
fplot('sin(2*pi*x)', [0 1], '-g')
legend('data', 'function');

disp(' ')
disp('Press any key to continue')
pause; clc;

disp('Next we create a two-layer MLP network having 3 hidden units and one')
disp('linear output. The model assumes Gaussian target noise governed by an')
disp('inverse variance hyperparmeter beta, and uses a simple Gaussian prior')
disp('distribution governed by an inverse variance hyperparameter alpha.')
disp(' ');
disp('The network weights and the hyperparameters are initialised and then')
disp('the weights are optimized with the scaled conjugate gradient')
disp('algorithm using the SCG function, with the hyperparameters kept')
disp('fixed. After a maximum of 500 iterations, the hyperparameters are')
disp('re-estimated using the EVIDENCE function. The process of optimizing')
disp('the weights with fixed hyperparameters and then re-estimating the')
disp('hyperparameters is repeated for a total of 3 cycles.')
disp(' ')
disp('Press any key to train the network and determine the hyperparameters.')
pause;

% Set up network parameters.
nin = 1;		% Number of inputs.
nhidden = 3;		% Number of hidden units.
nout = 1;		% Number of outputs.
alpha = 0.01;		% Initial prior hyperparameter.
beta_init = 50.0;	% Initial noise hyperparameter.

% Create and initialize network weight vector.
net = mlp(nin, nhidden, nout, 'linear', alpha, beta_init);

% Set up vector of options for the optimiser.
nouter = 3;			% Number of outer loops.
ninner = 1;			% Number of innter loops.
options = zeros(1,18);		% Default options vector.
options(1) = 1;			% This provides display of error values.
options(2) = 1.0e-7;		% Absolute precision for weights.
options(3) = 1.0e-7;		% Precision for objective function.
options(14) = 500;		% Number of training cycles in inner loop.

% Train using scaled conjugate gradients, re-estimating alpha and beta.
for k = 1:nouter
  net = netopt(net, options, x, t, 'scg');
  [net, gamma] = evidence(net, x, t, ninner);
  fprintf(1, '\nRe-estimation cycle %d:\n', k);
  fprintf(1, '  alpha =  %8.5f\n', net.alpha);
  fprintf(1, '  beta  =  %8.5f\n', net.beta);
  fprintf(1, '  gamma =  %8.5f\n\n', gamma);
  disp(' ')
  disp('Press any key to continue.')
  pause;
end

fprintf(1, 'true beta: %f\n', 1/(noise*noise));

disp(' ')
disp('Network training and hyperparameter re-estimation are now complete.')
disp('Compare the final value for the hyperparameter beta with the true')
disp('value.')
disp(' ')
disp('Notice that the final error value is close to the number of data')
disp(['points (', num2str(ndata),') divided by two.'])
disp(' ')
disp('Press any key to continue.')
pause; clc;
disp('We can now plot the function represented by the trained network. This')
disp('corresponds to the mean of the predictive distribution. We can also')
disp('plot ''error bars'' representing one standard deviation of the')
disp('predictive distribution around the mean.')
disp(' ')
disp('Press any key to add the network function and error bars to the plot.')
pause;

% Evaluate error bars.
[y, sig2] = netevfwd(mlppak(net), net, x, t, plotvals);
sig = sqrt(sig2);

% Plot the data, the original function, and the trained network function.
[y, z] = mlpfwd(net, plotvals);
figure(h); hold on;
plot(plotvals, y, '-r')
xlabel('Input')
ylabel('Target')
plot(plotvals, y + sig, '-b');
plot(plotvals, y - sig, '-b');
legend('data', 'function', 'network', 'error bars');

disp(' ')
disp('Notice how the confidence interval spanned by the ''error bars'' is')
disp('smaller in the region of input space where the data density is high,')
disp('and becomes larger in regions away from the data.')
disp(' ')
disp('Press any key to end.')
pause; clc; close(h);
%clear all


% function [x1,x2,P1,P2] = make_synthetic_data(N,range_model,range_image,verbose)
% makes a synthetic 3D model and makes two projective images of the model
% using arbitrary projective cameras Pi=[M|t]. The second cameras are normalized
% to Pi = Pi / Pi_{34}.
% inputs:
%               N               1x1     (optional) the maximum number of correspondences to generate.
%               range_model     2x1     (optional) defines a bounding box (starting from origin) containing the model
%               range_image     2x1     (optional) the size of the output image(scaling on correspondences)
%               verbose         1x1     (optional) plots the generated model and images
% outputs:
%               x1              3x1     homogeneous coordinates of the correspondences in image 1
%               x2              3x1     homogeneous coordinates of the correspondences in image 2
%               P1              3x4     the camera that generates x1(up to an affine transformation)
%               P2              3x4     the camera that generates x2(up to an affine transformation)
%
% Author: Omid Aghazadeh, KTH(Royal Institute of Technology), 2010/05/09
function [x1,x2,P1,P2] = make_synthetic_data(N,range_model,range_image,verbose)
if nargin < 1, N = 100; end
if nargin < 2, range_model = [10;10;10]; end
if nargin < 3, range_image = [640;480]; end
if nargin < 4, verbose = 1; end
X = rand(3,N); X = (X - repmat(min(X,[],2),1,N)) .* repmat(range_model ./ (max(X,[],2) - min(X,[],2)),1,N);
X = [X; ones(1,N)];
P1 = rand(3,4); P1(end) = 1;
P2 = rand(3,4); P2(end) = 1; %avoid cameras at infinity
x1 = P1*X; x1 = x1./repmat(x1(3,:),3,1); x2 = P2*X; x2 = x2./repmat(x2(3,:),3,1);
ixinv = sum(isinf(x1) | isnan(x1) | isinf(x2) | isnan(x2) | abs(x1) > 1e3 | abs(x2) > 1e3,1 )>0 ;
x1 = x1(:,~ixinv); x2 = x2(:,~ixinv); N = size(x1,2);
x1 = (x1-repmat([min(x1(1:2,:),[],2);0],1,N)).* repmat([range_image./(max(x1(1:2,:),[],2) - min(x1(1:2,:),[],2));1],1,N);

x2 = (x2-repmat([min(x2(1:2,:),[],2);0],1,N)).* repmat([range_image./(max(x2(1:2,:),[],2) - min(x2(1:2,:),[],2));1],1,N);
if verbose
    figure(1); subplot(1,3,1); plot3(X(1,:),X(2,:),X(3,:),'b.'); title('generated 3D model');
    subplot(1,3,2); plot(x1(1,:),x1(2,:),'rx'); title('generated first image');
    subplot(1,3,3); plot(x2(1,:),x2(2,:),'gx'); title('generated second image');
end
end


function res = parallelEdge(edge, dist)
%PARALLELEDGE Edge parallel to another edge.
%
%   EDGE2 = parallelEdge(EDGE, DIST)
%   Computes the edge parallel to the input edge EDGE and located at the
%   given signed distance DIST.
%
%   Example
%     obox = [30 40 80 40 30];
%     figure; hold on; axis equal;
%     drawOrientedBox(obox, 'LineWidth', 2);
%     edge1 = centeredEdgeToEdge(obox([1 2 3 5]));
%     edge2 = centeredEdgeToEdge(obox([1 2 4 5])+[0 0 0 90]);
%     drawEdge(edge1, 'LineWidth', 2, 'color', 'g');
%     drawEdge(edge2, 'LineWidth', 2, 'color', 'g');
%     drawEdge(parallelEdge(edge1, -30), 'LineWidth', 2, 'color', 'k');
%     drawEdge(parallelEdge(edge2, -50), 'LineWidth', 2, 'color', 'k');
%
%   See also
%     edges2d, parallelLine, drawEdge, centeredEdgeToEdge, edgeToLine

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2012-07-31, using Matlab 7.9.0.529 (R2009b)
% Copyright 2012-2022 INRA - Cepia Software Platform

% compute the line parallel to the supporting line of edge
line = parallelLine(edgeToLine(edge), dist);

% result edge is given by line positions 0 and 1.
res = [line(:, 1:2) line(:, 1:2)+line(:, 3:4)];


function sparse_test02 ( )

%*****************************************************************************80
%
%% SPARSE_TEST02 demonstrates a simple use of the SPARSE matrix facility.
%
%  Discussion:
%
%    This is a nice but very simple example.
%
%    Note in this example that we define three sparse matrices,
%    SUP, DIAG, and SUB, and then define A as the sum of those matrices.
%
%    However, again, each time we define a sparse matrix, we are assuming
%    we have all the information available at one time.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 August 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    George Lindfield, John Penny,
%    Numerical Methods Using MATLAB,
%    Prentice Hall, 1999
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_TEST02:\n' );
  fprintf ( 1, '  Demonstrate the use of MATLAB''s SPARSE facility\n' );
  fprintf ( 1, '  to define a sparse matrix, and solve an associated\n' );
  fprintf ( 1, '  linear system.\n' );

  n = 100;
%
%  We set up the three diagonals of the -1, 2, -1 matrix.
%
  sup  = sparse ( 1:n-1, 2:n,   -1.0, n, n );
  diag = sparse ( 1:n,   1:n,    2.0, n, n );
  sub  = sparse ( 2:n,   1:n-1, -1.0, n, n );
%
%  A is the matrix whose superdiagonal, diagonal, and subdiagonal
%  have been set above.  Because SUP, DIAG and SUB are sparse,
%  A will "inherit" this property.
%
  A = sup + diag + sub;
%
%  Set up a right hand side b that defines a linear system whose
%  solution is [ 1, 2, 3, ..., n ].
%
  b(1:n-1,1) = 0.0;
  b(n,1) = n + 1;
%
%  Use MATLAB's backslash command to solve the linear system.
%  Because MATLAB "knows" that A is a sparse matrix, it will
%  automatically do the efficient thing.
%
  x = A \ b;
%
%  Print the solution.
%
  x(1:n)

  return
end


% OZDEMO
% This program creates the Ornstein-Zernike example in Chapter 3.
% [H,C]=OZDEMO returns the solution on a grid with a mesh
% spacing of 1/256.
%
function [h,c]=ozdemo
global L U rho
n=257;
epsilon=.1; sigma=2.0; rho=.2; beta=10; L=9;
dx=L/(n-1); r=0:dx:L; r=r';
%
% Compute the potential and store it in a global variable.
%
U=elj(r,sigma,epsilon,beta);
%
tol=[1.d-8,1.d-8];
x=zeros(2*n,1);
parms=[40,80,-.1];
[sol, it_hist, ierr] = nsoli(x,'oz',tol);
%
% Unpack h and c.
%
h=sol(1:n); c=sol(n+1:2*n);
%
% Plot the solution.
%
figure(1)
subplot(1,2,1)
plot(r,h,'-');
ylabel('h','Rotation',1);
xlabel('r');
subplot(1,2,2)
plot(r,c,'-');
ylabel('c','Rotation',1);
xlabel('r');
%
% Do a second solve with constant forcing terms.
% Are you getting the same results each time?
%
fb=it_hist(1,1);
parms=[40,80,-.1]; x=zeros(2*n,1);
[sola, it_hist1, ierr] = nsoli(x,'oz',tol,parms);
norm(sola-sol)
%
% plot residual vs iteration counter
%
figure(2)
ni=length(it_hist(:,1));
n1=length(it_hist1(:,1));
semilogy(0:ni-1,it_hist(:,1)/fb,'-',...
0:n1-1,it_hist1(:,1)/fb,'--');
legend('default','.1');
xlabel('Nonlinear iterations');
ylabel('Relative residual');
%
% plot residual vs function counter
%
figure(3)
semilogy(it_hist(:,2),it_hist(:,1)/fb,'-',...
it_hist1(:,2),it_hist1(:,1)/fb,'--');
xlabel('Function evaluations');
ylabel('Relative residual');
legend('default','.1');

%
function u=elj(r,sigma,epsilon,beta)
n2=length(r);
ra=r(2:n2);
r12=(sigma./ra).^12; r6=(sigma./ra).^6;
ua=exp(-4*beta*epsilon*(r12-r6));
u=[0,ua']';


function [ o, x, w ] = cn_leg_03_xiu ( n )

%*****************************************************************************80
%
%% CN_LEG_03_XIU implements the Xiu precision 3 rule for region CN_LEG.
%
%  Discussion:
%
%    The rule has order
%
%      O = 2 * N.
%
%    The rule has precision P = 3.
%
%    CN_LEG is the cube [-1,+1]^N with the Legendre weight function
%
%      w(x) = 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dongbin Xiu,
%    Numerical integration formulas of degree two,
%    Applied Numerical Mathematics,
%    Volume 58, 2008, pages 1515-1520.
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Input, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  o = 2 * n;

  x = zeros ( n, o );
  w = zeros ( o, 1 );

  expon = 0;
  volume = c1_leg_monomial_integral ( expon );
  volume = volume ^ n;

  for j = 1 : o

    i = 0;
    for r = 1 : floor ( n / 2 )
      arg = ( 2 * r - 1 ) * j * pi / n;
      i = i + 1;
      x(i,j) = sqrt ( 2.0 ) * cos ( arg ) / sqrt ( 3.0 );
      i = i + 1;
      x(i,j) = sqrt ( 2.0 ) * sin ( arg ) / sqrt ( 3.0 );
    end

    if ( i < n )
      i = i + 1;
      x(i,j) = sqrt ( 2.0 ) * r8_mop ( j ) / sqrt ( 3.0 );
      if ( n == 1 )
        x(i,j) = x(i,j) / sqrt ( 2.0 );
      end
    end

  end

  w(1:o) = volume / o;

  return
end


% THIS PROGRAM IS FOR IMPLEMENTATION OF DISCRETE TIME PROCESS UNSCENTED KALMAN FILTER
% FOR GAUSSIAN AND LINEAR STOCHASTIC DIFFERENCE EQUATION.
% (19 JULY 2005).
% UNSCENTED KALMAN FILTER (UKF) AT ITS BEST.
%(Under Nonlinear conditions,UNSCENTED KALMAN FILTER
% performs to a much better extent as compared to EXTENDED KALMAN FILTER).

clc;  close all;  clear all;

format long g;
Xo = [1; 0; 0; 0; 0; 0; 0; 0; 0; 0];
nx = length(Xo);
beta = 2;
elfa = 1*(10^-1);
lambda = (((elfa^2)*(nx)) - nx);
mn1 = mean(Xo);                                   %FIRST STEP
CV1 = (Xo-mn1)*(Xo-mn1)';
%CV1 = (Xo)*(Xo)';
PO = CV1;
FX = size(CV1);
VTt = [1 0 0 0 0 0 0 0 0 0]';
ADP1 = randn(1,200);
mn2 = mean(VTt);
NTt = [1 0 0 0 0 0 0 0 0 0]';
ADP2 = randn(1,200);
mn3 = mean(NTt);
Qo = (VTt-mn2)*(VTt-mn2)';
Ro = (NTt-mn3)*(NTt-mn3)';
%Qo = (VTt)*(VTt)';
%Ro = (NTt)*(NTt)';
FX = zeros(10,10);
PAO = [PO FX FX; FX Qo FX;FX FX Ro];
Xao = [mn1 0 0]';
lam = sqrt((elfa^2)*(nx));

Wmo = lambda/(nx+lambda);
Wmi = 1/(2*(nx+lambda));
Wmin = 1/(2*(nx+lambda));

Wco =  (lambda/(nx+lambda)) + (1-(elfa^2)+ beta);
Wci = Wmi;
Wcin = Wmin;

%CALCULATION OF SIGMA POINTS
[Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33] = sigmacal(mn1,CV1,mn2,Qo,mn3,Ro,lam);

%TIME UPDATE EQUATIONS.
[Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark] = TMUPDT(Wmo,Wmi,Wmin,Wco,Wci,Wcin,Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33);

%MEASUREMENT UPDATE EQUATIONS.
[KGain,XNEW,PT1,PT2,PT3] = MSMTUPDT(Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark,Wco,Wci,Wcin,Xo,VTt);

for ii = 1:1:100

    VTt = [ADP1(ii+1) 0 0 0 0 0 0 0 0 0]';
    NTt = [ADP2(ii+1) 0 0 0 0 0 0 0 0 0]';

    %CV1 = (XNEW)*(XNEW)';
    mn1 = mean(XNEW);
    mn2 = mean(VTt);
    mn3 = mean(NTt);
    % Qo = (VTt)*(VTt)';
    % Ro = (NTt)*(NTt)';

    CV1 = (XNEW-mn1)*(XNEW-mn1)';
    Qo = (VTt-mn2)*(VTt-mn2)';
    Ro = (NTt-mn3)*(NTt-mn3)';

    lam = sqrt((elfa^2)*(nx));

    [Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33] = sigmacal(mn1,CV1,mn2,Qo,mn3,Ro,lam);

    [Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark] = TMUPDT(Wmo,Wmi,Wmin,Wco,Wci,Wcin,Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33);

    [KGain,XNEW,PT1,PT2,PT3,YNEW] = MSMTUPDT(Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark,Wco,Wci,Wcin,XNEW,VTt);

    INP1(ii) = XNEW(1,1);
    INP2(ii) = YNEW(1,1);

end

T = 1:1:100;
figure(1);     subplot(211);    plot(real(INP1));   title('ORIGINAL SIGNAL');
subplot(212);  plot(real(INP2));   title('ESTIMATED SIGNAL (UNDER Nonlinear MODEL)');

figure(2);    plot(T,abs(INP1),T,abs(INP2));  title('Combined plot'); legend('original','estimated');


function [Pw] = RFT_Pval(u,k,c,fwhm,L,type,dof)
% computes Pval according to Friston et al. (1996), Eq. 1-4.
% [Pw] = RFT_Pval(u,k,c,fwhm,L)
% This function ca be called to derive the p-value at different levels of
% inference, i.e.:
% - P_peak = RFT_Pval(u,0,1,fwhm,L)
% - P_cluster = RFT_Pval(U,k,1,fwhm,L), where U was the set-inducing
% threshold, and k is the observed spatial extent of the cluster.
% - P_set = RFT_Pval(U,K,c,fwhm,L), where U and K were the set-inducing
% thresholds, and c was the number of observed upcrossing clusters
% IN:
%   - u: RF value
%   - k: cluster's spatial extent
%   - c: number of upcrossing clusters
%   - fwhm: estimated FWHM
%   - L: search volume
%   - type: type of RF. Can be set to 'norm' (normal, default), 't'
%   (Student) or 'F' (Fisher).
%   - dof: degrees of freedom (only relevant for 't' or 'F' fields).
% OUT:
%   - Pw: the ensuing p-value

EC = RFT_expectedTopo(u,L,fwhm,1,type,dof);
switch type
    case 'norm'
        P0 = 1-VBA_spm_Ncdf(u,0,1);
    case 't'
        P0 = 1-VBA_spm_Tcdf(u,dof);
    case 'F'
        P0 = 1-VBA_spm_Fcdf(u,dof(1),dof(2));
end
beta = (gamma(3/2).*EC./(L.*P0)).^2;
Pnk = exp(-beta.*k.^2);
if k == 0, Pnk = 1; end; % solves the numerical issue when P0=0
Pw = 1;
for i=0:c-1
    Pw = Pw - myPoissonPMF(i,EC.*Pnk);
end

function p = myPoissonPMF(x,Ex)
p = (Ex.^x).*exp(-Ex)./factorial(x);

% function p = myPoissonCDF(x,Ex)
% p = 1 - gammainc(Ex,x+1);


options.filter = 'linear';
options.filter = '9-7';

%% 1D %%
n = 1024;
x = linspace(0,1,n)';
x = cos(10*pi*x) + (x>.4);

Jmin = 6;

options.ti = 0;
y = perform_lifting_transform(x, Jmin, +1, options);
x1 = perform_lifting_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);

options.ti = 1;
y = perform_lifting_transform(x, Jmin, +1, options);
x1 = perform_lifting_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);

%% test for RWT
options.ti = 1;
options.use_mex = 1;
options.wavelet_vm = 3;
options.wavelet_type = 'daubechies';
y = perform_wavelet_transform(x, Jmin, +1, options);
x1 = perform_wavelet_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);

%% test for wavelab
options.ti = 1;
options.use_mex = 0;
options.wavelet_vm = 3;
options.wavelet_type = 'daubechies';
y = perform_wavelet_transform(x, Jmin, +1, options);
x1 = perform_wavelet_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);

return;

%% 2D %%
n = 256;
Jmin = 3;
x = load_image('lena', n);
x = rescale(x);

options.ti = 0;
y = perform_lifting_transform(x, Jmin, +1, options);
x1 = perform_lifting_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);

options.ti = 1;
y = perform_lifting_transform(x, Jmin, +1, options);
x1 = perform_lifting_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);


function [l, u] = ComputeDistanceExtremes(X, a, b, M)
% [l, u] = ComputeDistanceExtremes(X, a, b, M)
%
% Computes sample histogram of the distances between rows of X and returns
% the value of these distances at the a^th and b^th percentils.  This
% method is used to determine the upper and lower bounds for
% similarity / dissimilarity constraints.
%
% X: (n x m) data matrix
% a: lower bound percentile between 1 and 100
% b: upper bound percentile between 1 and 100
% M: Mahalanobis matrix to compute distances
%
% Returns l: distance corresponding to a^th percentile
% u: distance corresponding the b^th percentile

if (a < 1 || a > 100),
    error('a must be between 1 and 100')
end
if (b < 1 || b > 100),
    error('b must be between 1 and 100')
end

n = size(X, 1);

num_trials = min(100, n*(n-1)/2);

% we will sample with replacement
dists = zeros(num_trials, 1);
for (i=1:num_trials),
    j1 = ceil(rand(1)*n);
    j2 = ceil(rand(1)*n);
    dists(i) = (X(j1,:) - X(j2,:))*M*(X(j1,:) - X(j2,:))';
end

[f, c] = hist(dists, 100);
l = c(floor(a));
u = c(floor(b));


function [EV, EVal] = ncuts(A, n_ev)
% Computes the n_ev smallest (non-zero) eigenvectors and eigenvalues of the
% of the Laplacian of A

D = sparse(1:size(A,1), 1:size(A,1), full(sum(A, 1)), size(A,1), size(A,2));

opts.issym = 0;
opts.isreal = 1;
opts.disp = 0;
nvec = n_ev+1;

[EV, EVal] = eigs((D - A) + (10^-10) * speye(size(D)), D, nvec, 'sm',opts);

[junk, sortidx] = sort(diag(EVal), 'descend');
EV = EV(:,sortidx(end-1:-1:1));
v = diag(EVal);
EVal = v(sortidx(end-1:-1:1));

EV = bsxfun(@rdivide, EV, sqrt(sum(EV.^2,1))); % makes the eigenvectors unit norm


function f = fx3 ( n, x )

%*****************************************************************************80
%
%% FX3 is the third 1D example.
%
%  Discussion:
%
%    The function should be plotted over [-1.0,+1.0].
%
%    Internally, this range is mapped to [-3.0,+3.0].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Rick Archibald, Anne Gelb, Jungho Yoon,
%    Polynomial fitting for edge detection in irregularly sampled signals
%    and images,
%    SIAM Journal on Numerical Analysis,
%    Volume 43, Number 1, 2006, pages 259-279.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the arguments.
%
%    Output, real F(N,1), the function values.
%

%
%  Destroy all row vectors!
%
  x = x ( : );
%
%  Map from the convenient range [-1,+1] to the physical range [-3,+3].
%
  x = ( ( 1.0 - x ) * ( -3.0 )   ...
      + ( 1.0 + x ) * ( +3.0 ) ) ...
      /   2.0;

  f = zeros ( n, 1 );

  i = find ( -2.0 <= x & x <= -1.0 );
  f(i) = 1.0;

  j = find ( -0.5 <= x & x <= 0.5 );
  f(j) = 0.5 + 4.0 * ( x(j) + 0.5 ).^2;

  k = find ( 1.25 <= x & 3.0 * x <= 7.0 );
  f(k) = 3.0 * ( 2.0 - x(k) );

  return
end


function [dop_buf] = rayleigh_dop(in_val)
% Function to generate Doppler-filtered Rayleigh fading simulator

% Doppler parameters
vo = (1.2*1000/3600);    % vo = 1.2 km/h  ->  0.333 m/s
lambda = 3e8 / 5.4e9;    % lambda = c / fc;
fd = vo/lambda;          % Doppler Spread (around 6 Hz)

% Generate Doppler Sf (256 points)
f_rng = 10*fd;
f = -f_rng/2 : f_rng/255 : f_rng/2;
dop_Sf = 1./(1+9*(f/fd).^2);

% Generate 256 samples of Doppler-filtered, Rayleigh fading sim.
dop_buf1(129:256) = randn(1,128)+j*randn(1,128);
dop_buf1(  1:128) = conj(dop_buf1(256:-1:129));
dop_buf2(129:256) = randn(1,128)+j*randn(1,128);
dop_buf2(  1:128) = conj(dop_buf2(256:-1:129));
dop_buf1 = ifft(dop_buf1 .* dop_Sf);
dop_buf2 = ifft(dop_buf2 .* dop_Sf);
dop_buf = sqrt(dop_buf1.^2 + dop_buf2.^2);


function coef=ref_dgtns_1(f,gamma,V)
%REF_DGT_1 Reference DGTNS using P.Prinz algorithm
%   Usage:  c=ref_dgtns_1(f,gamma,V);
%

a=V(1,1);
b=V(2,2);
r=-V(2,1);
L=size(gamma,1);
M=L/b;
N=L/a;
W=size(f,2);

c=gcd(a,M);
p=a/c;
q=M/c;
d=N/q;

if r==0
  % The grid is rectangular. Call another reference algorithm.
  coef=zeros(M*N,W);
  coef(:)=ref_dgt(f,gamma,a,M);

  return;
end;

% We can now assume that the grid is truly nonseperable,
% and so d>1.

% Conjugate.
gammac=conj(gamma);

% Level 2: Block diagonalize, and use that some blocks are
% the same up to permutations.

p1=stridep(M,L);
p2=stridep(N,M*N);

% Get shift offsets for stage 2 of the algorithm.
[mll,all]=shiftoffsets(a,M);

% Step 1: Permute
s1 = f(p1,:);

% Step 2: Multiply by DG'
s2=zeros(M*N,W);

% Do interpreter-language-optimized indexing.
[n_big,m_big]=meshgrid(0:N-1,0:b-1);
base=m_big*M-n_big*a+L;
base=base.';

% Work arrays.
work=zeros(b,M/c*W);
wk=zeros(N,b);
wkrect=zeros(N,b);

% Create fixed modulation matrix (Does not change with k)
fixedmod=zeros(N,b);
for n=0:N-1
  fixedmod(n+1,:)=exp(2*pi*i*r*n/L*(0:M:L-1));
end;

% This loop iterates over the number of truly different wk's.
for ko=0:c-1

  % Create the wk of the rectangular-grid case.
  wkrect(:)=gammac(mod(base+ko,L)+1);

  % Create wk of skewed case.
  wk=(fixedmod.*wkrect);

  % Setup work array.
  for l=0:M/c-1
    k=ko+l*c;
    rowmod=exp(2*pi*i*r*(0:b-1)/b*all(l+1)).';

    work(:,l*W+1:(l+1)*W)=circshift(rowmod.*s1(1+(ko+l*c)*b:(ko+l*c+1)*b,:),-mll(l+1));
  end;

  % Do the actual multiplication,
  work2=wk*work;

  % Place the result correctly.
  for l=0:M/c-1
    k=ko+l*c;
    kmod=exp(2*pi*i*r*(0:N-1)*k/L).';
    colmod=exp(2*pi*i*r*(0:N-1)/b*mll(l+1)).';
    doublefac=exp(-2*pi*i*r/b*all(l+1)*mll(l+1));

    s2(1+(ko+l*c)*N:(ko+l*c+1)*N,:)=doublefac*colmod.*kmod.*circshift(work2(:,l*W+1:(l+1)*W),all(l+1));
  end;

end;

% Step 3: Permute again.
coef = s2(p2,:);

% Apply fft.
for n=1:N
  coef((n-1)*M+1:n*M,:)=fft(coef((n-1)*M+1:n*M,:));
end;


function [beta_median beta_std beta_lbound beta_ubound sigma_median sigma_t_median sigma_t_lbound sigma_t_ubound gamma_median]=stvol2estimates(beta_gibbs,sigma_gibbs,sigma_t_gibbs,gamma_gibbs,n,T,cband)

% compute the median, variance, and credibility intervals for the posterior distribution of beta
beta_median=quantile(beta_gibbs,0.5,2);
beta_std=std(beta_gibbs,0,2);
beta_lbound=quantile(beta_gibbs,(1-cband)/2,2);
beta_ubound=quantile(beta_gibbs,1-(1-cband)/2,2);

% compute the results for sigma (long-run value)
sigma_median=reshape(quantile(sigma_gibbs,0.5,2),n,n);

% compute the rsults for sigma (sample values)
sigma_t_median=cell(n,n);
sigma_t_lbound=cell(n,n);
sigma_t_ubound=cell(n,n);
% loop over periods and entries
for ii=1:T
   for jj=1:n
      for kk=1:jj
      sigma_t_median{jj,kk}(ii,1)=quantile(sigma_t_gibbs{ii,1}(jj,kk,:),0.5,3);
      sigma_t_lbound{jj,kk}(ii,1)=quantile(sigma_t_gibbs{ii,1}(jj,kk,:),(1-cband)/2,3);
      sigma_t_ubound{jj,kk}(ii,1)=quantile(sigma_t_gibbs{ii,1}(jj,kk,:),1-(1-cband)/2,3);
      end
   end
end

% compute the estimates for gamma
gamma_median=quantile(gamma_gibbs,0.5,1);


function fem2d_bvp_linear_test01 ( )

%*****************************************************************************80
%
%% FEM2D_BVP_LINEAR_TEST01 carries out test case #1.
%
%  Discussion:
%
%    Use A1, C1, F1, EXACT1, EXACT_UX1, EXACT_UY1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 June 2014
%
%  Author:
%
%    John Burkardt
%
  nx = 3;
  ny = 3;

% nx = 5;
% ny = 5;

% nx = 9;
% ny = 9;

% nx = 17;
% ny = 17;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM2D_BVP_LINEAR_TEST01\n' );
  fprintf ( 1, '  Solve - del ( A del U ) + C U = F \n' );
  fprintf ( 1, '  on the unit square with zero boundary conditions.\n' );
  fprintf ( 1, '  A1(X,Y) = 1.0\n' );
  fprintf ( 1, '  C1(X,Y) = 0.0\n' );
  fprintf ( 1, '  F1(X,Y) = 2*X*(1-X)+2*Y*(1-Y).\n' );
  fprintf ( 1, '  U1(X,Y) = X * ( 1 - X ) * Y * ( 1 - Y )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The grid uses %d by %d nodes.\n', nx, ny );
%
%  Geometry definitions.
%
  x = linspace ( 0.0, 1.0, nx );
  y = linspace ( 0.0, 1.0, ny );

  u = fem2d_bvp_linear ( nx, ny, @a1, @c1, @f1, x, y );

  if ( 0 )
    [ X, Y ] = meshgrid ( x, y );
    surf ( X, Y, u )
  end

  if ( nx * ny <= 25 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '     I     J    X         Y         U         Uexact    Error\n' );
    fprintf ( 1, '\n' );

    for j = 1 : ny
      for i = 1 : nx
        uexact = exact1 ( x(i), y(j) );
        fprintf ( 1, '  %4d  %4d  %8f  %8f  %8f  %8f  %8e\n', ...
          i, j, x(i), y(j), u(i,j), uexact, abs ( u(i,j) - uexact ) );
      end
    end

  end

  e1 = fem2d_l1_error ( nx, ny, x, y, u, @exact1 );
  e2 = fem2d_l2_error_linear ( nx, ny, x, y, u, @exact1 );
  h1s = fem2d_h1s_error_linear ( nx, ny, x, y, u, @exact_ux1, @exact_uy1 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  l1 error   = %g\n', e1 );
  fprintf ( 1, '  L2 error   = %g\n', e2 );
  fprintf ( 1, '  H1S error  = %g\n', h1s );

  return
end
function value = a1 ( x, y )

%*****************************************************************************80
%
%% A1 evaluates A function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of A(X).
%
  value = 1.0;

  return
end
function value = c1 ( x, y )

%*****************************************************************************80
%
%% C1 evaluates C function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of C(X).
%
  value = 0.0;

  return
end
function value = exact1 ( x, y )

%*****************************************************************************80
%
%% EXACT1 evaluates exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of the solution.
%
  value = x .* ( 1.0 - x ) .* y .* ( 1.0 - y );

  return
end
function value = exact_ux1 ( x, y )

%*****************************************************************************80
%
%% EXACT_UX1 evaluates the derivative dUdX of exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of dUdX.
%
  value = ( 1.0 - 2.0 * x ) .* ( y - y .* y );

  return
end
function value = exact_uy1 ( x, y )

%*****************************************************************************80
%
%% EXACT_UY1 evaluates the derivative dUdY of exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of dUdX.
%
  value = ( x - x .* x ) .* ( 1.0 - 2.0 * y );

  return
end
function value = f1 ( x, y )

%*****************************************************************************80
%
%% F1 evaluates right hand side function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of the right hand side.
%
  value = 2.0 * x .* ( 1.0 - x ) ...
        + 2.0 * y .* ( 1.0 - y );

  return
end


function [ t, w ] = t_quadrature_rule ( n )

%*****************************************************************************80
%
%% T_QUADRATURE_RULE: quadrature rule for T(n,x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the rule.
%
%    Output, real T(N,1), W(N,1), the points and weights of the rule.
%
  aj = zeros ( n, 1 );

  bj = 0.5 * ones ( n, 1 );
  bj(1) = sqrt ( 0.5 );

  w = zeros ( n, 1 );
  w(1,1) = sqrt ( pi );

  [ t, w ] = imtqlx ( n, aj, bj, w );

  w(1:n,1) = w(1:n,1).^2;

  return
end