function MV = ffmInteractionsSparseHF(X,Xbox,Y,Ybox,V,Ibox,green,k,edg,tol)
%+========================================================================+
%|                                                                        |
%|         OPENFFM - LIBRARY FOR FAST AND FREE MEMORY CONVOLUTION         |
%|           openFfm is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2019.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : ffmInteractionsSparseHF.m                     |
%|    #    |   VERSION    : 0.6                                           |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2019                                    |
%| ( === ) |   SYNOPSIS   : Sparse product for high-frequency compressible|
%|  `---'  |                leaves                                        |
%+========================================================================+

% Initialisation du produit Matrice-Vecteur
MV = zeros(size(X,1),1,class(V));

% Quadrature spherique de Geggenbauer
[Xq,Wq,l] = ffmQuadratureHF(k,edg,tol);

% Unicite des vecteurs de translation
XY        = Ybox.ctr(Ibox(:,2),:) - Xbox.ctr(Ibox(:,1),:);
[~,Il,It] = unique(floor(XY*1e6),'rows','stable');
Nt        = length(Il);

% Fonctions de transfert
Txy = cell(Nt,1);
for i = 1:Nt
    Txy{i} = ffmTransfertHF(Xq,Wq,XY(Il(i),:),k,l);
end

% Convolution en Y
Vy = cell(size(Ybox.ind));
for i = unique(Ibox(:,2)')
    % Boite centree en Y
    iy = Ybox.ind{i};
    ny = length(iy);
    Ym = Y(iy,:) - ones(ny,1)*Ybox.ctr(i,:);

    % Transformee de Fourier inverse (Ym->Xq)
    Vy{i} = ffmInterpHF(Xq,Ym,V(iy),-1,tol);

    % Derivation du noyau en Y
    if strcmp(green(1:end-1),'grady[exp(ikr)/r]')
        j     = str2double(green(end));
        Vy{i} = -1i*Xq(:,j) .* Vy{i};
    end
end

% Translations
TVy = cell(size(Xbox.ind));
for i = 1:length(TVy)
    TVy{i} = 0;
end
for i = 1:size(Ibox,1)
    TVy{Ibox(i,1)} = TVy{Ibox(i,1)} + Txy{It(i)}.*Vy{Ibox(i,2)};
end

% Convolutions en X
for i = unique(Ibox(:,1)')
    % Boite centree en X
    ix = Xbox.ind{i};
    nx = length(ix);
    Xm = X(ix,:) - ones(nx,1)*Xbox.ctr(i,:);

    % Derivation du noyau en X
    if strcmp(green(1:end-1),'gradx[exp(ikr)/r]')
        j      = str2double(green(end));
        TVy{i} = 1i*Xq(:,j) .* TVy{i};
    end

    % Transformee de Fourier (Xq->X)
    MV(ix) = ffmInterpHF(Xm,Xq,TVy{i},+1,tol);
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Xq,Wq,l] = ffmQuadratureHF(k,edg,tol)
% Ordre harmonique (E. Darve)
l = floor( abs(k)*sqrt(3)*edg - log(tol) );

% Quadrature de Gauss sur [-1,1]
u     = 1:l;
u     = u ./ sqrt(4*u.^2 - 1);
[V,x] = eig( diag(u,-1) + diag(u,+1) );
[x,I] = sort(diag(x));
w     = 2*V(1,I)'.^2;

% Quadrature de Gauss sur [0,1] par transformation lineaire
a = 0;   b = 1;
x = 0.5*(b-a)*x + 0.5*(a+b);
w = 0.5*(b-a)*w;

% Quadrature de Gauss en elevation (phi)
phi = acos(2*x(end:-1:1)-1) - pi/2;
Wp  = 2*w(end:-1:1)';

% Quadrature reguliere en azimut (theta)
Ntheta = 2*(l+1);
theta  = 2*pi/Ntheta*(0:Ntheta-1)';
Wt     = 2*pi/Ntheta*ones(Ntheta,1);

% Quadrature spherique par produit tensoriel
[theta,phi] = ndgrid(theta,phi);
theta       = theta(:);
phi         = phi(:);
Wq          = Wt * Wp;
Wq          = Wq(:);

% Coordonnees carthesienne et produit par le nombre d'onde
[x,y,z] = sph2cart(theta,phi,1);
Xq      = k*[x,y,z];

% Securite
if (l>3)
    % Harmoniques spheriques jusqu'a l'ordre 3
    n   = 1;
    Ylm = zeros(length(theta),16);
    for ll = 0:3
        Plm = sqrt((2*ll+1)/(4*pi)) * legendre(ll,cos(pi/2-phi),'sch').';
        for m = -ll:ll
            if m<0
                Ylm(:,n) = Plm(:,abs(m)+1) .* sin(abs(m).*theta);
            else
                Ylm(:,n) = Plm(:,abs(m)+1) .* cos(abs(m).*theta);
            end
            n = n+1;
        end
    end

    % Test de l'integration spherique des harmoniques (produit scalaire)
    [m,n] = size(Ylm);
    if norm( (Ylm' * spdiags(Wq,0,m,m) * Ylm) - eye(n) , 'inf') > 1e-5
        error('ffmQuadratureHF.m - error 1');
    end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function T = ffmTransfertHF(Xq,ws,xy,k,l)
% Fonctions de hankel spherique 1ere espece
besselhs = @(n,z) sqrt(pi./(2.*z)) .* besselh(n+0.5,1,z);

% Distance
kr = k*norm(xy);

% cosinus(angle incidence)
x = Xq*(-xy/kr)';

% Initialisation de la recursion sur (besselhs(kr) * Pl(cos(phi))
P0 = ones(size(x));
P1 = x;
T  = besselhs(0,kr).*P0 + 3i*besselhs(1,kr).*P1;

% Construction recursive des polynomes de Legendre
for ll = 2:l
    P2 = 1/ll .* ( (2*(ll-1)+1).*x.*P1 - (ll-1).*P0 );
    T  = T + (2*ll+1) * 1i^ll * besselhs(ll,kr) .* P2;
    P0 = P1;
    P1 = P2;
end

% Operateur de Translation
T = 1i*abs(k)/(4*pi) .* ws .* T;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function MV = ffmInterpHF(X,Y,V,iflag,tol)
% Dimensions
Nx   = size(X,1);
Ny   = size(Y,1);
Nmax = max(Nx,Ny);
Nmin = min(Nx,Ny);

% DFT non uniforme
if (Nmin<100) || (Nmin<log(Nmax))
    MV = exp(1i*iflag*X*Y') * V;

% FFT non uniforme
else
    % Conversion de type
    if isa(X,'single') || isa(Y,'single') || isa(V,'single')
        sgl = 1;
        X = double(X); Y = double(Y); V = double(V);
    else
        sgl = 0;
    end

    % Initialisation produit Matrice-Vecteur
    MV = zeros(Nx,1) + 1i*zeros(Nx,1);

    % Prevention de coplanarite en X
    Nx         = Nx + 1;
    X(end+1,:) = X(end,:) + tol;
    MV(end+1)  = 0;

    % Prevention de coplanarite en X
    Ny         = Ny + 1;
    Y(end+1,:) = Y(end,:) + tol;
    V(end+1)   = 0;

    % Transformee de Fourier
    ier    = 0;
    mex_id = 'nufft3d3f90(i int[x], i double[], i double[], i double[], i dcomplex[], i int[x], i double[x], i int[x], i double[], i double[], i double[], io dcomplex[], io int[x])';
    MV     = nufft3d(mex_id,Ny,Y(:,1),Y(:,2),Y(:,3),V, iflag, tol, ...
        Nx,X(:,1),X(:,2),X(:,3),MV, ier, 1, 1, 1, 1, 1);

    % Supression dernier point
    MV = MV(1:end-1);

    % Conversion de type
    if sgl
        MV = single(MV);
    end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [mMedModF, mStdL, loopout] = brutebootloglike_a2(time_as, time_asf, bootloops,fT1, nMod)
    % BRUTEBOOTLOGLIKE_A2 Bootstrap analysis of Omori parameters calculated by brute force
    % (p1,p2,c1,c2,k1,k2)-pair is mean of the bootstrap values by determining the mean cumulative
    % number modeled a end of the learning period
    % Standard deviations are calculated as the 2nd moment, not to rely fully on normal distributions
    %
    % [mMedModF, mStdL, loopout] = BRUTEBOOTLOGLIKE_A2(time_as, time_asf, bootloops,fT1, nMod);
    % -------------------------------------------------------------------------------
    %
    % Input parameters:
    %   time_as     Delay times [days] of learning period
    %   time_asf    Delay times [days] until end of forecast period
    %   bootloops   Number of bootstraps
    %   fT1         Time of biggest aftershock in learning period
    %   nMod        Model to fit data, three models including a secondary aftershock sequence.
    %               Different models have varying amount of free parameters
    %               before (p1,c1,k1) and after (p2,c2,k2) the aftershock occurence
    %               1: modified Omori law (MOL): 3 free parameters
    %                  p1=p2,c1=c2,k1=k2
    %               2: MOL with one secondary aftershock sequence:4 free parameters
    %                  p1=p2,c1=c2,k1~=k2
    %               3: MOL with one secondary aftershock sequence:5 free parameters
    %                  p1~=p2,c1=c2,k1~=k2
    %               4: MOL with one secondary aftershock sequence:6 free parameters
    %                  p1~=p2,c1~=c2,k1~=k2
    %
    % Output parameters:
    %  mMedModF :  Result matrix including the values for the mean forecast at end of forecast period
    %  mStdL    :  Uncertainties of fit to the data in learning period
    %  loopout     contains all results
    %
    % Samuel Neukomm / S. Wiemer / J. Woessner
    % updated: 05.08.03

    time_as = sort(time_as);
    %bootloops = 50; % number of bootstrap samples
    n = length(time_as);
    loopout = nan(bootloops,9); % 8 from bruteforceloglike_a2, plus variate column.
    % Initialize random seed
    rng('shuffle');

    % i = (1:n)';
    for j = 1:bootloops
        newtas = sort(datasample(time_as, n, 'Replace',true));
        [pv1, pv2, cv1, cv2, kv1, kv2, fAIC, fL] = bruteforceloglike_a2(newtas, fT1, nMod);
        loopout(j,1:8) = [pv1, pv2, cv1, cv2, kv1, kv2, fAIC, fL];
    end

    % New version: Choose mean (p,c,k)-variables by modelling the cumulative number at end of
    % the learning period

    % 2nd moment i.e. Standard deviations
    [pstd1] = std(loopout(:,1),1,'omitnan');
    [pstd2] = std(loopout(:,2),1,'omitnan');
    [cstd1] = std(loopout(:,3),1,'omitnan');
    [cstd2] = std(loopout(:,4),1,'omitnan');
    [kstd1] = std(loopout(:,5),1,'omitnan');
    [kstd2] = std(loopout(:,6),1,'omitnan');

    % Uncertainties of fit
    mStdL = [pstd1 pstd2 cstd1 cstd2 kstd1 kstd2];

    %% Compute best fitting pair of variates
    % TODO (maybe) vectorize this
    n_time_asf = length(time_asf);

    pv1=loopout(:,1);
    pv2=loopout(:,2);
    cv1=loopout(:,3);
    cv2=loopout(:,4);
    kv1=loopout(:,5);
    kv2=loopout(:,6);

    cumnr_model = OmoriModel.doForecast(nMod,time_asf, pv1, cv1, kv1, fT1, kv2, pv2, cv2);
    loopout(:,9) = max(cumnr_model);

    %%
    [Y, in] = sort(loopout(:,9));
    loops = loopout(in,:);
    % % Median values: Old version
    % vMedian = abs(loops(:,9)-median(loops(:,9)));
    % nMedian = (find(vMedian == min(vMedian)));
    %
    % if length(nMedian(:,1)) > 1
    %     nMedian = nMedian(1,1);
    % end
    % pmedian1 = loops(nMedian,1);
    % pmedian2 = loops(nMedian,2);
    % cmedian1 = loops(nMedian,3);
    % cmedian2 = loops(nMedian,4);
    % kmedian1 = loops(nMedian,5);
    % kmedian2 = loops(nMedian,6);
    %
    % mMedModF = [pmedian1, pstd1, pmedian2, pstd2, cmedian1, cstd1, cmedian2, cstd2, kmedian1, kstd1, kmedian2, kstd2];

    % Mean values
    vMean = abs(loops(:,9)-mean(loops(:,9)));
    nMean = (find(vMean == min(vMean)));

    if length(nMean(:,1)) > 1
        nMean = nMean(1,1);
    end
    pMean1 = loops(nMean,1);
    pMean2 = loops(nMean,2);
    cMean1 = loops(nMean,3);
    cMean2 = loops(nMean,4);
    kMean1 = loops(nMean,5);
    kMean2 = loops(nMean,6);

    mMedModF = [pMean1, pstd1, pMean2, pstd2, cMean1, cstd1, cMean2, cstd2, kMean1, kstd1, kMean2, kstd2];


function fv= proc_rSquareSigned(fv, varargin)
%PROC_RSQUARESIGNED - computes signed r^2 values (measure for discriminance)
%
%Synopsis:
% 	fv = proc_rSquareSigned(fv, <opt>)
%
%Returns:
% FV_RVAL - data structure of signed squared biserial correlation coefficients
%  .x     - signed squared biserial correlation between each featur and the
%           class label
%  .se    - contains the standard error of atanh(r), if opt.Stats==1
%  .p     - contains the p value of null hypothesis that there is zero
%           correlation between feature and class-label, if opt.Stats==1
%  .sgnlogp - contains the signed log10 p-value, if opt.Stats==1
%             if opt.Bonferroni==1, the p-value is multiplied by
%             fv_rval.corrfac
%  .sgnlogp - contains the signed log10 p-value, if opt.Stats==1
%           if opt.Bonferroni==1, the p-value is multiplied by
%           fv_rval.corrfac and then logarithmized
%  .sigmask - binary array indicating significance at alpha level
%             opt.Alphalevel, if opt.Stats==1 and opt.Alphalevel > 0
%  .corrfac - Bonferroni correction factor (number of simultaneous tests),
%             if opt.Bonferroni==1
%
%Properties:
% 'TolerateNans': observations with NaN value are skipped
%    (nanmean/nanstd are used instead of mean/std). Deafult: 0
% 'ValueForConst': constant feauture dimensions are assigned this
%    value. Default: NaN.
% 'MulticlassPolicy': possible options: 'pairwise' (default),
%    'all-against-last', 'each-against-rest', or provide specified
%    pairs as an [nPairs x 2] sized matrix. ('specified_pairs' is obsolete)
% 'Stats' - if true, additional statistics are calculated, including the
%           standard error of atanh(r), the p-value for the null
%           Hypothesis that the correlation is zero,
%           and the "signed log p-value"
% 'Bonferroni' - if true, Bonferroni corrected is used to adjust p-values
%                and their logarithms
% 'Alphalevel' - if provided, a binary indicator of the significance to the
%                alpha level is returned for each feature in fv_rval.sigmask
%
%Description:
% Computes the r^2 value for each feature, multiplied by the sign of
% of r value. The r^2 value is a measure
% of how much variance of the joint distribution can be explained by
% class membership.
%
% Example:
%  [cnt, mrk]= file_readBV(some_file);   %load EEG-data in BV-format
%  mrk= mrk_defineClasses(mrk, {1, 2; 'target','nontarget'});
%  epo= proc_segmentation(cnt, mrk, [-200 800], 'CLab', {'Fz','Cz','Pz'});
%  epo_r = proc_rSquareSigned(epo);
%
% See also proc_classmeanDiff, proc_rValues, proc_rSquare
%
% 03-03 Benjamin Blankertz
% 09-2012 stefan.haufe@tu-berlin.de

if nargin==0,
  fv=proc_rValues; return
end

fv= proc_rValues(fv, varargin{:});
fv.x= fv.x .* abs(fv.x);
for cc= 1:length(fv.className),
  fv.className{cc}= ['sgn r^2' fv.className{cc}(2:end)];
end
fv.yUnit= 'sgn r^2';


function [rRelmTest] = relm_NTest(vRatesH, vRatesN, nNumberSimulation, fMagThreshold, bOptimized, bDrawFigure)
% function [rRelmTest] = relm_NTest(vRatesH, vRatesN, nNumberSimulation, fMagThreshold, bOptimized, bDrawFigure)
% --------------------------------------------------------------------------------------------------------------
% Computation of the N-test for the RELM framework
%
% Input parameters:
%   vRatesH                       Matrix with rates of the test hypothesis
%   vRatesN                       Matrix with rates of the null hypothesis
%   nNumberSimulation             Number of random simulations
%   fMagThreshold                 Magnitude threshold (Use only bins with magnitude >= threshold
%   bOptimized                    0 (default): use a for loop, 1: matrix-wise calculation (needs a lot of memory)
%   bDrawFigure                   Draw the cumulative density plot after testing (default: off)
%
% Output paramters:
%   rRelmTest.fAlpha              Alpha-value of the cumulative density
%   rRelmTest.fBeta               Beta-value of the cumulative density
%   rRelmTest.vSimValues_H        Vector containing the sorted simulated numbers of events for the test hypothesis
%   rRelmTest.vSimValues_N        Vector containing the sorted simulated numbers of events for the null hypothesis
%   rRelmTest.nNumberSimulation   Number of random simulations
%   rRelmTest.fObservedData       Observed total number of events
%
% Danijel Schorlemmer
% October 4, 2002

% Exit on empty rate matrices
if isempty(vRatesH)  ||  isempty(vRatesN)
  rRelmTest.fAlpha = nan;
  rRelmTest.fBeta = nan;
  rRelmTest.vSimValues_H = nan;
  rRelmTest.vSimValues_N = nan;
  rRelmTest.nNumberSimulation = nan;
  rRelmTest.fObservedData = nan;
  return;
end

if ~exist('bDrawFigure')
  bDrawFigure = 0;
end

if ~exist('bOptimized')
  bOptimized = 0;
end

% Randomize
rand('state',sum(100*clock));

% Get the necessary data from the rate matrices and weight them properly
[vLambdaH, vLambdaN, vNumberQuake] = relm_PrepareData(vRatesH, vRatesN, fMagThreshold);
nNumberQuake = sum(vNumberQuake);

% Get the number of bins (rows)
[nRow, nColumn] = size(vLambdaH);

if bOptimized
  % Create the random numbers for the simulation
  vRandom = rand(nRow, nNumberSimulation);

  % Replicate the rate vectors
  vLambdaH = repmat(vLambdaH, 1, nNumberSimulation);
  vLambdaN = repmat(vLambdaN, 1, nNumberSimulation);

  % Compute the simulated number of events and sum them up
  vNum_H = poissinv(vRandom, vLambdaH);
  vNum_N = poissinv(vRandom, vLambdaN);
  vSimNum_H = sum(vNum_H);
  vSimNum_N = sum(vNum_N);
else
  % Create empty vectors for the total number of events
  vSimNum_H = [];
  vSimNum_N = [];

  % Loop over the simulations
  for nCnt = 1:nNumberSimulation
    % Create the random numbers for the simulation
    vRandom = rand(nRow, 1);

    % Compute the simulated number of events and sum them up
    vNum_H = poissinv(vRandom, vLambdaH);
    vNum_N = poissinv(vRandom, vLambdaN);
    vSimNum_H = [vSimNum_H; nansum(vNum_H)];
    vSimNum_N = [vSimNum_N; nansum(vNum_N)];
  end
end

% Sort them for the cumulative density plot
rRelmTest.vSimValues_H = sort(vSimNum_H);
rRelmTest.vSimValues_N = sort(vSimNum_N);

% Compute Alpha and Beta and store the important parameters
rRelmTest.fAlpha = sum(rRelmTest.vSimValues_N > nNumberQuake)/nNumberSimulation;
rRelmTest.fBeta = sum(rRelmTest.vSimValues_H < nNumberQuake)/nNumberSimulation;
rRelmTest.nNumberSimulation = nNumberSimulation;
rRelmTest.fObservedData = nNumberQuake;

if bDrawFigure
  relm_PaintCumPlot(rRelmTest, 'Number of earthquakes');
end


%[2018]-"Tree growth algorithm (TGA): A novel approach for solving
%optimization problems"

function TGA = jTreeGrowthAlgorithm(feat,label,opts)
% Parameters
lb         = 0;
ub         = 1;
thres      = 0.5;
num_tree1  = 3;    % size of first group
num_tree2  = 5;    % size of second group
num_tree4  = 3;    % size of fourth group
theta      = 0.8;  % tree reduction rate of power
lambda     = 0.5;  % control nearest tree

if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'N1'), num_tree1 = opts.N1; end
if isfield(opts,'N2'), num_tree2 = opts.N2; end
if isfield(opts,'N4'), num_tree4 = opts.N4; end
if isfield(opts,'theta'), theta = opts.theta; end
if isfield(opts,'lambda'), lambda = opts.lambda; end
if isfield(opts,'thres'), thres = opts.thres; end

% Limit number of N4 to N1
if num_tree4 > num_tree1 + num_tree2
  num_tree4 = num_tree1 + num_tree2;
end
% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = zeros(N,dim);
for i = 1:N
	for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
	end
end
% Fitness
fit  = zeros(1,N);
fitG = inf;
for i = 1:N
  fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  % Best
  if fit(i) < fitG
    fitG = fit(i);
    Xgb  = X(i,:);
  end
end
% Sort tree from best to worst
[fit, idx] = sort(fit,'ascend');
X          = X(idx,:);
% Initial
dist = zeros(1,num_tree1 + num_tree2);
X1   = zeros(num_tree1,dim);
Xnew = zeros(num_tree4,dim);
Fnew = zeros(1,num_tree4);

curve = zeros(1,max_Iter);
curve(1) = fitG;
t = 2;
% Iterations
while t <= max_Iter
	% {1} Best trees group
  for i = 1:num_tree1
    r1 = rand();
    for d = 1:dim
      % Local search (1)
      X1(i,d) = (X(i,d) / theta) + r1 * X(i,d);
    end
  	% Boundary
    XB = X1(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    X1(i,:) = XB;
    % Fitness
    fitT = fun(feat,label,(X1(i,:) > thres),opts);
    % Greedy selection
    if fitT <= fit(i)
      X(i,:) = X1(i,:);
      fit(i) = fitT;
    end
  end
  % {2} Competitive for light tree group
  X_ori = X;
  for i = num_tree1 + 1 : num_tree1 + num_tree2
    % Neighbor tree
    for j = 1 : num_tree1 + num_tree2
      if j ~= i
        % Compute Euclidean distance (2)
        dist(j) = sqrt(sum((X_ori(j,:) - X_ori(i,:)) .^ 2));
      else
        % Solve same tree problem
        dist(j) = inf;
      end
    end
    % Find 2 trees with shorter distance
    [~, idx] = sort(dist,'ascend');
    T1       = X_ori(idx(1),:);
    T2       = X_ori(idx(2),:);
    % Alpha in [0,1]
    alpha    = rand();
    for d = 1:dim
      % Compute linear combination between 2 shorter tree (3)
      y = lambda * T1(d) + (1 - lambda) * T2(d);
      % Move tree i between 2 adjacent trees (4)
      X(i,d) = X(i,d) + alpha * y;
    end
    % Boundary
    XB = X(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    X(i,:) = XB;
    % Fitness
    fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  end
  % {3} Remove and replace group
  for i = num_tree1 + num_tree2 + 1 : N
    for d = 1:dim
      % Generate new tree by remove worst tree
      X(i,d) = lb + (ub - lb) * rand();
    end
    % Fitness
    fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  end
  % {4} Reproduction group
  for i = 1:num_tree4
    % Random a best tree
    r     = randi([1,num_tree1]);
    Xbest = X(r,:);
    % Mask operator
    mask  = randi([0,1],1,dim);
    % Mask opration between new & best trees
    for d = 1:dim
      % Generate new solution
      Xn = lb + (ub - lb) * rand();
      if mask(d) == 1
        Xnew(i,d) = Xbest(d);
      elseif mask(d) == 0
        % Generate new tree
        Xnew(i,d) = Xn;
      end
    end
    % Fitness
    Fnew(i) = fun(feat,label,(Xnew(i,:) > thres),opts);
  end
  % Sort population get best nPop trees
  XX        = [X; Xnew];
  FF        = [fit, Fnew];
  [FF, idx] = sort(FF,'ascend');
  X         = XX(idx(1:N),:);
  fit       = FF(1:N);
  % Global best
  if fit(1) < fitG
    fitG = fit(1);
    Xgb  = X(1,:);
  end
  curve(t) = fitG;
  fprintf('\nIteration %d Best (TGA)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
TGA.sf = Sf;
TGA.ff = sFeat;
TGA.nf = length(Sf);
TGA.c  = curve;
TGA.f  = feat;
TGA.l  = label;
end


function www=trajt3(para,chrom)

x0=para(1);
y0=para(2);
z0=para(3);
vx0=0;
vy0=0;
vz0=0;
ax0=0;
ay0=0;
az0=0;
x2=para(4);
y2=para(5);
z2=para(6);
vx2=0;
vy2=0;
vz2=0;
ax2=0;
ay2=0;
az2=0;
x1=chrom(1);
y1=chrom(2);
z1=chrom(3);
vx1=chrom(4);
vy1=chrom(5);
vz1=chrom(6);
t1=chrom(7);
t2=chrom(8);

a00=x0;
a01=vx0;
a02=ax0/2;
a03=(4*x1-vx1*t1-4*x0-3*vx0*t1-ax0*t1^2)/t1^3;
a04=(vx1*t1-3*x1+3*x0+2*vx0*t1+ax0*t1^2/2)/t1^4;
ax1=2*a02+6*a03*t1+12*a04*t1^2;

c00=y0;
c01=vy0;
c02=ay0/2;
c03=(4*y1-vy1*t1-4*y0-3*vy0*t1-ay0*t1^2)/t1^3;
c04=(vy1*t1-3*y1+3*y0+2*vy0*t1+ay0*t1^2/2)/t1^4;
ay1=2*c02+6*c03*t1+12*c04*t1^2;

e00=z0;
e01=vz0;
e02=az0/2;
e03=(4*z1-vz1*t1-4*z0-3*vz0*t1-az0*t1^2)/t1^3;
e04=(vz1*t1-3*z1+3*z0+2*vz0*t1+az0*t1^2/2)/t1^4;
az1=2*e02+6*e03*t1+12*e04*t1^2;
%--------------------------------------------------------------------------
b10=x1;
b11=vx1;
b12=ax1/2;
b13=(20*x2-20*x1-(8*vx2+12*vx1)*t2-(3*ax1-ax2)*t2^2)/(2*t2^3);
b14=(30*x1-30*x2+(14*vx2+16*vx1)*t2+(3*ax1-2*ax2)*t2^2)/(2*t2^4);
b15=(12*x2-12*x1-(6*vx2+6*vx1)*t2-(ax1-ax2)*t2^2)/(2*t2^5);

d10=y1;
d11=vy1;
d12=ay1/2;
d13=(20*y2-20*y1-(8*vy2+12*vy1)*t2-(3*ay1-ay2)*t2^2)/(2*t2^3);
d14=(30*y1-30*y2+(14*vy2+16*vy1)*t2+(3*ay1-2*ay2)*t2^2)/(2*t2^4);
d15=(12*y2-12*y1-(6*vy2+6*vy1)*t2-(ay1-ay2)*t2^2)/(2*t2^5);

f10=z1;
f11=vz1;
f12=az1/2;
f13=(20*z2-20*z1-(8*vz2+12*vz1)*t2-(3*az1-az2)*t2^2)/(2*t2^3);
f14=(30*z1-30*z2+(14*vz2+16*vz1)*t2+(3*az1-2*az2)*t2^2)/(2*t2^4);
f15=(12*z2-12*z1-(6*vz2+6*vz1)*t2-(az1-az2)*t2^2)/(2*t2^5);
%--------------------------------------------------------------------------

t=linspace(0,t1,20);

x01=a00+a01*t+a02*t.^2+a03*t.^3+a04*t.^4;
vx01=a01+2*a02*t+3*a03*t.^2+4*a04*t.^3;
ax01=2*a02+6*a03*t+12*a04*t.^2;

y01=c00+c01*t+c02*t.^2+c03*t.^3+c04*t.^4;
vy01=c01+2*c02*t+3*c03*t.^2+4*c04*t.^3;
ay01=2*c02+6*c03*t+12*c04*t.^2;

z01=e00+e01*t+e02*t.^2+e03*t.^3+e04*t.^4;
vz01=e01+2*e02*t+3*e03*t.^2+4*e04*t.^3;
az01=2*e02+6*e03*t+12*e04*t.^2;
%--------------------------------------------------------------------------
t=linspace(0,t2,20);

x12=b10+b11*t+b12*t.^2+b13*t.^3+b14*t.^4+b15*t.^5;
vx12=b11+2*b12*t+3*b13*t.^2+4*b14*t.^3+5*b15*t.^4;
ax12=2*b12+6*b13*t+12*b14*t.^2+20*b15*t.^3;

y12=d10+d11*t+d12*t.^2+d13*t.^3+d14*t.^4+d15*t.^5;
vy12=d11+2*d12*t+3*d13*t.^2+4*d14*t.^3+5*d15*t.^4;
ay12=2*d12+6*d13*t+12*d14*t.^2+20*d15*t.^3;

z12=f10+f11*t+f12*t.^2+f13*t.^3+f14*t.^4+f15*t.^5;
vz12=f11+2*f12*t+3*f13*t.^2+4*f14*t.^3+5*f15*t.^4;
az12=2*f12+6*f13*t+12*f14*t.^2+20*f15*t.^3;
%--------------------------------------------------------------------------
ppx=[x01 x12];ppy=[y01 y12];ppz=[z01 z12];
q1=ppx;q2=ppy;q3=ppz;

vvx=[vx01 vx12];vvy=[vy01 vy12];vvz=[vz01 vz12];
vq1=vvx;vq2=vvy;vq3=vvz;

aax=[ax01 ax12];aay=[ay01 ay12];aaz=[az01 az12];
aq1=aax;aq2=aay;aq3=aaz;

www=[q1;q2;q3;vq1;vq2;vq3;aq1;aq2;aq3];


% TIMEFDETAILS - details of the TIMEF function for time/frequency analysis
%                  of multiple epochs of single-channel event-related data.
%
% Global Description:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TIMEF performs normalized time/frequency averaging using either
% FFT-, wavelet-, or multitaper DFT estimates. The wavelets are N-cycle
% Hanning-windowed sinusoids. (Note: To substitute for HANNING windowing
% GAUSS or other windowing, replace the timef.m reference to HANNING).
%
% By default, the two image panels of the output plot show, respectively, the
% event-related spectral perturbation (ERSP) and inter-trial coherence (ITC)
% of the input data.
%
% The ERSP:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The ERSP (S. Makeig, Electroencephalogr Clin Neurophysiol 86:283-93, 1993)
% shows mean log event-loced deviations from epoch-mean (or baseline-mean) power
% at each frequency. If bootstrap statistics are computed (via the 'alpha'
% probability input parameter), (time, freq) points with non-significant
% differences from 0 are colored green in the image (but not in the 'ersp'
% output variable - use the output 'erspboot' variable to re-mask the 'ersp'
% output if desired). The baseline mean spectrum removed from each epoch
% is available as output parameter "powbase". Note that log(power) differences
% are equivalent to log(power ratios) between baseline and observed spectra,
% meaning the implicit ERSP model is one of (multiplicative) amplitude modulation
% of the EEG/MEG spectrum by e.g. subcortical and/or intra-cortical influences.
%
% In the default view, the thin bottom panel below the upper (ERSP) image shows
% the ERSP envelope (the most positive and most negative values at each output
% time point). The thin left panel shows the mean (or baseline) log spectrum
% (blue trace). When bootstrap statistics are computed (via the "alpha" argument),
% the left panel (green trace) also shows the bootstrap significant levels (+/-)
% at each frequency.
%
% The ITC:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% (Inter-trial Coherence, cf. Tallon-Baudry et al., "Phase-locking factor")
% The lower panel shows the degree of tendency for spectral phase at each
% (time, freq) point to repeat across the data epochs. If bootstrap statistics
% are computed (as per the 'alpha' input parameter), non-significant points
% are colored green (but again not in the 'itc' output variable - use the
% itcboot output to re-mask if desired in later plotting).
%
% The lower thin panel shows the time-domain average (ERP) of the input data
% (blue) plus a zero-line (green). The average (ERP) is created principally by
% partial phase resetting of the EEG as measured by the ITC. (While phase resetting
% dominates, event-related spectral power changes (as measured by the ERSP)
% may also play a minor role). The thin left panel shows the frequency-mean ITC
% (blue trace) and, if bootstrap statistics are computed, the ITC significance
% limits at each frequency (green trace).
%
% ITC Math Derivation:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% By definition, linear coherence is
%       R=mean(Fxy)/sqrt(mean(abs(Fxx))*mean(abs(Fyy)));
% where Fxy is the cross-spectrum (FxFy*) and Fxx and Fyy the autospectra of
% processes x and y.  We define the phase coherence to be
%       R=mean(Fxy/(abs(Fxx))*abs(Fyy));  % mean of individually normed Fxy
% To derive the ITC, we consider y to be a stimulus-locked process
% such that, at each time and frequency, angle(Fyy) = 0 and abs(Fyy) = 1.
% Thus Pxy = Pxx, and the (complex) inter-trial phase coherence between x and
% the constant stimulus-locked process y is
%       ITC=mean(Pxx/abs(Pxx));
%
% USAGE:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   >> [ersp,itc,powbase,times,freqs,erspboot,itcboot]  ...
%                = timef(data,frames,tlimits,srate,cycles,...
%                              'key1',value1,'key2',value2, ... );
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% NOTE:
% * Left-click on subplots to view and zoom in separate windows (uses AXCOPY).
%
% Required Inputs:
%   data        = Single-channel data vector (1,frames*ntrials) (required)
%
%   frames      = Frames per trial                        {750}
%  Here, a frame is a data point (one channel at one time point) and the data
%  is assumed to be composed of concatenated epochs of the same length (frames).
%
%   tlimits     = [mintime maxtime] (ms) Epoch time limits {[-1000 2000]}
%  These should be the starting and ending times of the input epochs.
%
%   srate       = data sampling rate (Hz)                 {250}
%  This is sample rate per channel (i.e., data frames per second).
%
%   cycles      = >0 -> Number of cycles in each analysis wavelet
%                 =0 -> Use FFTs (with constant window length) {0}
%                 If [wavecycles factor] -> wavelet cycles increase with frequency
%                 beginning at wavecyles (0<factor<1; factor=1 -> no increase,
%                 standard wavelets; factor=0 -> fixed epoch length, as in FFT.
%                 OR multitaper decomposition (with 'mtaper').
%  Here, the user chooses either to use the FFT method (fixed window size
%  for all frequencies) or the wavelet DFT method (the data window length
%  depends inversely on the frequency, e.g. '3' means that the data
%  windows will each be three cycles wide (at each frequency). A higher
%  number here (e.g., '5') will narrow the frequency band and widen the
%  time window.
%
% Optional Inter-Irial Coherence Type:
%   'type'      = ['coher'|'phasecoher'] Compute either linear coherence
%                  ('coher') or phase coherence ('phasecoher') also known
%                  as the phase coupling factor           {'phasecoher'}.
%
% Optional Detrending:
%   'detret'    = ['on'|'off'], Detrend data in time.       {'off'}
%   'detrep'    = ['on'|'off'], Detrend data across trials (at each time point
%                 compute the linear trend across the set of ordered trials and
%                 remove it; not that this also subtract the ERP) {'off'}
%
% Optional FFT/DFT Parameters:
%   'winsize'   = If cycles==0: data subwindow length (fastest is 2^n < frames);
%                 If cycles >0: *longest* window length to use. This determines
%                 the lowest output frequency  {default: ~frames/8}
%  When cycles>0, winsize determines the lowest computed frequency. For example,
%  with srate=100 and cycles=3, a winsize of 100 means that 3 cycles must fit
%  within a 1-sec ( 100-sample) window. So, the lowest output frequency is 3 Hz.
%
%  When cycles=0, winsize is the length of data in each FFT window. This may be
%  extended with zeroes (to give more output frequencies) using padratio (below).
%
%  'timesout'  = Number of output times (int<frames-winframes) {200}
%  The number of FFTs or wavelet DFTs computed and plotted.
%
%   'padratio'  = FFT-length/winframes (2^k)                    {2}
%                  Multiplies the number of output frequencies by
%                  dividing their spacing. When cycles==0, frequency
%                  spacing is (low_freq/padratio).
%  This factor multiplies the number of output frequencies. In the FFT method
%  (cycles=0), this is done by zero-padding each analysis window. In the wavelet
%  DFT method (cycles>0), this gives the number of frequencies per Hz.
%
%   'maxfreq'   = Maximum frequency (Hz) to plot (& to output, if cycles>0)
%                  If cycles==0, all FFT frequencies are output. {50}
%   'baseline'  = Spectral baseline end-time (in ms). Use NaN for no baseline
%                 removal{0}
%   'powbase'   = Baseline spectrum to log-subtract. 'baseline' parameter is
%                 ignored if this parameter is used {def|NaN->from data}
%  This is useful only when you want to use a known baseline spectrum (e.g. from
%  another condition) instead of using the actual mean baseline spectrum of the data.
%  Otherwise, leave this out or specify as 'NaN' (not a number).
%
% Optional Multitaper Parameters:
%   'mtaper'    = If [N W], performs multitaper decomposition.
%                  (N is the time resolution and W the frequency resolution;
%                  maximum taper number is 2NW-1). Overwrites 'winsize' and
%                  'padratio'.
%                  If [N W K], forces the use of K Slepian tapers (if possible).
%                  Phase is calculated using standard methods.
%                  The use of mutitaper with wavelets (cycles>0) is not
%                  recommended (as multiwavelets are not implemented).
%                  Uses Matlab functions DPSS, PMTM.   {no multitaper}
%
% Optional Bootstrap Parameters:
%   'alpha'     = If non-0, compute two-tailed bootstrap significance prob.
%                  level. Show non-signif. output values as green.   {0}
%  This optional parameter lengthens the computation time but gives bootstrap
%  estimates of which ERSP and ITC values are significantly different from 0,
%  by setting to 0 (green) all non-significant values in the ERSP and ITC images.
%  Normal values for alpha are 0 ([], or none) -> no bootstrap computation, or
%  0.01 (which should allow about 1% of random images to appear "significant"
%
%   'naccu'     = Number of bootstrap replications to accumulate     {200}
%   'baseboot'  = Bootstrap baseline subtract (0 -> use 'baseline';
%                                                  1 -> use whole trial) {0}
% Optional Scalp Map Plotting Parameters:
%   'topovec'   = Scalp topography (map) to plot                     {none}
%   'elocs'     = Electrode location file for scalp map   {no default}
%                     File should be ascii in format of  >> topoplot example
%  This is an optional map-plotting feature. Given an input map vector
%  (one weight at each channel, and a electrode location file, TIMEF plots
%  a TOPOPLOT-style 2-d scalp map on the left side of the figure. See
%  >> topoplot example % for the format of the electrode location file.
%
% Other Optional Plotting Parameters:
%   'vert'      = [vector of ms times] -> plot vertical dashed lines  {0 only}
%  Use this to add extra vertical dashed lines at significant epoch times.
%  Time 0 is marked by default.
%
%   'plotersp'  = ['on'|'off'] Plot power spectral perturbations    {'on'}
%   'plotitc'   = ['on'|'off'] Plot inter trial coherence            {'on'}
%   'title'     = Optional figure title                              {none}
%
%   'pboot'     = Bootstrap power limits (e.g., from TIMEF)   {from data}
%   'rboot'     = Bootstrap ITC limits (e.g., from TIMEF)     {from data}
%  These are useful if you want to apply significance limits from another condition
%  to new data. {default|NaN, compute from data}
%
%   'linewidth' = Line width for 'marktimes' traces (thick=2, thin=1) {2}
%   'axesfont'  = Axes text font size                                {10}
%   'titlefont' = Title text font size                               {8}
%
% Outputs:
%        ersp   = Matrix (nfreqs,timesout) of log spectral diffs. from baseline (dB)
%        itc    = Matrix (nfreqs,timesout) of inter-trial phase coherence (range: [0 1])
%   Note that when cycles=0, nfreqs is total number of FFT frequencies, which
%   typically include frequencies higher than maxfreq. When cycles>0, *no* extra
%   (higher) frequencies are computed.
%
%      powbase  = Baseline power spectrum (removed to compute the ERSP)
%        times  = Vector of output times (subwindow centers) (in ms).
%        freqs  = Vector of frequency bin centers (in Hz).
%
%     erspboot  = Matrix (2,nfreqs) of [lower;upper] ERSP significance diffs.
%      itcboot  = Matrix (2,nfreqs) of [lower;upper] ITC thresholds (abs., not diffs)
%  Note that the itcboot lower bound is practically meaningless.
%
%  Plot description:
%    Assuming both 'plotersp' and 'plotitc' options are 'on' (= default). The upper panel
%    presents the data ERSP (Event-Related Spectral Perturbation) in dB, with mean baseline
%    spectral activity (in dB) subtracted. Use "'baseline', NaN" to prevent TIMEF from
%    removing the baseline. The lower panel presents the data ITC (Inter-Trial Coherence).
%    Click on any plot axes to pop up a new window (using 'AXCOPY')
%    -- Upper left marginal panel presents the mean spectrum during the baseline period
%       (blue), and when significance is set, the significance threshold at each frequency
%       (dotted green-black trace).
%    -- The marginal panel under the ERSP image shows the maximum (green) and minimum
%       (blue) ERSP values relative to baseline power at each frequency.
%    -- The lower left marginal panel shows mean ITC across the imaged time range (blue),
%       and when significance is set, the significance threshold (dotted green-black).
%    -- The marginal panel under the ITC image shows the ERP (which is produced by ITC
%       across the data spectral pass band).
%
% Author: Sigurd Enghoff, Arnaud Delorme & Scott Makeig
%          CNL / Salk Institute 1998- | SCCN/INC, UCSD 2002-
%
% See also: CROSSF - event-related cross-spectral coherence between two input
%                      time series.
%
% History:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TIMEF was coded by Sigurd Enghoff and Scott Makeig at The Salk
% Institute, La Jolla CA in August, 1998, using methods developed
% in Makeig, 1993. Arno Delorme added the multitaper option, recoded
% the function to use 'keyword','parameter' argument pairs, and added the
% 'type' argument with advice from Joern Anemueller at SCCN/Institute for
% Neural Computation, UCSD in early 2002.

% Copyright (C) 8/01/00 Scott Makeig, SCCN/INC/UCSD, scott@sccn.ucsd.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

% 01-25-02 reformated help & license -ad


function [gradient, delta] = gradchek(w, func, grad, varargin)
%GRADCHEK Checks a user-defined gradient function using finite differences.
%
%	Description
%	This function is intended as a utility for other netlab functions
%	(particularly optimisation functions) to use.  It enables the user to
%	check whether a gradient calculation has been correctly implmented
%	for a given function. GRADCHEK(W, FUNC, GRAD) checks how accurate the
%	gradient  GRAD of a function FUNC is at a parameter vector X.   A
%	central difference formula with step size 1.0e-6 is used, and the
%	results for both gradient function and finite difference
%	approximation are printed. The optional return value GRADIENT is the
%	gradient calculated using the function GRAD and the return value
%	DELTA is the difference between the functional and finite difference
%	methods of calculating the graident.
%
%	GRADCHEK(X, FUNC, GRAD, P1, P2, ...) allows additional arguments to
%	be passed to FUNC and GRAD.
%
%	See also
%	CONJGRAD, GRADDESC, HMC, OLGD, QUASINEW, SCG
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Reasonable value for step size
epsilon = 1.0e-6;

func = fcnchk(func, length(varargin));
grad = fcnchk(grad, length(varargin));

% Treat
nparams = length(w);
deltaf = zeros(1, nparams);
step = zeros(1, nparams);
for i = 1:nparams
  % Move a small way in the ith coordinate of w
  step(i) = 1.0;
  fplus  = feval('linef', epsilon, func, w, step, varargin{:});
  fminus = feval('linef', -epsilon, func, w, step, varargin{:});
  % Use central difference formula for approximation
  deltaf(i) = 0.5*(fplus - fminus)/epsilon;
  step(i) = 0.0;
end
gradient = feval(grad, w, varargin{:});
fprintf(1, 'Checking gradient ...\n\n');
delta = gradient - deltaf;
fprintf(1, '   analytic   diffs     delta\n\n');
disp([gradient', deltaf', delta'])


classdef LossSmoothL1 < dagnn.Loss
%LossSmoothL1  Smooth L1 loss
%  `LossSmoothL1.forward({x, x0, w})` computes the smooth L1 distance
%  between `x` and `x0`, weighting the elements by `w`.
%
%  Here the smooth L1 loss between two vectors is defined as:
%
%     Loss = sum_i f(x_i - x0_i) w_i.
%
%  where f is the function (following the Faster R-CNN definition):
%
%              { 0.5 * sigma^2 * delta^2,         if |delta| < 1 / sigma^2,
%   f(delta) = {
%              { |delta| - 0.5 / sigma^2,         otherwise.
%
%  In practice, `x` and `x0` can pack multiple instances as 1 x 1 x C
%  x N arrays (or simply C x N arrays).

  properties
    sigma = 1.
  end

  methods
    function outputs = forward(obj, inputs, params)
      sigma2 = obj.sigma^2 ;
      delta = inputs{1} - inputs{2} ;
      absDelta = abs(delta) ;

      linearRegion = (absDelta > 1. / sigma2) ;
      absDelta(linearRegion) = absDelta(linearRegion) - 0.5/sigma2 ;
      absDelta(~linearRegion) = 0.5 * sigma2 * absDelta(~linearRegion).^2 ;

      % Mutliply by instance weights and sum.
      outputs{1} = inputs{3}(:)' * absDelta(:) ;

      % Accumulate loss statistics.
      n = obj.numAveraged ;
      m = n + gather(sum(inputs{3}(:))) + 1e-9 ;
      obj.average = (n * obj.average + gather(outputs{1})) / m ;
      obj.numAveraged = m ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
    % Function derivative:
    %
    %          { sigma^2 * x,             if |x| < 1 / sigma^2,
    %  f'(x) = {
    %          { sign(x),                 otherwise.

      sigma2 = obj.sigma^2 ;
      delta = inputs{1} - inputs{2} ;
      absDelta = abs(delta) ;

      linearRegion = (absDelta > 1. / sigma2) ;
      delta(linearRegion) = sign(delta(linearRegion));
      delta(~linearRegion) = sigma2 * delta(~linearRegion) ;

      derInputs = {inputs{3} .* delta .* derOutputs{1}, [], []} ;
      derParams = {} ;
    end

    function obj = LossSmoothL1(varargin)
      obj.load(varargin) ;
      obj.loss = 'smoothl1';
    end
  end
end


% Digital circuit sizing for an inverter chain (GP)
% Boyd, Kim, Patil, and Horowitz, "Digital circuit optimization
% via geometric programming"
% Written for CVX by Almir Mutapcic 02/08/06
%
% We consider a chain of N inverters driving a load capacitance CL.
% The problem is to find optimal scale factors for the inverter
% that minimize the sum of them (area), while obeying constraints
% on the maximum delay through the circuit, and minimum and maximum
% limits on scale factors. There are no limits on the total power.
% (For more details about the inverter chain see sec. 2.1.11 in the paper.)
%
%   minimize   sum(x)
%       s.t.   T_j <= Dmax          for j an output gate
%              T_j + d_i <= T_i     for j in FI(i)
%              x_min <= x <= x_max
%
% where variables are x and T.
% Here we use data structures and digital circuit models from the
% referenced paper.

%********************************************************************
% problem data
%********************************************************************
N  = 8;      % number of inverters
CL = 20;     % capacitance load
Dmax = 20;   % maximum delay through the circuit
x_min = 1;   % minimum scale factor
x_max = 20;  % maximum scale factor

% circuit labeling convention:
% label primary input (input to the first inverter in the chain) with N+1
% label primary output (output of the last inverter in the chain) with N+2
% label inverters in the chain with 1,2,...,N based on their location

% primary input and primary output labels (start with N+1)
primary_inputs  = [N+1];
primary_outputs = [N+2];
M = N + length( primary_inputs ) + length( primary_outputs );

% fan-in cell array for a straight chain of inverters
FI{1} = [N+1];   % fan-in of the first inverter is the primary input
for k = 2:N
  FI{k} = [k-1]; % fan-in of other inverters is the inverter feeding into them
end
FI{N+2} = [N];   % fan-in of the primary output is the last inverter in the chain

% fan-out cell array
% (will be computed from the fan-in cell array, no need to modify)
FO = cell(M,1);
for gate = [1:N primary_outputs]
  preds = FI{gate};
  for k = 1:length(preds)
    FO{preds(k)}(end+1) = gate;
  end
end

% input and internal capacitance of gates and the driving resistance
Cin_norm  = ones(N,1);
Cint_norm = ones(N,1);
Rdrv_norm = ones(N,1);

% place extra capacitance before the input of the 5th inverter
Cin_norm(5) = 80;

% primary output has Cin capacitance (but has no Cload)
Cin_po = sparse(M,1);
Cin_po(primary_outputs) = CL;

% primary input has Cload capacitance (but has no Cin)
Cload_pi = sparse(M,1);
Cload_pi(primary_inputs) = 1;

%********************************************************************
% optimization
%********************************************************************
cvx_begin gp
  % optimization variables
  variable x(N)                 % sizes
  variable T(N)                 % arrival times

  % minimize the sum of scale factors subject to above constraints
  minimize( sum(x) )
  subject to

    % input capacitance is an affine function of sizes
    Cin  = Cin_norm.*x;
    Cint = Cint_norm.*x;

    % driving resistance is inversily proportional to sizes
    R = Rdrv_norm./x;

    % gate delay is the product of its driving resistance and load cap.
    Cload = cvx( zeros(N,1) );
    for gate = 1:N
      if ~ismember( FO{gate}, primary_outputs )
        Cload(gate) = sum( Cin(FO{gate}) );
      else
        Cload(gate) = Cin_po( FO{gate} );
      end
    end

    % delay
    D = 0.69*ones(N,1).*R.*( Cint + Cload );

    % create timing constraints
    for gate = 1:N
      if ~ismember( FI{gate}, primary_inputs )
        for j = FI{gate}
          % enforce T_j + D_j <= T_i over all gates j that drive i
          D(gate) + T(j) <= T(gate);
        end
      else
        % enforce D_i <= T_i for gates i connected to primary inputs
        D(gate) <= T(gate);
      end
    end

    % circuit delay is the max of arrival times for output gates
    output_gates = [FI{primary_outputs}];
    circuit_delay = max( T(output_gates) );

    % collect all the constraints
    circuit_delay <= Dmax;
    x_min <= x <= x_max;
cvx_end

% message about extra capacitance and result display
disp(' ')
disp(['Note: there is an extra capacitance between the 4th and 5th inverter'...
     ' in the chain.'])
fprintf(1,'\nOptimal scale factors are: \n'), x

% plot scale factors and maximum delay for inverter i
close all;
subplot(2,1,1); plot([1:N],T,'g--',[1:N],T,'bo');
ylabel('maximum delay T')
subplot(2,1,2); stem([1:N],x);
ylabel('scale factor x')
xlabel('inverter stage')


classdef VehicleSimpleLinear < VehicleDynamicsLateral.VehicleSimple
    % VehicleSimpleLinear Linear simple vehicle model.
    %
    % It inherits properties from VehicleSimple.

    methods
        function self = VehicleSimpleLinear()
            % Constructor for the vehicle
            self.mF0 = 700;
            self.mR0 = 600;
            self.IT = 10000;
            self.lT = 3.5;
            self.nF = 2;
            self.nR = 2;
            self.wT = 2;
            self.muy = .8;
            self.deltaf = 0;
            self.Fxf = 0;
            self.Fxr = 0;
        end

        %% Model
        % Function with the model
        function dx = Model(self, t, states,tspan)
            % Data
            mT = self.mT;
            IT = self.IT;
            a = self.a;
            b = self.b;
            nF = self.nF;
            nR = self.nR;
            muy = self.muy;

            g = 9.81;                 % Gravity [m/s^2]

            FzF = self.mF0 * g;       % Vertical load @ F [N]
            FzR = self.mR0 * g;       % Vertical load @ R [N]

            v0 = 20;                  % [m/s]

            % State variables
            X = states(1,1);         % Not used
            Y = states(2,1);         % Not used
            PSI     = states(3,1);
            VT       = states(4,1);
            ALPHAT  = states(5,1);
            dPSI    = states(6,1);

            if isa(self.deltaf,'function_handle')
                deltaf = self.deltaf([X;Y;PSI;VT;ALPHAT;dPSI],t);
            elseif length(self.deltaf)>1
                deltaf = interp1(tspan,self.deltaf,t);
            else
                deltaf = self.deltaf;
            end

            % Slip angles
            ALPHAF = ALPHAT + a/v0*dPSI - deltaf;
            ALPHAR = ALPHAT - b/v0*dPSI;

            % Longitudinal forces
            if isa(self.Fxf,'function_handle')
                FxF = self.Fxf([X;Y;PSI;VT;ALPHAT;dPSI],t);
            elseif length(self.Fxf)>1
                FxF = interp1(tspan,self.Fxf,t);
            else
                FxF = self.Fxf;
            end

            if isa(self.Fxr,'function_handle')
                FxR = self.Fxr([X;Y;PSI;VT;ALPHAT;dPSI],t);
            elseif length(self.Fxr)>1
                FxR = interp1(tspan,self.Fxr,t);
            else
                FxR = self.Fxr;
            end

            % Lateral force
            FyF = nF * self.tire.Characteristic(ALPHAF, FzF / nF, muy);
            FyR = nR * self.tire.Characteristic(ALPHAR, FzR / nR, muy);

            % State equations
            dx(1,1) = VT;
            dx(2,1) = v0*(PSI + ALPHAT);
            dx(3,1) = dPSI;
            dx(4,1) = (FxF + FxR)/mT;
            dx(5,1) = (FyF + FyR)/(mT*v0) - dPSI;
            dx(6,1) = (a*FyF - b*FyR)/IT;

        end
    end
end

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


function hess = cstepHess(fun,x,isTril,h)
%CSTEPHESS  Complex Step Hessian of a Scalar Function
%
%  cstepHess uses a complex step approach to finite difference to ensure
%  robust estimate of the Hessian. Note your function MUST be
%  written in Matlab code only and must not contain any if/else statements.
%
%  If it does not work - ensure you are using .' (transpose) and not ' (complex conjugate).
%
%   hess = cstepHess(fun,x) calculates the Hessian of fun at the point x.
%
%   hess = cstepHess(fun,x,isTril) specifies if the returned Hessian
%   should be Symmetric Lower Triangular.
%
%   hess = cstepHess(fun,x,isTril,h) specifies the step-size. This defaults to
%   1e-3.

%   Copyright (C) 2013 Jonathan Currie (IPL)
%
%   This code follows ideas from "New Complex-Step Derivative Approximations
%   with Application to Second-Order Kalman Filtering", by Kok-Lam Lai, John
%   L. Crassidis and Yang Cheng.

if(nargin < 4), h = 1e-3; end
if(nargin < 3 || isempty(isTril)), isTril = false; end

%Constants from paper
I = sqrt(2)/2*(1i + 1);
n = length(x); hess = zeros(n,n);
%Diagonal Elements
for k = 1:n
    xu = x; xu(k) = xu(k) + I*h;
    xl = x; xl(k) = xl(k) - I*h;
    hess(k,k) = imag((fun(xu) + fun(xl))/h^2);
end
%Lower Triangular Elements
lam = 1; k = n-1;
while(k > 0)
    for phi = 1:k
        xu = x; xu(phi:phi+lam) = xu(phi:phi+lam) + I*h;
        xl = x; xl(phi:phi+lam) = xl(phi:phi+lam) - I*h;
        Fsum = sum(sum(hess(phi:phi+lam,phi:phi+lam)));
        hess(phi+lam,phi) = (imag((fun(xu) + fun(xl))/h^2) - Fsum)/2;
        hess(phi,phi+lam) = hess(phi+lam,phi); %copy to upper tri
    end
    k = k - 1;
    lam = lam + 1;
end
%Make tril if requested
if(isTril), hess = tril(hess); end


function [X_den,P]=denoise_TV_One(Xobs,lambda,l,u,P_init,pars)
%This function implements the FISTA method for TV denoising problems.
%
% INPUT
% Xobs ..............................an observed noisy image.
% lambda ........................ parameter
% pars.................................parameters structure
% pars.MAXITER ..................... maximum number of iterations
%                                                      (Default=100)
% pars.epsilon ..................... tolerance for relative error used in
%                                                       the stopping criteria (Default=1e-4)
% pars.print ..........................  1 if a report on the iterations is
%                                                       given, 0 if the  report is silenced
% pars.tv .................................. type of total variation
%                                                      penatly.  'iso' for isotropic (default)
%                                                      and 'l1' for nonisotropic
%
% OUTPUT
% X_den ........................... The solution of the problem
%                                            min{||X-Xobs||^2+2*lambda*TV(X)}

%%% Written by Chen Chen and Junzhou Huang at UT Arlington
%%% April. 13, 2012

%%% Related paper: Chen Chen and Junzhou Huang, "Compressive Sensing MRI
%%% with Wavelet Tree Sparsity"

%Define the Projection onto the box
if((l==-Inf)&&(u==Inf))
    project=@(x)x;
elseif (isfinite(l)&&(u==Inf))
    project=@(x)(((l<x).*x)+(l*(x<=l)));
elseif (isfinite(u)&&(l==-Inf))
     project=@(x)(((x<u).*x)+((x>=u)*u));
elseif ((isfinite(u)&&isfinite(l))&&(l<u))
    project=@(x)(((l<x)&(x<u)).*x)+((x>=u)*u)+(l*(x<=l));
else
    error('lower and upper bound l,u should satisfy l<u');
end

% Assigning parameres according to pars and/or default values
flag=exist('pars', 'var');
if (flag&&isfield(pars,'MAXITER'))
    MAXITER=pars.MAXITER;
else
    MAXITER=100;
end
% if (flag&&isfield(pars,'epsilon'))
%     epsilon=pars.epsilon;
% else
%     epsilon=1e-4;
% end
% if(flag&&isfield(pars,'print'))
%     prnt=pars.print;
% else
%     prnt=1;
% end
if(flag&&isfield(pars,'tv'))
    tv=pars.tv;
else
    tv='iso';
end

[m,n]=size(Xobs);
% clear P; clear R;
if(isempty(P_init))
    P{1}=zeros(m-1,n);    P{2}=zeros(m,n-1);
    R{1}=zeros(m-1,n);    R{2}=zeros(m,n-1);
else
    P{1}=P_init{1};    P{2}=P_init{2};
    R{1}=P_init{1};    R{2}=P_init{2};
end
tk=1;tkp1=1;count=0;i=0;

D=zeros(m,n);%fval=inf;fun_all=[];
while((i<MAXITER)&&(count<5))
%    fold=fval;
    i=i+1;
%     Dold=D;
    Pold=P;
    tk=tkp1;
    D=project(Xobs-lambda*Lforward(R, m, n));
    Q=Ltrans(D, m, n);
    %%%%%%%%%%
    % Taking a step towards minus of the gradient
    P{1}=R{1}+1/(8*lambda)*Q{1};
    P{2}=R{2}+1/(8*lambda)*Q{2};

    %%%%%%%%%%
    % Peforming the projection step
    switch tv
        case 'iso'
            A=[P{1}.^2;zeros(1,n)]+[P{2}.^2,zeros(m,1)];
            A=sqrt(max(A,1));
            P{1}=P{1}./A(1:m-1,:); P{2}=P{2}./A(:,1:n-1);
        case 'l1'
            P{1}=P{1}./(max(abs(P{1}),1));
            P{2}=P{2}./(max(abs(P{2}),1));
        otherwise
            error('unknown type of total variation. should be iso or l1');
    end

    %%%%%%%%%%
    %Updating R and t
    tkp1=(1+sqrt(1+4*tk^2))/2;

    R{1}=P{1}+(tk-1)/(tkp1)*(P{1}-Pold{1});
    R{2}=P{2}+(tk-1)/tkp1*(P{2}-Pold{2});

%     re=norm(D-Dold,'fro')/norm(D,'fro');
%     if (re<epsilon)
%         count=count+1;
%     else
%         count=0;
%     end
    C=Xobs-lambda*Lforward(P, m, n);
    PC=project(C);
%     fval=-norm(C-PC,'fro')^2+norm(C,'fro')^2;
%     fun_all=[fun_all;fval];
end
X_den=D;iter=i;

function X=Lforward(P, m, n)

%       [m2,n2]=size(P{1});
%       [m1,n1]=size(P{2});
%
%       if (n2~=n1+1)
%           error('dimensions are not consistent')
%       end
%       if(m1~=m2+1)
%           error('dimensions are not consistent')
%       end
%
%       m=m2+1;
%       n=n2;

      X=zeros(m,n);
      X(1:m-1,:)=P{1};
      X(:,1:n-1)=X(:,1:n-1)+P{2};
      X(2:m,:)=X(2:m,:)-P{1};
      X(:,2:n)=X(:,2:n)-P{2};
   end

   function P=Ltrans(X, m, n)
%       [m,n]=size(X);
      P{1}=X(1:m-1,:)-X(2:m,:);
      P{2}=X(:,1:n-1)-X(:,2:n);
   end
end


function [V]=quad_smooth(V,Ev,IND_V_not_Ev,Lc,n,Ls,w1)

L1=Ev>0;
L2=IND_V_not_Ev>0;
w2=1-w1;

p=V;

for i=1:n
    Xp=NaN(size(Ev));  Yp=NaN(size(Ev));  Zp=NaN(size(Ev));
    Xp(L1)=p(Ev(L1),1); Yp(L1)=p(Ev(L1),2); Zp(L1)=p(Ev(L1),3);
    Vp1=[gnanmean(Xp,2) gnanmean(Yp,2) gnanmean(Zp,2)];

    Xp=NaN(size(Ev));  Yp=NaN(size(Ev));  Zp=NaN(size(Ev));
    Xp(L2)=p(IND_V_not_Ev(L2),1); Yp(L2)=p(IND_V_not_Ev(L2),2); Zp(L2)=p(IND_V_not_Ev(L2),3);
    Vp2=[gnanmean(Xp,2) gnanmean(Yp,2) gnanmean(Zp,2)];

    Vp=(w1.*Vp1+w2.*Vp2)./(w1+w2);
    p=p+Ls.*(Vp-p);

    p(Lc,:)=V(Lc,:);
    if i>1
        D=gnansum((p-p_old).^2,2);
        SSQD_new=gnansum(D(:));
        if i>2
            SSQD_ratio=SSQD_new./SSQD_old;
            disp(num2str(SSQD_ratio));
        end
        SSQD_old=SSQD_new;
    end
    p_old=p;

end
V=p;

%%
% _*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 [mu, s2, MU, SIG2] = simulGPmc(hyp, inf, meanfunc, cov, lik, input, target, test, lag, Nsamples)
% simulGPmc - Simulation of the dynamic GP model, where the output variance is
% propagated using the Monte Carlo method
%
%% Syntax
%  [mu, s2, MU, SIG2] = simulGPmc(hyp, inf, mean, cov, lik, input, target, test,
%  lag, Nsamples)
%
%% Description
% Idea: at every time step the output of GP model is approximated with
% Nsamples samples, which are used as the future inputs of the GP model.
% Samples are re-used if necessary (ie. y(k-1) for y(k-2) if lag=2 etc.)
% Uses routines gpx and gmx_sample.
%
% Input:
% * hyp      ... the structure of optimized hyperparameters
% * inf      ... the function specifying the inference method
% * meanfunc ... the prior mean function
% * cov      ... the specified covariance function, see help covFun for more info
% * lik      ... the likelihood function
% * input    ... the input part of the training data,  NxD matrix
% * target   ... the output part of the training data (ie. target), Nx1 vector
% * test     ... the input matrix for simulation, kxD vector, see
%                construct.m for more info
% * lag      ... the order of the model (number of used lagged outputs)
% * Nsamples ... the number of samples used in algorithm (ie. runs of simulation)
%
% Output:
% * mu    ... the mean predicted output
% * s2    ... the associated variances (with noise variances)
% * MU    ... the matrix of all predicted means, kxNsamples
% * SIG2  ... the associated predicted variances
%
% See also:
% gpx, gmx_sample, simulGPnaive
%
% Examples:
% demo_example_gp_simulation
%
%%
% * Written by J. Prikryl, November 2010
% * Based on the work of C.E. Rasmussen, A. Girard, K. Azman.
%

% meanfunc ... 'mean' is used as a matlab core function in this file

Ndx = 800;
DSig = 3;
num_iters = length(test);
sum_time  = 0;

[N, D] = size(input);
PDF = zeros(Nsamples,lag);

% Preallocate mu and s2
mu = zeros ( num_iters, 1 );
s2 = zeros ( num_iters, 1 );

% 1st step - input is a point
test_ = test(1,:);
[mu(1), s2(1), post] = gpx(hyp, inf, meanfunc, cov, lik, input, target, test_);

MU(1,:) = mu(1)*ones(1,Nsamples);
SIG2(1,:) = s2(1)*ones(1,Nsamples);

pdf = gmx_sample(mu(1),s2(1),Nsamples);
PDF(:,lag) = pdf;
% instead of a cycle, create one monstrous test_ matrix
test_ = repmat( test(2,:), Nsamples, 1 );
test_(:,lag) = pdf;
[MU(2,:), SIG2(2,:), post] = gpx(hyp, inf, meanfunc, cov, lik, input, target, test_, post);
mu(2,1) = mean(MU(2,:));
s2(2,1) = mean(SIG2(2,:)) + mean((MU(2,:)-mu(2)).^2);

% steps from 3 on ...
for k=3:num_iters

    if(mod(k,50)==0 || k==3)
        disp(['simulGPmc, step: ',int2str(k),'/',int2str(length(test))]);
    end

    if(k>lag)
        % samples of previous distributions
        for jj=1:lag-1
            PDF(:,jj) = PDF(:,jj+1);
        end

    else  % k <= lag
        % part after 'else' not yet tested for lag>=3
        col0 = lag-(k-1);
        PDF(:,1:col0) = repmat(test(k,1:col0),Nsamples,1);
        for jj=col0+1:lag-1
            PDF(:,jj) = PDF(:,jj+1);
        end
    end

    pdf = gmx_sample(MU(k-1,:),SIG2(k-1,:),Nsamples);
    PDF(:,lag) = pdf;

    % simulate for all cases, again using the matrix version
    t0 = tic;
    test_ = repmat ( test(k,:), Nsamples, 1 );
    test_(:,1:lag) = PDF;
    [MU(k,:), SIG2(k,:), post] = gpx(hyp, inf, meanfunc, cov, lik, input, target, test_, post);
    calltime = toc(t0);
%     fprintf ( 'gpr_simul() ....... %f sec\n\n', calltime ); % uncomment
%     if you wish
    sum_time = sum_time + calltime;

    % approximate output distribution with gauss - calculate m and v
    mu(k,1) = mean(MU(k,:));
    s2(k,1) = mean(SIG2(k,:)) + mean((MU(k,:)-mu(k)).^2);

end

%     fprintf ( 'average time needed for gpr_simul() .... %f sec\n', sum_time/(num_iters-2) );
%     uncomment if you wish
return;


function gamma_inc_values_test ( )

%*****************************************************************************80
%
%% GAMMA_INC_VALUES_TEST demonstrates the use of GAMMA_INC_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAMMA_INC_VALUES_TEST:\n' );
  fprintf ( 1, '  GAMMA_INC_VALUES stores values of\n' );
  fprintf ( 1, '  the incomplete Gamma function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      A            X            GAMMA_INC(A)(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, x, fx ] = gamma_inc_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %12f  %24.16f\n', a, x, fx );

  end

  return
end


function [vert, fac] = voxel_image( pts, vox_sz, color, alpha, edgec )
%VOXEL_IMAGE Creates a 3D voxel image
%   Parameters:
%   pts    - n x 3 matrix with 3D points
%   vox_sz - 1 x 3 vector with voxel size - if vox_sz is a scalar, all
%            edges will have the same length
%   color  - face color
%   alpha  - face alpha (opacity)
%   edgec  - edge color
%
%   Return values:
%   vert   - 8n x 3 matrix containing the vertices of the voxels
%   fac    - 6n x 4 matrixes containing the indexes of vert that form a
%            face

% Example:
%
% pts = [1,1,1; 2,2,2];
% vs = [0.5, 0.5, 0.5];
% voxel_image(pts, vs);
% view([-37.5, 30]);
%
% This example creates two voxels (at (1,1,1) and (2,2,2)) with edges of
% length 0.5
%
% Author: Stefan Schalk, 11 Feb 2011

if (nargin < 1)
    error('No input arguments given');
end
if (nargin < 2)
    vox_sz = [1,1,1];
end
if (nargin < 3)
    color = 'b';
end
if (nargin < 4)
    alpha = 1;
end
if (nargin < 5)
    edgec = 'k';
end

if (size(pts,2) ~= 3)
    error('pts should be an n x 3 matrix');
end
if (isscalar(vox_sz))
    vox_sz = vox_sz*ones(1,3);
end
if (size(vox_sz,1) ~= 1 || size(vox_sz,2) ~= 3)
    error('vox_sz should be an 1 x 3 vector');
end

np = size(pts,1);
vert = zeros(8*np,3);
fac = zeros(6*np,4,'uint32');
vert_bas = [...
        -0.5,-0.5,-0.5;
        0.5,-0.5,-0.5;
        0.5,0.5,-0.5;
        -0.5,0.5,-0.5;
        -0.5,-0.5,0.5;
        0.5,-0.5,0.5;
        0.5,0.5,0.5;
        -0.5,0.5,0.5];
vert_bas = vert_bas.*([vox_sz(1).*ones(8,1), vox_sz(2).*ones(8,1), vox_sz(3).*ones(8,1)]);
fac_bas = [...
        1,2,3,4;
        1,2,6,5;
        2,3,7,6;
        3,4,8,7;
        4,1,5,8;
        5,6,7,8];
for vx = 1:np
    a = ((vx-1)*8+1):vx*8;
    for dim = 1:3
        vert( a,dim ) = vert_bas(:,dim) + pts(vx,dim);
    end
    fac ( ((vx-1)*6+1):vx*6,: ) = (vx - 1)*8*ones(6,4) + fac_bas;
end
patch('Vertices',vert,'Faces',fac,'FaceColor',color,'FaceAlpha',alpha,'Edgecolor',edgec);

end


function D = sparseDiag(d)

% SPARSEDIAG Create a diagonal matrix that is sparse from a vector.
% FORMAT
% DESC creates a diagonal matrix that is sparse from a vector.
% ARG d : the diagonal vector from which the sparse diagonal matrix
% is formed.
% RETURN D : the sparse diagonal matrix containing the vector as
% its diagonal.
%
% SEEALSO : diag, spdiags
%
% COPYRIGHT : Neil D. Lawrence, 2005

% NDLUTIL

if length(size(d)) ~=2
  error('Input must be a vector.');
end
if size(d, 1) ~= 1 & size(d, 2) ~=1
  error('Input must be a vector.');
end

D = spdiags(d, 0, length(d), length(d));
% % Can be made more efficient.
% n = length(d);
% D = spalloc(n, n, n);
% for i = 1:n
%   D(i, i) = d(i);
% end


function [near,in,on,B,L] = near_mesh(V,F,Q,epsilon)
  % NEAR_MESH test whether a list of points are in or near a given mesh
  %
  % [near] = near_mesh(V,F,Q)
  % [near,in,on,B,L] = near_mesh(V,F,Q)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by 3 list of face indices
  %   Q  #Q by dim list of query points
  %   epsilon  minium distance allowed after collapses are complete, default is to
  %     use fraction of maximum edge length
  % Outputs:
  %   near #Q list of flags revealing whether queries are near (V,F)
  %   in #Q list of flags revealing whether queries are in (V,F)
  %   on #Q list of flags revealing whether queries are on boundary of (V,F)
  %   B  #B by 1 list of mesh outline edges
  %   L  #loops+1 by 1 list of boundary loop start indices into B, the last
  %     entries is (by tradition) always the numel of B + 1
  %
  % See in_mesh, inpolygon
  %

  if ~exist('epsilon','var') || isempty(epsilon)
    EE = edges(F);
    % maximum edge length
    %maxD = max(sqrt(sum((V(EE(:,1),:) - V(EE(:,2),:)).^2,2)));
    minD = min(sqrt(sum((V(EE(:,1),:) - V(EE(:,2),:)).^2,2)));
    epsilon = minD/2;
  end

  dim = size(V,2);
  % only works in 2D
  assert(dim == 2);

  % first determine points strictly in or on mesh
  [in,on,B,L] = in_mesh(V,F,Q);

  % avoid sqrts
  sqr_eps = epsilon.^2;

  % boundary edges
  BE = [B; B(2:end) B(1)]';
  % compute projection of each point to each boundary line segment
  [T,sqrD] = project_to_lines(Q,V(BE(:,1),:),V(BE(:,2),:));
  % each vertex seen by each edge
  QBE = repmat(Q,[1 1 size(BE,1)]);
  % edge start positions
  S = V(BE(:,1),:);
  % edge destination positions
  D = V(BE(:,2),:);
  % distance of each point to each edge start
  sqrDS = ...
    squeeze(sum((QBE - permute(repmat(S,[1 1 size(Q,1)]),[3 2 1])).^2,2));
  % distance of each point to each edge dest
  sqrDD = ...
    squeeze(sum((QBE - permute(repmat(D,[1 1 size(Q,1)]),[3 2 1])).^2,2));
  % replace distances to edges when point is closest to start or dest endpoints
  % respectively
  sqrD(T<0) = sqrDS(T<0);
  sqrD(T>1) = sqrDD(T>1);
  % compute minimum distance to boundary
  [minD] = min(sqrD,[],2);
  % mask telling whether closest point for each edge is close enough
  near = minD<sqr_eps;
  % all strictly in points are also close
  near = in | near;
end


function K = covPPERard(cov, hyp, x, z, i)

% Stationary partially periodic covariance function for a stationary
% covariance function k0 such as covMaternard, covPPard, covRQard and covSEard.
% Partially periodic means that the covariance function is periodic only
% along some dimensions (as opposed to all dimensions).
% Stationary means that the covariance function k0(x,z) depends on the
% data points x,z only through the squared distance
% dxz = (x-z)'*inv(P)*(x-z) where the P matrix is diagonal with ARD parameters
% ell_1^2,...,ell_D^2, where D is the dimension of the input space.
% The covariance function is parameterized as:
%
% k(x,z) = k0(u(x),u(z)),
% [u(x)]([i,i+D]) = [sin(pi*x/p); cos(pi*x/p)]      if p_i is finite
% [u(x)]([i,i+D]) = [x; 0]                          if p_i is not finite
%
% where the period p belongs to covPPERard and hyp0 belong to k0:
%
% hyp = [ log(p_1)
%         log(p_2)
%          .
%         log(p_D)
%         hyp0 ]
%
% The first D hyperparameters of k0 are the log lengthscales such that
% hyp0(i) = log(ell(i)) for i=1..D.
% Note that for k0 = covSEard and D = 1, a faster alternative is covPeriodic.
%
% Copyright (c) by Luigi Acerbi, 2015-12-20.
% Original code by Hannes Nickisch.
%
% See also COVFUNCTIONS, COVPERARD.

nocov = false;                   % default case when no cov argument is provided
if nargin==0, cov = {@covSEard}; nocov = true; end                % default case
if isnumeric(cov)       % detect old version where the cov parameter was missing
  % i <- z, z <- x, x <- hyp, hyp <- cov
  if nargin>3, i = z; end
  if nargin>2, z = x; end
  if nargin>1, x = hyp; end
  hyp = cov; cov = {@covSEard}; nocov = true;
end

if nocov && nargin<2 || ~nocov && nargin<3         % report number of parameters
  K = ['(D+',feval(cov{:}),')']; return
end
if nocov && nargin<3 || ~nocov && nargin<4, z = []; end    % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

[n,D] = size(x);
p = exp(hyp(1:D));
lell = hyp(D+(1:D));
hyp0 =  [lell; lell; hyp(2*D+1:end)];

if nocov && nargin<4 || ~nocov && nargin<5
  [x,z] = u(x,z,p,dg);                      % apply the embedding u:IR^D->IR^2*D
  K = feval(cov{:},hyp0,x,z);
else
  if i<=D
    if isfinite(p(i))                             % periodic dimension
        if dg                                                   % compute distance d
          di = zeros([n,1]);
        else
          if xeqz                                             % symmetric matrix Kxx
            di = repmat(reshape(x(:,i),n, 1),[1, n])...
                -repmat(reshape(x(:,i),1, n),[n, 1]);
          else                                               % cross covariances Kxz
            nz = size(z,1);
            di = repmat(reshape(x(:,i),n, 1),[1,nz])...
                -repmat(reshape(z(:,i),1,nz),[n, 1]);
          end
        end
        di = 2*pi*di/p(i); dD2_dlpi = -2*sin(di).*di;   % derivative dD2i/dlog(p(i))
        [x,z] = u(x,z,p,dg);                    % apply the embedding u:IR^D->IR^2*D
        if dg                                            % compute squared distances
          D2 = zeros(n,1);
        else
          if xeqz
            D2 = sq_dist(x(:,[i,i+D])');
          else
            D2 = sq_dist(x(:,[i,i+D])',z(:,[i,i+D])');
          end
        end
        % reconstruct derivative w.r.t. D2i from derivative w.r.t. log(ell(i))
        dK_dD2 = feval(cov{:},hyp0,x,z,i) + feval(cov{:},hyp0,x,z,i+D);
        dK_dD2 = dK_dD2./(-2*D2); dK_dD2(D2<1e-12) = 0;
        K = dK_dD2.*dD2_dlpi;                                     % apply chain rule
    else                                            % non-periodic dimension
      K = zeros(n);
    end
  else
    [x,z] = u(x,z,p,dg);                    % apply the embedding u:IR^D->IR^2*D
    if i<=2*D
        if isfinite(p(i-D))
          K = feval(cov{:},hyp0,x,z,i-D) + feval(cov{:},hyp0,x,z,i);
        else
          K = feval(cov{:},hyp0,x,z,i-D);
        end
    else
      K = feval(cov{:},hyp0,x,z,i);
    end
  end
end

function [x,z] = u(x,z,p,dg)                % apply the embedding u:IR^D->IR^2*D
  [~,D] = size(x);
  pd = find(isfinite(p));     % periodic dimensions
  x(:,pd) = 2*pi*x(:,pd)*diag(1./p(pd));
  x = [x, zeros(size(x))];
  x(:,pd+D) = cos(x(:,pd)); x(:,pd) = sin(x(:,pd));
  if numel(z)>0 && ~dg
    z(:,pd) = 2*pi*z(:,pd)*diag(1./p(pd));
    z = [z, zeros(size(z))];
    z(:,pd+D) = cos(z(:,pd)); z(:,pd) = sin(z(:,pd));
  end


% Observation models and vision library.
%
% Sensor management
%   composeRobSen                   - Compose robot and sensor frames and parameters.
%
% Pin-hole model functions
%   distort                         - Distort projected point with radial distortion.
%   undistort                       - Undistorts projected point with radial distortion.
%   pixellise                       - Metric to pixellic conversion
%   depixellise                     - Pixellic to metric conversion
%   intrinsic                       - Build intrinsic matrix
%   invIntrinsic                    - Build inverse intrinsic matrix
%   essential                       - Essential matrix from frame specification.
%   fundamental                     - Fundamental matrix between 2 cameras.
%   pinHole                         - Pin-hole camera model, with optional radial distortion.
%   invPinHole                      - Inverse pin-hole camera model, with radial distortion correction.
%   pinHoleIdp                      - Pin-hole camera model for Inverse depth points, with radial distortion.
%   invPinHoleIdp                   - Inverse pin-hole camera model for IDP, with radial distortion correction.
%   pinHolePlucker                  - Projects plucker line.
%   invPinHolePlucker               - Retro-projects plucker line
%   aInvPinHolePlucker              - Inverse pin hole model for Plucker lines
%   pinHoleSegment                  - Pin hole projection of a segment.
%   pluckerInvCamera                - Inverse Plucker projection matrix
%   isVisible                       - Points visible from pinHole camera.
%   invDistortion                   - Radial distortion correction calibration.
%   invPinHoleHmg                   - Retro-project anchored homogeneous point AHP.
%   visibleSegment                  - Visible segment.
%   invPinHoleAPlucker              - Retro-projects anchored plucker line
%   invPinHoleIdpLin                - IDP line retro projection.
%   invPinHoleAhm                   - Retro-project anchored homogeneous point AHP.
%   persp_project                   - Project point into plane using pin-hole camera model
%   persp_retro                     - Retroproject pixel into 3D space.
%   pinHoleHmg                      - Pin-hole camera model for HMG points, with optional radial distortion.
%   projAhmPntIntoPinHole           - Project AHM pnt into pinhole camera.
%   projIdpPntIntoPinHole           - Project Idp pnt into pinhole.
%   invPinHoleDepth                 -
%   pinHoleDepth                    - Pin hole projection with distance measurement
%
% Functions related to Omnicam camera model
%   depixelliseOmniCam              - TODO: can inverse A up front - will save calculation esp in Jac part
%   invOmniCam                      - Inverse omnidirectional camera model
%   invOmniCamAhm                   - Retro-project anchored homogeneous point AHP.
%   invPinHoleAhmLin                - AHM line retro projection.
%   invPinHoleHmgLin                - IDP line retro projection.
%   omniCam                         - Gives projected pixel u of 3D point p for Omnidirectional Camera model
%   omni_project                    - Gives projected pixel u of 3D point p for Omnidirectional Camera model
%   omni_retro                      - Retroproject pixel into 3D space.
%   pixelliseOmniCam                - Omnicam Affine correction step
%
% Observation models, with 2 frame transforms
%   projEucPntIntoPinHoleOnRob      - Project Euc pnt into pinhole on robot.
%   projIdpPntIntoPinHoleOnRob      - Project Idp pnt into pinhole on robot.
%   projHmgPntIntoPinHoleOnRob      - Project Hmg pnt into pinhole on robot.
%   projSegLinIntoPinHoleOnRob      - Project segment line into pinhole on robot.
%   projPlkLinIntoPinHoleOnRob      - Project Plucker line into pinhole on robot.
%   projAplLinIntoPinHoleOnRob      - Project anchored Plucker line into pinhole on robot.
%   projIdpLinIntoPinHoleOnRob      - Project Idp line into pinhole on robot.
%   retroProjIdpPntFromPinHoleOnRob - Retro-project idp from pinhole on robot.
%   retroProjHmgPntFromPinHoleOnRob - Retro-proj. Hmg pnt from pinhole on rob.
%   retroProjPlkLinFromPinHoleOnRob - Retro-project Plucker line from pinhole on robot.
%   retroProjPlkEndPnts             - Retro project Plucker endpoints.
%   retroProjAplLinFromPinHoleOnRob - Retro-project anchored Plucker line from pinhole on robot.
%   retroProjAplEndPnts             - Retro project anchored Plucker endpoints.
%   retroProjIdpLinFromPinHoleOnRob - retroprj Idp Line from pinhole on robot.
%   projAhmPntIntoPinHoleOnRob      - Project Ahm pnt into pinhole on robot.
%   retroProjAhmPntFromPinHoleOnRob - Retro-project ahm from pinhole on robot.
%   projAhmLinIntoPinHoleOnRob      - Project Ahm line into pinhole on robot.
%   projHmgLinIntoPinHoleOnRob      - Project Hmg line into pinhole on robot.
%   retroProjAhmLinFromPinHoleOnRob - retroprj Ahm Line from pinhole on robot.
%   retroProjHmgLinFromPinHoleOnRob - retroprj Hmg Line from pinhole on robot.
%   projAhmPntIntoOmniCamOnRob      - Project Ahm pnt into omnidirectional camera on robot.
%   projEucPntIntoOmniCamOnRob      - Project Euc pnt into omnidirec cam on robot.
%   retroProjAhmPntFromOmniCamOnRob - Retro-project ahm from omnicam on robot.
%   projEucPntIntoPhdOnRob          - Project Eucliden point into Pinhole-depth in Robot
%   retroProjEucPntFromPhdOnRob     - Retro-proj Euc. pnt. from Pinhole-depth in Rob


function x = elastix_transf_imcoord2(x, t)
% elastix_transf_imcoord  Convert 2D pixel coordinates using
% elastix/transformix transform.
%
% Let x be the (x, y)-coordinates of a pixel in an image (instead of
% row/column). This function computes the coordinates that the same pixel
% will have in the new image after transformix or elastix transforms.
%
% XT = elastix_transf_imcoord2(X, T)
%
%   X is a two-column matrix with the Cartesian coordinates of one or more
%   points in the input image.
%
%   T is a struct produced by elastix with the details of the image
%   transformation. See help elastix for details.
%
%   XT has the same size as X, and contains the coordinates of the same
%   points in the output image.
%
% See also: elastix, transformix, elastix_read_file2param,
% elastix_read_reg_output, elastix_write_param2file,
% elastix_compose_afftransf.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2014 University of Oxford
% Version: 0.1.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(2, 2);
nargoutchk(0, 1);

if (size(x, 2) ~= 2)
    error('X must have two columns (2D points)')
end
if (~isstruct(t))
    error('T must be a struct with the transform parameters')
end
if (isfield(t, 'Direction') && any(t.Direction ~= [1 0 0 1]))
    error('Not implemented for Direction ~= [1 0 0 1]')
end

% defaults
if (~isfield(t, 'CenterOfRotationPoint'))
    t.CenterOfRotationPoint = [0 0];
end

% select type of transform
switch (t.Transform)

    case 'SimilarityTransform' % similarity transform

        if (length(t.TransformParameters) ~= 4)
            error('SimilarityTransform must have 4 parameters')
        end

        % transformation nomenclature
        s =     t.TransformParameters(1);
        theta = t.TransformParameters(2);
        tx =    t.TransformParameters(3);
        ty =    t.TransformParameters(4);
        cx =    t.CenterOfRotationPoint(1);
        cy =    t.CenterOfRotationPoint(2);

        % transform points
        %
        % note that this is the inverse of the similarity transform. The
        % reason is the way that ITK, and by extension elastix/transformix,
        % implement image transformations
        x = [cos(theta) sin(theta); -sin(theta) cos(theta)] ...
            * [x(:, 1)' - tx - cx; x(:, 2)' - ty - cy] / s;
        x = x';
        x(:, 1) = x(:, 1) + cx;
        x(:, 2) = x(:, 2) + cy;

    otherwise

        error('Transform not implemented')

end


function exact = p03_exact ( )

%*****************************************************************************80
%
%% P03_EXACT returns the exact integral for problem 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the value of the integral.
%
  exact = ( 16.0 / 3.0 ) * ( 2.0 - sqrt ( 2.0 ) );

  return
end


function [wl,wr] = cvwindow(x)

  wr = zeros(size(x));
  wl = zeros(size(x));

  eps = 1e-16;
  sml = find(x<eps); %too small
  mid = find(x>=eps & x<=1-eps); %just right
  lrg = find(x>1-eps); %too large

  wl(sml) = 0;  wr(sml) = 1;
  wl(lrg) = 1;  wr(lrg) = 0;

  xmid = x(mid);
  a = exp(1-1./(1-exp(1-1./(1-xmid))));
  b = exp(1-1./(1-exp(1-1./xmid)));
  n = sqrt(a.^2 + b.^2);
  wl(mid) = a./n;  wr(mid) = b./n;