%% brute_force_tune
% Code to test the performance of various tuning parameters
% Works sorta like RANSAC I guess?
% Adam Werries 2016, see Apache 2.0 license.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% pos_sd_min = linspace(0,2,2000);
% num_items = length(pos_sd_min);
% rms_error_filter = Inf*ones(1,num_items);
% max_error_filter = Inf*ones(1,num_items);
% parfor i = 1:num_items
%     fprintf('Iteration: %d, pos_sd_min: %08.7f\n', i, pos_sd_min(i));
%     temp_conf = LC_KF_config;
%     temp_conf.pos_sd_min = pos_sd_min(i);
%     [out_profile,out_IMU_bias_est,out_KF_SD, out_R_matrix, out_Q_matrix, corrections] = ...
%         Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, temp_conf, est_IMU_bias);
%     xyz = out_profile(:,2:4);
%     if ~any(any(isnan(xyz))) && ~any(any(isinf(xyz)))
%         llh = ecef2lla(xyz);
%         [x,y] = deg2utm(llh(:,1),llh(:,2));
%         x = x-min_x;
%         y = y-min_y;
%         distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
%         rms_error_filter(i) = rms(distance);
%         max_error_filter(i) = max(distance);
%     end
% end
%
% [minmax, i] = min(max_error_filter);
% fprintf('\nBest max: %08.7f, rms is %08.7f\n', minmax, rms_error_filter(i));
% fprintf('Best iteration for max: %d, pos_sd_min: %08.7f\n', i, pos_sd_min(i));
% [minrms, i] = min(rms_error_filter);
% fprintf('Best rms: %08.7f, max is %08.7f\n', minrms, max_error_filter(i));
% fprintf('Best iteration for rms: %d, pos_sd_min: %08.7f\n', i, pos_sd_min(i));
% figurec;
% subplot(211);
% plot(pos_sd_min, rms_error_filter);
% xlabel('pos_sd_min');
% ylabel('max error');
%
% subplot(212);
% plot(pos_sd_min, max_error_filter);
% xlabel('pos_sd_min');
% ylabel('max error');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% pos_sd_max = linspace(0,200,2000);
% num_items = length(pos_sd_max);
% rms_error_filter = Inf*ones(1,num_items);
% max_error_filter = Inf*ones(1,num_items);
% parfor i = 1:num_items
%     fprintf('Iteration: %d, pos_sd_max: %08.7f\n', i, pos_sd_max(i));
%     temp_conf = LC_KF_config;
%     temp_conf.pos_sd_max = pos_sd_max(i);
%     [out_profile,out_IMU_bias_est,out_KF_SD] = Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, temp_conf, est_IMU_bias);
%     xyz = out_profile(:,2:4);
%     if ~any(any(isnan(xyz))) && ~any(any(isinf(xyz)))
%         llh = ecef2lla(xyz);
%         [x,y] = deg2utm(llh(:,1),llh(:,2));
%         x = x-min_x;
%         y = y-min_y;
%         distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
%         rms_error_filter(i) = rms(distance);
%         max_error_filter(i) = max(distance);
%     end
% end
%
% [minmax, i] = min(max_error_filter);
% fprintf('\nBest max: %08.7f, rms is %08.7f\n', minmax, rms_error_filter(i));
% fprintf('Best iteration for max: %d, pos_sd_max: %08.7f\n', i, pos_sd_max(i));
% [minrms, i] = min(rms_error_filter);
% fprintf('Best rms: %08.7f, max is %08.7f\n', minrms, max_error_filter(i));
% fprintf('Best iteration for rms: %d, pos_sd_max: %08.7f\n', i, pos_sd_max(i));
% figurec;
% plot(pos_sd_max, max_error_filter); hold on;
% plot(pos_sd_max, rms_error_filter);

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vel_sd_min = linspace(0,30,2000);
% num_items = length(vel_sd_min);
% rms_error_filter = Inf*ones(1,num_items);
% max_error_filter = Inf*ones(1,num_items);
% parfor i = 1:num_items
%     fprintf('Iteration: %d, vel_sd_min: %08.7f\n', i, vel_sd_min(i));
%     temp_conf = LC_KF_config;
%     temp_conf.vel_sd_min = vel_sd_min(i);
%     [out_profile,out_IMU_bias_est,out_KF_SD] = Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, temp_conf, est_IMU_bias);
%     xyz = out_profile(:,2:4);
%     if ~any(any(isnan(xyz))) && ~any(any(isinf(xyz)))
%         llh = ecef2lla(xyz);
%         [x,y] = deg2utm(llh(:,1),llh(:,2));
%         x = x-min_x;
%         y = y-min_y;
% %         h = -llh(:,3);
%         distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
%         rms_error_filter(i) = rms(distance);
%         max_error_filter(i) = max(distance);
%     end
% end
%
% [minmax, i] = min(max_error_filter);
% fprintf('\nBest max: %08.7f, rms is %08.7f\n', minmax, rms_error_filter(i));
% fprintf('Best iteration for max: %d, vel_sd_min: %08.7f\n', i, vel_sd_min(i));
% [minrms, i] = min(rms_error_filter);
% fprintf('Best rms: %08.7f, max is %08.7f\n', minrms, max_error_filter(i));
% fprintf('Best iteration for rms: %d, vel_sd_min: %08.7f\n', i, vel_sd_min(i));
% figure;
% plot(vel_sd_min, max_error_filter); hold on;
% plot(vel_sd_min, rms_error_filter);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
vel_sd_max = linspace(1,50,200);
num_items = length(vel_sd_max);
rms_error_filter = Inf*ones(1,num_items);
max_error_filter = Inf*ones(1,num_items);
parfor i = 1:num_items
    fprintf('Iteration: %d, vel_sd_max: %08.7f\n', i, vel_sd_max(i));
    temp_conf = LC_KF_config;
    temp_conf.vel_sd_max = vel_sd_max(i);
    [out_profile,out_IMU_bias_est,out_KF_SD, out_R_matrix, out_Q_matrix, corrections] = ...
        Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, temp_conf, est_IMU_bias);
    xyz = out_profile(:,2:4);
    if ~any(any(isnan(xyz))) && ~any(any(isinf(xyz)))
        llh = ecef2lla(xyz);
        [x,y] = deg2utm(llh(:,1),llh(:,2));
        x = x-min_x;
        y = y-min_y;
%         h = -llh(:,3);
%         distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2 + (ground_truth_full(:,3)-h).^2).^0.5;
        distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
        rms_error_filter(i) = rms(distance);
        max_error_filter(i) = max(distance);
    end
end

[minmax, i] = min(max_error_filter);
fprintf('\nBest max: %08.7f, rms is %08.7f\n', minmax, rms_error_filter(i));
fprintf('Best iteration for max: %d, vel_sd_max: %08.7f\n', i, vel_sd_max(i));
[minrms, i] = min(rms_error_filter);
fprintf('Best rms: %08.7f, max is %08.7f\n', minrms, max_error_filter(i));
fprintf('Best iteration for rms: %d, vel_sd_max: %08.7f\n', i, vel_sd_max(i));
figurec;
plot(vel_sd_max, max_error_filter); hold on;
plot(vel_sd_max, rms_error_filter);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% gps_pos_stddev = linspace(.001,10,2000);
% num_items = length(gps_pos_stddev);
% rms_error_filter = Inf*ones(1,num_items);
% max_error_filter = Inf*ones(1,num_items);
% parfor i = 1:num_items
%     fprintf('Iteration: %d, gps_pos_stddev: %08.7f\n', i, gps_pos_stddev(i));
%     temp_conf = LC_KF_config;
%     temp_conf.gps_pos_stddev = gps_pos_stddev(i);
%     [out_profile,out_IMU_bias_est,out_KF_SD, out_R_matrix, out_Q_matrix, corrections] = ...
%         Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, temp_conf, est_IMU_bias);
%     xyz = out_profile(:,2:4);
%     if ~any(any(isnan(xyz))) && ~any(any(isinf(xyz)))
%         llh = ecef2lla(xyz);
%         [x,y] = deg2utm(llh(:,1),llh(:,2));
%         x = x-min_x;
%         y = y-min_y;
% %         h = -llh(:,3);
% %         distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2 + (ground_truth_full(:,3)-h).^2).^0.5;
%         distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
%         rms_error_filter(i) = rms(distance);
%         max_error_filter(i) = max(distance);
%     end
% end
%
% [minmax, i] = min(max_error_filter);
% fprintf('\nBest max: %08.7f, rms is %08.7f\n', minmax, rms_error_filter(i));
% fprintf('Best iteration for max: %d, gps_pos_stddev: %08.7f\n', i, gps_pos_stddev(i));
% [minrms, i] = min(rms_error_filter);
% fprintf('Best rms: %08.7f, max is %08.7f\n', minrms, max_error_filter(i));
% fprintf('Best iteration for rms: %d, gps_pos_stddev: %08.7f\n', i, gps_pos_stddev(i));
% figurec;
% plot(gps_pos_stddev, max_error_filter); hold on;
% plot(gps_pos_stddev, rms_error_filter);

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% gps_vel_stddev = linspace(.01,100,2000);
% num_items = length(gps_vel_stddev);
% rms_error_filter = Inf*ones(1,num_items);
% max_error_filter = Inf*ones(1,num_items);
% parfor i = 1:num_items
%     fprintf('Iteration: %d, vel_sd_max: %08.7f\n', i, gps_vel_stddev(i));
%     temp_conf = LC_KF_config;
%     temp_conf.gps_vel_stddev = gps_vel_stddev(i);
%     [out_profile,out_IMU_bias_est,out_KF_SD, out_R_matrix, out_Q_matrix, corrections] = ...
%         Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, temp_conf, est_IMU_bias);
%     xyz = out_profile(:,2:4);
%     if ~any(any(isnan(xyz))) && ~any(any(isinf(xyz)))
%         llh = ecef2lla(xyz);
%         [x,y] = deg2utm(llh(:,1),llh(:,2));
%         x = x-min_x;
%         y = y-min_y;
% %         h = -llh(:,3);
% %         distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2 + (ground_truth_full(:,3)-h).^2).^0.5;
%         distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;        rms_error_filter(i) = rms(distance);
%         max_error_filter(i) = max(distance);
%     end
% end
%
% [minmax, i] = min(max_error_filter);
% fprintf('\nBest max: %08.7f, rms is %08.7f\n', minmax, rms_error_filter(i));
% fprintf('Best iteration for max: %d, gps_vel_stddev: %08.7f\n', i, gps_vel_stddev(i));
% [minrms, i] = min(rms_error_filter);
% fprintf('Best rms: %08.7f, max is %08.7f\n', minrms, max_error_filter(i));
% fprintf('Best iteration for rms: %d, gps_vel_stddev: %08.7f\n', i, gps_vel_stddev(i));
% figurec;
% plot(gps_vel_stddev, max_error_filter); hold on;
% plot(gps_vel_stddev, rms_error_filter);

load handel
sound(y,Fs)


function [x,zo,xs]=estnoisem(yf,tz,pp)
%ESTNOISEM - estimate noise spectrum using minimum statistics
%
% Usage:    ninc=round(0.016*fs);   % frame increment [fs=sample frequency]
%           ovf=2;                  % overlap factor
%           f=rfft(enframe(s,hanning(ovf*ninc,'periodic'),ninc),ovf*ninc,2);
%           f=f.*conj(f);           % convert to power spectrum
%           x=estnoisem(f,ninc/fs); % estimate the noise power spectrum
%
% Inputs:
%   yf      input power spectra (one row per frame)
%   tz      frame increment in seconds
%           Alternatively, the input state from a previous call (see below)
%   pp      algorithm parameters [optional]
%
% Outputs:
%   x       estimated noise power spectra (one row per frame)
%   zo      output state
%   xs      estimated std error of x (one row per frame)
%           xs seems often to be an underestimate by a factor of 2 or 3
%
% The algorithm parameters are defined in reference [1] from which equation
% numbers are given in parentheses. They are as follows:
%
%        pp.taca      % (11): smoothing time constant for alpha_c [0.0449 seconds]
%        pp.tamax     % (3): max smoothing time constant [0.392 seconds]
%        pp.taminh    % (3): min smoothing time constant (upper limit) [0.0133 seconds]
%        pp.tpfall    % (12): time constant for P to fall [0.064 seconds]
%        pp.tbmax     % (20): max smoothing time constant [0.0717 seconds]
%        pp.qeqmin    % (23): minimum value of Qeq [2]
%        pp.qeqmax    % max value of Qeq per frame [14]
%        pp.av        % (23)+13 lines: fudge factor for bc calculation  [2.12]
%        pp.td        % time to take minimum over [1.536 seconds]
%        pp.nu        % number of subwindows to use [3]
%        pp.qith      % Q-inverse thresholds to select maximum noise slope [0.03 0.05 0.06 Inf ]
%        pp.nsmdb     % corresponding noise slope thresholds in dB/second   [47 31.4 15.7 4.1]
%
% Example use:      y=enframe(s,w,ni);                  % divide speech signal s(n) into
%                                                       % overlapping frames using window w(n)
%                   yf=rfft(y,nf,2);                    % take fourier transform
%                   dp=estnoisem(yf.*conj(yf),tinc);    % estimate the noise
%
% If convenient, you can call estnoisem in chunks of arbitrary size. Thus the following are equivalent:
%
%                   (a) dp=estnoisem(yp(1:300),tinc);
%
%                   (b) [dp(1:100),z]=estnoisem(yp(1:100),tinc);
%                       [dp(101:200),z]=estnoisem(yp(101:200),z);
%                       [dp(201:300),z]=estnoisem(yp(201:300),z);

