classdef KalmanFilterX < FilterX
% KalmanFilterX class
%
% Summary of KalmanFilterX:
% This is a class implementation of a standard Kalman Filter.
%
% KalmanFilterX Properties: (**)
%   + StatePrior - A structure used to store the state prior
%   + StatePrediction - A structure used to store the state prediction
%   + MeasurementPrediction - A structure used to store the measurement prediction
%   + StatePosterior - A structure used to store posterior information
%   + MeasurementList - A (yDim x 1) matrix used to store the received measurement
%   + ControlInput - A (uDim x 1) matrix used to store the last received control input
%   + KalmanGain - A (xDim x yDim) matrix representing the last computed Kalman Gain
%   + Model - An object handle to StateSpaceModelX object
%       + Transition (*)  = Object handle to TransitionModelX SubClass
%       + Measurement (*)  = Object handle to MeasurementModelX SubClass
%       + Control (*)  = Object handle to ControlModelX SubClass
%
%   (*)  Signifies properties necessary to instantiate a class object
%   (**) xDim, yDim and uDim denote the dimentionality of the state, measurement
%        and control vectors respectively.
%
% KalmanFilterX Methods:
%   + KalmanFilterX  - Constructor method
%   + predict        - Performs KF prediction step
%   + update         - Performs KF update step
%
% (+) denotes puplic properties/methods
%
% See also TransitionModelX, MeasurementModelX and ControlModelX template classes

    properties
        StatePrior
        StatePrediction
        MeasurementPrediction
        StatePosterior
        KalmanGain
        ControlInput
    end

    properties (Dependent)
        MeasurementLikelihoods
    end

    properties (Access=protected)
        MeasurementLikelihoods_ = [];
    end

    methods (Access = protected)
        function initialise_(this, config)
            initialise_@FilterX(this,config);
            if (isfield(config,'StatePrior'))
                this.StatePrior = config.StatePrior;
                this.StatePosterior = this.StatePrior;
            end
        end
        % MeasurementList
        function measurementList = setMeasurementList(this, newMeasurementList)
            measurementList = newMeasurementList;
            this.MeasurementLikelihoods_ = [];
        end
        function StatePrior = setStatePrior(this,newStatePrior)
            if(isa(newStatePrior,'GaussianStateX'))
                StatePrior = newStatePrior;
            else
                StatePrior = GaussianStateX(newStatePrior);
            end
        end
        function StatePrediction = setStatePrediction(this,newStatePrediction)
            if(isa(newStatePrediction,'GaussianStateX'))
                StatePrediction = newStatePrediction;
            else
                StatePrediction = GaussianStateX(newStatePrediction.Mean, newStatePrediction.Covar);
            end
        end
        function MeasurementPrediction = setMeasurementPrediction(this,newMeasurementPrediction)
            if(isa(newMeasurementPrediction,'GaussianStateX'))
                MeasurementPrediction = newMeasurementPrediction;
            else
                MeasurementPrediction = GaussianStateX(newMeasurementPrediction.Mean, newMeasurementPrediction.Covar);
            end
        end
        function MeasurementLikelihoods = getMeasurementLikelihoods(this)
            if(isempty(this.MeasurementLikelihoods_))
                this.MeasurementLikelihoods_ =  mvnpdf(this.MeasurementList.Vectors',this.MeasurementPrediction.Mean',this.MeasurementPrediction.Covar)';
            end
            MeasurementLikelihoods = this.MeasurementLikelihoods_;
        end
        function StatePosterior = setStatePosterior(this,newStatePosterior)
            if(isa(newStatePosterior,'GaussianStateX'))
                StatePosterior = newStatePosterior;
            else
                StatePosterior = GaussianStateX(newStatePosterior.Mean, newStatePosterior.Covar);
            end
        end
    end

    methods
        function this = KalmanFilterX(varargin)
        % KalmanFilterX Constructor method
        %
        % Parameters
        % ----------
        % Model: StateSpaceModelX
        %   An object handle to StateSpaceModelX object.
        % StatePrior: struct, optional
        %   A StateX subclass object describing the state prior. If StatePrior
        %   is not a GaussianStateX instance, then it will be converted in
        %   one using the extracted mean and covariance.
        %
        % Usage
        % -----
        % * kf = KalmanFilterX(___,Name,Value) instantiates an object handle,
        %   configured with the options specified by one or more Name,Value
        %   pair arguments.
        %
        %  See also predict, update, smooth.

            % Call SuperClass method
            %this@FilterX(varargin{:});

            if(nargin==0)
                return;
            end

            % First check to see if a structure was received
            if(nargin==1)
                if(isstruct(varargin{1}))
                    config = varargin{1};
                    this.initialise_(config);
                end
                return;
            end

            % Otherwise, fall back to input parser
            parser = inputParser;
            parser.KeepUnmatched = true;
            parser.parse(varargin{:});
            config = parser.Unmatched;
            this.initialise_(config);
        end

        function initialise(this,varargin)
        % initialise Initialise the KalmanFilter with a certain set of
        %   parameters.
        %
        % Parameters
        % ----------
        % Model: StateSpaceModelX
        %   An object handle to StateSpaceModelX object.
        % StatePrior: StateX, optional
        %   A StateX subclass object describing the state prior. If StatePrior
        %   is not a GaussianStateX instance, then it will be converted in
        %   one using the extracted mean and covariance.
        %
        % Usage
        % -----
        % * initialise(kf,___,Name,Value) initialises the KalmanFilterX
        %   object kf with the options specified by one or more Name,Value
        %   pair arguments.
        %
        %  See also predict, update, smooth.

            if(nargin==0)
                error("Not enough input arguments.");
            end

            initialise@FilterX(this);

            % First check to see if a structure was received
            if(nargin==2)
                if(isstruct(varargin{1}))
                    config = varargin{1};
                    this.initialise_(config);
                end
                return;
            end

            % Otherwise, fall back to input parser
            parser = inputParser;
            parser.KeepUnmatched = true;
            parser.parse(varargin{:});
            config = parser.Unmatched;
            this.initialise_(config);
        end

        function [statePrediction, measurementPrediction] = predict(this, varargin)
        % Predict Perform Kalman Filter prediction step
        %
        % Parameters
        % ----------
        % prior: GaussianStateX, optional
        %   The prior state estimate.
        % timestamp: datetime, optional
        %   A timestamp indicating the time at which prediction is
        %   performed.
        %
        % Returns
        % -------
        % GaussianStateX
        %   The generated state prediction
        % GaussianStateX, optional
        %   The generated measurement prediction
        %
        %  See also update, smooth.

            % Predict state and measurement
            statePrediction = this.predictState(varargin{:});
            if nargin>1 && isa(varargin{1},'StateX')
               % Replace a potential prior with the generated prediction
               % before forwarding the arguments to the measurement
               % prediction. Failure to do so will result in errors!!!
               varargin{1} = statePrediction;
            end
            measurementPrediction = this.predictMeasurement(varargin{:});
        end

        function statePrediction = predictState(this,varargin)
        % predictState Perform Kalman Filter state prediction step
        %
        % Usage
        % -----
        % * predictState(this) calculates the predicted system state and covariance.
        %
        % See also update, smooth.

            timestamp = [];
            timestamp_old = [];
            for i = 1:min([2,nargin-1])
                if isa(varargin{i},'StateX')
                    this.StatePosterior = varargin{i};
                    timestamp_old = this.StatePosterior.Timestamp;
                elseif isdatetime(varargin{i})
                    timestamp = varargin{i};
                end
            end

            if isempty(timestamp)
                dt = this.Model.Transition.TimestepDuration;
                timestamp = this.StatePosterior.Timestamp;
            else
                dt = timestamp - timestamp_old;
            end

            % Extract model parameters
            F = this.Model.Transition.matrix(dt);
            Q = this.Model.Transition.covar(dt);
            if(~isempty(this.Model.Control))
                B   = this.Model.Control.feval();
                Qu  = this.Model.Control.covar();
            else
                this.ControlInput   = 0;
                B   = 0;
                Qu  = 0;
            end

            % Perform state prediction
            [statePredictionMean, statePredictionCovar] = ...
                this.predictState_(this.StatePosterior.Mean, this.StatePosterior.Covar, F, Q, this.ControlInput, B, Qu);

            statePrediction = GaussianStateX(statePredictionMean, statePredictionCovar, timestamp);
            this.StatePrediction = statePrediction;
        end

        function measurementPrediction = predictMeasurement(this, varargin)
        % PREDICTOBS Perform Kalman Filter measurement prediction step
        %
        % Usage
        % -----
        % * predict(this) calculates the predicted measurement,
        %   as well as the associated uncertainty covariances.
        %
        % More details
        % ------------
        % * KalmanFilterX uses the Model class property, which should be an
        %   instance of the TrackingX.Models.StateSpaceModel class, in order
        %   to extract information regarding the underlying state-space model.
        % * State prediction is performed using the Model.Transition property,
        %   which must be a subclass of TrackingX.Abstract.TransitionModel and
        %   provide the following interface functions:
        %   - Model.Transition.feval(): Returns the model transition matrix
        %   - Model.Transition.covariance(): Returns the process noise covariance
        % * Measurement prediction and innovation covariance calculation is
        %   performed usinf the Model.Measurement class property, which should be
        %   a subclass of TrackingX.Abstract.TransitionModel and provide the
        %   following interface functions:
        %   - Model.Measurement.heval(): Returns the model measurement matrix
        %   - Model.Measurement.covariance(): Returns the measurement noise covariance
        %
        % See also update, smooth.

            if nargin>1
                this.StatePrediction = varargin{1};
            end

            % Extract model parameters
            H = this.Model.Measurement.feval();
            R = this.Model.Measurement.covar();

            % Perform prediction
            [measurementPredictionMean, measurementPredictionCovar, this.KalmanGain] = ...
                this.predictMeasurement_(this.StatePrediction.Mean, this.StatePrediction.Covar, H, R);

            measurementPrediction = GaussianStateX(measurementPredictionMean,...
                                                   measurementPredictionCovar,...
                                                   this.StatePrediction.Timestamp);
            this.MeasurementPrediction = measurementPrediction;
        end

        function posterior = update(this, varargin)
        % UPDATE Perform Kalman Filter update step
        %
        % Usage
        % -----
        % * update(this) calculates the corrected sytem state and the
        %   associated uncertainty covariance.
        %
        % See also KalmanFilterX, predict, iterate, smooth.

            if nargin>1
                if isa(varargin{1},'MeasurementX')
                    this.MeasurementList = MeasurementListX(varargin{1});
                elseif isa(varargin{1}, 'StateX')
                    this.StatePrediction = varargin{1};
                    this.MeasurementList =  MeasurementListX(varargin{2});
                end
            end
            if(this.MeasurementList.NumMeasurements)
                timestamp = this.MeasurementList.Timestamp;
            else
                timestamp = this.StatePrediction.Timestamp;
            end

            if(isempty(this.MeasurementPrediction.Mean) || isempty(this.MeasurementPrediction.Covar))
                [measurementPredictionMean, measurementPredictionCovar, this.KalmanGain] = ...
                    this.predictMeasurement_(this.StatePrediction.Mean, this.StatePrediction.Covar, H, R);
                measurementPrediction = GaussianStateX(measurementPredictionMean,...
                                                       measurementPredictionCovar,...
                                                       this.StatePrediction.Timestamp);
                this.MeasurementPrediction = measurementPrediction;
            end

            % Perform single measurement update
            [posteriorMean, posteriorCovar] = this.update_(this.StatePrediction.Mean,this.StatePrediction.Covar,...
                                                           this.MeasurementList.Vectors,this.MeasurementPrediction.Mean,...
                                                           this.MeasurementPrediction.Covar,this.KalmanGain);
            posteriorCovar = (posteriorCovar+posteriorCovar')/2;

            posterior = GaussianStateX(posteriorMean, posteriorCovar, timestamp);
            this.StatePosterior = posterior;
        end

        function posterior = updatePDA(this, assocWeights, varargin)
        % UPDATEPDA Performs KF update step, for multiple measurements
        %           Update is performed according to the generic (J)PDAF equations [1]
        %
        % Usage
        % -----
        %  * updatePDA(assocWeights) Performs KF-PDA update step for multiple
        %    measurements based on the provided (1-by-Nm+1) association weights
        %    matrix assocWeights.
        %
        %   [1] Y. Bar-Shalom, F. Daum and J. Huang, "The probabilistic data association filter," in IEEE Control Models, vol. 29, no. 6, pp. 82-100, Dec. 2009.
        %
        %   See also KalmanFilterX, Predict, Iterate, Smooth, resample.

            if(this.MeasurementList.NumMeasurements)
                timestamp = this.MeasurementList.Timestamp;
            else
                timestamp = this.StatePrediction.Timestamp;
            end

            [posteriorMean, posteriorCovar] = ...
                this.updatePDA_(this.StatePrediction.Mean,this.StatePrediction.Covar,this.MeasurementList.Vectors,...
                                assocWeights,this.MeasurementPrediction.Mean,this.MeasurementPrediction.Covar,this.KalmanGain);

            posterior = GaussianStateX(posteriorMean,posteriorCovar,timestamp);
            this.StatePosterior = posterior;
        end

        function measurementLikelihoods = get.MeasurementLikelihoods(this)
            measurementLikelihoods = getMeasurementLikelihoods(this);
        end

        function statePrior = get.StatePrior(this)
            statePrior = this.StatePrior;
        end

        function set.StatePrior(this, newStatePrior)
            this.StatePrior = setStatePrior(this, newStatePrior);
        end

        function statePrediction = get.StatePrediction(this)
            statePrediction = this.StatePrediction;
        end

        function set.StatePrediction(this, newStatePrediction)
            this.StatePrediction = setStatePrediction(this, newStatePrediction);
        end

        function measurementPrediction = get.MeasurementPrediction(this)
            measurementPrediction = this.MeasurementPrediction;
        end

        function set.MeasurementPrediction(this, newMeasurementPrediction)
            this.MeasurementPrediction = setMeasurementPrediction(this, newMeasurementPrediction);
        end

        function statePosterior = get.StatePosterior(this)
            statePosterior = this.StatePosterior;
        end

        function set.StatePosterior(this, newStatePosterior)
            this.StatePosterior = setStatePosterior(this, newStatePosterior);
        end

    end

    methods (Static)

        function [xPred, PPred, yPred, S, K] = predict_(x,P,F,Q,H,R,u,B,O)
        % PREDICT_ Perform the discrete-time KF state and measurement
        % prediction steps, under the assumption of additive process noise.
        %
        % Parameters
        % ----------
        % x: column vector
        %   The (xDim x 1) state estimate at the previous time-step.
        % P: matrix
        %   The (xDim x xDim) state covariance matrix at the previous
        %   time-step.
        % F: matrix
        %   An (xDim x xDim) state transition matrix.
        % Q: matrix
        %   The (xDim x xDim) process noise covariance matrix.
        % H: matrix
        %   A (xDim x yDim) measurement matrix.
        % R: matrix
        %   The (yDim x yDim) measurement noise covariance matrix.
        % u: column vector, optional
        %   A optional (xDim x 1) control input.
        %   If omitted, no control input is used.
        % B: matrix, optional
        %   An optional (xDim x xDim) control gain matrix.
        %   If omitted, B is assumed to be 1.
        % O: matrix, optional
        %   An optional (xDim x xDim) control noise covariance
        %   matrix. If omitted, Q is assumed to be 0.
        %
        % Returns
        % -------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix.
        % yPred: column vector
        %   The (yDim x 1) predicted measurement estimate.
        % Pxy: matrix
        %   The (xDim x yDim) cross-covariance matrix.
        % S: matrix
        %   The (yDim x yDim) innovation covariance matrix.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            switch(nargin)
                case(6)
                    u = 0;
                    B = 0;
                    O = 0;
                case(7)
                    B = 1;
                    O = 0;
                case(8)
                    O = 0;
            end

           [xPred, PPred] = KalmanFilterX.predictState_(x,P,F,Q,u,B,O);
           [yPred, S, K] = KalmanFilterX.predictMeasurement_(xPred,PPred,H,R);
        end

        function [xPred, PPred] = predictState_(x,P,F,Q,u,B,Qu)
        % PREDICTSTATE_ Perform the discrete-time KF state prediction
        % step, under the assumption of additive process noise.
        %
        % Parameters
        % ----------
        % x: column vector
        %   The (xDim x 1) state estimate at the previous time-step.
        % P: matrix
        %   The (xDim x xDim) state covariance matrix at the previous
        %   time-step.
        % F: matrix
        %   An (xDim x xDim) state transition matrix.
        % Q: matrix
        %   The (xDim x xDim) process noise covariance matrix.
        % u: column vector, optional
        %   An optional (xDim x 1) control input.
        %   If omitted, no control input is used.
        % B: matrix, optional
        %   An optional (xDim x xDim) control gain matrix.
        %   If omitted, B is assumed to be 1.
        % O: matrix, optional
        %   An optional (xDim x xDim) control noise covariance
        %   matrix. If omitted, Q is assumed to be 0.
        %
        % Returns
        % -------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            switch(nargin)
                case(4)
                    u  = 0;
                    B  = 0;
                    Qu = 0;
                case(5)
                    B  = 1;
                    Qu = 0;
                case(6)
                    Qu = 0;
            end

            % Compute predicted state mean and covariance
            xPred = F*x + B*u;
            PPred =F*P*F' + Q + B*Qu*B';
        end

        function [yPred, S, K] = predictMeasurement_(xPred,PPred,H,R)
        % PREDICTOBS_ Perform the discrete-time KF observation prediction
        % step, under the assumption of additive process noise.
        %
        % Parameters
        % ----------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate at the current
        %   time-step.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix at
        %   the current time-step.
        % H: matrix
        %   An (xDim x yDim) measurement matrix.
        % R: matrix
        %   The (yDim x yDim) measurement noise covariance matrix.
        %
        % Returns
        % -------
        % yPred: column vector
        %   The (yDim x 1) predicted measurement estimate.
        % Pxy: matrix
        %   The (xDim x yDim) cross-covariance matrix.
        % S: matrix
        %   The (yDim x yDim) innovation covariance matrix.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            % Compute predicted measurement mean and covariance
            yPred   = H*xPred;
            Pxy     = PPred*H';
            S       = H*PPred*H' + R;
            K       = Pxy/(S);
        end

        function [x,P] = update_(xPred,PPred,y,yPred,S,K)
        % UPDATE_ Perform the discrete-time KF update step, under the
        % assumption of additive process noisem for a single measurement.
        %
        % Parameters
        % ----------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix.
        % y: column vector
        %   The (yDim x 1) measurement vector.
        % yPred: column vector
        %   The (yDim x 1) predicted measurement estimate.
        % S: matrix
        %   The (yDim x yDim) innovation covariance matrix.
        % K: matrix
        %   The (xDim x yDim) Kalman gain matrix at the current
        %   time-step.
        %
        % Returns
        % -------
        % x: column vector
        %   The (xDim x 1) state estimate at the current time-step.
        % P: matrix
        %   The (xDim x xDim) state covariance matrix at the current
        %   time-step.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            % Compute the filtered estimates
            x = xPred + K * (y - yPred);
            P = PPred - K*S*K';
        end

        function [x,P] = updatePDA_(xPred,PPred,Y,W,yPred,S,K)
        % UPDATEPDA_ Perform the discrete-time Probabilistic Data
        % Association (PDA) KF update step, under the assumption of additive process
        % noise, for multiple measurements (as a Gaussian Mixture)
        %
        % Parameters
        % ----------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix.
        % Y: matrix
        %   The (yDim x nY) measurement vector.
        % W: row vector
        %   The (1 x nY+1) measurement association/mixture weights
        %   vector. (dummy measurement assumed at index 1)
        % yPred: column vector
        %   The (yDim x 1) predicted measurement estimate.
        % S: matrix
        %   The (yDim x yDim) innovation covariance matrix.
        % K: matrix
        %   The (xDim x yDim) Kalman gain matrix at the current
        %   time-step.
        %
        % Returns
        % -------
        % x: column vector
        %   The (xDim x 1) state estimate at the current time-step.
        % P: matrix
        %   The (xDim x xDim) state covariance matrix at the current
        %   time-step.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            % Get size of observation vector
            nY = size(Y,2);
            if(nY==0)
                x = xPred;
                P = PPred;
                return;
            end

            innov_err = Y - yPred;
            xupd = [xPred, xPred + K*innov_err];
            Pplus = PPred - K*S*K';

            try
                x = xupd*W';
            catch
                asd=2;
            end
            v_x = x - xupd;
            P = W(1)*(PPred + v_x(:,1)*v_x(:,1)');
            for j = 2:nY+1
                P = P + W(j)*(Pplus + v_x(:,j)*v_x(:,j)');
            end

%             % Compute innovation mean and (cross) covariance
%             innov_err       = Y - yPred(:,ones(1,nY));
%             tot_innov_err   = innov_err*W(2:end)';
%             Pc              = PPred - K*S*K';
%             Pgag            = K*((innov_err.*W(ones(yDim,1),2:end))*innov_err' - tot_innov_err*tot_innov_err')*K';
%
%             % Compute filtered estimates
%             x    = xPred + K*tot_innov_err;
%             P    = W(1)*PPred + (1-W(1))*Pc + Pgag;
             P    = (P+P')/2;
        end

        function config = getInitConfig()

        end
    end
end


function [hrf, fit, e, param] = Fit_sFIR(tc, TR, Run, T, mode)
% Fits FIR and smooth FIR model
%
% :Usage:
% ::
%
%     function [hrf, fit, e, param] = Fit_sFIR(tc,TR,Runs,T,mode)
%
% :Inputs:
%
%   **tc:**
%        time course
%
%   **TR:**
%        time resolution
%
%   **Runs:**
%        expermental design
%
%   **T:**
%        length of estimated HRF
%
%   **mode:**
%        FIR or smooth FIR
%
%        Options:
%           0 - standard FIR
%
%           1 - smooth FIR
%
% :Outputs:
%
%   **hrf:**
%        estimated hemodynamic response function
%
%   **fit:**
%        estimated time course
%
%   **e:**
%        residual time course
%
%   **param:**
%        estimated amplitude, height and width
%
% ..
%    Created by Martin Lindquist on 10/02/09
%    Last edited: 05/17/13 (ML)
% ..

numstim = length(Run);
len = length(Run{1});
t=1:TR:T;
tlen = length(t);

Runs = zeros(len,numstim);
for i=1:numstim,
    Runs(:,i) = Run{i};
end;

[DX] = tor_make_deconv_mtx3(Runs,tlen,1);

if mode == 1

    C=(1:tlen)'*(ones(1,tlen));
    h = sqrt(1/(7/TR));                       % 7 seconds smoothing - ref. Goutte

    v = 0.1;
    sig = 1;

    R = v*exp(-h/2*(C-C').^2);
    RI = inv(R);
    MRI = zeros(numstim*tlen+1);
    for i=1:numstim,
        MRI(((i-1)*tlen+1):(i*tlen),((i-1)*tlen+1):(i*tlen)) = RI;
    end;

    b = inv(DX'*DX+sig^2*MRI)*DX'*tc;
    fit = DX*b;
    e = tc - DX*b;

elseif mode == 0

    b = pinv(DX)*tc;
    fit = DX*b;
    e = tc - DX*b;

end

hrf =zeros(tlen,numstim);
param = zeros(3,numstim);

for i=1:numstim,
    hrf(:,i) = b(((i-1)*tlen+1):(i*tlen))';
    param(:,i) = get_parameters2(hrf(:,i),(1:tlen));
end;

end


function [U,UT] = isometric_curve_flow(V,varargin)
  % ISOMETRIC_CURVE_FLOW Isometric flow for curves as described by "Robust
  % Fairing via Conformal Curvature Flow" by [Crane et al. 2013]
  %
  % U = isometric_curve_flow(V)
  % U = isometric_curve_flow(V,'ParameterName',ParameterValue,...)
  %
  % Inputs:
  %   V  #V by 2 list of vertex positions
  %   Optional:
  %     'Tao' followed by timestep tao
  % Outputs:
  %   U  #V by 2 list of vertex positions
  %

  % time step
  tao = 1e-2;

  ii = 1;
  while ii < numel(varargin)
    switch varargin{ii}
    case 'Tao'
      assert(ii+1<=numel(varargin));
      ii = ii+1;
      tao = varargin{ii};
    otherwise
      error('Unsupported parameter: %s',varargin{ii});
    end
    ii = ii+1;
  end

  % number of points
  n = size(V,1);

  % Evaluate curvature and edge vectors
  [kappa,alpha,ev,l] = curvature(V);

  % desired flow direction
  kappa_dot = - 2 * kappa;

  % defining edge length at each point
  egde_vector = zeros(n,1);
  for k=2:n-1
    edge_vector(k) = 0.5*(norm(V(k+1,:)-V(k,:))+norm(V(k-1,:)-V(k,:)));
  end
  edge_vector(1) = 0.5*(norm(V(2,:)-V(1,:))+norm(V(size(V,1),:)-V(1,:)));
  edge_vector(n) = 0.5*(norm(V(1,:)-V(n,:))+norm(V(n,:)-V(n-1,:)));
  % mass matrix
  M = diag(sparse(0.5*(l([end 1:end-1]) + l)));

  % re-defining orthonormal basis
  C(:,1) = ones(n,1)/sqrt(ones(n,1)'*M*ones(n,1));

  C(:,2) = V(:,1) - ((V(:,1)'*M*C(:,1)))*C(:,1);
  C(:,2) = C(:,2)/sqrt(C(:,2)'*M*C(:,2));

  C(:,3) = V(:,2) - ((V(:,2)'*M*C(:,1)))*C(:,1) - ((V(:,2)'*M*C(:,2)))*C(:,2) ;
  C(:,3) = C(:,3)/sqrt(C(:,3)'*M*C(:,3));

  % kappa_dot = kappa_dot - ...
  %  (kappa_dot'*M*C(:,1))*C(:,1) - ...
  %  (kappa_dot'*M*C(:,2))*C(:,2) - ...
  %  (kappa_dot'*M*C(:,3))*C(:,3) ;
  kappa_dot = kappa_dot - sum(bsxfun(@times,sum(bsxfun(@times,kappa_dot,M*C)),C),2);

  % Take explicit euler step
  kappa = kappa + tao * kappa_dot;

  % arbitrarily let theta_0 = 0
  theta_0 = 0;
  % % angle between last segment and -x-axis
  %theta_0 = atan2( ...
  %  ev(end,1).*0 - ev(end,2).*-1, ...
  %  ev(end,1).*-1 + ev(end,2).*0 );
  theta_0 = atan2( ...
    -1.*ev(end,2) - 0.*ev(end,1), ...
    -1.*ev(end,1) + 0.*ev(end,2));
  theta = theta_0 + cumsum(kappa .* (0.5 * (l([end 1:end-1]) + l)));

  % recover tangents: T(i,:) is along V(i,:) to V(i+1,:)
  T = bsxfun(@times,l,[cos(theta) sin(theta)]);
  UT = [V(1,:); bsxfun(@plus,V(1,:),cumsum(-T))];

  % Recover positions
  % build laplacian
  L = sparse(1:n,[2:n 1],-1./l,n,n);
  % symmetric
  L = L+L';
  % diagonal diagonals
  L = L - diag(sum(L,2));
  % Bulid rhs
  b = bsxfun(@rdivide,T,l);
  b = b-b([end 1:end-1],:);
  % % solve
  %U = L\b;
  % Fix U(1,:) and solve
  U = V;
  U(2:end,:) = L(2:end,2:end) \ (-L(2:end,1) * U(1,:) + b(2:end,:));

  % % fit a rigid transformation
  %[R,T] = fit_rigid(V,U);
  %U = bsxfun(@plus,U*R,T);

end


function [lf] = eeg_infinite_monopole(monpos, elc, vol)

% EEG_INFINITE_MONOPOLE calculate the infinite medium potential for a monopole
%
% Use as
%   [lf] = eeg_infinite_monopole(monpos, elc, vol)
%
% Implemented from Malmivuo J, Plonsey R, Bioelectromagnetism (1993)
% http://www.bem.fi/book/08/08.htm
%
% See also EEG_INFINITE_DIPOLE, EEG_HALFSPACE_DIPOLE, EEG_HALFSPACE_MONOPOLE

% Copyright (C) 2011, Cristiano Micheli
% Copyright (C) 2019, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if ~isstruct(vol)
  % it only represents the conductivity, make a structure out of it
  vol = struct('cond', vol);
end

siz = size(monpos);
if any(siz==1)
  % positions are specified as a single vector
  Npoles = prod(siz)/3;
  monpos = monpos(:)'; % ensure that it is a row vector
elseif siz(2)==3
  % positions are specified as a Nx3 matrix -> reformat to a single vector
  Npoles = siz(1);
  monpos = monpos';
  monpos = monpos(:)'; % ensure that it is a row vector
else
  ft_error('incorrect specification of monopole locations');
end

cond     = vol.cond;
Nelc     = size(elc,1);
lf       = zeros(Nelc,Npoles);

mu0   = 4*pi*1e-7;         % Permeability of free space
c     = 2.99792458 * 1e8;  % Speed of light
e0    = 1 / (mu0*c^2);     % Permittivity of Free Space

for i=1:Npoles
  % this is the position of monopole "i"
  monopole = monpos((1:3) + 3*(i-1));

  % distances from electrodes to monopole
  r = elc - ones(Nelc,1) * monopole;
  r = sqrt(sum(r.^2,2));

  lf(:,i) = 1 ./ (4*pi*cond*r);
end


% Van Leer scheme for one-dimensional Euler equations

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

% Theory in Section 10.5 of:

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

% This program makes Figs. 10.15, 10.16 in the book

% Functions called: f, problem_specification, Riemann

global  PRL  CRL MACHLEFT  gamma  pleft  pright  rholeft  rhoright  uleft...
	uright  tend  lambda		% lambda = dt/dx

		% .....................Input............................
gamma = 1.4; 	% Ratio of specific heats
J = 48;		% Number of grid cells
bouncon = 0;	% bouncon chooses outflow boundary conditions
		% = 0: Nothing happens: infinite domain
		% = 1: Solid wall at x = 1 with direct prescription of uwall
		% = 2: Solid wall at x = 1 with reflection b.c.
		% ....................End of input........................

gammab = 1/(gamma - 1); gam1 = gamma-1; gamgam = gamma^gamma;
problem_specification

h = 1/J;  				% Cell size
dt = lambda*h;				% Time step
n = floor(tend/dt);			% Number of time-steps

% 		Definition of grid numbering
%       x=0    					 x=1
% grid   |---o---|---o---|---o---  ...  --|---o---|
%            1   1   2   2   3           J-1  J

xcenter = h*[1:J] - h/2;		% Location of cell centers

press = zeros(size(xcenter));		% Preallocation of pressure,
rhoold = press; uold = press;		%       density and velocity
rhonew = press; mnew = press;		%       momentum
totenew = press;			%	total energy

for j = 1:length(xcenter)		% Initial conditions
  if xcenter(j) < 0.5, press(j) = pleft; rhoold(j) = rholeft;  uold(j) = uleft;
  else,  	     press(j) = pright;  rhoold(j) = rhoright; uold(j) = uright;
  end
end

	% Initialization of cell center variables
totenold = rhoold.*(0.5*uold.*uold + gammab*press./rhoold); % Total energy rho*E
totenleft = totenold(1); totenright = totenold(J);
mold = rhoold.*uold;					    % Momentum m
c = sqrt(gamma*press./rhoold);				    % Sound speed
mach = uold./c;						    % Mach number

% Preallocation of split fluxes
plus1 = c; minus1 = c; plus2 = c; minus2 = c; plus3 = c; minus3 = c;
% Preallocation of van Leer fluxes
flux1 = zeros(J-1,1); flux2 = flux1; flux3 = flux1;

t = 0;
for i = 1:n,  t = t + dt;
  Eflux1 = rhoold.*uold;			% Eflux1,2,3 is Euler flux
  Eflux2 = Eflux1.*uold + (1/gamma)*rhoold.*c.^2;
  Eflux3 = 0.5*Eflux1.*uold.^2 + gammab*Eflux1.*c.^2;

    for j = 1:J
    if mach(j) > 1
      plus1(j) = Eflux1(j);  plus2(j) = Eflux2(j);  plus3(j) = Eflux3(j);
    elseif mach(j) < -1
      plus1(j) = 0;         plus2(j) = 0;         plus3(j) = 0;
    else
      plus1(j) = 0.25*rhoold(j)*c(j)*(1 + mach(j))^2;
      plus2(j) = plus1(j)*c(j)*(2 + gam1*mach(j))/gamma;
      plus3(j) = (plus2(j)^2/plus1(j))*gamma^2*0.5*gammab/(gamma+1);
    end
  end
  minus1 = Eflux1 - plus1; minus2 = Eflux2 - plus2; minus3 = Eflux3 - plus3;

  for j = 1:J-1					% van Leer fluxes
    flux1(j) = plus1(j) + minus1(j+1);
    flux2(j) = plus2(j) + minus2(j+1);
    flux3(j) = plus3(j) + minus3(j+1);
  end

	% Update of state variables
  rhonew(1) = rholeft;		  rhonew(J)  = rhoright;
  mnew(1) = rholeft*uleft; 	  mnew(J)    = rhoright*uright;
  totenew(1) = totenleft; 	  totenew(J) = totenright;
  for j = 2:J-1
    rhonew(j)  = rhoold(j)   - lambda*(flux1(j) - flux1(j-1));
    mnew(j)    = mold(j)     - lambda*(flux2(j) - flux2(j-1));
    totenew(j) = totenold(j) - lambda*(flux3(j) - flux3(j-1));
  end

  if bouncon ~= 0
    if bouncon == 1
      rhowall = rhoold(J); uwall = 0;
      pwall = (gamma-1)*(totenold(J) - 0.5*mold(J)^2/rhoold(J)); % pwall = p(J)
      totenwall = pwall/(gamma-1) + 0.5*rhowall*uwall^2;
    else
      rhowall = rhoold(J); uwall = - mold(J)/rhoold(J);
      pwall = (gamma-1)*(totenold(J) - 0.5*mold(J)^2/rhoold(J)); % pwall = p(J)
      totenwall = pwall/(gamma-1) + 0.5*rhowall*uwall^2;
    end
    wallflux1 = rhowall*uwall;
    pwall = gam1*(totenwall - 0.5*wallflux1*uwall);
    wallflux2 = wallflux1.*uwall + pwall;
    wallflux3 = 0.5*wallflux1*uwall^2 + gammab*gamma*uwall*pwall;
    cwall = sqrt(gamma*pwall/rhowall); machwall = uwall/cwall;
    if machwall > 1
      plus1wall = wallflux1; plus2wall = wallflux2; plus3wall = wallflux3 ;
    elseif machwall < -1
      plus1wall = 0;      plus2wall = 0;      plus3wall = 0;
    else
      plus1wall = 0.25*rhowall*cwall*(1 + machwall)^2;
      plus2wall = plus1wall*cwall*(2 + gam1*machwall)/gamma;
      plus3wall = (plus2wall^2/plus1wall)*gamma^2*0.5*gammab/(gamma+1);
    end
    minus1wall = wallflux1 - plus1wall;
    minus2wall = wallflux2 - plus2wall; minus3wall = wallflux3 - plus3wall;

    flux1wall = plus1(J) + minus1wall;			% van Leer fluxes
    flux2wall = plus2(J) + minus2wall;
    flux3wall = plus3(J) + minus3wall;

    rhonew(J)  = rhoold(J)   - lambda*(flux1wall - flux1(J-1));
    mnew(J)    = mold(J)     - lambda*(flux2wall - flux2(J-1));
    totenew(J) = totenold(J) - lambda*(flux3wall - flux3(J-1));
  end
 	% Update of state variables
  uold = mnew./rhonew; press = gam1*(totenew - 0.5*mnew.*uold);
  rhoold = rhonew; totenold = totenew; mold = mnew;

  c = sqrt(gamma*press./rhoold); mach = uold./c;
end

entropy = log(press./rhoold.^gamma);

figure(1), clf
subplot(2,3,1),hold on,title('DENSITY','fontsize',14),plot(xcenter,rhonew,'o')
subplot(2,3,2),hold on,title('VELOCITY','fontsize',14),plot(xcenter,uold,'o')
subplot(2,3,3),hold on,title('PRESSURE','fontsize',14),plot(xcenter,press,'o')
subplot(2,3,4),hold on,title('MACHNUMBER','fontsize',14),plot(xcenter,mach,'o')
subplot(2,3,5),hold on,title('ENTROPY','fontsize',14),plot(xcenter,entropy,'o')
subplot(2,3,6),axis('off'),hold on,title('Van Leer scheme','fontsize',14)

Riemann		% Plot exact solution


function [X, spectrum] = slcmds(D, d, w, ty)
%SLMDS Performs Classical Multidimensional scaling
%
% $ Syntax $
%   - X = slcmds(D, d)
%   - X = slcmds(D, d, w)
%   - X = slcmds(D, d, w, 'sqr')
%   - [X, spectrum] = slcmds(...)
%
% $ Arguments $
%   - D:        The pairwise distance matrix (n x n)
%   - d:        The dimension of the embedding space
%   - w:        The weights of samples (1 x n or [])
%   - X:        The embedded samples (d x n)
%
% $ Description $
%   - X = slcmds(D, d) performs classic multidimensional scaling to
%     pursue an embedding space of d-dimension and the vector
%     representation in that space of the objects, such that the
%     distances are optimally preserved.
%
%   - X = slcmds(D, d, w) If w is not empty, it performs classic
%     multidimensional scaling on weighted samples.
%
%   - X = slcmds(D, d, w, 'sqr') indicates that D contains the square
%     of distances.
%
%   - [X, spectrum] = slcmds(...) additionally outputs the spectrum of
%     the embedded space
%
% $ History $
%   - Created by Dahua Lin, on Sep 8th, 2006
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('slcmds', 2);
end

if ndims(D) ~= 2 || size(D, 1) ~= size(D, 2)
    error('sltoolbox:invalidarg', ...
        'The D should be a square matrix');
end
n = size(D, 1);

if d >= n
    error('sltoolbox:exceedbound', ...
        'The dimension d should be less than the number of samples n');
end

if nargin < 3
    w = [];
else
    if ~isempty(w)
        if ~isequal(size(w), [1, n])
            error('sltoolbox:sizmismatch', ...
                'If w is specified, it should be an 1 x n row vector');
        end
    end
end

if nargin >= 4 && strcmpi(ty, 'sqr')
    is_sqr = true;
else
    is_sqr = false;
end

%% compute

if ~is_sqr
    K = sldists2kernels(D);
else
    K = sldists2kernels(D, 'sqr');
end

[X, spectrum] = slkernelembed(K, d, w);


function [x,lambda_opt,svd_state,Y_hat] = ridge_gcv(varargin)
%[x_hat,lambda_opt] = ridgeGCV(Y,A,P)
%
% Estimates a ridge regression model, also know as Tikhonov regularization,
% or minimum norm with L2 prior.
%
% x_hat = argmin(x) ||Y-A*x||^2 + lambda*||P*x||^2
% with lambda > 0
%
%
% [..., svd_state] = ridge_gcv(...)  returns the svd decomposition. This can
% be reused if the target vector does not change.
%
% [..., Y_hat] = ridge_gcv(...)  returns the predicted target vector.
% Residuals can be computed via E = Y-Y_hat;
%
% This code is based on a previous implementation used in Valdes-Hernandez
% et al. (2009), written by Alejandro Ojeda and Pedro Valdez-Hernandez at
% the Cuban Neuroscience Center in 2009.
%
% Author: Alejandro Ojeda, SCCN/INC/UCSD, Jul-2012
%         Tim Mullen, SCCN/INC/UCSD, Jan-2013, Apr-2013
%
% References:
%   Pedro A. Valdes-Hernandez, Alejandro Ojeda, Eduardo Martinez-Montes, Agustin
%       Lage-Castellanos, Trinidad Virues-Alba, Lourdes Valdes-Urrutia, Pedro A.
%       Valdes-Sosa, 2009. White matter architecture rather than
%       cortical surface area correlates with the EEG alpha rhythm. NeuroImage 49
%       (2010) 2328–2339

arg_define([0 Inf],varargin, ...
    arg_norep({'Y','TargetVector','y'},mandatory,[],'The target vector'), ...
    arg_norep({'A','DesignMatrix'},mandatory,[],'The design matrix. This is the data matrix (ie X).'), ...
    arg({'P','PriorInvCov'},[],[],'Prior inverse covariance (precision) matrix for params. Can be an [nc x nc] matrix, where nc is the number of columns of A. Can also be a scalar, P, specifying the prior inverse variance (precision) of each parameter (diagonal covariance matrix). If empty, identity covariance matrix assumed. A sparse matrix is advised if precision matrix is not dense.'), ...
    arg_nogui({'blksz','DesignMatrixBlockSize','designMatrixBlockSize'},[],[],'Design matrix structure. Can be a tuple [numrows numcols], in which case A consists of identical blocks of this size, along the main diagonal'), ...
    arg_subswitch({'lambdaMode','LambdaSelectionMode'},'grid_gcv', { ...
        'manual' { ...
            arg({'lambda','RegularizationParam'},1,[0 Inf],'Regularization parameter (lambda)','type','denserealdouble') ...
            }, ...
         'grid_gcv' { ...
            arg({'gridSize','GridSize'},100,[0 Inf],'Grid size for regularization param search. This is used to automatically select the regularization parameter which minimizes the Generalized Cross-Validation (GCV) criteria.') ...
            arg({'plotGCV','PlotGCV'},false,[],'Plot GCV curve'), ...
            } ...
     },'Selection mode for lambda. Automatic (GCV grid search) or Manual (must provide lambda)'), ...
    arg_nogui('svd_state',[],[],'SVD decomposition structure'), ...
    arg({'verb','Verbosity'},false,[],'Verbose output') ...
    );

[nr,nc] = size(A);

% if A is not sufficiently sparse, convert to full
if issparse(A) && nnz(A)/numel(A) > 0.9
    A = full(A);
end

if isempty(svd_state)
    if verb, fprintf('Computing SVD of design matrix.\nr'); end

    % init prior covmat
    if isscalar(P)
        P=P*speye(nc);
    end
    if ~isempty(P)
        Pinv = inverse(P);
    end
    % compute SVD
    if ~isempty(P)
        APinv = A*Pinv;
    else
        APinv = A;
    end
    if isempty(blksz)
        [U,S,V] = svd_wrapper(APinv);
    else
        %warning('block optimization not yet implemented');
        [U,S,V] = svd_wrapper(APinv);
%         [U,S,V] = svd_wrapper(APinv(1:blksz(1),1:blksz(2)));
    end
    if isempty(P)
        iPV = V;
    else
        iPV = Pinv*V;
    end
    s   = diag(S);
    s2  = s.^2;
    Ut  = U';
else
    iPV = svd_state.iPV;
    s   = svd_state.s;
    s2  = svd_state.s2;
    Ut  = svd_state.Ut;
end

UtY = Ut*Y;

switch lambdaMode.arg_selection
    case 'grid_gcv'
        % search over a grid of lambda values for the value that minimizes
        % the Generalized Cross Validation (GCV) criteria
        % lambdaOpt = argmin(lambda) { GCV(lambda) }

        % automatically determine lambda range based on singular values
        tol     = max([nr nc])*eps(max(s));
        lgrid   = logspace(log10(tol),log10(max(s)),lambdaMode.gridSize);
        gcv     = zeros(lambdaMode.gridSize,1);
        for it=1:lambdaMode.gridSize
            % compute GCV criteria
            d       = lgrid(it)./(s2+lgrid(it));
            f       = diag(d)*UtY;
            gcv(it) = dot(f,f,1)/sum(d)^2;
        end
        loc = getMinima(gcv);
        if isempty(loc),
            % no minimum found, search for elbow instead
            if verb
                fprintf('no GCV minimum, finding elbow...\nr');
            end
            [val loc] = hlp_findElbow(gcv);  % min(gcv)
        end
        loc         = loc(end);
        lambda_opt  = lgrid(loc);
        if verb
            fprintf('lambda: %0.5g, GCV: %0.5g\nr',lambda_opt,gcv(loc));
        end
        if lambdaMode.plotGCV
            plotLambdaGCV(lgrid,gcv,lambda_opt,loc);
        end
    case 'manual'
        lambda_opt = lambdaMode.lambda;
    otherwise
        error('SIFT:ridge_gcv:BadLambdaRule', ...
              'Unknown lambda learning rule %s',lambdaMode.arg_selection);
end

% solve system for parameters x
x = iPV*bsxfun(@times,(s./(s2+lambda_opt^2)),UtY);

if nargout > 2
    Y_hat = A*x;
end
if nargout > 3
    svd_state.iPV = iPV;
    svd_state.s   = s;
    svd_state.s2  = s2;
    svd_state.UtY = UtY;
end

% Helper functions
% -------------------------------------------------------------------------
function [U,S,V] = svd_wrapper(A)
% compute SVD
if issparse(A)
    [U,S,V] = svds(A,min(size(A)));
else
    [U,S,V] = svd(A,'econ');
end

function plotLambdaGCV(lgrid,gcv,lambda_opt,loc)
% plot lambda versus GCV
figure;
semilogx(lgrid,gcv)
xlabel('log-lambda');
ylabel('GCV');
hold on;
plot(lambda_opt,gcv(loc),'rx','linewidth',2);
hold off; grid on;

function indmin = getMinima(x)
% get minimum of a function
fminor = diff(x)>=0;
fminor = ~fminor(1:end-1, :) & fminor(2:end, :);
fminor = [0; fminor; 0];
indmin = find(fminor);


function tests = SE2Test
    tests = functiontests(localfunctions);
end

% we will assume that the primitives rotx,trotx, etc. all work

function constructor_test(tc)

    verifyClass(tc, SE2(), 'SE2');

    %% null
    tc.verifyEqual(SE2().double, eye(3,3));

    %% translation only
    t = [1 2];
    tc.verifyEqual(SE2(t).double, transl2(t));
    tc.verifyEqual(SE2(t').double, transl2(t));
    tc.verifyEqual(SE2(t(1), t(2)).double, transl2(t));

    %% R
    R = rot2(-pi/2);
    tc.verifyEqual(SE2(R).double, r2t(R));

    %% R,t

    tc.verifyEqual(SE2(R,t).double, transl2(t)*r2t(R));

    %% T
    T = transl2(1, 2)*trot2(0.3);
    tc.verifyEqual(SE2(T).double, T);

    %% x,y,theta
    T = transl2(1, 2)*trot2(0.3);
    tc.verifyEqual(SE2(1, 2, 0.3).double, T);
    tc.verifyEqual(SE2([1, 2, 0.3]).double, T);
    tc.verifyEqual(SE2([1, 2], 0.3).double, T);

    %% T
    T = rt2tr(R,t);
    tc.verifyEqual(SE2(T).double, T);

    %% copy constructor
    TT = SE2(T);
    tc.verifyEqual(SE2(TT).double, T);

    %% vectorised versions

    T1 = transl2(1,2) * trot2(0.3);
    T2 = transl2(1,-2) * trot2(-0.4);

    TT = cat(3, T1, T2, T1, T2);

    tt = SE2(TT);
    tc.verifyEqual(length(tt), size(TT, 3) );
    tc.verifyEqual(tt.T, TT);

end

function concat_test(tc)
    x = SE2();
    xx = [x x x x];

    tc.verifyClass(xx, 'SE2');
    tc.verifySize(xx, [1 4]);
end

function staticconstructors_test(tc)

    %% exponential
    tc.verifyEqual(SE2.exp( skew(0.3) ).R, rot2(0.3), 'AbsTol', 1e-10  );

    %% exponential
    tc.verifyEqual(SE2.exp(zeros(3,3)).T, eye(3,3), 'AbsTol', 1e-10  );
    t = [1 2];
    tc.verifyEqual(SE2.exp(skewa([t 0])).T, transl2(t), 'AbsTol', 1e-10  );
end

function isa_test(tc)

    verifyTrue(tc, SE2.isa(trot2(0)) );
    verifyFalse(tc, SE2.isa(1) )
end

function resulttype_test(tc)

    t = SE2();
    verifyClass(tc, t, 'SE2');

    verifyClass(tc, t*t, 'SE2');

    verifyClass(tc, t/t, 'SE2');

    verifyClass(tc, inv(t), 'SE2');
    end

function inverse_test(tc)

    T1 = transl2(1, 2) * trot2(0.3);
    TT1 = SE2(T1);

    % test inverse
    tc.verifyEqual(double(TT1.inv()), inv(T1), 'AbsTol', 1e-10  );

    tc.verifyEqual(double(TT1*TT1.inv()), eye(3,3), 'AbsTol', 1e-10  );
    tc.verifyEqual(double(TT1.inv()*TT1), eye(3,3), 'AbsTol', 1e-10  );

    % vector case
    tc.verifyEqual(double(TT1.inv()*TT1), eye(3,3), 'AbsTol', 1e-10  );
end

function Rt_test(tc)

    TT1 = SE2.rand
    T1 = TT1.double; R1 = t2r(T1); t1 = transl2(T1);

    tc.verifyEqual(TT1.T, T1, 'AbsTol', 1e-10  );
    tc.verifyEqual(TT1.R, R1, 'AbsTol', 1e-10  );
    tc.verifyEqual(TT1.t, t1, 'AbsTol', 1e-10  );

    tc.verifyEqual(TT1.transl, t1', 'AbsTol', 1e-10  );
    TT = [TT1 TT1 TT1];
    tc.verifyEqual(TT.transl, [t1 t1 t1]', 'AbsTol', 1e-10  );
end

function arith_test(tc)

    R1 = rpy2r( randn(1,3) );  t1 = randn(3,1); T1 = rt2tr(R1, t1);
    R2 = rpy2r( randn(1,3) );  t2 = randn(3,1); T2 = rt2tr(R2, t2);

    TT1 = SE2.rand; T1 = TT1.double;
    TT2 = SE2.rand; T2 = TT2.double;

    I = SE2();

    %% SE2 * SE2 product
    % scalar x scalar

    tc.verifyEqual(double(TT1*TT2), T1*T2, 'AbsTol', 1e-10  );
    tc.verifyEqual(double(TT2*TT1), T2*T1, 'AbsTol', 1e-10  );
    tc.verifyEqual(double(TT1*I), T1, 'AbsTol', 1e-10  );
    tc.verifyEqual(double(TT2*I), T2, 'AbsTol', 1e-10  );

    % vector x vector
    tc.verifyEqual([TT1 TT1 TT2] * [TT2 TT1 TT1], [TT1*TT2 TT1*TT1 TT2*TT1]);

    % scalar x vector
    tc.verifyEqual(TT1 * [TT2 TT1], [TT1*TT2 TT1*TT1]);

    % vector x scalar
    tc.verifyEqual([TT1 TT2]*TT2, [TT1*TT2 TT2*TT2]);

    %% SE2 * vector product
    vx = [1 0]'; vy = [0 1]';

    % scalar x scalar

    tc.verifyEqual(TT1*vy, h2e( T1*e2h(vy) ), 'AbsTol', 1e-10);

    % vector x vector
    tc.verifyEqual([TT1 TT2] * [vx vy], [h2e(T1*e2h(vx)) h2e(T2*e2h(vy))], 'AbsTol', 1e-10);

    % scalar x vector
    tc.verifyEqual(TT1 * [vx vy], h2e( T1*e2h([vx vy]) ), 'AbsTol', 1e-10);

    % vector x scalar
    tc.verifyEqual([TT1 TT2 TT1] * vy, [h2e(T1*e2h(vy)) h2e(T2*e2h(vy)) h2e(T1*e2h(vy))], 'AbsTol', 1e-10);

end

function function_tests(tc)

    % log
    T = SE2.exp([2 3 0.5]);
    tc.verifyEqual(log(T), [0 -0.5 2; 0.5 0 3; 0 0 0], 'AbsTol', 1e-10  );

end

function conversions_test(tc)

    %%  SE2                     convert to SE2 class

    TT = SE2(1, 2, 0.3);

    verifyClass(tc, TT.SE3, 'SE3');
    tc.verifyEqual(double(TT.SE3), transl(1, 2, 0) * trotz(0.3), 'AbsTol', 1e-10 );

    %% xyt
    tc.verifyEqual(TT.xyt(), [1 2, 0.3]', 'AbsTol', 1e-10);

    %% Twist
    T = SE2.exp([2 3 0.5]);
    t = T.Twist();
    verifyInstanceOf(tc, t, 'Twist');
    tc.verifyEqual(t.v, [2 3]', 'AbsTol', 1e-10);
    tc.verifyEqual(t.w, 0.5, 'AbsTol', 1e-10);

    %% Lie stuff
    th = 0.3;
    RR = SO2(th);
    tc.verifyEqual(RR.log, skew(th), 'AbsTol', 1e-10 );

end

function interp_test(tc)
    TT = SE2.rand
    I = SE2;

    z = interp(I, TT, 0);
    tc.verifyClass(z, 'SE2')

    tc.verifyEqual(double(interp(I, TT, 0)),   double(I), 'AbsTol', 1e-10 );
    tc.verifyEqual(double(interp(I, TT, 1)),   double(TT), 'AbsTol', 1e-4 );
    tc.verifyEqual(double(interp(I, TT, 0.5)), double(trinterp2(TT.T, 0.5)), 'AbsTol', 1e-10  );

end

function miscellany_test(tc)

    TT = SE2(1, 2, 0.3);

    tc.verifyEqual(dim(TT), 3);

    tc.verifyEqual(isSE(TT), true );

    tc.verifyClass(TT.new, 'SE2');

    tc.verifyClass(SE2.convert(TT), 'SE2');
    tc.verifyClass(SE2.convert(TT.T), 'SE2');
    z = SE2.convert(TT);
    tc.verifyEqual(double(z), double(TT));

    z = SE2.convert(TT.T);
    tc.verifyEqual(double(z), TT.T);

end

function display_test(tc)

    T1 = SE2.rand;
    T2 = SE2.rand

    T1.print
    trprint2(T1)   % old style syntax

    T1.plot

    T1.print
    trprint2(T1)   % old style syntax

    T1.plot
    trplot2(T1)   % old style syntax

    T1.animate
    T1.animate(T2)
    tranimate2(T1)   % old style syntax
    tranimate2(T1, T2)   % old style syntax
    tranimate2(T1)   % old style syntax
    tranimate2(T1, T2)   % old style syntax
end


function R = GenerateRefPoints(Q,diff,alpha,N)
% Generate the reference points according to the combined population

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    Q    = Q(NDSort(Q.objs,1)==1);
    R    = [];
    subN = min(length(Q),ceil(alpha*N));
    CrowdDis = CrowdingDistanceInEachObj(Q.objs);
    [~,rank] = sort(CrowdDis,1,'descend');
    for m = 1 : length(Q(1).obj)
        Rm      = Q(rank(1:subN,m)).objs;
        Rm(:,m) = Rm(:,m) - diff(m);
        R       = [R;Rm];
    end
    R = R(NDSort(R,1)==1,:);
    if size(R,1) > N
        CrowdDis = CrowdingDistanceInEachObj(R);
        [~,rank] = sort(sum(CrowdDis,2),'descend');
        R        = R(rank(1:N),:);
    end
end

function CrowdDis = CrowdingDistanceInEachObj(PopObj)
% Calculate the crowding distance of each solution in each objective

    [N,M]    = size(PopObj);
    CrowdDis = zeros(N,M);
    Fmax     = max(PopObj,[],1);
    Fmin     = min(PopObj,[],1);
    for i = 1 : M
        [~,rank] = sortrows(PopObj(:,i));
        CrowdDis(rank(1),i)   = inf;
        CrowdDis(rank(end),i) = inf;
        for j = 2 : N-1
            CrowdDis(rank(j),i) = (PopObj(rank(j+1),i)-PopObj(rank(j-1),i))/(Fmax(i)-Fmin(i));
        end
    end
end


function T = auxstructure(elem)
%% AUXSTRUCTURE auxiliary structure for a 2-D triangulation.
%
%  T = AUXSTRUCTURE(elem) constucts the indices map between elements, edges
%  and nodes, and the boundary information. T is a structure.
%
%  T.neighbor(1:NT,1:3): the indices map of neighbor information of elements,
%  where neighbor(t,i) is the global index of the element oppoiste to the
%  i-th vertex of the t-th element.
%
%  T.elem2edge(1:NT,1:3): the indices map from elements to edges, elem2edge(t,i)
%  is the edge opposite to the i-th vertex of the t-th element.
%
%  T.edge(1:NE,1:2): all edges, where edge(e,i) is the global index of the
%  i-th vertex of the e-th edge, and edge(e,1) < edge(e,2)
%
%  T.bdEdge(1:Nbd,1:2): boundary edges with positive oritentation, where
%  bdEdge(e,i) is the global index of the i-th vertex of the e-th edge for
%  i=1,2. The positive oritentation means that the interior of the domain
%  is on the left moving from bdEdge(e,1) to bdEdge(e,2). Note that this
%  requires elem is positive ordered, i.e., the signed area of each
%  triangle is positive. If not, use elem = fixorder(node,elem) to fix the
%  order.
%
%  T.edge2elem(1:NE,1:4): the indices map from edge to element, where
%  edge2elem(e,1:2) are the global indexes of two elements sharing the e-th
%  edge, and edge2elem(e,3:4) are the local indices of e to edge2elem(e,1:2).
%
%  To save space all the data type in T is uint32. When use them as a input
%  of sparse(i,j,s,m,n), please change them into double type.
%
%  See also auxstructure3.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

totalEdge = uint32(sort([elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])],2));
matlabversion = version;
if str2double(matlabversion(end-5:end-2)) > 2012
    [edge, i2, j] = unique(totalEdge,'rows','legacy');
else
    [edge, i2, j] = unique(totalEdge,'rows');
end
NT = size(elem,1);
elem2edge = uint32(reshape(j,NT,3));
i1(j(3*NT:-1:1)) = 3*NT:-1:1;
i1 = i1';
k1 = ceil(i1/NT);
k2 = ceil(i2/NT);
t1 = i1 - NT*(k1-1);
t2 = i2 - NT*(k2-1);
ix = (i1 ~= i2);
neighbor = uint32(accumarray([[t1(ix),k1(ix)];[t2,k2]],[t2(ix);t1],[NT 3]));
edge2elem = uint32([t1,t2,k1,k2]);
bdElem = t1(t1 == t2);
bdk1 = k1(t1 == t2);
bdEdge = [elem(bdElem(bdk1==1),[2 3]); elem(bdElem(bdk1==2),[3 1]);...
          elem(bdElem(bdk1==3),[1 2])];
T = struct('neighbor',neighbor,'elem2edge',elem2edge,'edge',edge,...
           'edge2elem',edge2elem,'bdElem',bdElem,'bdEdge',bdEdge);


function M = DiagM(n)
% function M = DiagM(n)
% M = TraceM(n, 'lo')
% Return "Diagonal-matrix", when applied on vectorized matrix (of order n)
% It gives the diagonal:
%
% Ouput are sparse
%
% DiagM(size(A))*A(:) == diag(A); % <- all true
%
% Author: Bruno Luong <brunoluong@yahoo.com>
% Date: 21/March/2009

if isscalar(n)
    n = [n n];
end
I = idiag(n,0);

% Result
M = sparse((1:length(I)).', I, 1, length(I), prod(n));

end


classdef IMMOEA_F9 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing IM-MOEA

%------------------------------- Reference --------------------------------
% R. Cheng, Y. Jin, K. Narukawa, and B. Sendhoff, A multiobjective
% evolutionary algorithm using Gaussian process-based inverse modeling,
% IEEE Transactions on Evolutionary Computation, 2015, 19(6): 838-856.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = [1,zeros(1,obj.D-1)+10];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = X(:,2:obj.D).^(1./(1+3*repmat(2:obj.D,size(X,1),1)/obj.D)) - repmat(X(:,1),1,obj.D-1);
            g = sum(t.^2/4000,2) - prod(cos(t./repmat(sqrt(1:obj.D-1),size(X,1),1)),2) + 2;
            PopObj(:,1) = X(:,1);
            PopObj(:,2) = g.*(1-sqrt(PopObj(:,1)./g));
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


% Demo: learn a sinc. Run and compare all algorithms using their default
% parameters.
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

close all
clear
rng('default'); rng(1)

%% PARAMETERS

N = 1000; % number of training data
N_test = 500; % number of test data
SNR = 20; % SNR in dB

%% PROGRAM

% generate data
x = randn(N,1);
x_test = linspace(min(x),max(x),N_test)';
y_ref = sinc([x;x_test]);
y = y_ref + sqrt(10^(-SNR/10)*var(y_ref))*randn(N+N_test,1);
y_test = y_ref(N+1:N+N_test);

% get list of kernel adaptive filters in 'lib' folder
fdir = fileparts(which('kafbox_template.m'));
files = dir(fullfile(fdir,'*.m'));
[~,algorithms] = cellfun(@fileparts, {files.name}, 'UniformOutput',0);
for i=length(algorithms):-1:1
    if ~exist(algorithms{i},'class')
        algorithms(i) = []; % remove files that do not represent classes
    end
end

% perform online learning for each algorithm
fprintf('\n')
num_alg = length(algorithms);
titles = cell(num_alg,1);
MSE = zeros(num_alg,1);
Y_est = zeros(N_test,num_alg);
for algo_ind=1:num_alg
    t1 = tic;
    algorithm = algorithms{algo_ind};
    fprintf('%2d. %9s: ',algo_ind,upper(algorithm));
    titles{algo_ind} = strrep(upper(algorithm),'_','\_');

    kaf = feval(algorithm);
    for i=1:N
        if ~mod(i,floor(N/10)), fprintf('.'); end
        kaf.train(x(i),y(i));
    end
    y_est = kaf.evaluate(x_test);
    Y_est(:,algo_ind) = y_est;
    MSE(algo_ind) = mean((y_test-y_est).^2);

    fprintf(' %.2fs. MSE=%3.2fdB\n',toc(t1),10*log10(MSE(algo_ind)))
end

%% OUTPUT

% plot results in different "leagues"
[MSE_sorted,ind] = sort(MSE,'descend');
num_fig = ceil(num_alg/5);

remaining = num_alg;
titles{num_alg+1} = 'data';
for fig_ind=num_fig:-1:1
    figure; hold all
    plot(x,y(1:N),'.')

    rm = rem(remaining,5);
    num_in_league = (rm==0)*5 + rm;
    % plot the results for the num_in_league worst results
    league_inds = num_alg-remaining+num_in_league:-1:num_alg-remaining+1;
    for i=league_inds
        plot(x_test,Y_est(:,ind(i)),'LineWidth',2)
    end
    title(sprintf('League %d',fig_ind))
    legend(titles([num_alg+1; ind(league_inds)]))

    axis([min(x)-0.5 max(x)+0.5 min(y)-0.5 max(y)+0.5]);
    remaining = remaining - num_in_league;
end


function mCol=grColEdge(E)
% function mCol=grColEdge(E) solve the color graph problem
% for edges of the graph.
% Input parameter:
%   E(m,2) - the edges of graph;
%     1st and 2nd elements of each row is numbers of vertexes;
%     m - number of edges.
% Output parameter:
%   mCol(m,1) - the list of the colors of edges.
% Uses the sequential deleting of the maximal matching sets.
% Required the Optimization Toolbox v.3.0.1 or over.
% Author: Sergiy Iglin
% e-mail: siglin@yandex.ru
% personal page: http://iglin.exponenta.ru

% ============= Input data validation ==================
if nargin<1,
  error('There are no input data!')
end
[m,n,E] = grValidation(E); % E data validation
E=[E(:,1:2),[1:m]']; % numbers of vertexes and numbers of edges
mCol=zeros(m,1); % initial value
% ============= Main cycle with MaxMatch deleting ====
while any(mCol==0),
  ne=find(mCol==0); % uncolored edges
  E1=E(ne,:); % it's edges
  nMM=grMaxMatch(E1(:,1:2)); % the maximal matching
  mCol(E1(nMM,3))=max(mCol)+1; % the next colorend
end
return


function A=imresize3d(V,scale,tsize,ntype,npad)
% This function resizes a 3D image volume to new dimensions
% Vnew = imresize3d(V,scale,nsize,ntype,npad);
%
% inputs,
%   V: The input image volume
%   scale: scaling factor, when used set tsize to [];
%   nsize: new dimensions, when used set scale to [];
%   ntype: Type of interpolation ('nearest', 'linear', or 'cubic')
%   npad: Boundary condition ('replicate', 'symmetric', 'circular', 'fill', or 'bound')
%
% outputs,
%   Vnew: The resized image volume
%
% example,
%   load('mri','D'); D=squeeze(D);
%   Dnew = imresize3d(D,[],[80 80 40],'nearest','bound');
%
% This function is written by D.Kroon University of Twente (July 2008)

% Check the inputs
if(exist('ntype', 'var') == 0), ntype='nearest'; end
if(exist('npad', 'var') == 0), npad='bound'; end
if(exist('scale', 'var')&&~isempty(scale)), tsize=round(size(V).*scale); end
if(exist('tsize', 'var')&&~isempty(tsize)),  scale=(tsize./size(V)); end

% Make transformation structure
T = makehgtform('scale',scale);
tform = maketform('affine', T);

% Specify resampler
R = makeresampler(ntype, npad);

% Anti-aliasing
if(scale<1)
   r=ceil(2.5/scale(1)); H=sinc((-r:r)*scale(1)); H=H./sum(H);
   Hx=reshape(H,[length(H) 1 1]);
   r=ceil(2.5/scale(2)); H=sinc((-r:r)*scale(2)); H=H./sum(H);
   Hy=reshape(H,[1 length(H) 1]);
   r=ceil(2.5/scale(3)); H=sinc((-r:r)*scale(3)); H=H./sum(H);
   Hz=reshape(H,[1 1 length(H)]);
   V=imfilter(imfilter(imfilter(V,Hx, 'same' ,'replicate'),Hy, 'same' ,'replicate'),Hz, 'same' ,'replicate');
end

% Resize the image volueme
A = tformarray(V, tform, R, [1 2 3], [1 2 3], tsize, [], 0);


function [x,y,z,s,w,flag] = msquadsolve(Q,c,A,b,C),
% MSQUADSOLVE
%
% USAGE:   [x,y,z,s,w,flag] = msquadsolve(Q,c,A,b,C)
%
% PARAMETERS:  Q -> (n,n) symetric matrix (definite positive)
%              c -> (n,1) vector
%              A -> (m,n) matrix
%              b -> (m,1) vector
%              C -> (m,1) vector
%
%            x -> primal variables
%            y -> lagrangian coeff of equality constraints
%            z -> dual variables of x
%            s -> primal auxiliary variable (only if C < Inf)
%            w -> dual variable of s
%            flag -> set to 0 => no problem, set to 1 => problem
%
% DESCRIPTION: Primal-dual method for quadratic programming
%
%            minimize c'*x + 0.5*x'*Q*x
%
%            subject to  A*x=b
%                        0<= x <= C
%            The method used here is a primal dual method with a predictor-corrector
%            approach and a logarithmic barrier. I used the heuristic from two
%            existing methods LOQO and HOPDM. The method is an iterative method. The
%            maximal number of iteration is stored in the variable 'max_iter'.
%
% ERRORS AND BUGS: 1. There is no test about the conditionning of the matrix Q. If the iteration
%                  50 has been reached, then the optimization may not be finished and the output
%                  may be wrong.
%                  2. If C contains infinite values then, the algorithm will consider that all its
%                  components are actually infinite.
%
% NOTES: 50 iterations have always been sufficient to solve all problems.
%        This code should be read with the tech. report:
%                "Regularized Symmetric Indefinite Systems in Interior Point
%                 Methods for Linear and Quadratic Optimization",
%                 A. Altman and J. Gondzio, Logilab Tech. Report 1998.6
%
% Andre Elisseeff, May. 2001

  %% init
  verbose = 0;
  n = size(Q,1);
  m = size(A,1);
  H = zeros(n+m,n+m);
  flag=1;
  maxC = max(C);

  %% Values of the original HOPDM of Gondzio and Altmann
  dinf = 10^(-14);
  smallz = 10^(-14);
  smallt = 2.3*10^(-16);
  opttol = 10^(-6);
  itref = 1;
  mu = 1;
  maxiter = 50;

  %% init values of the primal and dual variables
  x=ones(n,1);
  z=ones(n,1);
  y=ones(m,1);
  if maxC < Inf,
    s=ones(n,1);
    w=ones(n,1);
  else
    s=[];w=[];
  end;

  %% Description of variables:
  %%
  %%    x,s     -> primal variables
  %%    z,w     -> dual variables
  %%    n       -> number of variables in the initial pb (size of x)
  %%    m       -> number of constraints in A
  %%
  %%    dinf     -> smallest value for all variables
  %%    smallz   -> smallest value of z
  %%    smallt   -> smallest value for t in the computation of the matrix theta
  %%    opttol   -> acceptable tolerance for optimality conditions
  %%    itref    -> iteration counter
  %%    maxiter  -> maximum number of iteration

  %% Analyze the constraints...
  disp(sprintf('Analyzing the equality constraints...\n'));
  [QQ,RR]=qr(A',0);
  [mm,nb] = size(QQ); %% number of eq constraints
  ind = 1:1:nb;
  for i=1:nb,
    if abs(RR(i,i)) < 100*eps
      disp(sprintf('Constraints %d removed because of dependence\n',i));
      ind(i)=0;
    end;
  end;
  indice = find(ind >0);

  if (isempty(indice))
    disp(sprintf('No equality constraints... \n'));
    A=[];
    m=0;
  else
    A = A(indice,:); %% new independent eq constraints
    b = b(indice);
    y = y(indice);
    m=length(indice);
  end;
  clear QQ;clear RR;
  u = C;
  %% init values before looping
  cont = 1;
  objQ = 0.5*x'*Q*x;

  %% init values of primal and dual objective functions
  pobjo = abs(c'*x+objQ) + 1;
  if maxC < inf,
    dobjo = abs(b'*y - u'*w - objQ);
  else
    dobjo = abs(b'*y - objQ);
  end;
%%%%%%%%%%%%%%
%% MAIN LOOP
%%%%%%%%%%%%%%
  while (cont)&(itref<=maxiter)
    %% Compute the primal objective function
    objQ = 0.5*x'*Q*x;
    pobj = c'*x+objQ;
    if (maxC<Inf)
      dobj = b'*y - u'*w - objQ;
    else
      dobj = b'*y - objQ;
    end;
    dlgap = pobj - dobj;
    dp = abs(pobj)/(abs(pobjo)+1);
    dd = abs(dobj)/(abs(dobjo)+1);
    dobjo = dobj;
    pobjo = pobj;
    if verbose,
        disp(sprintf('%d - pobj : %f - dobj : %f\n',itref,pobj,dobj));
    end;
    %% Check if the solution are bounded
    if (dp > 10^6)
      disp(sprintf('Solution not bounded in the primal. Exit.\n'));
      return;
    end;
    if (dd > 10^6)
      disp(sprintf('Solution not bounded in the dual. Exit.\n'));
      return;
    end;

    %% test if optimality
    oldgap = dlgap;
    dp = abs(dobj) + 1;
    if ((abs(dlgap)/dp) <= opttol)
      disp(sprintf('Optimal solution found. Exit.\n'));
      cont = 0;
      break;
    end;

    dp = dp + abs(pobj);
    T = abs(dlgap)/dp;

    %% put the variables away from zero (from HOPDM)
    if (itref <= 3)
      ax = 2*10^(-3);
      az = 10^(-3);
    elseif (T >= 0.8)
      ax = 2*10^(-4);
      az = 10^(-4);
    elseif (T >= 0.1)
      ax = 2*10^(-5);
      az = 10^(-5);
    elseif (T >=0.01)
      ax = 2*10^(-6);
      az = 10^(-6);
    elseif (T>=0.001)
      ax = 2*10^(-7);
      az = 10^(-7);
    elseif (T>=0.0001)
      ax = 2*10^(-8);
      az = 10^(-7);
    elseif (T>=0.00001)
      ax = 2*10^(-9);
      az = 10^(-9);
    else
      ax = T*10^(-5);
      az = ax;
    end;

    %% consider only variables that can be changed
    x = x + ax;
    z = z + az;
    if maxC < Inf
      s = s + ax;
      w = w + az;
    end;
    %% Compute the values of xi_b, xi_c and xi_u
    xi_b = -A*x + b;
    xi_c = c - A'*y - z + Q*x;
    xi_z =  - x.*z;
    if maxC < Inf,
      xi_c=xi_c + w;
      xi_u = u - x - s;
      xi_w = - s.*w;
    end;
    %% Compute theta = (z/x + w/s)

    %% for bounded variables
    if maxC<Inf,
      dp = x;
      if (max(abs(dp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      dpp= s;
      if (max(abs(dpp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      theta=z./dp + w./dpp;
    end;
    %% for unbounded variables
    if (maxC==Inf),
      dp = x;
      if (max(abs(dp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      theta = z./dp;
    end;

    %% neglect small elements of theta array
    neglect = find(theta < smallt);
    if ~isempty(neglect)
      theta(neglect)=zeros(size(neglect));
    end;

    %% and control large elements of theta
        neglect = find(theta >= 10^8);
    if ~isempty(neglect)
      theta(neglect)=(10^4)*sqrt(theta(neglect));
    end;
    %% factorize H = [-Q-theta^(-1)   A^T]
    %%               [ A               0 ]

    H = zeros(n+m,n+m);
    H(1:n,1:n) = -Q-diag(theta);
    H(n+1:n+m,1:n) = A;
    H(1:n,n+1:n+m) = A';

    %% Compute the predictor step
    if maxC < Inf,
      f = xi_c-xi_z./x+(xi_w - xi_u.*w)./s ;
      h = xi_b;
    else
      f = xi_c - xi_z./x;
      h = xi_b;
    end;
    delta=H\[f;h];
    dx = delta(1:n);
    dy = delta(n+1:n+m);
    dz = (xi_z-z.*dx)./x;
    if maxC<Inf,
      ds = xi_u - dx;
      dw = (xi_w-w.*ds)./s;
    end;

   %% determine the maximum step size alpha_p (primal) and
   %% alpha_d (dual) to stay in feasible region
   %% (x,s,z,w must be positive and greater than dinf)
      indz = find(dz<0);
      indx = find(dx<0);
      inds=[];mins=1;
      indw=[];minw=1;
      if maxC < Inf,
        inds = find(ds<0);
        indw = find(dw<0);
        if ~isempty(inds)
          mins = min(-(s(inds)-dinf)./ds(inds));
        else
      mins = 1;
        end;
        if ~isempty(indw)
          minw = min(-(w(indw)-dinf)./dw(indw));
        else
      minw = 1;
        end;
      end;
      if ~isempty(indx)
        minx = min(-(x(indx)-dinf)./dx(indx));
      else
        minx = 1;
      end;
      apk = min([minx,mins,1]);
      if ~isempty(indz),
        minz = min(-(z(indz)-dinf)./dz(indz));
      else
        minz = 1;
      end;
      adk = min([minw,minz,1]);

      ax = sum(x.*z);
      as = sum((x+apk*dx).*(z+adk*dz));
      az = sum(dx.^2+dz.^2);
      if maxC < Inf,
        ax = ax + sum(s.*w);
        as = as + sum((s+apk*ds).*(w+adk*dw));
        az = az + sum(ds.^2+dw.^2);
      end;
      %% check if complementary gap is less than opttol
      if (as <= opttol)
        disp(sprintf('Complementary gap is less than %f\n',opttol));
        cont = 0;
        x = x + apk*dx;
        y = y + adk*dy;
        z = z + adk*dz;
        if maxC < Inf,
         s = s + apk*ds;
         w = w + adk*dw;
        end;
        break;
      end;

      %% Set the barrier parameter : LOQO's heuristic
      ap = min(apk,adk);
      mu = (ax/(2*n))*(0.95*(1/ap) -1)^2/(0.95*(1/ap)+10)^2;

      %% Compute the new direction (algo. of Mehrotra) of order 1 (corrector step)
      xi_z =  - x.*z + mu*ones(size(x)) - dx.*dz;
      f = xi_c - xi_z./x;
      if maxC < Inf
        xi_w = - s.*w + mu*ones(size(s)) - ds.*dw;
        f=f+ xi_w./s - (w.*xi_u)./s;
      end;
      h = xi_b;

      delta=H\[f;h];
      dx = delta(1:n);
      dy = delta(n+1:n+m);
      dz = (xi_z-z.*dx)./x;
      if maxC<Inf,
        ds = xi_u - dx;
        dw = (xi_w-w.*ds)./s;
      end;

      %% determine the maximum step size alpha_p (primal) and
      %% alpha_d (dual) to stay in feasible region
      %% (x,s,z,w must be positiv)
      indz = find(dz<0);
      indx = find(dx<0);
      if maxC < Inf,
        inds = find(ds<0);
        indw = find(dw<0);
        if ~isempty(inds)
          mins = min(-s(inds)./ds(inds));
        else
        mins=1;
        end;
        if ~isempty(indw)
          minw = min(-w(indw)./dw(indw));
        else
        minw = 1;
        end;
      end;
      if ~isempty(indx)
        minx = min(-x(indx)./dx(indx));
      else
        minx = 1;
      end;
      alpha_p = min([minx,mins,1]);
      if ~isempty(indz)
        minz = min(-z(indz)./dz(indz));
      else
        minz = 1;
      end;
      alpha_d = min([minw,minz,1]);

      %% Compute step factors
      fp = 0.9*min(alpha_p,alpha_d);
      fd = 0.9*min(alpha_d,alpha_p);
      x = x +fp*dx;
      y = y +fd*dy;
      z = z + fd*dz;
      if maxC < Inf,
        w = w + fd*dw;
        s = s +fp*ds;
      end;
      itref=itref+1;
  end;
%%%%%%%%%%%%%%%%%%%%
%% End of main loop
%%%%%%%%%%%%%%%%%%%%

    if (cont==0)
      disp(sprintf('Optimal Solution found after %d iteration.\n',itref));
      disp(sprintf('Value of the objective : %f.\n',pobj));
      flag=0;
    end;
end; %% function


function [rr,ar]=v_lpcrf2rr(rf,p);
%V_LPCRR2AR convert reflection coefs to autocorrelation coefs [RR,AR]=(RF,P)
%
% Inputs:  rf(:,n+1)  reflection coefficients: one row per frame
%          p          specifies number of rr coefficients to calculate (default=n)
% Outputs: rr(:,p+1)  autocorrelation coefficients
%          ar(:,n+1)  AR filter coefficients

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

[nf,p1]=size(rf);
p0=p1-1;
if p0
   a = rf(:,2);
   rr=[ones(nf,1) -a zeros(nf,p0-1)];
   e = (a.^2-1);
   for n = 2:p0
      k=rf(:,n+1);
      rr(:,n+1) =k.*e - sum(rr(:,n:-1:2).*a,2);
      a = [a+k(:,ones(1,n-1)).*a(:,n-1:-1:1) k];
      e = e.*(1-k.^2);
   end
   ar = [ones(nf,1) a];
   r0=sum(rr.*ar,2).^(-1);
   rr=rr.*r0(:,ones(1,p1));
   if nargin>1 && ~isempty(p)
      if p<p0
         rr(:,p+2:p1)=[];
      else
         rr=[rr zeros(nf,p-p0)];
         af=-ar(:,p1:-1:2);
         for i=p0+1:p
            rr(:,i+1)=sum(af.*rr(:,i-p0+1:i),2);
         end
      end
   end
else
   rr=ones(nf,1);
   ar=rr;
end


function [Faces, iFacesRemove] = tess_threshold(Vertices, Faces, threshArea, threshRatio, threshAngle, threshEdge)
% TESS_THRESHOLD: Detect pathological triangles in a surface mesh.
%
% INPUTS:
%    - Vertices    : [Nvert x 3] surface vertices
%    - Faces       : [Nfaces x 3] surface triangles
%    - threshArea  : Detect large triangles (area > thresh * std)
%    - threshRatio : Detect asymetric triangles (ratio perimeter/area > thresh * std)
%    - threshAngle : Detect triangles with angles that are too open (angle in degrees > thresh)

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2012-2016

% ===== PARSE INPUTS =====
if (nargin < 6) || isempty(threshEdge)
    threshEdge = [];
end
if (nargin < 5) || isempty(threshAngle)
    threshAngle = [];
end
if (nargin < 4) || isempty(threshRatio)
    threshRatio = [];
end
if (nargin < 3)
    threshArea = [];
end
iFacesRemoveArea  = [];
iFacesRemoveRatio = [];
iFacesRemoveAngle = [];
iFacesRemoveEdge  = [];

% ===== COMPUTE SURFACE STATISTICS =====
% Triangles area
triArea = tess_area(Vertices, Faces);
% Compute the vector of each edge
v1 = Vertices(Faces(:,1),:) - Vertices(Faces(:,2),:);
v2 = Vertices(Faces(:,1),:) - Vertices(Faces(:,3),:);
v3 = Vertices(Faces(:,2),:) - Vertices(Faces(:,3),:);

% ===== THRESHOLD: AREA =====
% Detect the faces that have an area above the threshold
if ~isempty(threshArea) && (threshArea > 0)
    iFacesRemoveArea = find(triArea - mean(triArea) > threshArea * std(triArea));
end

% ===== THRESHOLD: PERIMETER/AREA =====
if ~isempty(threshRatio) && (threshRatio > 0)
    % Compute perimeter again
    triPerimeter = tess_perimeter(Vertices, Faces);
    % Ratio perimeter / area
    ratio = (triPerimeter ./ triArea);
    % Detect the Faces that have an area above the threshold
    iFacesRemoveRatio = find(ratio - mean(ratio) > threshRatio * std(ratio));
end

% ===== THRESHOLD: ANGLE =====
if ~isempty(threshAngle) && (threshAngle > 0)
    % Compute the angle between all the vectors
    maxAngle = zeros(size(Vertices,1),1);
    for i = 1:size(v1,1)
        maxAngle(i) = max([atan2(norm(cross(v1(i,:),v2(i,:))), dot(v1(i,:),v2(i,:))), ...
                        atan2(norm(cross(v1(i,:),v3(i,:))), dot(v1(i,:),v3(i,:))), ...
                        atan2(norm(cross(v2(i,:),v3(i,:))), dot(v2(i,:),v3(i,:)))]);
    end
    % Convert to degrees
    maxAngle = maxAngle / 2 / pi * 360;
    % Detect the Faces that have an area above the threshold
    iFacesRemoveAngle = find(maxAngle > threshAngle);
end

% ===== THRESHOLD: EDGE LENGTH =====
if ~isempty(threshEdge) && (threshEdge > 0)
    % Compute the length of all the edges
    edgeLength = sqrt(v1.^2 + v2.^2 + v3.^2);
    % Split long edges
    iFacesRemoveEdge = find(edgeLength - mean(edgeLength) > threshEdge * std(edgeLength));
end

% List of faces to remove
iFacesRemove = [iFacesRemoveArea(:); iFacesRemoveRatio(:); iFacesRemoveAngle(:)];
% Keep only the good faces
if ~isempty(iFacesRemove)
     Faces(iFacesRemove,:) = [];
end


% COMPUTE_ERSP_TIMES - computes the widest possible ERSP/ITC time window,
%        which depends on requested ERSP/ITC parameters such as epoch limits,
%        frequency range, wavelet parameters, sampling rate and frequency
%        resolution that are used by TIMEF.
%        This helper function is called by POP_PRECLUST & STD_ERSP.
% Example:
%    [time_range, winsize] = compute_ersp_times(cycles,  ALLEEG(seti).srate, ...
%                              [ALLEEG(seti).xmin ALLEEG(seti).xmax]*1000, freq(1),padratio);
%
% Authors: Hilit Serby & Arnaud Delorme, SCCN, INC, UCSD, Feb 03, 2005

% Copyright (C) Hilit Serby, SCCN, INC, UCSD, Feb 03, 2005, hilit@sccn.ucsd.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. 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 HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [time_range, winsize] = compute_ERSP_times(cycles, srate, epoch_lim, lowfreq, padratio)

if cycles == 0 %FFT option
    if ~exist('padratio')
        error('You must enter padratio value for FFT ERSP');
    end
    lowfreq = lowfreq*padratio;
    t = 1/lowfreq;%time window in sec
    winsize = t*srate;%time window in points
    %time window in points (must be power of 2) for FFT
    winsize =pow2(nextpow2(winsize));
    %winsize =2^round(log2(winsize));
else %wavelet
    t = cycles(1)/lowfreq; %time window in sec
    winsize  = round(t*srate); %time window in points
end

time_range(1) = epoch_lim(1) + .5*t*1000;
time_range(2) = epoch_lim(2) - .5*t*1000;


function f = ddivWexp(Wxk_r, varargin)
% function f = ddivW(Wxk_r, varargin)
% Wxk_r = reshape(Wxk,1,x*k) -> row vector
% Vxt = varargin{1};        %data
% Hkt = varargin{2};        %H matrix
% Wxk_fix=varargin{3};      %fixed part of the Wxk matrix (e.g. background)
% Hkt_fix = varargin{4};    %fixed part (lines) of the H matrix (e.g. background)

Vxt = varargin{1};      %data
Hkt_tmp = varargin{2};  %H matrix
Wxk_fix = varargin{3};  %fixed part of the Wxk matrix (e.g. background) ->rows
Hkt_fix = varargin{4};  %fixed part (lines) of the H matrix (e.g. background)
peval = varargin{5}; %parameters

if ~isfield(peval, 'w_lambda') peval.w_lambda=0; end

x=size(Vxt,1);
k=length(peval.w_dovec);

Wxk_tmp = exp(reshape(Wxk_r,x,k));

Wxk = zeros(peval.numpix, peval.ncomp+1);
Hkt = zeros(peval.ncomp+1, peval.nt);

Wxk(:,peval.w_dovec)=Wxk_tmp;
Hkt(peval.h_dovec,:)=Hkt_tmp;

Wxk(:,peval.w_fixvec)=Wxk_fix;
Hkt(peval.h_fixvec,:)=Hkt_fix;

%%%%
% sumw = sum(Wxk,1);
% Wxk = Wxk./repmat(sumw,size(Wxk,1),1);

% fxt = Vxt.*log(Vxt./(Wxk*Hkt))-Vxt+Wxk*Hkt; %d-divergence
% f = sum(fxt(:));
% peval.w_lambda*Wxk.^2
f = ddivergence(Vxt,Wxk*Hkt);


function VOV = generateVOV(obj,theta)
% Generate the virtual objective vectors

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Tomoaki Takagi

    obj    = normalize(obj,'range');
    [N,M]  = size(obj);
    [W,N2] = UniformPoint(2e4,M,'ILD');
    VOV    = zeros(N2,M);
    flag   = false(N2,1);

    normW   = sqrt(sum(W.^2,2));
    normObj = sqrt(sum(obj.^2,2));
    for i = 1 : N2
        CosineVOV = sum(obj.*repmat(W(i,:),N,1),2)./normW(i,:)./normObj;
        d2 = normObj.*sqrt(1-CosineVOV.^2);
        [mind2,I] = min(d2);
        d1 = normObj(I)*CosineVOV(I);
        r = d1/norm(W(i,:));

        VOV(i,:) = W(i,:).*r;
        flag(i)  = mind2 < theta;
    end
    VOV = VOV(flag,:);
end


function RHSdiv = divergenceTermCylindrical1D(F)
% This function calculates the divergence of a field using its face
% average value and the vector u, which is a face vector
%
% SYNOPSIS:
%       RHSdiv = divergenceTermCylindrical1D(F)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nr = F.domain.dims(1);
G = 1:Nr+2;
DX = F.domain.cellsize.x(2:end-1);
rp = F.domain.cellcenters.x;
rf = F.domain.facecenters.x;

% define the vector of cell index
row_index = reshape(G(2:Nr+1),Nr,1); % main diagonal (only internal cells)

% calculate the flux vector
% note: size(Fx) = [1:m+1]
Fx = F.xvalue;

% reassign the east, west, north, and south flux vectors for the
% code readability
Fe = Fx(2:Nr+1);		Fw = Fx(1:Nr);
re = rf(2:Nr+1);     rw = rf(1:Nr);

% compute the divergence
div_x = (re.*Fe - rw.*Fw)./(rp.*DX);

% define the RHS Vector
RHSdiv = zeros(Nr+2,1);

% assign the values of the RHS vector
RHSdiv(row_index) = reshape(div_x,Nr,1);


function result = simplex_unit_05_nd ( func, n )

%*****************************************************************************80
%
%% SIMPLEX_UNIT_05_ND approximates an integral inside a unit simplex in ND.
%
%  Integration region:
%
%    The unit simplex in N dimensions,
%      0 <= X(1:N),
%      Sum ( X(1:N) ) <= 1.
%
%  Discussion:
%
%    An N^2 + 3 N + 3 point formula of degree 5 is used.  This is
%    Stroud formula TN:5-1.
%
%    (For N = 2, the number of points is actually only 7, and
%     for N = 3, the number of points is actually only 15.)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2004
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    A Fifth Degree Integration Formula for the N-Simplex,
%    SIAM Journal on Numerical Analysis,
%    Volume 6, Number 1, March 1969.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F(X) at the N-dimensional point
%    X, of the form
%      function value = func ( n, x )
%
%    Input, integer N, the dimension of the space.  For this routine,
%    it must be the case that 2 <= N <= 16.
%
%    Output, real RESULT, the approximate integral of the function.
%
  coef1 = [ ...
    0.0E+00,             0.225E+00, ...
    0.118518518519E+00,  0.0631521898883E+00, ...
    0.235714285714E+00,  0.791575476992E+00, ...
    1.85798728021E+00,   3.53666958042E+00, ...
    5.90844340844E+00,   9.03765432098E+00, ...
    12.9758241758E+00,  17.7645108738E+00, ...
    23.4375030259E+00,  30.0224941950E+00, ...
    37.5423613501E+00,  46.0161454949E+00 ];
  coef21 = [ ...
    0.0E+00,              0.12593918054483E+00, ...
    0.0719370837790E+00,  0.0470456145702E+00, ...
    0.0333009774677E+00,  0.0248633014592E+00, ...
    0.0192679696358E+00,  0.0153322153879E+00, ...
    0.0124316229901E+00,  0.0102112988361E+00, ...
    0.00845730697460E+00, 0.00703433430999E+00, ...
    0.00585330520067E+00, 0.00485356735291E+00, ...
    0.00399261092720E+00, 0.00323988713017E+00 ];
  coef22 = [ ...
    0.0E+00,              0.13239415278851E+00, ...
    0.0690682072263E+00,  0.0371530185868E+00, ...
   -0.0719253160920E+00, -0.264323879461E+00, ...
   -0.537926779961E+00,  -0.886895605701E+00, ...
   -1.30409181465E+00,   -1.78227048964E+00, ...
   -2.31462336314E+00,   -2.89499045158E+00, ...
   -3.51790849765E+00,   -4.17858310668E+00, ...
   -4.87282884913E+00,   -5.59699944261E+00 ];
  coef31 = [ ...
    0.0E+00,             0.0E+00, ...
    0.0529100529100E+00, 0.0261368740713E+00, ...
    0.0499020181331E+00, 0.0782233395867E+00, ...
    0.109041040862E+00,  0.140874828568E+00,  ...
    0.172735353396E+00,  0.203992490408E+00,  ...
    0.234263814181E+00,  0.263332763315E+00,  ...
    0.291091849264E+00,  0.317504208212E+00,  ...
    0.342577872069E+00,  0.366348654344E+00 ];
  coef32 = [ ...
    0.0E+00,              0.0E+00, ...
    0.0E+00,              0.0254485903613E+00, ...
    0.0165000982690E+00,  0.0115218303668E+00,  ...
    0.00850478779483E+00, 0.00655297510968E+00, ...
    0.00522372456259E+00, 0.00428017828134E+00, ...
    0.00358722367033E+00, 0.00306362964360E+00, ...
    0.00265836687133E+00, 0.00233816221525E+00, ...
    0.00208061510846E+00, 0.00187022027571E+00 ];

  if ( n < 2 | 16 < n )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SIMPLEX_UNIT_05_ND - Fatal error!\n' );
    fprintf ( 1, '  Input spatial dimension N out of range.\n' );
    fprintf ( 1, '  N = %d\n', n );
    error ( 'SIMPLEX_UNIT_05_ND - Fatal error!' );
  end

  quad = 0.0;
%
%  S1
%
  x(1:n) = 1.0E+00 / ( n + 1 );
  quad = quad + coef1(n) * feval ( func, n, x );
%
%  S21
%
  r1 = ( ( n + 4 ) - sqrt ( 15.0E+00 ) ) / ( n * n + 8 * n + 1 );
  s1 = 1.0E+00 - n * r1;

  x(1:n) = r1;

  for i = 1 : n + 1

    quad = quad + coef21(n) * feval ( func, n, x );

    if ( 1 < i )
      x(i-1) = r1;
    end

    if ( i < n + 1 )
      x(i) = s1;
    end

  end
%
%  S22
%
  r2 = ( n + 4 + sqrt ( 15.0E+00 ) ) / ( n * n + 8 * n + 1 );
  s2 = 1.0E+00 - n * r2;

  x(1:n) = r2;

  for i = 1 : n + 1

    quad = quad + coef22(n) * feval ( func, n, x );

    if ( 1 < i )
      x(i-1) = r2;
    end

    if ( i < n + 1 )
      x(i) = s2;
    end

  end
%
%  S31
%
  u1 = ( n + 7 + 2.0E+00 * sqrt ( 15.0E+00 ) ) / ( n * n + 14 * n - 11 );
  v1 = ( ( 4 * n - 2 ) - ( n - 1 ) * sqrt ( 15.0E+00 ) ) / ( n * n + 14 * n - 11 );

  for i = 1 : n

    x(1:n) = u1;
    x(i) = v1;

    for j = i : n

      if ( i < j - 1 )
        x(j-1) = u1;
      end

      x(j) = v1;

      quad = quad + coef31(n) * feval ( func, n, x );

    end

  end
%
%  S32
%
  u2 = ( n + 7 - 2.0E+00 * sqrt ( 15.0E+00 ) ) / ( n^2 + 14 * n - 11 );
  v2 = ( ( 4 * n - 2 ) + ( n - 1 ) * sqrt ( 15.0E+00 ) ) / ( n^2 + 14 * n - 11 );

  for i = 1 : n

    x(1:n) = u2;
    x(i) = v2;

    for j = i : n

      if ( i < j - 1 )
        x(j-1) = u2;
      end

      x(j) = v2;

      quad = quad + coef32(n) * feval ( func, n, x );

    end

  end

  volume = simplex_unit_volume_nd ( n );

  result = quad * volume;

  return
end