%EIMM_DEMO2 Bearings Only Tracking of a Manouvering Target demonstration
%
% Simple demonstration for non-linear IMM using the following models:
%  1. Standard Wiener process velocity model
%  2. Coordinated turn model
%
% The measurement model is non-linear bearings only model.
% See BOT-demo or the documentation for more details.
%
% Copyright (C) 2007-2008 Jouni Hartikainen
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

print_figures = 1;
save_figures = 1;

% Dimensionality of the state space
dims = 5;

% The number of models in use
nmodels = 2;

% Step size
dt = 0.1;

% Space for function handles and parameters
a_func = {};
ia_func = {};
a_param = {};
h_func = {};
h_param = {};
dh_dx_func = {};

a_func{1} = [];
a_func{2} = @f_turn;
ia_func{1} = [];
ia_func{2} = @f_turn_inv;
a_param{1} = [];
a_param{2} = {dt};

% Space for model parameters
ind = cell(1,nmodels);
F   = cell(1,nmodels);
L   = cell(1,nmodels);
Qc  = cell(1,nmodels);
A   = cell(1,nmodels);
Q   = cell(1,nmodels);
H   = cell(1,nmodels);
R   = cell(1,nmodels);

% Index vector of model 1
ind{1} = [1 2 3 4]';

% Transition matrix for the continous-time velocity model.
F{1} = [0 0 1 0;
        0 0 0 1;
        0 0 0 0;
        0 0 0 0];

% Noise effect matrix for the continous-time system.
L{1} = [0 0;
        0 0;
        1 0;
        0 1];

% Process noise variance
q1 = 0.01;
Qc{1} = diag([q1 q1]);

% Discretization of the continous-time system.
[A{1},Q{1}] = lti_disc(F{1},L{1},Qc{1},dt);

% Process noise variance for model 1 using EKF
EKF_q1 = .05;
EKF_Qc1 = diag([EKF_q1 EKF_q1]);

% Discretization of the continous-time system.
[EKF_A1,EKF_Q1] = lti_disc(F{1},L{1},EKF_Qc1,dt);

%%% Specification of the turning model

% System components. 5th parameter is the turning rate
ind{2} = [1 2 3 4 5]';

% Derivative of the dynamic function
A{2} = @f_turn_dx;
% Process noise for the turning rate
Qc{2} = 0.15;

% Noise effect matrix
L{2} = [0 0 0 0 1]';

% Process noise covariance
Q{2} = L{2}*Qc{2}*L{2}'*dt;

hdims = 2;

%mu_ip = [0.90 0.05 0.05];
mu_ip = [0.95 0.05];
mu_0j = mu_ip;
%p_ij = [0.65 0.35;
%        0.10 0.90];

p_ij = [0.90 0.10;
        0.10 0.90];

% Number of data points
n = 200;

% Space for real states and modes
X_r = zeros(dims,n);
mstate = zeros(1,n);

%%%%%%% Creation of trajectory %%%%%%%

% Start with constant velocity 1 toward right
mstate(1:40) = 1;
X_r(:,1) = [0 0 1 0 0]';

% At 4s make a turn left with rate 1
mstate(41:90) = 2;
X_r(5,40) = 1;

% At 9s move straight for 2 seconds
mstate(91:110) = 1;

% At 11s commence another turn right with rate -1
mstate(111:160) = 2;
X_r(5,110) = -1;

% At 16s move straight for 4 seconds
mstate(161:200) = 1;

% Generate object state values
for i = 2:n
   st = mstate(i);
   if isstr(a_func{st}) | strcmp(class(a_func{st}),'function_handle')
       X_r(ind{st},i) = feval(a_func{st},X_r(ind{st},i-1),a_param{st});
   else
       X_r(ind{st},i) = A{st}*X_r(ind{st},i-1);
   end
end

% Positions of sensors
% $$$ S1 = [-0.5; 3];
% $$$ S2 = [-0.5;-3];
% $$$ S3 = [   7;-3];
% $$$ S4 = [   7; 3];

S1 = [-0.5; 3.5];
S2 = [-0.5;-3.5];
S3 = [   7;-3.5];
S4 = [   7; 3.5];

s = [S1 S2 S3 S4];

% Handles to measurement models
H{1} = @bot_dh_dx;
H{2} = @bot_dh_dx;

h_func{1} = @bot_h;
h_func{2} = @bot_h;

h_param{1} = s;
h_param{2} = s;

% Standard deviation
sd = 0.1*ones(1,size(s,2));
R = {};
R{1} = diag(sd.^2);
R{2} = R{1};
% Generate measurements
Y = bot_h(X_r,s);
% Add noise
for i = 1:size(Y,2)
    st = mstate(i);
    for j = 1:size(Y,1)
        Y(j,i) = Y(j,i) + sqrt(R{st}(j,j)) * randn;
    end
end

% Print the trajectory
if print_figures
    h = plot(X_r(1,:),X_r(2,:),'-g',...
             s(1,:),s(2,:),'k^',...
             X_r(1,1),X_r(2,1),'ro','MarkerSize',12);
    legend('Real trajectory',...
           'Positions of sensors',...
           'Starting position', 'Location', 'North');
    set(h,'markersize',5);
    set(h,'linewidth',0.5);
    set(gca,'FontSize',8);
    xlim([-1 7.5])
    ylim([-4 4])
    if save_figures
        print('-depsc','eimm2_trajectory.eps');
    end
    pause
end

m = [0 0 0 -1 0]';
P = diag([10.1 10.1 1.1 1.1 1]);

%% Space for the estimates.

% EKF with model 1
EKF_MM = zeros(size(A{1},1), size(Y,2));
EKF_PP = zeros(size(A{1},1), size(A{1},1), size(Y,2));

% UKF with model 1
UKF_MM = zeros(size(A{1},1), size(Y,2));
UKF_PP = zeros(size(A{1},1), size(A{1},1), size(Y,2));

% EKF based IMM
EIMM_MM = zeros(size(m,1), size(Y,2));
EIMM_PP = zeros(size(m,1), size(m,1), size(Y,2));
EIMM_MM_i = cell(nmodels,n);
EIMM_PP_i = cell(nmodels,n);
EIMM_MU = zeros(nmodels,size(Y,2));

% UKF based IMM
UIMM_MM = zeros(size(m,1), size(Y,2));
UIMM_PP = zeros(size(m,1), size(m,1), size(Y,2));
UIMM_MM_i = cell(nmodels,n);
UIMM_PP_i = cell(nmodels,n);
UIMM_MU = zeros(nmodels,size(Y,2));

%%% Initial estimates %%%

% EKF with model 1
EKF_M = [0 0 0 -1]';
EKF_P = diag([1.1 1.1 0.1 0.1]);

% UKF with model 1
UKF_M = [0 0 0 -1]';
UKF_P = diag([1.1 1.1 0.1 0.1]);

% EKF based IMM
x_ip1{1} = [0 0 1 0]';
x_ip1{2} = [0 0 1 0 0]';
mu_ip1 = mu_ip;

P_ip1{1} = diag([0.1 0.1 0.1 0.1]);
P_ip1{2} = diag([0.1 0.1 0.1 0.1 1]);

% UKF based IMM
x_ip2{1} = [0 0 1 0]';
x_ip2{2} = [0 0 1 0 0]';
mu_ip2 = mu_ip;

P_ip2{1} = diag([0.1 0.1 0.1 0.1]);
P_ip2{2} = diag([0.1 0.1 0.1 0.1 1]);

% Filtering steps.
for i = 1:size(Y,2)
    % EKF with model 1
    [EKF_M,EKF_P] = kf_predict(EKF_M,EKF_P,EKF_A1,EKF_Q1);
    [EKF_M,EKF_P] = ekf_update1(EKF_M,EKF_P,Y(:,i),H{1},R{1},h_func{1},[],h_param{1});

    EKF_MM(:,i)   = EKF_M;
    EKF_PP(:,:,i) = EKF_P;

    % UKF with model 2
    [UKF_M,UKF_P] = kf_predict(UKF_M,UKF_P,EKF_A1,EKF_Q1);
    [UKF_M,UKF_P] = ukf_update1(UKF_M,UKF_P,Y(:,i),h_func{1},R{1},h_param{1});

    UKF_MM(:,i)   = UKF_M;
    UKF_PP(:,:,i) = UKF_P;

    % EKF based IMM
    [x_p1,P_p1,c_j1] = eimm_predict(x_ip1,P_ip1,mu_ip1,p_ij,ind,dims,A,a_func,a_param,Q);
    [x_ip1,P_ip1,mu_ip1,m1,P1] = eimm_update(x_p1,P_p1,c_j1,ind,dims,Y(:,i),H,h_func,R,h_param);
    EIMM_MM(:,i)   = m1;
    EIMM_PP(:,:,i) = P1;
    EIMM_MU(:,i)   = mu_ip1';
    EIMM_MM_i(:,i) = x_ip1';
    EIMM_PP_i(:,i) = P_ip1';

    % UKF based IMM
    [x_p2,P_p2,c_j2] = uimm_predict(x_ip2,P_ip2,mu_ip2,p_ij,ind,dims,A,a_func,a_param,Q);
    [x_ip2,P_ip2,mu_ip2,m2,P2] = uimm_update(x_p2,P_p2,c_j2,ind,dims,Y(:,i),H,h_func,R,h_param);

    UIMM_MM(:,i)   = m2;
    UIMM_PP(:,:,i) = P2;
    UIMM_MU(:,i)   = mu_ip2';
    UIMM_MM_i(:,i) = x_ip2';
    UIMM_PP_i(:,i) = P_ip2';

    % Plot the estimates so far
    if print_figures
        plot(EKF_MM(1,1:i),EKF_MM(2,1:i),'y-',...
             EIMM_MM(1,1:i),EIMM_MM(2,1:i),'r-',...
             UIMM_MM(1,1:i),UIMM_MM(2,1:i),'b-',...
             X_r(1,1:i),X_r(2,1:i),'g-');

        % Measurement directions
        hold on
        for k = 1:size(s,2)
            len = sqrt(sum((X_r(1:2,i)-s(:,k)).^2,1));
            dx = len*cos(Y(k,i));
            dy = len*sin(Y(k,i));

            plot([s(1,k);s(1,k)+dx], [s(2,k);s(2,k)+dy], 'k--')
        end
        plot(s(1,:),s(2,:),'k^')
        xlim([-1 7.5])
        ylim([-4 4])
        hold off
        drawnow
    end
end

% Smooth with EKF based IMM smoother
[SMI_1,SPI_1,SMI_i_1,SPI_i_1,MU_S_1] = eimm_smooth(EIMM_MM,EIMM_PP,EIMM_MM_i,EIMM_PP_i,EIMM_MU,p_ij,mu_0j,ind,dims,A,ia_func,a_param,Q,R,H,h_func,h_param,Y);

% Smooth with UKF based IMM smoother
[SMI_2,SPI_2,SMI_i_2,SPI_i_2,MU_S_2] = uimm_smooth(UIMM_MM,UIMM_PP,UIMM_MM_i,UIMM_PP_i,UIMM_MU,p_ij,mu_0j,ind,dims,A,ia_func,a_param,Q,R,H,h_func,h_param,Y);

% Smooth the EKF estimates with RTS smoother using model 1
[SM1,SP1] = rts_smooth(EKF_MM,EKF_PP,EKF_A1,EKF_Q1);

% Smooth the UKF estimates with RTS smoother using model 1
[SM2,SP2] = rts_smooth(UKF_MM,UKF_PP,EKF_A1,EKF_Q1);

% Calculate the MSEs
MSE_EKF1_1 = mean((X_r(1,:)-EKF_MM(1,:)).^2);
MSE_EKF1_2 = mean((X_r(2,:)-EKF_MM(2,:)).^2);
MSE_EKF1 = 1/2*(MSE_EKF1_1 + MSE_EKF1_2);

