%% Copyright (C) 2014-2022 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym erf (@var{x})
%% Symbolic erf function.
%%
%% Example:
%% @example
%% @group
%% syms x
%% y = erf (x)
%%   @result{} y = (sym) erf(x)
%% @end group
%% @end example
%%
%% Note: this file is autogenerated: if you want to edit it, you might
%% want to make changes to 'generate_functions.py' instead.
%%
%% @end defmethod

function y = erf(x)
  if (nargin ~= 1)
    print_usage ();
  end
  y = elementwise_op ('erf', x);
end

%!error erf (sym(1), 2)
%!assert (isequaln (erf (sym(nan)), sym(nan)))

%!shared x, d
%! d = 1;
%! x = sym('1');

%!test
%! f1 = erf(x);
%! f2 = erf(d);
%! assert( abs(double(f1) - f2) < 1e-15 )

%!test
%! D = [d d; d d];
%! A = [x x; x x];
%! f1 = erf(A);
%! f2 = erf(D);
%! assert( all(all( abs(double(f1) - f2) < 1e-15 )))

%!test
%! % round trip
%! y = sym('y');
%! A = erf (d);
%! f = erf (y);
%! h = function_handle (f);
%! B = h (d);
%! assert (A, B, -eps)


function triangle_to_fem ( prefix )

%*****************************************************************************80
%
%% MAIN is the main program for TRIANGLE_TO_FEM.
%
%  Discussion:
%
%    TRIANGLE_TO_FEM converts mesh data from TRIANGLE format to FEM format.
%
%  Usage:
%
%    triangle_to_fem prefix
%
%    where 'prefix' is the common filename prefix:
%
%    * 'prefix'.node contains the triangle node coordinates,
%    * 'prefix'.ele contains the triangle element node connectivity.
%    * 'prefix'_nodes.txt will contain the node coordinates.
%    * 'prefix'_elements.txt will contain the element node connectivity.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2014
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGLE_TO_FEM\n' );
  fprintf ( 1, '  MATLAB version:\n' );
  fprintf ( 1, '  Read a mesh description created by TRIANGLE:\n' );
  fprintf ( 1, '  * "prefix".node, node coordinates.\n' );
  fprintf ( 1, '  * "prefix".ele, element connectivity.\n' );
  fprintf ( 1, '  Write two simple FEM files listing nodes and elements.\n' );
  fprintf ( 1, '  * "prefix"_nodes.txt, node coordinates.\n' );
  fprintf ( 1, '  * "prefix"_elements.txt, element connectivity.\n' );
%
%  Get the filename prefix.
%
  if ( nargin < 1 )

    prefix = input ( 'Enter the filename prefix:  ' );

  end
%
%  Create the filenames.
%
  triangle_node_filename = strcat ( prefix, '.node' );
  triangle_element_filename = strcat ( prefix, '.ele' );
  fem_node_filename = strcat ( prefix, '_nodes.txt' );
  fem_element_filename = strcat ( prefix, '_elements.txt' );
%
%  Read the triangle node size information.
%
  [ node_num, m, node_att_num, node_marker_num ] = node_size_read ( ...
    triangle_node_filename );
%
%  Read the triangle element size information.
%
  [ element_num, element_order, element_att_num ] = element_size_read ( ...
    triangle_element_filename );
%
%  Report sizes.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Size information from TRIANGLE files:\n' );
  fprintf ( 1, '  Spatial dimension M = %d\n', m );
  fprintf ( 1, '  Number of nodes NODE_NUM = %d\n', node_num );
  fprintf ( 1, '  NODE_ATT_NUM = %d\n', node_att_num );
  fprintf ( 1, '  NODE_MARKER_NUM = %d\n', node_marker_num );
  fprintf ( 1, '  Number of elements ELEMENT_NUM = %d\n', element_num );
  fprintf ( 1, '  Element order ELEMENT_ORDER = %d\n', element_order );
  fprintf ( 1, '  ELEMENT_ATT_NUM = %d\n', element_att_num );
%
%  Read data.
%
  [ node_x, node_att, node_marker ] = node_data_read ( ...
    triangle_node_filename, node_num, m, node_att_num, node_marker_num );

  [ element_node, element_att ] = element_data_read ( ...
    triangle_element_filename, element_num, element_order, element_att_num );
%
%  Write data.
%
  r8mat_write ( fem_node_filename, m, node_num, node_x );

  i4mat_write ( fem_element_filename, element_order, element_num, ...
    element_node );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGLE_TO_FEM:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function [ element_node, element_att ] = element_data_read ( ...
  element_filename, element_num, element_order, element_att_num )

