function y = vl_nnpdist(x, x0, p, varargin)
%VL_NNPDIST CNN p-distance from target.
%   VL_NNPDIST(X, X0, P) computes the P distance raised of each feature
%   vector in X to the corresponding feature vector in X0:
%
%     Y(i,j,1) = (SUM_d (X(i,j,d) - X0(i,j,d))^P)^(1/P)
%
%   X0 should have the same size as X; the outoput Y has the same
%   height and width as X, but depth equal to 1. Optionally, X0 can
%   be a 1 x 1 x D x N array, in which case the same target feature
%   vector in X0 is compared to all feature vectors in X.
%
%   Setting the `noRoot` option to `true` does not take the 1/P power
%   in the formula, computing instead
%
%     Y(i,j,1) = SUM_d (X(i,j,d) - X0(i,j,d))^P
%
%   For example, `vl_nnpdist(x, x0, 2, 'noRoot', true)` computes the
%   squared L2 distance.
%
%   DZDX = VL_NNPDISTP(X, X0, P, DZDY) computes the derivative of the
%   block projected onto DZDY. DZDX and DZDY have the same dimensions
%   as X and Y, respectively.
%
%   VL_NNPDIST(___, 'OPT', VAL, ...) accepts the following options:
%
%   `NoRoot`:: `false`
%      If set to true, compute the P-distance to the P-th power.
%
%   `Epsilon`:: 1e-6
%      When computing derivatives, quantities that are divided in are
%      lower boudned by this value. For example, the L2 distance is
%      not smooth at the origin; this option prevents the
%      derivative from diverging.
%
%   `Aggregate`:: false
%      Instead of returning one scalar for each spatial location in
%      the inputs, sum all of them into a single scalar.
%
%   `InstanceWeights``:: `[]`
%      Optionally weight individual instances. This parameter can be
%      eigther a scalar or a weight mask, one for each pixel in the
%      input tensor.

% Copyright (C) 2015  Karel Lenc and Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

% -------------------------------------------------------------------------
%                                                             Parse options
% -------------------------------------------------------------------------

opts.noRoot = false ;
opts.epsilon = 1e-6 ;
opts.aggregate = false ;
opts.instanceWeights = [] ;
backMode = numel(varargin) > 0 && ~ischar(varargin{1}) ;
if backMode
  dzdy = varargin{1} ;
  opts = vl_argparse(opts, varargin(2:end)) ;
else
  dzdy = [] ;
  opts = vl_argparse(opts, varargin) ;
end

% -------------------------------------------------------------------------
%                                                             Parse options
% -------------------------------------------------------------------------

d = bsxfun(@minus, x, x0) ;

if ~isempty(dzdy) && ~isempty(opts.instanceWeights)
  dzdy = bsxfun(@times, opts.instanceWeights, dzdy) ;
end

if ~opts.noRoot
  if isempty(dzdy)
    if p == 1
      y = sum(abs(d),3) ;
    elseif p == 2
      y = sqrt(sum(d.*d,3)) ;
    else
      y = sum(abs(d).^p,3).^(1/p) ;
    end
  else
    if p == 1
      y = bsxfun(@times, dzdy, sign(d)) ;
    elseif p == 2
      y = max(sum(d.*d,3), opts.epsilon).^(-0.5) ;
      y = bsxfun(@times, bsxfun(@times, dzdy, y),  d) ;
    elseif p < 1
      y = sum(abs(d).^p,3).^((1-p)/p) ;
      y = bsxfun(@times, bsxfun(@times, dzdy, y), max(abs(d), opts.epsilon).^(p-1) .* sign(d)) ;
    else
      y = max(sum(abs(d).^p,3), opts.epsilon).^((1-p)/p) ;
      y = bsxfun(@times, bsxfun(@times, dzdy, y), abs(d).^(p-1) .* sign(d)) ;
    end
  end
else
  if isempty(dzdy)
    if p == 1
      y = sum(abs(d),3) ;
    elseif p == 2
      y = sum(d.*d,3) ;
    else
      y = sum(abs(d).^p,3) ;
    end
  else
    if p == 1
      y = bsxfun(@times, dzdy, sign(d)) ;
    elseif p == 2
      y = bsxfun(@times, 2 * dzdy, d) ;
    elseif p < 1
      y = bsxfun(@times, p * dzdy, max(abs(d), opts.epsilon).^(p-1) .* sign(d)) ;
    else
      y = bsxfun(@times, p * dzdy, abs(d).^(p-1) .* sign(d)) ;
    end
  end
end

if isempty(dzdy)
  if ~isempty(opts.instanceWeights)
    y = bsxfun(@times, opts.instanceWeights, y) ;
  end
  if opts.aggregate
    y = sum(sum(y)) ;
  end
end


function [ abd, info ] = cpbfa ( abd, lda, n, m )

%*****************************************************************************80
%
%% CPBFA factors a complex hermitian positive definite band matrix.
%
%  Discussion:
%
%    CPBFA is usually called by CPBCO, but it can be called
%    directly with a saving in time if RCOND is not needed.
%
%  Band storage:
%
%    If A is a hermitian positive definite band matrix,
%    the following program segment will set up the input.
%
%      m = (band width above diagonal)
%      do j = 1, n
%        i1 = max ( 1, j-m )
%        do i = i1, j
%          k = i-j+m+1
%          abd(k,j) = a(i,j)
%        end do
%      end do
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex ABD(LDA,N); the matrix to be factored.
%    The columns of the upper triangle are stored in the columns of ABD
%    and the diagonals of the upper triangle are stored in the rows of ABD.
%
%    Input, integer LDA, the leading dimension of ABD.
%    LDA must be at least M+1.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer M, the number of diagonals above the main diagonal.
%    0 <= M < N.
%
%    Output, integer INFO.
%    0, for normal return.
%    K, if the leading minor of order K is not positive definite.
%
%    Output, complex ABD(LDA,N); an upper triangular matrix R, stored in
%    band form, so that A = hermitian(R)*R.
%
  info = 0;

  for j = 1 : n

    s = 0.0;
    ik = m + 1;
    jk = max ( j - m, 1 );
    mu = max ( m + 2 - j, 1 );

    for k = mu : m
      t = abd(k,j) - abd(ik:ik+k-mu-1,jk)' * abd(mu:mu+k-mu-1,j);
      t = t / abd(m+1,jk);
      abd(k,j) = t;
      s = s + real ( t * conj ( t ) );
      ik = ik - 1;
      jk = jk + 1;
    end

    s = real ( abd(m+1,j) ) - s;

    if ( s <= 0.0 | imag ( abd(m+1,j) ) ~= 0.0 )
      info = j;
      break
    end

    abd(m+1,j) = sqrt ( s );

  end

  return
end


%  Script file: microphone.m
%
%  Purpose:
%    This program plots the gain pattern of a cardioid
%    microphone.
%
%  Record of revisions:
%      Date       Programmer          Description of change
%      ====       ==========          =====================
%    01/15/07    S. J. Chapman        Original code
%
% Define variables:
%   g         -- Microphone gain constant
%   gain      -- Gain as a function of angle
%   theta     -- Angle from microphone axis (radians)

% Calculate gain versus angle
g = 0.5;
theta = 0:pi/20:2*pi;
gain = 2*g*(1+cos(theta));

% Plot gain
polar (theta,gain,'r-');
title ('\bfGain versus angle \theta');


function Xr = pcaRandVec( U, mu, vars, k, n, hypershpere, show )
% Generate random vectors in PCA subspace.
%
% Used to generate random vectors from the subspace spanned by the first k
% principal components.  The points generated come from the gaussian
% distribution from within the subspace. Can optionally generate points on
% the subspace that are also on a hypershpere centered on the origin.  This
% may be useful if the original data points were all from a hypershpere --
% for example they were normalized via imNormalize.  Set the optional
% hypershpere flag to 1 to generate points only on the hypersphere.
%
% USAGE
%  Xr = pcaRandVec( U, mu, vars, k, n, [hypershpere], [show] )
%
% INPUTS
%  U           - returned by pca.m
%  mu          - returned by pca.m
%  vars        - returned by pca.m
%  k           - number of principal coordinates to use
%  n           - number of points to generate
%  hypershpere - [0] generate points on hypersphere (see above)
%  show        - [1] figure to use for display (no display if == 0)
%
% OUTPUTS
%  Xr          - resulting randomly generated vectors
%
% EXAMPLE
%
% See also PCA IMNORMALIZE
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<6 || isempty(hypershpere) ); hypershpere=0; end
if( nargin<7 || isempty(show) ); show=0; end

siz1 = size(mu);   nd = ndims(mu);
sizX = [siz1, n];  D = prod(siz1);

% generate random vectors inside of subspace.
C = diag( vars(1:k).^-1 );
Yr = C * randn(k,n);
Uk = U(:,1:k);
Xr = Uk * Yr;

if(hypershpere)
  % The final point must lie on hypershpere.  Need to scale each element xr
  % of Xr so that after adding it to mu the resulting vector has length D.
  % So need to find k>0 such that (k*xr + mu) has length D.  To find k
  % simply solve the quadratic equation induced by ||(k*xr + mu)||=D,
  % choosing the root such that k>0.  Note that the mean of resulting
  % vector will not necessarily be 0, but the variance will pretty much be
  % 1! Regardless, renomralize at the end.
  muv = mu(:);  muMag = dot(muv,muv);
  for i=1:n
    xr = Xr(:,i);
    rs = roots( [dot(xr,xr), 2 * dot(xr,muv), muMag-D] );
    Xr(:,i) = muv + max(rs)*xr;
  end
  Xr = reshape( Xr, sizX );
  Xr = fevalArrays( Xr, @imNormalize );
else
  % simply add the mean to reshaped Xr
  Xr = reshape( Xr, sizX );
  muRep = repmat(mu, [ones(1,nd), n ] );
  Xr = Xr + muRep;
end

% optionaly show resulting vectors
if(show && (nd==2 || nd==3)); figure(show); montage2(Xr); end

%%% Little test - see if eigenvectors induced by randomly generated vectors
%%% are the same as the original eigenvectors.  Assumes [U,mu,vars] exist.
%   Xr = pcaRandVec( U, mu, vars, 3, 100 );
%   [ Ur, mur, varsr ] = pca( Xr );
%   ind = 3;
%   Uim = reshape( U(:,ind), [ size(mu,1), size(mu,2) ]  );
%   Uimr = reshape( Ur(:,ind), [ size(mu,1), size(mu,2) ]  );
%   if( sum(abs(Uim-Uimr))>sum(abs(Uim+Uimr))) Uimr=Uimr*-1; end; %sign?
%   clf; subplot(3,1,1); im( Uim ); subplot(3,1,2); im( Uimr );
%   subplot(3,1,3); im( Uim - Uimr);
%%%


function [out] = saturation_10(p1,p2,p3,S,In)
%saturation_10

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Saturation excess flow from a store with different degrees
%               of saturation (min-max exponential variant)
% Constraints:  -
% @(Inputs):    p1   - maximum contributing fraction area [-]
%               p2   - minimum contributing fraction area [-]
%               p3   - exponentia scaling parameter [-]
%               S    - current storage [mm]
%               In   - incoming flux [mm/d]

out = min(p1,p2+p2.*exp(p3.*S)).*In;

end


function [xc,good,bad,type] = cornerfinder(xt,I,wintx,winty,wx2,wy2);

%[xc] = cornerfinder(xt,I);
%
%Finds the sub-pixel corners on the image I with initial guess xt
%xt and xc are 2xN matrices. The first component is the x coordinate
%(horizontal) and the second component is the y coordinate (vertical)
%
%Based on Harris corner finder method
%
%Finds corners to a precision below .1 pixel!
%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!!
%Sept 1998 - UPDATED to handle diverged points: we keep the original points
%good is a binary vector indicating wether a feature point has been properly
%found.
%
%Add a zero zone of size wx2,wy2
%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher
%resolution and larger number of iterations.

% California Institute of Technology
% (c) Jean-Yves Bouguet -- Oct. 14th, 1997

line_feat = 1; % set to 1 to allow for extraction of line features.

xt = xt';
xt = fliplr(xt);

if nargin < 4,
   winty = 5;
   if nargin < 3,
      wintx = 5;
   end;
end;

if nargin < 6,
   wx2 = -1;
   wy2 = -1;
end;

%mask = ones(2*wintx+1,2*winty+1);
mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2);

% another mask:
[X,Y] = meshgrid(-winty:winty,-wintx:wintx);
mask2 = X.^2 + Y.^2;
mask2(wintx+1,winty+1) = 1;
mask2 = 1./mask2;
%mask - mask2;

if (wx2>0) & (wy2>0),
   if ((wintx - wx2)>=2)&((winty - wy2)>=2),
      mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1);
   end;
end;

offx = [-wintx:wintx]'*ones(1,2*winty+1);
offy = ones(2*wintx+1,1)*[-winty:winty];

resolution = 0.005;

MaxIter = 10;

[nx,ny] = size(I);
N = size(xt,1);

xc = xt; % first guess... they don't move !!!

type = zeros(1,N);

for i=1:N,

   v_extra = resolution + 1; 		% just larger than resolution

   compt = 0; 				% no iteration yet

   while (norm(v_extra) > resolution) & (compt<MaxIter),

      cIx = xc(i,1); 			%
      cIy = xc(i,2); 			% Coords. of the point
      crIx = round(cIx); 		% on the initial image
      crIy = round(cIy); 		%
      itIx = cIx - crIx; 		% Coefficients
      itIy = cIy - crIy; 		% to compute
      if itIx > 0, 			% the sub pixel
	 vIx = [itIx 1-itIx 0]'; 	% accuracy.
      else
	 vIx = [0 1+itIx -itIx]';
      end;
      if itIy > 0,
	 vIy = [itIy 1-itIy 0];
      else
	 vIy = [0 1+itIy -itIy];
      end;

      % What if the sub image is not in?

      if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5;
      elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4;
      else
	 xmin = crIx-wintx-2; xmax = crIx+wintx+2;
      end;

      if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5;
      elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4;
      else
	 ymin = crIy-winty-2; ymax = crIy+winty+2;
      end;

      SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood
      SI = conv2(conv2(SI,vIx,'same'),vIy,'same');
      SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood
      [gy,gx] = gradient(SI); 		% The gradient image
      gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only
      gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients

      px = cIx + offx;
      py = cIy + offy;

      gxx = gx .* gx .* mask;
      gyy = gy .* gy .* mask;
      gxy = gx .* gy .* mask;

      bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))];

      a = sum(sum(gxx));
      b = sum(sum(gxy));
      c = sum(sum(gyy));

      dt = a*c - b^2;

      xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;

      %keyboard;

      if line_feat,

	 G = [a b;b c];
	 [U,S,V]  = svd(G);

	 %keyboard;

	 % If non-invertible, then project the point onto the edge orthogonal:

	 if (S(1,1)/S(2,2) > 50),
	    % projection operation:
	    xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)';
	    type(i) = 1;
	 end;

      end;

      %keyboard;

%      G = [a b;b c];
%      [U,S,V]  = svd(G);

%      if S(1,1)/S(2,2) > 150,
%	 bb2 = U'*bb;
%	 xc2 = (V*[bb2(1)/S(1,1) ;0])';
%      else
%	 xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
%      end;

      %if (abs(a)> 50*abs(c)),
%	 xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)];
%      elseif (abs(c)> 50*abs(a))
%	 xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt];
%      else
%	 xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
%      end;

      %keyboard;

      v_extra = xc(i,:) - xc2;

      xc(i,:) = xc2;

%      keyboard;

      compt = compt + 1;

   end
end;

% check for points that diverge:

delta_x = xc(:,1) - xt(:,1);
delta_y = xc(:,2) - xt(:,2);

%keyboard;

bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty);
good = ~bad;
in_bad = find(bad);

% For the diverged points, keep the original guesses:

xc(in_bad,:) = xt(in_bad,:);

xc = fliplr(xc);
xc = xc';

bad = bad';
good = good';


function h=entropyDist(npd)
%
% Compute entropy estimate using nearest neighbor estimate
%

% Copyright (C) 2003 Alexander Ihler; distributable under GPL -- see README.txt

Ce = .57721566490153286;

pts = getPoints(npd);

[N1,N2] = size(pts);
[tmp,D] = knn(npd,pts,2);

Sr = N1* pi^(N1/2) / gamma((N1/2) + 1);
h = N1/N2 * sum( log(D) ) + log(Sr * (N2-1)/N1 ) + Ce;


function [C,nc] = graph_coloring(A,nc)
  % GRAPH_COLORING Color a graph given an adjacency matrix. This heuristic is
  % designed to efficiently find a conservative coloring (i.e., using too many
  % colors). It does *not* attempt to find the minimal coloring. It is best used
  % when the provided nc is a few more than the optimal coloring. For example,
  % planar graphs can be colored with 4 colors, (slow) heuristics for finding the
  % optimal coloring will often find 6 colors, but this function will very
  % quickly find a 7-coloring (nc=7). On such a graph, if you passed nc=4, this
  % function will likely waste time attemping to find a 4-coloring, 5-coloring,
  % 6-coloring, and then very quickly find a 7-coloring. It is designed to give
  % up searching for parsimonious colorings and increase the number of colors
  % (throwing a warning).
  %
  % [C,nc] = graph_coloring(A,nc)
  %
  % Inputs:
  %   A  #V by #V adjacency matrix
  %   nc  desired number of colors (for planar/triangle meshes, 7 is a fast
  %     choice; for tetrahedral meshes, 13 appears to often be fast)
  % Outputs:
  %   C  #V list of color ids into (1:nc)
  %   nc  effective number of colors >= input nc

  n = size(A,1);
  [FI,FJ] = find(triu(A,1));
  % always marking the higher valence vertex as bad definitely makes things
  % worse 50x.
  %
  % if nc is much greater than optimal, always marking the lower valence vertex
  % doesn't much effect. But for nc close to optimal, this seems to make a very
  % big difference (300x)
  val = sum(A,2);
  swap = val(FI)<val(FJ);
  EI = FI.*swap + FJ.*~swap;
  EJ = FJ.*swap + FI.*~swap;

  onc = nc;
  % worst case just increase number of colors
  max_outer = 10;
  for outer = 1:max_outer
    % Might as well retry a few times for this nc
    for retry = 1:10
      C = ceil(nc*rand(n,1));
      for iter = 1:2*ceil(sqrt(size(A,1)))
        bad = unique(EI(C(EI)==C(EJ)));
        if numel(bad) == 0
          break;
        end
        C(bad) = nan;
        C(bad) = ceil(nc*rand(numel(bad),1));
      end
      if numel(bad) == 0
        break;
      end
    end
    if numel(bad) == 0
      break;
    end
    nc = nc+1;
    if nc>max_outer
      error('Failed to converge.\n');
    end
  end
  if nc ~= onc
    warning('Had to increasing number of colors')
  end

  %V2C = sparse(1:n,C,1,n,nc);
  % find all edges with the same color
  %A*V2C;
end


function rule_num = lyness_rule_num ( )

%*****************************************************************************80
%
%% LYNESS_RULE_NUM returns the number of Lyness quadrature rules.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    James Lyness, Dennis Jespersen,
%    Moderate Degree Symmetric Quadrature Rules for the Triangle,
%    Journal of the Institute of Mathematics and its Applications,
%    Volume 15, Number 1, February 1975, pages 19-32.
%
%  Parameters:
%
%    Output, integer RULE_NUM, the number of rules.
%
  rule_num = 21;

  return
end


function p = mstud_lpdf(x, mu, Cov, nu)
% p = mstud_lpdf(x, mu, Cov, nu)

D = length(x);

x = x(:);
mu = mu(:);

invCov = inv(Cov);

p = gammaln((D+nu)/2);
p = p - gammaln(nu/2);
p = p - D/2 * log(nu * pi);
p = p - 0.5 * log(det(Cov));
p = p + -(D+nu)/2 * log(1 + 1/nu*(x-mu)'*invCov*(x-mu));


function [cl, cp, cs] = dtiComputeWestinShapes(eigVal, denominator)
%
% [cl, cp, cs] = dtiComputeWestinShapes(eigVal, [denominator='lsum'])
% Computes Westin's shape indices
%'denominator' is a parameter allowing to choose an appropriate definition
%for Westin shapes.
%
% denominator='lsum' (default):
%    cl = (lambda_1 - lambda_2) / (lambda_1 + lambda_2 + lambda_3)
%    cp = 2 * (lambda_2 - lambda_3) / (lambda_1 + lambda_2 + lambda_3)
%    cs = 3 * lambda_3 / (lambda_1 + lambda_2 + lambda_3)
%
% This was the originial formulation described in:
%
% C-F. Westin, S. Peled, H. Gubbjartsson, R. Kikinis, and F.A. Jolesz.
% Geometrical diffusion measures for MRI from tensor basis analysis.
% In Proceedings 5th Annual ISMRM, 1997.
%
% denominator='l1':
%
% In later work, Westin et. al. adopted a simpler normalization
% formulation (e.g., see Westin et. al. 2002 Med. Image Anal.; PMID:
% 12044998) where the constants are dropped and the denominator is simply
% lambda_1. This definition produces cl+cp+cs=1 which is convenient for
% tensor shape representation in barycentric coordinates.
% In practice, the two methods produce very similar maps.
% cl = (lambda_1 - lambda_2) / (lambda_1)
% cp = 2 * (lambda_2 - lambda_3) / (lambda_1)
% cs = 3 * lambda_3 / (lambda_1)
%
% eigVal: XxYxZx3 or nx3xN array of tensor eigenvalues. Or, you can pass
% the dt6 array (XxYxZx6) and we'll compute the eigenvalues for you.
%
%
% HISTORY:
% 2004.07.22 RFD (bobd@stanford.edu) & ASH wrote it.
% 2007.01.02 SHC made changes to allow nx3xN format.
% 2009.01.05 EIR added option of using a more recent definition of Westin shapes

if ~exist('denominator', 'var')
    denominator='lsum';
end

if size(eigVal,4)==6
    [eigVec, eigVal] = dtiEig(eigVal);
    clear eigVec;
end

% Check inputs
switch ndims(eigVal)
    case 2,
        Ind    = 1; % Data in indexed nx6 format
        eigVal = shiftdim(eigVal, -2);
    case 3,
        if size(eigVal,2)==6 && size(eigVal,3)~=6
            Ind    = 1; % Data in indexed nx6xN format
            eigVal = shiftdim(eigVal, -2);
        else
            Ind    = 2; % Data in XxYx6 format
            eigVal = shiftdim(eigVal, -1);
        end
    otherwise,
        Ind = 0; % Data in XxYxZx6xN format
end

epsilon = 1e-10;
switch denominator
    case 'lsum'
        denum=sum(eigVal,4);

    case 'l1'
        denum=eigVal(:, :, :, 1);
    otherwise fprintf('Wrong denominator option');
end

nz = denum>epsilon;

% Avoid divide-by-zero (we'll replace these values with zeros below).
denum(~nz) = 1;

cl = (eigVal(:,:,:,1)-eigVal(:,:,:,2))./denum;
cp = 2*(eigVal(:,:,:,2)-eigVal(:,:,:,3))./denum;
cs = 3*eigVal(:,:,:,3)./denum;

if strcmp(denominator,'l1')
    cp=cp./2;
    cs=cs./3;
end

cl(~nz) = 0; cp(~nz) = 0; cs(~nz) = 0;
cl(cl>1.0) = 1.0;
cp(cp>1.0) = 1.0;
cs(cs>1.0) = 1.0;
cl(cl<0.0) = 0.0;
cp(cp<0.0) = 0.0;
cs(cs<0.0) = 0.0;

% Adjust output
switch Ind
    case 1,
        cl = shiftdim(cl, 2);
        cp = shiftdim(cp, 2);
        cs = shiftdim(cs, 2);
    case 2,
        cl = shiftdim(cl, 1);
        cp = shiftdim(cp, 1);
        cs = shiftdim(cs, 1);
end

return;


function [AA, aa, pp, rankA, p] = rowReduce(A, a, mode, printLevel)
% Eliminates dependent rows from `A` & `a` where :math:`A x = a`
%
% USAGE:
%
%    [AA, aa, pp, rankA, p] = rowReduce(A, a)
%
% INPUT:
%    A:        from :math:`A x = a`
%
% OPTIONAL INPUT:
%    a:        from :math:`A x = a`
%
%    mode:     If mode=1, LUSOL operates on A itself.
%              If mode=2, LUSOL operates on A'.
%
%    printLevel
%
% OUTPUT:
%    AA:       row reduced `A`
%    aa:       row reduced `a` i.e. `aa = a(pp)`
%    pp:       1:rankA indices of independent rows
%    rankA:    rank of `A`
%    p:        row permutation which leaves first `1:rankA` rows independent and
%              last rows dependent
%
% .. Author: - Ronan Fleming, with linear algebra advice from Michael Saunders
%            Dept of Management Science and Engineering (MS&E) Stanford University

if ~exist('a','var') %create a if not provided
    a=sparse(size(A,1),1);
else
    if size(A,1)~=length(a)
        error('Dimensions of A and a are inconsistent');
    end
end

if ~exist('mode','var')
    mode=1;
end

if ~exist('printLevel','var')
    printLevel=1;
end

[mlt,nlt]=size(A);
archstr = computer('arch');
archstr = lower(archstr);
%archstr='';%bypass until issue with lusol tolerance sorted.
switch archstr
    case {'glnx86','glnxa64','maci64'}
        %Eliminate dependent rows
        [AA,aa,p,rankA] = lusolCondense(A,a,mode,printLevel-1);
        if ~(nnz(A)>0 && rankA==0) && printLevel>0
            fprintf('%s',['Eliminated ' int2str(mlt-rankA) ' dependent rows, using lusol.']);
        end
        pp=p(1:rankA);
        %case {'PCWIN','PCWIN64'}
    otherwise
        [AA, aa, pp, rankA, p] = qrRowReduce(A,a, printLevel);
end

if nnz(A)>0 && rankA==0
    %backup in case something has gone wrong with lusolCondense
    [AA, aa, pp, rankA, p] = qrRowReduce(A, a, printLevel);
end

fprintf('\n')
end

function [AA, aa, pp, rankA, p] = qrRowReduce(A, a,  printLevel)
    [mlt,nlt]=size(A);
    A=full(A);
    %Eliminate dependent rows
    [Q,R,P] = qr(A');
    [p,q,s] = find(P);
    s      = diag(R);
    tol    = 1e-8;
    rankA  = length(find(abs(s) > tol));
    % nnz(abs(diag(R))>1.e-15)
    AA = sparse(A(p(1:rankA),:));
    pp = p(1:rankA);
    aa = sparse(a(pp));
    if  printLevel>0
             fprintf('%s',['Eliminated ' int2str(mlt-rankA) ' dependent rows, using qr factorisation.']);
    end
end


%% Trend-Cycle tutorial: Predicting recessions
% Authors:   Filippo Ferroni and  Fabio Canova
% Date:     27/05/2020, revised  15/12/2020

close all; clc; clear all;

addpath ../../cmintools/
addpath ../../bvartools/

% predicting  recession and the start of  a recession in the  euro  area.
% use recession indicator constructed with example_2_dating.m; regressors
% labor  productivity, commodity prices, long term  rates  or spread.

% Euro area AWM DATABASE: Quarterly
    [a,b,~] = xlsread('awm19up18.xlsx');
    % names of variables
    varnames = b(1,2:end);

    % time convention: Q1 = .00 and Q4 =0.75
    time = 1970 : .25 : 2017.75;
    time_start = find(time==1970.50);
    time_start1 = find(time==1999.50);
    time_end   = find(time==2017.75);
    time_break   = find(time==2007.75);

    % The  CREDIT DATA: Quartely
    [e,f,~]=xlsread('ECB_Credit_gaps.xlsx');
    varnames2= f(1,2:end);

    % real GDP, real consumption, real  investment, GDP defl, HICP,
    % short and long term interest rate, commodity  prices,
    % labor  productivity, total  employment, credit to NFC to  GDP
    yy = [a(:,1) a(:,2) a(:,4) a(:,7) a(:,19) a(:,33) a(:,34) ...
        a(:,35) a(:,42) a(:,29) e(time_start-2:time_end,25)];
    % names: YER PCR ITR YED HICP STN LTN COMPR LPROD LNN

    % data transformations:
    % 1. the log of output, consumption,investment
    ddata(:,1:3) = log(yy(time_start+1 : time_end,1:3));
    % 2. the log/log difference of GDP deflator index and CPI
    %ddata(:,4:5) = log(yy(time_start+1  : time_end,4:5));
    ddata(:,4:5) = diff(log(yy(time_start : time_end,4:5)))*400;
    % 3. the level of short and long term interest rate
    ddata(:,6:7) = yy(time_start +1 : time_end,6:7);
    % 4. the log/log difference of commodity prices
    ddata(:,8) = log(yy(time_start+1  : time_end,8));
    %ddata(:,8) = diff(log(yy(time_start : time_end,8)))*400;
    % 5. the taking the log of labor  productivity and  employment
    ddata(:,9:10) = log(yy(time_start+1 : time_end,9:10));
    %ddata(:,9) = diff(log(yy(time_start+1 : time_end,9)));
    %ddata(:,10) = log(yy(time_start+1 : time_end,10));

    % 6. spread
    ddata(:,11)= ddata(:,7)-ddata(:,6);
    %  data for  credit  to  GDP starts  only  at  time_start1 (1999:25)
    ddata(:,12) = yy(time_start+1:time_end,11);
    endd=length(ddata);

 % cc=ones(time_end-time_start,1);  % constant
 % pick log labor productivity, log commodity prices, long term rate
 % z=ddata(:,[9 8 7]);
 % pick log labor productivity, log commodity prices, spread
 z=ddata(:, [9 8 11]);

load  Eurorec
% recind has  the  recession indicator  created  with  dating_exa.m
x=recind;

% use contemporanous  values
% [estimator,cov_Hessian,ME1,ME2,ME_std] = probit(x,z);

% use  one  period lagged  values
zz=zeros(size(z,1), size(z,2));
zz(2:length(z),:)=z(1:length(z)-1,:);
timeplot = time(time_start:time_end-1);

% Estimate Probit model and the marginal effects
[estimator,cov_Hessian,ME1,ME2,ME1_std] = probit(x,zz);

% prediction
pz=[ones(length(zz),1) zz];
px=normpdf(pz*estimator);
half=0.5*ones(length(z),1);

plot(timeplot,px,'r-.','Linewidth',2); hold on;
plot(timeplot,half,'b:','Linewidth',2); hold  on;
plot(timeplot,x,'k-','Linewidth',2); hold  off; axis  tight;
legend('predition','halfline', 'actual')
pause

close  all;

% do  estimation recursively: predicting  the  beginning  of  a  recession
% which is  the  same  as  predicting  a  peak
% recession dates
%rec1b = find(time==1974.00);
%rec1e = find(time==1975.25);
%[estimator1,cov_Hessian1,ME11,ME21,ME11_std] = probit(x(1:rec1b),zz(1:rec1b,:));

rec2b = find(time==1980.00);
rec2e = find(time==1984.25);
disp('Predicting 1980 recession')
[estimator2,cov_Hessian2,ME12,ME22,ME12_std] = probit(x(1:rec2b),zz(1:rec2b,:));
pz2=[ones(rec2b,1) zz(1:rec2b,:)];
px2=normpdf(pz2*estimator2);
half2=0.5*ones(rec2b,1);

plot(timeplot(1:rec2b),x(1:rec2b),'k-','Linewidth',2); hold  on;
plot(timeplot(1:rec2b),half2(1:rec2b),'b:','Linewidth',2); hold  on;
plot(timeplot(1:rec2b),px2(1:rec2b),'r-.','Linewidth',2); hold  off;axis tight;
legend('actual', 'halfline', 'predition')
pause

close all;

rec3b = find(time==1992.00);
rec3e = find(time==1993.75);
disp('Predicting  1992 recession')
[estimator3,cov_Hessian3,ME13,ME23,ME13_std] = probit(x(1:rec3b),zz(1:rec3b,:));
pz3=[ones(rec3b,1) zz(1:rec3b,:)];
px3=normpdf(pz3*estimator3);
half3=0.5*ones(rec3b,1);

plot(timeplot(1:rec3b),x(1:rec3b),'k-','Linewidth',2); hold  on;
plot(timeplot(1:rec3b),half3(1:rec3b),'b:','Linewidth',2); hold  on;
plot(timeplot(1:rec3b),px3(1:rec3b),'r-.','Linewidth',2); hold  off; axis  tight;
legend('actual', 'halfline','predition')
pause
close all;

rec4b = find(time==2001.00);
rec4e = find(time==2002.75);
disp('Predicting  2001 recession')
[estimator4,cov_Hessian4,ME14,ME24,ME14_std] = probit(x(1:rec4b),zz(1:rec4b,:));
pz4=[ones(rec4b,1) zz(1:rec4b,:)];
px4=normpdf(pz4*estimator4);
half4=0.5*ones(rec4b,1);

plot(timeplot(1:rec4b),x(1:rec4b),'k-','Linewidth',2); hold  on;
plot(timeplot(1:rec4b),half4(1:rec4b),'b:','Linewidth',2); hold  on;
plot(timeplot(1:rec4b),px4(1:rec4b),'r-.','Linewidth',2); hold  off; axis  tight;
legend('actual', 'halfline','predition')
pause
close all;

rec5b = find(time==2008.00);
rec5e = find(time==2009.50);
disp('Predicting 2008 recession')
[estimator5,cov_Hessian5,ME15,ME25,ME15_std] = probit(x(1:rec5b),zz(1:rec5b,:));
pz5=[ones(rec5b,1) zz(1:rec5b,:)];
px5=normpdf(pz5*estimator5);
half5=0.5*ones(rec5b,1);

plot(timeplot(1:rec5b),x(1:rec5b),'k-','Linewidth',2); hold  on;
plot(timeplot(1:rec5b),half5(1:rec5b),'b:','Linewidth',2); hold  on;
plot(timeplot(1:rec5b),px5(1:rec5b),'r-.','Linewidth',2); hold  off; axis tight;
legend('actual', 'halfline', 'predition')
pause
close all;

rec6b = find(time==2011.25);
rec6e = find(time==2013.00);
disp('Predicting  2011 recession')
[estimator6,cov_Hessian6,ME16,ME26,ME16_std] = probit(x(1:rec6b),zz(1:rec6b,:));
pz6=[ones(rec6b,1) zz(1:rec6b,:)];
px6=normpdf(pz6*estimator6);
half6=0.5*ones(rec6b,1);

plot(timeplot(1:rec6b),x(1:rec6b),'k-','Linewidth',2); hold  on;
plot(timeplot(1:rec6b),half6(1:rec6b),'b:','Linewidth',2); hold  on;
plot(timeplot(1:rec6b),px6(1:rec6b),'r-.','Linewidth',2); hold  off; axis  tight;
legend('actual', 'halfline', 'predition')

return


% By Roger Aarenstrup, roger.aarenstrup@mathworks.com
% 2006-08-18
%
% This is a state-space DC motor model of a
% Maxon RE25 10 Watt, precious metal brushes, 118743
%
% This model also have a weak connection to a load.
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%            The full dc motor model                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Vin is the input voltage to the motor
% i is the motor current
% th_m is the rotor angle, theta
% dth_m is the rotor angular velocity sometimes called omega
% th_l is the load angle
% dth_l is the load angular velocity

% Controller Sample Time
Ts = 100e-6;

% PARAMETERS DC MOTOR
Rm  = 2.06;             % Motor resistance (ohm)
Lm  = 0.000238;         % motor inductance (Henrys)
Kb = 1/((406*2*pi)/60); % Back EMF constant (Volt-sec/Rad)
Kt = 0.0235;            % Torque constand (Nm/A)
Jm = 1.07e-6;           % Rotor inertia (Kg m^2)
bm = 12e-7;             % MEchanical damping (linear model of
                        % friction: bm * dth)
% PARAMETERS LOAD
Jl = 10.07e-6;   % Load inertia (10 times the rotor)
bl = 12e-6;      % Load damping (friction)
Ks = 100;        % Spring constant for connection rotor/load
b = 0.0001;      % Spring damping for connection rotor/load

% SYSTEM MATRICES
%
% States:  [i dth_m th_m dth_l th_l]'
% Input:   Vin the motor voltage
% Outputs: same as states
%
Afull = [-Rm/Lm    -Kb/Lm       0       0       0;
         Kt/Jm   -(bm+b)/Jm   -Ks/Jm    b/Jm     Ks/Jm;
           0          1         0       0        0;
           0         b/Jl    Ks/Jl   -(b+bl)/Jl    -Ks/Jl;
           0          0         0       1        0];

Bfull = [1/Lm 0 0 0 0]';

Cfull = [0 1 0 0 0;
         0 0 1 0 0;
         0 0 0 1 0;
         0 0 0 0 1];

Dfull = [0 0 0 0]';

sys_full = ss(Afull, Bfull, Cfull, Dfull);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     The reduced dc motor model for current control      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% SYSTEM MATRICES
%
% States:  [dth_m th_m dth_l th_l]'
% Input:   I The current to the dc motor
% Outputs: same as states
%
Ared = [ -(bm+b)/Jm   -Ks/Jm    b/Jm     Ks/Jm;
              1         0       0        0;
             b/Jl    Ks/Jl   -(b+bl)/Jl    -Ks/Jl;
              0         0       1        0];

Bred = [Kt/Jm 0 0 0]';

Cred = eye(4);

Dred = [0 0 0 0]';

sys_red = ss(Ared, Bred, Cred, Dred);

% Discrete version of the model (as seen from the controller)
sys_red_d = c2d(sys_red, Ts, 'zoh');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%        State-space controller design attempt 1          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% The discrete poles of the closed loop system
dpole1 = exp(-2*pi*Ts*[20 22 24 26]);

% Calculate the control parameters
L1 = place(sys_red_d.a, sys_red_d.b, dpole1);

% Keep the static gain of the closed loop system to 1
F=feedback(sys_red_d, L1);   % The closed loop system, F.
Kstatic = freqresp(F, 0);    % Get the static gain of F.
Kstat = 1/Kstatic(4);        % It is the fourth output (load position)

% The above commands requires toolboxes, incase you don't have them
% you can simulate the system with a unit step is see the output static
% gain. Kstat will be the inverse of that.

% to verify the pole placement
%pzmap(F);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  State-space controller design attempt 2 - Integrator   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% The discrete poles of the closed loop system
dpole2 = exp(-2*pi*Ts*[20 22 24 26 5]);

% We only consider one output here
Cred_one = [0 1 0 0];

% Calculate the control parameters
Aint = [sys_red_d.a [0 0 0 0]';
        -Cred_one 1];

Bint= [sys_red_d.b' 0]';

Cint = [Cred_one 0];

Dint = [0]';

sys_int_d2 = ss(Aint, Bint, Cint, Dint, Ts);

% Controllability VS Observability analysis
%ob = obsv(sys_int_d2.a, sys_int_d2.c);
%cr = ctrb(sys_int_d2.a, sys_int_d2.b);
%rank(ob)
%rank(cr)

[L2, a, m] = place(sys_int_d2.a, sys_int_d2.b, dpole2);

% Feed Forward gain
Kff = L2(5)/(dpole2(5) - 1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  State-space controller design attempt 3 - Observer     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% These should be about twice as fast as the state feedback
% for the controller.
dpole3 = exp(-2*pi*Ts*[1400 1450 1500 1550]);
%dpole3 = [-0.3 -0.33 -0.35 -0.37];

% Recalculate the reduced model with one ouput
Cred2 = [0 1 0 0];

Dred2 = 0;

sys_red2 = ss(Ared, Bred, Cred2, Dred2);

% Discrete version of the model (as seen from the controller)
sys_red_d2 = c2d(sys_red2, Ts, 'zoh');

set(sys_red_d2, 'inputname', {'Um'}, ...
                'outputname', {'Enc_out'});

% Calculate the observer state feedback
K = place(sys_red_d2.a', sys_red_d2.c', dpole3);

% Put the observer together
Aobs = [sys_red_d2.a-K'*sys_red_d2.c];
Bobs = [sys_red_d2.b K'];
Cobs = eye(4);
Dobs = [0 0 0 0; 0 0 0 0]';

observer = ss(Aobs, Bobs, Cobs, Dobs, Ts, 'inputname', {'Ue', 'Enc_in'}, ...
              'outputname', {'dth_m', 'th_m', 'dth_l', 'th_l'});

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  State-space controller design attempt 4 - The Servo Case  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Kinv = (Jl+Jm)/Kt;

dpole4 = exp(-2*pi*Ts*[200 204 220 224 300]);
[L3, a, m] = place(sys_int_d2.a, sys_int_d2.b, dpole4);


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function pathS = MC_VG_S(S0,r,d,T,nu,theta,sigma,NTime,NSim,NBatches)
% discretization of Variance Gamma process
% using subordination

pathS = zeros(NSim,NTime+1,NBatches);       % create the output
lnS = zeros(NSim,NTime+1);                  % used per batch
dT = T / NTime;                             % delta time
omegaT = -1/nu * log(1-theta(1)*nu ...
    - nu*sigma(1)^2/2);                     % martingale correction
lnS(:,1) = log(S0);                         % Set the starting spot price

for l = 1 : NBatches                        % batch loop
    % G = nu * gamrnd(dT/nu,1,NSim,NTime);
    % dW = randn(NSim,NTime);
    for m=2:NTime+1                         % time loop
        G = nu * gamrnd(dT/nu,1,NSim,1);      % Gamma subordinator
        dW = randn(NSim,1);                 % Gaussians
        lnS(:,m) = lnS(:,m-1) ...           % log VG
            + (r-d-omegaT) * dT ...
            + theta(1) * G + sqrt(G) * sigma .* dW;
    end

    pathS(:,:,l) = exp(lnS);                % simulated paths
end


%% demo code
% reconstruction of retrospective subsampled Shepp-Logan phantom

% set paths
addpath(genpath(pwd));
addpath(genpath(['..',filesep,'CS_LAB_matlab']));

%% ------------------------------------------
% 1.) load data: phantom (2 slices, 1 channel)
% -------------------------------------------
dImg = phantom('Modified Shepp-Logan',256);
dKSpace = fftnshift(dImg,1:2);

%% --------------------------
% 2.) create subsampling mask
% ---------------------------
if(ispc), sExt = '.exe'; else sExt = ''; end
system(['.',filesep,'sampling',filesep,'Subsample',sExt,sprintf(' %d %d %d %d', 256, 1, 4, 1)]);
iMask = readSamplingMaskFromFile(['.',filesep,'samplingPattern.txt']);

%% -------------------------
% 3.) apply subsampling mask
% --------------------------
% k-Space dimensions
% cell array: NSlice x NChannels x NRepetitions x NAverages
%             each cell: 2D: yPhase x xFreq x (nTime/nPhases)
%                        3D: yPhase x xFreq x zPhase
%                        4D: yPhase x xFreq x zPhase x nTime/nPhases
dKSpaceSub = {dKSpace .* repmat(iMask,[1 256])};

%% --------------------
% 4.) reconstruct image
% ---------------------
% set some recon parameters
para.cstype = 'FOCUSS';
para.transformation = 'fft';
para.lambda = 1e-12;
para.lambdaTV = 1e-6;
para.measPara.dim = [size(dKSpaceSub{1}), 1, 1, 1];
para.measPara.LCall = ones(1,4);
para.measPara.dimension = '2D';
para.espresso.state = false;
para.espresso.direction = 'off';
para.espresso.pfn = 1;
para.flagOversampling = logical([0 0 0]);
para.postproc.turnImage = false;
para.postproc.FreqOversamplingCorr = false;
para.prop.flagPlot = false;

% CS reconstruction
dCSRecon = CS_reconstruction(dKSpaceSub, para);

% zero-padded reconstruction
dZeropadded = abs(ifftnshift(dKSpaceSub{1}));

%% ------------------
% 5.) compare results
% -------------------
figure;
subplot(1,3,1)
imagesc(dImg); colormap('gray');
axis 'equal'; set(gca,'XTick',[],'YTick',[]); axis 'off';
title('original')
subplot(1,3,2)
imagesc(dZeropadded); colormap('gray');
axis 'equal'; set(gca,'XTick',[],'YTick',[]); axis 'off';
title('zero-padded recon')
subplot(1,3,3)
imagesc(dCSRecon); colormap('gray');
axis 'equal'; set(gca,'XTick',[],'YTick',[]); axis 'off';
title('CS recon')


%% DEMO_visualization_von_mises_plasticity_01
% Below is a demonstration for:
%
% * Visualization of a Von Mises yield surface in 3D

%%

clear; close all; clc

%% Plot settings.

fontSizeAxis=25;
fontSizeText=35;
colorSet=viridis(3);
lineWidth1=1;
lineWidth2=3;
lineWidthAxis=3;
f=2;

%% Control parameters

sy=1; %Yield stress
tau_octahedral=sy*sqrt(2/3); %Octahedral shear stress = cylinder radius

%%
% Create ellipsoids
t=linspace(0,2*pi,1000); %Angular coordinates
x=tau_octahedral*cos(t);
y=tau_octahedral*sqrt(3).*sin(t);
z=zeros(size(x));
v=[x(:) y(:) z(:)]; %Ellipse coordinates

%Make cylinder "slices" ellipses by rotating above ellipse
R12=euler2DCM([0 0 (45/180)*pi]);
v12=v*R12;

R13_1=euler2DCM([0 (90/180)*pi 0]);
R13_2=euler2DCM([(135/180)*pi 0 0]);
v13=v*R13_1*R13_2;

R23_1=euler2DCM([(90/180)*pi 0 0]);
R23_2=euler2DCM([0 (-45/180)*pi 0 ]);
v23=v*R23_1*R23_2;

%%
% Creating cylinder data

% Creating input structure
inputStruct.cylRadius=tau_octahedral;
inputStruct.numRadial=250;
inputStruct.cylHeight=2*sqrt(2)*tau_octahedral;
inputStruct.numHeight=10;
inputStruct.meshType='quad';
[F,V,C]=patchcylinder(inputStruct);
Q=euler2DCM([0 pi/2 0]);
V=V*Q;

R1=euler2DCM([0 asin(1/sqrt(3)) 0]);
R2=euler2DCM([0 0 -(45/180)*pi]);
V=V*R1*R2;

[x1,y1]=meshgrid(-f*sy:1:f*sy);
z1=zeros(size(x1));

[x2,z2]=meshgrid(-f*sy:1:f*sy);
y2=zeros(size(x2));

[y3,z3]=meshgrid(-f*sy:1:f*sy);
x3=zeros(size(y3));

%%
%Visualize
hf=cFigure; hold on;

h4=gpatch(F,V,'rw','none',0.5);
surf(x1,y1,z1,'EdgeColor','k','faceColor',1*ones(1,3),'EdgeAlpha',0.5,'FaceAlpha',0.1,'LineWidth',lineWidth1);
surf(x2,y2,z2,'EdgeColor','k','faceColor',1*ones(1,3),'EdgeAlpha',0.5,'FaceAlpha',0.1,'LineWidth',lineWidth1);
surf(x3,y3,z3,'EdgeColor','k','faceColor',1*ones(1,3),'EdgeAlpha',0.5,'FaceAlpha',0.1,'LineWidth',lineWidth1);

h1=quiverVec([0 0 0],[1 0 0],sy,colorSet(1,:));
h2=quiverVec([0 0 0],[0 1 0],sy,colorSet(2,:));
h3=quiverVec([0 0 0],[0 0 1],sy,colorSet(3,:));

h5=plotV([-1 -1 -1; 1 1 1],'k--','LineWidth',lineWidth2); %Hydrostatic axis

h6=plotV(v12,'r-','LineWidth',lineWidth2);
h6.Color=(colorSet(1,:)+colorSet(2,:))/2;

h7=plotV(v13,'r-','LineWidth',lineWidth2);
h7.Color=(colorSet(1,:)+colorSet(3,:))/2;

h8=plotV(v23,'r-','LineWidth',lineWidth2);
h8.Color=(colorSet(2,:)+colorSet(3,:))/2;

hAxis=gca;
hAxis.XRuler.FirstCrossoverValue  = 0; % X crossover with Y axis
hAxis.YRuler.FirstCrossoverValue  = 0; % Y crossover with X axis
hAxis.ZRuler.FirstCrossoverValue  = 0; % Z crossover with X axis
hAxis.XRuler.SecondCrossoverValue = 0; % X crossover with Z axis
hAxis.YRuler.SecondCrossoverValue = 0; % Y crossover with Z axis
hAxis.ZRuler.SecondCrossoverValue = 0; % Z crossover with Y axis

text(0.5+f*sy,0,0,'$\sigma_1$','Interpreter','Latex','FontSize',fontSizeText);
text(0,0.5+f*sy,0,'$\sigma_2$','Interpreter','Latex','FontSize',fontSizeText);
text(0,0,0.5+f*sy,'$\sigma_3$','Interpreter','Latex','FontSize',fontSizeText);

xticks(-f*sy:0.5:f*sy);
yticks(-f*sy:0.5:f*sy);
zticks(-f*sy:0.5:f*sy);

legend([h1 h2 h3 h4 h5 h6 h7 h8],{'$\sigma_1$ axis','$\sigma_2$ axis','$\sigma_3$ axis',...
                           'Von Mises yield surface',...
                           'Hydrostatic line',...
                           'yield ellipse $\sigma_3=0$','yield ellipse $\sigma_2=0$','yield ellipse $\sigma_1=0$'...
                           },'Interpreter','Latex');

axis tight; axis equal; axis vis3d; view(3); %box on;
camlight headlight;
axis(f*sy*[-1 1 -1 1 -1 1])
set(gca,'FontSize',fontSizeAxis,'LineWidth',lineWidthAxis);
gdrawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


%s = rand('twister');
%rand('twister', s);

len = 200;              % number of 1st level observations (time points within subjects)
sub = 20;               % number of second-level units (subjects)

fixed_slope = 0.5;      % population fixed-effect slope
fixed_int = 0.3;        % pop. fixed-effect intercept

rand_slope = 0.5;         % 'true' random effect standard dev: slope
rand_int = 0.5;           % 'true' random effect standard dev: intercept

noise_sigma = 0.5;      % measurement error/unexplained std of 1st level observations
between_sigma = 0.5;    % measurement error/unexplained std of 2nd level covariate

% Generate simulated data
% 1) create predictors
x = zeros(len,sub);
y = x;
x(11:20,:) = 1;
x(111:120,:) = 1;

% 2) Create instances of effects for each subject
c = normrnd(fixed_slope, rand_slope, sub, 1);       % slope between-subjects variations
d = normrnd(fixed_int, rand_int, sub, 1);                  % intercept between-subjects variations

% 3) Create a between-ss covariate that is related to the slope but not the
% intercept
covt = scale(c + normrnd(0, between_sigma, sub, 1), 1);

% 4) Create a between-ss covariate that is related to the intercept but not the
% intercept
covti = scale(d + normrnd(0, between_sigma, sub, 1), 1);

corrcoef([c d covt covti])

% Add between-subjects error (random effects) and measurement noise
% (within-subjects error)

for i=1:sub
    y(:,i) = d(i) + c(i) .* x(:,i) + normrnd(0, noise_sigma, len, 1);
end

%% Run the model - without a 2nd-level covariate
out = igls(y, x);  % for igls
fprintf('\t\t%s\t%s\t%s\t\n', 'Effect', 'Intercept', 'Slope');
disp('True random-effect variances:');
fprintf('\t\t\t%3.3f\t%3.3f\n', [rand_slope rand_int]);

disp('Input random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', std([d c]))

disp('Est.  random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', sqrt(out.betastar)');

%% Run the model - with an unrelated 2nd-level covariate
out = igls(y, x, 'covariate', (1:20)');  % for igls
fprintf('\t\t%s\t%s\t%s\t\n', 'Effect', 'Intercept', 'Slope');
disp('True random-effect variances:');
fprintf('\t\t\t%3.3f\t%3.3f\n', [rand_slope rand_int]);

disp('Input random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', std([d c]))

disp('Est.  random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', sqrt(out.betastar)');

%% Run the model - with a 2nd-level covariate related to slope
out = igls(y, x, 'covariate', covt);  % for igls
fprintf('\t\t%s\t%s\t%s\t\n', 'Effect', 'Intercept', 'Slope');
disp('True random-effect variances:');
fprintf('\t\t\t%3.3f\t%3.3f\n', [rand_slope rand_int]);

disp('Input random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', std([d c]))

disp('Est.  random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', sqrt(out.betastar)');

%% Run the model - with a 2nd-level covariate related to intercept
out = igls(y, x, 'covariate', covti);  % for igls
fprintf('\t\t%s\t%s\t%s\t\n', 'Effect', 'Intercept', 'Slope');
disp('True random-effect variances:');
fprintf('\t\t\t%3.3f\t%3.3f\n', [rand_slope rand_int]);

disp('Input random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', std([d c]))

disp('Est.  random-effect variances: ');
fprintf('\t\t\t%3.3f\t%3.3f\n', sqrt(out.betastar)');

%%
clear *pvals* *fpr*

mytype = 'i';
iterations = 200;

len = 100; sub = 10;

fixpvals = zeros(iterations, 2);
randpvals = zeros(iterations, 2);
LRTrandpvals = zeros(iterations, 2);
chic = zeros(iterations, 1);
chid = zeros(iterations, 1);

c_fixed = 0;  % slope pop. average
d_fixed = 0;   % intercept pop. average
c_rand = 0;    % slope std across Ss
d_rand = 0;    % intercept std.
noise_std = 2.0;  % within-subjects noise std

x = zeros(len,sub);
y = x;
x(1:2:end,:) = 1;                   % create signal

% --------------------------------------------------------------------------
% This block: No true random effects, so test false positive rate for
% p_randvariance

verbstr = 'noverbose';

for i = 1:iterations

    c = normrnd(c_fixed,c_rand,sub,1);       % slope between-subjects variations
    d = normrnd(d_fixed,d_rand,sub,1);         % intercept between-subjects variations

    % Add between-subjects error (random effects) and measurement noise
    % (within-subjects error)
           y = zeros(len, sub);
    for s = 1:sub
        y(:,s) = d(s) + c(s) .* x(:,s) + normrnd(0,noise_std,len,1);
    end
    out = igls(y, x, 'type', mytype, verbstr);  % for igls

    fpr_converged(i) = out.isconverged;

    fpr_betastar(i, :) = out.betastar';

    fixpvals(i, :) = out.p;
    randpvals(i, :) = out.p_randvariance'; %[out.p_randvariance_d out.p_randvariance_c];

    LRTrandpvals(i, :) = out.pLRT_randvariance';

    if mod(i, 10) == 0, fprintf(1, '%3.0f ', i); end

    verbstr = 'noverbose';
end
fprintf('\n')

%pvals_fpr = pvals;
alph = .1;

fixfpr =  sum(fixpvals < alph) ./ iterations;
randfpr =  sum(randpvals < alph) ./ iterations;
LRTfpr =  sum(LRTrandpvals < alph) ./ iterations;

fprintf('\tfixed\t\trand fx\t\t\n');
fprintf('TPR/FPR @ alpha = %3.3f:\t%3.5f\t%3.5f\t%3.5f\t%3.5f\t%3.5f\t%3.5f\n', alph, fixfpr, randfpr, LRTfpr);


classdef OrthogonalLeastSquares < handle
    %CSOLSAPPROX Implements OLS algorithm for sparse approximation

    properties
        % Maximum residual norm
        MaxResNorm = 1e-4
        % Indicates if we should stop on exceeding residual norm
        StopOnResidualNorm = true
        % Indicates if we should stop when residual norm stops improving
        StopOnResNormStable = true
        % Maximum number of iterations for approximation
        MaxIters
        Verbose = false
        % Minimum Sparsity
        MinK = 4
        % Ignored atom (which won't be considered in identification step)
        IgnoredAtom = -1
    end

    properties(SetAccess=private)
        % The dictionary
        Dict
        % Ambient signal dimensions
        N
        % Number of atoms in dictionary
        D
        % Sparsity level of representations (may be negative)
        K
        % Result of a solver
        result
    end

    methods
        function self  = OrthogonalLeastSquares(Dict, K)
            % We assume that all the columns in dictionary are normalized.
            if isa(Dict, 'spx.dict.Operator')
                self.Dict = Dict;
            elseif ismatrix(Dict)
                self.Dict = spx.dict.MatrixOperator(Dict);
            else
                error('Unsupported operator.');
            end
            [self.N, self.D] = size(Dict);
            if ~exist('K', 'var')
                % No sparsity level has been pre-specified.
                K = -1;
            end
            self.K = K;
            % Maximum number of iterations
            maxIter = self.N;
            if K > 0
                % We have to consider pre-specified sparsity level
                maxIter = K;
            end
            self.MaxIters = maxIter;
            if K > 0 && self.MinK >= K
                self.MinK = 0;
            end
        end

        function result  = solve(self,y)
            % Uses QR factorization to implement the OLS

            % Initialization
            % Solves approximation problem using OLS
            d = self.D;
            n = self.N;
            r = y;
            min_k  = self.MinK;
            dict = self.Dict;
            % Active indices
            omega = [];
            if self.StopOnResNormStable
                oldResNorm = norm(r);
            end
            maxIter = self.MaxIters;
            % Create space for storing the Q R factors
            Q = zeros(n, maxIter);
            R = zeros(maxIter);
            % zr stores projections of y along the directions
            % in Q.
            % Since vectors in Q are orthonormal,
            % hence computing projections is as easy as
            % taking inner product.
            % This also simplifies the process of updating
            % the residual as the residual is orthogonal
            % to the vectors selected in Q so far.
            zr = [];

            ignored_atom = self.IgnoredAtom;
            % Convert the dictionary into a matrix
            dict = double(dict);

            for iter=1:maxIter
                % The Q part of previous iteration
                QLast = Q(:, 1:iter-1); % n * (k - 1)
                % The Orthogonal Projector
                OrthoProjector = eye(n) -  QLast * QLast';
                % Orthogonalize the remaining atoms of the dictionary
                qdict = OrthoProjector * dict;
                % Normalize the columns
                qdict = spx.norm.normalize_l2(qdict);
                % Compute inner products
                innerProducts = qdict' * r;
                % Mark the inner products of already selected columns as 0.
                innerProducts(omega) = 0;
                innerProducts = abs(innerProducts);
                if ignored_atom > 0
                    % forcefully ignore this atom
                    innerProducts(ignored_atom) = 0;
                end
                % Find the highest inner product
                [~, index] = max(innerProducts);
                % Add this index to support
                omega = [omega, index];
                % Pick the new atom from the dictionary
                new_atom = dict(:, index);
                % Orthogonalize the new atom
                %Compute projections to previously selected vectors in Q
                projections = QLast'* new_atom;
                % Remove the projection
                new_q = new_atom - QLast * projections;
                % Normalize
                norm_q = norm(new_q);
                new_q = new_q / norm_q;
                % Place it
                Q(:, iter) = new_q;
                % Update R
                R(1:iter-1, iter) = projections;
                R(iter, iter) = norm_q;
                % Compute the projection of y on new q.
                zr(iter) = new_q' * y;
                % Let us update the residual.
                r = r - zr(iter) * new_q;
                if self.StopOnResidualNorm || self.StopOnResNormStable
                    resNorm = norm(r);
                    if resNorm < self.MaxResNorm && iter > min_k
                        break;
                    end
                    if self.StopOnResNormStable
                        change = abs(oldResNorm  - resNorm);
                        if change/oldResNorm < .01  && iter > min_k
                            % No improvement
                            break;
                        end
                    end
                end
            end
            % Estimate
            z = zeros(d, 1);
            % We need to use back-substitution  with R to get z from zr.
            opts.UT = true;
            tmp = linsolve(R(1:iter,1:iter), zr(1:iter)', opts);
            z(omega)= tmp;
            % Solution vector
            result.z = z;
            % Residual obtained
            result.r = r;
            % residual norm
            result.rnorm = resNorm;
            % Number of iterations
            result.iterations = iter;
            % Solution support
            result.support = omega;
            self.result = result;
        end

    end

end


function fem1d_bvp_linear_test02 ( )

%*****************************************************************************80
%
%% FEM1D_BVP_LINEAR_TEST02 carries out test case #2.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/fem1d_bvp_linear/fem1d_bvp_linear_test02.m
%
%  Discussion:
%
%    Use A2, C2, F2, EXACT2, EXACT_UX2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dianne O'Leary,
%    Scientific Computing with Case Studies,
%    SIAM, 2008,
%    ISBN13: 978-0-898716-66-5,
%    LC: QA401.O44.
%
  n = 11;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_BVP_LINEAR_TEST02\n' );
  fprintf ( 1, '  Solve -( A(x) U''(x) )'' + C(x) U(x) = F(x)\n' );
  fprintf ( 1, '  for 0 < x < 1, with U(0) = U(1) = 0.\n' );
  fprintf ( 1, '  A2(X)  = 1.0\n' );
  fprintf ( 1, '  C2(X)  = 2.0\n' );
  fprintf ( 1, '  F2(X)  = X * ( 5 - X ) * exp ( X )\n' );
  fprintf ( 1, '  U2(X)  = X * ( 1 - X ) * exp ( X )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', n );
%
%  Geometry definitions.
%
  x_first = 0.0;
  x_last = 1.0;
  x = linspace ( x_first, x_last, n );
  x = x(:);

  u = fem1d_bvp_linear ( n, @a2, @c2, @f2, x );

  uexact = exact2 ( x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     I    X         U         Uexact    Error\n' );
  fprintf ( 1, '\n' );

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

  e1 = l1_error ( n, x, u, @exact2 );
  e2 = l2_error_linear ( n, x, u, @exact2 );
  h1s = h1s_error_linear ( n, x, u, @exact_ux2 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  l1 norm of error  = %g\n', e1 );
  fprintf ( 1, '  L2 norm of error  = %g\n', e2 );
  fprintf ( 1, '  Seminorm of error = %g\n', h1s  );

  return
end
function value = a2 ( x )

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

  return
end
function value = c2 ( x )

%*****************************************************************************80
%
%% C2 evaluates C function #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of C(X).
%
  value = 2.0;

  return
end
function value = exact2 ( x )

%*****************************************************************************80
%
%% EXACT2 evaluates exact solution #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of U(X).
%
  value = x .* ( 1.0 - x ) .* exp ( x );

  return
end
function value = exact_ux2 ( x )

%*****************************************************************************80
%
%% EXACT_UX2 evaluates the derivative of exact solution #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of dUdX(X).
%
  value = ( 1.0 - x - x .* x ) .* exp ( x );

  return
end
function value = f2 ( x )

%*****************************************************************************80
%
%% F2 evaluates right hand side function #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of F(X).
%
  value = x .* ( 5.0 - x ) .* exp ( x );

  return
end


function varargout = projPointOnPolyline(point, poly, varargin)
%PROJPOINTONPOLYLINE Compute position of a point projected on a polyline.
%
%   POS = projPointOnPolyline(POINT, POLYLINE)
%   Compute the position of the orthogonal projection of a point on a
%   polyline.
%   POINT is a 1-by-2 row vector containing point coordinates
%   POLYLINE is a N-by-2 array containing coordinates of polyline vertices
%   POS is the position of the point on the polyline, between 0 and the
%   number of vertices of the polyline. POS can be a non-integer value, in
%   this case, the integer part corresponds to the polyline edge index
%   (between 0 and Nv-1), and the floating-point part corresponds to the
%   relative position on i-th edge (between 0 and 1, 0: edge start, 1: edge
%   end).
%
%   When POINT is an array of points, returns a column vector with as many
%   rows as the number of points.
%
%   POS = projPointOnPolyline(POINT, POLYLINE, CLOSED)
%   Specifies if the polyline is closed or not. CLOSED can be one of:
%     'closed' -> the polyline is closed
%     'open' -> the polyline is open
%     a column vector of logical with the same number of elements as the
%       number of points -> specify individually if each polyline is
%       closed (true=closed).
%
%   [POS, DIST] = projPointOnPolyline(...)
%   Also returns the distance between POINT and POLYLINE.
%
%   Example
%     poly = [10 10; 20 10;20 20;10 20];
%     projPointOnPolyline([15 0], poly)
%     ans =
%         0.5000
%     projPointOnPolyline([0 16], poly)
%     ans =
%         3.0000
%
%   See also
%   points2d, polygons2d, polylinePoint, projPointOnPolygon
%   distancePointPolyline
%

% ------
% Author: David Legland
% E-mail: david.legland@nantes.inra.fr
% Created: 2009-04-30, using Matlab 7.7.0.471 (R2008b)
% Copyright 2009-2022 INRA - Cepia Software Platform

% check if input polyline is closed or not
closed = false;
if ~isempty(varargin)
    var = varargin{1};
    if strcmp('closed', var)
        closed = true;
    elseif strcmp('open', var)
        closed = false;
    elseif islogical(var)
        closed = var;
    end
end

% closes the polyline if necessary
if closed
    poly = [poly ; poly(1,:)];
end

% number of points
Np = size(point, 1);

% allocate memory results
pos     = zeros(Np, 1);
minDist = inf*ones(Np, 1);

% iterate on points
for p = 1:Np
    % build set of edges
    edges = [poly(1:end-1, :) poly(2:end, :)];

    % compute distance between current point and all edges
    [dist, edgePos] = distancePointEdge(point(p, :), edges);

    % update distance and position if necessary
    [minDist(p), edgeIndex] = min(dist);
    pos(p) = edgeIndex - 1 + edgePos(edgeIndex);
end

% process output arguments
if nargout <= 1
    varargout{1} = pos;
elseif nargout == 2
    varargout{1} = pos;
    varargout{2} = minDist;
end


%GIKINE Shoulder inverse kinematics of HAL-like right shoulder
%
% Computes the inverse kinematics of the right shoulder which is the
% same kinematically as a HAL object. This function is mainly useful
% for procedures which require many, many calls, so time can be saved
% by not referencing a HAL object.
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% Syntax:
%  (1) [q1, q2] = gikine(Tg, Tu)
%
% Outputs:
%  q1 : First family of solutions (mx3 matrix where m = size(Tu,3))
%  q2 : Second family of solutions (mx3 matrix where m = size(Tu,3))
%
% Inputs:
%  Tg : Transformation matrix of the shoulder frame. x, y, z point to
%        the right, above, and behind the person. Translation is
%        shoulder center of rotation
%  Tu : Transformation matrix of the upper arm frame. May be a 4x4xm
%        series of frames (or higher order, which is compressed to 3D)
%
% See also HAL HAL.gikine HAL.ikine wikine

% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
%% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.
%
% RTB LIBRARY:
%
% Copyright (C) 199q3-2014, by Peter I. Corke
% http://www.petercorke.com
% Released under the GNU Lesser General Public license,
% Modified 16/7/2014 (HAL is a subclass of SerialLink)

function [q1, q2] = gikine(Tg, Tu)

Rg = Tg(1:3,1:3);
gRu0 = [0 0 -1; 0 1 0; 1 0 0]';
u0R = (Rg*gRu0)';

u0Ru = reshape(u0R*reshape(Tu(1:3,1:3,:),3,[]),3,3,[]);

%-X,Z,Y Tait-Bryan angles

% Family 1:
q1(:,1) = squeeze(atan2(-u0Ru(3,2,:),u0Ru(2,2,:)));
q1(:,2) = squeeze(asin(-u0Ru(1,2,:)));
q1(:,3) = squeeze(atan2(-u0Ru(1,3,:),u0Ru(1,1,:)));

% Make -pi values pi so that they are deemed within joint limits
fix = q1 <= (-pi+1e-4);
q1(fix) = pi;

% Family 2:
q2(:,1) = squeeze(atan2(u0Ru(3,2,:),-u0Ru(2,2,:)));
q2(:,2) = pi-q1(:,2);
q2(:,3) = squeeze(atan2(u0Ru(1,3,:),-u0Ru(1,1,:)));
end


function [azimuth, dist]=indirectRhumbProblem(latLonStart,latLonEnd,height,useHeightApprox,a,f,numSteps4Circ)
%INDIRECTRHUMBPROBLEM Given a starting and ending latitude and longitude on
%                     a reference ellipsoid, determine the heading (in
%                     radians East of North) and the distance one must
%                     travel on the shortest constant-heading course to go
%                     from the starting point to the stopping point. A
%                     constant heading course follows a rhumb line
%                     (loxodrome) and is usually not the shortest path
%                     between two points.
%
%INPUTS: latLonStart A 2X1 vector of the starting ellipsoidal latitude
%                    (North) and longitude (East) in radians. This cannot
%                    be a pole.
%          latLonEnd A 2X1 vector of the ending ellipsoidal latitude
%                    (North) and longitude (East) in radians.
%             height The height above the reference ellipsoid at which the
%                    trajectory should be determined. This changes the
%                    distance traveled, but not the azimuthal angle of
%                    departure. If this parameter is omitted, then the
%                    default value of 0 is used.
%    useHeightApprox If true, and the height is not zero, then an
%                    approximation is made for how dist scales with
%                    altitude. Specifically, an equiatorial trajectory will
%                    scale as (a+height)/a. Thus, this scaling factor is
%                    applied to any trajectory to scale dist with altitude.
%                    If height is false, a significantly slower iterative
%                    optimization technique is used. The default value is
%                    true. The difference made when useHeightApprox=false is
%                    can generally be assumed to be less than 80m. This
%                    parameter is ignored if height=0.
%                  a The semi-major axis of the reference ellipsoid. If
%                    this argument is omitted, the value in
%                    Constants.WGS84SemiMajorAxis is used.
%                  f The flattening factor of the reference ellipsoid. If
%                    this argument is omitted, the value in
%                    Constants.WGS84Flattening is used.
%      numSteps4Circ If height!=0 then an algorithm propagating a state
%                    in ECEF coordinates around the curved Earth is used to
%                    solve the direct geodetic problem as a step in solving
%                    the indirect geodetic problem. This parameter
%                    determines the number of steps that would be needed in
%                    the direct geodetic problem for a target that
%                    circumnavigates the globe around the equator. The
%                    default value if this parameter is not provided is
%                    2000. A value of 6000 appears to be about the best
%                    number for overall precision. Reducing the number of
%                    steps will speed up the function. This parameter is not
%                    used if height=0.
%
%OUTPUTS: azimuth The constant heading in radians East of North that one
%                 must travel to go on a constant-heading course from
%                 latLonStart to latLonEnd.
%            dist The distance that one must travel on a constant-heading
%                 course to go from latLonStart to latLonEnd.
%
%If height=0, the algorithm is mostly taken from [1]. However, a formula
%using isometric latitudes, which are described in Chapter 3 of [2] to get
%the azimuth angle was used, because it is simpler. The formula is also
%explicitly mentioned in Equation 3 of [3]. However, the expression for
%computing the distance from that paper is only for a sphere, not for an
%ellipsoid, which is why the Carlton-Wippern distance computation using an
%incomplete elliptic integral of the second kind is preferred.
%
%When the azimuth found by the technique is very close to +/-pi/2 (when one
%is traveling at nearly a constant latitude), the distance computation
%switches to assume that the latitude is indeed constant even if
%latLonStart(1)!=latLonStart(2). This avoid precision problems that arise
%as a very small number is multiplied by a very large number. However, this
%reduces the accuracy of the method.
%
%Generally, calling directRhumbProblem or directRhumbProbGen with the
%azimuth and dist returned by this function will return latLonStart.
%However, if the stopping point is at a pole, then directRhumbProblem will
%correctly return a polar location, but the longitude will generally be
%wrong.
%
%When height!=0, the algorithm can be significantly slower if no
%approximation is used. Around the equator, the distance scales as
%(a+height)/a*dist as one changes the ellipsoidal height. This scaling
%applied to dist in  non-equatorial trajectories is the approximation if
%useHeightApprox=true. When useHeightApprox=false, the approximate value
%is used to determine the bounds around which the fminbnd function
%searches. The maximum error in the approximation is expected to be less
%than 80m. The search region for the value of dist used in the fminbnd
%function was set to 0.9*dist to 1.1*dist, where dist is the distance
%obtained after scaling the distance from the zero-altitude solution.
%
%EXAMPLE:
%A trajectory that crosses the international date line and and goes from
%the Northern hemisphere to the southern hemisphere. We also compute the
%reverse path and show that the azimuth angles in each direction are
%consistent with each other. We then plot the trajectory on an image of the
%spherical Earth.
% N=100;
% latStart=degMinSec2Rad(37,47.5);
% lonStart=degMinSec2Rad(-122,-27.8);
% latEnd=degMinSec2Rad(-33,-51.7);
% lonEnd=degMinSec2Rad(151,12.7);
%
% latLonStart=[latStart;lonStart];
% latLonEnd=[latEnd;lonEnd];
% [azimuth,dist]=indirectRhumbProblem(latLonStart,latLonEnd);
% [azEnd,distRev]=indirectRhumbProblem(latLonEnd,latLonStart);
%
% %If the forward and reverse estimates agree, then these values will
% %ideally be zero.
% azimuth-(-azEnd)
% dist-distRev
%
% distVals=linspace(0,dist,N);
% latLonWayPoints=directRhumbProblem(latLonStart,azimuth,distVals);
%
% %Show that the approximate direct algorithm reaches nearly the same
% %endpoint as the indirect algorithm.
% xEndWay=ellips2Cart([latLonWayPoints(:,end);0]);
% xEnd=ellips2Cart([latLonEnd;0]);
% max(abs(xEndWay-xEnd))
% max(abs(wrapRange(latLonWayPoints(:,end)-latLonEnd,-pi,pi)))
%
% xStartCart=ellips2Cart([latLonStart;0]);
% xEndCart=ellips2Cart([latLonEnd;0]);
% %The path is displayed slightly above the Earth's surface to make it
% %easier to see.
% pathPoints=ellips2Cart([latLonWayPoints;0.02*ones(1,N)]);
%
% figure(1)
% clf
% hold on
% plotMapOnEllipsoid([]);
% scatter3(xStartCart(1),xStartCart(2),xStartCart(3),100,'filled')
% scatter3(xEndCart(1),xEndCart(2),xEndCart(3),100,'filled')
% plot3(pathPoints(1,:),pathPoints(2,:),pathPoints(3,:),'-r','linewidth',4)
%
%REFERENCES:
%[1] K. C. Carlton-Wippern, "On loxodromic navigation," Journal of
%    Navigation, vol. 45, no. 2, pp. 292-297, May 1992.
%[2] J. P. Snyder, "Map projections- a working manual," U.S. Geological
%    Survey, Tech. Rep. 1395, 1987.
%[3] J. Alexander, "Loxodromes: A rhumb way to go," Mathematics Magazine,
%    vol. 77, no. 5, pp. 349-356, Dec. 2004.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<7||isempty(numSteps4Circ))
   numSteps4Circ=2000;
end

if(nargin<6||isempty(f))
    f=Constants.WGS84Flattening;
end

if(nargin<5||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

if(nargin<4||isempty(useHeightApprox))
   useHeightApprox=true;
end

if(nargin<3||isempty(height))
   height=0;
end

%Extract the components
latStart=latLonStart(1);
lonStart=latLonStart(2);
latEnd=latLonEnd(1);
lonEnd=latLonEnd(2);

%The first numerical eccentricity of the ellipsoid.
e=sqrt(2*f-f^2);

%Convert the ellipsoidal latitudes to reduced co-latitudes. A co-latitude
%is pi/2 minus the latitude. Also,
nu1=pi/2-ellipsLat2ReducedLat(latStart,f);
nu2=pi/2-ellipsLat2ReducedLat(latEnd,f);

%Though not mentioned in the above papers, the difference in the longitudes
%must be wrapped to the range of -pi/pi or else one will get useless
%results when crossing the -pi/pi boundary.
num=wrapRange(lonEnd-lonStart,-pi,pi,false);
%Equation 11 in the paper provides an expression to get the azimuth.
%however, it is simpler if one just uses isometric latitudes.
val1=ellipsLat2IsoLat(latStart,f);
val2=ellipsLat2IsoLat(latEnd,f);

if(~isfinite(val1))
    warning('The starting point is located at a geographic pole. Azimuth values will be inaccurate.')
end

if(isfinite(val1)||isfinite(val2))
    %This will properly return 0 and pi for infinite values of val1 when
    %val2 is finite and vice versa, which corresponds to headings to or
    %from a pole.
    denom=val2-val1;
    azimuth=atan2(num,denom);
else%If neither is finite, then that is the case where one is going from
    %pole to pole. In such an instance, just set the heading to 0, if going
    %North, and pi, if going South.

    if(latStart>latEnd)
        azimuth=pi;
    else
        azimuth=0;
    end
end

%The distance
if(abs(abs(azimuth)-pi/2)>2e-8)
    %Equation 12 in the paper.
    dist=a*abs(sec(azimuth))*abs(ellipIntInc2Kind(nu2,e^2)-ellipIntInc2Kind(nu1,e^2));
else
    %Equation 14b in the paper.
    dist=a*abs(sin(nu1))*abs(wrapRange(lonEnd-lonStart,-pi,pi));
end

%If a non-zero height is given, then iterate over the direct rhumb
%problem at height to determine the solution.
if(height~=0)
    endCart=ellips2Cart([latLonEnd;height],a,f);

    %The approximate scaling for the height.
    dist=((a+height)/a)*dist;

    %If a computationally-intensive but more precise algorithm to search
    %for the true distance at altitude should be used instead of a
    %simple approximation of scaling the distance.
    if(useHeightApprox==false)
        %Assume that the correct height-adjusted distance is within 10% of the
        %scaled distance value.
        distFun=@(distCur)distCostFunc(distCur,endCart,latLonStart,azimuth,height,a,f,numSteps4Circ);
        dist=fminbnd(distFun,0.9*dist,1.1*dist);
    end
end
end

function cost=distCostFunc(distCur,endCart,latLonStart,azStart,height,a,f,numSteps4Circ)
    latLonCalc=directRhumbProbGen(latLonStart,azStart,distCur,height,false,a,f,numSteps4Circ);
    cost=norm(ellips2Cart([latLonCalc;height],a,f)-endCart);
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.


% Fast Inter-Harmonic Reconstruction as a pre-process for LPC-based
% spectral envelope estimation.
%
%
% Description
%  This technique reconstructs inter-harmonics of the voice signal,
% as a pre-process for an efficient spectral envelope estimation in
% high-pitched voices. The technique is fully described in [1].

%
% Inputs
%  wave             : [samples] [Nx1] input signal (speech signal)
%  Fs               : [Hz]      [1x1] sampling frequency
%  f0               : F0 estimate (a 0 value is for an unvoiced frame)
%  order            : Order used for the LP analysis
%
%
% Outputs
%  LP               : LP coefficients
%  Energy           : energy of the frame
%
%
% Example
%  Please see the HOWTO_envelope.m example file.
%  Please see http://tcts.fpms.ac.be/~drugman/Toolbox/ for more details.
%
% References
%  [1] T.Drugman, Y. Stylianou, "Fast Inter-Harmonic Reconstruction for
%      Spectral Envelope Estimation in High-Pitched Voices", vol. 21, pp.
%      1418-1422, IEEE Signal Processing Letters, 2014.
%      http://tcts.fpms.ac.be/~drugman/files/SPL-FIHR.pdf
%
% Copyright (c) 2014 Toshiba Cambridge Research Laboratory
%
% License
%  This code will be part of the GLOAT toolbox with the following
%  licence:
%  This program is free software: you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation, either version 3 of the License, or
%  (at your option) any later version.
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
% This function will also be part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Thomas Drugman thomas.drugman@umons.ac.be

function [LP,Energy] = env_fihr(Seg,Fs,F0_local,order)

F0_target=100;
% F0* in the paper. Here set to 100 Hz.

Energy=sum(Seg.^2);

if F0_local>0

    TmpSignal=ones(1,length(Seg));
    t_tmp=1:length(TmpSignal);

    I=ceil(F0_local/F0_target);
    for k=1:I-1
        W=(I-k)/I;
        % We use here a linear weighting function. Other functions are
        % possible, as long as they meet the properties mentioned in
        % the paper.
        TmpSignal=TmpSignal+2*W*cos(2*pi*(k/I)*F0_local/Fs*t_tmp);

        % Equation (2) in the paper
    end

    Seg2=Seg.*TmpSignal';
else
    Seg2=Seg;
end

LP=lpc(Seg2,order);


function [ a_lu, info ] = r8pbu_fa ( n, mu, a )

%*****************************************************************************80
%
%% R8PBU_FA factors a R8PBU matrix.
%
%  Discussion:
%
%    The R8PBU storage format is for a symmetric positive definite band matrix.
%
%    To save storage, only the diagonal and upper triangle of A is stored,
%    in a compact diagonal format that preserves columns.
%
%    The diagonal is stored in row MU+1 of the array.
%    The first superdiagonal in row MU, columns 2 through N.
%    The second superdiagonal in row MU-1, columns 3 through N.
%    The MU-th superdiagonal in row 1, columns MU+1 through N.
%
%    The matrix A must be a positive definite symmetric band matrix.
%
%    Once factored, linear systems A*x=b involving the matrix can be solved
%    by calling R8PBU_SL.  No pivoting is performed.  Pivoting is not necessary
%    for positive definite symmetric matrices.  If the matrix is not positive
%    definite, the algorithm may behave correctly, but it is also possible
%    that an illegal divide by zero will occur.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 1998
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Bunch, Moler, Stewart,
%    LINPACK User's Guide,
%    SIAM, Philadelphia, 1979.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer MU, the number of superdiagonals of the matrix.
%    MU must be at least 0, and no more than N-1.
%
%    Input, real A(MU+1,N), the N by N matrix, stored in LINPACK
%    positive definite symmetric band matrix storage.
%
%    Output, real A_LU(MU+1,N), information describing a factored form
%    of the matrix, that can be used to solve linear systems
%    A*x=b, using R8PBU_SL.
%
%    Output, integer INFO, singularity flag.
%    0, the matrix is nonsingular.
%    nonzero, the matrix is singular.
%
  info = 0;
  a_lu(1:mu+1,1:n) = a(1:mu+1,1:n);

  for j = 1 : n

    ik = mu + 1;
    jk = max ( j - mu, 1 );
    mm = max ( mu + 2 - j, 1 );

    s = 0.0;

    for k = mm : mu

      a_lu(k,j) = ( a_lu(k,j) - a_lu(ik:ik+k-mm-1,jk)' * a_lu(mm:k-1,j) )...
        / a_lu(mu+1,jk);

      s = s + a_lu(k,j) * a_lu(k,j);

      ik = ik - 1;
      jk = jk + 1;

    end

    s = a_lu(mu+1,j) - s;

    if ( s <= 0.0 )
      info = j;
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8PBU_FA - Fatal error!\n' );
      fprintf ( 1, '  Nonpositive pivot on step %d\n', info );
      return;
    end

    a_lu(mu+1,j) = sqrt ( s );

  end

  return
end


function model = ml_trainrvm(varargin)
% Learn a probabilistic (non-)linear model, via the Relevance Vector Machine.
% Model = ml_trainrvm(Trials, Targets, Options...)
%
% The Relevance Vector Machine [1] is a Bayesian equivalent to the popular Support Vector Machines
% (SVMs) [2]. RVMs can be used for general-purpose linear or non-linear (kernelized) classification
% or regression, and produce state-of-the-art results in most cases. Various kernels are supplied,
% where the rbf kernel is usually the best initial choice. In the non-linear case, the kernel
% scaling parameter should be found via parameter search, which can be time-consuming. In contrast
% to SVMs, Relevance Vector Machines give probablistic outputs, which can be practical when multiple
% uncertain predictions are to be fused, etc. In the future, an implementation of RVMs using convex
% optimization will be provided, which is assumed to give better optimality guarantees than the
% current implementation [3,4,5].
%
% RVMs (as well as kernel SVMs) are the most versatile general-purpose classifiers currently
% available in the toolbox, and are a good default choice if very little is known about the data.
% For best results, care must be taken to search over the respective regularization parameter(s)
% appropriately. If more is known about the structure of the data (e.g. that it should be linearly
% separable, or that that sparsity can be exploited across features), specialized methods may be
% more appropriate (and give similar results faster or reach better performance by overfitting less
% strongly).
%
% In:
%   Trials       : training data, as in ml_train
%
%   Targets      : target variable, as in ml_train
%
%   Options  : optional name-value parameters to control the training details:
%              'ptype': problem type: 'classification' (default) or 'regression'
%
%              'kernel': one of several kernel types:
%                         * 'linear':   Linear
%                         * 'rbf':      Gaussian / radial basis functions (default)
%                         * 'laplace':  Laplacian
%                         * 'poly':		Polynomial
%                         * 'cauchy':	Cauchy
%
%              'gamma': scaling parameter of the kernel (for regularization); if multiple values
%                       are given, the optimal gamma will be searched via evidence maximization
%                       default: 2.^(-16:0.2:10)
%
%              'degree': degree of the polynomial kernel, if used (default: 3)
%
%              'bias': whether to add a bias to the data (default: 1)
%
%              misc options:
%              'iterations': Number of interations to run for
%              'time':       time limit to run for, e.g. '1.5 hours', '30 minutes', '1 second'
%              'fixednoise': whether the gaussian noise is to be fixed 0/1
%              'beta':       (Gaussian) noise precision (inverse variance)
%              'noisestd':   (Gaussian) noise standard deviation
%              'scaling':    pre-scaling of the data (see hlp_findscaling for options) (default: 'std')
%              'diagnosticlevel':   verbosity level, 0-4
%
% Out:
%   Model   : the computed model...
%             classes indicates the class labels which the model predicts
%             additional parameters determine a posterior distribution over the weights
%
% Examples:
%   % learn a standard Relevance Vector Machine classifier
%   model = ml_trainrvm(trials,targets)
%
%   % as before, but this time use a regression approach
%   model = ml_trainrvm(trials,targets,'ptype','regression')
%
%   % use a Laplacian kernel
%   model = ml_trainrvm(trials,targets,'kernel','laplace')
%
%   % find the optimal kernel scale using parameter search
%   model = utl_searchmodel({trials,targets},'args',{{'rvm','gamma',seach(2.^(-16:2:4)))
%
%
% See also:
%   ml_predictrvm, SparseBayes
%
% References:
%  [1] Vladimir Vapnik. "The Nature of Statistical Learning Theory."
%      Springer-Verlag, 1995
%  [2] Michael E. Tipping and Alex Smola, "Sparse Bayesian Learning and the Relevance Vector Machine".
%      Journal of Machine Learning Research 1: 211?244. (2001)
%  [3] Michael E. Tipping and A. C. Faul. "Fast marginal likelihood maximisation for sparse Bayesian models."
%      In C. M. Bishop and B. J. Frey (Eds.), Proceedings of the Ninth International Workshop on Artificial Intelligence and Statistics, Key West, FL, Jan 3-6 (2003)
%  [4] David P. Wipf and Srikantan Nagarajan, "A New View of Automatic Relevance Determination,"
%      In J.C. Platt, D. Koller, Y. Singer, and S. Roweis, editors, Advances in Neural Information Processing Systems 20, MIT Press, 2008.
%  [5] David P. Wipf and Srikantan Nagarajan, "Sparse Estimation Using General Likelihoods and Non-Factorial Priors,"
%      In Advances in Neural Information Processing Systems 22, 2009.
%
%                           Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                           2010-04-06
dp;

opts = arg_define([0 2],varargin, ...
    arg_norep('trials'), ...
    arg_norep('targets'), ...
    arg({'ptype','Type'}, 'classification', {'classification','regression'}, 'Type of problem to solve.','cat','Core Parameters'), ...
    arg({'kernel','Kernel'}, 'rbf', {'linear','rbf','laplace','poly','cauchy'}, 'Kernel type. Linear, or Non-linear kernel types: Radial Basis Functions (general-purpose), Laplace (sparse), Polynomial (rarely preferred), and Cauchy (slightly experimental).','cat','Core Parameters'), ...
    arg({'gammap','KernelScale','gamma'}, 2.^(-16:0.5:10), [0 2^-20 2^10 Inf], 'Scaling of the kernel functions. Should match the size of structures in the data. A reasonable range is 2.^(-16:2:4).','cat','Core Parameters','shape','row'), ...
    arg({'polydegree','PolyDegree','degree'}, 3, uint32([1 100]), 'Degree of the polynomial kernel, if chosen.','cat','Core Parameters'), ...
    arg({'bias','Bias'}, true, [], 'Include a bias term in the model.','cat','Core Parameters'), ...
    arg({'scaling','Scaling'}, 'std', {'none','center','std','minmax','whiten'}, 'Pre-scaling of the data. For the regulariation to work best, the features should either be naturally scaled well, or be artificially scaled.','cat','Core Parameters'), ...
    ...
    arg({'iterations','MaxIterations'}, 100, uint32([1 100000]), 'Number of iterations to run.','cat','Miscellaneous'), ...
    arg({'time','MaxTime'}, '1000 seconds', [], 'Maximum time to run. Can use ''seconds'', ''minutes'', ''hours'' in the string.','cat','Miscellaneous'), ...
    arg({'fixednoise','NoiseFixed'}, false, [], 'Keep the Gaussian noise estimate fixed.','cat','Miscellaneous'), ...
    arg({'noiseinvvar','NoiseInvVariance','beta'}, [], [], 'Inverse variance of the Gaussian noise term.','cat','Miscellaneous','shape','scalar'), ...
    arg({'noisestd','NoiseVariance'}, [], [], 'Variance of the Gaussian noise term.','cat','Miscellaneous','shape','scalar'), ...
    arg({'diagnosticlevel','Verbosity'}, 'none', {'none','minimal','low','medium','high','ultra'}, 'Verbosity level.','cat','Miscellaneous'),...
    arg({'votingScheme','VotingScheme'},'1vR',{'1v1','1vR'},'Voting scheme. If multi-class classification is used, this determine how binary classifiers are arranged to solve the multi-class problem. 1v1 gets slow for large numbers of classes (as all pairs are tested), but can be more accurate than 1vR.'), ...
    arg({'monitor','DisplayInterval'}, uint32(0), [], 'Iterations between diagnostic outputs.','cat','Miscellaneous'));

arg_toworkspace(opts);

if is_search(gammap)
    gammap = 0.3; end

% pre-process arguments
ptype = hlp_rewrite(ptype,'classification','c','regression','r'); %#ok<*NODEF>
likelihood = hlp_rewrite(ptype,'c','bernoulli','r','gaussian');
args1 = [hlp_struct2varargin(opts,'restrict',{'iterations','time','monitor','fixednoise','freebasis','callback','callbackdata'}),{'diagnosticlevel',hlp_rewrite(opts.diagnosticlevel,'minimal','none')}];
args2 = hlp_struct2varargin(opts,'restrict',{'beta','noisestd','relevant','weights','alpha'},'rewrite',{'beta','noiseinvvar'});

% remap targets for classification
if strcmp(ptype,'c')
    classes = unique(targets);
    if length(classes) > 2
        % multiclass case: use the voter
        model = ml_trainvote(trials,targets,votingScheme,@ml_trainrvm,@ml_predictrvm,varargin{:});
        return;
    elseif length(classes) == 1
        error('BCILAB:only_one_class','Your training data set has no trials for one of your classes; you need at least two classes to train a classifier.\n\nThe most likely reasons are that one of your target markers does not occur in the data, or that all your trials of a particular class are concentrated in a single short segment of your data (10 or 20 percent). The latter would be a problem with the experiment design.');
    end
    % remap target labels to 0/1
    targets(targets==classes(1)) = 0;
    targets(targets==classes(2)) = 1;
else
    classes = [];
end

% prescale the data
sc_info = hlp_findscaling(trials,scaling);
trials = hlp_applyscaling(trials,sc_info);
basis = trials;

if length(gammap)>1
    if ~strcmp(opts.diagnosticlevel,'none')
        disp('Now optimizing gamma parameter using evidence maximization...'); end
    % optimize gamma parameter using marginal log-likelihood
    bestgam = NaN;      % best gamma parameter so far
    likelihoods = nan(length(gammap),1);
    bestlike = -Inf;    % best likelihood so far
    for k=1:length(gammap)
        gam = gammap(k);
        if ~strcmp(opts.diagnosticlevel,'none')
            fprintf('gamma=%.3f\n',gam); end
        % kernelize the data and add bias
        ktrials = utl_kernelize(trials,basis,kernel,gam,polydegree);
        ktrials = quickif(bias,[ones(size(ktrials,1),1) ktrials],ktrials);
        % run the RVM
        [param, hyperparam, diag] = hlp_diskcache('predictivemodels',@SparseBayes,likelihood,ktrials,targets,SB2_UserOptions(args1{:}),SB2_ParameterSettings(args2{:})); %#ok<ASGLU>
        if ~isempty(diag.Likelihood)
            likelihoods(k) = diag.Likelihood(end);
            if diag.Likelihood(end) > bestlike
                bestlike = diag.Likelihood(end);
                bestgam = gam;
            end
        end
    end
    gammap = bestgam;
else
    likelihoods = [];
end

% kernelize the data and add bias
ktrials = utl_kernelize(trials,basis,kernel,gammap,polydegree);
ktrials = quickif(bias,[ones(size(ktrials,1),1) ktrials],ktrials);
% run the RVM
[param, hyperparam, diag] = hlp_diskcache('predictivemodels',@SparseBayes,likelihood,ktrials,targets,SB2_UserOptions(args1{:}),SB2_ParameterSettings(args2{:}));

% preselect relevant basis vectors
if ~strcmp(kernel,'linear')
    if bias
        feature_sel = param.Relevant-1;
        if feature_sel(1) == 0
            % bias was relevant, remove it from the basis vector selection (it will be added after kernelization)
            feature_sel = feature_sel(2:end);
        else
            % bias was not relevant, forget about it
            bias = 0;
        end
    else
        feature_sel = param.Relevant;
    end
    basis = basis(feature_sel,:);
else
    feature_sel = param.Relevant;
end

model = struct('sc_info',{sc_info},'classes',{classes},'basis',{basis},'param',{param},'hyperparam',{hyperparam},...
               'diag',{diag},'ptype',{ptype},'feature_sel',{feature_sel},'bias',{bias}, ...
               'kernel',{kernel},'gamma',{gammap},'gamma_likelihoods',likelihoods,'degree',{polydegree});


function [U,sm,X,V,W] = cgsvd(A,L)
%CGSVD Compact generalized SVD of a matrix pair in regularization problems.
%
% sm = cgsvd(A,L)
% [U,sm,X,V] = cgsvd(A,L) ,  sm = [sigma,mu]
% [U,sm,X,V,W] = cgsvd(A,L) ,  sm = [sigma,mu]
%
% Computes the generalized SVD of the matrix pair (A,L). The dimensions of
% A and L must be such that [A;L] does not have fewer rows than columns.
%
% If m >= n >= p then the GSVD has the form:
%    [ A ] = [ U  0 ]*[ diag(sigma)      0    ]*inv(X)
%    [ L ]   [ 0  V ] [      0       eye(n-p) ]
%                     [  diag(mu)        0    ]
% where
%    U  is  m-by-n ,    sigma  is  p-by-1
%    V  is  p-by-p ,    mu     is  p-by-1
%    X  is  n-by-n .
%
% Otherwise the GSVD has a more complicated form (see manual for details).
%
% A possible fifth output argument returns W = inv(X).

% Reference: C. F. Van Loan, "Computing the CS and the generalized
% singular value decomposition", Numer. Math. 46 (1985), 479-491.

% Per Christian Hansen, IMM, March 17, 2008.

% Initialization.
[m,n] = size(A); [p,n1] = size(L);
if (n1 ~= n)
  error('No. columns in A and L must be the same')
end
if (m+p < n)
  error('Dimensions must satisfy m+p >= n')
end

% Call Matlab's GSVD routine.
[U,V,W,C,S] = gsvd(full(A),full(L),0);

if (m >= n)
  % The overdetermined or square case.
  sm = [diag(C(1:p,1:p)),diag(S(1:p,1:p))];
  if (nargout < 2)
    U = sm;
  else
    % Full decomposition.
    X = inv(W');
  end
else
  % The underdetermined case.
  sm = [diag(C(1:m+p-n,n-m+1:p)),diag(S(n-m+1:p,n-m+1:p))];
  if (nargout < 2)
    U = sm;
  else
    % Full decomposition.
    X = inv(W');
    X = X(:,n-m+1:n);
  end
end

if (nargout==5), W = W'; end


function [img,nrg] = rayCubeAnalytic(L,mat,air,Xsrc,Xmic,Rmax)
%+========================================================================+
%|                                                                        |
%|           OPENRAY - LIBRARY FOR TRI-DIMENSIONAL RAY TRACING            |
%|           openRay is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : rayCubeAnalytic.m                             |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Analytical source image method for a cube     |
%|  `---'  |                                                              |
%+========================================================================+