MSE_EKS1_1 = mean((X_r(1,:)-SM1(1,:)).^2);
MSE_EKS1_2 = mean((X_r(2,:)-SM1(2,:)).^2);
MSE_EKS1 = 1/2*(MSE_EKS1_1 + MSE_EKS1_2);

% Calculate the MSEs
MSE_UKF1_1 = mean((X_r(1,:)-UKF_MM(1,:)).^2);
MSE_UKF1_2 = mean((X_r(2,:)-UKF_MM(2,:)).^2);
MSE_UKF1 = 1/2*(MSE_UKF1_1 + MSE_UKF1_2);

MSE_UKS1_1 = mean((X_r(1,:)-SM2(1,:)).^2);
MSE_UKS1_2 = mean((X_r(2,:)-SM2(2,:)).^2);
MSE_UKS1 = 1/2*(MSE_UKS1_1 + MSE_UKS1_2);

MSE_EIMM1 = mean((X_r(1,:)-EIMM_MM(1,:)).^2);
MSE_EIMM2 = mean((X_r(2,:)-EIMM_MM(2,:)).^2);
MSE_EIMM = 1/2*(MSE_EIMM1 + MSE_EIMM2);

MSE_EIMMS1 = mean((X_r(1,:)-SMI_1(1,:)).^2);
MSE_EIMMS2 = mean((X_r(2,:)-SMI_1(2,:)).^2);
MSE_EIMMS = 1/2*(MSE_EIMMS1 + MSE_EIMMS2);

MSE_UIMM1 = mean((X_r(1,:)-UIMM_MM(1,:)).^2);
MSE_UIMM2 = mean((X_r(2,:)-UIMM_MM(2,:)).^2);
MSE_UIMM = 1/2*(MSE_UIMM1 + MSE_UIMM2);

MSE_UIMMS1 = mean((X_r(1,:)-SMI_2(1,:)).^2);
MSE_UIMMS2 = mean((X_r(2,:)-SMI_2(2,:)).^2);
MSE_UIMMS = 1/2*(MSE_UIMMS1 + MSE_UIMMS2);

fprintf('Mean square errors of position estimates:\n');
fprintf('EKF1-RMSE = %.4f\n',MSE_EKF1);
fprintf('EKS1-RMSE = %.4f\n',MSE_EKS1);
fprintf('UKF1-RMSE = %.4f\n',MSE_UKF1);
fprintf('UKS1-RMSE = %.4f\n',MSE_UKS1);
fprintf('EIMM-RMSE = %.4f\n',MSE_EIMM);
fprintf('EIMMS-RMSE = %.4f\n',MSE_EIMMS);
fprintf('UIMM-RMSE = %.4f\n',MSE_UIMM);
fprintf('UIMMS-RMSE = %.4f\n',MSE_UIMMS);

