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

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

  for tetra = 1 : tetra_num

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

    tetrahedron_quality(tetra) = tetrahedron_quality2_3d ( tetrahedron );

  end

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

  return
end


function p = bspeval(d,c,k,u)
% BSPEVAL  Evaluate B-Spline at parametric points
% -------------------------------------------------------------------------
% ADAPTATION of BSPEVAL from C Routine
% -------------------------------------------------------------------------
%
% Calling Sequence:
%
%   p = bspeval(d,c,k,u)
%
%    INPUT:
%
%       d - Degree of the B-Spline.
%       c - Control Points, matrix of size (dim,nc).
%       k - Knot sequence, row vector of size nk.
%       u - Parametric evaluation points, row vector of size nu.
%
%    OUTPUT:
%
%       p - Evaluated points, matrix of size (dim,nu)
%

nu = numel(u);
[mc,nc] = size(c);
                                                %   int bspeval(int d, double *c, int mc, int nc, double *k, int nk, double *u,int nu, double *p){
                                                %   int ierr = 0;
                                                %   int i, s, tmp1, row, col;
                                                %   double tmp2;
                                                %
                                                %   // Construct the control points
                                                %   double **ctrl = vec2mat(c,mc,nc);
                                                %
                                                %   // Contruct the evaluated points
p = zeros(mc,nu);                               %   double **pnt = vec2mat(p,mc,nu);
                                                %
                                                %   // space for the basis functions
N = zeros(d+1,1);                               %   double *N = (double*) mxMalloc((d+1)*sizeof(double));
                                                %
                                                %   // for each parametric point i
for col=1:nu                                    %   for (col = 0; col < nu; col++) {
                                                %     // find the span of u[col]
    s = findspan(nc-1, d, u(col), k);           %     s = findspan(nc-1, d, u[col], k);
    N = basisfun(s,u(col),d,k);                 %     basisfun(s, u[col], d, k, N);
                                                %
    tmp1 = s - d + 1;                           %     tmp1 = s - d;
    for row=1:mc                                %     for (row = 0; row < mc; row++)  {
        tmp2 = 0;                               %       tmp2 = 0.0;
        for i=0:d                               %       for (i = 0; i <= d; i++)
           tmp2 = tmp2 + N(i+1)*c(row,tmp1+i);  % 	tmp2 += N[i] * ctrl[tmp1+i][row];
        end                                     %
        p(row,col) = tmp2;                      %       pnt[col][row] = tmp2;
    end                                         %     }
end                                             %   }
                                                %
                                                %   mxFree(N);
                                                %   freevec2mat(pnt);
                                                %   freevec2mat(ctrl);
                                                %
                                                %   return ierr;
                                                %   }


function [edge, isInside] = clipRay(ray, bb)
%CLIPRAY Clip a ray with a box.
%
%   EDGE = clipRay(RAY, BOX);
%   RAY is a straight ray given as a 4 element row vector: [x0 y0 dx dy],
%   with (x0 y0) being the origin of the ray and (dx dy) its direction
%   vector, BOX is the clipping box, given by its extreme coordinates:
%   [xmin xmax ymin ymax].
%   The result is given as an edge, defined by the coordinates of its 2
%   extreme points: [x1 y1 x2 y2].
%   If the ray does not intersect the box, [NaN NaN NaN NaN] is returned.
%
%   Function works also if RAY is a N-by-4 array, if BOX is a Nx4 array, or
%   if both RAY and BOX are N-by-4 arrays. In these cases, EDGE is a N-by-4
%   array.
%
%   See also
%     rays2d, boxes2d, edges2d, clipLine, drawRay
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2010-05-13, using Matlab 7.4.0.287 (R2007a)
% Copyright 2010-2022 INRA - Cepia Software Platform

% adjust size of two input arguments
if size(ray, 1) == 1
    ray = repmat(ray, size(bb, 1), 1);
elseif size(bb, 1) == 1
    bb = repmat(bb, size(ray, 1), 1);
elseif size(ray, 1) ~= size(bb, 1)
    error('bad sizes for input');
end

% first compute clipping of supporting line
edge = clipLine(ray, bb);

% detects valid edges (edges outside box are all NaN)
inds = find(isfinite(edge(:, 1)));

% compute position of edge extremities relative to the ray
pos1 = linePosition(edge(inds,1:2), ray(inds,:), 'diag');
pos2 = linePosition(edge(inds,3:4), ray(inds,:), 'diag');

% if first point is before ray origin, replace by origin
edge(inds(pos1 < 0), 1:2) = ray(inds(pos1 < 0), 1:2);

% if last point of edge is before origin, set all edge to NaN
edge(inds(pos2 < 0), :) = NaN;

% eventually returns result about inside or outside
if nargout > 1
    isInside = isfinite(edge(:,1));
end


function J=calcRuvConvJacob(zRUV,useHalfRange,lTx,lRx,M)
%%CALCRUVCONVJACOB Calculate the Jacobian for a monostatic or bistatic
%            range and direction cosines measurement in 3D, ignoring
%            atmospheric effects, with respect to Cartesian position.
%            This type of Jacobian is useful when performing tracking using
%            Cartesian-converted measurements where the clutter density is
%            specified in the measurement coordinate system, not the
%            converted measurement coordinate system.
%
%INPUTS: zRUV A 3X1 point in bistatic range and u and v coordinates in the
%           format [bistatic range;u;v].
% useHalfRange A boolean va0lue specifying whether the bistatic range value
%           should be divided by two. This normally comes up when operating
%           in monostatic mode, so that the range reported is a one-way
%           range. The default if this parameter is not provided, or an
%           empty matrix is passed, is false.
%       lTx The 3X1 [x;y;z] location vector of the transmitter in global
%           Cartesian coordinates. If this parameter is omitted or an
%           empty matrix is passed, then the transmitter is assumed to be
%           at the origin.
%       lRx The 3X1 [x;y;z] location vector of the receiver in Cartesian
%           coordinates. If this parameter is omitted or an empty matrix
%           is passed, then the receiver is assumed to be at the origin.
%         M A 3X3 rotation matrix to go from the alignment of the global
%           coordinate system to that at the receiver. The z-axis of the
%           local coordinate system of the receiver is the pointing
%           direction of the receiver. If omitted or an empty matrix is
%           passed, then it is assumed that the local coordinate system is
%           aligned with the global and M=eye(3) --the identity matrix is
%           used.
%
%OUTPUTS: J The 3X3 Jacobian matrix. Each row is a components of
%           [range;u;v] in that order with derivatives taken with respect
%           to [x,y,z] by column.
%
%This function converts the measurement into Cartesian coordinates and then
%calls rangeGradient and uvGradient.
%
%February 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(M))
    M=eye(3,3);
end

if(nargin<4||isempty(lRx))
    lRx=zeros(3,1);
end

if(nargin<3||isempty(lTx))
    lTx=zeros(3,1);
end

if(nargin<2||isempty(useHalfRange))
    useHalfRange=false;
end

x=ruv2Cart(zRUV,useHalfRange,lTx,lRx,M);

J=zeros(3,3);
J(1,:)=rangeGradient(x,useHalfRange,lTx,lRx);
J(2:3,:)=uvGradient(x,lRx,M);

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 [ vX ] = ConsensusAdmm( cProxFun, numElements, paramRho, numIterations, stopThr )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = ConsensusAdmm( cProxFun, numElements, paramRho, numIterations, stopThr )
%   Solves the Consensus ADMM problem given the set of Proximal Operators.
% Input:
%   - cProxFun      -   Set of Proximal Operators.
%                       Each cell elemnts is the i-th proximal operator.
%                       Each function handler input is a (vV, paramLambda).
%                       The implementation is for the prox in its lambda
%                       form: 0.5 * || x - v ||^2 + paramLamba * g(x).
%                       Structure: Cell Array (numSets x 1).
%                       Type: Function Handler.
%                       Range: NA.
% Output:
%   - vX            -   Solution Vector.
%                       The solution to the optimization problem..
%                       Structure: Vector (m x 1).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  ELE 522 - Large Scale Optimization for Data Science - Alternating Direction Method of Multipliers.
% Remarks:
%   1.  Pay attention that the references are using (1 / paramLambda) form
%       of the Prox. Hence the adaptation in the code.
%   2.  This is ultra efficient way to solve the Orthogonal Projection
%       Problem in case having projection onto sets which the intersection
%       is the objective set.
% TODO:
%   1.  C
% Release Notes:
%   -   1.0.000     25/03/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

numSets     = size(cProxFun, 1);
% numElements = size(vX, 1);

paramRhoInv = 1 / paramRho;

mX      = zeros(numElements, numSets);
vZ      = mean(mX, 2);
mLambda = zeros(numElements, numSets);

for ii = 1:numIterations
    for jj = 1:numSets
        mX(:, jj) = cProxFun{jj}(vZ - (paramRhoInv * mLambda(:, jj)), paramRhoInv);
    end

    vZ = mean(mX + (paramRhoInv * mLambda), 2);

    for jj = 1:numSets
        mLambda(:, jj) = mLambda(:, jj) + (paramRho * (mX(:, jj) - vZ)); %<! Can be done mLambda = mLambda + (paramRho * (mX - vZ));
    end

    % To calculate the difference from the previous iteration.
    stopCond = max(abs(mX - vZ), [], 'all') < stopThr;

    if(stopCond)
        break;
    end
end

vX = mean(mX, 2);

end


function [r,v] = keplerUniversal(r0,v0,t,mu)
%Purpose:
%Most effecient way to propagate any type of two body orbit using kepler's
%equations.
%-------------------------------------------------------------------------%
%                                                                         %
% Inputs:                                                                 %
%--------
%r_ECI                  [3 x N]                         Position Vector in
%                                                       ECI coordinate
%                                                       frame of reference
%
%v_ECI                  [3 x N]                         Velocity vector in
%                                                       ECI coordinate
%                                                       frame of reference
%
%t                      [1 x N]                         time vector in
%                                                       seconds
%
%mu                     double                          Gravitational Constant
%                                                       Defaults to Earth if
%                                                       not specified
% Outputs:
%---------                                                                %
%r_ECI                  [3 x N]                         Final position
%                                                       vector in ECI
%
%v_ECI                  [3 x N]                         Final velocity
%                                                       vector in ECI
%--------------------------------------------------------------------------
% Programmed by Darin Koblick 03-04-2012                                  %
%--------------------------------------------------------------------------
if ~exist('mu','var'); mu = 398600.4418; end
tol = 1e-9;
v0Mag = sqrt(sum(v0.^2,1));  r0Mag = sqrt(sum(r0.^2,1));
alpha = -(v0Mag.^2)./mu + 2./r0Mag;
%% Compute initial guess (X0) for Newton's Method
X0 = NaN(size(t));
%Check if there are any Eliptic/Circular orbits
idx = alpha > 0.000001;
if any(idx)
    X0(idx) = sqrt(mu).*t(idx).*alpha(idx);
end
%Check if there are any Parabolic orbits
idx = abs(alpha) < 0.000001;
if any(idx)
   h = cross(r0(:,idx),v0(:,idx)); hMag = sqrt(sum(h.^2,1));
   p = (hMag.^2)./mu; s = acot(3.*sqrt(mu./(p.^3)).*t(idx))./2;
   w = atan(tan(s).^(1/3)); X0(idx) = sqrt(p).*2.*cot(2.*w);
end
%Check if there are any Hyperbolic orbits
idx = alpha < -0.000001;
if any(idx)
   a = 1./alpha(idx);
   X0(idx) = sign(t(idx)).*sqrt(-a).*...
       log(-2.*mu.*alpha(idx).*t(idx)./ ...
       (dot(r0(:,idx),v0(:,idx))+sign(t(idx)).*sqrt(-mu.*a).*...
       (1-r0Mag(idx).*alpha(idx))));
end
%% Newton's Method to converge on solution
% Declare Constants that do not need to be computed within the while loop
err = Inf;
dr0v0Smu = dot(r0,v0)./sqrt(mu);
Smut = sqrt(mu).*t;
while any(abs(err) > tol)
    X02 = X0.^2;
    X03 = X02.*X0;
    psi = X02.*alpha;
    [c2,c3] = c2c3(psi);
    X0tOmPsiC3 = X0.*(1-psi.*c3);
    X02tC2 = X02.*c2;
    r = X02tC2 + dr0v0Smu.*X0tOmPsiC3 + r0Mag.*(1-psi.*c2);
    Xn = X0 + (Smut-X03.*c3-dr0v0Smu.*X02tC2-r0Mag.*X0tOmPsiC3)./r;
    err = Xn-X0; X0 = Xn;
