%% GENERATE CODE FOR VARIABLES
% This document demonstrates the usage of GENCODE, GENCODE_RVALUE and
% GENCODE_SUBSTRUCT to show and modify the contents of complicated MATLAB
% variables.
%%
%% A TEST VARIABLE
% The variable x is a struct with two fields: f1 and f2. f1 is a cell array
% of arbitrary items.
x.f1        = {1 'somestring' false};
%%
% f2 is a struct array with 2 members and fields f21, f22, f23. The fields
% in f2(1) hold different numeric types.
x.f2(1).f21 = diag([1 1 -Inf NaN]);
x.f2(1).f22 = speye(5);
x.f2(1).f23 = 17 * ones(5,6,2,'int8');
%%
% The fields in f2(2) hold a cell string array, a logical array and a
% (anonymous) function handle.
x.f2(2).f21 = {'string 1'; 'string 2'; 'A third string'};
x.f2(2).f22 = rand(5) > .5;
x.f2(2).f23 = @(x)mod(x,2);
%% GENERATE CODE FOR RIGHT HAND SIDE OF ASSIGNMENTS
% GENCODE uses GENCODE_RVALUE to generate code for the right hand side of
% assignments. This works for
%
% * all scalar, vector, 2D data
% * cells with scalar, vector, 2D members
% * function handles
%
[str, sts] = gencode_rvalue(x.f1);
display(sts)
char(str)
%%
% x.f2(1).f23 is a 3D array, therefore GENCODE_RVALUE does not work here -
% sts is false
size(x.f2(1).f23)
[str, sts] = gencode_rvalue(x.f2(1).f23);
display(sts)
char(str)
%% GENERATE CODE FOR TEST VARIABLE
% The simplest way to invoke GENCODE is with just the variable as input
% argument. Code for arrays with more than 2 dimensions is split up into
% code for 2D subarrays.
% Code for sparse matrices is generated using three temporary variables
% tmpi, tmpj and tmps.
strx = gencode(x);
char(strx)
%%
% If the name of the variable should be different from the input variable
% name, GENCODE can be called with an alternative name. This can also be an
% struct reference (|y.a|) or cell entry (|y{2}|) or array index (|y(1)|).
stry = gencode(x,'y.a');
char(stry)
%% USE GENERATED CODE TO RECREATE VARIABLE
% The generated code can be used to recreate the variable:
clear x
eval(sprintf('%s\n', strx{:}))
display(x)
%%
% Usually, one would write the code to a file using a sequence of commands
% like this:
%
% <html>
% <pre class="codeinput">
% fid = fopen('test.m','w');
% fprintf(fid, '%s\n', strx{:});
% fclose(fid)
% </pre>
% </html>
%
% This file can then be modified to recreate new instances of x.


function [MSE, sequences, indexes, mx, sds, mn, sd] = motionCompDetectMotionMSE(view,ROI,scans,plotMSE,histogramClip)

%    [MSE, sequences, indexes, mx, sds, mn, sd] = motionCompDetectMotionMSE(view, [ROI], [scans], [plotMSE], [histogramClip])
%
% gb 01/26/05
%
% Remove outliers within a scan and returns an array 'sequences' whose rows
% are the sequences between the outliers. The error computed is a Mean
% Squared Error.
%
% Input arguments :
%       - view : current inplane
%       - ROI : Region of interest where the error has to be computed
%           Default = ones
%       - scans : scans to analyse.
%           Default = all scans
%       - plotMSE : indicates if the user wants a plot of the MSE error.
%       Should be the value '1' if so.
%           Default = 1
%       - histogramClip : sets the clipping value of the histogram
%           Default = 0.9
%
% It returns :
%       - MSE = normalized squared sum of the difference of two
%       consecutive frames
%       - sequences = array representing the resulting sequences between the outliers
%       - indexes = indexes correponding to the outliers
%       - mx = maximum of MSE
%       - sds = array representing the standard deviations of each sequence
%       - mn = mean of MSE after having been clipped
%       - sd = standard deviation of MSE after having been clipped

% Initializes arguments and variables
if ieNotDefined('histogramClip')
    histogramClip = .9;
end

if ieNotDefined('scans')
    scans = selectScans(view);
end

if ieNotDefined('ROI')
    ROI = '';
end

if ieNotDefined('resize')
    resize = 0;
end

% Starts the main calculation
% For each scan, it computes the difference between the first frame
% of this scan and the last frame of the preceding scan before the error
% between consecutive frames.

MSE = [];
lastFrame = 0;
h = mrvWaitbar(0,'Computing Mean Squared Error...');

for scan = scans

    mrvWaitbar((scan - 1)/scans(end),h,['Computing Mean Squared Error for scan ' num2str(scan)]);

    % Loads the tSeries
    clear tSeriesAllSlices;
    tSeriesAllSlices = motionCompLoadImages(view,scan);
    nFrames  = size(tSeriesAllSlices,1);
    nVoxelsX = size(tSeriesAllSlices,2);
    nVoxelsY = size(tSeriesAllSlices,3);
    nSlices  = size(tSeriesAllSlices,4);

    % Computes the Mean Squared Error when changing scans
    firstFrame = tSeriesAllSlices(1,:,:,:);

    if isequal(lastFrame,0)
        MSE = [MSE;0];
    else
        MSE = [MSE;motionCompMSE(firstFrame,ROI,lastFrame)];
    end

    lastFrame = tSeriesAllSlices(nFrames,:,:,:);

    % Computes the difference between two consecutive frames
    tSeriesAllSlices = diff(tSeriesAllSlices,1,1);

	% Computes the Mean Square Error
	MSE = [MSE;motionCompMSE(tSeriesAllSlices,ROI)];

end

mrvWaitbar(1,h);

% Clips the histogram to calculate the mean and the standard deviation
% of MSE without taking into account the outliers
histClip = mrAnatHistogramClip(MSE,0,histogramClip,0);

% Computes the mean and the standard deviation of the clipped MSE
mn = mean(histClip);
sd =  std(histClip);

% Finds the outliers. Are considered outliers all the values above the mean
% + two standard deviations
indexes = (find(MSE > mn + 2*sd));
mx = max(MSE);

% Finds the resulting sequences between the outliers.
sequences = horzcat([1;indexes + 2],[indexes - 2;nFrames - 1]);
sequences = sequences(find(sequences(:,2) - sequences(:,1) > 5),:);

% Computes all the standard deviations
sds = zeros(size(sequences,1),1);
for i = 1:size(sequences,1)
    sds(i) = std(MSE(sequences(i,1):sequences(i,2)));
end

% Plots the curves
if ieNotDefined('plotMSE')
    plotMSE = 0;
end

if plotMSE > 0
    figure
    title(['MSE for ' view.sessionCode ]);

    plot(MSE)
    hold on
    plot(1:length(MSE),mn*ones(1,length(MSE)),'-.')
    plot([1 length(MSE)],[mn + 2*sd, mn + 2*sd],'r')

    % Another measure can be used. Sometimes we can assume that the signal
    % is equally distributed around its mean. The explicit measure is :
    % mn + (mn - min(MSE))
    %
    % plot([1 length(MSE)],[2*mn - min(MSE),2*mn - min(MSE)],'black')

    for scanIndex = 1:(length(scans) - 1)
        plot([nFrames*scanIndex nFrames*scanIndex],[min(MSE) mx],'-.','Color','m');
    end

end

close(h)


function robot = parse_urdf(file)
% Loading file from urdf
% The function is tailored for UR robots: serial robots with 6 DOF
% Modify it if you have  a robot with different degrees of freedom
robot = xml2struct(file);
no_dof = 6;

% Extracting parameters of the robot
for i = 1:no_dof
    % axis of rotation of a joint i in coordinate system of joint i
    axis_of_rot = str2num(robot.robot.joint{i}.axis.Attributes.xyz)';
    % mass of link (i+1) because joint i rotates link (i+1) as the numbering of
    % links starts from base link that it not moving
    link_mass = str2double(robot.robot.link{i+1}.inertial.mass.Attributes.value);
    % poistion of the com in frame attached to link
    com_pos = str2num(robot.robot.link{i+1}.inertial.origin.Attributes.xyz)';
    com_vec2mat = vec2skewSymMat(com_pos);
    % inertial parameters of the link expressed in coordinate system attached
    % the center of mass.
    ixx = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.ixx);
    ixy = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.ixy);
    ixz = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.ixz);
    iyy = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.iyy);
    iyz = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.iyz);
    izz = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.izz);
    % the inertia tensor wrt the frame oriented as the body frame and with the
    % origin in the COM
    link_inertia = [ixx, ixy, ixz; ixy, iyy iyz; ixz, iyz, izz];
    % manipulator regressor
    robot.m(i) = link_mass;
    robot.k(:,i) = axis_of_rot;
    robot.r_com(:,i) = com_pos;
    robot.I(:,:,i) = link_inertia;
    robot.h(:,i) = link_mass*com_pos;
    robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia-...
                            link_mass*com_vec2mat*com_vec2mat);
    robot.pi(:,i) = [robot.I_vec(:,i); robot.h(:,i); robot.m(i)];
end


% SCRIPT TEST FOR THE UR10 ROBOT KINEMATICS

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
close all
robot = load_robot('practicals', 'UR10');
adjust_view(robot)

% DESCOMENTE LA L�NEA SIGUIENTE PARA PROBAR UN PUNTO SINGULAR
q0 = [0.1 0.1 0.1 0.1 0.1 0.1]';
q = [0.1 -pi/2 pi/2 pi/4 pi/4 pi/4]';

T = directkinematic(robot, q);

fprintf('\nSimple test: try to reach T')
% Llame a la cinemática inversa
qinv = inversekinematic(robot, T, q0)

T_reach = directkinematic(robot, qinv)
'diff T-Treach'
T-T_reach


function [w, state] = adadelta(w, state, grad, opts, ~)
%ADADELTA
%   Example AdaDelta solver, for use with CNN_TRAIN and CNN_TRAIN_DAG.
%
%   AdaDelta sets its own learning rate, so any learning rate set in the
%   options of CNN_TRAIN and CNN_TRAIN_DAG will be ignored.
%
%   If called without any input argument, returns the default options
%   structure.
%
%   Solver options: (opts.train.solverOpts)
%
%   `epsilon`:: 1e-6
%      Small additive constant to regularize variance estimate.
%
%   `rho`:: 0.9
%      Moving average window for variance update, between 0 and 1 (larger
%      values result in slower/more stable updating).

% Copyright (C) 2016 Joao F. Henriques.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if nargin == 0  % Return the default solver options
  w = struct('epsilon', 1e-6, 'rho', 0.9) ;
  return ;
end

if isequal(state, 0)  % First iteration, initialize state struct
  state = struct('g_sqr', 0, 'delta_sqr', 0) ;
end

rho = opts.rho ;

state.g_sqr = state.g_sqr * rho + grad.^2 * (1 - rho) ;
new_delta = -sqrt((state.delta_sqr + opts.epsilon) ./ ...
                  (state.g_sqr + opts.epsilon)) .* grad ;
state.delta_sqr = state.delta_sqr * rho + new_delta.^2 * (1 - rho) ;

w = w + new_delta ;


function [feaNew] = SRKDAtest(fea, model)
% SRKDAtest: Spectral Regression Kernel Discriminant Analysis Testing
%               SRKDAtest uses SRKDA as a dimensionality reduction tool.
%
%       [feaNew,elapse] = SRKDAtest(fea, model);
%
%             Input:
%
%               fea     - data matrix. Each row is a data point.
%             model     - model trained by SRKDAtrain.m
%
%             Output:
%
%             feaNew    - The data in the c-1 SRKDA subspace, where c is the
%                         number of classes.
%
%    Examples:
%
%
% See also SRKDAtrain, SRKDApredict, KSR, KSR_caller
%
%Reference:
%
%   [1] Deng Cai, Xiaofei He, and Jiawei Han. "Speed Up Kernel Discriminant
%   Analysis", The VLDB Journal, vol. 20, no. 1, pp. 21-33, January, 2011.
%
%   [2] Deng Cai, Xiaofei He and Jiawei Han, "SRDA: An Efficient Algorithm for
%   Large Scale Discriminant Analysis" IEEE Transactions on Knowledge and
%   Data Engineering, vol. 20, no. 1, pp. 1-12, January, 2008.
%
%   [3] Deng Cai, "Spectral Regression: A Regression Framework for
%   Efficient Regularized Subspace Learning", PhD Thesis, Department of
%   Computer Science, UIUC, 2009.
%
%   version 3.0 --Jan/2012
%   version 2.0 --December/2011
%   version 1.0 --May/2006
%
%   Written by Deng Cai (dengcai AT gmail.com)
%

MAX_MATRIX_SIZE = 8000; % You can change this number based on your memory.

if ~strcmp(model.TYPE,'SRKDA')
    error('model does not match!');
end

nTrain = size(model.fea,1);
nTest = size(fea,1);
nBlock = ceil(MAX_MATRIX_SIZE*MAX_MATRIX_SIZE/nTrain);
if model.LARs
    feaNew = cell(length(model.LassoCardi),1);
    for i=1:length(model.LassoCardi)
        feaNew{i} = zeros(nTest,size(model.projection{i},2));
    end
    for j = 1:ceil(nTest/nBlock)
        if j == ceil(nTest/nBlock)
            smpIdx = (j-1)*nBlock+1:nTest;
        else
            smpIdx = (j-1)*nBlock+1:j*nBlock;
        end
        KTest= constructKernel(fea(smpIdx,:),model.fea,model.options);
        if model.bSemi
            KTest = KTest*model.KtestHat;
        end
        for i=1:length(model.LassoCardi)
            if model.bSemi
                feaNew{i}(smpIdx,:) = KTest(:,1:model.nLabel)*model.projection{i};
            else
                feaNew{i}(smpIdx,:) = KTest*model.projection{i};
            end
        end
        clear KTest;
    end
