function [nav_e] = ins_visual(imu, gnss, visual, att_mode)
% ins_visual: loosely-coupled integrated navigation system.
%
% ins_visual integrates IMU and visual measurements by using an Extended Kalman filter.
%
% INPUT
%   imu, IMU data structure.
%         t: Ix1 time vector (seconds).
%        fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
%        wb: Ix3 turn rates vector in body frame XYZ (radians/s).
%       arw: 1x3 angle random walks (rad/s/root-Hz).
%       vrw: 1x3 velocity random walks (m/s^2/root-Hz).
%     g_std: 1x3 gyros standard deviations (radians/s).
%     a_std: 1x3 accrs standard deviations (m/s^2).
%    gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
%    ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
%    gb_dyn: 1x3 gyros dynamic biases or bias instabilities (radians/s).
%    ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
%   gb_corr: 1x3 gyros correlation times (seconds).
%   ab_corr: 1x3 accrs correlation times (seconds).
%    gb_psd: 1x3 gyros dynamic biases root-PSD (rad/s/root-Hz).
%    ab_psd: 1x3 accrs dynamic biases root-PSD (m/s^2/root-Hz);
%      freq: 1x1 sampling frequency (Hz).
% ini_align: 1x3 initial attitude at t(1).
% ini_align_err: 1x3 initial attitude errors at t(1).
%
%	gnss, GNSS data structure.
%         t: Gx1 time vector (seconds).
%       lat: Gx1 latitude (radians).
%       lon: Gx1 longitude (radians).
%         h: Gx1 altitude (m).
%       vel: Gx3 NED velocities (m/s).
%       std: 1x3 position standard deviations (rad, rad, m).
%      stdm: 1x3 position standard deviations (m, m, m).
%      stdv: 1x3 velocity standard deviations (m/s).
%      larm: 3x1 lever arm from IMU to GNSS antenna (x-fwd, y-right, z-down) (m).
%      freq: 1x1 sampling frequency (Hz).
%   zupt_th: 1x1 ZUPT threshold (m/s).
%  zupt_win: 1x1 ZUPT time window (seconds).
%       eps: 1x1 time interval to compare current IMU time to current GNSS time vector (s).
%
%   visual, visual data structure
%         t: Vx1 time vector (seconds)
%       lat: Vx1 latitude (radians).
%       lon: Vx1 longitude (radians).
%         h: Vx1 altitude (m).
%       vel: Vx3 NED velocities (m/s).
%     covvm: Vx36 velocity and position covariance matrices (m^2 and m^2/s^2).
%      covv: Vx9 velocity covariance matrices (m^2/s^2).
%      larm: 3x1 lever arm (m).
%      freq: 1x1 sampling frequency (Hz).
%       eps: 1x1 time interval to compare current IMU time to current visual time vector (s).
%
%  att_mode: attitude mode string.
%      'quaternion': attitude updated in quaternion format. Default value.
%             'dcm': attitude updated in Direct Cosine Matrix format.
%
% OUTPUT
%   nav_e, INS/visual navigation estimates data structure.
%         t: Ix1 INS time vector (seconds).
%        tg: Vx1 VISUAL time vector, when Kalman filter was executed (seconds).
%      roll: Ix1 roll (radians).
%     pitch: Ix1 pitch (radians).
%       yaw: Ix1 yaw (radians).
%       vel: Ix3 NED velocities (m/s).
%       lat: Ix1 latitude (radians).
%       lon: Ix1 longitude (radians).
%         h: Ix1 altitude (m).
%        xi: Vxn Kalman filter a priori states.
%        xp: Vxn Kalman filter a posteriori states.
%         z: Vxr INS/visual measurements
%         v: Vxr Kalman filter innovations.
%         b: Vxr Kalman filter biases compensations, [gb_dyn ab_dyn].
%         A: Vxn^2 Kalman filter transition-state matrices, one matrix per
%            row ordered by columns.
%        Pp: Vxn^2 Kalman filter a posteriori covariance matrices, one
%         matrix per row ordered by columns.
%        Pi: Vxn^2 Kalman filter a priori covariance matrices, one matrix
%            per row ordered by columns.
%         K: Vx(n*r) Kalman gain matrices
%         S: Vxr^2 Innovation matrices
%        ob: Vx1 Number of observable states after each visual data arriving
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%   R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
% simulation framework for low-cost integrated navigation systems,
% Journal of Control Engineering and Applied Informatics, vol. 17,
% issue 2, pp. 110-120, 2015. Alg. 2.
%
%   Groves, P.D. (2013), Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems (2nd Ed.). Artech House.
%
%   ZUPT algothim based on Groves, Chapter 15, "INS Alignment, Zero Updates,
% and Motion Constraints".
%
%   ins_gps.m, ins_gnss function is based on that previous NaveGo function.
%
% Version: 002
% Date:    2022/07/19
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

if nargin < 3, att_mode  = 'quaternion'; end

%% ZUPT ALGORITHM

zupt_flag = false;

%% PREALLOCATION

% Kalman filter dimensions
n = 15; % number of states
r = 6;  % number of sensors
q = 12; % number of inputs

% Constant matrices
I = eye(3);
O = zeros(3);

% Length of INS time vector
LI = length(imu.t);

% Length of visual time vector
LV = length(visual.t);

% Preallocation of attitude vectors
roll_e  = zeros (LI, 1);
pitch_e = zeros (LI, 1);
yaw_e   = zeros (LI, 1);

% Preallocation of velocity vector
vel_e   = zeros (LI, 3);

% Preallocation of gravity vector
gn_e   = zeros (LI, 3);

% Preallocation of position vectors
lat_e    = zeros (LI, 1);
lon_e    = zeros (LI, 1);
h_e      = zeros (LI, 1);

% Preallocation of Kalman filter matrices for later performance analysis
xi = zeros(LV, n);      % Evolution of Kalman filter a priori states
xp = zeros(LV, n);      % Evolution of Kalman filter a posteriori states
z = zeros(LV, r);       % INS/visual measurements
v = zeros(LV, r);       % Kalman filter innovations

A  = zeros(LV, n^2);    % Transition-state matrices
Pi = zeros(LV, n^2);    % A priori covariance matrices
Pp = zeros(LV, n^2);    % A posteriori covariance matrices
K  = zeros(LV, n*r);    % Kalman gain matrices
S  = zeros(LV, r^2);    % Innovation matrices
ob = zeros(LV, 1);      % Number of observable states at each visual data arriving

b = zeros(LV, r);       % Biases compensantions after Kalman filter correction

%% INITIAL VALUES AT INS TIME = 1

% Initial attitude
roll_e(1)  = imu.ini_align(1);
pitch_e(1) = imu.ini_align(2);
yaw_e(1)   = imu.ini_align(3);
DCMnb = euler2dcm([roll_e(1); pitch_e(1); yaw_e(1);]);
DCMbn = DCMnb';
qua   = euler2qua([roll_e(1) pitch_e(1) yaw_e(1)]);

% Initial velocity
vel_e(1,:) = gnss.vel(1,:);

% Initial position
lat_e(1) = gnss.lat(1);
lon_e(1) = gnss.lon(1);
h_e(1)   = gnss.h(1);

% Initial dynamic biases
gb_dyn = imu.gb_dyn';
ab_dyn = imu.ab_dyn';

% Turn-rates update with both updated velocity and position
omega_ie_n = earth_rate(lat_e(1));
omega_en_n = transport_rate(lat_e(1), vel_e(1,1), vel_e(1,2), h_e(1));

% Gravity update
gn_e(1,:) = gravity(lat_e(1), h_e(1));

%% INITIALIZATION OF KALMAN FILTER MATRICES

% Prior estimates
kf.xi = [ zeros(1,9), imu.gb_dyn, imu.ab_dyn ]';  % Error vector state
kf.Pi = diag([imu.ini_align_err, gnss.stdv, gnss.std, imu.gb_dyn, imu.ab_dyn].^2);

kf.Q  = diag([imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd].^2);

fb_corrected = imu.fb(1,:)' - ab_dyn - imu.ab_sta';
fn = DCMbn * fb_corrected;
wn_corrected = imu.wb(1,:)' - gb_dyn - imu.gb_sta';
wn = DCMbn * wn_corrected;

% Vector to update matrix F
upd = [gnss.vel(1,:) gnss.lat(1) gnss.h(1) fn' wn'];

% Update matrices F and G
[kf.F, kf.G] = F_update(upd, DCMbn, imu);

[RM,RN] = radius(gnss.lat(1));
Tpr = diag([(RM + gnss.h(1)), (RN + gnss.h(1)) * cos(gnss.lat(1)), -1]);  % radians-to-meters

% Update matrix H
kf.H = [ O I O O O ;
    O O Tpr O O ; ];
kf.R = diag([gnss.stdv gnss.stdm]).^2;
kf.z = [ gnss.stdv, gnss.stdm ]';

% Propagate prior estimates to get xp(1) and Pp(1)
kf = kf_update( kf );

% Initial matrices for Kalman filter performance analysis
xi(1,:) = kf.xi';
xp(1,:) = kf.xp';
Pi(1,:) = reshape(kf.Pi, 1, n^2);
Pp(1,:) = reshape(kf.Pp, 1, n^2);
K(1,:)  = reshape(kf.K, 1, n*r);
S(1,:)  = reshape(kf.S, 1, r^2);
v(1,:)  = kf.v';
z(1,:)  = kf.z';
b(1,:) = [gb_dyn', ab_dyn'];