end
f = 1 - (Xn.^2).*c2./r0Mag; g = t - (Xn.^3).*c3./sqrt(mu);
gdot = 1 - c2.*(Xn.^2)./r; fdot = Xn.*(psi.*c3-1).*sqrt(mu)./(r.*r0Mag);
r = bsxfun(@times,f,r0) + bsxfun(@times,g,v0);
v = bsxfun(@times,fdot,r0) + bsxfun(@times,gdot,v0);
%% Ensure Solution Integrity
%idx = round((f.*gdot - fdot.*g)./tol).*tol ~= 1; r(:,idx) = NaN; v(:,idx) = NaN;
end

function [c2,c3] = c2c3(psi)
%Vallado pg. 71 Algorithm 1
c2 = NaN(size(psi));
c3 = NaN(size(psi));
idx = psi > 1e-6;
if any(idx)
    c2(idx) = (1-cos(sqrt(psi(idx))))./psi(idx);
    c3(idx) = (sqrt(psi(idx))-sin(sqrt(psi(idx))))./sqrt(psi(idx).^3);
end
idx = psi < -1e-6;
if any(idx)
    c2(idx) = (1 - cosh(sqrt(-psi(idx))))./psi(idx);
    c3(idx) = (sinh(sqrt(-psi(idx)))-sqrt(-psi(idx)))./sqrt(-psi(idx).^3);
end
idx = abs(psi) <= 1e-6;
if any(idx)
    c2(idx) = 0.5;
    c3(idx) = 1/6;
end
end


%DEMO_HURDLE Demonstration of Logit Negative-binomial hurdle model
%            using Gaussian process prior
%
%  Description
%    Hurdle models can be used to model excess number of zeros
%    compared to usual Poisson and negative binomial count models.
%    Hurdle models assume a two-stage process, where the first
%    process determines whether the count is larger than zero, and
%    the second process determines the non-zero count. Both
%    processes are given a zero mean Gaussian process prior. The
%    two stage model formulation makes it possible to make
%    inference for the two latent processes separately.
%
%    In this demo we construct logit negative binomial hurdle model
%    by using logit and zero truncated negative binomial models.
%    The posterior inference is made with parallel-EP
%    approximation.
%
%    See also  DEMO_SPATIAL2, DEMO_CLASSIFIC1

% Copyright (c) 2008-2010 Jarno Vanhatalo
% Copyright (c) 2010-2011 Aki Vehtari
% Copyright (c) 2011 Jaakko Riihimäki

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

% load the data
S = which('demo_spatial1');
data = load(strrep(S,'demo_spatial1.m','demodata/spatial1.txt'));

x = data(:,1:2);
ye = data(:,3);
y = data(:,4);

x0=x;
x=bsxfun(@rdivide,bsxfun(@minus,x0,mean(x0)),std(x0));

% for zero-process
yz=double(y>0)*2-1;
% for count-process
ci=find(y>0);
yc=y(ci);
xc=x(ci,:);
yec=ye(ci,:);

% Create the covariance functions
pl = prior_t('s2',10);
pm = prior_sqrtunif();
cf = gpcf_matern32('lengthScale', 5, 'magnSigma2', 0.05, ...
                   'lengthScale_prior', pl, 'magnSigma2_prior', pm);

% Create the zero part
likz=lik_probit();
gpz=gp_set('lik',likz,'cf',cf,'jitterSigma2',1e-6,'latent_method','EP','latent_opt',struct('parallel','on'));
% Create the count part
likc=lik_negbinztr();
gpc=gp_set('lik',likc,'cf',cf,'jitterSigma2',1e-6,'latent_method','EP','latent_opt',struct('parallel','on'));
% Note that although both parts use 'cf', they have separate hyperparameters

% Set the options for the quasi-Newton optimization
opt=optimset('TolFun',1e-2,'TolX',1e-2,'Display','iter');
gpz=gp_optim(gpz,x,yz,'opt',opt,'optimf',@fminlbfgs);
gpc=gp_optim(gpc,xc,yc,'z',yec,'opt',opt,'optimf',@fminlbfgs);

% make prediction to the data points
[Efz, Varfz] = gp_pred(gpz, x, yz, x);
[Efc, Varfc] = gp_pred(gpc, xc, yc, xc, 'z', yec);

% Define help parameters for plotting
xii=sub2ind([60 35],x0(:,2),x0(:,1));
[X1,X2]=meshgrid(1:35,1:60);

% Plot the figures
figure
subplot(1,2,1)
G=repmat(NaN,size(X1));
G(xii)=Efz(:);
pcolor(X1,X2,G),shading flat
colorbar
axis equal
axis([0 35 0 60])
title('Posterior mean of latent zero process')

subplot(1,2,2)
G=repmat(NaN,size(X1));
G(xii(ci))=Efc(:);
pcolor(X1,X2,G),shading flat
colorbar
axis equal
axis([0 35 0 60])
title('Posterior mean of latent count process')


function t = bartlett_unit_sample ( m, df )

%*****************************************************************************80
%
%% BARTLETT_UNIT_SAMPLE samples the unit Bartlett distribution.
%
%  Discussion:
%
%    If the matrix T is sampled from the unit Bartlett distribution, then
%    the matrix W = T' * T is a sample from the unit Wishart distribution.
%
%    This function requires functions from the PDFLIB and RNGLIB libraries.
%
%    The "initialize()" function from RNGLIB must be called before using
%    this function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Patrick Odell, Alan Feiveson,
%    A numerical procedure to generate a sample covariance matrix,
%    Journal of the American Statistical Association,
%    Volume 61, Number 313, March 1966, pages 199-203.
%
%    Stanley Sawyer,
%    Wishart Distributions and Inverse-Wishart Sampling,
%    Washington University,
%    30 April 2007, 12 pages.
%
%  Parameters:
%
%    Input, integer M, the order of the matrix.
%
%    Input, integer DF, the number of degrees of freedom.
%    M <= DF.
%
%    Output, real T(M,M), the sample matrix from the unit Bartlett distribution.
%
  if ( df < m )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BARTLETT_UNIT_SAMPLE - Fatal error!\n' );
    fprintf ( 1, '  DF = %d < M = %d.\n', df, m );
    error ( 'BARTLETT_UNIT_SAMPLE - Fatal error!\n' );
  end

  t = zeros ( m, m );

  for i = 1 : m - 1
    df_chi = df - i + 1;
    t(i,i) = sqrt ( r8_chi_sample ( df_chi ) );
    for j = i + 1 : m
      t(i,j) = r8_normal_01_sample ( );
    end
  end

  return
end


function yv = dif_vals ( nd, xd, yd, nv, xv )

%*****************************************************************************80
%
%% DIF_VALS evaluates a divided difference polynomial at a set of points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carl deBoor,
%    A Practical Guide to Splines,
%    Springer, 2001,
%    ISBN: 0387953663,
%    LC: QA1.A647.v27.
%
%  Parameters:
%
%    Input, integer ND, the order of the difference table.
%
%    Input, real XD(ND), the X values of the difference table.
%
%    Input, real YD(ND), the divided differences.
%
%    Input, integer NV, the number of evaluation points.
%
%    Input, real XV(NV), the evaluation points.
%
%    Output, real YV(NV), the value of the divided difference
%    polynomial at the evaluation points.
%

%
%  Deal with the blasted curse of row vectors.
%
  yd = yd ( : );
  xd = xd ( : );
  xv = xv ( : );

  yv = zeros ( nv, 1 );

  yv(1:nv) = yd(nd);

  for i = 1 : nd - 1
    yv(1:nv) = yd(nd-i) + ( xv(1:nv) - xd(nd-i) ) .* yv(1:nv);
  end

  return
end


% MAIN  --  Quad-Rotor  --  Minimal-Acceleration trajectory
%
% Fin the minimal acceleration-squared trajectory to move the quad-rotor from an
% arbitrary state to the origin.
%
% NOTES:
%   X = [x;y;q] = [x pos, y pos, angle] = configuration
%  dX = [dx;dy;dq] = [x vel, y vel, angle rate] = rate
% ddX = [ddx;ddy;ddq] = acceleration
%
% PROBLEM:
%
% ddX = f(X,dX,u);     <-- dynamics
%
% cost = integral(  ddX^2  );     <-- cost function
%
% subject to:
%   X(0) = X0;
%   X(1) = XF;
%   dX(0) = dX0;
%   dX(1) = dXF;
%
% How to pose as a standard trajectory optimization problem?
%
% dX = V1;
% dV1 = f(X,V1,U1)
%
% V2 == V1;   % <-- Key line.
% dV2 = U2;
% cost = integral(  U2^2  );
%
% z = [X;V1;V2]
% u = [U1;U2]
%

clc; clear;

addpath ../../

% Dynamics paramters
p.g = 9.81; % (m/s^2) gravity
p.d = 0.3;  % (m) half-width of quad rotor
p.m = 0.2;  % (m) half-mass of the quad rotor

% Trajectory Parameters:
duration = 1;
uMax = 5*p.g*p.m;

% Initial State:
X0 = [1;0;0];   %  initial configuration
dX0 = zeros(3,1);  % initial rates
z0 = [X0; dX0; dX0];  % initial state

XF = [0;0;0];   % final configuration
dXF = zeros(3,1);  % final rates
zF = [XF; dXF; dXF];  % final state

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                     Set up function handles                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
w = 1./[1,1,1];  %weighting vector for path objective

problem.func.dynamics = @(t,z,u)( dynAcc(z,u,p) );
problem.func.pathObj = @(t,z,u)( pathObj(u,w) );  %accel-squared cost function
problem.func.pathCst = @(t,z,u)( pathCst(z) );

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                     Set up problem bounds                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

problem.bounds.initialTime.low = 0;
problem.bounds.initialTime.upp = 0;
problem.bounds.finalTime.low = duration;
problem.bounds.finalTime.upp = duration;

problem.bounds.initialState.low = z0;
problem.bounds.initialState.upp = z0;
problem.bounds.finalState.low = zF;
problem.bounds.finalState.upp = zF;

problem.bounds.control.low = [-uMax*[1;1];  -inf(3,1)];   %[torque, accel]
problem.bounds.control.upp = [uMax*[1;1];  inf(3,1)];

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                    Initial guess at trajectory                          %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

problem.guess.time = [0,duration];
problem.guess.state = [z0, zF];
problem.guess.control = [p.g*p.m*ones(2,2); zeros(3,2)];

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                         Solver options                                  %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

problem.options.nlpOpt = optimset(...
    'Display','iter',...
    'MaxFunEvals',1e5);

% problem.options.method = 'trapezoid';
% problem.options.trapezoid.nGrid = 40;
%
% problem.options.method = 'hermiteSimpson';
% problem.options.hermiteSimpson.nSegment = 10;

problem.options.method = 'chebyshev';
problem.options.chebyshev.nColPts = 15;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                            Solve!                                       %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

soln = optimTraj(problem);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Display Solution                                 %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

%%%% Unpack the simulation
t = linspace(soln(end).grid.time(1), soln(end).grid.time(end), 150);

z = soln(end).interp.state(t);
x = z(1,:);
y = z(2,:);
q = z(3,:);
dx = z(4,:);
dy = z(5,:);
dq = z(6,:);

X1 = z(1:3,:);
V1 = z(4:6,:);
V2 = z(7:9,:);

u = soln(end).interp.control(t);
u1 = u(1,:);
u2 = u(2,:);
A2 = u(3:5,:);

[dObj,uStar] = pathObj(u,w);

%%%% Plots:

%%%% Plots:

figure(2); clf;

subplot(2,2,1)
plot(t,X1);
legend('x','y','q')
title('configuration')

subplot(2,2,3)
plot(t,V2);
legend('x','y','q')
title('rates')

subplot(2,2,2)
plot(t,A2);
legend('x','y','q')
title('acceleration')

subplot(2,2,4); hold on;
plot(t,u1);  plot(t,u2);
title('actuators')
legend('u1','u2');

% Configuration trajectories
figure(1); clf;

subplot(2,2,1); hold on;
plot(t,x);
xlabel('t')
ylabel('x')
title('Minimum acceleration-squared trajectory')

subplot(2,2,2); hold on;
plot(t,y);
xlabel('t')
ylabel('y')

subplot(2,2,3); hold on;
plot(t,q);
xlabel('t')
ylabel('q')

subplot(2,2,4); hold on;
plot(t,u1);  plot(t,u2);
xlabel('t')
ylabel('u')
legend('u1','u2');


%otyp==0->cont, otyp==1->disc
function [A N]=sys_bd_dcm(llh, vel_b, Cbn, gyro, otyp, dt)
% position   (1-3)
% velocity   (4-6)
% attitude   (7-9)

%system disturbance coefs
N=zeros(9,6);
N(7:9,4:6)=-Cbn; %attitude
N(4:6,1:3)=eye(3); %velocity
N(4:6,4:6)=skew(vel_b); %velocity

