function [Jul1,Jul2]=UTC2TDB(Jul1,Jul2,deltaT,clockLoc)
%%UTC2TDB Convert from universal coordinated time (UTC) to barycentric
%         dynamical time (TDB) to an accuracy of nanoseconds (if deltaT
%         is accurate) using the routines from the International
%         Astronomical Union's library that do not require external
%         ephemeris data.
%
%INPUTS: Jul1,Jul2 Two parts of a pseudo-Julian date given in UTC. The
%                  units of the date are days. The full date is the sum of
%                  both terms. The date is broken into two parts to provide
%                  more bits of precision. It does not matter how the date
%                  is split.
%           deltaT An optional parameter specifying the offset between UTC
%                  and UT1 in seconds. If this parameter is omitted or an
%                  empty matrix is passed, then the value of the function
%                  deltaTTUT1 with an appropriate offset will be used.
%         clockLoc An optional 3X1 vector specifying the location of the
%                  clock in WGS-84 ECEF Cartesian [x;y;z] coordinates with
%                  units of meters. Due to relativistic effects, clocks
%                  that are synchronized with respect to UTC are not
%                  synchronized with respect to TDB. If this parameter is
%                  omitted, then a clock at the center of the Earth is
%                  used and the precision declines to microseconds.
%
%OUTPUTS:Jul1,Jul2 Two parts of a Julian date given in TDB.
%
%This function converts the date to a terrestrial time and then calls
%TT2TDB.
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3)
    deltaT=[];
end

if(nargin<4)
    clockLoc=[];
end

[Jul1,Jul2]=UTC2TT(Jul1,Jul2);
[Jul1,Jul2]=TT2TDB(Jul1,Jul2,deltaT,clockLoc);

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [fBValue, fAValue, fStdDev, fMc, fMeanMag] = plot_FMD(mCatalog, varargin)
        % Creates and plots a frequency magnitude distribution including the b-value
    %  [fBValue, fAValue, fStdDev, fMc, fMeanMag] = PLOT_FMD(mCatalog,showCumulative, hAxes, sSymbol, sColor, bPlotB, nCalculateMC, binWidth)
    % -------------------------------------------------------------------------------------------

    %
    % PLOT_FMD(mCatalog) opens a figure and plots the frequency magnitude distribution
    %   with standard parameters
    %
    % Input parameters:
    %   mCatalog        Earthquake catalog
    %
    % Named Input Parameters:  use as PLOT_FMD(... , Name, value)
    %   ShowCumulative  Plot cumulative frequency magnitude distribution (true) or non-cumulative (false)
    %   Axes           Handle of axes to plot the frequency magnitude distribution
    %   Symbol         Type of symbol for plotting (refer to Matlab 'plot')
    %   Color          Color of plot (refer to Matlab 'plot')
    %   ShowBval          Also plot the b-value line with markers
    %   CalcMethod    Method to determine the magnitude of completeness
    %                   1: Maximum curvature
    %                   2: Fixed Mc = minimum magnitude (Mmin)
    %                   3: Mc90 (90% probability)
    %                   4: Mc95 (95% probability)
    %                   5: Best combination (Mc95 - Mc90 - maximum curvature)
    %   BinWidth        Magnitude binning of the catalog (default 0.1)
    %
    % Output parameters:
    %   fBValue         Calculated b-value
    %   fAValue         Calculateda-value
    %   fStdDev         Standard deviation of b-value
    %   fMc             Magnitude of completeness
    %   fMeanMag        Determined mean magnitude
    %
    % Danijel Schorlemmer
    % June 16, 2003

    report_this_filefun();

    p = inputParser();
    p.addRequired('mCatalog');
    p.addParameter('ShowCumulative', true);
    p.addParameter('Axes',          []);
    P.addParameter('Symbol',        's');
    p.addParameter('Color',         'k');
    p.addParameter('ShowBval',      true)
    p.addParameter('CalcMethod',    2);
    p.addParameter('BinWidth',      0.1);
    p.addParameter('MarkerSize',    12);
    p.KeepUnmatched = true;
    p.parse(varargin{:});

    if isempty(p.Results.Axes)
        figure;
        hAxes=newplot;
    else
        hAxes = p.Results.Axes;
    end

    binWidth = p.Results.BinWidth;
    sColor = p.Results.Color;
    % Activate given axes
    axes(hAxes);

    % Create the frequency magnitude distribution vector
    [nEvsCum, nEvsNonCum, magX] = calc_FMD(mCatalog.Magnitude);

    % Plot the frequency magnitude distribution
    if p.Results.showCumulative
        hPlot = semilogy(magX, nEvsCum);
    else
        hPlot = semilogy(magX, nEvsNonCum);
    end

    hPlot.Symbol = p.Results.Symbol;
    hPlot.Color = p.Results.Color;
    hPlot.MarkerSize = p.Results.MarkerSize;

    if p.Results.ShowBval
        % Add further plots to the axes
        hAxes.NextPlot = 'add';

        % Calculate magnitude of completeness
        fMc = calc_Mc(mCatalog, p.Results.CalcMethod, binWidth);

        % Determine the positions of 'x'-markers
        nIndexLo = find((magX < fMc + 0.05) & (magX > fMc - 0.05));

        % Plot the 'x'-marker
        hPlot = semilogy(magX(nIndexLo), nEvsCum(nIndexLo));
        hPlot.Marker = 'x';
        hPlot.Color = sColor;
        hPlot.LineWidth = 2.5;
        hPlot.MarkerSize = 12;

        hPlot = semilogy(magX(1), nEvsCum(1));
        hPlot.Marker = 'x';
        hPlot.Color = sColor;
        hPlot.LineWidth = 2.5;
        hPlot.MarkerSize = 12;

        % Calculate the b-value etc. for M > Mc
        vSel = mCatalog.Magnitude >= fMc-(binWidth/2);
        fMeanMag = mean(mCatalog.Magnitude(vSel));
        [ fBValue, fStdDev, fAValue] =  calc_bmemag(mCatalog.Magnitude(vSel), binWidth);

        % Plot the line representing the b-value
        vPoly = [-1*fBValue fAValue];

        fMagHi = magX(1);
        vMagnitudes = magX(fMc - 0.0001 <= magX & magX <= fMagHi);

        fBFunc = 10.^(polyval(vPoly, vMagnitudes));
        hPlot = semilogy(vMagnitudes, fBFunc, sColor);
        hPlot.LineWidth = 2.0;
    end
end


function sphere_llt_grid_display ( r, pc, lat_num, long_num, node_num, node_xyz, ...
    line_num, line_data, filename )

%*****************************************************************************80
%
%% SPHERE_LLT_GRID_DISPLAY displays points and lines of an LLT grid on a sphere.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the sphere.
%
%    Input, real PC(1,3), the center of the sphere.
%
%    Input, integer LAT_NUM, LONG_NUM, the number of latitude and longitude
%    lines to draw.  The latitudes do not include the North and South
%    poles, which will be included automatically, so LAT_NUM = 5, for instance,
%    will result in points along 7 lines of latitude.
%
%    Input, integer NODE_NUM, the number of grid points.
%
%    Input, real NODE_XYZ(NODE_NUM,3), the grid points.
%
%    Input, integer LINE_NUM, the number of grid lines.
%
%    Input, integer LINE_DATA(LINE_NUM,2), contains pairs of point indices for
%    line segments that make up the grid.
%
%    Input, string FILENAME, the name of a file in which to store a copy of the plot.
%

%
%  Get the scale.
%
  xyz_min(1) = min ( node_xyz(:,1) );
  xyz_max(1) = max ( node_xyz(:,1) );

  xyz_min(2) = min ( node_xyz(:,2) );
  xyz_max(2) = max ( node_xyz(:,2) );

  xyz_min(3) = min ( node_xyz(:,3) );
  xyz_max(3) = max ( node_xyz(:,3) );

  xyz_range(1:3) = xyz_max(1:3) - xyz_min(1:3);

  margin = 0.025 * max ( xyz_range(1), ...
                   max ( xyz_range(2), xyz_range(3) ) );

  x_min = xyz_min(1) - margin;
  x_max = xyz_max(1) + margin;
  y_min = xyz_min(2) - margin;
  y_max = xyz_max(2) + margin;
  z_min = xyz_min(3) - margin;
  z_max = xyz_max(3) + margin;
%
%  Draw the picture.
%
  figure ( )
  clf
  hold on
  point_size = 50;
  point_color = [ 0.0, 1.0, 0.0 ];
  scatter3 ( node_xyz(:,1), node_xyz(:,2), node_xyz(:,3), point_size, ...
    'k', 'filled' );

  for i = 1 : line_num
    p = [ line_data(i,1), line_data(i,2) ];
    line ( node_xyz(p,1), node_xyz(p,2), node_xyz(p,3), 'LineWidth', 2 );
  end
%
%  We want to include a sphere in the picture.  But if the sphere is close to
%  the correct radius, lines that should lie on the surface because they are
%  curved will actually tunnel through the surface because we draw them straight.
%
  [ x, y, z ] = sphere ( 40 );
  x = 0.95 * r * x;
  y = 0.95 * r * y;
  z = 0.95 * r * z;

  c = z ./ z;
  surf ( x, y, z, c, 'EdgeColor', 'None' );

  axis equal
  grid on
  xlabel ( '--X axis--' )
  ylabel ( '--Y axis--' )
  zlabel ( '--Z axis--' )
  title ( filename, 'FontSize', 24 )
  view ( 3 )
  hold off

  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saving plot as "%s".\n', filename );

  return
end


%--------------------------------------------------------------------------------------------------------
% The system is created based on the principles described in the following paper
% Jimmy SJ. Ren and Li Xu, "On Vectorization of Deep Convolutional Neural Networks for Vision Tasks",
% The 29th AAAI Conference on Artificial Intelligence (AAAI-15). Austin, Texas, USA, January 25-30, 2015
% email: jimmy.sj.ren@gmail.com
%--------------------------------------------------------------------------------------------------------
function mnist_configure()
    global config;
    % general configurations for both training and testing
    config.input_size = [32 32];
    config.chs = 1;
    config.forward_pass_scheme = {'conv_v', 'pool', 'conv_v', 'pool', 'conv_v', 'full', 'full', 'full', 'out'};
    config.nonlinearity = 'relu';   % 'relu', 'tanh', 'sigmoid'
    config.output_activation = 'softmax';   % 'softmax', 'inherit'
    config.cost_function = 'cross entropy'; % 'cross entropy', 'L2 norm'
    config.kernel_size = [5 5; 5 5; 5 5];
    config.conv_hidden_size = [6 20 150];
    config.full_hidden_size = [150 150];
    config.output_size = [1 1 10];
    config.batch_size = 100;
    config.compute_device = 'GPU';

    % the following items are only for training
    config.learning_rate = 0.01;
    config.weight_range = 2;
    config.decay = 5e-7 / 10;
    config.normalize_init_weights = 1;
    config.dropout_full_layer = 1;
    config.optimization = 'adagrad';
end


