function [varsigma] = ppcaPosteriorVar(model, X);

% PPCAPOSTERIORVAR Mean and variances of the posterior at points given by X.
% FORMAT
% DESC returns the posterior mean and variance for a given set of
% points.
% ARG model : the model for which the posterior will be computed.
% ARG x : the input positions for which the posterior will be
% computed.
% RETURN sigma : the variances of the posterior distributions.
%
% SEEALSO : ppcaCreate, ppcaPosteriorMeanVar
%
% COPYRIGHT : Neil D. Lawrence, 2008

% MLTOOLS

varsigma = repmat(1/model.beta, size(X, 1), 1);


function varargout = det(varargin)
%DET (overloaded)
%
% t = DET(X)

switch class(varargin{1})

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.

        X = varargin{1};
        [n,m] = size(X);
        if n~=m
            error('Matrix must be square.')
        end
        if nargin == 2
            if strcmp(varargin{2},'polynomial')
                varargout{1} = polynomialform(X);
                return
            else
                error('If you use two arguments in @sdpvar/det, the second should be ''polynomial''');
            end
        end
        if n==1
            varargout{1} = X;
            return
        else
            y = yalmip('define','det_internal',reshape(X,[],1));
        end
        varargout{1} = y;

    otherwise
end

function d = polynomialform(X)

n = X.dim(1);
m = X.dim(2);

if n~=m
    error('Matrix must be square.');
else
    switch n
        case 1
            d = X;
        case 2
            % Freakin overloading on multiplication doesn't work. Probalby
            % stupid code...
            Y1.type = '()';
            Y2.type = '()';
            Y3.type = '()';
            Y4.type = '()';
            Y1.subs = {1,1};
            Y2.subs = {2,2};
            Y3.subs = {1,2};
            Y4.subs = {2,1};
            d = subsref(X,Y1)*subsref(X,Y2)-subsref(X,Y3)*subsref(X,Y4);
        otherwise
            d = 0;
            Y.type = '()';
            for i = 1:n
                Y.subs = {i,1};
                xi = subsref(X,Y);
                if ~isequal(xi,0)
                    Y.subs = {[1:1:i-1 i+1:1:n],2:n};
                    subX = subsref(X,Y);
                    if isa(subX,'sdpvar')
                        d = d + (-1)^(i+1)*xi*polynomialform(subX);
                    else
                        d = d + (-1)^(i+1)*xi*det(subX);
                    end
                end
            end
    end
end
% Reset info about conic terms
if isa(d,'sdpvar')
    d.conicinfo = [0 0];
end


function check = discrete_check ( a, b )

%*****************************************************************************80
%
%% DISCRETE_CHECK checks the parameters of the Discrete CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer A, the number of probabilities assigned.
%
%    Input, real B(A), the relative probabilities of
%    outcomes 1 through A.  Each entry must be nonnegative.
%
%    Output, logical CHECK, is true if the parameters are legal.
%
  for j = 1 : a
    if ( b(j) < 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'DISCRETE_CHECK - Fatal error!\n' );
      fprintf ( 1, '  Negative probabilities not allowed.\n' );
      check = 0;
      return
    end
  end

  b_sum = sum ( b(1:a) );

  if ( b_sum == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DISCRETE_CHECK - Fatal error!\n' );
    fprintf ( 1, '  Total probablity is zero.\n' );
    check = 0;
    return
  end

  check = 1;

  return
end


function p29_title ( )

%*****************************************************************************80
%
%% P29_TITLE prints a title for problem 29.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Problem 29\n' );
  fprintf ( 1, '  Name:       Genz #3 / Patterson #8, Corner Peak\n' );
  fprintf ( 1, '  Region:     0 <= X(i) <= 1\n' );
  fprintf ( 1, '  Integrand:  F(X) = 1 / ( 1 + sum( C(i) * X(i) ) )^R\n' );
  fprintf ( 1, '  Parameters:\n' );
  fprintf ( 1, '              R, defaults to 0.3\n' );
  fprintf ( 1, '              C(1:DIM_NUM) defaults to 1/DIM_NUM.\n' );

  return
end


function y = tifs2cv(f,m,d,q)
%TIFS2CV Compresses a multi-frame TIFF image sequence.
%   Y = TIFS2CV(F,M,D,Q) compresses multiframe TIFF F using motion
%   compensated frames, 8 x 8 DCT transforms, and Huffman coding. If
%   parameter Q is omitted or is 0, only Huffman encoding is used and
%   the compression is lossless; for Q > 0, lossy JPEG encoding is
%   performed. The inputs are:
%
%      F     A multi-frame TIFF file        (e.g., 'file.tif')
%      M     Macroblock size                (e.g., 8)
%      D     Search displacement            (e.g., [16 8])
%      Q     JPEG quality for IM2JPEG       (e.g., 1)
%
%   Output Y is an encoding structure with fields:
%
%      Y.blksz      Size of motion compensation blocks
%      Y.frames     The number of frames in the image sequence
%      Y.quality    The reconstruction quality
%      Y.motion     Huffman encoded motion vectors
%      Y.video      An array of MAT2HUFF or IM2JPEG coding structures
%
%   See also CV2TIFS.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% The default reconstruction quality is lossless.
if nargin < 4
    q = 0;
end

% Compress frame 1 and reconstruct for the initial reference frame.
if q == 0
    cv(1) = mat2huff(imread(f,1));
    r = double(huff2mat(cv(1)));
else
    cv(1) = im2jpeg(imread(f,1),q);
    r = double(jpeg2im(cv(1)));
end
fsz = size(r);

% Verify that image dimensions are multiples of the macroblock size.
if ((mod(fsz(1),m) ~= 0) || (mod(fsz(2),m) ~= 0))
    error('Image dimensions must be multiples of the block size.');
end

% Get the number of frames and preallocate a motion vector array.
fcnt = size(imfinfo(f),1);
mvsz = [fsz/m 2 fcnt];
mv = zeros(mvsz);

% For all frames except the first, compute motion conpensated
% prediction residuals and compress with motion vectors.
for i = 2:fcnt
    frm = double(imread(f,i));
    frmC = im2col(frm,[m m],'distinct');
    eC = zeros(size(frmC));

    for col = 1:size(frmC,2)
        lookfor = col2im(frmC(:,col),[m m],[m m],'distinct');

        x = 1 + mod(m * (col - 1),fsz(1));
        y = 1 + m * floor((col - 1) * m / fsz(1));
        x1 = max(1,x - d(1));
        x2 = min(fsz(1),x + m + d(1) - 1);
        y1 = max(1,y - d(2));
        y2 = min(fsz(2),y + m + d(2) - 1);

        here = r(x1:x2,y1:y2);
        hereC = im2col(here,[m m],'sliding');
        for j = 1:size(hereC,2)
            hereC(:,j) = hereC(:,j) - lookfor(:);
        end
        sC = sum(abs(hereC));
        s = col2im(sC,[m m],size(here),'sliding');
        mins = min(min(s));
        [sx,sy] = find(s == mins);

        ns = abs(sx) + abs(sy);         % Get the closest vector
        si = find(ns == min(ns));
        n = si(1);

        mv(1 + floor((x - 1)/m), 1 + floor((y - 1)/m), 1:2, i) = ...
            [x - (x1 + sx(n) - 1) y - (y1 + sy(n) - 1)];
        eC(:,col) = hereC(:, sx(n) + (1 + size(here,1) - m) ...
            * (sy(n) - 1));
    end

    % Code the prediction residual and reconstruct it for use in
    % forming the next reference frame.
    e = col2im(eC,[m m],fsz,'distinct');
    if q == 0
        cv(i) = mat2huff(int16(e));
        e = double(huff2mat(cv(i)));
    else
        cv(i) = im2jpeg(uint16(e + 255),q,9);
        e = double(jpeg2im(cv(i)) - 255);
    end

    % Decode the next reference frame. Use the motion vectors to get
    % the subimages needed to subtract from the prediction residual.
    rC = im2col(e,[m m],'distinct');
    for col = 1:size(rC,2)
        u = 1 + mod(m * (col - 1),fsz(1));
        v = 1 + m * floor((col - 1) * m / fsz(1));
        rx = u - mv(1 + floor((u - 1)/m), 1 + floor((v - 1)/m), 1, i);
        ry = v - mv(1 + floor((u - 1)/m), 1 + floor((v - 1)/m), 2, i);
        temp = r(rx:rx + m - 1,ry:ry + m - 1);
        rC(:,col) = temp(:) - rC(:,col);
    end
    r = col2im(double(uint16(rC)),[m m],fsz,'distinct');
end

y = struct;
y.blksz = uint16(m);
y.frames = uint16(fcnt);
y.quality = uint16(q);
y.motion = mat2huff(mv(:));
y.video = cv;


function [X,E,obj,err,iter] = lrtcR_tnn(M,omega,lambda,opts)

% Solve the Noisy Low-Rank Tensor Completion (LRTC) problem by ADMM
%
% min_{X,E} ||X||_*+lambda*loss(E), s.t. P_Omega(X) + E = M.
% loss(E) = ||E||_1 or 0.5*||E||_F^2
%
% ---------------------------------------------
% Input:
%       M       -    d1*d2*d3 tensor
%       omega   -    index of the observed entries
%       lambda  -    >=0, parameter
%       opts    -    Structure value in Matlab. The fields are
%           opts.loss       -   'l1' (default): loss(E) = ||E||_1
%                               'l2': loss(E) = 0.5*||E||_F^2
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       X       -    d1*d2*d3 tensor
%       E       -    d1*d2*d3 tensor
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 27/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;
loss = 'l1';

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'loss');        loss = opts.loss;            end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

dim = size(M);
X = zeros(dim);
Z = X;
E = X;
Y1 = X;
Y2 = X;
omegac = setdiff(1:prod(dim),omega);