%system matrix
A=zeros(9);
[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(llh);
tL=sL/cL;
Rn_h=Rn+llh(3);
Re_h=Re+llh(3);
R=diag([1/Rn_h,1/Re_h,-1]);
O=[0 1/Re_h 0;-1/Rn_h 0 0;0 -tL/Re_h 0];
vel_n=Cbn*vel_b;
wen_n=[vel_n(2)/Re_h; -vel_n(1)/Rn_h; -vel_n(2)*tL/Re_h];
wie_n=[WIE_E*cL; 0; -WIE_E*sL];
win_n=wen_n+wie_n;

%Position
A(1:3,4:6)=R*Cbn;
A(1:3,7:9)=R*skew(vel_n);
A(1,3)=-vel_n(1)/Rn_h^2;
A(2,1)=vel_n(2)*tL/Re_h/cL;
A(2,3)=-vel_n(2)/Re_h^2/cL;

%Velocity
A(4:6,4:6)=-skew(Cbn'*wie_n+gyro);
A(4:6,7:9)=Cbn'*skew([0;0;-g])+skew(vel_b)*Cbn'*skew(wie_n);
A(4:6,1)=skew(vel_b)*Cbn'*[-WIE_E*sL;0;-WIE_E*cL];

%Attitude
A(7,1)=-WIE_E*sL;
A(7,3)=-vel_n(2)/Re_h^2;
A(8,3)=vel_n(1)/Rn_h^2;
A(9,1)=-(WIE_E*cL+vel_n(2)/Re_h/cL/cL);
A(9,3)=vel_n(2)*tL/Re_h^2;
A(7:9,4:6)=O*Cbn;
A(7:9,7:9)=-skew(win_n)+O*skew(vel_n);

%discretize A;
if (otyp==0)
    A=A;
elseif(otyp==1)
    A=expm(A*dt);
end


function [gd,nlen]=ptpfundual(w,a,M,L,varargin)
%PTPFUNDUAL Sampled, periodized dual TP function of finite type
%   Usage: gd=ptpfundual(w,a,M,L)
%          gd=ptpfundual({w,width},a,M,L)
%          gd=ptpfundual(...,inc)
%          [gd,nlen]=ptpfundual(...)
%
%   Input parameters:
%         w      : Vector of reciprocals $w_j=1/\delta_j$ in Fourier representation of *g*
%         width  : Integer stretching factor for the essential support
%         a      : Length of time shift.
%         M      : Number of channels.
%         L      : Window length.
%         inc    : Extension parameter
%
%   Output parameters:
%         gd    : The periodized totally positive function dual window.
%         nlen  : Number of non-zero elements in gd.
%
%   `ptpfundual(w,a,M,L)` computes samples of a dual window of totally
%   positive function of finite type >=2 with weights *w*. Please see
%   |ptpfun| for definition of totally positive functions.
%   The lattice parameters $a,M$ must satisfy $M > a$ to ensure the
%   system is a frame.
%
%   `ptpfundual({w,width},a,M,L)` works as above but in addition the *width*
%   parameter determines the integer stretching factor of the original TP
%   function. For explanation see help of |ptpfun|.
%
%   `ptpfundual(...,inc)` or `ptpfundual(...,'inc',inc)` works as above,
%   but integer *inc* denotes number of additional columns to compute window
%   function *gd*. 'inc'-many are added at each side. It should be smaller
%   than 100 to have comfortable execution-time. The higher the number the
%   closer *gd* is to the canonical dual window.
%   The default value is 10.
%
%   `[gd,nlen]=ptpfundual(...)` as *gd* might have a compact support,
%   *nlen* contains a number of non-zero elements in *gd*. This is the case
%   when *gd* is symmetric. If *gd* is not symmetric, *nlen* is extended
%   to twice the length of the longer tail.
%
%   If $nlen = L$, *gd* has a 'full' support meaning it is a periodization
%   of a dual TP function.
%
%   If $nlen < L$, additional zeros can be removed by calling
%   `gd=middlepad(gd,nlen)`.
%
%   Examples:
%   ---------
%
%   The following example compares dual windows computed using 2 different
%   approaches.:::
%
%     w = [-3,-1,1,3];a = 25; M = 31; inc = 10;
%     L = 1e6; L = dgtlength(L,a,M);
%     width = M;
%
%     % Create the window
%     g = ptpfun(L,w,width);
%
%     % Compute a dual window using pebfundual
%     tic
%     [gd,nlen] = ptpfundual({w,width},a,M,L,inc);
%     ttpfundual=toc;
%
%     % We know that gd has only nlen nonzero samples, lets shrink it.
%     gd = middlepad(gd,nlen);
%
%     % Compute the canonical window using gabdual
%     tic
%     gdLTFAT = gabdual(g,a,M,L);
%     tgabdual=toc;
%
%     fprintf('PTPFUNDUAL elapsed time %f s\n',ttpfundual);
%     fprintf('GABDUAL elapsed time    %f s\n',tgabdual);
%
%     % Test on random signal
%     f = randn(L,1);
%
%     fr = idgt(dgt(f,g,a,M),gd,a,numel(f));
%     fprintf('Reconstruction error PTPFUNDUAL: %e\n',norm(f-fr)/norm(f));
%
%     fr = idgt(dgt(f,g,a,M),gdLTFAT,a,numel(f));
%     fprintf('Reconstruction error GABDUAL:    %e\n',norm(f-fr)/norm(f));
%
%   See also: dgt, idgt, ptpfun
%
%   References: grst13 kl12 bagrst14 klst14

% AUTHORS: Joachim Stoeckler, Tobias Kloos, 2012-2014

complainif_notenoughargs(nargin,4,upper(mfilename));
complainif_notposint(L,'L',upper(mfilename));
complainif_notposint(a,'a',upper(mfilename));
complainif_notposint(M,'M',upper(mfilename));

% Check lattice
if M<=a
    error('%s: Lattice parameters must satisfy M>a.',upper(mfilename));
end

% Check w
if iscell(w)
    if numel(w)~=2
        error('%s: w must be a 2 element cell array.',upper(mfilename));
    end
    width = w{2};
    w = w{1};
    complainif_notposint(width,'width',upper(mfilename));
else
    width = floor(sqrt(L));
end

if isempty(w) || ~isnumeric(w) || numel(w)<2
    error(['%s: w must be a nonempty numeric vector with at least',...
    ' 2 elements.'], upper(mfilename));
end

if any(w==0)
    error('%s: All weights w must be nonzero.', upper(mfilename));
    % TO DO: Also add a warning if w is very small or big?
end

% Define initial value for flags and key/value pairs.
%definput.import={'setnorm'};
definput.keyvals.inc = 10;
%definput.flags.scale = {'nomatchscale','matchscale'};
[flags,~,inc]=ltfatarghelper({'inc'},definput,varargin);
complainif_notnonnegint(inc,'inc',upper(mfilename));

% TP functions are scale invariant so we do scaling directly on w.
wloc = w/width;
% Converting a, M to alpha, beta
alpha = a;
beta = 1/M;

% check alpha beta
if (alpha<=0) || (beta<=0)
    error('lattice parameters alpha, beta must be positive')
end
if (width*beta > 10)
    warning('width/M should be smaller than 10: numerical instability may occur')
end

% compute m n and check that a has nonzero entries
if all(wloc<0)
    wloc = -wloc;
    case0 = 2;
else
    case0 = 0;
end
m = length(find(wloc>0));
n = length(find(wloc<0));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% preparations specially for computation of gd
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r = floor(1/(1-alpha*beta)+eps);

% check special cases according to n and m
if n == 0
    if m >= 2
        k2 = (m-1)*(r+1)-1;
        k1 = -k2;
        if case0 == 0
            case0 = 1;
        end
    end
elseif n == 1
    N = m*(r+1)+1;       % minimal column size
    k1 = -m*(r+1)+1;     % column index k1 from the paper
    k2 = k1+N-1;         % column index k2 from the paper
else
    N = (m+n-1)*(r+1);   % minimal column size
    k1 = -m*(r+1)+1;     % column index k1 from the paper
    k2 = k1+N-1;         % column index k2 from the paper
end

k1 = k1-inc;
k2 = k2+inc;

% minimal values for x and y
varl = floor((k1+m-1)/(alpha*beta))-1;
varr = ceil((k2-n+1)/(alpha*beta))+1;
x = varl*alpha:alpha:varr*alpha;
i0 = abs(varl)+1; % index of "central" row of P(x)
y = (k1-1)/beta:(1/beta):(k2+1)/beta;
k0 = abs(k1-1)+1; % index of "central" column of P(x)

[yy,xx] = meshgrid(y,x);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% discretization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t = 0:(a-1); % for stepping through the interval [0,alpha)
tt = varl*alpha:varr*alpha; % choose same stepsize for t and tt
% left and right bounds large enough for the support of gamma
tt0 = abs(varl*a)+1; % index for tt == 0
gd = zeros(1,length(tt)); % dual window

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% computation of gamma
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

k1 = k1+k0;
k2 = k2+k0;

for k=1:length(t)
    % step through the interval [0,alpha)
    x0 = t(k); % compute dual window at points (x+j*alpha)

    % row indices for rectangular P0
    i1 = floor((k1-k0+m-1)/alpha/beta-x0/alpha)+1+i0;
    i2 = ceil((k2-k0-n+1)/alpha/beta-x0/alpha)-1+i0;

    % Computation of P0(x0)
    % z0 is the matrix of the abscissa x0+j*alpha-k/beta, j=i1:i2, k=k1:k2,
    % z1 puts all these abscissae into a row vector.
    % The computation of g(z1) is done as described above for the
    % vector tt.
    z0 = x0+xx(:,k1:k2)-yy(:,k1:k2);
    z1 = z0(:)';

    Y = zeros((n+m)-1,length(z1));
    for q = 1:(n+m)-1
        if wloc(q) == wloc(q+1)
            Y(q,:) = abs(wloc(q))^2*abs((z1.*((wloc(q)*z1)>=0))).*exp(-wloc(q)*(z1.*((wloc(q)*z1)>=0))).*((wloc(q)*z1)>=0);
        else
            if wloc(q)*wloc(q+1) < 0
                Y(q,:) = abs(wloc(q)*wloc(q+1))/(abs(wloc(q))+abs(wloc(q+1)))*(exp(-wloc(q)*(z1.*((wloc(q)*z1)>=0))).*((wloc(q)*z1)>=0) + ...
                exp(-wloc(q+1)*(z1.*((wloc(q+1)*z1)>=0))).*((wloc(q+1)*z1)>0));
            else
                Y(q,:) = wloc(q)*wloc(q+1)/(abs(wloc(q+1))-abs(wloc(q)))*(exp(-wloc(q)*(z1.*((wloc(q)*z1)>=0)))-exp(-wloc(q+1)*(z1.*((wloc(q)*z1)>=0)))).*((wloc(q)*z1)>=0);
            end
        end
    end

    for q = 2:(n+m)-1
        for j = 1:(n+m)-q
            if wloc(j) == wloc(j+q)
                Y(j,:) = Y(j,:).*abs(z1)/q*abs(wloc(j));
            else
                Y(j,:) = (wloc(j)*Y(j+1,:)-wloc(j+q)*Y(j,:))/(wloc(j)-wloc(j+q));
            end
        end
    end

    if (n+m) == 1
        A0 = abs(wloc)*exp(-wloc*(z1.*((wloc*z1)>=0))).*((wloc*z1)>=0);
    else
        A0 = Y(1,:);
    end

    A0 = reshape(A0,size(z0))*sqrt(width);%*L^(1/4);
    P0 = A0(i1:i2,:);

    % computation of pseudo-inverse matrix of P0
    P0inv = pinv(P0);
    gd(k-1+tt0-a*(i0-i1):a:k-1+tt0+a*(i2-i0)) = beta*P0inv(k0-k1+1,:); % row index k0-k1a+1
    % points to the "j=0" row of P0inv
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% periodization of gamma
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nlen = length(gd);
nr = ceil(nlen/L);
v = zeros(1,nr*L);
v(1:length(gd)) = gd;
v = [v(tt0:end),v(1:tt0-1)];

gd = sum(reshape(v,L,nr),2);
gd = gd(:);
if case0 == 2
    gd =  flipud(gd);
    gd = [gd(end);gd(1:end-1)];
end

% Determine nlen
if nlen<L
    negsupp = tt0-1;
    possupp = nlen-tt0; % excluding zero pos.
    nlen = 2*max([negsupp,possupp])+1;
end
nlen = min([L,nlen]);

% if flags.do_matchscale
%    g = ptpfun(L,w,width,flags.norm);
%    [scal,err] = gabdualnorm(g,gd,a,M,L);
%     assert(err<1e-10,sprintf(['%s: Assertion failed. This is not a valid ',...
%                               ' dual window.'],upper(mfilename)));
%    gd = gd/scal;
% else
%    gd = setnorm(gd,flags.norm);
% end

gd = gd(:);

end


function indx = multigrid_index_one ( dim_num, order_1d, order_nd )

%*****************************************************************************80
%
%% MULTIGRID_INDEX_ONE returns an indexed multidimensional grid.
%
%  Discussion:
%
%    For dimension DIM, the number of points is ORDER_1D(DIM).
%
%    We index the points as
%      1,   2,   3,   ...,  ORDER_1D(DIM).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension of the points.
%
%    Input, integer ORDER_1D(DIM_NUM), the order of the
%    rule in each dimension.
%
%    Input, integer ORDER_ND, the product of the entries of ORDER_1D.
%
%    Output, integer INDX(DIM_NUM,ORDER_ND), the indices of the points in
%    the grid.  The second dimension of this array is equal to the
%    product of the entries of ORDER_1D.
%
  a = [];
  more = 0;
  p = 0;

  while ( 1 )

    [ a, more ] = vec_colex_next2 ( dim_num, order_1d, a, more );

    if ( ~more )
      break
    end

    p = p + 1;
%
%  The values of A(DIM) are between 0 and ORDER_1D(DIM)-1 = N - 1 = 2 * M.
%  Subtracting M sets the range to -M to +M, as we wish.
%
    indx(1:dim_num,p) = a(1:dim_num) + 1;

  end

  return
end


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

%*****************************************************************************80
%
%% GOODWIN_VALUES returns some values of the Goodwin and Staton function.
%
%  Discussion:
%
%    The function is defined by:
%
%      GOODWIN(x) = Integral ( 0 <= t < infinity ) exp ( -t^2 ) / ( t + x ) dt
%
%    The data was reported by McLeod.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Allan McLeod,
%    Algorithm 757, MISCFUN: A software package to compute uncommon
%      special functions,
%    ACM Transactions on Mathematical Software,
%    Volume 22, Number 3, September 1996, pages 288-301.
%
%  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 = 20;

  fx_vec = [ ...
      0.59531540040441651584E+01, ...
      0.45769601268624494109E+01, ...
      0.32288921331902217638E+01, ...
      0.19746110873568719362E+01, ...
      0.96356046208697728563E+00, ...
      0.60513365250334458174E+00, ...
      0.51305506459532198016E+00, ...
      0.44598602820946133091E+00, ...
      0.37344458206879749357E+00, ...
      0.35433592884953063055E+00, ...
      0.33712156518881920994E+00, ...
      0.29436170729362979176E+00, ...
      0.25193499644897222840E+00, ...
      0.22028778222123939276E+00, ...
      0.19575258237698917033E+00, ...
      0.17616303166670699424E+00, ...
      0.16015469479664778673E+00, ...
      0.14096116876193391066E+00, ...
      0.13554987191049066274E+00, ...
      0.11751605060085098084E+00 ];

  x_vec = [ ...
      0.0019531250E+00, ...
      0.0078125000E+00, ...
      0.0312500000E+00, ...
      0.1250000000E+00, ...
      0.5000000000E+00, ...
      1.0000000000E+00, ...
      1.2500000000E+00, ...
      1.5000000000E+00, ...
      1.8750000000E+00, ...
      2.0000000000E+00, ...
      2.1250000000E+00, ...
      2.5000000000E+00, ...
      3.0000000000E+00, ...
      3.5000000000E+00, ...
      4.0000000000E+00, ...
      4.5000000000E+00, ...
      5.0000000000E+00, ...
      5.7500000000E+00, ...
      6.0000000000E+00, ...
      7.0000000000E+00 ];

  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


%ARDEM	Demonstrates modules of the ARfit package.

%   Revised: 30-Dec-99 Tapio Schneider

format short
format compact

echo on
clc

%  ARfit is a collection of Matlab modules for the modeling of
%  multivariate time series with autoregressive (AR) models. ARfit
%  contains modules for estimating parameters of AR models from given
%  time series data; for checking the adequacy of an estimated
%  AR model; for analyzing eigenmodes of an estimated AR model; and
%  for simulating AR processes.
%
%  This demo illustrates the use of ARfit with a bivariate AR(2)
%  process
%
%       v(k,:)' = w' + A1*v(k-1,:)' + A2*v(k-2,:)' + eta(k,:)',
%
%  where the row vectors eta(k,:) are independent and identically
%  distributed Gaussian noise vectors with zero mean and covariance
%  matrix C. The kth row v(k,:) of the 2-column matrix v represents an
%  observation of the process at instant k. The intercept vector w is
%  included to allow for a nonzero mean of the AR(p) process.

pause   	% Press any key to continue.

clc

%  Let us simulate observations from a bivariate AR(2) process,
%  choosing the parameters

w = [ 0.25 ;  0.1 ];

% for the intercept vector,

A1 = [ 0.4   1.2;   0.3   0.7 ];

%  and

A2 = [ 0.35 -0.3;  -0.4  -0.5 ];

%  for the AR coefficient matrices, and

C = [ 1.00  0.50;   0.50  1.50 ];

%  for the noise covariance matrix. The two 2x2 matrices A1 and A2 are
%  assembled into a single 2x4 coefficient matrix:

A = [ A1 A2 ];

%  We use the module ARSIM to simulate 200 observations of this AR
%  process:

v = arsim(w, A, C, 200);

pause   	% Press any key to continue.

clc

%  Suppose that we have no information about how the time series
%  data v are generated, but we want to try to fit an AR model to the
%  time series. That is, we must estimate the AR model parameters,
%  including the model order. Assuming that the correct model order
%  lies between 1 and 5, we use the module ARFIT to determine the
%  optimum model order using Schwarz's Bayesian Criterion (SBC):

pmin = 1;
pmax = 5;
[west, Aest, Cest, SBC, FPE, th] = arfit(v, pmin, pmax);

%  The output arguments west, Aest, and Cest of ARFIT are the
%  estimates of the intercept vector w, of the coefficient matrix A,
%  and of the noise covariance matrix C. (The matrix th will be needed
%  later in the computation of confidence intervals.) These parameters
%  are estimated for a model of order popt, where the optimum model
%  order popt is chosen as the minimizer of an approximation to
%  Schwarz's Bayesian Criterion. The selected order popt in our
%  example is:

m    = 2;                 % dimension of the state space
popt = size(Aest, 2) / m;

disp(['popt = ', num2str(popt)])

pause   	% Press any key to continue.

clc

%  Besides the parameter estimates for the selected model, ARFIT has
%  also returned approximations to Schwarz's Bayesian Criterion SBC
%  and to Akaike's Final Prediction Error FPE, each for models of
%  order pmin:pmax. In this demo, the model order was chosen as the
%  minimizer of SBC. Here are the SBCs for the fitted models of order
%  1,...,5:

disp(SBC)

%  To see if using Akaike's Final Prediction Error as a criterion to
%  select the model order would have resulted in the same optimum
%  model order, compare the FPEs:

disp(FPE)

%  Employing FPE as the order selection criterion, the optimum model
%  order would have been chosen as the minimizer of FPE. The values of
%  the order selection criteria are approximations in that in
%  evaluating an order selection criterion for a model of order p <
%  pmax, the first pmax-p observations of the time series are ignored.

pause   	% Press any key to continue.

clc

%  Next it is necessary to check whether the fitted model is adequate
%  to represent the time series v. A necessary condition for model
%  adequacy is the uncorrelatedness of the residuals. The module ARRES

[siglev,res] = arres(west,Aest,v);

%  returns the time series of residuals res as well as the
%  significance level siglev with which a modified Li-McLeod
%  portmanteau test rejects the null hypothesis that the residuals are
%  uncorrelated. A model passes this test if, say, siglev > 0.05.  In
%  our example, the significance level of the modified Li-McLeod
%  portmanteau statistic is

disp(siglev);

%  (If siglev is persistently smaller than about 0.05, even over
%  several runs of this demo, then there is most likely a problem with
%  the random number generator that ARSIM used in the simulation of
%  v.)

pause   	% Press any key to continue.

clc

if exist('xcorr')    % If the Signal Processing Toolbox is installed, plot
                     % autocorrelation function of residuals ...

%  Using ACF, one can also plot the autocorrelation function of, say,
%  the first component of the time series of residuals:

acf(res(:,1));

%  95% of the autocorrelations for lag > 0 should lie between the
%  dashdotted confidence limits for the autocorrelations of an IID
%  process of the same length as the time series of residuals res.

%  Since uncorrelatedness of the residuals is only a necessary
%  condition for model adequacy, further diagnostic tests should, in
%  practice, be performed. However, we shall go on to the estimation
%  of confidence intervals.

pause   	% Press any key to continue.
end

clc

%  Being reasonably confident that the model adequately represents the
%  data, we compute confidence intervals for the AR parameters with
%  the module ARCONF:

[Aerr, werr] = arconf(Aest, Cest, west, th);

%  The output arguments Aerr and werr contain margins of error such
%  that (Aest +/- Aerr) and (west +/- werr) are approximate 95%
%  confidence intervals for the individual AR coefficients and for the
%  components of the intercept vector w. Here is the estimated
%  intercept vector with margins of error for the individual
%  components in the second column:

disp([west werr])

%  For comparison, the `true' vector as used in the simulation:

disp(w)

pause   	% Press any key to display the other parameter estimates.

clc

%  The estimated coefficient matrix:

disp(Aest)

%  with margins of error for its elements:

disp(Aerr)

%  For comparison, the `true' AR coefficients:

disp(A)

pause   	% Press any key to continue.

echo off
%  Compute `true' eigenmodes from model parameters:
%  Eigenvectors and eigenvalues of corresponding AR(1) coefficient
%  matrix:
[Strue,EvTrue] = eig([A; eye(2) zeros(2)]);
EvTrue         = diag(EvTrue)';   % `true' eigenvalues
Strue          = adjph(Strue);    % adjust phase of eigenmodes

clc
echo on

%  Finally, ARMODE computes the eigendecomposition of the fitted AR
%  model:

[S, Serr, per, tau, exctn] = armode(Aest, Cest, th);

%  The columns of S are the estimated eigenmodes:

disp(S)

%  with margins of error Serr:

disp(Serr)

%  The intervals (S +/- Serr) are approximate 95% confidence intervals
%  for the individual components of the eigenmodes.  Compare the
%  estimated eigenmodes above with the eigenmodes obtained from the
%  `true' model parameters:

disp(Strue(3:4,:))

%  (Note that the estimated modes can be a permutation of the `true'
%  modes. The sign of the modes is also ambiguous.)

pause   	% Press any key to continue.

echo off
pertrue = 2*pi./abs(angle(EvTrue)); % `true' periods

clc

echo on
%  Associated with the eigenmodes are the following oscillation periods:

disp(per)

%  The second row contains margins of error for the periods such that
%  (per(1,k) +/- per(2,k)) is an approximate 95% confidence interval
%  for the period of eigenmode S(:,k). [Note that for a purely
%  relaxatory eigenmode, the period is infinite (Inf).] Compare the
%  above estimated periods with the `true' periods:

disp(pertrue)

pause   	% Press any key to get the damping time scales.

echo off
tautrue = -1./log(abs(EvTrue)); % `true' damping time scales

clc

echo on

%  The damping times associated with each eigenmode are:

disp(tau)

%  with margins of error again in the second row. For comparison, the
%  `true' damping times:

disp(tautrue)

pause   	% Press any key to get the excitation of each eigenmode.
echo off
%  Compute `true' excitation of eigenmodes from the designed parameters:
p  = 2;              % true model order

invStr = inv(Strue); % inverse of matrix with eigenvectors as columns

% covariance matrix of corresponding decoupled AR(1) system
CovDcpld = invStr*[C zeros(2,(p-1)*2); zeros((p-1)*2, p*2)]*invStr';

% diagonal of that covariance matrix
DgCovDcpld = real(diag(CovDcpld))';

% excitation
TrueExctn = DgCovDcpld(1:2*p)./(1-abs(EvTrue).^2);

% normalize excitation
TrueExctn = TrueExctn./sum(TrueExctn);

clc

echo on
%  ARMODE has also returned the excitations, measures of the relative
%  dynamical importance of the eigenmodes:

disp(exctn)

%  Compare the estimated excitations with the `true' excitations
%  computed from the parameters used in the simulation:

disp(TrueExctn)

echo off
disp('End')


function [signature] = getAertsSign_DICOMconvention(vol,mask,spatialRef,scanType)

% PARAMETERS
if strcmp(scanType,'CT')
    res = [1,1,3]; % In (Aerts,2014), all features were calculated from 1 X 1 X 3 mm^3 raw images
    levels = 0:25:3000; % Division of quantized bins in (Aerts,2014) for texture analysis
elseif strcmp(scanType,'PET') % Novelty tested in this work
    levels = 0:1:100; % Division of quantized bins for texture analysis, similarly to (Aerts,2014)
    res = [4,4,4]; % Resolution is here set up to the approximate resolution of PET scans
end

% INITIALIZATION
signature = zeros(1,4);
nLevels = numel(levels) - 1;

% STEP 0: RESAMPLING OF VOLUME AND MASK
pixelW = spatialRef.PixelExtentInWorldX;
sliceS = spatialRef.PixelExtentInWorldZ;
a = pixelW/res(1); b = pixelW/res(2); c = sliceS/res(3);
maskBox = imresize3D(mask,[],[round(double(size(mask,1))*a),round(double(size(mask,2))*b),round(double(size(mask,3))*c)],'nearest','fill');
ROIbox = imresize3D(vol,[],[round(double(size(vol,1))*a),round(double(size(vol,2))*b),round(double(size(vol,3))*c)],'cubic','fill');
ROIonly = ROIbox; ROIonly(~maskBox) = NaN;

% STEP 1: CALCULATE FEATURE 1
energy = ROIonly .^2; energy = sum(energy(~isnan(energy)));
signature(1) = energy;

% STEP 2: CALCULATE FEATURE 2
maskArea = imfill(maskBox,'holes'); maskArea = padarray(maskArea,[1,1,1],0); % Not using inner surfaces
volume = sum(maskBox(:)) * prod(res); % In mm^3
[p,q,r] = meshgrid(res(1).*(1:1:size(maskArea,2)),res(2).*(1:1:size(maskArea,1)),res(3).*(1:1:size(maskArea,3)));
[faces,vertices] = isosurface(p,q,r,maskArea,.5);
a = vertices(faces(:,2),:) - vertices(faces(:,1),:);
b = vertices(faces(:,3),:) - vertices(faces(:,1),:);
c = cross(a,b,2);
surface = 1/2 * sum(sqrt(sum(c.^2,2)));
compactness = volume/(sqrt(pi)*surface^(2/3));
signature(2) = compactness;

% STEP 3: CALCULATE FEATURE 3
ROIquant = ROIonly;
ROIquant(ROIonly < levels(1)) = 1;
for n = 1:nLevels
    ROIquant(ROIonly>=levels(n) & ROIonly<levels(n+1)) = n;
end
ROIquant(ROIonly >= levels(end)) = nLevels;
newLevels = 1:max(ROIquant(~isnan(ROIquant)));
[GLN] = getGLN_Aerts(ROIquant,newLevels);
signature(3) = GLN;

% STEP 4: CALCULATE FEATURE 4 (HLH --> 1) H: DICOM = left-right, Axial image = left-right; 2) L: DICOM = anterior posterior, Axial image = top-bottom; 3) H: DICOM = superior-inferior, Axial image = 3rd dimension)
% - DICOM XYZ coordinates are assumed in HLH feature definition of (Aerts et al, 2014)
% - From the swt2 function, we have in DICOM XYZ coordinates: [LL,LH,HL,HH] = swt2(image,N,'wavName')
sizeV = size(ROIbox);
if mod(sizeV(1),2)
    sizeV(1) = sizeV(1) + 1;