%*****************************************************************************80
%
%% ELEMENT_DATA_READ reads the data from an element file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string ELEMENT_FILENAME, the name of the file.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer ELEMENT_ATT_NUM, number of element attributes
%    listed on each node record.
%
%    Output, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM),
%    the indices of the nodes that make up each element.
%
%    Output, real ELEMENT_ATT(ELEMENT_ATT_NUM,ELEMENT_NUM), the
%    attributes of each element.
%
  element = 0;

  input = fopen ( element_filename, 'rt' );

  if ( input < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ELEMENT_DATA_READ - Fatal error!\n' );
    fprintf ( 1, '  Unable to open file.\n' );
    error ( 'ELEMENT_DATA_READ - Fatal error!' );
  end
%
%  Create format.
%
  format = '%d';
  for i = 1 : element_order
    format = strcat ( format, '%d' );
  end
  for i = 1 : element_att_num
    format = strcat ( format, '%g' );
  end
%
%  Create arrays.
%
  element_node = zeros ( element_order, element_num );
  element_att = zeros ( element_att_num, element_num );
%
%  Read values and place in the arrays.
%
  while ( 1 )

    text = fgets ( input );

    if ( text == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'ELEMENT_DATA_READ - Fatal error!\n' );
      fprintf ( 1, '  Unexpected end of file while reading.\n' );
      error ( 'ELEMENT_DATA_READ - Fatal error!' );
    end

    if ( s_len_trim ( text ) == 0 )
      continue
    end

    if ( text(1) == '#' )
      continue
    end
%
%  Ignore the dimension line.
%
    if ( element == 0 )

    else

      [ value, count ] = sscanf ( text, format );

      if ( count < 1 + element_order + element_att_num )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'ELEMENT_DATA_READ - Fatal error!\n' );
        fprintf ( 1, '  Line did not have enough data.\n' );
        error ( 'ELEMENT_DATA_READ - Fatal error!' );
      end

      j = 1;

      for i = 1 : element_order
        j = j + 1;
        element_node(i,element) = value(j);
      end

      for i = 1 : element_att_num
        element_att(i,element) = value(j);
      end

    end

    element = element + 1;

    if ( element_num < element )
      break
    end

  end

  fclose ( input );

  return
end
function [ element_num, element_order, element_att_num ] = ...
  element_size_read ( element_filename )

