function [xi,w]=eighthOrderTriangleCubPoints(algorithm)
%%EIGHTHORDERTRIANGLECUBPOINTS Obtain eighth-order cubature points for
%   integration over a triangle in 2D. The points and weights are for the
%   triangle with vertices (1,0), (0,1), (0,0), but can be transformed to
%   any triangle using transformSimplexTriPoints.
%
%INPUTS: algorithm An optional parameter selecting the algorithm for the
%                  specific point set. Possible values are:
%                  0 (The default if omitted or an empty matrix is passed)
%                    Use the algorithm of [1] (16 points).
%                  1 Use the 8th order points found using the algorithm of
%                    [2], given in the supplementary material of [2] (16
%                    points).
%
%OUTPUTS: xi A 2XnumCubPoints set of points for the standard triangle.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the triangle (1/2).
%
%This function implements the points given in [1] and [2].
%
%EXAMPLE:
%Given the vertices of the simplex, we compare an eighth-order moment
%computed using these cubature points to one computed using
%monomialIntSimplex. The results are the same within typical finite
%precision limits.
% [xi,w]=eighthOrderTriangleCubPoints();
% alpha=[6;2];
% theMoment=findMomentFromSamp(alpha,xi,w)
% intVal=monomialIntSimplex(alpha)
%
%REFERENCES:
%[1]L. Zhang and T. Cui, "A set of symmetric quadrature rules on triangles
%   and tetrahedra," Journal of Computational Mathematics, vol. 27, no. 1,
%   pp. 89-96, Jan. 2009.
%[2] F. D. Witherden and P. E. Vincent, "On the identification of symmetric
%    quadrature rules for finite element methods," Computer and Mathematics
%    with Applications, vol. 69, no. 10, pp. 1232-1241, May 2015.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<1||isempty(algorithm))
    algorithm=0;
end

switch(algorithm)
    case 0
        w1=0.1443156076777871682510911104890646;
        w2=0.1032173705347182502817915502921290;
        w3=0.0324584976231980803109259283417806;
        w4=0.0950916342672846247938961043885843;
        w5=0.0272303141744349942648446900739089;
        w=[w1;w2;w2;w2;w3;w3;w3;w4;w4;w4;w5;w5;w5;w5;w5;w5];

        p1=0.3333333333333333333333333333333333;
        p2=0.1705693077517602066222935014914645;
        p3=0.0505472283170309754584235505965989;
        p4=0.4592925882927231560288155144941693;
        p5a=0.2631128296346381134217857862846436;
        p5b=0.0083947774099576053372138345392944;

        xiBary=zeros(3,16);
        xiBary(:,1)=[p1;p1;p1];
        xiBary(:,2:4)=genAllMultisetPermutations([p2;p2;1-2*p2]);
        xiBary(:,5:7)=genAllMultisetPermutations([p3;p3;1-2*p3]);
        xiBary(:,8:10)=genAllMultisetPermutations([p4;p4;1-2*p4]);
        xiBary(:,11:16)=genAllMultisetPermutations([p5a;p5b;1-p5a-p5b]);

        %Adjust w for the area of the standard triangle.
        w=w/2;

        %Convert the barycentric points into normal cubature points for the
        %standard triangle given the vertices.
        vertices=[1,0,0;
                  0,1,0];
        xi=barycentricCoords2Pt(xiBary,vertices);
    case 1
        M=[-0.33333333333333333333333333333333333333,   -0.33333333333333333333333333333333333333,     0.2886312153555743365021822209781292496;
          -0.081414823414553687942368971011661355879,   -0.83717035317089262411526205797667728824,     0.1901832685345692495877922087771686332;
           -0.83717035317089262411526205797667728824,  -0.081414823414553687942368971011661355879,     0.1901832685345692495877922087771686332;
          -0.081414823414553687942368971011661355879,  -0.081414823414553687942368971011661355879,     0.1901832685345692495877922087771686332;
           -0.65886138449647958675541299701707099796,    0.31772276899295917351082599403414199593,    0.20643474106943650056358310058425806003;
            0.31772276899295917351082599403414199593,   -0.65886138449647958675541299701707099796,    0.20643474106943650056358310058425806003;
           -0.65886138449647958675541299701707099796,   -0.65886138449647958675541299701707099796,    0.20643474106943650056358310058425806003;
           -0.89890554336593804908315289880680210631,    0.79781108673187609816630579761360421262,   0.064916995246396160621851856683561193593;
            0.79781108673187609816630579761360421262,   -0.89890554336593804908315289880680210631,   0.064916995246396160621851856683561193593;
           -0.89890554336593804908315289880680210631,   -0.89890554336593804908315289880680210631,   0.064916995246396160621851856683561193593;
           -0.98321044518008478932557233092141110162,    0.45698478591080856248200075835212392604,    0.05446062834886998852968938014781784832;
            0.45698478591080856248200075835212392604,   -0.98321044518008478932557233092141110162,    0.05446062834886998852968938014781784832;
           -0.47377434073072377315642842743071282442,    0.45698478591080856248200075835212392604,    0.05446062834886998852968938014781784832;
            0.45698478591080856248200075835212392604,   -0.47377434073072377315642842743071282442,    0.05446062834886998852968938014781784832;
           -0.47377434073072377315642842743071282442,   -0.98321044518008478932557233092141110162,    0.05446062834886998852968938014781784832;
           -0.98321044518008478932557233092141110162,   -0.47377434073072377315642842743071282442,    0.05446062834886998852968938014781784832];

        w=M(:,3);
        xi=M(:,1:2)';
        %Transform the points to the standard triangle.
        v1=[-1,-1, 1;
            -1, 1,-1];
        v2=[1,0,0;
            0,1,0];
        [A,d]=affineTransBetweenTriangles(v1,v2);
        xi=bsxfun(@plus,A*xi,d);
        w=w/4;
    otherwise
        error('Unknown Algorithm Specified')
end
end

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


function X = traj_opt3c(path, total_time, ts)
% 3rd order trajectory optimization (minimum acceleration)
% by solving 4m linear equations. Assumes constant speed
% @author       Yiren Lu
% @email        luyiren [at] seas [dot] upenn [dot] edu
%
% @input:       path                (m+1) by 3 planning trajectory
%               total_time          total time
% @output       X                   4mx3 solution vector, where
%                                   X(4*(k-1)+1:4*k,d) is the coefficients
%                                   of x_k(t), i.e.,
%                                   x_k(t)=X(4*(k-1)+1:4*k,d)'*[t^3;t^2,t,1]
%                                   where d \in [1,2,3] to indicate dimension xyz
   path0 = path;
   % generate the trajectory here, and decide the total_time
   [m,n] = size(path0); % n == 3
   % there we set m = m - 1 for convenience
   m = m-1;
   % there are now in total m+1 points in the path, which seperate the path into
   % m subpaths.

   % ts(k) = t_{k+1}, e.g. ts(1) = t_0,
   % time planning according to the segment length

   % ts(m+1) = t_m
   % In 3rd order trajectory optimization, for each subpath there are 4
   % parameters
   % x_k(t) = c_{k,3}*t^3 + c_{k,2}*t^2 + c_{k,1}*t^1 + c_{k,0}
   %    for k = 1..(m-1)

   % X contains the parameters
   X = zeros(4*m,n);
   A = zeros(4*m, 4*m, n);
   Y = zeros(4*m,n);

   for i = 3
       A(:,:,i) = eye(4*m)*eps;
       % constraint 1: x_k(t_k) = x_{k+1}(t_k) = p_k, where p_k is a
       % waypoint
       % x_k(t) = c_{k,3}*t^3 + c_{k,2}*t^2 + c_{k,1}*t^1 + c_{k,0}
       %    for k = 1..(m-1)
       % e.g.   x_1(t_1) = x_2(t_1) = p_1;
       % there are in total 2*(m-1) constraints
       idx = 1; % constraint counter
       for k = 1:(m-1)
           A(idx, 4*(k-1)+1:4*k, i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1];
           Y(idx,i) = path0(k+1,i);
           idx = idx + 1;
           A(idx, 4*(k)+1:4*(k+1), i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1];
           Y(idx,i) = path0(k+1,i);
           idx = idx + 1;
       end
       % constraint 2: \dot{x}_k(t_k) = \dot{x}_{k+1}(t_k)
       % \dot{x}_k(t) = 3*c_{k,3}*t^2 + 2*c_{k,2}*t + c_{k,1}
       % e.g.  \dot{x}_1(t_1) = \dot{x}_2(t_1)
       % there are in total m-1 constraints
       for k = 1:(m-1)
           A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0];
%            A(idx, 4*(k)+1:4*(k+1), i) = -[3*ts(k+1)^2, 2*ts(k+1), 1, 0];
           Y(idx,i) = 0;
           idx = idx + 1;
           A(idx, 4*(k)+1:4*(k+1), i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0];
           Y(idx,i) = 0;
           idx = idx + 1;
       end

%        % constraint 3: \ddot{x}_k(t_k) = \ddot{x}_{k+1}(t_k)
%        % \ddot{x}_k(t) =6*c_{k,3}*t + 2*c_{k,2}
%        % e.g. \ddot{x}_1(t_1) = \ddot{x}_2(t_1)
%        % there are in total m-1 constraints
%        for k = 1:(m-1)
%            A(idx, 4*(k-1)+1:4*k, i) = [6*ts(k+1), 2, 0, 0];
%            A(idx, 4*(k)+1:4*(k+1), i) = -[6*ts(k+1), 2, 0, 0];
%            Y(idx,i) = 0;
%            idx = idx + 1;
%        end

       % so far there are 2*(m-1) + (m-1) + (m-1) = 4m - 4 constraints
       % there are 4 left:
       %    x_1(t_0) = p_0
       %    x_T(t_T) = p_T
       %    \dot{x}_0(t_0) = 0
       %    \dot{x}_T(t_T) = 0
       k = 1;
       A(idx, 4*(k-1)+1:4*k, i) = [ts(k)^3, ts(k)^2, ts(k), 1];
       Y(idx,i) = path0(k,i);
       idx = idx + 1;
       A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k)^2, 2*ts(k), 1, 0];
       Y(idx,i) = 0;
       idx = idx + 1;
       k = m;
       A(idx, 4*(k-1)+1:4*k, i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1];
       Y(idx,i) = path0(k+1,i);
       idx = idx + 1;
       A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0];
       Y(idx,i) = 0;
       idx = idx + 1;
%        assert(rank(A(:,:,i))==4*m);
       X(:,i) = A(:,:,i)\Y(:,i);
   end
%    for i = 1:n
%        A(:,:,i) = eye(4*m)*eps;
%        % constraint 1: x_k(t_k) = x_{k+1}(t_k) = p_k, where p_k is a
%        % waypoint
%        % x_k(t) = c_{k,3}*t^3 + c_{k,2}*t^2 + c_{k,1}*t^1 + c_{k,0}
%        %    for k = 1..(m-1)
%        % e.g.   x_1(t_1) = x_2(t_1) = p_1;
%        % there are in total 2*(m-1) constraints
%        idx = 1; % constraint counter
%        for k = 1:(m-1)
%            A(idx, 4*(k-1)+1:4*k, i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1];
%            Y(idx,i) = path0(k+1,i);
%            idx = idx + 1;
%            A(idx, 4*(k)+1:4*(k+1), i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1];
%            Y(idx,i) = path0(k+1,i);
%            idx = idx + 1;
%        end
%        % constraint 2: \dot{x}_k(t_k) = \dot{x}_{k+1}(t_k)
%        % \dot{x}_k(t) = 3*c_{k,3}*t^2 + 2*c_{k,2}*t + c_{k,1}
%        % e.g.  \dot{x}_1(t_1) = \dot{x}_2(t_1)
%        % there are in total m-1 constraints
%        for k = 1:(m-1)
%            A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0];
%            A(idx, 4*(k)+1:4*(k+1), i) = -[3*ts(k+1)^2, 2*ts(k+1), 1, 0];
%            Y(idx,i) = 0;
%            idx = idx + 1;
%        end
%
%        % constraint 3: \ddot{x}_k(t_k) = \ddot{x}_{k+1}(t_k)
%        % \ddot{x}_k(t) =6*c_{k,3}*t + 2*c_{k,2}
%        % e.g. \ddot{x}_1(t_1) = \ddot{x}_2(t_1)
%        % there are in total m-1 constraints
%        for k = 1:(m-1)
%            A(idx, 4*(k-1)+1:4*k, i) = [6*ts(k+1), 2, 0, 0];
%            A(idx, 4*(k)+1:4*(k+1), i) = -[6*ts(k+1), 2, 0, 0];
%            Y(idx,i) = 0;
%            idx = idx + 1;
%        end
%
%        % so far there are 2*(m-1) + (m-1) + (m-1) = 4m - 4 constraints
%        % there are 4 left:
%        %    x_1(t_0) = p_0
%        %    x_T(t_T) = p_T
%        %    \dot{x}_0(t_0) = 0
%        %    \dot{x}_T(t_T) = 0
%        k = 1;
%        A(idx, 4*(k-1)+1:4*k, i) = [ts(k)^3, ts(k)^2, ts(k), 1];
%        Y(idx,i) = path0(k,i);
%        idx = idx + 1;
%        A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k)^2, 2*ts(k), 1, 0];
%        Y(idx,i) = 0;
%        idx = idx + 1;
%        k = m;
%        A(idx, 4*(k-1)+1:4*k, i) = [ts(k+1)^3, ts(k+1)^2, ts(k+1), 1];
%        Y(idx,i) = path0(k+1,i);
%        idx = idx + 1;
%        A(idx, 4*(k-1)+1:4*k, i) = [3*ts(k+1)^2, 2*ts(k+1), 1, 0];
%        Y(idx,i) = 0;
%        idx = idx + 1;
% %        assert(rank(A(:,:,i))==4*m);
%        X(:,i) = A(:,:,i)\Y(:,i);
%    end