% This is intended to be a precise implementation of [1] with Table III
% replaced by the updated table 5 from [2]. The only deliberate algorithm
% change is the introduction of a minimum value for 1/Qeq in equation (23).
% This change only affects the first few frames and improves the
% convergence of the algorithm. A minor improveemnt was reported in [3] but
% this has not yet been included.
%
% Refs:
%    [1] Rainer Martin.
%        Noise power spectral density estimation based on optimal smoothing and minimum statistics.
%        IEEE Trans. Speech and Audio Processing, 9(5):504-512, July 2001.
%    [2] Rainer Martin.
%        Bias compensation methods for minimum statistics noise power spectral density estimation
%        Signal Processing, 2006, 86, 1215-1229
%    [3] Dirk Mauler and Rainer Martin
%        Noise power spectral density estimation on highly correlated data
%        Proc IWAENC, 2006

%	   Copyright (C) Mike Brookes 2008
%      Version: $Id: estnoisem.m 1718 2012-03-31 16:40:41Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nr,nrf]=size(yf);          % number of frames and freq bins
x=zeros(nr,nrf);            % initialize output arrays
xs=zeros(nr,nrf);           % will hold std error in the future
if isempty(yf) && isstruct(tz)             % no real data
    zo=tz;              % just keep the same state
else
    if isstruct(tz)       % take parameters from a previous call
        nrcum=tz.nrcum;
        p=tz.p;          % smoothed power spectrum
        ac=tz.ac;               % correction factor (9)
        sn2=tz.sn2;              % estimated noise power
        pb=tz.pb;               % smoothed noisy speech power (20)
        pb2=tz.pb2;
        pminu=tz.pminu;
        actmin=tz.actmin;   % Running minimum estimate
        actminsub=tz.actminsub;           % sub-window minimum estimate
        subwc=tz.subwc;                   % force a buffer switch on first loop
        actbuf=tz.actbuf;  % buffer to store subwindow minima
        ibuf=tz.ibuf;
        lminflag=tz.lminflag;      % flag to remember local minimum
        tinc=tz.tinc;     % frame increment
        qq=tz.qq;         % parameter structure
    else
        tinc = tz;          % second argument is frame increment
        nrcum=0;            % no frames so far
        % default algorithm constants

        qq.taca=0.0449;    % smoothing time constant for alpha_c = -tinc/log(0.7) in equ (11)
        qq.tamax=0.392;    % max smoothing time constant in (3) = -tinc/log(0.96)
        qq.taminh=0.0133;    % min smoothing time constant (upper limit) in (3) = -tinc/log(0.3)
        qq.tpfall=0.064;   % time constant for P to fall (12)
        qq.tbmax=0.0717;   % max smoothing time constant in (20) = -tinc/log(0.8)
        qq.qeqmin=2;       % minimum value of Qeq (23)
        qq.qeqmax=14;      % max value of Qeq per frame
        qq.av=2.12;             % fudge factor for bc calculation (23 + 13 lines)
        qq.td=1.536;       % time to take minimum over
        qq.nu=8;           % number of subwindows
        qq.qith=[0.03 0.05 0.06 Inf]; % noise slope thresholds in dB/s
        qq.nsmdb=[47 31.4 15.7 4.1];

        if nargin>=3 && ~isempty(pp)
            qqn=fieldnames(qq);
            for i=1:length(qqn)
                if isfield(pp,qqn{i})
                    qq.(qqn{i})=pp.(qqn{i});
                end
            end
        end
    end

    % unpack parameter structure

    taca=qq.taca;    % smoothing time constant for alpha_c = -tinc/log(0.7) in equ (11)
    tamax=qq.tamax;    % max smoothing time constant in (3) = -tinc/log(0.96)
    taminh=qq.taminh;    % min smoothing time constant (upper limit) in (3) = -tinc/log(0.3)
    tpfall=qq.tpfall;   % time constant for P to fall (12)
    tbmax=qq.tbmax;   % max smoothing time constant in (20) = -tinc/log(0.8)
    qeqmin=qq.qeqmin;       % minimum value of Qeq (23)
    qeqmax=qq.qeqmax;      % max value of Qeq per frame
    av=qq.av;             % fudge factor for bc calculation (23 + 13 lines)
    td=qq.td;       % time to take minimum over
    nu=qq.nu;           % number of subwindows
    qith=qq.qith; % noise slope thresholds in dB/s
    nsmdb=qq.nsmdb;   % maximum permitted +ve noise slope in dB/s

    % derived algorithm constants

    aca=exp(-tinc/taca); % smoothing constant for alpha_c in equ (11) = 0.7
    acmax=aca;          % min value of alpha_c = 0.7 in equ (11) also = 0.7
    amax=exp(-tinc/tamax); % max smoothing constant in (3) = 0.96
    aminh=exp(-tinc/taminh); % min smoothing constant (upper limit) in (3) = 0.3
    bmax=exp(-tinc/tbmax); % max smoothing constant in (20) = 0.8
    snrexp = -tinc/tpfall;
    nv=round(td/(tinc*nu));    % length of each subwindow in frames
    if nv<4            % algorithm doesn't work for miniscule frames
        nv=4;
        nu=max(round(td/(tinc*nv)),1);
    end
    nd=nu*nv;           % length of total window in frames
    [md,hd]=mhvals(nd); % calculate the constants M(D) and H(D) from Table III
    [mv,hv]=mhvals(nv); % calculate the constants M(D) and H(D) from Table III
    nsms=10.^(nsmdb*nv*tinc/10);  % [8 4 2 1.2] in paper
    qeqimax=1/qeqmin;  % maximum value of Qeq inverse (23)
    qeqimin=1/qeqmax; % minumum value of Qeq per frame inverse

    if isempty(yf)      % provide dummy initialization
        ac=1;               % correction factor (9)
        subwc=nv;                   % force a buffer switch on first loop
        ibuf=0;
        p=x;          % smoothed power spectrum
        sn2=p;              % estimated noise power
        pb=p;               % smoothed noisy speech power (20)
        pb2=pb.^2;
        pminu=p;
        actmin=repmat(Inf,1,nrf);   % Running minimum estimate
        actminsub=actmin;           % sub-window minimum estimate
        actbuf=repmat(Inf,nu,nrf);  % buffer to store subwindow minima
        lminflag=zeros(1,nrf);      % flag to remember local minimum
    else

        if ~nrcum       % initialize values for first frame
            p=yf(1,:);          % smoothed power spectrum
            ac=1;               % correction factor (9)
            sn2=p;              % estimated noise power
            pb=p;               % smoothed noisy speech power (20)
            pb2=pb.^2;
            pminu=p;
            actmin=repmat(Inf,1,nrf);   % Running minimum estimate
            actminsub=actmin;           % sub-window minimum estimate
            subwc=nv;                   % force a buffer switch on first loop
            actbuf=repmat(Inf,nu,nrf);  % buffer to store subwindow minima
            ibuf=0;
            lminflag=zeros(1,nrf);      % flag to remember local minimum
        end

        % loop for each frame

        for t=1:nr              % we use t instead of lambda in the paper
            yft=yf(t,:);        % noise speech power spectrum
            acb=(1+(sum(p)./sum(yft)-1).^2).^(-1);  % alpha_c-bar(t)  (9)
            ac=aca*ac+(1-aca)*max(acb,acmax);       % alpha_c(t)  (10)
            ah=amax*ac.*(1+(p./sn2-1).^2).^(-1);    % alpha_hat: smoothing factor per frequency (11)
            snr=sum(p)/sum(sn2);
            ah=max(ah,min(aminh,snr^snrexp));       % lower limit for alpha_hat (12)

            p=ah.*p+(1-ah).*yft;            % smoothed noisy speech power (3)
            b=min(ah.^2,bmax);              % smoothing constant for estimating periodogram variance (22 + 2 lines)
            pb=b.*pb + (1-b).*p;            % smoothed periodogram (20)
            pb2=b.*pb2 + (1-b).*p.^2;     	% smoothed periodogram squared (21)

            qeqi=max(min((pb2-pb.^2)./(2*sn2.^2),qeqimax),qeqimin/(t+nrcum));   % Qeq inverse (23)
            qiav=sum(qeqi)/nrf;             % Average over all frequencies (23+12 lines) (ignore non-duplication of DC and nyquist terms)
            bc=1+av*sqrt(qiav);             % bias correction factor (23+11 lines)
            bmind=1+2*(nd-1)*(1-md)./(qeqi.^(-1)-2*md);      % we use the simplified form (17) instead of (15)
            bminv=1+2*(nv-1)*(1-mv)./(qeqi.^(-1)-2*mv);      % same expression but for sub windows
            kmod=bc*p.*bmind<actmin;        % Frequency mask for new minimum
            if any(kmod)
                actmin(kmod)=bc*p(kmod).*bmind(kmod);
                actminsub(kmod)=bc*p(kmod).*bminv(kmod);
            end
            if subwc>1 && subwc<nv              % middle of buffer - allow a local minimum
                lminflag=lminflag | kmod;    	% potential local minimum frequency bins
                pminu=min(actminsub,pminu);
                sn2=pminu;
            else
                if subwc>=nv                    % end of buffer - do a buffer switch
                    ibuf=1+rem(ibuf,nu);     	% increment actbuf storage pointer
                    actbuf(ibuf,:)=actmin;    	% save sub-window minimum
                    pminu=min(actbuf,[],1);
                    i=find(qiav<qith);
                    nsm=nsms(i(1));          	% noise slope max
                    lmin=lminflag & ~kmod & actminsub<nsm*pminu & actminsub>pminu;
                    if any(lmin)
                        pminu(lmin)=actminsub(lmin);
                        actbuf(:,lmin)=repmat(pminu(lmin),nu,1);
                    end
                    lminflag(:)=0;
                    actmin(:)=Inf;
                    subwc=0;
                end
            end
            subwc=subwc+1;
            x(t,:)=sn2;
            qisq=sqrt(qeqi);
            % empirical formula for standard error based on Fig 15 of [2]
            xs(t,:)=sn2.*sqrt(0.266*(nd+100*qisq).*qisq/(1+0.005*nd+6/nd)./(0.5*qeqi.^(-1)+nd-1));
        end
    end
    if nargout>1    % we need to store the state for next time
        zo.nrcum=nrcum+nr;      % number of frames so far
        zo.p=p;          % smoothed power spectrum
        zo.ac=ac;               % correction factor (9)
        zo.sn2=sn2;              % estimated noise power
        zo.pb=pb;               % smoothed noisy speech power (20)
        zo.pb2=pb2;
        zo.pminu=pminu;
        zo.actmin=actmin;   % Running minimum estimate
        zo.actminsub=actminsub;           % sub-window minimum estimate
        zo.subwc=subwc;                   % force a buffer switch on first loop
        zo.actbuf=actbuf;  % buffer to store subwindow minima
        zo.ibuf=ibuf;
        zo.lminflag=lminflag;      % flag to remember local minimum
        zo.tinc=tinc;     % must be the last one
        zo.qq=qq;
    end
    if ~nargout
        clf;
        subplot(212);
        plot((1:nr)*tinc,10*log10([sum(yf,2) sum(x,2)]))
        ylabel('Frame Energy (dB)');
        xlabel(sprintf('Time (s)   [%d ms frame incr]',round(tinc*1000)));
        axisenlarge([-1 -1.05]);
        legend('input','noise','Location','Best');
        subplot(211);
        plot(1:nrf,10*log10([sum(yf,1)'/nr sum(x,1)'/nr]))
        ylabel('Power (dB)');
        xlabel('Frequency bin');
        axisenlarge([-1 -1.05]);
        legend('input','noise','Location','Best');
    end