%*****************************************************************************80
%
%% ELEMENT_SIZE_READ reads the header information from an element file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string ELEMENT_FILENAME, the name of the
%    element file.
%
%    Output, integer ELEMENT_NUM, the number of elements.
%
%    Output, integer ELEMENT_ORDER, the order of the elements.
%
%    Output, integer ELEMENT_ATT_NUM, the number of
%    element attributes.
%
  input = fopen ( element_filename, 'rt' );

  if ( input < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ELEMENT_SIZE_READ - Fatal error!\n' );
    fprintf ( 1, '  Unable to open file.\n' );
    error ( 'ELEMENT_SIZE_READ - Fatal error!' );
  end

  while ( 1 )

    text = fgets ( input );

    if ( text == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'ELEMENT_SIZE_READ - Fatal error!\n' );
      fprintf ( 1, '  Unexpected end of file while reading.\n' );
      error ( 'ELEMENT_SIZE_READ - Fatal error!' );
    end

    if ( s_len_trim ( text ) == 0 )
      continue
    end

    if ( text(1) == '#' )
      continue
    end

    [ value, count ] = sscanf ( text, '%d%d%d%d' );

    if ( count < 3 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'ELEMENT_SIZE_READ - Fatal error!\n' );
      fprintf ( 1, '  Header line did not have 3 value integers.\n' );
      error ( 'ELEMENT_SIZE_READ - Fatal error!' );
    end

    element_num = value(1);
    element_order = value(2);
    element_att_num = value(3);

    break

  end

  fclose ( input );

  return
end
function i4mat_write ( output_filename, m, n, table )

%*****************************************************************************80
%
%% I4MAT_WRITE writes an I4MAT file.
%
%  Discussion:
%
%    An I4MAT is an array of I4's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 June 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the output filename.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, integer TABLE(M,N), the points.
%

%
%  Open the file.
%
  output_unit = fopen ( output_filename, 'wt' );

  if ( output_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_WRITE - Error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'I4MAT_WRITE - Error!' );
  end
%
%  Write the data.
%
  for j = 1 : n
    for i = 1 : m
      fprintf ( output_unit, '  %d', round ( table(i,j) ) );
    end
    fprintf ( output_unit, '\n' );
  end
%
%  Close the file.
%
  fclose ( output_unit );

  return
end
function [ node_coord, node_att, node_marker ] = node_data_read ( ...
  node_filename, node_num, node_dim, node_att_num, node_marker_num )

%*****************************************************************************80
%
%% NODE_DATA_READ reads the data from a node file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2014
%
%  Author:
%
%    John Burkardt
%
% Parameters:
%
%   Input, string NODE_FILENAME, the name of the node file.
%
%   Input, integer NODE_NUM, the number of nodes.
%
%   Input, integer NODE_DIM, the spatial dimension.
%
%   Input, integer NODE_ATT_NUM, number of node attributes
%   listed on each node record.
%
%   Input, integer NODE_MARKER_NUM, 1 if every node record
%   includes a final boundary marker value.
%
%   Output, real NODE_COORD(NODE_DIM,NODE_NUM), the nodal
%   coordinates.
%
%   Output, real NODE_ATT(NODE_ATT_NUM,NODE_NUM), the nodal
%   attributes.
%
%   Output, integer NODE_MARKER(NODE_MARKER_NUM,NODE_NUM), the
%   node markers.
%
  node = 0;

  input = fopen ( node_filename, 'rt' );

  if ( input < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'NODE_DATA_READ - Fatal error!\n' );
    fprintf ( 1, '  Unable to open file.\n' );
    error ( 'NODE_DATA_READ - Fatal error!' );
  end
%
%  Create format.
%
  format = '%d';
  for i = 1 : node_dim
    format = strcat ( format, '%g' );
  end
  for i = 1 : node_att_num
    format = strcat ( format, '%g' );
  end
  for i = 1 : node_marker_num
    format = strcat ( format, '%d' );
  end
%
%  Create arrays.
%
  node_coord = zeros ( node_dim, node_num );
  node_att = zeros ( node_att_num, node_num );
  node_marker = zeros ( node_marker_num, node_num );
%
%  Read values and place in the arrays.
%
  while ( 1 )

    text = fgets ( input );

    if ( text == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'NODE_DATA_READ - Fatal error!\n' );
      fprintf ( 1, '  Unexpected end of file while reading.\n' );
      error ( 'NODE_DATA_READ - Fatal error!' );
    end

    if ( s_len_trim ( text ) == 0 )
      continue
    end

    if ( text(1) == '#' )
      continue
    end
%
%  Ignore the dimension line.
%
    if ( node == 0 )

    else

      [ value, count ] = sscanf ( text, format );

      if ( count < 1 + node_dim + node_att_num + node_marker_num )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'NODE_DATA_READ - Fatal error!\n' );
        fprintf ( 1, '  Line did not have enough data.\n' );
        error ( 'NODE_DATA_READ - Fatal error!' );
      end

      j = 1;

      for i = 1 : node_dim
        j = j + 1;
        node_coord(i,node) = value(j);
      end

      for i = 1 : node_att_num
        j = j + 1;
        node_att(i,node) = value(j);
      end

      for i = 1 : node_marker_num
        j = j + 1;
        node_marker(i,node) = value(j);
      end

    end

    node = node + 1;

    if ( node_num < node )
      break
    end

  end

  fclose ( input );

  return
end
function [ node_num, node_dim, node_att_num, node_marker_num ] = ...
  node_size_read ( node_filename )

%*****************************************************************************80
%
%% NODE_SIZE_READ reads the header information from a node file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string NODE_FILENAME, the name of the node file.
%
%    Output, integer NODE_NUM, the number of nodes.
%
%    Output, integer NODE_DIM, the spatial dimension.
%
%    Output, integer NODE_ATT_NUM, number of node attributes
%    listed on each node record.
%
%    Output, integer NODE_MARKER_NUM, 1 if every node record
%    includes a final boundary marker value.
%
  input = fopen ( node_filename, 'rt' );

  if ( input < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'NODE_SIZE_READ - Fatal error!\n' );
    fprintf ( 1, '  Unable to open file.\n' );
    error ( 'NODE_SIZE_READ - Fatal error!' );
  end

  while ( 1 )

    text = fgets ( input );

    if ( text == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'NODE_SIZE_READ - Fatal error!\n' );
      fprintf ( 1, '  Unexpected end of file while reading.\n' );
      error ( 'NODE_SIZE_READ - Fatal error!' );
    end

    if ( s_len_trim ( text ) == 0 )
      continue
    end

    if ( text(1) == '#' )
      continue
    end

    [ value, count ] = sscanf ( text, '%d%d%d%d' );

    if ( count < 4 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'NODE_SIZE_READ - Fatal error!\n' );
      fprintf ( 1, '  Header line did not have 4 value integers.\n' );
      error ( 'NODE_SIZE_READ - Fatal error!' );
    end

    node_num = value(1);
    node_dim = value(2);
    node_att_num = value(3);
    node_marker_num = value(4);

    break

  end

  fclose ( input );

  return