function [ responses ] = CCNF_ncc_response( patches, patch_experts, normalisation_options, window_size, patch_length)
%PATCHRESPONSESVM Computing a patch response from a CCNF patch expert
%   Using convolution, for testing purposes and not for actual speed

    SigmaInv = patch_experts.SigmaInv;

    patchSize = normalisation_options.patchSize;

    if(~iscell(patches))
        patches = {patches};
    end

    num_modalities = numel(patches);

    responses = zeros(size(patches{1},1), patch_length);

    % prepare the patches by normalising them to zscore (if used)
    if(normalisation_options.zscore)
        for i=1:num_modalities
            patches{i} = zscore(patches{i});
        end
    end

    for i = 1:size(patches{1},1)

        norm_cross_corr = normalisation_options.useNormalisedCrossCorr == 1;

        b = zeros(patch_length,1);

        hl_per_modality = size(patch_experts.thetas,1);

        for p=1:num_modalities
            smallRegionVec = patches{p}(i,:);
            smallRegion = reshape(smallRegionVec, window_size(1), window_size(2));

            for hls = 1:hl_per_modality

                % because the normalised cross correlation calculates the
                % responses from a normalised template and a normalised image,
                % normalise the thetas here and then apply the normalisation to
                % the response

                w = patch_experts.thetas(hls, 2:end, p);
                norm_w = norm(w);
                w = w/norm(w);
                w = reshape(w, patchSize);

                response = -norm_w * Cross_corr_resp(smallRegion, w, norm_cross_corr, patchSize) - patch_experts.thetas(hls,1,p);

                % here we include the bias term as well, as it wasn't added
                % during the response calculation
                h1 = 1./(1 + exp(response(:)));
                b = b + (2 * patch_experts.alphas((p-1)*hl_per_modality + hls) * h1);

            end
        end
        response = SigmaInv \ b;

        responses(i,:) = response(:);

    end
    responses = responses';
    responses = responses(:);
end

function response = Cross_corr_resp(region, patchExpert, normalise_x_corr,patchSize)

    if(normalise_x_corr)
        [response] = normxcorr2(patchExpert, region);
        response = response(patchSize(1):end-patchSize(1)+1,patchSize(2):end-patchSize(2)+1);
    else
        % this assumes that the patch is already normed, so just use
        % cross-correlation
        template = rot90(patchExpert,2);
        response = conv2(region, template, 'valid');

    end
end


function [varargout]=hex8_hex20(varargin)

% function [E_hex20,V_hex20,V_hex20_cell,Fb_hex20,S]=hex8_hex20(E_hex8,V_hex8,V_hex8_cell,Fb_hex8)
% ------------------------------------------------------------------------
% This function converts 8 node (e.g. linear) hexahedral elements into 20
% node (e.g. quadratic) hexahedral elements compatible with FEBio.
%
%
% varargout{1}=E_hex20;
% varargout{2}=V_hex20;
% varargout{3}=V_hex20_cell;
% varargout{4}=Fb_hex20;
% varargout{5}=S;
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2018/10/19 Created based on tet4_tet10
%------------------------------------------------------------------------
%%

%% Parse input
switch nargin
    case 2
        E_hex8=varargin{1};
        V_hex8=varargin{2};
        V_hex8_cell={};
        Fb_hex8={};
    case 3
        E_hex8=varargin{1};
        V_hex8=varargin{2};
        V_hex8_cell=varargin{3};
        Fb_hex8={};
    case 4
        E_hex8=varargin{1};
        V_hex8=varargin{2};
        V_hex8_cell=varargin{3};
        Fb_hex8=varargin{4};
    otherwise
        error('wrong number of input arguments');
end

%% Create element set

%Creating "egdge indices" matrices
matVirtSize=size(V_hex8,1)*ones(1,2);

%Edges matrix
indMatrix_1=[E_hex8(:,1) E_hex8(:,2) E_hex8(:,3) E_hex8(:,4)  E_hex8(:,5) E_hex8(:,6) E_hex8(:,7) E_hex8(:,8)  E_hex8(:,5) E_hex8(:,6) E_hex8(:,7) E_hex8(:,8)];
indMatrix_2=[E_hex8(:,2) E_hex8(:,3) E_hex8(:,4) E_hex8(:,1)  E_hex8(:,6) E_hex8(:,7) E_hex8(:,8) E_hex8(:,5)  E_hex8(:,1) E_hex8(:,2) E_hex8(:,3) E_hex8(:,4)];

%3D edges matrix, first layer, first points, second layer, second points
edgeMatrix=indMatrix_1;
edgeMatrix(:,:,2)=indMatrix_2;
edgeMatrix=sort(edgeMatrix,3);

%Convert to virtual indices
edgeIndexMatrix=sub2ind(matVirtSize,edgeMatrix(:,:,1),edgeMatrix(:,:,2));

%Compose unique set
[edgeIndexUni,ind1,ind2]=unique(edgeIndexMatrix(:));

indhex20_new=reshape(ind2,size(edgeIndexMatrix));

%Create unique point index matrices
[indMatrix_1_uni,indMatrix_2_uni]=ind2sub(matVirtSize,edgeIndexUni);

E_hex20=[E_hex8 indhex20_new+size(V_hex8,1)];

%% Create and add new coordinates
% Calculate coordinates for unique new points
V_new=0.5*(V_hex8(indMatrix_1_uni,:)+V_hex8(indMatrix_2_uni,:));%
V_hex20=[V_hex8;V_new];

%% Process cell data

if ~isempty(V_hex8_cell)
    V_hex20_cell=V_hex8_cell;
    for qc=1:1:numel(V_hex20_cell)
        XX_hex8=V_hex8_cell{qc};
        XX_new=0.5*(XX_hex8(indMatrix_1_uni,:)+XX_hex8(indMatrix_2_uni,:));
        XX_hex20=[XX_hex8;XX_new];
        V_hex20_cell{qc}=XX_hex20;
    end
else
    V_hex20_cell={};
end

%% Compose output
varargout{1}=E_hex20;
varargout{2}=V_hex20;
varargout{3}=V_hex20_cell;

if nargout>3

    % Process boundary faces
    if ~isempty(Fb_hex8)

        S = sparse(indMatrix_1_uni,indMatrix_2_uni,ind2(ind1),matVirtSize(1),matVirtSize(2),numel(ind1));

        %Edges matrix
        indMatrix_1=[Fb_hex8(:,1) Fb_hex8(:,2) Fb_hex8(:,3) Fb_hex8(:,4)];
        indMatrix_2=[Fb_hex8(:,2) Fb_hex8(:,3) Fb_hex8(:,4) Fb_hex8(:,1)];

        %3D edges matrix, first layer, first points, second layer, second points
        edgeMatrix=indMatrix_1;
        edgeMatrix(:,:,2)=indMatrix_2;
        edgeMatrix=sort(edgeMatrix,3);

        edgeIndexMatrix=sub2ind(matVirtSize,edgeMatrix(:,:,1),edgeMatrix(:,:,2));

        indhex20_new=full(S(edgeIndexMatrix));

        Fb_hex20_quad8=[Fb_hex8 indhex20_new+size(V_hex8,1)];
        Fb_hex20=Fb_hex20_quad8(:,[1 5 2 6 3 7 4 8]);
    else
        Fb_hex20_quad8=[];
        Fb_hex20=[];
    end
    varargout{4}=Fb_hex20;
    varargout{5}=Fb_hex20_quad8;
end

%%
% _*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 [x,y,l]=simulatej(a,b,c,d,g,h,lambda,x0,T)
%Simulation of dynamical system
x(1)=x0;
for t=2:T
if rand>lambda
l(t)=0;
x(t)=random('normal',mx(a,x(t-1)),b);
else
l(t)=1;
x(t)=random('normal',mx([a,g],x(t-1)),sqrt((1+h))*b);
end
y(t)=random('normal',my(c,x(t)),d);
end


function cS = staurolite

%
cs_St = crystalSymmetry('2/m',[0.469 1 0.34], [90, 90, 90]*degree);
N = Miller({1,0,0},{0,1,0},{0,0,1},{1,1,0},{2,0,1},{2,0,-1},cs_St);
dist = [2.43, 1.3, 6.5, 2.45, 9.85, 9.85];
cS = crystalShape(N./dist);


% test_cgal_check_self_intersect.m

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2013 University of Oxford
% Version: 0.1.0
%
% 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/>.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% nice mesh without self intersections

xyz = [
    -1     1    0
     0     1    0
    -0.5   0.5  0
    -1     0    0
     0     0    0
     1     0    0
     0    -1    0
    ];

x = xyz(:, 1);
y = xyz(:, 2);
z = xyz(:, 3);

tri = [
    1 2 3
    1 4 3
    3 2 5
    3 4 5
    2 5 6
    4 5 7
    5 7 6
    ];

% interior vertices that are nearest neighbors
I = 3;
J = 5;

% plot mesh, with each face in a different colour
hold off
gplot3d(dmatrix_mesh(tri), xyz)
hold on
plot3(x, y, z, 'o')
axis equal xy
view(2)

% check triangle intersections
cross = cgal_check_self_intersect(tri, xyz);
cross'
% expected: all zeros

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% move vertex I down so that triangle 1 overlaps with triangles 4 and 6

y(I) = -1/4.5;
xyz = [x y z];

% plot mesh, with each face in a different colour
hold off
gplot3d(dmatrix_mesh(tri), xyz)
hold on
plot3(x, y, z, 'o')
axis equal xy
view(2)

% check triangle intersections
cross = cgal_check_self_intersect(tri, xyz);
cross'
% expected: 2     2     2     4     0     4     0

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% spherical mesh

% uniformly distribute particles across the surface of the unit sphere
% using Anton Semechko's implementation of Reisz s-energy minimisation
% [xyz, tri] = ParticleSampleSphere('N', 20);

% use always the same points for reproducibility
xyz = [
    0.1865    0.1323   -0.9735
    0.8520    0.0405   -0.5219
    0.8562    0.4596    0.2358
    0.5982   -0.1356    0.7897
    0.2115    0.5448    0.8115
    0.4822    0.7204   -0.4986
   -0.9091    0.3984   -0.1217
    0.1588    0.9741    0.1609
   -0.5982    0.1356   -0.7897
   -0.5127    0.6897    0.5113
   -0.3189    0.8227   -0.4706
   -0.8520   -0.0405    0.5219
   -0.1866   -0.1323    0.9735
    0.5127   -0.6897   -0.5113
   -0.1588   -0.9741   -0.1609
   -0.2115   -0.5448   -0.8115
   -0.4822   -0.7204    0.4986
   -0.8562   -0.4596   -0.2358
    0.3189   -0.8227    0.4706
    0.9091   -0.3984    0.1217
];

tri = [
     7    10    11
     8    11    10
     1    11     6
     1     6     2
     8     6    11
    20     2     3
    20     3     4
     2     6     3
     8     3     6
     8    10     5
     8     5     3
     4     3     5
    13     5    10
    13     4     5
    20    14     2
     1     2    14
    20     4    19
    20    19    14
    15    14    19
    13    19     4
     7    11     9
     1     9    11
     7    12    10
    13    10    12
    15    16    14
     1    14    16
     1    16     9
    15    18    16
     9    16    18
     7     9    18
     7    18    12
    15    19    17
    15    17    18
    12    18    17
    13    17    19
    13    12    17
     ];

% check triangle intersections
cross = cgal_check_self_intersect(tri, xyz);
cross'

% plot mesh, colouring the facets according to whether they are overlapping
% or not
subplot(1, 2, 1)
hold off
trisurf(tri, xyz(:, 1), xyz(:, 2), xyz(:, 3), cross);
axis equal

%% create some triangle overlap

% select one vertex
I = 17;

% select a nearest neighbour
J = 19;

% plot them on the mesh
hold on
plot3(xyz(I, 1), xyz(I, 2), xyz(I, 3), 'ro')
plot3(xyz(J, 1), xyz(J, 2), xyz(J, 3), 'r*')

% move the nearest neighbor in a way such that I is still within the convex
% hull, but the nearest neighbour causes overlap
lat(J) = 13/180*pi;
lon(J) = -120/180*pi;

[xyz(J, 1), xyz(J, 2), xyz(J, 3)] = sph2cart(lon(J), lat(J), 1);

% check triangle intersections
cross = cgal_check_self_intersect(tri, xyz);
cross'

% triangles that contain the vertex we moved
find(sum(tri == J, 2)>0)
find(cross)