end

function [m,h,d]=mhvals(d)
% Values are taken from Table 5 in [2]
%[2] R. Martin,"Bias compensation methods for minimum statistics noise power
%               spectral density estimation", Signal Processing Vol 86, pp1215-1229, 2006.

% approx: plot(d.^(-0.5),[m 1-d.^(-0.5)],'x-'), plot(d.^0.5,h,'x-')
persistent dmh
if isempty(dmh)
    dmh=[
        1   0       0;
        2   0.26    0.15;
        5   0.48    0.48;
        8   0.58    0.78;
        10  0.61    0.98;
        15  0.668   1.55;
        20  0.705   2;
        30  0.762   2.3;
        40  0.8     2.52;
        60  0.841   3.1;
        80  0.865   3.38;
        120 0.89    4.15;
        140 0.9     4.35;
        160 0.91    4.25;
        180 0.92    3.9;
        220 0.93    4.1;
        260 0.935   4.7;
        300 0.94    5];
end

if nargin>=1
    i=find(d<=dmh(:,1));
    if isempty(i)
        i=size(dmh,1);
        j=i;
    else
        i=i(1);
        j=i-1;
    end
    if d==dmh(i,1)
        m=dmh(i,2);
        h=dmh(i,3);
    else
        qj=sqrt(dmh(i-1,1));    % interpolate using sqrt(d)
        qi=sqrt(dmh(i,1));
        q=sqrt(d);
        h=dmh(i,3)+(q-qi)*(dmh(j,3)-dmh(i,3))/(qj-qi);
        m=dmh(i,2)+(qi*qj/q-qj)*(dmh(j,2)-dmh(i,2))/(qi-qj);
    end
else
    d=dmh(:,1);
    m=dmh(:,2);
    h=dmh(:,3);
end


% GEO_LOAD: create a geometry structure from a file or a cell-array of functions.
%
% geometry = geo_load (input)
%
% INPUT :
%
%   The input variable may be either
%   - a structure representing a NURBS surface or volume, as in the NURBS toolbox
%   - a string variable with the name of the file to be read (see doc/geo_specs_v21.txt)
%   - a cell-array of function handles, to evaluate the function, the first order
%       derivatives, and eventually the second order derivatives
%   - a 4x4 matrix representing an affine transformation
%
% OUTPUT:
%
%   geometry: a structure that contains, at least, the following fields
%             map:      a function handle to evaluate the parameterization
%             map_der:  a function handle to evaluate the derivatives of the parameterization
%             map_der2: a function handle to evaluate the second derivatives of the parameterization
%   The structure may contain further information. See the documentation.
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2013 Rafael Vazquez
% Copyright (C) 2014 Elena Bulgarello, Carlo de Falco, Sara Frizziero
% Copyright (C) 2015 Rafael Vazquez
%
% 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 Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

function geometry = geo_load (in)

  if (isstruct (in) && isfield (in, 'form') && strcmpi (in.form, 'B-NURBS'))
%% geometry is given as a NURBS struct
    geometry.nurbs   = in;

  elseif (ischar (in))
%% load geometry from a file
    if (strcmpi (in(end-3:end), '.mat'))
      tmp  = load (in);
      geometry.nurbs   = tmp.geo;
    elseif (strcmpi (in(end-3:end), '.txt'))
      geometry = geo_read_nurbs (in);
%% load geometry from an xml file
    elseif (strcmpi (in(end-3:end), '.xml'))
      geometry.nurbs = xml_import (in);
    else
      error ('geo_load: unknown file extension');
    end

  elseif (iscell (in))
%% geometry is given as a cell array of functions
    geometry.map     =  in{1};
    geometry.map_der =  in{2};
    if (length (in) > 2)
      geometry.map_der2 =  in{3};
    end

  elseif (isnumeric (in) && all (size (in) == [4, 4]))
%% geometry is given as a 4x4 matrix representing an affine transformation
    geometry.map = @(ps) affine_map  (ps, in);
    geometry.map_der = @(ps) affine_map_der  (ps, in);
    geometry.map_der2 = @affine_map_der2;
  else
    error ('geo_load: wrong input type');
  end

% In case the geometry is a NURBS structure, we use the NURBS toolbox
% This allows to use a rectangular parametric domain, instead of [0,1]^n.
  if (isfield (geometry, 'nurbs'))
    if (any (abs(geometry.nurbs.coefs(3,:)) > 1e-12))
      rdim = 3;
    elseif (any (abs(geometry.nurbs.coefs(2,:)) > 1e-12) || (isfield(in, 'boundary_flag') && in.boundary_flag))
      rdim = 2;
    else
      rdim = 1;
    end
    geometry.rdim = rdim;

    wrn_struct = warning ('query', 'nrbderiv:SecondDerivative');
    warning ('off', 'nrbderiv:SecondDerivative')
    [deriv, deriv2] = nrbderiv (geometry.nurbs);
    geometry.dnurbs = deriv;
    geometry.dnurbs2 = deriv2;

    geometry.map      =  @(PTS) geo_nurbs (geometry.nurbs, deriv, deriv2, PTS, 0, rdim);
    geometry.map_der  =  @(PTS) geo_nurbs (geometry.nurbs, deriv, deriv2, PTS, 1, rdim);
    geometry.map_der2 =  @(PTS) geo_nurbs (geometry.nurbs, deriv, deriv2, PTS, 2, rdim);

    if (numel (geometry.nurbs.order) > 1)
      bnd = nrbextract (geometry.nurbs);
      for ibnd = 1:numel (bnd)
        [deriv, deriv2] = nrbderiv (bnd(ibnd));
        geometry.boundary(ibnd).nurbs    = bnd(ibnd);
        geometry.boundary(ibnd).dnurbs   = deriv;
        geometry.boundary(ibnd).dnurbs2  = deriv2;
        geometry.boundary(ibnd).rdim     = rdim;
        geometry.boundary(ibnd).map      = @(PTS) geo_nurbs (bnd(ibnd), deriv, deriv2, PTS, 0, rdim);
        geometry.boundary(ibnd).map_der  = @(PTS) geo_nurbs (bnd(ibnd), deriv, deriv2, PTS, 1, rdim);
        geometry.boundary(ibnd).map_der2 = @(PTS) geo_nurbs (bnd(ibnd), deriv, deriv2, PTS, 2, rdim);
      end
    end
    if (strcmpi (wrn_struct.state, 'on'))
      warning ('on', 'nrbderiv:SecondDerivative')
    end
  else
    for ibnd = 1:6 % This loop should be until 2*ndim, but ndim is not known
      geometry.boundary(ibnd).map     = @(PTS) boundary_map (geometry.map, ibnd, PTS);
      geometry.boundary(ibnd).map_der = @(PTS) boundary_map_der (geometry.map, geometry.map_der, ibnd, PTS);
    end
  end

end