%% INS (IMU) TIME IS THE MASTER CLOCK
for i = 2:LI

    %% INERTIAL NAVIGATION SYSTEM (INS)

    % Print a dot on console every 10,000 INS executions
    if (mod(i,10000) == 0), fprintf('. ');  end
    % Print a return on console every 200,000 INS executions
    if (mod(i,200000) == 0), fprintf('\n'); end

    % IMU sampling interval
    dti = imu.t(i) - imu.t(i-1);

    % Inertial sensors corrected with a posteriori KF biases estimation and
    % deterministic static biases
    wb_corrected = imu.wb(i,:)' - gb_dyn - imu.gb_sta';
    fb_corrected = imu.fb(i,:)' - ab_dyn - imu.ab_sta';
    fn = DCMbn * fb_corrected;
    wn = DCMbn * wb_corrected;

    % Velocity update
    vel = vel_update(fn, vel_e(i-1,:), omega_ie_n, omega_en_n, gn_e(i,:)', dti);
    vel_e (i,:) = vel;

    % Position update
    pos = pos_update([lat_e(i-1) lon_e(i-1) h_e(i-1)], vel_e(i,:), dti);
    lat_e(i) = pos(1);
    lon_e(i) = pos(2);
    h_e(i)   = pos(3);

    % Turn-rates update with both updated velocity and position
    omega_ie_n = earth_rate(lat_e(i));
    omega_en_n = transport_rate(lat_e(i), vel_e(i,1), vel_e(i,2), h_e(i));

    % Gravity update
    gn_e(i,:) = gravity(lat_e(i), h_e(i));

    % Attitude update
    [qua, DCMbn, euler] = att_update(wb_corrected, DCMbn, qua, ...
        omega_ie_n, omega_en_n, dti, att_mode);
    roll_e(i) = euler(1);
    pitch_e(i)= euler(2);
    yaw_e(i)  = euler(3);

    %% ZUPT DETECTION ALGORITHM
    idz = floor( gnss.zupt_win / dti ); % Index to set ZUPT window time

    if ( i > idz )

        % Mean velocity value for the ZUPT window time
        vel_m = mean (vel_e(i-idz:i , :));

        % If mean velocity value is under the ZUPT threshold velocity...
        if (abs(vel_m) < gnss.zupt_th)
            disp("ZUPT condition applied");

            % Current attitude is equal to the mean of previous attitudes
            % inside the ZUPT window time
            roll_e(i)  = mean (roll_e(i-idz:i , :));
            pitch_e(i) = mean (pitch_e(i-idz:i , :));
            yaw_e(i)   = mean (yaw_e(i-idz:i , :));

            % Current position is equal to the mean of previous positions
            % inside the ZUPT window time
            lat_e(i) = mean (lat_e(i-idz:i , :));
            lon_e(i) = mean (lon_e(i-idz:i , :));
            h_e(i)   = mean (h_e(i-idz:i , :));

            % Alternative attitude ZUPT correction
            % roll_e(i)  = (roll_e(i-idz , :));
            % pitch_e(i) = (pitch_e(i-idz , :));
            % yaw_e(i)   = (yaw_e(i-idz, :));
            % lat_e(i) = (lat_e(i-idz:i , :));
            % lon_e(i) = (lon_e(i-idz:i , :));
            % h_e(i)   = (h_e(i-idz:i , :));

            zupt_flag = true;

            %             fprintf(' z\n')       % DEBUG
        end
    end

    %% KALMAN FILTER UPDATE

    % Check if there is a new visual measurement to process at current INS time
    gdx =  find (visual.t >= (imu.t(i) - visual.eps) & visual.t < (imu.t(i) + visual.eps));

    if (~isempty(gdx) && gdx(1) > 1)
        for index = 1:size(gdx,1)
            % disp("updated with visual measurements");

            %% MEASUREMENTS

            % Meridian and normal radii of curvature update
            [RM,RN] = radius(lat_e(i));

            % Radians-to-meters matrix
            Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]);

            % Position innovations in meters with lever arm correction
            zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [visual.lat(gdx(index)); visual.lon(gdx(index)); visual.h(gdx(index));]) ...
                + (DCMbn * visual.larm);

            % Velocity innovations with lever arm correction
            zv = (vel_e(i,:) - visual.vel(gdx(index),:) - ((omega_ie_n + omega_en_n) * (DCMbn * visual.larm))' ...
                + (DCMbn * skewm(wb_corrected) * visual.larm )' )';

            %% KALMAN FILTER

            % visual sampling interval
            dtg = visual.t(gdx(index)) - visual.t(gdx(index)-1);

            % Vector to update matrix F
            upd = [vel_e(i,:) lat_e(i) h_e(i) fn' wn'];

            % Matrices F and G update
            [kf.F, kf.G] = F_update(upd, DCMbn, imu);

            % Matrix H update
            if(zupt_flag == false)
                kf.H = [ O I O O O ;
                    O O Tpr O O ; ];
                kf.R = reshape(visual.covvm(gdx(index),:),[6,6])';
                kf.z = [ zv' zp' ]';
            else
                kf.H = [ O I O O O ; ];
                kf.R = reshape(visual.covv(gdx(index),:),[3,3])';
                kf.z = zv;
            end

            % a posteriori states are forced to be zero (error-state approach)
            kf.xp = zeros(n , 1);
            % Execution of the extended Kalman filter
            kf = kalman(kf, dtg);

            %% OBSERVABILITY

            % Number the observable states at current GNSS time
            ob(gdx) = rank(obsv(kf.F, kf.H));

            %% INS/GNSS CORRECTIONS

            % Quaternion correction
            qua_skew = -skewm(qua(1:3));    % According to Crassidis, qua_skew should be
            % positive, but if positive NaveGo diverges.
            % Crassidis, Eq. A.174a
            Xi = [qua(4)*eye(3) + qua_skew; -qua(1:3)'];

            % Crassidis, Eq. 7.34
            qua = qua + 0.5 .* Xi * kf.xp(1:3);
            qua = qua / norm(qua);          % Brute-force normalization

            % DCM correction
            DCMbn = qua2dcm(qua);

            % Attitude correction, method 1
            %         euler = qua2euler(qua);
            %         roll_e(i) = euler(1);
            %         pitch_e(i)= euler(2);
            %         yaw_e(i)  = euler(3);

            % Attitude correction, method 2
            roll_e(i)  = roll_e(i)  - kf.xp(1);
            pitch_e(i) = pitch_e(i) - kf.xp(2);
            yaw_e(i)   = yaw_e(i)   - kf.xp(3);

            % Velocity correction
            vel_e(i,1) = vel_e(i,1) - kf.xp(4);
            vel_e(i,2) = vel_e(i,2) - kf.xp(5);
            vel_e(i,3) = vel_e(i,3) - kf.xp(6);

            % Position correction
            lat_e(i) = lat_e(i) - kf.xp(7);
            lon_e(i) = lon_e(i) - kf.xp(8);
            h_e(i)   = h_e(i)   - kf.xp(9);

            % Biases estimation
            gb_dyn   = -kf.xp(10:12);
            ab_dyn   = -kf.xp(13:15);

            % Matrices for later Kalman filter performance analysis
            xi(gdx(index),:) = kf.xi';
            xp(gdx(index),:) = kf.xp';
            b(gdx(index),:) = [gb_dyn', ab_dyn'];
            A(gdx(index),:)  = reshape(kf.A,  1, n^2);
            Pi(gdx(index),:) = reshape(kf.Pi, 1, n^2);
            Pp(gdx(index),:) = reshape(kf.Pp, 1, n^2);

            if(zupt_flag == false)
                v(gdx(index),:)  = kf.v';
                z(gdx(index),:)  = kf.z';
                K(gdx(index),:)  = reshape(kf.K, 1, n*r);
                S(gdx(index),:)  = reshape(kf.S, 1, r^2);
            else
                zupt_flag = false;
                z(gdx(index),:)  = [ kf.z' 0 0 0 ]';
                v(gdx(index),:)  = [ kf.v' 0 0 0 ]';
                K(gdx(index),1:n*3) = reshape(kf.K, 1, n*3);
                S(gdx(index),1:9)  = reshape(kf.S, 1, 3^2);
            end
        end
    end

    if imu.t(i) > max(visual.t)
        break;
    end
end

%% Summary from INS/visual integration

nav_e.t     = imu.t(1:i, :);    % INS time vector
nav_e.tg    = visual.t;         % visual time vector, which is the time vector when the Kalman filter was executed
nav_e.roll  = roll_e(1:i, :);   % Roll
nav_e.pitch = pitch_e(1:i, :);  % Pitch
nav_e.yaw   = yaw_e(1:i, :);    % Yaw
nav_e.vel   = vel_e(1:i, :);    % NED velocities
nav_e.lat   = lat_e(1:i, :);    % Latitude
nav_e.lon   = lon_e(1:i, :);    % Longitude
nav_e.h     = h_e(1:i, :);      % Altitude
nav_e.gn    = gn_e(1:i, :);     % Gravity estimation in the nav-frame.

nav_e.xi    = xi;       % A priori states
nav_e.xp    = xp;       % A posteriori states
nav_e.z     = z;        % INS/visual measurements
nav_e.v     = v;        % Kalman filter innovations
nav_e.b     = b;        % Biases compensations

nav_e.A     = A;        % Transition matrices
nav_e.Pi    = Pi;       % A priori covariance matrices
nav_e.Pp    = Pp;       % A posteriori covariance matrices
nav_e.K     = K;        % Kalman gain matrices
nav_e.S     = S;        % Innovation matrices
nav_e.ob    = ob;       % Number of observable states after each visual data arriving

fprintf('\n');
end


function triangle_io_test02 ( )

%*****************************************************************************80
%
%% TRIANGLE_IO_TEST02 gets the example element data and writes it to a file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2014
%
%  Author:
%
%    John Burkardt
%
  element_filename = 'example.ele';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGLE_IO_TEST02:\n' );
  fprintf ( 1, '  Get example element data, write to a triangle element file.\n' );
%
%  Get element example size.
%
  [ element_num, element_order, element_att_num ] = ...
    triangle_element_size_example ( );
%
%  Print the sizes.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of elements = %d\n', element_num );
  fprintf ( 1, '  Order of elements = %d\n', element_order );
  fprintf ( 1, '  Number of element attributes = %d\n', element_att_num );
%
%  Get the data.
%
  [ element_node, element_att ] = triangle_element_data_example ( element_num, ...
    element_order, element_att_num );
%
%  Print some of the data.
%
  i4mat_transpose_print_some ( element_order, element_num, element_node, ...
    1, 1, element_order, 10, '  Node connectivity of first 10 elements:' );

  r8mat_transpose_print_some ( element_att_num, element_num, element_att, ...
    1, 1, element_att_num, 10, '  Attributes for first 10 elements:' ) ;
%
%  Write the node information to node file.
%
  triangle_element_write ( element_filename, element_num, element_order, ...
    element_att_num, element_node, element_att );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Element data written to file "%s"\n', element_filename );

  return
end


classdef nnoffset < nntest
  methods (Test)
    function basic(test)
      param = [.34, .5] ;
      x = test.randn(4,5,10,3) ;
      y = vl_nnnoffset(x,param) ;
      dzdy = test.randn(size(y)) ;
      dzdx = vl_nnnoffset(x,param,dzdy) ;
      test.der(@(x) vl_nnnoffset(x,param), x, dzdy, dzdx, 1e-3*test.range) ;
    end
  end
end


function [inputs, labels, set] = patches_generation(sigma,size_input,size_label,stride,folder,mode,max_numPatches,batchSize)

inputs  = zeros(size_input, size_input, 1, 1,'single');
labels  = zeros(size_label, size_label, 1, 1,'single');
count   = 0;
padding = abs(size_input - size_label)/2;

ext               =  {'*.jpg','*.png','*.bmp'};
filepaths           =  [];

for i = 1 : length(ext)
    filepaths = cat(1,filepaths, dir(fullfile(folder, ext{i})));
end

for i = 1 : length(filepaths)
    image = imread(fullfile(folder,filepaths(i).name)); % uint8
    %[~, name, exte] = fileparts(filepaths(i).name);
    if size(image,3) == 3
        image = rgb2gray(image); % uint8
    end

    for j = 1:8
        image_aug = data_augmentation(image, j);  % augment data
        im_label  = im2single(image_aug); % single
        [hei,wid] = size(im_label);
        im_input  = im_label; % single
        for x = 1 : stride : (hei-size_input+1)
            for y = 1 :stride : (wid-size_input+1)
                subim_input = im_input(x : x+size_input-1, y : y+size_input-1);
                subim_label = im_label(x+padding : x+padding+size_label-1, y+padding : y+padding+size_label-1);
                count       = count+1;
                inputs(:, :, 1, count)   = subim_input + single(sigma/255*randn(size(subim_input)));
                labels(:, :, 1, count) = subim_label;
            end
        end
    end
end

inputs = inputs(:,:,:,1:(size(inputs,4)-mod(size(inputs,4),batchSize)));
labels = labels(:,:,:,1:(size(labels ,4)-mod(size(labels ,4),batchSize)));
labels = shave(inputs,[padding,padding])-labels; %%% residual image patches; pay attention to this!!!

order  = randperm(size(inputs,4));
inputs = inputs(:, :, 1, order);
labels = labels(:, :, 1, order);

set    = uint8(ones(1,size(inputs,4)));
if mode == 1
    set = uint8(2*ones(1,size(inputs,4)));
end

disp('-------Original Datasize-------')
disp(size(inputs,4));

subNum = min(size(inputs,4),max_numPatches);
inputs = inputs(:,:,:,1:subNum);
labels = labels(:,:,:,1:subNum);
set    = set(1:subNum);

disp('-------Now Datasize-------')
disp(size(inputs,4));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  JOINT_REFERENCES
%  Obtain q when the robot makes a linear trajectory of the end effector
%  in cartesian space
%
%  See also INVERSEKINEMATIC
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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/>.
function q=joint_references(time)

global robot

time;
if time == 0
    robot.q=[0 0 0 0 0 0]';
end

simulation_time = 0.5;%s

%this is important, as this will be the initial joint
%values in the first integrator
%initial position
T1 = directkinematic(robot, [0 0 0 0 0.1 0]);
%final position
T2 = directkinematic(robot, [0.3 0.3 0.3 0.1 0.1 0.1]);

Q1 = T2quaternion(T1);
Q2 = T2quaternion(T2);
%interpolate between the two orientations
[Qm] = slerp(Q1, Q2, time/simulation_time, 0.01);

initial_point=T1(1:3,4);
end_point=T2(1:3,4);

% %NOA matrix end point
% T2=[1 0 0 0.5;
%     0 1 0 0.4;
%     0 0 1 0.3;
%     0 0 0  1];

%speed of the movement
speed = norm(initial_point-end_point)/simulation_time;%m/s

v=(end_point-initial_point);
v=v/norm(v); %unit vector in the direction of the line

point = initial_point + speed*time*v;

%T1(1:3,4)=point(1:3);

Ttotal = quaternion2T(Qm, point(1:3));
qinv = inversekinematic(robot, Ttotal);
q = select_closest_joint_coordinates(qinv, robot.q);

%update current robot joints
robot.q=q;


function [E]=EnergyOfLabelField(segmentation,potential,width,height,class_number)
n=size(segmentation,1);
segmentation=reshape(segmentation,[width height]);
Nei8=imstack2vectors(NeiX(segmentation));
E=zeros(n,class_number);
for i=1:class_number
    E(:,i)=sum(Nei8~=i,2);
end
E=E*potential;
end


function [fMshift, fProbability, fBic, mProblikelihood] = calc_loglikelihood_dM_Mc(mCat1, mCat2)
% function [fMshift, fProbability, fBic, mProblikelihood] = calc_loglikelihood_dM_Mc(mCat1, mCat2);
% ----------------------------------------------------------------------------------------------
% Calculate log-likelihood estimation of magnitude shift dM between to periods
%
% Incoming variable
% mCat1 : EQ catalog period 1 (Catalog to be modified)
% mCat2 : EQ catalog period 2 (Observed catalog)
%
% Outgoing variable
% fProbability : log-likelihood probabilty
% fMshift      : Magnitude shift with the lowest  max. lieklihood score
% fBic         : Bayesian Information Criterion value
% mProblikelihood : Solution matrix shift and likelihood score
%
% Author: J. Woessner, woessner@seismo.ifg,.ethz.ch
% last update: 28.10.02

% Initialize
vProbability = [];
vMc = [];
vABValue =[];
mFitRes = [];
vX_res = [];
vNCumTmp = [];
mDataPred = [];
mProblikelihood = [];
vfProbability = [];
vMshift = [];
fBinning = 0.1;

% Cut catalog for magnitudes M >= 0
mCatalog = [mCat1; mCat2];
vSel0 = (mCatalog(:,6) >= 0);
mCatalog = mCatalog(vSel0,:);
mCat = mCatalog;

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

% Determine max. and min. magnitude
fMinMag = floor(min(mCatalog(:,6)));
% if fMinMag > 0
%     fMinMag = 0;
% end
fMaxMag = ceil(10 * max(mCatalog(:,6))) / 10;

% Calculate FMD for original catalog
[vFMDorg, vNonCFMDorg] = calc_FMD(mCatalog);
for fMc =fMinMag:0.1:fMaxMag-1
    vFMD = vFMDorg;
    vNonCFMD = vNonCFMDorg;
    vNonCFMD = fliplr(vNonCFMD);
    mCatalog = mCat;
    % Calculate a and b-value for GR-law and distribution vNCum
    [nIndexLo, fMagHi, vSel, vMagnitudes] = fMagToFitBValue(mCatalog, vFMD, fMc);
    if (length(mCatalog(vSel,1)) >= 20)
        [fMeanMag, fBValue, fStdDev, fAValue] =  calc_bmemag(mCatalog(vSel,:), fBinning);
        % Normalize to time period
        vFMD(2,:) = ceil(vFMD(2,:)./fPeriod1);
        vNonCFMD(2,:) = ceil(vNonCFMD(2,:)./fPeriod1);
        % Compute quantity of earthquakes by power law
        vMstep = [fMinMag:0.1:fMaxMag];
        vNCum = 10.^(fAValue-fBValue.*vMstep); % Cumulative number

        % Compute non-cumulative numbers vN
        fNCumTmp = 10^(fAValue-fBValue*(fMaxMag+0.1));
        vNCumTmp  = [vNCum fNCumTmp];
        vN = abs(diff(vNCumTmp));
        %     % Normalizea-value
        fAValue = fAValue./fPeriod1;
        % Normlize vN
        vN = vN./fPeriod1;
        % Data selection
        % Adjust magnitude ranges of vN and vNonCFMD
        vSelM = (vNonCFMD(1,:) >= fMinMag);
        vNonCFMD = vNonCFMD(:,vSelM);
        mData = [vN' vNonCFMD']; % Non cumulative
        vSel = (mData(:,2) >= fMc);
        mDataTest = mData(~vSel,:);
        fNmax = max(mDataTest(:,3));
        % For cutted catalog move lowest magnitude to 0
        mDataTest(:,2) = mDataTest(:,2)-fMinMag;
        if (~isempty(fNmax) & ~isnan(fNmax) & fNmax ~= 0 & length(mDataTest(:,1) > 4))
            mDataTest(:,3) = mDataTest(:,3)/fNmax; % Normalize datavalues for fitting with CDF
            mDataTmp = mData.subset(vSel);
            % Curve fitting: Non cumulative part below Mc
            options = optimset;
            options = optimset('Display','iter','Tolfun',1e-5,'TolX',0.001,'MaxFunEvals', 100000);
            [vX, resnorm, resid, exitflag, output, lambda, jacobian]=lsqcurvefit(@calc_lognormal,[0.1 0.1], mDataTest(:,2), mDataTest(:,3));
            mDataTest(:,1) = logncdf(mDataTest(:,2), vX(1), vX(2))*fNmax;
            if (length(mDataTest(:,2)) > length(vX(1,:)))
                %% Confidence interval
                [vPred,delta] = nlpredci(@calc_lognormal,mDataTest(:,2),vX, resid, jacobian);
            else
                vPred = nan;
                delta = nan;
            end; % END: This section is due for errors produced with datasets less long than amount of parameters in vX
            % For cutted catalog move lowest magnitude back to what it is
            mDataTest(:,2) = mDataTest(:,2)+fMinMag;
            % Results of fitting procedure
            mFitRes = [mFitRes; vX resnorm exitflag];
            %% Set data together
            mDataTest(:,3) = mDataTest(:,3)*fNmax;
            mDataPred = [mDataTest; mDataTmp];
            %%%% Now search dM for best fitting model
            mCat1Mod = mDataPred;
            for fMshift = -0.5:0.1:0.5
                % Apply shift
                mCat1Mod(:,6) = mCat1Mod(:,6)+fMshift;
                % Initialize values
                fMinMag = min([min(mCat1Mod(:,6)) min(mCat2(:,6))]);
                fMaxMag = max([max(mCat1Mod(:,6)) max(mCat2(:,6))]);

                [vPredFMD,vBin1] = hist(mCat1Mod(:,6),0:0.1:fMaxMag);
                [vObsFMD,vBin2] = hist(mCat2(:,6),0:0.1:fMaxMag);
                % Time normalization and round due to Poisson distribution calculation in calc_log10poisspdf
                vPredFMD = ceil(vPredFMD./fPeriod1);
                vObsFMD = ceil(vObsFMD./fPeriod2);

                % Calculate the likelihoods for both models
                vProb_ = calc_log10poisspdf(vObsFMD', vPredFMD');
                % Sum the probabilities
                fProbability = (-1) * sum(vProb_);
                vfProbability = [vfProbability; fProbability];
                vMshift = [vMshift; fMshift];
                mCat1Mod = mCat1;
            end
            vProb_ = calc_log10poisspdf(mDataPred(:,3), ceil(mDataPred(:,1))); % Non-cumulative
            % Sum the probabilities
            fProbability = (-1) * sum(vProb_);
            vProbability = [vProbability; fProbability];
            vMc = [vMc; fMc];
            vABValue = [vABValue; fAValue fBValue];
            % Keep best fitting model
            if (fProbability == min(vProbability))
                fProbMin = fProbability;
                vPredBest = vPred*fNmax;
                vDeltaBest = delta;
                mDat = mDataTest;
                vNBest = vN;
                fMcBest = fMc;
                mDatPredBest = mDataPred;
                vX_res = [vX resnorm exitflag];
                fNmaxBest = fNmax;
            end
        else
            fProbMin = nan;
            fMcBes = nan;
        end; % END of IF fNmax
    end; % END of IF length(mCatalog(vSel,1))

    % Clear variables
    vNCumTmp = [];
    mModelDat = [];
    vNCum = [];
    vSel = [];
    mDataTest = [];
    mDataPred = [];
end; % END of FOR fMc
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Initialize
% mProblikelihood = [];
% vfProbability = [];
% vMshift = [];
% fBinning = 0.1;
%
% % Determine exact time period
% fPeriod1 = max(mCat1(:,3)) - min(mCat1(:,3));
% fPeriod2 = max(mCat2(:,3)) - min(mCat2(:,3));
%
% % Calculate Mc for the two periods
% [fProbMin1, fMcBest1] = calc_McCdf(mCat1, fBinning);
% [fProbMin2, fMcBest2] = calc_McCdf(mCat2, fBinning);
%
% mCat1Mod = mCat1;
%
% for fMshift = -0.5:0.1:0.5
%     % Apply shift
%     mCat1Mod(:,6) = mCat1Mod(:,6)+fMshift;
%     % Initialize values
%     fMinMag = min([min(mCat1Mod(:,6)) min(mCat2(:,6))]);
%     fMaxMag = max([max(mCat1Mod(:,6)) max(mCat2(:,6))]);
%
%     [vPredFMD,vBin1] = hist(mCat1Mod(:,6),0:0.1:fMaxMag);
%     [vObsFMD,vBin2] = hist(mCat2(:,6),0:0.1:fMaxMag);
%     % Time normalization and round due to Poisson distribution calculation in calc_log10poisspdf
%     vPredFMD = ceil(vPredFMD./fPeriod1);
%     vObsFMD = ceil(vObsFMD./fPeriod2);
%
%     % Calculate the likelihoods for both models
%     vProb_ = calc_log10poisspdf(vObsFMD', vPredFMD');
%     % Sum the probabilities
%     fProbability = (-1) * sum(vProb_);
%     vfProbability = [vfProbability; fProbability];
%     vMshift = [vMshift; fMshift];
%     mCat1Mod = mCat1;
% end
%
% %%% Find the minimum loglikelihodd score: if the minimum score is obtained several times, calculate MEAN
% %%% of the magnitude shift
% vdMloglikeli = [vfProbability vMshift];
% vSel = (vdMloglikeli == min(vdMloglikeli(:,1)));
% vdMloglikeli = vdMloglikeli(vSel,:);
% if length(vdMloglikeli(:,1)) > 1
%     fProbability = min(vdMloglikeli(:,1));
%     fMshift = mean(vdMloglikeli(:,2));
% else
%     fProbability = vdMloglikeli(:,1);
%     fMshift = vdMloglikeli(:,2);
% end
% % Solution matrix
% mProblikelihood = [vMshift vfProbability];
%
% %% Bayesian Information Criterion (BIC)
% nDegFree = 1; % Magnitude shift is the degree of freedom
% n_samples = length(mCat1(:,6));
% fBic = 2*fProbability + 2*log(n_samples)*nDegFree;


function [Jsl] = singleSL(model, cutoff, eliList, atpm)
%% [Jsl] = singleSL(model,cutoff,eliList,atpm)
% INPUT
% model (the following fields are required - others can be supplied)
%   S            Stoichiometric matrix
%   b            Right hand side = dx/dt
%   c            Objective coefficients
%   lb           Lower bounds
%   ub           Upper bounds
%   rxns         Reaction Names
% OPTIONAL
% cutoff         cutoff percentage value for lethality.Default is 0.01.
% eliList        List of reactions to be ignored for lethality
% analysis:Exchange Reactions, ATPM etc.
% atpm           ATPM Reaction Id in model.rxns if other than 'ATPM'
% OUTPUT
% Jsl            Single lethal reactions identified
% Aditya Pratapa       6/26/14.

if exist('cutoff', 'var')
    if isempty(cutoff)
        cutoff = 0.01;
    end
else
    cutoff = 0.01;
end

if exist('atpm', 'var')
    if isempty(atpm)
        atpm = 'ATPM'; % Reaction Id of ATP maintenance reaction- by default it takes 'ATPM'
    end
else
    atpm = 'ATPM';
end

if exist('eliList', 'var')
    if isempty(eliList)
        eliList = model.rxns(ismember(model.rxns, atpm)); % To eliminate ATPM.
    end
else
        eliList = model.rxns(ismember(model.rxns, atpm));
end

Jsl = [];

% Step1 Identify Single Lethal Reactions...
% Identify minNorm flux distribution
solWT = optimizeCbModel(model, 'max', 'one');
grWT = solWT.f;
Jnz = find(~eq(solWT.x, 0));
if (~isempty(eliList))
    eliIdx = find(ismember(model.rxns, eliList)); % Index of reactions not considered for lethality analysis
    Jnz = Jnz(~ismember(Jnz, eliIdx)); % Jnz
end
h = waitbar(0, '0.00', 'Name', 'Identifying Jsl...');

% Identify Single Lethal Reaction Deletions...
modeldel = model;

for iRxn = 1:length(Jnz)
    delIdx_i = Jnz(iRxn);
    modeldel.lb(delIdx_i) = 0;
    modeldel.ub(delIdx_i) = 0;
    solKO_i = optimizeCbModel(modeldel);
    if (solKO_i.f < cutoff*grWT || isnan(solKO_i.f))
        Jsl = [Jsl; delIdx_i];
    end
   % Reset bounds on idx reaction
    modeldel.lb(delIdx_i) = model.lb(delIdx_i);
    modeldel.ub(delIdx_i) = model.ub(delIdx_i);
    waitbar(iRxn / length(Jnz), h, [num2str(round(iRxn * 100 / length(Jnz))) '% completed...']);
end

close(h);
Jsl = model.rxns(Jsl);
end


function c8mat_uniform_01_test ( )

%*****************************************************************************80
%
%% C8MAT_UNIFORM_01_TEST tests C8MAT_UNIFORM_01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2013
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'C8MAT_UNIFORM_01_TEST\n' );
  fprintf ( 1, '  C8MAT_UNIFORM_01 computes a "random" complex matrix.\n' );

  seed = 123456789;

  [ a, seed ] = c8mat_uniform_01 ( m, n, seed );

  c8mat_print ( m, n, a, '  The matrix:' );

  return
end


% Test file for ADCHEBFUN trigonometric and related functions, part 3/4

function pass = test_trig3

% List of trigonometric functions to test.
funcList = {@atand, @atanh, @cos, @cosd, @cosh, @cot, @cotd, @coth, ...
    @csc, @cscd};

% Call the ADCHEBFUN testUnary() method to do the tests.
pass = adchebfun.testUnary(funcList);

end


% batch input data setup:
isFullData = 1;
data_masterdir = GetCurrentDataDir();

const_ClusGroup = 2;
const_Cluster = 1; % This is all cells
M_fishset = GetFishStimset();
M_stimrange = GetStimRange();

range_fish =  1:18; % range_fish = GetFishRange();

%% custom params here:

%%
for i = 1:length(range_fish),
    i_fish = range_fish(i);
    disp(i_fish);
    fishset = M_fishset(i_fish);

    LoadFullFish(hfig,i_fish,isFullData);
    absIX = getappdata(hfig,'absIX');

    %% Cluster indexing
    i_ClusGroup = const_ClusGroup;% M_ClusGroup(i);
    i_Cluster = const_Cluster;% M_Cluster(i);
    cIX = LoadCluster_Direct(i_fish,i_ClusGroup,i_Cluster,absIX);

    %% get time index
    timelists = getappdata(hfig,'timelists');
%     timelists_names = getappdata(hfig,'timelists_names'); % use to screen
    stimrange = M_stimrange(i);

    tIX = GetTimeIndex_Direct(stimrange,timelists,fishset);

    %% get data matrix
    M = GetTimeIndexedData_Default_Direct(hfig,cIX,tIX);
    %M_0 = GetTimeIndexedData_Default_Direct(hfig,[],tIX,'isAllCells');

    %% ------custom code here---------

%         gIX = Kmeans_Direct(M,numK);

    %% (optional) save cluster
%     name = 'test';
%     clusgroupID = 2;
%     clusIDoverride = ; %Saved in Group2, Clusters 3&4
%     SaveCluster_Direct(cIX,gIX,absIX,i_fish,name,clusgroupID,clusIDoverride);

end


% *********************************************************************
% Identify and plot block boundaries
% (requires job object with calculated variant Ids)
% *********************************************************************
home; close all; clearvars -except job
%% Application - Determine Variant pairing
%Derive variant types 1 to 6 from variants 1 to 24
laths = job.transformedGrains;
laths.prop.variantType = laths.variantId - (laths.packetId-1) * max(job.variantId)/max(job.packetId);
%Get the boundary segments and neighbouring laths
lathboundaries = laths.boundary(job.csChild,job.csChild);
boundaryIds = lathboundaries.grainId;

[c,ia] = ismember(boundaryIds,laths.id);
boundaryIds(any(~c,2),:) = [];
lathboundaries(any(~c,2)) = [];
%Identify special boundaries, here V1-V2, V1-V3(V5), V1-V6 and V1-V4
%[S. Morito, A.H. Pham, T. Hayashi, T. Ohba, Mater. Today Proc. 2 (2015)
%S913–S916.]
varTypes = laths(laths.id2ind(boundaryIds)).variantType;
cond(1,:) = any(ismember(varTypes,1),2) & any(ismember(varTypes,2),2) | ...
            any(ismember(varTypes,3),2) & any(ismember(varTypes,4),2) | ...
            any(ismember(varTypes,5),2) & any(ismember(varTypes,6),2);     %V1-V2
cond(2,:) = any(ismember(varTypes,1),2) & any(ismember(varTypes,3),2) | ...
            any(ismember(varTypes,1),2) & any(ismember(varTypes,5),2) | ...
            any(ismember(varTypes,2),2) & any(ismember(varTypes,4),2) | ...
            any(ismember(varTypes,2),2) & any(ismember(varTypes,6),2) | ...
            any(ismember(varTypes,3),2) & any(ismember(varTypes,5),2) | ...
            any(ismember(varTypes,4),2) & any(ismember(varTypes,6),2);     %V1-V3(V5)
cond(3,:) = any(ismember(varTypes,1),2) & any(ismember(varTypes,6),2) | ...
            any(ismember(varTypes,2),2) & any(ismember(varTypes,3),2) | ...
            any(ismember(varTypes,4),2) & any(ismember(varTypes,5),2);     %V1-V6
cond(4,:) = any(ismember(varTypes,1),2) & any(ismember(varTypes,4),2) | ...
            any(ismember(varTypes,2),2) & any(ismember(varTypes,5),2) | ...
            any(ismember(varTypes,3),2) & any(ismember(varTypes,6),2);     %V1-V4

%% Application - Plot a map of variant pairing
%Plot the boundaries
colors = {'r','g','b','k'};
ebsdC = job.ebsdPrior;
try data = ebsdC.bc; catch data = ebsdC.imagequality; end
figure; plot(ebsdC,data);
setColorRange([mean(data)-2*std(data),mean(data)+2*std(data)])
mtexColorMap black2white
hold on
labelstr = {'V1-V2','V1-V3(V5)','V1-V6','V1-V4'};
for i = 1:size(cond,1)
    plot(lathboundaries(cond(i,:)),'linewidth',2,'linecolor',colors{i},'DisplayName',labelstr{i});
end
legend

%% Application - Plot the fractions of variant pairing
%Plot bar diagram to show the special boundary fractions
for i = 1:size(cond,1)
    frac(i) = sum(lathboundaries(cond(i,:)).segLength)/sum(lathboundaries(any(cond)).segLength);
end
figure;
bar(categorical(labelstr),frac,'r');
ylabel('Fraction of block boundaries')


function i4vec_sort_heap_d_test ( )

%*****************************************************************************80
%
%% I4VEC_SORT_HEAP_D_TEST tests I4VEC_SORT_HEAP_D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  n = 20;
  b = 0;
  c = 3 * n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_SORT_HEAP_D_TEST\n' );
  fprintf ( 1, '  For a vector of integers,\n' );
  fprintf ( 1, '  I4VEC_SORT_HEAP_D descending sorts.\n' );

  seed = 123456789;

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

  i4vec_print ( n, a, '  Unsorted:' );

  a = i4vec_sort_heap_d ( n, a );

  i4vec_print ( n, a, '  Descending sorted:' );

  return
end


function x = creation_null_right ( m, n )

%*****************************************************************************80
%
%% CREATION_NULL_RIGHT returns a right null vector of the CREATION matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the order of A.
%
%    Output, real X(N,1), the null vector.
%
  x = zeros ( n, 1 );

  x(n,1) = 1.0;

  return
end


function y = mldivide(X,Y)
%MLDIVIDE (overloaded)

if (isa(X,'sdpvar'))
  error('Division of matrix variables not possible.')
end

try
  lmi_variables = getvariables(Y);
  nv = length(lmi_variables);
  y  = Y;
  n = Y.dim(1);
  m = Y.dim(2);
  if m==1
    y.basis = X\Y.basis;
    y.dim(1) = size(y.basis,1);
    y.dim(2) = 1;
  else % FIX : VECTORIZE THIS...
    [L,U] = qr(X);
    temp = U\(L\reshape(Y.basis(:,1),n,m));
    y.basis = temp(:);
    for i = 1:nv
        temp = U\(L\reshape(Y.basis(:,i+1),n,m));
        y.basis(:,i+1) = temp(:);
    end;
    y.dim(1) = size(temp,1);
    y.dim(2) = size(temp,2);
  end
catch
  error(lasterr);
end


% Reference performance
% We have to expected values depending of the OS
% (see https://github.com/ayrna/orca/issues/19)
if ispc
    accTestRef = 0.920000;
else
    accTestRef = 0.960000;
end
allowedError = 0.001;
method = 'SVMOP';

% Create the algorithm object
algorithmObj = SVMOP();

% Clear parameter struct
clear param;

% Parameter C (Cost)
param.C = 10;

% Parameter k (kernel width)
param.k = 10;

% Run the algorithm
info = algorithmObj.fitpredict(train,test,param);

trainCM = confusionmat(info.predictedTrain,train.targets);
testCM = confusionmat(info.predictedTest,test.targets);

accTrain = CCR.calculateMetric(trainCM);
accTest  = CCR.calculateMetric(testCM);

% Report accuracy
fprintf('Performing test for %s\n', method);
fprintf('Accuracy Train %f, Accuracy Test %f\n',accTrain,accTest);

if abs(accTestRef-accTest)<allowedError
    fprintf('Test accuracy matches reference accuracy\n');
else
    warning('Test accuracy does NOT match reference accuracy');
end


function ori = calcOrientations(odf,points,varargin)
% draw random orientations from ODF
%
% Syntax
%   ori = calcOrientations(odf,points)
%
% Input
%  odf    - @SO3Fun
%  points - number of orientation to be simualted
%
% Output
%  ori   - @orientation
%
% See also
% ODF_calcPoleFigure, ODF_calcEBSD

warning('The command calcOrientations is depreciated! Please use discreteSample instead.')

ori = discreteSample(odf,points,varargin{:});


function pass = test_max2(pref)
% Test the chebfun3/max2 command.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e7*pref.cheb3Prefs.chebfun3eps;

f = chebfun3(@(x,y,z) cos(x.*y.*z));
g = chebfun(@(x) 1 + 0*x);

h1 = max2(f);
h2 = max2(f, []);
h3 = max2(f, [], [1 2]);
h4 = max2(f, [], [2 1]);

h5 = max2(f, [], [1 3]);
h6 = max2(f, [], [3 1]);

h7 = max2(f, [], [2 3]);
h8 = max2(f, [], [3 2]);

pass(1) = norm(h1 - g) < tol;
pass(2) = norm(h2 - g) < tol;
pass(3) = norm(h3 - g) < tol;
pass(4) = norm(h4 - g) < tol;
pass(5) = norm(h5 - g) < tol;
pass(6) = norm(h6 - g) < tol;
pass(7) = norm(h7 - g) < tol;
pass(8) = norm(h8 - g) < tol;

end


function [im2, H] = repeatability_saltnpepper(im, n_pixels)
  seed = sum(im(:));
  s = RandStream('mt19937ar', 'Seed', seed);

  [h,w,~] = size(im);
  num_missing = n_pixels;
  coords = [];
  while num_missing > 0
    x = randi(s, [1 w], num_missing, 1);
    y = randi(s, [1 h], num_missing, 1);
    coords = unique([coords; [x y]], 'rows');
    num_missing = n_pixels - size(coords, 1);
  end

  im2 = im;
  H = eye(3);
  if isempty(coords)
    return;
  end

  im_gray = rgb2gray(im);
  coord_inds = sub2ind(size(im_gray), coords(:,2), coords(:,1));
  is_dark = im_gray(coord_inds) < 128;
  chan_stride = h * w;
  for ch = 1:3
    % dark goes bright
    im2(coord_inds(is_dark) + (ch - 1) * chan_stride) = 255;
    % bright goes dark
    im2(coord_inds(~is_dark) + (ch - 1) * chan_stride) = 0;
  end
end


% 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

mdl_puma560

T = p560.fkine(qn);
q = qn;

while true
    T=joy2tr(T, 'tool');
    trprint(T, 'fmt', '%.1f')
    q = p560.ikine6s(T, q);
    p560.plot(q)
end


function PopDec = EvolEI(Problem,Dec,Obj,model,IFEs)
% Solution update in EGO, where a solution with the best expected
% improvement is re-evaluated

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

    Off   = [OperatorGA(Problem,Dec(TournamentSelection(2,size(Dec,1),Obj),:));OperatorGA(Problem,Dec,{0,0,1,20})];
    N     = size(Off,1);
    EI    = zeros(N,1);
    Gbest = min(Obj);
    E0    = inf;

    while IFEs > 0
        drawnow('limitrate');
        for i = 1 : N
            [y,~,mse] = predictor(Off(i,:),model);
            s         = sqrt(mse);
            EI(i)     = -(Gbest-y)*normcdf((Gbest-y)/s)-s*normpdf((Gbest-y)/s);
        end
        [~,index] = sort(EI);
        if EI(index(1)) < E0
            Best = Off(index(1),:);
            E0   = EI(index(1));
        end
        Parent = Off(index(1:ceil(N/2)),:);
        Off    = [OperatorGA(Problem,Parent(TournamentSelection(2,size(Parent,1),EI(index(1:ceil(N/2)))),:));OperatorGA(Problem,Parent,{0,0,1,20})];
        IFEs   = IFEs - size(Off,1);
    end
    PopDec = Best;
end


filename='Bridge_tetrahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'PROJECTED GRADIENT'; incrementFactor = 1;
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;


function z = g2z(g);

% Z = g2z(G)
%
% Hybrid G to Impedance transformation
% only for 2-by-2 matrices
%
% martie 27

if g(1,1) == 0
  disp('correspondent admittance matrix non-existent');
else
z(1,1) = 1/g(1,1);
z(1,2) = -g(1,2)/g(1,1);
z(2,1) = g(2,1)/g(1,1);
z(2,2) = g(2,2) - g(1,2)*g(2,1)/g(1,1);
end;


function [c, ceq ] = multiFlybyNonlcon(x, fitnessfcn, minRadiiSingle, maxRadiiSingle, minXferRad, maxDepartVInf, maxArriveVInf, maxDeltaVSingle)
%multiFlybyNonlcon Summary of this function goes here
%   Detailed explanation goes here
    ceq = [];

    numPop = size(x,1);

    minRadii = [];
    maxRadii = [];
    maxDeltaV = [];
%     minRadii = NaN(1, numPop*numel(minRadiiSingle));
%     maxRadii = NaN(1, numPop*numel(maxRadiiSingle));
    for(i=1:length(minRadiiSingle))
        minRadii = [minRadii, minRadiiSingle(i) * ones(1, numPop)]; %#ok<AGROW>
        maxRadii = [maxRadii, maxRadiiSingle(i) * ones(1, numPop)]; %#ok<AGROW>
        maxDeltaV = [maxDeltaV, maxDeltaVSingle(i)*ones(1, numPop)]; %#ok<AGROW>
    end

    [~, rp, ~, ~, deltaVVect, vInfDNorm, ~, vInfArrive, ~, ~, ~, ~, ~, xferRp] = fitnessfcn(x);

    deltaV = sqrt(sum(abs(deltaVVect).^2,1));

    c = (minRadii(1:length(rp)) - rp)';
    c = reshape(c,numPop,size(c,1)/numPop);

    c2 = (rp - maxRadii(1:length(rp)))';
    c2 = reshape(c2,numPop,size(c2,1)/numPop);

    c3 = -(xferRp - minXferRad)'; %need the minus sign to make greater than values negative for the constraint
    c3 = reshape(c3,numPop,size(c3,1)/numPop);

%     c = horzcat(c,c2,c3);

    c4 = vInfDNorm - maxDepartVInf;
    c5 = (sqrt(sum(abs(vInfArrive).^2,1)))' - maxArriveVInf;

    c6 = (deltaV - maxDeltaV)';
    c6 = reshape(c6,numPop,size(c6,1)/numPop);

    c = horzcat(c,c2,c3,c4,c5,c6);

    c(isnan(c)) = 1;
    c(c == -Inf) = -realmax;
    c(c == Inf) = realmax;
end


mb = 300;    % kg
mw = 60;     % kg
bs = 1000;   % N/m/s
ks = 16000 ; % N/m
kt = 190000; % N/m
kn = 0*ks/10;  % N/m


function LFs = LRFforPntCldFunc(mesh, keypntIdx, neighborSize)

%  Author: Yulan Guo {yulan.guo@nudt.edu.cn}
%  NUDT, China & CSSE, UWA, Australia
%
% This function takes a point cloud as an input, and generate local reference frames (LRFs) for a set of
% keypoints as an output.
%
% Arguments : mesh - with vertices and faces
%                       keypntIdx - the indices of keypoints on a mesh
%                       neighborSize - the size of neighborhood to define a
%                                                  local surface for a selected keypoint
% Return :         LRFs - local reference frames (LRFs) corresponding to all
%                                     keypoints
% Copyright : This code is written by Yulan Guo {yulan.guo@nudt.edu.cn}, NUDT.
%               The code may be used, modified and distributed for research purposes with
%              acknowledgement of the author and inclusion this copyright information.
%References:
%           [1] Yulan Guo, Ferdous Sohel, Mohammed Bennamoun, Min Lu, Jianwei Wan.
%           Rotational Projection Statistics for 3D Local Surface Description and Object Recognition.
%           Internation Journal of Computer Vision. 2013, 105 (1), 63-86
%           [2] Yulan Guo, Mohammed Bennamoun, Ferdous A Sohel, Min Lu, Jianwei Wan.
%           3D Object Recognition in Cluttered Scenes with Local Surface Features: A Survey.
%           IEEE Transactions on Pattern Analysis and Machine Intelligence,2014
%
% Disclaimer : This code is provided as is without any warranty.

%%%%%%%%%%%% parallel computing to accelerate.
p = gcp('nocreate'); % If no pool, do not create new one.
if isempty(p)   % if exists no parallel computing handle, create one.
    poolsize = parpool;
    % disp('Create a Parallel Computing Pool to accelarate process' );
else
    poolsize = p.NumWorkers;
    % disp(sprintf( 'Parpool has %d workers', poolsize) );
end

%%%%%%
% BucketSize = floor(length(mesh.vertices)/100);
% kdtreeVertices = KDTreeSearcher(mesh.vertices,'Distance','euclidean','BucketSize',BucketSize);
kdtreeVertices = KDTreeSearcher(mesh.vertices,'Distance','euclidean');
%%%%%%
parfor i = 1:length(keypntIdx)
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    keypnt = mesh.vertices(keypntIdx(i),:);
    [neighborIdx,neighborDis]= rangesearch(kdtreeVertices,keypnt,neighborSize);
    neighborIdx = cell2mat(neighborIdx);
    neighborIdx = neighborIdx(2:end);
    neighborDis = cell2mat(neighborDis);
    neighborDis = neighborDis(2:end);
    %%%%%
    M = zeros(3,3);
    dis = 0;
    for j = 1:length(neighborIdx)
        M = M+(mesh.vertices(neighborIdx(j),:)-mesh.vertices(keypntIdx(i),:))'*(mesh.vertices(neighborIdx(j),:)-mesh.vertices(keypntIdx(i),:))*(neighborSize-neighborDis(j));
        dis = dis+(neighborSize-neighborDis(j));
    end
    M = M/dis;
    if isnan(M(1,1)) ==1
        LFs{i,1} = eye(3,3);
        continue;
    end
    [V,D] = eig(M);
    lamda = [D(1,1),D(2,2),D(3,3)];
    [temp, idxMinLam] = min(lamda);
    [temp, idxMaxLam] = max(lamda);
    xtemp = V(:,idxMaxLam);
    ztemp = V(:,idxMinLam);
    xPlus = 0;
    xMinus = 0;
    zPlus = 0;
    zMinus = 0;
    for j = 1:length(neighborIdx)
        if (mesh.vertices(neighborIdx(j),:)-mesh.vertices(keypntIdx(i),:))*xtemp>0
            xPlus = xPlus+1;
        else
            xMinus = xMinus+1;
        end
        if (mesh.vertices(neighborIdx(j),:)-mesh.vertices(keypntIdx(i),:))*ztemp>0
            zPlus = zPlus+1;
        else
            zMinus = zMinus+1;
        end
    end
    if xPlus>xMinus
        xAxis = xtemp;
    else
        xAxis = -xtemp;
    end
   if zPlus>zMinus
        zAxis = ztemp;
    else
        zAxis = -ztemp;
    end
    yAxis = cross(zAxis,xAxis);
    rotation = [xAxis';yAxis';zAxis'];
    LFs{i,1} = rotation;
end


function [ a, R, T, T3D, params, error, shapeOrtho ] = fit_PDM_ortho_proj_to_2D_no_reg( M, E, V, shape2D)
%FITPDMTO2DSHAPE Summary of this function goes here
%   Detailed explanation goes here

    hidden = false;

    % if some of the points are unavailable modify M, V, and shape2D (can
    % later infer the actual shape from this)
    if(sum(shape2D(:)==0) > 0)

        hidden = true;
        % which indices to remove
        inds_to_rem = shape2D(:,1) == 0 | shape2D(:,2) == 0;

        shape2D = shape2D(~inds_to_rem,:);

        inds_to_rem = repmat(inds_to_rem, 3, 1);

        M_old = M;
        V_old = V;

        M = M(~inds_to_rem);
        V = V(~inds_to_rem,:);

    end

    num_points = numel(M) / 3;

    m = reshape(M, num_points, 3)';
    width_model = max(m(1,:)) - min(m(1,:));
    height_model = max(m(2,:)) - min(m(2,:));

    bounding_box = [min(shape2D(:,1)), min(shape2D(:,2)),...
                    max(shape2D(:,1)), max(shape2D(:,2))];

    a = (((bounding_box(3) - bounding_box(1)) / width_model) + ((bounding_box(4) - bounding_box(2))/ height_model)) / 2;

    tx = (bounding_box(3) + bounding_box(1))/2;
    ty = (bounding_box(4) + bounding_box(2))/2;

    % correct it so that the bounding box is just around the minimum
    % and maximum point in the initialised face
    tx = tx - a*(min(m(1,:)) + max(m(1,:)))/2;
    ty = ty - a*(min(m(2,:)) + max(m(2,:)))/2;

    R = eye(3);
    T = [tx; ty];

    params = zeros(size(E));

    currShape = getShapeOrtho(M, V, params, R, T, a);

    currError = getRMSerror(currShape, shape2D);

    reg_rigid = zeros(6,1);
    regFactor = 0.25;
    regularisations = [reg_rigid; regFactor ./ E]; % the above version, however, does not perform as well
    regularisations = diag(regularisations)*diag(regularisations);

    red_in_a_row = 0;

    for i=1:1000

        shape3D = M + V * params;
        shape3D = reshape(shape3D, numel(shape3D) / 3, 3);

        % Now find the current residual error
        currShape = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
        currShape = currShape';

        error_res = shape2D - currShape;

        eul = Rot2Euler(R);

        p_global = [a; eul'; T];

        % get the Jacobians
        J = CalcJacobian(M, V, params, p_global);

        % RLMS style update
        p_delta = (J'*J + regularisations) \ (J'*error_res(:) - regularisations*[p_global;params]);

        % not to overshoot
        p_delta = 0.5 * p_delta;

        [params, p_global] = CalcReferenceUpdate(p_delta, params, p_global);

        a = p_global(1);
        R = Euler2Rot(p_global(2:4));
        T = p_global(5:6);

        shape3D = M + V * params;
        shape3D = reshape(shape3D, numel(shape3D) / 3, 3);
        currShape = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
        currShape = currShape';

        error = getRMSerror(currShape, shape2D);

        if(0.999 * currError < error)
            red_in_a_row = red_in_a_row + 1;
            if(red_in_a_row == 5)
                break;
            end
        end

        currError = error;

    end

    if(hidden)
        shapeOrtho = getShapeOrtho(M_old, V_old, params, R, T, a);
    else
        shapeOrtho = currShape;
    end

    T3D = [0;0;0];

end

function [shape2D] = getShapeOrtho(M, V, p, R, T, a)

    % M - mean shape vector
    % V - eigenvectors
    % p - parameters of non-rigid shape
    % R - rotation matrix
    % T - translation vector (tx, ty)
    shape3D = getShape3D(M, V, p);
    shape2D = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
    shape2D = shape2D';
end

function [shape2D] = getShapeOrthoFull(M, V, p, R, T, a)

    % M - mean shape vector
    % V - eigenvectors
    % p - parameters of non-rigid shape
    % R - rotation matrix
    % T - translation vector (tx, ty)
    T = [T; 0];
    shape3D = getShape3D(M, V, p);
    shape2D = a * R*shape3D' + repmat(T, 1, numel(M)/3);
    shape2D = shape2D';
end

function [shape3D] = getShape3D(M, V, params)

    shape3D = M + V * params;
    shape3D = reshape(shape3D, numel(shape3D) / 3, 3);

end

function [error] = getRMSerror(shape2Dv1, shape2Dv2)

    error = sqrt(mean(reshape(shape2Dv1 - shape2Dv2, numel(shape2Dv1), 1).^2));

end

% This calculates the combined rigid with non-rigid Jacobian
function J = CalcJacobian(M, V, p, p_global)

    n = size(M, 1)/3;

    non_rigid_modes = size(V,2);

    J = zeros(n*2, 6 + non_rigid_modes);

    % now the layour is
    % ---------- Rigid part -------------------|----Non rigid part--------|
    % dx_1/ds, dx_1/dr1, ... dx_1/dtx, dx_1/dty dx_1/dp_1 ... dx_1/dp_m
    % dx_2/ds, dx_2/dr1, ... dx_2/dtx, dx_2/dty dx_2/dp_1 ... dx_2/dp_m
    % ...
    % dx_n/ds, dx_n/dr1, ... dx_n/dtx, dx_n/dty dx_n/dp_1 ... dx_n/dp_m
    % dy_1/ds, dy_1/dr1, ... dy_1/dtx, dy_1/dty dy_1/dp_1 ... dy_1/dp_m
    % ...
    % dy_n/ds, dy_n/dr1, ... dy_n/dtx, dy_n/dty dy_n/dp_1 ... dy_n/dp_m

    % getting the rigid part
    J(:,1:6) = CalcRigidJacobian(M, V, p, p_global);

    % constructing the non-rigid part
    R = Euler2Rot(p_global(2:4));
    s = p_global(1);

    % 'rotate' and 'scale' the principal components

    % First reshape to 3D
    V_X = V(1:n,:);
    V_Y = V(n+1:2*n,:);
    V_Z = V(2*n+1:end,:);

    J_x_non_rigid = s*(R(1,1)*V_X + R(1,2)*V_Y + R(1,3)*V_Z);
    J_y_non_rigid = s*(R(2,1)*V_X + R(2,2)*V_Y + R(2,3)*V_Z);

    J(1:n, 7:end) = J_x_non_rigid;
    J(n+1:end, 7:end) = J_y_non_rigid;

end

function J = CalcRigidJacobian(M, V, p, p_global)

 	n = size(M, 1)/3;

	% Get the current 3D shape (not affected by global transform, as this
	% is how the Jacobian was derived (for derivation please see
	% ../derivations/orthoJacobian
	shape3D = GetShape3D(M, V, p);

	% Get the rotation matrix corresponding to current global orientation
	R = Euler2Rot(p_global(2:4));
	s = p_global(1);

    % Rigid Jacobian is laid out as follows
    % dx_1/ds, dx_1/dr1, dx_1/dr2, dx_1/dr3, dx_1/dtx, dx_1/dty
    % dx_2/ds, dx_2/dr1, dx_2/dr2, dx_2/dr3, dx_2/dtx, dx_2/dty
    % ...
    % dx_n/ds, dx_n/dr1, dx_n/dr2, dx_n/dr3, dx_n/dtx, dx_n/dty
    % dy_1/ds, dy_1/dr1, dy_1/dr2, dy_1/dr3, dy_1/dtx, dy_1/dty
    % ...
    % dy_n/ds, dy_n/dr1, dy_n/dr2, dy_n/dr3, dy_n/dtx, dy_n/dty

    J = zeros(n*2, 6);

    % dx/ds = X * r11  + Y * r12 + Z * r13
    % dx/dr1 =  s*(r13 * Y - r12 * Z)
    % dx/dr2 = -s*(r13 * X - r11 * Z)
    % dx/dr3 =  s*(r12 * X - r11 * Y)
    % dx/dtx = 1
    % dx/dty = 0

    % dy/ds = X * r21  + Y * r22 + Z * r23
    % dy/dr1 =  s * (r23 * Y - r22 * Z)
    % dy/dr2 = -s * (r23 * X - r21 * Z)
    % dy/dr3 =  s * (r22 * X - r21 * Y)
    % dy/dtx = 0
    % dy/dty = 1

    % set the Jacobian for x's

    % with respect to scaling factor
    J(1:n,1) = shape3D * R(1,:)';

    % with respect to angular rotation around x, y, and z axes

    % Change of x with respect to change in axis angle rotation
	dxdR = [      0,  R(1,3), -R(1,2);
            -R(1,3),       0,  R(1,1);
             R(1,2), -R(1,1),      0];

    J(1:n,2:4) = s*(dxdR * shape3D')';

    % with respect to translation
    J(1:n,5) = 1;
    J(1:n,6) = 0;

    % set the Jacobian for y's

    % with respect to scaling factor
    J(n+1:end,1) = shape3D * R(2,:)';

     % with respect to angular rotation around x, y, and z axes

    % Change of y with respect to change in axis angle rotation
    dydR = [      0,  R(2,3), -R(2,2);
            -R(2,3),       0,  R(2,1);
             R(2,2), -R(2,1),      0];

     J(n+1:end,2:4) = s*(dydR * shape3D')';

     % with respect to translation
     J(n+1:end,5) = 0;
     J(n+1:end,6) = 1;

end

% This updates the parameters based on the updates from the RLMS
function [non_rigid, rigid] = CalcReferenceUpdate(params_delta, current_non_rigid, current_global)

    rigid = zeros(6, 1);
    % Same goes for scaling and translation parameters
    rigid(1) = current_global(1) + params_delta(1);
    rigid(5) = current_global(5) + params_delta(5);
    rigid(6) = current_global(6) + params_delta(6);

    % for rotation however, we want to make sure that the rotation matrix
    % approximation we have
	% R' = [1, -wz, wy
	%       wz, 1, -wx
	%       -wy, wx, 1]
    % is a legal rotation matrix, and then we combine it with current
    % rotation (through matrix multiplication) to acquire the new rotation

	R = Euler2Rot(current_global(2:4));

    wx = params_delta(2);
    wy = params_delta(3);
    wz = params_delta(4);

    R_delta = [1, -wz, wy;
               wz, 1, -wx;
               -wy, wx, 1];

	% Make sure R_delta is orthonormal
	R_delta = OrthonormaliseRotation(R_delta);

    % Combine rotations
	R_final = R * R_delta;

	% Extract euler angle
	euler = Rot2Euler(R_final);

	rigid(2:4) = euler;

    if(length(params_delta) > 6)
        % non-rigid parameters can just be added together
        non_rigid = params_delta(7:end) +  current_non_rigid;
    else
        non_rigid = current_non_rigid;
    end

end

function R_ortho = OrthonormaliseRotation(R)

    % U * V' is basically what we want, as it's guaranteed to be
    % orthonormal
    [U, ~, V] = svd(R);

    % We also want to make sure no reflection happened

    % get the orthogonal matrix from the initial rotation matrix
    X = U*V';

    % This makes sure that the handedness is preserved and no reflection happened
    % by making sure the determinant is 1 and not -1
    W = eye(3);
    W(3,3) = det(X);
    R_ortho = U*W*V';
end


%plot_vehicle Draw ground vehicle pose
%
% PLOT_VEHICLE(X,OPTIONS) draws a representation of ground robot as an
% oriented triangle with pose X (1x3) = [x,y,theta] or X (3x3) as an SE(2)
% homogeneous transform.
%
% Options::
% 'scale',S    Draw vehicle with length S x maximum axis dimension (default
%              1/60)
% 'size',S     Draw vehicle with length S
%
% See also Vehicle.plot.

% Copyright (C) 1993-2015, 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

% TODO needs to work for 3D point

function plot_vehicle(x, varargin)

    opt.scale = 1/60;
    opt.size = [];

    [opt,args] = tb_optparse(opt, varargin);

    % get the current axes
    a = axis;

    % compute the dimensions of the robot
    if ~isempty(opt.size)
        d = opt.size;
    else
        d = (a(2)+a(4) - a(1)-a(3)) * opt.scale;
    end

    if numel(x) == 3
        % convert vector form of pose to SE(2)
        T = se2(x(1), x(2), x(3));
    else
        T = x;
    end

    % draw it
%     points = [
%         d 0 1
%         -d -0.6*d 1
%         -d 0.6*d 1
%         d 0 1]';

        points = [
        d 0
        -d -0.6*d
        -d 0.6*d]';

    points = homtrans(T, points);

    plot_poly(points, args{:});

    end


function file_name = file_name_inc ( file_name )

%*****************************************************************************80
%
%% FILE_NAME_INC increments a partially numeric filename.
%
%  Discussion:
%
%    It is assumed that the digits in the name, whether scattered or
%    connected, represent a number that is to be increased by 1 on
%    each call.  If this number is all 9's on input, the output number
%    is all 0's.  Non-numeric letters of the name are unaffected..
%
%    If the name is empty, then the routine stops.
%
%    If the name contains no digits, the empty string is returned.
%
%  Example:
%
%      Input            Output
%      -----            ------
%      'a7to11.txt'     'a7to12.txt'  (typical case.  Last digit incremented)
%      'a7to99.txt'     'a8to00.txt'  (last digit incremented, with carry.)
%      'a9to99.txt'     'a0to00.txt'  (wrap around)
%      'cat.txt'        ' '           (no digits in input name.)
%      ' '              STOP!         (error.)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string FILE_NAME, the string to be incremented.
%
%    Output, string FILE_NAME, the incremented string.
%
  lens = s_len_trim ( file_name );

  if ( lens <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_NAME_INC - Fatal error!\n' );
    fprintf ( 1, '  The input filename is empty.\n' );
    error ( 'FILE_NAME_INC - Fatal error!' );
  end

  change = 0;

  for i = lens : -1 : 1

    c = file_name(i);

    if ( '0' <= c && c <= '8' )

      c = c + 1;

      file_name(i) = c;

      return

    elseif ( c == '9' )

      change = change + 1;

      c = '0';

      file_name(i) = c;

    end

  end
%
%  No digits were found.  Return blank.
%
  if ( change == 0 )
    file_name = ' ';
  end

  return
end


function b = sprepmat(a,m,n)
% function B = sprepmat(A,M,N)
%
% DESCRIPTION
%   B is a sparse matrix consisting of an MxN tiling of the
%   sparse matrix A.
%   [repmat fails on scalar, sparse matrices in Matlab 6.5:
%     >>repmat(sparse(5),2,3)
%    This yields an error.]
%
% INPUTS
%   A: sparse matrix
%   M,N: Copies of A in the row and column directions
%
% OUTPUTS
%   B: sparse matrix
%
% SYNTAX
%   B = sprepmat(A,M,N);

% This file is part of SOSTOOLS - Sum of Squares Toolbox ver 3.00.
%
% Copyright (C)2002, 2004, 2013  A. Papachristodoulou (1), J. Anderson (1),
%                                G. Valmorbida (1), S. Prajna (2),
%                                P. Seiler (3), P. A. Parrilo (4)
% (1) Department of Engineering Science, University of Oxford, Oxford, U.K.
% (2) Control and Dynamical Systems - California Institute of Technology,
%     Pasadena, CA 91125, USA.
% (3) Aerospace and Engineering Mechanics Department, University of
%     Minnesota, Minneapolis, MN 55455-0153, USA.
% (4) Laboratory for Information and Decision Systems, M.I.T.,
%     Massachusetts, MA 02139-4307
%
% Send bug reports and feedback to: sostools@cds.caltech.edu
%
% 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, see <http://www.gnu.org/licenses/>.

% 1/30/2003: PJS  Initial Coding

[nra,nca] = size(a);
ridx = (1:nra)';
ridx = ridx(:,ones(1,m));
cidx = (1:nca)';
cidx = cidx(:,ones(1,n));
b = a(ridx,cidx);


function disp( x, name )
    %DISP Display TT/MPS tensor.
    %   disp(X) displays the TT/MPS tensor X as a tensor network.

    %   TTeMPS Toolbox.
    %   Michael Steinlechner, 2013-2016
    %   Questions and contact: michael.steinlechner@epfl.ch
    %   BSD 2-clause license, see LICENSE.txt

    if (nargin < 2 || ~ischar(name))
      name = inputname(1);
    end

    disp([name, ' is a TT/MPS tensor of order ', num2str(x.order), ...
                        ' with size (', num2str(x.size), ...
                        ') and ranks (', num2str(x.rank), ')']);
    disp('');

    row1 = '';
    row2 = '';
    row3 = '';

    for i=1:x.order
        row1 = [row1, sprintf('%3i--(U%2i)--', x.rank(i), i)];
        row2 = [row2, '       |    '];
        row3 = [row3, sprintf('     %3i    ', x.size(i))];
    end
    row1 = [row1, sprintf( '%3i', x.rank(end) )];
    disp(row1)
    disp(row2)
    disp(row3)

end


function [indices, pot] = find_most_prob_entry(pot)
% function [indices, pot] = find_most_prob_entry(pot)
% function [indices, pot] = find_most_prob_entry(pot)
% Find the indices of the argmax, and set all other enties to 0.

%indices = argmax(pot.T);
[m i] = max(pot.T(:));
indices = ind2subv(pot.sizes, i);
pot.T = 0*myones(pot.sizes);
pot.T(i) = m;