end
function r8mat_write ( output_filename, m, n, table )

%*****************************************************************************80
%
%% R8MAT_WRITE writes an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the output filename.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real TABLE(M,N), the points.
%

%
%  Open the file.
%
  output_unit = fopen ( output_filename, 'wt' );

  if ( output_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_WRITE - Error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'R8MAT_WRITE - Error!' );
  end
%
%  Write the data.
%
%  Alternative print statements include:
%
%     fprintf ( output_unit, '  %24.16e', table(i,j) );
%     fprintf ( output_unit, '  %14.6e', table(i,j) );
%
  for j = 1 : n
    for i = 1 : m
      fprintf ( output_unit, '  %g', table(i,j) );
    end
    fprintf ( output_unit, '\n' );
  end
%
%  Close the file.
%
  fclose ( output_unit );

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LENGTH, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


%%%  PosControl_Sim
clear
path('./icon/',path);
Init;

%Constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
%throttle when UAV is hovering
THR_HOVER = 0.609;
%% Initial condition
ModelInit_PosE = [0, 0, -100];
ModelInit_VelB = [0, 0, 0];
ModelInit_AngEuler = [0, 0, 0];
ModelInit_RateB = [0, 0, 0];
ModelInit_Rads = 0;
%% control parameter
%Attitude PID parameters
Kp_PITCH_ANGLE = 6.5;
Kp_PITCH_AngleRate = 0.1;
Ki_PITCH_AngleRate = 0.02;
Kd_PITCH_AngleRate = 0.001;
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;

Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;
%Position PID parameters
Kpxp = 1.0;
Kpyp = 1.0;
Kpzp = 4.0;
Kvxp = 2.5; Kvxi = 0.4; Kvxd = 0.01;
Kvyp = 2.5; Kvyi = 0.4; Kvyd = 0.01;
Kvzp = 0.45; Kvzi = 0.01; Kvzd = 0.005;
%integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;
Saturation_I_az = 5;

%max control angle,default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
%max control angle rate,rad/s
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
%Maximum control speed, m/s
MAX_CONTROL_VELOCITY_XY = 5;
MAX_CONTROL_VELOCITY_Z = 3;
%Throttle amplitude
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;
%% run simulink model
PosControl_Sim


% dim_red_type: dimension reduction type: one of the strings: '', 'pca', 'ica'
% new_dim: relevant only if dim_red_type is not ''
function hglmm(numClusters, is_sampled, dim_red_type, new_dim, numOfEMIter, numOfCpus)

    fv_init;

    if ~exist('numOfEMIter', 'var')
        numOfEMIter = 10;
    end

    if ~exist('numOfCpus', 'var')
        numOfCpus = 4;
    end

    word2vec_sqrt = false;

    mult_factor = LMM_HGLMM_MULT_FACTOR;

    output(1, 'LMM_HGLMM_MULT_FACTOR = %d\n', LMM_HGLMM_MULT_FACTOR);

    vectors_file_name = add_data_dir_base('GoogleNews_vectors_norm.mat');
    output_file_name = add_data_dir_base(sprintf('GoogleNews_norm_hglmm_%d.mat', numClusters));

    if word2vec_sqrt
        vectors_file_name = fname_concat(vectors_file_name, '_sqrt');
        output_file_name = fname_concat(output_file_name, '_sqrt');
    end

    if is_sampled
        vectors_file_name = fname_concat(vectors_file_name, '_sampled');
        output_file_name = fname_concat(output_file_name, '_sampled');
    end

    switch dim_red_type
        case {'pca', 'ica', 'wht'}
            post_fix = sprintf('_%s_%d', dim_red_type, new_dim);
            vectors_file_name = fname_concat(vectors_file_name, post_fix);
            output_file_name = fname_concat(output_file_name, post_fix);
    end

    output(1, 'reading vectors file %s\n', vectors_file_name);
    load(vectors_file_name);
    output(1, 'done\n');

    output(1, 'Calculating HGLMM, %d clusters\n', numClusters);

    % multiply by LMM_HGLMM_MULT_FACTOR=10 due to numerical issue
    % transpose because the rows should be the samples
    vectors_ = mult_factor * vectors_';

    % calculate HGLMM

    output(1, 'numOfCpus = %d\n', numOfCpus);
    output(1, 'numOfEMIter = %d\n', numOfEMIter);

    [m_out, s_out, mu_out, sigma_out, b_out, prior_out, samplesWeightsOut ]=HybridEM(vectors_, numClusters, numOfEMIter, numOfCpus);

    save(output_file_name, 'm_out', 's_out', 'mu_out', 'sigma_out', 'b_out', 'prior_out', 'mult_factor', '-v7.3');

    output(1, 'saved to file: %s\n', output_file_name);

    num_lmm = sum(sum(b_out));
    [rows, cols] = size(b_out);
    lmm_fraction = num_lmm / ( rows*cols );
    output(1, 'num_lmm: %d\n', num_lmm);
    output(1, 'lmm_fraction: %.3f\n', lmm_fraction);