end


function e = legendre_monomial_quadrature ( n, x, w, p )

%*****************************************************************************80
%
%% LEGENDRE_MONOMIAL_QUADRATURE applies a quadrature rule to a monomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 February 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points in the rule.
%
%    Input, real X(N), the quadrature points.
%
%    Input, real W(N), the quadrature weights.
%
%    Input, integer P, the exponent.
%
%    Output, real E, the quadrature error.
%

%
%  Get the exact value of the integral.
%
  t = legendre_integral ( p );
%
%  Evaluate the monomial at the quadrature points.
%
  v(1:n,1) = x(1:n).^p;
%
%  Compute the weighted sum.
%
  q = w' * v;
%
%  Error:
%
  if ( t == 0.0 )
    e = abs ( q - t );
  else
    e = abs ( ( q - t ) / t );
  end

  return
end


%COMPUTEEXACTMARGINALSBP Runs exact inference and returns the marginals
%over all the variables (if isMax == 0) or the max-marginals (if isMax == 1).
%
%   M = COMPUTEEXACTMARGINALSBP(F, E, isMax) takes a list of factors F,
%   evidence E, and a flag isMax, runs exact inference and returns the
%   final marginals for the variables in the network. If isMax is 1, then
%   it runs exact MAP inference, otherwise exact inference (sum-prod).
%   It returns an array of size equal to the number of variables in the
%   network where M(i) represents the ith variable and M(i).val represents
%   the marginals of the ith variable.
%
% Copyright (C) Daphne Koller, Stanford University, 2012

function M = ComputeExactMarginalsBP(F, E, isMax)

% initialization
% you should set it to the correct value in your code
M = [];
vars = unique([F.var]);

N = length(vars);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% YOUR CODE HERE
%
% Implement Exact and MAP Inference.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CliqTree = CreateCliqueTree(F,E);
P = CliqueTreeCalibrate(CliqTree,isMax);
M = repmat(struct('var',[],'card',[],'val',[]),1,N);
for i = 1:N
	for j = 1:length(CliqTree.cliqueList)
		inter = intersect(vars(i),CliqTree.cliqueList(j).var);
		if length(inter)==1
			V = setdiff(CliqTree.cliqueList(j).var,vars(i));
			if isMax == 0
				M(i) = FactorMarginalization(P.cliqueList(j),V);
			else
				M(i) = FactorMaxMarginalization(P.cliqueList(j),V);
			end
			break;
		end
	end
end
if isMax==0
for i=1:N
	M(i).val = M(i).val/sum(M(i).val);
end
end
end


function [x,state] = struct_band(z,task,size_mat,band)
%STRUCT_BAND Band matrix.
%   [x,state] = struct_band(z,[],size_mat,band) generates a band matrix x
%   of size size_mat, where the diagonals band(1) to band(2) are filled
%   column by column with entries from the vector z. For example, if x is a
%   square matrix of order n, the vector z should have length
%   sum(n-abs(band(1):band(2))). The structure state stores information
%   which is reused in computing the right and left Jacobian-vector
%   products.
%
%   struct_band(z,task,size_mat,band) computes the right or left
%   Jacobian-vector product of this transformation, depending on the
%   structure task. Use the structure state and add the field 'r' of the
%   same shape as z or the field 'l' of the same shape as x to obtain the
%   structure task for computing the right and left Jacobian-vector
%   products
%
%      (dF(:)/dz(:).')*task.r(:) and
%      (dF(:)/dz(:).')'*task.l(:) + conj((dF(:)/dconj(z(:)).')'*task.l(:)),
%
%   respectively. Here, F(z) represents this transormation, (:) signifies
%   vectorization and the derivative w.r.t. z (conj(z)) is a partial
%   derivative which treats conj(z) (z) as constant. The output has the
%   same shape as x or z for the right and left Jacobian-vector products,
%   respectively.
%
%   See also struct_diag, struct_tridiag, struct_tril, struct_triu.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Structured data fusion,"
%       ESAT-SISTA Internal Report 13-177, KU Leuven, 2013.

if nargin < 2, task = []; end
if nargin < 3 || ~isvector(size_mat)
    error('struct_band:size_mat','Missing integer matrix order.');
end
if nargin < 4 || ~isvector(band)
    error('struct_band:band','Missing definition of band.');
end

if isempty(task) || (isempty(task.l) && isempty(task.r))
    state.idx = bsxfun(@plus,(size_mat(1)-1:-1:0).', ...
        (0:size_mat(2)-1)-size_mat(1)+1);
    state.idx = find(state.idx >= band(1) & state.idx <= band(2));
    x = zeros(size_mat);
    x(state.idx) = z;
elseif ~isempty(task.r)
    x = zeros(size_mat);
    x(task.idx) = task.r;
    state = [];
elseif ~isempty(task.l)
    x = task.l(task.idx);
    state = [];
end

end


%% Basis Pursuit with Douglas Rachford
% Test for DR algorithm for L1 minimization (BP).
% We do here a compressed sensing resolution
% (random matrix).

%%
% Add the toolbox.

addpath('../');
addpath('../toolbox/');

%%
% Dimensionality of the signal and number of measurements.

n = 200;
p = n/4;

%%
% Sensing matrix.

A = randn(p,n);

%%
% Measurements.

y = randn(p,1);

%%
% We aim at solving

%%
% |min_{A*x=y} norm(x,1)|

%%
% This can be rewriten as the minimization of |F(x)+G(x)|
% where |F=norm(x,1)| and |G=i_{A*x=y}| is the indicator function.

%%
% The proximity operator of the L1 norm is the soft thresholding.

ProxF = @(x,tau)perform_soft_thresholding(x, tau);

%%
% The proximity operator of the indicator of |A*x=y| is the orthogonal
% projection on A*x=y.

pA = A'*(A*A')^(-1);
ProxG = @(x,tau)x + pA*(y-A*x);

%%
% Create a function to record the values of F and the constraint at each iteration.

F = @(x)norm(x,1);
Constr = @(x)1/2*norm(y-A*x)^2;
options.report = @(x)struct('F', F(x), 'Constr', Constr(x));

%%
% Run the algorithm.

options.gamma = 5;
options.niter = 5000;
[x,R] = perform_dr(zeros(n,1), ProxF, ProxG, options);

%%
% Display the solution. At convergence, it should be of sparsity |p|.

clf;
plot(x);
axis tight;

%%
% Retrieve the F and constraint function values.

f = s2v(R,'F');
constr = s2v(R,'Constr');

%%
% Display.

clf;
subplot(2,1,1);
plot(f(2:end));
axis tight; title('Objective');
subplot(2,1,2);
plot(constr(2:end));
axis tight; title('Constraint');


%% Example 8.15: Duffing van der Pol oscillator
%
% Copyright:
%   2018 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

%% Duffing van der Pol

  % Time-span
  tspan = 0:2^-5:20;

  % Parameters
  alpha = 1;

  % Define arrow (for visalization)
  arrow1 = [-1 1 0 -1; -.5 -.5 2 -.5]';

  % The model
  f = @(x,t) [x(2,:); x(1,:).*(alpha - x(1,:).^2)-x(2,:)];
  L = @(x,t) [zeros(1,size(x,2)); x(1,:)];

%% ODE

  figure(1); clf; hold on

  for j=1:10
    %x = rk4(f,tspan,[-2-.2*j; 0]);
    x = rk4simple(f,tspan,[-2-.2*j; 0]);
    %[~,x] = ode45(@(t,x) f(x,t),tspan,[-2-.2*j; 0]); x = x';

    % Plot trajectory
    plot(x(1,:),x(2,:),'-k','LineWidth',.25)

    % Plot direction
    uv = f(x,[]); ind = 10;
    newquiver(x(1,ind),x(2,ind),uv(2,ind),-uv(1,ind), ...
      'X',arrow1,'scale',.04*[1 14.8/8.8])

  end

  % Set axis limits
  axis([-4.4 4.4 -4.8 10]), axis square
  %set(gca,'XTick',-4:2:4,'YTick',-4:2:8)
  xlabel('$x_1$'); ylabel('$x_2$')
  box on

%% SDE

  figure(2); clf; hold on
  figure(3); clf; hold on

  for j=1:10

    figure(2);

    % Lock random seed
    if exist('rng') % Octave doesn't have rng
      rng(3,'twister')
    else
      randn('state',2);
      rand('state',2);
    end

    % Use the strong order 1.0 method
    x = srkS10scalarnoise(f,L,tspan,[-2-.2*j; 0],.5^2);

    % Plot trajectory
    plot(x(1,:),x(2,:),'-k','LineWidth',.25)

    % Plot direction
    uv = f(x,[]); ind = 10;
    newquiver(x(1,ind),x(2,ind),uv(2,ind),-uv(1,ind), ...
      'X',arrow1,'scale',.04*[1 14.8/8.8])

    figure(3);
    plot(tspan,x(1,:),'-k')
    plot(tspan,x(2,:),'-','Color',[.7 .7 .7])

  end

  % Set axis limits
  figure(2)
  axis([-4.4 4.4 -4.8 10]), axis square
  %set(gca,'XTick',-4:2:4,'YTick',-4:2:8)
  xlabel('$x_1$'); ylabel('$x_2$')
  box on

  % Set axis limits
  figure(3)
  xlim([0 20])
  xlabel('Time, $t$'); ylabel('$x$')
  legend('$x_1(t)$','$x_2(t)$')
  box on

%% Weak approximation

  % Time-span
  tspan = 0:2^-4:20;

  % Reset random seed
  % Lock random seed
  if exist('rng') % Octave doesn't have rng
    rng(1,'twister')
  else
    randn('state',2);
    rand('state',2);
  end

  % Initial point
  x0 = [-3;0];

  % Number of smaples
  x = zeros(2,10000);

  % Iterate
  for j=1:size(x,2)

    % Weak SRK scheme
    foo = srkW20(f,L,tspan,x0,.5^2,true);

    % Store
    x(:,j) = foo(:,end);

    % Report
    if rem(j,100)==0,
      figure(4); clf
        hist(x(1,1:j),ceil(sqrt(j)))
        drawnow
      j
    end

  end

%% Histogram

  % Bins for histogram
  t = linspace(min(x(1,:)),max(x(1,:)),64);

  figure(5); clf

    % Show solution w2.0
    n = histc(x(1,:),t);
    stairs(t-(t(2)-t(1))/2,n/size(x,2),'-k')

    % Label
    xlabel('$x_1$')

    % Ticks
    box off
    xlim([-2.2 2.2])
    %set(gca,'XTick',0:.2:1.2)

  figure(6); clf

    % Show solution w2.0
    plot(x(1,:),x(2,:),'.k')

    % Label
    xlabel('$x_1$')
    ylabel('$x_2$')

    % Ticks
    box on


% function F = det_F_gold(x1,x2,L_COST,SAMPSON_APPROXIMATION,NORMALIZE)
% Determines the F by iteratively minimizing the geometric error
% Algorithm 11.2 in Hartley & Zisserman, Multiple View Geometry in Computer
% Vision
% Inputs:
%           x1                      3xN coordinates of matched points in image 1(homogeneous)
%           x2                      3xN coordinates of matched points in image 2(homogeneous)
%           L_COST                  1x1 (optional) controls penalization scheme
%                                       for the cost function: L_COST 1 leads to L_1 minimization of
%                                       the average geometric cost (the one mentioned in the book)
%           SAMPSON_APPROXIMATION   1x1 (optional) if enabled,
%                                       approximates the geometric mean with the sampson cost
%           NORMALIZE               1x1 (optional)determines if the algorithm
%                                       should use the normalized points
% Outputs:
%           F                       3x3 the fundamental matrix
%
% Author: Omid Aghazadeh, KTH(Royal Institute of Technology), 2010/05/09
function F = det_F_gold(x1,x2,L_COST,SAMPSON_APPROXIMATION,NORMALIZE)
if sum(size(x1)~=size(x2)), error('size of correspondences do not match!'), end
if size(x1,1) ~= 3, error('invalid points'), end
global MAX_FUN_EVAL MAX_ITER TOL_X TOL_FUN;
if nargin<3, L_COST = 1; end;
if nargin<4, SAMPSON_APPROXIMATION = 0; end
if nargin<5, NORMALIZE = 1; end;
if NORMALIZE
    nmat1 = get_normalization_matrix(x1); % isotropic normalization (translation/scaling)
    nmat2 = get_normalization_matrix(x2);
    x1n = nmat1*x1;
    x2n = nmat2*x2;
else
    nmat1 = eye(3); nmat2 = eye(3); x1n = x1; x2n = x2;
end
x1n = x1n./repmat(x1n(3,:),3,1); x2n = x2n./repmat(x2n(3,:),3,1); % normalizing points so their last coordinate is 1
F_0 = det_F_normalized_8point(x1n,x2n);
%% (ii)

[e,e_prime] = get_epipole(F_0);
e_prime_cross = get_x_cross(e_prime);
P2 = [e_prime_cross*F_0 e_prime];

%% (iii) minimze the cost

if ~ SAMPSON_APPROXIMATION
    [P2] = lsqnonlin(@(p2)costGold(x1n,x2n,p2,L_COST),P2,[],[],optimset('Display','off','TolX',TOL_X,'TolFun',TOL_FUN,'MaxFunEval',MAX_FUN_EVAL,'MaxIter',MAX_ITER,'Algorithm',{'levenberg-marquardt' 0.01}));
else
    [P2] = lsqnonlin(@(p2)costSampson(x1n,x2n,p2),P2,[],[],optimset('Display','off','TolX',TOL_X,'TolFun',TOL_FUN,'MaxFunEval',MAX_FUN_EVAL,'MaxIter',MAX_ITER,'Algorithm',{'levenberg-marquardt' 0.01}));
end

Fhat = get_x_cross(P2(:,4))*P2(:,1:3);

F= nmat2' * Fhat* nmat1; % denormalization

end

%% function scost = costGold(x1,x2,P2,L_COST)
% this is the cost function for the Gold Standard algoritghm. The
% triangulation method is the inhomogeneous one(chapter 12 of the book)
function scost = costGold(x1,x2,P2,L_COST)
Xhat = triangulate(x1,x2,P2,1);
xhat1 = Xhat(1:3,:)./repmat(Xhat(3,:),3,1); % the first camera is assumed to be [I|0]
xhat2 = P2 * Xhat;
xhat2 = xhat2./repmat(xhat2(3,:),3,1);
cost = ((x1(:)-xhat1(:)).^2 + (x2(:)-xhat2(:)).^2);
scost = sqrt(sum(cost))^L_COST;
end

%% function scost = costSampson(x1,x2,P2)
% this is the cost function for the sampson approximation. It implements an
% over-parametrization of F, however the minimal solution can be easily
% integrated here.
function scost = costSampson(x1,x2,P2)
F = get_x_cross(P2(:,4))*P2(:,1:3);
Fx1 = F*x1;
Ftx2 = F'*x2;
num = sum(x2 .* Fx1,1).^2;
denum= sum(Fx1(1:2,:).^2,1) + sum(Ftx2(1:2,:).^2,1);
cost = num./denum;
scost = sqrt(sum(cost));
end


function D = prtDistanceLNorm(dataSet1,dataSet2,Lnorm)
% prtDistanceLNorm   L Norm distance function.
%
%   DIST = prtDistanceCityBlock(DS1,DS2) calculates the LNorm distance
%   from all the observations in datasets DS1 to DS2, and ouputs a distance
%   matrix of size DS1.nObservations x DS2.nObservations. DS1 and DS2
%   should have the same number of features. DS1 and DS2 should be
%   prtDataSet objects.
%
%   For more information, see:
%
%   http://en.wikipedia.org/wiki/Norm_(mathematics)#p-norm
%
% Example:
%
%   % Create 2 data sets
%   dsx = prtDataSetStandard('Observations', [0 0; 1 1]);
%   dsy = prtDataSetStandard('Observations', [1 0;2 2; 3 3]);
%   % Compute distance
%   distance = prtDistanceLnorm(dsx,dsy)
%
% See also: prtDistanceCityBlock, prtDistanceChebychev
% prtDistanceMahalanobis, prtDistanceSquare, prtDistanceEuclidean

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A portion of IPDM from MATLAB Central is used in this function see
% prtExternal.IPDM.ipdm(). The license information from that file is below.
%
% Copyright (c) 2009, John D'Errico
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[data1,data2] = prtUtilDistanceParseInputs(dataSet1,dataSet2);

% Used to handle memory efficiency paths see IPDM
chunkSize = 2^25;

[nSamples1, nDim1] = size(data1);
[nSamples2, nDim2] = size(data2);

if nDim1 ~= nDim2
    error('Dimensionality of data1 and data2 must be equal')
end

if (nDim1>1) && ((nSamples1*nSamples2*nDim1)<=chunkSize)
    switch Lnorm
        case 1
            D = sum(abs(bsxfun(@minus,reshape(data1,[nSamples1,1,nDim1]),reshape(data2,[1,nSamples2,nDim1]))),3);
        case inf
            D = max(abs(bsxfun(@minus,reshape(data1,[nSamples1,1,nDim1]),reshape(data2,[1,nSamples2,nDim1]))),[],3);
        case 0
            D = min(abs(bsxfun(@minus,reshape(data1,[nSamples1,1,nDim1]),reshape(data2,[1,nSamples2,nDim1]))),[],3);

            % This code has overflow problems for large data1 and data2
            %         case 2
            %             %un-rolled((x-y)^2)) - sqrt below; this takes less time than
            %             %the generic code below for the most common L-norm (2)
            %
            %             %D = repmat(sum((data1.^2), 2), [1 nSamples2]) + repmat(sum((data2.^2),2), [1 nSamples1]).' - 2*data1*(data2.');
            %
            %             %             %Handle overflow issues for large data2
            %             %             muData2 = prtUtilNanMean(data2);
            %             %             data2 = bsxfun(@minus,data2,muData2);
            %             %             data1 = bsxfun(@minus,data1,muData2);
            %
            %             D = bsxfun(@minus,bsxfun(@plus,sum((data1.^2), 2),sum((data2.^2),2).'),2*data1*(data2.'));
        otherwise
            D = sum(bsxfun(@minus,reshape(data1,[nSamples1,1,nDim1]),reshape(data2,[1,nSamples2,nDim1])).^Lnorm,3);
    end