% plot mesh, colouring the facets according to whether they are overlapping
% or not
subplot(1, 2, 2)
hold off
trisurf(tri, xyz(:, 1), xyz(:, 2), xyz(:, 3), double(cross~=0));
axis equal
hold on
plot3(xyz(I, 1), xyz(I, 2), xyz(I, 3), 'ro')
plot3(xyz(J, 1), xyz(J, 2), xyz(J, 3), 'r*')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% two 3D triangles orthogonal and intersecting, intersection=segment

xyz = [
    -1 -1 0   % point for horizontal triangle
    1 -1 0    % point for horizontal triangle
    -1 1 0    % point for horizontal triangle
    -.5 0 1   % point for vertical triangle
    .5 0 1    % point for vertical triangle
    -.5 0 -1  % point for vertical triangle
    ];

tri = [
    1 2 3     % horizontal
    4 5 6     % vertical
    ];

% check triangle intersections
cross = cgal_check_self_intersect(tri, xyz);
cross'

% plot mesh, colouring the facets according to whether they are overlapping
% or not
subplot(1, 1, 1)
hold off
trisurf(tri, xyz(:, 1), xyz(:, 2), xyz(:, 3), cross);
axis equal

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% two 3D triangles orthogonal and intersecting, intersection=point

xyz = [
    -1 -1 0   % point for horizontal triangle
    1 -1 0    % point for horizontal triangle
    -1 1 0    % point for horizontal triangle
    -.5 0 2   % point for vertical triangle
    .5 0 2    % point for vertical triangle
    -.5 0 0  % point for vertical triangle
    ];

tri = [
    1 2 3     % horizontal
    4 5 6     % vertical
    ];

% check triangle intersections
cross = cgal_check_self_intersect(tri, xyz);
cross'

% plot mesh, colouring the facets according to whether they are overlapping
% or not
subplot(1, 1, 1)
hold off
trisurf(tri, xyz(:, 1), xyz(:, 2), xyz(:, 3), cross);
axis equal


%% This is for testing the SerialLink functions in the robotics Toolbox
function tests = SerialLinkTest

    tests = functiontests(localfunctions);
    clc

end

function setupOnce(tc)

    mdl_puma560;
    tc.TestData.p560 = p560;
    mdl_planar2
    tc.TestData.p2 = p2;
    mdl_puma560akb
    tc.TestData.p560m = p560m;
    mdl_stanford
    tc.TestData.stanf = stanf;
end

function teardownOnce(tc)
    close all
end

function SerialLink_test(tc)
    % test making a robot from links
    L(1)=Link([1 1 1 1 1]);
    L(2)=Link([0 1 0 1 0]);
    R1 = SerialLink(L,'name','robot1','comment', 'test robot','manufacturer', 'test',...
        'base', eye(4,4), 'tool', eye(4,4), 'offset', [1 1 0 0 0 0 ] );
    % test makin a robot from DH matrix
    DH = [pi/2 0 0 1; 0 2 0 0];
    R2 = SerialLink(DH,'name','Robot2');

end

function SerialLinkMDH_test(tc)
    % minimalistic test of modified DH functionality
    %  most code is common, need to exercise MDH code in Link and also for
    %  RNE

    mdl_puma560akb

    % qr is not set by the script, it clashes with the builtin function QR(tc)
    qz = [0 0 0 0 0 0];

    T = p560m.fkine(qz);
    J = p560m.jacobe(qz);
    tau = p560m.rne(qz, qz, qz);
end

function dyn_test(tc)

    mdl_puma560
    p560.dyn;

    mdl_puma560akb
    p560m.dyn;
end

%    *                          - compound two robots
function compound_test(tc)
    L(1)=Link([1 1 1 1 1]);
    L(2)=Link([0 1 0 1 0]);
    R1 = SerialLink(L,'name','robot1','comment', 'test robot','manufacturer', 'test',...
        'base', eye(4,4), 'tool', eye(4,4), 'offset', [1 1 0 0 0 0 ] );
    DH = [pi/2 0 0 1; 0 2 0 0];
    R2 = SerialLink(DH,'name','Robot2');
    %Test two different compunding situations
    R3 = R1*R2;
    R4 = R1*R3;
end

%%     Kinematic methods
%        fkine                  - forward kinematics
function fkine_test(tc)

    % create simple RP robot we can reason about
    L(1)=Link([0 2 3 0 0]);
    L(2)=Link([0 0 0 0 1]);
    R1 = SerialLink(L,'name','robot1','comment', 'test robot','manufacturer', 'test',...
        'base', eye(4,4), 'tool', eye(4,4) );

    tc.verifyTrue(isa( R1.fkine([0,0]), 'SE3') );

    tc.verifyEqual(R1.fkine([0,0]).T, transl(3,0,2), 'absTol',1e-6);
    tc.verifyEqual(R1.fkine([pi/2,0]).T, transl(0,3,2)*trotz(pi/2), 'absTol',1e-6);
    tc.verifyEqual(R1.fkine([0,1]).T, transl(3,0,3), 'absTol',1e-6);

    mdl_puma560

    T=p560.fkine( [0 0 0 0 0 0; 0 0.03 -0.0365 0 0 0] );
    tc.verifyEqual(length(T), 2, 'SE3' );;
end

function plot_test(tc)
    clf
    L(1)=Link([1 1 1 1 0]);
    L(1).qlim = [-5 5];
    L(2)=Link([0 1 0 1 0]);
    R1 = SerialLink(L,'name','robot1','comment', 'test robot','manufacturer', 'test',...
        'base', eye(4,4), 'tool', eye(4,4), 'offset', [1 1 0 0 0 0 ] );
    R1.plot([1 1]);
    close all
end

function plot_animate_test(tc)
    tc.assumeTrue(ispc || ismac);
    fname = fullfile(tempdir, 'puma.mp4');
    qt = jtraj(tc.TestData.p560.qz, tc.TestData.p560.qr, 50);
    tc.TestData.p560.plot(qt, 'movie', fname);
    tc.verifyTrue(exist(fname, 'file') == 2);
    delete(fname)
end

function plot3d_test(tc)
    clf
    tc.TestData.p560.plot3d( tc.TestData.p560.qn );
    close(gcf)
end

function teach_test(tc)
    tc.TestData.p560.teach();
    close all

    tc.TestData.p560.teach('eul');
    close all
    tc.TestData.p560.teach('eul', 'deg');
    close all
    tc.TestData.p560.teach('rpy');
    close all
    tc.TestData.p560.teach('rpy/xyz');
    close all
    tc.TestData.p560.teach('rpy/zyx');
    close all
    tc.TestData.p560.teach('approach');
    close all

    tc.TestData.p560.teach('callback', @(r, q) fprintf('hello') );
    close all

    tc.TestData.p2.teach();
    close all

    tc.TestData.p2.teach('2d');
    close all

    figure
    figure
    tc.TestData.p2.teach([0.2 0.3]);
    close all

    close all

end

%%--------------------------------------------------------------------------
%inverse kinematics for 6-axis arm with sph.wrist
function ikine6s_puma_test(tc)

    % puma case
    mdl_puma560;
    qn = [0 pi/4 -pi 1 pi/4 0];
    T = p560.fkine(qn);
    qik = p560.ikine6s(T,'ru');
    tc.verifyEqual(p560.fkine(qik).T, T.T, 'absTol', 1e-6);
    tc.verifyTrue(qik(2) > 0);

    T = p560.fkine(qn);
    qik = p560.ikine6s(T,'ld');
    tc.verifyEqual(p560.fkine(qik).T, T.T, 'absTol', 1e-6);
    tc.verifyTrue(qik(2) < 0);

    % error handling
    mdl_puma560akb

    tc.verifyError( @() p560m.ikine6s(T), 'RTB:ikine:notsupported' );
    tc.verifyError( @() tc.TestData.p2.ikine6s(T), 'RTB:ikine:notsupported' );

end

function ikine6s_stanford_test(tc)

    % stanford arm case
    q = [pi/4 pi/4 0.3 pi/4 -pi/4 pi/4];
    T = tc.TestData.stanf.fkine(q);
    qik = tc.TestData.stanf.ikine6s(T);
    tc.verifyEqual(tc.TestData.stanf.fkine(qik).T, T.T, 'absTol', 1e-6);

end

function ikine6s_KR5_test(tc)

    % kr5 arm case
    mdl_KR5
    q = [0 pi/4 -pi 1 pi/4 0];
    T = KR5.fkine(q);
    qik = KR5.ikine6s(T);
    tc.verifyEqual(KR5.fkine(qik).T, T.T, 'absTol', 1e-6);

end

function ikine6s_IRB140_test(tc)

    tc.assumeTrue(false);  %HACK

    % nooffset type
    mdl_irb140
    q = [0 -3*pi/4 0 0 pi/4 0];
    T = irb140.fkine(q);
    qik = irb140.ikine6s(T);
    tc.verifyEqual(irb140.fkine(qik).T, T.T, 'absTol', 1e-6);

end

function ikine_test(tc)
    T = tc.TestData.p560.fkine(tc.TestData.p560.qn);
    qik = tc.TestData.p560.ikine(T, [0 0 3 0 0 0]);

    T2 = tc.TestData.p560.fkine(qik);
    tc.verifyEqual(T.T, T2.T,'absTol',1e-6);
end

function ikunc_optim_test(tc)
    T = tc.TestData.p560.fkine(tc.TestData.p560.qn);
    qik = tc.TestData.p560.ikunc(T);

    T2 = tc.TestData.p560.fkine(qik);
    tc.verifyEqual(T.T, T2.T,'absTol',1e-6);
end

function ikcon_optim_test(tc)

    tc.assumeTrue(false);  %HACK

    qn = [0 pi/4 -pi 0 pi/4 0];
    T = tc.TestData.p560.fkine(qn);
    qik = tc.TestData.p560.ikcon(T);

    T2 = tc.TestData.p560.fkine(qik);
    tc.verifyEqual(T.T, T2.T,'absTol',1e-6);
end

function ikine_sym_test(tc)
    % 2DOF test
    p2 = SerialLink(tc.TestData.p2); % clone it

    q = p2.ikine_sym(2);

    % is the solution sane
    tc.verifyLength(q, 2);
    tc.verifyTrue(iscell(q));
    tc.verifyTrue(isa(q{1}, 'sym'));
    tc.verifyLength(q{1}, 2);
    tc.verifyTrue(isa(q{2}, 'sym'));
    tc.verifyLength(q{1}, 2);

    % process the solutions
    q1 = subs(q{1}, {'tx', 'ty'}, {1,1});   % convert to numeric
    sol = eval(q1)';
    q2 = subs(q{2}, {'tx', 'ty', 'q1'}, {1,1, q1(1)});
    x = eval(q2);  % first solution
    sol(1,2) = x(1);
    q2 = subs(q{2}, {'tx', 'ty', 'q1'}, {1,1, q1(2)}); % second solution
    x = eval(q2);
    sol(2,2) = x(1);

    % check the FK is good
    tc.verifyEqual(transl(tc.TestData.p2.fkine(sol(1,:))), [1 1 0]);
    tc.verifyEqual(transl(tc.TestData.p2.fkine(sol(2,:))), [1 1 0]);

    tc.verifyError( @() tc.TestData.p2.ikine_sym(4), 'RTB:ikine_sym:badarg');
end

