function md_fast ( nd, np, step_num, dt )

%*****************************************************************************80
%
%% MD_FAST is the main program for the molecular dynamics simulation.
%
%  Discussion:
%
%    MD_FAST is a modification of the MD code.
%
%    The compute() function used in the original version of MD has been
%    rewritten to take advantage of some of MATLAB's vector capabilities.
%    It is hoped that MD_FAST will execute faster than MD does.
%
%    Like MD, MD_FAST implements a simple molecular dynamics simulation.
%
%    The velocity Verlet time integration scheme is used.
%
%    The particles interact with a central pair potential.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2014
%
%  Author:
%
%    Original FORTRAN90 version by Bill Magro.
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, integer ND, the spatial dimension.
%    A value of 2 or 3 is usual.
%    The default value is 3.
%
%    Input, integer NP, the number of particles.
%    A value of 1000 or 2000 is small but "reasonable".
%    The default value is 1000.
%
%    Input, integer STEP_NUM, the number of time steps.
%    A value of 500 is a small but reasonable value.
%    The default value is 100.
%
%    Input, real DT, the time step.
%    A value of 0.1 is large; the system will begin to move quickly but the
%    results will be less accurate.
%    A value of 0.0001 is small, but the results will be more accurate.
%    The default value is 0.1.
%
  if ( nargin < 1 )
    nd = 3;
  end

  if ( nargin < 2 )
    np = 1000;
  end

  if ( nargin < 3 )
    step_num = 100;
  end

  if ( nargin < 4 )
    dt = 0.1;
  end

  mass = 1.0;
%
%  Set SHOW_PLOTS to 1 if you want to see a plot each time.
%
  show_plots = 0;
%
%  Report to the user.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MD_FAST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  A molecular dynamics program.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ND, the spatial dimension, is %d\n', nd );
  fprintf ( 1, '  NP, the number of particles in the simulation is %d.\n', np );
  fprintf ( 1, '  STEP_NUM, the number of time steps, is %d.\n', step_num );
  fprintf ( 1, '  DT, the time step size, is %f seconds.\n', dt );
%
%  Set the dimensions of the box.
%
  box(1:nd) = 10.0;
%
%  Initialize the random number generator to a prescribed state.
%  RNG is the new recommended interface to the MATLAB random number generator.
%
  seed = 123456789;
  rng ( seed );
%
%  Set initial positions, velocities, and accelerations.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initializing positions, velocities, and accelerations.\n' );

  [ pos, vel, acc ] = initialize ( np, nd, box, seed );
%
%  Compute the forces and energies.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Computing initial forces and energies.\n' );

  [ force, potential, kinetic ] = compute ( np, nd, pos, vel, mass );
%
%  Save the initial total energy for use in the accuracy check.
%
  e0 = potential + kinetic;
%
%  This is the main time stepping loop:
%    Compute forces and energies,
%    Update positions, velocities, accelerations.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  At each step, we report the potential and kinetic energies.\n' );
  fprintf ( 1, '  The sum of these energies should be a constant.\n' );
  fprintf ( 1, '  As an accuracy check, we also print the relative error\n' );
  fprintf ( 1, '  in the total energy.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Step      Potential       Kinetic        (P+K-E0)/E0\n' );
  fprintf ( 1, '                Energy P        Energy K       Relative Energy Error\n' );
  fprintf ( 1, '\n' );

  step_print_index = 0;
  step_print_num = 10;

  step = 0;
  fprintf ( 1, '  %8d  %14f  %14f  %14e\n', ...
    step, potential, kinetic, ( potential + kinetic - e0 ) / e0 );
  step_print_index = step_print_index + 1;
  step_print = floor ( ( step_print_index * step_num ) / step_print_num );

  tic;

  for step = 1 : step_num

    [ force, potential, kinetic ] = compute ( np, nd, pos, vel, mass );

    if ( step == step_print )
      fprintf ( 1, '  %8d  %14f  %14f  %14e\n', ...
        step, potential, kinetic, ( potential + kinetic - e0 ) / e0 );
      step_print_index = step_print_index + 1;
      step_print = floor ( ( step_print_index * step_num ) / step_print_num );
    end
%
%  Plot the particles.
%
    if ( show_plots )

      if ( nd == 2 || nd == 3 )
        clf
        if ( nd == 2 )
          scatter ( pos(1,:), pos(2,:), 10.0, 'r', 'filled' )
        elseif ( nd == 3 )
          scatter3 ( pos(1,:), pos(2,:), pos(3,:), 10.0, 'r', 'filled' )
        end
        title ( sprintf ( 'Step %d\n', step ) );
        pause ( 0.5 )

      end

    end
%
%  Update the data for the next time step.
%
    [ pos, vel, acc ] = update ( np, nd, pos, vel, force, acc, mass, dt );

  end

  wtime = toc;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Main computation:\n' );
  fprintf ( 1, '    Wall clock time = %f seconds.\n', wtime );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MD_FAST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function [ f, pot, kin ] = compute ( np, nd, pos, vel, mass )

%*****************************************************************************80
%
%% COMPUTE computes the forces and energies.
%
%  Discussion:
%
%    This is a modified version of the original compute90 function.  It has
%    been rewritten in an attempt to speed it up, by taking advantage of
%    MATLAB's vector capabilities.
%
%    The potential function V(X) is a harmonic well which smoothly
%    saturates to a maximum value at PI/2:
%
%      v(x) = ( sin ( min ( x, PI2 ) ) )^2
%
%    The derivative of the potential is:
%
%      dv(x) = 2.0 * sin ( min ( x, PI2 ) ) * cos ( min ( x, PI2 ) )
%            = sin ( 2.0 * min ( x, PI2 ) )
%
%    Thanks to Abouzar Moshfegh for suggesting a revision to the kinetic
%    energy computation, 05 July 2012.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 July 2012
%
%  Author:
%
%    Original FORTRAN90 version by Bill Magro.
%    MATLAB version by John Burkardt and Gene Cliff.
%
%  Parameters:
%
%    Input, integer NP, the number of particles.
%
%    Input, integer ND, the number of spatial dimensions.
%
%    Input, real POS(ND,NP), the position of each particle.
%
%    Input, real VEL(ND,NP), the velocity of each particle.
%
%    Input, real MASS, the mass of each particle.
%
%    Output, real F(ND,NP), the forces.
%
%    Output, real POT, the total potential energy.
%
%    Output, real KIN, the total kinetic energy.
%
  f = zeros ( nd, np );

  pot = 0.0;

  pi2 = pi / 2.0;

  for i = 1 : np
%
%  Set up an array of vectors to particle I.
%
    Ri = pos - repmat ( pos( :, i ), 1, np );
%
%  Set up an array of distances.
%
    D = sqrt ( sum ( Ri.^2 ) );

    Ri = Ri( :, ( D > 0.0 ) );

    D = D( D > 0.0 );
%
%  Truncate the potential.
%
    D2 = D .* ( D <= pi2 ) + pi2 * ( D > pi2 );
%
%  Accumulate the potential energy.
%
    pot = pot + 0.5 * sum ( sin ( D2 ).^2 );
%
%  Compute force on particle I.
%
    f( :, i) = Ri * ( sin ( 2*D2 ) ./ D )';

  end
%
%  Compute the kinetic energy.
%
  kin = 0.5 * mass * sum ( sum ( vel.^2 ) );

  return
end
function [ pos, vel, acc, seed ] = initialize ( np, nd, box, seed )

%*****************************************************************************80
%
%% INITIALIZE initializes the positions, velocities, and accelerations.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 July 2008
%
%  Author:
%
%    Original FORTRAN90 version by Bill Magro.
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, integer NP, the number of particles.
%
%    Input, integer ND, the number of spatial dimensions.
%
%    Input, real BOX(ND), specifies the maximum position
%    of particles in each dimension.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real POS(ND,NP), the position of each particle.
%
%    Output, real VEL(ND,NP), the velocity of each particle.
%
%    Output, real ACC(ND,NP), the acceleration of each particle.
%
%    Output, integer SEED, a seed for the random number generator.
%

%
%  Start by setting the positions to random numbers between 0 and 1.
%
  pos(1:nd,1:np) = rand ( nd, np );
%
%  Use these random values as scale factors to pick random locations
%  inside the box.
%
  for i = 1 : nd
    pos(i,1:np) = box(i) * pos(i,1:np);
  end
%
%  Velocities and accelerations.
%
  vel(1:nd,1:np) = randn ( nd, np );
  acc(1:nd,1:np) = 0.0;

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end
function [ pos, vel, acc ] = update ( np, nd, pos, vel, f, acc, mass, dt )

%*****************************************************************************80
%
%% UPDATE updates positions, velocities and accelerations.
%
%  Discussion:
%
%    The time integration is fully parallel.
%
%    A velocity Verlet algorithm is used for the updating.
%
%    x(t+dt) = x(t) + v(t) * dt + 0.5 * a(t) * dt * dt
%    v(t+dt) = v(t) + 0.5 * ( a(t) + a(t+dt) ) * dt
%    a(t+dt) = f(t) / m
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 July 2008
%
%  Author:
%
%    Original FORTRAN90 version by Bill Magro.
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, integer NP, the number of particles.
%
%    Input, integer ND, the number of spatial dimensions.
%
%    Input, real POS(ND,NP), the position of each particle.
%
%    Input, real VEL(ND,NP), the velocity of each particle.
%
%    Input, real F(ND,NP), the force on each particle.
%
%    Input, real ACC(ND,NP), the acceleration of each
%    particle.
%
%    Input, real MASS, the mass of each particle.
%
%    Input, real DT, the time step.
%
%    Output, real POS(ND,NP), the updated position of each particle.
%
%    Output, real VEL(ND,NP), the updated velocity of each particle.
%
%    Output, real ACC(ND,NP), the updated acceleration of each
%    particle.
%
  rmass = 1.0 / mass;

  pos(1:nd,1:np) = pos(1:nd,1:np) + vel(1:nd,1:np) * dt ...
    + 0.5 * acc(1:nd,1:np) * dt * dt;

  vel(1:nd,1:np) = vel(1:nd,1:np) ...
    + 0.5 * dt * ( f(1:nd,1:np) * rmass + acc(1:nd,1:np) );

  acc(1:nd,1:np) = f(1:nd,1:np) * rmass;

  return
end


