function [ mK ] = CreateConvMtx1D( vK, numElements, convShape )
% ----------------------------------------------------------------------------------------------- %
% [ mK ] = CreateConvMtx1D( vK, numElements, convShape )
% Generates a Convolution Matrix for 1D Kernel (The Vector vK) with
% support for different convolution shapes (Full / Same / Valid). The
% matrix is build such that for a signal 'vS' with 'numElements = size(vS
% ,1)' the following are equivalent: 'mK * vS' and conv(vS, vK,
% convShapeString);
% Input:
%   - vK                -   Input 1D Convolution Kernel.
%                           Structure: Vector.
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - numElements       -   Number of Elements.
%                           Number of elements of the vector to be
%                           convolved with the matrix. Basically set the
%                           number of columns of the Convolution Matrix.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3, ...}.
%   - convShape         -   Convolution Shape.
%                           The shape of the convolution which the output
%                           convolution matrix should represent. The
%                           options should match MATLAB's conv2() function
%                           - Full / Same / Valid.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3}.
% Output:
%   - mK                -   Convolution Matrix.
%                           The output convolution matrix. The product of
%                           'mK' and a vector 'vS' ('mK * vS') is the
%                           convolution between 'vK' and 'vS' with the
%                           corresponding convolution shape.
%                           Structure: Matrix (Sparse).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References:
%   1.  MATLAB's 'convmtx()' - https://www.mathworks.com/help/signal/ref/convmtx.html.
% Remarks:
%   1.  The output matrix is sparse data type in order to make the
%       multiplication by vectors to more efficient.
%   2.  In case the same convolution is applied on many vectors, stacking
%       them into a matrix (Each signal as a vector) and applying
%       convolution on each column by matrix multiplication might be more
%       efficient than applying classic convolution per column.
% TODO:
%   1.
%   Release Notes:
%   -   1.1.000     19/07/2021  Royi Avital
%       *   Updated to use modern MATLAB arguments validation.
%   -   1.0.000     20/01/2019  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

arguments
    vK (:, :) {mustBeNumeric, mustBeVector}
    numElements (1, 1) {mustBeNumeric, mustBeReal, mustBePositive, mustBeInteger}
    convShape (1, 1) {mustBeNumeric, mustBeMember(convShape, [1, 2, 3])} = 1
end

CONVOLUTION_SHAPE_FULL         = 1;
CONVOLUTION_SHAPE_SAME         = 2;
CONVOLUTION_SHAPE_VALID        = 3;

kernelLength    = length(vK);

switch(convShape)
    case(CONVOLUTION_SHAPE_FULL)
        rowIdxFirst = 1;
        rowIdxLast  = numElements + kernelLength - 1;
        outputSize  = numElements + kernelLength - 1;
    case(CONVOLUTION_SHAPE_SAME)
        rowIdxFirst = 1 + floor(kernelLength / 2);
        rowIdxLast  = rowIdxFirst + numElements - 1;
        outputSize  = numElements;
    case(CONVOLUTION_SHAPE_VALID)
        rowIdxFirst = kernelLength;
        rowIdxLast  = (numElements + kernelLength - 1) - kernelLength + 1;
        outputSize  = numElements - kernelLength + 1;
end

mtxIdx = 0;

% The sparse matrix constructor ignores values of zero yet the Row / Column
% indices must be valid indices (Positive integers). Hence 'vI' and 'vJ'
% are initialized to 1 yet for invalid indices 'vV' will be 0 hence it has
% no effect.
vI = ones(numElements * kernelLength, 1);
vJ = ones(numElements * kernelLength, 1);
vV = zeros(numElements * kernelLength, 1);

for jj = 1:numElements
    for ii = 1:kernelLength
        if((ii + jj - 1 >= rowIdxFirst) && (ii + jj - 1 <= rowIdxLast))
            % Valid otuput matrix row index
            mtxIdx = mtxIdx + 1;
            vI(mtxIdx) = ii + jj - rowIdxFirst;
            vJ(mtxIdx) = jj;
            vV(mtxIdx) = vK(ii);
        end
    end
end

mK = sparse(vI, vJ, vV, outputSize, numElements);

end


function sigma_values_test ( )

%*****************************************************************************80
%
%% SIGMA_VALUES_TEST demonstrates the use of SIGMA_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2005
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SIGMA_VALUES_TEST:\n' );
  fprintf ( 1, '  SIGMA_VALUES returns values of \n' );
  fprintf ( 1, '  the SIGMA function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N         SIGMA(N)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, fn ] = sigma_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %4d  %10d\n', n, fn );

  end

  return
end


function x = r83t_cg ( n, a, b, x )

%*****************************************************************************80
%
%% R83T_CG uses the conjugate gradient method on an R83T system.
%
%  Discussion:
%
%    The R83T storage format is used for a tridiagonal matrix.
%    The superdiagonal is stored in entries (1:N-1,3), the diagonal in
%    entries (1:N,2), and the subdiagonal in (2:N,1).  Thus, the
%    original matrix is "collapsed" horizontally into the array.
%
%    The matrix A must be a positive definite symmetric band matrix.
%
%    The method is designed to reach the solution after N computational
%    steps.  However, roundoff may introduce unacceptably large errors for
%    some problems.  In such a case, calling the routine again, using
%    the computed solution as the new starting estimate, should improve
%    the results.
%
%  Example:
%
%    Here is how an R83T matrix of order 5 would be stored:
%
%       *  A11 A12
%      A21 A22 A23
%      A32 A33 A34
%      A43 A44 A45
%      A54 A55  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frank Beckman,
%    The Solution of Linear Equations by the Conjugate Gradient Method,
%    in Mathematical Methods for Digital Computers,
%    edited by John Ralston, Herbert Wilf,
%    Wiley, 1967,
%    ISBN: 0471706892,
%    LC: QA76.5.R3.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, real A(N,3), the matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input/output, real X(N).
%    On input, an estimate for the solution, which may be 0.
%    On output, the approximate solution vector.
%
  b = b(:);
  x = x(:);
%
%  Initialize
%    AP = A * x,
%    R  = b - A * x,
%    P  = b - A * x.
%
  ap = r83t_mv ( n, n, a, x );

  r(1:n,1) = b(1:n,1) - ap(1:n,1);
  p(1:n,1) = b(1:n,1) - ap(1:n,1);
%
%  Do the N steps of the conjugate gradient method.
%
  for it = 1 : n
%
%  Compute the matrix*vector product AP=A*P.
%
    ap = r83t_mv ( n, n, a, p );
%
%  Compute the dot products
%    PAP = P*AP,
%    PR  = P*R
%  Set
%    ALPHA = PR / PAP.
%
    pap = p' * ap;
    pr = p' * r;

    if ( pap == 0.0 )
      return
    end

    alpha = pr / pap;
%
%  Set
%    X = X + ALPHA * P
%    R = R - ALPHA * AP.
%
    x(1:n,1) = x(1:n,1) + alpha * p(1:n,1);
    r(1:n,1) = r(1:n,1) - alpha * ap(1:n,1);
%
%  Compute the vector dot product
%    RAP = R*AP
%  Set
%    BETA = - RAP / PAP.
%
    rap = r' * ap;

    beta = - rap / pap;
%
%  Update the perturbation vector
%    P = R + BETA * P.
%
    p(1:n,1) = r(1:n,1) + beta * p(1:n,1);

  end

  return
end


function [assignment,cost] = munkres(costMat)
% MUNKRES   Munkres Assign Algorithm
%
% [ASSIGN,COST] = munkres(COSTMAT) returns the optimal assignment in ASSIGN
% with the minimum COST based on the assignment problem represented by the
% COSTMAT, where the (i,j)th element represents the cost to assign the jth
% job to the ith worker.
%

% This is vectorized implementation of the algorithm. It is the fastest
% among all Matlab implementations of the algorithm.