end


function test_minkowski(P, closeAll)
if nargin<2
    closeAll=false;
end
if closeAll
    close all
end
tmplt=['ust_s1_samusikImported_minkowski_' String.encodeBank(P) '_29D_15nn_3D.mat'];
run_umap('s1_samusikImported_29D.csv', 'label_column', 'end', ...
    'label_file', 's1_29D.properties', ...
    'n_components', 3, ...
    'save_template_file', tmplt, ...
    'metric', 'minkowski', ...
    'dist_args', P);
run_umap('s2_samusikImported_29D.csv', ...
    'template_file', tmplt, 'label_column', 'end', ...
    'label_file', 's2_samusikImported_29D.properties', ...
    'match_scenarios', 4,  ...
    'see_training', true, ...
    'match_table_fig', false, ...
    'match_histogram_fig', false, ...
    'false_positive_negative_plot', true, 'match_supervisors', 3);


function varargout = contourf( v, data, varargin )
% spherical filled contour plot
%
% Syntax
%   contourf(v,data)
%
% Input
%  v - @vector3d
%  data - double
%
% Options
%  contours - number of contours
%
% See also
% vector3d/plot vector3d/contour

if nargin == 1, data = []; end
if ischar(data)
  varargin = [data,varargin];
  data = [];
end

% in older matlab version we have to plot contour and countour lines
% seperately to avoid artefacts
if verLessThan('matlab','8.5')

  [varargout{1:nargout}] = v.smooth(data,'contours',10,'LineStyle','none',varargin{:});

  v.smooth(data,'contours',10,'fill','off','LineStyle','-','LineColor','k','hold',varargin{:});
else
  [varargout{1:nargout}] = v.smooth(data,'contours',10,'LineStyle','-','LineColor','k',varargin{:});
end

if nargout == 0, clear h; end

% TODO: data may not set


function [Y,skel, channels] = loadMocapData()

%YA = vargplvmLoadData('hierarchical/demHighFiveHgplvm1',[],[],'YA');
%{
curDir = pwd;
cd ../../../vargplvmDEPENDENCIES/DATASETS0p1371/mocap/cmu/02/
fileNameAsf='02.asf';
fileNameAmc='02_05.amc';
skel = acclaimReadSkel(fileNameAsf);
[channels, skel] = acclaimLoadChannels(fileNameAmc, skel);
% Remove root node?
channels(:, [1 3]) = zeros(size(channels, 1), 2);
skelPlayData(skel, channels, 1/40);
cd(curDir)
%}
%%
clear
close all
[Y, lbls, Ytest, lblstest,skel] = lvmLoadData2('cmuXNoRoot', '02', {'01','05','10'});
Yorig = Y;
% REmove motion in xz?

seq = cumsum(sum(lbls)) - [1:31];
% Ywalk = Y(1:3:70,:); % Orig: Y(1:85,:);
% Ypunch = Y(86:13:548,:); % Orig: Y(86:548,:);
% Ywash = Y(549:20:1209,:); % Orig:  Y(549:1209,:);
Ywalk = Y(1:2:70,:); % Orig: Y(1:85,:);
Ypunch = Y(86:13:548,:); % Orig: Y(86:548,:);
Ywash = Y(830:6:1140,:); % Orig:  Y(549:1209,:);

[channels xyzDiffIndices] = skelGetChannels(Ywalk);
Ywalk(:, xyzDiffIndices) = zeros(size(Ywalk(:, xyzDiffIndices) ));
[channels xyzDiffIndices] = skelGetChannels(Ypunch);
Ypunch(:, xyzDiffIndices) = zeros(size(Ypunch(:, xyzDiffIndices) ));
[channels xyzDiffIndices] = skelGetChannels(Ywash);
Ywash(:, xyzDiffIndices) = zeros(size(Ywash(:, xyzDiffIndices) ));

Y = [Ywalk; Ywash];
[channels] = skelGetChannels(Y);
close; skelPlayData(skel, channels, 1/5);

