function [Y] = mnbinpdf(X, W, K)
%MNBINPDF       Maszle's negative binomial probability density function
%
%  Y = MNBINPDF(X, W, K)
%
%  returns the probabilities of the values X using a negative binomial
%  distribution with mean W and clumping parameter K.
%
%  This is an alternate form of the negative binomial commonly
%  employed in biology to describe aggregated count data.
%  W and K are related to the traditional parametrization by the
%  relationships:
%
%      P = K/(K + W),  and K = R,
%
%  with the distinction that K is any positive real number, not just
%  positive integers.
%
%  X should only be integers and is rounded if not.  Calculation is
%  optimized by taking the exponential of the GAMMALN function.
%
%  See also MNBINRND

%----------------------------------------------------------------------
% Copyright (c) 1997.  Don R. Maszle.  All rights reserved.
%
%   -- Revisions -----
%      Author:  Don R. Maszle
%      E-mail:  maze@sparky.berkeley.edu
%   -- SCCS  ---------
%----------------------------------------------------------------------

%----------------------------------------------------------------------
% We use the definition from Bradley and May, Trans. R.Soc.Trop.Med.Hyg.
% v72(3), 1978, p262.

% A notational convenience
a = W/(W + K);
X = round(X);

Y = ((1 - a).^K) ...
    .* (exp( gammaln(X + K) + X.*log(a) - gammaln(K) - gammaln(X + 1)));


%dcm (Cba) to rotation vector (r^a)
%Cba: B to A transformation matrix
%r^a=Rotation vector defined in A. (Rotate A around r with mag(r) to obtain B)
%the magnitude of the returning rotation vector will be between [0 and PI]

%This is the implementaion of savage(3-31).
function rvec=dcm2rot(dcm)

sinPHI=0.5*sqrt((dcm(3,2)-dcm(2,3))^2+(dcm(1,3)-dcm(3,1))^2+(dcm(2,1)-dcm(1,2))^2);
cosPHI=0.5*(dcm(1,1)+dcm(2,2)+dcm(3,3)-1);

PHI=atan2(sinPHI,cosPHI);

if (cosPHI>=0)
    if (sinPHI==0)
        F=1;
    else
        F=PHI/sinPHI;
    end
    rvec=0.5*F*[dcm(3,2)-dcm(2,3);dcm(1,3)-dcm(3,1);dcm(2,1)-dcm(1,2)];
else
    sr_a=1-cosPHI;
    mu1=sqrt((dcm(1,1)-1)/sr_a+1);
    mu2=sqrt((dcm(2,2)-1)/sr_a+1);
    mu3=sqrt((dcm(3,3)-1)/sr_a+1);
    if (mu1>=mu2 && mu1>=mu3)
       u1=mu1*sign(dcm(3,2)-dcm(2,3));
       u2=1/2/u1*(dcm(1,2)+dcm(2,1))/sr_a;
       u3=1/2/u1*(dcm(1,3)+dcm(3,1))/sr_a;
    elseif (mu2>=mu3)
       u2=mu2*sign(dcm(1,3)-dcm(3,1));
       u3=1/2/u2*(dcm(2,3)+dcm(3,2))/sr_a;
       u1=1/2/u2*(dcm(1,2)+dcm(2,1))/sr_a;
    else
       u3=mu3*sign(dcm(2,1)-dcm(1,2));
       u1=1/2/u3*(dcm(1,3)+dcm(3,1))/sr_a;
       u2=1/2/u3*(dcm(2,3)+dcm(3,2))/sr_a;
    end
    rvec=PHI*[u1;u2;u3];
end


function [p]=rmEvaluate2Fits(rss1,rss2,df1,df2,n,units,wtest)
% rmEvaluate2Fits - probability that fit1 == fit2
%
% output = rmEvaluate2Fits(rss1,rss2,df1,df2,n,units,wtest);
%
% Performs f-test and general likelihood ratio (Wilks likelihood)
% tests. Outputs the probability of Ho: data1 == data2.
% Assumes df1>=df2.
%
% rss  : residual sum of squares (model 1 and 2)
% df   : degrees of freedom      (model 1 and 2)
% n    : number of data points
% units: 'p', 'log10p' ['p']
% wtest: 'ftest','glr'
%
% 2006/02 SOD: wrote it.

if ieNotDefined('rss1'), error('Need rss1.'); end;
if ieNotDefined('rss2'), error('Need rss2.'); end;
if ieNotDefined('df1'),  error('Need df1.');  end;
if ieNotDefined('df2'),  error('Need df2.');  end;
if ieNotDefined('n'),    error('Need n.');    end;
if ieNotDefined('units'),wtest = 'p';         end;
if ieNotDefined('wtest'),wtest = 'glr';       end;

% some sanity checks
if df1 == df2,
  disp(sprintf('[%s]:error: df1 == df2.',mfilename));
  return;
end;
if df1<df2,
  disp(sprintf('[%s]:warning: df1>df2 flipping datasets!',mfilename));
  dftmp = df1;
  df1   = df2;
  df2   = dftmp;
  rsstmp = rss1;
  rss1   = rss2;
  rss2   = rsstmp;
end;

% now do tests
switch lower(wtest),
 case {'f','ftest'}
  % ftest
  if df1~=df2,
    f = ((rss1-rss2)./rss2) ./ ((df1-df2)./df2);
    p = 1-fcdf(f,df1-df2,df2);
  else
    f = rss1./rss2;
    p = 1-fcdf(f,df2,df1);
  end;

 case {'g','glr','wilks'}
  % From Fan et al (2001) Generalized likelihood ratio statistics and Wilks
  % phenomenon. The Annals of Statistics. 29(1): 153-193.
  % calculate lambda (glr computation)
  lambda = (n./2).*log(rss1./rss2);

  % probability function: Wilks phenomenon lambda approximates
  % chi-squared distribution.
  df = df1-df2;
  p = 1-chi2cdf(lambda,df);
end;

% convert units if requested
switch lower(units),
 case 'log10p',
  sp         = sign(p);
  sp(sp==0)  = 1;
  p          = abs(p);
  p(p<1e-50) = 10^-50; % remove 0
  p          = -log10(p) .* sp;
 case 'p',
  % do nothing
 otherwise,
  error('[%s]:Unknown unit: %s',mfilename,units);
end;

return;


function chebyshev_polynomial_test09 ( )

%*****************************************************************************80
%
%% CHEBYSHEV_POLYNOMIAL_TEST09 compares a function and projection over [-1,+1].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBYSHEV_POLYNOMIAL_TEST09:\n' );
  fprintf ( 1, '  T_PROJECT_COEFFICIENTS computes the Chebyshev interpolant C(F)(N,X)\n' );
  fprintf ( 1, '  of a function F(X) defined over [-1,+1].\n' );
  fprintf ( 1, '  T_PROJECT_VALUE evaluates that projection.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compute projections of order N to exp(x) over [-1,+1],\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   N   Max||F(X)-C(F)(N,X)||\n' );
  fprintf ( 1, '\n' );
  for n = 0 : 10
    c = t_project_coefficients ( n, @exp );
    m = 101;
    x = ( linspace ( -1.0, +1.0, m ) )';
    v = t_project_value ( m, n, x, c );
    r = v - exp ( x );
    fprintf ( 1, '  %2d  %12.4g\n', n, max ( abs ( r ) ) );
  end

  return
end


function u = bsv ( a, b, alpha, beta, nu, n, output )