else
    % too big, so that the ChunkSize will have been exceeded, or just 1-d
    if isfinite(Lnorm) && Lnorm ~= 1
        D = bsxfun(@minus,data1(:,1),data2(:,1)').^Lnorm;
    else
        D = abs(bsxfun(@minus,data1(:,1),data2(:,1)'));
    end
    for i=2:nDim1
        switch Lnorm
            case 1
                D = D + abs(bsxfun(@minus,data1(:,i),data2(:,i)'));
            case inf
                D = max(D,abs(bsxfun(@minus,data1(:,i),data2(:,i)')));
            case 0
                D = min(D,abs(bsxfun(@minus,data1(:,i),data2(:,i)')));
            otherwise
                D = D + bsxfun(@minus,data1(:,i),data2(:,i)').^Lnorm;
        end
    end
end

if isfinite(Lnorm) && Lnorm ~= 1
    if Lnorm == 2
        D = sqrt(D);
        if isreal(data1) && isreal(data2)
            D = real(D);
        end
    else
        D = D.^(1./Lnorm);
    end
end


function visible_probability = hidden_state_to_visible_probabilities(rbm_w, hidden_state)
  % <rbm_w> is a matrix of size <number of hidden units> by <number of visible units>
  % <hidden_state> is a binary matrix of size <number of hidden units> by <number of configurations that we're handling in parallel>.
  % The returned value is a matrix of size <number of visible units> by <number of configurations that we're handling in parallel>.
  % This takes in the (binary) states of the hidden units, and returns the activation probabilities of the visible units, conditional on those states.
  m = size(hidden_state, 2);
  number_hidden = size(rbm_w, 1);
  number_visible = size(rbm_w, 2);
  visible_probability = zeros(number_visible, m);

  for k = 1:m
    hidden_state_k = hidden_state(:, k);
    visible_probability(:, k) = 1 ./ (1 + exp(-(rbm_w' * hidden_state_k)));
  end
end


function a = c8mat_exp_a ( test, n )

%*****************************************************************************80
%
%% C8MAT_EXP_A returns the matrix for a given test.
%
%  Discussion:
%
%    1) Diagonal, real;
%    2) Diagonal, pure imaginary.
%    3) Diagonal, complex.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer TEST, the index of the test case.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(N,N), the matrix.
%
  if ( test == 1 )
    a = [ 1,  0;
          0,  2 ];
  elseif ( test == 2 )
    a = [  3 * i,  0;
           0,   - 4 * i ];
  elseif ( test == 3 )
    a = [  5 + 6 * i,    0;
          0,  7 - 8 * i ];
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'C8MAT_EXP_A - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of TEST = %d\n', test );
    error ( 'C8MAT_EXP_A - Fatal error!' );
  end

  return
end


function [grad,err,finaldelta] = gradest(fun,x0)
% gradest: estimate of the gradient vector of an analytical function of n variables
% usage: [grad,err,finaldelta] = gradest(fun,x0)
%
% Uses derivest to provide both derivative estimates
% and error estimates. fun needs not be vectorized.
%
% arguments: (input)
%  fun - analytical function to differentiate. fun must
%        be a function of the vector or array x0.
%
%  x0  - vector location at which to differentiate fun
%        If x0 is an nxm array, then fun is assumed to be
%        a function of n*m variables.
%
% arguments: (output)
%  grad - vector of first partial derivatives of fun.
%        grad will be a row vector of length numel(x0).
%
%  err - vector of error estimates corresponding to
%        each partial derivative in grad.
%
%  finaldelta - vector of final step sizes chosen for
%        each partial derivative.
%
%
% Example:
%  [grad,err] = gradest(@(x) sum(x.^2),[1 2 3])
%  grad =
%      2     4     6
%  err =
%      5.8899e-15    1.178e-14            0
%
%
% Example:
%  At [x,y] = [1,1], compute the numerical gradient
%  of the function sin(x-y) + y*exp(x)
%
%  z = @(xy) sin(diff(xy)) + xy(2)*exp(xy(1))
%
%  [grad,err ] = gradest(z,[1 1])
%  grad =
%       1.7183       3.7183
%  err =
%    7.537e-14   1.1846e-13
%
%
% Example:
%  At the global minimizer (1,1) of the Rosenbrock function,
%  compute the gradient. It should be essentially zero.
%
%  rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;
%  [g,err] = gradest(rosen,[1 1])
%  g =
%    1.0843e-20            0
%  err =
%    1.9075e-18            0
%
%
% See also: derivest, gradient
%
%
% Author: John D'Errico
% e-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 2/9/2007

% get the size of x0 so we can reshape
% later.
sx = size(x0);

% total number of derivatives we will need to take
nx = numel(x0);

grad = zeros(1,nx);
err = grad;
finaldelta = grad;
for ind = 1:nx
  [grad(ind),err(ind),finaldelta(ind)] = lf_derivest( ...
    @(xi) fun(swapelement(x0,ind,xi)), ...
    x0(ind),'deriv',1,'vectorized','no', ...
    'methodorder',2);
end

end % mainline function end

% =======================================
%      sub-functions
% =======================================
function vec = swapelement(vec,ind,val)
% swaps val as element ind, into the vector vec
vec(ind) = val;

end % sub-function end

function [der,errest,finaldelta] = lf_derivest(fun,x0,varargin)
% DERIVEST: estimate the n'th derivative of fun at x0, provide an error estimate
% usage: [der,errest] = DERIVEST(fun,x0)  % first derivative
% usage: [der,errest] = DERIVEST(fun,x0,prop1,val1,prop2,val2,...)
%
% Derivest will perform numerical differentiation of an
% analytical function provided in fun. It will not
% differentiate a function provided as data. Use gradient
% for that purpose, or differentiate a spline model.
%
% The methods used by DERIVEST are finite difference
% approximations of various orders, coupled with a generalized
% (multiple term) Romberg extrapolation. This also yields
% the error estimate provided. DERIVEST uses a semi-adaptive
% scheme to provide the best estimate that it can by its
% automatic choice of a differencing interval.
%
% Finally, While I have not written this function for the
% absolute maximum speed, speed was a major consideration
% in the algorithmic design. Maximum accuracy was my main goal.
%
%
% Arguments (input)
%  fun - function to differentiate. May be an inline function,
%        anonymous, or an m-file. fun will be sampled at a set
%        of distinct points for each element of x0. If there are
%        additional parameters to be passed into fun, then use of
%        an anonymous function is recommended.
%
%        fun should be vectorized to allow evaluation at multiple
%        locations at once. This will provide the best possible
%        speed. IF fun is not so vectorized, then you MUST set
%        'vectorized' property to 'no', so that derivest will
%        then call your function sequentially instead.
%
%        Fun is assumed to return a result of the same
%        shape as its input x0.
%
%  x0  - scalar, vector, or array of points at which to
%        differentiate fun.
%
% Additional inputs must be in the form of property/value pairs.
%  Properties are character strings. They may be shortened
%  to the extent that they are unambiguous. Properties are
%  not case sensitive. Valid property names are:
%
%  'DerivativeOrder', 'MethodOrder', 'Style', 'RombergTerms'
%  'FixedStep', 'MaxStep'
%
%  All properties have default values, chosen as intelligently
%  as I could manage. Values that are character strings may
%  also be unambiguously shortened. The legal values for each
%  property are:
%
%  'DerivativeOrder' - specifies the derivative order estimated.
%        Must be a positive integer from the set [1,2,3,4].
%
%        DEFAULT: 1 (first derivative of fun)
%
%  'MethodOrder' - specifies the order of the basic method
%        used for the estimation.
%
%        For 'central' methods, must be a positive integer
%        from the set [2,4].
%
%        For 'forward' or 'backward' difference methods,
%        must be a positive integer from the set [1,2,3,4].
%
%        DEFAULT: 4 (a second order method)
%
%        Note: higher order methods will generally be more
%        accurate, but may also suffere more from numerical
%        problems.
%
%        Note: First order methods would usually not be
%        recommended.
%
%  'Style' - specifies the style of the basic method
%        used for the estimation. 'central', 'forward',
%        or 'backwards' difference methods are used.
%
%        Must be one of 'Central', 'forward', 'backward'.
%
%        DEFAULT: 'Central'
%
%        Note: Central difference methods are usually the
%        most accurate, but sometiems one must not allow
%        evaluation in one direction or the other.
%
%  'RombergTerms' - Allows the user to specify the generalized
%        Romberg extrapolation method used, or turn it off
%        completely.
%
%        Must be a positive integer from the set [0,1,2,3].
%
%        DEFAULT: 2 (Two Romberg terms)
%
%        Note: 0 disables the Romberg step completely.
%
%  'FixedStep' - Allows the specification of a fixed step
%        size, preventing the adaptive logic from working.
%        This will be considerably faster, but not necessarily
%        as accurate as allowing the adaptive logic to run.
%
%        DEFAULT: []
%
%        Note: If specified, 'FixedStep' will define the
%        maximum excursion from x0 that will be used.
%
%  'Vectorized' - Derivest will normally assume that your
%        function can be safely evaluated at multiple locations
%        in a single call. This would minimize the overhead of
%        a loop and additional function call overhead. Some
%        functions are not easily vectorizable, but you may
%        (if your matlab release is new enough) be able to use
%        arrayfun to accomplish the vectorization.
%
%        When all else fails, set the 'vectorized' property
%        to 'no'. This will cause derivest to loop over the
%        successive function calls.
%
%        DEFAULT: 'yes'
%
%
%  'MaxStep' - Specifies the maximum excursion from x0 that
%        will be allowed, as a multiple of x0.
%
%        DEFAULT: 100
%
%  'StepRatio' - Derivest uses a proportionally cascaded
%        series of function evaluations, moving away from your
%        point of evaluation. The StepRatio is the ratio used
%        between sequential steps.
%
%        DEFAULT: 2.0000001
%
%        Note: use of a non-integer stepratio is intentional,
%        to avoid integer multiples of the period of a periodic
%        function under some circumstances.
%
%
% See the document DERIVEST.pdf for more explanation of the
% algorithms behind the parameters of DERIVEST. In most cases,
% I have chosen good values for these parameters, so the user
% should never need to specify anything other than possibly
% the DerivativeOrder. I've also tried to make my code robust
% enough that it will not need much. But complete flexibility
% is in there for your use.
%
%
% Arguments: (output)
%  der - derivative estimate for each element of x0
%        der will have the same shape as x0.
%
%  errest - 95% uncertainty estimate of the derivative, such that
%
%        abs(der(j) - f'(x0(j))) < erest(j)
%
%  finaldelta - The final overall stepsize chosen by DERIVEST
%
%
% Example usage:
%  First derivative of exp(x), at x == 1
%   [d,e]=derivest(@(x) exp(x),1)
%   d =
%       2.71828182845904
%
%   e =
%       1.02015503167879e-14
%
%  True derivative
%   exp(1)
%   ans =
%       2.71828182845905
%
% Example usage:
%  Third derivative of x.^3+x.^4, at x = [0,1]
%   derivest(@(x) x.^3 + x.^4,[0 1],'deriv',3)
%   ans =
%       6       30
%
%  True derivatives: [6,30]
%
%
% See also: gradient
%
%
% Author: John D'Errico
% e-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 12/27/2006

par.DerivativeOrder = 1;
par.MethodOrder = 4;
par.Style = 'central';
par.RombergTerms = 2;
par.FixedStep = [];
par.MaxStep = 100;
% setting a default stepratio as a non-integer prevents
% integer multiples of the initial point from being used.
% In turn that avoids some problems for periodic functions.
par.StepRatio = 2.0000001;
par.NominalStep = [];
par.Vectorized = 'yes';

na = length(varargin);
if (rem(na,2)==1)
  error 'Property/value pairs must come as PAIRS of arguments.'
elseif na>0
  par = parse_pv_pairs(par,varargin);
end
par = check_params(par);

% Was fun a string, or an inline/anonymous function?
if (nargin<1)
  help derivest
  return
elseif isempty(fun)
  error 'fun was not supplied.'
elseif ischar(fun)
  % a character function name
  fun = str2func(fun);
end

% no default for x0
if (nargin<2) || isempty(x0)
  error 'x0 was not supplied'
end
par.NominalStep = max(x0,0.02);

% was a single point supplied?
nx0 = size(x0);
n = prod(nx0);

% Set the steps to use.
if isempty(par.FixedStep)
  % Basic sequence of steps, relative to a stepsize of 1.
  delta = par.MaxStep*par.StepRatio .^(0:-1:-25)';
  ndel = length(delta);
else
  % Fixed, user supplied absolute sequence of steps.
  ndel = 3 + ceil(par.DerivativeOrder/2) + ...
     par.MethodOrder + par.RombergTerms;
  if par.Style(1) == 'c'
    ndel = ndel - 2;
  end
  delta = par.FixedStep*par.StepRatio .^(-(0:(ndel-1)))';
end

% generate finite differencing rule in advance.
% The rule is for a nominal unit step size, and will
% be scaled later to reflect the local step size.
fdarule = 1;
switch par.Style
  case 'central'
    % for central rules, we will reduce the load by an
    % even or odd transformation as appropriate.
    if par.MethodOrder==2
      switch par.DerivativeOrder
        case 1
          % the odd transformation did all the work
          fdarule = 1;
        case 2
          % the even transformation did all the work
          fdarule = 2;
        case 3
          % the odd transformation did most of the work, but
          % we need to kill off the linear term
          fdarule = [0 1]/fdamat(par.StepRatio,1,2);
        case 4
          % the even transformation did most of the work, but
          % we need to kill off the quadratic term
          fdarule = [0 1]/fdamat(par.StepRatio,2,2);
      end
    else
      % a 4th order method. We've already ruled out the 1st
      % order methods since these are central rules.
      switch par.DerivativeOrder
        case 1
          % the odd transformation did most of the work, but
          % we need to kill off the cubic term
          fdarule = [1 0]/fdamat(par.StepRatio,1,2);
        case 2
          % the even transformation did most of the work, but
          % we need to kill off the quartic term
          fdarule = [1 0]/fdamat(par.StepRatio,2,2);
        case 3
          % the odd transformation did much of the work, but
          % we need to kill off the linear & quintic terms
          fdarule = [0 1 0]/fdamat(par.StepRatio,1,3);
        case 4
          % the even transformation did much of the work, but
          % we need to kill off the quadratic and 6th order terms
          fdarule = [0 1 0]/fdamat(par.StepRatio,2,3);
      end
    end
  case {'forward' 'backward'}
    % These two cases are identical, except at the very end,
    % where a sign will be introduced.

    % No odd/even trans, but we already dropped
    % off the constant term
    if par.MethodOrder==1
      if par.DerivativeOrder==1
        % an easy one
        fdarule = 1;
      else
        % 2:4
        v = zeros(1,par.DerivativeOrder);
        v(par.DerivativeOrder) = 1;
        fdarule = v/fdamat(par.StepRatio,0,par.DerivativeOrder);
      end
    else
      % par.MethodOrder methods drop off the lower order terms,
      % plus terms directly above DerivativeOrder
      v = zeros(1,par.DerivativeOrder + par.MethodOrder - 1);
      v(par.DerivativeOrder) = 1;
      fdarule = v/fdamat(par.StepRatio,0,par.DerivativeOrder+par.MethodOrder-1);
    end

    % correct sign for the 'backward' rule
    if par.Style(1) == 'b'
      fdarule = -fdarule;
    end

end % switch on par.style (generating fdarule)
nfda = length(fdarule);

% will we need fun(x0)?
if (rem(par.DerivativeOrder,2) == 0) || ~strncmpi(par.Style,'central',7)
  if strcmpi(par.Vectorized,'yes')
    f_x0 = fun(x0);
  else
    % not vectorized, so loop
    f_x0 = zeros(size(x0));
    for j = 1:numel(x0)
      f_x0(j) = fun(x0(j));
    end
  end
else
  f_x0 = [];
end

% Loop over the elements of x0, reducing it to
% a scalar problem. Sorry, vectorization is not
% complete here, but this IS only a single loop.
der = zeros(nx0);
errest = der;
finaldelta = der;
for i = 1:n
  x0i = x0(i);
  h = par.NominalStep(i);

  % a central, forward or backwards differencing rule?
  % f_del is the set of all the function evaluations we
  % will generate. For a central rule, it will have the
  % even or odd transformation built in.
  if par.Style(1) == 'c'
    % A central rule, so we will need to evaluate
    % symmetrically around x0i.
    if strcmpi(par.Vectorized,'yes')
      f_plusdel = fun(x0i+h*delta);
      f_minusdel = fun(x0i-h*delta);
    else
      % not vectorized, so loop
      f_minusdel = zeros(size(delta));
      f_plusdel = zeros(size(delta));
      for j = 1:numel(delta)
        f_plusdel(j) = fun(x0i+h*delta(j));
        f_minusdel(j) = fun(x0i-h*delta(j));
      end
    end

    if ismember(par.DerivativeOrder,[1 3])
      % odd transformation
      f_del = (f_plusdel - f_minusdel)/2;
    else
      f_del = (f_plusdel + f_minusdel)/2 - f_x0(i);
    end
  elseif par.Style(1) == 'f'
    % forward rule
    % drop off the constant only
    if strcmpi(par.Vectorized,'yes')
      f_del = fun(x0i+h*delta) - f_x0(i);
    else
      % not vectorized, so loop
      f_del = zeros(size(delta));
      for j = 1:numel(delta)
        f_del(j) = fun(x0i+h*delta(j)) - f_x0(i);
      end
    end
  else
    % backward rule
    % drop off the constant only
    if strcmpi(par.Vectorized,'yes')
      f_del = fun(x0i-h*delta) - f_x0(i);
    else
      % not vectorized, so loop
      f_del = zeros(size(delta));
      for j = 1:numel(delta)
        f_del(j) = fun(x0i-h*delta(j)) - f_x0(i);
      end
    end
  end

  % check the size of f_del to ensure it was properly vectorized.
  f_del = f_del(:);
  if length(f_del)~=ndel
    error 'fun did not return the correct size result (fun must be vectorized)'
  end

  % Apply the finite difference rule at each delta, scaling
  % as appropriate for delta and the requested DerivativeOrder.
  % First, decide how many of these estimates we will end up with.
  ne = ndel + 1 - nfda - par.RombergTerms;

  % Form the initial derivative estimates from the chosen
  % finite difference method.
  der_init = vec2mat(f_del,ne,nfda)*fdarule.';

  % scale to reflect the local delta
  der_init = der_init(:)./(h*delta(1:ne)).^par.DerivativeOrder;

  % Each approximation that results is an approximation
  % of order par.DerivativeOrder to the desired derivative.
  % Additional (higher order, even or odd) terms in the
  % Taylor series also remain. Use a generalized (multi-term)
  % Romberg extrapolation to improve these estimates.
  switch par.Style
    case 'central'
      rombexpon = 2*(1:par.RombergTerms) + par.MethodOrder - 2;
    otherwise
      rombexpon = (1:par.RombergTerms) + par.MethodOrder - 1;
  end
  [der_romb,errors] = rombextrap(par.StepRatio,der_init,rombexpon);

  % Choose which result to return

  % first, trim off the
  if isempty(par.FixedStep)
    % trim off the estimates at each end of the scale
    nest = length(der_romb);
    switch par.DerivativeOrder
      case {1 2}
        trim = [1 2 nest-1 nest];
      case 3
        trim = [1:4 nest+(-3:0)];
      case 4
        trim = [1:6 nest+(-5:0)];
    end

    [der_romb,tags] = sort(der_romb);

    der_romb(trim) = [];
    tags(trim) = [];
    errors = errors(tags);
    trimdelta = delta(tags);

    [errest(i),ind] = min(errors);

    finaldelta(i) = h*trimdelta(ind);
    der(i) = der_romb(ind);
  else
    [errest(i),ind] = min(errors);
    finaldelta(i) = h*delta(ind);
    der(i) = der_romb(ind);
  end
end

end % mainline end

% ============================================
% subfunction - romberg extrapolation
% ============================================
function [der_romb,errest] = rombextrap(StepRatio,der_init,rombexpon)
% do romberg extrapolation for each estimate
%
%  StepRatio - Ratio decrease in step
%  der_init - initial derivative estimates
%  rombexpon - higher order terms to cancel using the romberg step
%
%  der_romb - derivative estimates returned
%  errest - error estimates
%  amp - noise amplification factor due to the romberg step

srinv = 1/StepRatio;

% do nothing if no romberg terms
nexpon = length(rombexpon);
rmat = ones(nexpon+2,nexpon+1);
switch nexpon
  case 0
    % rmat is simple: ones(2,1)
  case 1
    % only one romberg term
    rmat(2,2) = srinv^rombexpon;
    rmat(3,2) = srinv^(2*rombexpon);
  case 2
    % two romberg terms
    rmat(2,2:3) = srinv.^rombexpon;
    rmat(3,2:3) = srinv.^(2*rombexpon);
    rmat(4,2:3) = srinv.^(3*rombexpon);
  case 3
    % three romberg terms
    rmat(2,2:4) = srinv.^rombexpon;
    rmat(3,2:4) = srinv.^(2*rombexpon);
    rmat(4,2:4) = srinv.^(3*rombexpon);
    rmat(5,2:4) = srinv.^(4*rombexpon);
end

% qr factorization used for the extrapolation as well
% as the uncertainty estimates
[qromb,rromb] = qr(rmat,0);

% the noise amplification is further amplified by the Romberg step.
% amp = cond(rromb);

% this does the extrapolation to a zero step size.
ne = length(der_init);
rhs = vec2mat(der_init,nexpon+2,max(1,ne - (nexpon+2)));
rombcoefs = rromb\(qromb.'*rhs);
der_romb = rombcoefs(1,:).';

% uncertainty estimate of derivative prediction
s = sqrt(sum((rhs - rmat*rombcoefs).^2,1));
rinv = rromb\eye(nexpon+1);
cov1 = sum(rinv.^2,2); % 1 spare dof
errest = s.'*12.7062047361747*sqrt(cov1(1));

end % rombextrap

% ============================================
% subfunction - vec2mat
% ============================================
function mat = vec2mat(vec,n,m)
% forms the matrix M, such that M(i,j) = vec(i+j-1)
[i,j] = ndgrid(1:n,0:m-1);
ind = i+j;
mat = vec(ind);
if n==1
  mat = mat.';
end

end % vec2mat

% ============================================
% subfunction - fdamat
% ============================================
function mat = fdamat(sr,parity,nterms)
% Compute matrix for fda derivation.
% parity can be
%   0 (one sided, all terms included but zeroth order)
%   1 (only odd terms included)
%   2 (only even terms included)
% nterms - number of terms

% sr is the ratio between successive steps
srinv = 1./sr;

switch parity
  case 0
    % single sided rule
    [i,j] = ndgrid(1:nterms);
    c = 1./factorial(1:nterms);
    mat = c(j).*srinv.^((i-1).*j);
  case 1
    % odd order derivative
    [i,j] = ndgrid(1:nterms);
    c = 1./factorial(1:2:(2*nterms));
    mat = c(j).*srinv.^((i-1).*(2*j-1));
  case 2
    % even order derivative
    [i,j] = ndgrid(1:nterms);
    c = 1./factorial(2:2:(2*nterms));
    mat = c(j).*srinv.^((i-1).*(2*j));
end

end % fdamat

% ============================================
% subfunction - check_params
% ============================================
function par = check_params(par)
% check the parameters for acceptability
%
% Defaults
% par.DerivativeOrder = 1;
% par.MethodOrder = 2;
% par.Style = 'central';
% par.RombergTerms = 2;
% par.FixedStep = [];

% DerivativeOrder == 1 by default
if isempty(par.DerivativeOrder)
  par.DerivativeOrder = 1;
else
  if (length(par.DerivativeOrder)>1) || ~ismember(par.DerivativeOrder,1:4)
    error 'DerivativeOrder must be scalar, one of [1 2 3 4].'
  end
end

% MethodOrder == 2 by default
if isempty(par.MethodOrder)
  par.MethodOrder = 2;
else
  if (length(par.MethodOrder)>1) || ~ismember(par.MethodOrder,[1 2 3 4])
    error 'MethodOrder must be scalar, one of [1 2 3 4].'
  elseif ismember(par.MethodOrder,[1 3]) && (par.Style(1)=='c')
    error 'MethodOrder==1 or 3 is not possible with central difference methods'
  end
end

% style is char
valid = {'central', 'forward', 'backward'};
if isempty(par.Style)
  par.Style = 'central';
elseif ~ischar(par.Style)
  error 'Invalid Style: Must be character'
end
ind = find(strncmpi(par.Style,valid,length(par.Style)));
if (length(ind)==1)
  par.Style = valid{ind};
else
  error(['Invalid Style: ',par.Style])
end

% vectorized is char
valid = {'yes', 'no'};
if isempty(par.Vectorized)
  par.Vectorized = 'yes';
elseif ~ischar(par.Vectorized)
  error 'Invalid Vectorized: Must be character'
end
ind = find(strncmpi(par.Vectorized,valid,length(par.Vectorized)));
if (length(ind)==1)
  par.Vectorized = valid{ind};
else
  error(['Invalid Vectorized: ',par.Vectorized])
end

% RombergTerms == 2 by default
if isempty(par.RombergTerms)
  par.RombergTerms = 2;
else
  if (length(par.RombergTerms)>1) || ~ismember(par.RombergTerms,0:3)
    error 'RombergTerms must be scalar, one of [0 1 2 3].'
  end
end

% FixedStep == [] by default
if (length(par.FixedStep)>1) || (~isempty(par.FixedStep) && (par.FixedStep<=0))
  error 'FixedStep must be empty or a scalar, >0.'
end

% MaxStep == 10 by default
if isempty(par.MaxStep)
  par.MaxStep = 10;
elseif (length(par.MaxStep)>1) || (par.MaxStep<=0)
  error 'MaxStep must be empty or a scalar, >0.'
end

end % check_params

% ============================================
% Included subfunction - parse_pv_pairs
% ============================================
function params=parse_pv_pairs(params,pv_pairs)
% parse_pv_pairs: parses sets of property value pairs, allows defaults
% usage: params=parse_pv_pairs(default_params,pv_pairs)
%
% arguments: (input)
%  default_params - structure, with one field for every potential
%             property/value pair. Each field will contain the default
%             value for that property. If no default is supplied for a
%             given property, then that field must be empty.
%
%  pv_array - cell array of property/value pairs.
%             Case is ignored when comparing properties to the list
%             of field names. Also, any unambiguous shortening of a
%             field/property name is allowed.
%
% arguments: (output)
%  params   - parameter struct that reflects any updated property/value
%             pairs in the pv_array.
%
% Example usage:
% First, set default values for the parameters. Assume we
% have four parameters that we wish to use optionally in
% the function examplefun.
%
%  - 'viscosity', which will have a default value of 1
%  - 'volume', which will default to 1
%  - 'pie' - which will have default value 3.141592653589793
%  - 'description' - a text field, left empty by default
%
% The first argument to examplefun is one which will always be
% supplied.
%
%   function examplefun(dummyarg1,varargin)
%   params.Viscosity = 1;
%   params.Volume = 1;
%   params.Pie = 3.141592653589793
%
%   params.Description = '';
%   params=parse_pv_pairs(params,varargin);
%   params
%
% Use examplefun, overriding the defaults for 'pie', 'viscosity'
% and 'description'. The 'volume' parameter is left at its default.
%
%   examplefun(rand(10),'vis',10,'pie',3,'Description','Hello world')
%
% params =
%     Viscosity: 10
%        Volume: 1
%           Pie: 3
%   Description: 'Hello world'
%
% Note that capitalization was ignored, and the property 'viscosity'
% was truncated as supplied. Also note that the order the pairs were
% supplied was arbitrary.

npv = length(pv_pairs);
n = npv/2;

if n~=floor(n)
  error 'Property/value pairs must come in PAIRS.'
end
if n<=0
  % just return the defaults
  return
end

if ~isstruct(params)
  error 'No structure for defaults was supplied'
end

% there was at least one pv pair. process any supplied
propnames = fieldnames(params);
lpropnames = lower(propnames);
for i=1:n
  p_i = lower(pv_pairs{2*i-1});
  v_i = pv_pairs{2*i};

  ind = strmatch(p_i,lpropnames,'exact');
  if isempty(ind)
    ind = find(strncmp(p_i,lpropnames,length(p_i)));
    if isempty(ind)
      error(['No matching property found for: ',pv_pairs{2*i-1}])
    elseif length(ind)>1
      error(['Ambiguous property name: ',pv_pairs{2*i-1}])
    end
  end
  p_i = propnames{ind};

  % override the corresponding default in params
  params = setfield(params,p_i,v_i); %#ok

end

end % parse_pv_pairs


function cordic_test007 ( )

%*****************************************************************************80
%
%% TEST007 demonstrates the use of EXP_CORDIC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 June 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST007:\n' );
  fprintf ( 1, '  EXP_CORDIC computes the exponential function\n' );
  fprintf ( 1, '  using the CORDIC algorithm.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '      X    N         Exp(X)      Exp(X)          Difference\n' );
  fprintf ( 1, ...
    '                     Tabulated   CORDIC\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, theta, t1 ] = exp_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '\n' );

    for n = 0 : 5 : 25

      t2 = exp_cordic ( theta, n );
      d = t1 - t2;

      fprintf ( 1, '  %12f  %4d  %16.8e  %16.8e  %9e\n', theta, n, t1, t2, d );

    end

  end

  return
end


function [ a, seed ] = r83_random ( n, seed )

%*****************************************************************************80
%
%% R83_RANDOM randomizes a R83 matrix.
%
%  Discussion:
%
%    The R83 storage format is used for a tridiagonal matrix.
%    The superdiagonal is stored in entries (1,2:N), the diagonal in
%    entries (2,1:N), and the subdiagonal in (3,1:N-1).  Thus, the
%    original matrix is "collapsed" vertically into the array.
%
%  Example:
%
%    Here is how a R83 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the linear system.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(3,N), the R83 matrix.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  a(1,1) = 0.0;
  [ a(1,2:n), seed ] = r8vec_uniform_01 ( n-1, seed );

  [ a(2,1:n), seed ] = r8vec_uniform_01 ( n,   seed );

  [ a(3,1:n-1), seed ] = r8vec_uniform_01 ( n-1, seed );
  a(3,n) = 0.0;

  return
end


function [empty,c] = emptyintersect(a,b)
%EMPTYINTERSECT    Compute intersection and check for empty components
%
%   [empty,c] = emptyintersect(a,b)
%
%Result c is that of intersect(a,b), and
%  empty(i) = 1     intersection of a(i) and b(i) is empty
%             0     intersection of a(i) and b(i) is not empty
%             NaN   at least one of a(i) and b(i) is NaN
%
%Input a and b must be both real or both complex
%

% written  04/22/09     S.M. Rump
% written  05/14/09     S.M. Rump  identical midpoints
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if prod(size(a))>1
    if prod(size(b))==1
      if issparse(a)
        b = b*spones(a);
      else
        b = b*ones(size(a));
      end
    end
  else
    if prod(size(b))>1
      if issparse(b)
        a = a*spones(b);
      else
        a = a*ones(size(b));
      end
    end
  end

  if ~isequal(size(a),size(b))
    error('intersect called with non-matching dimensions')
  end

  if ~isa(a,'intval')
    a = intval(a);
  end
  if ~isa(b,'intval')
    b = intval(b);
  end

  if a.complex & b.complex

    c.complex = 1;
    c.inf = [];
    c.sup = [];
    c.mid = b.mid;
    c.rad = b.rad;
    empty = zeros(size(b.mid));

    v = b.mid - intval(a.mid);         % connection of midpoints
    d2 = sqr(intval(real(v))) + sqr(intval(imag(v)));
    d = sqrt(d2);                      % inclusion of distance of midpoints
    arad = intval(a.rad);
    Delta = (arad+b.rad).*(arad-b.rad);
    wng = warning;
    warning off
    sumrad = arad+b.rad;
    index = ( d.inf > sumrad.sup );    % empty intersection
    if any(index(:))
      empty(index) = 1;
      c.mid(index) = NaN;
      c.rad(index) = NaN;
    end
    index0 = ( Delta.inf<=-d2 );
    if any(index0(:))                  % diameter of a in intersection
      c.mid(index0) = a.mid(index0);
      c.rad(index0) = a.rad(index0);
    end
    index1 = ( Delta.sup>=d2 );
    if any(index1(:))                  % diameter of b in intersection
      c.mid(index1) = b.mid(index1);
      c.rad(index1) = b.rad(index1);
    end
    index = ~( index | index0 | index1 );
    if any(index(:))
      x = ( 1 + (arad+b.rad).*(arad-b.rad)./d2 )/2;
      %VVVV  cmid = a.mid(index) + x(index).*v(index);
      %VVVV  c.mid(index) = mid(cmid);
      %VVVV  c.rad(index) = sup( rad(cmid) + sqrt(sqr(arad(index))-sqr(x(index).*d(index))) );
      s.type = '()'; s.subs = {index};
      cmid = a.mid(index) + subsref(x,s).*subsref(v,s);
      c.mid = subsasgn(c.mid,s,mid(cmid));
      c.rad = subsasgn(c.rad,s,sup( rad(cmid) + sqrt(sqr(subsref(arad,s))-sqr(subsref(x,s).*subsref(d,s))) ));
      %AAAA  Matlab bug fix
    end
    warning(wng);

    index = ( imag(c.rad)~=0 );
    if any(index(:))
      empty(index) = 1;
      c.mid(index) = NaN;
      c.rad(index) = NaN;
    end
    index = isnan(a.mid) | isnan(a.rad) | isnan(b.mid) | isnan(b.rad);
    if any(index(:))
      empty(index) = NaN;
      c.mid(index) = NaN;
      c.rad(index) = NaN;
    end

  elseif ~a.complex & ~b.complex    % two real intervals

    c.complex = 0;
    c.inf = max(a.inf,b.inf);
    c.sup = min(a.sup,b.sup);
    empty = zeros(size(a.inf));
    index = ( c.inf>c.sup );        % empty intersection
    if any(index(:))
      empty(index) = 1;
      c.inf(index) = NaN;
      c.sup(index) = NaN;
    end
    index = isnan(a.inf) | isnan(a.sup) | isnan(b.inf) | isnan(b.sup);
    if any(index(:))
      empty(index) = NaN;
      c.inf(index) = NaN;
      c.sup(index) = NaN;
    end
    c.mid = [];
    c.rad = [];

  else
    error('operands of intersect must be both real or both complex')
  end

  c = class(c,'intval');

  setround(rndold)


function xy = bud_radial ( n, xy )

%*****************************************************************************80
%
%% BUD_RADIAL sets the coordinates for a "radial" bud.
%
%  Discussion:
%
%    A radial bud is created by a generator on the boundary.  The initial
%    position of the bud should be created by copying the generator's
%    coordinates, reducing the radial component slightly.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of buds.
%
%    Input, real P_XY(2,N), the coordinates of the bud generators.
%
%    Output, real P_XY(2,N), the coordinates of the buds.
%
  r(1,1:n) = 0.999 * sqrt ( sum ( xy.^2, 1 ) );
  t(1,1:n) = atan2 ( xy(2,:), xy(1,:) );

  xy(1,:) = r(1,1:n) .* cos ( t(1,1:n) );
  xy(2,:) = r(1,1:n) .* sin ( t(1,1:n) );

  return
end


function f = cumsum2(f, dims)
%CUMSUM2   Double indefinite integral of a CHEBFUN3.
%   F = CUMSUM2(F) returns the double indefinite integral of a CHEBFUN3 F.
%   By default, that means cumsum in the first 2 variables, i.e., x and y:
%                   y  x
%                  /  /
%   CUMSUM2(F) =  |  |   F(x,y,z) dx dy
%                 /  /
%                c  a
%
%   where [a,b] x [c,d] x [e,g] is the domain of F.
%
%   DIMS is a vector containing two of the three indices 1,2,3 to show
%   which two of the dimensions are to be used.
%
% See also CHEBFUN3/CUMSUM, CHEBFUN3/CUMSUM3, CHEBFUN3/SUM, CHEBFUN3/SUM2
% and CHEBFUN3/SUM3.

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

% Check for empty:
if ( isempty(f) )
    f = [];
    return
end

% Default to cumsum in the first two variables x and y:
if ( nargin == 1 )
    dims = [1, 2];
end

if ( numel(dims) ~= 2 )
    error('CHEBFUN:CHEBFUN3:cumsum2:dims', 'Dims should have 2 entries.');
end

if ismember(1, dims)
    % cumsum along the 1st variable
    f.cols = cumsum(f.cols);
end
if ismember(2, dims)
    % cumsum along the 2nd variable
    f.rows = cumsum(f.rows);
end
if ismember(3, dims)
    % cumsum along the 3rd variable
    f.tubes = cumsum(f.tubes);
end

end


 % Copyright 2001, Brown University, Providence, Rhode Island.
 %
 % All Rights Reserved
 %
 % Permission to use this software for noncommercial research and
 % educational purposes is hereby granted without fee.
 % Redistribution, sale, or incorporation of this software into a
 % commercial product is prohibited.
 %
 % BROWN UNIVERSITY DISCLAIMS ANY AND ALL WARRANTIES WITH REGARD TO
 % THIS SOFTWARE,INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
 % AND FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL BROWN
 % UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
 % DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE,
 % DATA OR PROFITS.

function [dmodedr, dmodeds] = am282jacobideriv2dab(a,b,ID,JD)

  dims = size(a);

  j2deriv = zeros(dims);

  fa  = am282jacobi1d(a,ID,0,0);
  dfa = am282jacobideriv1d(a,ID,0,0);

  gb  = am282jacobi1d(b,JD,2*ID+1,0);
  dgb = am282jacobideriv1d(b,JD,2*ID+1,0);

  % r-derivative
  % d/dr = da/dr d/da + db/dr d/db = (2/(1-s)) d/da = (2/(1-b)) d/da
  dmodedr = dfa.*gb;
  if(ID>0)
   dmodedr = dmodedr.*((0.5*(1-b)).^(ID-1));
  end

  % s-derivative
  % d/ds = da/ds d/da + db/ds d/db = (2(r+1)/(1-s)^2) d/da + d/db
  %                                = ((1+a)/2)/((1-b)/2) d/da + d/db
  dmodeds = dfa.*(gb.*(0.5*(1+a)));
  if(ID>0)
   dmodeds = dmodeds.*((0.5*(1-b)).^(ID-1));
  end

  tmp = dgb.*((0.5*(1-b)).^ID);
  if(ID>0)
   tmp = tmp-0.5*ID*gb.*((0.5*(1-b)).^(ID-1));
  end

  dmodeds = dmodeds+fa.*tmp;

   % normalise
  dmodedr = dmodedr*sqrt((ID+0.5)*(ID+JD+1));
  dmodeds = dmodeds*sqrt((ID+0.5)*(ID+JD+1));


function dn = Dmult(dn0,dn1)

% DMULT dual number multiplication
%
%   DN = DMULT(DN0,DN1) returns the dual number DN which is the dual number
%     multiplication of the dual numbers DN0 and DN1
%      - DN0 (resp. DN1) is a dual number (DN0 = a0 + eps*b0, eps^2 = 0).
%         It is a 2-vector or a 2*N array (column i represents dual number
%         i) where N is the number of dual numbers. DN0 and DN1 must have
%         the same size.
%      - DN is a dual number. It is a 2*N array (each column is the dual
%          multiplication of the corresponding columns in DN0 and DN1)
%
% See also DQINV

s0 = size(dn0);
s1 = size(dn1);
if s0 == [1 2], dn0 = dn0';s0 = size(dn0);end
if s1 == [1 2], dn1 = dn1';s1 = size(dn1);end

% wrong format
if s0(1) ~= 2 || s1(1) ~= 2
    error('DualQuaternion:Dmult:wrongsize',...
        '%d rows in the DN0 array and %d rows in the DN1 array. It should be 2 for both.',...
        s0(1),s1(1));
end

% sizes do not match
n1 = s0(2);
n2 = s1(2);
if n1 ~= n2
    error('DualQuaternion:Dmult:wrongsize',...
        '%d dual numbers in DN0 array and %d dual numbers in DN1 array.They should be equal.',...
        n1,n2);
end

dn = zeros(2,n1);
dn(1,:) = dn0(1,:).*dn1(1,:);
dn(2,:) = dn0(1,:).*dn1(2,:)+dn0(2,:).*dn1(1,:);


function [ x, seed ] = uniform_01_order_sample ( n, seed )

%*****************************************************************************80
%
%% UNIFORM_01_ORDER_SAMPLE samples the Uniform 01 Order PDF.
%
%  Discussion:
%
%    In effect, this routine simply generates N samples of the
%    Uniform 01 PDF; but it generates them in order.  (Actually,
%    it generates them in descending order, but stores them in
%    the array in ascending order).  This saves the work of
%    sorting the results.  Moreover, if the order statistics
%    for another PDF are desired, and the inverse CDF is available,
%    then the desired values may be generated, presorted, by
%    calling this routine and using the results as input to the
%    inverse CDF routine.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jerry Banks, editor,
%    Handbook of Simulation,
%    Engineering and Management Press Books, 1998, page 168.
%
%  Parameters:
%
%    Input, integer N, the number of elements in the sample.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X(N), N samples of the Uniform 01 PDF, in
%    ascending order.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  v = 1.0;
  for i = n : -1 : 1
    [ u, seed ] = r8_uniform_01 ( seed );
    v = v * u^( 1.0 / i );
    x(i) = v;
  end

  return
end


function determ = fibonacci3_determinant ( n )

%*****************************************************************************80
%
%% FIBONACCI3_DETERMINANT returns the determinant of the FIBONACCI3 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM, the determinant.
%
  f1 = 0;
  f2 = 0;
  f3 = 1;

  for i = 1 : n
    f1 = f2;
    f2 = f3;
    f3 = f1 + f2;
  end

  determ = f3;

  return
end


function [vertices, faces] = steinerPolytope(vectors)
%STEINERPOLYTOPE Create a steiner polytope from a set of vectors
%
%   [VERTICES FACES] = steinerPolygon(VECTORS)
%   Creates the Steiner polytope defined by the set of vectors VECTORS.
%
%   Example
%     % Creates and display a planar Steiner polytope (ie, a polygon)
%     [v f] = steinerPolytope([1 0;0 1;1 1]);
%     fillPolygon(v);
%
%     % Creates and display a 3D Steiner polytope
%     [v f] = steinerPolytope([1 0 0;0 1 0;0 0 1;1 1 1]);
%     drawMesh(v, f);
%     view(3); axis vis3d
%
%   See also
%   meshes3d, drawMesh, steinerPolygon, mergeCoplanarFaces
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2006-04-28
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

% History
% 2013-02-22 merge coplanar faces, add management of 2D case, update doc

% compute vectors dimension
nd = size(vectors, 2);

% create candidate vertices
vertices = zeros(1, size(vectors, 2));
for i = 1:length(vectors)
    nv = size(vertices, 1);
    vertices = [vertices; vertices+repmat(vectors(i,:), [nv 1])]; %#ok<AGROW>
end

if nd == 2
    % for planar case, use specific function convhull
    K = convhull(vertices(:,1), vertices(:,2));
    vertices = vertices(K, :);
    faces = 1:length(K);

else
    % Process the general case (tested only for nd==3)

    % compute convex hull
    K = convhulln(vertices);

    % keep only relevant points, and update faces indices
    ind = unique(K);
    for i = 1:length(ind)
        K(K==ind(i)) = i;
    end

    % return results
    vertices = vertices(ind, :);
    faces = K;

    % in case of 3D meshes, merge coplanar faces
    if nd == 3
        faces = mergeCoplanarFaces(vertices, faces);
    end
end


function s=dev(t);
%
%s=dev(t)
%
%This function calculates the deviatoric part s(ij) of a tensor t(ij)
%But, for the purpose of computations we use the vector representation
%of a tensor t(ij), so that:
%t - is a vector representation of a tensor t(ij)
%    with the components in the following order:
%
%t=[txx tyy tzz txy txz tzx tyx tyz txz]
%or
%t=[txx tyy tzz txy txz tyz]
%or
%t=[txx tyy tzz]
%
%Dimension of t can be [m n]
%where: m - represents the number of tensors to calculate
%       n = 9, 6 or 3 (see above)
%
%   Copyright (c) 2001 by Aleksander Karolczuk.
%   $Revision: 1.0 $  $Date: 2001/02/01

error(nargchk(1,1,nargin))

[m n]=size(t);

if n==3
   I=[1 1 1];
elseif n==6
   I=[1 1 1 0 0 0];
elseif n==9
   I=[1 1 1 0 0 0 0 0 0];
else
   error('Improper matrix dimension')
end

s=t-((1/3)*(t*I'))*I;


% Description:
%
%     Estimate the dimensionality of Y using the Bayesian information criterion
%     (BIC) and perform an economy SVD of Y such that Y = U * S * V'.
%
% Syntax:
%
%     [ BIC, U, S, V, stats ] = mSVD(Y)
%
% Inputs:
%
%     Y       - [ N x M ] (double)
%     options - [ 1 x P ] (cell)
%
% Outputs:
%
%     BIC - [ 1 x B ] (double)
%     U   - [ N x N ] (double)
%     S   - [ N x N ] (double)
%     V   - [ M x N ] (double)
%
% Details:
%
% Examples:
%
% Notes:
%
% Author(s):
%
%     William Gruner (williamgruner@gmail.com)
%
% References:
%
% Acknowledgements:
%
%     Many thanks to Dr. Erik Erhardt and Dr. Elena Allen of the Mind Research
%     Network (www.mrn.org) for their continued collaboration.
%
% Version:
%
%     $Author: williamgruner $
%     $Date: 2010-04-01 12:52:08 -0600 (Thu, 01 Apr 2010) $
%     $Revision: 483 $

function [ BIC, U, S, V ] = mSVD(Y, options)

    if ~exist('options', 'var')
        options = cell(0);
    end

    if ~isempty(strmatch('verbose', options, 'exact'))
        fprintf('\n')
        fprintf('Using an economy SVD to reduce the dimensionality of Y ...\n\n')
    end

    [ U, S, V ] = svd(Y, 'econ');

    b = [];
    n = size(U, 2);
    r = Inf;

    for i = 1 : n

        e = Y - U(:, 1 : i) * S(1 : i, 1 : i) * V(:, 1 : i)';

        b(i) = log(std(e(:)).^2) + (i / n) * log(n);

        if ~isempty(strmatch('verbose', options))
            fprintf('\tBIC(%d) = %g\n', i, b(i))
        end

        if i > 1 && b(i) > b(i - 1) && r == Inf
            r = i - 1;
        end

        if i > 2 * r
            break
        end

    end

    if ~isempty(strmatch('verbose', options))
        fprintf('\n... to the Bayesian information criterion estimate of %d.\n', r)
    end

    BIC = b;


function beta = volumeCompressibility(S)
% computes the volume compressibility of an elasticity tensor
%
% Syntax
%   beta = volumeCompressibility(S)
%
% Input
%  C - elastic stiffness @tensor
%
% Output
%  beta - volume compressibility
%
% Description
%
% $$\beta(x) = S_{iikk}$$
%

% compute tensor product
beta = EinsteinSum(S,[-1 -1 -2 -2]);

% beta = 1/S.bulkModulus(S)


function cn_leg_test ( n, expon )

%*****************************************************************************80
%
%% CN_LEG_TEST tests the rules for CN with Legendre weight on a monomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 March 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  N = %d\n', n );
  fprintf ( 1, '  EXPON = ' );
  for d = 1 : n
    fprintf ( 1, '  %2d', expon(d) );
  end
  fprintf ( 1, '\n' );
  d = sum ( expon(1:n) );
  fprintf ( 1, '  Degree = %d\n', d );
  fprintf ( 1, '\n' );

  exact = cn_leg_monomial_integral ( n, expon );

  p = 1;

  if ( d <= p )
    [ o, x, w ] = cn_leg_01_1 ( n );
    v = monomial_value ( n, o, x, expon );
    quad = w' * v';
    err = abs ( quad - exact );
    fprintf ( 1, '  CN_LEG_01_1:    %6d  %14.6g  %14.6e\n', o, quad, err );
  end

  p = 2;

  if ( d <= p )

    [ o, x, w ] = cn_leg_02_xiu ( n );
    v = monomial_value ( n, o, x, expon );
    quad = w' * v';
    err = abs ( quad - exact );
    fprintf ( 1, '  CN_LEG_02_XIU:  %6d  %14.6g  %14.6e\n', o, quad, err );

    gamma0 = 1.0;
    delta0 = 0.0;
    c1 = 1.0 / 3.0;
    volume_1d = 2.0;
    [ o, x, w ] = gw_02_xiu ( n, gamma0, delta0, c1, volume_1d );
    v = monomial_value ( n, o, x, expon );
    quad = w' * v';
    err = abs ( quad - exact );
    fprintf ( 1, '  GW_02_XIU:      %6d  %14.6g  %14.6e\n', o, quad, err );

  end

  p = 3;

  if ( d <= p )

    [ o, x, w ] = cn_leg_03_1 ( n );
    v = monomial_value ( n, o, x, expon );
    quad = w' * ( monomial_value ( n, o, x, expon ) )';
    err = abs ( quad - exact );
    fprintf ( 1, '  CN_LEG_03_1:    %6d  %14.6g  %14.6e\n', o, quad, err );

    [ o, x, w ] = cn_leg_03_xiu ( n );
    v = monomial_value ( n, o, x, expon );
    quad = w' * ( monomial_value ( n, o, x, expon ) )';
    err = abs ( quad - exact );
    fprintf ( 1, '  CN_LEG_03_XIU:  %6d  %14.6g  %14.6e\n', o, quad, err );

  end

  p = 5;

  if ( d <= p )

    if ( 4 <= n && n <= 6 )
      option = 1;
      [ o, x, w ] = cn_leg_05_1 ( n, option );
      v = monomial_value ( n, o, x, expon );
      quad = w' * ( monomial_value ( n, o, x, expon ) )';
      err = abs ( quad - exact );
      fprintf ( 1, '  CN_LEG_05_1(1): %6d  %14.6g  %14.6e\n', o, quad, err );
    end

    if ( 4 <= n && n <= 5 )
      option = 2;
      [ o, x, w ] = cn_leg_05_1 ( n, option );
      v = monomial_value ( n, o, x, expon );
      quad = w' * ( monomial_value ( n, o, x, expon ) )';
      err = abs ( quad - exact );
      fprintf ( 1, '  CN_LEG_05_1(2): %6d  %14.6g  %14.6e\n', o, quad, err );
    end

    if ( 2 <= n )
      [ o, x, w ] = cn_leg_05_2 ( n );
      v = monomial_value ( n, o, x, expon );
      quad = w' * ( monomial_value ( n, o, x, expon ) )';
      err = abs ( quad - exact );
      fprintf ( 1, '  CN_LEG_05_2:    %6d  %14.6g  %14.6e\n', o, quad, err );
    end

  end

  fprintf ( 1, '  EXACT:                  %14.6g\n', exact );

  return
end


function triangle_lyness_rule_test02 ( )

%*****************************************************************************80
%
%% TRIANGLE_LYNESS_RULE_TEST02 performs the weight sum test on Lyness rules.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGLE_LYNESS_RULE_TEST02\n' );
  fprintf ( 1, '  LYNESS_RULE returns the points and weights\n' );
  fprintf ( 1, '  of a Lyness rule for the triangle.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this test, we simply check that the weights\n' );
  fprintf ( 1, '  sum to 1.\n' );

  rule_num = lyness_rule_num ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of available rules = %d\n', rule_num );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Rule    Sum of weights\n' );
  fprintf ( 1, '\n' );

  for rule = 0 : rule_num

    order = lyness_order ( rule );

    [ w, x ] = lyness_rule ( rule, order );

    w_sum = sum ( w(1:order) );

    fprintf ( 1, '  %8d  %25.16f\n', rule, w_sum );

  end

  return
end


function activations = virtual_electrode_activation(phis, thetas, sigmas, chanlocs, scalpmap, sph_radius)
% virtual_electrode_activation estimates the activation of electrodes in
% arbirtrary locations given a scalp map for an IC and the locations of the
% electrodes in that scalp map.
%
% Input:
% phis: Longitudes of points for which a virtual activation is desired.
%
% thetas: Latitudes of points for which a virtual activation is desired.
%
% sigmas: Standard devations of Gaussian kernels used to weight the
% original electrode activations in the calculation of the virtual
% electrode activations.
%
% chanlocs: Locations of original electrodes. Must be a struct containing
% the fields sph_phi and sph_theta. These fields should contain the
% longitudes (sph_phi) and latitudes (sph_theta) of the original
% electrodes.
%
% scalpmap: Scalp map (topography, i.e. a column of the field icawinv in
% the EEGLab data structure) for a single IC.
%
% sph_radius: Radius of the head.
%
% Output:
% activations: Topography of IC on the virtual electrode cap, i.e.
% activations of the specified virtual electrodes on this IC's scalp map.

% Copyright (C) 2013  Laura Froelich (laura.frolich@gmail.com)
%
% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

all_electrodes_phi = cell2mat({chanlocs.sph_phi});
all_electrodes_theta = cell2mat({chanlocs.sph_theta});
nelectrodes = length(all_electrodes_theta);
nvirtual_electrodes = length(phis);

activations = NaN(nvirtual_electrodes, 1);
for ivirtual_electrode = 1:nvirtual_electrodes
    distances = spherical_distance(repmat(phis(ivirtual_electrode), 1, nelectrodes),...
        repmat(thetas(ivirtual_electrode), 1, nelectrodes),...
        all_electrodes_phi, all_electrodes_theta, sph_radius);
    distances = distances.^2;
    exp_distances = exp(-distances/(2*sigmas(ivirtual_electrode)^2));
    exp_distances=exp_distances/sum(exp_distances);
    activations(ivirtual_electrode) = sum(scalpmap.*exp_distances);

end
end


function v = compute_rbf(x,d,xi, name)

switch name
    case 'abs'
        f = @(x)abs(x);
    case 'gauss'
        sigma = .03;
        f = @(x)exp(-x.^2/(2*sigma^2));
    case 'poly3'
        q = 3;
        f = @(x)abs(x).^q;
    case 'sqrt'
        q = .5;
        f = @(x)abs(x).^q;
    case 'thinplate'
        f = @(x)(x.^2).*log(abs(x)+.001);
end

n = length(xi);
m = length(x);

[Y,X] = meshgrid(x,x);

D = f(X-Y);

% solve for weights
a = pinv(D)*d;

v = f( repmat(xi, [1 m]) - repmat(x', [n 1]) ) .* repmat( a', [n 1] );
v = sum(v, 2);


function [pbest,perror,nchi2]=cnonlin(mfunc,x,y,sy,pt,v);
% function [pbest,perror,nchi2]=cnonlin(mfunc,x,y,sy,pt,v);
%
% Levenberg-Marquardt non-linear regression for complex functions.
% mfunc=name of the function file that calculates the function to be
%    fitted to the set of data. mfunc is a string variable, so
%    the name of the file must be put inside quotes, eg. 'model'.
%    The function file written to calculate your function should
%    be of the form:
%
%    function yfit=model(x,p)
%    yfit=p(1)+p(2)*x;
%
%    The example above implements y=a+bx.
%    The vector x contains the values of the independent variable.
%    The vector p contains whatever parameters you are using.
%
% x=vector of the independent variable (e.g. Time).
% y=vector of the dependent variable (e.g. Counts recorded).
% sy=vector of standard error in y values.
% pt=initial estimate of parameters to be fitted. All parameters
%   must be non-zero, and are assumed to be real.
% v=vector indicating which parameters are to be varied (fitted), and
%   which are to be held fixed. In most cases we will want all parameters
%   to be varied, but in some circumstances it is useful to hold certain
%   parameters fixed while the others are varied. v is a vector of the same
%   length as p, and should contain only ones and zeros; a one indicates that
%   the corresponding parameter should be varied (fitted), and a zero
%   indicates that the corresponding parameter should be held constant.
%   eg. [1,0,1,1] would be used to keep the second parameter fixed while
%   varying the rest (in the case of four parameters).
%
% OUTPUT VARIABLES
% pbest=fitted parameters
% perror=standard error in fitted parameters
% nchi2=normalised chi-squared parameter (=chi-Squared/degrees of freedom). This
%    is expected to be close to 1 for a good fit.

% Written 4/12/95 by Michael Fleming, University Of Auckland

chiCut=0.01;  lambda=0.001;  stepSize=0.001;  chiOld=Inf;
y=y(:);  x=x(:);  sy=sy(:);  pt=pt(:);  v=v(:);  maxIter=50;
m=length(pt); n=length(x);

evalstr=[mfunc '(x,['];      % Create string containing function call
count=0;
for i=1:m
 if v(i)==1         % Extract vector p of parameters which will be varied
  count=count+1;    % from vector pt which contains all input parameters
  p(count)=pt(i);
  evalstr=[evalstr 'p(' int2str(count) ') '];
 else
  evalstr=[evalstr num2str(pt(i)) ' '];
 end;
 if (pt(i)==0)   % Check all parameters are non-zero
  error('You have entered a zero parameter.All parameters must be nonzero');
 end;
end;
evalstr=[evalstr '])'];

dof=n-count;    p=p(:);   delta=p*stepSize;
% delta is the size of the small increment used for calculating numerical derivatives
% dof is the number of degrees of freedom

chiSqr=sum((abs(y-eval(evalstr))./sy).^2);     % Calc ChiSqr from initial parameters
if chiSqr/dof>5000;
 disp('You have made a bad choice of initial parameters');
end;

iter=0;
while (abs(chiOld-chiSqr)>chiCut)&(iter<maxIter);
 iter=iter+1;
 chiOld=chiSqr;
 [alpha,beta]=cnonlin2(evalstr,x,y,sy,p,delta,lambda);
 if det(alpha)==0;
  error('No convergence - try a different set of parameters');
 end;
 dp=alpha\beta;    % Evaluate parameter increments
 p=p+dp;
 chiSqr=sum((abs(y-eval(evalstr))./sy).^2);
 while (chiSqr>(chiOld+chiCut));
  p=p-dp;
  iter=iter+1;  lambda=lambda*10;
  [alpha,beta]=cnonlin2(evalstr,x,y,sy,p,delta,lambda);
  if det(alpha)==0;
   error('No convergence - try a different set of parameters');
  end;
  dp=alpha\beta;    % Evaluate parameter increments
  p=p+dp;
  chiSqr=sum((abs(y-eval(evalstr))./sy).^2);
 end;
 lambda=0.1*lambda;
end;

if iter==maxIter,
 disp('Maximum number of iterations exceeded - convergence not achieved');
end;

%xfine=linspace(x(1),x(n),201);    % Print graph of function
%yfitted=model(xfine,p);
%errorbar(x,y,sy);
% Calculate correlation coefficicent R^2
%f=eval(evalstr);
%r=corrcoef(y,f);
%r2=r(1,2).^2;

% Calculate standard errors in parameters
[alpha,beta]=cnonlin2(evalstr,x,y,sy,p,delta,0);	% Calculate final alpha matrix
sp=sqrt(diag(inv(alpha)));   % Evaluate standard errors in parameters.
disp('Parameters');
count=0;
for i=1:m
 if v(i)==1
  count=count+1;
  pbest(i)=p(count); perror(i)=sp(count);
  disp(['p(', int2str(i), ') = ' sprintf('%5.3e',p(count)), '+-' sprintf('%5.3e',sp(count))]);
 else
  disp(['p(', int2str(i), ') = ' sprintf('%5.3e',pt(i)), '+-0']);
  pbest(i)=pt(i); perror(i)=0;
 end;
end;

format compact
%disp('Correlation Coefficient R^2');
%disp(r2);
disp('Normalised chi-squared value');
nchi2=chiSqr/dof;
disp(nchi2);
format;


%%%%%script to generate figure 7 from the paper 'Invariant Scattering Convolution Networks'
%%%%%feb 2012%%%%%%%
%%%%%%Joan Bruna and Stephane Mallat$$$$$$$$$

%%%%%This figure displays the scattering coefficients of
%%%%%% a digit taken from the MNIST dataset.

Nim=32;
copts.renorm_process=0;
copts.l2_renorm=1;
foptions.J=3;
foptions.L=8;
soptions.M=2;
soptions.oversampling = 0;
[Wop,filters]=wavelet_factory_2d([Nim Nim], foptions, soptions);

dirac=zeros(Nim);
dirac(1)=1;
dirac=fftshift(dirac);
[scdirac]=scat(dirac,Wop);

tmp=load('mnist_sample.mat');
im=tmp.im;

%compute scattering coefficients
[sc_digit]=scat(im,Wop);

sc=format_scat(sc_digit);
S=size(sc);

%construct the grid with the scattering displays

drawing1=[];
drawing2=[];
for s1=1:S(2)
	row1=[];
	row2=[];
	for s2=1:S(3)
		tempo=scat_display(sc_digit,scdirac,copts,sc(:,s1,s2));
		row1=[row1 tempo{1}];
		row2=[row2 tempo{2}];
	end
	drawing1=[drawing1 ; row1];
	drawing2=[drawing2 ; row2];
end

[N,M]=size(drawing1);

digit_bg=imresize(im,[N M]);
digit_bg=min(255,max(0,digit_bg));

atten=0.0;

fact1=atten* max(drawing1(:)) / max(digit_bg(:));
fact2=atten* max(drawing2(:)) / max(digit_bg(:));

gg=colormap(gray);
invgg=1-gg;
figure
imagesc(drawing1)
set(gca,'XTick',[]);
set(gca,'YTick',[]);
axis square

figure
imagesc(drawing2)
set(gca,'XTick',[]);
set(gca,'YTick',[]);
axis square

figure
imagesc(digit_bg);
colormap(invgg);
set(gca,'XTick',[]);
set(gca,'YTick',[]);
axis square


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

%Empty workspace and close figures
close all;
clear;

%% Define parameters
S = [1, 16, 256]; %Number of subarrays
M = 256./S; %Number of antennas per subarray
d0 = 10; %Guard distance to the closest subarray
W = 350; %Cell radii
numberOfSetups = 5000; %Number of random subarray drops

%Propagation model parameters as in (2.3)
alpha = 3.76;
Upsilon = -148.1;
sigma_sf = 8;
pathlossdB = @(d) Upsilon-10*alpha*log10(d/1000);
addShadowing = @(pldB) 10.^((pldB +  sigma_sf*randn(size(pldB)))/10);

%% Monte Carlo simulations
betaValues = zeros(numel(S), numberOfSetups);

%Go through the number of subarrays
for s = 1:numel(S)

    %Go through all drops of subarrays
    for it1 = 1:numberOfSetups

        d = sort(sqrt(rand(S(s),1)*(W^2-d0^2) + d0^2)); %The squared distances of uniformly distributed points between two circles are uniformly distributed
        pldB = pathlossdB(d); %Compute pathloss without shadow fading
        pl = addShadowing(pldB); %Add random shadow fading

        %Compute average channel gain
        betaValues(s,it1) = sum(pl)/S(s);

    end

end

%% Plot the simulation results
figure;
hold on; box on;
plot(sort(10*log10(betaValues(1,:))),linspace(0,1,numberOfSetups), 'LineStyle', '-', 'Color', 'k', 'LineWidth', 1);
plot(sort(10*log10(betaValues(2,:))),linspace(0,1,numberOfSetups), 'LineStyle', '--', 'Color', 'r',  'LineWidth', 1);
plot(sort(10*log10(betaValues(3,:))),linspace(0,1,numberOfSetups), 'LineStyle', '-.', 'Color', 'b',  'LineWidth', 1);

xlabel('Average channel gain [dB]')
ylabel('CDF')
legend({['S=', num2str(S(1))],['S=', num2str(S(2))],['S=', num2str(S(3))]}, 'Location', 'NorthWest')
xlim([-160 -60]);


function [ x, r, z, p, q, job ] = cg_rc ( n, b, x, r, z, p, q, job )

%*****************************************************************************80
%
%% CG_RC is a reverse communication conjugate gradient routine.
%
%  Discussion:
%
%    This routine seeks a solution of the linear system A*x=b
%    where b is a given right hand side vector, A is an n by n
%    symmetric positive definite matrix, and x is an unknown vector
%    to be determined.
%
%    Under the assumptions that the matrix A is large and sparse,
%    the conjugate gradient method may provide a solution when
%    a direct approach would be impractical because of excessive
%    requirements of storage or even of time.
%
%    The conjugate gradient method presented here does not require the
%    user to store the matrix A in a particular way.  Instead, it only
%    supposes that the user has a way of calculating
%      y = alpha * A * x + b * y
%    and of solving the preconditioned linear system
%      M * x = b
%    where M is some preconditioning matrix, which might be merely
%    the identity matrix, or a diagonal matrix containing the
%    diagonal entries of A.
%
%    This routine was extracted from the "templates" package.
%    There, it was not intended for direct access by a user;
%    instead, a higher routine called "cg()" was called once by
%    the user.  The cg() routine then made repeated calls to
%    cgrevcom() before returning the result to the user.
%
%    The reverse communication feature of cgrevcom() makes it, by itself,
%    a very powerful function.  It allows the user to handle issues of
%    storage and implementation that would otherwise have to be
%    mediated in a fixed way by the function argument list.  Therefore,
%    this version of cgrecom() has been extracted from the templates
%    library and documented as a stand-alone procedure.
%
%    The user sets the value of JOB to 1 before the first call,
%    indicating the beginning of the computation, and to the value of
%    2 thereafter, indicating a continuation call.
%    The output value of JOB is set by cgrevcom(), which
%    will return with an output value of JOB that requests a particular
%    new action from the user.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Richard Barrett, Michael Berry, Tony Chan, James Demmel,
%    June Donato, Jack Dongarra, Victor Eijkhout, Roidan Pozo,
%    Charles Romine, Henk van der Vorst,
%    Templates for the Solution of Linear Systems:
%    Building Blocks for Iterative Methods,
%    SIAM, 1994,
%    ISBN: 0898714710,
%    LC: QA297.8.T45.
%
%  Parameters:
%
%    Input, integer N, the dimension of the matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input, real X(N).  On first call, the user
%    should store an initial guess for the solution in X.
%
%    Input, real R(N), Z(N), P(N), Q(N), work arrays.  The user should
%    create each of these before the first call, using the zeros() command.
%    On subsequent calls, the user may be asked to assign a value to one
%    of these vectors.
%
%    Input, integer JOB, communicates the task to be done.
%    The user needs to set the input value of JOB to 1, before the first call,
%    and then to 2 for every subsequent call for the given problem.
%
%    Output, real X(N), the current solution estimate.
%    Each time JOB is returned as 4, X has been updated.
%
%    Output, real R(N), Z(N), P(N), Q(N), work arrays.  Depending on the
%    output value of JOB, the user may be asked to carry out a computation
%    involving some of these vectors.
%
%    Output, integer JOB, communicates the task to be done.
%    * JOB = 1, compute Q = A * P;
%    * JOB = 2: solve M*Z=R, where M is the preconditioning matrix;
%    * JOB = 3: compute R = R - A * X;
%    * JOB = 4: check the residual R for convergence.
%               If satisfactory, terminate the iteration.
%               If too many iterations were taken, terminate the iteration.
%

%
%  Local variables should be SAVED between calls.
%
  persistent iter
  persistent rho
  persistent rho_old
  persistent rlbl
%
%  Initialization.
%  Ask the user to compute the initial residual.
%
  if ( job == 1 )

    r(1:n) = b(1:n);

    job = 3;
    rlbl = 2;
%
%  Begin first conjugate gradient loop.
%  Ask the user for a preconditioner solve.
%
  elseif ( rlbl == 2 )

    iter = 1;

    job = 2;
    rlbl = 3;
%
%  Compute the direction.
%  Ask the user to compute ALPHA.
%  Save A*P to Q.
%
  elseif ( rlbl == 3 )

    rho = r(:)' * z(:);

    if ( 1 < iter )
      beta = rho / rho_old;
      z(1:n) = z(1:n) + beta * p(1:n);
    end

    p(1:n) = z(1:n);

    job = 1;
    rlbl = 4;
%
%  Compute current solution vector.
%  Ask the user to check the stopping criterion.
%
  elseif ( rlbl == 4 )

    pdotq = p(:)' * q(:);
    alpha = rho / pdotq;
    x(1:n) = x(1:n) + alpha * p(1:n);
    r(1:n) = r(1:n) - alpha * q(1:n);

    job = 4;
    rlbl = 5;
%
%  Begin the next step.
%  Ask for a preconditioner solve.
%
  elseif ( rlbl == 5 )

    rho_old = rho;
    iter = iter + 1;

    job = 2;
    rlbl = 3;

  end

  return
end