% Examples
% Example 1: a 5 x 5 example
%{
[assignment,cost] = munkres(magic(5));
[assignedrows,dum]=find(assignment);
disp(assignedrows'); % 3 2 1 5 4
disp(cost); %15
%}
% Example 2: 400 x 400 random data
%{
n=400;
A=rand(n);
tic
[a,b]=munkres(A);
toc                 % about 6 seconds
%}

% Reference:
% "Munkres' Assignment Algorithm, Modified for Rectangular Matrices",
% http://csclab.murraystate.edu/bob.pilgrim/445/munkres.html

% version 1.0 by Yi Cao at Cranfield University on 17th June 2008

assignment = false(size(costMat));
cost = 0;

costMat(costMat~=costMat)=Inf;
validMat = costMat<Inf;
validCol = any(validMat);
validRow = any(validMat,2);

nRows = sum(validRow);
nCols = sum(validCol);
n = max(nRows,nCols);
if ~n
    return
end

dMat = zeros(n);
dMat(1:nRows,1:nCols) = costMat(validRow,validCol);

%*************************************************
% Munkres' Assignment Algorithm starts here
%*************************************************

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   STEP 1: Subtract the row minimum from each row.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 dMat = bsxfun(@minus, dMat, min(dMat,[],2));

%**************************************************************************
%   STEP 2: Find a zero of dMat. If there are no starred zeros in its
%           column or row start the zero. Repeat for each zero
%**************************************************************************
zP = ~dMat;
starZ = false(n);
while any(zP(:))
    [r,c]=find(zP,1);
    starZ(r,c)=true;
    zP(r,:)=false;
    zP(:,c)=false;
end

while 1
%**************************************************************************
%   STEP 3: Cover each column with a starred zero. If all the columns are
%           covered then the matching is maximum
%**************************************************************************
    primeZ = false(n);
    coverColumn = any(starZ);
    if ~any(~coverColumn)
        break
    end
    coverRow = false(n,1);
    while 1
        %**************************************************************************
        %   STEP 4: Find a noncovered zero and prime it.  If there is no starred
        %           zero in the row containing this primed zero, Go to Step 5.
        %           Otherwise, cover this row and uncover the column containing
        %           the starred zero. Continue in this manner until there are no
        %           uncovered zeros left. Save the smallest uncovered value and
        %           Go to Step 6.
        %**************************************************************************
        zP(:) = false;
        zP(~coverRow,~coverColumn) = ~dMat(~coverRow,~coverColumn);
        Step = 6;
        while any(any(zP(~coverRow,~coverColumn)))
            [uZr,uZc] = find(zP,1);
            primeZ(uZr,uZc) = true;
            stz = starZ(uZr,:);
            if ~any(stz)
                Step = 5;
                break;
            end
            coverRow(uZr) = true;
            coverColumn(stz) = false;
            zP(uZr,:) = false;
            zP(~coverRow,stz) = ~dMat(~coverRow,stz);
        end
        if Step == 6
            % *************************************************************************
            % STEP 6: Add the minimum uncovered value to every element of each covered
            %         row, and subtract it from every element of each uncovered column.
            %         Return to Step 4 without altering any stars, primes, or covered lines.
            %**************************************************************************
            M=dMat(~coverRow,~coverColumn);
            minval=min(min(M));
            if minval==inf
                return
            end
            dMat(coverRow,coverColumn)=dMat(coverRow,coverColumn)+minval;
            dMat(~coverRow,~coverColumn)=M-minval;
        else
            break
        end
    end
    %**************************************************************************
    % STEP 5:
    %  Construct a series of alternating primed and starred zeros as
    %  follows:
    %  Let Z0 represent the uncovered primed zero found in Step 4.
    %  Let Z1 denote the starred zero in the column of Z0 (if any).
    %  Let Z2 denote the primed zero in the row of Z1 (there will always
    %  be one).  Continue until the series terminates at a primed zero
    %  that has no starred zero in its column.  Unstar each starred
    %  zero of the series, star each primed zero of the series, erase
    %  all primes and uncover every line in the matrix.  Return to Step 3.
    %**************************************************************************
    rowZ1 = starZ(:,uZc);
    starZ(uZr,uZc)=true;
    while any(rowZ1)
        starZ(rowZ1,uZc)=false;
        uZc = primeZ(rowZ1,:);
        uZr = rowZ1;
        rowZ1 = starZ(:,uZc);
        starZ(uZr,uZc)=true;
    end
end

% Cost of assignment
assignment(validRow,validCol) = starZ(1:nRows,1:nCols);
cost = sum(costMat(assignment));


%vgg_X_from_xP_nonlin  Estimation of 3D point from image matches and camera matrices, nonlinear.
%   X = vgg_X_from_xP_lin(x,P,imsize) computes max. likelihood estimate of projective
%   3D point X (column 4-vector) from its projections in K images x (2-by-K matrix)
%   and camera matrices P (K-cell of 3-by-4 matrices). Image sizes imsize (2-by-K matrix)
%   are needed for preconditioning.
%   By minimizing reprojection error, Newton iterations.
%
%   X = vgg_X_from_xP_lin(x,P,imsize,X0) takes initial estimate of X. If X0 is omitted,
%   it is computed by linear algorithm.
%
%   See also vgg_X_from_xP_lin.

% werner@robots.ox.ac.uk, 2003

function X = vgg_X_from_xP_nonlin(u,P,imsize,X)

if iscell(P)
  P = cat(3,P{:});
end
K = size(P,3);
if K < 2
  error('Cannot reconstruct 3D from 1 image');
end

if nargin==3
  X = vgg_X_from_xP_lin(u,P,imsize);
end

if nargin==2
  X = vgg_X_from_xP_lin(u,P);
end

% precondition
if nargin>2
  for k = 1:K
    H = [2/imsize(1,k) 0 -1
         0 2/imsize(2,k) -1
         0 0              1];
    P(:,:,k) = H*P(:,:,k);
    u(:,k) = H(1:2,1:2)*u(:,k) + H(1:2,3);
  end
end

% Parametrize X such that X = T*[Y;1]; thus x = P*T*[Y;1] = Q*[Y;1]
[dummy,dummy,T] = svd(X',0);
T = T(:,[2:end 1]);
for k = 1:K
  Q(:,:,k) = P(:,:,k)*T;
end

% Newton
Y = [0;0;0];
eprev = inf;
for n = 1:10
  [e,J] = resid(Y,u,Q);
  if 1-norm(e)/norm(eprev) < 1000*eps
    break
  end
  eprev = e;
  Y = Y - (J'*J)\(J'*e);
end

X = T*[Y;1];

return

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

function [e,J] = resid(Y,u,Q)
K = size(Q,3);
e = [];
J = [];
for k = 1:K
  q = Q(:,1:3,k);
  x0 = Q(:,4,k);
  x = q*Y + x0;
  e = [e; x(1:2)/x(3)-u(:,k)];
  J = [J; [x(3)*q(1,:)-x(1)*q(3,:)
           x(3)*q(2,:)-x(2)*q(3,:)]/x(3)^2];
end
return


function X = mtraceprod(A,B)
% X = MTRACEPROD(A, B)
%
% Evaluates trace(A*B') efficiently.
%
% X = MTRACEPROD(A)
%
% Evaluates trace(A*A') efficiently.
%
% If A and/or B has third dimension, the third dimension of A results in
% rows to X and the third dimension of B columns to X:
%
% X(I,J) = trace(A(:,:,I)*B(:,:,J)')

%warning('Deprecated.');

if nargin < 2
  [M,N,D] = size(A);
  X = reshape(A,[M*N,D])' * reshape(A,[M*N,D]);
else
  [M,N,Da] = size(A);
  [M,N,Db] = size(B);
  X = reshape(A,[M*N,Da])' * reshape(B,[M*N,Db]);
end


% HOMOTRANS - homogeneous transformation of points
%
% Function to perform a transformation on homogeneous points/lines
% The resulting points are normalised to have a homogeneous scale of 1
%
% Usage:
%           t = homoTrans(P,v);
%
% Arguments:
%           P  - 3 x 3 or 4 x 4 transformation matrix
%           v  - 3 x n or 4 x n matrix of points/lines

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB 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.
%
% MVTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.

%  Peter Kovesi
%  School of Computer Science & Software Engineering
%  The University of Western Australia
%  pk @ csse uwa edu au
%  http://www.csse.uwa.edu.au/~pk
%
%  April 2000
%  September 2007

function t = homotrans(P,v);

    [dim,npts] = size(v);

    if ~all(size(P)==dim)
	error('Transformation matrix and point dimensions do not match');
    end

    t = P*v;  % Transform

    for r = 1:dim-1     %  Now normalise
	t(r,:) = t(r,:)./t(end,:);
    end

    t(end,:) = ones(1,npts);


function [Mean, Covar] = ecmninit(Data, InitMethod)
%ECMNINIT Calculate initial mean and covariance for ECMNMLE.
%	Initial estimates for the Mean and Covariance of Data, where Data has
%	NUMSAMPLES samples of NUMSERIES random variables with missing data.
%
%	[Mean, Covar] = ecmninit(Data);
%	[Mean, Covar] = ecmninit(Data, InitMethod);
%
% Inputs:
%	Data - NUMSAMPLES x NUMSERIES matrix with NUMSAMPLES samples of a
%		NUMSERIES-dimensional random vector. Missing values are indicated
%		by NaNs. Data is the only required argument.
%
% Optional Inputs:
%	InitMethod - String to identify one of three initialization methods to
%		compute initial estimates for the mean and covariance of the data.
%		The default method is 'nanskip'. The initialization methods are:
%		'nanskip' - (default) Skip all records with NaNs.
%		'twostage' - Estimate mean, fill NaNs with mean, then estimate covar.
%		'diagonal' - Form a diagonal covar.
%
% Outputs:
%	Mean - NUMSERIES x 1 column vector initial estimate for mean of Data.
%	Covar - NUMSERIES x NUMSERIES matrix initial estimate for covariance of
%		Data.
%
% See also ECMNMLE.

%	Author(s): R.Taylor, 4-11-2005
%	Copyright 2005 The MathWorks, Inc.
%	$Revision: $ $Date: $

% Step 1 - check arguments

if nargin < 2
	InitMethod = 'NANSKIP';
end
if nargin < 1
	error('Finance:ecmninit:MissingInputArg', ...
		'The required input argument Data is missing.');
end

if isempty(Data)
    error('Finance:ecmninit:EmptyInputData', ...
		'The required input argument Data is empty.');
end

% Step 2 - initialization

[NumSamples, NumSeries] = size(Data);

if any(sum(isnan(Data),1) == NumSamples)
	error('Finance:ecmninit:TooManyNaNs', ...
		'One or more data series has all NaN values.');
end

if sum(sum(isinf(Data)))
	error('Finance:ecmninit:InfiniteValue', ...
		'One or more infinite values found in data.');
end

InitMethod = upper(InitMethod);
if ~any(strcmp(InitMethod,{'NANSKIP','TWOSTAGE','DIAGONAL'}))
	warning('Finance:ecmninit:UnknownInitMethodString', ...
		'Unknown InitMethod string. Will use default NANSKIP.');
	InitMethod = 'NANSKIP';
end

if strcmp(InitMethod,'NANSKIP') && (NumSamples - sum(any(isnan(Data),2)) <= NumSeries)
	warning('Finance:ecmninit:ChangeInitMethod', ...
		'Unable to do NANSKIP initialization. Switching to TWOSTAGE method ...');
	InitMethod = 'TWOSTAGE';
end

% Step 3 - estimate initial mean and covariance according to selected method

if strcmp(InitMethod,'TWOSTAGE')
	Mean = nanmean(Data)';
	Covar = zeros(NumSeries,NumSeries);
	for k = 1:NumSamples
		Z = zeros(NumSeries,1);
		for i = 1:NumSeries
			if ~isnan(Data(k,i))
				Z(i) = Data(k,i) - Mean(i);
			end
		end
		Covar = Covar + Z * Z';
	end
	Covar = (1.0/NumSamples) .* Covar;
elseif strcmp(InitMethod,'DIAGONAL')
	Mean = nanmean(Data)';
	Covar = diag(nanvar(Data,1));
else									% default method is NANSKIP
	P = ~any(isnan(Data),2);
	Mean = mean(Data(P,:))';
	Covar = cov(Data(P,:),1);
end


function lobatto_set_test ( )

%*****************************************************************************80
%
%% LOBATTO_SET_TEST tests LOBATTO_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LOBATTO_SET_TEST\n' );
  fprintf ( 1, '  LOBATTO_SET sets a Lobatto rule;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '         I      X             W\n' );

  for n = 4 : 3 : 12

    [ x, w ] = lobatto_set ( n );

    fprintf ( 1, '\n' );
    for i = 1 : n
      fprintf ( 1, '  %8d  %12f  %12f\n', i, x(i), w(i) );
    end

  end

  return
end


function [latout,lonout] = my_interpm(lat,lon,maxdiff)
% [latout,lonout] = my_interpm(lat,lon,maxdiff)
% This function  fills in any gaps in latitude (lat) or longitude (lon) data vectors
% that are greater than a defined tolerance maxdiff apart in either dimension.
% lat and lon should be row vectors.
%
% latout and lonout are the new latitude and longitude data vectors, in which any gaps
% larger than maxdiff in the original vectors have been filled with additional points.
%
% by Jindong Wang on March 1, 2018
%
if isrow(lat)
    lat=lat';
end
if isrow(lon)
    lon=lon';
end
ny=size(lat,1);
nx=size(lon,1);
if nx~=ny || nx<2 || ny<2 || maxdiff<10^-8
    error('Error: Wrong input!');
end

dlat=abs(lat(2:end)-lat(1:end-1));
dlon=abs(lon(2:end)-lon(1:end-1));
nin=ceil(max([dlat,dlon],[],2)/maxdiff)-1;
sumnin=sum(nin,'omitnan');
if sumnin==0
    disp('No incertion needed.');
    latout=lat;
    lonout=lon;
    return
end
nout=sumnin+nx;
latout=nan(nout,1);
lonout=nan(nout,1);

n=1;
for i=1:nx-1;
    ni=nin(i);
    if ni==0 || isnan(ni)
        latout(n)=lat(i);
        lonout(n)=lon(i);
        nstep=1;
    else
        ilat=linspace(lat(i),lat(i+1),ni+2);
        ilon=linspace(lon(i),lon(i+1),ni+2);
        latout(n:n+ni)=ilat(1:ni+1);
        lonout(n:n+ni)=ilon(1:ni+1);
        nstep=ni+1;
    end
    n=n+nstep;
end
latout(end)=lat(end);
lonout(end)=lon(end);


function parInfo = sweepExample()
% Copyright 2010 The MathWorks, Inc.

%% Initialize Problem
m     =         5;  % mass
bVals =  .1:.1 :5;  % damping values
kVals = 1.5:.05:5;  % stiffness values
[kGrid, bGrid] = meshgrid(bVals, kVals);
peakVals = nan(size(kGrid));

%% Parameter Sweep
% disp('Computing ...');drawnow;

parInfo = Par(numel(kGrid));

parfor idx = 1:numel(kGrid)
   Par.tic;
  % Solve ODE
  [T,Y] = ode45(@(t,y) odesystem(t, y, m, bGrid(idx), kGrid(idx)), ...
    [0, 25], ...  % simulate for 25 seconds
    [0, 1]);      % initial conditions

  % Determine peak value
  peakVals(idx) = max(Y(:,1));
  parInfo(idx) = Par.toc;
end

stop(parInfo);

function dy = odesystem(t, y, m, b, k)
% 2nd-order ODE
%
%   m*X'' + b*X' + k*X = 0
%
% --> system of 1st-order ODEs
%
%   y  = X'
%   y' = -1/m * (k*y + b*y')

dy(1) = y(2);
dy(2) = -1/m * (k * y(1) + b * y(2));

dy = dy(:); % convert to column vector


function Rx=xrot(phi)

Rx = [1 0 0; 0 cos(phi) -sin(phi);0 sin(phi) cos(phi)];


function [r, pol, res, zer, zj, fj, wj, errvec, wt] = aaatrig(F, varargin)
%AAATRIG   Trigonometric AAA and AAA-Lawson (near-minimax) real or complex
%      rational approximation.
%   R = AAATRIG(F, Z) computes an trigonometric AAA rational approximant R
%   (function handle) to data F on the set of sample points Z.  The rational
%   approximant is periodic with period 2*pi. F may be given by its values at
%   Z, or as a function handle or a chebfun.  R = AAATRIG(F, Z, 'degree', N)
%   computes the minimax approximation of degree N (i.e., rational type
%   (N,N)).
%
%   [R, POL, RES, ZER] = AAATRIG(F, Z) returns vectors of poles POL, residues
%   RES, and zeros ZER of R. The poles and zeros are repeated at intervals
%   of 2*pi.
%
%   [R, POL, RES, ZER, ZJ, FJ, WJ] = AAATRIG(F, Z) also returns the vectors of
%   support points ZJ, approximation values FJ = r(ZJ), and weights WJ of
%   the barycentric representation of R.
%
%   [R, POL, RES, ZER, ZJ, FJ, WJ, ERRVEC] = AAATRIG(F, Z) also returns the
%   vector of errors ||f-r||_infty in successive iteration steps of AAATRIG.
%
%   R = AAATRIG(F,Z,FORM) computes a rational approximant of type FORM.
%   FORM can either be 'odd' (default) or 'even'.
%
%   R = AAATRIG(F, Z, NAME, VALUE) sets the following parameters:
%   - 'tol', TOL: relative tolerance (default TOL = 1e-13),
%   - 'degree', N: maximal degree (default N = 99).
%      Output rational approximant will be at most of type (N,N).
%      Identical to 'mmax', N+1.
%      By default, this will turn on Lawson iteration: see next paragraph.
%   - 'mmax', MMAX: maximal number of terms in the barycentric representation
%       (default MMAX = 100). R will be of degree MMAX-1.
%       Identical to 'degree', MMAX-1. Also turns on Lawson iteration.
%   - 'dom', DOM: domain (default DOM = [0, 2*pi]). No effect if Z is provided.
%   - 'cleanup', 'off' or 0: turns off automatic removal of numerical Froissart
%       doublets
%   - 'cleanuptol', CLEANUPTOL: cleanup tolerance (default CLEANUPTOL = TOL).
%       Poles with residues less than this number times the geometric mean size
%       of F times the minimum distance to Z are deemed spurious by the cleanup
%       procedure. If TOL = 0, then CLEANUPTOL defaults to 1e-13.
%   - 'lawson', NLAWSON: take NLAWSON iteratively reweighted least-squares steps
%       to bring approximation closer to minimax; specifying NLAWSON = 0
%       ensures there is no Lawson iteration.  See next paragraph.
%
%   If 'degree' or equivalently 'mmax' is specified and 'lawson' is not, then
%   AAATRIG attempts to find a minimax approximant of degree N by Lawson iteration.
%   This will generally be successful only if the minimax error is well
%   above machine precision, and is more reliable for complex problems than
%   real ones.  If 'degree' and 'lawson' are both specified, then exactly
%   NLAWSON Lawson steps are taken (so NLAWSON = 0 corresponds to AAA
%   approximation with no Lawson iteration).  The final weight vector WT of
%   the Lawson iteration is available with
%   [R, POL, RES, ZER, ZJ, FJ, WJ, ERRVEC, WT] = AAATRIG(F, Z).
%
%   Note that R may have fewer than N poles and zeros.  This may happen,
%   for example, if N is too large, or if F is even and N is odd, or if F is
%   odd and N is even.
%
%   One can also execute R = AAATRIG(F), with no specification of a set Z.
%   If F is a vector, this is equivalent to R = AAATRIG(F, Z) with
%   Z = LINSPACE(0, 2*pi, LENGTH(F)).  If F is a function handle or a chebfun,
%   AAATRIG attempts to resolve F on its domain, which defaults to [0,2*pi] for
%   a function handle.
%
% Examples:
%
%   f = @(x) exp(cos(x));
%   r = aaatrig(f); xx = linspace(-pi,pi); plot(xx,r(xx)-f(xx))
%
%   r = aaatrig(f,'degree',4); xx = linspace(-pi,pi); plot(xx,r(xx)-f(xx))
%
%   Z = exp(2i*pi*linspace(0,1,500));
%   [r,pol,res] = aaatrig(@tan,Z); disp([pol res])
%
%   X = linspace(0,2*pi,1000); F = gamma(2+sin(X));
%   subplot(1,2,1)
%   r = aaatrig(F,X,'degree',10,'lawson',0); plot(X,F-r(X)), hold on
%   r = aaatrig(F,X,'degree',10); plot(X,F-r(X)), hold off
%
%   Z = exp(1i*linspace(0,2*pi,1000)); G = exp(1i*tan(Z));
%   subplot(1,2,2)
%   r = aaatrig(G,Z,'degree',5,'lawson',0); plot(G-r(Z)), axis equal, hold on
%   r = aaatrig(G,Z,'degree',5); plot(G-r(Z)), axis equal, hold off
%
%   References:
%   [1] Yuji Nakatsukasa, Olivier Sete, Lloyd N. Trefethen, "The AAA algorithm
%   for rational approximation", SIAM J. Sci. Comp. 40 (2018), A1494-A1522.
%
%   [2] Yuji Nakatsukasa and Lloyd N. Trefethen, "An algorithm for real and
%   complex rational minimax approximation", SIAM J. Sci. Comp. (2020).
%
%   [3] Peter J. Baddoo, "The AAAtrig algorithm for rational approximation
%   of periodic functions", SIAM J. Sci. Comp. (2021).
%
% See also AAA, TRIGRATINTERP, CHEBPADE, MINIMAX, PADEAPPROX.

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

% Parse inputs:
[F, Z, M, form, dom, tol, mmax, cleanup_flag, cleanup_tol, needZ, mmax_flag, nlawson] ...
    = parseInputs(F, varargin{:});

if ( needZ )
    % Z was not provided.  Try to resolve F on its domain.
    [r, pol, res, zer, zj, fj, wj, errvec] = ...
        aaatrig_autoZ(F, form, dom, tol, mmax, cleanup_flag, cleanup_tol, mmax_flag, nlawson);
    return
end

% Remove any infinite or NaN function values (avoid SVD failures):
toKeep = ~isinf(F);
F = F(toKeep); Z = Z(toKeep);
toKeep = ~isnan(F);
F = F(toKeep); Z = Z(toKeep);

% Find sample points at infity
infP = find(Z==+1i*Inf);
infM = find(Z==-1i*Inf);
finfP = F(infP); % value at +1i*Inf
finfM = F(infM); % value at -1i*Inf
F([infP,infM])=[]; Z([infP,infM])=[];
if strcmp(form,'even') && numel([finfP,finfM])==2 && finfP~=finfM
    error('The even representation must take the same values at +/-i*Inf.')
end

% Define basis functions
if strcmp(form,'even')
    cst = @(x) cot(x);
elseif strcmp(form,'odd')
    cst = @(x) csc(x);
end

% Project sample points onto a single period window
Z = Z - 2*pi*floor(real(Z/(2*pi)));

% Remove repeated elements of Z and corresponding elements of F:
[Z, uni] = unique(Z,'stable'); F = F(uni);

M = length(Z);

% Relative tolerance:
reltol = tol * norm(F, inf);

% Left scaling matrix:
SF = spdiags(F, 0, M, M);

% Initialization for AAA iteration:
J = 1:M;
zj = [];
fj = [];
C = [];
errvec = [];
R = mean(F);

% AAA iteration:
for m = 1:mmax
    % Select next support point where error is largest:
    [~, jj] = max(abs(F - R));          % Select next support point.
    zj = [zj; Z(jj)];                   % Update support points.
    fj = [fj; F(jj)];                   % Update data values.
    J(J == jj) = [];                    % Update index vector.
    C = [C cst((Z - Z(jj))/2)];
    % Compute weights:
    Sf = diag(fj);                      % Right scaling matrix.
    A = SF*C - C*Sf;                    % Loewner matrix.
    % Add value(s) at infinity to least-squares problem
    Jv = J;
    if ~isempty([finfP,finfM])
        if strcmp(form,'odd')
            if ~isempty(finfP)
                A = [A; (finfP - fj.').*exp(-1i*zj.'/2)];
                Jv = [Jv, size(A,1)];
            end
            if ~isempty(finfM)
                A = [A; (finfM - fj.').*exp(+1i*zj.'/2)];
                Jv = [Jv, size(A,1)];
            end
        elseif strcmp(form,'even')
        A = [A; (finfP-fj.')];
        Jv = [Jv, size(A,1)];
        end
    end
    [~, ~, V] = svd(A(Jv,:), 0); % Reduced SVD. Includes the bottom
    wj = V(:,m);                        % weight vector = min sing vector
    % Rational approximant on Z:
    N = C*(wj.*fj);                     % Numerator
    D = C*wj;                           % Denominator
    R = F;
    R(J) = N(J)./D(J);

    % Error in the sample points and at infinity:
    err = F - R;
    if isempty([finfP,finfM])
    maxerr = norm(1, inf);
    else
        if strcmp(form,'odd')
            if ~isempty(finfP)
            errInfP = finfP - (fj.'.*exp(-1i*zj.'/2)*wj)./(exp(-1i*zj.'/2)*wj); % error at +1i*infinity
            err = [err; errInfP];
            end
            if ~isempty(finfM)
            errInfM = finfM - (fj.'.*exp(+1i*zj.'/2)*wj)./(exp(+1i*zj.'/2)*wj); % error at -1i*infinity
            err = [err; errInfM];
            end
        elseif strcmp(form,'even')
            errInf = finfP - fj.'*wj./sum(wj); % error at +/-1i*infinity
            err = [err; errInf];
        end
    end
    maxerr = norm(err,inf);
    errvec = [errvec; maxerr];
    % Check if converged:
    if ( maxerr <= reltol )
        break
    end
end

maxerrAAA = maxerr;                     % error at end of AAA

% When M == 2, one weight is zero and r is constant.
% To obtain a good approximation, interpolate in both sample points.
if ( M == 2 )
    zj = Z;
    fj = F;
    wj = [1; -1];
    wj = wj/norm(wj);   % Impose norm(w) = 1 for consistency.
    errvec(2) = 0;
    maxerrAAA = 0;
end

% We now enter Lawson iteration: barycentric IRLS = iteratively reweighted
% least-squares if 'lawson' is specified with NLAWSON > 0 or 'mmax' is
% specified and 'lawson' is not.  In the latter case the number of steps
% is chosen adaptively.  Note that the Lawson iteration is unlikely to be
% successful when the errors are close to machine precision.

wj0 = wj; fj0 = fj;     % Save parameters in case Lawson fails
wt = NaN(M,1); wt_new = ones(M,1);
if ( nlawson > 0 )      % Lawson iteration

  if ~isempty([finfP,finfM])
    warning('Specifying the function values at infinity is not currently compatible with Lawson iteration.')
  end

    maxerrold = maxerrAAA;
    maxerr = maxerrold;
    nj = length(zj);
    A = [];
    for j = 1:nj                              % Cauchy/Loewner matrix
        A = [A cst((Z-zj(j))/2) F.*cst((Z-zj(j))/2)];
    end
    for j = 1:nj
        [i,~] = find(Z==zj(j));               % support pt rows are special
        A(i,:) = 0;
        A(i,2*j-1) = 2;
        A(i,2*j) = 2*F(i);
    end
    stepno = 0;
    while ( (nlawson < inf) & (stepno < nlawson) ) |...
          ( (nlawson == inf) & (stepno < 20) ) |...
          ( (nlawson == inf) & (maxerr/maxerrold < .999) & (stepno < 1000) )
        stepno = stepno + 1;
        wt = wt_new;
        W = spdiags(sqrt(wt),0,M,M);
        [U,S,V] = svd(W*A,0);
        c = V(:,end);
        denom = zeros(M,1); num = zeros(M,1);
        for j = 1:nj
            denom = denom + c(2*j).*cst((Z-zj(j))/2);
            num = num - c(2*j-1).*cst((Z-zj(j))/2);
        end
        R = num./denom;
        for j = 1:nj
            [i,~] = find(Z==zj(j));           % support pt rows are special
            R(i) = -c(2*j-1)/c(2*j);
        end
        err = F - R; abserr = abs(err);
        wt_new = wt.*abserr; wt_new = wt_new/norm(wt_new,inf);
        maxerrold = maxerr;
        maxerr = max(abserr);
    end
    wj = c(2:2:end);
    fj = -c(1:2:end)./wj;
    % If Lawson has not reduced the error, return to pre-Lawson values.
    if (maxerr > maxerrAAA) & (nlawson == Inf)
        wj = wj0; fj = fj0;
    end
end
% Remove support points with zero weight:
I = find(wj == 0);
zj(I) = [];
wj(I) = [];
fj(I) = [];

% Construct function handle:
r = @(zz) revaltrig(zz, zj, fj, wj, form);

% Compute poles, residues and zeros:
[pol, res, zer] = prztrig(zj, fj, wj, form);

if ( cleanup_flag & nlawson == 0)       % Remove Froissart doublets
    [r, pol, res, zer, zj, fj, wj] = ...
        cleanuptrig(r, form, finfP, finfM,pol, res, zer, zj, fj, wj, Z, F, cleanup_tol);
end

end % of AAATRIG()

%% parse Inputs:

function [F, Z, M, form, dom, tol, mmax, cleanup_flag, cleanup_tol, ...
    needZ, mmax_flag, nlawson] = parseInputs(F, varargin)
% Input parsing for AAATRIG.

% Check if F is empty:
if ( isempty(F) )
    error('CHEBFUN:aaatrig:emptyF', 'No function given.')
elseif ( isa(F, 'chebfun') )
    if ( size(F, 2) ~= 1 )
        error('CHEBFUN:aaatrig:nColF', 'Input chebfun must have one column.')
    end
end

% Sample points:
if ( ~isempty(varargin) && isfloat(varargin{1}) )
    % Z is given.
    Z = varargin{1};
    if ( isempty(Z) )
        error('CHEBFUN:aaatrig:emptyZ', ...
            'If sample set is provided, it must be nonempty.')
    end
    varargin(1) = [];
end

% Set defaults for other parameters:
tol = 1e-13;         % Relative tolerance.
mmax = 100;          % Maximum number of terms.
cleanup_tol = 1e-13; % Cleanup tolerance.
nlawson = Inf;       % number of Lawson steps (Inf means adaptive)
% Domain:
if ( isa(F, 'chebfun') )
    dom = F.domain([1, end]);
else
    dom = [0, 2*pi];
end
cleanup_flag = 1;   % Cleanup on.
mmax_flag = 0;      % Checks if mmax manually specified.
cleanup_set = 0;    % Checks if cleanup_tol manually specified.
form = 'odd';
% Check if parameters have been provided:
while ( ~isempty(varargin) )

    if  strncmpi(varargin{1},'even',4)
          form = 'even';
          varargin(1) = [];
    elseif strncmpi(varargin{1},'odd',3)
          form = 'odd';
          varargin(1) = [];

    elseif ( strncmpi(varargin{1}, 'tol', 3) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            tol = varargin{2};
            if ~cleanup_set & tol > 0 % If not manually set, set cleanup_tol to tol.
              cleanup_tol = tol;
            end
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'degree', 6) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            if ( mmax_flag == 1 ) && ( mmax ~= varargin{2}+1 )
                error('CHEBFUN:aaatrig:degmmaxmismatch', ' mmax must equal degree+1.')
            end
            mmax = varargin{2}+1;
            mmax_flag = 1;
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'mmax', 4) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            if ( mmax_flag == 1 ) && ( mmax ~= varargin{2})
                error('CHEBFUN:aaatrig:degmmaxmismatch', ' mmax must equal degree+1.')
            end
            mmax = varargin{2};
            mmax_flag = 1;
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'lawson', 6) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            nlawson = varargin{2};
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'dom', 3) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 2]) )
            dom = varargin{2};
        end
        varargin([1, 2]) = [];
        if ( isa(F, 'chebfun') )
            if ( ~isequal(dom, F.domain([1, end])) )
                warning('CHEBFUN:aaatrig:dom', ...
                    ['Given domain does not match the domain of the chebfun.\n', ...
                    'Results may be inaccurate.'])
            end
        end

    elseif ( strncmpi(varargin{1}, 'cleanuptol', 10) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
          cleanup_tol = varargin{2};
          cleanup_set = 1;
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'cleanup', 7) )
        if ( strncmpi(varargin{2}, 'off', 3) || ( varargin{2} == 0 ) )
            cleanup_flag = 0;
        end
        varargin([1, 2]) = [];

    else
        error('CHEBFUN:aaatrig:UnknownArg', 'Argument unknown.')
    end
end

% Deal with Z and F:
if ( ~exist('Z', 'var') && isfloat(F) )
    % F is given as data values, pick same number of sample points:
    Z = linspace(dom(1), dom(2), length(F)).';
end

if ( exist('Z', 'var') )
    % Z is given:
    needZ = 0;

    % Work with column vector:
    Z = Z(:);
    M = length(Z);

    % Function values:
    if ( isa(F, 'function_handle') || isa(F, 'chebfun') )
        % Sample F on Z:
        F = F(Z);
    elseif ( isnumeric(F) )
        % Work with column vector and check that it has correct length.
        F = F(:);
        if ( length(F) ~= M )
            error('CHEBFUN:aaatrig:lengthFZ', ...
                'Inputs F and Z must have the same length.')
        end
    elseif ( ischar(F) )
        % F is given as a string input. Convert it to a function handle.
        F = str2op(vectorize(F));
        F = F(Z);
    else
        error('CHEBFUN:aaatrig:UnknownF', 'Input for F not recognized.')
    end

else
    % Z was not given.  Set flag that Z needs to be determined.
    % Also set Z and M since they are needed as output.
    needZ = 1;
    Z = [];
    M = length(Z);
end

if ~mmax_flag & (nlawson == Inf)
    nlawson = 0;
end

end % End of PARSEINPUT().

%% Cleanup.  In June 2022 the residue size test was changed to be relative to
%            the distance to the approximation set Z.

function [r, pol, res, zer, z, f, w] = ...
    cleanuptrig(r, form, finfP, finfM, pol, res, zer, z, f, w, Z, F, cleanup_tol)
% Remove spurious pole-zero pairs.

% Find negligible residues:
if any(F)
   geometric_mean_of_absF = exp(mean(log(abs(F(F~=0)))));
else
   geometric_mean_of_absF = 0;
end
Zdistances = NaN(size(pol));
for j = 1:length(Zdistances);
   Zdistances(j) = min(abs(pol(j)-Z));
end
ii = find(abs(res)./Zdistances < cleanup_tol * geometric_mean_of_absF);
ni = length(ii);
if ( ni == 0 )
    % Nothing to do.
    return
elseif ( ni == 1 )
    warning('CHEBFUN:aaatrig:Froissart','1 Froissart doublet');
else
    warning('CHEBFUN:aaatrig:Froissart',[int2str(ni) ' Froissart doublets']);
end
% For each spurious pole find and remove closest support point.
for j = 1:ni
    % Find the closest support point modulo the period
    np = fix(real((z-pol(ii(j)))/pi));
    azp = abs(z-(pol(ii(j)) + np*2*pi));
    jj = find(azp == min(azp),1);
    % Remove support point(s):
    z(jj) = [];
    f(jj) = [];
end

% Remove support points z from sample set:
for jj = 1:length(z)
    F(Z == z(jj)) = [];
    Z(Z == z(jj)) = [];
end

m = length(z);
M = length(Z);

% Define basis functions
if strcmp(form,'even')
    cst = @(x) cot(x);
elseif strcmp(form,'odd')
    cst = @(x) csc(x);
end

% Build Loewner matrix:
SF = spdiags(F, 0, M, M);
Sf = diag(f);
C = cst(bsxfun(@minus, Z, z.')/2);      % Cauchy matrix.
A = SF*C - C*Sf;                    % Loewner matrix.

% Enforce value(s) at infinity
if ~isempty([finfP,finfM])
    if strcmp(form,'odd')
        if ~isempty(finfP)
            A = [A; (finfP - f.').*exp(-1i*z.'/2)];
        end
        if ~isempty(finfM)
            A = [A; (finfM - f.').*exp(+1i*z.'/2)];
        end
    elseif strcmp(form,'even')
    A = [A; (finfP-f.')];   % Add a row to A to enforce behaviour at infinity.
    end
end

% Solve least-squares problem to obtain weights:
[~, ~, V] = svd(A, 0);
w = V(:,m);

% Build function handle and compute poles, residues and zeros:
r = @(zz) revaltrig(zz, z, f, w, form);
[pol, res, zer] = prztrig(z, f, w, form);

end % End of CLEANUPTRIG().

%% Automated choice of sample set

function [r, pol, res, zer, zj, fj, wj, errvec] = ...
    aaatrig_autoZ(F, form, dom, tol, mmax, cleanup_flag, cleanup_tol, mmax_flag, nlawson)
%

% Flag if function has been resolved:
isResolved = 0;

% Main loop:
for n = 5:14
    % Sample points:
    % Next line enables us to do pretty well near poles
    Z = linspace(dom(1)+1.37e-7*diff(dom), dom(2), 2 + 2^n).'; Z(end) = [];
    [r, pol, res, zer, zj, fj, wj, errvec] = aaatrig(F, Z, form, 'tol', tol, ...
        'mmax', mmax, 'cleanup', cleanup_flag, 'cleanuptol', cleanup_tol, 'lawson', nlawson);

    % Test if rational approximant is accurate:
    reltol = tol * norm(F(Z), inf);

    % On Z(n):
    err(1,1) = norm(F(Z) - r(Z), inf);
    Zrefined = linspace(dom(1)+1.37e-8*diff(dom), dom(2)-3.08e-9*diff(dom), ...
        1 + round(1.5 * (1 + 2^(n+1)))).';
    Zrefined(end) = [];
    err(2,1) = norm(F(Zrefined) - r(Zrefined), inf);

    if ( all(err < reltol) )
        % Final check that the function is resolved, inspired by sampleTest().
        % Pseudo random sample points in [-1, 1]:
        xeval = [-0.357998918959666; 0.036785641195074];
        % Scale to dom:
        xeval = (dom(2) - dom(1))/2 * xeval + (dom(2) + dom(1))/2;

        if ( norm(F(xeval) - r(xeval), inf) < reltol )
            isResolved = 1;
            break
        end
    end
end

if ( ( isResolved == 0 ) && ~mmax_flag )
    warning('CHEBFUN:aaatrig:notResolved', ...
        'Function not resolved using %d pts.', length(Z))
end

end % End of AAATRIG_AUTOZ().

function op = str2op(op)
    % Convert string inputs to either numeric format or function_handles.
    sop = str2num(op);
    if ( ~isempty(sop) )
        op = sop;
    else
        depVar = symvar(op);
        if ( numel(depVar) ~= 1 )
            error('CHEBFUN:CHEBFUN:str2op:indepvars', ...
             'Incorrect number of independent variables in string input.');
        end
        op = eval(['@(' depVar{:} ')', op]);
    end
end % End of STR2OP().


function coords = indices2Coords(indices,dims)
%
% coords = indices2Coords(indices,dims)
%
% AUTHOR:  Boynton
% PURPOSE: (Author: Wandell)
%   Well, the comments are pretty hard to figure out and the code ain't
% that easy either.  But I think that this routine suppose that there is
% an array, say X, with dims = size(X).
% Then, if we address the array via X(indices), this routine
% tells us which of the array coordinates, in the form (x1, x2 ... xNdims),
% where Ndims = length(dims), correspond to the list of values in indices.
% This is quite what it says below (the original comments).  But I think
% it does this.
%
% indices: 1xN vector that can be used to pick off the indexed
%   values of an array with dimensions dims.
%
% dims: size of each dimension.  E.g., dims=[100,200,8] means
%   that the 1st row of the returned values, coords,
%   takes on values between 1:100,
%   2nd row between 1:200, 3rd row between 1:8.
%
% coords: MxN array of coordinates, M is the dimensionality. (I think this
%   means that M is length(dims)).
%   e.g., coords might be 3x100 with y,x,z values in each
%   column.
%   And he doesn't say, but I think that N is the number of indices.
%
% gmb, 1/23/98

% Why is this a find?  Does he mean to check whether indices is empty?
if find(indices)

   % Force indices to be a row vector
   indices = indices(:)';

   % Allocates an array to store the coords that will be returned
   coords = zeros([length(dims),length(indices)]);

   % Create the array coords using a Boyntonesque formula
   for d = length(dims):-1:1
      coords(d,:) = floor((indices-1) / prod(dims(1:d-1))) + 1;
      indices = indices - (coords(d,:)-1)*prod(dims(1:d-1));
   end

else
   coords = [];
end

return;

%%% Debug

dims=[10];
coords = [1:10];
indices=coords2Indices(coords,dims)
coords=indices2Coords(indices,dims)

dims=[3 3];
coords = [1 2 3;
          1 2 3];
indices=coords2Indices(coords,dims)
coords=indices2Coords(indices,dims)

dims=[3 3 3];
coords = [1 2 3 1 2 3;
          1 2 3 1 2 3;
	  1 1 1 3 3 3];
indices=coords2Indices(coords,dims)
coords=indices2Coords(indices,dims)


function bf10 = corrbfrep(r,n)
%
% bf10 = corrbfrep(r,n)
%
% Calculates replication correlation Bayes Factor using a uniform prior
% between 0-1 (one sided test) as used by Boekel et al, 2014, Cortex.
%

% Function to be integrated
F = @(rho,r,n) ((1-rho.^2).^(1./2*(n-1))) ./ (1-rho*r).^(n-3./2);

% Bayes factor calculation
bf10 = integral(@(rho) F(rho,r,n), 0, 1);


    function xsol = fzero_data(x,y,y0)
        N=length(x);
        vectsign=sign(y-y0);
        pos=zeros(1,N);
        for i=1:(N-1)
            if vectsign(i)~=vectsign(i+1)
                pos(i)=1;
                pos(i+1)=1;
            end
        end

        indices=find(pos);
        Nmax=2*(length(indices)-1);
        vectint=zeros(1,Nmax);
        vectint(1)=indices(1);
        vectint(Nmax)=indices(length(indices));
        for j=2:2:(Nmax-2)
            vectint(j)=indices(j/2+1);
            vectint(j+1)=indices(j/2+1);
        end
        Nmaxsol=Nmax/2;
        indsol=zeros(1,2*Nmaxsol);
        for k=1:Nmaxsol
            if (y(vectint(2*k-1))-y0>=0 && y(vectint(2*k))-y0<=0)||(y(vectint(2*k-1))-y0<=0 && y(vectint(2*k))-y0>=0)
                indsol(2*k-1)=1;
                indsol(2*k)=1;
            else
                indsol(2*k-1)=0;
                indsol(2*k)=0;
            end
        end
        vectpos=vectint(find(indsol));
        Nsol=length(vectpos)/2;
        xsol=zeros(1,Nsol);
        for n=1:Nsol
            xsol(n)=interp1(y(vectpos(2*n-1:2*n))-y0,x(vectpos(2*n-1:2*n)),0);
        end

        clear vectsign pos indices Nmax vectint Nmaxsol indsol vectpos Nsol

    end


function [cum_ret, cumprod_ret, daily_ret, daily_portfolio]...
    = olmar2_run(fid, data, epsilon, alpha, tc, opts)
% This program simulates the OLMAR-2 algorithm
%
% function [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
%    = olmar2_run(fid, data, epsilon, alpha, tc, opts)
%
% cum_ret: a number representing the final cumulative wealth.
% cumprod_ret: cumulative return until each trading period
% daily_ret: individual returns for each trading period
% daily_portfolio: individual portfolio for each trading period
%
% data: market sequence vectors
% fid: handle for write log file
% epsilon: mean reversion threshold
% alpha: trade off parameter for calculating moving average [0, 1]
% tc: transaction cost rate parameter
% opts: option parameter for behvaioral control
%
% Example: [cum_ret, cumprod_ret, daily_ret, daily_portfolio, exp_ret] ...
%           = olmar2_run(fid, data, epsilon, alpha, tc, opts)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Steven C.H. Hoi
% Contributors:
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[n, m] = size(data);

% Return variables
cum_ret = 1;
cumprod_ret = ones(n, 1);
daily_ret = ones(n, 1);

% Portfolio weights, starting with uniform portfolio
day_weight = ones(m, 1)/m;  %#ok<*NASGU>
day_weight_o = zeros(m, 1);  % Last closing price adjusted portfolio
daily_portfolio = zeros(n, m);

% print file head
fprintf(fid, '-------------------------------------\n');
fprintf(fid, 'Parameters [epsilon:%.2f, alpha:%.2f, tc:%.4f]\n', ...
    epsilon, alpha, tc);
fprintf(fid, 'day\t Daily Return\t Total return\n');

fprintf(1, '-------------------------------------\n');
if(~opts.quiet_mode)
    fprintf(1, 'Parameters [epsilon:%.2f, alpha:%.2f, tc:%.4f]\n', ...
        epsilon, alpha, tc);
    fprintf(1, 'day\t Daily Return\t Total return\n');
end

data_phi = ones(1, m);

%% Trading
if (opts.progress)
	progress = waitbar(0,'Executing Algorithm...');
end
for t = 1:1:n,
    % Step 1: Receive stock price relatives
    if (t >= 2)
        [day_weight, data_phi] ...
            = olmar2_kernel(data(1:t-1, :), data_phi, day_weight, epsilon, alpha);
    end

    % Normalize the constraint, always useless
    day_weight = day_weight./sum(day_weight);
    daily_portfolio(t, :) = day_weight';

    if or((day_weight < -0.00001+zeros(size(day_weight))), (day_weight'*ones(m, 1)>1.00001))
        fprintf(1, 'mrpa_expert: t=%d, sum(day_weight)=%d, pause', t, day_weight'*ones(m, 1));
        pause;
    end

    % Step 2: Cal t's daily return and total return
    daily_ret(t, 1) = (data(t, :)*day_weight)*(1-tc/2*sum(abs(day_weight-day_weight_o)));
    cum_ret = cum_ret * daily_ret(t, 1);
    cumprod_ret(t, 1) = cum_ret;

    % fprintf(1, '%d\t%.2f\t%.2f\t%.2f\n', t, day_weight(1), day_weight(2), daily_ret(t, 1));
    % Adjust weight(t, :) for the transaction cost issue
    day_weight_o = day_weight.*data(t, :)'/daily_ret(t, 1);

    % Debug information
    % Time consuming part, other way?
    fprintf(fid, '%d\t%f\t%f\n', t, daily_ret(t, 1), cumprod_ret(t, 1));
    if (~opts.quiet_mode),
        if (~mod(t, opts.display_interval)),
            fprintf(1, '%d\t%f\t%f\n', t, daily_ret(t, 1), cumprod_ret(t, 1));
        end
    end
    if (opts.progress)
        if mod(t, 50) == 0
            waitbar((t/n));
        end
    end
end

% Debug Information
fprintf(fid, 'OLMAR-2(epsilon:%.2f, alpha:%.2f, tc:%.4f), Final return: %.2f\n', ...
    epsilon, alpha, tc, cum_ret);
fprintf(fid, '-------------------------------------\n');
fprintf(1, 'OLMAR-2(epsilon:%.2f, alpha:%.2f, tc:%.4f), Final return: %.2f\n', ...
    epsilon, alpha, tc, cum_ret);
fprintf(1, '-------------------------------------\n');
    if (opts.progress)
        close(progress);
    end
end


function f = cloister(xmin,xmax,ymin,ymax,n)

% CLOISTER  Generates features in a 2D cloister shape.
%   CLOISTER(XMIN,XMAX,YMIN,YMAX,N) generates a 2D cloister in the limits
%   indicated as parameters.
%
%   N is the number of rows and columns; it defaults to N = 9.
%
%   See also THICKCLOISTER.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargin < 5
    n = 9;
end

% Center of cloister
x0 = (xmin+xmax)/2;
y0 = (ymin+ymax)/2;

% Size of cloister
hsize = xmax-xmin;
vsize = ymax-ymin;
tsize = diag([hsize vsize]);

% Integer ordinates of points
outer = (-(n-3)/2 : (n-3)/2);
inner = (-(n-3)/2 : (n-5)/2);

% Outer north coordinates
No = [outer; (n-1)/2*ones(1,numel(outer))];
% Inner north
Ni = [inner ; (n-3)/2*ones(1,numel(inner))];
% East (rotate 90 degrees the North points)
E = [0 -1;1 0] * [No Ni];
% South and West are negatives of N and E respectively.
points = [No Ni E -No -Ni -E];

% Rescale
f = tsize*points/(n-1);

% Move
f(1,:) = f(1,:) + x0;
f(2,:) = f(2,:) + y0;

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function sparse_grid_test013 ( dim_min, dim_max, level_max_min, ...
  level_max_max )

%*****************************************************************************80
%
%% TEST013 tests SPARSE_GRID_F2S_SIZE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 December 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_MIN, the minimum spatial dimension to consider.
%
%    Input, integer DIM_MAX, the maximum spatial dimension to consider.
%
%    Input, integer LEVEL_MAX_MIN, the minimum value of LEVEL_MAX to consider.
%
%    Input, integer LEVEL_MAX_MAX, the maximum value of LEVEL_MAX to consider.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST013\n' );
  fprintf ( 1, '  SPARSE_GRID_F2S_SIZE returns the number of\n' );
  fprintf ( 1, '  distinct points in a sparse grid made up of all \n' );
  fprintf ( 1, '  product grids formed from Fejer Type 2 Slow \n' );
  fprintf ( 1, '  quadrature rules.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The sparse grid is the sum of all product grids\n' );
  fprintf ( 1, '  of order LEVEL, with\n' );
  fprintf ( 1, '    0 <= LEVEL <= LEVEL_MAX.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEVEL is the sum of the levels of the 1D rules,\n' );
  fprintf ( 1, '  the order of the 1D rule is 2^(LEVEL+1) - 1,\n' );
  fprintf ( 1, '  the region is [-1,1]^DIM_NUM.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  For this kind of rule, there is complete nesting,\n' );
  fprintf ( 1, '  that is, a sparse grid of a given level includes\n' );
  fprintf ( 1, '  ALL the points on grids of lower levels.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   DIM: ' );

  for dim_num = dim_min : dim_max
    fprintf ( 1, '  %10d', dim_num );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   LEVEL_MAX\n' );
  fprintf ( 1, '\n' );

  for level_max = level_max_min : level_max_max
    fprintf ( 1, '    %4d', level_max );
    for dim_num = dim_min : dim_max
      point_num = sparse_grid_f2s_size ( dim_num, level_max );
      fprintf ( 1, '  %10d', point_num );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function R = quat2rot(h)

% Uso: R = quat2rot(h)
%
% costruisce la matrice di rotazione R
% a partire dal quaternione h corrispondente
%
% From Luca Carlone's
% https://bitbucket.org/lucacarlone/pgo3d-duality-opencode/src/ebb6e1b8cebaad7f2aaf581b1d0c0bad737faebb/lib/quat2rot.m?at=master&fileviewer=file-view-default

s(4)=h(1);
s(1)=h(2);
s(2)=h(3);
s(3)=h(4);

R(1,1)=s(1)^2-s(2)^2-s(3)^2+s(4)^2;
R(1,2)=2*(s(1)*s(2)-s(3)*s(4));
R(1,3)=2*(s(1)*s(3)+s(2)*s(4));

R(2,1)=2*(s(1)*s(2)+s(3)*s(4));
R(2,2)=-s(1)^2+s(2)^2-s(3)^2+s(4)^2;
R(2,3)=2*(s(2)*s(3)-s(1)*s(4));

R(3,1)=2*(s(1)*s(3)-s(2)*s(4));
R(3,2)=2*(s(2)*s(3)+s(1)*s(4));
R(3,3)=-s(1)^2-s(2)^2+s(3)^2+s(4)^2;


% CANFIS-ART Feed-Forward operation.
function y = canfis_art_forward(x,u2,v2,gamma,ThetaL4)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%        								 							     				                                       %
%   			                 	NETWORK FUNCTIONALITY SECTION					   %
%      																					                                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
NumInVars    = size(x,1);
NumInTerms = size(u2,2);
NumRules     = NumInTerms;

% LAYER 1 - MF NODES
% Each node in this layer acts as a one-dimensional membership function.
 In1mat = x*ones(1,NumInTerms);

 % Matrix containing a copy of Out1 for each Layer 2 Term Node.
 Out1 = 1 - g(In1mat-v2,gamma) - g(u2-In1mat,gamma);

% LAYER 2 - PRECONDITION MATCHING OF FUZZY LOGIC RULES
% NumRules == NumInTerms
 Out2 = prod(Out1);
 S_2 = sum(Out2);

% LAYER 3 - NORMALIZATION NODES - All Node Activity Adds-up to unity.
if S_2~=0
     Out3 = Out2/S_2;
end

% LAYERS 4 - 5: CONSEQUENT NODES - SUMMING NODE
 Aux1 = [x; 1]*Out3;

 % New Input Training Data shaped as a column vector.
 a = reshape(Aux1,(NumInVars+1)*NumRules,1);
 y = ThetaL4'*a;


function y = vl_dsigmoid(x)
% VL_DSIGMOID  Derivative of the sigmoid function
%   Y = VL_DSIGMOID(X) returns the derivative of VL_SIGMOID(X). This is
%   calculated as - VL_SIGMOID(X) * (1 - VL_SIGMOID(X)).
%
%   See also: VL_SIGMOID(X), VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% 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).

t = vl_sigmoid(x) ;
y = t .* (1 - t) ;


% reachableForwardRel
%
% A script for computing the forward reachable set, using an relative
% coordinate system. It seems to give roughly reasonable reasonable
% results, but there seems to be some numerical dissapation during the run.
% For example, points that we know are stable appear to be unstable, which
% I think is due to the feature size of the reachable set approaching the
% grid-spacing. This will occur for any grid spacing, since it seems like
% the reachable set becomes an very long and thin ellipsoid-like shape.
%

nReach = 40;  %Number of times at which to display level sets
if (tSpan(2)~=0), error('tSpan(2) must be equal to 0 for reachability calculations!'); end;
% tReach = linspace(tSpan(1),tSpan(2),nReach);
 tReach = linspace(tSpan(1),-1.5,nReach);

radius = 0.3;

nGrid = 400;  %Discritization of the state space in each dimension
grid.dim = 2;  %Two-dimensional problem
grid.min = 8*radius*[-1;-1];
grid.max = 8*radius*[1;1];
grid.dx = (1/(nGrid-1))*(grid.max-grid.min);
grid.bdry = @addGhostExtrapolate;
grid = processGrid(grid);

% Create initial conditions (a circle around the target)
center = [0;0];   %We're working in relative coordinates here!
data = shapeSphere(grid, center, radius); %Initialize level-set function
levelSet = 0;

% Set up the level-set problem:
schemeFunc = @termConvection;
schemeData.velocity = @reachableDynamicsRel;
schemeData.grid = grid;

schemeData.fit.x = xFit;
schemeData.fit.y = yFit;
schemeData.fit.u = uFit;
schemeData.fit.kx = kxFit;
schemeData.fit.ky = kyFit;

% Set up time approximation scheme.
integratorOptions = odeCFLset('factorCFL', 0.5, 'stats', 'on');

% Choose approximations at appropriate level of accuracy.
accuracy = 'medium';
switch(accuracy)
    case 'low'
        schemeData.derivFunc = @upwindFirstFirst;
        integratorFunc = @odeCFL1;
    case 'medium'
        schemeData.derivFunc = @upwindFirstENO2;
        integratorFunc = @odeCFL2;
    case 'high'
        schemeData.derivFunc = @upwindFirstENO3;
        integratorFunc = @odeCFL3;
    case 'veryHigh'
        schemeData.derivFunc = @upwindFirstWENO5;
        integratorFunc = @odeCFL3;
    otherwise
        error('Unknown accuracy level %s', accuracy);
end

%Data structure for storing the reachable set, as contour lines:
Reach(nReach).C = [];

%Store the initial contour line:
Reach(1).C = getContour(grid,data,levelSet);

%Run the backwards reachability calculation:
startTime = cputime;
for idx = 1:(nReach-1)

    %Display data to user:
    figure(12); clf; contour(grid.xs{1},grid.xs{2}, data);

    % Reshape data array into column vector for ode solver call.
    y0 = data(:);

    % How far to step?
    tSpanReach = [tReach(idx), tReach(idx+1)];

    % Take a timestep.
    [t, y] = feval(integratorFunc, schemeFunc, tSpanReach, y0,...
        integratorOptions, schemeData);
    if t~=tSpanReach(2)
        error('Something funny happened!')
    end

    % Get back the correctly shaped data array
    data = reshape(y, grid.shape);

    if min(min(data)) > 0, break;   end

    %Store the level curve:
    Reach(idx+1).C = getContour(grid,data,levelSet);

end

%Plot the results:
figure(7); clf; hold on;
colors = jet(nReach);
xNomReach = polyval(xFit,tReach);
yNomReach = polyval(yFit,tReach);
for i=1:nReach
    C = Reach(i).C;
    if ~isempty(Reach(i).C)
        for j=1:length(C)
            x = C(j).x + xNomReach(i);
            y = C(j).y +yNomReach(i);
            plot(x,y,'LineWidth',3,'color',colors(i,:));
        end
    end
end
plot(xNomReach,yNomReach,'k.','MarkerSize',20);
plot(xSol,ySol,'k')
xlabel('x');
ylabel('y');
zlabel('t');
title('Forward reachable set')


%SE3 Lift SE(2) transform to SE(3)
%
% T3 = SE3(T2) returns a homogeneous transform (4x4) that represents
% the same X,Y translation and Z rotation as does T2 (3x3).
%
% See also SE2, SE2.SE3, TRANSL, ROTX.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math
function T = se3(x)

    if all(size(x) == [3 3])
        T = [x(1:2,1:2) [0 0]' x(1:2,3); 0 0 1 0; 0 0 0 1];
     end


% [PYR, INDICES, STEERMTX, HARMONICS] = buildSFpyr(IM, HEIGHT, ORDER, TWIDTH)
%
% Construct a steerable pyramid on matrix IM, in the Fourier domain.
% This is similar to buildSpyr, except that:
%
%    + Reconstruction is exact (within floating point errors)
%    + It can produce any number of orientation bands.
%    - Typically slower, especially for non-power-of-two sizes.
%    - Boundary-handling is circular.
%
% HEIGHT (optional) specifies the number of pyramid levels to build. Default
% is maxPyrHt(size(IM),size(FILT));
%
% The squared radial functions tile the Fourier plane, with a raised-cosine
% falloff.  Angular functions are cos(theta-k\pi/(K+1))^K, where K is
% the ORDER (one less than the number of orientation bands, default= 3).
%
% TWIDTH is the width of the transition region of the radial lowpass
% function, in octaves (default = 1, which gives a raised cosine for
% the bandpass filters).
%
% PYR is a vector containing the N pyramid subbands, ordered from fine
% to coarse.  INDICES is an Nx2 matrix containing the sizes of
% each subband.  This is compatible with the MatLab Wavelet toolbox.
% See the function STEER for a description of STEERMTX and HARMONICS.

% Eero Simoncelli, 5/97.
% See http://www.cis.upenn.edu/~eero/steerpyr.html for more
% information about the Steerable Pyramid image decomposition.

function [pyr,pind,steermtx,harmonics] = buildSFpyr(im, ht, order, twidth)

%-----------------------------------------------------------------
%% DEFAULTS:

max_ht = floor(log2(min(size(im)))+2);

if (exist('ht') ~= 1)
  ht = max_ht;
else
  if (ht > max_ht)
    error(sprintf('Cannot build pyramid higher than %d levels.',max_ht));
  end
end

if (exist('order') ~= 1)
  order = 3;
elseif ((order > 15)  | (order < 0))
  fprintf(1,'Warning: ORDER must be an integer in the range [0,15]. Truncating.\n');
  order = min(max(order,0),15);
else
  order = round(order);
end
nbands = order+1;

if (exist('twidth') ~= 1)
  twidth = 1;
elseif (twidth <= 0)
  fprintf(1,'Warning: TWIDTH must be positive.  Setting to 1.\n');
  twidth = 1;
end

%-----------------------------------------------------------------
%% Steering stuff:

if (mod((nbands),2) == 0)
  harmonics = [0:(nbands/2)-1]'*2 + 1;
else
  harmonics = [0:(nbands-1)/2]'*2;
end

steermtx = steer2HarmMtx(harmonics, pi*[0:nbands-1]/nbands, 'even');

%-----------------------------------------------------------------

dims = size(im);
ctr = ceil((dims+0.5)/2);

[xramp,yramp] = meshgrid( ([1:dims(2)]-ctr(2))./(dims(2)/2), ...
    ([1:dims(1)]-ctr(1))./(dims(1)/2) );
angle = atan2(yramp,xramp);
log_rad = sqrt(xramp.^2 + yramp.^2);
log_rad(ctr(1),ctr(2)) =  log_rad(ctr(1),ctr(2)-1);
log_rad  = log2(log_rad);

%% Radial transition function (a raised cosine in log-frequency):
[Xrcos,Yrcos] = rcosFn(twidth,(-twidth/2),[0 1]);
Yrcos = sqrt(Yrcos);

YIrcos = sqrt(1.0 - Yrcos.^2);
lo0mask = pointOp(log_rad, YIrcos, Xrcos(1), Xrcos(2)-Xrcos(1), 0);
imdft = fftshift(fft2(im));
lo0dft =  imdft .* lo0mask;

[pyr,pind] = buildSFpyrLevs(lo0dft, log_rad, Xrcos, Yrcos, angle, ht, nbands);

hi0mask = pointOp(log_rad, Yrcos, Xrcos(1), Xrcos(2)-Xrcos(1), 0);
hi0dft =  imdft .* hi0mask;
hi0 = ifft2(ifftshift(hi0dft));

pyr = [real(hi0(:)) ; pyr];
pind = [size(hi0); pind];


function detX = det_2x2(X)

[d1,d2,~] = size(X);
assert(d1==2 && d2==2);

detX = X(1,1,:).*X(2,2,:)-X(1,2,:).*X(2,1,:);


function c = computeDirectionWeights2d4(delta)
%COMPUTEDIRECTIONWEIGHTS2D4 Direction weights for 4 directions in 2D
%
%   C = computeDirectionWeights2d4
%   Returns an array of 4-by-1 values, corresponding to directions:
%   [+1  0]
%   [ 0 +1]
%   [+1 +1]
%   [-1 +1]
%
%   C = computeDirectionWeights2d4(DELTA)
%   With DELTA = [DX DY].
%
%   Example
%   computeDirectionWeights2d4
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-10-18,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% check case of empty argument
if nargin == 0
    delta = [1 1];
end

% angle of the diagonal
theta   = atan2(delta(2), delta(1));

% angular sector for direction 1 ([1 0])
alpha1  = theta;

% angular sector for direction 2 ([0 1])
alpha2  = (pi/2 - theta);

% angular sector for directions 3 and 4 ([1 1] and [-1 1])
alpha34 = pi/4;

% concatenate the different weights
c = [alpha1 alpha2 alpha34 alpha34]' / pi;


function d=distitpf(pf1,pf2,mode)
%DISTITPF calculates the Itakura spectral distance between power spectra D=(PF1,PF2,MODE)
%
% Inputs: PF1,PF2     Power spectra to be compared. Each row represents a power spectrum: the first
%                     and last columns represent the DC and Nyquist terms respectively.
%                     PF1 and PF2 must have the same number of columns.
%
%         MODE        Character string selecting the following options:
%                         'x'  Calculate the full distance matrix from every row of PF1 to every row of PF2
%                         'd'  Calculate only the distance between corresponding rows of PF1 and PF2
%                              The default is 'd' if PF1 and PF2 have the same number of rows otherwise 'x'.
%
% Output: D           If MODE='d' then D is a column vector with the same number of rows as the shorter of PF1 and PF2.
%                     If MODE='x' then D is a matrix with the same number of rows as PF1 and the same number of columns as PF2'.
%
% If ave() denotes the average over +ve and -ve frequency, the Itakura spectral distance is
%
%                               log(ave(pf1/pf2)) - ave(log(pf1/pf2))
%
% The Itakura distance is gain-independent, i.e. distitpf(g*pf1,pf2) is independent of g.

% The Itakura distance can also be calculated directly from AR coefficients; providing np is large
% enough, the values of d0 and d1 in the following will be very similar:
%
%         np=255; d0=distitar(ar1,ar2); d1=distitpf(lpcar2pf(ar1,np),lpcar2pf(ar2,np))
%

% Ref: A.H.Gray Jr and J.D.Markel, "Distance measures for speech processing", IEEE ASSP-24(5): 380-391, Oct 1976
%      L. Rabiner abd B-H Juang, "Fundamentals of Speech Recognition", Section 4.5, Prentice-Hall 1993, ISBN 0-13-015157-2
%      F. Itakura, "Minimum prediction residual principle applied to speech recognition", IEEE ASSP-23: 62-72, 1975

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

[nf1,p2]=size(pf1);
p1=p2-1;
nf2=size(pf2,1);
if nargin<3 | isempty(mode) mode='0'; end
if any(mode=='d') | (mode~='x' & nf1==nf2)
   nx=min(nf1,nf2);
   r=pf1(1:nx,:)./pf2(1:nx,:);
   q=log(r);
   d=log((sum(r(:,2:p1),2)+0.5*(r(:,1)+r(:,p2)))/p1)-(sum(q(:,2:p1),2)+0.5*(q(:,1)+q(:,p2)))/p1;
else
   r=permute(pf1(:,:,ones(1,nf2)),[1 3 2])./permute(pf2(:,:,ones(1,nf1)),[3 1 2]);
   q=log(r);
   d=log((sum(r(:,:,2:p1),3)+0.5*(r(:,:,1)+r(:,:,p2)))/p1)-(sum(q(:,:,2:p1),3)+0.5*(q(:,:,1)+q(:,:,p2)))/p1;
end


 function displace = penalty_displace(offsets, sizes)
%function displace = penalty_displace(offsets, sizes)
%|
%| Convert scalar offsets to vector displacements, i.e.,
%| find d1,d2,... such that d1*1 + d2*n1 + d3*n1*n2 + ... = offset.
%| Example: if offset = n1, then [d1 d2 d3] = [0 1 0]
%| Assumes that d <= floor(n/2) for n > 2
%|
%| in
%|	offsets	[LL]		see penalty_offsets.m
%|	sizes	[1 ndim]
%|
%| out
%|	displace [LL ndim]	[d1 d2 ...] for each offset
%|
%| Copyright 2006-12-6, Jeff Fessler, University of Michigan

if nargin == 1 && streq(offsets, 'test'), penalty_displace_test, return, end
if nargin < 2, ir_usage, end

ndim = numel(sizes);
offsets = offsets(:); % [LL 1]
displace = nan(length(offsets), ndim);

if 1 % simple way that assumes d / n < 1/2, which is fine for n > 2
	if length(sizes) == 2 && sizes(1) == 2 % [2 ny] special case!
		if isequal(offsets, [1 2 3 1]') % [1 nx nx+1 nx-1]
			displace = [1 0; 0 1; 1 1; -1 1];
		elseif isequal(offsets, [1]) % [1]
			displace = [1 0];
			warn 'caution: offset=1 is ambiguous when n1=2'
		elseif isequal(offsets, [2]) % [n1]
			displace = [0 1];
		elseif isequal(offsets, [1 2]') % [2 n2]
			displace = [1 0; 0 1];
		else
			pr offsets
			fail 'not done'
		end

	elseif any(sizes(1:end-1) == 2)
		fail 'not done: ambiguous'

	else
		residual = offsets;
		for id=ndim:-1:1
			nd = prod(sizes(1:id-1));
			displace(:,id) = round(residual / nd);
			residual = residual - displace(:,id) * nd;
		end
		if any(residual ~= 0), fail 'bug', end
	end

else

	half = max(floor(sizes/2), 1); % upper bound on dx is almost half the size
	%half = sizes/2;

	for ll=1:length(offsets)
		offset = double(offsets(ll)); % trick: necessary!
		subval = 0;
		dis = zeros(1,ndim);
		for id=ndim:-1:2
			tmp = offset + sum((half(1:id-1)-1) .* [1 sizes(1:id-2)]);
			tmp = tmp + prod(sizes(1:id)) - subval;
			dis(id) = floor(tmp / prod(sizes(1:id-1))) - sizes(id);
			subval = subval + dis(id) * prod(sizes(1:id-1));
		end
		dis(1) = offset - subval;

%		if ~(all(abs(dis) < half || (dis == 1 && half == 1)))
		if ~all(abs(dis) <= half)
			pr dis
			pr half
			warn 'bug?'
		end
		displace(ll,:) = dis;
	end
end

offset_check = displace * [1 cumprod(sizes(1:end-1))]';
jf_equal(offsets, offset_check)

end % penalty_displace

% penalty_displace_test()
function penalty_displace_test

jf_equal(penalty_displace([], [1]), nan(0,1))
jf_equal(penalty_displace([], [1 1]), nan(0,2))

if 1 % check tiny
	% when nx=ny=2 then [1 nx nx+1 nx-1] = [1 2 3 1] so it is ambiguous!
	nx = 2; ny = 2;
	offsets = [1 nx nx+1 nx-1];
	dd = penalty_displace(offsets, [nx ny]);
	jf_equal(dd, [1 0; 0 1; 1 1; -1 1])

	nx = 2; ny = 3;
	dd = penalty_displace(offsets, [nx ny]);
	jf_equal(dd, [1 0; 0 1; 1 1; -1 1])
end

nx = 3; ny = 3;
offsets = [1 nx nx+1 nx-1];
dd = penalty_displace(offsets, [nx ny]);
jf_equal(dd, [1 0; 0 1; 1 1; -1 1])

nx = 100; ny = 80; % 2d
[ix iy] = ndgrid(-2:2, -2:2);
offsets = col(ix + iy * nx);
dd = penalty_displace(offsets, [nx ny]);
jf_equal(dd, [ix(:) iy(:)])

nx = 10; ny = 8; nz = 7; % 3d
[ix iy iz] = ndgrid(-2:2, -2:2, -2:2);
offsets = col(ix + iy * nx + iz * nx * ny);
dd = penalty_displace(offsets, [nx ny nz]);
jf_equal(dd, [ix(:) iy(:) iz(:)])

dim = [8 9]; % for won
offsets2 = penalty_offsets('2d:hvd', dim);
dd2 = penalty_displace(offsets2, dim);

for nz=1:3
	dim3 = [dim nz];
	dd3 = penalty_displace(offsets2, dim3);
	jf_equal(dd3, [dd2 zeros(4,1)])
end

if 1
	dim3 = [10 20 30];
	offsets = penalty_offsets('3d:26', dim3);
	[[1:13]', penalty_displace(offsets, dim3)]
end

end % penalty_displace_test()


%% Steering Control Simple
% Steering Control of Autonomous Vehicles in Obstacle Avoidance Maneuvers.
%
% <<SteeringControlSimple.gif>>
%
% <html>
% <script src='https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML'></script>
% </html>
%
%% Vehicle model
% *Nonlinear model*
%
% State vector
%
% \[ {\bf x} = \left[ \begin{array}{c} {\rm x}_1 \\ {\rm x}_2 \\ {\rm x}_3 \\ {\rm x}_4 \\ {\rm x}_5 \\ {\rm x}_6 \end{array} \right] = \left[ \begin{array}{c} x \\ y \\ \psi \\ v_{\rm T} \\ \alpha_{\rm T} \\ \dot{\psi} \end{array} \right] \]
%
% State equations
%
% \[ \dot{{\rm x}}_1 = {\rm x}_4 \cos \left( {\rm x}_3 + {\rm x}_5 \right) \]
%
% \[ \dot{{\rm x}}_2 = {\rm x}_4 \sin \left( {\rm x}_3 + {\rm x}_5 \right) \]
%
% \[ \dot{{\rm x}}_3 = {\rm x}_6 \]
%
% \[ \dot{{\rm x}}_4 = \frac{F_{y,{\rm F}} \sin \left( {\rm x}_5 - \delta \right) + F_{y,{\rm R}} \sin {\rm x}_5}{m_{T}} \]
%
% \[ \dot{{\rm x}}_5 = \frac{F_{y,{\rm F}} \cos \left( {\rm x}_5 - \delta \right) + F_{y,{\rm R}} \cos \alpha_{\rm T} - m_{T} {\rm x}_4 {\rm x}_6}{m_{T} {\rm x}_4} \]
%
% \[ \dot{{\rm x}}_6 = \frac{F_{y,{\rm F}} a \cos \delta - F_{y,{\rm R}} b}{I_{T}} \]
%
% Slip angles
%
% \[ \alpha_{\rm F} = \arctan \left( \frac{v_{\rm T} \sin \alpha_{\rm T} + a \dot{\psi}}{ v_{\rm T} \cos \alpha_{\rm T}} \right) - \delta \]
%
% \[ \alpha_{\rm R} = \arctan \left( \frac{v_{\rm T} \sin \alpha_{\rm T} - b \dot{\psi}}{ v_{\rm T} \cos \alpha_{\rm T}} \right) \]
%
%
% *Linear model*
%
% \[ \dot{x} = v_{\rm T} \]
%
% \[ \dot{y} = v_{{\rm T},0} \left( \psi + \alpha_{{\rm T}}\right) \]
%
% \[ \dot{\psi} = \dot{\psi} \]
%
% \[ \dot{v}_{\rm T} = 0 \]
%
% \[ \dot{\alpha}_{\rm T} = \frac{F_{y,{\rm F}} + F_{y,{\rm R}}}{m_{T} v_{{\rm T},0}} - \dot{\psi} \]
%
% \[ \ddot{\psi} = \frac{a F_{y,{\rm F}} -  b F_{y,{\rm R}}}{I_{T}} \]
%
% Neglecting equations of \(x\) and \(v_T\)
%
%
% \[ \left[ \begin{array}{c} \dot{y} \\ \dot{\psi} \\ \dot{\alpha}_T \\ \ddot{\psi} \end{array} \right] = \left[ \begin{array}{cccc} 0 & v_{T,0} & v_{T,0} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & -\frac{K_F+K_R}{m_T v_{T,0}} & - \frac{m_T v_{T,0} + \frac{a K_F - b K_R}{v_{T,0}}}{m_T v_{T,0}} \\ 0 & 0 & - \frac{a K_F - b K_R}{I_T} & - \frac{a^2 K_F + b^2 K_R}{I_T v_{T,0}} \end{array} \right] \left[ \begin{array}{c} y \\ \psi \\ \alpha_T \\ \dot{\psi} \end{array} \right] + \left[ \begin{array}{c} 0 \\ 0 \\ \frac{K_F}{m_T v_{T,0}} \\ \frac{a K_F}{I_T}  \end{array} \right] \delta \]
%
% Slip angles
%
% \[ \alpha_{{\rm F},lin} = \alpha_{{\rm T}} + \frac{a}{v_{{\rm T},0}} \dot{\psi} - \delta \]
%
% \[ \alpha_{{\rm F},lin} = \alpha_{{\rm T}} - \frac{b}{v_{{\rm T},0}} \dot{\psi} \]
%
%% Tire model
%
% Typical characteristic curve and slip angle definition
%
% <<../../../docs/illustrations/CurvaCaracteristica.svg>>
%
% *Pacejka*
%
% \[ F_{y} = D \sin \left[ C \arctan{B \alpha - E( B \alpha -\arctan(B \alpha))} \right] \]
%
% *Linear*
%
% \[ F_ y = K \alpha \]
%

clear ; close all ; clc

slipAngle = (0:0.1:15)*pi/180;         % Slip angle [rad]

% Pacejka tire parameters
a0  = 1;
a1  = 0;
a2  = 800;
a3  = 10000;
a4  = 50;
a5  = 0;
a6  = 0;
a7  = -1;
a8  = 0;
a9  = 0;
a10 = 0;
a11 = 0;
a12 = 0;
a13 = 0;

TirePac = VehicleDynamicsLateral.TirePacejka();

Fz          = 4e+03;
camber      = 0;
TirePac.a0  = a0;
TirePac.a1  = a1;
TirePac.a2  = a2;
TirePac.a3  = a3;
TirePac.a4  = a4;
TirePac.a5  = a5;
TirePac.a6  = a6;
TirePac.a7  = a7;
TirePac.a8  = a8;
TirePac.a9  = a9;
TirePac.a10 = a10;
TirePac.a11 = a11;
TirePac.a12 = a12;
TirePac.a13 = a13;

muy0    = TirePac.a1 * Fz/1000 + TirePac.a2;
D       = muy0 * Fz/1000;
BCD     = TirePac.a3 * sin(2 * atan(Fz/1000/TirePac.a4))*(1-TirePac.a5 * abs(camber));

% Pneu linear equivalente

Ktire   = BCD * 180/pi;

TireLin     = VehicleDynamicsLateral.TireLinear();
TireLin.k   = Ktire;

% Lateral force
FyPac = TirePac.Characteristic(slipAngle, Fz, muy0/1000);
FyLin = TireLin.Characteristic(slipAngle);

% Graphics
g = VehicleDynamicsLateral.Graphics(TirePac);

%%
% Comparison of tire models

figure(1)
ax = gca;
set(ax, 'NextPlot', 'add', 'Box', 'on', 'XGrid', 'on', 'YGrid', 'on')
p = plot(slipAngle * 180/pi,-FyLin, 'Color', 'g', 'Marker', 's', 'MarkerFaceColor', 'g', 'MarkeredgeColor', 'k', 'MarkerSize', 7);
g.changeMarker(p, 10);
p = plot(slipAngle * 180/pi,-FyPac, 'Color', 'r', 'Marker', 'o', 'MarkerFaceColor', 'r', 'MarkeredgeColor', 'k', 'MarkerSize', 7);
g.changeMarker(p, 10);
xlabel('\(\alpha\) [grau]', 'Interpreter', 'Latex')
ylabel('\(F_y\) [N]', 'Interpreter', 'Latex')
l = legend('Linear', 'Pacejka');
set(l, 'Interpreter', 'Latex', 'Location', 'NorthWest')

%
%% Plant model
% Nonlinear vehicle + Pacejka tire

% Choosing vehicle
% System = VehicleDynamicsLateral.VehicleSimpleLinear();
VehiclePlant = VehicleDynamicsLateral.VehicleSimpleNonlinear();
% Defining vehicle parameters
VehiclePlant.mF0 = 700;
VehiclePlant.mR0 = 600;
VehiclePlant.IT = 10000;
VehiclePlant.lT = 3.5;
VehiclePlant.nF = 1;
VehiclePlant.nR = 1;
VehiclePlant.wT = 1.8;
VehiclePlant.muy = 1;
VehiclePlant.tire = TirePac;
VehiclePlant.deltaf = @ControlLaw;

disp(VehiclePlant)

% Choosing simulation
T       = 9;                     % Total simulation time             [s]
resol   = 500;                    % Resolution
TSPAN   = 0:T/resol:T;            % Time span                         [s]

simulator = VehicleDynamicsLateral.Simulator(VehiclePlant, TSPAN);
simulator.V0 = 16.7;

%% Controller design
%
% *Vehicle parameters*
mT  = 1300;
IT  = 10000;
a   = 1.6154;
b   = 1.8846;
vT0 = 16.7;
KF  = Ktire;
KR  = Ktire;

%%
% *Linear system*
%

A = [      0   vT0            vT0                         0                       ;...
           0    0              0                          1                       ;...
           0    0      -(KF+KR)/(mT*vT0)  -(mT*vT0+(a*KF-b*KR)/(vT0))/(mT*vT0)    ;...
           0    0      -(a*KF-b*KR)/IT    -(a^2*KF+b^2*KR)/(IT*vT0)               ];

B = [   0                  ;...
        0                  ;...
        KF/(mT*vT0)        ;...
        a*KF/IT            ];

C = [1 0 0 0];

%%
% A
disp(A)
%%
% B
disp(B)
%%
% C
disp(C)

%%
% *LQR design*
%

Q = [   0.3 0 0 0 ;...
        0   1 0 0 ;...
        0   0 1 0 ;...
        0   0 0 1 ];

%%
% Q

disp(Q)

R = 1;

%%
% R

disp(R)

Klqr = lqr(A,B,Q,R);

%%
% Klqr

disp(Klqr)

%%
% *Pole placement design*
%

polos = [-6 -6.3 -6.7 -7];

Kplace = place(A,B,polos);

%%
% Kplace

disp(Kplace)

%%
% Control law
%
% \[\delta = - {\bf K} {\bf z} + K_1 r \]
%
%% Double Lane Change Maneuver

% Simulation
simulator.Simulate();

g = VehicleDynamicsLateral.Graphics(simulator);
g.Frame();

    % Adding the double lane change track to the frame figure
    carWidth = 2;
    LaneOffset = 3.5;

    section1width = 1.1*carWidth + 0.25;
    section3width = 1.2*carWidth + 0.25;
    section5width = 1.3*carWidth + 0.25;

    section1Inf = -section1width/2;
    section1Sup = section1width/2;

    section3Inf = section1Inf+LaneOffset;
    section3Sup = section3Inf+section3width;
    section3Center = (section3Inf+section3Sup)/2;

    section5Inf = -section5width/2;
    section5Sup = section5width/2;

    % Section 1
    plot([0 15],[section1Inf section1Inf],'k')            % linha inferior
    plot([0 15],[section1Sup section1Sup],'k')            % linha superior
    plot([0 15],[0 0],'k--')                % linha central
    % Section 2
    plot([15 45],[0 section3Center],'k--')  % linha central
    % Section 3
    plot([45 70],[section3Inf section3Inf],'k')        % linha inferior
    plot([45 70],[section3Sup section3Sup],'k')        % linha superior
    plot([45 70],[section3Center section3Center],'k--')               % linha central
    % Section 4
    plot([70 95],[section3Center 0],'k--')
    % Section 5
    plot([95 130],[section5Inf section5Inf],'k')
    plot([95 130],[section5Sup section5Sup],'k')
    plot([95 130],[0 0],'k--')

g.Animation();
g.Animation();
% g.Animation('html/SteeringControlSimple');       % Uncomment to save animation gif

% Retrieving states
XT      = simulator.XT;
YT      = simulator.YT;
PSI     = simulator.PSI;
VEL     = simulator.VEL;
ALPHAT  = simulator.ALPHAT;
dPSI    = simulator.dPSI;

x = [YT PSI ALPHAT dPSI];

u = zeros(length(TSPAN),1);
output = zeros(length(TSPAN),1);

LateralDisp = 3.6;

for ii = 1:length(TSPAN)
    if XT(ii) <= 15
        r = 0;
    end
    if XT(ii) > 15 && XT(ii) <= 70
        r = LateralDisp;
    end
    if XT(ii) > 70
        r = 0;
    end

    u(ii) = - Kplace*x(ii,:)' + Kplace(1)*r;

    % Saturation
    if abs(u(ii)) < 42*pi/180
        output(ii) = u(ii);
    else
        output(ii) = sign(u(ii))*42*pi/180;
    end
end

% States
f = figure;
set(f,'PaperUnits','centimeters')
set(f,'PaperPosition',[0 0 8.9 5])
PaperPos = get(f,'PaperPosition');
set(f,'PaperSize',PaperPos(3:4))
hold on; box on; grid on
plot(TSPAN,YT,'r')
plot(TSPAN,PSI,'g')
plot(TSPAN,ALPHAT,'b')
plot(TSPAN,dPSI,'c')
xlabel('Time [s]')
ylabel('States')
l = legend('\(y\)','\(\psi\)','\(\alpha_T\)','\(\dot{\psi}\)');
set(l,'Interpreter','Latex','Location','NorthEast')

% Steering input
f = figure;
set(f,'PaperUnits','centimeters')
set(f,'PaperPosition',[0 0 8.9 3.5])
PaperPos = get(f,'PaperPosition');
set(f,'PaperSize',PaperPos(3:4))
hold on; box on; grid on
plot(TSPAN,output*180/pi,'k')
xlabel('Time [s]')
y = ylabel('\(\delta [deg]\)');
set(y,'Interpreter','Latex')

%% See Also
%
% <../../../index.html Home>
%


function [R2,out]=rsquared(x,y,kmax,method,h)

%RSQUARED calculates the R-squared value of the robust or classical PCR/PLS analysis. This function
% is used in rpcr.m and rsimpls.m.
%
% The Robust R-squared is described in:
%    Hubert, M., Verboven, S. (2003),
%    "A robust PCR method for high-dimensional regressors",
%    Journal of Chemometrics, 17, 438-452.
%
% The required input arguments
%    x    : the explanatory variables
%    y    : the response variables
%    kmax : the number of components to choose in the ROBPCA method
%  method : the method for which the R2 has to be computed. It can be 'RSIMPLS' or 'RPCR'.
%
%Optional input arguments
%    h    : the quantile used in RPCR/RSIMPLS
%
% I/O: R2=rsquared(x,y,10)
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by S.Verboven on 31-10-2002
% Last updated on 18-02-2003

if nargin<4
    error('Missing input arguments')
end
[n,p]=size(x);
[n,q]=size(y);
cutoffWeights = sqrt(chi2inv(0.975,q));

if nargin<5
    h=floor(0.75*n);
end

count=1;
if q>1
    while count*q+q+(q*(q+1)/2) <= h
        count=count+1;
    end
else
    while count+2<h
        count=count+1;
    end
end
ktot=max(1,min(count-1,kmax));
if count-1<kmax
    disp(['Warning (rpcr): The maximal number of components is set to ', num2str(ktot),' to avoid overfitting.'])
end
weight=zeros(n,1);
bound=floor((n+ktot+q)/2);
h=max(h,bound);
switch method
case 'RPCR'
    outcv = cvRpcr(x,y,ktot,0,h);
case 'RSIMPLS'
    outcv = cvRsimpls(x,y,ktot,0,h);
end
R2 = outcv.R2;
W = outcv.outWeights.w_min;
rss = outcv.rss;

% place the two figures next to each other in the middle of the screen
bdwidth=5;
topbdwidth=30;
set(0,'Units','pixels');
scnsize=get(0,'ScreenSize');
pos1=[bdwidth, 1/3*scnsize(4)+bdwidth, scnsize(3)/2-2*bdwidth, scnsize(4)/2-(topbdwidth+bdwidth)];
pos2=[pos1(1)+scnsize(3)/2, pos1(2), pos1(3), pos1(4)];

figure('Position',pos1)
set(gcf,'Name', 'R-squared curve', 'NumberTitle', 'off');
plot(1:ktot,R2,'o-');
xlabel('Number of components');
ylabel('R2');
title(method)
ylabel('Robust R^2-value');
set(gca,'XTick',1:1:ktot)

figure('Position',pos2)
set(gcf,'Name', 'Square Root of Residual Sum of Squares curve', 'NumberTitle', 'off');
plot(1:ktot,sqrt(rss),'o-');
xlabel('Number of components');
title(method)
ylabel('Square Root of Robust RSS-value');
set(gca,'XTick',1:1:ktot)

k=input('How many components would you like to retain? ');
out.k=k;
out.weights=W(:,1:k);
out.rss = rss;

% closing the figures.
% close
% close


function b = r83p_ml ( n, a_lu, x, job )

%*****************************************************************************80
%
%% R83P_ML computes A * x or x * A, where A has been factored by R83P_FA.
%
%  Discussion:
%
%    The R83P storage format stores a periodic tridiagonal matrix as
%    a 3 by N array, in which each row corresponds to a diagonal, and
%    column locations are preserved.  The matrix value
%    A(1,N) is stored as the array entry A(3,N), and the matrix value
%    A(N,1) is stored as the array entry A(1,1).
%
%  Example:
%
%    Here is how a R83P matrix of order 5 would be stored:
%
%      A51 A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54 A15
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be at least 3.
%
%    Input, real A_LU(3,N), the factors computed by R83P_FA.
%
%    Input, real X(N), the vector to be multiplied by the matrix.
%
%    Input, integer JOB, indicates what product should be computed.
%    0, compute A * x.
%    nonzero, compute A' * x.
%
%    Output, real B(N), the result of the multiplication.
%

%
%  Multiply A(1:N-1,1:N-1) and X(1:N-1).
%
  b(1:n-1) = r83_np_ml ( n-1, a_lu(1:3,1:n-1), x, job );
%
%  Add terms from the border.
%
  if ( job == 0 )
    b(1) = b(1) + a_lu(3,n) * x(n);
    b(n-1) = b(n-1) + a_lu(1,n) * x(n);
    b(n) = a_lu(1,1) * x(1) + a_lu(3,n-1) * x(n-1) + a_lu(2,n) * x(n);
  else
    b(1) = b(1) + a_lu(1,1) * x(n);
    b(n-1) = b(n-1) + a_lu(3,n-1) * x(n);
    b(n) = a_lu(3,n) * x(1) + a_lu(1,n) * x(n-1) + a_lu(2,n) * x(n);
  end

  return
end


function [X,Y,Z] = adjacency_plot_und(aij,coor)
%ADJACENCY_PLOT_UND     Quick visualization tool
%
%   [X,Y,Z] = ADJACENCY_PLOT(AIJ,COOR) takes adjacency matrix AIJ and node
%   spatial coordinates COOR and generates three vectors that can be used
%   for quickly plotting the edges in AIJ. If no coordinates are specified,
%   then each node is assigned a position on a circle. COOR can, in
%   general, be 2D or 3D.
%
%   Example:
%
%   >> load AIJ;                                % load your adjacency matrix
%   >> load COOR;                               % load 3D coordinates for each node
%   >> [x,y,z] = adjacency_plot_und(AIJ,COOR);  % call function
%   >> plot3(x,y,z);                            % plots network as a single line object
%
%   If COOR were 2D, the PLOT3 command changes to a PLOT command.
%
%   NOTE: This function is similar to MATLAB's GPLOT command.
%
%   Richard Betzel, Indiana University, 2013

n = length(aij);
if nargin < 2
    coor = zeros(n,2);
    for i = 1:n
        coor(i,:) = [cos(2*pi*(i - 1)./n), sin(2*pi*(i - 1)./n)];
    end
end

[i,j] = find(triu(aij,1));
[~, p] = sort(max(i,j));
i = i(p);
j = j(p);

X = [ coor(i,1) coor(j,1)]';
Y = [ coor(i,2) coor(j,2)]';
if size(coor,2) == 3
    Z = [ coor(i,3) coor(j,3)]';
end
if isfloat(coor) || nargout ~= 0
    X = [X; NaN(size(i))'];
    Y = [Y; NaN(size(i))'];
    if size(coor,2) == 3
        Z = [Z; NaN(size(i))'];
    end
end

X = X(:);
Y = Y(:);
if size(coor,2) == 3
    Z = Z(:);
end


function g = p40_g ( n, x )

%*****************************************************************************80
%
%% P40_G evaluates the gradient for problem 40.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  g = zeros ( n, 1 );

  g(1) = 2.0 * x(1) + 0.9 * pi * sin ( 3.0 * pi * x(1) );
  g(2) = 4.0 * x(2) - 4.0 * pi * sin ( 4.0 * pi * x(2) );

  return
end


function [iHi,iLo,iCr] = findextrema(x)
%FINDEXTREMA find indices of local extrema and zero-crossings.
%   [IMAX,IMIN,ICRS] = FINDEXTREMA(X) returns the indices of local maxima
%   in IMAX, minima in IMIN and zero-crossing in ICRS for input vector X.
%
%   Example:
%       x = [0 1 0 0 -1 -1 -2 -2 1 0];
%       [i1,i2,i3] = findextrema(x)
%       % Returns:
%       %   i1 = 2 9
%       %   i2 = 8
%       %   i3 = 1 4 8 10
%
%   See also FIND.

% Siyi Deng; 05-29-2009;
% sdeng@uci.edu; UCI HNL;

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

%% Generate Plot for demo;
% x = [1 2 1 3 3 3 3 4 4 4 4 3 3 3 2 2 2 1 3 3 2 2 6 6 6 5 5 4 4 4 1 1 2]-3;
% figure; plot(x); set(gca,'xtick',0:numel(x)+2); ylim([-4 4])
% [iP,iT,iC] = findextrema(x),
% hold on; plot(iP,x(iP),'r*'); plot(iT,x(iT),'g*'); plot(iC,x(iC),'ks');
% legend({'Data','Peak','Trough','Zero-Crossing'},'location','NorthWest');

%% Code starts here;
if ~isvector(x), error('Input must be a vector;'); end
dx = diff(x);
gz = dx > 0;
lz = dx < 0;
ez = dx == 0;
hi = gz(1:end-1) & lz(2:end);
hasCorner = any(ez);
if hasCorner
    cornerVec = double(gz(1:end-1) & ez(2:end))+...
        double(ez(1:end-1) & lz(2:end)).*2+...
        double(ez(1:end-1) & gz(2:end)).*110+...
        double(lz(1:end-1) & ez(2:end)).*100;
    cornerLoc = find(cornerVec);
    cornerType = diff(cornerVec(cornerLoc));
    n = find(cornerType == 1); % plateu;
    hi(ceil((cornerLoc(n+1)+cornerLoc(n))/2)) = true;
end
iHi = find(hi)+1;
if nargout > 1
    lo = gz(2:end) & lz(1:end-1);
    if hasCorner
        u = find(cornerType == 10); % valley;
        lo(ceil((cornerLoc(u+1)+cornerLoc(u))/2)) = true;
    end
    iLo = find(lo)+1;
end
if nargout > 2
    xc = (x(1:end-1).*x(2:end)) < 0;
    xz = false(length(x)+2,1);
    xz(2:end-1) = x == 0;
    if any(xz)
        xc(fix((find(~xz(1:end-1) & xz(2:end))+...
            find(~xz(2:end) & xz(1:end-1)))/2)) = true;
    end
    iCr = find(xc);
end

end % FINDEXTREMA;


function linplus_test295 ( )

%*****************************************************************************80
%
%% TEST295 tests R8GE_DILU.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  ncol = 3;
  nrow = 3;
  n = nrow * ncol;
  m = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST295\n' );
  fprintf ( 1, '  For a matrix in general storage,\n' );
  fprintf ( 1, '  R8GE_DILU returns the DILU factors.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix rows M =    %d\n', m );
  fprintf ( 1, '  Matrix columns N = %d\n', n );

  for i = 1 : nrow * ncol
    for j = 1 : nrow * ncol

      if ( i == j )
        a(i,j) = 4.0E+00;
      elseif ( i == j + 1 | i == j - 1 | i == j + nrow | i == j - nrow )
        a(i,j) = -1.0E+00;
      else
        a(i,j) = 0.0E+00;
      end

    end
  end

  r8ge_print ( m, n, a, '  Matrix A:' );
%
%  Compute the incomplete LU factorization.
%
  d = r8ge_dilu ( m, n, a );

  r8vec_print ( m, d, '  DILU factor:' );

  return
end


function [A, B] = mult_myForm(M, N, P, Q, C)
    % return myForm(M, N, C) * myForm(P, Q, C) = myForm(A, B, C);
    % -----------------------------------------------
    % Author: Tiep Vu, thv102@psu.edu, 4/8/2016
    %         (http://www.personal.psu.edu/thv102/)
    % -----------------------------------------------
    if nargin == 0
        d = 10;
        C = 10;
        M = rand(d, d);
        N = rand(d, d);
        P = rand(d, d);
        Q = rand(d, d);

    end
    %%
    A = M*P;
    B = M*Q + N*P + C*N*Q;
    % From here, we can see that if N == 0 => A = M*P; B = M*Q;
    % If Q == 0 => A = M*P; B = N*P.
    % toc;
    if nargin == 0
        M1 = myForm(A, B, C);
        M2 = myForm(M, N, C)*myForm(P, Q, C);
        norm(M1 - M2)
        pause
    end
end


function [ g ] = gsp_design_simoncelli(G, param)
%GSP_DESIGN_SIMONCELLI Create a Simoncelli filterbank
%   Usage: g = gsp_design_simoncelli( G );
%          g = gsp_design_simoncelli( G, param );
%
%   Inputs parameters:
%       G       : Graph structure or lmax
%       param   : Structure of optional parameters
%
%   Outputs parameters:
%       g       : filterbank
%
%   This function creates a parseval filterbank of $2$ filters. The low-pass
%   filter is defined by a function $f_l(x)$:
%
%   ..              /  1                                   if x <= a
%   ..    f_l(x) = |   cos( pi/2 * log(x/a) / log(2) )     if a < x <= 2a
%   ..              \  0                                   if x > 2a
%
%   .. math:: f_{l}=\begin{cases} 1 & \mbox{if }x\leq a\\ \cos\left(\frac{\pi}{2}\frac{\log\left(\frac{x}{2}\right)}{\log(2)}\right) & \mbox{if }a<x\leq2a\\ 0 & \mbox{if }x>2a \end{cases}
%
%   The high pass filter is adaptated to obtain a tight frame.
%
%   This function will compute the maximum eigenvalue of the laplacian. To
%   be more efficient, you can precompute it using::
%
%       G = gsp_estimate_lmax(G);
%
%   Example:::
%
%         G = gsp_sensor(100);
%         G = gsp_estimate_lmax(G);
%         g = gsp_design_simoncelli(G);
%         gsp_plot_filter(G,g);
%         [A,B] = gsp_filterbank_bounds(G,g)
%
%   *param* is an optional structure containing the following fields
%
%   * *param.verbose*: verbosity level. 0 no log - 1 display warnings.
%     (default 1)
%   * *param.a*: see equations above for this parameter. Note that the
%     spectrum is scaled between 0 and 2 (default 2/3).
%

% Author: Nathanael Perraudin, David Shuman
% Date  : 21 June 2014
% Testing: test_filter

if nargin < 2
    param = struct;
end

if ~isfield(param,'verbose'), param.verbose = 1; end
if ~isfield(param,'a'), param.a = 2/3; end

if isstruct(G)
    if ~isfield(G,'lmax')
        if param.verbose
            fprintf('GSP_DESIGN_SIMONCELLI has to compute lmax \n')
        end
        G = gsp_estimate_lmax(G);
    end
   lmax = G.lmax;
else
   lmax = G;
end

a = param.a;

g = cell(2,1);
g{1} = @(x) simoncelli(x*(2/lmax),a);
g{2} = @(x) real(sqrt(1-(simoncelli(x*(2/lmax),a)).^2));

end

function y = simoncelli(val,a)

y = zeros(size(val));

l1 = a;
l2 = 2*a;

r1ind = val >= 0     &    val < l1;
r2ind = val >= l1    &    val < l2;
r3ind = val >= l2;

y(r1ind) = 1;
y(r2ind) = cos(pi/2*log(val(r2ind)/a)/log(2));
y(r3ind) = 0;

end


function lobatto_compute_test ( )

%*****************************************************************************80
%
%% LOBATTO_COMPUTE_TEST tests LOBATTO_COMPUTE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LOBATTO_COMPUTE_TEST\n' );
  fprintf ( 1, '  LOBATTO_COMPUTE computes a Lobatto rule;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '         I      X             W\n' );

  for n = 4 : 3 : 12

    [ x, w ] = lobatto_compute ( n );

    fprintf ( 1, '\n' );
    for i = 1 : n
      fprintf ( 1, '  %8d  %12f  %12f\n', i, x(i), w(i) );
    end

  end

  return
end