function ikine_sym2_test(tc)
    tc.assumeTrue(false);  %HACK
    % 3DOF test

    % create robot arm with no offset (IRB140 style)
    robot = SerialLink([0 0.5 0 pi/2; 0 0 0.5 0; 0 0 0.5 0])
    test_sym_ik(robot)

    % create robot arm with offset (Puma560 style)
    robot = SerialLink(tc.TestData.p560.links(1:3));
    robot = SerialLink(robot); % clone it
    test_sym_ik(robot)

    function test_sym_ik(robot)
        % test all 8 solutions are good

        q = robot.ikine_sym(3);

        % process the solutions
        T = robot.fkine([0.2, 0.3, 0.4]); % choose joint angles

        pe = num2cell(T.t');

        q1 = subs(q{1}, {'tx', 'ty', 'tz'}, pe);   % convert to numeric

        qik = [];
        ss = [];

        for s1=1:2
            try
                sol1 = eval(q1(s1));
            catch
                fprintf('** q1(%d) eval failure\n', s1);
            end

            q2 = subs(q{2}, {'tx', 'ty', 'tz', 'q1'}, [pe q1(s1)]);

            for s2=1:2
                try
                    sol2 = eval(q2(s2));
                catch
                    fprintf('** q2(%d) eval failure\n', s2);
                    continue;
                end
                q3 = subs(q{3}, {'tx', 'ty', 'tz', 'q1', 'q2'}, [pe q1(s1), q2(s2)]);

                for s3=1:2
                    try
                        sol3 = eval(q3(s3));
                        qik = [qik; sol1 sol2 sol3];
                        ss = [ss; s1 s2 s3];
                    catch
                        fprintf('** q3(%d) eval failure\n', s3);
                    end
                end
            end
        end

        % check the FK is good, zero residual
        nn = [];
        for qq=qik'
            nn = [nn; norm(transl(robot.fkine(qq')-T))];
        end
        tc.verifyEqual(sum(nn), 0, 'absTol', 1e-6);
    end
end

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

function jacob0_test(tc)

    function J = jacob0_approx(robot, q, d)
        e = eye(robot.n);
        T0 = robot.fkine(q);
        R0 = t2r(T0);
        J = [];
        for i=1:robot.n
            Ji = (robot.fkine(q + e(i,:)*d) - T0) / d;
            J = [J [Ji(1:3,4); vex(Ji(1:3,1:3)*R0')]];
        end
    end
    % implictly tests jacobe
    qz = [0 1 0 0 2 0];
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = tc.TestData.p560.jacob0(qz);
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    tc.verifyEqual(tc.TestData.p560.jacob0(qz), jacob0_approx(tc.TestData.p560, qz, 1e-8), 'absTol', 1e-4);
    tc.verifyEqual(tc.TestData.p560.jacob0(qn), jacob0_approx(tc.TestData.p560, qn, 1e-8), 'absTol', 1e-4);

    expected_out = jacob0_approx(tc.TestData.p560, qn, 1e-8)
    out = tc.TestData.p560.jacob0(qn*180/pi, 'deg');
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    out = tc.TestData.p560.jacob0(qn, 'rpy');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'eul');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'exp');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'trans');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [3 6]);
    out = tc.TestData.p560.jacob0(qn, 'rot');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [3 6]);

    tc.verifyEqual(tc.TestData.p560.jacob0(qn), jacob0_approx(tc.TestData.p560, qn, 1e-8), 'absTol', 1e-4);

    out = tc.TestData.stanf.jacob0([0 0 0 0 0 0]);
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);
    tc.verifyEqual(tc.TestData.stanf.jacob0(qn), jacob0_approx(tc.TestData.stanf, qn, 1e-8), 'absTol', 1e-4);

end

function jacobe_test(tc)

    function J = jacobe_approx(robot, q, d)
        e = eye(robot.n);
        T0 = robot.fkine(q);
        R0 = t2r(T0);
        J = [];
        for i=1:robot.n
            Ji = (robot.fkine(q + e(i,:)*d) - T0) / d;
            J = [J [R0'*Ji(1:3,4); vex(R0'*Ji(1:3,1:3))]];
        end
    end

    qz = [0 1 0 0 2 0];
    qn = [0 pi/4 -pi 1 pi/4 0];

    out = tc.TestData.p560.jacobe(qz)
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);
    expected_out = jacobe_approx(tc.TestData.p560, qz, 1e-8)
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    out = tc.TestData.p560.jacobe(qz*180/pi, 'deg');
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    out = tc.TestData.p560.jacobe(qn);
    expected_out = jacobe_approx(tc.TestData.p560, qn, 1e-8)

    tc.verifyEqual(out,expected_out,'absTol',1e-4);
    out = tc.TestData.p560.jacobe(qn*180/pi, 'deg');
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    out = tc.TestData.p560.jacob0(qn, 'rpy');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'eul');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'exp');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'trans');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [3 6]);
    out = tc.TestData.p560.jacob0(qn, 'rot');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [3 6]);

    tc.verifyWarning( @() tc.TestData.p560.jacobn(qn), 'RTB:SerialLink:deprecated');
end

function maniplty_test(tc)
    mdl_puma560;
    q = [0 pi/4 -pi 1 pi/4 0];
    tc.verifyEqual(p560.maniplty(q), 0.0786, 'absTol',1e-4);
    tc.verifyEqual(p560.maniplty(q, 'trans'), 0.1112, 'absTol',1e-4);
    tc.verifyEqual(p560.maniplty(q, 'rot'), 2.5936, 'absTol',1e-4);
    tc.verifyEqual(p560.maniplty(q, 'asada', 'trans'), 0.2733, 'absTol',1e-4);
end

function jacob_dot_test(tc)

    tc.assumeTrue(false);  %HACK

    function Jdqd = jacob_dot_approx(robot, q, qd, d)
        e = eye(robot.n);
        J0 = robot.jacob0(q);
        Jdqd = zeros(6,1);
        for i=1:robot.n
            Ji = (robot.jacob0(q + e(i,:)*d) - J0) / d;
            Jdqd = Jdqd + Ji * qd(i);
        end
    end

    q = rand(1,6);
    qd = rand(1,6);

    jacob_dot_approx(tc.TestData.p560, q, qd, 1e-8)*qd'
    tc.TestData.p560.jacob_dot(q, qd)
end

%%     Dynamics methods

function rne_test(tc)

    % use something simple we can reason about

    mdl_twolink
    g = twolink.gravity(3);

    qz = [0 0];  % need this to get around a MATLAB bug passing values from a script

    % different poses
    Q = twolink.rne([0 0], qz, qz, 'slow');
    tc.verifyEqual(Q, g*[2 0.5], 'absTol', 1e-6);

    Q = twolink.rne([0 pi/2], qz, qz);
    tc.verifyEqual(Q, g*[1.5 0], 'absTol', 1e-6);

    Q = twolink.rne([pi/2 0], qz, qz);
    tc.verifyEqual(Q, g*[0 0], 'absTol', 1e-6);

    % change gravity
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0]);
    tc.verifyEqual(Q, g*[0 0], 'absTol', 1e-6);
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0]');
    tc.verifyEqual(Q, g*[0 0], 'absTol', 1e-6);
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0]);
    tc.verifyEqual(Q, g*[0 0], 'absTol', 1e-6);
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 -g]);
    tc.verifyEqual(Q, g*[-2 -0.5], 'absTol', 1e-6);

    % add an external force in end-effector frame
    %  y-axis is vertically upward

    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0], 'fext', [1 0 0 0 0 0]);
    tc.verifyEqual(Q, [0 0], 'absTol', 1e-6);
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0], 'fext', [0 1 0 0 0 0]);
    tc.verifyEqual(Q, [2 1], 'absTol', 1e-6);
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0], 'fext', [0 0 1 0 0 0]);
    tc.verifyEqual(Q, [0 0], 'absTol', 1e-6);

    % test the [q qd qdd] case
    Q1 = twolink.rne([1 2], [3 4], [5 6]);
    Q2 = twolink.rne([1 2 3 4 5 6]);
    tc.verifyEqual(Q1, Q2, 'absTol', 1e-6);

    % test the matrix input case
    Q1 = twolink.rne([1 2], [3 4], [5 6]);
    Q2 = twolink.rne([5 6], [1 2], [3 4]);
    Q3 = twolink.rne([3 4], [5 6], [1 2]);

    Q4 = twolink.rne([1 2; 5 6; 3 4], [3 4; 1 2; 5 6], [5 6; 3 4; 1 2]);
    tc.verifyEqual([Q1; Q2; Q3], Q4, 'absTol', 1e-6);

    % probably should do a robot with a prismatic axis (or two)

end

function rne_mdh_test(tc)
    q = rand(1,6);
    tc.TestData.p560m.rne(q, q, q);
end

%        accel                  - forward dynamics
function accel_test(tc)
    qd = 0.5 * [1 1 1 1 1 1];
    qz = [0 1 0 0 2 0];
    Q = tc.TestData.p560.rne(tc.TestData.p560.qn, qz, qz);

    out = tc.TestData.p560.accel(qz, qd, Q);
    expected_out = [  -9.3397 4.9666 1.6095 -5.4305 5.9885 -2.1228]';

    tc.verifyEqual(out, expected_out,'absTol',1e-4);

    out = tc.TestData.p560.accel([qz, qd,Q]);
    tc.verifyEqual(out, expected_out,'absTol',1e-4);

    out = tc.TestData.p560.accel([qz, qd,Q]');
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    qd = [0.1 0.1 0.1 0.1 0.1 0.1;0.2 0.2 0.2 0.2 0.2 0.2];
    qz = [0 1 0 0 2 0;0 0.5 0 0 1 0];
    qd1 = [0.1 0.1 0.1 0.1 0.1 0.1];
    qd2 = [0.2 0.2 0.2 0.2 0.2 0.2];
    qz1 = [0 1 0 0 2 0];
    qz2 = [0 0.5 0 0 1 0];
    qn1 = [0 pi/4 -pi 1 pi/4 0];
    qn2 = [0 pi/2 -pi 1 pi/2 0];

    Q1 = tc.TestData.p560.rne(qn1,qz1,qz1);
    Q2 = tc.TestData.p560.rne(qn2,qz2,qz2);

    q3 = [Q1;Q2];

    out = tc.TestData.p560.accel(qz, qd,q3);
    expected_out = [
        -8.2760    5.8119    3.1487   -4.6392    6.9558   -1.6774
        -8.3467   -4.8514    6.0575   -4.9232    3.1244   -1.7861 ];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    tc.verifyError( @() tc.TestData.p560.accel(), 'RTB:accel:badarg');
    tc.verifyError( @() tc.TestData.p560.accel( [ 1 2]), 'RTB:accel:badarg');
    tc.verifyError( @() tc.TestData.p560.accel( qz, qz, qd1), 'RTB:accel:badarg');
    tc.verifyError( @() tc.TestData.p560.accel( qz, qd1, qd1), 'RTB:accel:badarg');

    tc.verifyError( @() tc.TestData.p560.accel( [1 2], qd1, qd1), 'RTB:accel:badarg');
    tc.verifyError( @() tc.TestData.p560.accel( qz1, [1 2], qd1), 'RTB:accel:badarg');
    tc.verifyError( @() tc.TestData.p560.accel( qz1, qz1, [1 2]), 'RTB:accel:badarg');

    tc.verifyError( @() tc.TestData.p560.accel( [ 1 2]), 'RTB:accel:badarg');

end

%        cinertia               - Cartesian manipulator inertia matrix
function cinertia_test(tc)
    mdl_puma560;
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = p560.cinertia(qn);
    expected_out = [18.0741   -2.2763   -8.8446   -0.0283    0.7541   -0.4015
        -2.2763   11.2461    1.7238   -0.0750    0.2214   -0.4733
        -8.8446    1.7238   14.1135   -0.0300    0.7525   -0.4032
        -0.0283   -0.0750   -0.0300    0.1377   -0.0171    0.0492
        0.7541    0.2214    0.7525   -0.0171    0.4612   -0.2461
        -0.4015   -0.4733   -0.4032    0.0492   -0.2461    0.3457];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);;
end