classdef SPX_CGLeastSquare < handle
% Conjugate gradient algorithm for least squares  or
% normal equations implementation
% We are solving the problem minimize \| A x - b \|_2^2.
% A is of size M x N.
% M > N.
% A^T A is of size N x N.
% b is of size M x 1.
% x is of size N x 1.
% A is full rank. Rank(A) = N.
% We assume that A is well conditioned.
% We can simultaneously solve S problems.

    properties
        % Settable and gettable properties

        % Maximum number of iterations for which the algorithm can run
        MaxIterations
        % Threshold of norm (in terms of percentage)
        NormThreshold
    end

    properties(SetAccess=private)
        % Gettable properties

        % The full rank tall matrix operator M x N
        A
        % The vectors to be solved M x S
        B
        % Measurements dimension
        M
        % Problem dimension
        N
        % Number of equations to be solved
        S
        % Number of iterations taken for solving the problem
        Iterations
        % The solution vectors
        X
        % Residual vectors in the CG algorithm
        CGResiduals
        % Residual norms at the end of the corresponding CG problem.
        CGResidualNorms
        % Residual vectors of the LS problem
        LSResiduals
        % Norms of the LS residuals at the end
        LSResidualNorms
        % Indicates if the problems converged
        % This would be false only if the algorithm crossed max iterations
        Converged
    end

    methods
        % Public methods
        function self = SPX_CGLeastSquare(A, B)
            % Constructor
            if isa(A, 'spx.dict.Operator')
                self.A = A;
            elseif ismatrix(A)
                self.A = spx.dict.MatrixOperator(A);
            else
                error('Unsupported operator.');
            end
            [self.M, self.N] = size(A);
            if self.M < self.N
                error('Only over-determined systems are supported')
            end
            % Ideally we should check the rank too.
            % But that would be too time consuming.
            [bm, self.S] = size(B);
            if bm ~= self.M
                error('Dimensions mismatch.');
            end
            self.B = B;
            self.MaxIterations = self.N ;
            self.NormThreshold = 1e-6;
        end

        function result = solve(self)
            aa = self.A; % M x N
            % We compute A' B in advance.
            atbb = self.A.apply_ctranspose(self.B); % N x S
            % Initial estimate vectors are all zeros
            xx = zeros(self.N, self.S); % N x S
            result = xx;
            self.Iterations = zeros(self.S, 1);
            self.CGResidualNorms = zeros(self.S, 1);
            self.Converged = false(self.S, 1);
            % Initial residual vectors
            rr = atbb  - aa.apply_ctranspose(aa.apply(xx)); % N x S
            self.CGResiduals  = rr;
            % Norm squared of initial residual vectors
            deltas = spx.norm.inner_product_cw(rr, rr);
            % The factor with which the norm needs to be reduced
            epsilon = self.NormThreshold;
            % Target limits on norm squared of residuals
            limits = epsilon^2 * deltas;
            % Maximum number of iterations for which the algorithm
            % is allowed to run
            imax = self.MaxIterations;
            % Number of problems being solved
            ns = self.S;
            for s=1:ns
                % Initialize iteration counter
                i = 0;
                % The quantities for this problem
                limit = limits(s);
                delta = deltas(s);
                % First residual
                r = rr(:, s); % N x 1.
                % First estimate
                x = xx(:, s); % N x 1.
                % Target b
                atb = atbb(:, s); % N x 1.
                % First direction
                d = r;
                while i < imax && delta > limit
                    % Compute the intermediate variable
                    p = aa.apply(d); % M x 1
                    q = aa.apply_ctranspose(p); % N x 1
                    % the line search scale factor in current direction
                    % Note that d' * q = p' * p.
                    alpha = delta / (p' * p);
                    % Update estimate in current direction
                    x = x + alpha * d;
                    if mod(i , 50) == 0
                        % In order to avoid propagation of floating point
                        % errors, we will recompute the value of residual
                        r = atb - aa.apply_ctranspose(aa.apply(x));
                    else
                        % Otherwise we use a shortcut
                        r = r  - alpha * q;
                    end
                    % hold the current residual norm squared
                    delta_old = delta;
                    % Update residual norm squared
                    delta = r' * r;
                    % Compute the ratio
                    beta = delta / delta_old;
                    % choose the new direction
                    d = r + beta * d;
                    % Increase iteration counter
                    i = i + 1;
                end
                % The problem has been solved
                result(:, s) = x;
                % Number of iterations taken to solve this problem
                self.Iterations(s) = i;
                self.CGResidualNorms(s) = sqrt(delta);
                self.CGResiduals(:, s) = r;
                self.Converged(s) = delta <= limit;
            end
            % Maintain the result for reference
            self.X = result;
            % Compute the corresponding LS residuals
            self.LSResiduals = self.B - aa.apply(self.X);
            self.LSResidualNorms = spx.norm.norms_l2_cw(self.LSResiduals);
        end

        function result = hasConverged(self)
            % Returns if all the solutions have converged
            result = all(self.Converged);
        end

        function printResults(self)
            ns = self.S;
            nn = self.N;
            mm = self.N;
            for s = 1:ns
                fprintf('Problem: %d\n', s);
                fprintf('Iterations: %d\n', self.Iterations(s));
                fprintf('CG Residual norm: %.2f, LS Residual norm: %.2f, Converged: %d\n', ...
                    self.CGResidualNorms(s), self.LSResidualNorms(s), self.Converged(s));
                if nn < 10
                    % We will print the solutions too
                    fprintf('Solution vector: ');
                    fprintf('%.4f ', self.X(:, s));
                    fprintf('\n');
                    fprintf('CG Residual vector: ');
                    fprintf('%.4f ', self.CGResiduals(:, s));
                    fprintf('\n');
                end
                if mm < 10
                    fprintf('LS Residual vector: ');
                    fprintf('%.4f ', self.LSResiduals(:, s));
                    fprintf('\n');
                end
            end
        end
    end

    methods(Access=private)
        % Private methods

    end

    methods(Static)
        % Public static methods

    end

end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
% problem 5 - convolution of x(t) and h(t)

t1=0:.1:.9;
t2=1:.1:3;
t3=3.1:.1:5;
x1=zeros(size(t1));
x2=t2;
x3=ones(size(t3));
x=[x1 x2 x3];
h1=zeros(size(t1));
h2=exp(-t2);
h3=zeros(size(t3));
h=[h1 h2 h3];
y=conv(x,h)*0.1;
plot(0:.1:10,y);
legend('y(t)')

figure
th=1:.01:3;
h=exp(-th);
tx1=1:.01:3;
x1=tx1;
tx2=3.01:.01:5;
x2=ones(size(tx2));
x=[x1 x2];
y=conv(x,h)*0.01;
plot(2:.01:8,y);
legend('y(t)')


function D = mahalDist(x, m, C, use_log)
% p=gaussian_prob(x, m, C, use_log)
%
% Evaluate the multi-variate density with mean vector m and covariance
% matrix C for the input vector x.
% Vectorized version: Here X is a matrix of column vectors, and p is
% a vector of probabilities for each vector.

if nargin<4, use_log = 0; end

d   = length(m);

if size(x,1)~=d
   x=x';
end
N       = size(x,2);

m       = m(:);
M       = m*ones(1,N);
denom   = (2*pi)^(d/2)*sqrt(abs(det(C)));
invC    = inv(C);
mahal   = sum(((x-M)'*invC).*(x-M)',2);   % Chris Bregler's trick

switch use_log,
case 2,
  D     = mahal;
case 1,
  D     = -0.5*mahal - log(denom);
case 0,
  numer = exp(-0.5*mahal);
  D     = numer/denom;
otherwise
    error('Unsupported log type')
end


function [spath,mpath,stdpath,fwd,stdfwd,xpath,epath,r1,r2]=schwartzsmithsim(start,param,ftemp,T,rmatrix,sim)
kappa=param(1);
mue=param(2);
sigmax=param(3);
sigmae=param(4);
lambdae=param(5);
lambdax=param(6);
pxe=param(7);
x0=param(8);
e0=param(9);

iter=start;
for k=1:length(T)
    loc=mod(iter,12);

    if loc==0
        f(k)=ftemp(12);
    else
        f(k)=ftemp(loc);
    end
    iter=iter+1;
end

for i=1:length(T)
    explns(i,1)=exp(-kappa*T(i))*x0+e0-((1-exp(-kappa*T(i)))*lambdax/kappa)+(mue-lambdae)*T(i);
    varlns(i,1)=((1-exp(-2*kappa*T(i)))*sigmax^2/(2*kappa))+sigmae^2*T(i)+2*(1-exp(-kappa*T(i)))*pxe*sigmax*sigmae/kappa;
    fwd(i,1)=f(i)*exp(explns(i,1)+.5*varlns(i,1));
    varlnfwd(i,1)= T(i)*(sigmae)^2 + (1-exp(-2*kappa*T(i)))*(sigmax^2)/(2*kappa) + 2*(1-exp(-kappa*T(i)))*pxe*sigmax*sigmae/kappa;
    stdfwd(i,1)=f(i)*sqrt((exp(varlnfwd(i,1))-1)*exp(2*explns(i,1)+varlnfwd(i,1)));
    stdspot(i,1)=f(i)*sqrt((exp(varlnfwd(i,1))-1)*exp(2*explns(i,1)+varlnfwd(i,1)));
    ve(i,1)=sigmae^2*T(i);
    vx(i,1)=(1-exp(-2*kappa*T(i)))*(.5*sigmax^2)/kappa;
end

m=length(T);

r1(:,:)=rmatrix;
r2(:,:)=pxe.*r1(:,:)+sqrt(1-pxe^2).*randn(sim,m);

for i=1:sim

x(1)=x0-lambdax*T(1)-kappa*x0*T(1)+sigmax*sqrt(T(1))*r1(i,1);
e(1)=e0+(mue-lambdae)*T(1)+sigmae*sqrt(T(1))*r2(i,1);
s(1)=f(1)*exp(e(1)+x(1));

for t=1:m-1
    x(t+1)=x(t)-lambdax*(T(t+1)-T(t))-kappa*x(t)*(T(t+1)-T(t))+sigmax*sqrt((T(t+1)-T(t)))*r1(i,t+1);
    e(t+1)=e(t)+(mue-lambdae)*(T(t+1)-T(t))+sigmae*sqrt((T(t+1)-T(t)))*r2(i,t+1);
    s(t+1)=f(t+1)*exp(e(t+1)+x(t+1));
end
epath(i,:)=e(1:end);
xpath(i,:)=x(1:end);
spath(i,:)=s(1:end);
lspath(i,:)=log(s(1:end));
c(i)=corr(r1(i,:)',r2(i,:)');
c2(i)=corr(xpath(i,:)',epath(i,:)');
end
evar = var(epath)';
xvar = var(xpath)';

%E(S(T))=E(F(T,T))
mpath=mean(spath)';
%V(ln(S(T)))=V[ln(F(T,T)]
varlnpath=var(lspath)';
%E(ln(S(T)))=E[ln(F(T,T)]
mlnpath=mean(lspath)';
stdpath=sqrt((exp(varlnpath)-1).*exp(2*mlnpath+varlnpath));


function inurbs = nrbdegelev(nurbs, ntimes)
%
% Function Name:
%
%   nrbdegelev - Elevate the degree of the NURBS curve or surface.
%
% Calling Sequence:
%
%   ecrv = nrbdegelev(crv,utimes);
%   esrf = nrbdegelev(srf,{utimes,vtimes});
%
% Parameters:
%
%   crv		: NURBS curve, see nrbmak.
%
%   srf		: NURBS surface, see nrbmak.
%
%   utimes	: Increase the degree along U direction utimes.
%
%   vtimes	: Increase the degree along V direction vtimes.
%
%   ecrv	: new NURBS structure for a curve with degree elevated.
%
%   esrf        : new NURBS structure for a surface with degree elevated.
%
%
% Description:
%
%   Degree elevates the NURBS curve or surface. This function uses the
%   B-Spline function bspdegelev, which interface to an internal 'C'
%   routine.
%
% Examples:
%
%   Increase the NURBS surface twice along the V direction.
%   esrf = nrbdegelev(srf, 0, 1);
%
% See:
%
%   bspdegelev

%  D.M. Spink
%  Copyright (c) 2000.

if nargin < 2
  error('Input argument must include the NURBS and degree increment.');
end

if ~isstruct(nurbs)
  error('NURBS representation is not structure!');
end

if ~strcmp(nurbs.form,'B-NURBS')
  error('Not a recognised NURBS representation');
end

degree = nurbs.order-1;

if iscell(nurbs.knots)
  % NURBS represents a surface
  [dim,num1,num2] = size(nurbs.coefs);

  % Degree elevate along the v direction
  if ntimes(2) == 0
    coefs = nurbs.coefs;
    knots{2} = nurbs.knots{2};
  else
    coefs = reshape(nurbs.coefs,4*num1,num2);
    [coefs,knots{2}] = bspdegelev(degree(2),coefs,nurbs.knots{2},ntimes(2));
    num2 = size(coefs,2);
    coefs = reshape(coefs,[4 num1 num2]);
  end

  % Degree elevate along the u direction
  if ntimes(1) == 0
    knots{1} = nurbs.knots{1};
  else
    coefs = permute(coefs,[1 3 2]);
    coefs = reshape(coefs,4*num2,num1);
    [coefs,knots{1}] = bspdegelev(degree(1),coefs,nurbs.knots{1},ntimes(1));
    coefs = reshape(coefs,[4 num2 size(coefs,2)]);
    coefs = permute(coefs,[1 3 2]);
  end

else

  % NURBS represents a curve
  if isempty(ntimes)
    coefs = nurbs.coefs;
    knots = nurbs.knots;
  else
    [coefs,knots] = bspdegelev(degree,nurbs.coefs,nurbs.knots,ntimes);
  end

end

% construct new NURBS
inurbs = nrbmak(coefs,knots);


function determ = gk324_determinant ( n, x )

%*****************************************************************************80
%
%% GK324_DETERMINANT returns the determinant of the GK324 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%   18 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real X(N-1), the first N-1 entries of the
%    last row.
%
%    Output, real DETERM, the determinant.
%
  determ = prod ( 1.0 - x(1:n-1) );

  return
end


%  Figure 3.24      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%script to generate Fig. 3.24

clf;
zeta=.5;
k=1/zeta;
den=[1 1 1];

a=10;
num=[k/a 1];
t=0:.1:10;
y1=step(num,den,t);

a=4;
num=[k/a 1];
t=0:.1:10;
y2=step(num,den,t);

a=2;
num=[k/a 1];
t=0:.1:10;
y3=step(num,den,t);

a=1;
num=[k/a 1];
t=0:.1:10;
y4=step(num,den,t);

plot(t,y1,'-',t,y2,'-',t,y3,'-',t,y4,'-'),grid
title('Fig. 3.24 Step response with \xi = 0.5')
xlabel('\omega_n t')
ylabel('Step response of H(s)')
text(.1,.9,'\alpha=')
text(.6,.9,'1')
text(1.1,.9,'2')
text(1.5,.85,'4')
text(1.8,.8,'100')


function [y,deriv] = neg_gaussll_taylor(w,x)
% This is an MV2DF. See MV2DF_API_DEFINITION.readme.
% This function represents the part of log N(x|0,W) that is dependent on
%   W = reshape(w,...), where w is variable and x is given.
%
% y = -0.5*x'*inv(W)*x - 0.5*log(det(W)), where W is positive definite and W = reshape(w,...)

if nargin==0
    test_this();
    return;
end

if isempty(w)
    y = @(w)neg_gaussll_taylor(w,x);
    return;
end

if isa(w,'function_handle')
    outer = neg_gaussll_taylor([],x);
    y = compose_mv(outer,w,[]);
    return;
end

dim = length(x);
W = reshape(w,dim,dim);

[inv_map,logdet] = invchol_taylor(W);
z = inv_map(x);
y = 0.5*x'*z + 0.5*logdet;
deriv = @(dy) deriv_this(dy,z,inv_map);

end

function [g,hess,linear] = deriv_this(dy,z,inv_map)
G1 = z*z.';
G2 = inv_map(eye(length(z)));
grad = 0.5*(G2(:)-G1(:));
g = dy*grad;
linear = false;
hess = @(d) hess_this(grad,z,inv_map,dy,d);
end

function [h,Jd] = hess_this(grad,z,inv_map,dy,d)
dim = sqrt(length(d));
D = reshape(d,dim,dim);
H1 = inv_map(D*z)*z' + z*inv_map(D'*z)';
H2 = inv_map(inv_map(D)');
h = 0.5*dy*(H1(:)-H2(:));
if nargout>1
    Jd =  grad.'*d(:);
end
end

function test_this()
m = 3;
n = 10;

w = [];
A = UtU(w,n,m); %A is m-by-m
x = randn(m,1);

f = neg_gaussll_taylor(A,x);
w = randn(m*n,1);

test_MV2DF(f,w,true);
end


function [P,Gvec] = autoGen_getPoints(q1,q2,q3,q4,q5,l1,l2,l3,l4,l5,c1,c2,c3,c4,c5)
%AUTOGEN_GETPOINTS
%    [P,GVEC] = AUTOGEN_GETPOINTS(Q1,Q2,Q3,Q4,Q5,L1,L2,L3,L4,L5,C1,C2,C3,C4,C5)

%    This function was generated by the Symbolic Math Toolbox version 6.2.
%    22-Oct-2015 19:14:40

t2 = sin(q1);
t3 = cos(q1);
t4 = l1.*t3;
t5 = sin(q2);
t6 = cos(q2);
t7 = l2.*t6;
t8 = sin(q4);
t9 = l4.*t8;
t10 = cos(q4);
t11 = sin(q3);
t12 = cos(q3);
t13 = l3.*t12;
t14 = sin(q5);
t15 = cos(q5);
P = [-l1.*t2;t4;-l1.*t2-l2.*t5;t4+t7;-l1.*t2-l2.*t5-l3.*t11;t4+t7+t13;t9-l1.*t2-l2.*t5;t4+t7-l4.*t10;t9-l1.*t2-l2.*t5+l5.*t14;t4+t7-l4.*t10-l5.*t15];
if nargout > 1
    Gvec = [c1.*t2-l1.*t2;t4-c1.*t3;c2.*t5-l1.*t2-l2.*t5;t4+t7-c2.*t6;c3.*t11-l1.*t2-l2.*t5-l3.*t11;t4+t7+t13-c3.*t12;c4.*t8-l1.*t2-l2.*t5;t4+t7-c4.*t10;t9+c5.*t14-l1.*t2-l2.*t5;t4+t7-c5.*t15-l4.*t10];
end


function [ x, w ] = laguerre_ek_compute ( n )

%*****************************************************************************80
%
%% LAGUERRE_EK_COMPUTE: Laguerre quadrature rule by the Elhay-Kautsky method.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2011
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer N, the order.
%
%    Output, real X(N), the abscissas.
%
%    Output, real W(N), the weights.
%

%
%  Define the zero-th moment.
%
  zemu = 1.0;
%
%  Define the Jacobi matrix.
%
  bj = zeros ( n, 1 );

  for i = 1 : n
    bj(i) = i;
  end

  x = zeros ( n, 1 );
  for i = 1 : n
    x(i) = 2 * i - 1;
  end

  w = zeros ( n, 1 );
  w(1) = sqrt ( zemu );
%
%  Diagonalize the Jacobi matrix.
%
  [ x, w ] = imtqlx ( n, x, bj, w );

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

  return
end


function zi=interptri(tri,x,y,z,xi,yi)
% INTERPTRI Interpolate Triangulation.
% Zi = INTERPTRI(TRI,X,Y,Z,Xi,Yi) linearly interpolates the scattered data
% in vectors X,Y,Z as described by the triangulation TRI as returned by
% DELAUNAY at the individual pairs of points in Xi and Yi. That is, Zi(k)
% is the linear interpolation at the point (Xi(k),Yi(k)). Xi and Yi are not
% MESHGRIDed as they are in GRIDDATA when Xi is a row vector and and Yi is
% a column vector. Use MESHGRID when desired. For example,
%                  [Xii,Yii] = MESHGRID(Xi,Yi); % meshgrid yourself
%                         Zi = INTERPTRI(TRI,x,y,z,Xii,Yii);
% is the same as          Zi = GRIDDATA(x,y,z,Xi(:).',Yi(:));
%
% INTERPTRI is faster than GRIDDATA when the same data is interpolated more
% than once because the triangulation TRI is precomputed and passed into
% INTERPTRI, whereas it is computed within GRIDDATA each time it is called.
%
% See also GRIDDATA, MESHGRID, DELAUNAY, TRIPLOT, TRIMESH, TRISURF, TSEARCH

% D.C. Hanselman, University of Maine, Orono, ME 04469
% MasteringMatlab@yahoo.com
% Mastering MATLAB 7
% 2006-05-03

if nargin~=6
   error('Six Input Arguments Required.')
end
x=x(:); % make input data into vectors
y=y(:);
z=z(:).';
xlen=length(x);
if ~isequal(xlen,length(y),length(z))
   error('X, Y, and Z Must Have the Same Number of Elements.')
end
if size(tri,2)~=3 || any(tri(:)<0) || any(tri(:)>xlen)
   error('TRI Must Be a Valid Triangulation of the Data in X, Y, Z.')
end

zisiz=size(xi);
xi=xi(:);
yi=yi(:);
if length(xi)~=length(yi)
   error('Xi and Yi Must Have the Same Number of Elements.')
end
%ti=tsearch(x,y,tri,xi,yi); % find triangle associated with each data point.

% use tsearchn because tsearch is now gone
ti=tsearchn([x y],tri,[xi yi]);

tinan=isnan(ti);  % True for xi, yi outside the convex hull
ti(tinan)=1;      % point nan points to triangle one for now
tri=tri(ti,:);    % keep only those triangles where xi and yi exist

x1=x(tri(:,1));   % x data at vertices
x2=x(tri(:,2));
x3=x(tri(:,3));
y1=y(tri(:,1));   % y data at vertices
y2=y(tri(:,2));
y3=y(tri(:,3));

A2=(x2-x1).*(y3-y1) - (x3-x1).*(y2-y1); % shape functions
N(:,3)=((x1-xi).*(y2-yi) - (x2-xi).*(y1-yi))./A2;
N(:,2)=((x3-xi).*(y1-yi) - (x1-xi).*(y3-yi))./A2;
N(:,1)=((x2-xi).*(y3-yi) - (x3-xi).*(y2-yi))./A2;
N(tinan,:)=0;           % give zero weight to nan data
zi = sum(z(tri).*N,2);  % interpolate
zi(tinan)=nan;          % poke in nans where needed
zi=reshape(zi,zisiz);   % reshape output to match xi and yi input


function [ x, w ] = tetrahedron_arbq ( degree, n )

%*****************************************************************************80
%
%% TETRAHEDRON_ARBQ returns a quadrature rule for a tetrahedron.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the desired degree of exactness.
%    1 <= DEGREE <= 15.
%
%    Input, integer N, the number of points in the rule.
%    This value should be requested first from TETRAHEDRON_ARBQ_SIZE.
%
%    Output, real X(3,N), the quadrature nodes.
%
%    Output, real W(N), the quadrature weights.
%
  if ( degree == 1 )
    [ x, w ] = rule01 ( n );
  elseif ( degree == 2 )
    [ x, w ] = rule02 ( n );
  elseif ( degree == 3 )
    [ x, w ] = rule03 ( n );
  elseif ( degree == 4 )
    [ x, w ] = rule04 ( n );
  elseif ( degree == 5 )
    [ x, w ] = rule05 ( n );
  elseif ( degree == 6 )
    [ x, w ] = rule06 ( n );
  elseif ( degree == 7 )
    [ x, w ] = rule07 ( n );
  elseif ( degree == 8 )
    [ x, w ] = rule08 ( n );
  elseif ( degree == 9 )
    [ x, w ] = rule09 ( n );
  elseif ( degree == 10 )
    [ x, w ] = rule10 ( n );
  elseif ( degree == 11 )
    [ x, w ] = rule11 ( n );
  elseif ( degree == 12 )
    [ x, w ] = rule12 ( n );
  elseif ( degree == 13 )
    [ x, w ] = rule13 ( n );
  elseif ( degree == 14 )
    [ x, w ] = rule14 ( n );
  elseif ( degree == 15 )
    [ x, w ] = rule15 ( n );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TETRAHEDRON_ARBQ - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of DEGREE.\n' );
    error ( 'TETRAHEDRON_ARBQ - Fatal error!' );
  end

  d = sum ( w(1:n) );

  volume = sqrt ( 8.0 ) / 3.0;
  w(1:n) = w(1:n) * volume / d;

  return
end


function value = conex2_condition ( alpha )

%*****************************************************************************80
%
%% CONEX2_CONDITION returns the L1 condition of the CONEX1 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 February 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the scalar defining A.
%    A common value is 100.0.
%
%    Output, real VALUE, the L1 condition.
%
  c1 = 1.0;
  c2 = abs ( 1.0 - 1.0 / alpha^2 ) + 1.0 / abs ( alpha );
  c3 = 3.0 + 1.0 / abs ( alpha );
  a_norm = max ( c1, max ( c2, c3 ) );
  c1 = 1.0;
  c2 = abs ( ( 1.0 - alpha * alpha ) / alpha ) + abs ( alpha );
  c3 = abs ( ( 1.0 + alpha * alpha ) / alpha ^ 2 ) + 2.0;
  b_norm = max ( c1, max ( c2, c3 ) );
  value = a_norm * b_norm;

  return
end


function fx = p14_fx ( x )

%*****************************************************************************80
%
%% P14_FX evaluates the Camel.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(*), the point at which F is to be evaluated.
%
%    Output, real FX(*), the value of the function at X.
%
  fx =   1.0 ./ ( ( x - 0.3 ).^2 + 0.01 ) ...
       + 1.0 ./ ( ( x - 0.9 ).^2 + 0.04 ) ...
       + 2.0 * x - 5.2;

  return
end


function stage = BOPointInPolygon( P, q )
%% BOPointInPolygon - find point in polygon
%
%   REFERENCE:
%       This code is described in "Computational Geometry in C", Chapter 7.
%       The Matlab version is based on C code written by Joseph O'Rourke,
%       contributions by Min Xu, June 1997.
%
%   INPUT:
%       P       - input vector Nx2
%       q       - input query vector 1x2
%
%   OUTPUT:
%       stage   - output stage:
%                   'v' - q is a vertex
%                   'i' - q is inside
%                   'o' - q is outside
%                   'e' - q on the edge
%   USAGE:
%       t = 0:0.6:6.28;
%       seedx = 100; seedy = 100; scalefactor = 20;
%       P(:,1) = (seedx(1) + scalefactor*cos(t))';
%       P(:,2) = (seedy(1) + scalefactor*sin(t))';
%       PP = [P; P(1,:)];
%       plot(PP(:,1),PP(:,2),'-bs'); hold on
%       q = PP(10,:);
%       plot(q(:,1),q(:,2),'r*');
%       stage = BOPointInPolygon(PP,q);
%
%   AUTHOR:
%       Boguslaw Obara, http://boguslawobara.net/
%
%   VERSION:
%       0.1 - 25/05/2008 First implementation
%

%%
% i, i1     - point index; i1 = i-1 mod n
% d         - dimension index
% x         - x intersection of e with ray
% Rcross    - number of right edge/ray crossings
% Lcross    - number of left edge/ray crossings
%%
Rcross = 0; Lcross = 0;
n = length(P);
%%
% Shift so that q is the origin. Note this destroys the polygon.
for i=1:n
    for d=1:2
      P(i,d) = P(i,d) - q(d);
    end
end
%%
% For each edge e=(i-1,i), see if crosses ray.
for i=1:n
    % First see if q=(0,0) is a vertex.
    if P(i,1)==0 && P(i,2)==0
        stage = 'v';
        return;
    end
    i1 = mod((i + n - 2), n) + 1;

    % if e "straddles" the x-axis...
    % The commented-out statement is logically equivalent to the one following.
    % if( ((P(i,2)>0) && (P(i1,2)<=0)) || ((P(i1,2)>0) && (P(i,2)<=0))

    if (P(i,2)>0) ~= (P(i1,2)>0)
      % e straddles ray, so compute intersection with ray.
        x = (P(i,1) * P(i1,2) - P(i1,1) * P(i,2)) / (P(i1,2) - P(i,2));

      % crosses ray if strictly positive intersection.
      if x>0
          Rcross = Rcross + 1;
      end
    end

    % if e straddles the x-axis when reversed...
    % if ((P(i,2)<0) && (P(i1,2)>=0)) || ((P(i1,2)<0) && (P(i,2)>=0)

    if (P(i,2) < 0) ~= (P(i1,2) < 0)
      % e straddles ray, so compute intersection with ray.
      x = (P(i,1) * P(i1,2) - P(i1,1) * P(i,2)) / (P(i1,2) - P(i,2));
      % crosses ray if strictly positive intersection.
      if (x < 0)
          Lcross = Lcross + 1;
      end
    end
end
%%
% q on the edge if left and right cross are not the same parity.
if mod(Rcross,2) ~= mod(Lcross,2)
    stage = 'e';
    return;
end
% q inside if an odd number of crossings.
if mod(Rcross,2) == 1
    stage = 'i';
    return;
else
    stage = 'o';
    return;
end
%%
end


function [x_g,w_g,phi,p_x,p_y] = twod_shapeiso(x_local,r,s,w)
%-----------------------------------------------------------------------
%  twod_shapeiso.m - computes test functions and derivatives on an
%                    element given element coordinates and Gauss points.
%
%                    ! Note: specifically set up for isoparametric
%                    ! elements.  Use `twod_shape' for linear elements.
%
%  Copyright (c) 2001, Jeff Borggaard, Virginia Tech
%  Version: 1.0
%
%  Usage:    [x_g,w_g,phi,p_x,p_y] = twod_shapeiso(x_local,r,w)
%
%  Variables:     x_local
%                        Coordinates of the element nodes
%                 (r,s)
%                        Coordinates of Gauss points in unit triangle
%                 w
%                        Gauss weights associated with (r,s)
%
%                 x_g
%                        Coordinates of Gauss points in the element
%                 w_g
%                        Gauss weights scaled by the element Jacobian
%                 phi
%                        Value of element shape functions at x_g
%                 p_x
%                 p_y
%                        First spatial derivatives of phi
%                 p_xx
%                        Second spatial derivatives of phi
%                        (only applicable with 2nd order or higher)
%                        (currently not implemented)
%-----------------------------------------------------------------------
  x      = x_local;
  [n,t1] = size(x);  % t1 had better be 2
  rule   = length(r);

  if (n == 6)
    % Transform coordinates for quadratic element
    c0 =    x(1,:);
    c1 =-3.*x(1,:) -    x(2,:) + 4.*x(4,:);
    c2 =-3.*x(1,:) -    x(3,:) + 4.*x(6,:);
    c3 = 2.*x(1,:) + 2.*x(2,:) - 4.*x(4,:);
    c4 = 4.*x(1,:) - 4.*x(4,:) + 4.*x(5,:) - 4.*x(6,:);
    c5 = 2.*x(1,:) + 2.*x(3,:) - 4.*x(6,:);

    x_g(:,1) = c0(1) + c1(1)*r + c2(1)*s + c3(1)*r.*r +...
               c4(1)*r.*s + c5(1)*s.*s;
    xr  = c1(1) + 2.*c3(1)*r + c4(1)*s;
    xs  = c2(1) + c4(1)*r + 2.*c5(1)*s;

    x_g(:,2) = c0(2) + c1(2)*r + c2(2)*s + c3(2)*r.*r +...
               c4(2)*r.*s + c5(2)*s.*s;
    yr  = c1(2) + 2.*c3(2)*r + c4(2)*s;
    ys  = c2(2) + c4(2)*r + 2.*c5(2)*s;

    w_g = (xr.*ys-yr.*xs).*w;

    % Compute the Jacobian of the (r,s) -> (x,y) transformation
    jac = 1./( xr.*ys - yr.*xs );
    rx  = ys.*jac;
    sx  =-yr.*jac;
    ry  =-xs.*jac;
    sy  = xr.*jac;

    % Compute shape function and derivatives at Gauss points
    phi = zeros(rule,n);
    phi(:,1) = 1. - 3.*r - 3.*s + 2.*r.*r + 4.*r.*s + 2.*s.*s;
    phi(:,2) =    - 1.*r        + 2.*r.*r                    ;
    phi(:,3) =           - 1.*s                     + 2.*s.*s;
    phi(:,4) =      4.*r        - 4.*r.*r - 4.*r.*s          ;
    phi(:,5) =                              4.*r.*s          ;
    phi(:,6) =             4.*s           - 4.*r.*s - 4.*s.*s;

    p_x = zeros(rule,n);
    p_x(:,1) = ( -3. + 4.*r + 4.*s ).*rx + ( -3. + 4.*r + 4.*s ).*sx;
    p_x(:,2) = ( -1. + 4.*r        ).*rx                            ;
    p_x(:,3) =                             ( -1.        + 4.*s ).*sx;
    p_x(:,4) = (  4. - 8.*r - 4.*s ).*rx + (     - 4.*r        ).*sx;
    p_x(:,5) = (              4.*s ).*rx + (       4.*r        ).*sx;
    p_x(:,6) = (            - 4.*s ).*rx + (  4. - 4.*r - 8.*s ).*sx;

    p_y = zeros(rule,n);
    p_y(:,1) = ( -3. + 4.*r + 4.*s ).*ry + ( -3. + 4.*r + 4.*s ).*sy;
    p_y(:,2) = ( -1. + 4.*r        ).*ry                            ;
    p_y(:,3) =                             ( -1.        + 4.*s ).*sy;
    p_y(:,4) = (  4. - 8.*r - 4.*s ).*ry + (     - 4.*r        ).*sy;
    p_y(:,5) = (              4.*s ).*ry + (       4.*r        ).*sy;
    p_y(:,6) = (            - 4.*s ).*ry + (  4. - 4.*r - 8.*s ).*sy;
  else
    fprintf('element not supported');
  end


% PartitionTable.m by David Terr, Raytheon, 6-7-04

% Given nonnegative integer n, compute the unrestricted partition function p(k) for k<=n.
function pt = partitiontable(n)

if n==0
    pt = 1;
    return;
end

pt = zeros( n+1, 2 );
pt( n+1, 1 ) = n;

% Precompute tables of (-1)^k and k*(3k +/- 1)/2.
s = sqrt( 24*n + 1 );
kp = floor( ( s - 1 ) / 6 );
km = floor( ( s + 1 ) / 6 );
tp = zeros( kp );
tm = zeros( km );

for k = 1:kp
    tp( k ) = k*(3*k+1)/2;
end

for k = 1:km
    tm( k ) = k*(3*k-1)/2;
end

% Compute p(m) for all m <= n.
pt( 1, 2 ) = 1;

for m=1:n
    pt(m,1) = m-1;
    s = sqrt( 24*m + 1 );
    kp = floor( ( s - 1 ) / 6 );
    km = floor( ( s + 1 ) / 6 );

    for k = 1:kp
        pt( m+1, 2 ) = pt( m+1, 2 ) + (-1)^(k+1) * pt( m + 1 - tp(k), 2 );
    end

    for k = 1:km
        pt( m+1, 2 ) = pt( m+1, 2 ) + (-1)^(k+1) * pt( m + 1 - tm(k), 2 );
    end
end


function f = spm_Dpdf(x,a)
% Probability Density Function (PDF) of Dirichlet distribution
% FORMAT f = spm_Dpdf(x,a)
%
% x - Dirichlet variate
% a - Dirichlet parameters (a>0)
% f - PDF of Dirichlet-distribution at point x
%__________________________________________________________________________
%
% spm_Dpdf implements the Probability Density Function for Dirichlet
% distribution.
%
% Definition:
%--------------------------------------------------------------------------
% See http://en.wikipedia.org/wiki/Dirichlet_distribution
%
% Algorithm:
%--------------------------------------------------------------------------
% Direct computation using logs and MATLAB's implementation of the log of
% the gamma function (gammaln).
%__________________________________________________________________________
% Copyright (C) 2008-2011 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_Dpdf.m 4182 2011-02-01 12:29:09Z guillaume $

%-Check enough arguments
%--------------------------------------------------------------------------
if nargin<2, error('Insufficient arguments'), end

%-Computation
%--------------------------------------------------------------------------
a = a(:);
x = x(:);

f = exp( gammaln(sum(a)) + sum((a-1).*log(x+eps)) - sum(gammaln(a)) );


function y = log_normcdf( x, approx ) %#ok

%LOG_NORMCDF   Logarithm of the cumulative normal distribution.
%   Y = LOG_NORMCDF(X) is the logarithm of the CDF of the normal
%   distribution at the point X.
%
%                                1    / x
%       LOG_NORMCDF(X) = LOG( ------- |   exp(-t^2/2) dt )
%                             sqrt(2) / -Inf
%
%   For numeric X, LOG_NORMCDF(X) is computed using the equivalent
%   expression LOG(0.5*ERFC(-X*SQRT(0.5))). When X is a CVX variable, a
%   a piecewise quadratic *approximation* is employed instead. This
%   approximation gives good results when -4 <= x <= 4, and will be
%   improved in future releases of CVX.
%
%   For array values of X, the LOG_NORMCDF returns an array of identical
%   size with the calculation applied independently to each element.
%
%   X must be real.
%
%   Disciplined convex programming information:
%       LOG_NORMCDF is concave and nondecreasing in X. Therefore, when used
%       in CVX specifications, X must be concave.

narginchk(1,2);
if ~isreal( x ),
    error( 'Argument must be real.' );
end
if nargin > 1,
    % For debugging purposes only
    y = cvx_constant(log_normcdf(cvx(x)));
else
    y = log(0.5*erfc(-x*sqrt(0.5)));
end

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


function [vOut,data]=getNextMultipartition(data)
%%GETNEXTMULTIPARTITION Get the next multipartition of a set of elements
%           with possible repeats, or get the first multipartition in the
%           sequence. The multipartitions are given in decreasing
%           lexicographic order.
%
%INPUTS: data If the first multipartition is desired, then this is an mX1
%             or 1Xm vector containing how many times each element in the
%             set to be partitioned is repeated. For example, if one wishes
%             to partition the set (1,2,3,4), then this is [1;1;1;1]. For
%             the set [1;2;2;2;3;3;4], then this is [1;3;2;1]. Otherwise,
%             if one wishes to get the next multipartition in the sequence,
%             then this is the data variable returned from the previous
%             call to this function.
%
%OUTPUTS: vOut This is the current multipartition, or an empty vector if
%              one has passed the final multipartition in the sequence. The
%              multipartition is an mXnumParts matrix. The number of
%              columns is the number of parts of the partition. Each column
%              has the number of repeats of each item (corresponding to the
%              positions in the original data passed). See the examples
%              below for more explanation.
%         data This is a structure that can be passed to this function to
%              get the next multipartition in the sequence.
%
%This function implements Algorithm M of Chapter 7.2.1.5 of [1].
%
%As an example, there are nine multipartitions of the set (1,1,2,2). These
%are
%(1,1,2,2);  (1,1,2),(2);  (1,1),(2,2);  (1,1),(2),(2); (1,2,2),(1);
%(1,2),(1,2);  (1,2),(1),(2);  (1),(1),(2,2); (1),(1),(2),(2)
%However, represented in terms of the output vOut, we have 9 matrices:
% [2;  [2,0; [2,0; [2,0,0;  [1,1;  [1,1;  [1,1,0;  [1,1,0;  [1,1,0,0
%  2]   1,1]  0,2]  0,1,1]   2,0]   1,1]   1,0,1]   0,0,2]   0,0,1,1]
%The example below shows how to use this function and how to display the
%outputs in a more recognizable form as partitions.
%
%EXAMPLE:
%Here, we display all multipartitions of 1123, which means that n=[2;1;1].
% vTotal=[1;1;2;3];
% vUnique=unique(vTotal);
% n=[2;1;1];
% m=length(n);
% [vOut,data]=getNextMultipartition(n);
% while(~isempty(vOut))
%     %Display the current multipartition.
%     %We will build a string showing the partitions. of [1,1,2,3].
%     strDisp=[];
%
%     numParts=size(vOut,2);
%     for curPart=1:numParts
%         strDisp=[strDisp,sprintf('(')];
%         for k=1:m
%             for i=1:vOut(k,curPart)
%                 strDisp=[strDisp,sprintf('%i',vUnique(k))];
%             end
%         end
%         strDisp=[strDisp,sprintf(')')];
%     end
%     disp(strDisp)
%
%     %Get the next one.
%     [vOut,data]=getNextMultipartition(data);
% end
%
%REFERENCES:
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4, Fascicle 3:
%    Generating all Combinations and Partitions, Upper Saddle River, NJ:
%    Addison-Wesley, 2009.
%
%February 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%If the first multipartition is desired.
if(~isstruct(data))
    n=data;
    data=[];

    m=length(n);
    nSum=sum(n);

    data.nSum=nSum;
    data.m=m;

    %Allocate space
    f=zeros(nSum+1,1);
    c=zeros(m*nSum+1,1);
    u=zeros(m*nSum+1,1);
    v=zeros(m*nSum+1,1);

    %Step M1
    c(1:m)=1:m;
    u(1:m)=n(1:m);
    v(1:m)=n(1:m);
    f(0+1)=0;
    a=0;
    l=0;

    f(1+1)=m;
    b=m;
else
    %If the algorithm is finished.
    if(data.algFinished)
       vOut=[];
       return;
    end

    f=data.f;
    c=data.c;
    u=data.u;
    v=data.v;
    a=data.a;
    b=data.b;
    l=data.l;
    m=data.m;
    nSum=data.nSum;
end

while(1)
    %Step M2
    j=a;
    k=b;
    x=0;

    while(j<b)
        u(k+1)=u(j+1)-v(j+1);

        if(u(k+1)==0)
            x=1;
            j=j+1;
        elseif(x==0)
            c(k+1)=c(j+1);
            v(k+1)=min(v(j+1),u(k+1));
            x=u(k+1)<v(j+1);
            k=k+1;
            j=j+1;
        else
            c(k+1)=c(j+1);
            v(k+1)=u(k+1);
            k=k+1;
            j=j+1;
        end
    end

    %Step M3
    if(k>b)
        a=b;
        b=k;
        l=l+1;
        f(l+1+1)=b;
        continue;%Return to M2
    end

    %Step M4, visit the partition
    numOut=0;
    vOut=zeros(m,nSum);%Allocate the maximum possible space needed.
    for k=0:l
        numOut=numOut+1;
        for j=f(k+1):(f(k+1+1)-1)
            vOut(c(j+1),numOut)=v(j+1);
        end
    end
    vOut=vOut(:,1:numOut);

    %Step M5
    while(1)

        j=b-1;
        while(v(j+1)==0)
            j=j-1;
        end

        if(~(j==a&&v(j+1)==1))
            v(j+1)=v(j+1)-1;

            for k=(j+1):(b-1)
               v(k+1)=u(k+1);
            end

            %Return to M2 to visit the next one, so return from this
            %function.
            data.f=f;
            data.c=c;
            data.u=u;
            data.v=v;
            data.a=a;
            data.b=b;
            data.l=l;
            data.algFinished=false;
            return
        end

        %Step M6
        if(l==0)%All multiset partitions have been found.
            %There is no need to update the other things in data, because
            %there are no more multipartitions to find.
            data.algFinished=true;
            return;
        end

        l=l-1;
        b=a;
        a=f(l+1);
    end
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.


%% "Spirograph" Code
%  Jonathan Jamieson - September 2013
%  unigamer@gmail.com
%  www.jonathanjamieson.com

function [P_x_global,P_y_global ] = CalculatePenPosition(A_x_global,A_y_global,C_x_global,C_y_global,extension  )

dx = C_x_global-A_x_global;
dy = C_y_global-A_y_global;

theta = atan2(dy,dx);

P_x_global = extension*cos(theta)+C_x_global;
P_y_global = extension*sin(theta)+C_y_global;

end


function dist_signed = quad_point_dist_signed_2d ( q, p )

%*****************************************************************************80
%
%% QUAD_POINT_DIST_SIGNED_2D: signed distanct ( quadrilateral, point ) in 2D.
%
%  Discussion:
%
%    The quadrilateral must be convex.  DIST_SIGNED is actually the maximum
%    of the signed distances from the point to each of the four lines that
%    make up the quadrilateral.
%
%    Essentially, if the point is outside the convex quadrilateral,
%    only one of the signed distances can be positive, or two can
%    be positive and equal.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real Q(2,4), the vertices of the quadrilateral.
%
%    Input, real P(2,1), the point which is to be checked.
%
%    Output, real DIST_SIGNED, the signed distance from the
%    point to the convex quadrilateral.  If DIST_SIGNED is
%    0.0, the point is on the boundary;
%    negative, the point is in the interior;
%    positive, the point is in the exterior.
%

%
%  Compare the signed distance from each line segment to the point,
%  with the signed distance to the midpoint of the opposite line.
%
%  The signed distances should all be negative if the point is inside.
%
%  Side 12
%
  dis12 = line_exp_point_dist_signed_2d ( q(1:2,1), q(1:2,2), p );

  pm(1:2,1) = 0.5 * ( q(1:2,3) + q(1:2,4) );

  dis = line_exp_point_dist_signed_2d ( q(1:2,1), q(1:2,2), pm );

  if ( 0.0 < dis )
    dis = -dis;
    dis12 = -dis12;
  end
%
%  Side 23
%
  dis23 = line_exp_point_dist_signed_2d ( q(1:2,2), q(1:2,3), p );

  pm(1:2,1) = 0.5 * ( q(1:2,4) + q(1:2,1) );

  dis = line_exp_point_dist_signed_2d ( q(1:2,2), q(1:2,3), pm );

  if ( 0.0 < dis )
    dis = -dis;
    dis23 = -dis23;
  end
%
%  Side 34
%
  dis34 = line_exp_point_dist_signed_2d ( q(1:2,3), q(1:2,4), p );

  pm(1:2,1) = 0.5 * ( q(1:2,1) + q(1:2,2) );

  dis = line_exp_point_dist_signed_2d ( q(1:2,3), q(1:2,4), pm );

  if ( 0.0 < dis )
    dis = -dis;
    dis34 = -dis34;
  end
%
%  Side 41
%
  dis41 = line_exp_point_dist_signed_2d ( q(1:2,4), q(1:2,1), p );

  pm(1:2,1) = 0.5 * ( q(1:2,2) + q(1:2,3) );

  dis = line_exp_point_dist_signed_2d ( q(1:2,4), q(1:2,1), pm );

  if ( 0.0 < dis )
    dis = -dis;
    dis41 = -dis41;
  end

  dist_signed = max ( dis12, max ( dis23, max ( dis34, dis41 ) ) );

  return
end


function value = r8_gamma_sample ( a, r )

%*****************************************************************************80
%
%% R8_GAMMA_SAMPLE generates a Gamma random deviate.
%
%  Discussion:
%
%    This procedure generates random deviates from the gamma distribution whose
%    density is (A^R)/Gamma(R) * X^(R-1) * Exp(-A*X)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2013
%
%  Author:
%
%    Original FORTRAN77 version by Barry Brown, James Lovato.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joachim Ahrens, Ulrich Dieter,
%    Generating Gamma Variates by a Modified Rejection Technique,
%    Communications of the ACM,
%    Volume 25, Number 1, January 1982, pages 47-54.
%
%    Joachim Ahrens, Ulrich Dieter,
%    Computer Methods for Sampling from Gamma, Beta, Poisson and
%    Binomial Distributions,
%    Computing,
%    Volume 12, Number 3, September 1974, pages 223-246.
%
%  Parameters:
%
%    Input, real A, the rate parameter.
%
%    Input, real R, the shape parameter.
%
%    Output, real VALUE, a random deviate
%    from the distribution.
%
  value = r8_gamma_01_sample ( r ) / a;

  return
end


function [G]=TabulateGenGam2dft(Rksi,Rgam,nu);
% function [G]=TabulateGenGam1dft(Rksi,Rgam,nu,K);
% This function tabulates the gain functions G for MMSE estimation
% of speech DFT coefficients, under the assumption of a
% Generalized Gamma speech amplitude prior with gamma=2. For explanations,
% derivations and motivations, see:
% J.S. Erkelens, R.C. Hendriks, R. Heusdens, and J. Jensen,
% "Minimum mean-square error estimation of discrete Fourier coefficients
% with Generalized Gamma priors", IEEE Trans. Audio, Speech and Language
% Processing, August 2007.
% and
%J.S. Erkelens, R.C. Hendriks and  R. Heusdens
%"On the Estimation of Complex Speech DFT Coefficients without Assuming Independent Real and Imaginary Parts",
%IEEE signal processing letters 2008.
%
% INPUT variables:
% Rksi: Array of "a priori" SNR (SNRprior) values for which the gains are
% computed. NOTE: The values must be in dBs.
% Rgam: Array of "a posteriori" SNR (SNRpost) values for which the gains
% are computed. NOTE: The values must be in dBs.
% 'nu': parameter of the Generalized Gamma distribution.
%
% OUTPUT variables:
% G: Matrix with gain values for speech DFT estimation,
% evaluated at all combinations of a priori and a posteriori SNR in the
% input variables Rksi and Rgam. To be multiplied with the noisy complex DFT coefficient.
%
%
% Copyright 2007: Delft University of Technology, Information and
% Communication Theory Group. The software is free for non-commercial use.
% This program comes WITHOUT ANY WARRANTY.
%
% Last modified: 22-11-2007.

Rgam=10.^(Rgam(:)'/10);%Rksi and Rgam are in dBs.
Rksi=10.^(Rksi/10);
G=zeros(length(Rksi),length(Rgam));
for k=1:length(Rgam)
    [g]=gam2gains(nu,Rgam(k),Rksi);
    G(:,k)=g(:);
end

function [G]=gam2gains(nu,gamm,ksi);

Q=ksi./(nu+ksi);
Teller=ConflHyperGeomFun(nu+1,2,Q.*gamm);%evaluate the confluent hypergeometric functions
Noemer=ConflHyperGeomFun(nu,1,Q.*gamm);
G=(nu.*Q).*Teller./Noemer;
% asymptotic result for Q.*gamm>700 (A&S, 13.5.1)  For very high values of nu and a posteriori SNR a value lower than 700 might be more appropriate.

I=find(Q.*gamm>700);
G(I)=Q(I);
%limit G for problematic cases
I=find(~isfinite(G));
G(I)=1;
I=find(G>1e4);
G(I)=1e4;


function pde = Lshapedata3
%% LSHAPEDATA data of Lshape Problem
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

pde = struct('f',0,'exactu',@exactu,'g_D',@exactu,'Du',@Du);

    function u = exactu(p) % exact solution
    x = p(:,1); y = p(:,2);
    r = sqrt(x.^2+y.^2);
    theta = atan2(p(:,2),p(:,1));
    theta = (theta>=0).*theta + (theta<0).*(theta+2*pi);
    u = r.^(2/3).*sin(2*theta/3);
    end

    function uprime = Du(p) % exact solution
    x = p(:,1); y = p(:,2);
    r = sqrt(x.^2+y.^2);
    theta = atan2(y,x);
    theta = (theta>=0).*theta + (theta<0).*(theta+2*pi);
    uprime(:,1) = 2/3*r.^(-1/3).*sin(2*theta/3).*x./r ...
                - 2/3*r.^(2/3).*cos(2*theta/3).*y./r.^2;
    uprime(:,2) = 2/3*r.^(-1/3).*sin(2*theta/3).*y./r ...
                + 2/3*r.^(2/3).*cos(2*theta/3).*x./r.^2;
    uprime(:,3) = 0;
    end
end


function varargout = goldsteinprice(X)
% Goldstein-Price function
%
%   GOLDSTEINPRICE([x1, x2]) returns the value of the value of the
%   Goldstein-Price function at the specified points. [x1] and [x2]
%   may be vectors. The search domain is
%
%               -2 < x_i < 2
%
%   The golbal minimum is
%
%               f(x1, x2) = f(0, -1) = 3.

% Author: Rody P.S. Oldenhuis
% Delft University of Technology
% E-mail: oldenhuis@dds.nl
% Last edited 20/Jul/2009

    % if no input is given, return dimensions, bounds and minimum
    if (nargin == 0)
        varargout{1} = 2;  % # dims
        varargout{2} = [-2, -2]; % LB
        varargout{3} = [+2, +2]; % UB
        varargout{4} = [0, -1]; % solution
        varargout{5} = 3; % function value at solution

    % otherwise, output function value
    else

        % keep values in the search domain
        X(X < -2) = inf;  X(X > 2) = inf;

        % split input vector X into x1, x2
        if size(X, 1) == 2
            x1 = X(1, :);        x2 = X(2, :);
        else
            x1 = X(:, 1);        x2 = X(:, 2);
        end

        % output function value
        varargout{1} = (1  + (x1 + x2 + 1).^2.*(19 - 14*x1 +  3*x1.^2 - 14*x2 +  6*x1.*x2 +  3*x2.^2)).*...
            (30 + (2*x1 - 3*x2).^2.*(18 - 32*x1 + 12*x1.^2 + 48*x2 - 36*x1.*x2 + 27*x2.^2));

    end

end


function y = lnCumGaussSum(u1, u2, w1, w2)

% LNCUMGAUSSSUM The log of the weighted sum of two cumulative Gaussians.
% FORMAT
% DESC returns the logarithm of the weighted sum of two cumulative
% Gaussians.
% ARG u1 : argument of the first cumulative Gaussian.
% ARG u2 : argument of the second cumulative Gaussian.
% ARG w1 : weight of the first cumulative Gaussian.
% ARG w2 : weight of the second cumulative Gaussian.
%
% SEEALSO : cumGaussian, lnCumGaussian, lnDiffCumGaussian
%
% COPYRIGHT : Neil D. Lawrence, 2004

% NDLUTIL

y = zeros(size(u1));
safeCond = u1 > 0 & u2 > 0;
index = find(safeCond);
if ~isempty(index)
  y(index) = log(w1.*cumGaussian(u1(index)) ...
                 + w2*cumGaussian(u2(index)));
end
index = find(~safeCond & u1>u2);
if ~isempty(index)
  y(index) = log(w1) + lnCumGaussian(u1(index))...
      + log(1 + w2/w1*exp(lnCumGaussian(u2(index))...
                          -lnCumGaussian(u1(index))));
end
index = find(~safeCond & u2>=u1);
if ~isempty(index)
  y(index) = log(w2) + lnCumGaussian(u2(index))...
      + log(1 + w1/w2*exp(lnCumGaussian(u1(index))...
                          -lnCumGaussian(u2(index))));
end


function op = prox_hinge( q , r, y)

%PROX_HINGE    Hinge-loss function.
%    OP = PROX_HINGE( q , r, y ) implements the nonsmooth function
%        OP(X) = q * sum( max( r - y.*x, 0 ) ).
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be a positive real scalar.
%    R is also optional; if omitted, R = 1 is assumed. R may be any real number.
%    Y is also optional; if omitted, Y = 1 is assumed. Y may be any scalar
%       or vector of the same size as X
% Dual: prox_hingeDual.m
%
% See also PROX_HINGEDUAL

if nargin < 3
    y = [];
elseif ~isempty(y) && ( ~isnumeric(y) || ~isreal(y) )
    error( 'Argument 3 must be a real vector');
end
if nargin < 2
	r = 1;
elseif ~isnumeric( r ) || ~isreal( r ) %|| numel( r ) ~= 1
	error( 'Argument 2 must be real.' );
end
if nargin < 1
	q = 1;
elseif ~isnumeric( q ) || ~isreal( q ) || numel( q ) ~= 1 || q <= 0,
	error( 'Argument 1 must be positive.' );
end

if isempty(y)
    op = tfocs_prox( @(x)hinge(x,r,q), @(x,t)prox_hinge(x,t,r,q) );
else
    ry  = r./y;
    op = tfocs_prox( @(x)hingeY(x,r,q,y), @(x,t)prox_hingeY(x,t,r,q,ry,y) );
end

% -- the actual functions --

function v = hingeY(x,r,q,y)
    if ~isscalar(r), assert( numel(r) == numel(x),'r is wrong size' ); end
    if ~isscalar(y), assert( numel(y) == numel(x),'y is wrong size'); end
    v = q*sum( max( r(:) - y(:).*x(:), 0 ) );
end
function v = hinge(x,r,q)
    if ~isscalar(r), assert( numel(r) == numel(x),'r is wrong size' ); end
    v = q*sum( max( r(:) - x(:), 0 ) );
end

%   PROX_F( Y, t ) = argmin_X  F(X) + 1/(2*t)*|| X - Y ||^2
function x = prox_hinge(x,t,r,q)
    tq = t * q;
    x = r + (x-r).*( x > r ) + (x + tq - r).*( x + tq < r  );
end
%{
    in the q = r = t = 1 case, the prox is:
prox(x) = {     x, if x > 1
                1, if x <= 1, and x > 0
                x + q, if x <= 0
%}

function x = prox_hingeY(x,t,r,q,ry,y)
    tq = t * q;
    x = ry + (x-ry).*( y.*x > r ) + (x + tq*y - ry).*( y.*(x + tq*y) < r );
end

end

% Added Feb, 2011; modified Dec, 2011

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


%DEMO_MODELASSESMENT1 Demonstration for model assessment with WAIC,
% DIC, number of effective parameters and ten-fold cross validation
%
%  Description
%    We will consider the regression problem in demo_regression1.
%    The analysis is conducted with full Gaussian process, and FIC
%    and PIC sparse approximations. The performance of these models
%    are compared by evaluating ten-fold cross validation,
%    leave-one-out cross-validation, WAIC, DIC and the effective
%    number of parameters. The inference will be conducted using
%    maximum a posterior (MAP) estimate for the parameters, via
%    full Markov chain Monte Carlo (MCMC) and with an integration
%    approximation (IA) for the parameters.
%
%    This demo is organised in three parts:
%     1) data analysis with full GP model
%     2) data analysis with FIC approximation
%     3) data analysis with PIC approximation
%
%  See also DEMO_REGRESSION1, DEMO_REGRESSION_SPARSE1

% Copyright (c) 2009-2010 Jarno Vanhatalo
% Copyright (c) 2010-2012 Aki Vehtari

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

%========================================================
% PART 1 data analysis with full GP model
%========================================================
disp('Full GP model with Gaussian noise model')

% Load the data
S = which('demo_regression1');
L = strrep(S,'demo_regression1.m','demodata/dat.1');
data=load(L);
x = [data(:,1) data(:,2)];
y = data(:,3);
[n, nin] = size(x);

DIC=repmat(NaN,1,9);DIC2=repmat(NaN,1,9);DIC_latent=repmat(NaN,1,9);
p_eff=repmat(NaN,1,9);p_eff2=repmat(NaN,1,9);p_eff_latent=repmat(NaN,1,9);p_eff_latent2=repmat(NaN,1,9);

% ---------------------------
% --- Construct the model ---
gpcf = gpcf_sexp('lengthScale', [1 1], 'magnSigma2', 0.2^2);
lik = lik_gaussian('sigma2', 0.2^2);
gp = gp_set('lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-9);

% -----------------------------
% --- Conduct the inference ---
%
% We will make the inference first by finding a maximum a posterior estimate
% for the parameters via gradient based optimization. After this we will
% perform an extensive Markov chain Monte Carlo sampling for the parameters.
%
disp('MAP estimate for the parameters')

% --- MAP estimate ---
%     (see gp_optim for more details)

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

% Evaluate the effective number of parameters, DIC and WAIC with focus on
% latent variables.
models{1} = 'full_MAP';
p_eff_latent = gp_peff(gp, x, y);
% For easier comparison to other methods, compute mean log
% predictive density (mlpd) instead of deviance (-2n*mlpd)
[DIC_latent, p_eff_latent2] = gp_dic(gp, x, y, 'focus', 'latent', 'output', 'mlpd');
WAICV(1) = gp_waic(gp,x,y);
WAICG(1) = gp_waic(gp,x,y, 'method', 'G');

% Evaluate the 10-fold cross-validation results.
disp('MAP estimate for the parameters - k-fold-CV')
cvres =  gp_kfcv(gp, x, y, 'display', 'fold');
mlpd_cv(1) = cvres.mlpd_cv;
rmse_cv(1) = cvres.mrmse_cv;

disp('MAP estimate for the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gp, x, y);
mlpd_loo(1) = mean(lpy);
rmse_loo(1) = sqrt(mean((y-Ef).^2));

% --- MCMC approach ---
disp('MCMC integration over the parameters')

% Do the sampling (this takes about 1 minute)
[rfull,g,opt] = gp_mc(gp, x, y, 'nsamples', 220, 'display', 20);

% After sampling delete the burn-in and thin the sample chain
rfull = thin(rfull, 21, 2);

% Evaluate the effective number of parameters, DIC and WAIC.
models{2} = 'full_MCMC';
% For easier comparison to other methods, compute mean log
% predictive density (mlpd) instead of deviance (-2n*mlpd)
[DIC(2), p_eff(2)] =  gp_dic(rfull, x, y, 'focus', 'param', 'output', 'mlpd');
[DIC2(2), p_eff2(2)] =  gp_dic(rfull, x, y, 'focus', 'all', 'output', 'mlpd');
WAICV(2) = gp_waic(rfull,x,y);
WAICG(2) = gp_waic(rfull,x,y, 'method', 'G');

% Evaluate the 10-fold cross validation results.
%
% We reduce the number of samples so that the sampling takes less time.
% 50 is too small sample size, though, and for reliable results the 10-CV
% should be run with larger sample size.
disp('MCMC integration over the parameters - k-fold-CV')
opt.nsamples= 50;
cvres =  gp_kfcv(gp, x, y, 'inf_method', 'MCMC', 'opt', opt, 'rstream', 1, 'display', 'fold');
mlpd_cv(2) = cvres.mlpd_cv;
rmse_cv(2) = cvres.mrmse_cv;

disp('MCMC integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(rfull, x, y);
mlpd_loo(2) = mean(lpy);
rmse_loo(2) = sqrt(mean((y-Ef).^2));

% --- Integration approximation approach ---
disp('Grid integration over the parameters')

gp_array = gp_ia(gp, x, y, 'int_method', 'grid');

models{3} = 'full_IA';
% For easier comparison to other methods, compute mean log
% predictive density (mlpd) instead of deviance (-2n*mlpd)
[DIC(3), p_eff(3)] =  gp_dic(gp_array, x, y, 'focus', 'param', 'output', 'mlpd');
[DIC2(3), p_eff2(3)] =  gp_dic(gp_array, x, y, 'focus', 'all', 'output', 'mlpd');
WAICV(3) = gp_waic(gp_array,x,y);
WAICG(3) = gp_waic(gp_array,x,y, 'method', 'G');

% Then the 10 fold cross-validation.
disp('Grid integration over the parameters - k-fold-CV')
clear opt
opt.int_method = 'grid';
cvres = gp_kfcv(gp, x, y, 'inf_method', 'IA', 'opt', opt, 'display', 'fold');
mlpd_cv(3) = cvres.mlpd_cv;
rmse_cv(3) = cvres.mrmse_cv;

disp('Grid integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gp_array, x, y);
mlpd_loo(3) = mean(lpy);
rmse_loo(3) = sqrt(mean((y-Ef).^2));

%========================================================
% PART 2 data analysis with FIC GP
%========================================================
disp('GP with FIC sparse approximation')

% ---------------------------
% --- Construct the model ---

% Here we conduct the same analysis as in part 1, but this time we
% use FIC approximation

% Initialize the inducing inputs in a regular grid over the input space
[u1,u2]=meshgrid(linspace(-1.8,1.8,6),linspace(-1.8,1.8,6));
X_u = [u1(:) u2(:)];

% Create the FIC GP structure
gp_fic = gp_set('type', 'FIC', 'lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-6, 'X_u', X_u)

% -----------------------------
% --- Conduct the inference ---

% --- MAP estimate using scaled conjugate gradient algorithm ---
disp('MAP estimate for the parameters')

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp_fic=gp_optim(gp_fic,x,y,'opt',opt,'optimf',@fminlbfgs);

% Evaluate the effective number of parameters and DIC with focus on
% latent variables.
models{4} = 'FIC_MAP';
p_eff_latent(4) = gp_peff(gp_fic, x, y);
[DIC_latent(4), p_eff_latent2(4)] = gp_dic(gp_fic, x, y, 'focus', 'latent', 'output', 'mlpd');
WAICV(4) = gp_waic(gp_fic,x,y);
WAICG(4) = gp_waic(gp_fic,x,y, 'method', 'G');

% Evaluate the 10-fold cross validation results.
disp('MAP estimate for the parameters - k-fold-CV')
cvres = gp_kfcv(gp_fic, x, y, 'display', 'fold');
mlpd_cv(4) = cvres.mlpd_cv;
rmse_cv(4) = cvres.mrmse_cv;

disp('MAP estimate for the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gp_fic, x, y);
mlpd_loo(4) = mean(lpy);
rmse_loo(4) = sqrt(mean((y-Ef).^2));

% --- MCMC approach ---
% (the inducing inputs are fixed)
disp('MCMC integration over the parameters')

% Do the sampling (this takes about 1 minute)
rfic = gp_mc(gp_fic, x, y, 'nsamples', 220, 'display', 20);

% After sampling we delete the burn-in and thin the sample chain
rfic = thin(rfic, 21, 2);

% Evaluate the effective number of parameters, DIC and WAIC. Note that
% the effective number of parameters as a second output, but here
% we use explicitly the gp_peff function
models{5} = 'FIC_MCMC';
[DIC(5), p_eff(5)] =  gp_dic(rfic, x, y, 'focus', 'param', 'output', 'mlpd');
[DIC2(5), p_eff2(5)] =  gp_dic(rfic, x, y, 'focus', 'all', 'output', 'mlpd');
WAICV(5) = gp_waic(rfic,x,y);
WAICG(5) = gp_waic(rfic,x,y, 'method', 'G');

% We reduce the number of samples so that the sampling takes less time.
% 50 is too small sample size, though, and for reliable results the 10-CV
% should be run with larger sample size. We also set the save option to 0.
clear opt
opt.nsamples= 50; opt.display=20;
disp('MCMC integration over the parameters - k-fold-CV')
cvres = gp_kfcv(gp_fic, x, y, 'inf_method', 'MCMC', 'opt', opt, 'display', 'fold');
mlpd_cv(5) = cvres.mlpd_cv;
rmse_cv(5) = cvres.mrmse_cv;

disp('MCMC integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(rfic, x, y);
mlpd_loo(5) = mean(lpy);
rmse_loo(5) = sqrt(mean((y-Ef).^2));

% --- Integration approximation approach ---
disp('Grid integration over the parameters')
gpfic_array = gp_ia(gp_fic, x, y, 'int_method', 'grid');

models{6} = 'FIC_IA';
[DIC(6), p_eff(6)] =  gp_dic(gpfic_array, x, y, 'focus', 'param', 'output', 'mlpd');
[DIC2(6), p_eff2(6)] =  gp_dic(gpfic_array, x, y, 'focus', 'all', 'output', 'mlpd');
WAICV(6) = gp_waic(gpfic_array,x,y);
WAICG(6) = gp_waic(gpfic_array,x,y, 'method', 'G');

% Then the 10 fold cross-validation.
disp('Grid integration over the parameters - k-fold-CV')
clear opt
opt.int_method = 'grid';
cvres = gp_kfcv(gp_fic, x, y, 'inf_method', 'IA', 'opt', opt, 'display', 'fold');
mlpd_cv(6) = cvres.mlpd_cv;
rmse_cv(6) = cvres.mrmse_cv;

disp('Grid integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gpfic_array, x, y);
mlpd_loo(6) = mean(lpy);
rmse_loo(6) = sqrt(mean((y-Ef).^2));

%========================================================
% PART 3 data analysis with PIC approximation
%========================================================
disp('GP with PIC sparse approximation')

[u1,u2]=meshgrid(linspace(-1.8,1.8,6),linspace(-1.8,1.8,6));
X_u = [u1(:) u2(:)];

% Initialize test points
[p1,p2]=meshgrid(-1.8:0.1:1.8,-1.8:0.1:1.8);
p=[p1(:) p2(:)];

% set the data points into clusters. Here we construct two cell arrays.
%  trindex  contains the block index vectors for training data. That is
%           x(trindex{i},:) and y(trindex{i},:) belong to the i'th block.
b1 = [-1.7 -0.8 0.1 1 1.9];
mask = zeros(size(x,1),size(x,1));
trindex={};
for i1=1:4
  for i2=1:4
    ind = 1:size(x,1);
    ind = ind(: , b1(i1)<=x(ind',1) & x(ind',1) < b1(i1+1));
    ind = ind(: , b1(i2)<=x(ind',2) & x(ind',2) < b1(i2+1));
    trindex{4*(i1-1)+i2} = ind';
  end
end

% Create the PIC GP structure and set the inducing inputs and block indexes
gpcf = gpcf_sexp('lengthScale', [1 1], 'magnSigma2', 0.2^2);
lik = lik_gaussian('sigma2', 0.2^2);

gp_pic = gp_set('type', 'PIC', 'lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-6, 'X_u', X_u);
gp_pic = gp_set(gp_pic, 'tr_index', trindex);

% -----------------------------
% --- Conduct the inference ---

% --- MAP estimate using scaled conjugate gradient algorithm ---
disp('MAP estimate for the parameters')

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp_pic=gp_optim(gp_pic,x,y,'opt',opt,'optimf',@fminlbfgs);

models{7} = 'PIC_MAP';
p_eff_latent(7) = gp_peff(gp_pic, x, y);
[DIC_latent(7), p_eff_latent2(7)] = gp_dic(gp_pic, x, y, 'focus', 'latent', 'output', 'mlpd');
WAICV(7) = gp_waic(gp_pic, x, y);
WAICG(7) = gp_waic(gp_pic, x, y, 'method', 'G');

% Evaluate the 10-fold cross validation results.
disp('MAP estimate for the parameters - k-fold-CV')
cvres = gp_kfcv(gp_pic, x, y, 'display', 'fold');
mlpd_cv(7) = cvres.mlpd_cv;
rmse_cv(7) = cvres.mrmse_cv;

disp('MAP estimate for the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gp_pic, x, y);
mlpd_loo(7) = mean(lpy);
rmse_loo(7) = sqrt(mean((y-Ef).^2));

% --- MCMC approach ---
disp('MCMC integration over the parameters')

% Do the sampling (this takes about 1 minute)
rpic = gp_mc(gp_pic, x, y, 'nsamples', 220, 'display', 20);

% After sampling we delete the burn-in and thin the sample chain
rpic = rmfield(rpic, 'tr_index');
rpic = thin(rpic, 21, 2);
rpic.tr_index = trindex;

% Evaluate the effective number of parameters and DIC. Note that
% the effective number of parameters as a second output, but here
% we use explicitly the gp_peff function
models{8} = 'PIC_MCMC';
[DIC(8), p_eff(8)] =  gp_dic(rpic, x, y, 'focus', 'param', 'output', 'mlpd');
[DIC2(8), p_eff2(8)] =  gp_dic(rpic, x, y, 'focus', 'all', 'output', 'mlpd');
WAICV(8) = gp_waic(rpic, x, y);
WAICG(8) = gp_waic(rpic, x, y, 'method', 'G');

% We reduce the number of samples so that the sampling takes less time.
% 50 is too small sample size, though, and for reliable results the 10-CV
% should be run with larger sample size. We also set the save option to 0.
clear opt
opt.nsamples= 50; opt.display=20;
disp('MCMC integration over the parameters - k-fold-CV')
cvres = gp_kfcv(gp_pic, x, y, 'inf_method', 'MCMC', 'opt', opt, 'display', 'fold');
mlpd_cv(8) = cvres.mlpd_cv;
rmse_cv(8) = cvres.mrmse_cv;

disp('MCMC integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(rpic, x, y);
mlpd_loo(8) = mean(lpy);
rmse_loo(8) = sqrt(mean((y-Ef).^2));

% --- Integration approximation approach ---
disp('Grid integration over the parameters')

gppic_array = gp_ia(gp_pic, x, y, 'int_method', 'grid');

models{9} = 'PIC_IA';
[DIC(9), p_eff(9)] =  gp_dic(gppic_array, x, y, 'focus', 'param', 'output', 'mlpd');
[DIC2(9), p_eff2(9)] =  gp_dic(gppic_array, x, y, 'focus', 'all', 'output', 'mlpd');
WAICV(9) = gp_waic(gppic_array, x, y);
WAICG(9) = gp_waic(gppic_array, x, y, 'method', 'G');

% Then the 10 fold cross-validation.
disp('Grid integration over the parameters - k-fold-CV')
clear opt
opt.int_method = 'grid';
cvres = gp_kfcv(gp_pic, x, y, 'inf_method', 'IA', 'opt', opt, 'display', 'fold');
mlpd_cv(9) = cvres.mlpd_cv;
rmse_cv(9) = cvres.mrmse_cv;

disp('Grid integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gppic_array, x, y);
mlpd_loo(9) = mean(lpy);
rmse_loo(9) = sqrt(mean((y-Ef).^2));

%========================================================
% PART 4 Print the results
%========================================================
disp('Summary of the results')
S = '      ';
for i = 1:length(models)
    S = [S '  ' models{i}];
end

S = sprintf([S '\n CV-mlpd  %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], mlpd_cv);
S = sprintf([S '\n CV-rmse  %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], rmse_cv);
S = sprintf([S '\n LOO-mlpd %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], mlpd_loo);
S = sprintf([S '\n LOO-rmse %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], rmse_loo);
S = sprintf([S '\n ']);
S = sprintf([S '\n WAIC_V   %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], WAICV);
S = sprintf([S '\n WAIC_G   %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], WAICG);
S = sprintf([S '\n ']);
S = sprintf([S '\n DIC_h    %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], DIC);
S = sprintf([S '\n DIC_a    %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], DIC2);
S = sprintf([S '\n DIC_l    %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], DIC_latent);
S = sprintf([S '\n peff_h   %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], p_eff);
S = sprintf([S '\n peff_a   %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], p_eff2);
S = sprintf([S '\n peff_l   %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], p_eff_latent);
S = sprintf([S '\n peff_l2  %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], p_eff_latent2);
S = sprintf([S '\n ']);
S = sprintf([S '\n ']);
S = sprintf([S '\n The notation is as follows:']);
S = sprintf([S '\n CV-mlpd  = mean log predictive density from the 10-fold CV. ']);
S = sprintf([S '\n CV-rmse  = root mean squared error from the 10-fold LOO-CV. ']);
S = sprintf([S '\n LOO-mlpd = mean log predictive density from the LOO-CV. ']);
S = sprintf([S '\n LOO-rmse = root mean squared error from the 10-fold CV. ']);
S = sprintf([S '\n WAIC_V   = WAIC via variance method ']);
S = sprintf([S '\n WAIC_G   = WAIC via Gibbs training utility method ']);
S = sprintf([S '\n DIC_h    = DIC with focus on parameters. ']);
S = sprintf([S '\n DIC_a    = DIC with focus on parameters and latent variables (all). ']);
S = sprintf([S '\n DIC_l    = DIC with focus on latent variables. ']);
S = sprintf([S '\n peff_h   = effective number of parameters (latent variables marginalized). ']);
S = sprintf([S '\n peff_a   = effective number of parameters and latent variables. ']);
S = sprintf([S '\n peff_l   = effective number of latent variables evaluated with gp_peff. ']);
S = sprintf([S '\n peff_l2  = effective number of latent variables evaluated with gp_dic. ']);
S = sprintf([S '\n '])


function [x cutvalue cutvalue_upperbound Y] = maxcut(L, r)
% Algorithm to (try to) compute a maximum cut of a graph, via SDP approach.
%
% function x = maxcut(L)
% function [x cutvalue cutvalue_upperbound Y] = maxcut(L, r)
%
% L is the Laplacian matrix describing the graph to cut. The Laplacian of a
% graph is L = D - A, where D is the diagonal degree matrix (D(i, i) is the
% sum of the weights of the edges adjacent to node i) and A is the
% symmetric adjacency matrix of the graph (A(i, j) = A(j, i) is the weight
% of the edge joining nodes i and j). If L is sparse, this will be
% exploited.
%
% If the graph has n nodes, then L is nxn and the output x is a vector of
% length n such that x(i) is +1 or -1. This partitions the nodes of the
% graph in two classes, in an attempt to maximize the sum of the weights of
% the edges that go from one class to the other (MAX CUT problem).
%
% cutvalue is the sum of the weights of the edges 'cut' by the partition x.
%
% If the algorithm reached the global optimum of the underlying SDP
% problem, then it produces an upperbound on the maximum cut value. This
% value is returned in cutvalue_upperbound if it is found. Otherwise, that
% output is set to NaN.
%
% If r is specified (by default, r = n), the algorithm will stop at rank r.
% This may prevent the algorithm from reaching a globally optimal solution
% for the underlying SDP problem (but can greatly help in keeping the
% execution time under control). If a global optimum of the SDP is reached
% before rank r, the algorithm will stop of course.
%
% Y is a matrix of size nxp, with p <= r, such that X = Y*Y' is the best
% solution found for the underlying SDP problem. If cutvalue_upperbound is
% not NaN, then Y*Y' is optimal for the SDP and cutvalue_upperbound is its
% cut value.
%
% By Goemans and Williamson 1995, it is known that if the optimal value of
% the SDP is reached, then the returned cut, in expectation, is at most at
% a fraction 0.878 of the optimal cut. (This is not exactly valid because
% we do not use random projection here; sign(Y*randn(size(Y, 2), 1)) will
% give a cut that respects this statement -- it's usually worse though).
%
% The algorithm is essentially that of:
% Journee, Bach, Absil and Sepulchre, 2010
% Low-rank optimization on the code of positive semidefinite matrices.
%
% It is itself based on the famous SDP relaxation of MAX CUT:
% Goemans and Williamson, 1995
% Improved approximation algorithms for maximum cut and satisfiability
% problems using semidefinite programming.

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, July 18, 2013
% Contributors:
%
% Change log:
%

    % If no inputs are provided, generate a random Laplacian.
    % This is for illustration purposes only.
    if ~exist('L', 'var') || isempty(L)
        n = 20;
        A = triu(randn(n) <= .4, 1);
        A = A+A';
        D = diag(sum(A, 2));
        L = D-A;
    end

    n = size(L, 1);
    assert(size(L, 2) == n, 'L must be square.');

    if ~exist('r', 'var') || isempty(r) || r > n
        r = n;
    end

    % We will let the rank increase. Each rank value will generate a cut.
    % We have to go up in the rank to eventually find a certificate of SDP
    % optimality. This in turn will give us an upperbound on the MAX CUT
    % value and assure us that we're doing well, according to Goemans and
    % Williamson's argument. In practice though, the good cuts often come
    % up for low rank values, so we better keep track of the best one.
    best_x = ones(n, 1);
    best_cutvalue = 0;
    cutvalue_upperbound = NaN;

    time = [];
    cost = [];

    for rr = 2 : r

        manifold = elliptopefactory(n, rr);

        if rr == 2

            % At first, for rank 2, generate a random point.
            Y0 = manifold.rand();

        else

            % To increase the rank, we could just add a column of zeros to
            % the Y matrix. Unfortunately, this lands us in a saddle point.
            % To escape from the saddle, we may compute an eigenvector of
            % Sy associated to a negative eigenvalue: that will yield a
            % (second order) descent direction Z. See Journee et al ; Sy is
            % linked to dual certificates for the SDP.
            Y0 = [Y zeros(n, 1)];
            LY0 = L*Y0;
            Dy = spdiags(sum(LY0.*Y0, 2), 0, n, n);
            Sy = (Dy - L)/4;
            % Find the smallest (the "most negative") eigenvalue of Sy.
            [v, s] = eigs(Sy, 1, 'SA');
            % If there is no negative eigenvalue for Sy, than we are not at
            % a saddle point: we're actually done!
            if s >= -1e-8
                % We can stop here: we found the global optimum of the SDP,
                % and hence the reached cost is a valid upper bound on the
                % maximum cut value.
                cutvalue_upperbound = max(-[info.cost]);
                break;
            end

            % This is our escape direction.
            Z = manifold.proj(Y0, [zeros(n, rr-1) v]);

            % % These instructions can be uncommented to see what the cost
            % % function looks like at a saddle point. But will require the
            % % problem structure which is not defined here: see the helper
            % % function.
            % plotprofile(problem, Y0, Z, linspace(-1, 1, 101));
            % drawnow; pause;

            % Now make a step in the Z direction to escape from the saddle.
            % It is not obvious that it is ok to do a unit step ... perhaps
            % need to be cautious here with the stepsize. It's not too
            % critical though: the important point is to leave the saddle
            % point. But it's nice to guarantee monotone decrease of the
            % cost, and we can't do that with a constant step (at least,
            % not without a proper argument to back it up).
            stepsize = 1;
            Y0 = manifold.retr(Y0, Z, stepsize);

        end

        % Use the Riemannian optimization based algorithm lower in this
        % file to reach a critical point (typically a local optimizer) of
        % the max cut cost with fixed rank, starting from Y0.
        [Y info] = maxcut_fixedrank(L, Y0);

        % Some info logging.
        thistime = [info.time];
        if ~isempty(time)
            thistime = time(end) + thistime;
        end
        time = [time thistime]; %#ok<AGROW>
        cost = [cost [info.cost]]; %#ok<AGROW>

        % Time to turn the matrix Y into a cut.
        % We can either do the random rounding as follows:
        % x = sign(Y*randn(rr, 1));
        % or extract the "PCA direction" of the points in Y and cut
        % orthogonally to that direction, as follows:
        [u, ~, ~] = svds(Y, 1);
        x = sign(u);

        cutvalue = (x'*L*x)/4;
        if cutvalue > best_cutvalue
            best_x = x;
            best_cutvalue = cutvalue;
        end

    end

    x = best_x;
    cutvalue = best_cutvalue;

    plot(time, -cost, '.-');
    xlabel('Time [s]');
    ylabel('Relaxed cut value');
    title('The relaxed cut value is an upper bound on the optimal cut value.');

end

function [Y info] = maxcut_fixedrank(L, Y)
% Try to solve the (fixed) rank r relaxed max cut program, based on the
% Laplacian of the graph L and an initial guess Y. L is nxn and Y is nxr.

    [n r] = size(Y);
    assert(all(size(L) == n));

    % The fixed rank elliptope geometry describes symmetric, positive
    % semidefinite matrices of size n with rank r and all diagonal entries
    % are 1.
    manifold = elliptopefactory(n, r);

    % % If you want to compare the performance of the elliptope geometry
    % % against the (conceptually simpler) oblique manifold geometry,
    % % uncomment this line.
    % manifold = obliquefactory(r, n, true);

    problem.M = manifold;

    % % For rapid prototyping, these lines suffice to describe the cost
    % % function and its gradient and Hessian (here expressed using the
    % % Euclidean gradient and Hessian).
    % problem.cost = @(Y)  -trace(Y'*L*Y)/4;
    % problem.egrad = @(Y) -(L*Y)/2;
    % problem.ehess = @(Y, U) -(L*U)/2;

    % Instead of the prototyping version, the functions below describe the
    % cost, gradient and Hessian using the caching system (the store
    % structure). This alows to execute exactly the required number of
    % multiplications with the matrix L. These multiplications are counted
    % using the Lproducts_counter and registered for each iteration in the
    % info structure outputted by solvers, via the statsfun function.
    % Notice that we do not use the store structure to count: this does not
    % behave well in general and is not advised.

    Lproducts_counter = 0;

    % For every visited point Y, we will need L*Y. This function makes sure
    % the quantity L*Y is available, but only computes it if it wasn't
    % already computed.
    function store = prepare(Y, store)
        if ~isfield(store, 'LY')
            store.LY = L*Y;
            Lproducts_counter = Lproducts_counter + 1;
        end
    end

    problem.cost = @cost;
    function [f store] = cost(Y, store)
        store = prepare(Y, store);
        LY = store.LY;
        f = -(Y(:)'*LY(:))/4; % = -trace(Y'*LY)/4;
    end

    problem.grad = @grad;
    function [g store] = grad(Y, store)
        store = prepare(Y, store);
        LY = store.LY;
        g = manifold.egrad2rgrad(Y, -LY/2);
    end

    problem.hess = @hess;
    function [h store] = hess(Y, U, store)
        store = prepare(Y, store);
        LY = store.LY;
        LU = L*U;
        Lproducts_counter = Lproducts_counter + 1;
        h = manifold.ehess2rhess(Y, -LY/2, -LU/2, U);
    end

    % statsfun is called exactly once after each iteration (including after
    % the evaluation of the cost at the initial guess). We then register
    % the value of the Lproducts counter (which counts how many product
    % were needed since the last iteration), and reset it to zero.
    options.statsfun = @statsfun;
    function stats = statsfun(problem, Y, stats, store) %#ok
        stats.Lproducts = Lproducts_counter;
        Lproducts_counter = 0;
    end

    % % Diagnostics tools: to make sure the gradient and Hessian are
    % % correct during the prototyping stage.
    % checkgradient(problem); pause;
    % checkhessian(problem); pause;

    % % To investigate the effect of the rotational invariance when using
    % % the oblique or the elliptope geometry, or to study the saddle point
    % % issue mentioned above, it is sometimes interesting to look at the
    % % spectrum of the Hessian. For large dimensions, this is slow!
    % stairs(sort(hessianspectrum(problem, Y)));
    % drawnow; pause;

    % % When facing a saddle point issue as described in the master
    % % function, and when no sure mechanism exists to find an escape
    % % direction, it may be helpful to set useRand to true and raise
    % % miniter to more than 1, when using trustregions. This will tell the
    % % solver to not stop before at least miniter iterations were
    % % accomplished (thus disregarding the zero gradient at the saddle
    % % point) and to use random search directions to kick start the inner
    % % solve (tCG) step. It is not as efficient as finding a sure escape
    % % direction, but sometimes it's the best we have.
    % options.useRand = true;
    % options.miniter = 5;

    options.verbosity = 2;
    Lproducts_counter = 0;
    [Y Ycost info] = trustregions(problem, Y, options); %#ok

    % fprintf('Products with L: %d\n', sum([info.Lproducts]));

end


function [min_width, min_angle] = minimumCaliperDiameter(points)
%MINIMUMCALIPERDIAMETER Minimum caliper diameter of a set of points.
%
%   WIDTH = minimumCaliperDiameter(POINTS)
%   Computes the minimum width of a set of points. As polygons and
%   polylines are represented as point lists, this function works also for
%   polygons and polylines.
%
%   [WIDTH THETA] = minimumCaliperDiameter(POINTS)
%   Also returns the direction of minimum width. The direction corresponds
%   to the horizontal angle of the edge that minimizes the width. THETA is
%   given in radians, between 0 and PI.
%
%
%   Example
%   % Compute minimal caliper diameter, and check coords of rotated points
%   % have expected extent
%     points = randn(30, 2);
%     [width theta] = minimumCaliperDiameter(points);
%     points2 = transformPoint(points, createRotation(-theta));
%     diff = max(points2) - min(points2);
%     abs(width - diff(2)) < 1e-10
%     ans =
%         1
%
%   References
%   Algorithms use rotating caliper. Implementation was based on that of
%   Wikipedia:
%   http://en.wikipedia.org/wiki/Rotating_calipers
%
%   See also
%   polygons2d, convexHull, orientedBox
%

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

% first, compute convex hull of the polygon
inds = convhull(points(:,1), points(:,2));
hull = points(inds, :);

% if first and last points are the same, remove the last one
if inds(1) == inds(end)
    hull = hull(1:end-1, :);
end

% number of hull vertices
nV = size(hull, 1);

% default values
rotated_angle = 0;
min_width = inf;
min_angle = 0;

% avoid degenerated cases
if nV < 3
    return;
end

[tmp, p_a] = min(hull(:, 2)); %#ok<ASGLU>
[tmp, p_b] = max(hull(:, 2)); %#ok<ASGLU>

caliper_a = [ 1 0];    % Caliper A points along the positive x-axis
caliper_b = [-1 0];    % Caliper B points along the negative x-axis

while rotated_angle < pi
    % compute the direction vectors corresponding to each edge
    ind_a2 = mod(p_a, nV) + 1;
    vector_a = hull(ind_a2, :) - hull(p_a, :);

    ind_b2 = mod(p_b, nV) + 1;
    vector_b = hull(ind_b2, :) - hull(p_b, :);

    % Determine the angle between each caliper and the next adjacent edge
    % in the polygon
    angle_a = vectorAngle(caliper_a, vector_a);
    angle_b = vectorAngle(caliper_b, vector_b);

    % Determine the smallest of these angles
    minAngle = min(angle_a, angle_b);

    % Rotate the calipers by the smallest angle
    caliper_a = rotateVector(caliper_a, minAngle);
    caliper_b = rotateVector(caliper_b, minAngle);

    rotated_angle = rotated_angle + minAngle;

    % compute current width, and update opposite vertex
    if angle_a < angle_b
        line = createLine(hull(p_a, :), hull(ind_a2, :));
        width = distancePointLine(hull(p_b, :), line);
        p_a = mod(p_a, nV) + 1;

    else
        line = createLine(hull(p_b, :), hull(ind_b2, :));
        width = distancePointLine(hull(p_a, :), line);
        p_b = mod(p_b, nV) + 1;

    end

    % update minimum width and corresponding angle if needed
    if width < min_width
        min_width = width;
        min_angle = rotated_angle;
    end

end


%% Using Cook's Distance to Detect Outliers
% Copyright (c) 2011, The MathWorks, Inc.

% Create a vector of X values
clear all
clc
hold off

X = 1:100;
X = X';

% Create a noise vector
noise = randn(100,1);

% Create a second noise value where sigma is much larger
noise2 = 10*randn(100,1);

% Substitute noise2 for noise1 at obs# (11, 31, 51, 71, 91)
% Many of these points will have an undue influence on the model

noise(11:20:91) = noise2(11:20:91);

% Specify Y = F(X)
Y = 3*X + 2 + noise;

% Cook's Distance for a given data point measures the extent to
% which a regression model would change if this data point
% were excluded from the regression. Cook's Distance is
% sometimes used to suggest whether a given data point might be an outlier.

% Use regstats to calculate Cook's Distance
stats = regstats(Y,X,'linear');

% if Cook's Distance > n/4 is a typical treshold that is used to suggest
% the presence of an outlier
potential_outlier = stats.cookd > 4/length(X);

% Display the index of potential outliers and graph the results
X(potential_outlier)
scatter(X,Y, 'b.')
hold on
scatter(X(potential_outlier),Y(potential_outlier), 'r.')


function [gJacobMat] = gJacob(p_pc_c, calibParams)
%GJACOBMAT Returns the 4x3 Jacobian of the observation model with respect
%to the feature position in the camera frame

%Redefine local shorthand notation
x = p_pc_c(1);
y = p_pc_c(2);
z = p_pc_c(3);

c_u = calibParams.c_u;
c_v = calibParams.c_v;
f_u = calibParams.f_u;
f_v = calibParams.f_v;
b = calibParams.b;

%Evaluate the Jacobian
gJacobMat = [f_u/z, 0, -(1/z^2)*(f_u*x )
             0, f_v/z, -(1/z^2)*(f_v*y )
             f_u/z, 0, -(1/z^2)*(f_u*(x-b) )
             0, f_v/z, -(1/z^2)*(f_v*y )];
end


function [ x, y, z, w ] = ld0014 ( )

%*****************************************************************************80
%
%% LD0014 computes the 14 point Lebedev angular grid.
%
%  Modified:
%
%    14 September 2010
%
%  Author:
%
%    Dmitri Laikov
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Output, real X(N), Y(N), Z(N), W(N), the coordinates
%    and weights of the points.
%
  n = 0;
  x = zeros(14,1);
  y = zeros(14,1);
  z = zeros(14,1);
  w = zeros(14,1);
  a = 0.0;
  b = 0.0;
  v = 0.6666666666666667E-01;
  [ n, x, y, z, w ] = gen_oh ( 1, n, a, b, v, x, y, z, w );
  v = 0.7500000000000000E-01;
  [ n, x, y, z, w ] = gen_oh ( 3, n, a, b, v, x, y, z, w );

  return
end


function value = nco_abscissa ( order, i )

%*****************************************************************************80
%
%% NCO_ABSCISSA returns the I-th abscissa for the Newton Cotes open rule.
%
%  Discussion:
%
%    Our convention is that the abscissas are numbered from left to
%    right.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ORDER, the order of the rule.
%    1 <= ORDER.
%
%    Input, integer I, the index of the desired abscissa.
%    1 <= I <= ORDER.
%
%    Output, real VALUE, the value of the I-th
%    abscissa in the Newton Cotes open rule of order ORDER.
%
  x_min = -1.0;
  x_max = +1.0;

  if ( order < 1 )
    value = - Inf;
    return
  end

  if ( i < 1 | order < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'NCO_ABSCISSA - Fatal error!\n' );
    fprintf ( 1, '  1 <= I <= ORDER is required.\n' );
    error ( 'NCO_ABSCISSA - Fatal error!' );
  end

  value = ( ( order - i + 1 ) * x_min   ...
          + (         i     ) * x_max ) ...
          / ( order     + 1 );

  return
end