%*****************************************************************************80
%
%% BSV applies Newton's method to a discretized steady viscous Burgers equation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the left and right endpoints.
%
%    Input, real ALPHA, BETA, the Dirichlet boundary values at the left
%    and right.
%
%    Input, real NU, the viscosity.  Normally, 0 < NU.
%
%    Input, integer N, the number of nodes to use between A and B.
%
%    Input, logical OUTPUT, is TRUE if printout is desired.
%
%    Output, real U(N), the computed discretized solution.
%
  if ( n < 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BSV - Fatal error!\n' );
    fprintf ( 1, '  N < 2.\n' );
    error ( 'BSV - Fatal error!\n' );
  end
%
%  Set some iteration parameters.
%
  newton_step_max = 50;
  newton_resid_tol = sqrt ( eps );
  newton_step_tol = sqrt ( eps );
%
%  Use equally spaced nodes from A to B.
%
  dx = ( b - a ) / ( n - 1 );
%
%  The initial guess will be the linear interpolant to the boundary conditions.
%
  u = ( linspace ( alpha, beta, n ) )';
%
%  Prepare for the Newton iteration.
%
  J = sparse ( [], [], [], n, n, 3 * n );
  newton_step = 0;

  if ( output )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Step  ||F(U)||\n' );
    fprintf ( 1, '\n' );
  end

  while ( newton_step <= newton_step_max )

    f(1,1) = u(1) - alpha;

%   for i = 2 : n - 1
%     f(i,1) = 0.5 * ( u(i+1)^2 - u(i-1)^2 ) / ( 2.0 * dx ) ...
%          - nu * ( u(i+1) - 2.0 * u(i) + u(i-1) ) / ( dx^2 );
%   end

    f(2:n-1,1) = 0.5 * ( u(3:n).^2 - u(1:n-2).^2 ) / ( 2.0 * dx ) ...
           - nu * ( u(3:n) - 2.0 * u(2:n-1) + u(1:n-2) ) / ( dx^2 );

    f(n,1) = u(n) - beta;

    f_norm = norm ( f, inf );

    if ( output )
      fprintf ( 1, '  %4d  %g\n', newton_step, f_norm );
    end

    if ( f_norm < newton_resid_tol )
      break
    end
%
%  Define the Jacobian matrix.
%
    J(1,1) = 1.0;
    for i = 2 : n - 1
      J(i,i-1) = - 2.0 * u(i-1) / ( 4.0 * dx )       - nu / dx^2;
      J(i,i)   =                                 2.0 * nu / dx^2;
      J(i,i+1) =   2.0 * u(i+1) / ( 4.0 * dx )       - nu / dx^2;
    end
    J(n,n) = 1.0;
%
%  Solve the linear system J * du = -f to get the Newton update.
%
    du = J \ f;
    du_norm = norm ( du, inf );

    u_norm = norm ( u, inf );
    if ( du_norm < newton_step_tol * ( u_norm + 1.0 ) )
      break
    end

    u = u - du;
    newton_step = newton_step + 1;

  end

  if ( newton_step_max < newton_step )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BSV - Warning!\n' );
    fprintf ( 1, '  The Newton iteration did not converge.\n' );
  end

  return
end


function b = center_logistic(u,y);
s = mean(y);
b = log( s / (1 - s ) ) - mean(u);
for i=1:20
	sigma = 1./ ( 1 + exp( - u - b ) );
	phider = sum(sigma - y) ;
	phider2 = sum(sigma .* ( 1- sigma));
	bnew = b - phider / phider2;
	if norm(bnew-b) < 1e-16, break; end
	b=bnew;

end


function point = pointOnLine(line, pos)
%POINTONLINE Create a point on a line at a given position on the line
%
%   P = pointOnLine(LINE, POS);
%   Creates the point belonging to the line LINE, and located at the
%   distance D from the line origin.
%   LINE has the form [x0 y0 dx dy].
%   LINE and D should have the same number N of rows. The result will have
%   N rows ans 2 column (x and y positions).
%
%   See also:
%   lines2d, points2d, onLine, onLine, linePosition
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 07/04/2004.
%

angle = lineAngle(line);
point = [line(:,1) + pos .* cos(angle), line(:,2) + pos .* sin(angle)];


%% MULTIGRID OF FOR THE STOKES EQNS IN 2D
%
% This example is to show the convergence of multigrid methods for various
% finite element approximation of the Stokes equation on the unit square:
%
%       -div(mu*grad u) + grad p = f in \Omega,
%                        - div u = 0  in \Omega,
%                              u = g_D  on \Gamma.
%
% with the pure Dirichlet boundary condition.
%

%% Setting
clear all; close all;
[node,elem] = squaremesh([0,1,0,1],0.5);
% [node,elem] = circlemesh(0,0,1,0.25);
[node,elem] = uniformrefine(node,elem);
showmesh(node,elem);
pde = Stokesdata1;
%  pde = StokesZulehnerdata;
bdFlag = setboundary(node,elem,'Dirichlet');
option.L0 = 1;
option.maxIt = 4;
option.printlevel = 1;
option.plotflag = 0;
option.rateflag = 0;

%% MG options
option.solver = 'mg';
option.smoothingstep = 2;
option.smootherbarSp = 'SGS';

%% RT0-P0
display('RT0-P0')
option.elemType = 'RT0-P0';
option.refType = 'bisect';
femStokesHdiv(node,elem,pde,bdFlag,option);

% %% RT0-P0
% display('BDM1B-P0')
% option.elemType = 'BDM1B-P0';
% femStokesHdiv(node,elem,pde,bdFlag,option);

%% CR-P0 element
display('CR-P0')
option.elemType = 'CRP0';
femStokes(node,elem,pde,bdFlag,option);

%% P2-P0 element
display('P2-P0')
option.elemType = 'P2P0';
femStokes(node,elem,pde,bdFlag,option);

%% isoP2-P0 element
display('isoP2-P0')
option.elemType = 'isoP2P0';
femStokes(node,elem,pde,bdFlag,option);

%% isoP2-P1 element
display('isoP2-P1')
option.elemType = 'isoP2P1';
femStokes(node,elem,pde,bdFlag,option);

% %% P1b-P1 element
% display('P1b-P0')
% option.elemType = 'P1bP1';
% femStokes(node,elem,pde,bdFlag,option);

%% P2-P1 element
display('P2-P1')
option.elemType = 'P2P1';
% option.smoothingStep = 3;
% option.smootherbarSp   = 'VCYCLE';
% option.smootherbarSpPara = 0.75;
femStokes(node,elem,pde,bdFlag,option);


function [param]=sine_fit(x,y,fixed_params,initial_params,plot_flag)
% Optimization of parameters of the sine function to time series
%
% Syntax:
%       [param]=sine_fit(x,y)
%
%       that is the same that
%       [param]=sine_fit(x,y,[],[],[])     % no fixed_params, automatic initial_params
%
%       [param]=sine_fit(x,y,fixed_params)        % automatic initial_params
%       [param]=sine_fit(x,y,[],initial_params)   % use it when the estimation is poor
%       [param]=sine_fit(x,y,fixed_params,initial_params,plot_flag)
%
% param = [offset, amplitude, phaseshift, frequency]
%
% if fixed_params=[NaN, NaN , NaN , NaN]        % or fixed_params=[]
% optimization of offset, amplitude, phase shift and frequency (default)
%
% if fixed_params=[NaN, 1 , NaN , 1/(2*pi)]
% optimization of offset and phase shift of a sine of amplitude=1 and frequency=1/(2*pi)
%
% if fixed_params=[0, NaN , NaN , NaN]
% optimization of amplitude, phase shift and frequency of a sine of offset=0
%
%
% Example:
% %% generate data vectors (x and y)
% fsine = @(param,timeval) param(1) + param(2) * sin( param(3) + 2*pi*param(4)*timeval );
% param=[0 1 0 1/(2*pi)];  % offset, amplitude, phaseshift, frequency
% timevec=0:0.1:10*pi;
% x=timevec;
% y=fsine(param,timevec) + 0.1*randn(size(x));
%
% %% standard parameter estimation
% [estimated_params]=sine_fit(x,y)
%
% %% parameter estimation with forced 1.5 fixed amplitude
% [estimated_params]=sine_fit(x,y,[NaN 1.5 NaN NaN])
%
% %% parameter estimation without plotting
% [estimated_params]=sine_fit(x,y,[],[],0)
%
%
% Doubts, bugs: rpavao@gmail.com

% warning off

if nargin<=1 %fail
    fprintf('');
    help sine_fit
    return
end

automatic_initial_params=NaN(1,4);
automatic_initial_params(1)=median(y);
automatic_initial_params(2)=mean(minmax(y));
automatic_initial_params(3)=0;
[~,pos]=findpeaks(smooth(y,10));
automatic_initial_params(4)=1/max(diff(x(pos)));

% phas_shift estimator, not working...
% [~,pos]=max(y);
% automatic_initial_params(3)=mod(x(pos),automatic_initial_params(4));

if nargin==2 %simplest valid input
    fixed_params=NaN(1,4);
    initial_params=automatic_initial_params;
    plot_flag=1;
end
if nargin==3
    initial_params=automatic_initial_params;
    plot_flag=1;
end
if nargin==4
    plot_flag=1;
end

if exist('fixed_params','var')
    if isempty(fixed_params)
        fixed_params=NaN(1,4);
    end
end
if exist('initial_params','var')
    if isempty(initial_params)
        initial_params=automatic_initial_params;
    end
end
if exist('plot_flag','var')
    if isempty(plot_flag)
        plot_flag=1;
    end
end

f_str='f = @(param,timeval)';
free_param_count=0;
bool_vec=NaN(1,4);
for i=1:4;
    if isnan(fixed_params(i))
        free_param_count=free_param_count+1;
        f_str=[f_str ' param(' num2str(free_param_count) ')'];
        bool_vec(i)=1;
    else
        f_str=[f_str ' ' num2str(fixed_params(i))];
        bool_vec(i)=0;
    end
    if i==1; f_str=[f_str ' +']; end
    if i==2; f_str=[f_str ' * sin(']; end
    if i==3; f_str=[f_str ' + 2*pi*']; end
    if i==4; f_str=[f_str '*timeval );']; end
end

eval(f_str)

[BETA,RESID,J,COVB,MSE] = nlinfit(x,y,f,initial_params(bool_vec==1));

free_param_count=0;
for i=1:4;
    if isnan(fixed_params(i))
        free_param_count=free_param_count+1;
        param(i)=BETA(free_param_count);
    else
        param(i)=fixed_params(i);
    end
end

if plot_flag==1
    x_vector=min(x):mean(diff(x))/2:max(x);
    plot(x,y,'k.',x_vector,f(BETA,x_vector),'r-')
    xlim(minmax(x_vector))
end


clear all, close all, clc
nu=0.001;   % Diffusion constant

% Define spatial domain
L = 20;             % Length of domain
N = 1000;           % Number of discretization points
dx = L/N;
x = -L/2:dx:L/2-dx; % Define x domain

% Define discrete wavenumbers
kappa = (2*pi/L)*[-N/2:N/2-1];
kappa = fftshift(kappa');    % Re-order fft wavenumbers

% Initial condition
u0 = sech(x);

% Simulate PDE in spatial domain
dt = 0.025;
t = 0:dt:100*dt;
[t,u] = ode45(@(t,u)rhsBurgers(t,u,kappa,nu),t,u0);

% Plot solution in time
subplot(1,2,1)
h=waterfall(real(u(1:10:end,:)));
set(h,'LineWidth',2,'FaceAlpha',.5);
colormap(jet/1.5)
view(5,55)

subplot(1,2,2)
imagesc(flipud(real(u)));
colormap jet

%% FIGURES (PRODUCTION)
figure
CC = colormap(jet(100));
dt = 0.1;
for k = 1:100
    if(mod(k-1,10)==0)
        plot(x,real(u(k,:)),'k','LineWidth',1.5)
%         plot(x,real(u(k,:)),'Color',CC(k,:),'LineWidth',1.5)
        hold on, grid on, drawnow
    end
end
% xlabel('Spatial variable, x')
% ylabel('Temperature, u(x,t)')
axis([-L/2 L/2 -.1 1.1])
set(gca,'LineWidth',1.2,'FontSize',12);
set(gcf,'Position',[100 100 550 220]);
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', '../../figures/FFTBurgers1');

%
figure
subplot(1,2,1)
h=waterfall(real(u(1:10:end,:)));
set(h,'LineWidth',2,'FaceAlpha',.5);
colormap(jet/1.5)
view(5,55)
set(gca,'LineWidth',1.5)
set(gca,'XTick',[0 500 1000],'XTickLabels',{})
set(gca,'ZTick',[0 .5 1],'ZTickLabels',{})
set(gca,'YTick',[0 5 10],'YTickLabels',{})
set(gca,'YLim',[1 11])
set(gca,'ZLim',[-.1 1.1])

subplot(1,2,2)
imagesc(flipud(real(u)));
set(gca,'LineWidth',1.5)
set(gca,'XTick',[0 500 1000],'XTickLabels',{})
set(gca,'YTick',[0 50 100],'YTickLabels',{})

colormap jet
set(gcf,'Position',[100 100 600 250])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', '../../figures/FFTBurgers2');


function [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,y,k,numCentralKnots,useSparse)
%%BSPLINEPOLYFITMULTIDIM Given a set of real vector (or scalar) points as
%         well as real or complex scalar function values, this function
%         returns the knots and coefficients for a set of b-splines that
%         can be used to interpolate to a desired polynomial degree within
%         the sampled region of the function. B-splines can have continuous
%         derivatives across the entire sampled region and can be easily
%         differentiated. This function determines the knots to use by
%         taking the tensor product of the approximate knots from
%         Equation 10 of Chapter 13 of [1]. The knots are not optimal. The
%         first and last knots in each dimensions are repeated. This
%         function can provide interpolation coefficients for multiple sets
%         of values at once.
%
%INPUTS: tau A numPointsXnumDims matrix of the coordinates of the points in
%            each dimension in ascending order. The full grid of points
%            in all dimensions could be found using
%            [tau1,tau2,etc.]=ndgrid(tau(1:tauLengths(1),1),tau(1:tauLengths(2),2),...)
%            and the dimensions can be put together into the full set of
%            numDimsXtotalNumPoints points as
%            tauTotal=[tau1(:).';tau2(:).';...]. Note that using meshgrid
%            in 2D/3D will put the points in the wrong order; one has to
%            use the ordering of ndgrid. These values must be real.
% tauLengths A numDimsX1 or a 1XnumDims vector this lists the number of
%            points in each dimension of tau. The total number of sample
%            points is totalNumPoints=prod(tauLengths).
%          y This is the totalNumPointsXnumSets full sets of function
%            values taken at all of the points implied by the vectors in
%            tau for numSets different interpolation problems. The ordering
%            of the points is the same as the ordering of tauTotal, which
%            is described above. These values can be real or complex.
%          k A numDimsX1 or 1XnumDims set of the order of the b-splines in
%            each dimension. The value k-1 is the polynomial order of the
%            approximation being performed. If the order is the same in all
%            dimensions, then a single scalar can be passed.
% numCentralKnots This optional parameter is used if one passes a higher
%            density of points in tau than is needed, because one wants to
%            perform least squares interpolation. This is the numDimsX1 or
%            1XnumDims list of the number of knots in the central region of
%            knots in each dimension. It varies from 0 to n-k. The higher
%            the number, the more knots are used total. A total of
%            numCentralKnots+2*k will be returned by this function, though,
%            as explained below, the first and last knots in each dimension
%            are not used.
%  useSparse The linear system being solved for the coefficients can be
%            quite large, but is not necessarily dense. If useSparse is
%            true, then the matrix for the system will be allocated as a
%            sparse matrix. This can slow down the algorithm, but it can
%            also keep Matlab from running out of memory on large systems.
%            The default if this parameter is omitted or an empty matrix is
%            passed is true.
%
%OUTPUTS: a A hypermatrix with numDim+1 indices containing the set of
%           coefficients for the b-splines covering all of the
%           interpolation intervals with the final index selecting the set
%           if numSets>1. If numSets=1, then the final index is unitary,
%           meaning that there are effectively only numDim dimensions. This
%           and the following outputs can be passed to
%           BSplineInterpValMultiDim to perform interpolation over the
%           region spanned by the tau values.
%         t The maxNumKnotsXnumDims set of knots for the interpolation
%           function. The first and last k(curDim)-1 knots in each
%           dimension are outside of the ends of the region with data or
%           mark the ends of the region with data. The number of rows is
%           the maximum number needed for all of the dimensions. tLength
%           says how many items in each column are actually used. The knots
%           are the same for each set.
%   tLength A numDimX1 vector where tLength(i) says the number of elements
%           in t(:,i). The lengths are the same for each set.
%
%The comments ot the function BSplinePolyFit describe how Equation 7 in
%Chapter IX is used in one dimension. The issue of multiple dimensions is
%addressed in Chapter 17 of [1] with a focus on solving the problem in
%2D. The basic equation to be solved in 2D is given by Equation 11 in
%Chapter 17 of [1] and it can be easily extended to an arbitrary number
%of dimensions. Chapter 17 gives a solution that is more efficient in 2D
%than solving Equation 11. However, we choose to solve the equation to make
%the problem valid for an arbitrary number of dimensions.
%
%The ability to do a least-squares fit involves making fewer inner knots
%than suggested in the text. Rather than making n-k knots, we make
%numCentralKnots. However, this invalidates the approximation in Equation
%10 of Chapter 13 of [1] for selecting knots. The correction applied is to
%make the average taken in the equation to involve more points.
%Additionally, the knots in Chapter 13 are univariate. We make them
%multivariate by taking a tensor product.
%
%EXAMPLE 1:
%Here, we fit a curve to 2D data on an irregular grid. We choose a
%sufficiently high order that it recreates the polynomial exactly and we
%verify this by looking at the maximum error on a fine grid and plotting
%the results.
% f=@(x,y)(x.^4-2*x.^2+x).*(y.^4-2*y.^2+y);
% numDims=2;
% numPointsX=10;
% numPointsY=11;
% tauLengths=[numPointsX;numPointsY];
% tau=zeros(max(tauLengths),numDims);
% tau(1:tauLengths(1),1)=linspace(-1.5,1.5,numPointsX);
% tau(1:tauLengths(2),2)=linspace(-1.5,1.5,numPointsY);
% %Note that using meshgrid would have put the elements in the wong order.
% %ndgrid must be used.
% [tau1,tau2]=ndgrid(tau(1:tauLengths(1),1),tau(1:tauLengths(2),2));
% y=f(tau1,tau2);
%
% k=[5;5];
% [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,y(:),k);
%
% numPointsX=100;
% numPointsY=101;
% pointsX=linspace(-1.5,1.5,numPointsX);
% pointsY=linspace(-1.5,1.5,numPointsY);
% [X,Y]=ndgrid(pointsX,pointsY);
% x=[X(:).';Y(:).'];
%
% zTrue=f(X,Y);
%
% z=BSplineInterpValMultiDim(x,t,tLength,a,k);
% z=reshape(z,[numPointsX,numPointsY]);
%
% figure(1)
% clf
% surface(X,Y,z,'EdgeColor','none')
% title('Interpolated Surface')
%
% figure(2)
% clf
% surface(X,Y,zTrue,'EdgeColor','none')
% title('True Surface')
% max(abs(zTrue(:)-z(:)))
%One sees that the curves are the same and that the difference at any point
%is on the order of finite precision error. This is consistent with the
%fact that the order of the spline fit was chosen to be the same as the
%order of the true polynomial.
%
%EXAMPLE 2:
%Here, we fit a curve to 3D data on an irregular grid. We choose a
%sufficiently high order that it recreates the polynomial exactly and we
%verify this by looking at the maximum error on a fine grid.
% f=@(x,y,z)(x.^4-2*x.^2+x).*(y.^4-2*y.^2+y).*(z.^4-3*z.^2+z);
% numDims=3;
% numPointsX=10;
% numPointsY=11;
% numPointsZ=9;
% tauLengths=[numPointsX;numPointsY;numPointsZ];
% tau=NaN(max(tauLengths),numDims);
% tau(1:tauLengths(1),1)=linspace(-1.5,1.5,numPointsX);
% tau(1:tauLengths(2),2)=linspace(-1.5,1.5,numPointsY);
% tau(1:tauLengths(3),3)=linspace(-1.5,1.5,numPointsZ);
% %Note that using meshgrid would have put the elements in the wong order.
% %ndgrid must be used.
% [tau1,tau2,tau3]=ndgrid(tau(1:tauLengths(1),1),tau(1:tauLengths(2),2),tau(1:tauLengths(3),3));
% y=f(tau1,tau2,tau3);
%
% k=[5;5;5];
% [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,y(:),k);
%
% numPointsX=50;
% numPointsY=51;
% numPointsZ=49;
% pointsX=linspace(-1.5,1.5,numPointsX);
% pointsY=linspace(-1.5,1.5,numPointsY);
% pointsZ=linspace(-1.5,1.5,numPointsZ);
% [X,Y,Z]=ndgrid(pointsX,pointsY,pointsZ);
% x=[X(:).';Y(:).';Z(:).'];
%
% wTrue=f(X,Y,Z);
% w=BSplineInterpValMultiDim(x,t,tLength,a,k);
% max(abs(wTrue(:)-w(:)))
%As was the case with Example 1, the maximum error is on the order of what
%one would expect due to finite precision limitations.
%
%EXAMPLE 3:
%In this case, a complex 2D function is fit and interpolated.
% numPoints=50;
% xMin=0;
% xMax=3;
% yMin=0;
% yMax=3;
% x=linspace(xMin,xMax,numPoints);
% y=linspace(yMin,yMax,numPoints);
% [X,Y]=ndgrid(x,y);
% f=@(x,y)((sin(5*y)+1j*cos(10*x)).*exp(1j*(x.^2-2*y.^2)));
%
% fxy=f(X(:),Y(:));
% k=[5;5];
% tau=[x(:),y(:)];
% tauLengths=[numPoints,numPoints];
% [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,fxy,k);
%
% %Interpolate on a finer grid.
% numPoints=100;
% x=linspace(xMin,xMax,numPoints);
% y=linspace(xMin,xMax,numPoints);
% [X,Y]=ndgrid(x,y);
%
% fxy=f(X(:),Y(:));
% pts=[X(:).';Y(:).'];
% fxyInterp=reshape(BSplineInterpValMultiDim(pts,t,tLength,a,k),[numPoints,numPoints]);
%
% fxy=reshape(fxy,[numPoints,numPoints]);
%
% figure(1)
% clf
% hold on
% surface(X,Y,real(fxy),'EdgeColor','none')
% title('Real Function Values')
% colorbar()
%
% figure(2)
% clf
% hold on
% surface(X,Y,imag(fxy),'EdgeColor','none')
% title('Imaginary Function Values')
% colorbar()
%
% figure(3)
% clf
% hold on
% surface(X,Y,abs(real(fxyInterp)-real(fxy)),'EdgeColor','none')
% title('Real Interpolation Error')
% colorbar()
%
% figure(4)
% clf
% hold on
% surface(X,Y,abs(imag(fxyInterp)-imag(fxy)),'EdgeColor','none')
% title('Imaginary Interpolation Error')
% colorbar()
%
%REFERENCES:
%[1] C. de Boor, A Practical Guide to Splines. New York: Springer-Verlag,
%    1978.
%
%April 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDims=size(tau,2);
n=tauLengths(:);
numSets=size(y,2);

if(nargin<6)
    useSparse=false;
end

%The total number of points.
NTotal=prod(n);
if(NTotal~=size(y,1))
    error('The dimensionalities of the inputs are not consistent.')
end

if(isscalar(k))
    k=repmat(k,[numDims,1]);
end
k=k(:);

if(nargin<5||isempty(numCentralKnots))
    numCentralKnots=n-k;
elseif(any(numCentralKnots<0)||any(numCentralKnots(:)>n-k))
    error('The value of numCentralKnots is invalid.')
end
numCentralKnots=numCentralKnots(:);

deltaVal=n-k-numCentralKnots;

tLength=numCentralKnots+2*k;
maxTLength=max(tLength);

t=zeros(maxTLength,numDims);
for curDim=1:numDims
    taLengthCur=tauLengths(curDim);
    tauCur=tau(1:taLengthCur,curDim);
    kCur=k(curDim);
    deltaValCur=deltaVal(curDim);
    numCentKnotsCur=numCentralKnots(curDim);

    t(1:kCur,curDim)=tauCur(1);
    t((numCentKnotsCur+kCur+1):(numCentKnotsCur+2*kCur),curDim)=tauCur(n(curDim));

    %The ad-hoc point choice from 10 of Chapter 13, modified to take more
    %points if a least-squares solution is being performed.
    for i=1:numCentKnotsCur
        t(k+i,curDim)=sum(tauCur((i+1):(i+kCur-1+deltaValCur)))/(kCur-1+deltaValCur);
    end
end

%Given the locations of the knots, we build the equations. The a values are
%of the form a(d1,d2,..dnumDims). To access them, we stack them one after
%another in the same order as a(:) would in Matlab.
totalNumA=prod(numCentralKnots+k);

if(useSparse)
    %The amount of memory used will grow as B is filled in.
    B=sparse(NTotal,totalNumA);
else
    B=zeros(NTotal,totalNumA);
end

spanMin=ones(numDims,1);
tIdxVals=k;

%tauIdxVals holds the index of the value of tau used in each dimension
tauIdxVals=ones(numDims,1);
tauVal=zeros(numDims,1);
%Fill in tauVal with the current value.
for curDim=1:numDims
    tauVal(curDim)=tau(1,curDim);
end

%The number of B values produced per call to evalBSplinePolysMultiDim.
numBCurEls=prod(k);

%We have to go through all of the indices.
for curEq=1:NTotal
    BCur=evalBSplinePolysMultiDim(t,tLength,tauVal,k,tIdxVals);
    %We have to put the elements in BCur into the proper spots in B. These
    %spots start at spanMin in each dimension.
    for curEl=1:numBCurEls
        dimIdx=index2NDim(k,curEl);
        %The multidimensional index of the a value by which the current B
        %value is multiplied.
        aDimIdx=spanMin+dimIdx-1;
        aIdx=nDim2Index(n-deltaVal,aDimIdx);

        B(curEq,aIdx)=BCur(curEl);
    end

    if(curEq==NTotal)
        break;
    end

    %Now, we increment tauIdxVals to move onto a new value of tau in each
    %dimension. the values are kept in the tauVal array. Each time a value
    %is incremented, we have to check whether the corresponding entry in
    %tIdxVals and spanMin should be incremented.
    curIdx=1;
    tauIdxVals(curIdx)=tauIdxVals(curIdx)+1;
    while(tauIdxVals(curIdx)>tauLengths(curIdx))
        %If it exceeds k, then go back to 1.
        tauIdxVals(curIdx)=1;
        tauVal(curIdx)=tau(1,curIdx);

        tIdxVals(curIdx)=k(curIdx);
        spanMin(curIdx)=1;

        %Increment tIdxVals and spanMin as necessary to reach the valid
        %region.
        while(tauVal(curIdx)>t(tIdxVals(curIdx)+1,curIdx))
            tIdxVals(curIdx)=tIdxVals(curIdx)+1;
            spanMin(curIdx)=spanMin(curIdx)+1;
        end

        curIdx=curIdx+1;
        tauIdxVals(curIdx)=tauIdxVals(curIdx)+1;
    end

    %Increment tIdxVals and spanMin as necessary to reach the valid
    %region.
    tauVal(curIdx)=tau(tauIdxVals(curIdx),curIdx);
    while(tauVal(curIdx)>t(tIdxVals(curIdx)+1,curIdx))
        tIdxVals(curIdx)=tIdxVals(curIdx)+1;
        spanMin(curIdx)=spanMin(curIdx)+1;
    end
end

a=B\y;
if(numDims>1)
    a=reshape(a,[(numCentralKnots+k)',numSets]);
end
end

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


function [d] = spm_kl_eig_normal (m_q,c_q,c_p)
% KL divergence between normal densities using eigendecomposition
% function [d] = spm_kl_eig_normal (m_q,c_q,c_p)
%
% Calculate the KL distance
%
% KL (Q||P) = <log Q/P> where avg is wrt Q
%
% between two Normal densities Q and P where P is
% zero mean and has a diagonal covariance.
%
% m_q, c_q    Mean and covariance of first Normal density
% c_p         Covariance of second (zero-mean) Normal density
%___________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_kl_eig_normal.m 1143 2008-02-07 19:33:33Z spm $

d=length(m_q);
m_q=m_q(:);

ldcp=0.5*sum(log(diag(c_p)));
[v,eigvals]=eig(c_q);
ldcq=-0.5*sum(log(diag(eigvals)));
Term1=ldcp+ldcq;

inv_c_p=diag(1./diag(c_p));
Term2=0.5*trace(inv_c_p*c_q)+0.5*m_q'*inv_c_p*m_q;
d=Term1+Term2-0.5*d;


function jdate = julian (month, day, year)

% Julian date

% Input

%  month = calendar month [1 - 12]
%  day   = calendar day [1 - 31]
%  year  = calendar year [yyyy]

% Output

%  jdate = Julian date

% special notes

%  (1) calendar year must include all digits

%  (2) will report October 5, 1582 to October 14, 1582
%      as invalid calendar dates and stop

% Orbital Mechanics with Matlab

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

y = year;
m = month;
b = 0;
c = 0;

if (m <= 2)
   y = y - 1;
   m = m + 12;
end

if (y < 0)
   c = -.75;
end

% check for valid calendar date

if (year < 1582)
   % null
elseif (year > 1582)
   a = fix(y / 100);
   b = 2 - a + floor(a / 4);
elseif (month < 10)
   % null
elseif (month > 10)
   a = fix(y / 100);
   b = 2 - a + floor(a / 4);
elseif (day <= 4)
   % null
elseif (day > 14)
   a = fix(y / 100);
   b = 2 - a + floor(a / 4);
else
   clc; home;

   fprintf('\n\n  this is an invalid calendar date!!\n');

   keycheck;

   return;
end

jd = fix(365.25 * y + c) + fix(30.6001 * (m + 1));

jdate = jd + day + b + 1720994.5;


function value = fedn ( dim_num, x )

%*****************************************************************************80
%
%% FEDN(X(1:DIM_NUM)) = exp ( sum ( X(1:DIM_NUM) ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real X(DIM_NUM), the argument.
%
%    Output, real VALUE, the value of the function at X.
%
  value = exp ( sum ( x(1:dim_num) ) );

  return
end


function [t,t1] = geyer_icse(x,maxlag)
% GEYER_ICSE - Compute autocorrelation time tau using Geyer's
%              initial convex sequence estimator
%
%   C = GEYER_ICSE(X) returns autocorrelation time tau.
%   C = GEYER_ICSE(X,MAXLAG) returns autocorrelation time tau with
%       MAXLAG . Default MAXLAG = M-1.
%
%   References:
%      [1] C. J. Geyer, (1992). "Practical Markov Chain Monte Carlo",
%          Statistical Science, 7(4):473-511
%
% Copyright (C) 2002 Aki Vehtari

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

% compute autocorrelation
if nargin > 1
  cc=acorr(x,maxlag);
else
  cc=acorr(x);
end
[n,m]=size(cc);

% acorr returns values starting from lag 1, so add lag 0 here
cc=[ones(1,m);cc];
n=n+1;

% now make n even
if mod(n,2)
  n=n-1;
  cc(end,:)=[];
end

% loop through variables
t=zeros(1,m);
t1=zeros(1,m);
opt=optimset('LargeScale','off','display','off');
for i1=1:m
  c=cc(:,i1);
  c=sum(reshape(c,2,n/2),1);
  ci=find(c<0);
  if isempty(ci)
    warning(sprintf('Inital positive could not be found for variable %d, using maxlag value',i1));
    ci=n/2;
  else
    ci=ci(1)-1; % initial positive
  end
  c=[c(1:ci) 0];    % initial positive sequence
  t1(i1)=-1+2*sum(c); % initial positive sequence estimator
  if ci>2
    ca=fmincon(@se,c,[],[],[],[],0*c,c,@sc,opt,c); % monotone convex sequence
  else
    ca=c;
  end
  t(i1)=-1+2*sum(ca); % monotone convex sequence estimator
end

function e = se(x,xx)
% SE - Error in monotone convex sequene estimator
e=sum((xx-x).^2);

function [c,ceq] = sc(x,xx)
% SE - Constraint in monotone convex sequene estimator
ceq=0*x;
c=ceq;
d=diff(x);
dd=-diff(d);
d(d<0)=0;d=d.^2;
dd(dd<0)=0;dd=dd.^2;
c(1:end-1)=d;c(2:end)=c(2:end)+d;
c(1:end-2)=dd;c(2:end-1)=c(2:end-1)+dd;c(3:end)=c(3:end)+dd;


% Cobb-Douglas production function
% Y=F(K,N)=K^{alpha}*N^{1-alpha}
% In per capita form y=k^{alpha}

function y=cobb(k)
global alpha
if alpha>0 & alpha<1
   y=k.^alpha;
else
   y=-inf*abs(k);
   disp('invalid capital share')
end


function [ unique_num, seed ] = point_radial_tol_unique_count ( m, n, a, ...
  tol, seed )

%*****************************************************************************80
%
%% POINT_RADIAL_TOL_UNIQUE_COUNT counts the tolerably unique points.
%
%  Discussion:
%
%    The input data is an M x N array A, representing the M-dimensional
%    coordinates of N points.
%
%    The output is the number of tolerably unique points in the list.
%
%    This program performs the same task as POINT_TOL_UNIQUE_COUNT.
%    But that program is guaranteed to use N^2 comparisons.
%
%    It is hoped that this function, on the other hand, will tend
%    to use O(N) comparisons after an O(NLog(N)) sort.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 July 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows.
%
%    Input, integer N, the number of columns.
%
%    Input, real A(M,N), the array of N columns of data.
%
%    Input, real TOL, a tolerance for equality.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, integer UNIQUE_NUM, the number of tolerably
%    unique points.
%
  if ( n <= 0 )
    unique_num = 0;
    return
  end
%
%  Assign a base point Z randomly in the convex hull.
%
  [ w, seed ] = r8vec_uniform_01 ( n, seed );

  z = zeros ( m, 1 );
  for i = 1 : m
    z(i) = a(i,1:n) * w(1:n,1) / sum ( w(1:n) );
  end
%
%  Compute the radial distance R of each point to Z.
%
  r = zeros ( n, 1 );
  for j = 1 : n
    r(j) = sqrt ( sum ( ( a(1:m,j) - z(1:m) ).^2 ) );
  end
%
%  Implicitly sort the R array.
%
  indx = r8vec_sort_heap_index_a ( n, r );
%
%  To determine if a point I is tolerably unique, we only have to check
%  whether it is distinct from all points J such that R(I) <= R(J) <= R(J)+TOL.
%
  unique_num = 0;
  unique(1:n) = 1;

  for i = 1 : n

    if ( unique(indx(i)) )
%
%  Point INDX(I) is unique, in that no earlier point is near it.
%
      unique_num = unique_num + 1;
%
%  Look for later points which are close to point INDX(I)
%  in terms of R.
%
      hi = i;

      while ( hi < n )
        if ( r(indx(i)) + tol < r(indx(hi+1)) )
          break
        end
        hi = hi + 1;
      end
%
%  Points INDX(I+1) through INDX(HI) have an R value close to
%  point INDX(I).  Are they truly close to point INDEX(I)?
%
      for j = i + 1 : hi
        if ( unique(indx(j)) )
          dist = sqrt ( sum ( ( a(1:m,indx(i)) - a(1:m,indx(j)) ).^2 ) );
          if ( dist <= tol )
            unique(indx(j)) = 0;
          end
        end
      end

    end

  end

  return
end


%
% n=erlangbinv(p,rho)
%
% This function finds the smallest n such the Erlang B probability that
% a system with n servers, no waiting line, Poisson arrival rate lambda,
% service rate (per server) mu, and intensity rho=lambda/mu will have
% a probability <=p of having all servers busy.
%
% The Erlang B probability is given by
%
%  B=(rho^m/m!)/(sum(rho^k/k!),k=0..m)
%
% We use a recurrence relation which is more accurate than direct evaluation
% of the formula.  This recurrence relation is a "folk theorem".  The author
% would appreciate a reference to its first publication.  The recurrence is
%
%  B(0,rho)=1
%
%  B(n,rho)=(rho*B(n-1,rho)/n)/(1+rho*B(n-1,rho)/n)
%
% This routine simply loops through the recursion until B is <= p, and
% then returns n.
%
function n=erlangbinv(p,rho)
%
% Start the recursion with B=1.
%
B=1;
%
% Loop, iterating the recursion until the probability is <= p.
%
n=1;
while (1 == 1),
  B=((rho*B)/n)/(1+rho*B/n);
  if (B <= p),
    return;
  end;
  n=n+1;
end;


function out = nrlremap(x, a, c)
% NRLREMAP Lin-log style remap
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

if nargin<3
    c = 220; % Output "knee" in lin-log curve
end
if nargin<2
    a = 1; % Scale factor of 99th percentile for input "knee"
end

A = abs(single(x));
Amin = min(A(:));
P99 = pctl(A(:),99);
b = (255-c)/log10((max(A(:))-Amin)/((a*P99)-Amin));

out = uint8(zeros(size(x)));
linear_region = (A<=a*P99);
out(linear_region) = (A(linear_region) - Amin)*c/((a*P99)-Amin);
out(~linear_region) = b*log10((A(~linear_region)-Amin)/((a*P99)-Amin))+c;

end

function ptile = pctl(v, p)
ptile = interp1(linspace(0.5/numel(v), 1-0.5/numel(v), numel(v))', sort(v(:)), p*0.01, 'spline');
end
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [ypred, ll, mse] = eval_AR_perf(coef, C, y, model)
% Evaluate the performance of an AR model.
%
% Inputs
% coef(:,:,k,m) - coef. matrix to use for k steps back, model m
% C(:,:,m)      - cov. matrix for model m
% y(:,t)        - observation at time t
% model(t)      - which model to use at time t (defaults to 1 if not specified)
%
% Outputs
% ypred(:,t)    - the predicted value of y at t based on the evidence thru t-1.
% ll            - log likelihood
% mse           - mean squared error = sum_t d_t . d_t, where d_t = pred(y_t) - y(t)

[s T] = size(y);
k = size(coef, 3);
M = size(coef, 4);

if nargin<4, model = ones(1, T); end

ypred = zeros(s, T);
ypred(:, 1:k) = y(:, 1:k);
mse = 0;
ll = 0;
for j=1:M
  c(j) = log(normal_coef(C(:,:,j)));
  invC(:,:,j) = inv(C(:,:,j));
end
coef = reshape(coef, [s s*k M]);

for t=k+1:T
  m = model(t-k);
  past = y(:,t-1:-1:t-k);
  ypred(:,t) = coef(:, :, m) * past(:);
  d = ypred(:,t) - y(:,t);
  mse = mse + d' * d;
  ll = ll + c(m) - 0.5*(d' * invC(:,:,m) * d);
end
mse = mse / (T-k+1);


function result = tetra_07 ( func, x, y, z )

%*****************************************************************************80
%
%% TETRA_07 approximates an integral inside a tetrahedron in 3D.
%
%  Integration region:
%
%    Points inside a tetrahedron whose four corners are given.
%
%  Discussion:
%
%    A 64 point 7-th degree conical product Gauss formula is used,
%    Stroud number T3:7-1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2004
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%    Stroud and Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966, pages 42-43.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function of three variables which is to be integrated,
%    of the form:
%      function value = func ( x, y, z )
%
%    Input, real X(4), Y(4), Z(4), the X, Y and Z coordinates of
%    the vertices.
%
%    Output, real RESULT, the approximate integral of the function.
%
  norder = 4;
  weight2 = [ ...
    0.1355069134, 0.2034645680, 0.1298475476, 0.0311809709 ];
  weight3 = [ ...
    0.1108884156, 0.1434587898, 0.0686338872, 0.0103522407 ];
  xtab2 = [ ...
    0.0571041961E+00, 0.2768430136E+00, 0.5835904324E+00, 0.8602401357E+00 ];
  xtab3 = [ ...
    0.0485005495E+00, 0.2386007376E+00, 0.5170472951E+00, 0.7958514179E+00 ];
%
%  Get the Gauss-Legendre weights and abscissas for [-1,1].
%
  [ xtab1, weight1 ] = legendre_set ( norder );
%
%  Adjust the rule for the interval [0,1].
%
  a = -1.0E+00;
  b = +1.0E+00;

  c =  0.0E+00;
  d =  1.0E+00;

  [ xtab1, weight1 ] = rule_adjust ( a, b, c, d, norder, xtab1, weight1 );
%
%  Carry out the quadrature.
%
  quad = 0.0;

  for i = 1 : norder
    for j = 1 : norder
      for k = 1 : norder
%
%  Compute the barycentric coordinates of the point in the unit triangle.
%
        t =                                                 xtab3(k);
        u =                        xtab2(j)   * ( 1.0E+00 - xtab3(k) );
        v = xtab1(i) * ( 1.0E+00 - xtab2(j) ) * ( 1.0E+00 - xtab3(k) );
        w = 1.0E+00 - t - u - v;
%
%  Compute the corresponding point in the triangle.
%
        xval = t * x(1) + u * x(2) + v * x(3) + w * x(4);
        yval = t * y(1) + u * y(2) + v * y(3) + w * y(4);
        zval = t * z(1) + u * z(2) + v * z(3) + w * z(4);
        quad = quad + 6.0 * weight1(i) * weight2(j) * weight3(k) ...
          * feval ( func, xval, yval, zval );

      end
    end
  end

  volume = tetra_volume ( x, y, z );
  result = quad * volume;

  return
end


function [x,t]=meanLinTrajConstEnds(x0,x2,deltaT,q0,order,N,FFun,QFun)
%%MEANLINTRAJCONSTENDS Generate a the mean trajectory based on a linear
%               dynamic model with additive noise, as is typically used in
%               tracking systems, given known locations of the start and
%               end points. A polynomial dynamic model is used by default,
%               but one can specify other models.
%
%INPUTS: x0, x2 The xDimX1 target states at the beginning and end of the
%               region over which paths are to be simulated. These are
%               treated as known values.
%        deltaT The time between when state x0 occurred and when state x2
%               occurred. This is a positive value.
%            q0 The power spectral density of the process noise. This is
%               passed to QFun, as described below and by default would
%               corresponds to the same-named input in QPolyKal. With the
%               default polynomial dynamic model, FPolyKal and QPolyKal, q0
%               has no effect on the result and is just set to 1 if an
%               empty matrix is passed.
%         order The order >=0 of the filter. If order=1, then it is
%               constant velocity, 2 means constant acceleration, 3 means
%               constant jerk, etc. This is passed to FFun and QFun. If
%               custom FFun and QFun functions are used and this is not
%               needed, then an empty matrix can be passed. The default if
%               omitted or an empty matrix is passed is 1.
%             N The number of points in the random path to generate between
%               x0 and x2. The default if omitted or an empty matrix is
%               passed is 100; N>=0.
%    FFun, QFun Optional function handles to functions that respectively
%               provide the state transition matrix and the process noise
%               covariance matrix as a function of the time duration over
%               which the prediction is taken. The defaults if omitted or
%               empty matrices are passed are handles to FPolyKal and
%               QPolyKal. The functions are called as FFun(T,xDim,order)
%               and QFun(T,xDim,order,q0), where T is the duration over
%               which the prediction is taken.
%
%OUTPUTS: x The xDimX(N+2) set of mean trajectory according to the
%           specified model. x(:,1) is always x0 and x(:,N+2) is always x2.
%         t The (N+2)X1 set of time offsets (starting at 0) where the
%           values in x are generated.
%
%Consider a process at points 0, 1, and 2, where we are given the value at
%time x0. The transition models are:
%x1=F0*x0+w1
%x2=F1*x1+w2
%Where x0 is given (deterministic) and w1 and w2 are independent zero-mean
%Gaussian random variables with covariance matrices Q1 and Q1.
%x1 and x2 are jointly normal with mean mu and covariance matrix Sigma:
%mu=[x1Pred; = [F0*x0
%    x2Pred]    F1*F0*x0];
%Sigma=[   Q1,        Q1*F1'; =[Pxx, Pxz;
%       F1*Q1,  F1*Q1*F1'+Q2]   Pxz',Pzz];
%When given the value of x2, the conditional distribution of x1|x0,x2 is
%the same as the Kalman filter update taking the measurement covariance
%matrix to be Q2, the measurement matrix H to be F1, the prior state to be
%F0*x0, and the covariance of the prior state to be Q1. Thus, as derived,
%for example, in [Ch. 5,1], [2], the posterior distribution of x1 is
%Gaussian with mean and covariance matrix
%x1Bar=x1Pred+Pxz/Pzz*(x2-x2Pred)
%This function is similar to randLinTrajConstEnds.
%
%EXAMPLE:
%This plots a trajectory that eventually has to turn around, since the
%endpoint is not close to the direction of the initial velocity vector.
% x0=[1e3;0;30;20];
% x2=[709;
%     527;
%     -39;
%       4];
% deltaT=100;
% x=meanLinTrajConstEnds(x0,x2,deltaT);
% figure(1)
% clf
% plot(x(1,:),x(2,:))
% h1=xlabel('x');
% h2=ylabel('y');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<6||isempty(N))
    N=100;
end

if(nargin<5||isempty(order))
    order=1;
end

if(nargin<4||isempty(q0))
    q0=1;
end

xDim=size(x0,1);

if(nargin<8||isempty(QFun))
    Q=@(T)QPolyKal(T,xDim,order,q0);
else
    Q=@(T)QFun(T,xDim,order,q);
end

if(nargin<7||isempty(FFun))
    F=@(T)FPolyKal(T,xDim,order);
else
    F=@(T)FFun(T,xDim,order);
end

x=zeros(xDim,N+2);

%The increment between the sample points.
deltaTInc=deltaT/(N+1);

x(:,1,:)=x0;
x(:,N+2,:)=x2;

F0=F(deltaTInc);
Q1=Q(deltaTInc);
for curPoint=1:N
    deltaT0=deltaTInc*(curPoint-1);
    deltaT2=deltaT-deltaT0-deltaTInc;

    F1=F(deltaT2);
    Q2=Q(deltaT2);

    Pxz=Q1*F1';
    Pzz=F1*Q1*F1'+Q2;
    x1Pred=F0*x(:,curPoint);
    x2Pred=F1*F0*x(:,curPoint);
    x1Mean=x1Pred+Pxz*inv(Pzz)*(x2-x2Pred);

    x(:,curPoint+1)=x1Mean;
end

if(nargout>1)
    t=[0;deltaTInc*(1:(N+1)).'];
    t(end)=deltaT;%Make it exact despite finite precision limitations.
end
end

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


function [soln,eqn,info] = Poisson(node,elem,bdFlag,pde,option)
%% POISSON Poisson equation: P1 linear element.
%
%   u = Poisson(node,elem,bdFlag,pde) produces the linear finite element
%   approximation of the Poisson equation
%
%       -div(d*grad(u))=f  in \Omega, with
%       Dirichlet boundary condition u=g_D on \Gamma_D,
%       Neumann boundary condition   d*grad(u)*n=g_N on \Gamma_N,
%       Robin boundary condition     g_R*u + d*grad(u)*n=g_N on \Gamma _R
%
%   The mesh is given by node and elem and the boundary edge is by
%   bdFlag. See meshdoc, bddoc for details. The data is given by the
%   structure pde which contains function handles f, g_D, g_N, g_R, or d.
%   For general elliptic equations with convection and reaction
%   coefficients, see ellipticpde.
%
%   soln = Poisson(node,elem,bdFlag,pde,option) specifies the options.
%
%   In the output, soln structure contains
%     - soln.u: solution u
%     - soln.Du: gradient of u
%
%   In the input, option structures contains
%    - option.dquadorder: quadrature order for diffusion coefficients
%    - option.fquadorder: quadrature order for computing right hand side f
%    - option.solver
%      'direct': the built in direct solver \ (mldivide)
%      'mg':     multigrid-type solvers mg is used.
%      'amg':    algebraic multigrid method is used.
%      'none':   only assemble the matrix equation but not solve.
%   The default setting is to use the direct solver for small size problems
%   and multigrid solvers for large size problems. For more options on the
%   multigrid solver mg, type help mg.
%
%   The function Poisson assembes the matrix equation AD*u = b and solves
%   it by the direct solver (small size <= 2e3) or the multigrid solver
%   (large size > 2e3). The Dirichlet boundary condition is built into the
%   matrix AD and the Neumann boundary condition is build into b.
%
%   The diffusion coefficient d is a scalar function or a column array with
%   the same length as the elem array.
%
%   When only one type of boundary condition is imposed, the input argument
%   bdFlag can be skipped. The boundary condition is implicitly given in
%   the pde structure by specifying g_D or g_N only. See examples below.
%
%   [soln,eqn] = Poisson(node,elem,bdFlag,pde) returns also the equation
%   structure eqn, which includes:
%     - eqn.AD:  modified stiffness matrix AD;
%     - eqn.b:   the right hand side.
%     - eqn.Lap: non-modified stiffness matrix
%
%   The solution u = AD\b. The matrix eqn.Lap can be used to evulate the
%   bilinear form a(u,v) = u'*eqn.Lap*v, especially the enery norm of a finite
%   element function u is given by by sqrt(u'*eqn.Lap*u).
%
%   [soln,eqn,info] = Poisson(node,elem,bdFlag,pde) returns also the
%   information on the assembeling and solver, which includes:
%     - info.assembleTime: time to assemble the matrix equation
%     - info.solverTime:   time to solve the matrix equation
%     - info.itStep:       number of iteration steps for the mg solver
%     - info.error:        l2 norm of the residual b - A*u
%     - info.flag:         flag for the mg solver.
%       flag = 0: converge within max iteration
%       flag = 1: iterated maxIt times but did not converge
%       flag = 2: direct solver
%       flag = 3: no solve
%
%   Example
%     squarePoisson; Poissonfemrate;
%
%   Example
%     clear all
%     node = [0,0; 1,0; 1,1; 0,1];
%     elem = [2,3,1; 4,1,3];
%     for k = 1:4
%       [node,elem] = uniformrefine(node,elem);
%     end
%     % Homogenous Dirichlet boundary condition
%     pde.f = inline('ones(size(p,1),1)','p');
%     pde.g_D = 0;
%     u = Poisson(node,elem,[],pde);
%     figure(1);
%     showresult(node,elem,u);
%
%   See also Poisson3, femPoisson, squarePoisson, Poissonfemrate
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Preprocess
if ~exist('bdFlag','var'), bdFlag = []; end
if ~exist('option','var'), option = []; end
% important constants
N = size(node,1);
NT = size(elem,1);
Ndof = N;

%% Diffusion coefficient
time = cputime;  % record assembling time
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder'), option.dquadorder = 1; end
if ~isempty(pde.d) && isnumeric(pde.d)
   K = pde.d;                                 % d is an array
end
if ~isempty(pde.d) && ~isnumeric(pde.d)       % d is a function
    [lambda,weight] = quadpts(option.dquadorder);
    nQuad = size(lambda,1);
    K = zeros(NT,1);
    for p = 1:nQuad
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:);
        K = K + weight(p)*pde.d(pxy);
   end
end

%% Compute geometric quantities and gradient of local basis
[Dphi,area] = gradbasis(node,elem);

%% Assemble stiffness matrix
A = sparse(Ndof,Ndof);
for i = 1:3
    for j = i:3
        % $A_{ij}|_{\tau} = \int_{\tau}K\nabla \phi_i\cdot \nabla \phi_j dxdy$
        Aij = (Dphi(:,1,i).*Dphi(:,1,j) + Dphi(:,2,i).*Dphi(:,2,j)).*area;
        if ~isempty(pde.d)
            Aij = K.*Aij;
        end
        if (j==i)
            A = A + sparse(elem(:,i),elem(:,j),Aij,Ndof,Ndof);
        else
            A = A + sparse([elem(:,i);elem(:,j)],[elem(:,j);elem(:,i)],...
                           [Aij; Aij],Ndof,Ndof);
        end
    end
end
clear K Aij

%% Assemble the right hand side
b = zeros(Ndof,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if isreal(pde.f) % f is a real number or vector and not a function
   switch length(pde.f)
       case NT  % f is piecewise constant
         bt = pde.f.*area/3;
         b = accumarray(elem(:),[bt; bt; bt],[Ndof 1]);
       case N   % f is piecewise linear
         bt = zeros(NT,3);
         bt(:,1) = area.*(2*pde.f(elem(:,1)) + pde.f(elem(:,2)) + pde.f(elem(:,3)))/12;
         bt(:,2) = area.*(2*pde.f(elem(:,2)) + pde.f(elem(:,3)) + pde.f(elem(:,1)))/12;
         bt(:,3) = area.*(2*pde.f(elem(:,3)) + pde.f(elem(:,1)) + pde.f(elem(:,2)))/12;
         b = accumarray(elem(:),bt(:),[Ndof 1]);
       case 1   % f is a scalar e.g. f = 1
         bt = pde.f*area/3;
         b = accumarray(elem(:),[bt; bt; bt],[Ndof 1]);
   end
end
if ~isempty(pde.f) && ~isreal(pde.f)  % f is a function
    [lambda,weight] = quadpts(option.fquadorder);
    phi = lambda;                 % linear bases
	nQuad = size(lambda,1);
    bt = zeros(NT,3);
    for p = 1:nQuad
		% quadrature points in the x-y coordinate
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:);
		fp = pde.f(pxy);
        for i = 1:3
            bt(:,i) = bt(:,i) + weight(p)*phi(p,i)*fp;
        end
    end
    bt = bt.*repmat(area,1,3);
    b = accumarray(elem(:),bt(:),[Ndof 1]);
end
clear pxy bt

%% Set up boundary conditions
[AD,b,u,freeNode,isPureNeumann] = getbd(b);

%% Record assembling time
assembleTime = cputime - time;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(freeNode), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')  || isfield(option,'mgoption')   % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
if isPureNeumann
    option.solver = 'mg';
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        t = cputime;
        u(freeNode) = AD(freeNode,freeNode)\b(freeNode);
        residual = norm(b - AD*u);
        info = struct('solverTime',cputime - t,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
        if ~isfield(option,'mgoption')   % no option.mgoption
            option.mgoption.x0 = u;
            option.mgoption.solver = 'CG';
        end
        [u,info] = mg(AD,b,elem,option.mgoption);
    case 'amg'
        if ~isfield(option,'amgoption')  % no option.amgoption
            option.amgoption.x0 = u;
            option.amgoption.solver = 'CG';
        end
        [u(freeNode),info] = amg(AD(freeNode,freeNode),b(freeNode),option.amgoption);
end
% post-process for pure Neumann problem
if isPureNeumann
    patchArea = accumarray(elem(:),[area;area;area]/3, [N 1]);
    uc = sum(u.*patchArea)/sum(area);
    u = u - uc;   % int u = 0
end

%% Compute Du
dudx =  u(elem(:,1)).*Dphi(:,1,1) + u(elem(:,2)).*Dphi(:,1,2) ...
      + u(elem(:,3)).*Dphi(:,1,3);
dudy =  u(elem(:,1)).*Dphi(:,2,1) + u(elem(:,2)).*Dphi(:,2,2) ...
      + u(elem(:,3)).*Dphi(:,2,3);
Du = [dudx, dudy];

%% Output
if nargout == 1
    soln = u;
else
    soln = struct('u',u,'Du',Du);
    eqn = struct('A',AD,'b',b,'freeNode',freeNode,'Lap',A);
    info.assembleTime = assembleTime;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbd
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,b,u,freeNode,isPureNeumann] = getbd(b)
    %% Set up of boundary conditions.
    %
    % 1) Modify the matrix for Dirichlet boundary nodes, which are not degree
    % of freedom. Values at these nodes are evaluatation of pde.g_D. The
    % original stiffness matrix A is turn into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    %
    % 2) Modify the right hand side b. The Neumann boundary integral is added
    % to b. For Dirichlet boundary ndoes, b(fixedNode) is the evaluation of
    % pde.g_D.
    %
    % Special attentation should be given for the pure Neumann boundary
    % condition. To enforce the compatible condition, the vector b should have
    % mean value zero. To avoid a singular matrix, the 1st node is chosen as
    % fixedNode.
    %
    % The order of assigning Neumann and Dirichlet boundary condition is
    % important to get the right setting at the intersection nodes of Dirichlet
    % and Neumann boundary edges.
    %
    % Reference: Long Chen. Finite Element Methods and its Programming. Lecture
    % Notes.

    u = zeros(Ndof,1);
    %% Initial check
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Modify the matrix for Dirichlet and Robin condition
    % Robin boundary condition
    Robin = [];
    isRobin = (bdFlag(:) == 3);
    if any(isRobin)
        allEdge = [elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])];
        Robin = allEdge(isRobin,:);
    end
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        ve = node(Robin(:,1),:) - node(Robin(:,2),:);
        edgeLength = sqrt(sum(ve.^2,2));
        mid = (node(Robin(:,1),:) + node(Robin(:,2),:))/2;
        % use Simplson rule to compute int g_R phi_iphi_j ds
        ii = [Robin(:,1),Robin(:,1),Robin(:,2),Robin(:,2)];
        jj = [Robin(:,1),Robin(:,2),Robin(:,1),Robin(:,2)];
        temp = pde.g_R(mid).*edgeLength;
        ss = [1/3*temp, 1/6*temp, 1/6*temp, 1/3*temp];
        A = A + sparse(ii,jj,ss,Ndof,Ndof);
    end

    % Find Dirichlet boundary nodes: fixedNode
    fixedNode = []; freeNode = [];
    if ~isempty(bdFlag) % find boundary edges and boundary nodes
        [fixedNode,bdEdge,isBdNode] = findboundary(elem,bdFlag);
        freeNode = ~isBdNode;
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        % no bdFlag, only pde.g_D is given
        [fixedNode,bdEdge,isBdNode] = findboundary(elem);
        freeNode = ~isBdNode;
    end

    % Modify the matrix for different boundary conditions
    % Dirichlet boundary condition
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    if ~isempty(fixedNode)
        bdidx = zeros(Ndof,1);
        bdidx(fixedNode) = 1;
        Tbd = spdiags(bdidx,0,Ndof,Ndof);
        T = spdiags(1-bdidx,0,Ndof,Ndof);
        AD = T*A*T + Tbd;
    end
    % Neumann boundary condition
    isPureNeumann = false;
    if isempty(fixedNode) && isempty(Robin) % pure Neumann boundary condition
        isPureNeumann = true;
        AD = A;
        AD(1,1) = AD(1,1) + 1e-6;
%         fixedNode = 1;
%         freeNode = 2:Ndof;    % eliminate the kernel by enforcing u(1) = 0;
    end
    % Robin boundary condition
    if isempty(fixedNode) && ~isempty(Robin)
        AD = A;
    end

    %% Part 2: Find boundary edges and modify the right hand side b
    % Find boundary edges: Neumann
    Neumann = [];
    if ~isempty(bdFlag)  % bdFlag specifies different bd conditions
        Neumann = bdEdge;
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        [tempvar,Neumann] = findboundary(elem); %#ok<ASGLU>
    end

    % Neumann boundary condition
    if  isnumeric(pde.g_N) && all(pde.g_N == 0)
        pde.g_N = [];
    end
    if ~isempty(Neumann) && ~isempty(pde.g_N)
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 2;   % default order exact for linear gN
        end
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        phigN = lambdagN;                 % linear bases
        nQuadgN = size(lambdagN,1);
        ge = zeros(size(Neumann,1),2);
        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNp = pde.g_N(ppxy);
            for igN = 1:2
                ge(:,igN) = ge(:,igN) + weightgN(pp)*phigN(pp,igN)*gNp;
            end
        end
        ge = ge.*repmat(el,1,2);
        b = b + accumarray(Neumann(:), ge(:),[Ndof,1]);
    end
    % The case with non-empty Neumann edges but g_N=0 or g_N=[] corresponds to
    % the zero flux boundary condition on Neumann edges and no modification of
    % A,u,b is needed.

    % Dirichlet boundary condition
    if isnumeric(pde.g_D) && all(pde.g_D == 0)   % zero g_D
        pde.g_D = [];
    end
    if ~isPureNeumann && ~isempty(fixedNode) && ~isempty(pde.g_D)
        if isnumeric(pde.g_D)  % pde.g_D could be a numerical array
            u(fixedNode) = pde.g_D(fixedNode);
        else % pde.g_D is a function handle
            u(fixedNode) = pde.g_D(node(fixedNode,:));
        end
        b = b - A*u;
    end
    if ~isempty(fixedNode) % non-empty Dirichlet boundary condition
        b(fixedNode) = u(fixedNode);
    end
    % The case with non-empty Dirichlet nodes but g_D=0 or g_D=[] corresponds
    % to the zero Dirichlet boundary condition and no modification of u,b is
    % needed.

    % Pure Neumann boundary condition
    if isPureNeumann
        b = b - mean(b); % compatiable condition (f,1) + <gN,1> = 0
%         b(1) = 0;
    end
    end % end of getbd
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end % end of Poisson


function [ n_data, x, fx ] = arctan_values ( n_data )

%*****************************************************************************80
%
%% ARCTAN_VALUES returns some values of the arc tangent function.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      ArcTan[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 11;

  fx_vec = [ ...
    0.00000000000000000000, ...
    0.24497866312686415417, ...
    0.32175055439664219340, ...
    0.46364760900080611621, ...
    0.78539816339744830962, ...
    1.1071487177940905030, ...
    1.2490457723982544258, ...
    1.3258176636680324651, ...
    1.3734007669450158609, ...
    1.4711276743037345919, ...
    1.5208379310729538578 ];

  x_vec = [ ...
    0.00000000000000000000, ...
    0.25000000000000000000, ...
    0.33333333333333333333, ...
    0.50000000000000000000, ...
    1.0000000000000000000, ...
    2.0000000000000000000, ...
    3.0000000000000000000, ...
    4.0000000000000000000, ...
    5.0000000000000000000, ...
    10.000000000000000000, ...
    20.000000000000000000 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function gray_3x3 = image_denoise_gray_3x3 ( gray )

%*****************************************************************************80
%
%% IMAGE_DENOISE_GRAY_3X3 filters out noise from a gray scale image.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, uint8 GRAY(:,:), the noisy grayscale data.
%
%    Output, uint8 GRAY_3X3(:,:), the grayscale data for the filtered image.
%
  [ m, n ] = size ( gray );
  gray3d = zeros ( 9, m, n );
%
%  For pixel (I,J), GRAY3D(*,I,J) contains the current pixel value
%  and the 8 neighboring values.
%
  gray3d(1,:,:) = circshift ( gray, [ -1, -1 ] );
  gray3d(2,:,:) = circshift ( gray, [ -1,  0 ] );
  gray3d(3,:,:) = circshift ( gray, [ -1,  1 ] );
  gray3d(4,:,:) = circshift ( gray, [  0, -1 ] );
  gray3d(5,:,:) = circshift ( gray, [  0,  0 ] );
  gray3d(6,:,:) = circshift ( gray, [  0,  1 ] );
  gray3d(7,:,:) = circshift ( gray, [  1, -1 ] );
  gray3d(8,:,:) = circshift ( gray, [  1,  0 ] );
  gray3d(9,:,:) = circshift ( gray, [  1,  1 ] );
%
%  By taking the median of the 9 values, we hope to drop any noisy pixels.
%
  gray3d = median ( gray3d );
%
%  The MEDIAN command returned a 3D array with a first dimension of 1.
%  For convenience, we suppress the first dimension.
%
  gray_3x3(:,:) = gray3d(1,:,:);
%
%  GRAY is a UINT8 array.  The MEDIAN operation returned a DOUBLE value.
%  So we have to convert the array back to UINT8.
%
  gray_3x3 = uint8 ( gray_3x3 );

  return
end


% MAIN Illustrates how to use the EPnP algorithm described in:
%
%       Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua.
%       Accurate Non-Iterative O(n) Solution to the PnP Problem.
%       In Proceedings of ICCV, 2007.
%
% Copyright (C) <2007>  <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
%
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
% General Public License for more details.
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
%
% Francesc Moreno-Noguer, CVLab-EPFL, September 2007.
% fmorenoguer@gmail.com, http://cvlab.epfl.ch/~fmoreno/

clear all; close all;

addpath data;
addpath error;
addpath EPnP;

fprintf('\n---------EPnP--------------\n');
%1.-Generate simulated input data------------------------------------------
load_points=0;
if ~load_points
    n=50; %number of points
    std_noise=10; %noise in the measurements (in pixels)
    [A,point,Rt]=generate_noisy_input_data(n,std_noise);
    save('data\input_data_noise.mat','A','point','Rt');
else
    load('data\input_data_noise.mat','A','point','Rt');
    n=size(point,2);
    draw_noisy_input_data(point);
end

%2.-Inputs format--------------------------------
x3d=zeros(n,4);
x2d=zeros(n,3);
A=A(:,1:3);
for i=1:n
    x3d_h(i,:)=[point(i).Xworld',1];
    x2d_h(i,:)=[point(i).Ximg(1:2)',1];

    %world and camera coordinates
    X3d_world(i,:)=point(i).Xworld';
    X3d_cam(i,:)=point(i).Xcam';
end

%3.-EPnP----------------------------------------------------
Xw=x3d_h(:,1:3);
U=x2d_h(:,1:2);

[Rp,Tp,Xc,sol]=efficient_pnp(x3d_h,x2d_h,A);

%draw Results
for i=1:n
    point(i).Xcam_est=Xc(i,:)';
end
figure; h=gcf;
plot_3d_reconstruction(point,'EPnP (Old)',h);
xlim([-2 2]); ylim([-2 2]);

%compute error
error=reprojection_error_usingRT(Xw,U,Rp,Tp,A);
fprintf('error EPnP: %.3f\n',error);

%3.-EPnP_GAUSS_NEWTON----------------------------------------------------
Xw=x3d_h(:,1:3);
U=x2d_h(:,1:2);

[Rp,Tp,Xc,sol]=efficient_pnp_gauss(x3d_h,x2d_h,A);

%draw Results
for i=1:n
    point(i).Xcam_est=Xc(i,:)';
end
figure; h=gcf;
plot_3d_reconstruction(point,'EPnP Gauss Newton',h);

%compute error
error=reprojection_error_usingRT(Xw,U,Rp,Tp,A);
fprintf('error EPnP_Gauss_Newton: %.3f\n',error);
xlim([-2 2]); ylim([-2 2]);


function y = poly_env( p, x )

%POLY_ENV Evaluate the convex or concave envelope of a polynomial.
%   POLY_ENV( P, X ) uses a semidefinite program to compute the value of the
%   convex or concave envelope of the polynomial represented by the vector
%   P. The format of the vector P is identical to that required by POLYVAL,
%   with two additional restrictions. First, the elements of P must be real
%   and finite. Second, the length of P must 0, 2, or odd (1,3,5,...)
%
%   If the polynomial described by P is convex or concave, then a call to
%   POLY_ENV( P, X ) produces the same result as POLYVAL( P, X ).
%
%   POLY_ENV looks at the first nonzero element of P (presumably, but not
%   necessarily, P(1)) to determine if a convex or concave envelope is to
%   be selected. If P(1)>0, then a convex envelope is produced---i.e., the
%   function that is the tightest convex lower bound for POLYVAL(P,X).
%   Otherwise, a concave envelope is produced---the function that is the
%   tightest concave upper bound for POLYVAL(P,X).
%
%   If the degree N of P is odd---except for the special case N==1---no
%   proper convex/concave envelope exists. Therefore, POLY_ENV returns an
%   error in such cases.
%
%   If X is an array, the result will be computed elementwise.
%
%   Disciplined convex programming information:
%       POLY_ENV(P,X) is convex or concave and nonmotonic in X; therefore,
%       in CVX expressions, X must be affine, unless the polynomial
%       described by P has a degree of 0 or 1. P must be constant.

%
% Check the polynomial
%

sp = size( p );
if isempty( p ),
    p = zeros( 1, 0 );
elseif ~isa( p, 'double' ) || ~isreal( p ) || length( sp ) > 2 || ~any( sp == 1 ),
    cvx_throw( 'First argument must be a non-empty real vector.' );
elseif any( isnan( p ) | isinf( p ) ),
    cvx_throw( 'First argument must not contain Inf or NaN.' );
end
n = prod( sp );
if n > 2 && rem( n, 2 ) == 0,
    cvx_throw( 'The length of the vector p must be odd.' );
end

%
% Check the second argument
%

if n > 1 && ( ~cvx_isaffine( x ) || ~isreal( x ) ),
    cvx_throw( 'The second argument must be real and affine.' );
end

%
% Handle the special cases
%

sx = size( x );
ndxs = find( p );
if isempty( ndxs ),
    y = zeros( sx );
    return
else
    for k = ndxs(:)',
        pt = p( k : end );
        if ~any( isinf( pt ./ pt(1) ) ),
            p = pt;
            break;
        end
    end
end
n = length( p );
switch n,
    case 0,
        y = zeros(sx);
        return
    case 1,
        y = p(1) * ones(sx);
        return
    case 2,
        y = p(1) * x + p(2);
        return
    case 3,
        % Quadratic
        b2a = p(2) ./ ( 2 * p(1) );
        y = p(1) * ( square( x + b2a ) - b2a * b2a ) + p(3);
        return
    otherwise,
        if all( p(2:end) == 0 ),
            y = p(1) * ( x .^ (n-1) ) + p(end);
            return
        end
end

%
% Build and solve the SDP
%

degr  = n - 1;
deg2  = 0.5 * degr + 1;
nv    = prod( sx );
psign = sign(p(1));
p     = psign * reshape( p, 1, n );
cvx_begin sdp
    variable y(sx);
    variable P(deg2,deg2,sx) hankel;
    if cvx_isconstant( x )
        minimize(sum(y)); %#ok
    else
        minimize(y); %#ok
    end
    P >= 0; %#ok
    1 == P(1,1,:); %#ok
    x == reshape( P(2,1,:), sx ); %#ok
    y == reshape( p(end:-1:1) * [ reshape( P(1,:,:), deg2, nv ) ; reshape( P(2:end,end,:), deg2-1, nv ) ], sx ); %#ok
cvx_end

y = cvx_optval * psign;

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


%KCENTRES Finds K center objects from a distance matrix
%
%  [LAB,J,DM] = KCENTRES(D,K,N)
%
% INPUT
%   D    Distance matrix between, e.g. M objects (may be a dataset)
%   K    Number of center objects to be found (optional; default: 1)
%   N    Number of trials starting from a random initialization
%        (optional; default: 1)
%
% OUTPUT
%   LAB  Integer labels: each object is assigned to its nearest center
%   J    Indices of the center objects
%   DM   A list of distances corresponding to J: for each center in J
%        the maximum distance of the objects assigned to this center.
%
% DESCRIPTION
% Finds K center objects from a symmetric distance matrix D. The center
% objects are chosen from all M objects such that the maximum of the
% distances over all objects to the nearest center is minimized. For K > 1,
% the results depend on a random initialization. The procedure is repeated
% N times and the best result is returned.
%
% If N = 0, initialisation is not random, but done by a systematic
% selection based on a greedy approach.
%
% SEE ALSO (PRTools Guide)
% HCLUST, PRKMEANS, EMCLUST, MODESEEK

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: kcentres.m,v 1.5 2008/07/03 09:08:43 duin Exp $

function [labels,Jopt,dm] = kcentres(d,k,n)

	if (nargin < 3) | isempty(n)
      n = 1;
      prwarning(4,'Number of trials not supplied, assuming one.');
   end

	if (nargin < 2) | isempty(k)
      k = 1;
      prwarning(4,'Number of centers not supplied, assuming one.');
   end

	if(isdataset(d))
		d = +d;
		prwarning(4,'Distance matrix is convert to double.');
	end

	[m,m2] = size(d);
	if ( ~issym(d,1e-12) | any(diag(d) > 1e-14) )
		error('Distance matrix should be symmetric and have zero diagonal')
	end

% checking for a zero diagonal
	t = eye(m) == 1;
	if(~all(d(t)==0))
		error('Distance matrix should have a zero diagonal.')
	end

	if (k == 1)
		dmax = max(d);
		[dm,Jopt] = min(dmax);
		labels = repmat(1,m,1);
		return;
	end

	if k > m
		error('Number of centres should not exceed number of objects')
	end

	% We are here only if K (> 1) centers are to be found.
	% Loop over number of trials.
	dmax = max(max(d));
	dopt = inf;
	s = sprintf('k-centres, %i attempts: ',n);
	prwaitbar(n,s,n>1);
	if n == 0
		nrep = 1;
	else
		nrep = n;
	end
	for tri = 1:nrep
		prwaitbar(n,tri,[s int2str(tri)]);
		if n == 0
			M = kcentresort(d,k);           % systematic initialisation
		else
			M = randperm(m); M = M(1:k);		% Random initializations
		end
 		J = zeros(1,k); 					    	  % Center objects to be found.

		% Iterate until J == M. See below.
		while 1,
			[dm,I] = min(d(M,:));

			% Find K centers.
			for i = 1:k
				JJ = find(I==i);
				if (isempty(JJ))
					%JJ can be empty if two or more objects are in the same position of
					% feature space in dataset
					J(i) = 0;
				else
						% Find objects closest to the object M(i)
					[dummy,j,dm] = kcentres(d(JJ,JJ),1,1);
					J(i) = JJ(j);
				end
			end

			J(find(J==0)) = [];
			k = length(J);
			if k == 0
				error('kcentres fails as some objects are identical: add some noise')
			end
			if (length(M) == k) & (all(M == J))
				% K centers are found.
				[dmin,labs] = min(d(J,:));
				dmin = max(dmin);
				break;
			end
			M = J;
		end

		% Store the optimal centers in JOPT.

		if (dmin <= dopt)
			dopt = dmin;
			labels = labs';
			Jopt = J;
		end

	end
	prwaitbar(0)

	% Determine the best centers over the N trials.
	dm = zeros(1,k);
	for i=1:k
		L = find(labels==i);
		dm(i) = max(d(Jopt(i),L));
	end

return;

%KCENTRESORT Sort objects given by dissimilarity matrix
%
%               N = KCENTRESORT(D,P,CRIT)
%
% INPUT
%   D     Square dissimilarity matrix, zeros on diagonal
%   P     Number of prototypes to be selected
%   CRIT  'dist' or 'centre'
%
% OUTPUT
%   N     Indices of selected prototypes
%
% DESCRIPTION
% Sort objects given by square dissim matrix D using a greedy approach
% such that the maximum NN distance from all objects (prototypes)
% to the first K: max(min(D(:,N(1:K),[],2)) is minimized.
%
% This routines tries to sample the objects such that they are evenly
% spaced judged from their dissimilarities. This may be used as
% initialisation in KCENTRES. It works reasonably, but not very good.
%
% SEE ALSO (PRTools Guide)
% KCENTRES

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function N = kcentresort(d,p,crit);

d = +d;
m = size(d,1);
if nargin < 3, crit = 'dist'; end
if nargin < 2 | isempty(p), p = m; end
L = [1:m];
N = zeros(1,p);
[dd,n] = min(max(d,[],2));  % this is the first (central) prototype
e = d(:,n);                 % store here the distances to the nearest prototype (dNNP)
f = min(d,repmat(e,1,m));   % replace distances that are larger than dNNP by dNNP
N(1) = n;                   % ranking of selected prototypes
L(n) = [];                  % candidate prototypes (all not yet selected objects)

for k=2:p                   % extend prototype set
        if strcmp(crit,'centre')
        [dd,n] = min(max(f(L,L),[],1)); % This selects the next prototype out of candidates in L
        e = min([d(:,L(n)) e],[],2);    % update dNNP
        f = min(d,repmat(e,1,m));       % update replacement of distances that are larger
                                              % than dNNP by dNNP
        elseif strcmp(crit,'dist')
                [dd,n] = max(mean(d(N(find(N > 0)),L)));
        else
                error('Illegal crit')
        end
  N(k) = L(n);                    % update list of selected prototypes
  L(n) = [];                      % update list of candidate prototypes
end


function b = r8mat_l1_inverse ( n, a )

%*****************************************************************************80
%
%% R8MAT_L1_INVERSE inverts a unit lower triangular R8MAT.
%
%  Discussion:
%
%    A unit lower triangular matrix is a matrix with only 1's on the main
%    diagonal, and only 0's above the main diagonal.
%
%    The inverse of a unit lower triangular matrix is also
%    a unit lower triangular matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 October 2005
%
%  Author:
%
%    Original FORTRAN77 version by Albert Nijenhuis, Herbert Wilf.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    A Nijenhuis and H Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, number of rows and columns in the matrix.
%
%    Input, real A(N,N), the unit lower triangular matrix.
%
%    Output, real B(N,N), the inverse matrix.
%
  for i = 1 : n

    for j = 1 : n

      if ( i < j )
        b(i,j) = 0.0;
      elseif ( j == i )
        b(i,j) = 1.0;
      else
        b(i,j) = -( a(i,1:i-1) * b(1:i-1,j) );
      end

    end
  end

  return
end


function [x fx] = newtonArmijo(func,init,arguments,varargin)
%function [x fx] = newton(func,init,arguments,varargin)
% do a newton minimization
% func must return the likelihood and the value of the update -H^{-1}g

[max_its init_alpha thresh debug min_alpha max_inner_its] = process_options(varargin,'max-its',1000,'init-alpha',1,'thresh',1e-8,'debug',0,'min-alpha',1e-15,'max-inner-its',1000);
x = init;
beta = 1e-4; tau = 0.25;

iterator = newIterator(max_its,'thresh',thresh,'debug',debug);
alpha = init_alpha;
while ~iterator.done
    [old_score gradient step] = feval(func,x,arguments{:});

    %do armijo linear-searchto find step size
    new_score = feval(func,x+alpha*step,arguments{:});
    inner_its = 0;
    while inner_its < max_inner_its && alpha > min_alpha && (isnan(new_score) || isinf(new_score) || new_score > old_score + beta * gradient' * (alpha * step))
        %if debug, fprintf('alpha: %.2e -> %.2e\n',alpha,alpha*tau); end
        alpha = alpha * tau;
        new_score = feval(func,x+alpha*step,arguments{:});
        inner_its = inner_its + 1;
    end
    if alpha < min_alpha, alpha == 0; end
    try %try to take a step, if you can
        x = x + alpha * step;
        alpha = alpha ./ tau; %walk back one step
        fx(iterator.its+1) = new_score;
        iterator = updateIterator(iterator,-new_score);
    catch
        iterator.done = true;
    end
end


function [ v, d ] = pts_pca( x, kernel )
% PTS_PCA  Linear and Kernel Principal Component Analysis (PCA and KPCA)
%
% [ V, D ] = PTS_PCA(X)
%
%   Center X and compute eigenvectors V and eigenvalues D using linear PCA.
%
%   X is a matrix where each column is a vector. Each row is a sample of a
%   variable.
%
%   The eigenproblem solved is
%
%     S * v = d * v
%
%   where v is each eigenvector, and d each corresponding eigenvalue. S is
%   the biased covariance matrix
%
%     S = 1/M * Xc * Xc'
%
%   where Xc is the matrix of centered vectors X.
%
%   NOTE: If there are less vectors than variables, or kernel PCA is
%   selected, MDS is used to speed up computation of eigenvectors.
%
%   NOTE: Tiny eigenvalues can give numerical errors, so eigenvalues <
%   1e-13 and the corresponding eigenvectors are removed from the output.
%   Negative eigenvalues are assumed to arise from numerical errors, and
%   removed too.
%
%   NOTE: Matlab's function cov() computes the _unbiased_ covariance.
%
% [ A, DA ] = PTS_PCA(X, KERNEL)
%
%   Generalization to any type of kernel. KERNEL is a struct with the
%   kernel description
%
%     Linear PCA, as above
%
%       KERNEL.type = 'linear'
%
%     Kernel PCA, Polynomial homogeneous order q
%
%       KERNEL.type = 'polyh'
%       KERNEL.params = q
%
%     Kernel PCA, Gaussian with standard deviation sigma
%
%       KERNEL.type = 'gauss'
%       KERNEL.params = sigma
%
%   Because in general we want to avoid any explicit computations in
%   feature space, we solve the eigenproblem [1]
%
%     Kc * a = da * a
%
%   where a is each "coefficient eigenvector", and da = m * d, where m is
%   the number of vectors X. Kc is the kernel matrix of centered feature
%   vectors phi(X).
%
%   If you want to obtain the eigenvectors and eigenvalues in feature space
%   (for those cases where feature space is finite dimensional), you need
%   to compute the feature space training vectors phi(X), center them to
%   phic, and do
%
%     v = phic * a * diag(1./sqrt(da))
%     d = da / m
%
%   where m is the number of training vectors. (Note that v computed as
%   above is equal to v computed from the feature vectors save possibly the
%   sign).
%
% [1] B. Schölkopf, A.J. Smola and K.-R. Müller. "Kernel Principal
% Component Analysis". In "Advances in Kernel Methods - SV Learning", B.
% Schölkopf, C.J.C. Burges and A.J. Smola, eds. pp. 327--352, MIT Press,
% 1999.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2009-2011 University of Oxford
% Version: 1.1.0
% $Rev: 434 $
% $Date: 2011-06-03 17:16:33 +0100 (Fri, 03 Jun 2011) $
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
error( nargchk( 1, 2, nargin, 'struct' ) );
error( nargoutchk( 0, 2, nargout, 'struct' ) );

if ndims( x ) > 2
    error( 'X must be a matrix, not a volume' )
end

% get sizes
%   N: vector length
%   M: number of training vectors
[ N, M ] = size( x );

% defaults
if ( nargin < 2 || isempty( kernel ) )
    kernel.type = 'linear';
end

switch kernel.type

    case 'linear'

        % center training vectors
        xmean = mean( x, 2 );
        x = x - repmat( xmean, 1, M );

        % compute matrix for eigenproblem
        if ( M < N ) % "kernel matrix" trick to speed up computation
            x_cov = x' * x;
        else % normal case, covariance matrix
            x_cov = x * x' / M;
        end

        % compute eigenvalues and eigenvectors
        [ v, d ] = eig( x_cov );

        % save memory by reducing eigenvalues to a vector
        d = diag( d );

        % reoder in decreasing order of signed modulus value
        [~, idx] = sort(abs(d).*sign(d), 1, 'descend');
        d = d(idx);
        v = v(:, idx);

        % if "kernel matrix" trick was used, it is necessary to convert the
        % coefficient eigenvectors and eigenvalues to feature space
        % eigenvectors and eigenvalues
        if ( M < N )

            % scale eigenvectors
            v = x * ( v * diag( 1 ./ sqrt( d ) ) );

            % scale eigenvalues
            d = d / M;

        end

    otherwise

        % for kernel PCA, we use a generalization of the speed up trick
        % above
        k = pts_kmat( kernel, x );

        % center kernel matrix (this is the same as centering the feature
        % space training vectors and then computing the kernel matrix)
        onesm = ones( M ) / M;
        k = k - onesm * k - k * onesm + onesm * k * onesm;

        % k must be symmetric in order to avoid complex eigenvalues, and in
        % theory it should be, but there are small errors due to numeric
        % precision when the centered kernel matrix is computed
        k = ( k + k' ) / 2;

        % compute eigenvalues and eigenvectors
        [ v, d ] = eig( k );

        % save memory by reducing eigenvalues to a vector
        d = diag( d );

        % reoder in decreasing order of signed modulus value
        [~, idx] = sort(abs(d).*sign(d), 1, 'descend');
        d = d(idx);
        v = v(:, idx);

end


function value = c1_geg_monomial_integral ( alpha, expon )

%*****************************************************************************80
%
%% C1_GEG_MONOMIAL_INTEGRAL: integral of monomial with Gegenbauer weight on C1.
%
%  Discussion:
%
%    C1_GEG is the interval [-1,+1] with the Gegenbauer weight function
%
%      w(alpha;x) = (1-x^2)^alpha
%
%    with -1.0 < alpha.
%
%    value = integral ( -1 <= x <= +1 ) x^expon (1-x^2)^alpha dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the exponent of (1-X^2).
%    - 1.0 < ALPHA.
%
%    Input, integer EXPON, the exponent.
%    0 <= EXPON.
%
%    Output, real VALUE, the value of the integral.
%
  if ( alpha <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'C1_GEG_MONOMIAL_INTEGRAL - Fatal error!\n' );
    fprintf ( 1, '  ALPHA <= -1.0\n' );
    error ( 'C1_GEG_MONOMIAL_INTEGRAL - Fatal error!' );
  end

  if ( mod ( expon, 2 ) == 1 )
    value = 0.0;
    return
  end

  c = expon;

  arg1 = - alpha;
  arg2 =   1.0 + c;
  arg3 =   2.0 + alpha + c;
  arg4 = - 1.0;

  value1 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );

  value = 2.0 * gamma ( 1.0 + c ) * gamma ( 1.0 + alpha ) ...
    * value1 / gamma ( 2.0 + alpha  + c );

  return
end


% Stability domain of second order extrapolated BDF scheme

% Copyright 2001 P. Wesseling
% This program and its subprograms may be freely used, modified and distributed
% under the GNU General Public License: http://www.gnu.org/copyleft/gpl.html

% Theory is given in Section 5.8 of
% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program generates Fig. 5.7  in the book

figure(1), clf, hold on

		% Plot of stability domain
theta = 0:0.01:0.5; theta = theta*pi;
a = (6*cos(theta)-9)./(2*cos(2*theta) - 4*cos(theta)) - 3/2;
b = (sin(theta).*(2-cos(theta)))./(cos(2*theta) - 2*cos(theta));
z = - a - i*b; plot(z)

		% Plot of oval
a = 1; b =(2*a/4)^(1/4); theta = 0:0.002:1; theta = theta*pi;
z =  - a*(1-cos(theta)) + i*b*(sin(theta)).^0.5; plot(z,'--')

		% Plot of parabola
yy = 0:0.02:1.8; b = 1; par = -(yy/b).^2; plot(par,yy,'.')


function [tout,yout] = pendulumDE(tVals,omega0,startAngle)

%   Copyright 2008-2009 The MathWorks, Inc.

% Equations of motion
eqnsOfMotion = @(t,x) [x(2); -omega0^2*sin(x(1))];

[tout,yout] = ode45(eqnsOfMotion,tVals,[startAngle; 0]);


function A = sdt_A(H, F)
% Calculate signal detection theory A statistic:
% a non-parametric estimate of sensitivity in ROC analysis
% per Zhang and Mueller (2005)
%
% :Usage:
% ::
%
%     A = sdt_A(H, F)
%
% :Inputs:
%
%   **H:**
%        hit rate
%
%   **F:**
%        false alarm rate
%
% :Outputs:
%
%   **A:**
%        A measure of sensitivity of a detector, essentially the area under
%        the ROC curve for either "normal" (H>=F) or "reverse skill" (F>H)
%        case (ranges from 0.5 to 1).
%
% Meaning of values where F > H ("reverse skill"):
%
% The meaningful measure of sensitivity in this case is A(F,H)
% as opposed to A(H,F).  Subject is assumed to still be sensitive,
% but to have inverted their interpretation (or "reverse skill").
%
% To distinguish from the normal case, such results are returned
% as (1 - sdt_A(F,H)), so the full range from 0 to 1 is used.
% For comparisons of sensitivity use abs(sdt_A(H,F) - 0.5).
%
% :Example:
%
% sdt_A(0.6,0.8) = 0.325. This indicates the same sensitivity as
% sdt_A(0.8,0.6) = 0.675, but with reversed skill.
%
% ..
%    Author: Joe Wielgosz 8/13/2009
% ..

if F <= H % Normal case

    if (F <= 0.5) && (0.5 <= H)
        A = 0.75 + (H-F)/4 - F*(1-H);
    elseif (F <= H) && (H <= 0.5)
        A = 0.75 + (H-F)/4 - F/(4*H);
    else % (0.5 < F) && (F <= H)
        A = 0.75 + (H-F)/4 - (1-H)/(4*(1-F));
    end

else % "Reverse skill" - more false positives than hits
    % Using Jason Buhle's approach
    A = 1 - sdt_A(F, H);
%    A = NaN;
end

end


function [fevd_estimates]=olsfevd(irf_estimates,IRFperiods,gamma,n)

% function [fevd_estimates]=olsfevd(irf_estimates,IRFperiods,gamma,n,endo,datapath)
% computes and displays fevd values for the OLS VAR model
% inputs:  - cell 'irf_estimates': lower bound, point estimates, and upper bound for the IRFs
%          - integer 'IRFperiods': number of periods for IRFs
%          - matrix 'gamma': structural disturbance variance-covariance matrix (defined p 48 of technical guide)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - cell 'endo': list of endogenous variables of the model
%          - string 'datapath': user-supplied path to excel data spreadsheet
% outputs: - cell 'fevd_estimates': lower bound, point estimates, and upper bound for the FEVD

% this function implements the procedure described p55-56

% preliminary tasks
% create the first cell
temp=cell(n,n+1);

% start by filling the first column of every Tij matrix in the cell
% loop over rows of temp
for jj=1:n
   % loop over columns of temp
   for ii=1:n
   % square each element
   temp{jj,ii}(:,1)=irf_estimates{jj,ii}(:,1).^2;
   end
end
% fill all the other entries of the Tij matrices
% loop over rows of temp
for jj=1:n
   % loop over columns of temp
   for ii=1:n
      % loop over remaining columns
      for kk=2:IRFperiods
      % define the column as the square of the corresponding column in orthogonalised_irf_record
      % additioned to the value of the preceeding columns, which creates the cumulation
      temp{jj,ii}(:,kk)=irf_estimates{jj,ii}(:,kk).^2+temp{jj,ii}(:,kk-1);
      end
   end
end
% multiply each matrix in the cell by the variance of the structural shocks
% loop over rows of temp
for ii=1:n
% loop over columns of temp
   for jj=1:n
   % multiply column jj of the matrix by the variance of the structural shock
   temp{ii,jj}(1,:)=temp{ii,jj}(1,:)*gamma(jj,jj);
   end
end

% obtain now the values for Ti, the (n+1)th matrix of each row
% loop over rows of temp
for ii=1:n
% start the summation over Tij matrices
temp{ii,n+1}=temp{ii,1};
   % sum over remaining columns
   for jj=2:n
   temp{ii,n+1}=temp{ii,n+1}+temp{ii,jj};
   end
end

% create the output cell fevd_record
fevd_estimates=cell(n,n);
% fill the cell
% loop over rows of fevd_estimates
for ii=1:n
   % loop over columns of fevd_estimates
   for jj=1:n
   % define the matrix Vfij as the division (pairwise entry) of Tfij by Tfj
   fevd_estimates{ii,jj}=temp{ii,jj}./temp{ii,n+1};
   end
end


function x = complex_lorentz( sx, dim )

%COMPLEX_LORENTZ   Complex second-order cone.
%   COMPLEX_LORENTZ(N), where N is a positive integer, creates a column
%   variable of length N and a scalar variable, and constrains them
%   to lie in a second-order cone. That is, given the declaration
%       variable x(n) complex
%       variable y
%   the constraint
%       {x,y} == complex_lorentz(n)
%   is equivalent to
%       norm(x,2) <= y
%   The inequality form is more natural, and preferred in most cases. But
%   in fact, the COMPLEX_LORENTZ set form is used by CVX itself to convert
%   complex NORM()-based constraints to solvable form.
%
%   COMPLEX_LORENTZ(SX,DIM), where SX is a valid size vector and DIM is a
%   positive integer, creates an array variable of size SX and an array
%   variable of size SY (see below) and applies the second-order cone
%   constraint along dimension DIM. That is, given the declarations
%       sy = sx; sy(min(dim,length(sx)+1))=1;
%       variable x(sx) complex
%       variable y(sy)
%   the constraint
%       {x,y} == complex_lorentz(sx,dim)
%   is equivalent to
%       norms(x,2,dim) <= y
%   Again, the inequality form is preferred, but CVX uses the set form
%   internally. DIM is optional; if it is omitted, the first non-singleton
%   dimension is used.
%
%   LORENTZ(SX,DIM,CPLX) creates real second-order cones if CPLX is FALSE,
%   and complex second-order cones if CPLX is TRUE. The latter case is
%   equivalent to COMPLEX_LORENTZ(SX,DIM).
%
%   Disciplined convex programming information:
%       LORENTZ is a cvx set specification. See the user guide for
%       details on how to use sets.

if nargin == 1,
    x = lorentz( sx, [], true );
else
    x = lorentz( sx, dim, true );
end

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


function [x,y] = snakedeform(x,y,alpha,beta,gamma,kappa,fx,fy,ITER)
% -------In this function, the initial contour of Active Coutour Model(Snake)
%  will be deformed in the given external force field.
%
%     alpha:   elasticity parameter
%     beta:    rigidity parameter
%     gamma:   viscosity parameter
%     kappa:   external force weight
%     fx,fy:   external force field

N = length(x);

alpha = alpha* ones(1,N);
beta = beta*ones(1,N);

% produce the five diagnal vectors
alpham1 = [alpha(2:N) alpha(1)];
alphap1 = [alpha(N) alpha(1:N-1)];
betam1 = [beta(2:N) beta(1)];
betap1 = [beta(N) beta(1:N-1)];

a = betam1;
b = -alpha - 2*beta - 2*betam1;
c = alpha + alphap1 +betam1 + 4*beta + betap1;
d = -alphap1 - 2*beta - 2*betap1;
e = betap1;

% generate the parameters matrix
A = diag(a(1:N-2),-2) + diag(a(N-1:N),N-2);
A = A + diag(b(1:N-1),-1) + diag(b(N), N-1);
A = A + diag(c);
A = A + diag(d(1:N-1),1) + diag(d(N),-(N-1));
A = A + diag(e(1:N-2),2) + diag(e(N-1:N),-(N-2));

invAI = inv(A + gamma * diag(ones(1,N)));

for count = 1:ITER,
   vfx = interp2(fx,x,y,'*linear');
   vfy = interp2(fy,x,y,'*linear');

   % deform snake
   x = invAI * (gamma* x + kappa*vfx);
   y = invAI * (gamma* y + kappa*vfy);
end