function mps = affine_map  (ps, in)
  if (iscell (ps))
    ndim = numel (ps);
    npts = cellfun (@numel, ps);
    nps = prod (npts);
    if (ndim == 2)
      u = reshape (repmat (ps{1}(:), 1, npts(2)), 1, []);
      v = reshape (repmat (ps{2}(:)', npts(1), 1), 1, []);
      ps = [u; v];
    elseif (ndim == 3)
      u = reshape (ps{1}, npts(1), 1, 1);
      u = reshape (repmat (u, [1, npts(2), npts(3)]), 1, []);
      v = reshape (ps{2}, 1, npts(2), 1);
      v = reshape (repmat (v, [npts(1), 1, npts(3)]), 1, []);
      w = reshape (ps{3}, 1, 1, npts(3));
      w = reshape (repmat (w, [npts(1), npts(2), 1]), 1, []);
      ps = [u; v; w];
    end
  else
    ndim = size (ps, 1);
    nps  = size (ps, 2);
  end
  mps  = in([1:ndim, 4], [1:ndim, 4]) * [ps; ones(1, nps)];
  mps  = mps (1:ndim, :);
end

function mps = affine_map_der  (ps, in)
  if (iscell (ps))
    ndim = numel (ps);
    npts = cellfun (@numel, ps);
    nps = prod (npts);
    if (ndim == 2)
      u = reshape (repmat (ps{1}(:), 1, npts(2)), 1, []);
      v = reshape (repmat (ps{2}(:)', npts(1), 1), 1, []);
      ps = [u; v];
    elseif (ndim == 3)
      u = reshape (ps{1}, npts(1), 1, 1);
      u = reshape (repmat (u, [1, npts(2), npts(3)]), 1, []);
      v = reshape (ps{2}, 1, npts(2), 1);
      v = reshape (repmat (v, [npts(1), 1, npts(3)]), 1, []);
      w = reshape (ps{3}, 1, 1, npts(3));
      w = reshape (repmat (w, [npts(1), npts(2), 1]), 1, []);
      ps = [u; v; w];
    end
  else
    ndim = size (ps, 1);
    nps  = size (ps, 2);
  end
  mps  = repmat (in(1:ndim, 1:ndim), [1, 1, nps]);
end

function mps = affine_map_der2  (ps)
  if (iscell (ps))
    ndim = numel (ps);
    nps = prod (cellfun (@numel, ps));
  else
    ndim = size (ps, 1);
    nps  = size (ps, 2);
  end
  mps  = zeros (ndim, ndim, ndim, nps);
end

% These two functions are to compute boundary entities from the global ones
function F = boundary_map (map, iside, pts)

%%    ind  = [2 3; 2 3; 1 3; 1 3; 1 2; 1 2] in 3D, %ind  = [2 2 1 1] in 2D;
%%    ind2 = [1 1 2 2 3 3] in 3D,                  %ind2 = [1 1 2 2] in 2D
  ind2 = ceil (iside/2);

  if (iscell (pts))
    ndim = numel (pts) + 1;
    ind = setdiff (1:ndim, ind2);

    pts_aux(ind) = pts;
    if (mod (iside, 2) == 1)
      pts_aux{ind2} = 0;
    else
      pts_aux{ind2} = 1;
    end
  else
    error ('For the boundary, a cell array should be passed as the argument')
  end

  F = map (pts_aux);

end

function varargout = boundary_map_der (map, map_der, iside, pts)

%%    ind  = [2 3; 2 3; 1 3; 1 3; 1 2; 1 2] in 3D, %ind  = [2 2 1 1] in 2D;
%%    ind2 = [1 1 2 2 3 3] in 3D,                  %ind2 = [1 1 2 2] in 2D
  ind2 = ceil (iside/2);

  if (iscell (pts))
    ndim = numel (pts) + 1;
    ind = setdiff (1:ndim, ind2);

    pts_aux(ind) = pts;
    if (mod (iside, 2) == 1)
      pts_aux{ind2} = 0;
    else
      pts_aux{ind2} = 1;
    end
  else
    error ('For the boundary, a cell array should be passed as the argument')
  end

  DF = map_der (pts_aux);
  DF = DF(:,ind,:);
  if (nargout == 1)
    varargout{1} = DF;
  elseif (nargout == 2)
    F = map (pts_aux);
    varargout{1} = F;
    varargout{2} = DF;
  end

end

%!shared g1,g2,x
%!test
%! g1 = geo_load ('geo_ring.txt');
%! g2 = geo_load ('ring.mat');
%! for [v, k] = g1.nurbs
%!  assert (isequal (v, g2.nurbs.(k)))
%! endfor
%! x = rand (2, 10);
%!test
%! assert (g1.map (x), g2.map (x))
%!test
%! assert (g1.map_der (x), g2.map_der (x))


% EDICON Multi-edit and condense a training set
%
% 	J = EDICON(D,NSETS,NITERS,NTRIES)
%
% INPUT
%   D       Distance matrix dataset
%   NSETS   Number of subsets for editing, or [] for no editing (default: 3)
%   NITERS  Number of iterations for editing (default: 5)
%   NTRIES  Number of tries for condensing, or [] for no condensing (dflt: 10)
%
% OUTPUT
%   J       Indices of retained samples
%
% DESCRIPTION
% Returns the set of objects J such that the nearest neigbour gives zero error
% on the remaining objects. If MODE = 0, multi-edit the dataset represented by
% distance matrix D first. D can be computed from a dataset A by A*proxm(A).
%
% REFERENCES
% Devijver, P. and Kittler, J. "Pattern recognition", Prentice-Hall, 1982.
%
% SEE ALSO (PRTools Guide)
% PRDATASET, KNNC, PROXM

% Copyright: E. Pekalska and D. de Ridder, {E.Pekalska,D.deRidder}@ewi.tudelft.nl
% Faculty of Electrical Engineering, Mathematics and Computer Science
% Delft University of Technology, Mekelweg 4, 2628 CD Delft, The Netherlands

function J = edicon (D,no_subsets,max_iter,no_tries)

    if (nargin < 4 | isempty(no_tries))
        prwarning(4,'number of tries for condensing not specified, assuming 10');
        no_tries = 10;
    end;
    if (nargin < 3 | isempty(max_iter))
        prwarning(4,'number of iterations for editing not specified, assuming 5');
        max_iter = 5;
    end;
    if (nargin < 2 | isempty(no_subsets))
        prwarning(4,'number of subsets for editing not specified, assuming 3');
        no_subsets = 5;
    end;

    % Extract dataset information.

	nlab    = getnlab(D); lablist = getlablist(D);
	[m,k,c] = getsize(D); p       = getprior(D);

    if (~isdataset(D) | (m ~= k))
        error('require a square distance matrix dataset');
    end

    J = 1:m;            % Initialise index array.

    % If requested, apply the multi-edit algorithm.

    if (~isempty(no_subsets))
        iter = 1;
        while (iter <= max_iter)

      	    % 1. Create NO_SUBSETS distinct subsets out of the sample indices
            %    remaining in J. Store the subset indices in subset_ind{.}.
            %    Note: this is slightly more complicated than strictly
            %    necessary, to enforce that each class is represented equally
            %    in each subset.

    		m = length(J); J = J(randperm(m));
		    subset_size = floor(m/no_subsets);

            % Find the MC(J) indices of samples belonging to class J in CLASS_IND{J}.
            % Also create a random permutation for class J.

            for j = 1:c
			    class_ind{j} = find(nlab(J)==j);
                mc(j)        = length(class_ind{j});
			    perm{j}      = randperm(mc(j));
    		end;

            % Distribute the samples over the subsets, per class.

    		for i = 1:no_subsets
		        subset_ind{i} = [];
			    for j = 1:c
                    num           = min(floor(mc(j)/no_subsets),length(class_ind{j}));
        		    subset_ind{i} = [subset_ind{i}; class_ind{j}(1:num)];
				    class_ind{j}  = class_ind{j}(num+1:end);
    			end;
			    perm_subset_ind{i} = J(subset_ind{i});
    		end;

          	% 2. In turn, classify each subset I using 1-NN on subset I+1.

          	drop = [];
  	        for i = 1:no_subsets
    			L1 = perm_subset_ind{i};
                L2 = perm_subset_ind{mod(i,no_subsets)+1};
    			if (length(L2)>1)
	  	    	    [dummy,nearest] = min(D(L2,L1));
    			else
				    nearest = 1;
    			end;
                drop = [drop; subset_ind{i}(find(nlab(L1)~=nlab(L2(nearest))))];
            end;

         	% 3. Discard incorrectly classified samples from J.

  	        if (isempty(drop))
  		        iter = iter + 1;
         	else
   		        J(drop) = []; iter = 0;
          	end;
        end;

        D = D(J,J);
    end;

    % Condense.

    if (~isempty(no_tries))
        % Extract dataset information.

    	nlab    = getnlab(D); lablist = getlablist(D);
	    [m,k,c] = getsize(D); p       = getprior(D);

        D = +D;

        % The whole procedure starts from a random object and continues to add
        % objects, so it is not optimal. Therefore it is repeated NO_TRIES times
        % and the smallest set found is returned.

        K = zeros(m,no_tries); storesz = zeros(1,no_tries);
        for o = 1:no_tries

            % Start with 1 sample index in STORE, the others in GRABBAG.
            p = randperm(m); store = p(1); grabbag = p(2:m);

            % While there are changes...
            transfer = 0;
            while (~transfer) & (~isempty(grabbag))
    	        storelab    = nlab(store);
	            grabbaglab  = nlab(grabbag);
	            new_grabbag = [];

                % For all samples in GRABBAG...
            	transfer = 0;
	            for k = 1:length(grabbag)
                    % ... find the nearest sample in STORE...
   	                [dummy,z] = min(D(grabbag(k),store));

                    % ... if it has a different label, move it to STORE.
   	                if (storelab(z) ~= grabbaglab(k))
                        store    = [store; grabbag(k)];
                        storelab = [storelab; grabbaglab(k)];
                        transfer = 1;
               	    else
                        new_grabbag = [new_grabbag; grabbag(k)];
                   	end;
    	        end;

            	grabbag = new_grabbag;

              end

              % Remember the STORE and its size for this repetition.
              storesz(o)        = length(store);
              K(1:storesz(o),o) = store;

        end

        % Take the STORE with minimal size.
        [minsz,minszind] = min(storesz);
        J = J(K(1:minsz,minszind));
    end;

return


function [one_in_K,mapping] = ml_labels2Kencoding( labels )
%ML_LABELS2KENCODING Takes class label data and transforms it to one in K
% encoding.
%
%   input -----------------------------------------------------------------
%
%       o lables : (N x 1), class labels.
%
%   output ----------------------------------------------------------------
%
%       o one_in_K : (N x K), one-in-K encoding of classes
%
%       o mapping  : (num_classes x 1)
%
%

labels      = labels(:);
N           = size(labels,1);
class_label = unique(labels);
num_classes = length(class_label);
mapping     = zeros(num_classes,1);
one_in_K    = zeros(N,num_classes);

for i=1:num_classes
    mapping(i)                           = class_label(i);
    one_in_K(class_label(i) == labels,i) = 1;
end

end


%PREX_PLOTC  PRTools example on the dataset scatter and classifier plot
help prex_plotc
echo on
              % Generate Higleyman data
A = gendath([100 100]);
              % Split the data into the training and test sets
[C,D] = gendat(A,[20 20]);
              % Compute classifiers
w1 = ldc(C);        % linear
w2 = qdc(C);        % quadratic
w3 = parzenc(C);    % Parzen
w4 = dtc(C);        % decision tree
              % Compute and display errors
              % Store classifiers in a cell
W = {w1,w2,w3,w4};
              % Plot errors
disp(D*W*testc);
              % Plot the data and classifiers
figure
              % Make a scatter-plot
scatterd(A);
              % Plot classifiers
plotc({w1,w2,w3,w4});
echo off


function r82row_sort_quick_a_test ( )

%*****************************************************************************80
%
%% R82ROW_SORT_QUICK_A_TEST tests R82ROW_SORT_QUICK_A.
%
%  Discussion:
%
%    An R82ROW is a (2,N) array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 12;

  b = 0.0;
  c = 10.0;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R82ROW_SORT_QUICK_A_TEST\n' );
  fprintf ( 1, '  R82ROW_SORT_QUICK_A sorts an R82ROW\n' );
  fprintf ( 1, '  using quick sort.\n' );
  fprintf ( 1, '  Using initial random number seed = %d\n', seed );

  [ a, seed ] = r8mat_uniform_ab ( 2, n, b, c, seed );
%
%  Give a few elements the same first component.
%
  a(1,3) = a(1,5);
  a(1,4) = a(1,12);
%
%  Give a few elements the same second component.
%
  a(2,6) = a(2,1);
  a(2,2) = a(2,9);
%
%  Make two entries equal.
%
  a(1:2,7) = a(1:2,11);

  r82row_print ( n, a, '  Before rearrangement:' );

  a = r82row_sort_quick_a ( n, a );

  r82row_print ( n, a, '  Sorted array:' );

  return
end


function matrixIndices = blkIndices(blockIndex, blockSize)
% The function returns the indices corresponding to the blockIndex-th block
% for a matrix or vector with blocks of size blockIndex X blockIndex
%
% Luca Carlone
% Georgia Institute of Technology
% 1/5/2013

numBlocks = length(blockIndex);
matrixIndices = zeros(numBlocks*blockSize,1);

for i=1:numBlocks
  matrixIndices(blockSize*i-(blockSize-1) : blockSize*i) = [blockSize*blockIndex(i)-(blockSize-1) : blockSize*blockIndex(i)];
end

end


function status = test_exp_mono(modus)
%TEST_EXP_MONO tests the different expansions of sound fields in frequency-domain
%
%   Usage: status = test_exp_mono(modus)
%
%   Input parameters:
%       modus   - 0: numerical
%                 1: visual
%
%   Output parameters:
%       status  - true or false
%
%   TEST_EXP_MONO(modus) checks, if the circular basis expansions for plane
%   waves and point sources are working. The circular basis expansions are
%   converted to plane wave decompositions. Additionally, modal weighting
%   functions are tested. Optionally, sound field plots of the plane wave
%   decompositions are used for verification.

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

status = false;

%% ===== Checking of input  parameters ===================================
nargmin = 1;
nargmax = 1;
narginchk(nargmin,nargmax);

%% ===== Configuration ===================================================
% Parameters
conf = SFS_config;
conf.plot.loudspeakers = false;  % do not plot loudspeakers
conf.modal_window_parameter = 2.0;  % parameter for kaiser window

c = conf.c;  % speed of sound

% For sound field plots
X = [-2, 2];
Y = [-2, 2];
Z = 0;

f = 500;

% Plane waves with equi-angular distribution
Npw = 1024;
phi0 = (0:Npw-1).'*2*pi/Npw;
x0 = [cos(phi0) sin(phi0)];
x0(:,3) = 0;
x0(:,4:6) = x0(:,1:3);
x0(:,7) = 1;

% Test scenarios
scenarios = { ...
  'pw', [ 0.0 -1.0 0.0],  5, 'rect'   , [0.0 0.0 0.0]
  'pw', [ 0.0 -1.0 0.0], 15, 'rect'   , [0.0 0.0 0.0]
  'pw', [ 0.0 -1.0 0.0], 15, 'kaiser' , [0.0 0.0 0.0]
  'pw', [ 0.0 -1.0 0.0],  5, 'rect'   , [0.5 1.0 0.0]
  'pw', [ 0.0 -1.0 0.0], 15, 'rect'   , [0.5 1.0 0.0]
  'pw', [ 0.0 -1.0 0.0], 15, 'kaiser' , [0.5 1.0 0.0]
  'ps', [ 0.0  2.5 0.0],  5, 'rect'   , [0.0 0.0 0.0]
  'ps', [ 0.0  2.5 0.0], 15, 'rect'   , [0.0 0.0 0.0]
  'ps', [ 0.0  2.5 0.0], 15, 'kaiser' , [0.0 0.0 0.0]
  'ps', [ 0.0  2.5 0.0],  5, 'rect'   , [0.5 1.0 0.0]
  'ps', [ 0.0  2.5 0.0], 15, 'rect'   , [0.5 1.0 0.0]
  'ps', [ 0.0  2.5 0.0], 15, 'kaiser' , [0.5 1.0 0.0]
  };

%% ===== Main ============================================================

for ii=1:size(scenarios)

    src = scenarios{ii,1};  % source type
    xs = scenarios{ii,2};  % source position / direction of plane wave
    Nce = scenarios{ii,3};  % modal order
    conf.modal_window = scenarios{ii,4};  % type of modal weighting function
    xq = scenarios{ii,5};  % expansion centre

    % Circular expansion coefficients
    switch src
    case 'pw'
        Pm = circexp_mono_pw(xs,Nce,f,xq,conf);
        g = 1;
    case 'ps'
        Pm = circexp_mono_ps(xs,Nce,f,xq,conf);
        g = 1./(4*pi*norm(xs-xq));
    end

    % Modal weighting of coefficients
    wm = modal_weighting(Nce, conf);
    Pm = bsxfun(@times, Pm, [wm(end:-1:2) wm]);

    % Conversion to plane wave decomposition
    Ppwd = pwd_mono_circexp(Pm, Npw);

    if modus
        % Sound field plot
        P = sound_field_mono(X,Y,Z,x0,'pw',Ppwd,f,conf);
        plot_sound_field(P.*g,X,Y,Z,[],conf);

        % Title string
        str = 'Plane wave decompostion of modally bandlimited';
        switch src
        case 'pw'
            str = sprintf('%s plane wave', str);
        case 'ps'
            str = sprintf('%s point source', str);
        end
        str = sprintf(['%s ([%1.1f %1.1f %1.1f]):\n%s-window (M=%d), ' ...
          'center of modal expansion at [%1.1f %1.1f %1.1f]'], ...
          str, xs, conf.modal_window, Nce, xq);
        title(str);
    end
end

status = true;


function [areaErrorMap,meanX,meanY] = mfmAreaErrorMap(unfoldMesh,nFaces,unfolded2D,errorList)
%
%  [areaErrorMap,meanX,meanY] = mfmAreaErrorMap(unfoldMesh,nFaces,errorList)
%
%Author: Wandell
%Purpose:
%   Compute the area distortions.  Extracted from ARW code.
%

Xcogs=unfolded2D(unfoldMesh.uniqueFaceIndexList(:),1);
Xcogs=reshape(Xcogs,nFaces,3);
meanX=mean(Xcogs,2);

Ycogs=unfolded2D(unfoldMesh.uniqueFaceIndexList(:),2);
Ycogs=reshape(Ycogs,nFaces,3);
meanY=mean(Ycogs,2);
areaErrorMap=flipud(makeMeshImage([meanY(:),meanX(:)],errorList,128));

return;


% eql_sps_os_test.m
% compare aspire and matlab versions of E-QPL-SPS-OS
% Copyright Apr 2000, Jeff Fessler, The University of Michigan

%
% generate data
%
if ~isvar('yi'), printm 'data'
	if has_aspire
		f.dir	= test_dir;
		f.wtf	= [f.dir 't,g.wtf'];
		f.wtr	= strrep(f.wtf, 'wtf', 'wtr');
		f.yi	= [f.dir 'yi.fld'];
		f.ci	= [f.dir 'ci.fld'];
		f.ri	= [f.dir 'ri.fld'];
		f.mask	= [f.dir 'mask.fld'];
	end
	em_test_setup
prompt
end

if ~isvar('Gb'), printm 'Gb'
	f.nblock = 5;
	Gb = Gblock(G, f.nblock);
prompt
end

if ~isvar('R'), printm 'R'
	f.nbrs = 8;
	f.l2b = 2;
	R = Robject(ig.mask, 'edge_type', 'leak', 'type_denom', 'aspire', ...
		'beta', 2^f.l2b);
prompt
end

%
% matlab iterations
%
if ~isvar('xmat'), printm 'matlab E-QPL-SPS-OS'
	f.niter = 9;
	f.pixmax = 6;
	curvs = {'pc', 'oc'};
	xinit = max(xfbp,0);
	for ic = 1:length(curvs)
		tmp = eql_sps_os(xinit(ig.mask), Gb, yi, ci, ri, R, ...
			f.niter, f.pixmax, curvs{ic});
		xmat{ic} = ig.embed(tmp);
	end
	im clf, im(xmat{1}, 'Matlab E-QPL-SPS-OS iterations')
prompt
end

if ~has_aspire, return, end

%
% aspire iterations
%
if ~isvar('xasp'), printm 'aspire E-QPL-SPS-OS'

	f.init	= [f.dir 'init.fld'];
	f.out	= [f.dir 'out.fld'];
	fld_write(f.init, xinit, 'check', 0)

	f.saver	= 'stack,1';
	f.fitype = ['2z@' f.wtr '@-'];

	for ic = 1:length(curvs)
		if exist(f.out, 'file'), delete(f.out), end
		f.alg = sprintf('ospsc,%s,%d,%d,1,0', ...
			curvs{ic}, f.nblock, sg.na);
		f.penal	= sprintf('%g,quad,%d,-', f.l2b, f.nbrs/4);
		f.method = sprintf('@%d@%s@%s', f.niter-1, f.alg, f.penal);
		f.com = sprintf(['i -chat 0 empl3 %s %s  %s %s 1 %s 1 %s -' ...
				' %s %s 0 1 %g 0 -'], ...
			f.out, f.init, f.yi, f.ci, f.ri, f.fitype, ...
			f.method, f.saver, f.pixmax);

		os_run(f.com)
		xasp{ic} = double(fld_read(f.out));
	end
end

if 1
	for ic = 1:length(xasp)
		t = vcorrcoef(xasp{ic}, xmat{ic});
		printf('corr. %g,%g', t, t-1)
		err = (xasp{ic} - xmat{ic}) / max(col(xmat{ic}));
		printf('Normalized error range %g %g', min(err(:)), max(err(:))),
	end

	im clf, im(221, xmat{1}, 'xhat matlab'), cbar
	im(222, xasp{1}, 'xhat aspire'), cbar
	im(223, (xasp{1}-xmat{1})/max(col(xmat{1})), 'aspire-matlab'), cbar

	t1 = eql_obj(xmat{1}, G, yi(:), ci(:), ri(:), R, ig.mask);
	t2 = eql_obj(xasp{1}, G, yi(:), ci(:), ri(:), R, ig.mask);

	if im
		subplot(224)
		plot(0:f.niter-1, t1-t1(1), '-o', 0:f.niter-1, t2-t1(1), '-x')
		xlabel iteration, ylabel '\Phi change', legend('mat', 'asp', 4)
		title(sprintf('E-QPL-SPS-OS, Nsubset=%d', f.nblock))
	end
end


% MHGIBBSTRANS
%
%  MCMC Metropolis-Hastings transition function that
%  utilizes the Gibbs sampling distribution for proposals.
%  A - The current joint assignment.  This should be
%      updated to be the next assignment
%  G - The network
%  F - List of all factors
%
% Copyright (C) Daphne Koller, Stanford University, 2012

function A = MHGibbsTrans(A, G, F)

% Draw proposed new state from Gibbs Transition distribution
A_prop = GibbsTrans(A, G, F);

% Compute acceptance probability
p_acceptance = 1.0;
lnum = LogProbOfJointAssignment(F,A_prop);
lden = LogProbOfJointAssignment(F,A);

p_acceptance = min(1,exp(lnum-lden));
% Accept or reject proposal
if rand() < p_acceptance
    A = A_prop;
end


function dists = compute( pairspath, descs, geom_noise, varargin )

% Copyright (C) 2016-2017 Karel Lenc
% 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).

opts.metric = 'L2';
[opts, ~] = vl_argparse(opts, varargin);

pairs = readtable(pairspath);

descsA = descs.getdesc(descs, pairs.s1, geom_noise, pairs.t1 + 1, pairs.idx1 + 1);
descsB = descs.getdesc(descs, pairs.s2, geom_noise, pairs.t2 + 1, pairs.idx2 + 1);

switch opts.metric
  case 'L1'
    dists = sum(abs(descA - descB), 1);
  case 'L2'
    dists = sum((descsA - descsB).^2, 1);
  otherwise
    error('Invalid metric.');
end

end


 function a = loss(comp,par,spr)

%============================================================================
% Loss object - for calculating loss functions
%============================================================================
% a=loss(lossType,param)
%
%   calculates the difference between the input X and the ouput Y depending
%   on the specified loss type (for further information see below).
%   The loss can be calculated in two ways. The first is to it by training,
%   the second is to call the function with a data object as first
%   parameter (loss(d,loss_type,param)). The results are stored in the Y
%   part of the data object.
%
%   Attributes (with defaults):
%       type='class_loss'  -- type of loss (class_loss,linear_loss...)
%       param=[]           -- used parameters (can also be empty)
%
%   Methods:
%       train,test,calc
%
%   LOSS              |   PARAMETERS & DESCRIPTION
%   -------------------------------------------------------------------
%   class_loss        -- zero/one loss, L(x,y)=1 if x=y, 0 otherwise
%   confusion_matrix  -- matrix of [true-pos, false-pos; false-neg, true-neg]
%   epsilon_loss      -- L(x,y)= |x-y|, if |x-y|>epsilon, 0 otherwise
%   linear_loss       -- 1-norm, L(x,y)=|x-y|
%   one_class_loss    -- for one-class, e.g novelty detection, etc.
%   quadratic_loss    -- 2-norm, L(x,y)=|x-y|_2^2
%   roc               -- receiver/operator characteristic
%   roc50             -- receiver/operator characteristic, first n fps
%   sensitivity       -- tp/(tp+fn)
%   specificity       -- tn/(fp+tn)
%   kernel            -- loss derived from kernel matrix (param)
%                     -- inner products in 'loss' space between examples.
%   alignment         -- L(x,y)= sum(sum( (x*x') .* (y*y'))) / normalization
%===================================================================================
% Reference :
% Author    :
% Link      :
%===================================================================================

  %% <<--calculate loss like function (no objects is created)------>>
  if nargin>0 & isa(comp,'algorithm')
    a.type='class_loss';
    if nargin>1
        a.type=par;
    end;

    a.param=[];
    if nargin==3,
        a.param=spr;
    end;

    algoTemp=algorithm(a.type);
    a= class(a,'loss',algoTemp);
    a=train(a,comp);
    return;
  end
  %% --------------------------------------------------------------------

   if nargin==0
     a.type='class_loss';
   else
     a.type=comp;
   end

   a.param=[];

   if nargin==2
     a.param=par;
   end;

   algoTemp=algorithm(a.type);
   a= class(a,'loss',algoTemp);


function [thresh,cntR,sumR,cntP,sumP,miHist] = affinityPRMI(pairs,segs,nthresh)
% function [thresh,cntR,sumR,cntP,sumP,miHist] = affinityPRMI(pairs,segs,nthresh)
%
% Calcualte precision/recall and mutual information data.
%
% INPUT
%	pairs		3xN array of (i,j,wij) triples.
%	segs		Array of segmentations.
%	[nthresh]	Number of points in PR curve.
%
% OUTPUT
%	thresh		Vector of threshold values.
%	cntR,sumR	Ratio gives recall.
%	cntP,sumP	Ratio gives precision.
%	miHist		Joint histogram of (sameseg,Wij) for MI.
%
% For pairs, the i,j indices are 1-based 1D indices into the
% segmentations.
%
% See also calcMI.
%
% David Martin <dmartin@eecs.berkeley.edu>
% January 2003

if nargin<3, nthresh = 100; end
nthresh = max(1,nthresh);

if size(pairs,1)~=3,
  error('pairs must be 3xN');
end
n = size(pairs,2);

if pairs(3,:)<0 | pairs(3,:)>1,
  error('illegal wij value in pairs(3,:); wij must be in [0,1]');
end

nsegs = length(segs);
if nsegs==0,
  error('segs is empty');
end

[height,width] = size(segs{1});
thresh = linspace(1/(nthresh+1),1-1/(nthresh+1),nthresh)';

% For the mutual information, we need the joint distribution of the
% same segment indicator (given by the segmentations) and Wij.  We
% will bin the Wij values, and so get a 2D histogram estimate of the
% joint.  This histogram is also sufficient information to compute
% precision and recall.
miHist = zeros(nthresh,2);
for index = 1:n,
  % groundtruth is same-segment iff all humans give same-segment
  i = pairs(1,index);
  j = pairs(2,index);
  same = 1;
  for s = 1:nsegs,
    same = same & (segs{s}(i)==segs{s}(j));
  end
  % bin the wij value
  wij = pairs(3,index);
  bin = 1+round(wij*(nthresh-1));
  % increment histogram
  miHist(bin,1+same) = miHist(bin,1+same) + 1;
end

% compute precision and recall
cumHist = flipud(cumsum(flipud(miHist)));
cntR = cumHist(:,2);
sumR = zeros(size(cntR))+cumHist(1,2);
cntP = cumHist(:,2);
sumP = sum(cumHist,2);


function [ y, m, d ] = nyt_to_ymd ( volume, issue )

%*****************************************************************************80
%
%% NYT_TO_YMD converts an NYT date to a YMD date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer VOLUME, ISSUE, the New York Times
%    volume and issue.
%
%    Output, integer Y, M, D, the year, month and day.
%
  jed = nyt_to_jed ( volume, issue );

  [ y, m, d, f ] = jed_to_ymdf_common ( jed );

  return
end


%DEMOMESHSHAPES Display various 3D demo shapes, together with surfacic mesh
%
%   output = demoImShapes(input)
%
%   Example
%   demoImShapes
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-06-29,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

%% initialisations

% generate cubic images
lx = 1:100;
ly = 1:100;
lz = 1:100;

% choose a center not aligned with the grid
center = [50+sqrt(2)-1 50+sqrt(3)-1 50+sqrt(5)-2];

angles = [...
    0 0; ...
    30 0; ...
    30 30];

% set of ellipsoid orientations, in format YAW PITCH ROLL
orientations = [...
     0   0   0; ...
    30   0   0; ...
    30  30   0; ...
    30  30  30];

%% Ball

% sphere is defined by center and radius
sphere = [center 40];

% generation of 3D image
img = discreteBall(lx, ly, lz, sphere);

% display image isosurface
f = figure; set(gca, 'fontsize', 14);
isosurface(img, .5);

% setup display
hold on; axis equal; l = light;
axis ([0 100 0 100 0 100]);
view([40 20]);
snapnow;

% add surfacic mesh
drawSphere(sphere);

% decorate
title('Sphere, radius=40');
legend('isosurface', 'shape mesh', 'location', 'northeast');

%% Ellipsoid

clf;

% iterate on orientations
for i = 1:4
    elli = [center 50 30 10 orientations(i, :)];

    % generation of 3D image
    img = discreteEllipsoid(lx, ly, lz, elli);

    % display image isosurface
    clf; set(gca, 'fontsize', 14);
    patch(isosurface(img, .5), 'FaceColor', 'g', 'LineStyle', 'none');

    % setup display
    hold on; axis equal; l = light;
    axis ([0 100 10 90  20 80]);
    view([40 20]);

    % decorate
    strTitle = sprintf('Ellipsoid, ori=[%d %d %d]', orientations(i, :));
    title(strTitle);

    snapnow;

    % add surfacic mesh
    drawEllipsoid(elli, 'FaceColor', 'r');

    % decorate
    title(strTitle);
    legend('isosurface', 'shape mesh', 'location', 'northeast');
    snapnow;
end

%% Cuboid

clf;

% iterate on orientations
for i = 1:4
    cubo = [center 90 40 10 orientations(i, :)];

    % generation of 3D image
    img = discreteCuboid(lx, ly, lz, cubo);

    % display image isosurface
    clf; set(gca, 'fontsize', 14);
    patch(isosurface(img, .5), 'FaceColor', 'g', 'LineStyle', 'none');

    % setup display
    hold on; axis equal; l = light;
    axis ([0 100 10 90  20 80]);
    view([40 20]);

    % decorate
    strTitle = sprintf('Cuboid, ori=[%d %d %d]', orientations(i, :));
    title(strTitle);

    snapnow;

    % add surfacic mesh
    drawCuboid(cubo, 'FaceColor', 'r');

    % decorate
    title(strTitle);
    legend('isosurface', 'shape mesh', 'location', 'northeast');
    snapnow;
end

%% Cube

% iterate on orientations
for i = 1:4

    % cylinder representation
    cube = [center 60 orientations(i, :)];

    % generation of 3D image
    img = discreteCube(lx, ly, lz, cube);

    % display image isosurface
    clf; set(gca, 'fontsize', 14);
    patch(isosurface(img, .5), 'FaceColor', 'g', 'LineStyle', 'none');

    % setup display
    hold on; axis equal; l = light;
    axis ([0 100 0 100 0 100]);
    view([40 20]);

    % decorate
    strTitle = sprintf('Cube, ori=[%d %d %d]', orientations(i, :));
    title(strTitle);

    snapnow;

    % add surfacic mesh
    drawCube(cube, 'FaceColor', 'r');

    % decorate
    title(strTitle);
    legend('isosurface', 'shape mesh', 'location', 'northeast');
    snapnow;
end

%% Torus

% iterate on orientations
for i = 1:3
    torus = [center 30 10 angles(i, :)];

    % generation of 3D image
    img = discreteTorus(lx, ly, lz, torus);

    % display image isosurface
    clf; set(gca, 'fontsize', 14);
    patch(isosurface(img, .5), 'FaceColor', 'g', 'LineStyle', 'none');

    % setup display
    hold on; axis equal; l = light;
    axis ([0 100 10 90  20 80]);
    view([40 20]);

    % decorate
    strTitle = sprintf('Torus, ori=[%d %d]', angles(i, :));
    title(strTitle);

    snapnow;

    % add surfacic mesh
    drawTorus(torus, 'FaceColor', 'r', 'LineStyle', 'none');

    % decorate
    title(strTitle);
    legend('isosurface', 'shape mesh', 'location', 'northeast');
    snapnow;
end

%% Cylinder

clf;

% iterate on orientations
for i = 1:3

    % cylinder representation
    cart = sph2cart2d(angles(i, :));
    p1 = center - 30*cart;
    p2 = center + 30*cart;
    cyl = [p1 p2 10];

    % generation of 3D image
    img = discreteCylinder(lx, ly, lz, cyl);

    % display image isosurface
    clf; set(gca, 'fontsize', 14);
    patch(isosurface(img, .5), 'FaceColor', 'g', 'LineStyle', 'none');

    % setup display
    hold on; axis equal; l = light;
    axis ([20 80 20 80 0 100]);
    view([40 20]);

    % decorate
    strTitle = sprintf('Cylinder, ori=[%d %d]', angles(i, :));
    title(strTitle);

    snapnow;

    % add surfacic mesh
    drawCylinder(cyl, 'FaceColor', 'r', 'LineStyle', 'none');

    % decorate
    title(strTitle);
    legend('isosurface', 'shape mesh', 'location', 'northeast');
    snapnow;
end


function newelem=meshreorient(node,elem)
%
% newelem=meshreorient(node,elem)
%
% reorder nodes in a surface or tetrahedral mesh to ensure all
% elements are oriented consistently
%
% author: Qianqian Fang, <q.fang at neu.edu>
% date: 2010/05/05
%
% input:
%    node: list of nodes
%    elem: list of elements (each row are indices of nodes of each element)
%
% output:
%    newelem: the element list with consistent ordering
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

% calculate the canonical volume of the element (can be a 2D or 3D)
vol=elemvolume(node,elem,'signed');

% make sure all elements are positive in volume
idx=find(vol<0);
elem(idx,[end-1,end])=elem(idx,[end,end-1]);
newelem=elem;


function exercise4(varargin)
% EXERCISE4   Part 4 of the VGG CNN practical

setup ;

% -------------------------------------------------------------------------
% Part 4.1: prepare the data
% -------------------------------------------------------------------------

% Load character dataset
imdb = load('data/charsdb.mat') ;

% Visualize some of the data
figure(10) ; clf ; colormap gray ;
subplot(1,2,1) ;
vl_imarraysc(imdb.images.data(:,:,imdb.images.label==1 & imdb.images.set==1)) ;
axis image off ;
title('training chars for ''a''') ;

subplot(1,2,2) ;
vl_imarraysc(imdb.images.data(:,:,imdb.images.label==1 & imdb.images.set==2)) ;
axis image off ;
title('validation chars for ''a''') ;

% -------------------------------------------------------------------------
% Part 4.2: initialize a CNN architecture
% -------------------------------------------------------------------------

net = initializeCharacterCNN() ;

% -------------------------------------------------------------------------
% Part 4.3: train and evaluate the CNN
% -------------------------------------------------------------------------

trainOpts.batchSize = 100 ;
trainOpts.numEpochs = 15 ;
trainOpts.continue = true ;
trainOpts.gpus = [] ;
trainOpts.learningRate = 0.001 ;
trainOpts.expDir = 'data/chars-experiment' ;
trainOpts = vl_argparse(trainOpts, varargin);

% Take the average image out
imdb = load('data/charsdb.mat') ;
imageMean = mean(imdb.images.data(:)) ;
imdb.images.data = imdb.images.data - imageMean ;

% Convert to a GPU array if needed
if numel(trainOpts.gpus) > 0
  imdb.images.data = gpuArray(imdb.images.data) ;
end

% Call training function in MatConvNet
[net,info] = cnn_train(net, imdb, @getBatch, trainOpts) ;

% Move the CNN back to the CPU if it was trained on the GPU
if numel(trainOpts.gpus) > 0
  net = vl_simplenn_move(net, 'cpu') ;
end

% Save the result for later use
net.layers(end) = [] ;
net.imageMean = imageMean ;
save('data/chars-experiment/charscnn.mat', '-struct', 'net') ;

% -------------------------------------------------------------------------
% Part 4.4: visualize the learned filters
% -------------------------------------------------------------------------

figure(2) ; clf ; colormap gray ;
vl_imarraysc(squeeze(net.layers{1}.weights{1}),'spacing',2)
axis equal ; title('filters in the first layer') ;

% -------------------------------------------------------------------------
% Part 4.5: apply the model
% -------------------------------------------------------------------------

% Load the CNN learned before
net = load('data/chars-experiment/charscnn.mat') ;
%net = load('data/chars-experiment/charscnn-jit.mat') ;

% Load the sentence
[im,cmap] = imread('data/sentence-lato.png') ;
if isempty(cmap)
  im = im2single(im) ;
else
  im = im2single(ind2gray(p,cmap)) ;
end
im = 256 * (im - net.imageMean) ;

% Apply the CNN to the larger image
res = vl_simplenn(net, im) ;

% Visualize the results
figure(3) ; clf ;
decodeCharacters(net, imdb, im, res) ;

% -------------------------------------------------------------------------
% Part 4.6: train with jitter
% -------------------------------------------------------------------------

trainOpts.batchSize = 100 ;
trainOpts.numEpochs = 15 ;
trainOpts.continue = true ;
trainOpts.learningRate = 0.001 ;
trainOpts.expDir = 'data/chars-jit-experiment' ;

% Initlialize a new network
net = initializeCharacterCNN() ;

% Call training function in MatConvNet
[net,info] = cnn_train(net, imdb, @getBatchWithJitter, trainOpts) ;

% Move the CNN back to CPU if it was trained on GPU
if numel(trainOpts.gpus) > 0
  net = vl_simplenn_move(net, 'cpu') ;
end

% Save the result for later use
net.layers(end) = [] ;
net.imageMean = imageMean ;
save('data/chars-experiment/charscnn-jit.mat', '-struct', 'net') ;

% Visualize the results on the sentence
figure(4) ; clf ;
decodeCharacters(net, imdb, im, vl_simplenn(net, im)) ;

% --------------------------------------------------------------------
function [im, labels] = getBatch(imdb, batch)
% --------------------------------------------------------------------
im = imdb.images.data(:,:,batch) ;
im = 256 * reshape(im, 32, 32, 1, []) ;
labels = imdb.images.label(1,batch) ;

% --------------------------------------------------------------------
function [im, labels] = getBatchWithJitter(imdb, batch)
% --------------------------------------------------------------------
im = imdb.images.data(:,:,batch) ;
labels = imdb.images.label(1,batch) ;

n = numel(batch) ;
train = find(imdb.images.set == 1) ;

sel = randperm(numel(train), n) ;
im1 = imdb.images.data(:,:,sel) ;

sel = randperm(numel(train), n) ;
im2 = imdb.images.data(:,:,sel) ;

ctx = [im1 im2] ;
ctx(:,17:48,:) = min(ctx(:,17:48,:), im) ;

dx = randi(11) - 6 ;
im = ctx(:,(17:48)+dx,:) ;
sx = (17:48) + dx ;

dy = randi(5) - 2 ;
sy = max(1, min(32, (1:32) + dy)) ;

im = ctx(sy,sx,:) ;

% Visualize the batch:
% figure(100) ; clf ;
% vl_imarraysc(im) ;

im = 256 * reshape(im, 32, 32, 1, []) ;


function linpack_s_test27 ( )

%*****************************************************************************80
%
%% TEST27 tests SSPFA and SSPSL.
%
%  Discussion:
%
%    SSPFA and SSPSL are for packed symmetric indefinite matrices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 100;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST27\n' );
  fprintf ( 1, '  For a symmetric indefinite packed matrix,\n' );
  fprintf ( 1, '  SSPFA factors the matrix,\n' );
  fprintf ( 1, '  SSPSL solves a factored linear system.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Assign values to the matrix A and the right hand side B.
%
  b(1:n-1) = 0.0;
  b(n)= n + 1;

  k = 0;
  for j = 1 : n
    for i = 1 : j
      k = k + 1;
      if ( i == j )
        a(k) = 2.0;
      elseif ( j == i+1 )
        a(k) = -1.0;
      else
        a(k) = 0.0;
      end
    end
  end
%
%  Factor the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix.\n' );

  [ a, ipvt, info ] = sspfa ( a, n );

  if ( info ~= 0 )
    fprintf ( 1, '  Error!  SSPFA returns INFO = %d\n', info );
    return
  end
%
%  Solve the linear system.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  b = sspsl ( a, n, ipvt, b );
%
%  Print the result.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first and last five entries of the solution:\n' );
  fprintf ( 1, '  (Should be 1,2,3,4,5,...,n-1,n):\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    if ( i <= 5 | n-5 < i )
      fprintf ( 1, '  %6d  %14f\n', i, b(i) );
    end
    if ( i == 5 )
      fprintf ( 1, '  ......  ..............\n' );
    end
  end

  return
end


function linpack_z_test35 ( )

%*****************************************************************************80
%
%% TEST35 tests ZTRCO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST35\n' );
  fprintf ( 1, '  For a double precision complex (C)\n' );
  fprintf ( 1, '  triangular matrix (TR),\n' );
  fprintf ( 1, '  ZTRCO estimates the condition.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix order is N = %d\n', n );
%
%  Set the matrix.
%
  seed = 123456789;

  for i = 1 : n
    for j = 1 : i
      [ a(i,j), seed ] = c8_uniform_01 ( seed );
    end
    a(i,i+1:n) = 0.0;
  end
%
%  Get the condition of the lower triangular matrix.
%
  job = 0;
  rcond = ztrco ( a, lda, n, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimated reciprocal condition RCOND = %f\n', rcond );

  return
end


function [params, names] = rbfExtractParam(model)

% RBFEXTRACTPARAM Wrapper for NETLAB's rbfpak.
% FORMAT
% DESC returns a vector of all the weights and biases from a
% RBF network model. For single hidden layer models the
% function is a wrapper for the rbfpak command.
% ARG model : the model from which we wish to extract the weights
% and biases.
% RETURN params : vector of all the weights and biases returned by
% the model. The structure is governed by rbfpak.
% RETURN names : optional additional returned cell array of the
% names of the parameters.
%
% SEEALSO : rbfpak, rbfCreate, rbfExpandParam, modelExtractParam
%
% COPYRIGHT : Neil D. Lawrence, 2006, 2007, 2008

% MLTOOLS

params = rbfpak(model);
if nargout > 1
  counter = 0;
  for j = 1:size(model.c, 2)
    for i = 1:size(model.c, 1)
      counter = counter + 1;
      names{counter} = ['Input centre ' num2str(i) '-' num2str(j)];
    end
  end
  for j = 1:size(model.wi, 2)
    counter = counter + 1;
    names{counter} = ['Hidden node width ' num2str(j)];
  end
  for j = 1:size(model.w2, 2)
    for i = 1:size(model.w2, 1)
      counter = counter + 1;
      names{counter} = ['Output weight ' num2str(i) '-' num2str(j)];
    end
  end
  for j = 1:size(model.b2, 2)
    counter = counter + 1;
    names{counter} = ['Output node bias ' num2str(j)];
  end
end


function calpak_test014 ( )

%*****************************************************************************80
%
%% CALPAK_TEST014 tests JED_TO_YMDF_BAHAI and YMDF_TO_JED_BAHAI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 March 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST014\n' );
  fprintf ( 1, '  For the Bahai calendar:\n' );
  fprintf ( 1, '  JED_TO_YMDF_BAHAI: JED -> YMDF.\n' );
  fprintf ( 1, '  YMDF_TO_JED_BAHAI: YMDF -> JED.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  JED (in)    YMDF	       JED (out)\n' );
  fprintf ( 1, '\n' );

  jed_epoch = epoch_to_jed_bahai ( );

  i = 0;

  while ( 1 )

    i = i + 1;
    jed1 = jed_test ( i );

    if ( jed1 < 0.0 )
      break
    end

    if ( jed_epoch <= jed1 )

      [ y2, m2, d2, f2 ] = jed_to_ymdf_bahai ( jed1 );

      s2 = ymdf_to_s_numeric ( y2, m2, d2, f2 );

      jed3 = ymdf_to_jed_bahai ( y2, m2, d2, f2 );

      fprintf ( 1, '  %11.2f  %20s  %11.2f\n', jed1, s2, jed3 );

    end

  end

  return
end


function [A, b, x, ProbInfo] = PRtomo(varargin)
% IRtomo Generates data for X-ray tomographic reconstruction problems
%
% [A, b, x, ProbInfo] = PRtomo
% [A, b, x, ProbInfo] = PRtomo(n)
% [A, b, x, ProbInfo] = PRtomo(n, options)
% [A, b, x, ProbInfo] = PRtomo(options)
%
% This function uses the "line model" to create a 2D X-ray tomography test
% problem with an N-times-N pixel domain, using p rays for each angle in
% the vector theta.
%
% The severity of the problem is almost unaffected by the problem parameters.
%
% Input:
%  n : size of the image; a scalar such that the size is n x n.
%      Default: n = 256.
%  options : Structure containing the following optional fields:
%    phantomImage - user supplied test image of size n x n, of type numeric,
%             2-D only, or character string indicating
%             'shepplogan' : Shepp-Logan phantom
%             'smooth' : a smooth image
%             'binary' : a binary image
%             'threephases' : random image with pixel values 0, 0.5, 1
%                arranged in domains
%             'threephasessmooth' : similar to threephases, but the
%                domains have smoothly varying pixel values and there is
%                a smooth background
%             'fourphases' : similar to 'binary' but with three phases
%                separated by (thin) structures that form the fourth phase
%             'grains' : a random image with Voronoi cells
%             'ppower' : a random image with patterns of nonzero pixels
%             Default: 'shepplogan'.
%             This image is then stored in the output vector x.
%    CTtype - string that defines the type of CT problem:
%             'parallel'  : parallel beam geometry (default),
%             'fancurved' : fan beam with curved detector.
%    sm     - logical; if true (default )then A is a sparse matrix,
%             otherwise it is a function handle.
%    angles - vector of projection angles, in degrees.
%             Default: 0:1:179 for parallel beam, 0:2:358 for fan beam.
%    p      - number of rays for each source angle.
%             Default: p = round(sqrt(2)*n).
%    d      - Parallel beam only: scalar denoting the distance from the
%             first ray to the last; default: d = p-1.
%    R      - Fan beam only: the distance from the source to the center
%             of the phantom is R*N.  Default: R = 2.
%    span   - Fan beam only: scalar that determines the angular span of
%             the rays, in degrees. The default value is defined such
%             that from the source at (0,R*N) the first and last rays
%             hit the image corners (-n/2,n/2) and (n/2,n/2).
%
% Output:
%  A : Sparse matrix or function handle for forward/adjoint problem.
%  b : Vector with projection data (the sinogram).
%  x : Vector with image (i.e., exact image with stacked columns).
%  ProbInfo : structure containing some information about problem
%      problemType - kind of test problem (in this case: 'tomography')
%      xType       - solution type (in this case 'image2D')
%      bType       - data type (in this case 'image2D')
%      xSize       - size of image x
%      bSize       - size of sinogram b
%
% See also: PRblur, PRdiffusion, PRinvinterp2, PRnmr, PRseismic,
% PRspherical, PRtomo, PRnoise, PRshowb, PRshowx, radon

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% This file is part of the IR Tools package and is distributed under the
% 3-Clause BSD Licence. A separate license file should be provided as part
% of the package.

% Initialization: set the default image size.
default_N = 256;

% Check for acceptable number of optional input arguments.
switch length(varargin)
    case 0
        N = default_N; options = [];
    case 1
        if isa(varargin{1}, 'double')
            N = varargin{1}; options = [];
        else
            N = default_N; options = varargin{1};
        end
    case 2
        if isa(varargin{1}, 'double')
            N = varargin{1}; options = varargin{2};
        else
            N = varargin{2}; options = varargin{1};
        end
    otherwise
        error('Too many input parameters')
end

% Set default values for options.
defaultopt = struct('phantomImage', 'shepplogan', 'CTtype', 'parallel', ...
    'sm', true, 'angles', 0:1:179, 'p', NaN, 'R', 2, 'd', NaN, 'span', NaN);

% If input is 'defaults,' return the default options in A.
if nargin == 1 && nargout <= 1 && strcmp(varargin,'defaults')
    A = defaultopt;
    return;
end

if isempty(options)
    options = defaultopt;
end

options = PRset(defaultopt, options);

phantom    = PRget(options, 'phantomImage', [], 'fast');
CTtype     = PRget(options, 'CTtype',       [], 'fast');
angles     = PRget(options, 'angles',       [], 'fast');
sm         = PRget(options, 'sm',           [], 'fast');
p          = PRget(options, 'p',            [], 'fast');
R          = PRget(options, 'R',            [], 'fast');
d          = PRget(options, 'd',            [], 'fast');
span       = PRget(options, 'span',         [], 'fast');

% If a phantom image is given, then this defines N and the other parametes.
if isnumeric(phantom)
    % Make sure user input image is a matrix
    if ~ismatrix(phantom)
        error('Expected user supplied phantom image to be a 2-D array of type numeric')
    else
        N = size(phantom,1);
        if size(phantom,2)~=N, error('phantomImage image must be square'); end
        x = double(phantom(:));
    end
else
    x = phantomgallery(phantom,N);
    x = x(:);
end
if isnan(p), p = round(sqrt(2)*N); end

switch CTtype
    case 'parallel'
        if isnan(d), d = p-1; end
        A = paralleltomo(N,angles,p,d,0,sm);
    case 'fancurved'
        if isnan(span), span = 2*atand(1/(2*R-1)); end
        A = fancurvedtomo(N,angles,p,R,span,0,sm);
   otherwise
        error('Type of CT problem not provided')
end
if sm
    b = A*x;
else
    b = A(x,'notransp');
end

% Providing information about the test problem.
ProbInfo.problemType = 'tomography';
ProbInfo.xType = 'image2D';
ProbInfo.bType = 'image2D';
ProbInfo.xSize = [N,N];
ProbInfo.bSize = [p,length(angles)];


function pass = test_points(pref)

% Get preferences:
if ( nargin < 1 )
    pref = chebfunpref();
end

n = 10;
tol = 10*eps;
r = rand(n,1);
dom = [0, 1];

for k = 1:2
    f = chebfun(r, dom, 'chebkind', k);
    pass(k) = norm(f.points - chebpts(n, dom, k)) < tol;
end


function [F,V,regionInd]=multiRegionTriMesh2D(regionSpec,pointSpacing,resampleCurveOpt,plotOn)

% function [F,V,regionInd]=multiRegionTriMesh2D(regionSpec,pointSpacing,plotOn)
% ------------------------------------------------------------------------
% This function creates a 2D triangulation for each of the regions
% specified in the variable regionSpec. The mesh aims to obtain a point
% spacing as defined by the input pointSpacing. The triangulation is based
% on a 2D constrained Delaunary triangulation. The constraints are formed
% by the boundary curves inside the regionSpec cell. Large areas, with
% respect to the pointSpacing, will contain a near homogeneous and
% aproximately equilateral triangulations. Other regions (e.g. at
% boundaries and thin/complex shapes) will contain other triangulation
% types.
% The function output contains the triangular faces in F, the vertices in V
% and the per triangle region indices in regionInd. By setting plotOn to 0
% or 1 plotting can be switched on or off.
%
% More on the specification of regions:
% Regions are defined as cell entries in the input variable regionSpec for
% instnace region 1 is found in regionSpec{1}. Each region entry is itself
% also a cell array containing all the boundary curves, e.g. for a two
% curve region 1 we would have something like regionSpec{1}={V1,V2} where
% V1 and V2 are the boundary curves. Multiple curves may be given here. The
% first curve should form the outer boundary of the entire region, the
% curves that follow should define holes inside this boundary and the
% space inside them is therefore not meshed. The boundary vertices for
% regions that share boundaries are merged and will share these boundary
% vertices.
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 2013/14/08
%------------------------------------------------------------------------

%% PLOT SETTINGS
if plotOn==1
    fontSize=20;
    hf1=cFigure;
    title('Smoothened triangulated mesh','FontSize',fontSize);
    xlabel('X','FontSize',fontSize);ylabel('Y','FontSize',fontSize);zlabel('Z','FontSize',fontSize);
    hold on;
end

%% MESHING REGIONS

%The total vertex, face and color (=region number) matrices
V=[]; F=[]; regionInd=[];
for qRegion=1:1:numel(regionSpec)

    %Define region cell
    regionCell=regionSpec{qRegion};

    %Meshing region
    [Fs,Vs]=regionTriMesh2D(regionCell,pointSpacing,resampleCurveOpt,0);

    %Joining regions
    F=[F;Fs+size(V,1)]; %Add new faces and fix vertex indices
    V=[V;Vs]; %Add points
    regionInd=[regionInd; qRegion*ones(size(Fs,1),1)]; %Create region index for faces
end

%% PLOTTING
if plotOn==1
    figure(hf1);
    gpatch(F,V,regionInd);
    colormap(gjet(numel(regionSpec))); icolorbar;
    axisGeom(gca,fontSize);
    drawnow;
end

%% REMOVING DOUBLE POINTS
%Removing double points (region curve points may appear multiple times)
[F,V]=mergeVertices(F,V);

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


%SF_LINE_H3 Third order 1D C1 Hermite shape functions for lines.
%
%   [ VBASE, NLDOF, XLDOF, SFUN ] = SF_LINE_H3( I_EVAL, N_SDIM, N_VERT, I_DOF, XI, AINVJAC, VBASE )
%   Evaluates C1 Hermite shape functions on 1D line elements with value and
%   first derivatives defined in the nodes. XI are Barycentric coordinates.
%
%       Input       Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       i_eval      scalar:  1             Evaluate function values
%                           >1             Evaluate values of derivatives
%       n_sdim      scalar: 1              Number of space dimensions
%       n_vert      scalar: 2              Number of vertices per cell
%       i_dof       scalar: 1-4            Local basis function to evaluate
%       xi          array  [2,1]           Local coordinates of evaluation point
%       aInvJac     [n,3]                  Inverse of transformation Jacobian
%       vBase       [n]                    Preallocated output vector
%                                                                                .
%       Output      Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       vBase       [n]                    Evaluated function values
%       nLDof       [2,4]                  Number of local degrees of freedom on
%                                          vertices, edges, faces, cell interiors,
%                                          and vertices without boundary conditions
%       xLDof       [2,n_ldof]             Local coordinates of local dofs
%       sfun        string                 Function name of called shape function
%
%   See also SF_LINE_P3

% Copyright 2013-2022 Precise Simulation, Ltd.


function [P, Transf] = cs_convert(sMri, src, dest, P, isNanAllowed)
% CS_CONVERT: Convert 3D points between coordinates systems.
%
% USAGE:       P = cs_convert(sMri, src, dest, P, isNanAllowed=0)
%         Transf = cs_convert(sMri, src, dest)
%
% INPUT:
%     - sMri         : Brainstorm MRI structure
%     - src          : Current coordinate system {'voxel','mri','scs','mni','acpc','world','captrak'}
%     - dest         : Target coordinate system {'voxel','mri','scs','mni','acpc','world','captrak'}
%     - P            : a Nx3 matrix of point coordinates to convert
%     - isNanAllowed : If 0, throw an error and if the conversion of some points P leads to NaN,
%                      and if there is no linear MNI registration to be used instead
%                      (case of non-linear MNI registration with points outside of the defined area)
%
% DESCRIPTION:   https://neuroimage.usc.edu/brainstorm/CoordinateSystems
%     - voxel : X=left>right,  Y=posterior>anterior,   Z=bottom>top
%               Coordinate of the center of the first voxel at the bottom-left-posterior of the MRI volume: (1,1,1)
%     - mri   : Same as 'voxel' but in millimeters instead of voxels:  mriXYZ = voxelXYZ * Voxsize
%     - scs   : Based on: Nasion, left pre-auricular point (LPA), and right pre-auricular point (RPA).
%               Origin: Midway on the line joining LPA and RPA
%               Axis X: From the origin towards the Nasion (exactly through)
%               Axis Y: From the origin towards LPA in the plane defined by (NAS,LPA,RPA), and orthogonal to X axis
%               Axiz Z: From the origin towards the top of the head
%     - mni   : MNI coordinates based on SPM affine or non-linear registration
%     - acpc  : Based on: Anterior commissure (AC), Posterior commissure (PC) and an interhemisperic point (IH)
%               Origin: AC
%               Axis X: From the origin towards the right
%               Axis Y: Negative y-axis is passing from AC through PC
%               Axis Z: Passing through a mid-hemispheric point in the superior direction
%     - world : Transformation available in the initial file loaded as the default MRI (vox2ras/qform/world transformation)
%     - captrak: RAS orientation and the origin approximately between LPA and RPA
%               Axis X: From LPA through RPA exactly
%               Axis Y: Orthogonal to the X-axis through the nasion (NAS)
%               Axis Z: Orthogonal to the XY-plane through the vertex of the head

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2008-2022

% Throw errors by default if NaN
if (nargin < 5) || isempty(isNanAllowed)
    isNanAllowed = 0;
end
% Check matrices orientation
if (nargin < 4) || isempty(P)
    P = [];
elseif (size(P,2) ~= 3) && (size(P,1) == 3)
    P = P';
elseif (size(P,2) ~= 3)
    error('P must have 3 columns (X,Y,Z).');
end
% If the coordinate system didn't change
if strcmpi(src, dest)
    return;
end
% Keep track of points that cannot be transformed
iMissing = [];
isApplied = 0;
Porig = P;
% Transform to homogeneous coordinates
if ~isempty(P)
    P = [P'; ones(1,size(P,1))];
end

% ===== GET MRI=>WORLD TRANSFORMATION =====
if strcmpi(src, 'world') || strcmpi(dest, 'world') || (strcmpi(src, 'mni') && isfield(sMri,'NCS') && isfield(sMri.NCS,'y') && ~isempty(sMri.NCS.y))
    % Get the vox2ras transformation
    if isempty(sMri) || isempty(sMri.InitTransf)
        P = [];
        return;
    end
    iTransf = find(strcmpi(sMri.InitTransf(:,1), 'vox2ras'));
    if isempty(iTransf)
        P = [];
        return;
    end
    vox2ras = sMri.InitTransf{iTransf,2};
    % 2nd operation: Change reference from (0,0,0) to (1,1,1)
    vox2ras = vox2ras * [1 0 0 -1; 0 1 0 -1; 0 0 1 -1; 0 0 0 1];
    % 1st operation: Convert from MRI(mm) to voxels
    vox2ras = vox2ras * diag(1 ./ [sMri.Voxsize, 1]);
    % Convert to meters: transformation MRI=>WORLD (in meters)
    mri2world = vox2ras .* [ones(3,3), 1e-3.*ones(3,1); ones(1,4)];
    % Compute inverse transformation WORLD=>MRI (in meters)
    world2mri = inv(mri2world);
end

% ===== COMPUTE ACPC TRANSFORMATION =====
if strcmpi(src, 'acpc') || strcmpi(dest, 'acpc')
    tACPC = cs_compute(sMri, 'acpc');
    if isempty(tACPC) || isempty(tACPC.R)
        P = [];
        return;
    end
    scs2acpc = [tACPC.R, tACPC.T; 0 0 0 1];
end

% ===== COMPUTE CAPTRAK TRANSFORMATION =====
if strcmpi(src, 'captrak') || strcmpi(dest, 'captrak')
    tCapTrak = cs_compute(sMri, 'captrak');
    if isempty(tCapTrak) || isempty(tCapTrak.R)
        P = [];
        return;
    end
    scs2captrak = [tCapTrak.R, tCapTrak.T; 0 0 0 1];
end

% ===== CONVERT SRC => MRI =====
% Evaluate the transformation to apply
switch lower(src)
    case 'voxel'
        RT1 = diag([sMri.Voxsize(:) ./ 1000; 1]);
    case 'mri'
        RT1 = eye(4);
    case 'scs'
        if ~isfield(sMri,'SCS') || ~isfield(sMri.SCS,'R') || isempty(sMri.SCS.R) || ~isfield(sMri.SCS,'T') || isempty(sMri.SCS.T)
            P = [];
            return;
        end
        RT1 = inv([sMri.SCS.R, sMri.SCS.T./1000; 0 0 0 1]);
    case 'mni'
        % Transformation of each point by indirection in the deformation field y
        if ~isempty(P) && isfield(sMri,'NCS') && isfield(sMri.NCS,'y') && ~isempty(sMri.NCS.y)
            % Convert MNI => voxel space of the registration matrix
            P_reg = inv(sMri.NCS.y_vox2ras) * (P .* [1000;1000;1000;1]);
            % Convert from 0-based to 1-based??
            % => This solution was obtained empirically by minimizing:
            %    sqrt(sum((cs_convert(sMri, 'mri', 'mni', cs_convert(sMri, 'mni', 'mri', P)) - P).^2)).*1000 => around 0.003 with this adjustment
            P_reg = P_reg + [1;1;1;0];
            % Convert Voxel => World
            P_world = [...
                interp3(sMri.NCS.y(:,:,:,1), P_reg(2,:), P_reg(1,:), P_reg(3,:), 'linear', NaN); ...
                interp3(sMri.NCS.y(:,:,:,2), P_reg(2,:), P_reg(1,:), P_reg(3,:), 'linear', NaN); ...
                interp3(sMri.NCS.y(:,:,:,3), P_reg(2,:), P_reg(1,:), P_reg(3,:), 'linear', NaN)] ./ 1000;
            % Check if some points could not be converted
            if ~isNanAllowed
                iMissing = find(any(isnan(P_world),1));
            end
            % Convert World => MRI
            P = world2mri * [double(P_world); ones(1, size(P_world,2))];
            RT1 = eye(4);
        elseif isfield(sMri,'NCS') && isfield(sMri.NCS,'R') && ~isempty(sMri.NCS.R) && isfield(sMri.NCS,'T') && ~isempty(sMri.NCS.T)
            RT1 = inv([sMri.NCS.R, sMri.NCS.T./1000; 0 0 0 1]);
        else
            P = [];
            return;
        end
    case 'acpc'
        % ACPC => SCS => MRI
        RT1 = inv(scs2acpc);
    case 'captrak'
        % CapTrak => SCS => MRI
        RT1 = inv(scs2captrak);
    case 'world'
        RT1 = world2mri;
    otherwise
        error(['Invalid coordinate system: ' src]);
end

% ===== CONVERT MRI => DEST =====
% Evaluate the transformation to apply
switch lower(dest)
    case 'voxel'
        RT2 = diag([1000 ./ sMri.Voxsize(:); 1]);
    case 'mri'
        RT2 = eye(4);
    case 'scs'
        if ~isfield(sMri,'SCS') || ~isfield(sMri.SCS,'R') || isempty(sMri.SCS.R) || ~isfield(sMri.SCS,'T') || isempty(sMri.SCS.T)
            P = [];
            return;
        end
        RT2 = [sMri.SCS.R, sMri.SCS.T./1000; 0 0 0 1];
    case 'mni'
        % Using non-linear MNI normalization: Transformation of each point by indirection in the deformation field iy
        if ~isempty(P) && isfield(sMri,'NCS') && isfield(sMri.NCS,'iy') && ~isempty(sMri.NCS.iy)
            % Convert: src => MRI => voxel
            P_vox = diag([1000 ./ sMri.Voxsize(:); 1]) * RT1 * P;
            % Get values from the iy volumes
            Pmni = [interp3(sMri.NCS.iy(:,:,:,1), P_vox(2,:), P_vox(1,:), P_vox(3,:), 'linear', NaN); ...
                 interp3(sMri.NCS.iy(:,:,:,2), P_vox(2,:), P_vox(1,:), P_vox(3,:), 'linear', NaN); ...
                 interp3(sMri.NCS.iy(:,:,:,3), P_vox(2,:), P_vox(1,:), P_vox(3,:), 'linear', NaN)] ./ 1000;
            % Check if some points could not be converted
            if ~isNanAllowed
                iMissing = find(any(isnan(Pmni),1));
            end
            % Transpose the matrix back
            P = double(Pmni');
            Transf = [];
            isApplied = 1;
        elseif isfield(sMri,'NCS') && isfield(sMri.NCS,'R') && ~isempty(sMri.NCS.R) && isfield(sMri.NCS,'T') && ~isempty(sMri.NCS.T)
            RT2 = [sMri.NCS.R, sMri.NCS.T./1000; 0 0 0 1];
        else
            P = [];
            return;
        end
    case 'acpc'
        % MRI => SCS => ACPC
        RT2 = scs2acpc;
    case 'captrak'
        % MRI => SCS => CapTrak
        RT2 = scs2captrak;
    case 'world'
        RT2 = mri2world;
    otherwise
        error(['Invalid coordinate system: ' dest]);
end

% If the final transformation is not already applied
if ~isApplied
    % Compute the final transformation matrix
    Transf = RT2 * RT1;
    % Apply the transformation matrix to the points
    if ~isempty(P)
        % Apply rotation-translation
        P = Transf * P;
        % Remove the last coordinate and transpose the matrix back
        P = P(1:3,:)';
    else
        P = Transf;
    end
end

% If there are NaN points from non-linear MNI: Try to fix them with linear MNI
if ~isempty(iMissing)
    % If there is a linear transformation available as well, use it for missing points (same code as below)
    if isfield(sMri,'NCS') && isfield(sMri.NCS,'R') && ~isempty(sMri.NCS.R) && isfield(sMri.NCS,'T') && ~isempty(sMri.NCS.T)
        % Remove the non-linear transformation, so that it forces using the linear one
        sMri.NCS.iy = [];
        sMri.NCS.y = [];
        P(iMissing,:) = cs_convert(sMri, src, dest, Porig(iMissing,:), isNanAllowed);
    % If NaN not allowed and no linear transformation: check for missing values
    elseif ~isNanAllowed
        error(['Some points are outside the definition of the non-linear MNI registration.' 10 'Please compute a linear MNI normalization to convert these points.']);
    end
end