iter = 0;
for iter = 1 : max_iter
    Xk = X;
    Zk = Z;
    Ek = E;
    % first super block {X,E}
    [X,tnnX] = prox_tnn(Z-Y2/mu,1/mu);
    temp = M-Y1/mu;
    temp(omega) = temp(omega)-Z(omega);
    if strcmp(loss,'l1')
        E = prox_l1(temp,lambda/mu);
    elseif strcmp(loss,'l2')
        E = temp*(mu/(lambda+mu));
    else
        error('not supported loss function');
    end

    % second super block {Z}
    Z(omega) = (-E(omega)+M(omega)-(Y1(omega)-Y2(omega))/mu+X(omega))/2;
    Z(omegac) = X(omegac)+Y2(omegac)/mu;

    dY1 = E-M;
    dY1(omega) = dY1(omega)+Z(omega);
    dY2 = X-Z;
    chgX = max(abs(Xk(:)-X(:)));
    chgE = max(abs(Ek(:)-E(:)));
    chgZ = max(abs(Zk(:)-Z(:)));
    chg = max([chgX chgE chgZ max(abs(dY1(:))) max(abs(dY2(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = tnnX+lambda*comp_loss(E,loss);
            err = sqrt(norm(dY1(:))^2+norm(dY2(:))^2);
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y1 = Y1 + mu*dY1;
    Y2 = Y2 + mu*dY2;
    mu = min(rho*mu,max_mu);
end
obj = tnnX+lambda*comp_loss(E,loss);
err = sqrt(norm(dY1(:))^2+norm(dY2(:))^2);


%FEATSELI Trainable mapping for individual feature selection
%
%   [W,R] = FEATSELI(A,CRIT,K,T)
%   [W,R] = A*FEATSELI([],CRIT,K,T)
%   [W,R] = A*FEATSELI(CRIT,K,T)
%   [W,R] = FEATSELI(A,CRIT,K,N)
%   [W,R] = A*FEATSELI([],CRIT,K,N)
%   [W,R] = A*FEATSELI(CRIT,K,N)
%
% INPUT
%   A    Training dataset
%   CRIT Name of the criterion or untrained mapping
%        (default: 'NN', i.e. the LOO 1-Nearest Neighbor error)
%   K    Number of features to select (default: sort all features)
%   T    Tuning dataset (optional)
%   N    Number of cross-validations (optional)
%
% OUTPUT
%   W    Feature selection mapping
%   R    Matrix with criterion values
%
% DESCRIPTION
% Individual selection of K features using the dataset A. CRIT sets the
% criterion used by the feature evaluation routine FEATEVAL. If the dataset
% T is given, it is used as test set for FEATEVAL. For K = 0 all features are
% selected, but reordered according to the criterion. The result W can be
% used for selecting features using B*W.
% The selected features are stored in W.DATA and can be found by +W.
% In R, the search is reported step by step as:
%
% 	R(:,1) : number of features
% 	R(:,2) : criterion value
% 	R(:,3) : added / deleted feature
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, FEATEVAL, FEATSELO, FEATSELB, FEATSELF,
% FEATSEL, FEATSELP,FEATSELM

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Sciences, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% $Id: featseli.m,v 1.5 2009/07/01 07:48:05 davidt Exp $

function [w,r] = featseli(varargin)

  varargin = shiftargin(varargin,{'char','prmapping'});
  argin = setdefaults(varargin,[],'NN',0,[]);
  if mapping_task(argin,'definition')
    w = define_mapping(argin,'untrained','Individual FeatSel');
    return
  end

  [a,crit,ksel,t] = deal(argin{:});

	[m,k,c] = getsize(a); featlist = getfeatlab(a);

	% If KSEL is not given, return all features.

	if (ksel == 0), ksel = k; end

	isvaldfile(a,1,2); % at least 1 object per class, 2 classes
	a = testdatasize(a);
	if isdataset(t), iscomdset(a,t); end

	critval = zeros(k,1);

	% Evaluate each feature in turn.

	s = sprintf('Evaluation of %i features: ',k);
	prwaitbar(k,s);
  a = setprior(a,getprior(a));
	if (isempty(t))
		for j = 1:k
			prwaitbar(k,j,[s int2str(j)]);
			critval(j) = feateval(a(:,j),crit);
		end
  elseif isdataset(t)
		for j = 1:k
			prwaitbar(k,j,[s int2str(j)]);
			critval(j) = feateval(a(:,j),crit,t(:,j));
    end
  else
		for j = 1:k
			prwaitbar(k,j,[s int2str(j)]);
			critval(j) = feateval(a(:,j),crit,t);
    end
	end
	prwaitbar(0);

	% Sort the features by criterion value (maximum first).
	[critval_sorted,J] = sort(-critval);
	r = [[1:k]', -critval_sorted, J];
	J = J(1:ksel)';

	% Return the mapping found.
	w = featsel(k,J);
  w = setmapping_type(w,'trained');
  w = setsize(w,[k length(J)]);
	if ~isempty(featlist)
		w = setlabels(w,featlist(J,:));
	end
	w = setname(w,'Individual FeatSel');

return


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Max manipulability index ALONG A LINE.
% Use stomp like to optimize along a surface/line
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function experiment1B
close all;
global robot
global parameters
global hfigures

%STOMP PARAMETERS
%conversion from cost to Prob factor
parameters.lambda = .4;
parameters.lambda_obstacles = .2;
%height of the obstacle
parameters.yo = 2.5;
%cost function starts at this distance
%must be below 0.3 for the 4 DOF robot
parameters.epsilon = 0.2;
%multiply noise by this facto
%parameters.noise_k = 5;
%parameters.noise_sigma_null_space = 0.01;
parameters.alpha=0.02;
parameters.time_step=0.01;

%number of waypoints
parameters.N = 12;
%number of particles
parameters.K = 10;
parameters.n_repeat = 30;
parameters.experiment_name = 'experiment1B_K10_N30.mat';

parameters.animate = 0;

parameters.obstacles = [];

%LINE 1
x1 = -1.5;
y1 = .5; %m
x2 = 0;
y2 = 2; %m
phi = 3*pi/4;
p0 = [x1 y1 0]';
pf = [x2 y2 0]';
T0 = build_T_4dof(p0, phi);
parameters.obstacles{1}.line = [p0 pf];
parameters.obstacles{1}.T0 = T0;

%repeat the experiment E times
random_manips=[];
Gout = [];
for i=1:parameters.n_repeat
    close all
    [pk, final_manip] = stomp_null_space(robot);
    Gout{i}=pk;
    random_manips = [random_manips; final_manip];
    save(parameters.experiment_name)
end

function T = build_T_4dof(p, phi)
T = [cos(phi) -sin(phi) 0 p(1);
     sin(phi) cos(phi) 0 p(2);
     0            0     1  p(3);
     0             0    0   1];

 function T = build_T_sawyer(p, phi)
T = [1  -sin(phi) 0 p(1);
     0  cos(phi) 0 p(2);
     0            0     1  p(3);
     0             0    0   1];


classdef LIRCMOP14 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP with large infeasible regions

%------------------------------- Reference --------------------------------
% Z. Fan, W. Li, X. Cai, H. Huang, Y. Fang, Y. You, J. Mo, C. Wei, and E.
% Goodman, An improved epsilon constraint-handling method in MOEA/D for
% CMOPs with large infeasible regions, Soft Computing, 2019, 23:
% 12491-12510.
%------------------------------- 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 Wenji Li

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 3;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            [popsize,variable_length] = size(X);
            sum1 = zeros(popsize,1);
            for j = 3 : variable_length
                sum1 = sum1+10*(X(:,j)-0.5).^2;
            end
            PopObj(:,1) = (1.7057+sum1).*cos(0.5*pi*X(:,1)).*cos(0.5*pi*X(:,2));
            PopObj(:,2) = (1.7057+sum1).*cos(0.5*pi*X(:,1)).*sin(0.5*pi*X(:,2));
            PopObj(:,3) = (1.7057+sum1).*sin(0.5*pi*X(:,1));
            gx          = PopObj(:,1).^2+PopObj(:,2).^2+PopObj(:,3).^2;
            PopCon(:,1) = (gx-9).*(4-gx);
            PopCon(:,2) = (gx-3.61).*(3.24-gx);
            PopCon(:,3) = (gx-3.0625).*(2.56-gx);
            Population  = SOLUTION(X,PopObj,PopCon,varargin{2:end});
            obj.FE      = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,3);
            R = sqrt(3.0625)*R./repmat(sqrt(sum(R.^2,2)),1,3);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            a = linspace(0,pi/2,10)';
            R = {sin(a)*cos(a')*sqrt(3.0625),sin(a)*sin(a')*sqrt(3.0625),cos(a)*ones(size(a'))*sqrt(3.0625)};
        end
    end
end


function i4vec_indexed_heap_d_insert_test ( )

%*****************************************************************************80
%
%% I4VEC_INDEXED_HEAP_D_INSERT_TEST tests I4VEC_INDEXED_HEAP_D_INSERT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2014
%
%  Author:
%
%    John Burkardt
%
  m = 20;
  n_max = 20;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_INDEXED_HEAP_D_INSERT_TEST\n' );
  fprintf ( 1, '  For an indexed I4VEC,\n' );
  fprintf ( 1, '  I4VEC_INDEXED_HEAP_D_INSERT inserts a value into the heap.\n' );
%
%  Set the data array.  To keep things easy, we will use the indicator vector.
%
  a = i4vec_indicator1 ( m );
%
%  The index array will initially be a random subset of the numbers 1 to M,
%  in random order.
%
  n = 5;
  indx(1:11,1) = [ 9, 2, 8, 14, 5, 7, 15, 1, 19, 20, 3 ]';

  i4vec_print ( m, a, '  The data vector:' );
  i4vec_print ( n, indx, '  The index vector:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(INDX):\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %4d\n', i, a(indx(i)) );
  end
%
%  Create a descending heap from the indexed array.
%
  indx = i4vec_indexed_heap_d ( n, a, indx );

  i4vec_print ( n, indx, '  The index vector after heaping:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(INDX) after heaping:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %4d\n', i, a(indx(i)) );
  end
%
%  Insert five entries, and monitor the maximum.
%
  for i = 1 : 5

    indx_insert = indx(n+1);

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Inserting value %d\n', a(indx_insert) );

    [ n, indx ] = i4vec_indexed_heap_d_insert ( n, a, indx, indx_insert );

    indx_max = i4vec_indexed_heap_d_max ( n, a, indx );

    fprintf ( 1, '  Current maximum is %d\n', a(indx_max) );

  end
  i4vec_print ( m, a, '  The data vector after insertions:' )
  i4vec_print ( n, indx, '  The index vector after insertions:' )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(INDX) after insertions:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %4d\n', i, a(indx(i)) );
  end

  return
end


function [ ap, kpvt, info ] = sspfa ( ap, n )

%*****************************************************************************80
%
%% SSPFA factors a real symmetric matrix stored in packed form.
%
%  Discussion:
%
%    To solve A*X = B, follow SSPFA by SSPSL.
%
%    To compute inverse(A)*C, follow SSPFA by SSPSL.
%
%    To compute determinant(A), follow SSPFA by SSPDI.
%
%    To compute inertia(A), follow SSPFA by SSPDI.
%
%    To compute inverse(A), follow SSPFA by SSPDI.
%
%  Packed storage:
%
%    The following program segment will pack the upper triangle of a
%    symmetric matrix.
%
%      k = 0
%      do j = 1, n
%        do i = 1, j
%          k = k + 1
%          ap(k) = a(i,j)
%        end
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real AP(N*(N+1)/2), the packed form of a
%    symmetric matrix A.  The columns of the upper triangle are stored
%    sequentially in a one-dimensional array.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real AP(N*(N+1)/2), a block diagonal matrix and the
%    multipliers which were used to obtain it stored in
%    packed form.  The factorization can be written A = U*D*U' where U
%    is a product of permutation and unit upper triangular matrices, U'
%    is the transpose of U, and D is block diagonal with 1 by 1 and 2
%    by 2 blocks.
%
%    Output, integer KPVT(N), the pivot indices.
%
%    Output, integer INFO, error flag.
%    0, normal value.
%    K, if the K-th pivot block is singular.  This is not an error
%    condition for this subroutine, but it does indicate that SSPSL or
%    SSPDI may divide by zero if called.
%

%
%  ALPHA is used in choosing pivot block size.
%
  alpha = ( 1.0 + sqrt ( 17.0 ) ) / 8.0;

  info = 0;
%
%  Main loop on K, which goes from N to 1.
%
  k = n;
  ik = floor ( ( n * ( n - 1 ) ) / 2 );

  while ( 1 )
%
%  Leave the loop if K = 0 or K = 1.
%
    if ( k == 0 )
      break
    end

    if ( k == 1 )
      kpvt(1) = 1;
      if ( ap(1) == 0.0 )
        info = 1;
      end
      break
    end
%
%  This section of code determines the kind of elimination to be performed.
%  When it is completed, KSTEP will be set to the size of the pivot block,
%  and SWAP will be set to .true. if an interchange is required.
%
    km1 = k - 1;
    kk = ik + k;
    absakk = abs ( ap(kk) );
%
%  Determine the largest off-diagonal element in column K.
%
    imax = isamax ( k-1, ap(ik+1:ik+k-1), 1 );
    imk = ik + imax;
    colmax = abs ( ap(imk) );

    if ( alpha * colmax <= absakk )

      kstep = 1;
      swap = 0;
%
%  Determine the largest off-diagonal element in row IMAX.
%
    else

      rowmax = 0.0;
      imaxp1 = imax + 1;
      im = floor ( imax * ( imax - 1 ) ) / 2;
      imj = im + 2 * imax;

      for j = imaxp1 : k
        rowmax = max ( rowmax, abs ( ap(imj) ) );
        imj = imj + j;
      end

      if ( imax ~= 1 )
        jmax = isamax ( imax-1, ap(im+1:im+imax-1), 1 );
        jmim = jmax + im;
        rowmax = max ( rowmax, abs ( ap(jmim) ) );
      end

      imim = imax + im;

      if ( alpha * rowmax <= abs ( ap(imim) ) )
        kstep = 1;
        swap = 1;
      elseif ( alpha * colmax * ( colmax / rowmax ) <= absakk )
        kstep = 1;
        swap = 0;
      else
        kstep = 2;
        swap = ( imax ~= km1 );
      end

    end
%
%  Column K is zero.  Set INFO and iterate the loop.
%
    if ( max ( absakk, colmax ) == 0.0 )

      kpvt(k) = k;
      info = k;

    else

      if ( kstep ~= 2 )
%
%  1 x 1 pivot block.
%
        if ( swap )
%
%  Perform an interchange.
%
          temp(1:imax) = ap(im+1:im+imax);
          ap(im+1:im+imax) = ap(ik+1:ik+imax);
          ap(ik+1:ik+imax) = temp(1:imax);

          imj = ik + imax;

          for jj = imax : k
            j = k + imax - jj;
            jk = ik + j;
            t = ap(jk);
            ap(jk) = ap(imj);
            ap(imj) = t;
            imj = imj - ( j - 1 );
          end

        end
%
%  Perform the elimination.
%
        ij = ik - ( k - 1 );

        for jj = 1 : km1
          j = k - jj;
          jk = ik + j;
          mulk = -ap(jk) / ap(kk);
          t = mulk;
          ap(ij+1:ij+j) = saxpy ( j, t, ap(ik+1:ik+j), 1, ap(ij+1:ij+j), 1 );
          ijj = ij + j;
          ap(jk) = mulk;
          ij = ij - ( j - 1 );
        end
%
%  Set the pivot array.
%
        if ( swap ) then
          kpvt(k) = imax;
        else
          kpvt(k) = k;
        end

      else
%
%  2 x 2 pivot block.
%
        km1k = ik + k - 1;
        ikm1 = ik - ( k - 1 );
%
%  Perform an interchange.
%
        if ( swap )

          temp(1:imax) = ap(im+1:im+imax);
          ap(im+1:im+imax) = ap(ikm1+1:ikm1+imax);
          ap(ikm1+1:ikm1+imax) = temp(1:imax);

          imj = ikm1 + imax;

          for jj = imax : km1
            j = km1 + imax - jj;
            jkm1 = ikm1 + j;
            t = ap(jkm1);
            ap(jkm1) = ap(imj);
            ap(imj) = t;
            imj = imj - ( j - 1 );
          end

          t = ap(km1k);
          ap(km1k) = ap(imk);
          ap(imk) = t;

        end
%
%  Perform the elimination.
%
        if ( k-2 ~= 0 )

          ak = ap(kk) / ap(km1k);
          km1km1 = ikm1 + k - 1;
          akm1 = ap(km1km1) / ap(km1k);
          denom = 1.0 - ak * akm1;
          ij = ik - ( k - 1 ) - ( k - 2 );

          for jj = 1 : k-2

            j = km1 - jj;
            jk = ik + j;
            bk = ap(jk) / ap(km1k);
            jkm1 = ikm1 + j;
            bkm1 = ap(jkm1) / ap(km1k);
            mulk = ( akm1 * bk - bkm1 ) / denom;
            mulkm1 = ( ak * bkm1 - bk ) / denom;
            t = mulk;
            ap(ij+1:ij+j) = saxpy ( j, t, ap(ik+1:ik+j), 1, ap(ij+1:ij+j), 1 );
            t = mulkm1;
            ap(ij+1:ij+j) = saxpy ( j, t, ap(ikm1+1:ikm1+j), 1, ap(ij+1:ij+j), 1 );
            ap(jk) = mulk;
            ap(jkm1) = mulkm1;
            ijj = ij + j;
            ij = ij - ( j - 1 );
          end

        end
%
%  Set the pivot array.
%
        if ( swap )
          kpvt(k) = -imax;
        else
          kpvt(k) = 1 - k;
        end

        kpvt(k-1) = kpvt(k);

      end

    end

    ik = ik - ( k - 1 );
    if ( kstep == 2 )
      ik = ik - ( k - 2 );
    end

    k = k - kstep;

  end

  return
end


function [R2,R2_states] = tudaR2(X,Y,T,tuda,Gamma)
% Training explained variance per time point per each state
%
% R2_states is per state, R2 is for the entire model
% X and Y must be in matrix format (2D)
% All trials must have equal size, ie all elements of T must be equal
%
% Diego Vidaurre

if any(T(1)~=T)
    error(['tudaR2 requires that all trials have the same length, ' ...
        'i.e. that all elements in T are equal']);
end

[X,Y,T] = preproc4hmm(X,Y,T,tuda.train);

intercept = all(X(:,1)==1);

N = length(T); ttrial = T(1); p = size(X,2);
K = length(tuda.state); q = size(Y,2);

if intercept
    mY = repmat(mean(Y),size(Y,1),1); % one single mean value for the entire data
    mY = reshape(mY,[ttrial N q]);
else
    mY = zeros(ttrial,N,q);
end

mat1 = ones(ttrial,q);
Y = reshape(Y,[ttrial N q]);
Yhat = zeros(ttrial,N,q);

R2_states = zeros(ttrial,q,K);
beta = tudabeta(tuda);
for k = 1:K
    Yhatk = X * beta(:,:,k); %tuda.state(k).W.Mu_W(1:p,p+1:end);
    Yhatk = reshape(Yhatk,[ttrial N q]);
    if k > size(Gamma,2) % NESS model baseline state
        break%
        noGamma = prod(1-Gamma,2);
        Yhat = Yhat + Yhatk .* reshape(repmat(noGamma,1,q),[ttrial,N,q]);
    else
        Yhat = Yhat + Yhatk .* reshape(repmat(Gamma(:,k),1,q),[ttrial,N,q]);
    end
    if intercept
        mYk = mean(reshape(Y,[ttrial*N q]) .* repmat(Gamma(:,k),1,q));
        mYk = reshape(repmat(mYk,size(Y,1),1),[ttrial N q]);
    else
        mYk = zeros(ttrial,N,q);
    end
    ek = permute(sum((Yhatk - Y).^2,2),[1 3 2]); % ttrial x q
    e0k = permute(sum((mYk - Y).^2,2),[1 3 2]); % ttrial x q
    R2_states(:,:,k) = mat1 - ek ./ e0k ; % we do not compute the
end

e = permute(sum((Yhat - Y).^2,2),[1 3 2]); % ttrial x q
e0 = permute(sum((mY - Y).^2,2),[1 3 2]); % ttrial x q
R2 = mat1 - e ./ e0 ;

end


function [f,nodes] = evalSectionsEquispacedFFT(SO3F,varargin)
% Evaluate an @SO3FunHarmonic on @ODFSections by using an equispaced grid
% along the other 2 Euler angles
%     $$(\alpha_a,\beta_b,\gamma_c) = (\frac{2\pi a}{H_1},\frac{\pi b}{H_2-1},\gamma_c)$$
% where $a=0,...,H_1-1$, $b=0,...,H_2-1$ and $c=0,...,S_{num}$.
%
% Therefore we transform the SO(3) Fourier series to an usual Fourier series
% equivalent as in the function <SO3FunHarmonic.evalV2.html |evalV2|>.
% But we use an 2-variate equispaced FFT instead of the NFFT analogously to
% <SO3FunHarmonic.evalEquispacedFFT.html |evalEquispacedFFT|>
%
% Syntax
%   f = evalSectionsEquispacedFFT(SO3F)
%   f = evalSectionsEquispacedFFT(SO3F,'resolution',2.5*degree)
%   [f,nodes] = evalSectionsEquispacedFFT(SO3F,'resolution',[2*degree,2.5*degree])
%   f = evalSectionsEquispacedFFT(SO3F,oS)
%
% Input
%  SO3F - @SO3FunHarmonic
%  oS - @ODFSection (phi2,gamma,phi1)
%
% Options
%  'resolution' - shape constant along Euler angles. (default = 2.5°)
%
% Output
%  nodes - @orientation
%  f - values at this grid points
%
% See also
% SO3FunHarmonic/evalV2 SO3FunHarmonic/evalEquispacedFFT SO3FunHarmonic/eval

N = SO3F.bandwidth;

if isa(varargin{1},'ODFSections'), oS = varargin{1}; else, oS =[]; end
[a_max,b_max,g_max] = fundamentalRegionEuler(SO3F.CS,SO3F.SS,varargin{:});
if isempty(oS)
  gamma = mod((0:5)*g_max/6+pi/2,2*pi);
  shift = 1;
elseif isa(oS,'gammaSections')
  gamma = oS.gamma;
  shift = 0;
elseif isa(oS,'phi2Sections')
  gamma = mod(oS.phi2+pi/2,2*pi);
  shift = 1;
elseif isa(oS,'phi1Sections')
  % phi1-sections are analogous to alpha sections. We transform SO3F(R(a,b,g))
  % to SO3F(R(g,b,a)).
  fhat = SO3F.fhat;
  for n=0:N
    ind = deg2dim(n)+1 : deg2dim(n+1);
    fhat(ind) = (-1).^((1:2*n+1)+(1:2*n+1).') .* reshape(fhat(ind),2*n+1,2*n+1).';
  end
  a_max = g_max;
  SO3F = SO3FunHarmonic(fhat,SO3F.SS,SO3F.CS);
  gamma = mod(oS.phi1-pi/2,2*pi);
  shift = -1;
end

% get grid size along 1st and 2nd Euler angles
res = get_option(varargin,'resolution',2.5*degree);
if length(res)==1
  res = [1,1]*res;
end
% [a_max,b_max,~] = fundamentalRegionEuler(SO3F.CS,SO3F.SS,varargin{:});
s = ceil([a_max,b_max]./res);
res = [a_max,b_max]./s;
H = 2*pi./res;
if any(s<=1)
  error('The resolution is to big.')
end

shiftGrid = get_option(varargin,'shiftGrid',[0,0]);
if check_option(varargin,'shiftGrid')
  s = s-1;
end

% compute ghat
if SO3F.isReal

  % compute ghat -> k x j x l
  % with  k = -N:N
  %       j = -N:N    -> use ghat(k,-j,l) = (-1)^(k+l) * ghat(k,-j,l)   (*)
  %       l =  0:N    -> use ghat(-k,-j,-l) = conj(ghat(k,j,l))        (**)
  % flags: 2^0 -> use L_2-normalized Wigner-D functions
  %        2^2 -> fhat are the fourier coefficients of a real valued function
  %        2^4 -> use right and left symmetry
  flags = 2^0+2^2+2^4;
  sym = [min(SO3F.SRight.multiplicityPerpZ,2),SO3F.SRight.multiplicityZ,...
         min(SO3F.SLeft.multiplicityPerpZ,2),SO3F.SLeft.multiplicityZ];
  ghat = representationbased_coefficient_transform(N,SO3F.fhat,flags,sym);
  ghat = permute(ghat,[3,2,1]);
  % correct ghat by i^(-k+l)
  z = - (0:N)'+ reshape(-N:N,1,1,[]) + shift * (0:N)';
  ghat = ghat.*(1i).^z;

  if check_option(varargin,'shiftGrid')
    ghat = ghat.*exp(-1i * (shiftGrid(1)*(0:N)' + shiftGrid(2)*(-N:N)) );
  end

else

  % compute ghat
  % flags: 2^0 -> use L_2-normalized Wigner-D functions
  %        2^4 -> use right and left symmetry
  flags = 2^0+2^4;
  sym = [min(SO3F.SRight.multiplicityPerpZ,2),SO3F.SRight.multiplicityZ,...
         min(SO3F.SLeft.multiplicityPerpZ,2),SO3F.SLeft.multiplicityZ];
  ghat = representationbased_coefficient_transform(N,SO3F.fhat,flags,sym);
  ghat = permute(ghat,[3,2,1]);
  % correct ghat by i^(-k+l)
  z = - (-N:N)' + reshape(-N:N,1,1,[]) + shift * (-N:N)';
  ghat = ghat.*(1i).^z;

  if check_option(varargin,'shiftGrid')
    ghat = ghat.*exp(-1i * (shiftGrid(1)*(-N:N)' + shiftGrid(2)*(-N:N)) );
  end

end

% use rotational symmetries around Z-axis to speed up (cut zeros in ghat)
SRightZ = SO3F.SRight.multiplicityZ;
SLeftZ = SO3F.SLeft.multiplicityZ;
H(1) = H(1) / SLeftZ;
if SLeftZ>1 || SRightZ>1
  if SO3F.isReal
    ind1 =  (0:SLeftZ:N)+1;
  else
    ind1 =  [-flip(SLeftZ:SLeftZ:N),(0:SLeftZ:N)] + (N+1);
  end
  ind3 =  [-flip(SRightZ:SRightZ:N),(0:SRightZ:N)] + (N+1);
  ghat = ghat(ind1,:,ind3);
end

% For small H we go through a smaller FFT(H) several times. Hence we reduce
% the size of the fourier coefficient matrix ghat by adding the coefficients
% with same complex exponentials.
sz = size(ghat,1,2);
if any(H<sz)
  dim = ceil(sz./H);
  A = zeros([dim.*H,size(ghat,3)]);
  A(1:size(ghat,1),1:2*N+1,1:size(ghat,3)) = ghat;
  % Note that H(1) and H(2) should be bigger than 1 to avoid errors by squeezing
  ghat = squeeze(sum(reshape(A,H(1),dim(1),H(2),dim(2),[]),[2,4]));
  clear A;
end

% Do 2-variate fft for all Euler angles gamma
f = zeros([s+1,length(gamma)]);
for g = 1:length(gamma)
  exponentials = exp( -1i*gamma(g) * reshape([-flip(SRightZ:SRightZ:N),(0:SRightZ:N)],1,1,[]) );
  A = sum(ghat.*exponentials,3);
  % fft
  f2 = fft2(A,H(1),H(2));
  % cut beta to [0,pi]
  if s(1) == H(1)
    f2 = f2([1:s(1),1],1:s(2)+1);
  else
    f2 = f2(1:s(1)+1,1:s(2)+1);
  end
  % shift the summation of fft from [-|_N/r_|:|_N/r_|]x[-N:N]x[-|_N/s_|:|_N/s_|] to
  % [0:2*|_N/r_|]x[0:2N]x[0:2*|_N/s_|]. With r- & s-fold rotational symmetry
  % around Z-axis and |_ ... _| denotes the round off operator.
  if SO3F.isReal
    z = (0:s(2)) * (N/H(2));    % oder fftshift
    f(:,:,g) = 2*real( exp(2i*pi*z) .* f2 );
  else
    z = (0:s(1)).' * (floor(N/SLeftZ)/H(1)) + (0:s(2)) * (N/H(2));
    f(:,:,g) = exp(2i*pi*z) .* f2;
  end
end

% compute the corresponding nodes
if nargout>1
  if check_option(varargin,'shiftGrid')
    grid = combvec((0:s(1))*res(1)-shift*pi/2,(0:s(2))*res(2),gamma);
    grid = grid + [shiftGrid,0];
  else
    grid = combvec((0:s(1))*res(1)-shift*pi/2,(0:s(2))*res(2),gamma);
  end
  if isa(oS,'phi1Sections')
    grid = grid(:,[3,2,1]);
    grid = orientation.byEuler(grid,'nfft',SO3F.SS,SO3F.CS);
  else
    grid = orientation.byEuler(grid,'nfft',SO3F.CS,SO3F.SS);
  end
  nodes = reshape(grid,size(f));
end

end

function v = combvec(v1,v2,v3)
% This function does the same as the combvec() function from Digital
% Processing Toolbox and additionaly transpose the output matrix.
% It is a simple version for the special case of three input vectors.
  l1 = length(v1);
  l2 = length(v2);
  l3 = length(v3);

  V1 = repmat(v1.',l2*l3,1);
  V2 = repmat(v2,l1,l3);
  V3 = repmat(v3,l1*l2,1);

  v = [V1,V2(:),V3(:)];
end


function [dLdp] = spm_mci_adjoint_sun (Pr,M,U,Y)
% Gradient of log joint from adjoint method (via Sundials)
% FORMAT [dLdp] = spm_mci_adjoint_sun (Pr,M,U,Y)
%
% Pr        Parameters (vectorised and in M.V subspace)
% M         Model structure
% U         Inputs  [Nin x N]
% Y         Data
%
% dLdp      Gradient [Np x 1]
%
% For M.adjlike=1, dLdp is gradient of log likelihood not log joint
% (useful for debugging).
%
% For M.backint=1 (default), compute the integral underlying dLdp
% *during* backwards integration of adjoint. For M.backint=0, this
% integral is computed *after* adjoint (useful for debugging).
%
% B. Sengupta, K. Friston and W. Penny (2014) Efficient Gradient
% Computation for Dynamical Models. Neuroimage,98, 521-527.
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny and Biswa Sengupta
% $Id: spm_mci_adjoint_sun.m 6697 2016-01-27 14:57:28Z spm $

try, backint=M.backint; catch, backint=1; end
try, adjlike=M.adjlike; catch, adjlike=0; end

% Tolerances
tol_scale = 1e-3;
reltol = 1e-3;
abstol = 1e-4;

% Parameters in original space
P = M.V*Pr+M.vpE;

%tDur=[0 M.t(end)];
%tDur=[M.t(1) M.t(end)];

t0=0;
%tf=M.T;
%dt=M.t(2)-M.t(1);
%t0=M.t(1)-dt;
tf=M.T;

if isempty(U)
    U=zeros(1,M.N);
end

data.U=U;
data.P=P;
data.M=M;
data.Y=Y;

options = CVodeSetOptions('UserData', data,...
    'RelTol',reltol, ...
    'AbsTol',abstol, ...
    'LinearSolver','Dense', ...
    'JacobianFn',@djacfn);

CVodeInit(@spm_mci_flow_sun, 'BDF', 'Newton', t0, M.x0, options);

CVodeAdjInit(150, 'Hermite');

optionsB = CVodeSetOptions('UserData',data,...
    'MaxNumSteps',50000, ...
    'RelTol',reltol,...
    'AbsTol',abstol,...
    'LinearSolver','Dense',...
    'JacobianFn',@djacBfn);

lambda_init=zeros(M.n,1);
iB = CVodeInitB(@rhsadjoint, 'BDF', ...
    'Newton', tf, lambda_init, optionsB);

if backint
    % Use CVODE to compute the integral underlying dLdp
    % *during* backwards integration of adjoint (backint=1)

    [status,t,y] = CVode(tf,'Normal');

    optionsQB = CVodeQuadSetOptions('ErrControl',true,...
        'RelTol',reltol,...
        'AbsTol',abstol);

    qB1 = zeros(M.Np,1);
    CVodeQuadInitB(iB, @paramgrad, qB1, optionsQB);

    % Backward integration of the adjoint equation
    % (yB is the adjoint vector)
    [status,t,yB,qB] = CVodeB(M.t(2),'Normal');
    %[status,t,yB,qB] = CVodeB(t0,'Normal');
    if status == -1
        error('Adjoint integration failed');
    end
    qB = -qB';
    dt = M.t(2)-M.t(1);
    dLdp = qB/dt;
else
    % Compute integral underlying dLdp *after* backwards
    % integration of adjoint

    ntout = 1000;
    dt = (tf-t0)/ntout;
    tt = linspace(t0+dt,tf,ntout-1);

    %[status,ttf,x] = CVode(M.t(2:end),'Normal');
    %[status,ttf,x] = CVode(tt,'Normal');
    %[status,ttf,x] = CVode(M.t(2:end),'Normal');
    [status,ttf,x] = CVode(M.t,'Normal');
    interp_x = (interp1q(ttf',x',M.t))';

    %tm(1) = tDur(2);
    %t = tDur(2);
    tm(1) = tf;
    t = tf;
    it = 1;
    % Integrate adjoint equation
    while t > M.t(2)
    %while t > t0+2*dt
        it = it+1;
        % The adjoint vector is yB
        %[status,t,yB] = CVodeB(tDur(1),'OneStep');
        [status,t,yB] = CVodeB(M.t(1),'OneStep');
        %[status,t,yB] = CVodeB(t0+2*dt,'OneStep');
        if status == -1
            error('Adjoint integration failed');
        end
        tm(it) = t;
        dldp(it,:) = feval(@paramgrad,t, interp_x, yB, data);
    end
    dldp_int=interp1(tm,dldp,M.t,'spline');
    dLdp=sum(dldp_int);
end
CVodeFree;

if ~adjlike
    dlogpriordp = spm_mci_gprior_deriv (Pr,M);
    dLdp=dLdp+dlogpriordp;
end

end

% -----------------------------------------------------------
function [qBd, flag, new_data] = paramgrad(t, x, yB, data)
% Np quadratures for parametric gradient
% t     time
% x     state
% yB    adjoint vector, lambda^T
% data  contains P,M,U,Y
%
% yBd   dlambda^T/dt

P = data.P;
M = data.M;
U = data.U;

% If observation function becomes dependent on parameters
% we'll need to update term1
term1 = 0;

% Find nearest time point for which we have pre-computed input
if isempty(U)
    ut=[];
else
    [tmp,ind]=min(abs(t-M.t));
    ut=U(:,ind);
end

% Evaluate parameter Jacobian
if isfield(M,'dfdp')
    dfdp = feval(M.dfdp,x,ut,P,M);
else
    dfdp = spm_diff(M.f,x,ut,P,M,3);
end
term2 = -yB'*dfdp;
qBd = term1 + term2;

flag = 0;
new_data = [];

end

% -----------------------------------------------------------
function [yBd, flag, new_data] = rhsadjoint(t, x, yB, data)
% Adjoint equation
% FORMAT [yBd, flag, new_data] = rhsadjoint(t, x, yB, data)
%
% t     time
% x     state
% yB    adjoint vector, lambda^T
% data  contains P,M,U,Y
%
% yBd   dlambda^T/dt

Y = data.Y;
M = data.M;
U = data.U;
P = data.P;

% Interpolate data to required time point
for j=1:M.l,
    ydata(j)=interp1q(M.t,Y(:,j),t);
end
[y,L]=feval(M.g,x,U(:,1),P,M);
e=ydata-y';

term1 = (djacfn(t,x,[],data))'*yB; % (f_y)^T \lambda

% When computing output sensitivities, assume dydx=L, ie not a
% function of x. Generalise later
dydx=L;

term2 = e*M.iCe*dydx;
yBd = (-term1 + term2')';

flag = 0;
new_data = [];

end

% -----------------------------------------------------------
function [JB, flag, new_data] = djacBfn(t, y, yB, fyB, data)
% Backward problem Jacobian function

J               = djacfn(t,y,[],data);
JB              = -J';

flag            = 0;
new_data        = [];

end

% -----------------------------------------------------------
function [J, flag, new_data] = djacfn(t, y, fy, data)
% State Jacobian at time t

P=data.P;
M=data.M;
U=data.U;

% Find nearest time point for which we have pre-computed input
if isempty(U)
    ut=[];
else
    [tmp,ind]=min(abs(t-M.t));
    ut=U(:,ind);
end

% Evaluate state Jacobian
if isfield(M,'dfdx')
    J = feval(M.dfdx,y,ut,P,M);
else
    J = spm_diff(M.f,y,ut,P,M,1);
end

flag            = 0;
new_data        = [];

end


function y=Tsallis_entro(x,q)
  [M,N]=size(x);
       y=zeros(1,N);
          for l=1:N
          sum1=sum(x(:,l)-(x(:,l)).^q);
          sum2=sum1/(q-1);
          y(1,l)=sum2;
       end


function [P,E,logw,acc,traj] = spm_mci_ais_single (mcmc,M,U,Y)
% Produce a single independent sample using AIS
% FORMAT [P,E,logw,acc,traj] = spm_mci_ais_single (mcmc,M,U,Y)
%
% mcmc      Sampling settings
% M         Model structure
% U         Input structure
% Y         Data
%
% P         [Np x 1] sample
% E         Negative log joint
% logw      Contribution to model evidence
% acc       acc(j) is acceptance rate at temperature j
% traj      traj(p,j) is value of parameter p at temperature j
%           (only set if mcmc.rec_traj=1)
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mci_ais_single.m 6548 2015-09-11 12:39:47Z will $

beta=mcmc.beta;
nprop=mcmc.nprop;
prop=mcmc.prop;
scale=mcmc.scale;

J=length(beta);

if isstruct(M.pC)
    pC = full(diag(spm_vec(M.pC)));
else
    pC = M.pC;
end
% prior cov in subspace
pC=M.V'*pC*M.V;

Np=size(pC,1);

% Sample from prior (in subspace)
x(:,1) = spm_normrnd(zeros(Np,1),pC,1);
[L(1),L2] = spm_mci_joint (x(:,1),M,U,Y);
Lsum = (beta(2)-beta(1))*L2;
acc(1) = 1;
for j=2:J,
    xs=x(:,j-1); Ls=L(:,j-1);acc(j)=0;
    switch prop,
        case 'mh',
            % Generate sample at next temperature by composing
            % Metropolis moves at different scales
            for s=1:nprop,
                dx=spm_normrnd(zeros(Np,1),scale(s)^2*pC,1);
                xcand=xs+dx;
                [Lcand,L2cand] = spm_mci_joint (xcand,M,U,Y,beta(j));
                dL = Lcand-Ls;
                r = exp(dL);
                alpha = min(1,r);
                test_prob = rand(1);
                if alpha > test_prob
                    % Accept
                    xs = xcand;
                    Ls = Lcand;
                    L2 = L2cand;
                    acc (j) = 1;
                end
            end

        case 'lmc',
            % Generate sample at next temperature using
            % Langevin Monte Carlo
            M.beta=beta(j);
            lgv_mcmc.init=xs;
            lgv_mcmc.maxits=nprop;
            lgv_mcmc.update_obs_noise=0;

            [tmp,stats] = spm_mci_lgv (lgv_mcmc,M,U,Y);

            xs = stats.P(:,end);
            Ls = -stats.E(:,end);
            L2 = stats.L2(end);
            acc (j) = any(stats.acc(2:end));

        otherwise
            disp('Unknown proposal type in spm_mci_ais_single.m');
    end
    x(:,j)=xs;
    L(j)=Ls;
    if j < J
        Lsum = Lsum + (beta(j+1)-beta(j))*L2;
    end
end
logw=Lsum;
P=x(:,J);
E=-L(J);

if mcmc.rec_traj
    nj=size(x,2);
    traj=spm_vec(M.pE)*ones(1,nj)+M.V*x;
else
    traj=[];
end


function [fdMc, bH, fPval, fKsstat] = calc_KStest_dMc(mCat1, mCat2)
% ----------------------------------------------------------------------------------------------
%
% Incoming variable
% mCat1 : EQ catalog period 1 (Catalog to be modified)
% mCat2 : EQ catalog period 2 (Observed catalog)
%
% Outgoing variable
%
% Author: J. Woessner, woessner@seismo.ifg,.ethz.ch
% last update: 21.01.04

fBinning = 0.1;
nSample = 100;
nMethod = 6;

% Determine exact time period
fPeriod1 = max(mCat1(:,3)) - min(mCat1(:,3));
fPeriod2 = max(mCat2(:,3)) - min(mCat2(:,3));

% Initialize values
fMinMag = min([min(mCat1(:,6)) min(mCat2(:,6))]);
fMaxMag = max([max(mCat1(:,6)) max(mCat2(:,6))]);

%% Calculate model for best fitting Mc, both periods
[mResult1, fMls1, fMc1, fMu1, fSigma1, mDataPredBest1, vPredBest1, fBValue1, fAvalue1] = calc_McCdfnormal(mCat1, fBinning);
[mResult, fMls, fMc2, fMu, fSigma, mDataPredBest, vPredBest, fBValue2, fAvalue2] = calc_McCdfnormal(mCat2, fBinning);

% Using bootstrap approach
% [fMc1, fStd_Mc1, fBvalue1, fStd_B1, fAvalue1, fStd_A1, vMc1, mBvalue1] = calc_McBboot(mCat1, fBinning, nSample, nMethod)
% [fMc2, fStd_Mc2, fBvalue2, fStd_B2, fAvalue2, fStd_A2, vMc2, mBvalue2] = calc_McBboot(mCat2, fBinning, nSample, nMethod)

% Mc difference
fdMc = fMc2-fMc1;

try
    % Select part of catalog
    vSel = mCat1(:,6) >= fMc2-fBinning/2;
    mCat1tmp=mCat1(vSel,:);

    % Create Model distribution
    vMagnitudes = [fMc2:0.1:floor(max(mCat2(:,6)))+0.1];
    % Productuvity for fMc2
    nNumberEvents = length(mCat1tmp(:,1));
    %vNumbers = 10.^(log10(nNumberEvents) - fBValue1*(vMagnitudes-fMc2));
    vNumbers = 10.^(fAvalue1- fBValue1*(vMagnitudes));%-fMc2
    vNumbers = round(vNumbers);
    vNCumFMD = round(-diff(vNumbers));
    % Calculate synthetic data below Mc
    fMinMag = min(mCat2(:,6));
    vMagstep = fMinMag:0.1:fMc2-0.1;
    vProb = normcdf(vMagstep,fMu1, fSigma1);
    vProb = vProb';
    vMagstep = vMagstep';

    % Calculate number of EQs in bins
    fN_Mc = vNCumFMD(1,1);
    vN = round(vProb(:,1)*fN_Mc);
    mNonCumModel = [vN vMagstep; vNCumFMD' vMagnitudes(:,1:end-1)'];

    % vMags = mDataPredBest(:,2)';
    % FMD to be modeled (second period)
    [vObsFMD,vBin2] = hist(mCat2(:,6),roundn(min(mCat2(:,6)),-1):0.1:floor(max(mCat2(:,6))));

    % Select bins to calculate loglikelihood
    vSel = (mNonCumModel(:,2) < min(vBin2) | mNonCumModel(:,2) > max(vBin2));
    vPredFMD = mNonCumModel(~vSel,:);
    % Normalize
    vObsFMD = ceil(vObsFMD./fPeriod2);
    vPredFMD(:,1) = vPredFMD(:,1)./fPeriod1;

    if length(vObsFMD') ~= length(vPredFMD(:,1))
        disp('warning')
    end
    % Calculate the likelihood
    vProb_ = calc_log10poisspdf2(vObsFMD', vPredFMD(:,1));
    % Sum the probabilities
    fProbability = (-1) * sum(vProb_);

    nDegFree = 1; % degree of freedom
    n_samples = length(mCat2(:,6))+length(mCat1(:,6));
    %% Corrected Akaike Information Criterion (AICc)
    fAICc = -2*(-fProbability)+2*nDegFree+2*nDegFree*(nDegFree+1)/(n_samples-nDegFree-1);

catch
    fdMc = nan;
    fProbability = nan;
    fAICc = nan;
end

% % Create model
% fMinMag = min(mCat2(:,6));
% vMagstep = fMinMag:0.1:fMc2-0.1;
% vProb = normcdf(vMagstep,fMu1, fSigma1);
% vProb = vProb';
% vMagstep = vMagstep';
% mDataPredBest1(:,1) = mDataPredBest1(:,1)./fPeriod1;
%
% % Calculate number of EQs in bins
% vSel = (roundn(mDataPredBest1(:,2),-1) == roundn(fMc2,-1));
% fN_Mc = mDataPredBest1(vSel,1);
% vN = round(vProb(:,1)*fN_Mc);
%
% vSel = mDataPredBest1(:,2) >= fMc2;
% mModelFMD = [vN vMagstep; mDataPredBest1(vSel,1) mDataPredBest1(vSel,2)];

% Reconstruct vector of magnitudes from model for Period 1
vMag = [];
% % Normalize to time period
% mModelFMD(:,1) = round(mModelFMD(:,1));
mNonCumModel(:,1) = round(mNonCumModel(:,1)./fPeriod1);
vSel = (mNonCumModel(:,1) ~= 0); % Remove bins with zero frequency of zero events
mData = mNonCumModel(vSel,:);
% mModelFMD = [round(mDataPredBest1(:,1)./fPeriod1) mDataPredBest1(:,2)];
% vSel = (mModelFMD(:,1) ~= 0); % Remove bins with zero frequency of zero events
% mData = mModelFMD(vSel,:);
for nCnt=1:length(mData(:,1))
    fM = repmat(mData(nCnt,2),mData(nCnt,1),1);
    vMag = [vMag; fM];
end

% Reconstruct vector of magnitudes from model for Period 2
% from normalized FMD
vMag2 = [];
%mObsFMD = [vObsFMD' vBin2'];
mObsFMD = [round(mDataPredBest(:,1)./fPeriod2) mDataPredBest(:,2)];
vSel = (mObsFMD(:,1) ~= 0); % Remove bins with zero frequency of zero events
mData2 = mObsFMD(vSel,:);
for nCnt=1:length(mData2(:,1))
    fM = repmat(mData2(nCnt,2),mData2(nCnt,1),1);
    vMag2 = [vMag2; fM];
end

[bH,fPval,fKsstat] = kstest2(roundn(vMag2,-1),roundn(vMag,-1),0.05,0);

figure
h1=cdfplot(vMag2)
set(h1,'Color',[1 0 0]);
hold on
h2=cdfplot(vMag)
set(h2,'Color',[0 0 0]);

h3=cdfplot(mCat2(:,6))
set(h2,'Color',[0 1 0]);
h4=cdfplot(mCat1(:,6))
set(h4,'Color',[0 0 1]);

figure
histogram(vMag2,min(vMag2):0.1:max(vMag2))
figure
histogram(vMag,min(vMag):0.1:max(vMag))


function metaboliteMassBalancePlot(model, metAbbr, solution, N)
% Plots the top N reactions producing and consuming a metabolite in a flux solution
%
% USAGE:
%
%    metaboliteMassBalancePlot(model, metAbbr, solution, N)
%
% INPUTS:
%    model:       COBRA model structure
%    metAbbr:     metabolite abbreviation
%    solution:    `solveCobraLP` output of a solution to FBA problem
%    N:           Number of reactions to include for production/consumption
%
% .. Author: - Ronan M.T. Fleming

[nMet,nRxn]=size(model.S);

metInd=find(strcmp(metAbbr,model.mets));
if isempty(metInd)
    error('metabolite abbreviation not found')
end

tol = 1e-6;

negS=(model.S(metInd,:)<0)';
posS=(model.S(metInd,:)>0)';

v=solution.full;

consumeBool=( negS & v>tol ) | ( posS & v<-tol );
produceBool=( posS & v>tol ) | ( negS & v<-tol );

vConsume=zeros(nRxn,1);
vProduce=zeros(nRxn,1);

vConsume(consumeBool)=v(consumeBool);
vProduce(produceBool)=v(produceBool);

[sortedConsume,XIMax]=sort(abs(vConsume),'descend');
NConsume=nnz(sortedConsume~=0);

[sortedProduce,XIMin]=sort(abs(vProduce),'descend');
NProduce=nnz(sortedProduce~=0);

figure
h1=subplot(2,1,1);
NConsume=min([NConsume,N]);
YI=(1:NConsume)';
barh(YI,v(XIMax(1:NConsume)));
% title(['Consumers (top) and Producers of ' metAbbr],'FontSize',16)

if NConsume==0
    set(h1,'YTickLabel','NONE','FontSize',8);
else
    for n=1:NConsume
        if strcmp(model.rxn(XIMax(n)).directionality,'forward')
            YTickLabelMax{n}=['*' model.rxns{XIMax(n)}];
        else
            YTickLabelMax{n}=model.rxns{XIMax(n)};
        end
    end
    set(h1,'Ytick', 1:NConsume)
    set(h1,'YTickLabel',YTickLabelMax,'FontSize',8);
    ylim([0.5 NConsume+0.5])
end
xlabel(['Consumer of ' metAbbr],'FontSize',12)

h2=subplot(2,1,2);
NProduce=min([NProduce,N]);
YI=(1:NProduce)';
barh(YI,v(XIMin(1:NProduce)));
if NProduce==0
    set(h1,'YTickLabel','NONE','FontSize',8);
else
    for n=1:NProduce
        if strcmp(model.rxn(XIMin(n)).directionality,'forward')
            YTickLabelMin{n}=['*' model.rxns{XIMin(n)}];
        else
            YTickLabelMin{n}=model.rxns{XIMin(n)};
        end
    end
    set(h2,'Ytick', 1:NProduce)
    set(h2,'YTickLabel',YTickLabelMin,'FontSize',8);
    ylim([0.5 NProduce+0.5])
end
xlabel(['Producer of ' metAbbr],'FontSize',12)


function tests = test_intersectEdgePolygon3d
% Test suite for the file intersectEdgePolygon3d.
%
%   Test suite for the file intersectEdgePolygon3d
%
%   Example
%   test_intersectEdgePolygon3d
%
%   See also
%     intersectEdgePolygon3d

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2021-02-24,    using Matlab 9.9.0.1570001 (R2020b) Update 4
% Copyright 2021 INRAE - BIA-BIBS.

tests = functiontests(localfunctions);

function test_Simple(testCase) %#ok<*DEFNU>
% Test computation for one edge and one polygon

% Compute intersection between a 3D edge and a 3D triangle
pts3d = [3 0 0; 0 6 0;0 0 9];
edge1 = [0 0 0 3 6 9];

inter = intersectEdgePolygon3d(edge1, pts3d);

assertEqual(testCase, size(inter), [1 3]);
assertEqual(testCase, inter, [1 2 3]);

function test_TwoEdges(testCase) %#ok<*DEFNU>
% Test computation for two edges

pts3d = [3 0 0; 0 6 0;0 0 9];
edges = [0 0 0 3 6 9;10 0 0 10 2 3];

[inter, inside] = intersectEdgePolygon3d(edges, pts3d);

assertEqual(testCase, size(inter), [2 3]);
assertEqual(testCase, size(inside), [2 1]);
assertEqual(testCase, inside, [true ; false]);
assertEqual(testCase, inter(inside, :), [1 2 3]);


function a = softMaxSelection(Q, state, actions, T)

    n_actions = length(actions);
    P = zeros(1, n_actions);

    random = rand();

    % Boltzmann distribution
    P = exp(Q(state,:)./T)./sum(exp(Q(state,:)./T));

    if( find(isnan(P),1) ~= 0 )

       a = getBestAction(Q, state, actions);
    else

        if(random < P(1))

            a = 1;
        elseif(random >= P(1) && random < sum(P(1:2)) )

            a = 2;
        elseif(random >= sum(P(1:2)))

            a = 3;
        end
    end

end


function [y] = local2globalVel2(V, lon,lat)

% SYNTAX:
%   [y] = local2globalVel2(V, X);
%
% INPUT:
%   V = local position vector(s)
%   lon = longitude of orifin vector in radians
%   lat = latirude of orifin vector in radians
%
% OUTPUT:
%   y = global position vector(s)
%
% DESCRIPTION:
%   Rototation from local-level reference frame to Earth-fixed reference frame

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

%initialize new position vector
y = zeros(size(V));

for i = 1 : size(V,2)
    %rotation matrix from global to local reference system
    R = [-sin(lon) cos(lon) 0;
         -sin(lat)*cos(lon) -sin(lat)*sin(lon) cos(lat);
         +cos(lat)*cos(lon) +cos(lat)*sin(lon) sin(lat)];

    %rototraslation
    y(:,i) = R\V(:,i);
end


function [ xnew, ynew, w ] = quaequad0 ( mmax, kk )

%*****************************************************************************80
%
%% QUAEQUAD0 returns the requested quadrature rule.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    28 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer MMAX, the degree of the quadrature (the
%    maximum degree of the polynomials of two variables that are integrated
%    exactly.  1 <= MMAX <= 50.
%
%    Input, integer KK, the number of points in the compressed rule.
%
%    Output, real XNEW(KK), YNEW(KK), the
%    coordinates of the nodes.
%
%    Output, real W(Kk), the weights.
%

%
%  Copy the arrays defining the compressed rule.
%
  if ( mmax == 1 )
    [ xnew, ynew, w ] = rule01 ( );
  elseif ( mmax == 2 )
    [ xnew, ynew, w ] = rule02 ( );
  elseif ( mmax == 3 )
    [ xnew, ynew, w ] = rule03 ( );
  elseif ( mmax == 4 )
    [ xnew, ynew, w ] = rule04 ( );
  elseif ( mmax == 5 )
    [ xnew, ynew, w ] = rule05 ( );
  elseif ( mmax == 6 )
    [ xnew, ynew, w ] = rule06 ( );
  elseif ( mmax == 7 )
    [ xnew, ynew, w ] = rule07 ( );
  elseif ( mmax == 8 )
    [ xnew, ynew, w ] = rule08 ( );
  elseif ( mmax == 9 )
    [ xnew, ynew, w ] = rule09 ( );
  elseif ( mmax == 10 )
    [ xnew, ynew, w ] = rule10 ( );
  elseif ( mmax == 11 )
    [ xnew, ynew, w ] = rule11 ( );
  elseif ( mmax == 12 )
    [ xnew, ynew, w ] = rule12 ( );
  elseif ( mmax == 13 )
    [ xnew, ynew, w ] = rule13 ( );
  elseif ( mmax == 14 )
    [ xnew, ynew, w ] = rule14 ( );
  elseif ( mmax == 15 )
    [ xnew, ynew, w ] = rule15 ( );
  elseif ( mmax == 16 )
    [ xnew, ynew, w ] = rule16 ( );
  elseif ( mmax == 17 )
    [ xnew, ynew, w ] = rule17 ( );
  elseif ( mmax == 18 )
    [ xnew, ynew, w ] = rule18 ( );
  elseif ( mmax == 19 )
    [ xnew, ynew, w ] = rule19 ( );
  elseif ( mmax == 20 )
    [ xnew, ynew, w ] = rule20 ( );
  elseif ( mmax == 21 )
    [ xnew, ynew, w ] = rule21 ( );
  elseif ( mmax == 22 )
    [ xnew, ynew, w ] = rule22 ( );
  elseif ( mmax == 23 )
    [ xnew, ynew, w ] = rule23 ( );
  elseif ( mmax == 24 )
    [ xnew, ynew, w ] = rule24 ( );
  elseif ( mmax == 25 )
    [ xnew, ynew, w ] = rule25 ( );
  elseif ( mmax == 26 )
    [ xnew, ynew, w ] = rule26 ( );
  elseif ( mmax == 27 )
    [ xnew, ynew, w ] = rule27 ( );
  elseif ( mmax == 28 )
    [ xnew, ynew, w ] = rule28 ( );
  elseif ( mmax == 29 )
    [ xnew, ynew, w ] = rule29 ( );
  elseif ( mmax == 30 )
    [ xnew, ynew, w ] = rule30 ( );
  elseif ( mmax == 31 )
    [ xnew, ynew, w ] = rule31 ( );
  elseif ( mmax == 32 )
    [ xnew, ynew, w ] = rule32 ( );
  elseif ( mmax == 33 )
    [ xnew, ynew, w ] = rule33 ( );
  elseif ( mmax == 34 )
    [ xnew, ynew, w ] = rule34 ( );
  elseif ( mmax == 35 )
    [ xnew, ynew, w ] = rule35 ( );
  elseif ( mmax == 36 )
    [ xnew, ynew, w ] = rule36 ( );
  elseif ( mmax == 37 )
    [ xnew, ynew, w ] = rule37 ( );
  elseif ( mmax == 38 )
    [ xnew, ynew, w ] = rule38 ( );
  elseif ( mmax == 39 )
    [ xnew, ynew, w ] = rule39 ( );
  elseif ( mmax == 40 )
    [ xnew, ynew, w ] = rule40 ( );
  elseif ( mmax == 41 )
    [ xnew, ynew, w ] = rule41 ( );
  elseif ( mmax == 42 )
    [ xnew, ynew, w ] = rule42 ( );
  elseif ( mmax == 43 )
    [ xnew, ynew, w ] = rule43 ( );
  elseif ( mmax == 44 )
    [ xnew, ynew, w ] = rule44 ( );
  elseif ( mmax == 45 )
    [ xnew, ynew, w ] = rule45 ( );
  elseif ( mmax == 46 )
    [ xnew, ynew, w ] = rule46 ( );
  elseif ( mmax == 47 )
    [ xnew, ynew, w ] = rule47 ( );
  elseif ( mmax == 48 )
    [ xnew, ynew, w ] = rule48 ( );
  elseif ( mmax == 49 )
    [ xnew, ynew, w ] = rule49 ( );
  elseif ( mmax == 50 )
    [ xnew, ynew, w ] = rule50 ( );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'QUAEQUAD0 - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of MMAX.\n' );
    fprintf ( 1, '  1 <= MMAX <= 50 required.\n' );
    error ( 'QUAEQUAD0 - Fatal error!' );
  end

  for i = 1 : kk
%
%  The lower-left 1/6.
%
    iitype = 2;
    nbool2 = quaeinside ( iitype, xnew(i), ynew(i) );
%
%  The lower 1/3.
%
    iitype = 1;
    nbool1 = quaeinside ( iitype, xnew(i), ynew(i) );
%
%  The whole triangle.
%
    iitype = 0;
    nbool0 = quaeinside ( iitype, xnew(i), ynew(i) );

    if ( nbool2 == 1 )

    elseif ( nbool1 == 1 )
      xnew(i) = -xnew(i);
      ynew(i) = ynew(i);
    elseif ( nbool0 == 1 )
      x0 = xnew(i);
      y0 = ynew(i);
      [ x1, y1 ] = quaerotate ( x0, y0 );
      xnew(i) = x1;
      ynew(i) = y1;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'QUAEQUAD0 - Fatal error!\n' );
      fprintf ( 1, '  Point does not lie inside triangle.\n' );
      error ( 'QUAEQUAD0 - Fatal error!' );
    end

  end

  return
end


function [V,E,F,C] = voronoi_tetgen(P,varargin)
  % VORONOI_TETGEN Compute a voronoi diagram using tetgen
  %
  %
  % [V,E,F,C] = voronoi_tetgen(P,varargin)
  %
  % Inputs:
  %   P  #P by 3 list of points
  % Outputs:
  %   V  #V by 3 list of orthocenters (?)
  %   E  #E by 2 list of edge indices into V
  %   F  #F by 3 list of face indices into ?
  %   C  #C list of cells (not supported)
  %
  % I don't trust the tetgen output...
  %

  % default values
  flags = '';
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Flags'}, {'flags'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  % get a temporary file name prefix
  prefix = tempname;
  prefix = tempname;
  node_filename = [prefix '.node'];
  writeNODE(node_filename,P);

  % call tetgen
  voronoi_flags = '-vNEF';
  command = [path_to_tetgen ' ' voronoi_flags ' ' flags ' ' node_filename];
  [status, result] = system(command);
  if status~=0
    error(result)
  end

  edge_filename = [prefix '.1.v.edge'];
  face_filename = [prefix '.1.v.face'];
  node_filename = [prefix '.1.v.node'];
  cell_filename = [prefix '.1.v.cell'];

  F = readFACE(face_filename,'ForceNoBoundary',true);
  E = readEDGE(edge_filename);
  V = readNODE(node_filename);
  C = [];
  if nargout>3
    error('Cell output not handled yet.');
  end

end


function [k, innerProd, arg, denom, numer, vec] = polyardKernCompute(kern, x, x2)

% POLYARDKERNCOMPUTE Compute the POLYARD kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the automatic relevance determination polynomial
% kernel given inputs associated with rows and columns.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN k : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel matrix for the automatic relevance determination polynomial
% kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : the kernel matrix computed at the given points.
%
% SEEALSO : polyardKernParamInit, kernCompute, kernCreate, polyardKernDiagCompute
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006

% KERN

scales = sparse(diag(sqrt(kern.inputScales)));
x = x*scales;

if nargin < 3
  innerProd = x*x';
  arg = innerProd*kern.weightVariance + kern.biasVariance;
  k = kern.variance*arg.^kern.degree;
else
  x2 = x2*scales;
  innerProd = x*x2';
  arg = innerProd*kern.weightVariance + kern.biasVariance;
  k = kern.variance*arg.^kern.degree;
end


function model = SRModel
%SRMODEL Super-resolution model parameter structure.

    % Magnification factor used for super-resolution (default: 2)
    model.magFactor = 2.0;

    % Width of isotropic Gaussian PSF (default: 0.4)
    model.psfWidth = 0.4;

    % Motion parameters used for super-resolution. This can be either a
    % 2-D homography to model parametric motion or displacement vector
    % fields in case of local motion estimation (optical flow).
    %   - 2-D homography:
    %       Motion is given by a cell array of 2-D homographies where the
    %       k-th element models the motion for the k-th low-resolution
    %       frame:
    %           motionParams{1} = [h11 h12 h13; h21 h22 h23; h31 h32 h33]
    %           motionParams{2} = ...
    %   - Displacement fields:
    %       Motion is given by cell array of displacement fields where the
    %       k-th element models the motion for the k-th low-resolution.
    %       A displacement field is given by a structure with matrices to
    %       model displacements in x- and y-direction:
    %           v.vx = [...]    Displacements in x-direction (pixel-wise)
    %           v.vy = [...]    Displacements in y-direction (pixel-wise)
    %           motionParams{1} = v;
    %           motionParams{2} = ...
    model.motionParams = [];

    % Photometric parameters used for super-resolution (optional).
    % see also SRPhotometricParams
    model.photometricParams = [];

    % Error (noise) model assumed for the low-resolution observations
    % (optional). Can be either 'l2NormErrorModel' (least square, default)
    % in case  of Gaussian noise or 'l1NormErrorModel' (least absolute
    % deviation) in case of Laplacian noise. Additionally, user-defined
    % error models can be specified, e.g. a Lorentzian M-estimator:
    %   sigma = 0.05;
    %   lorentzian = @(r) deal(log(1 + 0.5*(r/sigma).^2), ... Function
    %                          (2*r) ./ (r.^2 + 2*sigma^2));  Gradient
    %   model.errorModel = lorentzian;
    model.errorModel = 'l2NormErrorModel';

    % Image prior used for regularization of super-resolved images.
    % see also SRPrior
    model.imagePrior = [];

    % Confidence map for the low-resolution observations (optional). A 3-D
    % array of size M x N x K can be used to weight K low-resolution frames
    % of size M x N pixel-wise.
    model.confidence = [];

    % Initial guess for the super-resolved image (optional). This can be
    % used as starting point for numerical optimization algorithms for
    % image reconstruction.
    model.SR = [];


function d=dcirc(p,xc,yc,r)

%   Copyright (C) 2004-2006 Per-Olof Persson. See COPYRIGHT.TXT for details.

d=sqrt((p(:,1)-xc).^2+(p(:,2)-yc).^2)-r;


function ins=ins_init(opt,avp0)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INS initilization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2020-2025, by Kai Chen, All rights reserved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% sample date
ins.nt = 1/opt.sample_rate;
ins.old_imud = zeros(1,6);

% initialize position,attitude and velocity
ins.att = avp0(1:3);
ins.vel = avp0(4:6);
ins.pos = avp0(7:9);
ins.acc = zeros(3,1);
ins.Cnb = att2Cnb(ins.att);

% initialize imu error
ins.bg = zeros(3,1);
ins.ba = zeros(3,1);
ins.Kg = eye(3);
ins.Ka = eye(3);
ins.tauG = [inf;inf;inf];
ins.tauA = [inf;inf;inf];

% initialize earth related parameters
ins.eth = earth_update(ins.pos,ins.vel);
ins.Mpv = [0, ins.eth.Mpv4, 0; ins.eth.Mpv2, 0, 0; 0, 0, 1];
ins.wib = zeros(3,1);
ins.fb  = zeros(3,1);
ins.fn  = -ins.eth.gn;
ins.web = zeros(3,1);

% initialize state, state covariance matrix, system noise matrix
% state transition matrix, lever arm
init_att_unc = opt.init_att_unc;
init_vel_unc = opt.init_vel_unc;
init_pos_unc = opt.init_pos_unc;
init_bg_unc  = repmat(opt.init_bg_unc,1,3);
init_ba_unc  = repmat(opt.init_ba_unc,1,3);
psd_gyro = repmat(opt.psd_gyro,1,3);
psd_acce = repmat(opt.psd_acce,1,3);
psd_bg   = repmat(opt.psd_bg,1,3);
psd_ba   = repmat(opt.psd_ba,1,3);

ins.x  = [ins.att;ins.vel;ins.pos;ins.bg;ins.ba];
ins.P  = diag([init_att_unc,init_vel_unc,init_pos_unc,init_bg_unc,init_ba_unc].^2);
ins.Q  = diag([psd_gyro, psd_acce, zeros(1,3), psd_bg, psd_ba])*ins.nt;
ins.Phi= update_trans_mat(ins);
ins.lever = opt.lever';

ins.xa = zeros(15,1);
ins.Pa = zeros(15,15);

return


clear; close all; clc;

%%
% Plot settings

markerSize=10;
markerSize2=10;
fontSize=15;
faceAlpha1=0.3;
faceAlpha2=0.5;
cMap=gjet(4);

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=[febioFebFileNamePart,'.txt']; %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_force=[febioFebFileNamePart,'_force_out.txt']; %Log file name for exporting force
febioLogFileName_sed=[febioFebFileNamePart,'_stress_out.txt']; %Log file name for exporting strain energy density

tireWidth = 120;
tireRadius = 150;
tireThickness = 8;
threadHeight = 8;
tireBulge=tireWidth/8;
pointSpacing = 8;
tireCircumference = 2*pi.*tireRadius;

numFeatures = 20;
groveWidth = 8;

distKeep=75; %Distance after which model is cropped, empty = full model
displacementVert=0.8*tireThickness;
displacementHor=-tireRadius/20;
contactInitialOffset=0.1;

%Material parameter set
c1=1e-3; %Shear-modulus-like parameter
m1=2; %Material parameter setting degree of non-linearity
k_factor=100; %Bulk modulus factor
k=c1*k_factor; %Bulk modulus

% FEA control settings
numTimeSteps=20; %Number of time steps desired
max_refs=40; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=25; %Optimum number of iterations
max_retries=5; %Maximum number of retires
dtmin=(1/numTimeSteps)/100; %Minimum time step size
dtmax=1/numTimeSteps; %Maximum time step size
runMode='external';%'internal';
min_residual=1e-20;

%Contact parameters
contactPenalty=5;
laugon=0;
minaug=1;
maxaug=10;
fric_coeff=0.5;

%%

tireHalfWidth=tireWidth/2;
featureHeight = (tireCircumference-(numFeatures*groveWidth))./numFeatures;
featureWidth = tireHalfWidth-2*groveWidth;

% Coordinates of 1x1 square
v=[-1 -1 0;...
    1 -1 0;...
    1  1 0;...
    -1  1 0]/2;

% Deformation gradient tensor for shear
f=eye(3,3);
f(1,2)=1;

%Create boundary coordinates
V1=v; %Copy square
V1(:,1)=V1(:,1).*tireCircumference+tireCircumference/2; %Stretch to create circumference
V1(:,2)=V1(:,2).*tireHalfWidth+tireHalfWidth/2; %Stretch to get desired width
V1=evenlySpaceCurve(V1,pointSpacing,'linear',1,1:1:size(v,1)); %Evenly sample
V1=(f*V1')'; %Shear

%Create thread feature
Vf=v; %Copy square
Vf(:,1)=Vf(:,1).*featureHeight; %Stretch to create circumference
Vf(:,2)=Vf(:,2).*(featureWidth+groveWidth/2)+tireHalfWidth/2-groveWidth/4; %Stretch to get desired width
Vf=evenlySpaceCurve(Vf,pointSpacing,'linear',1,1:1:size(v,1)); %Evenly sample
Vf(:,1)=Vf(:,1)+featureHeight/2+groveWidth/2;

VF=cell(1,numFeatures);
for q=1:1:numFeatures
    Vff=Vf;
    Vff(:,1)=Vf(:,1)+(q-1).*(featureHeight+groveWidth); %Shift
    VF{q}=(f*Vff')'; %Shear and store
end

%%

%Defining 4 regions
regionSpec{1}={V1(:,[1 2])};
for q=1:1:numFeatures
    regionSpec{1}{q+1}=VF{q}(:,[1 2]);
end

for q=1:1:numFeatures
    regionSpec{q+1}{1}=VF{q}(:,[1 2]);
end

[Ft,Vt,Ct]=multiRegionTriMesh2D(regionSpec,pointSpacing,0,0);
Vt(:,3)=0;

Fb=fliplr(Ft);
Vb=Vt;
Vb(:,2)=-Vb(:,2);

Cb=Ct;
Cb(Cb>1)=Cb(Cb>1)-1+max(Ct);

[F,V,C]=joinElementSets({Ft,Fb},{Vt,Vb},{Ct,Cb});
[F,V]=mergeVertices(F,V);

%%

cFigure; hold on;
plotV(V1,'r.-','MarkerSize',markerSize);
plotV(VF,'b.-','MarkerSize',markerSize);
gpatch(F,V,C);

axisGeom(gca,fontSize); view(2);
colormap spectral; icolorbar;
gdrawnow;

%%

cMax=max(C);
numVert=size(V,1);
F2=F(C>1,:);
C2=C(C>1,:)+cMax;
Eb1=patchBoundary(F2);

Eb2=Eb1+numVert;
F=[F(C==1,:);F2+numVert];
V=[V;V];
C=[C(C==1,:);C2+max(C(:))];

%%
%

if tireBulge>eps(0)
    w=tireWidth;
    h=tireBulge;
    r=h/2+(w^2/(8*h));
    a=atan((w/2)./(r-h));

    TH = (V(:,2)./tireHalfWidth)*a;
    R = r.*ones(size(V,1),1);
    R(numVert+1:end)=R(numVert+1:end)+threadHeight;
    Z = V(:,1);
    [V(:,1),V(:,2),V(:,3)] = pol2cart(TH,R,Z);
    V=V(:,[3 2 1]);

    V(:,3)=V(:,3)-r+h;
else
    V(numVert+1:end,3)=V(numVert+1:end,3)+threadHeight;
end

%%
cFigure; hold on;
plotV(V1,'r.-','MarkerSize',markerSize);
plotV(VF,'b.-','MarkerSize',markerSize);
gpatch(F,V,C);

axisGeom(gca,fontSize); view(2);
colormap spectral; icolorbar;
gdrawnow;

%%

TH = V(:,1)./tireRadius;
R = tireRadius+V(:,3);

Z = V(:,2);
[V(:,1),V(:,2),V(:,3)] = pol2cart(TH,R,Z);
V=V(:,[1 3 2]); F=fliplr(F);

[Eb1,V1]=patchCleanUnused(Eb1,V);
[Eb2,V2]=patchCleanUnused(Eb2,V);

[F,V,~,indFix]=mergeVertices(F,V);

numSteps=ceil(threadHeight./pointSpacing)+1;

%Create quadrilateral faces
X=linspacen(V1(:,1),V2(:,1),numSteps);
Y=linspacen(V1(:,2),V2(:,2),numSteps);
Z=linspacen(V1(:,3),V2(:,3),numSteps);

Vn=[X(:) Y(:) Z(:)]; %Vertex set
Fn=repmat([fliplr(Eb1) Eb2],numSteps-1,1);
q=repmat(0:(numSteps-2),size(Eb1,1),1);
q=q(:);
Q=[q q q+1 q+1]*size(V1,1);
Fn=Fn+Q;

[Fn,Vn]=quad2tri(Fn,Vn,'a');

%%

[F,V]=patchCleanUnused(F,V);
C(C>1)=2; %Switch to use 2 for all thread profile parts
[F_out,V_out,C_out]=joinElementSets({F,Fn},{V,Vn},{C,max(C(:))+1*ones(size(Fn,1),1)});
[F_out,V_out]=patchCleanUnused(F_out,V_out);
[F_out,V_out]=mergeVertices(F_out,V_out);
Eb_out=patchBoundary(F_out);
%%

cFigure; hold on;
gpatch(F_out,V_out,C_out);
% patchNormPlot(Fq,Vq);
axisGeom(gca,fontSize); camlight headlight;
colormap spectral; icolorbar;
gdrawnow;

%%
% Create inner cylinder
inputStructCyl.cylRadius=tireRadius-tireThickness;
inputStructCyl.numRadial=ceil((2*pi*inputStructCyl.cylRadius)/pointSpacing);
inputStructCyl.cylHeight=tireWidth;
numheight=ceil(tireWidth./pointSpacing);
numheight=numheight+iseven(numheight); %Force uneven
inputStructCyl.numHeight=numheight;
inputStructCyl.meshType='tri';

%%
% Derive patch data for a cylinder
[F_in,V_in]=patchcylinder(inputStructCyl);
V_in=V_in(:,[1 3 2]);
Eb_in=patchBoundary(F_in);
C_in=(max(C_out)+1)*ones(size(F_in,1),1);

%%
inputStructGroup.outputType='label';
G_out=tesgroup(Eb_out,inputStructGroup);
G_in=tesgroup(Eb_in,inputStructGroup);

yMean_out=nan(2,1);
yMean_in=nan(2,1);
for q=1:1:2
    ind_out=Eb_out(G_out==q,:);
    yMean_out(q)=mean(V_out(ind_out(:),2));

    ind_in=Eb_in(G_in==q,:);
    yMean_in(q)=mean(V_in(ind_in(:),2));
end

[~,ind1_out]=min(yMean_out);
[~,ind2_out]=max(yMean_out);
indCurve1_out=edgeListToCurve(Eb_out(G_out==ind1_out,:));
indCurve1_out=indCurve1_out(1:end-1);
indCurve2_out=edgeListToCurve(Eb_out(G_out==ind2_out,:));
indCurve2_out=indCurve2_out(1:end-1);

[~,ind1_in]=min(yMean_in);
[~,ind2_in]=max(yMean_in);
indCurve1_in=edgeListToCurve(Eb_in(G_in==ind1_in,:));
indCurve1_in=indCurve1_in(1:end-1);
indCurve2_in=edgeListToCurve(Eb_in(G_in==ind2_in,:));
indCurve2_in=indCurve2_in(1:end-1);

[Fs1,Vs1]=regionTriMesh2D({V_out(indCurve1_out,[1 3]),V_in(indCurve1_in,[1 3])},pointSpacing,0);
Vs1(:,3)=-tireWidth/2;
Vs1=Vs1(:,[1 3 2]);
Cs1=(max(C_in)+1)*ones(size(Fs1,1),1);

[Fs2,Vs2]=regionTriMesh2D({V_out(indCurve2_out,[1 3]),V_in(indCurve2_in,[1 3])},pointSpacing,0);
Vs2(:,3)=tireWidth/2;
Vs2=Vs2(:,[1 3 2]);
Fs2=fliplr(Fs2);
Cs2=(max(Cs1)+1)*ones(size(Fs2,1),1);

%%

[FT,VT,CT]=joinElementSets({F_out,F_in,Fs1,Fs2},{V_out,V_in,Vs1,Vs2},{C_out,C_in,Cs1,Cs2});
[FT,VT]=patchCleanUnused(FT,VT);
[FT,VT]=mergeVertices(FT,VT);

%%

cFigure; hold on;

gpatch(FT,VT,CT,'none');
patchNormPlot(FT,VT);

axisGeom(gca,fontSize); camlight headlight;
colormap spectral; icolorbar;
gdrawnow;

%%

V_regions=getInnerPoint(FT,VT); %Define region points
V_holes=[]; %Define hole points
[regionTetVolumes]=tetVolMeanEst(FT,VT); %Volume estimate for regular tets
stringOpt='-pq1.2AaY'; %Options for tetgen

%%
% Mesh using TetGen

%Create tetgen input structure
inputStruct.stringOpt=stringOpt; %Tetgen options
inputStruct.Faces=FT; %Boundary faces
inputStruct.Nodes=VT; %Nodes of boundary
inputStruct.faceBoundaryMarker=CT;
inputStruct.regionPoints=V_regions; %Interior points for regions
inputStruct.holePoints=V_holes; %Interior points for holes
inputStruct.regionA=regionTetVolumes; %Desired tetrahedral volume for each region

% Mesh model using tetrahedral elements using tetGen
[meshOutput]=runTetGen(inputStruct); %Run tetGen

%%
% Access mesh output structure

E=meshOutput.elements; %The elements
V=meshOutput.nodes; %The vertices or nodes
Fb=meshOutput.facesBoundary; %The boundary faces
Cb=meshOutput.boundaryMarker; %The boundary markers

%%
% Visualization

hf=cFigure;
subplot(1,2,1); hold on;
title('Input boundaries','FontSize',fontSize);
hp(1)=gpatch(Fb,V,Cb,'k',faceAlpha1);
hp(2)=plotV(V_regions,'r.','MarkerSize',markerSize);
legend(hp,{'Input mesh','Interior point(s)'},'Location','NorthWestOutside');
axisGeom(gca,fontSize); camlight headlight;
colormap(cMap); icolorbar;

hs=subplot(1,2,2); hold on;
title('Tetrahedral mesh','FontSize',fontSize);

% Visualizing using |meshView|
optionStruct.hFig=[hf,hs];
meshView(meshOutput,optionStruct);

axisGeom(gca,fontSize);
gdrawnow;

%%

if ~isempty(distKeep)
    Fb_full=Fb;
    V_full=V;
    [Fb_full,V_full]=patchCleanUnused(Fb_full,V_full);

    VE=patchCentre(E,V);
    logicKeepElements = VE(:,3) <= (min(VE(:,3))+distKeep);
    E=E(logicKeepElements,:);
    indKeepNodes=unique(E);
    logicKeep_Fb=all(ismember(Fb,indKeepNodes),2);
    Fb=Fb(logicKeep_Fb,:);
    Cb=Cb(logicKeep_Fb,:);

    logicKeep_F=all(ismember(F,indKeepNodes),2);
    F=F(logicKeep_F,:);

    [E,V,indFix]=patchCleanUnused(E,V);
    Fb=indFix(Fb);
    F=indFix(F);
    meshOutput.nodes=V;
    meshOutput.facesBoundary=Fb;
    meshOutput.boundaryMarker=Cb;
    meshOutput.faces=F;
    meshOutput.elements=E;
    meshOutput.elementMaterialID=ones(size(E,1),1);
else
    Fb_full=Fb;
    V_full=V;
end

%%

hf=cFigure;
subplot(1,2,1); hold on;
title('Input boundaries','FontSize',fontSize);
hp(1)=gpatch(Fb,V,Cb,'k',faceAlpha1);
hp(2)=plotV(V_regions,'r.','MarkerSize',markerSize);
legend(hp,{'Input mesh','Interior point(s)'},'Location','NorthWestOutside');
axisGeom(gca,fontSize); camlight headlight;
colormap(cMap); icolorbar;

hs=subplot(1,2,2); hold on;
title('Tetrahedral mesh','FontSize',fontSize);

% Visualizing using |meshView|
optionStruct.hFig=[hf,hs];
meshView(meshOutput,optionStruct);

axisGeom(gca,fontSize);
gdrawnow;

%%

plateDim=[2*tireRadius tireWidth];
plateEl=ceil(plateDim./pointSpacing);
[Eg,Vg]=quadPlate(plateDim,plateEl);
Vg(:,3)=Vg(:,3)+min(VT(:,3))-contactInitialOffset;
center_of_mass=mean(Vg,1);

%% Joining node sets
Eg=Eg+size(V,1); %Fixed element indices
V=[V;Vg;]; %Combined node sets

%%

cFigure; hold on;
hp(1)=gpatch(Fb,V,Cb,'k',faceAlpha1);
hp(2)=gpatch(Eg,V,'kw','k',faceAlpha1);
% legend(hp,{'Input mesh','Interior point(s)'},'Location','NorthWestOutside');
axisGeom(gca,fontSize); camlight headlight;
colormap(cMap); icolorbar;
gdrawnow;

%% Define contact surfaces

% The rigid master surface of the sphere
F_contact_secondary=Eg;

% The deformable slave surface of the slab
logicContactSurf1=Cb==2;
F_contact_primary=fliplr(Fb(logicContactSurf1,:));

% Plotting surface models
cFigure; hold on;
title('Contact sets and normal directions','FontSize',fontSize);

gpatch(Fb,V,'kw','none',faceAlpha2);
hl(1)=gpatch(F_contact_secondary,V,'g','k',1);
patchNormPlot(F_contact_secondary,V);
hl(2)=gpatch(F_contact_primary,V,'b','k',1);
patchNormPlot(F_contact_primary,V);

legend(hl,{'Secondary','Primary'});

axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Define boundary conditions

%Supported nodes
bcSupportList=unique(Fb(Cb==4,:));

%%
% Visualize BC's
hf=cFigure;
title('Boundary conditions model','FontSize',fontSize);
xlabel('X','FontSize',fontSize); ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;

gpatch(Fb,V,'w','none',faceAlpha2);

hl2(1)=gpatch(Eg,V,'gw','k',1);
hl2(2)=plotV(V(bcSupportList,:),'k.','MarkerSize',markerSize);

legend(hl2,{'Rigid body plate','BC support'});

axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='3.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Create control structure for use by all steps
febio_spec.Control.analysis='STATIC';
stepStruct.Control.time_steps=numTimeSteps;
stepStruct.Control.step_size=1/numTimeSteps;
stepStruct.Control.solver.max_refs=max_refs;
stepStruct.Control.solver.max_ups=max_ups;
stepStruct.Control.solver.symmetric_stiffness=0;
febio_spec.Control.solver.min_residual=min_residual;
stepStruct.Control.time_stepper.dtmin=dtmin;
stepStruct.Control.time_stepper.dtmax=dtmax;
stepStruct.Control.time_stepper.max_retries=max_retries;
stepStruct.Control.time_stepper.opt_iter=opt_iter;

%Add template based default settings to proposed control section
[stepStruct.Control]=structComplete(stepStruct.Control,febio_spec.Control,1); %Complement provided with default if missing

%Remove control field (part of template) since step specific control sections are used
febio_spec=rmfield(febio_spec,'Control');

febio_spec.Step.step{1}.Control=stepStruct.Control;
febio_spec.Step.step{1}.ATTR.id=1;
febio_spec.Step.step{2}.Control=stepStruct.Control;
febio_spec.Step.step{2}.ATTR.id=2;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='Ogden';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.c1=c1;
febio_spec.Material.material{1}.m1=m1;
febio_spec.Material.material{1}.c2=c1;
febio_spec.Material.material{1}.m2=-m1;
febio_spec.Material.material{1}.k=k;

materialName2='Material2';
febio_spec.Material.material{2}.ATTR.name=materialName2;
febio_spec.Material.material{2}.ATTR.type='rigid body';
febio_spec.Material.material{2}.ATTR.id=2;
febio_spec.Material.material{2}.density=1;
febio_spec.Material.material{2}.center_of_mass=center_of_mass;

%Mesh section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='nodeSet_all'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% -> Elements
partName1='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of this part
febio_spec.Mesh.Elements{1}.ATTR.type='tet4'; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E; %The element matrix

partName2='Part2';
febio_spec.Mesh.Elements{2}.ATTR.name=partName2; %Name of this part
febio_spec.Mesh.Elements{2}.ATTR.type='quad4'; %Element type
febio_spec.Mesh.Elements{2}.elem.ATTR.id=size(E,1)+(1:1:size(Eg,1))'; %Element id's
febio_spec.Mesh.Elements{2}.elem.VAL=Eg; %The element matrix

% -> NodeSets
nodeSetName1='bcSupportList';
febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.node.ATTR.id=bcSupportList(:);

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

febio_spec.MeshDomains.ShellDomain.ATTR.name=partName2;
febio_spec.MeshDomains.ShellDomain.ATTR.mat=materialName2;

% -> Surfaces
surfaceName1='contactSurface1';
febio_spec.Mesh.Surface{1}.ATTR.name=surfaceName1;
febio_spec.Mesh.Surface{1}.quad4.ATTR.id=(1:1:size(F_contact_secondary,1))';
febio_spec.Mesh.Surface{1}.quad4.VAL=F_contact_secondary;

surfaceName2='contactSurface2';
febio_spec.Mesh.Surface{2}.ATTR.name=surfaceName2;
febio_spec.Mesh.Surface{2}.tri3.ATTR.id=(1:1:size(F_contact_primary,1))';
febio_spec.Mesh.Surface{2}.tri3.VAL=F_contact_primary;

% -> Surface pairs
febio_spec.Mesh.SurfacePair{1}.ATTR.name='Contact1';
febio_spec.Mesh.SurfacePair{1}.primary=surfaceName2;
febio_spec.Mesh.SurfacePair{1}.secondary=surfaceName1;

%Boundary condition section
% -> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.type='fix';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.dofs='x,y,z';

%Rigid section
% -> Prescribed rigid body boundary conditions
febio_spec.Step.step{1}.Rigid.rigid_constraint{1}.ATTR.name='RigidFix_1';
febio_spec.Step.step{1}.Rigid.rigid_constraint{1}.ATTR.type='fix';
febio_spec.Step.step{1}.Rigid.rigid_constraint{1}.rb=2;
febio_spec.Step.step{1}.Rigid.rigid_constraint{1}.dofs='Rx,Ry';

febio_spec.Step.step{1}.Rigid.rigid_constraint{2}.ATTR.name='RigidPrescribe';
febio_spec.Step.step{1}.Rigid.rigid_constraint{2}.ATTR.type='prescribe';
febio_spec.Step.step{1}.Rigid.rigid_constraint{2}.rb=2;
febio_spec.Step.step{1}.Rigid.rigid_constraint{2}.dof='Rz';
febio_spec.Step.step{1}.Rigid.rigid_constraint{2}.value.ATTR.lc=1;
febio_spec.Step.step{1}.Rigid.rigid_constraint{2}.value.VAL=(displacementVert+contactInitialOffset);
febio_spec.Step.step{1}.Rigid.rigid_constraint{2}.relative=0;

febio_spec.Step.step{2}.Rigid.rigid_constraint{1}.ATTR.name='RigidFix_1';
febio_spec.Step.step{2}.Rigid.rigid_constraint{1}.ATTR.type='fix';
febio_spec.Step.step{2}.Rigid.rigid_constraint{1}.rb=2;
febio_spec.Step.step{2}.Rigid.rigid_constraint{1}.dofs='Ry,Rz,Ru,Rv,Rw';

febio_spec.Step.step{2}.Rigid.rigid_constraint{2}.ATTR.name='RigidPrescribe';
febio_spec.Step.step{2}.Rigid.rigid_constraint{2}.ATTR.type='prescribe';
febio_spec.Step.step{2}.Rigid.rigid_constraint{2}.rb=2;
febio_spec.Step.step{2}.Rigid.rigid_constraint{2}.dof='Rx';
febio_spec.Step.step{2}.Rigid.rigid_constraint{2}.value.ATTR.lc=2;
febio_spec.Step.step{2}.Rigid.rigid_constraint{2}.value.VAL=displacementHor;
% febio_spec.Step.step{2}.Rigid.rigid_constraint{2}.relative=1;

%Contact section
febio_spec.Contact.contact{1}.ATTR.type='sliding-elastic';
febio_spec.Contact.contact{1}.ATTR.surface_pair=febio_spec.Mesh.SurfacePair{1}.ATTR.name;
febio_spec.Contact.contact{1}.two_pass=0;
febio_spec.Contact.contact{1}.laugon=laugon;
febio_spec.Contact.contact{1}.tolerance=0.2;
febio_spec.Contact.contact{1}.gaptol=0;
febio_spec.Contact.contact{1}.minaug=minaug;
febio_spec.Contact.contact{1}.maxaug=maxaug;
febio_spec.Contact.contact{1}.search_tol=0.01;
febio_spec.Contact.contact{1}.search_radius=0.1*sqrt(sum((max(V,[],1)-min(V,[],1)).^2,2));
febio_spec.Contact.contact{1}.symmetric_stiffness=0;
febio_spec.Contact.contact{1}.auto_penalty=1;
febio_spec.Contact.contact{1}.penalty=contactPenalty;
febio_spec.Contact.contact{1}.fric_coeff=fric_coeff;

%LoadData section
% -> load_controller
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{1}.points.point.VAL=[0 0; 1 1; 2 1];

febio_spec.LoadData.load_controller{2}.ATTR.id=2;
febio_spec.LoadData.load_controller{2}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{2}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{2}.points.point.VAL=[0 0; 1 0; 2 1];

%Output section
% -> log file
febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{1}.VAL=1:size(V,1);

febio_spec.Output.logfile.node_data{2}.ATTR.file=febioLogFileName_force;
febio_spec.Output.logfile.node_data{2}.ATTR.data='Rx;Ry;Rz';
febio_spec.Output.logfile.node_data{2}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{2}.VAL=1:size(V,1);

febio_spec.Output.logfile.element_data{1}.ATTR.file=febioLogFileName_sed;
febio_spec.Output.logfile.element_data{1}.ATTR.data='sed';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.element_data{1}.VAL=1:size(E,1);

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% |febView(febio_spec); %Viewing the febio file|

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

%% Running the FEBio analysis
% To run the analysis defined by the created FEBio input file the
% |runMonitorFEBio| function is used. The input for this function is a
% structure defining job settings e.g. the FEBio input file name. The
% optional output runFlag informs the user if the analysis was run
% succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.runMode=runMode;

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!

%% Import FEBio results

if runFlag==1 %i.e. a succesful run

    %%
    % Importing nodal displacements from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp),1,1);

    %Access data
    N_disp_mat=dataStruct.data; %Displacement
    timeVec=dataStruct.time; %Time

    %Create deformed coordinate set
    V_DEF=N_disp_mat+repmat(V,[1 1 size(N_disp_mat,3)]);

    %%
    % Importing element stress from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_sed),1,1);

    %Access data
    E_sed_mat=dataStruct.data;
    E_sed_mat(isnan(E_sed_mat))=0;

        %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    [CV]=faceToVertexMeasure(E,V,E_sed_mat(:,:,end));

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
    title('Strain energy density','Interpreter','Latex')
    gpatch(Fb_full,V_full,'w','none',0.25);
    hp=gpatch(Fb,V_DEF(:,:,end),CV,'none',1); %Add graphics object to animate
%     hp.Marker='.';
%     hp.MarkerSize=markerSize2;
    hp.FaceColor='interp';

    hp2=gpatch(Eg,V_DEF(:,:,end),'w','none',0.5); %Add graphics object to animate
    Ebb=patchBoundaryLabelEdges(Fb,V,Cb);
    hp3=gpatch(Ebb,V_DEF(:,:,end),'none','k',1,3);
    hp4=gpatch(patchBoundary(Eg),V_DEF(:,:,end),'none','kw',1,3);

    axisGeom(gca,fontSize);
    colormap(spectral(250)); colorbar;
    caxis([0 0.25*max(E_sed_mat(:))]);
    axis(axisLim(V_DEF,V_full)); %Set axis limits statically
    camlight headlight;

    % Set up animation features
    animStruct.Time=timeVec; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments

        [CV]=faceToVertexMeasure(E,V,E_sed_mat(:,:,qt));

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp hp hp2 hp3 hp4]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData','Vertices','Vertices','Vertices'}; %Properties of objects to animate
        animStruct.Set{qt}={V_DEF(:,:,qt),CV,V_DEF(:,:,qt),V_DEF(:,:,qt),V_DEF(:,:,qt)}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

end

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

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


function [model,b] = robustlssvm(model,ab,X,Y)
% Robust training in the case of non-Gaussian noise or outliers
%(only possible with the object oriented interface)
%
% >> model     = robustlssvm(model)
%
% Robustness towards outliers can be achieved by reducing the
% influence of support values corresponding to large errors.
%
%
% Full syntax
%
%     1. Using the object oriented interface:
%
% >> model = robustlssvm(model)
%
%       Outputs
%         model : Robustly trained object oriented representation of the LS-SVM model
%       Inputs
%         model : Object oriented representation of the LS-SVM model
%
% See also:
%   trainlssvm, tunelssvm, crossvalidate

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

if iscell(model),
    func = 1;
    model = initlssvm(model{:});
else
    func = 0;
end

if model.type(1)~='f',
    error('Robustly weighted least squares only implemented for regression case...');
end

if nargin>1,
    if iscell(ab) && ~isempty(ab),
        model.alpha = ab{1};
        model.b = ab{2};
        model.status = 'trained';
        if nargin>=4,
            model = trainlssvm(model,X,Y);
        end
    else
        model = trainlssvm(model,ab,X);
    end
else
    model = trainlssvm(model);
end

% model errors
ek = model.alpha./model.gam';
g = model.gam;
%
% robust estimation of the variance
%
eval('delta=model.delta;','delta=[];')
for j=1:500
    vare = 1.483*median(abs((ek)-median(ek)));
    alphaold = model.alpha;
    %
    % robust re-estimation of the alpha's and the b
    %
    cases = reshape((ek./vare),1,model.nb_data);
    W = weightingscheme(cases,model.weights,delta);
    W = g*W;

    model = changelssvm(model,'gam',W);
    %     model = changelssvm(model,'implementation','MATLAB');
    model = trainlssvm(model);
    ek = model.alpha./model.gam';

    if norm(abs(alphaold-model.alpha),'fro')<=1e-4,
        fprintf('Converged after %.0f iteration(s)', j);
        if func && nargout~=1,
            b = model.b;
            model = model.alpha;
        end
        return
    end
    model.status = 'changed';
end


function K = simwhiteXsimwhiteKernCompute(simKern1, simKern2, t1, t2)

% SIMWHITEXSIMWHITEKERNCOMPUTE Compute a cross kernel between two SIM-WHITE
% kernels.
% FORMAT
% DESC computes cross kernel terms between two SIM-WHITE kernels for
% the multiple output kernel.
% ARG simKern1 : the kernel structure associated with the first SIM-WHITE
% kernel.
% ARG simKern2 : the kernel structure associated with the second SIM-WHITE
% kernel.
% ARG t1 : inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between two SIM-WHITE kernels for
% the multiple output kernel.
% ARG simKern1 : the kernel structure associated with the first SIM-WHITE
% kernel.
% ARG simKern2 : the kernel structure associated with the second SIM-WHITE
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% SEEALSO : multiKernParamInit, multiKernCompute, simwhiteKernParamInit
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 4
  t2 = t1;
end
if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end
if simKern1.variance ~= simKern2.variance
  error('Kernels cannot be cross combined if they have different variances.')
end

% Parameters of the kernels required in the computation
variance = simKern1.variance;
sensitivity1 = simKern1.sensitivity;
sensitivity2 = simKern2.sensitivity;
decay1 = simKern1.decay;
decay2 = simKern2.decay;

isStationary = (simKern1.isStationary == true) & (simKern2.isStationary == true);

% Auxiliary constants and matrices
c = variance * sensitivity1 * sensitivity2 / (decay1 + decay2);
T1 = repmat(t1, 1, size(t2, 1));
T2 = repmat(t2.', size(t1, 1), 1);
ind = (T1 < T2);
Dv = decay2 .* ind + decay1 .* (~ind);
K = exp(-Dv .* abs(T1-T2));
if (isStationary == false)
    K = K - exp(-(decay1 * T1 + decay2 * T2));
end
K = c*K;


function aa = fitnesstra2(pop,para)
[pops,numvar]=size(pop);
for i=1:pops
    time=pop(i,5)+pop(i,6);
    kk=trajt(para,pop(i,:));
    tt=torque(kk);
    ft=ftorque(tt);
    poo=kk(1:2,:);
    fq=sum(sum(abs(diff(poo'))));
    pos=forkin(poo);xx=pos(1,:);yy=pos(2,:);
    x=diff(xx);
    y=diff(yy);
    dis=sqrt(x.^2+y.^2);
    fdis=sum(dis);
    a1=2;
    a2=1.8;
    a3=2;
    a4=1;
    f=a1*ft+a2*fq+a3*fdis+a4*time;
    aa(i)=1/f;
end


% BARH Horizontal bar graph.
%    BARH(X,Y) draws the columns of the M-by-N matrix Y as M groups of
%    N horizontal bars. The vector X must not have duplicate values.
%
%    BARH(Y) uses the default value of X=1:M.  For vector inputs,
%    BARH(X,Y) or BARH(Y) draws LENGTH(Y) bars.  The colors are set by
%    the colormap.
%
%    BARH(X,Y,WIDTH) or BARH(Y,WIDTH) specifies the width of the
%    bars. Values of WIDTH > 1, produce overlapped bars.  The
%    default value is WIDTH=0.8.
%
%    BARH(...,'grouped') produces the default horizontal grouped bar chart.
%    BARH(...,'stacked') produces a horizontal stacked bar chart.
%    BARH(...,LINESPEC) uses the line color specified (one of 'rgbymckw').
%
%    BARH(AX,...) plots into AX instead of GCA.
%
%    H = BARH(...) returns a vector of handles to barseries objects.
%
%    Use SHADING FACETED to put edges on the bars.  Use SHADING FLAT to
%    turn them off.
%
%    Examples: subplot(3,1,1), barh(rand(10,5),'stacked'), colormap(cool)
%              subplot(3,1,2), barh(0:.25:1,rand(5),1)
%              subplot(3,1,3), barh(rand(2,3),.75,'grouped')
%
%    See also PLOT, BAR, BAR3, BAR3H, HISTOGRAM.
%
%    Copyright 1984-2017 The MathWorks, Inc.
%
%    Reference page in Doc Center
%       doc barh
%
%    Other functions named barh
%
%       fints/barh    ts/barh
%


function varargout = mtimes(varargin)
%*   Pointwise multiplication for DISKFUN objects.
%   c*F or F*c multiplies a DISKFUN F by a scalar c.
%
%   F*G computes the integral of F(s,t)G(l,s) over s, and this is the
%   continuous analogue of matrix-matrix multiplication.
%
% See also DISKFUN/TIMES.

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

[varargout{1:nargout}] = mtimes@separableApprox(varargin{:});

end


function [au] = m2au(m)
% Convert length from meters to astronomical units.
% Chad A. Greene 2012
au = m*6.684587122671e-12;