%        coriolis               - centripetal/coriolis torque
function coriolis_test(tc)
    mdl_puma560;
    qd = 0.5 * [1 1 1 1 1 1];
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = p560.coriolis(qn, qd);
    expected_out = [
        -0.1336   -0.6458    0.0845    0.0005   -0.0008    0.0000
        0.3136    0.1922    0.3851   -0.0018   -0.0007    0.0000
        -0.1803   -0.1934   -0.0005   -0.0009   -0.0014    0.0000
        0.0007    0.0008    0.0003    0.0001    0.0003   -0.0000
        -0.0004    0.0005    0.0005   -0.0003   -0.0000   -0.0000
        0.0000    0.0000    0.0000   -0.0000    0.0000         0
        ];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    qd = [0.1 0.1 0.1 0.1 0.1 0.1;0.2 0.2 0.2 0.2 0.2 0.2];
    qn = [0 pi/4 -pi 1 pi/4 0; 0 pi/2 -pi 1 pi/2 0];
    out = p560.coriolis(qn, qd);
    expected_out(:,:,1) = [
        -0.0267   -0.1292    0.0169    0.0001   -0.0002    0.0000
        0.0627    0.0384    0.0770   -0.0004   -0.0001    0.0000
        -0.0361   -0.0387   -0.0001   -0.0002   -0.0003    0.0000
        0.0001    0.0002    0.0001    0.0000    0.0001   -0.0000
        -0.0001    0.0001    0.0001   -0.0001   -0.0000   -0.0000
        0.0000    0.0000    0.0000   -0.0000    0.0000         0
        ];
    expected_out(:,:,2) = [
        -0.0715   -0.1242   -0.0534    0.0008   -0.0001   -0.0000
        0.0731    0.0765    0.1535   -0.0009   -0.0007    0.0000
        -0.0023   -0.0772   -0.0003   -0.0004   -0.0007    0.0000
        0.0004    0.0006    0.0003    0.0000    0.0002   -0.0000
        0.0002    0.0004    0.0004   -0.0002    0.0000    0.0000
        -0.0000    0.0000    0.0000   -0.0000   -0.0000         0
        ];

    tc.verifyEqual(out,expected_out,'absTol',1e-4);
end

function gravload_test(tc)
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = tc.TestData.p560.gravload(qn);
    expected_out = [-0.0000 31.6334 6.0286 -0.0119 0.0218 0];
    tc.verifyEqual(out, expected_out, 'absTol',1e-4);

    qn = [0 pi/4 -pi 1 pi/4 0; 0 pi/2 -pi 1 pi/2 0];
    out = tc.TestData.p560.gravload(qn);
    expected_out = [-0.0000   31.6334    6.0286   -0.0119    0.0218         0
        0.0000    7.7198    8.7439   -0.0238   -0.0000         0];
    tc.verifyEqual(out, expected_out, 'absTol', 1e-4);

    % zero gravity
    out = tc.TestData.p560.gravload(tc.TestData.p560.qn, [0 0 0]);
    tc.verifyEqual(out, [0 0 0 0 0 0], 'absTol', 1e-4);

    tc.verifyError( @() tc.TestData.p560.gravload([1 2 3]), 'RTB:SerialLink:gravload:badarg' );
end

function inertia_test(tc)
    mdl_puma560;
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = p560.inertia(qn);
    expected_out = [3.6591   -0.4042    0.1013   -0.0021   -0.0015   -0.0000
        -0.4042    4.4128    0.3504   -0.0008    0.0017    0.0000
        0.1013    0.3504    0.9377   -0.0008    0.0008    0.0000
        -0.0021   -0.0008   -0.0008    0.1925    0.0000    0.0000
        -0.0015    0.0017    0.0008    0.0000    0.1713    0.0000
        -0.0000    0.0000    0.0000    0.0000    0.0000    0.1941];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    qn = [0 pi/4 -pi 1 pi/4 0; 0 pi/2 -pi 1 pi/2 0];
    out = p560.inertia(qn);
    expected_out(:,:,1) = [3.6591   -0.4042    0.1013   -0.0021   -0.0015   -0.0000
        -0.4042    4.4128    0.3504   -0.0008    0.0017    0.0000
        0.1013    0.3504    0.9377   -0.0008    0.0008    0.0000
        -0.0021   -0.0008   -0.0008    0.1925    0.0000    0.0000
        -0.0015    0.0017    0.0008    0.0000    0.1713    0.0000
        -0.0000    0.0000    0.0000    0.0000    0.0000    0.1941];
    expected_out(:,:,2) = [2.6621   -0.6880    0.0035   -0.0007   -0.0010    0.0000
        -0.6880    4.4114    0.3487   -0.0010    0.0015    0.0000
        0.0035    0.3487    0.9359   -0.0010    0.0003    0.0000
        -0.0007   -0.0010   -0.0010    0.1926    0.0000    0.0000
        -0.0010    0.0015    0.0003    0.0000    0.1713    0.0000
        0.0000    0.0000    0.0000    0.0000    0.0000    0.1941];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);
end

function dynamic_component_test(tc)
    % test that inertial components
    q = rand(1,6);
    qd = rand(1,6);
    qdd = rand(1,6);

    robot = tc.TestData.p560.nofriction('all');

    I = robot.inertia(q);
    C = robot.coriolis(q, qd);
    g = robot.gravload(q)';

    tau = robot.rne(q, qd, qdd)';

    tc.verifyEqual(norm(I*qdd'+C*qd'+g-tau), 0, 'absTol', 1e-8);
end

%        itorque                - inertia torque
function itorque_test(tc)
    mdl_puma560;
    qn = [0 pi/4 -pi 1 pi/4 0];
    qdd = 0.5 * [1 1 1 1 1 1];
    out = p560.itorque(qn,qdd);
    expected_out = [1.6763    2.1799    0.6947    0.0944    0.0861    0.0971];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    %        rne                    - inverse dynamics