end
if mod(sizeV(2),2)
    sizeV(2) = sizeV(2) + 1;
end
if mod(sizeV(3),2)
    sizeV(3) = sizeV(3) + 1;
end
HLH = zeros(sizeV);
maskBox = imresize3D(maskBox,[],sizeV,'nearest','fill'); % Making sure the dimensions are even for decomposition at level 1
ROIbox = imresize3D(ROIbox,[],sizeV,'cubic','fill');
for k = 1:sizeV(3) % In this image plane (axial), we need the HL sub-band
    image = ROIbox(:,:,k);
    [~,~,HL,~] = swt2(image,1,'coif1');
    HLH(:,:,k) = HLH(:,:,k) + HL;
end
for i = 1:sizeV(1) % In this image plane (coronal), we need the HH sub-band
    image = squeeze(ROIbox(i,:,:))';
    [~,~,~,HH] = swt2(image,1,'coif1');
    for k = 1:sizeV(3)
        HLH(i,:,k) = HLH(i,:,k) + HH(k,:);
    end
end
for j = 1:sizeV(2) % In this image plane (sagittal), we need the HL sub-band
    image = squeeze(ROIbox(:,j,:));
    [~,~,HL,~] = swt2(image,1,'coif1');
    for k = 1:sizeV(3)
        HLH(:,j,k) = HLH(:,j,k) + HL(:,k);
    end