%%
%{
[channelsWalk] = skelGetChannels(Ywalk);
[channelsPunch] = skelGetChannels(Ypunch);
[channelsWash] = skelGetChannels(Ywash);
close; skelPlayData(skel, channelsWalk, 1/20);
close; skelPlayData(skel, channelsPunch, 1/20);
close; skelPlayData(skel, channelsWash, 1/20);
%}

%{
try
    load '../cmu13Data.mat'
catch
    [Y, lbls] = lvmLoadData2('cmu13');
    seq = cumsum(sum(lbls)) - [1:31];

    % load data
    [Y, lbls, Ytest, lblstest] = lvmLoadData2('cmu13NoRoot');
    skel = acclaimReadSkel('../../../vargplvmDEPENDENCIES/DATASETS0p1371/mocap/cmu/13/13.asf');
    % (I think) any motion of the specific subject would do here, just to get
    % the channels
    [tmpchan, skel] = acclaimLoadChannels('../../../vargplvmDEPENDENCIES/DATASETS0p1371/mocap/cmu/13/13_13.amc', skel);
end
%}

%{
See cmu49 for more coherent motions
try
    load '../cmu13Data.mat'
catch
    [Y, lbls] = lvmLoadData2('cmu13');
    seq = cumsum(sum(lbls)) - [1:31];

    % load data
    [Y, lbls, Ytest, lblstest] = lvmLoadData2('cmu13NoRoot');
    skel = acclaimReadSkel('../../../vargplvmDEPENDENCIES/DATASETS0p1371/mocap/cmu/13/13.asf');
    % (I think) any motion of the specific subject would do here, just to get
    % the channels
    [tmpchan, skel] = acclaimLoadChannels('../../../vargplvmDEPENDENCIES/DATASETS0p1371/mocap/cmu/13/13_13.amc', skel);
end
Yjump = Y(35:90,:);
Yjacks = Y(677:end,:);
channels = skelGetChannels(Yjump);
close; skelPlayData(skel, channels, 1/20);
%}

%%
function createCmuData(subject, motions)
baseDir = datasetsDirectory;
dirSep = filesep;
[Y, lbls, Ytest, lblstest] = lvmLoadData2('cmu13');
skel = acclaimReadSkel([baseDir 'mocap' dirSep 'cmu' dirSep '13' dirSep '13.asf']);
seq = cumsum(sum(lbls)) - [1:31];
[tmpchan, skel] = acclaimLoadChannels([baseDir 'mocap' dirSep 'cmu' dirSep '13' dirSep '13_16.amc'], skel);
[Y, lbls, Ytest, lblstest] = lvmLoadData2('cmu13NoRoot');

try
    load '../cmu13Data.mat'
catch
    [Y, lbls] = lvmLoadData2('cmu13');
    seq = cumsum(sum(lbls)) - [1:31];

    % load data
    [Y, lbls, Ytest, lblstest] = lvmLoadData2('cmu13NoRoot');
    skel = acclaimReadSkel('../../../vargplvmDEPENDENCIES/DATASETS0p1371/mocap/cmu/13/13.asf');
    % (I think) any motion of the specific subject would do here, just to get
    % the channels
    [tmpchan, skel] = acclaimLoadChannels('../../../vargplvmDEPENDENCIES/DATASETS0p1371/mocap/cmu/13/13_13.amc', skel);
end

%%
%{
%%%%%%%%%%%%%%%%%
%baseDir = datasetsDirectory;
%dirSep = filesep;
%[Y, lbls, Ytest, lblstest] = lvmLoadData2('cmu13');
%skel = acclaimReadSkel([baseDir 'mocap' dirSep 'cmu' dirSep '13' dirSep '13.asf']);
%seq = cumsum(sum(lbls)) - [1:31];
%[tmpchan, skel] = acclaimLoadChannels([baseDir 'mocap' dirSep 'cmu' dirSep '13' dirSep '13_16.amc'], skel);
%[Y, lbls, Ytest, lblstest] = lvmLoadData2('cmu13NoRoot');

try
    load '../cmu13Data.mat'
catch
    [Y, lbls] = lvmLoadData2('cmu13');
    seq = cumsum(sum(lbls)) - [1:31];

    % load data
    [Y, lbls, Ytest, lblstest] = lvmLoadData2('cmu13NoRoot');
    skel = acclaimReadSkel('../../../vargplvmDEPENDENCIES/DATASETS0p1371/mocap/cmu/13/13.asf');
    % (I think) any motion of the specific subject would do here, just to get
    % the channels
    [tmpchan, skel] = acclaimLoadChannels('../../../vargplvmDEPENDENCIES/DATASETS0p1371/mocap/cmu/13/13_13.amc', skel);
end

Y = Y(1:12:1430,:);
Y = Y([1:45 72:end],:);
Y = Y([1:50 70:end],:);

[Ywalk, lbls, Ytest, lblstest] = lvmLoadData('cmu35gplvm');
Y = [Y; Ywalk(1:12:100,:)];

channels = skelGetChannels(Y);

%skelPlayData(skel, channels, 1/20);
%}