end
function rne2_test(tc)
    mdl_puma560;
    qz = [0 1 0 0 2 0];
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = p560.rne(qn,qz,qz);
    expected_out = [-0.9748   59.1306    5.9889   -0.0108    1.8872    0.0001];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    pnf = p560.nofriction('all');

    q = rand(1,6);
    qd = rand(1,6);
    qdd = rand(1,6);

    tau1 = pnf.rne(q,qd,qdd);
    tau2 = pnf.inertia(q)*qdd' +pnf.coriolis(q,qd)*qd' + pnf.gravload(q)';
    tc.verifyEqual(tau1', tau2, 'absTol', 1e-6);
end

%        fdyn                   - forward dynamics
function fdyn_test(tc)
    mdl_puma560;
    qn = [0 pi/4 -pi 1 pi/4 0];
    qd = 0.5 * [1 1 1 1 1 1];
    qdo = [0 0 0 0 0 0];
    T = 0.0001;

    p560 = p560.nofriction();

    [TI,Q,QD] = p560.fdyn(T, @(r,t,q,qd) zeros(1,6), qn, qdo);
end

function nofriction_test(tc)
    mdl_puma560;
    verifyFalse(tc,  p560.links(3).B == 0);
    verifyFalse(tc,  all(p560.links(3).Tc == 0) );

    nf = p560.nofriction();
    verifyFalse(tc,  nf.links(3).B == 0);
    tc.verifyTrue( all(nf.links(3).Tc == 0) );

    mdl_puma560;
    nf = p560.nofriction('all');
    tc.verifyTrue( nf.links(3).B == 0);
    tc.verifyTrue( all(nf.links(3).Tc == 0) );
end

function jtraj_test(tc)
    mdl_puma560
    qz = [0 0 0 0 0 0];
    qr = [0 pi/2 -pi/2 0 0 0];
    T1 = p560.fkine(qz);
    T2 = p560.fkine(qr);
    qt = p560.jtraj(T1, T2, 50, 'r');
    tc.verifyEqual( size(qt), [50 6]);

    %tc.verifyEqual(qt(1,:), qz, 'abstol', 1e-10);
    %tc.verifyEqual(qt(end,:), qr, 'abstol', 1e-10);
end

function edit_test(tc)
    robot = SerialLink(tc.TestData.p560);

    robot.edit()

    table = findobj('Type', 'uitable');
    table.Data(1,2) = 7;

    button = findobj('String', 'Save')
    f = button.Callback{1};
    f(button, [], table);

    tc.verifyEqual(robot.links(1).d, 7);
    close(gcf)

    robot.edit('dyn')

    table = findobj('Type', 'uitable');
    table.Data(1,9) = 7;

    button = findobj('String', 'Save')
    f = button.Callback{1};
    f(button, [], table);

    tc.verifyEqual(robot.links(1).m, 7);
    close(gcf)
end

function ellipse_test(tc)
    tc.TestData.p560.plot( tc.TestData.p560.qn );
    tc.TestData.p560.fellipse( tc.TestData.p560.qn );
    tc.TestData.p560.fellipse( tc.TestData.p560.qn, 'trans' );
    tc.TestData.p560.fellipse( tc.TestData.p560.qn, 'rot' );

    clf
    tc.TestData.p560.fellipse( tc.TestData.p560.qn, '2d' );

    close(gcf)

    tc.TestData.p560.plot( tc.TestData.p560.qn );
    tc.TestData.p560.vellipse( tc.TestData.p560.qn );
    tc.TestData.p560.vellipse( tc.TestData.p560.qn, 'trans' );
    tc.TestData.p560.vellipse( tc.TestData.p560.qn, 'rot' );
    clf
    tc.TestData.p560.fellipse( tc.TestData.p560.qn, '2d' );

    close(gcf)
end

function dh_mdh_test(tc)

    tc.verifyTrue(tc.TestData.p560.isdh)
    tc.verifyFalse(tc.TestData.p560.ismdh)

    tc.verifyClass(tc.TestData.p560.isdh, 'logical')
    tc.verifyClass(tc.TestData.p560.ismdh, 'logical')

    tc.verifyFalse(tc.TestData.p560m.isdh)
    tc.verifyTrue(tc.TestData.p560m.ismdh)

    p560m = tc.TestData.p560.MDH;
    tc.verifyTrue(isa(p560m, 'SerialLink'));
    tc.verifyEqual(p560m.n, 6);
    tc.verifyTrue(p560m.ismdh);

    p560 = tc.TestData.p560m.DH;
    tc.verifyTrue(isa(p560, 'SerialLink'));
    tc.verifyEqual(p560.n, 6);
    tc.verifyTrue(p560.isdh);

    p560m = tc.TestData.p560.MDH;
    p560 = p560m.DH;  % should be the same robot

    T1 = tc.TestData.p560.fkine(tc.TestData.p560.qn);
    T2 = p560.fkine(tc.TestData.p560.qn);
    tc.verifyEqual(T1, T2);

end

function twists_test(tc)
    [tw,T0] = tc.TestData.p560.twists(tc.TestData.p560.qz);
    tc.verifyClass(tw, 'Twist');
    tc.verifyLength(tw, 6);
    tc.verifyClass(T0, 'SE3');
    tc.verifyLength(T0, 1);

    tc.verifyEqual( double(prod( [tw.exp(tc.TestData.p560.qn) T0] )), ...
        double(tc.TestData.p560.fkine(tc.TestData.p560.qn)), ...
        'absTol', 1e-6);
end

function predicates_test(tc)
    % isdh
    x = tc.TestData.p560.isdh
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);
    x = tc.TestData.p560m.isdh
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyFalse(x);

    % ismdh
    x = tc.TestData.p560.ismdh
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyFalse(x);
    x = tc.TestData.p560m.ismdh
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);

    % isspherical
    x = tc.TestData.p560.isspherical
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);
    x = tc.TestData.p560.isspherical
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);

    % isprismatic
    x = tc.TestData.stanf.isprismatic
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 6); tc.verifyEqual(x, logical([0 0 1 0 0 0]));
    % isrevolute
    x = tc.TestData.stanf.isrevolute
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 6); tc.verifyEqual(x, ~logical([0 0 1 0 0 0]));

    % issym
    x = tc.TestData.p560.issym
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyFalse(x);
    mdl_twolink_sym;
    x = twolink.issym
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);

    % isconfig
    x = tc.TestData.p560.isconfig('RRRRRR');
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);
    x = tc.TestData.p560.isconfig('RRPRRR');
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyFalse(x);
    x = tc.TestData.stanf.isconfig('RRRRRR');
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyFalse(x);
    x = tc.TestData.stanf.isconfig('RRPRRR');
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);

    % islimit
    x = tc.TestData.p560.islimit([0 0 0 0 0 0]);
    tc.verifyClass(x, 'double'); tc.verifySize(x, [6 2]);
    tc.verifyEqual(x, zeros(6,2));
    x = tc.TestData.p560.islimit([0 10 0 0 0 0]);
    tc.verifyClass(x, 'double'); tc.verifySize(x, [6 2]);
    tc.verifyEqual(x, [0 1 0 0 0 0]'*[1 1]);
end

function test_get(tc)
    x = tc.TestData.p560.d;
    tc.verifyClass(x, 'double'); tc.verifyLength(x, 6);
    tc.verifyEqual(x, [0  0  0.15005  0.4318  0  0], 'absTol', 1e-6);

    x = tc.TestData.p560.a;
    tc.verifyClass(x, 'double'); tc.verifyLength(x, 6);
    tc.verifyEqual(x, [0 0.4318 0.0203  0  0 0], 'absTol', 1e-6);

    x = tc.TestData.p560.alpha;
    tc.verifyClass(x, 'double'); tc.verifyLength(x, 6);
    tc.verifyEqual(x, [1 0 -1 1 -1 0]*pi/2, 'absTol', 1e-6);

    x = tc.TestData.p560.theta;
    tc.verifyClass(x, 'double'); tc.verifyLength(x, 0);

end

function mat2str_test(tc)
    mdl_twolink_sym;
    twolink
end

function rad_deg_test(tc)
    q = [1 2 3 4 5 6];

    q2 = tc.TestData.p560.todegrees(q);
    tc.verifyEqual(q*180/pi, q2);
    q2 = tc.TestData.p560.toradians(q);
    tc.verifyEqual(q*pi/180, q2);

    q2 = tc.TestData.stanf.todegrees(q);
    qq = q*180/pi;
    qq(3) = q(3);
    tc.verifyEqual(qq, q2);
    q2 = tc.TestData.stanf.toradians(q);
    qq = q*pi/180;
    qq(3) = q(3);
    tc.verifyEqual(qq, q2);

end

function trchain_test(tc)
    % TODO need to test return values here
    s = tc.TestData.p560.trchain();
    tc.verifyClass(s, 'char');

    s = tc.TestData.p560m.trchain();
    tc.verifyClass(s, 'char');
end

function jointdynamics_test(tc)
    jd = tc.TestData.p560.jointdynamics( zeros(1,6), zeros(1,6));

    tc.verifySize(jd, [1 6]);
    tc.verifyClass(jd, 'tf');
end


function cvt_movie5 ( )

%*****************************************************************************80
%
%% CVT_MOVIE5 creates a movie from data files.
%
%  Discussion:
%
%    This simple example creates a movie by reading in a set of point
%    values at each step in an iteration.
%
%    The point values are 389 points inside the P08 "holey pie" region.
%    They were computed in a CVT calculation external to this procedure.
%
%    This routine simply reads in the data, adds a picture of the boundary
%    of the region, and computes the Voronoi diagram, before "taking a picture"
%    which creates a JPEG image.  The JPEG images can easily be made into a movie.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 July 2005
%
%  Author:
%
%    John Burkardt
%
%  Local parameters:
%
%    Local parameter FILE_NAME, the name of the data file being read.
%
  data_file = 'p08_hbf_000.txt';

  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_MOVIE5:\n' );
  fprintf ( 1, '  Create a movie of a CVT computation.\n' );
  fprintf ( 1, '  This example uses the "holey pie" geometry.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The data has been precomputed.\n' );
  fprintf ( 1, '  The first data file to read is %s\n', data_file );
%
%  This switch controls whether the Delaunay triangulation is displayed.
%
  delaunay_display = 0;

  if ( delaunay_display )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'The Delaunay triangulation will be displayed.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'The Delaunay triangulation will NOT be displayed.\n' );
  end

  frame_num = 0;

  frame_file = 'cvt_movie5_000.jpg';
  frame_title = 'cvt\_movie5\_000.jpg';

  while ( 1 )

    if ( ~file_exist ( data_file ) )
      break
    end

    p = load ( data_file );
    frame_num = frame_num + 1;
%
%  Compute the Delaunay triangle information T for the current nodes.
%
    t = delaunay ( p(:,1), p(:,2) );
%
%  Display a graph of the generators.
%
%  This was a little tricky.  The TRIMESH command must come before
%  the VORONOI command in order for both items to appear.
%
%  There are obvious discrepancies between the Delaunay and Voronoi
%  graphs which suggest that there are mistakes in the Voronoi diagram
%  as computed by MATLAB's VORONOI command!
%
    if ( delaunay_display )
      trimesh ( t, p(:,1), p(:,2), zeros(n,1) )
      hold on
    end
%
%  Here, we use a new, unreleased version of the VORONOI command, which
%  does a better job of handling the semi-infinite sides.
%
    voronoi ( p(:,1), p(:,2), t );

    title ( frame_title );
    axis equal
    axis ( [ 0.0, 1.0, -0.29, +0.29 ] )
    segment_num = p08_boundary_segment_num ( 'DUMMY' );
    h = 0.05;
    for segment_index = 1 : segment_num
      segment_length = p08_boundary_segment_length ( segment_index, h );
      segment = p08_boundary_segment ( segment_index, 2, segment_length );
      line ( segment(1,:), segment(2,:), 'Color', 'r' );
    end
    view ( 2 )
    drawnow

    if ( delaunay_display )
      hold off
    end

    F = getframe;
    [ X, map ] = frame2im ( F );
    imwrite ( X, frame_file, 'JPEG' );
%
%  Prepare for next step.
%
    data_file = file_name_inc ( data_file );
    frame_file = file_name_inc ( frame_file );
    frame_title = file_name_inc ( frame_title );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of frames created is %d\n', frame_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_MOVIE5:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

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

  return
end


function [ node_num2, triangle_num2, edge_data ] = refine_size ( node_num1, ...
  triangle_num1, triangle_node1 )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER3_REFINE_SIZE sizes a refined order 3 triangulation.
%
%  Discussion:
%
%    Given a triangle defined by nodes 1, 2, 3, we need to generate
%    nodes 12, 23, and 13, and create 4 new subtriangles, T1, T2, T3
%    and T4.
%
%    The task is more complicated by the fact that we are working with
%    a mesh of triangles, so that we want to create a node only once,
%    even though it may be shared by other triangles.
%
%          3
%         / \
%        /T3 \
%      13----23
%      / \T4 / \
%     /T1 \ /T2 \
%    1----12-----2
%
%    This routine simply determines the sizes of the resulting node
%    and triangle arrays.
%
%    The primary amount of work occurs in sorting a list of 3 * TRIANGLE_NUM
%    data items, one item for every edge of every triangle.  Each
%    data item records, for a given edge, the global indices
%    of the two endpoints, the local indices of the two endpoints,
%    and the index of the triangle.
%
%    Through careful sorting, it is possible to arrange this data in
%    a way that allows the proper generation of the interpolated nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM1, the number of nodes in the original mesh.
%
%    Input, integer TRIANGLE_NUM1, the number of triangles in the
%    original mesh.
%
%    Input, integer TRIANGLE_NODE1(TRIANGLE_NUM1,3), the indices of the nodes
%    that form the triangles in the input mesh.
%
%    Output, integer NODE_NUM2, the number of nodes in the refined mesh.
%
%    Output, integer TRIANGLE_NUM2, the number of triangles in the
%    refined mesh.
%
%    Output, integer EDGE_DATA(3*TRIANGLE_NUM1,5), edge data that will
%    be needed by TRIANGULATION_ORDER3_REFINE_COMPUTE.
%
  edge_data = zeros(3*triangle_num1,5);
%
%  Step 1.
%  From the list of nodes for triangle T, of the form: (I,J,K)
%  construct the edge relations:
%
%    (I,J,1,2,T)
%    (I,K,1,3,T)
%    (J,K,2,3,T)
%
%  In order to make matching easier, we reorder each pair of nodes
%  into ascending order.
%
  row = 0;

  for triangle = 1 : triangle_num1

    i = triangle_node1(triangle,1);
    j = triangle_node1(triangle,2);
    k = triangle_node1(triangle,3);

    a = min ( i, j );
    b = max ( i, j );
    row = row + 1;
    edge_data(row,1:5) = [ a, b, 1, 2, triangle ];

    a = min ( j, k );
    b = max ( j, k );
    row = row + 1;
    edge_data(row,1:5) = [ a, b, 2, 3, triangle ];

    a = min ( k, i );
    b = max ( k, i );
    row = row + 1;
    edge_data(row,1:5) = [ a, b, 1, 3, triangle ];

  end
%
%  Step 2. Perform an ascending dictionary sort on the neighbor relations.
%  We only intend to sort on items in columns 1 and 2; the routine we call here
%  sorts on the full column but that won't hurt us.
%
%  What we need is to find all cases where triangles share an edge.
%  By sorting the columns of the EDGE_DATA array, we will put shared edges
%  next to each other.
%
  edge_data = sortrows ( edge_data );
% edge_data = i4col_sort_a ( 5, 3*triangle_num1, edge_data );
%
%  Step 3. All the triangles which share an edge show up as consecutive
%  columns with identical first two entries.  Figure out how many new
%  nodes there are, and allocate space for their coordinates.
%
  node_num2 = node_num1;

  n1_old = -1;
  n2_old = -1;

  for edge = 1 : 3 * triangle_num1
    n1 = edge_data(edge,1);
    n2 = edge_data(edge,2);
    if ( n1 ~= n1_old || n2 ~= n2_old )
      node_num2 = node_num2 + 1;
      n1_old = n1;
      n2_old = n2;
    end
  end

  triangle_num2 = 4 * triangle_num1;

  return
end


function showrate3(N1,err1,k1,opt1,str1,N2,err2,k2,opt2,str2,N3,err3,k3,opt3,str3)
%% SHOWRATE3 rate of two error sequences
%
% showrate3(N1,err1,k1,opt1,str1,N2,err2,k2,opt2,str2,N3,err3,k3,opt3,str3)
% plots the err1 vs N1, err2 vs N2, and err3 vs N3 in the loglog scale.
% Additional input
%
%   - k1, k2, k3: specify the starting indices; see showrate
%   - opt1, opt2, opt3: the line color and style
%   - str1, str2, str3: strings used in legend
%
% Example
%
% showrate2(N,energyErr,1,'r-+','||u-u_h||_A',...
%           N,L2Err,1,'b-+','||u-u_h||');
%
% See also showrate, showresult, showmesh, showsolution
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if (nargin<=2)
    k1 = 1; opt1 = '-*';
end
r1 = showrate(N1,err1,k1,opt1);
hold on
r2 = showrate(N2,err2,k2,opt2);
r3 = showrate(N3,err3,k3,opt3);
h_legend = legend(str1,['C_1N^{' num2str(r1) '}'],...
                  str2,['C_2N^{' num2str(r2) '}'],...
                  str3,['C_3N^{' num2str(r3) '}'],'LOCATION','Best');
set(h_legend,'FontSize',12);


function daefun(dh,x,z,u,p)

  conf = dh.userdata;

  ddp = - 1/conf.m * z.lambda*x.p - [0;9.81] + [u.F;0];

  dh.setODE('p',x.v);
  dh.setODE('v',ddp);
  dh.setODE('time', 1);

  % The algebraic equation constraints the pendulum's mass to be on a circular
  % path if the initial conditions are satisfied.
  dh.setAlgEquation(dot(ddp,x.p)+x.v(1)^2+x.v(2)^2);
end


function p = linear_search(A,t)
%% linear Search
% This function linear searches target value (t) in array A.
% For this, It sequentially checks each entry of the array until a match is found
% or  or the whole list has been searched.
% If it can find the target returns 1 otherwise 0.

array_length = length(A);
i = 1;
searchTermination = 0;

while searchTermination == 0 && i < array_length+1
    if A(i) == t
        p = 1;
        searchTermination = 1;
        disp('the target is found in the array')
    else
        i = i+1;
    end
end
if i == array_length+1
    p = 0;
    disp('the target is not found in the array')
end
end


function [varargout]=bss_energy_ratios(F_s_target,F_e_interf,varargin)

% compute energy ratios corresponding to SDR/SIR/SNR/SAR given a decomposition of an estimated source into target/interference/noise/artifacts over frames.
%
% Usage:
%
%    [SDR,SIR,SAR]    =bss_energy_ratios(F_s_target,F_e_interf,F_e_artif)
%    [SDR,SIR,SNR,SAR]=bss_energy_ratios(F_s_target,F_e_interf,F_e_noise,F_e_artif)
%
% Input:
%   - F_s_target: n_frames x T matrix containing the frames of the target source contribution,
%   - F_e_interf: n_frames x T matrix containing the frames of the interferences contribution,
%   - F_e_noise: n_frames x T matrix containing the frames of the noise contribution (if any),
%   - F_e_artif: n_frames x T matrix containing the frames of the artifacts contribution.
%
% Ouput:
%   - SDR: n_frames x 1 vector contaning the Source to Distortion Ratios per frame,
%   - SIR: n_frames x 1 vector contaning the Source to Interferences Ratios per frame,
%   - SNR: n_frames x 1 vector contaning the Signal to Noise Ratios (if noise) per frame,
%   - SAR: n_frames x 1 vector contaning the Signal to Artifacts Ratios per frame.
%
% Developers:  - Cedric Fevotte (cf269@cam.ac.uk) - Emmanuel Vincent
% (vincent@ircam.fr) - Remi Gribonval (remi.gribonval@irisa.fr)

switch nargin
    case 3
        F_e_artif=varargin{1};
        % SDR
        F_e_total=F_e_interf+F_e_artif;
        varargout{1}= sum(F_s_target.^2,2)./sum(F_e_total.^2,2);
        % SIR
        varargout{2}=sum(F_s_target.^2,2)./sum(F_e_interf.^2,2);
        % SAR
        varargout{3}=sum((F_s_target+F_e_interf).^2,2)./sum(F_e_artif.^2,2);

    case 4
        F_e_noise=varargin{1};
        F_e_artif=varargin{2};
        % SDR
        F_e_total=F_e_interf+F_e_noise+F_e_artif;
        varargout{1}=sum(F_s_target.^2,2)./sum(F_e_total.^2,2);
        % SIR
        varargout{2}=sum(F_s_target.^2,2)./sum(F_e_interf.^2,2);
        % SNR
        varargout{3}=sum((F_s_target+F_e_interf).^2,2)./sum(F_e_noise.^2,2);
        % SAR
        varargout{4}=sum((F_s_target+F_e_interf+F_e_noise).^2,2)./sum(F_e_artif.^2,2);
end


function [traj, infStates] = tapas_hgf(r, p, varargin)
% Calculates the trajectories of the agent's representations under the HGF
%
% This function can be called in two ways:
%
% (1) tapas_hgf(r, p)
%
%     where r is the structure generated by tapas_fitModel and p is the parameter vector in native space;
%
% (2) tapas_hgf(r, ptrans, 'trans')
%
%     where r is the structure generated by tapas_fitModel, ptrans is the parameter vector in
%     transformed space, and 'trans' is a flag indicating this.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform paramaters back to their native space if needed
if ~isempty(varargin) && strcmp(varargin{1},'trans');
    p = tapas_hgf_transp(r, p);
end

% Number of levels
l = length(p)/5;

if l ~= floor(l)
    error('tapas:hgf:UndetNumLevels', 'Cannot determine number of levels');
end

% Unpack parameters
mu_0 = p(1:l);
sa_0 = p(l+1:2*l);
rho  = p(2*l+1:3*l);
ka   = p(3*l+1:4*l-1);
om   = p(4*l:5*l-2);
th   = exp(p(5*l-1));
al   = 1/p(5*l);

% Add dummy "zeroth" trial
u = [0; r.u(:,1)];

% Number of trials (including prior)
n = length(u);

% Assume that if u has more than one column, the last contains t
try
    if r.c_prc.irregular_intervals
        if size(u,2) > 1
            t = [0; r.u(:,end)];
        else
            error('tapas:hgf:InputSingleColumn', 'Input matrix must contain more than one column if irregular_intervals is set to true.');
        end
    else
        t = ones(n,1);
    end
catch
    if size(u,2) > 1
        t = [0; r.u(:,end)];
    else
        t = ones(n,1);
    end
end

% Initialize updated quantities

% Representations
mu = NaN(n,l);
pi = NaN(n,l);

% Other quantities
muhat = NaN(n,l);
pihat = NaN(n,l);
v     = NaN(n,l);
w     = NaN(n,l-1);
da    = NaN(n,l);
dau   = NaN(n,1);

% Representation priors
% Note: first entries of the other quantities remain
% NaN because they are undefined and are thrown away
% at the end; their presence simply leads to consistent
% trial indices.
mu(1,:) = mu_0;
pi(1,:) = 1./sa_0;

% Representation update loop
% Pass through trials
for k = 2:1:n
    if not(ismember(k-1, r.ign))

        %%%%%%%%%%%%%%%%%%%%%%
        % Effect of input u(k)
        %%%%%%%%%%%%%%%%%%%%%%

        % 1st level
        % ~~~~~~~~~
        % Prediction
        muhat(k,1) = mu(k-1,1) +t(k) *rho(1);

        % Precision of prediction
        pihat(k,1) = 1/(1/pi(k-1,1) +t(k) *exp(ka(1) *mu(k-1,2) +om(1)));

        % Input prediction error
        dau(k) = u(k) -muhat(k,1);

        % Updates
        pi(k,1) = pihat(k,1) +1/al;
        mu(k,1) = muhat(k,1) +1/pihat(k,1) *1/(1/pihat(k,1) +al) *dau(k);

        % Volatility prediction error
        da(k,1) = (1/pi(k,1) +(mu(k,1) -muhat(k,1))^2) *pihat(k,1) -1;

        if l > 2
            % Pass through higher levels
            % ~~~~~~~~~~~~~~~~~~~~~~~~~~
            for j = 2:l-1
                % Prediction
                muhat(k,j) = mu(k-1,j) +t(k) *rho(j);

                % Precision of prediction
                pihat(k,j) = 1/(1/pi(k-1,j) +t(k) *exp(ka(j) *mu(k-1,j+1) +om(j)));

                % Weighting factor
                v(k,j-1) = t(k) *exp(ka(j-1) *mu(k-1,j) +om(j-1));
                w(k,j-1) = v(k,j-1) *pihat(k,j-1);

                % Updates
                pi(k,j) = pihat(k,j) +1/2 *ka(j-1)^2 *w(k,j-1) *(w(k,j-1) +(2 *w(k,j-1) -1) *da(k,j-1));

                if pi(k,j) <= 0
                    error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
                end

                mu(k,j) = muhat(k,j) +1/2 *1/pi(k,j) *ka(j-1) *w(k,j-1) *da(k,j-1);

                % Volatility prediction error
                da(k,j) = (1/pi(k,j) +(mu(k,j) -muhat(k,j))^2) *pihat(k,j) -1;
            end
        end

        % Last level
        % ~~~~~~~~~~
        % Prediction
        muhat(k,l) = mu(k-1,l) +t(k) *rho(l);

        % Precision of prediction
        pihat(k,l) = 1/(1/pi(k-1,l) +t(k) *th);

        % Weighting factor
        v(k,l)   = t(k) *th;
        v(k,l-1) = t(k) *exp(ka(l-1) *mu(k-1,l) +om(l-1));
        w(k,l-1) = v(k,l-1) *pihat(k,l-1);

        % Updates
        pi(k,l) = pihat(k,l) +1/2 *ka(l-1)^2 *w(k,l-1) *(w(k,l-1) +(2 *w(k,l-1) -1) *da(k,l-1));

        if pi(k,l) <= 0
            error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
        end

        mu(k,l) = muhat(k,l) +1/2 *1/pi(k,l) *ka(l-1) *w(k,l-1) *da(k,l-1);

        % Volatility prediction error
        da(k,l) = (1/pi(k,l) +(mu(k,l) -muhat(k,l))^2) *pihat(k,l) -1;
    else

        mu(k,:) = mu(k-1,:);
        pi(k,:) = pi(k-1,:);

        muhat(k,:) = muhat(k-1,:);
        pihat(k,:) = pihat(k-1,:);

        v(k,:)  = v(k-1,:);
        w(k,:)  = w(k-1,:);
        da(k,:) = da(k-1,:);

    end
end

% Remove representation priors
mu(1,:)  = [];
pi(1,:)  = [];

% Check validity of trajectories
if any(isnan(mu(:))) || any(isnan(pi(:)))
    error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
else
    % Check for implausible jumps in trajectories
    dmu = diff(mu);
    dpi = diff(pi);
    rmdmu = repmat(sqrt(mean(dmu.^2)),length(dmu),1);
    rmdpi = repmat(sqrt(mean(dpi.^2)),length(dpi),1);

    jumpTol = 256;
    if any(abs(dmu(:)) > jumpTol*rmdmu(:)) || any(abs(dpi(:)) > jumpTol*rmdpi(:))
        error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
    end
end

% Remove other dummy initial values
muhat(1,:) = [];
pihat(1,:) = [];
v(1,:)     = [];
w(1,:)     = [];
da(1,:)    = [];
dau(1)     = [];

% Create result data structure
traj = struct;

traj.mu     = mu;
traj.sa     = 1./pi;

traj.muhat  = muhat;
traj.sahat  = 1./pihat;

traj.v      = v;
traj.w      = w;
traj.da     = da;
traj.dau    = dau;

% Updates with respect to prediction
traj.ud = mu -muhat;

% Psi (precision weights on prediction errors)
psi        = NaN(n-1,l);
psi(:,1)   = 1./(al*pi(:,1));
psi(:,2:l) = pihat(:,1:l-1)./pi(:,2:l);
traj.psi   = psi;

% Epsilons (precision-weighted prediction errors)
epsi        = NaN(n-1,l);
epsi(:,1)   = psi(:,1) .*dau;
epsi(:,2:l) = psi(:,2:l) .*da(:,1:l-1);
traj.epsi   = epsi;

% Full learning rate (full weights on prediction errors)
wt        = NaN(n-1,l);
wt(:,1)   = psi(:,1);
wt(:,2:l) = 1/2 *(v(:,1:l-1) *diag(ka(1:l-1))) .*psi(:,2:l);
traj.wt   = wt;

% Create matrices for use by the observation model
infStates = NaN(n-1,l,4);
infStates(:,:,1) = traj.muhat;
infStates(:,:,2) = traj.sahat;
infStates(:,:,3) = traj.mu;
infStates(:,:,4) = traj.sa;

return;


%compute tail-inflection direction and correct trx if necessary
function [data,units]=compute_tailinflang(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
tailinflang=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
     tailinflang{1,i}=bsxfun(@atan2,trx(larva).yinflection_mm-trx(larva).ytail_mm,trx(larva).xinflection_mm-trx(larva).xtail_mm);

end
units=parseunits('rad');
data=tailinflang;


function out = dbv(in)

out = 20 * log10(abs(in));


function [ortho_mask_IND,ortho_mask_I,ortho_mask_J,ortho_mask_K]=oblique_mask(V_ort_vec,IND_mask,mask_height,siz,v,opt)

[I_mask,J_mask,K_mask] = ind2sub(siz,IND_mask);

%Factor for number of points across mask
f=1+(2.*abs(sqrt((V_ort_vec(:,1)-V_ort_vec(:,2)).^2+(V_ort_vec(:,2)-V_ort_vec(:,3)).^2+(V_ort_vec(:,1)-V_ort_vec(:,3)).^2)-sqrt(2))./sqrt(2));
f=max(f(:));

%Defining orthogonal neighbourhoods
if size(V_ort_vec,1)~=numel(IND_mask)
    V_ort_vec=ones(numel(IND_mask),1)*V_ort_vec;
end
V_ort_vec=V_ort_vec.*mask_height; %Lengthen orthogonal vector by mask_height
V_ort_vec_X=V_ort_vec(:,1); V_ort_vec_Y=V_ort_vec(:,2); V_ort_vec_Z=V_ort_vec(:,3);

%Creating mask points in cartesian coordinates
n_ort=ceil(f.*((2.*mask_height)./min(v))); %Number of points from -vector to +vector
n_ort=n_ort+iseven(n_ort); %Ensure its uneven so middle element will be self
ortho_mask_X=linspacen(-V_ort_vec_X,V_ort_vec_X,n_ort);
ortho_mask_Y=linspacen(-V_ort_vec_Y,V_ort_vec_Y,n_ort);
ortho_mask_Z=linspacen(-V_ort_vec_Z,V_ort_vec_Z,n_ort);

%Creating mask points in image coordinates
ortho_mask_I=round(ortho_mask_Y./v(1))+I_mask*ones(1,size(ortho_mask_Y,2));
ortho_mask_J=round(ortho_mask_X./v(2))+J_mask*ones(1,size(ortho_mask_Y,2));
ortho_mask_K=round(ortho_mask_Z./v(3))+K_mask*ones(1,size(ortho_mask_Y,2));

%Removing out of bound indices
L_not_valid= (ortho_mask_I<1 | ortho_mask_I>siz(1)) | ...
             (ortho_mask_J<1 | ortho_mask_J>siz(2)) |...
             (ortho_mask_K<1 | ortho_mask_K>siz(3));
ortho_mask_IND=NaN(size(L_not_valid));
ortho_mask_ind = sub2ind(siz,ortho_mask_I(~L_not_valid),ortho_mask_J(~L_not_valid),ortho_mask_K(~L_not_valid));
ortho_mask_IND(~L_not_valid)=ortho_mask_ind;

%Removing doubles
[ortho_mask_IND_sort,J_sort] = sort(ortho_mask_IND,2); %Sorting to prepare for diff
[j_sort,J_sort_inv] = sort(J_sort,2); clear j_sort; %Determine inverse of sorting
I_sort=(1:size(ortho_mask_IND,1))'*ones(1,size(ortho_mask_IND,2));
IND_sort_inv=sub2ind(size(ortho_mask_IND),I_sort,J_sort_inv); %Inverse indices
L_double=[ones(size(ortho_mask_IND_sort,1),1) diff(ortho_mask_IND_sort,1,2)]==0; %Find doubles
ortho_mask_IND_sort(L_double)=NaN; %Set doubles to NaN
ortho_mask_IND=ortho_mask_IND_sort(IND_sort_inv); %Fixing order

%Ensuring only centre element is self
L_self=(ortho_mask_IND-(IND_mask*ones(1,size(ortho_mask_IND,2))))==0;
ortho_mask_IND(L_self)=NaN;
ortho_mask_IND(:,round(size(ortho_mask_IND,2)./2))=IND_mask;

if opt==1
    %Removing non-mask elements
    L_mask=ismembc(ortho_mask_IND,sort(IND_mask));
    ortho_mask_IND(~L_mask)=NaN;
    ortho_mask_I(~L_mask)=NaN;
    ortho_mask_J(~L_mask)=NaN;
    ortho_mask_K(~L_mask)=NaN;
end

end

%%
% _*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/>.


classdef SMEA < ALGORITHM
% <multi> <real/integer>
% Self-organizing multiobjective evolutionary algorithm
% D    ---     --- Number of neurons in each dimension of the latent space
% tau0 --- 0.7 --- Initial learning rate
% H    ---   5 --- Size of neighborhood mating pools

%------------------------------- Reference --------------------------------
% H. Zhang, A. Zhou, S. Song, Q. Zhang, X. Gao, and J. Zhang, A self-
% organizing multiobjective evolutionary algorithm, IEEE Transactions on
% Evolutionary Computation, 2016, 20(5): 792-806.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [D,tau0,H] = Algorithm.ParameterSet(repmat(ceil(Problem.N.^(1/(Problem.M-1))),1,Problem.M-1),0.7,5);
            Problem.N  = prod(D);
            sigma0     = sqrt(sum(D.^2)/(Problem.M-1))/2;

            %% Generate random population
            Population = Problem.Initialization();
            FrontNo    = NDSort(Population.objs,inf);

            %% Initialize the SOM
            % Training set
            S = Population.decs;
            % Weight vector of each neuron
            W = S;
            % Position of each neuron
            D = arrayfun(@(S)1:S,D,'UniformOutput',false);
            eval(sprintf('[%s]=ndgrid(D{:});',sprintf('c%d,',1:length(D))))
            eval(sprintf('Z=[%s];',sprintf('c%d(:),',1:length(D))))
            % Distance between each two neurons in latent space
            LDis = pdist2(Z,Z);
            % H nearest neurons of each neuron in latent space
            [~,B] = sort(LDis,2);
            B     = B(:,2:min(H+1,end));

            %% Optimization
            while Algorithm.NotTerminated(Population)
                % Update SOM
                for s = 1 : size(S,1)
                    sigma  = sigma0*(1-(Problem.FE+s)/Problem.maxFE);
                    tau    = tau0*(1-(Problem.FE+s)/Problem.maxFE);
                    [~,u1] = min(pdist2(S(s,:),W));
                    U      = LDis(u1,:) < sigma;
                    W(U,:) = W(U,:) + tau.*repmat(exp(-LDis(u1,U))',1,size(W,2)).*(repmat(S(s,:),sum(U),1)-W(U,:));
                end

                % Associate each solution with a neuron
                A  = 1 : Problem.N;
                U  = 1 : Problem.N;
                XU = zeros(1,Problem.N);
                for i = 1 : Problem.N
                    x        = randi(length(A));
                    [~,u]    = min(pdist2(Population(A(x)).dec,W(U,:)));
                    XU(U(u)) = A(x);
                    A(x)     = [];
                    U(u)     = [];
                end

                % Evolution
                A = Population.decs;
                for u = 1 : Problem.N
                    drawnow('limitrate');
                    if rand < 0.9
                        Q = XU(B(u,:));
                    else
                        Q = 1 : Problem.N;
                    end
                    Q = Q(randperm(end,2));
                    y = OperatorDE(Problem,Population(u),Population(Q(1)),Population(Q(2)));
                    [Population,FrontNo] = Select(Population,FrontNo,y);
                end

                % Update the training set
                S = setdiff(Population.decs,A,'rows');
            end
        end
    end
end


function f = example(varargin)
% Construct the odf from the dubna data set as example for an SO3FunRBF.

CS = crystalSymmetry('-3m',[4.9 4.9 5.4]);
SS = specimenSymmetry;

% specify file names
fname = {...
  fullfile(mtexDataPath,'PoleFigure','dubna','Q(10-10)_amp.cnv'),...
  fullfile(mtexDataPath,'PoleFigure','dubna','Q(10-11)(01-11)_amp.cnv'),...
  fullfile(mtexDataPath,'PoleFigure','dubna','Q(11-22)_amp.cnv')};

% specify crystal directions
h = {Miller(1,0,-1,0,CS),...
     [Miller(0,1,-1,1,CS),Miller(1,0,-1,1,CS)],... % superposed pole figures
     Miller(1,1,-2,2,CS)};

% specify structure coefficients
c = {1,[0.52 ,1.23],1};

% import data
pf = PoleFigure.load(fname,h,CS,SS,'interface','dubna','superposition',c);

[~,f] = evalc('calcODF(pf)');

end


function obj = SIPEVD_wbmethod(obj)

global H W_mopt Nk Nt Nr  Ns Vn ifVFD  n;
t1 = clock;
j = 0;
w = zeros(Nk,1);
v = zeros(Nk,1);
%init
if (ifVFD)
    W_equal = W_mopt;
else
    W_equal = exp( 1i*unifrnd(0,2*pi,Nr,Ns,Nk) );
end
V_equal = zeros(Nt,Ns,Nk);
H_equal = zeros(Ns,Ns,Nk);
m_mse = zeros(Nk,1);
for i = 1:Nk
    w(i) = trace(W_equal(:,:,i)'*W_equal(:,:,i));
end

H1 = zeros(Nt,Ns,Nk);
H2 = zeros(Nr,Ns,Nk);
trigger = 1;
m_MSE_new = 100;

%limit the iterations number by i<10
while (trigger > 1e-5 && j<10)

    Vn1 = Vn * w;
    for i = 1: Nk
        H1(:,:,i) = H(:,:,i)'*W_equal(:,:,i);
    end
    [V_RF,V_U] = SIPEVD_method(H1,Vn1);

    for i = 1:Nk
        V_equal(:,:,i) = V_RF * V_U(:,:,i);
        v(i) = trace(V_equal(:,:,i)'*V_equal(:,:,i));
        H2(:,:,i) = H(:,:,i)*V_equal(:,:,i);
    end
    Vn2 = Vn * v;
    [W_RF,W_B] = SIPEVD_method(H2,Vn2);

    m_MSE_old = m_MSE_new;

    for k = 1:Nk
        W_equal(:,:,k) = W_RF * W_B(:,:,k);
        w(k) = trace(W_equal(:,:,k)'*W_equal(:,:,k));
        H_equal(:,:,k) = W_equal(:,:,k)'*H2(:,:,k);
        m_mse(k) = trace(H_equal(:,:,k) * H_equal(:,:,k)' - H_equal(:,:,k) - H_equal(:,:,k)')...
            + Vn * v(k) * w(k);
    end
    m_MSE_new = sum(m_mse)/Nk;
    trigger = m_MSE_old - m_MSE_new;
    j = j + 1;
    obj.modmse(j,n) = m_MSE_new + Ns;
end

for i = 1:Nk
    V_B(:,:,i)= V_U(:,:,i) /sqrt(v(i));
end

t2 = clock;
runtime  = etime(t2,t1);
obj.V_B = V_B;
obj.W_B = W_B;
obj.V_RF = V_RF;
obj.W_RF = W_RF;
obj.runtime = obj.runtime + runtime;
obj.iter = obj.iter + j;
obj = get_wbmetric(obj);


function gX = ardKernDiagGradX(kern, X)

% ARDKERNDIAGGRADX Gradient of ARD kernel's diagonal with respect to X.
% FORMAT
% DESC computes the gradient of the diagonal of the pre-built RBF and linear ARD kernel matrix with
% respect to the elements of the design matrix given in X.
% ARG kern : the kernel structure for which gradients are being computed.
% ARG X : the input data in the form of a design matrix.
% RETURN gX : the gradients of the diagonal with respect to each element
% of X. The returned matrix has the same dimensions as X.
%
% SEEALSO : ardKernParamInit, kernDiagGradX, ardkernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2004

% KERN

gX = 2*kern.linearVariance*X.*repmat(kern.inputScales, [size(X, 1) 1]);