% Indices
i = -30:30;
a = i + 0.5 - 0.5*(-1).^i;
b = (-1).^i;

% Position relatives des sources images
x       = b*Xsrc(1) + a*L(1) - Xmic(1);
y       = b*Xsrc(2) + a*L(2) - Xmic(2);
z       = b*Xsrc(3) + a*L(3) - Xmic(3);
[x,y,z] = meshgrid(x,y,z);
img     = [x(:),y(:),z(:)];

% Dissipation de l'energie par propagation spherique
dst = sqrt(sum(img.^2,2));
nrg = 1./(dst.^2);

% Dissipation de l'energie par les parois
nrg   = nrg * ones(1,size(mat,2));
rfl   = (1-mat);
i0    = abs(0.5*i - 0.25 + 0.25*(-1).^i);
i1    = abs(0.5*i + 0.25 - 0.25*(-1).^i);
for j = 1:size(nrg,2)
    [rx,ry,rz] = meshgrid(...
        (rfl(1,j).^i0).*(rfl(2,j).^i1),...
        (rfl(3,j).^i0).*(rfl(4,j).^i1),...
        (rfl(5,j).^i0).*(rfl(6,j).^i1));
    nrg(:,j) = (rx(:).*ry(:).*rz(:)) .* nrg(:,j);
end

% Energie dissipee par l'air en fonction de la distance
dst = sqrt(sum(img.^2,2));
nrg = nrg .* exp(-dst*air);

% Selections des images selon l'ordre initial
ind = find(dst<Rmax);
dst = dst(ind);
img = img(ind,:);
nrg = nrg(ind,:);

% Sort in phase
[~,ind] = sort(dst);
img       = img(ind,:);
nrg       = nrg(ind,:);

% Normalisation de l'energie
nrg = nrg./max(max(nrg));

end