function string = ASAversion2numstr(version)
%ASAVERSION2NUMSTR Version identifier to number as string type
%   NUMSTRING = ASAVERSION2NUMSTR(VERSION) converts a numeric ARMASA
%   version identifier into a character string NUMSTRING containing a
%   numeric conversion of the version identifier.
%
%   ARMASA function version identifiers are implemented as date vectors
%   according to the Matlab format, see DATEVEC. Each date vector can be
%   associated with an equivalent serial date number, see DATENUM.
%   ASAVERSION2NUMSTR accepts both numeric formats and converts them into
%   the alternative numeric format which is subsequently converted into a
%   character string.
%
%   Example:
%
%   This function can be used to quickly generate a version identifier
%   for an ARMASA main function that has just been created or modified.
%   Typing,
%
%     ASAversion2numstr(now)
%
%   returns the current date and time in a date vector string that can
%   directly be pasted at the appropriate location in the ARMASA function
%   m-file.
%
%   See also: ASACONTROL, ASAVERSIONCHK, DATENUM, DATESTR, DATEVEC, NOW.

if ~isnumeric(version) |  ~(isequal(length(version),6) | isequal(length(version),1))
   error(ASAerr(10,{'version';mfilename}))
end

if isequal(length(version),6)
   v = version;
   string = sprintf('%0.5f',datenum(v(1),v(2),v(3),v(4),v(5),v(6)));
else
   string = sprintf('%0.0f %0.0f %0.0f %0.0f %0.0f %0.0f',datevec(version));
end

%Program history
%======================================================================
%
% Version                Programmer(s)          E-mail address
% -------                -------------          --------------
% [2000 12 30 20 0 0]    W. Wunderink           wwunderink01@freeler.nl


function [ y, j ] = j_borrow_republican ( y, j )

%*****************************************************************************80
%
%% J_BORROW_REPUBLICAN borrows year-days from years in a Republican date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2000
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, J, a YJ date.
%
  while ( j <= 0 )

    y = y - 1;

    days = year_length_republican ( y );

    j = j + days;

  end

  return
end


function ci = cspselect_fixedNumber(score, ~, nComps, mode)
%CSPSELECT_FIXEDNUMBER - Select a fixed number of components
%
%Synopsis:
% CI = select_fixedNumber(SCORE,~,NCOMPS,MODE)
%
%Arguments:
% SCORE  - score of components
% NCOMPS - number of components (total number depends on MODE)
% MODE   - 'absolutemax' : chooses NCOMPS components corresponding to the
%          NCOMPS maximal absolute scores)
%          'equalperclass' : chooses 2*NCOMPS components corresponding to
%          the NCOMPS lowest and the NCOMPS highest scores (default)
%          'onlyclass1' : chooses NCOMPS components corresponding to the
%          NCOMPS lowest scores
%          'onlyclass2' : chooses NCOMPS components corresponding to the
%          NCOMPS highest scores
%
%Returns:
% CI     - index of components
%
%See also processing/proc_csp

nChans = length(score);

switch mode
    case 'absolutemax'
        [~,ix] = sort(-abs(score));
        ci = ix(1:nComps);
    case 'equalperclass'
        [~,ix] = sort(score);
        ci = [ix(1:nComps); ix(end:-1:nChans-nComps+1)];
    case 'onlyclass1'
        [~,ix] = sort(score);
        ci = ix(1:nComps);
    case 'onlyclass2'
        [~,ix] = sort(score);
        ci = ix(end:-1:nChans-nComps+1);
    otherwise
        error('Unknown mode')
end


function exact = p08_exact ( )

%*****************************************************************************80
%
%% P08_EXACT returns the estimated integral for problem 8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the estimated value of the integral.
%
  exact = 0.86697298733991103757;

  return
end


function out = seconds2human(secs, varargin)
%SECONDS2HUMAN( seconds )   Converts the given number of seconds into a
%                           human-readable string.
%
%   str = SECONDS2HUMAN(seconds) returns a human-readable string from a
%   given (usually large) amount of seconds. For example,
%
%       str = seconds2human(1463456.3)
%
%       str =
%       'About 2 weeks and 2 days.'
%
%   You may also call the function with a second input argument; either
%   'short' (the default) or 'full'. This determines the level of detail
%   returned in the string:
%
%       str = seconds2human(1463456.3, 'full')
%
%       str =
%       '2 weeks, 2 days, 22 hours, 30 minutes, 56 seconds.'
%
%   The 'short' format returns only the two largest units of time.
%
%   [secs] may be an NxM-matrix, in which case the output is an NxM cell
%   array of the corresponding strings.
%
%   NOTE: SECONDS2HUMAN() defines one month as an "average" month, which
%   means that the string 'month' indicates 30.471 days.
%
%   See also datestr, datenum, etime.