% Plot the final filtering and smoothing results
if print_figures
    h = plot(X_r(1,:),X_r(2,:),'g-',...
             EKF_MM(1,:),EKF_MM(2,:),'-k',...
             SM1(1,:),SM1(2,:),'--k',...
             EIMM_MM(1,:),EIMM_MM(2,:),'-r',...
             SMI_1(1,:),SMI_1(2,:),'--r',...
             UIMM_MM(1,:),UIMM_MM(2,:),'-b',...
             SMI_2(1,:),SMI_2(2,:),'--b');
    legend('True trajectory',...
           'EKF',...
           'ERTS',...
           'IMM-EKF',...
           'IMM-EKS',...
           'IMM-UKF',...
           'IMM-UKS');
    %title('Estimates produced by IMM-filter.')
    set(h,'markersize',2);
    set(h,'linewidth',0.5);
    set(gca,'FontSize',8);
    xlim([-1 7.5])
    ylim([-4 4])
    if save_figures
        print('-depsc','eimm2_1.eps');
    end
    pause

    % Determine the real model probabilities
    p_models = zeros(nmodels,n);
    I1 = find(mstate == 1);
    p_models(1,I1) = 1;
    I2 = find(mstate == 2);
    p_models(2,I2) = 1;

    % Plot model 2 probability for filters
    h = plot(1:n, p_models(2,:),'g--',...
             1:n,EIMM_MU(2,:)','-r',...
             1:n,MU_S_1(2,:)','--r',...
             1:n,UIMM_MU(2,:)','-b',...
             1:n,MU_S_2(2,:)','--b');
    %1:n,MU_S_2(2,:)','b-');
    legend('True',...
           'IMM-EKF',...
           'IMM-EKS',...
           'IMM-UKF',...
           'IMM-UKS');
    %title('Probability of model 2');
    ylim([-0.1,1.1]);
    set(h,'markersize',2);
    set(h,'linewidth',0.5);
    set(gca,'FontSize',8);
    if save_figures
        print('-depsc','eimm2_2.eps');
    end
    pause

    % Collect the turn rate estimates
    EMM_t = zeros(5,n);
    EMM_s = zeros(5,n);
    UMM_t = zeros(5,n);
    UMM_s = zeros(5,n);
    for i = 1:n
        EMM_t(:,i) = EIMM_MM_i{2,i};
        EMM_s(:,i) = SMI_i_1{2,i};
        UMM_t(:,i) = UIMM_MM_i{2,i};
        UMM_s(:,i) = SMI_i_2{2,i};
    end

    % Plot the filtered turn rates
    h = plot(1:n, X_r(5,:),'-g',...
             1:n,EMM_t(5,:),'-r',...
             1:n,EMM_s(5,:),'--r',...
             1:n,UMM_t(5,:),'-b',...
             1:n,UMM_s(5,:),'--b');
    %title('Turn rate estimates')
    legend('True',...
           'IMM-EKF',...
           'IMM-EKS',...
           'IMM-UKF',...
           'IMM-UKS');
    set(h,'markersize',2);
    set(h,'linewidth',0.5);
    set(gca,'FontSize',8);
    if save_figures
        print('-depsc','eimm2_3.eps');
    end

end


function T2 = sltensor_multiply(T, varargin)
%SLTENSOR_MULTIPLY Multiplies a tensor and a matrix
%
% $ Syntax $
%   - T2 = sltensor_multiply(T, M, k)
%   - T2 = sltensor_multiply(T, Ms)
%   - T2 = sltensor_multiply(T, Ms, ks)
%   - T2 = sltensor_multiply(T, M1, k1, M2, k2, ...)
%
% $ Description $
%   - T2 = sltensor_multiply(T, M, k) Computes the tensor multiplication between
%   a tensor and a 2D matrix along the k-th mode.
%
%   - T2 = sltensor_multiply(T, Ms) Sequentially multiplies the tensor with
%   the elements in Ms, which is a cell array from the mode 1 to n. n is
%   the number of matrices in Ms. It should be that n == ndims(T).
%
%   - T2 = sltensor_multiply(T, Ms, ks) Sequentially multiplies the tensor
%   with the matrices in the cell array Ms along the modes specified in
%   corresponding element in ks. ks should be an array with the same size
%   as Ms.
%
%   - T2 = sltensor_multiply(T, M1, k1, M2, k2, ...) Sequentially multiplies
%   the tensor with the matrices M1, M2, ... along the modes k1, k2, ...
%
% $ History $
%   - Created by Dahua Lin on Dec 17th, 2005
%

%% parse and verify input arguments
if nargin < 2
    raise_lackinput('sltensot_multiply', 2);
end
n0 = ndims(T);
if iscell(varargin{1});
    Ms = varargin{1};
    if nargin == 2
        n = numel(Ms);
        if n ~= n0
            error('sltoolbox:invalidarg', ...
                'For the case no specifying mode indices, it should be n == ndims(T)');
        end
        ks = 1:n;
    elseif nargin == 3
        ks = varargin{2};
        if ~isequal(size(Ms), size(k))
            error('sltoolbox:argmismatch', ...
                'The size of ks does not match that of Ms');
        end
    else
        error('sltoolbox:invalidarg', ...
            'Invalid input arguments');
    end
else
    if mod(length(varargin), 2) ~= 0
        error('sltoolbox:invalidarg', ...
            'Invalid input arguments');
    end
    Ms = varargin(1:2:end);
    ks = [varargin{2:2:end}];
    if numel(Ms) ~= numel(ks)
        error('sltoolbox:invalidarg', ...
            'Invalid input arguments');
    end
end
if any(ks(:) < 1)
    error('sltoolbox:invalidarg', ...
        'mode indices should all be positive integers');
end
maxk = max(ks(:));
dims = size(T);
if maxk > n0
    dims = [dims, ones(1, maxk-n0)];
end
nmat = numel(ks);

%% compute
if nmat == 1
    T2 = multiply_tensor_matrix(T, Ms{1}, ks, dims);
else
    T2 = T;
    for i = 1 : nmat
        T2 = multiply_tensor_matrix(T2, Ms{i}, ks(i), dims);
    end
end

%% compute (by converting to matrix product)

function T2 = multiply_tensor_matrix(T, M, k, dims)

Tk = sltensor_unfold(T, k);
T2 = M * Tk;
clear Tk;
dims(k) = size(M, 1);
T2 = sltensor_fold(T2, dims, k);


% LFHelperBuild4DFreq - Helper function used to construct 4D frequency-domain filters
%
% Much of the complexity in constructing MD frequency-domain filters, especially around including
% aliased components and controlling the filter rolloff, is common between filter shapes.  This
% function wraps much of this complexity.
%
% This gets called by the LFBuild4DFreq* functions.

% Copyright (c) 2013-2020 Donald G. Dansereau

function [H, FiltOptions] = LFHelperBuild4DFreq( LFSize, BW, FiltOptions, DistFunc )

FiltOptions = LFDefaultField('FiltOptions', 'Precision', 'single');
FiltOptions = LFDefaultField('FiltOptions', 'Rolloff', 'Gaussian'); %Gaussian or Butter
FiltOptions = LFDefaultField('FiltOptions', 'Aspect4D', 1);
FiltOptions = LFDefaultField('FiltOptions', 'Window', false);
FiltOptions = LFDefaultField('FiltOptions', 'Extent4D', 1.0);
FiltOptions = LFDefaultField('FiltOptions', 'IncludeAliased', false);

% avoid rounding error during comparisons
FiltOptions.Extent4D = cast(FiltOptions.Extent4D, FiltOptions.Precision);
FiltOptions.Aspect4D = cast(FiltOptions.Aspect4D, FiltOptions.Precision);

if( length(LFSize) == 1 )
	LFSize = LFSize .* [1,1,1,1];
end
if( length(LFSize) == 5 )
	LFSize = LFSize(1:4);
end
if( length(FiltOptions.Extent4D) == 1 )
	FiltOptions.Extent4D = FiltOptions.Extent4D .* [1,1,1,1];
end
if( length(FiltOptions.Aspect4D) == 1 )
	FiltOptions.Aspect4D = FiltOptions.Aspect4D .* [1,1,1,1];
end
ExtentWithAspect = FiltOptions.Extent4D.*FiltOptions.Aspect4D / 2;

t = LFNormalizedFreqAxis( LFSize(1), FiltOptions.Precision ) .* FiltOptions.Aspect4D(1);
s = LFNormalizedFreqAxis( LFSize(2), FiltOptions.Precision ) .* FiltOptions.Aspect4D(2);
v = LFNormalizedFreqAxis( LFSize(3), FiltOptions.Precision ) .* FiltOptions.Aspect4D(3);
u = LFNormalizedFreqAxis( LFSize(4), FiltOptions.Precision ) .* FiltOptions.Aspect4D(4);
[tt,ss,vv,uu] = ndgrid(cast(t,FiltOptions.Precision),cast(s,FiltOptions.Precision), ...
	cast(v,FiltOptions.Precision),cast(u,FiltOptions.Precision));
P = [tt(:),ss(:),vv(:),uu(:)]';
clear s t u v ss tt uu vv

if( ~FiltOptions.IncludeAliased )
	Tiles = [0,0,0,0];
else
	Tiles = ceil(ExtentWithAspect) % todo[optimization]: optimization possible for large extents
end

if( FiltOptions.IncludeAliased )
	AADist = inf(LFSize, FiltOptions.Precision);
end
WinDist = 0;

% Tiling proceeds only in positive directions, symmetry is enforced afterward, saving some computation overall
for( TTile = 0:Tiles(1) )
	for( STile = 0:Tiles(2) )
		for( VTile = 0:Tiles(3) )
			for( UTile = 0:Tiles(4) )
				Dist = inf(LFSize, FiltOptions.Precision);

				if( FiltOptions.Window )
					ValidIdx = ':';
					WinDist = bsxfun(@minus, abs(P)', ExtentWithAspect);
					WinDist = sum(max(0,WinDist).^2, 2);
					WinDist = reshape(WinDist, LFSize);
				else
					ValidIdx = find(all(bsxfun(@le, abs(P), ExtentWithAspect')));
				end

				Dist(ValidIdx) = DistFunc(P(:,ValidIdx), FiltOptions);
				Dist = Dist + WinDist;

				if( FiltOptions.IncludeAliased )
					AADist(:) = min(AADist(:), Dist(:));
				end

				P(4,:) = P(4,:) + FiltOptions.Aspect4D(4);
			end
			P(4,:) = P(4,:) - (Tiles(4)+1) .* FiltOptions.Aspect4D(4);
			P(3,:) = P(3,:) + FiltOptions.Aspect4D(3);
		end
		P(3,:) = P(3,:) - (Tiles(3)+1) .* FiltOptions.Aspect4D(3);
		P(2,:) = P(2,:) + FiltOptions.Aspect4D(2);
	end
	P(2,:) = P(2,:) - (Tiles(2)+1) .* FiltOptions.Aspect4D(2);
	P(1,:) = P(1,:) + FiltOptions.Aspect4D(1);
end

if( FiltOptions.IncludeAliased )
	Dist = AADist;
	clear AADist
end

H = zeros(LFSize, FiltOptions.Precision);

switch lower(FiltOptions.Rolloff)

	case 'gaussian'
		BW = BW.^2 / log(sqrt(2));
		H(:) = exp( -Dist / BW );  % Gaussian rolloff

	case 'butter'
		FiltOptions = LFDefaultField('FiltOptions', 'Order', 3);
		Dist = sqrt(Dist) ./ BW;
		H(:) = sqrt( 1.0 ./ (1.0 + Dist.^(2*FiltOptions.Order)) ); % Butterworth-like rolloff

	case 'sinc'
		Dist = sqrt(Dist) ./ BW;
		H(:) = sinc(Dist); % sinc-shaped rolloff

	otherwise
		error('unrecognized rolloff method');

end

H = ifftshift(H);

% force symmetric -- needed to visualize in 4D to get this one right
H = max(H, H(mod(LFSize(1):-1:1,LFSize(1))+1, mod(LFSize(2):-1:1,LFSize(2))+1, ...
	mod(LFSize(3):-1:1,LFSize(3))+1, mod(LFSize(4):-1:1,LFSize(4))+1));
H = max(H, H(:, mod(LFSize(2):-1:1,LFSize(2))+1, :, mod(LFSize(4):-1:1,LFSize(4))+1));
H = max(H, H(mod(LFSize(1):-1:1,LFSize(1))+1, :, mod(LFSize(3):-1:1,LFSize(3))+1, :));
% todo: confirm this is sufficient


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function [rho,drho] = rhoSpline(Tc,Rc,minT,maxT,nT,minR,maxR,nR,doDerivative)
%
% Parzen-Window Based density estimator for gray values of Tc and Rc, spline based.
%
% Input:
%  Tc, Rc          template and reference
%  minT, maxT, nT  discreted points in gray value range where splines are located
%  minR, maxR, nR  discreted points in gray value range where splines are located
%  doSerivative         FLAG for derivative compuatation
%
% Output:
%  rho          joint density estimator
%  drho         derivative of joint density estimator
%
% see also MIspline for an example.
%==============================================================================

function [rho,drho] = rhoSpline(Tc,Rc,minT,maxT,nT,minR,maxR,nR,doDerivative)

if nargin == 0,
  help(mfilename);
  return;
end;

if ~exist('doDerivative','var'), doDerivative = 1;       end;
doDerivative = doDerivative & (nargout>1);

% prepare the output and organize input
rho  = []; drho = []; Tc = reshape(Tc,[],1); Rc = reshape(Rc,[],1);

widthT = (maxT-minT)/nT;
widthR = (maxR-minR)/nR;

Tt   = linspace(minT,maxT,nT);
Rt   = linspace(minR,maxR,nR);
rho  = zeros(length(Tt),length(Rt));
drho = spalloc(numel(rho),length(Tc),5*length(Tc));

for j=1:length(Tc), % run over all samples
  IT = find(abs(Tt-Tc(j))<widthT); % find locations of interest in Tc
  IR = find(abs(Rt-Rc(j))<widthR); % find locations of interest in Rc
  if ~isempty(IT) && ~isempty(IR),
    [KT,dKT] = spline1D(Tc(j) - Tt(IT),widthT,doDerivative);
    KR       = spline1D(Rc(j) - Rt(IR),widthR,0);
    rho(IT,IR) = rho(IT,IR) +KT*KR';
    if doDerivative,
      drhoj = zeros(size(rho));
      drhoj(IT,IR) = dKT*KR';
      drho(:,j) = drho(:,j) + sparse(drhoj(:));
    end;
  end;
end;

% normalize rho
fac = (Tt(2)-Tt(1))*(Rt(2)-Rt(1))/length(Tc);
rho  = fac*rho(:);
drho = fac*drho;

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

% The Parzen Window Kernel is a spline function
%  function [s,ds] = spline1D(x,sigma,doDerivative);
% (c) Jan Modersitzki 2008/02/12, see FAIR.
% This evaluates a spline function s
% s(x,sigma) = 0 for x\notin[-sigma,sigma], \int s(x,sigma) dx = 1

function [s,ds] = spline1D(x,sigma,doDerivative);

if ~exist('doDerivative','var'), doDerivative = 1;       end;
doDerivative = doDerivative & (nargout>1);

% map [-sigm2,sigma] \to [-2,2]
x = 4/sigma*x;

if ~doDerivative,
  s  = 0*reshape(x,[],1);
  ds = [];
  J = find(x>=-2 & x<-1);   s(J)   = (x(J)+2).^3;
  J = find(x>=-1 & x< 0);   s(J)   = -x(J).^3 - 2*(x(J)+1).^3+6*(x(J)+1);
  J = find(x>= 0 & x< 1);   s(J)   = -2*(1-x(J)).^3 + x(J).^3 - 6*x(J) + 6;
  J = find(x>= 0 & x< 1);   s(J)   = -2*(1-x(J)).^3 + x(J).^3 - 6*x(J) + 6;
  J = find(x>= 1 & x< 2);   s(J)   = (2-x(J)).^3;
else
  s  = 0*reshape(x,[],1);
  ds = s;
  J = find(x>=-2 & x<-1);   s(J)   = (x(J)+2).^3;
                            ds(J)  = 3*(x(J)+2).^2;
  J = find(x>=-1 & x< 0);   s(J)   = -x(J).^3 - 2*(x(J)+1).^3+6*(x(J)+1);
                            ds(J)  = -3*x(J).^2 - 6*(x(J)+1).^2+6;
  J = find(x>= 0 & x< 1);   s(J)   = -2*(1-x(J)).^3 + x(J).^3 - 6*x(J) + 6;
                            ds(J)  = 6*(1-x(J)).^2 + 3*x(J).^2 - 6;
  J = find(x>= 1 & x< 2);   s(J)   = (2-x(J)).^3;
                            ds(J)  = -3*(2-x(J)).^2;
end;
% scale to make integral 1
s  = 2/(3*sigma)*s;
ds = 8/(3*sigma^2)*ds;
%==============================================================================


function [x,y,xy,xyp,ee] = q2p1grid(x,y,xy,mv,bound);
%q2p1grid   Q2-P1 element grid generator
%   [x,y,xy,xyp,ee] = q2p1grid(x,y,xy,mv,bound);
%   input
%          x          x coordinate vector
%          y          y coordinate vector
%          xy         nodal coordinate vector
%          mv         Q2 macroelement mapping matrix
%          bound      boundary vertex vector
%   output
%          xyp        centroid coordinate vector
%          ee         element edge connection matrix
%
%   IFISS function: DJS; 28 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
xx=xy(:,1); yy=xy(:,2); nvtx=length(xx);
nel=length(mv(:,1));
%% recompute mid-side points in the case of stretched grids
% y-direction
yv=yy; ny=length(y);
for k=2:2:ny;
yold=y(k); ynew=0.5*(y(k+1)+y(k-1));
l=find(yy==yold); yv(l)=ynew; y(k)=ynew;
end
% x-direction
xv=xx; nx=length(x);
for k=2:2:nx;
xold=x(k); xnew=0.5*(x(k+1)+x(k-1));
l=find(xx==xold); xv(l)=xnew; x(k)=xnew;
end
xy=[xv,yv];
% centroid coordinates
for ielem=1:nel
xc(ielem)=mean(xx(mv(ielem,1:4))); yc(ielem)=mean(yy(mv(ielem,1:4)));
end
xyp=[xc',yc'];
%
%% compute edge to edge connection array ee
      np=nel;
% initialise global matrices
      adj = sparse(nvtx,nvtx);
      ee = zeros(nel,4);
%
% evaluate element number on each edge in turn
% and assemble into adjacency matrix
%% nx= 0, ny=-1
		 adj=adj + sparse(mv(:,1),mv(:,2),1:np,nvtx,nvtx);
%% nx= 1, ny= 0
		 adj=adj + sparse(mv(:,2),mv(:,3),1:np,nvtx,nvtx);
%% nx= 0, ny= 1
		 adj=adj + sparse(mv(:,3),mv(:,4),1:np,nvtx,nvtx);
%% nx=-1, ny= 0
		 adj=adj + sparse(mv(:,4),mv(:,1),1:np,nvtx,nvtx);
%
       for el=1:nel
		   [ii,jj]=find(adj==el);
           ee(el,:)=diag(adj(jj,ii))';
	   end
       ee=ee(:,[2,4,3,1]);

%
% plotting of the grid
%if nel <=64,
adj=sparse(nvtx,nvtx);
mel=length(mv(:,1));
for i=1:nel
adj(mv(i,1),mv(i,2))=1;
adj(mv(i,2),mv(i,3))=1;
adj(mv(i,3),mv(i,4))=1;
adj(mv(i,4),mv(i,1))=1;
end
figure(30)
gplot(adj,xy,'b')
axis('square')
hold on
plot(xy(:,1),xy(:,2),'ro')
xybd=xy(bound,:);
plot(xybd(:,1),xybd(:,2),'ko')
plot(xyp(:,1),xyp(:,2),'k*',xyp(:,1),xyp(:,2),'ro')
hold off
title('Q2-P1 finite element subdivision')
drawnow
%end
return


function [w,u,eqn,info] = biharmonicP2(node,elem,bdFlag,pde,option)
%%
%   [w,u] = biharmonicP1(node,elem,bdFlag,pde) produces the mixed cubic finite element
%   approximation of the biharmonic equation, where w = laplace u
%   See also biharmonicP1, biharmonicP2, biharmonicP3.
%
%   Created by Jie Zhou. Clean up is needed
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if nargin<5, option = []; end
tic;
%% Construct Data Structure
[elem2dof,edge,bdDof] = dofP2(elem);
N = size(node,1);  NT = size(elem,1);  Ndof = N+size(edge,1);

%% Compute geometric quantities and gradient of local basis
[Dlambda,area] = gradbasis(node,elem);

%% Assemble stiffness matrix
% Since Dphi_i*Dphi_j is quadratic, numerical quadrature rule is used here
if ~isfield(option,'quadorder')
    option.quadorder = 4;   % default order
end
[lambda, weight] = quadpts(option.quadorder);
nQuad = size(lambda,1);
ii = zeros(21*NT,1); jj = zeros(21*NT,1); sA = zeros(21*NT,1);sB = zeros(21*NT,1);
index = 0;
for i = 1:6
    for j = i:6
        Bij = 0;
        Aij = 0;
        for p = 1:nQuad
                 Bij = Bij + weight(p)*dot(Dphi(p,i),Dphi(p,j),2);
                 Aij = Aij + weight(p)*dot(phi(p,i),phi(p,j),2);
        end
        Bij = Bij.*area;
        Aij = Aij.*area;

        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        sB(index+1:index+NT) = Bij;
        sA(index+1:index+NT) = Aij;
        index = index + NT;
    end
end
clear Aij Bij
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
B = sparse(ii(diagIdx),jj(diagIdx),sB(diagIdx),Ndof,Ndof);
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
% A = spdiags(accumarray(ii(diagIdx),sA(diagIdx),[Ndof 1]),0,Ndof,Ndof);
BU = sparse(ii(upperIdx),jj(upperIdx),sB(upperIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
B = B + BU + BU';
A = A + AU + AU';

%% boundary condition
%%
    fixedDof = [];
    isFixedDof = false(Ndof,1);
    if ~isempty(bdFlag)
        elem2edge = elem2dof(:,4:6)-N;
        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        isFixedDof(edge(isDirichlet,:)) = true;
        isFixedDof(N + find(isDirichlet')) = true;
        fixedDof = find(isFixedDof);
        freeDof = find(~isFixedDof);
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunction Dphi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function s = Dphi(p,i) % gradient of basis phi
    switch i
        case 1
            s = (4*lambda(p,1)-1).*Dlambda(:,:,1);
        case 2
            s = (4*lambda(p,2)-1).*Dlambda(:,:,2);
        case 3
            s = (4*lambda(p,3)-1).*Dlambda(:,:,3);
        case 4
            s = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
        case 5
            s = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
        case 6
            s = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunction Dphi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function s = phi(p,i) % gradient of basis phi
    switch i
        case 1
            s = (2*lambda(p,1)-1).*lambda(p,1);
        case 2
            s = (2*lambda(p,2)-1).*lambda(p,2);
        case 3
            s = (2*lambda(p,3)-1).*lambda(p,3);
        case 4
            s = 4*lambda(p,3).*lambda(p,2);
        case 5
            s = 4*lambda(p,1).*lambda(p,3);
        case 6
            s = 4*lambda(p,1).*lambda(p,2);
    end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Assemble right hand side by high order quadrature rule
% To reduce the effect of the error introduced by the numerical quadrature,
% the load term is computed using the 3rd order qudrature rule.
b = zeros(Ndof,1);
% u=zeros(Ndof,1);
w = zeros(Ndof,1);

if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,w] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
%     phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
%     phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
%     phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
%     phi(:,4) = 4*lambda(:,2).*lambda(:,3);
%     phi(:,5) = 4*lambda(:,3).*lambda(:,1);
%     phi(:,6) = 4*lambda(:,1).*lambda(:,2);
    bt = zeros(NT,6);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        if isfield(pde,'f') && isnumeric(pde.f)
            fp = pde.f;        % piecewise constant
        else
            fp = pde.f(pxy);   % function handle
        end
        for j = 1:6
            bt(:,j) = bt(:,j) + w(p)*phi(p,j)*fp;
        end
    end
    bt = bt.*repmat(area,1,6);
    b = accumarray(elem2dof(:),bt(:),[Ndof 1]);
end

[b1,u] = getbdP2(b);
B(:,fixedDof)=[];
Nu=size(freeDof,1);

b(fixedDof)=[];

switch solver
    case 'direct'
        t = cputime;
        bigA = [A, B; ...
                B', sparse(Nu,Nu)];
        bigF = [b1; -b];
        bigu = bigA\bigF;
        w = bigu(1:Ndof);
        u(freeDof)=bigu(Ndof+1:Ndof+Nu);
        residual = norm(bigF-bigA*bigu);
        info = struct('solverTime',cputime - t,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
end % end of four order.

%     soln = struct('u',u,'Du',Du);
    eqn = struct('A',A,'B',B','f',b1,'g',-b,'freeNode',freeNode,'Lap',A);
    info.assembleTime = assembleTime;

 function [b1,u] = getbdP2
    %% Boundary conditions for Poisson equation: P2 quadratic FEM.
    %
    % The set up of boundary condition consists of two parts:
    %

    %
    %  Modify the right hand side b. The Neumann boundary integral is added
    %  to b. For Dirichlet boundary ndoes, b(fixedDof) is the evaluation of
    % pde.g_D.
    %
    u = zeros(Ndof,1);

    %% Part 1: Find boundary edges and modify the load b
    % Neumann boundary condition
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 6;
        end
                idxN = (bdFlag(:) == 1);      % all Neumann edges in bdFlag
        Neumannidx = elem2edge(idxN ); % index of Neumann and Robin edges
        % since boundary integral is also needed for Robin edges
        Neumann   = edge(Neumannidx,:);
        b1 = zeros(Ndof,1);
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        nQuadgN = size(lambdagN,1);
        % quadratic bases (1---3---2)
        bdphi = zeros(nQuadgN,3);
        bdphi(:,1) = (2*lambdagN(:,1)-1).*lambdagN(:,1);
        bdphi(:,2) = (2*lambdagN(:,2)-1).*lambdagN(:,2);
        bdphi(:,3) = 4*lambdagN(:,1).*lambdagN(:,2);
        % length of edge
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        ge = zeros(size(Neumann,1),3);
        for pp = 1:nQuadgN
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNu = pde.g_N(ppxy);
            ge(:,1) = ge(:,1) + weightgN(pp)*gNu*bdphi(pp,1);
            ge(:,2) = ge(:,2) + weightgN(pp)*gNu*bdphi(pp,2);
            ge(:,3) = ge(:,3) + weightgN(pp)*gNu*bdphi(pp,3); % interior bubble
        end
        % update RHS
        ge = ge.*repmat(el,1,3);
        b1(1:N) = accumarray(Neumann(:), [ge(:,1); ge(:,2)],[N,1]);
        b1(N+Neumannidx) = b1(N+Neumannidx) + ge(:,3);

    %% Part 2: Find Dirichlet boundary edges and compute the boundary value
    % Dirichlet boundary conditions

        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        % interpolation
                idx = (fixedDof > N);                         % index of edge nodes
        u(fixedDof(~idx)) = pde.g_D(node(fixedDof(~idx),:)); % bd value at vertex dofs
        bdEdgeIdx = fixedDof(idx) - N;
        bdEdgeMid = (node(edge(bdEdgeIdx,1),:) + node(edge(bdEdgeIdx,2),:))/2;
        u(fixedDof(idx)) = pde.g_D(bdEdgeMid);
        b1 = b1 - B*u;

    end % end of getbdP2

end                 % end of function biharmonicP2


% Fast Inter-Harmonic Reconstruction using the zero-phase version of the
% speech signal, as a pre-process for LPC-based spectral envelope estimation.
%
%
% Description
%  This technique reconstructs inter-harmonics of the voice signal,
% as a pre-process for an efficient spectral envelope estimation in
% high-pitched voices. The technique is fully described in [1].

%
% Inputs
%  wave            : [samples] [Nx1] input signal (speech signal)
%  Fs              : [Hz]      [1x1] sampling frequency
%  f0              : F0 estimate (a 0 value is for an unvoiced frame)
%  order           : Order used for the LP analysis
%
%
% Outputs
%  LP              : LP coefficients
%  Energy          : Energy of the frames
%
%
% Example
%  Please see the HOWTO_envelope.m example file.
%  Please see http://tcts.fpms.ac.be/~drugman/Toolbox/ for more details.
%
% References
%  [1] T.Drugman, Y. Stylianou, "Fast Inter-Harmonic Reconstruction for
%      Spectral Envelope Estimation in High-Pitched Voices", vol. 21, pp.
%      1418-1422, IEEE Signal Processing Letters, 2014.
%      http://tcts.fpms.ac.be/~drugman/files/SPL-FIHR.pdf
%
% Copyright (c) 2014 Toshiba Cambridge Research Laboratory
%
% License
%  This code will be part of the GLOAT toolbox with the following
%  licence:
%  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.
%
% This function will also be part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Thomas Drugman thomas.drugman@umons.ac.be

function [LP, Energy] = env_fihrzp(Seg,Fs,F0_local,order)

F0_target=100;
% F0* in the paper. Here set to 100 Hz.

Energy=sum(Seg.^2);

if F0_local>0

    %Zero-phase version of the input signal
    Nfft=2048;
    ZP=real(ifft(abs(fft(Seg',Nfft))));
    ZP=[ZP(Nfft/2+1:end) ZP(1:Nfft/2)]';
    Seg=ZP(round(length(ZP)/2)-round(length(Seg)/2):round(length(ZP)/2)+round(length(Seg)/2)-1);

    % The rest is similar to the FIHR technique
    TmpSignal=ones(1,length(Seg));
    t_tmp=1:length(TmpSignal);

    I=ceil(F0_local/F0_target);
    for k=1:I-1
        W=(I-k)/I;
        % We use here a linear weighting function. Other functions are
        % possible, as long as they meet the properties mentioned in
        % the paper.
        TmpSignal=TmpSignal+2*W*cos(2*pi*(k/I)*F0_local/Fs*t_tmp);

        % Equation (2) in the paper
    end

    Seg2=Seg.*TmpSignal';
else
    Seg2=Seg;
end

LP=lpc(Seg2,order);


%kvgef 'First Derivative Operator for Symmetric Exponential Filter (K1)'
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros vgef.pane file
%
% Parameters:
% InputFile: i 'Input Image', required: 'input image'
% Double: a0 ' a0 ', default: 0.45: 'Exponential Filter Parameter for GEF'
% Integer: t1 ' T1 ', default: 3: '1st Hysteresis Threshold'
% Integer: t2 ' T2 ', default: 4: '2nd Hysteresis Threshold'
% Integer: l 'Length', default: 5: 'Minimum Number of Pixels in a Segment'
% OutputFile: o 'Output Image', required: 'output image'
%
% Example: o = kvgef(i, {'i','';'a0',0.45;'t1',3;'t2',4;'l',5;'o',''})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% vgef - First Derivative Operator for Symmetric Exponential Filter  (K1)
%
%  DESCRIPTION
%
% This Algorithm has been written by the team of:
%
%
% Professor SERGE CASTAN
%
%
% IRIT, URA 1399
%
%
% 118, route de Narbonne  31062 Toulouse  FRANCE
%
% OPTIMAL FILTER(ISEF) FOR EDGE DETECTION
%
% PRINCIPLE :
% I. Introduction
% Edge detection is one of the most important subjects in image processing,
% which finds wide applications in the pattern recognition, the scene analysis and
% the 3-D vision, because the edges correspond in general to the important changes
% of physical or geometrical properties of objects in the scene and they are widely
% used as primitives in the pattern recognition, the image matching etc.
%
% The edges coincide, generally speaking, grey level transition, they can
% be detected by maxima of gradient or the zero-crossing of the second
% derivatives calculated by some differential operators. Because the
% differential operators are sensitive to noise, a preprocessing such as
% smoothing is often necessary to eliminate the noise. A well-known smoothing
% filter is Gaussian filter and the edges can therefore be detected by a
% Laplacian-Gaussian filter. But there is an essential difficulty of
% the Laplacian-Gaussian filter which is the contradiction between the
% smoothing effect and the precision of edge localization. To overcome this
% difficulty,  We proposed the optimal linear filter based on one step model
% (a step edge and the white noise) and the multi-edge model [9][10][11].
% This optimal smoothing filter is a symmetric exponential filter of an
% infinitely large window size and can be realized by very simple recursive
% algorithm. It  is proved that the band limited Laplacian of an input image
% filtered by this filter can be calculated from the Difference between the input
% and the output of this Recursive Filter (DRF). The edges detected by DRF
% method are less noisy and with a much better precision of localization.
%
% The maxima of gradient or zeros of the second directional derivative
% along the gradient are a natural definition of intensity edges. Zeros of the
% Laplacian are only extensively used for their computational convenience.
% However, we must stress here that the zeros crossing of the Laplacian are
% not always coincided with the maxima of gradient, for example, the zeros
% of the Laplacian are farther apart than the maxima of gradient for circularly
% symmetric patterns, this lack of localization by the Laplacian can also be
% seen in the fact that zeros of Laplacian "swing wide" of corners. Therefore,
% it had better to detect the edges by maxima of gradient or zeros of the second
% directional derivative along the gradient .
%
% So we propose two methods for edges detection, one uses maxima of Gradient
% (GEF), another uses the zeros crossing of Second directional Derivative along
% the gradient (SDEF).
%
% II. The First and the Second Directional Derivative Operators for Symmetric
% Exponential Filter
% A normalized symmetric exponential filter on 1-D can be written :
% fL(x)=C*a0*(1-a0)**|x|=f1(x)#f2(x)=C*(f1(x)+f2(x)-a0*d(x)) (1)
%
% where:
% C=1/(2-a0), # means the convolution, d(x) is dirac function.
% f1(x)={0 if x<0  and a0*(1-a0)**x   if x>= 0  (2)
% f2(x)={0 if x>0  and a0*(1-a0)**-x  if x<= 0  (2)
% we can write the first derivative operator of exponential filter:
%
% fL'(x)=f2(x)-f1(x)         (3)
% And we can obtain the normalized second derivative operator of
% exponential filter :
% fL"(x)=f1(x)+f2(x)-2*d(x)  (4)
%
% Because the exponential function is separable, we can write out 2-D
% exponential filter:
% f(x,y) = fL(x)*fL(y)                                            (5)
%
% From the equations (18),(19) and (20),  the first and the second
% directional derivative operators for symmetric exponential filter can be
% written like this:
%
%
% fx(x,y) = fL(y)*(f2(x)-f1(x))          (6)
%
%
% fy(x,y) = fL(x)*(f2(y)-f1(y))          (7)
%
%
% fxx(x,y) = fL(y)*(f1(x)+f2(x)-2*d(x))  (8)
%
%
% fyy(x,y) = fL(x)*(f1(y)+f2(y)-2*d(y))  (9)
%
% III. The Recursive Algorithm for realizing the These Directional Derivative
% Operators of Symmetric Exponential Filter
%
% The exponential filter is an IIR filter corresponding to an infinite
% window size, so we should realize the functions f1(x) and f2(x) (see formula
% (2)) by a recursive algorithm.
%
% Supposing I(x,y) is the input image,
% I1(x,y)=I(x,y)#f1(x) and I2(x,y)=I(x,y)#f2(x),
% we have the recursive algorithm :
% I1(x,y)=I1(x-1,y)+a0*(I(x,y)-I1(x-1,y))    (10)
% I2(x,y)=I2(x+1,y)+a0*(I(x,y)-I2(x+1,y))
%
% From the equations (1),(6),(7),(8) and (9), the band-limited first
% and second directional derivative of input image can be calculated by the
% recursive algorithm f1 and f2 as follows
% Ix(x,y)=I(x,y)#f1(y)#f2(y)#(f2(x)-f1(x))   (11)
% Ixx(x,y)=I(x,y)#f1(y)#f2(y)#(f2(x)+f1(x))-2*I(x,y)#f1(y)#f2(y)  (12)
% Iy(x,y) = I(x,y)#f1(x)#f2(x)#(f2(y)-f1(y))                        (13)
% Iyy(x,y) = I(x,y)#f1(x)#f2(x)#(f2(y)+f1(y))-2*I(x,y)#f1(x)#f2(x)  (14)
%
% With this algorithm, we can calculate at the same time the band-limited
% first and second directional derivative Ix and Ixx ( or Iy and Iyy ) of
% input image.
%
% IV. Edges Detection
% The band-limited first and second directional derivative of input image
% can be obtained by the algorithms as stated above. Using them, we can then
% realize the edge detection for an image.
%
% The maxima of gradient or zeros of the second directional derivative
% along the gradient are a natural way of characterizing and localizing intensity
% edges,  so we present here to detect the edges from the maxima of gradient or
% zeros of the second directional derivative along the gradient by using the
% differential operators of exponential filter.
%
%
% 1. Edges from the maxima of gradient
% Using the first directional derivative operator of the exponential filter,
% the two band-limited first directional derivatives Ix and Iy can be calculated,
% and the gradient vector can be therefore determined approximatively for every
% point in image. The gradient magnitude image is then non maxima suppressed in
% the gradient direction and thresholded with hysteresis, i.e. if the entire
% segment of the contour lies above a low threshold T1, and at least one of part of
% which is above a high threshold T2, that contour is output. The non maxima
% suppression scheme requires three points, one of which will be the current
% point, and the other two should be estimated of the gradient magnitude at
% points displaced from the current point by vector normal to the edge direction.
%
%
% 2. Edges from the zero-crossings of the second directional derivative along the
% gradient direction
% Because edges detected from local gradient maxima can not be a pixel
% width (less good precision of localization), we propose an other method which
% detect the edges from the zeros crossing of the second directional derivative
% along the gradient direction.
%
% We can calculate Ix,Iy,Ixx and Iyy by using the method shown in
% paragraph III, and therefore obtain approximatively the gradient vector
% and the second derivative in the gradient direction for every point in image.
% We extract at first the zero crossing of second derivative along the gradient
% direction on which the gradient magnitude must be above a low threshold, so an
% edges image is obtained. To this image, the entire segment of the contour will
% be kept, if the gradient magnitude on at least one part of this contour is above
% a high threshold.
% V. Comparison of Performance of the Filters
% Filtering is a problem of estimation from noisy signal, and edge
% detection is a problem of estimating the position of maximal local signal
% change. Up to now, many works are done for edges detection in image, and
% different filters are proposed, for example, Gaussian filter, Canny filter,
% exponential filter, Deriche filter etc...
%
% We appreciate the performance of the filters as follows :
%
%
% (1) Precision of edge localization
% According to our analysis [10], we can calculate the average
% localization error xe for Gaussian filter, Canny filter [15], Deriche
% filter [16] and the exponential filter :
%
%
% xeG = (4*(2*e*3.14)**0.5)/a
%
%
% xec = 0.81/a
%
%
% xeD = 4*exp(-1)/a = 1.47/ a
%
%
% xeE = 0
%
%
% i.e. xeG > xeD > xec > xeE = 0.
%
% So,  we can see that the exponential filter localizes edge points with
% the best precision.
%
%
% (2) Signal/Noise ratio on the edge point detected
% Because xe is the average estimation for the position of the edge point
% detected, we propose to calculate Signal/Noise ratio (Eq.(7)) at the point xe.
%
% And the signal/noise ratio for the Gaussian filter, Canny filter,
% Deriche filter and the exponential filter is :
%
%
% SNRG = 2*s*exp(-32*s)/ ((3.14)**0.5)
%
%
% SNRc = 0.39/ a
%
%
% SNRD = 0.64/ a
%
%
% SNRE =1/ a
%
%
% i.e. SNRE > SNRD > SNRc > SNRG.
%
% Then, we see that the exponential filter has the best noise eliminating
% effect among the above four.
%
%
% (3) Complexity of calculation
% For the complexity of calculation, we only tell the difference from
% exponential filter and Deriche filter [16], because they are implemented by
% recursive algorithms which have a simpler calculation.
%
% Because ISEF can be realized by first order recursive filter, the
% ISEF algorithms are much simpler than that of Deriche filter. Besides, the
% ISEF algorithms can be implemented independently to every line and every column,
% it can be easily realized by a parallel system.
%
% According to the analysis results above, the ISEF filter is superior
% to the others at the 3 principal aspects of the performance of the filter.
%
% VI. Conclusion
% The symmetric exponential filter of an infinite large window size is
% an optimal linear filter deduced from one step edge model and the multi-edge
% model, now we further prove that the symmetric exponential filter is the optimal
% edge detection filter in the criteria of the signal to noise ratio, the
% localization precision and unique maximum. Obviously, the real images will be
% still more complicated than these models, however DRF method has already provided
% good results for different type of images. The results obtained through the
% two new methods further show the superior performance of this filter.
% The theoretical analysis for the performance of the filters shows also that
% the exponential filter is superior to the other current filters.
%
% The first and second directional derivative operators can be realized by
% recursive algorithm and calculated at the same time. The new algorithms
% are therefore very simple as well as DRF algorithm, and they are also easy
% to implemented in a parallel way.
%
%
%
%  EXAMPLES
% vgef -i cross.xv -o output.xv
%
% will compute a edge extraction by using the first directional
% derivative operator for symmetric exponential filter on
% image cross.xv and write the result in output.xv
%
% For the GEF edge extraction the filter parameter value is generally
% a0 between 0.40 and 0.9 (0.45  [default] is a good value)
%
% The hysteresis Thresholds T1 and T2 can be set to 12 and 15.
%
% The minimum Length of segment is up to the user. It is set by default to 10.
%
%
%  "SEE ALSO"
%
%  RESTRICTIONS
% works only with single band byte images.
%
% Note that this routine was converted directly from Khoros 1.0.  Due to
% the mixing of unsigned versus signed char, and the difference in the rules
% that resolve the mixing of these data types between ANSI C and the original
% K&R C that the code was written in, the results of running this program
% on an image will not be the same as with vdrf in Khoros 1.0.  There is
% really no way to resolve which version is more correct, but the ANSI C version
% should at least be more consistent across machine architectures.
%
%  REFERENCES
%
% [1] W.K. PRATT, Digital Image Processing, New York, 1978.
%
% [2] J. PREWITT, Object Enhancement and Extraction,Picture Processing and
% Psychopictories, Etd. by B. Lipkin and A. Rosenfeld, New York, pp.75-149, 1970.
%
% [3] M. HUECKEL, An Operator Which Locates Edges in Digitized Pictures.
% J.A.C.M., Vol. 18, pp 113-125, 1971.
%
% [4] R.O. DUDA and P.E. HART, Pattern Classification and Scene Analysis.
% Wiley, New York, 1973.
%
% [5] R. HARALICK,Edge and Region Analysis for Digital Image Data. C.G.I.P.,
% Vol. 12, pp 60-73, 1980.
%
% [6] R.HARALICK and L.WATSON, A Facet Model for Image Data. C.G.I.P., Vol. 15,
% pp 113-129, 1981.
%
% [8] D. MARR and E.C. HILDRETH, Theory of Edge Detection. Proc. R. Soc.
% Lond. B, Vol. 207, pp 187-217, 1980.
%
% [9] J. SHEN and S. CASTAN, Un nouvel algorithme de detection de contours,
% proceedings of 5th Conf. on P.R.&.A.I. (in French), Grenoble, 1985.
%
% [10] J. SHEN and S. CASTAN, An Optimal Linear Operator for Edge Detection.
% Proc. CVPR'86,  Miami,1986.
%
% [11] J. SHEN and S. CASTAN, Edge Detection Based on Multi-Edge Models.Proc.
% SPIE'87, Cannes, 1987.
%
% [12] J. SHEN and S. CASTAN, Further Results on DRF Method for Edge Detection.
% 9th I.C.P.R., ROME, 1988.
%
% [13] V.TORRE and T.A.POGGIO, On Edge Detection IEEE Transaction on Pattern
% Analysis and    Machine Intelligence, Vol. Pami-8, N 2, March 1986.
%
% [14] J.S.CHEN and G.MEDIONI, Detection, Localization, and Estimation of Edges.
% IEEE  Transaction on Pattern  Analysis and Machine Intelligence, Vol. 11, N 2,
% February  1989.
%
% [15] J.F.CANNY, Finding Edges And Lines in Images.
% MIT Technical Report N 720, 1983.
%
% [16] R. DERICHE, Optimal Edge Detection Using Recursive Filtering.
% In proc. First International Conference on Computer Vision,
% London, June 8-12 1987.
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kvgef(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kvgef(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i', '__input';'a0', 0.45;'t1', 3;'t2', 4;'l', 5;'o', '__output'};
maxval={0,1,255,255,100000,0};
minval={0,0,0,0,0,0};
istoggle=[0,1,1,1,1,0];
was_set=istoggle * 0;
paramtype={'InputFile','Double','Integer','Integer','Integer','OutputFile'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=1; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'vgef"  '],Inputs,narglist);


function [inpoly onboundary] = insidepoly(varargin)
% [inpoly onboundary] = insidepoly(X, Y, PX, PY)
%
% Check if (X,Y) are inside the interior of a 2D polygon delimited by the
% polygon vertices (PX,PY).
%
% INPUTS:
%   - X, Y: arrays of same size, coordinates of N data points
%   - PX, PY: arrays of same size, coordinates of M vertices
%   - Provide optionally EDGE: insidepoly(x, y, Px, Py, edges)
%   EDGES are (m x 2) linear indexes of (Px,Py) that will
%   be considered as vertices of the polygon. This allows user to provide
%   polygons with multiple connexed boundaries (e.g., having holes)
%
% Alternate calling:
%   >> inpoly = insidepoly(XY, P) % or
%   >> inpoly = insidepoly(XY, P, EDGES)
%   - XY: (n x 2) array of n points, ranged by row
%   - P: (m x 2) array of m vertices, ranged by row
%   - EDGES (if provided in third argument) correponds to the
%   first-dimension indexed of P: P(EDGES,:) is the polygone boundary
%
% Advanced control the algorithm:
%   Optionally, user can provide parameters to control the algorithm
%   >> insidepoly(..., 'property1', value1, ...)
% Valid properties (case sensitive) are
%   - 'tol': ['auto'] or positive value: eulidian distance tolerance to
%      detect boundary points. When 'tol' is is set to 'auto', the
%      tolerance value of 1e-9*max(dPx,dPy) is used, where dPx, dPy are
%      respectively the horizontal and vertical size of the polygon.
%   - 'presortflag':  ['auto'], 0, or 1:
%     For large number of vertices and points, INSIDEPOLY uses a sorting
%     strategy to reduce the data to be scanned by each polygonal edge.
%     This reduction comes at the cost of additional processing. The
%     complexity of the sorting step is O((N + M)*log(N+M)). If the numbers
%     of vertices and/or data are small, the sorting is relatively expensive
%     with respect to the complexity of O(M*N) needed for the calculation
%     for the polygonal region determitaion. It might be preferable to
%     disable this step. By default, the presorting is enabled when:
%                      M>32 and M*N>25000.
%     Set the presortflag to 0 or 1 to enable/disable the sorting step.
%
% OUTPUTS:
%   - INPOLY: logical array same size as (X,Y), TRUE if the point is
%   inside or on the boundary of the polygon, FALSE otherwise
%   - ONBOUNDARY: logical array same size as (X,Y) to determine points
%   on the boundary (within numercial tolerance)
%
% This functions has C-MEX engine to speed up the calculation. These Mex
% files must be compiled by lauching insidepoly_install.m
%
% See also: inpolygon, inpoly, insidepoly_dblengine, insidepoly_sglengine
%
% Acknowlegment: The idea of sorting coordinates is first implemented by
% Darren Engwirda, http://www.mathworks.com/matlabcentral/fileexchange/10391
%
% Author: Bruno Luong <brunoluong@yahoo.com>
% History:
%     Original: 06-Jun-2010

% Processing the argument list

% Default options
options = struct('tol', 'auto', ...
                 'presortflag', 'auto');

optionloc = cellfun('isclass',varargin,'char');
if any(optionloc)
    optionloc = find(optionloc,1,'first');
    mainargin = varargin(1:optionloc-1);
    % retreive property/value pairs
    for k=optionloc:2:nargin-1
        options.(varargin{k})= varargin{k+1};
    end
else
    mainargin = varargin;
end

edges = NaN;
if length(mainargin)<=3
    [xy P] = deal(varargin{1:2});
    x = xy(:,1);
    y = xy(:,2);
    if length(mainargin)==3 % edge provided
        edges = varargin{3};
    end
    Px = P(:,1);
    Py = P(:,2);
else
    [x y Px Py] = deal(varargin{1:4});
     if length(mainargin)==5 % edge provided
        edges = varargin{5};
     end
end

edgeprovided = ~isnan(edges);

% Orginal size of the data
sz = size(x);

% Reshape in columns
Px = Px(:);
Py = Py(:);
x = x(:);
y = y(:);

% Cast to double of one of them is (required by Mex)
isxdbl = isa(x,'double');
isydbl = isa(y,'double');
isPxdbl = isa(Px,'double');
isPydbl = isa(Py,'double');
doubleengine = isxdbl || isydbl || isPxdbl|| isPydbl;
if doubleengine
    if ~isxdbl, x = double(x); end
    if ~isydbl, y = double(y); end
    if ~isPxdbl, Px = double(Px); end
    if ~isPydbl, Py = double(Py); end
end

Pxmin = min(Px);
Pxmax = max(Px);
Pymin = min(Py);
Pymax = max(Py);

% Select the tolerance for determining on-boundary points
if isequal(options.tol,'auto')
    ontol = 1e-9*max(Pxmax-Pxmin,Pymax-Pymin);
else
    ontol = options.tol;
end

if ~edgeprovided
    % We don't want duplicate end vertices
    if Px(1)==Px(end) && Py(1)==Py(end)
        Px(end) = [];
        Py(end) = [];
    end
    % Linear wrap around the points
    Px1 = Px;
    Py1 = Py;
    Px2 = Px([2:end 1]);
    Py2 = Py([2:end 1]);
else
    Px1 = Px(edges(:,1),:);
    Py1 = Py(edges(:,1),:);
    Px2 = Px(edges(:,2),:);
    Py2 = Py(edges(:,2),:);
end

% Filter data outside the rectangular box
inpoly = x>=Pxmin-ontol & x<=Pxmax+ontol & ...
         y>=Pymin-ontol & y<=Pymax+ontol;
x = x(inpoly);
y = y(inpoly);

if isequal(options.presortflag,'auto')
    n = size(x,1);
    m = size(Px1,1);
    % We don't do presorting for small size polygon or small size data
    % Empirical law from experimental tests (Bruno)
    presortflag = (m>32) && (n*m > 25000);
else
    presortflag = options.presortflag;
end

if presortflag
    % Sort the array in x, and find the brackets. This is used to find
    % easily which data points have abscissa fall into the abcissa bracket
    % of each edge of the polygon.
    [x ix first last] = presort(Px1, Px2, x);
    y = y(ix); % arrange y in the same order

    % Call mex engine
    if doubleengine
        [in on] = insidepoly_dblengine(x, y, Px1, Py1, Px2, Py2, ontol, ...
                                       first, last);
    else % single arrays
        [in on] = insidepoly_sglengine(x, y, Px1, Py1, Px2, Py2, ontol, ...
                                       first, last);
    end

    % Restore the original order
    in(ix) = in;
    on(ix) = on;
else % No presorting
    % Call mex engine, without presorting
    if doubleengine
        [in on] = insidepoly_dblengine(x, y, Px1, Py1, Px2, Py2, ontol);
    else % single arrays
        [in on] = insidepoly_sglengine(x, y, Px1, Py1, Px2, Py2, ontol);
    end
end

in = in | on;

if nargout>=2
    onboundary = inpoly;
    onboundary(inpoly) = on;
    % Reshape to original size
    onboundary = reshape(onboundary, sz);
end

inpoly(inpoly) = in;
% Reshape to original size
inpoly = reshape(inpoly, sz);

end % insidepoly

%%
function [xsorted ix first last] = presort(Px1, Px2, x)
% (Px1, Px2) abscissa of vertices, x abscissa of data, they are supposed
% to be ranged in column.
% Return:
%   xsorted as sort(x) = x(ix)
%   "first" & "last" indexes such that, for each vertice point
%       min(Px1,Px2) <= xsorted(first:last) <= max(Px1,Px2)

% left and right brackets of the segment
Pmin = min(Px1,Px2);
Pmax = max(Px1,Px2);

nvertices = size(Px1,1);

% We seek to see how x interveaves with Pmin by sorting the ensemble
[trash is] = sort([Pmin; x],1); %#ok
isdata = is>nvertices; % tail index, i.e., belong to data abscissa x
anchor = find(~isdata);
% Get the sorted data alone
ix = is(isdata)-nvertices;
xsorted = x(ix); % sorted x
% Index of the first element in xsorted such that
%   xsorted(first)>=sort(Pmin)
first = anchor-(0:nvertices-1).';
% Rearrange first corresponds to the original order
ip = is(anchor);
first(ip) = first;

% determine how Pmax interleaves with xsorted, i.e.,
% index of the last element in xsorted such that xsorted(first)<=Pmax
% Note: in case of draw in binning edges, HISTC must return the last edge
[trash last] = histc(Pmax, [xsorted; inf]); %#ok

end % presort


function [B,twom] = multicatbipartite_f(A,gamma,omega)
% MULTICATBIPARTITE_F  returns multilayer Barber modularity matrix for unordered undirected bipartite networks, function handle version
%
% Version: 2.2.0
% Date: Thu 11 Jul 2019 12:25:42 CEST
%
% MULTICATBIPARTITE_F [B,twomu] = MULTICATBIPARTITE_F(A,gamma,omega)
%
% Input: A: Cell array of MxN adjacency matrices for each layer of a
%           multilayer undirected bipartite network
%        gamma: resolution parameter
%        omega: interlayer coupling strength
%
% Output: B: function handle where B(i) returns the ith column of the
%            [(M+N)xT]x[(M+N)xT] flattened modularity tensor for the
%            multilayer bipartite network with uniform ordinal coupling (T
%            is the number of layers of the network)
%         twomu: normalisation constant
%
% Usage: [B,twomu]=multicatbipartite_f(A,gamma,omega);
%        [S,Q]=genlouvain(B); % see iterated_genlouvain.m and
%          postprocess_categorical_multilayer.m for how to improve output
%          multilayer partition
%        Q=Q/twom;
%        S=reshape(S,M+N,T);
%
%  [B,twom] = MULTICATBIPARTITE_F(A,GAMMA, OMEGA) with A a cell array of
%   matrices of equal size each representing an undirected bipartite network
%   "layer" computes the multilayer Barber modularity matrix using the quality
%   function described in Mucha et al. 2010, with intralayer resolution
%   parameter GAMMA, and with interlayer coupling OMEGA connecting all-to-all
%   layers. Once the mulilayer modularity matrix is computed,
%   optimization can be performed by the generalized Louvain code GENLOUVAIN
%   or ITERATED_GENLOUVAIN. The  output B can be used with other heuristics,
%   provided the same mapping is used to go from the multilayer tensor to
%   the multilayer flattened matrix. That is, the node-layer tuple (i,s)
%   is mapped to i + (s-1)*(M+N). [Note that we can define a mapping between
%   a multilayer partition S_m stored as an (M+N) by T matrix and the
%   corresponding flattened partition S stored as an MNT by 1 vector. In
%   particular S_m = reshape(S,M+N,T) and S = S_m(:). Note that nodes i=1:M
%   correspond to the first class (i.e. the rows of A) and nodes i=M+1:M+N
%   correspond to the second class (i.e. the columns of A) of the bipartite
%   network.]
%
%   Notes:
%     The matrices in the cell array A are assumed to be of equal size.
%     This assumption is not checked here.
%
%     For smaller systems, it is potentially more efficient (and easier) to
%     directly use the sparse quality/modularity matrix B in MULTICATBIPARTITE.
%
%     This code serves as a template and can be modified for situations
%     with other wrinkles (e.g., different intralayer null models,
%     different numbers of nodes from layer-to-layer, or systems which are
%     both multiplex and longitudinal).  That is, this code is only a
%     starting point; it is by no means exhaustive.
%
%     By using this code, the user implicitly acknowledges that the authors
%     accept no liability associated with that use.  (What are you doing
%     with it anyway that might cause there to be a potential liability?!?)
%
% References:
%       Barber, M. Modularity and community detection in bipartite networks.
%           Phys. Rev. E 76, 066102 (2007).
%
%       Mucha, P. J., Richardson, T., Macon, K., Porter, M. A. & Onnela, J.-P.
%           Community structure in time-dependent, multiscale, and multiplex networks.
%           Science 328, 876-878 (2010).

if nargin<2||isempty(gamma)
    gamma=1;
end

if nargin<3
	omega=1;
end

[m,n]=size(A{1});
N=m+n;
T=length(A);

if length(gamma)==1
    gamma=repmat(gamma,T,1);
end

k=zeros(m,T);
d=zeros(T,n);
mm=zeros(T,1);

twom=0;
for j=1:T
    twom = twom + sum(sum(A{j}));
    k(:,j)=sum(A{j},2);
    d(j,:)=sum(A{j});
    mm(j)=sum(k(:,j));
end

%interslice connections
all2all= N*[(-T+1):-1,1:(T-1)];
C=omega*spdiags(ones(N*T,2*T-2),all2all,N*T,N*T);

%bipartite modularity matrix
    function modi=modf(i)

        s=ceil(i/(N+eps));
        if mm(s)~=0
            ii=i-(s-1)*N;
            if ii<=m
                indx=(m+1:N)+(s-1)*N;
                v=A{s}(ii,:)-gamma(s)*k(ii,s)*d(s,:)/mm(s);

                modi=sparse(indx,1,v,N*T,1,n+2);
            else
                indx=(1:m)+(s-1)*N;
                v=A{s}(:,ii-m)-gamma(s)*k(:,s)*d(s,ii-m)/mm(s);

                modi=sparse(indx,1,v,N*T,1,m+2);
            end

            modi=modi+C(:,i);
        else
            modi=C(:,i);

        end
    end

B=@modf;
twom=2*twom+2*N*(T-1)*T*omega;
end


function [source] = estimate_fwhm2(source, maxdist)

% ESTIMATE_FWHM2(SOURCE, MAXDIST)
%
% This function computes the Gaussian fwhm of the spatial filters, according to
% least-squares Gaussian fit including data points up until MAXDIST from the
% locations of interest.
%
% This function can only deal with scalar filters.

if nargin<2, maxdist = 2.5; end % maxdist should be in units of the pos in source
if isempty(maxdist), maxdist = inf; end

if islogical(source.inside)
  inside = find(source.inside);
else
  inside = source.inside;
end
ninside = numel(inside);

if ~isfield(source.avg, 'filter')
  ft_error('the input should contain spatial filters in');
end

nchan   = size(source.avg.filter{inside(1)},2);
ndir    = size(source.avg.filter{inside(1)},1);
if ndir~=1,
  ft_error('only scalar filters are allowed as input');
end

%get filters and positions
filter = cat(1,source.avg.filter{inside});
pos    = source.pos(inside,:);

%get the filter correlation matrix
Cmat   = filter*filter';
Cmat   = abs(Cmat)./sqrt(diag(Cmat)*diag(Cmat)');

fwhm    = zeros(size(source.pos,1),1);
onesvec = ones(ninside,1);
for k = 1:ninside
 d   = sqrt(sum( (pos-pos(k*onesvec,:)).^2, 2));
 sel = d<=maxdist;
 s   = gaussfit(Cmat(sel,k)',d(sel)');
 fwhm(inside(k)) = s;
end
source.fwhm = fwhm;

% fwhm    = zeros(size(source.pos,1),3,3);
% onesvec = ones(ninside,1);
% for k = 1:ninside
%   dpos = pos-pos(k*onesvec,:);
%   d   = sqrt(sum(dpos.^2, 2));
%   sel = d<=maxdist;
%   [s,dum] = gaussfit3D(dpos(sel,:), Cmat(sel,k));
%   fwhm(inside(k),:,:) = s;
% end
% source.fwhm = fwhm;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [fwhm] = gaussfit(dat, design)

%this function performs a least-squares gaussian fit

%create independent variable
ivar = design.^2;
dat  = log(dat+eps);
beta = dat*ivar'*pinv(ivar*ivar');

sigma = sqrt(-0.5./beta(:,1));
fwhm  = 2.*sqrt(2.*log(2)).*sigma;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [invsigma, R] = gaussfit3D(x, y)

% function for a linear least squares fit of a Gaussian function in 3D
% (with a peak of 1 at (0,0,0)).
%
% use as
%   sigma = gaussfit3D(x, y)
%
% x = Nx3 and y = Nx1, sigma is the covariance describing the gaussian

% create design matrix based in the independent variable x
design = [x(:,1).^2 x(:,2).^2 x(:,3).^2 2.*(x(:,1).*x(:,2)) 2.*(x(:,1).*x(:,3)) 2.*(x(:,2).*x(:,3))];
%design = design-repmat(mean(design,1),[size(design,1) 1]);
design = cat(2, design, ones(size(design,1),1));

% log-transform the dependent variable y
dat = -2.*log(y+eps);

% regression
beta = design\dat;

% residuals
res  = dat - design*beta;
R    = 1-sum(res.^2)./sum(dat.^2);

% create output
invsigma = [beta(1) beta(4) beta(5);beta(4) beta(2) beta(6);beta(5) beta(6) beta(3)];
%sigma = inv(invsigma);

% by construction the exponent to the gaussian looks like this
%
% exp( -0.5.*(-x'*invsigma*x) )
%
% -x'*invsigma*x = [x1 x2 x3]*[a d e [x1
%                              d b f  x2
%                              e f c] x3] =
%
% a*x1^2+b*x2^2+c*x3^2+2d*(x1x2)+2e*(x1x3)+2f*(x2x3)
%
% the variables a through f correspond with the ordered beta weights


function V = est_calcInvCovMatFourier(Rinv,E,foi,fs,M,p, verb)
%
% Obtain the frequency domain transform of the (inverse) covariance matrix
% of an M-variate VAR[p] process.
%
% Let
% This is needed for calculating the renormalized PDC [2]
%
% Inputs:
%
%     Rinv: inverse process covariance matrix obtained from est_calcInvCovMat
%     E:    noise covariance matrix
%     foi:  frequencies of interest (Hz)
%     fs:   sampling rate
%     M:    # chans
%     p:    model order
%     verb: verbosity level. 0 = no output, 1=text.
%
% Outputs:
%
%     V:    Frequency-domain transform of the inverse covariance matrix
%
% References:
%
% [1] Lutkepohl, H. (2007) New Introduction to Time Series Analysis. Springer.
% [2] Schelter et al, (2009). Assessing the strength of directed influences
% among neural signals using renormalized partial directed coherence.
% Journal of Neuroscience Methods. 179:121-130.
%
% See Also: est_calcInvCovMat()
%
% Author: Tim Mullen 2010, SCCN/INC, UCSD
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

DEBUG = 0;

%% extract the diagonal elements of H=Rinv
% structure of Hd is (e.g., for p=2):
% [diag(H(1,1)), diag(H(2,1)), diag(H(1,2)), diag(H(2,2))]
% where diag(H(u,v)) is the column vector formed by the diagonal of
% submatrix H(u,v) of H
Hd = zeros(M,p^2);
cnt=1;
for v=1:p
    for u=1:p
        Hd(:,cnt)=diag(Rinv((u-1)*M+1:u*M,(v-1)*M+1:v*M));
        cnt=cnt+1;
    end
end

if DEBUG
    for j=1:N
        try chol(reshape(Hd(j,:),[p p])); catch; fprintf('Hj<0: j=%d - ',j); keyboard; end;
    end
end

%% create u,v index vectors
% us = [1 2 ... p 1 2 ... p ... ]'   (p^2 length)
% vs = [1 1 ... 1 2 2 ... 2 ... ]'   (p^2 length)
us = repmat(1:p,1,p)';
vs = zeros(p^2,1);
for ii=1:p
    vs((ii-1)*p+1:(ii-1)*p+p)=ones(p,1)*ii;
end

%% construct the V matrix for all freqs
fi=0;
COS=zeros(p^2,4);
freqs=(2*pi*foi)/fs;
V = zeros(length(freqs),M,M,2,2); % NOTE: V will end up (M,M,freqs,2,2)
if verb, h=waitbar(0,'calculating V^-1...'); end
for f=freqs
    fi=fi+1;

    %% construct cosine matrix
    COS(:,1) = cos(us*f).*cos(vs*f);
    COS(:,2) = sin(us*f).*cos(vs*f);
    COS(:,3) = cos(us*f).*sin(vs*f);
    COS(:,4) = sin(us*f).*sin(vs*f);

    %% multiply Hd and COS matrices to get matrix where row j is
    % [sum_{u,v=1 : p} Hjj(u,v)COS_11(u,v), ...
    %  sum_{u,v=1 : p} Hjj(u,v)COS_21(u,v), ...
    %  sum_{u,v=1 : p} Hjj(u,v)COS_12(u,v), ...
    %  sum_{u,v=1 : p} Hjj(u,v)COS_22(u,v)]
    %
    % where COSab(u,v) is the a,bth element of the sine-cos transform
    % matrix evaluated at u,v.
    %
    % NOTE: reshaping row j to 2x2 yeilds a matrix proportional to V_ij(f)
    % for some specific i
    Vm = Hd*COS;

    % now we multiply in the variances of the i's (E(i,i)) to generate the
    % full V matrix
    Vm = kron(diag(E)',Vm);   % Vm = kron(diag(p^2*E)',Vm);

    % next, reshape to desired structure (NxNx2x2)
    V(fi,:,:,:,:) = permute(reshape(Vm',2,2,M,M),[3 4 1 2]);

    if verb, waitbar(fi/length(freqs),h); end
end % for freqs

% permute V: (chs,chs,freqs,2,2)
V = permute(V,[2 3 1 4 5]);
if verb, close(h); end


function [alpha, beta, gamma, loglik, xi, gamma2] = fwdback_twoslice(engine, init_state_distrib, transmat, obslik, varargin)
% FWDBACK Compute the posterior probs. in an HMM using the forwards backwards algo.
%
% [alpha, beta, gamma, loglik, xi, gamma2] = fwdback(init_state_distrib, transmat, obslik, ...)
%
% Notation:
% Y(t) = observation, Q(t) = hidden state, M(t) = mixture variable (for MOG outputs)
% A(t) = discrete input (action) (for POMDP models)
%
% INPUT:
% init_state_distrib(i) = Pr(Q(1) = i)
% transmat(i,j) = Pr(Q(t) = j | Q(t-1)=i)
%  or transmat{a}(i,j) = Pr(Q(t) = j | Q(t-1)=i, A(t-1)=a) if there are discrete inputs
% obslik(i,t) = Pr(Y(t)| Q(t)=i)
%   (Compute obslik using eval_pdf_xxx on your data sequence first.)
%
% Optional parameters may be passed as 'param_name', param_value pairs.
% Parameter names are shown below; default values in [] - if none, argument is mandatory.
%
% For HMMs with MOG outputs: if you want to compute gamma2, you must specify
% 'obslik2' - obslik(i,j,t) = Pr(Y(t)| Q(t)=i,M(t)=j)  []
% 'mixmat' - mixmat(i,j) = Pr(M(t) = j | Q(t)=i)  []
%
% For HMMs with discrete inputs:
% 'act' - act(t) = action performed at step t
%
% Optional arguments:
% 'fwd_only' - if 1, only do a forwards pass and set beta=[], gamma2=[]  [0]
% 'scaled' - if 1,  normalize alphas and betas to prevent underflow [1]
% 'maximize' - if 1, use max-product instead of sum-product [0]
%
% OUTPUTS:
% alpha(i,t) = p(Q(t)=i | y(1:t)) (or p(Q(t)=i, y(1:t)) if scaled=0)
% beta(i,t) = p(y(t+1:T) | Q(t)=i)*p(y(t+1:T)|y(1:t)) (or p(y(t+1:T) | Q(t)=i) if scaled=0)
% gamma(i,t) = p(Q(t)=i | y(1:T))
% loglik = log p(y(1:T))
% xi(i,j,t-1)  = p(Q(t-1)=i, Q(t)=j | y(1:T))
% gamma2(j,k,t) = p(Q(t)=j, M(t)=k | y(1:T)) (only for MOG  outputs)
%
% If fwd_only = 1, these become
% alpha(i,t) = p(Q(t)=i | y(1:t))
% beta = []
% gamma(i,t) = p(Q(t)=i | y(1:t))
% xi(i,j,t-1)  = p(Q(t-1)=i, Q(t)=j | y(1:t))
% gamma2 = []
%
% Note: we only compute xi if it is requested as a return argument, since it can be very large.
% Similarly, we only compute gamma2 on request (and if using MOG outputs).
%
% Examples:
%
% [alpha, beta, gamma, loglik] = fwdback(pi, A, multinomial_prob(sequence, B));
%
% [B, B2] = mixgauss_prob(data, mu, Sigma, mixmat);
% [alpha, beta, gamma, loglik, xi, gamma2] = fwdback(pi, A, B, 'obslik2', B2, 'mixmat', mixmat);

if nargout >= 5, compute_xi = 1; else compute_xi = 0; end
if nargout >= 6, compute_gamma2 = 1; else compute_gamma2 = 0; end

[obslik2, mixmat, fwd_only, scaled, act, maximize, compute_xi, compute_gamma2] = process_options(varargin, 'obslik2', [], 'mixmat', [], 'fwd_only', 0, 'scaled', 1, 'act', [], 'maximize', 0, 'compute_xi', compute_xi, 'compute_gamma2', compute_gamma2);

[Q T] = size(obslik);

if isempty(obslik2)
  compute_gamma2 = 0;
end

if isempty(act)
  act = ones(1,T);
  transmat = { transmat } ;
end

scale = ones(1,T);

% scale(t) = Pr(O(t) | O(1:t-1)) = 1/c(t) as defined by Rabiner (1989).
% Hence prod_t scale(t) = Pr(O(1)) Pr(O(2)|O(1)) Pr(O(3) | O(1:2)) = Pr(O(1), ... ,O(T))
% or log P = sum_t log scale(t).
% Rabiner suggests multiplying beta(t) by scale(t), but we can instead
% normalise beta(t) - the constants will cancel when we compute gamma.

loglik = 0;

alpha = zeros(Q,T);
gamma = zeros(Q,T);
if compute_xi
  xi = zeros(Q,Q,T-1);
else
  xi = [];
end

%%%%%%%%% Forwards %%%%%%%%%%

t = 1;
alpha(:,1) = init_state_distrib(:) .* obslik(:,t);
if scaled
  %[alpha(:,t), scale(t)] = normaliseC(alpha(:,t));
  [alpha(:,t), scale(t)] = normalise(alpha(:,t));
end
if scaled, assert(approxeq(sum(alpha(:,t)),1)), end
for t=2:T
  %trans = transmat(:,:,act(t-1))';
  trans = transmat{act(t-1)};
  if maximize
    m = max_mult(trans', alpha(:,t-1));
    %A = repmat(alpha(:,t-1), [1 Q]);
    %m = max(trans .* A, [], 1);
  else
    m = trans' * alpha(:,t-1);
  end
  alpha(:,t) = m(:) .* obslik(:,t);
  if scaled
    %[alpha(:,t), scale(t)] = normaliseC(alpha(:,t));
    [alpha(:,t), scale(t)] = normalise(alpha(:,t));
  end
  if compute_xi & fwd_only  % useful for online EM
    %xi(:,:,t-1) = normaliseC((alpha(:,t-1) * obslik(:,t)') .* trans);
    xi(:,:,t-1) = normalise((alpha(:,t-1) * obslik(:,t)') .* trans);
  end
  if scaled, assert(approxeq(sum(alpha(:,t)),1)), end
end
if scaled
  if any(scale==0)
    loglik = -inf;
  else
    loglik = sum(log(scale));
  end
else
  loglik = log(sum(alpha(:,T)));
end

if fwd_only
  gamma = alpha;
  beta = [];
  gamma2 = [];
  return;
end

%%%%%%%%% Backwards %%%%%%%%%%

beta = zeros(Q,T);
if compute_gamma2
  M = size(mixmat, 2);
  gamma2 = zeros(Q,M,T);
else
  gamma2 = [];
end

beta(:,T) = ones(Q,1);
%gamma(:,T) = normaliseC(alpha(:,T) .* beta(:,T));
gamma(:,T) = normalise(alpha(:,T) .* beta(:,T));
t=T;
if compute_gamma2
  denom = obslik(:,t) + (obslik(:,t)==0); % replace 0s with 1s before dividing
  gamma2(:,:,t) = obslik2(:,:,t) .* mixmat .* repmat(gamma(:,t), [1 M]) ./ repmat(denom, [1 M]);
  %gamma2(:,:,t) = normaliseC(obslik2(:,:,t) .* mixmat .* repmat(gamma(:,t), [1 M])); % wrong!
end
for t=T-1:-1:1
  b = beta(:,t+1) .* obslik(:,t+1);
  %trans = transmat(:,:,act(t));
  trans = transmat{act(t)};
  if maximize
    B = repmat(b(:)', Q, 1);
    beta(:,t) = max(trans .* B, [], 2);
  else
    beta(:,t) = trans * b;
  end
  if scaled
    %beta(:,t) = normaliseC(beta(:,t));
    beta(:,t) = normalise(beta(:,t));
  end
  %gamma(:,t) = normaliseC(alpha(:,t) .* beta(:,t));
  gamma(:,t) = normalise(alpha(:,t) .* beta(:,t));
  if compute_xi
    %xi(:,:,t) = normaliseC((trans .* (alpha(:,t) * b')));
    xi(:,:,t) = normalise((trans .* (alpha(:,t) * b')));
    %xi(:,:,t) = (trans .* (alpha(:,t) * b'));
  end
  if compute_gamma2
    denom = obslik(:,t) + (obslik(:,t)==0); % replace 0s with 1s before dividing
    gamma2(:,:,t) = obslik2(:,:,t) .* mixmat .* repmat(gamma(:,t), [1 M]) ./ repmat(denom, [1 M]);
    %gamma2(:,:,t) = normaliseC(obslik2(:,:,t) .* mixmat .* repmat(gamma(:,t), [1 M]));
  end
end

% We now explain the equation for gamma2
% Let zt=y(1:t-1,t+1:T) be all observations except y(t)
% gamma2(Q,M,t) = P(Qt,Mt|yt,zt) = P(yt|Qt,Mt,zt) P(Qt,Mt|zt) / P(yt|zt)
%                = P(yt|Qt,Mt) P(Mt|Qt) P(Qt|zt) / P(yt|zt)
% Now gamma(Q,t) = P(Qt|yt,zt) = P(yt|Qt) P(Qt|zt) / P(yt|zt)
% hence
% P(Qt,Mt|yt,zt) = P(yt|Qt,Mt) P(Mt|Qt) [P(Qt|yt,zt) P(yt|zt) / P(yt|Qt)] / P(yt|zt)
%                = P(yt|Qt,Mt) P(Mt|Qt) P(Qt|yt,zt) / P(yt|Qt)
%


function [ a, u, v, seed ] = r8sm_random ( m, n, seed )

%*****************************************************************************80
%
%% R8SM_RANDOM randomizes a R8SM matrix.
%
%  Discussion:
%
%    The R8SM storage format is used for an M by N Sherman Morrison matrix B,
%    which is defined by an M by N matrix A, an M vector U, and
%    an N vector V, by B = A - U * V'
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of the matrix.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(M,N), the R8SM matrix.
%
%    Output, real U(M), V(N), the R8SM vectors.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  for j = 1 : n
    for i = 1 : m
      [ a(i,j), seed ] = r8_uniform_01 ( seed );
    end
  end

  for i = 1 : m
    [ u(i), seed ] = r8_uniform_01 ( seed );
  end

  for j = 1 : n
    [ v(j), seed ] = r8_uniform_01 ( seed );
  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KUKA_KR5_SIXX_R650(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR5_SIXX_R650 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_KUKA_KR5_SIXX_R650 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('kuka', 'KR5_sixx_R650');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_kuka_kr5_sixx_r650(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=abs(d(6));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - eta;
q3(2) = pi - phi + eta;


function varargout = sin(varargin)
%SIN (overloaded)

switch class(varargin{1})

    case 'double'
        error('Overloaded SDPVAR/SIN CALLED WITH DOUBLE. Report error')

    case 'sdpvar'
        varargout{1} = InstantiateElementWiseUnitary(mfilename,varargin{:});
        %varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char'

        % General operator
        operator = struct('convexity','none',...
            'monotonicity','none',...
            'definiteness','none',...
            'model','callback');

        operator.bounds     = @bounds;
        operator.convexhull = @convexhull;
        operator.derivative = @(x)(cos(x));
        operator.range = [-1 1];
        operator.domain = [-inf inf];

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error('SDPVAR/SIN called with CHAR argument?');
end

function [L,U] = bounds(xL,xU)
if xU-xL >= 2*pi
    L = -1;
    U = 1;
else
    n = floor(( (xL + xU)/2/(2*pi)));
    xL = xL - n*2*pi;
    xU = xU - n*2*pi;
    yL = sin(xL);
    yU = sin(xU);
    L = min([yL yU]);
    U = max([yL yU]);
    if (xL<pi/2 & xU>pi/2) |  (xL<-3*pi/2 & xU>-3*pi/2)
        U = 1;
    end
    if (xL < 3*pi/2 & xU > 3*pi/2) | (xL < -pi/2 & xU > -pi/2)
        L = -1;
    end
end

function [Ax, Ay, b] = convexhull(xL,xU)
if sin(xL)>=0 & sin(xU)>=0 & xU-xL<pi
    xM = (xL+xU)/2;
    fL = sin(xL);
    fM = sin(xM);
    fU = sin(xU);
    dfL = cos(xL);
    dfM = cos(xM);
    dfU = cos(xU);
    [Ax,Ay,b] = convexhullConcave(xL,xM,xU,fL,fM,fU,dfL,dfM,dfU);
elseif sin(xL)<=0 & sin(xU)<=0 & xU-xL<pi
    fL = sin(xL);
    fU = sin(xU);
    dfL = cos(xL);
    dfU = cos(xU);
    [Ax,Ay,b] = convexhullConvex(xL,xU,fL,fU,dfL,dfU);
else
    [Ax,Ay,b] = convexhullGeneral(xL,xU,@sin);
end