else
    feaNew = zeros(nTest,size(model.projection,2));
    for i = 1:ceil(nTest/nBlock)
        if i == ceil(nTest/nBlock)
            smpIdx = (i-1)*nBlock+1:nTest;
        else
            smpIdx = (i-1)*nBlock+1:i*nBlock;
        end
        KTest= constructKernel(fea(smpIdx,:),model.fea,model.options);
        if model.bSemi
            KTest = KTest*model.KtestHat;
            feaNew(smpIdx,:) = KTest(:,1:model.nLabel)*model.projection;
        else
            feaNew(smpIdx,:) = KTest*model.projection;
        end
        clear KTest;
    end
end


function t = transmission_postprocessing_guided_filter(t_in, I, varargin)
%TRANSMISSION_POSTPROCESSING_GUIDED_FILTER  Process input transmission map with
%guided filter. Parameters for the filter are set to their recommended values.

window_size = 41;
mu = 1e-3;
t = clip_to_unit_range(transmission_guided_filtering(t_in, I, window_size, mu));

end


function i4vec_index_test ( )

%*****************************************************************************80
%
%% I4VEC_INDEX_TEST tests I4VEC_INDEX;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_INDEX_TEST\n' );
  fprintf ( 1, '  For an integer vector:\n' );
  fprintf ( 1, '  I4VEC_INDEX:              first index of given value;\n' );

  seed = 123456789;
  b = -n;
  c = n;

  [ a, seed ] = i4vec_uniform_ab ( n, b, c, seed );

  i4vec_print ( n, a, '  Input vector:' );

  fprintf ( 1, '\n' );

  aval = a(floor(n/2));
  fprintf ( 1, '\n' );
  j = i4vec_index ( n, a, aval );
  fprintf ( 1, '  Index of first occurrence of %d is %d\n', aval, j );

  aval = aval + 1;
  j = i4vec_index ( n, a, aval );
  fprintf ( 1, '  Index of first occurrence of %d is %d\n', aval, j );

  return
end


function [poses] = InterpolatePoses(ins_file, pose_timestamps, origin_timestamp, use_rtk)