%   Author: Rody P.S. Oldenhuis
%   Delft University of Technology
%   E-mail: oldnhuis@dds.nl
%   Last edited 11/Feb/2010.

    % default error
    error(nargchk(1,2,nargin));%#ok

    % define some intuitive variables
    Seconds   = round(1                 );
    Minutes   = round(60     * Seconds  );
    Hours     = round(60     * Minutes  );
    Days      = round(24     * Hours    );
    Weeks     = round(7      * Days     );
    Months    = round(30.471 * Days     );
    Years     = round(365.26 * Days     );
    Centuries = round(100    * Years    );
    Millennia = round(10     * Centuries);

    % put these into an array, and define associated strings
    units   = [Millennia, Centuries, Years, Months, Weeks, ...
               Days, Hours, Minutes, Seconds];
    singles = {'millennium'; 'century'; 'year'; 'month'; ...
               'week'; 'day'; 'hour'; 'minute'; 'second'};
    plurals = {'millennia' ; 'centuries'; 'years'; 'months'; ...
               'weeks'; 'days'; 'hours'; 'minutes'; 'seconds'};

    % cut off all decimals from the given number of seconds
    assert(isnumeric(secs), 'seconds2human:seconds_mustbe_numeric', ...
        'The argument ''secs'' must be a scalar or matrix.');
    secs = round(secs);

    % parse second argument
    short = true;
    if (nargin > 1)
        % extract argument
        short = varargin{1};
        % check its type
        assert(ischar(short), 'seconds2human:argument_type_incorrect', ...
            'The second argument must be either ''short'' or ''full''.');
        % check its contents
        switch lower(short)
            case 'full' , short = false;
            case 'short', short = true;
            otherwise
                error('seconds2human:short_format_incorrect',...
                    'The second argument must be either ''short'' or ''full''.');
        end
    end

    % pre-allocate appropriate output-type
    numstrings = numel(secs);
    if (numstrings > 1), out = cell(size(secs)); end

    % build (all) output string(s)
    for j = 1:numstrings

        % initialize nested loop
        secsj   = secs(j);
        counter = 0;
        if short, string = 'About ';
        else      string = '';
        end

        % possibly quick exit
        if (secsj < 1), string = 'Less than one second.'; end

        % build string for j-th amount of seconds
        for i = 1:length(units)

            % amount of this unit
            amount = fix(secsj/units(i));

            % include this unit in the output string
            if amount > 0

                % increase counter
                counter = counter + 1;

                % append (single or plural) unit of time to string
                if (amount > 1)
                    string = [string, num2str(amount), ' ', plurals{i}];%#ok
                else
                    string = [string, num2str(amount), ' ', singles{i}];%#ok
                end

                % Finish the string after two units if short format is requested
                if (counter > 1 && short), string = [string, '.']; break, end%#ok

                % determine whether the ending should be a period (.) or a comma (,)
                if (rem(secsj, units(i)) > 0)
                    if short, ending = ' and ';
                    else ending = ', ';
                    end
                else ending = '.';
                end
                string = [string, ending];%#ok

            end

            % subtract this step from given amount of seconds
            secsj = secsj - amount*units(i);
        end

        % insert in output cell, or set output string
        if (numstrings > 1)
            out{j} = string;
        else
            out = string;
        end
    end % for

end % seconds2human


% Add land and sea-ice grid boxes to the reconstructed fields
function Yrec_out = metoffice_add_land( Yrec, dataset )

[ mask, lat, lon ] = metoffice_get_mask( dataset );

% path from system
datapath = metoffice_get_path();
load( [ datapath, dataset ], 'time' )

% Add land and ice areas to the reconstruction fields
Yrec_out = NaN( length(lat)*length(lon), length(time) );
Yrec_out( logical(mask(:)), : ) = Yrec;

function Y = metoffice_add_land_old(X, maskfile)

S = load('/share/climate/data/UK_Met_Office/RecTest/mask');
Y = nan(numel(S.mask),size(X,2));
Y(S.mask(:)==1,:) = X;
% $$$ S = load('/share/climate/data/UK_Met_Office/RecTest/land_hadgem');
% $$$ land = (S.land(:) >= 0.5);
% $$$ Y = nan(length(land),size(X,2));
% $$$ Y(~land,:) = X;