end
HLH = HLH/3;

ROIquant = HLH; ROIquant(~maskBox) = NaN;
Ng = numel(newLevels); [ROIquant,levels] = uniformQuantization(ROIquant,Ng);
[GLN] = getGLN_Aerts(ROIquant,levels);
signature(4) = GLN;

end


function pdf = english_word_length_pdf ( x )

%*****************************************************************************80
%
%% ENGLISH_WORD_LENGTH_PDF evaluates the English Word Length PDF.
%
%  Discussion:
%
%    PDF(A,B;X) = B(X) if 1 <= X <= A
%                = 0    otherwise
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 July 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Henry Kucera, Winthrop Francis,
%    Computational Analysis of Present-Day American English,
%    Brown University Press, 1967.
%
%  Parameters:
%
%    Input, integer X, the word length whose probability is desired.
%
%    Output, real PDF, the value of the PDF.
%
  word_length_max = 27;

  pdf_vec = [ ...
    0.03160, ...
    0.16975, ...
    0.21192, ...
    0.15678, ...
    0.10852, ...
    0.08524, ...
    0.07724, ...
    0.05623, ...
    0.04032, ...
    0.02766, ...
    0.01582, ...
    0.00917, ...
    0.00483, ...
    0.00262, ...
    0.00099, ...
    0.00050, ...
    0.00027, ...
    0.00022, ...
    0.00011, ...
    0.00006, ...
    0.00005, ...
    0.00002, ...
    0.00001, ...
    0.00001, ...
    0.00001, ...
    0.00001, ...
    0.00001 ];
  pdf_sum = 0.99997;

  if ( 1 <= x & x <= word_length_max )
    pdf = pdf_vec(x) / pdf_sum;
  else
    pdf = 0.0;
  end

  return
end


function [pf,f]=v_lpcar2pf(ar,np)
%V_LPCAR2PF Convert AR coefs to power spectrum PF=(AR,NP)
%
%  Inputs: ar(nf,n)     AR coefficients, one frame per row
%          np           Size of output spectrum is np+1 [n]
%
% Outputs: pf(nf,np+1)  Power spectrum from DC to Nyquist
%          f(1,np+1)    Normalized frequencies (0 to 0.5)
%
% For high speed make np equal to a power of 2

%      Copyright (C) Mike Brookes 1998-2014
%      Version: $Id: v_lpcar2pf.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p1]=size(ar);
if nargin<2
    if nargout
        np=p1-1;
    else
        np=128;
    end