% InterpolatePoses - interpolate INS poses to find poses at given timestamps
%
% [poses] = InterpolatePoses(ins_file, pose_timestamps, origin_timestamp)
%
% INPUTS:
%   ins_file: csv file containing INS data
%   pose_timestamps: array of UNIX timestamps at which interpolated poses are
%     required
%   origin_timestamp: timestamp for origin frame, relative to which poses are
%     reported
%
% OUTPUTS:
%   poses: cell array of 4x4 matrices, representing SE3 poses at the times
%     specified in pose_timestamps

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright (c) 2016 University of Oxford
% Authors:
%  Geoff Pascoe (gmp@robots.ox.ac.uk)
%  Will Maddern (wm@robots.ox.ac.uk)
%
% This work is licensed under the Creative Commons
% Attribution-NonCommercial-ShareAlike 4.0 International License.
% To view a copy of this license, visit
% http://creativecommons.org/licenses/by-nc-sa/4.0/ or send a letter to
% Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  ins_file_id = fopen(ins_file);
  if ~use_rtk
      header_count = 15;
  else
      header_count = 23;
  end
  headers = textscan(ins_file_id, '%s', header_count, 'Delimiter',',');
  if ~use_rtk
      format_str = '%u64 %s %f %f %f %f %f %f %s %f %f %f %f %f %f';
  else
      format_str = '%u64 %f %f %f %f %f %f %s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f';
  end
  ins_data = textscan(ins_file_id, ...
      format_str,'Delimiter',',');
  fclose(ins_file_id);

  ins_timestamps = ins_data{1};

  lower_index = max(find(ins_timestamps(:,1)<=min(pose_timestamps), 1, ...
      'last')-1,1);
  upper_index = find(ins_timestamps(:,1)>max(pose_timestamps), 1, 'first')+1;

  lower_index = max(lower_index, 1);
  upper_index = min(upper_index, numel(ins_timestamps));

  ins_poses = cell(1, upper_index - lower_index + 1);
  ins_quaternions = cell(1, upper_index - lower_index + 1);

  if ~use_rtk
    northing_col = 6;
    easting_col = 7;
    down_col = 8;
    roll_col = 13;
    pitch_col = 14;
    yaw_col = 15;
  else
    northing_col = 5;
    easting_col = 6;
    down_col = 7;
    roll_col = 12;
    pitch_col = 13;
    yaw_col = 14;
  end
  northings = ins_data{northing_col};
  eastings = ins_data{easting_col};
  downs = ins_data{down_col};
  rolls = ins_data{roll_col};
  pitches = ins_data{pitch_col};
  yaws = ins_data{yaw_col};

  pose_timestamps = [origin_timestamp pose_timestamps];

  for i=1:upper_index
    ins_poses{i} = SE3MatrixFromComponents(...
      northings(i), eastings(i), downs(i), rolls(i), pitches(i), yaws(i));
    ins_quaternions{i} = SO3ToQuaternion(ins_poses{i}(1:3,1:3))';
  end

  [lower_index_rows, lower_index_cols] = ...
    find(and(bsxfun(@le,ins_timestamps,pose_timestamps)',...
    circshift(bsxfun(@gt,ins_timestamps,pose_timestamps)',[0 -1])));

  lower_indices = zeros(size(lower_index_rows));
  lower_indices(lower_index_rows) = lower_index_cols;
  lower_indices = max(lower_indices, 1);

  ins_timestamps = cast(ins_timestamps, 'double');
  pose_timestamps = cast(pose_timestamps, 'double');
  fractions = (pose_timestamps - ins_timestamps(lower_indices)')./...
    (ins_timestamps(lower_indices+1)'-ins_timestamps(lower_indices)');

  quaternions_lower = [ins_quaternions{lower_indices}];
  quaternions_upper = [ins_quaternions{lower_indices+1}];

  % interpolate quaternions
  d_array = sum(quaternions_lower.*quaternions_upper,1);

  linear_interp_indices = find(~(abs(d_array)<1.0));
  sin_interp_indices = find(abs(d_array)<1.0);

  scale0_array = zeros(size(d_array));
  scale1_array = zeros(size(d_array));

  scale0_array(linear_interp_indices) = 1-fractions(linear_interp_indices);
  scale1_array(linear_interp_indices) = fractions(linear_interp_indices);

  theta_array = acos(abs(d_array(sin_interp_indices)));

  scale0_array(sin_interp_indices) = sin((1-fractions(sin_interp_indices)).*...
      theta_array)./sin(theta_array);
  scale1_array(sin_interp_indices) = sin(fractions(sin_interp_indices).*...
      theta_array)./sin(theta_array);

  negative_d_indices = find(d_array < 0);
  scale1_array(negative_d_indices) = -scale1_array(negative_d_indices);

  quaternions_interp = repmat(scale0_array,4,1).*quaternions_lower + ...
      repmat(scale1_array,4,1).*quaternions_upper;

  % interpolate positions
  ins_poses_array = [ins_poses{:}];
  positions_lower = ins_poses_array(1:3,4*lower_indices);
  positions_upper = ins_poses_array(1:3,4*(lower_indices+1));
  positions_interp = repmat((1-fractions),3,1).*positions_lower + ...
      repmat(fractions,3,1).*positions_upper;

  poses_array = zeros(4, 4*numel(pose_timestamps));
  poses_array(1,1:4:end) = 1-2*quaternions_interp(3,:).^2 ...
      -2*quaternions_interp(4,:).^2;
  poses_array(1,2:4:end) = 2*quaternions_interp(2,:).*...
    quaternions_interp(3,:) - 2*quaternions_interp(4,:).*...
    quaternions_interp(1,:);
  poses_array(1,3:4:end) = 2*quaternions_interp(2,:).*...
      quaternions_interp(4,:) + 2*quaternions_interp(3,:).*...
      quaternions_interp(1,:);

  poses_array(2,1:4:end) = 2*quaternions_interp(2,:).*...
      quaternions_interp(3,:) + 2*quaternions_interp(4,:).*...
      quaternions_interp(1,:);
  poses_array(2,2:4:end) = 1-2*quaternions_interp(2,:).^2 ...
      -2*quaternions_interp(4,:).^2;
  poses_array(2,3:4:end) = 2*quaternions_interp(3,:).*...
      quaternions_interp(4,:) - 2*quaternions_interp(2,:).*...
      quaternions_interp(1,:);

  poses_array(3,1:4:end) = 2*quaternions_interp(2,:).*...
      quaternions_interp(4,:) - 2*quaternions_interp(3,:).*...
      quaternions_interp(1,:);
  poses_array(3,2:4:end) = 2*quaternions_interp(3,:).*...
      quaternions_interp(4,:) + 2*quaternions_interp(2,:).*...
      quaternions_interp(1,:);
  poses_array(3,3:4:end) = 1-2*quaternions_interp(2,:).^2 ...
      -2*quaternions_interp(3,:).^2;

  poses_array(1:3,4:4:end) = positions_interp;
  poses_array(4,4:4:end) = 1;
  poses_array = poses_array(1:4,1:4) \ poses_array;

  poses = cell(numel(pose_timestamps)-1,1);
  for i=2:numel(pose_timestamps)
    poses{i-1} = poses_array(:,(i-1)*4+1:i*4);
  end

end


function G = diffy( F, n )
%DIFFY   Differentiate a DISKFUNV with respect to its second argument
%   DIFFY(F) returns a DISKFUNV representing the derivative of F in its
%   second argument. This is the same as DIFF(F,2,1).
%
%   DIFFY(F,N) returns a DISKFUNV representing the Nth derivative of F in
%   its second argument. This is the same as DIFF(F, 2, N).
%
%   This command is for convenience as the syntax for DIFF, inherited from
%   the DIFF command for matrices, can be confusing.
%
% See also DIFFX, DIFF.

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

if ( nargin == 1 )
    % Default to first derivative.
    n = 1;
end

% Call DISKFUNV/DIFF:
G = diff( F, 2, n);

end


function [gx] = g_vgo(x,P,u,in)
gx = VBA_sigmoid(exp(P(1))*x+P(2));


%%begin
% We will begin with the trophy from the CollisionModel demo, which we
% do not want to hit.
trophy = cmdl_trophy

% We will load a model of the SerialLink object with STL data included.
load('p560_col.mat');
p560

% The two are precariously close!
fig1 = figure;
qz = zeros(1,6);
p560.plot3d(qz);
hold on
trophy.plot;

% The point data from the STL data is not dense. We can only check for
% collisions where there are points. You can rectify this in a solid
% model program, such as SolidWorks, by adding a grid of split lines on
% the relevant surfaces.

% We can check for collisions
c = p560.collisions(qz, trophy)

% The false/zero value indicates the two are not colliding. If the 2nd
% joint turns a little to move the red link lower,
q = [0 -0.1 0 0 0 0]

c = p560.collisions(q, trophy)

% There is now a collision. Let's verify this
close(fig1); figure;
p560.plot3d(q);
hold on
trophy.plot;

% The robot does indeed collide with the trophy. It is important to
% note that the program sees the cup of the plot as a solid object,
% even though we have plotted is as an open, concave vessel. If the arm
% was to be in the void of the cup but not actually touching, it would
% still be considered colliding.


function [coherogram,phase,t,f] = MTCoherogram(lfp1,lfp2,varargin)

%MTCoherogram - Compute LFP coherogram by multi-taper estimation.
%
%  USAGE
%
%    [coherogram,phase,t,f] = MTCoherogram(lfp1,lfp2,<options>)
%
%    lfp1,lfp2      wide-band LFPs (one channel each).
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'frequency'   sampling rate (in Hz) (default = from timestamps if
%                   available, otherwise 1250Hz)
%     'range'       frequency range (in Hz) (default = all)
%     'window'      duration (in s) of the time window (default = 5)
%     'overlap'     overlap between successive windows (default = window/2)
%     'step'        step between successive windows (default = window/2)
%     'tapers'      relative resolution and order of the tapers [NW K]
%                   (default = [3 5])
%     'pad'         FFT padding (see help for cohgramc) (default = 0)
%     'show'        plot results (default = 'off')
%     'cutoffs'     cutoff values for color plot (default = [0 1])
%    =========================================================================
%
%  NOTES
%
%    The LFP can be provided either as a time stamped matrix (list of time-voltage
%    pairs), or as a voltage vector - in which case the frequency must be specified.
%
%    The time displacement between successive short time coherences can be supplied
%    either as a 'step' (explicit time difference) or as an 'overlap' (between
%    successive time windows).
%
%  OUTPUT
%
%    coherogram     coherogram magnitude
%    phase          coherogram phase
%    t              time bins
%    f              frequency bins
%
%  DEPENDENCIES
%
%    This function requires the chronux toolbox.
%
%  SEE
%
%    See also MTCoherence, MTSpectrum, MTSpectrogram, PlotColorMap.

% Copyright (C) 2010-2014 by Michaël Zugaro
%
% 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.
warning('This function has been deprecated, and will be removed in the future. Try using bz_MTCoherogram.m')
% Make sure chronux is installed and functional
CheckChronux('cohgramc');

% Defaults
f = 1250;
frequency = [];
window = 5;
range = [];
overlap = [];
step = [];
show = 'off';
tapers = [3 5];
pad = 0;
cutoffs = [0 1];

% Check number of parameters
if nargin < 2 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help MTCoherogram'' for details).');
end

% Check parameter sizes
if size(lfp1,2) ~= 1 && size(lfp1,2) ~= 2,
	error('Parameter ''lfp1'' is not a vector or a Nx2 matrix (type ''help MTCoherogram'' for details).');
end
if size(lfp2,2) ~= 1 && size(lfp2,2) ~= 2,
	error('Parameter ''lfp2'' is not a vector or a Nx2 matrix (type ''help MTCoherogram'' for details).');
end

% Parse parameter list
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		error(['Parameter ' num2str(i+2) ' is not a property (type ''help MTCoherogram'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'frequency',
			frequency = varargin{i+1};
			if ~isdscalar(frequency,'>0'),
				error('Incorrect value for property ''frequency'' (type ''help MTCoherogram'' for details).');
			end
		case 'range',
			range = varargin{i+1};
			if ~isdvector(range,'#2','<','>=0'),
				error('Incorrect value for property ''range'' (type ''help MTCoherogram'' for details).');
			end
		case 'window',
			window = varargin{i+1};
			if ~isdscalar(window,'>0'),
				error('Incorrect value for property ''window'' (type ''help MTCoherogram'' for details).');
			end
		case 'overlap',
			overlap = varargin{i+1};
			if ~isdscalar(overlap,'>0'),
				error('Incorrect value for property ''overlap'' (type ''help MTCoherogram'' for details).');
			end
		case 'step',
			step = varargin{i+1};
			if ~isdscalar(step,'>0'),
				error('Incorrect value for property ''step'' (type ''help MTCoherogram'' for details).');
			end
		case 'tapers',
			tapers = varargin{i+1};
			if ~isivector(tapers,'#2','>0'),
				error('Incorrect value for property ''tapers'' (type ''help MTCoherogram'' for details).');
			end
		case 'pad',
			pad = varargin{i+1};
			if ~isiscalar(pad,'>-1'),
				error('Incorrect value for property ''pad'' (type ''help MTCoherogram'' for details).');
			end
		case 'show',
			show = varargin{i+1};
			if ~isstring_FMAT(show,'on','off'),
				error('Incorrect value for property ''show'' (type ''help MTCoherogram'' for details).');
			end
		case 'cutoffs',
			cutoffs = varargin{i+1};
			if ~isdvector(cutoffs,'#2','>=0','<'),
				error('Incorrect value for property ''cutoffs'' (type ''help MTCoherogram'' for details).');
			end
		otherwise,
			error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help MTCoherogram'' for details).']);
	end
end

% Determine LFP frequency
if isempty(frequency),
	if size(lfp1,2) == 2,
		frequency = 1/median(diff(lfp1(:,1)));
	else
		frequency = f;
	end
end

% Determine step/overlap
if isempty(step),
	if isempty(overlap),
		overlap = window/2;
	end
else
	if isempty(overlap),
		overlap = window-step;
	elseif overlap ~= window-step,
		error('Incompatible ''step'' and ''overlap'' parameters (type ''help MTCoherogram'' for details).');
	end
end

% Compute and plot coherogram
parameters.Fs = frequency;
if ~isempty(range), parameters.fpass = range; end
parameters.tapers = tapers;
parameters.pad = pad;
[coherogram,phase,~,~,~,t,f] = cohgramc(lfp1(:,2),lfp2(:,2),[window window-overlap],parameters);
t = t'+lfp1(1,1);
f = f';
coherogram = coherogram';
% coherogram = permute(coherogram,[2 1 3]);  % Previous code by Gabrielle Girardeau, keep it around just in case
phase = phase';
if strcmp(lower(show),'on'),
  figure;hold on;
  subplot(2,1,1);
  PlotColorMap(coherogram,'x',t,'y',f,'cutoffs',cutoffs,'newfig','off');
  xlabel('Time (s)');
  ylabel('Frequency (Hz)');
  title('Coherogram Amplitude');
  subplot(2,1,2);
  PlotColorMap(phase,'x',t,'y',f,'cutoffs',[-pi pi],'newfig','off');
  xlabel('Time (s)');
  ylabel('Frequency (Hz)');
  title('Coherogram Phase');
end


function [P, G] = getPoints(q,p)
% [P, G] = getPoints(q,p)
%
% This function computes the joint positions (P) and center of mass
% positions (G) for the five-link biped, in configuration given by q with
% physical parameters in p.
%
% INPUTS:
%   q = [5, n] = link configuration
%   p = parameters struct
%
% OUTPUTS:
%   P = [10, n] = joint positions [x;y;x;y;...]
%   G = [10, n] = CoM positions {x;y;x;y;...]
%

q1 = q(1,:);  %stance leg tibia angle
q2 = q(2,:);  %stance leg femur angle
q3 = q(3,:);  %torso angle
q4 = q(4,:);  %swing leg femur angle
q5 = q(5,:);  %swing leg tibia angle

[P,G] = autoGen_getPoints(...
    q1,q2,q3,q4,q5,...
    p.l1 ,p.l2 ,p.l3 ,p.l4 ,p.l5 ,p.c1 ,p.c2 ,p.c3 ,p.c4 ,p.c5);

end


classdef prtClassMaryLogDiscSparseKernel < prtClassMaryLogDisc

    properties (SetAccess=private)
    end

    properties (SetAccess = protected)
        % Lambda
        lambda = 10;  %2; for dataBiModal;
        k
    end

    methods

        function self = prtClassMaryLogDiscSparseKernel(varargin)

            self = prtUtilAssignStringValuePairs(self,varargin{:});
            self.wChangeTolerance = 1;
        end

    end

    methods (Access=protected, Hidden = true)

        function self = trainAction(self,dataSet)
            %self = trainAction(self,dataSet)

            x = dataSet.getObservations;
            %x = cat(2,ones(size(x,1),1),x); %DC component
            k = prtKernelRbf;
            k = k.train(dataSet);
            self.k = k;

            x = getObservations(k.run(dataSet));
            y = dataSet.getTargetsAsBinaryMatrix;

            nClasses = dataSet.nClasses;
            d = size(x,2);

            %random initialization; last set of weights is set to 0
            numWeights = d*(nClasses-1);
            weightMatrix = randn(nClasses-1,d);
            weightMatrix = cat(1,weightMatrix,zeros(1,size(weightMatrix,2)));
            weightMatrixOld = weightMatrix;

            %Can calculate B matrix outside loop; makes life fast
            xx = x'*x;
            B = kron(-1/2*(eye(nClasses-1)-ones(nClasses-1)/dataSet.nClasses),xx);

            self.converged = false;

            for j = 1:self.maxIter
                for k = 1:numWeights
                    psi = (weightMatrix*x')';
                    py = bsxfun(@rdivide,exp(psi),sum(exp(psi),2));

                    %label error
                    yError = y-py;

                    if j == 1
                        kronIndex1 = repmat(1:size(yError,2),size(x,2),1);
                        kronIndex2 = repmat(1:size(x,2),1,size(y,2));
                        xRepmat = x(:,kronIndex2);
                    end
                    %Can we speed this up?  KRON is needlessly slow
                    %                     g = 0;
                    %                     for i = 1:size(yError,1)
                    %                         g = g + kron(yError(i,:),x(i,:));
                    %                     end
                    %                     g = g(:);
                    %                     tic;
                    g = sum(yError(:,kronIndex1(:)).*xRepmat);
                    g = g(:);

                    wVec = weightMatrix(1:end-1,:)';
                    wVec = wVec(:);
                    wVec(k) = prtUtilSoft(wVec(k)-g(k)/B(k,k),-self.lambda./B(k,k));
                    weightMatrix(1:end-1,:) = reshape(wVec,size(weightMatrix(1:end-1,:),2),size(weightMatrix(1:end-1,:),1))';
                end
                stem(weightMatrix(1:end-1,:)');
                title(j);
                drawnow;
                if norm(weightMatrix(:)-weightMatrixOld(:)) < self.wChangeTolerance
                    self.converged = true;
                    break;
                end
                weightMatrixOld = weightMatrix;

            end
            self.wMat = weightMatrix;
        end

        function ClassifierResults = runAction(self,dataSet)
            %ClassifierResults = runAction(self,DataSet)

            x = getObservations(self.k.run(dataSet));

            psi = (self.wMat*x')';
            y = bsxfun(@rdivide,exp(psi),sum(exp(psi),2));

            ClassifierResults = dataSet.setObservations(y);
        end
    end
end


filename='Cantilever_hexahedra_fine';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'PROJECTED GRADIENT';
incrementFactor = 1;
designVariable = 'Density';
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.05;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


classdef EuclideanLoss < dagnn.Loss

  methods
    function outputs = forward(obj, inputs, params)
      outputs{1} = vl_nneuclideanloss(inputs{1}, inputs{2}, ...
                                      'instanceWeights', inputs{3}) ;
      n = obj.numAveraged ;
      m = n + size(inputs{1}, 4) ;
      obj.average = (n * obj.average + gather(outputs{1})) / m ;
      obj.numAveraged = m ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      derInputs{1} = vl_nneuclideanloss(inputs{1}, inputs{2}, derOutputs{1}, ...
                                        'instanceWeights', inputs{3}) ;
      derInputs{2} = [] ;
      derInputs{3} = [] ;
      derParams = {} ;
    end

    function obj = EuclideanLoss(varargin)
      obj.load(varargin) ;
    end
  end
end


%MDL_QUADCOPTER Dynamic parameters for a quadrotor.
%
% MDL_QUADCOPTER is a script creates the workspace variable quad which
% describes the dynamic characterstics of a quadrotor flying robot.
%
% Properties::
%
% This is a structure with the following elements:
%
% nrotors   Number of rotors (1x1)
% J         Flyer rotational inertia matrix (3x3)
% h         Height of rotors above CoG (1x1)
% d         Length of flyer arms (1x1)
% nb        Number of blades per rotor (1x1)
% r         Rotor radius (1x1)
% c         Blade chord (1x1)
% e         Flapping hinge offset (1x1)
% Mb        Rotor blade mass (1x1)
% Mc        Estimated hub clamp mass (1x1)
% ec        Blade root clamp displacement (1x1)
% Ib        Rotor blade rotational inertia (1x1)
% Ic        Estimated root clamp inertia (1x1)
% mb        Static blade moment (1x1)
% Ir        Total rotor inertia (1x1)
% Ct        Non-dim. thrust coefficient (1x1)
% Cq        Non-dim. torque coefficient (1x1)
% sigma     Rotor solidity ratio (1x1)
% thetat    Blade tip angle (1x1)
% theta0    Blade root angle (1x1)
% theta1    Blade twist angle (1x1)
% theta75   3/4 blade angle (1x1)
% thetai    Blade ideal root approximation (1x1)
% a         Lift slope gradient (1x1)
% A         Rotor disc area (1x1)
% gamma     Lock number (1x1)
%
%
% Notes::
% - SI units are used.
%
% References::
% - Design, Construction and Control of a Large Quadrotor micro air vehicle.
%   P.Pounds, PhD thesis,
%   Australian National University, 2007.
%   http://www.eng.yale.edu/pep5/P_Pounds_Thesis_2008.pdf
% - This is a heavy lift quadrotor
%
% See also sl_quadrotor.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

quadrotor.nrotors = 4;                %   4 rotors
quadrotor.g = 9.81;                   %   g       Gravity                             1x1
quadrotor.rho = 1.184;                %   rho     Density of air                      1x1
quadrotor.muv = 1.5e-5;               %   muv     Viscosity of air                    1x1

% Airframe
quadrotor.M = 4;                      %   M       Mass                                1x1
% Ixx = 0.082;
% Iyy = 0.082;
% Izz = 0.149;%0.160;
quadrotor.J = diag([0.082 0.082 0.149]);    %   I       Flyer rotational inertia matrix     3x3

quadrotor.h = -0.007;                 %   h       Height of rotors above CoG          1x1
quadrotor.d = 0.315;                  %   d       Length of flyer arms                1x1

%Rotor
quadrotor.nb = 2;                      %   b       Number of blades per rotor          1x1
quadrotor.r = 0.165;                  %   r       Rotor radius                        1x1

quadrotor.c = 0.018;                  %   c       Blade chord                         1x1

quadrotor.e = 0.0;                    %   e       Flapping hinge offset               1x1
quadrotor.Mb = 0.005;                 %   Mb      Rotor blade mass                    1x1
quadrotor.Mc = 0.010;                 %   Mc      Estimated hub clamp mass            1x1
quadrotor.ec = 0.004;                 %   ec      Blade root clamp displacement       1x1
quadrotor.Ib = quadrotor.Mb*(quadrotor.r-quadrotor.ec)^2/4 ;        %   Ib      Rotor blade rotational inertia      1x1
quadrotor.Ic = quadrotor.Mc*(quadrotor.ec)^2/4;           %   Ic      Estimated root clamp inertia        1x1
quadrotor.mb = quadrotor.g*(quadrotor.Mc*quadrotor.ec/2+quadrotor.Mb*quadrotor.r/2);    %   mb      Static blade moment                 1x1
quadrotor.Ir = quadrotor.nb*(quadrotor.Ib+quadrotor.Ic);             %   Ir      Total rotor inertia                 1x1

quadrotor.Ct = 0.0048;                %   Ct      Non-dim. thrust coefficient         1x1
quadrotor.Cq = quadrotor.Ct*sqrt(quadrotor.Ct/2);         %   Cq      Non-dim. torque coefficient         1x1

quadrotor.sigma = quadrotor.c*quadrotor.nb/(pi*quadrotor.r);         %   sigma   Rotor solidity ratio                1x1
quadrotor.thetat = 6.8*(pi/180);      %   thetat  Blade tip angle                     1x1
quadrotor.theta0 = 14.6*(pi/180);     %   theta0  Blade root angle                    1x1
quadrotor.theta1 = quadrotor.thetat - quadrotor.theta0;   %   theta1  Blade twist angle                   1x1
quadrotor.theta75 = quadrotor.theta0 + 0.75*quadrotor.theta1;%   theta76 3/4 blade angle                     1x1
quadrotor.thetai = quadrotor.thetat*(quadrotor.r/quadrotor.e);      %   thetai  Blade ideal root approximation      1x1
quadrotor.a = 5.5;                    %   a       Lift slope gradient                 1x1

% derived constants
quadrotor.A = pi*quadrotor.r^2;                 %   A       Rotor disc area                     1x1
quadrotor.gamma = quadrotor.rho*quadrotor.a*quadrotor.c*quadrotor.r^4/(quadrotor.Ib+quadrotor.Ic);%   gamma   Lock number                         1x1

quadrotor.b = quadrotor.Ct*quadrotor.rho*quadrotor.A*quadrotor.r^2; % T = b w^2
quadrotor.k = quadrotor.Cq*quadrotor.rho*quadrotor.A*quadrotor.r^3; % Q = k w^2

quadrotor.verbose = false;


%%*********************************************************************
%% sortA: sort columns of At{p} in ascending order according to the
%%        number of nonzero elements.
%%
%% [At,C,b,X0,Z0,permA,permZ] = sortA(blk,At,C,b,X0,Z0);
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*********************************************************************

   function [At,C,Cnew,X0,Z0,permA,invpermA,permZ] = sortA(blk,At,C,Cnew,b,X0,Z0);

   global spdensity smallblkdim
%%
   numblk = size(blk,1);
   m = length(b);
   nnzA = zeros(numblk,m);
   permA = kron(ones(numblk,1),[1:m]);
   invpermA = kron(ones(numblk,1),[1:m]);
   permZ = cell(size(blk,1),1);
%%
   for p=1:size(blk,1)
      pblk = blk(p,:);
      n = sum(pblk{2});
      numblk = length(pblk{2});
      if strcmp(pblk{1},'s') & (max(pblk{2}) > smallblkdim)
         n2 = sum(pblk{2}.*pblk{2});  n22 = sum(pblk{2}.*(pblk{2}+1))/2;
         m1 = size(At{p,1},2);
         if (length(pblk{2}) == 1)
            tmp = abs(C{p}) + abs(Z0{p});
            if  (~isempty(At{p,1}))
                tmp = tmp + smat(blk(p,:),abs(At{p,1})*ones(m1,1),1);
            end
            if (nnz(tmp) < spdensity*n22);
               per = symamd(tmp);
               invper = zeros(n,1); invper(per) = [1:n];
               permZ{p} = invper;
               if (~isempty(At{p,1}))
                  isspAt = issparse(At{p,1});
                  for k = 1:m1
                     Ak = smat(pblk,At{p,1}(:,k),1);
                     At{p,1}(:,k) = svec(pblk,Ak(per,per),isspAt);
                  end
               end
               C{p}  = C{p}(per,per);
               Z0{p} = Z0{p}(per,per);
               X0{p} = X0{p}(per,per);
               Cnew{p} = Cnew{p}(per,per);
            else
               per = [];
            end
            if (length(pblk) > 2) & (~isempty(per))
               m2 = length(pblk{3});
               P = spconvert([(1:n)', per', ones(n,1)]);
               At{p,2} = P*At{p,2};
            end
         end
         if ~isempty(At{p,1}) & (mexnnz(At{p,1}) < m*n22/2)
            for k = 1:m1
               Ak = At{p,1}(:,k);
               nnzA(p,k) = length(find(abs(Ak) > eps));
            end
            [dummy,permAp] = sort(nnzA(p,1:m1));
            At{p,1}  = At{p,1}(:,permAp);
            permA(p,1:m1) = permAp;
            invpermA(p,permAp) = [1:m1];
         end
      elseif strcmp(pblk{1},'q') | strcmp(pblk{1},'l') | strcmp(pblk{1},'u');
         if ~issparse(At{p,1});
            At{p,1} = sparse(At{p,1});
         end
      end
   end
%%*********************************************************************


%% triSurfSlice
% Below is a demonstration of the features of the |triSurfSlice| function

%%
clear; close all; clc;

%% Syntax
% |[F,V,C,logicSide,Eb]=triSurfSlice(F1,V1,C1,P,n,snapTolerance);|

%% Description
% This function slices the input mesh (defined by faces F, vertices V and
% optional color data C) using a plane specified by a point P and a normal
% vector to the plane n. Edges crossing the plane are cut at the plane, and
% new points are introduced at the intersection, and new triangles are
% created. Points within snapTolerance from the plane are snapped to the
% plane.

%% Examples

%%
% Plot settings
fontSize=15;

%% Example 1: Slicing a surface
% Create example surface data consisting of faces F and vertices V

[F,V]=stanford_bunny; %Get faces and vertices
C=[]; %No color data used in this example so left empty
snapTolerance=mean(patchEdgeLengths(F,V))/100;
n=vecnormalize([0 0 1]); %Normal direction to plane
P=mean(V,1); %Point on plane

%Slicing surface (note 3rd color data output is supressed)
[Fc,Vc,~,logicSide,Eb]=triSurfSlice(F,V,C,P,n,snapTolerance);

%%
% Visualize slices mesh

cFigure;
subplot(1,2,1); hold on;
hp1=gpatch(Fc(~logicSide,:),Vc,'bw','k',1);
hp2=gpatch(Fc(logicSide,:),Vc,'rw','k',1);
legend([hp1 hp2],{'Surface above plane','Surface below plane'})
axisGeom; axis manual; camlight headligth;
colormap gjet;
set(gca,'FontSize',fontSize);

subplot(1,2,2); hold on;
gpatch(Fc(logicSide,:),Vc,'w','k',1);
gpatch(Fc(~logicSide,:),Vc,'w','none',0.25);
hp1=gpatch(Eb,Vc,'none','b',1,3);
hp2=quiverVec(P,n,50,'k');
legend([hp1 hp2],{'Intersection curve','Plane normal vector'})
axisGeom; axis manual; camlight headligth;
set(gca,'FontSize',fontSize);
gdrawnow;

%% Example 2: Slicing while keeping track of surface color data
% Create example surface data consisting of faces F, vertices V, and
% (optionally) color data C.

[F,V]=stanford_bunny; %Get faces and vertices

%Create example color data
FV=patchCentre(F,V)/5; %Face centre coordinates
C=sin(FV(:,1)); %Optional surface color data to "keep hold off"

snapTolerance=mean(patchEdgeLengths(F,V))/100;
n=vecnormalize([0 0 1]); %Normal direction to plane
P=mean(V,1); %Point on plane

%Slicing surface
[Fc,Vc,Cc,logicSide,Eb]=triSurfSlice(F,V,C,P,n,snapTolerance);

%%
% Visualize color data handling

cFigure;
subplot(1,2,1); hold on;
title('original color data');
gpatch(F,V,C,'k',1);
axisGeom; axis manual; camlight headligth;
set(gca,'FontSize',fontSize);
colormap gjet;

subplot(1,2,2); hold on;
title('color data on "cut" surface');
gpatch(Fc,Vc,Cc,'k',1);
gpatch(Eb,Vc,'none','k',1,3);
axisGeom; axis manual; camlight headligth;
set(gca,'FontSize',fontSize);
gdrawnow;

%% Example 3: Slicing a surface and keeping one side
% Create example surface data consisting of faces F, vertices V

[F,V]=stanford_bunny; %Get faces and vertices
snapTolerance=mean(patchEdgeLengths(F,V))/100;
n=vecnormalize([0 0 1]); %Normal direction to plane
P=mean(V,1); %Point on plane

%Slicing surface
[Fc,Vc,Cc,logicSide,Eb]=triSurfSlice(F,V,[],P,n,snapTolerance);

%Keep only part under plane and clean up
[Fcc,Vcc,indFix]=patchCleanUnused(Fc(logicSide,:),Vc);
Eb_cc=indFix(Eb);

%%
% Visualize slices mesh

cFigure; hold on;
hp1=gpatch(F,V,'kw','none',0.25);
hp2=gpatch(Fcc,Vcc,'rw','k',1);
gpatch(Eb_cc,Vcc,'none','b',1,3);
legend([hp1 hp2],{'Original','Cut surface'})
axisGeom; axis manual; camlight headligth;
colormap gjet;
set(gca,'FontSize',fontSize);
gdrawnow;

%% Example 4: Demo animation for changing orientation
% Visualizing slicing operation for varying angles

hf=cFigure;
subplot(1,2,1); hold on;
hp4=gpatch(Fc,Vc,logicSide,'k',1);
axisGeom; axis manual; camlight headligth;
colormap gjet;

subplot(1,2,2); hold on;
hp1=gpatch(Fc(logicSide,:),Vc,'w','k',1);
hp2=gpatch(Fc(~logicSide,:),Vc,'w','none',0.25);
hp3=gpatch(Eb,Vc,'none','b',1,3);
hp5=quiverVec(P,n,25,'k');
axisGeom; axis manual; camlight headligth;
gdrawnow;

vn=hp5.Vertices;

nSteps=50; %Number of animation steps

%Create the time vector
animStruct.Time=linspace(0,1,nSteps);

%The vector lengths
a=linspace(0,2*pi,nSteps);
b=linspace(0,2*pi,nSteps);
for q=1:1:nSteps
    R=euler2DCM([a(q) b(q) 0]);
    nn=n*R;
    [Fc,Vc,~,logicSide,Eb]=triSurfSlice(F,V,C,P,nn,snapTolerance);

    vnn=(vn-P)*R+P;
    %Set entries in animation structure
    animStruct.Handles{q}=[hp1 hp1 hp2 hp2 hp3 hp3 hp4 hp4 hp4 hp5]; %Handles of objects to animate
    animStruct.Props{q}={'Vertices','Faces',...
                         'Vertices','Faces',...
                         'Vertices','Faces',...
                         'Vertices','Faces','CData',...
                         'Vertices'...
                         }; %Properties of objects to animate
    animStruct.Set{q}={Vc,Fc(logicSide,:),Vc,Fc(~logicSide,:),Vc,Eb,Vc,Fc,double(logicSide),vnn}; %Property values for to set in order to animate
end
anim8(hf,animStruct);

%% Example 5: Demo animation illustrating "sharp triangle" problem

[F,V]=quadBox([1 1 1],[5 5 5]);
[F,V]=quad2tri(F,V,'x');
FV=patchCentre(F,V)/5;
C=sin(FV(:,1))+sin(FV(:,2))+sin(FV(:,3));
snapTolerance=mean(patchEdgeLengths(F,V))/50; %Snapping tolerance
n=vecnormalize([0 0 1]); %Plane normal vector

P=mean(V,1); %Point on plane

[Fc,Vc,Cc,logicSide]=triSurfSlice(F,V,C,P,n,snapTolerance);
Eb=patchBoundary(Fc(logicSide,:));

%%
% Visualizing slicing operation and sharp triangles. Animate this figure to
% view effect.

hf=cFigure;
subplot(1,2,1); hold on;
hp4=gpatch(Fc,Vc,logicSide,'k',1,3);
axisGeom; axis manual; camlight headligth;
colormap gjet; axis off;
view(-90,0); zoom(1.25);

subplot(1,2,2); hold on;
hp1=gpatch(Fc(logicSide,:),Vc,'w','k',1,3);
hp2=gpatch(Fc(~logicSide,:),Vc,'w','none',0.25);
hp3=gpatch(Eb,Vc,'none','b',1,4);

axisGeom; axis manual; camlight headligth;
view(-90,0); zoom(1.25);axis off;
gdrawnow;

nSteps=75; %Number of animation steps
animStruct.Time=linspace(0,1,nSteps); %Create the time vector
z=linspace(min(V(:,3)),max(V(:,3)),nSteps);
for q=1:1:nSteps
    P=[0 0 z(q)];
    [Fc,Vc,~,logicSide,Eb]=triSurfSlice(F,V,C,P,n,snapTolerance);

    %Set entries in animation structure
    animStruct.Handles{q}=[hp1 hp1 hp2 hp2 hp3 hp3 hp4 hp4 hp4]; %Handles of objects to animate
    animStruct.Props{q}={'Vertices','Faces',...
                         'Vertices','Faces',...
                         'Vertices','Faces',...
                         'Vertices','Faces','CData'...
                         }; %Properties of objects to animate
    animStruct.Set{q}={Vc,Fc(logicSide,:),Vc,Fc(~logicSide,:),Vc,Eb,Vc,Fc,double(logicSide)}; %Property values for to set in order to animate
end
anim8(hf,animStruct);

%% Example 6: Cutting non-closed surfaces

[F,V]=graphicsModels(10); %Get faces and vertices

snapTolerance=mean(patchEdgeLengths(F,V))/100;
n=vecnormalize([0 0 1]); %Normal direction to plane
P=mean(V,1); %Point on plane

%Slicing surface
[Fc,Vc,Cc,logicSide,Eb]=triSurfSlice(F,V,C,P,n,snapTolerance);

%%
% Visualize slices mesh

cFigure; hold on;
gpatch(Fc,Vc,logicSide,'k',1);
gpatch(Eb,Vc,'none','g',1,3);
axisGeom; axis manual; camlight headligth;
colormap gjet;
set(gca,'FontSize',fontSize);
gdrawnow;

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


% This file create a simple regression based network for speech
% dereverberation or enhancement
%
% Created by Xiong Xiao, Temasek Laboratories, NTU, Singapore.
% Last Modified: 08 Feb 2017
%
function layer = genNetworkSeparation_MaskingMagnitude(para)
para.freqBin = (0:1/para.fft_len:0.5)*2*pi; % w = 2*pi*f, where f is the normalized frequency k/N is from 0 to 0.5.
nFreqBin = length(para.freqBin);

% Part 1: generate the BF weight predicting subnet

if para.useWav  % input is row waveform
    layer{1}.name = 'Input';
    layer{end}.inputIdx = 1;
    layer{end}.dim = [1 1]*para.nCh;

    layer{end+1}.name = 'stft';
    layer{end}.prev = -length(layer)+1;
    layer{end}.fft_len = para.fft_len;
    layer{end}.frame_len = para.frame_len;
    layer{end}.frame_shift = para.frame_shift;
    layer{end}.removeDC = para.removeDC;
    layer{end}.win_type = para.win_type;
    layer{end}.dim = [(para.fft_len/2+1)*para.nCh layer{length(layer)+layer{end}.prev}.dim(1)];
    layer{end}.skipBP = 1;  % skip backpropagation

    layer{end+1}.name = 'Affine';       % scaling the Fourier transform
    layer{end}.prev = -1;
    layer{end}.W = [];
    layer{end}.b = [];
    layer{end}.dim = [1 1] * layer{length(layer)+layer{end}.prev}.dim(1);
    layer{end}.update = 0;
    layer{end}.skipBP = 1;  % skip backpropagation

    % extract a subset of dimensions for prediction. Not used currently, so we
    % will use all the dimensions.
    layer{end+1}.name = 'ExtractDims';
    layer{end}.prev = -1;
    layer{end}.dimIndex = 1:nFreqBin*para.nCh;
    layer{end}.dim = [length(layer{end}.dimIndex) layer{length(layer)+layer{end}.prev}.dim(1)];
    layer{end}.skipBP = 1;

    % get the log power spectrum and perform CMN
    layer{end+1}.name = 'Power';
    layer{end}.prev = -1;
    layer{end}.dim = [1 1] * layer{end-1}.dim(1);
    layer{end}.skipBP = 1;

    layer{end+1}.name = 'log';
    layer{end}.const = 0.01;
    layer{end}.prev = -1;
    layer{end}.dim = [1 1]*layer{end-1}.dim(1);
    layer{end}.skipBP = 1;

else    % input is log spectrogram
    layer{1}.name = 'Input';
    layer{end}.inputIdx = 1;
    layer{end}.dim = [1 1]*(para.fft_len/2+1)*para.nCh;
end

outputDim = nFreqBin;

switch para.NetType
    case 'DNN'
        layer{end+1}.name = 'Splice';
        layer{end}.prev = -1;
        layer{end}.context = para.contextSize;
        layer{end}.dim = [layer{end}.context 1]*layer{length(layer)+layer{end}.prev}.dim(1);
        layer{end}.skipBP = 1;

        layer{end+1}.name = 'Affine';       % scaling the Fourier transform
        layer{end}.prev = -1;
        layer{end}.W = [];
        layer{end}.b = [];
        layer{end}.dim = [1 1] * layer{length(layer)+layer{end}.prev}.dim(1);
        layer{end}.update = 0;

        layerRegression = genNetworkFeedForward_v2(layer{end}.dim(1), para.hiddenLayerSize, outputDim, 'mse', 'sigmoid');
    case 'LSTM'
        layer{end+1}.name = 'Affine';       % scaling the Fourier transform
        layer{end}.prev = -1;
        layer{end}.W = [];
        layer{end}.b = [];
        layer{end}.dim = [1 1] * layer{length(layer)+layer{end}.prev}.dim(1);
        layer{end}.update = 0;

        tmpTopology.inputDim = layer{end}.dim(1);
        tmpTopology.hiddenLayerSizeLSTM = para.hiddenLayerSize;
        tmpTopology.usePastState = zeros(1,length(para.hiddenLayerSize)); % do not use peeping hole
        tmpTopology.hiddenLayerSizeFF = [];
        tmpTopology.outputDim = outputDim;
        tmpTopology.costFn = 'mse';
        tmpTopology.LastActivation4MSE = 'sigmoid';
        layerRegression = genNetworkLSTM(tmpTopology);
end
layer = [layer layerRegression(2:end-2)];

if ~isempty(para.hiddenLayerSizeFF)
    layerFF = genNetworkFeedForward_v2(para.hiddenLayerSize(end), para.hiddenLayerSizeFF, outputDim, 'mse', 'sigmoid');
    layer = [layer(1:end-1) layerFF(2:end-2)];
end

layer = [layer layer(end-1:end)];
layer{end-1}.prev = -3;
mask_idx = [-2 0] + length(layer);

% apply the masking
if para.useWav  % input is row waveform
    power_idx = ReturnLayerIdxByName(layer, 'power');

    layer{end+1}.name = 'sqrt';
    layer{end}.prev = power_idx-length(layer);
    layer{end}.dim = [1 1]*layer{power_idx}.dim(1);
    layer{end}.skipBP= 1;

    mag_idx = length(layer);
else
    input_idx = ReturnLayerIdxByName(layer, 'input');
    layer{end+1}.name = 'exp';
    layer{end}.prev = input_idx(1)-length(layer);
    layer{end}.dim = [1 1]*layer{input_idx(1)}.dim(1);
    power_idx = length(layer);
end

MaskingLayer{1}.name = 'hadamard';
MaskingLayer{end}.dim = [1 1]*layer{end}.dim(1);
MaskingLayer{end}.prev = [mask_idx(1) mag_idx(1)] - length(layer) -1;

layer = [layer MaskingLayer];
output_idx(1) = length(layer);

MaskingLayer2 = MaskingLayer;
MaskingLayer2{1}.prev = [mask_idx(2) mag_idx(1)] - length(layer) -1;

layer = [layer MaskingLayer2];
output_idx(2) = length(layer);

if para.useWav
    power_idx = ReturnLayerIdxByName(layer, 'power');
    CleanLayer = layer(1:power_idx(1));
    CleanLayer{1}.inputIdx = 2;
    CleanLayer{1}.dim(:) = 1;
    CleanLayer{2}.dim(:) = CleanLayer{2}.dim(:)/para.nCh;
    CleanLayer{3}.dim(:) = CleanLayer{3}.dim(:)/para.nCh;
    CleanLayer(4) = [];
else
    CleanLayer{1}.name = 'Input';
    CleanLayer{end}.inputIdx = 2;
    CleanLayer{end}.dim = [1 1]*(para.fft_len/2+1);
end
CleanLayer{end+1}.name = 'sqrt';
CleanLayer{end}.prev = -1;
CleanLayer{end}.dim = [1 1]*CleanLayer{end-1}.dim(1);

for i=1:length(CleanLayer)
    CleanLayer{i}.skipBP = 1;
end

layer = [layer CleanLayer];
clean1_idx = length(layer);
CleanLayer{1}.inputIdx = 3;
layer = [layer CleanLayer];
clean2_idx = length(layer);

layer{end+1}.name = 'mixture_MSE';
layer{end}.prev = [output_idx clean1_idx clean2_idx] - length(layer);
layer{end}.dim = [1 layer{length(layer)+layer{end}.prev(1)}.dim(1)];

layer = FinishLayer(layer);
end


function output=fcn_LeaderAcceleration(u)
global leaderNum dim samplePos_all nodenum formationType
global rotateFlag tempp2 tempp3

u_all = reshape(u(1:dim*nodenum*3),dim,nodenum*3);
v_all=u_all(:,1:nodenum);
p_all=u_all(:,nodenum+1:2*nodenum);
control_all=u_all(:,2*nodenum+1:3*nodenum);
currentTime=u(end);
%output
a_leader_all=zeros(dim,leaderNum);

% speed up to speed=1
time_start_speedup=0;
time_span_speedup=5;
if currentTime>time_start_speedup && currentTime<time_start_speedup+time_span_speedup
    ax=fcn_sin(time_start_speedup,time_span_speedup,currentTime)/pi;
    a_leader_all(:,1)=[ax,0]';
    a_leader_all(:,2)=[ax,0]';
    a_leader_all(:,3)=[ax,0]';
end

% slow down to speed=0
time_start_slowdown=10;
time_span_slowdown=5;
if currentTime>time_start_slowdown && currentTime<time_start_slowdown+time_span_slowdown
    %ax=fcn_normalDistribution(time_start_slowdown,time_span_slowdown, 2, currentTime);
    ax=-fcn_sin(time_start_slowdown,time_span_slowdown,currentTime)/pi;
    a_leader_all(:,1)=[ax,0]';
    a_leader_all(:,2)=[ax,0]';
    a_leader_all(:,3)=[ax,0]';
end

% rotate
v3=v_all(:,3);
v2=v_all(:,2);
v1=v_all(:,1);
p3=p_all(:,3);
p2=p_all(:,2);
p1=p_all(:,1);
R90=[cos(pi/2),-sin(pi/2);
     sin(pi/2),cos(pi/2)];
time_start_rotate=20;
time_span_rotate=10;
if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate
    if rotateFlag==1
        tempp2=p2;
        tempp3=p3;
        rotateFlag=0;
    end
    % agent 2
    acce_mag=-fcn_SlowDownSpeedUp(time_start_rotate, time_span_rotate, currentTime, 4);
%     acce_vec=(p1+R90'*(tempp2-p1))-tempp2;
%     acce_vec=acce_vec/norm(acce_vec);
    acce_vec=[1 0]';
    a_leader_all(:,2)=acce_mag*acce_vec;
    % agent 3
    acce_mag=-fcn_SlowDownSpeedUp(time_start_rotate, time_span_rotate, currentTime, 4);
%     acce_vec=(p1+R90'*(tempp3-p1))-tempp3;
%     acce_vec=acce_vec/norm(acce_vec);
    acce_vec=[0 1]';
    a_leader_all(:,3)=acce_mag*acce_vec;
end

% move downward
time_start_downward=time_start_rotate;
time_span_downward=time_span_rotate;
if currentTime>=time_start_downward && currentTime<=time_start_downward+time_span_downward
    % agent 1
    ay=fcn_sin(time_start_downward,time_span_downward,currentTime)/pi;
    a_leader_all(:,1)=[0,-ay]';
    % agent 2
    a_leader_all(:,2)=a_leader_all(:,2)+[0,-ay]';
    % agent 3
    a_leader_all(:,3)=a_leader_all(:,3)+[0,-ay]';
end

% slow down to speed=0
time_start_slowdown2=time_start_downward+time_span_downward+5;
time_span_slowdown2=5;
if currentTime>time_start_slowdown2 && currentTime<time_start_slowdown2+time_span_slowdown2
    %ax=fcn_normalDistribution(time_start_slowdown,time_span_slowdown, 2, currentTime);
    ay=-fcn_sin(time_start_slowdown2,time_span_slowdown2,currentTime)/pi;
    a_leader_all(:,1)=[0,-ay]';
    a_leader_all(:,2)=[0,-ay]';
    a_leader_all(:,3)=[0,-ay]';
end

% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate
%     % agent 3
%     aUnit=(p1-p3)/norm(p1-p3);
%     tht_dot=fcn_sin(time_start_rotate,time_span_rotate,currentTime);
%     speed_dot=fcn_sin(time_start_rotate,time_span_rotate,currentTime);
%     a_leader_all(:,3)=aUnit*tht_dot*norm(v3)+speed_dot*R90*aUnit;
%     % agent 2
%     aUnit=(p1-p2)/norm(p1-p2);
%     %tht_dot=fcn_sin(time_start_rotate,time_span_rotate,currentTime);
%     speed_dot=fcn_sin(time_start_rotate,time_span_rotate,currentTime);
%     a_leader_all(:,2)=aUnit*tht_dot*norm(v2)+speed_dot*R90*aUnit;
% end

% % scaling
% a_leader_all(2,2)=-fcn_TanhManeuver(20, 1, 10, currentTime);
% a_leader_all(2,3)=fcn_TanhManeuver(20, 1, 10, currentTime);

% % rotation
% v3=v_all(:,3);
% v2=v_all(:,2);
% v1=v_all(:,1);
% p3=p_all(:,3);
% p2=p_all(:,2);
% p1=p_all(:,1);
% R90=[cos(pi/2),-sin(pi/2);
%      sin(pi/2),cos(pi/2)];
% time_start_rotate=20;
% % agent 3
% time_span_rotate3=10;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate3
%     if rotateCenterFlag==1
%         roateCenter(:,3)=p3-[0,5]';
%         rotateCenterFlag=0;
%         r3=norm(roateCenter(:,3)-p3);
%         roateCenter(:,2)=roateCenter(:,3);
%         r2=norm(roateCenter(:,2)-p2);
%         roateCenter(:,1)=roateCenter(:,3);
%         r1=norm(roateCenter(:,1)-p1);
%     end
%     tht_dot=pi/time_span_rotate3;
%     speed_dot=0;
%     a_leader_all(:,3)=R90'*v3/norm(v3)*tht_dot*norm(v3)+speed_dot*v3/norm(v3);
% end
% % agent 2
% time_span_rotate2=time_span_rotate3;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate2
%     tht_dot=pi/time_span_rotate2;
%     if currentTime>=time_start_rotate && currentTime<=time_start_rotate+1
%         speed_dot=fcn_sin(time_start_rotate,1,currentTime)/2.6;
%     else
%         speed_dot=0;
%     end
%     a_leader_all(:,2)=R90'*v2/norm(v2)*tht_dot*norm(v2)+speed_dot*v2/norm(v2);
% end
% if currentTime>=time_start_rotate+time_span_rotate2 && currentTime<=time_start_rotate+time_span_rotate2+1
%         speed_dot=-fcn_sin(time_start_rotate,1,currentTime)/2.6;
%         tht_dot=0;
%         a_leader_all(:,2)=R90'*v2/norm(v2)*tht_dot*norm(v2)+speed_dot*v2/norm(v2);
% end
% % agent 1
% time_span_rotate1=time_span_rotate3;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate1
%     tht_dot=pi/time_span_rotate3;%fcn_sin(time_start_rotate,time_span_rotate1,currentTime);
%     if currentTime>=time_start_rotate && currentTime<=time_start_rotate+1
%         speed_dot=fcn_sin(time_start_rotate,1,currentTime)/3;
% %         tht_dot=
%     else
%         speed_dot=0;
%     end
%     aUnit=(roateCenter(:,1)-p1)/norm(roateCenter(:,1)-p1);
%     a_leader_all(:,1)=aUnit*tht_dot*norm(v1)+speed_dot*v1/norm(v1);
% end
% if currentTime>=time_start_rotate+time_span_rotate2 && currentTime<=time_start_rotate+time_span_rotate2+1
%         speed_dot=-fcn_sin(time_start_rotate,1,currentTime)/2.6;
%         tht_dot=0;
%         a_leader_all(:,2)=R90'*v2/norm(v2)*tht_dot*norm(v2)+speed_dot*v2/norm(v2);
% end

% % agent 1: distance-based formation control law
% time_span_rotate1=time_span_rotate3;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate1
%     delta12=norm(p1-p2)-2*sqrt(2);
%     delta13=norm(p1-p3)-2*sqrt(2);
%     a_leader_all(:,1)=5*(delta12*(p2-p1)+delta13*(p3-p1)) + 0.5*(delta12*(v2-v1)+delta13*(v3-v1));
% end

% % agent 2
% time_span_rotate2=26;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate2
%     v2=v_all(:,2);
%     p2=p_all(:,2);
%     R90=[cos(pi/2),-sin(pi/2);
%              sin(pi/2),cos(pi/2)];
%     tht_dot=fcn_sin(time_start_rotate,time_span_rotate2,currentTime);
%     speed_dot=0;
%     a_leader_all(:,2)=R90'*v2/norm(v2)*tht_dot*norm(v2)+speed_dot*v2/norm(v2);
% end
% % agent 1
% time_span_rotate1=18;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate1
%     v1=v_all(:,1);
%     p1=p_all(:,1);
%     R90=[cos(pi/2),-sin(pi/2);
%              sin(pi/2),cos(pi/2)];
%     tht_dot=fcn_sin(time_start_rotate,time_span_rotate1,currentTime);
%     speed_dot=0;
%     a_leader_all(:,1)=R90'*v1/norm(v1)*tht_dot*norm(v1)+speed_dot*v1/norm(v1);
% end
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate
%     v3=v_all(:,3);
%     v2=v_all(:,2);
%     v1=v_all(:,1);
%     p3=p_all(:,3);
%     p2=p_all(:,2);
%     p1=p_all(:,1);
%     R90=[cos(pi/2),-sin(pi/2);
%          sin(pi/2),cos(pi/2)];
%     % agent 3
%     tht_dot=-fcn_normalDistribution(time_start_rotate,time_span_rotate, 3.15, currentTime);
%     speed_dot=0;
%     a_leader_all(:,3)=R90'*v3/norm(v3)*tht_dot*norm(v3)+speed_dot*v3/norm(v3);
%     % agent 2
%     tht_dot=-fcn_normalDistribution(time_start_rotate,time_span_rotate*1.5, 3.15, currentTime);
%     speed_dot=0;
%     a_leader_all(:,2)=R90'*v2/norm(v2)*tht_dot*norm(v2)+speed_dot*v2/norm(v2);
%     % agent 1
%     tht_dot=-fcn_normalDistribution(time_start_rotate,time_span_rotate*, 2, currentTime);
%     speed_dot=0;%fcn_normalDistribution(time_start_rotate,time_span_rotate, 1, currentTime);
%     a_leader_all(:,1)=R90'*v1/norm(v1)*tht_dot*norm(v1)+speed_dot*v1/norm(v1);
% end

% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate
%     offset=4;
%     v3=v_all(:,3);
%     v2=v_all(:,2);
%     v1=v_all(:,1);
%     p3=p_all(:,3);
%     p2=p_all(:,2);
%     p1=p_all(:,1);
%     R90=[cos(pi/2),-sin(pi/2);
%          sin(pi/2),cos(pi/2)];
%     threshhold=0.005;
%     if rotateCenterFlag==1
%         roateCenter(:,3)=p3-[0,offset]';
%         rotateCenterFlag=0;
%         r3=norm(roateCenter(:,3)-p3);
%         rotateTime(3)=pi*r3/norm(v3);
%         roateCenter(:,2)=roateCenter(:,3);
%         r2=norm(roateCenter(:,2)-p2);
%         rotateTime(2)=pi*r2/norm(v2);
% %         roateCenter(:,1)=roateCenter(:,3)+(p1-p3);
%         roateCenter(:,1)=roateCenter(:,2)+[p1(1)-p2(1),0]';
%         r1=norm(roateCenter(:,1)-p1);
%         rotateTime(1)=pi*r1/norm(v1);
%     end
%     % agent 3
%     tht=atan2(v3(2),v3(1));
%     if ~(tht>pi-threshhold || tht<-pi+threshhold)
%         r3=norm(roateCenter(:,3)-p3);
%         aUnit=(roateCenter(:,3)-p3)/norm(roateCenter(:,3)-p3);
%         a_leader_all(:,3)=aUnit*norm(v3)^2/r3;%+speed_dot*v3/norm(v3);
%     end
%     % agent 2
%     tht=atan2(v2(2),v2(1));
%     if ~(tht>pi-threshhold || tht<-pi+threshhold)
%         r2=norm(roateCenter(:,2)-p2);
%         aUnit=(roateCenter(:,2)-p2)/norm(roateCenter(:,2)-p2);
%         speed_dot=-fcn_SlowDownSpeedUp(time_start_rotate, 5, currentTime, 8);
%         a_leader_all(:,2)=aUnit*norm(v2)^2/r2;%+speed_dot*v2/norm(v2);
%     end
%     % agent 1
%     tht=atan2(v1(2),v1(1));
%     if ~(tht>pi-threshhold || tht<-pi+threshhold)
%         r1=norm(roateCenter(:,1)-p1);
%         aUnit=(roateCenter(:,1)-p1)/norm(roateCenter(:,1)-p1);
%         speed_dot=-fcn_SlowDownSpeedUp(time_start_rotate, 5, currentTime, 8);
%         a_leader_all(:,1)=aUnit*norm(v1)^2/r1;%+speed_dot*v1/norm(v1);
%     end
% end

% % slow down and speed up to align the agents
% time_start_align=20;
% time_span=5;
% if currentTime>time_start_align && currentTime<time_start_align+time_span
% %     % agent 3: back 4 meters
% %     distance_change=4;
% %     a_leader_all(2,3)=-fcn_SlowDownSpeedUp(time_start_align, time_span, currentTime, distance_change);
% %     % agent 2: foward 2 meters
% %     distance_change=2.3;
% %     a_leader_all(2,2)=fcn_SlowDownSpeedUp(time_start_align, time_span, currentTime, distance_change);
%     % agent 1 slow down
%     distance_change=2;
%     a_leader_all(1,1)=fcn_SlowDownSpeedUp(time_start_align, time_span, currentTime, distance_change);
% end

% output: replace the leaders' velocity with the one prescribed above
control_all(:,1:leaderNum) = a_leader_all;
output=reshape(control_all, dim*nodenum, 1);


function D = LRSDL_updateD(Y, D, D0, Y_range, D_range, opts, X, X0, pars)
    if nargin == 0
        addpath('../utils');
        addpath('../sparse_coding');
        tic
        d = 30;
        N = 7;
        k = 5;
        k0 = 5;
        C = 3 ;
        Y = normc(rand(d,N*C));
        D = normc(rand(d,k*C));
        D0 = normc(rand(d, k0));
        Y_range = N* (0:C);
        D_range = k* (0:C);
        opts.lambda1 = 0.01;
        opts.lambda2 = 0.002;
        opts.lambda3 = 0.1;
        opts.max_iter = 250;
        opts.show = true;
        X = randn(size(D,2), size(Y,2));
        X0 = randn(size(D0, 2), size(Y, 2));
    end

    Z = Y - D0*X0;
    W = Z - D*X;

    %% ========= update D1 ==============================

    X1 = X(1:D_range(2), :);
    X11 = X1(:, 1: Y_range(2));
    D1 = D(:, 1: D_range(2));
    W = W + D1*X1;
    E = W*X1' + get_block_col(Z, 1, Y_range)*X11';
    F = 2*X1*X1';
    D_prev = updateD_EF(D1, E, F, pars.max_iter);
    D(:, 1: D_range(2)) = D_prev;

    %% ========= update Di, i > 1 ==============================
    for i = 2: numel(Y_range) - 1
        D_cur = get_block_col(D, i, D_range);
        D_com = [-D_prev, D_cur];
        X_com = X(D_range(i-1) + 1: D_range(i+1), :);
        W = W + D_com*X_com; %W = W - D_{i-1}X^{i-1} + D_iX^i
        Xi = X(D_range(i) + 1: D_range(i+1), :);
        Xii = Xi(:, Y_range(i) + 1: Y_range(i+1));
        E = W*Xi' + get_block_col(Z, i, Y_range)*Xii';
        F = 2*Xi*Xi';
        D_prev = updateD_EF(D_cur, E, F, pars.max_iter);
        D(:, D_range(i) + 1: D_range(i+1)) = D_prev;
    end
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function varargout = regularizer(varargin)
%
% Main function for the regularization model, uses persistent parameter
%
% The regularization functional is either based on a linear differential
% operator B, i.e.
%
%        S(Y) = 0.5*alpha*hd*|B*Y|^2
%
%  or a general (non-linear) functional S(Y)
%
% Typical call
%    [Sc,dS,d2S] = regularizer(Y,omega,m)
%
%  1.     initialize (pick regularizer and set parameters)
%
%         regularizer('reset','regularizer','mfElastic','alpha',1e3,...
%               'mu',1,'lambda',0','rigid2D','center',center);
%
%  2.     [Sc,dS,d2S] = regularizer(Y,omega,m)
%        evaluates the regularization model, e.g. Sc = 0.5*yc'*B'*B*yc and its derivatives
%
%  A = alpha*hd*B'*B is made persistent for eficiency
%  hd = prod(omega./m)
%  an option grid is introduce to indicate the appropriate discretization:
%  staggered for elastic and diffusive,
%  cell-centered for curvature
%  nodal for TV, hyper elastic or diffusive EPI regularization
%==============================================================================

function varargout = regularizer(varargin)

persistent OPTN A

if nargin == 0 && nargout == 0 && isempty(OPTN),
    help(mfilename);
    return;
end;

% -----------------------------------------------------------------------------
% handle options
[method,OPTN,task,stop] = dealOptions(mfilename,OPTN,varargin{:});

% check the grid according to the regularizer to be used
% setup default solver for Gauss-Newton systems

if strcmp(task,'set') || strcmp(task,'reset'),

    switch method,
        case 'mfElastic',
            scheme     = 'elastic';
            matrixFree = 1;
            grid       = 'staggered';
            solver     = 'MG-elastic';

        case 'mbElastic',
            scheme     = 'elastic';
            matrixFree = 0;
            grid       = 'staggered';
            solver     = 'backslash';

        case 'mbElasticNodal',
            scheme     = 'elasticNodal';
            matrixFree = 0;
            grid       = 'nodal';
            solver     = 'backslash';

        case 'mfElasticNodal',
            scheme     = 'elasticNodal';
            matrixFree = 1;
            grid       = 'nodal';
            solver     = 'PCG';

        case 'mbCurvature',
            scheme     = 'curvature';
            matrixFree = 0;
            grid       = 'cell-centered';
            solver     = 'backslash';

        case 'mfCurvature',
            scheme     = 'curvature';
            matrixFree = 1;
            grid       = 'cell-centered';
            solver     = 'PCG-curvature';

        case 'mbHyperElastic',
            scheme     = 'hyperElastic';
            matrixFree = 0;
            grid       = 'nodal';
            solver     = 'backslash';

        case 'mfHyperElastic',
            scheme     = 'hyperElastic';
            matrixFree = 1;
            grid       = 'nodal';
            solver     = 'PCG-hyperElastic';

        case 'mbHyperElasticFEM',
            scheme     = 'hyperElasticFEM';
            matrixFree = 0;
            grid       = 'FEM';
            solver     = 'backslash';

        case 'mbElasticFEM',
            scheme     = 'elasticFEM';
            matrixFree = 0;
            grid       = 'FEM';
            solver     = 'backslash';

      case 'mfElasticFEM',
            scheme     = 'elasticFEM';
            matrixFree = 1;
            grid       = 'FEM';
            solver     = 'MG';

        otherwise
            scheme = method;
            [grid,matrixFree,solver] = feval(scheme,'para',[],[],OPTN{:});
    end;

    [dummy,OPTN] = dealOptions(mfilename,OPTN,'set','scheme',scheme,...
        'grid',grid,'matrixFree',matrixFree,'solver',solver);
end;

% return, if no further work has to be handled
if stop,
    varargout{1} = method;
    if nargout > 1, varargout{2} = OPTN;  varargout{3} = A;  end;
    return;
end
% -----------------------------------------------------------------------------
% do the work

% extract regularization parameters
scheme      = dealOptions(mfilename,OPTN,'get','scheme');
alpha       = dealOptions(mfilename,OPTN,'get','alpha');
matrixFree  = dealOptions(mfilename,OPTN,'get','matrixFree');

% extract variables
yc     = varargin{1};
omega  = varargin{2};
m      = varargin{3};
doDerivative = (nargout>1);
varargin = varargin(4:end);

for k=1:2:length(varargin), % overwrites defaults
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;
[Sc,dS,d2S] = feval(scheme,yc,omega,m,...
    'alpha',alpha,'matrixFree',matrixFree,...
    'doDerivative',doDerivative,OPTN{:},varargin{:});
varargout = {Sc,dS,d2S};

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

function runMinimalExample
fprintf('run minimal example for %s\n',mfilename)
keyboard
%==============================================================================


t1 = t1/1000;
sl = 181;

figure;imagesc(ni.data(:,:,sl));axis image;colormap gray;colorbar;title('bob');
figure;imagesc(t1(:,:,sl));axis image;colormap gray;colorbar;title('deoni');

wm = roipoly;
[x,y] = ind2sub(ni.dim(1:2),find(wm));
wmInds = sub2ind(ni.dim(1:3),x,y,ones(size(x))*sl);
gm = roipoly;
[x,y] = ind2sub(ni.dim(1:2),find(gm));
gmInds = sub2ind(ni.dim(1:3),x,y,ones(size(x))*sl);

dm = [mean(t1(wmInds)) std(t1(wmInds)); mean(t1(gmInds)) std(t1(gmInds))];
km = [mean(ni.data(wmInds)),std(ni.data(wmInds)); mean(ni.data(gmInds)) std(ni.data(gmInds))];

(dm(2,1)-dm(1,1))/mean(dm(:,2))

(km(2,1)-km(1,1))/mean(km(:,2))

% Save a bunch of slices
sl = 115;

im = flipud(t1(:,:,sl)');
im = im./max(im(:));
imwrite(im,sprintf('images/deoni_T1_%03d.png',sl));

im = flipud(ni.data(:,:,sl)');
im = im./max(im(:));
imwrite(im,sprintf('images/bob_T1_%03d.png',sl));

allData = cat(4,s(:).imData);
[img, clipVals] = mrAnatHistogramClip(allData(:),.4,.99);
clear img allData;
for(ii=1:length(s))
   im = flipud(s(ii).imData(:,:,sl)');
   im(im>clipVals(2)) = clipVals(2);
   im(im<clipVals(1)) = clipVals(1);
   im = im-clipVals(1);
   im = im./max(im(:));
   if(ii==find(tiInds))
      imwrite(im,sprintf('images/raw_IR_%03d.png',sl));
   else
      imwrite(im,sprintf('images/raw_%02ddeg_%03d.png',s(ii).flipAngle,sl));
   end
end


function [lat, long ] = gcj2wgs_exact(gcjLat, gcjLng)
% adapted from https://github.com/googollee/eviltransform

    initDelta = 0.01;
    threshold = 0.000001;
    dLat = initDelta;
    dLng = initDelta;
    mLat = gcjLat - dLat;
    mLng = gcjLng - dLng;
    pLat = gcjLat + dLat;
    pLng = gcjLng + dLng;

    for i=1:30
        wgsLat = (mLat + pLat) / 2;
        wgsLng = (mLng + pLng) / 2;
        [tmplat, tmplng] = wgs2gcj(wgsLat, wgsLng);
        dLat = tmplat - gcjLat;
        dLng = tmplng - gcjLng;
        if all(abs(dLat) < threshold & abs(dLng) < threshold)
            lat = wgsLat;
            long =wgsLng;
            return;
        end
        pLat(dLat>=0) = wgsLat(dLat>=0);
        mLat(dLat<=0) = wgsLat(dLat<=0);
        pLng(dLng>=0) = wgsLng(dLng>=0);
        mLng(dLng<=0) = wgsLng(dLng<=0);
    end

   lat = wgsLat;
   long =wgsLng;

end


function varargout = EnvironmentalSelection(varargin)
% The environmental selection of PM-MOEA

%------------------------------- 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".
%--------------------------------------------------------------------------

    %% Delete duplicated solutions and non-dominated sorting
    if nargin == 3
        [PopDec,PopObj,N] = deal(varargin{:});
        [PopDec,uni] = unique(PopDec,'rows','stable');
        PopObj = PopObj(uni,:);
        [FrontNo,MaxFNo] = NDSort(PopObj,N);
    elseif nargin == 4
        [Population,Dec,Mask,N] = deal(varargin{:});
        PopObj = Population.objs;
        [FrontNo,MaxFNo] = NDSort(PopObj,Population.cons,N);
    end
    Next = FrontNo <= MaxFNo;

    %% Truncate the solutions in the last front
    Last = find(FrontNo==MaxFNo);
    if nargin == 3
        Del = Truncation(double(PopDec(Last,:)),sum(Next)-N);
    elseif nargin == 4
        Del = Truncation(PopObj(Last,:),sum(Next)-N);
    end
    Next(Last(Del)) = false;

    %% Population for next generation
    FrontNo = FrontNo(Next);
    if nargin == 3
        PopDec    = PopDec(Next,:);
        PopObj    = PopObj(Next,:);
        varargout = {PopDec,PopObj,FrontNo};
    elseif nargin == 4
        Population = Population(Next);
        Dec        = Dec(Next,:);
        Mask       = Mask(Next,:);
        varargout  = {Population,Dec,Mask,FrontNo};
    end
end

function Del = Truncation(PopObj,K)
% Select part of the solutions by truncation

    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Del = false(1,size(PopObj,1));
    while sum(Del) < K
        Remain   = find(~Del);
        Temp     = sort(Distance(Remain,Remain),2);
        [~,Rank] = sortrows(Temp);
        Del(Remain(Rank(1))) = true;
    end
end


function [dM1m dM2m dM3m Edges dEdges A1m A2m M1m M2m M3m W1m W2m COm dW1m dW2m dCOm dAreasImgm dA1m dA2m dA3m] = showResults2(yOpt, yOpt_inv, corners, filename, images)

load(filename);

m=[2^maxLevel 2^maxLevel];
Omega = [1, 1];
X=getGrid(Omega,m);
h=Omega./m;

yCentered=stg2center(yOpt,m,'Py');

% % Check whether yOpt + yOpt_inv generaets original image
% dA1i1=interpolation(TD1,Omega,yCentered);
% dA1m1=flipud(reshape(dA1i1,m)');
%
% dA1i2=interpolation(dA1m1,Omega,yOpt_inv);
% dA1m2=flipud(reshape(dA1i2,m)');
%
% figure
% subplot(311)
% imagesc(TD1)
% subplot(312)
% imagesc(dA1m1)
% subplot(313)
% imagesc(dA1m2)

% interpolate raw data
interpolation('set','MODE','linear-periodic','period',2*pi);
M2i=interpolation(images.M2,Omega,X);
dA2i=interpolation(TD2,Omega,yCentered);
interpolation('set','MODE','linear');
% M1i=interpolation(RD1,Omega,X);
M1i=interpolation(images.M1,Omega,X);
M3i=interpolation(images.M3,Omega,X);
dA1i=interpolation(TD1,Omega,yCentered);
if isfield(images,'A3')
    dA3i=interpolation(images.A3,Omega,yCentered);
    dA3m=ip2mat(dA3i,m);
end

if max(areasImg(:))==6 % LO/TO maps
    areasImg = areaNumTransform(areasImg);
end
dAreasImgi=(interpolation(areasImg,Omega,yCentered));
W1i=interpolation(WD1,Omega,X);
W2i=interpolation(WD2,Omega,X);
COi=interpolation(images.CO,Omega,X);

M2m=ip2mat(M2i,m);
M1m=ip2mat(M1i,m);
M3m=ip2mat(M3i,m);
dA1m=ip2mat(dA1i,m);
dA2m=ip2mat(dA2i,m);
W1m=ip2mat(W1i,m);
W2m=ip2mat(W2i,m);
COm=ip2mat(COi,m);
dAreasImgm=ip2mat(dAreasImgi,m);

% transform raw data into atlas space
interpolation('set','MODE','linear-periodic','period',2*pi);
dM2i=interpolation(images.M2,Omega,yOpt_inv);
A2i=interpolation(TD2,Omega,X);
interpolation('set','MODE','linear');
dM1i=interpolation(images.M1,Omega,yOpt_inv);
dM3i=interpolation(images.M3,Omega,yOpt_inv);
A1i=interpolation(TD1,Omega,X);
dW1i=interpolation(WD1,Omega,yOpt_inv);
dW2i=interpolation(WD2,Omega,yOpt_inv);
dCOi=interpolation(COm,Omega,yOpt_inv);
AreasImgi=(interpolation(areasImg,Omega,X));

dM2m=ip2mat(dM2i,m);
dM1m=ip2mat(dM1i,m);
dM3m=ip2mat(dM3i,m);
A1m=ip2mat(A1i,m);
A2m=ip2mat(A2i,m);
AreasImgm=ip2mat(AreasImgi,m);
dW1m=ip2mat(dW1i,m);
dW2m=ip2mat(dW2i,m);
dCOm=ip2mat(dCOi,m);

% remove walls to draw edges
% areasImg(find(areasImg==5))=NaN;
% areasImg(find(areasImg==6))=NaN;

% get interporated areas of interest
OV=ceil(interpolation(areasImg,Omega,X));
OVm=ip2mat(OV,m);
OVm(isnan(OVm))=0;
if isfield(images,'A3') % for LO-1/2, TO-1/2
    Edges = detectAreaBorders(OVm);
else % for V1-3
    Edges=double(edge(OVm, 'Canny'));
end

% get deformed areas of interest
OV=round(interpolation(areasImg,Omega,yCentered));
OVm=ip2mat(OV,m);
OVm(isnan(OVm))=0;
if isfield(images,'A3') % for LO-1/2, TO-1/2
    dEdges = detectAreaBorders(OVm);
else % for V1-3
    dEdges=double(edge(OVm, 'Canny'));
end

% images_size = size(images.A1,1);
% figure
% subplot(231)
% hold on
% imagesc(1:(images_size-1)/(2^maxLevel-1):images_size,1:(images_size-1)/(2^maxLevel-1):images_size,mergedImage(M1m,dEdges,hsv,15))
% % I=flipdim(mergedImage(dM2m,Edges,hsv,2*pi),1);
% % image(h(1)/2:h(1):1,h(2)/2:h(2):1,I);
% axis image; axis xy;axis off;
% set(gca,'YDir','reverse');
%
% subplot(234)
% hold on
% imagesc(1:(images_size-1)/(2^maxLevel-1):images_size,1:(images_size-1)/(2^maxLevel-1):images_size,mergedImage(dM1m,Edges,hsv,15))
% plot(corners{1}(:,1),corners{1}(:,2),'k+','LineWidth',2)
% plot(corners{2}(:,1),corners{2}(:,2),'k+','LineWidth',2)
% plot(corners{3}(:,1),corners{3}(:,2),'k+','LineWidth',2)
% plot(corners{4}(:,1),corners{4}(:,2),'k+','LineWidth',2)
% % I=flipdim(mergedImage(dM2m,Edges,hsv,2*pi),1);
% % image(h(1)/2:h(1):1,h(2)/2:h(2):1,I);
% axis image; axis xy;axis off;
% set(gca,'YDir','reverse');
%
% subplot(232)
% hold on
% imagesc(1:(images_size-1)/(2^maxLevel-1):images_size,1:(images_size-1)/(2^maxLevel-1):images_size,mergedImage(M2m,dEdges,hsv,2*pi))
% % I=flipdim(mergedImage(dM2m,Edges,hsv,2*pi),1);
% % image(h(1)/2:h(1):1,h(2)/2:h(2):1,I);
% axis image; axis xy;axis off;
% set(gca,'YDir','reverse');
%
% subplot(235)
% hold on
% imagesc(1:(images_size-1)/(2^maxLevel-1):images_size,1:(images_size-1)/(2^maxLevel-1):images_size,mergedImage(dM2m,Edges,hsv,2*pi))
% plot(corners{1}(:,1),corners{1}(:,2),'k+','LineWidth',2)
% plot(corners{2}(:,1),corners{2}(:,2),'k+','LineWidth',2)
% plot(corners{3}(:,1),corners{3}(:,2),'k+','LineWidth',2)
% plot(corners{4}(:,1),corners{4}(:,2),'k+','LineWidth',2)
% % I=flipdim(mergedImage(dM2m,Edges,hsv,2*pi),1);
% % image(h(1)/2:h(1):1,h(2)/2:h(2):1,I);
% axis image; axis xy;axis off;
% set(gca,'YDir','reverse');
%
% tmp=cool_springCmap;
% subplot(233)
% hold on
% imagesc(1:(images_size-1)/(2^maxLevel-1):images_size,1:(images_size-1)/(2^maxLevel-1):images_size,mergedImage(M3m,dEdges,tmp(129:224,:),15))
% % I=flipdim(mergedImage(dM2m,Edges,hsv,2*pi),1);
% % image(h(1)/2:h(1):1,h(2)/2:h(2):1,I);
% axis image; axis xy;axis off;
% set(gca,'YDir','reverse');
%
% subplot(236)
% hold on
% imagesc(1:(images_size-1)/(2^maxLevel-1):images_size,1:(images_size-1)/(2^maxLevel-1):images_size,mergedImage(dM3m,Edges,tmp(129:224,:),15))
% plot(corners{1}(:,1),corners{1}(:,2),'k+','LineWidth',2)
% plot(corners{2}(:,1),corners{2}(:,2),'k+','LineWidth',2)
% plot(corners{3}(:,1),corners{3}(:,2),'k+','LineWidth',2)
% plot(corners{4}(:,1),corners{4}(:,2),'k+','LineWidth',2)
% % I=flipdim(mergedImage(dM2m,Edges,hsv,2*pi),1);
% % image(h(1)/2:h(1):1,h(2)/2:h(2):1,I);
% axis image; axis xy;axis off;
% set(gca,'YDir','reverse');


function c8_sinh_test ( )

%*****************************************************************************80
%
%% C8_SINH_TEST tests C8_SINH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2015
%
%  Author:
%
%    John Burkardt
%
  seed = 123456678;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'C8_SINH_TEST\n' );
  fprintf ( 1, '  C8_SINH computes the hyperbolic sine of a C8.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '       C1=C8_UNIFORM_01          C2 = C8_SINH(C1)           C3 = C8_ASINH(C1)\n' );
  fprintf ( 1, '     ---------------------     ---------------------     ---------------------\n' );
  fprintf ( 1, '\n' );

  for test = 1 : 10

    [ c1, seed ] = c8_uniform_01 ( seed );
    c2 = c8_sinh ( c1 );
    c3 = c8_asinh ( c2);

    fprintf ( 1, '  (%12f  %12f)  (%12f  %12f)  (%12f  %12f)\n', ...
      real ( c1 ), imag ( c1 ), real ( c2 ), imag ( c2 ), real ( c3 ), imag ( c3 ) );

  end

  return
end


%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
%function tfrppagt
%TFRPPAGT Unit test for the time frequency representation TFRPPAGE.

%       O. Lemoine - March 1996.

% We test each property of the corresponding TFR :

N=128;

% Covariance by translation in time
t1=55; t2=70; f=0.3;
sig1=amgauss(N,t1).*fmconst(N,f,t1);
sig2=amgauss(N,t2).*fmconst(N,f,t2);
tfr1=tfrppage(sig1);
tfr2=tfrppage(sig2);
[tr,tc]=size(tfr1);
nu=round(f*(tc-1)*2)+1;
tfr=tfr1-tfr2(:,modulo((1:tc)-t1+t2,tc));
if any(any(abs(tfr)>sqrt(eps))),
 error('tfrppage test 1 failed');
end

% Reality of the TFR
sig=noisecg(N);
tfr=tfrppage(sig);
if sum(any(abs(imag(tfr))>sqrt(eps)))~=0,
 error('tfrppage test 2 failed');
end

% Energy conservation
sig=noisecg(N);
tfr=tfrppage(sig);
Es=norm(sig)^2;
Etfr=sum(mean(tfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrppage test 3 failed');
end

% Time-marginal
sig=noisecg(N);
tfr=tfrppage(sig);
ip1=abs(sig).^2;
ip2=mean(tfr)';
if any(abs(ip1-ip2)>sqrt(eps)),
 error('tfrppage test 4 failed');
end

% Conservation of the time support (wide-sense)
sig=[zeros(N/4,1);noisecg(N/2);zeros(N/4,1)];
tfr=tfrppage(sig);
if sum(any(abs(tfr(:,1:N/4-1))>sqrt(eps))) | ...
   sum(any(abs(tfr(:,(3*N/4+1):N))>sqrt(eps))),
 error('tfrppage test 5 failed');
end

% time localization
t0=30; sig=((1:N)'==t0);
tfr=tfrppage(sig);
[ik,jk]=find(tfr~=0.0);
if any(jk~=t0)|any(ik'-(1:N)),
 error('tfrppage test 6 failed');
end;

% A PPAGED with a constant window is a PAGED
sig=noisecg(N);
tfr1=tfrppage(sig,1:N,N,ones(N+1,1));
tfr2=tfrpage(sig);
if any(any(abs(tfr1-tfr2)>sqrt(eps))),
 error('tfrppage test 7 failed');
end

N=129;

% Covariance by translation in time
t1=55; t2=70; f=0.3;
sig1=amgauss(N,t1).*fmconst(N,f,t1);
sig2=amgauss(N,t2).*fmconst(N,f,t2);
tfr1=tfrppage(sig1);
tfr2=tfrppage(sig2);
[tr,tc]=size(tfr1);
nu=round(f*(tc-1)*2)+1;
tfr=tfr1-tfr2(:,modulo((1:tc)-t1+t2,tc));
if any(any(abs(tfr)>sqrt(eps))),
 error('tfrppage test 8 failed');
end

% Reality of the TFR
sig=noisecg(N);
tfr=tfrppage(sig);
if sum(any(abs(imag(tfr))>sqrt(eps)))~=0,
 error('tfrppage test 9 failed');
end

% Energy conservation
sig=noisecg(N);
tfr=tfrppage(sig);
Es=norm(sig)^2;
Etfr=sum(mean(tfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrppage test 10 failed');
end

% Time-marginal
sig=noisecg(N);
tfr=tfrppage(sig);
ip1=abs(sig).^2;
ip2=mean(tfr)';
if any(abs(ip1-ip2)>sqrt(eps)),
 error('tfrppage test 11 failed');
end

% Conservation of the time support (wide-sense)
sig=[zeros(round(N/4),1);noisecg(round(N/2));zeros(round(N/4),1)];
tfr=tfrppage(sig);
if sum(any(abs(tfr(:,1:round(N/4)-1))>sqrt(eps))) | ...
   sum(any(abs(tfr(:,(round(3*N/4)+1):N))>sqrt(eps))),
 error('tfrppage test 12 failed');
end

% time localization
t0=30; sig=((1:N)'==t0);
tfr=tfrppage(sig);
[ik,jk]=find(tfr~=0.0);
if any(jk~=t0)|any(ik'-(1:N)),
 error('tfrppage test 13 failed');
end;

% A PPAGED with a constant window is a PAGED
sig=noisecg(N);
tfr1=tfrppage(sig,1:N,N,ones(2*N+1,1));
tfr2=tfrpage(sig);
if any(any(abs(tfr1-tfr2)>sqrt(eps))),
 error('tfrppage test 14 failed');
end


function pts = critical_points(V,F,u,tol)
%REVERSE_SUBDIVISION Find out which face contains the critical points of a
%function
%
% pts = critical_points(V,F,u,tol);
%
% Inputs:
%  V,F  the coarse input mesh
%  u  the function for which to find critical points
%  tol  the tolerance for writical points (if the gradient norm is below this,
%                                          we are at a critical point)
% Outputs:
%  pts  a list of face indices into the rows of F that tell us which faces
%       contain the critical points.
%

G = grad(V,F);
pts = ...

end


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_func10N(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

MergeM=get(handles.Matrix_name_edit,'String');
if isfloat(handles.TMatrix(1))
    set(handles.Matrix_name_edit,'String',['10.^[' MergeM ']']);
else
    set(handles.Matrix_name_edit,'String',['10.^double([' MergeM '])']);
end
MU_calc_matrix(handles);

end


function charlier_test ( )

%*****************************************************************************80
%
%% CHARLIER_TEST tests CHARLIER.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 March 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 5;
  a_test = [ 0.25, 0.5, 1.0, 2.0, 10.0 ];
  n = 5;
  OFFSET = 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHARLIER_TEST:\n' );
  fprintf ( 1, '  CHARLIER evaluates Charlier polynomials.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       N      A         X        P(N,A,X)\n' );

  for test = 1 : test_num

    n = 5;
    a = a_test(test);

    for j = 0 : 5

      x = j / 2.0;

      value = charlier ( n, a, x );

      fprintf ( 1, '\n' );

      for i = 0 : n

        fprintf ( 1, '  %8d  %8f  %8f  %14f\n', i, a, x, value(i+OFFSET) );

      end

    end

  end

  return
end


function ray = rayTracer(ray,ord,rMax)
%+========================================================================+
%|                                                                        |
%|           OPENRAY - LIBRARY FOR TRI-DIMENSIONAL RAY TRACING            |
%|           openRay is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : rayTracer.m                                   |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Ray tracer with octree speedup                |
%|  `---'  |                                                              |
%+========================================================================+

% Infos
disp('====> RAY TRACING <====')
tps = time();

% Mesh dimension
Nelt = length(ray.msh);

% Prepare sphere
tree = rayTreeInit(ray);

% Available ray
ind = find( (ray.dst<rMax) & (sum(ray.dir.^2,2)~=0) );

% Current order
n = length(ray.pos)-1;

% Infos
disp([' ~~> Tree with ',num2str(length(tree)),' stage(s) - Elapsed time is ', ...
                num2str(time()-tps),' seconds.'])

% Iterative loop
while ~isempty(ind) && (n < ord)
    % Infos
    tps = time();

    % Full repartition for smallest meshes
    if (Nelt < 50)
        Iray = cell(Nelt,1);
        for el = 1:Nelt
            Iray{el} = ind;
        end

    % Hierarchical repartition
    else
        Iray = rayTree(ray,tree,ind);
    end

    % Intersection between mesh and ray
    ray = rayCollision(ray,Iray);

    % Update available ray
    ind = find( (ray.dst<rMax) & (sum(ray.dir.^2,2)~=0) );

    % Order incrementation
    n = n + 1;

    % Infos
    disp([' + Step ',num2str(n), ' - Elapsed time is ', ...
                num2str(time()-tps),' seconds.'])
end
end

function tps = time()
tps = clock;
tps = tps(4)*3600 + tps(5)*60 + tps(6);
end

%     % Representation graphique
%     figure
%     hold on
%     for el = 1:Nelt
%         subElt = ray.msh.sub(el);
%         subRay = ray.sub(Iray{el});
%         plot(subElt,1)
%         plot(subRay)
%         pause
%     end
%     hold off


function [err,yy,Value]=energyclassify(L,x,y,xTest,yTest,Kg,varargin);
% function [err,yy,Value]=energyclassify(L,xTr,yTr,xTe,yTe,Kg,varargin);
%
% INPUT:
%  L	:   transformation matrix (learned by LMNN)
%  xTr	:   training vectors (each column is an instance)
%  yTr	:   training labels  (row vector!!)
%  xTe  :   test vectors
%  yTe  :   test labels
%  Kg	:   number of nearest neighbors
%
% Good luck!
%
% copyright by Kilian Q. Weinberger, 2006

% checks
D=length(L);
x=x(1:D,:);
xTest=xTest(1:D,:);
if(size(x,1)>length(L)) error('x and L must have matching dimensions!\n');end;

% set parameters
pars.alpha=1e-09;
pars.tempid=0;
pars.save=0;
pars.speed=10;
pars.skip=0;
pars.factor=1;
pars.correction=15;
pars.prod=0;
pars.thresh=1e-16;
pars.ifraction=1;
pars.scale=0;
pars.obj=0;
pars.union=1;
pars.margin=0;
pars.tabularasa=Inf;
pars.blocksize=500;
pars=extractpars(varargin,pars);

pars

tempname=sprintf('temp%i.mat',pars.tempid);
% Initializationip
[D,N]=size(x);
[gen,NN]=getGenLS(x,y,Kg,pars);

if(pars.scale)
 fprintf('Scaling input vectors!\n');
 sc=sqrt(mean(sum( ((x-x(:,NN(end,:)))).^2)));
 x=x./sc;
 xTest=xTest./sc;
end;

Lx=L*x;
Lx2=sum(Lx.^2);
LxT=L*xTest;

for inn=1:Kg
 Ni(inn,:)=sum((Lx-Lx(:,NN(inn,:))).^2)+1;
end;

MM=min(y);
y=y-MM+1;
un=unique(y);
Value=zeros(length(un),length(yTest));

B=pars.blocksize;
if(size(x,2)>50000) B=250;end;
NTe=size(xTest,2);
for n=1:B:NTe
  fprintf('%2.2f%%: ',n/NTe*100);
  nn=n:n+min(B-1,NTe-n);
  DD=distance(Lx,LxT(:,nn));
 for i=1:length(un)
 % Main Loopfor iter=1:maxiter
  testlabel=un(i);
  fprintf('%i.',testlabel+MM-1);

  enemy=find(y~=testlabel);
  friend=find(y==testlabel);

  Df=mink(DD(friend,:),Kg);
  Value(i,nn)=sumiflessv2(DD,Ni(:,enemy),enemy)+sumiflessh2(DD,Df,enemy)+sum(Df);
%  Value(i,nn)=sumiflessh2(DD,Df+pars.margin,enemy)+sum(Df);
 end;
 fprintf('\n');
end;

 fprintf('\n');
 [temp,yy]=min(Value);

 yy=un(yy)+MM-1;
err=sum(yy~=yTest)./length(yTest);
fprintf('Energy error:%2.2f%%\n',err*100);

function [gen,NN]=getGenLS(x,y,Kg,pars);
fprintf('Computing nearest neighbors ...\n');
[D,N]=size(x);
if(pars.skip) load('.LSKGnn.mat');
else
un=unique(y);
Gnn=zeros(Kg,N);
for c=un
fprintf('%i nearest genuine neighbors for class %i:',Kg,c);
i=find(y==c);
nn=LSKnn(x(:,i),x(:,i),2:Kg+1);
Gnn(:,i)=i(nn);
fprintf('\n');
end;

end;
NN=Gnn;
gen1=vec(Gnn(1:Kg,:)')';
gen2=vec(repmat(1:N,Kg,1)')';

gen=[gen1;gen2];

if(pars.save)
save('.LSKGnn.mat','Gnn');
end;

function NN=LSKnn(X1,X2,ks,pars);
B=2000;
[D,N]=size(X2);
NN=zeros(length(ks),N);
DD=zeros(length(ks),N);

for i=1:B:N
  BB=min(B,N-i);
  fprintf('.');
  Dist=distance(X1,X2(:,i:i+BB));
  fprintf('.');
%  [dist,nn]=sort(Dist);
  [dist,nn]=mink(Dist,max(ks));
  clear('Dist');
  fprintf('.');
%  keyboard;
  NN(:,i:i+BB)=nn(ks,:);
  clear('nn','dist');
  fprintf('(%i%%) ',round((i+BB)/N*100));
end;

function v=vec(M);
% vectorizes a matrix

v=M(:);