end
pf=abs(v_rfft(ar.',2*np).').^(-2);
f=(0:np)/(2*np);
if ~nargout
        plot(f,pf.');
        xlabel('Normalized frequency f/f_s');
        ylabel('LPC Power Spectrum');
end


function hermite_cubic_test09 ( )

%*****************************************************************************80
%
%% HERMITE_CUBIC_TEST09 tests HERMITE_CUBIC_SPLINE_INTEGRATE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_CUBIC_TEST09:\n' );
  fprintf ( 1, '  HERMITE_CUBIC_SPLINE_INTEGRATE integrates a Hermite\n' );
  fprintf ( 1, '  cubic spline from A to B.\n' );
%
%  Define the cubic spline.
%
  x1 = 0.0;
  x2 = 10.0;

  nn = 11;
  xn = linspace ( x1, x2, nn );
  [ fn, dn, sn, tn ] = cubic_value ( xn );

  n = 25;
  a = linspace ( 2.5, 2.5, n );
  b = linspace ( x1 - 1.0, x2 + 1.0, n );

  q = hermite_cubic_spline_integrate ( nn, xn, fn, dn, n, a, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '                                 Exact       Computed\n' );
  fprintf ( 1, '    I      A           B         Integral    Integral\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n

    q_exact = cubic_integrate ( a(i), b(i) );

    fprintf ( 1, '  %3d  %10f  %10f  %10.6g  %10.6g\n', ...
      i, a(i), b(i), q_exact, q(i) );

  end

  return
end


function R = sw_han_converse(a, n_vals, eps)
% Compute the converse sum rate (at the symmetrical rate point) from Han's SW converse
% for the joint source distribution: [a 1/3*(1-a); 1/3*(1-a) 1/3*(1-a)].

% Input:
% a: parameter to define the joint source distribution, should be in range [0.25, 1)
% n_vals: a vector containing the blocklengths
% eps: target error probability
% Output:
% R: the rates for the blocklengths specified in n_vals.

    p = [a 1/3*(1-a); 1/3*(1-a) 1/3*(1-a)];                                 % Stores joint probabilities
    H = sum(sum(p.*log(1./p)));                                             % Joint entropy
    b = [a/(a+1/3*(1-a)) 1/3*(1-a)/(2*1/3*(1-a)) 1/3*(1-a)/(a+1/3*(1-a))];  % Stores conditional probabilities

    % Compute log binomial coefficients
    log_b = binomial_coeff(max(n_vals));

    R = zeros(length(n_vals),1);

    for i = 1:length(n_vals)
        n = n_vals(i);
        m = 0:n;
        log_Pr = m*log(a)+(n-m)*log(1/3*(1-a));

        % Search the range of gamma specified in r_vals
        % The range of r_vals could be set smaller for large n to save computation time.
        if n <= 500
            r_vals = 0.0001:0.00005:2;
        else
            r_vals = 0.0001:0.00005:0.1;
        end

        R_r = zeros(1,length(r_vals));
        for ri = 1:length(r_vals)
            r = r_vals(ri);
            x = 0;
            y = H + 0.1;

            %%%%%%%%%% Use this part to check if the range is valid for bisection algorithm %%%%%%%%%%
            c = x;
            m0 = max(0, floor(n*(log(1/3*(1-a))+c+r)/log(1/3*(1-a)/a)));
            if m0 >= n
                err = 1 - 3*exp(-n*r);
            else
                log_err_m1 = my_logsumexp(log_Pr(1:m0+1)+log_b{n+1}(1:m0+1)+(n-(0:m0))*log(3));
                log_err_m2 = [];
                for m = m0+1:n
                    B_m = ceil((m*log(b(1))+(n-m)*log(b(2))+n*(c/2+r))/(log(b(2)/b(3))));
                    if B_m <= 0
                        C_m = log_Pr(m+1) + log_b{n+1}(m+1) + (n-m)*log(3);
                        log_err_m2 = [log_err_m2 C_m];
                    elseif B_m <= n-m
                        C_m1 = my_logsumexp(log_b{n-m+1}(B_m+1:n-m+1)+(n-m-(B_m:n-m))*log(2));
                        log_err_k = [];
                        for k = 0:B_m-1
                            if B_m <= n-m-k
                                D_m = log_b{n-m+1}(k+1) + my_logsumexp(log_b{n-m-k+1}(B_m+1:n-m-k+1));
                                log_err_k = [log_err_k D_m];
                            end
                        end
                        C_m2 = my_logsumexp(log_err_k);
                        C_m = log_Pr(m+1) + log_b{n+1}(m+1) + my_logsumexp([C_m1 C_m2]);
                        log_err_m2 = [log_err_m2 C_m];
                    end
                end
                log_err = my_logsumexp([log_err_m1 log_err_m2]);
                err = exp(log_err) - 3*exp(-n*r);
            end
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

            % Start of bisection iteration
            iter = 0;
            while abs(x - y) >= 0.00001 && iter <= 30
                c = (x+y)/2;
                %%%%%%%%%%%%%%%%%%% body of iteration %%%%%%%%%%%%%%%%%%%%
                m0 = max(0, floor(n*(log(1/3*(1-a))+c+r)/log(1/3*(1-a)/a)));
                if m0 >= n
                    err = 1 - 3*exp(-n*r);
                else
                    log_err_m1 = my_logsumexp(log_Pr(1:m0+1)+log_b{n+1}(1:m0+1)+(n-(0:m0))*log(3));
                    log_err_m2 = [];
                    for m = m0+1:n
                        B_m = ceil((m*log(b(1))+(n-m)*log(b(2))+n*(c/2+r))/(log(b(2)/b(3))));
                        if B_m <= 0
                            C_m = log_Pr(m+1) + log_b{n+1}(m+1) + (n-m)*log(3);
                            log_err_m2 = [log_err_m2 C_m];
                        elseif B_m <= n-m
                            C_m1 = my_logsumexp(log_b{n-m+1}(B_m+1:n-m+1)+(n-m-(B_m:n-m))*log(2));
                            log_err_k = [];
                            for k = 0:B_m-1
                                if B_m <= n-m-k
                                    D_m = log_b{n-m+1}(k+1) + my_logsumexp(log_b{n-m-k+1}(B_m+1:n-m-k+1));
                                    log_err_k = [log_err_k D_m];
                                end
                            end
                            C_m2 = my_logsumexp(log_err_k);
                            C_m = log_Pr(m+1) + log_b{n+1}(m+1) + my_logsumexp([C_m1 C_m2]);
                            log_err_m2 = [log_err_m2 C_m];
                        end
                    end
                    log_err = my_logsumexp([log_err_m1 log_err_m2]);
                    err = exp(log_err) - 3*exp(-n*r);
                end
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                if err - eps < 0
                    y = c;
                else
                    x = c;
                end
                iter = iter + 1;
            end
            R_r(ri) = c;
        end
        % Optimize with respect to gamma
        R(i) = max(R_r);
    end
end


function rpy = quat2elu(qw, qx, qy, qz)

sinr_cosp = 2 * qw .* qx + qy .* qz;
cosr_cosp = 1 - 2 * qx .* qx + qy .* qy;
roll = atan2(sinr_cosp, cosr_cosp);

sinp = 2 * (qw .* qy - qz .* qx);
% remove out of range
sinp(sinp > 1) = 1;
sinp(sinp < -1) = -1;

pitch = asin(sinp);

siny_cosp = 2 * (qw .* qz + qx .* qy);
cosy_cosp = 1 - 2 * (qy .* qy + qz .* qz);
yaw = atan2(siny_cosp, cosy_cosp);

rpy = [roll, pitch, yaw];


% Y = HealpixNestedInvLinearTrans(X, A, m)
%
% Description
% This function calculates Y = WX where
% W = kron( eye(12), W_0 * W_1 * W_2 * ... * W_{m-2} * W_{m-1} )
% W_i = kron( eye( 4^{m-i-1} ), V_i )
% V_i = kron( eye(4), diag([0 1 1 ... 1]) ) + kron( A, diag([1 0 0 ...0]) )
%
% Parameters
% X : input row-vector
% Y : output row-vector (same size as X)
% A : linear transform applied to the adjacent elements
% m : log4(length(X) / 12)

function Y = HealpixNestedInvLinearTrans(X, A, m)

for itr = (m - 1):(-1):0
    num_trans = 12 * 4^(m - itr - 1);
    interval = 4^itr;
    begin_idx = 1;
    %itr
    for trans = 0:(num_trans - 1)
        IDX = begin_idx + interval * [0:3];
        %IDX
        X(IDX) = A * X(IDX);
        begin_idx = begin_idx + 4 * interval;
    end
end

Y = X;


function [cost,source] = cs_results(px,py,pz,rho,u,param)

e=1e-6;

h     = param.h;
dim   = param.dim;
gamma = param.gamma;
A1x   = param.A1x;
A1y   = param.A1y;
A1z   = param.A1z;
A2    = param.A2;
A3    = param.A3;
a     = param.a;
c     = param.c;
F1    = param.F1;
F2    = param.F2;

n  = dim(1);
nx = dim(2);
ny = dim(3);
nz = dim(4);
nt = dim(5);
hx = h(1);
hy = h(2);
hz = h(3);
ht = h(4);

%compute p^2 and u^2

pxsq  = px.^2;
pysq  = py.^2;
pzsq  = pz.^2;
usq   = u.^2;

% inverse of rho
rhoinv  = 1./rho;
rhoinv2 = 1./(F2'*rho(:));

E=eye(n);
E(n,n)=e;
W1=kron(speye((nx-1)*ny*nz*nt),E);
W2=kron(speye((ny-1)*nx*nz*nt),E);
W3=kron(speye((nz-1)*nx*ny*nt),E);

% cost f
cost = (A1x*W1*pxsq(:)+A1y*W2*pysq(:)+A1z*W3*pzsq(:))'*(A2*rhoinv(:) + a(:))*hx*hy*hz*ht + usq(:)'*(A3*rhoinv2(:)+ c(:))*hx*hy*hz*ht*gamma;
source=usq(:)'*(A3*rhoinv2(:)+ c(:))*hx*hy*hz*ht;
end


function [yr] = s2yr(s)
% Convert time from seconds to Julian years (365.25 days).
% Chad Greene 2012
yr = s*3.168808781403e-8;


avg_odd = 3.93;
max_odd = 4.27;

p_margin = 0.04;
p_real = (1 / avg_odd) - p_margin; % pur estimate of the "true" probability

p_max = 1 / max_odd;

payoff = p_real * max_odd - 1;

if payoff > 0
    msg = sprintf('Payoff = %2.2f. Place bet', payoff);
    disp(msg)
else
    msg = sprintf('Payoff = %2.2f. Do not Place bet', payoff);
    disp(msg)
end


function [kJ] = kWh2kJ(kWh)
% Convert energy or work from kilowatt-hours to kilojoules.
% Chad A. Greene 2012
kJ = kWh*3600;


function [ET_table,EV_table,ETV_index]=edge_tangents_double(V,Ne)
% Edge tangents table
ET_table=zeros(size(V,1)*4,3);
% Edge velocity table
EV_table=zeros(size(V,1)*4,1);
% Edge tangents/velocity index for tables
ETV_index=zeros(size(V,1)*4,2);
ETV_num=0;

% Calculate the tangents and velocity for each edge
Pn=zeros(length(Ne),3); Pnop=zeros(length(Ne),3);
for i=1:size(V,1)
    P=V(i,:);
    Pneig=Ne{i};

    % Find the opposite vertex of each neigbourh vertex.
    % incase of odd number of neigbourhs interpolate the opposite neigbourh
    if(mod(length(Pneig),2)==0)
       for k=1:length(Pneig)
            neg=k+length(Pneig)/2; if(neg>length(Pneig)), neg=neg-length(Pneig); end
            Pn(k,:) = V(Pneig(k),:); Pnop(k,:) = V(Pneig(neg),:);
       end
    else
       for k=1:length(Pneig)
           neg=k+length(Pneig)/2; neg1=floor(neg); neg2=ceil(neg);
           if(neg1>length(Pneig)), neg1=neg1-length(Pneig); end
           if(neg2>length(Pneig)), neg2=neg2-length(Pneig); end
           Pn(k,:) = V(Pneig(k),:); Pnop(k,:) = (V(Pneig(neg1),:)+V(Pneig(neg2),:))/2;
       end
    end

    for j=1:length(Pneig);
        % Calculate length edges of face
        Ec=sqrt(sum((Pn(j,:)-P).^2))+1e-14;
        Eb=sqrt(sum((Pnop(j,:)-P).^2))+1e-14;
        Ea=sqrt(sum((Pn(j,:)-Pnop(j,:)).^2))+1e-14;

        % Calculate face surface area
        s = ((Ea+Eb+Ec)/2);
        h = (2/Ea)*sqrt(s*(s-Ea)*(s-Eb)*(s-Ec))+1e-14;
        x = (Ea^2-Eb^2+Ec^2)/(2*Ea);

        % 2D triangle coordinates
        % corx(1)=0;    cory(1)=0;
        % corx(2)=x;    cory(2)=h;
        % corx(3)=Ea;   cory(3)=0;
        % corx(4)=0;    cory(4)=0;

        % Calculate tangent of 2D triangle
        Np=[-h x]; Np=Np/(sqrt(sum(Np.^2))+1e-14);
        Ns=[h Ea-x]; Ns=Ns/(sqrt(sum(Ns.^2))+1e-14);
        Nb=Np+Ns;
        Tb=[Nb(2) -Nb(1)];

        % Back to 3D coordinates
        Pm=(Pn(j,:)*x+Pnop(j,:)*(Ea-x))/Ea;
        X3=(Pn(j,:)-Pnop(j,:))/Ea;
        Y3=(P-Pm)/h;

        % 2D tangent to 3D tangent
        Tb3D=(X3*Tb(1)+Y3*Tb(2));  Tb3D=Tb3D/(sqrt(sum(Tb3D.^2))+1e-14);

        % Edge Velocity
        Vv=0.5*(Ec+0.5*Ea);

        ETV_num=ETV_num+1;
        ETV_index(ETV_num,:)=[i Pneig(j)];
        ET_table(ETV_num,:)= Tb3D;
        EV_table(ETV_num)=Vv;
    end
end


function [ fea, out ] = ex_heattransfer1( varargin )
%EX_HEATTRANSFER1 2D ceramic strip with radiation and convection.
%
%   [ FEA, OUT ] = EX_HEATTRANSFER1( VARARGIN ) 2D heat transfer of a ceramic strip with
%   both radiation and convection on the top boundary.
%
%         _                q = h*(T_inf-T) + epsilon*sigma*(T_inf^4-T^4)
%         ^            +------------------+
%         |            |                  |
%         |            |                  |
%       0.01m  T=900C  |                  |  T=900C
%         |            |                  |
%         |            |                  |
%         v            +------------------+
%                           dt/dn = 0
%                      |<---- 0.02m ----->|
%
%   The ceramic has a thermal conductivity of 3 W/mC and the sides are fixed
%   at a temperature of 900C while the bottom boundary is insulated. The surrounding
%   temperature is 50C. The top boundary is exposed to both natural convection (with
%   a film coefficient h=50W/m^2K) and radiation (with emissivity epsilon=0.7
%   and the Stefan-Boltzmann 5.669e-8 W/m^2K^4). The solution is sought at three
%   points along the vertical symmetry line.
%
%   Reference:
%
%      [1] Holman, J. P., Heat Transfer, Fifth Edition, New York: McGraw-Hill,
%          1981, page 96, Example 3-8.
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       hmax        scalar {0.001}         Grid cell size
%       igrid       scalar {0}/1/2         Cell type (0=quadrilaterals, 1=triangles,
%       solver      string fenics/{}       Use FEniCS or default solver
%       ischeme     scalar {0}             Time stepping scheme (0 = stationary)
%       sfun        string {sflag1}        Finite element shape function
%       iplot       scalar {1}/0           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { 'hmax',     0.001;
            'igrid',    0;
            'solver',   '';
            'ischeme',  0;
            'sfun',     'sflag1';
            'iplot',    1;
            'tol',      0.01;
            'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
if( opt.ischeme==2 && ~got.tol )
  opt.tol = 0.05;
end

% Geometry definition.
gobj = gobj_rectangle( 0, 0.02, 0, 0.01 );
fea.geom.objects = { gobj };

% Grid generation.
switch opt.igrid
  case 0
    fea.grid = rectgrid( round(0.02/opt.hmax), round(0.01/opt.hmax), [0 0.02;0 0.01] );
  case 1
    fea.grid = gridgen( fea, 'hmax', opt.hmax, 'fid', opt.fid );
  case 2
    fea.grid = rectgrid( round(0.02/opt.hmax), round(0.01/opt.hmax), [0 0.02;0 0.01] );
    fea.grid = quad2tri( fea.grid, 1 );
end

% Problem definition.
fea.sdim  = { 'x', 'y' };             % Space coordinate name.
fea = addphys( fea, @heattransfer );  % Add heat transfer physics mode.
fea.phys.ht.sfun = { opt.sfun };      % Set shape function.

% Equation coefficients.
fea.phys.ht.eqn.coef{3,end} = 3;      % Thermal conductivity.

% Boundary conditions.
fea.phys.ht.bdr.sel = [3 1 4 1];
fea.phys.ht.bdr.coef{1,end}   = { [] 900+273 [] 900+273 };
fea.phys.ht.bdr.coef{4,end}{3}{2} = 50;
fea.phys.ht.bdr.coef{4,end}{3}{3} = 50+273;
fea.phys.ht.bdr.coef{4,end}{3}{4} = 0.7*5.669e-8;
fea.phys.ht.bdr.coef{4,end}{3}{5} = 50+273;

% Parse physics modes and problem struct.
fea = parsephys(fea);
fea = parseprob(fea);

% Compute solution.
if( strcmp(opt.solver,'fenics') )
  fea = fenics( fea, 'fid', opt.fid, ...
                'tstep', 0.1, 'tmax', 1, 'ischeme', opt.ischeme );
else
  if( opt.ischeme<=0 )
    fea.sol.u = solvestat( fea, 'fid', opt.fid, 'init', {'T0_ht'} );
  else
    [fea.sol.u,fea.sol.t] = solvetime( fea, 'fid', opt.fid, 'init', {'T0_ht'}, ...
                                       'tstep', 0.1, 'tmax', 1, 'ischeme', opt.ischeme );
  end
end

% Postprocessing.
if( opt.iplot>0 )
  postplot( fea, 'surfexpr', 'T', 'isoexpr', 'T' )
  title('Temperature, T')
end

% Error checking.
T2_sol = evalexpr( 'T', [0.01;0.01], fea );
T2_ref = 984;
T5_sol = evalexpr( 'T', [0.01;0.005], fea );
T5_ref = 1064;
T8_sol = evalexpr( 'T', [0.01;0], fea );
T8_ref = 1088;
out.err  = abs([T2_sol-T2_ref T5_sol-T5_ref T8_sol-T8_ref])./[T2_ref T5_ref T8_ref];
out.pass = all(out.err<opt.tol);

if( nargout==0 )
  clear fea out
end


% DEMOPTIMISEGP Shows that there is an optimum for the covariance function length scale.
% DESC shows that by varying the length scale an artificial data
% set has different likelihoods, yet there is an optimum for which
% the likelihood is maximised.

% COPYRIGHT : Neil D. Lawrence, 2006, 2008

% GP

randn('seed', 1e5);
rand('seed', 1e5);

fillColor = [0.7 0.7 0.7];
markerSize = 20;
markerWidth = 2;
markerType = 'k.';
lineWidth = 2;

x = linspace(-1, 1, 6)';
trueKern = kernCreate(x, {'rbf', 'white'});
kern.comp{2}.variance = 0.001;
K = kernCompute(trueKern, x);
y = gsamp(zeros(1, 6), K, 1)';

xtest = linspace(-1.5, 1.5, 200)';
kern = trueKern;

lengthScale = [0.05 0.1 0.25 0.5 1 2 4 8 16];
counter = 0;

figure(1)
p = plot(x, y, markerType);
set(p, 'markersize', markerSize, 'lineWidth', markerWidth);
set(gca, 'fontname', 'times')
set(gca, 'fontsize', 18)
set(gca, 'ylim', [-2 1])
set(gca, 'xlim', [-1.5 1.5])

zeroAxes(gca);
fileName = ['demOptimiseGp' num2str(counter)];
if exist('printDiagram') && printDiagram
  printPlot(fileName, '../tex/diagrams', '../html');
end
clf

void = semilogx(NaN, NaN, 'k.-');
set(gca, 'fontname', 'times')
set(gca, 'fontsize', 18)
set(gca, 'ylim', [-12 -4])
set(gca, 'xlim', [0.025 32])
grid on
ylabel('log-likelihood')
xlabel('length scale')
fileName = ['demOptimiseGp' num2str(counter) '0'];
if exist('printDiagram') && printDiagram
  printPlot(fileName, '../tex/diagrams', '../html');
end

clf

for i = 1:length(lengthScale)
  kern.comp{1}.inverseWidth = 1/(lengthScale(i)*lengthScale(i));
  K = kernCompute(kern, x);
  [invK, U] = pdinv(K);
  logDetK = logdet(K, U);
  ll(i) = -0.5*(logDetK + y'*invK*y + size(y, 1)*log(2*pi));
  llLogDet(i) = -.5*(logDetK+size(y, 1)*log(2*pi));
  llFit(i) = -.5*y'*invK*y;
  Kx = kernCompute(kern, x, xtest);
  ypredMean = Kx'*invK*y;
  ypredVar = kernDiagCompute(kern, xtest) - sum((Kx'*invK).*Kx', 2);

  counter = counter + 1;
  figure(counter)
  clf
  fill([xtest; xtest(end:-1:1)], ...
       [ypredMean; ypredMean(end:-1:1)] ...
         + 2*[ypredVar; -ypredVar], ...
       fillColor,'EdgeColor',fillColor)
  hold on;
  t = plot(xtest, ypredMean, 'k-');

  p = plot(x, y, markerType);
  set(p, 'markersize', markerSize, 'lineWidth', markerWidth);
  set(t, 'linewidth', lineWidth);
  set(gca, 'fontname', 'times')
  set(gca, 'fontsize', 18)
  set(gca, 'ylim', [-2 1])

  zeroAxes(gca);
  fileName = ['demOptimiseGp' num2str(counter)];
  if exist('printDiagram') && printDiagram
    printPlot(fileName, '../tex/diagrams', '../html');
  end

  counter = counter + 1;
  figure(counter)
  t = semilogx(lengthScale(1:i), ll(1:i), 'k.-');
  hold on
  t = [t; semilogx(lengthScale(1:i), llLogDet(1:i), 'k.:')];
  t = [t; semilogx(lengthScale(1:i), llFit(1:i), 'k.--')];
  set(t, 'markersize', markerSize, 'lineWidth', markerWidth);
  set(gca, 'fontname', 'times')
  set(gca, 'fontsize', 18)
  set(gca, 'ylim', [-15 5])
  set(gca, 'xlim', [0.025 32])
  grid on
  ylabel('log-likelihood')
  xlabel('length scale')
  fileName = ['demOptimiseGp' num2str(counter)];
  if exist('printDiagram') && printDiagram
    printPlot(fileName, '../tex/diagrams', '../html');
  end
end


function c = tapas_logrt_linear_whatworld_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the linear log-reaction time response model according to as
% developed with Louise Marshall and Sven Bestmann
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The Gaussian noise observation model assumes that responses have a Gaussian distribution around
% the inferred mean of the relevant state. The only parameter of the model is the noise variance
% (NOT standard deviation) zeta.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2014 Christoph Mathys, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'Linear log-reaction time for WhatWorld models';

% Sufficient statistics of Gaussian parameter priors
%
% Beta_0
c.be0mu = log(500);
c.be0sa = 4;

% Beta_1
c.be1mu = 0;
c.be1sa = 4;

% Beta_2
c.be2mu = 0;
c.be2sa = 4;

% Beta_3
c.be3mu = 0;
c.be3sa = 4;

% Zeta
c.logzemu = log(log(20));
c.logzesa = log(2);

% Gather prior settings in vectors
c.priormus = [
    c.be0mu,...
    c.be1mu,...
    c.be2mu,...
    c.be3mu,...
    c.logzemu,...
         ];

c.priorsas = [
    c.be0sa,...
    c.be1sa,...
    c.be2sa,...
    c.be3sa,...
    c.logzesa,...
         ];

% Model filehandle
c.obs_fun = @tapas_logrt_linear_whatworld;

% Handle to function that transforms observation parameters to their native space
% from the space they are estimated in
c.transp_obs_fun = @tapas_logrt_linear_whatworld_transp;

return;


%  Figure 7.65      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
% script to generate Fig. 7.65
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Robust Servo Example
% Example 7.36; Figure 7.65
clf;
F=[0 1; 0 -1];
G=[0;1];
H=[1 0];
J=[0];
omega=1;
a=[0 1 0 0;-omega*omega 0 1 0;0 0 0 1;0 0 0 -1];
b=[0;0;G];

% desired closed-loop poles
pc=[-1+sqrt(3)*j;-1-sqrt(3)*j;-sqrt(3)+j;-sqrt(3)-j];
k=place(a,b,pc);

% form controller matrices
k1=k(:,1:2);
ko=k(:,3:4);
ac=[0 1;-omega*omega 0];
bc=-[k(2);k(1)];;
cc=[1 0];
dc=[0];

% closed-loop system
acl=[F-G*ko G*cc;bc*H ac];
bcl=[0;0;-bc];
ccl=[H 0 0];
dcl=[0];
syscl=ss(acl,bcl,ccl,dcl);
pole(syscl)
tzero(syscl)

% blocking zeros
dcle=-1;
syse=ss(acl,bcl,ccl,dcle);
tzero(syse)

% Closed-loop response
t=0:.01:25;
r=sin(t);
y=lsim(syscl,r,t);

% blocking zeros from w to y
G1=[0;1;0;0];
sysclw=ss(acl,G1,ccl,dcl);
tzero(sysclw)

% closed-loop response from w to y
w=sin(t);
y=lsim(sysclw,w,t);
figure(1)
plot(t,y,t,w);
text(0.5,0.9,'w');
text(1.5,0.1,'y');
xlabel('Time (sec)');
ylabel('Disturbance, output');
title('Fig. 7.65 (a): Disturbance response');
nicegrid;

% control effort
cclu=[-ko cc];
dclu=[0];
sysclu=ss(acl,G1,cclu,dclu);
u=lsim(sysclu,w,t);
figure(2)
plot(t,u);
xlabel('Time (sec)');
ylabel('Control, u');
title('Fig. 7.65 (b): Control effort');
nicegrid;

% error signal
ccle=[-H 0 0];
dcle=[0];
syscle=ss(acl,G1,ccle,dcle);
e=lsim(syscle,w,t);
figure(3)
plot(t,e);
xlabel('Time (sec)');
ylabel('Error, e');
title('Fig. 7.65 (c): Tracking error signal')
nicegrid


function [ ft ] = gsp_translate_old(G, f, i)
%GSP_TRANSLATE Generalized translation of the signal f to the node i
%   Usage: ft = gsp_translate(G, f, i);
%
%   Input parameters
%       G   : Graph
%       f   : Signal (column)
%       i   : Indices of vertex (int)
%   Output parameters
%       ft  : translate signal
%
%   This function translate the column vector f onto the node i. If f*
%   is a matrix, the translation will be done to each column.
%
%

fhat=gsp_gft(G,f);
nt = size(f,2);

ft = sqrt(G.N)*gsp_igft(G,fhat .* ...
    repmat(transpose(G.U(i,:)),1,nt));

end


%
% Precompute the rotation matrices to rotate the high-resolution kernels (500 directions)
%
% Parameters
% ----------
% lmax : unsigned int
%   Maximum spherical harmonics order to use for the rotation phase
%
function AMICO_PrecomputeRotationMatrices( lmax )
	if nargin < 1, lmax = 12; end
	global AMICO_data_path

	fprintf( '\n-> Precomputing rotation matrices for l_max=%d:\n', lmax );

	filename = fullfile(AMICO_data_path,sprintf('AUX_matrices__lmax=%d.mat',lmax) );
	if exist( filename, 'file' )
		fprintf( '   [ already computed ]\n' );
		return
	end

	TIME = tic();

	% load file with 500 directions
	grad500 = importdata( '500_dirs.txt' );
	for i = 1:size(grad500,1)
		grad500(i,:) = grad500(i,:) ./ norm( grad500(i,:) );
        if grad500(i,2) < 0
            grad500(i,:) = -grad500(i,:);
        end
	end

	% precompute the matrix to fit the SH coefficients
	[colatitude, longitude] = AMICO_Cart2sphere( grad500(:,1), grad500(:,2), grad500(:,3) );
	Ylm = AMICO_CreateYlm( lmax, colatitude, longitude );
	fit = pinv( Ylm'*Ylm ) * Ylm';

	% precompute the matrices to rotate the functions in SH space
	Ylm_rot = {};
	for ox = 0:180
	for oy = 0:180
		Ylm_rot{ox+1,oy+1} = AMICO_CreateYlm( lmax, ox/180.0*pi, oy/180.0*pi );
	end
	end

	save( filename , 'Ylm_rot', 'fit', 'lmax' );
	fprintf( '   [ %.1f seconds ]\n', toc(TIME) );
end


%KNNM Trainable K-Nearest Neighbour density estimation
%
%   W = KNNM(A,KNN)
%   W = A*KNNM([],KNN)
%   W = A*KNNM(KNN)
%
%   D = B*W
%
% INPUT
% 	A     Dataset used for training
%   B     Dataset used for evaluation
%   KNN   Number of nearest neighbours
%
% OUTPUT
%   W     Density estimate
%
% DESCRIPTION
% A density estimator is constructed based on the k-Nearest Neighbour rule
% using the objects in A. In case A is labeled, density estimates are
% performed classwise and combined by the class priors. The default KNN is
% the square root of the size of the class. The data is scaled by variance
% normalisation determined by the training set.
%
% The mapping W may be applied to a new dataset B using DENSITY = B*W.
%
% SEE ALSO (PRTools Guide)
% DATASETS, MAPPINGS, KNNC, PARZENM, GAUSSM

% 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 w = knnm(varargin)

  mapname = 'KNN density estimation';
  argin = shiftargin(varargin,'scalar');
  argin = setdefaults(argin,[],[]);

  if mapping_task(argin,'definition')

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

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

		[a,knn] = deal(argin{:});
		if isa(a,'prdataset')
			labname = getname(a);
		else
			labname = '';
		end
		if (~isdataset(a) && ~isdatafile(a)) || getsize(a,3) ~= 1
			w = mclassm(a,prmapping(mfilename,knn),'weight');
			w = setlabels(w,labname);
			w = setname(w,mapname);
			return
		end
		islabtype(a,'crisp');
		isvaldfile(a,1);
		a = testdatasize(a,'objects');
		a = remclass(a);
		[m,k] = size(a);
    knn = setdefaults({knn},round(sqrt(m)));
		w = prmapping(mfilename,'trained',{a,knn},labname,k,1);
		w = setname(w,mapname);

	else															% Execute a trained mapping V.

    [a,v] = deal(argin{:});
    [b,knn] = getdata(v);
		[m,k] = size(b);
		u = scalem(b,'variance');
		a = a*u;
		b = b*u;
		d = sqrt(distm(+a,+b));					% Calculate squared distances
		s = sort(d,2);						    	% and find nearest neighbours.
		ss = s(:,knn);									% Find furthest neighbour
		ss(ss==0) = realmin.^(1/k);	    % Avoid zero distances
		f = knn./(m*nsphere(k)*(ss.^k));% Normalize by the volume of sphere
																		% defined by the furthest neighbour.
		f = f*prod(u.data.rot);         % Correct for scaling

		w = setdat(a,f,v);							% proper normalisation has still to be done.

	end

return

function v = nsphere(k)

	% Volume k-dimensional sphere
	v = (2*(pi^(k/2))) / (k*gamma(k/2));

return


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Example code for estimating the HRF using the Inverse-Logit Model, a
% Finte Impluse Response Model and the Canonical HRF with 2 derivatives.
% Also the code illustrates our code for detecting model misspecification.
%
% By Martin Lindquist and Tor Wager
% Created  10/02/09
% Last edited 05/20/13
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Load time course
%

mypath = which('ilogit');
if isempty(mypath), error('Cannot find directory with ilogit.m and other functions. Not on path?'); end
[mydir] = fileparts(mypath)

load(fullfile(mydir,'timecourse'))

tc = (tc- mean(tc))/std(tc);
len = length(tc);

%% Or: create your own
[xBF] = spm_get_bf(struct('dt', .5, 'name', 'hrf (with time and dispersion derivatives)', 'length', 32));
clear Xtrue
for i = 1:1, xx = conv(xBF.bf(:,i), [1 1 1 1 1 1 ]');
    Xtrue(:, i) = xx(1:66);
end
for i = 2:3, xx = conv(xBF.bf(:,i), [1 1]');
    Xtrue(:, i) = xx(1:66);
end
hrf = Xtrue * [1 .3 .2]';
xsecs = 0:.5:32;

hrf = [ 0; 0; hrf];
hrf = hrf(1:length(xsecs));
hrf = hrf ./ max(hrf);
figure; plot(xsecs, hrf, 'k')
%hrf = hrf(1:4:end); % downsample to TR, if TR is > 0.5

R = randperm(640); R = sort(R(1:36));
Run = zeros(640,1);
for i=1:length(R), Run(R(i)) = 1; end;
true_sig = conv(Run, hrf);
true_sig = true_sig(1:640);

tc_noise = noise_arp(640, [.3 0]);
tc = true_sig +  0.5 * tc_noise;
% tc = true_sig;
%figure; plot(tc);

Runc{1} = Run;

%%

create_figure; subplot(3,1,1); han = plot(tc);
title('Sample time course'); drawnow

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Settings
%

TR = 0.5;
%T = round(30/TR);
T = 30;
t = 1:TR:T;                        % samples at which to get Logit HRF Estimate
FWHM = 4;                       % FWHM for residual scan
pval = 0.01;
df = 600;
alpha = 0.001;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Create stick function (sample event onsets)
% Variable R contains onset times
% Variable Run contains stick (a.k.a. delta or indicator) function

% R = [3, 21, 56, 65, 109, 126, 163, 171, 216, 232, 269, 282, 323, 341, 376, 385, 429, 446, 483, 491, 536, 552, 589, 602];
% Run = zeros(640,1);
% for i=1:length(R), Run(R(i)) = 1; end;
%

try
    hold on;
    hh = plot_onsets(R,'k',-3,1, 1);
    drawnow
catch
    disp('Couldn''t find function to add onset sticks to plot. Skipping.')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using IL-function

% Choose mode (deterministic/stochastic)

mode = 0;   % 0 - deterministic aproach
            % 1 - simulated annealing approach
            % Please note that when using simulated annealing approach you
            % may need to perform some tuning before use.

[h1, fit1, e1, param] = Fit_Logit2(tc,TR,Runc,T,mode);
[pv sres sres_ns1] = ResidScan(e1, FWHM);
[PowLoss1] = PowerLoss(e1, fit1, (len-7) , tc, TR, Runc, alpha);

hold on; han(2) = plot(fit1,'r');

disp('Summary: IL_function');

disp('Amplitude:'); disp(param(1));
disp('Time-to-peak:'); disp(param(2)*TR);
disp('Width:'); disp(param(3)*TR);

disp('MSE:'); disp((1/(len-1)*sum(e1.^2)));
disp('Mis-modeling:'); disp(pv);
disp('Power Loss:'); disp(PowLoss1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using FIR-model

% Choose mode (FIR/sFIR)

mode = 1;   % 0 - FIR
            % 1 - smooth FIR

[h2, fit2, e2, param] = Fit_sFIR(tc,TR,Runc,T,mode);
[pv sres sres_ns2] = ResidScan(e2, FWHM);
[PowLoss2] = PowerLoss(e2, fit2, (len-T) , tc, TR, Runc, alpha);

hold on; han(3) = plot(fit2,'g');

disp('Summary: FIR');

disp('Amplitude'); disp(param(1));
disp('Time-to-peak'); disp(param(2)*TR);
disp('Width'); disp(param(3)*TR);

disp('MSE:'); disp((1/(len-1)*sum(e2.^2)));
disp('Mis-modeling'); disp(pv);
disp('Power Loss:'); disp(PowLoss2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using Canonical HRF + 2 derivatives

p=1;

[h3, fit3, e3, param, info] = Fit_Canonical_HRF(tc,TR,Runc,30,p);
[pv sres sres_ns3] = ResidScan(e3, FWHM);
[PowLoss3] = PowerLoss(e3, fit3, (len-p) , tc, TR, Runc, alpha);

hold on; han(4) = plot(fit3,'m');

legend(han,{'Data' 'IL' 'sFIR' 'DD'})

disp('Summary: Canonical + 2 derivatives');

disp('Amplitude'); disp(param(1));
disp('Time-to-peak'); disp(param(2)*TR);
disp('Width'); disp(param(3)*TR);

disp('MSE:'); disp((1/(len-1)*sum(e3.^2)));
disp('Mis-modeling'); disp(pv);
disp('Power Loss:'); disp(PowLoss3);

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

%figure;
%%

subplot(3,2,5); hold on;
plot(xsecs, hrf, 'k')
xsecs1 = xsecs(1:length(h1));
han2 = plot(xsecs1, h1,'r');
xsecs2 = xsecs(1:length(h2));
han2(2) = plot(xsecs2, h2,'g');
xsecs3 = xsecs(1:length(h3));
han2(3) = plot(xsecs3, h3,'m');
legend(han2,{'IL' 'sFIR' 'DD'})
title('Estimated HRF');

subplot(3,1,2); hold on;
hh = plot_onsets(R,'k',-3,1);
drawnow

han3 = plot(sres_ns1,'r');
hold on; han3(2) = plot(sres_ns2,'g');
hold on; han3(3) = plot(sres_ns3,'m');
hold on; plot((1:len),zeros(len,1),'--k');
legend(han3,{'IL' 'sFIR' 'DD'})
title('Mis-modeling (time course)');

subplot(3,2,6); hold on;

[s1] = Fit_sFIR(sres_ns1,TR,Runc,T,0);
[s2] = Fit_sFIR(sres_ns2,TR,Runc,T,0);
[s3] = Fit_sFIR(sres_ns3,TR,Runc,T,0);

han4 = plot(s1(1:T),'r');
hold on; han4(2) = plot(s2(1:T),'g');
hold on; han4(3) = plot(s3(1:T),'m');
hold on; plot((1:T),zeros(T,1),'--k');
legend(han4,{'IL' 'sFIR' 'DD'})
title('Mis-modeling (HRF)');


function [Q, V, policy, mean_discrepancy] = mdp_Q_learning(P, R, discount, N)

% mdp_Q_learning   Evaluation of the matrix Q, using the Q learning algorithm
%
% Arguments
% -------------------------------------------------------------------------
% Let S = number of states, A = number of actions
%   P(SxSxA)  = transition matrix
%              P could be an array with 3 dimensions or
%              a cell array (1xA), each cell containing a sparse matrix (SxS)
%   R(SxSxA) or (SxA) = reward matrix
%              R could be an array with 3 dimensions (SxSxA) or
%              a cell array (1xA), each cell containing a sparse matrix (SxS) or
%              a 2D array(SxA) possibly sparse
%   discount  = discount rate in ]0; 1[
%   N(optional) = number of iterations to execute, default value: 10000.
%                 It is an integer greater than the default value.
% Evaluation --------------------------------------------------------------
%   Q(SxA) = learned Q matrix
%   V(S)   = learned value function.
%   policy(S) = learned optimal policy.
%   mean_discrepancy(N/100) = vector of V discrepancy mean over 100 iterations
%             Then the length of this vector for the default value of N is 100.

% MDPtoolbox: Markov Decision Processes Toolbox
% Copyright (C) 2009  INRA
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%    * Redistributions of source code must retain the above copyright notice,
%      this list of conditions and the following disclaimer.
%    * Redistributions in binary form must reproduce the above copyright notice,
%      this list of conditions and the following disclaimer in the documentation
%      and/or other materials provided with the distribution.
%    * Neither the name of the <ORGANIZATION> nor the names of its contributors
%      may be used to endorse or promote products derived from this software
%      without specific prior written permission.
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
% ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
% IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
% INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
% BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
% DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
% OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
% OF THE POSSIBILITY OF SUCH DAMAGE.

% check of arguments
if (discount <= 0 || discount >= 1)
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: Discount rate must be in ]0,1[')
    disp('--------------------------------------------------------')
elseif (nargin >= 4) && (N < 10000)
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: N must be upper than 10000')
    disp('--------------------------------------------------------')
else

    % initialization of optional arguments
    if (nargin < 4); N=10000; end;

    % Find number of states and actions
    if iscell(P)
        S = size(P{1},1);
        A = length(P);
    else
        S = size(P,1);
        A = size(P,3);
    end;

    % Initialisations
    Q = zeros(S,A);
    dQ = zeros(S,A);
    mean_discrepancy = [];
    discrepancy = [];

    % Initial state choice
    s = randi([1,S]);

    for n=1:N

        % Reinitialisation of trajectories every 100 transitions
        if (mod(n,100)==0); s = randi([1,S]); end;

        % Action choice : greedy with increasing probability
        % probability 1-(1/log(n+2)) can be changed
        pn = rand(1);
        if (pn < (1-(1/log(n+2))))
          [nil,a] = max(Q(s,:));
        else
          a = randi([1,A]);
        end;

        % Simulating next state s_new and reward associated to <s,s_new,a>
        p_s_new = rand(1);
        p = 0;
        s_new = 0;
        while ((p < p_s_new) && (s_new < S))
            s_new = s_new+1;
            if iscell(P)
                p = p + P{a}(s,s_new);
            else
                p = p + P(s,s_new,a);
            end;
        end;
        if iscell(R)
            r = R{a}(s,s_new);
        elseif ndims(R) == 3
            r = R(s,s_new,a);
        else
            r = R(s,a);
        end;

        % Updating the value of Q
        % Decaying update coefficient (1/sqrt(n+2)) can be changed
        delta = r + discount*max(Q(s_new,:)) - Q(s,a);
        dQ = (1/sqrt(n+2))*delta;
        Q(s,a) = Q(s,a) + dQ;

        % Current state is updated
        s = s_new;

        % Computing and saving maximal values of the Q variation
        discrepancy(mod(n,100)+1) = abs(dQ);

        % Computing means all over maximal Q variations values
        if (length(discrepancy) == 100)
           mean_discrepancy = [ mean_discrepancy mean(discrepancy)];
           discrepancy = [];
        end;

    end;

    %compute the value function and the policy
    [V, policy] = max(Q,[],2);

end;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%               successive approximation converter                 %
%            with finite DAC's slew-rate and bandwidth             %
%       code by Fabrizio Conso, university of pavia, student       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [counter,thresholds]=Approx(input,nbit,f_bw,sr,f_s)
% input= input sample
% nbit=  number of converter bits
% f_bw=  DAC bandwidth [f_s]
% sr=    DAC slew-rate [V_fs/T_s]
% f_s=   sampling frequency

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                global variables                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                 %
 threshold(1,nbit+1)=0;  % threshold array
 counter=0;              % converter decimal output
 threshold(1)=0.5;       % 0 threshold
 threshold(2)=0.5;       % first threshold
 threshold_id=0.5;       % next ideal threshold
 tau=1/(2*pi*f_bw);      % DAC output pole
 in=input;
 Tmax=1/(f_s*nbit);      % clock period
%                                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 for i=2:(nbit+1)       % conversion cycle

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                  finite  bandwidth & slew-rate                   %
%                       error calculation                          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                  %
   deltaV=abs(threshold_id-threshold(i-1));
   slope=deltaV/tau;
   if slope > sr
        tslew=(deltaV/sr) - tau;
            if tslew >= Tmax              % only slewing
                error = deltaV - sr*Tmax;
            else
                texp = Tmax - tslew;
                error = (deltaV-sr*tslew)*exp(-texp/tau);
            end

   else			                % only exponential settling
	    texp = Tmax;
	    error = deltaV*exp(-texp/tau);
   end

   threshold(i) = threshold_id - sign(threshold_id-threshold(i-1))*error;
%                                                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      successive approximation       %
%        conversion algorythm         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                     %
    if (in-threshold(i)) > 0
        threshold_id=threshold_id+1/2^i;
        bit=1;
    else
        threshold_id=threshold_id-1/2^i;
        bit=0;
    end
 counter=(counter+bit*2^(nbit-i+1));
 thresholds(i-1)=threshold(i);
 end
%                                     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%             Output             %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                %
 counter=counter;
 if nargout > 1
	thresholds=thresholds;
 end
%                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%