%% FUNCTION Least_SRMTL
%   Sparse Structure-Regularized Learning with Least Squares Loss.
%
%% OBJECTIVE
%   argmin_W { sum_i^t (0.5 * norm (Y{i} - X{i}' * W(:, i))^2)
%            + rho1 * norm(W*R, 'fro')^2 + rho2 * \|W\|_1}
%
%% R encodes structure relationship
%   1)Structure order is given by using [1 -1 0 ...; 0 1 -1 ...; ...]
%    e.g.: R=zeros(t,t-1);R(1:(t+1):end)=1;R(2:(t+1):end)=-1;
%   2)Ridge penalty term by setting: R = eye(t)
%   3)All related regularized: R = eye (t) - ones (t) / t
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   R: regularization structure
%   rho1: structure regularization parameter
%   rho2: sparsity controlling parameter
%
%% OUTPUT
%   W: model: d * t
%   funcVal: function value vector.
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
% [1] Evgeniou, T. and Pontil, M. Regularized multi-task learning, KDD 2004
% [2] Zhou, J. Technical Report. http://www.public.asu.edu/~jzhou29/Software/SRMTL/CrisisEventProjectReport.pdf
%
%% RELATED FUNCTIONS
%  Logistic_SRMTL, init_opts

%% Code starts here
function [W, funcVal] = Least_SRMTL(X, Y, R, rho1, rho2, opts)

if nargin <5
    error('\n Inputs: X, Y, R, rho1, and rho2 should be specified!\n');
end
X = multi_transpose(X);

if nargin <6
    opts = [];
end

% initialize options.
opts=init_opts(opts);

if isfield(opts, 'rho_L2')
    rho_L2 = opts.rho_L2;
else
    rho_L2 = 0;
end

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];

% precomputation.
RRt = R * R';
XY = cell(task_num, 1);
W0_prep = [];
for t_idx = 1: task_num
    XY{t_idx} = X{t_idx}*Y{t_idx};
    W0_prep = cat(2, W0_prep, XY{t_idx});
end

% initialize a starting point
if opts.init==2
    W0 = zeros(dimension, task_num);
elseif opts.init == 0
    W0 = W0_prep;
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .W0');
        end
    else
        W0=W0_prep;
    end
end

bFlag=0; % this flag tests whether the gradient step only changes a little

Wz= W0;
Wz_old = W0;

t = 1;
t_old = 0;

iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ws = (1 + alpha) * Wz - alpha * Wz_old;

    % compute function value and gradients of the search point
    gWs  = gradVal_eval(Ws, rho1);
    Fs   = funVal_eval  (Ws, rho1);

    while true
        [Wzp l1c_wzp] = l1_projection(Ws - gWs/gamma, 2 * rho2 / gamma);
        Fzp = funVal_eval  (Wzp, rho1);

        delta_Wzp = Wzp - Ws;
        r_sum = norm(delta_Wzp, 'fro')^2;
        Fzp_gamma = Fs + trace(delta_Wzp' * gWs) + gamma/2 * norm(delta_Wzp, 'fro')^2;

        if (r_sum <=1e-20)
            bFlag=1; % this shows that, the gradient step makes little improvement
            break;
        end

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Wz_old = Wz;
    Wz = Wzp;

    funcVal = cat(1, funcVal, Fzp + rho2 * l1c_wzp);

    if (bFlag)
        % fprintf('\n The program terminates as the gradient step changes the solution very small.');
        break;
    end

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

W = Wzp;

% private functions

    function [z l1_comp_val] = l1_projection (v, beta)
        % this projection calculates
        % argmin_z = \|z-v\|_2^2 + beta \|z\|_1
        % z: solution
        % l1_comp_val: value of l1 component (\|z\|_1)

        z = zeros(size(v));
        vp = v - beta/2;
        z (v> beta/2)  = vp(v> beta/2);
        vn = v + beta/2;
        z (v< -beta/2) = vn(v< -beta/2);

        l1_comp_val = sum(sum(abs(z)));
    end

    function [grad_W] = gradVal_eval(W, rho1)

        if opts.pFlag
            grad_W = zeros(size(W));
            parfor t_ii = 1:task_num
                XWi = X{t_ii}' * W(:,t_ii);
                XTXWi = X{t_ii}* XWi;
                grad_W(:, t_ii) = XTXWi - XY{t_ii};
                %grad_W = cat(2, grad_W, X{t_ii}*(X{t_ii}' * W(:,t_ii)-Y{t_ii}) );
            end
        else
            grad_W = [];
            for t_ii = 1:task_num
                XWi = X{t_ii}' * W(:,t_ii);
                XTXWi = X{t_ii}* XWi;
                grad_W = cat(2, grad_W, XTXWi - XY{t_ii});
                %grad_W = cat(2, grad_W, X{t_ii}*(X{t_ii}' * W(:,t_ii)-Y{t_ii}) );
            end
        end
        grad_W = grad_W + rho1 * 2 *  W * RRt...
            + rho_L2 * 2 * W;
    end

    function [funcVal] = funVal_eval (W, rho1)
        funcVal = 0;
        if opts.pFlag
            parfor i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        else
            for i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        end
        funcVal = funcVal + rho1 * norm(W*R, 'fro')^2 ...
            + rho_L2 * norm(W, 'fro')^2;
    end

end


function hyperDemo_detectors
% HYPERDEMO_DETECTORS Demonstrates target detector algorithms
clear; clc; dbstop if error; close all;
%--------------------------------------------------------------------------
% Parameters
resultsDir = 'results\\';
dataDir = 'data\\AVIRIS\\';
%--------------------------------------------------------------------------

mkdir(resultsDir);

% Read part of AVIRIS data file that we will further process
M = hyperReadAvirisRfl(sprintf('%s\\f970620t01p02_r03_sc02.a.rfl', dataDir), [1 100], [1 614], [1 224]);
M = hyperNormalize(M);

% Read AVIRIS .spc file
lambdasNm = hyperReadAvirisSpc(sprintf('%s\\f970620t01p02_r03.a.spc', dataDir));

% Isomorph
[h, w, p] = size(M);
M = hyperConvert2d(M);

% Resample AVIRIS image.
desiredLambdasNm = 400:(2400-400)/(224-1):2400;
M = hyperResample(M, lambdasNm, desiredLambdasNm);

% Remove low SNR bands.
goodBands = [10:100 116:150 180:216];
M = M(goodBands, :);
p = length(goodBands);

% Demonstrate difference spectral similarity measurements
M = hyperConvert3d(M, h, w, p);
target = squeeze(M(11, 77, :));
figure; plot(desiredLambdasNm(goodBands), target); grid on;
    title('Target Signature; Pixel (32, 257)');

M = hyperConvert2d(M);

% RX Anomly Detector
r = hyperRxDetector(M);
r = hyperConvert3d(r.', h, w, 1);
figure; imagesc(r); title('RX Detector Results'); axis image;
    colorbar;
hyperSaveFigure(gcf, sprintf('%s\\rx detector.png', resultsDir));

% Constrained Energy Minimization (CEM)
r = hyperCem(M, target);
r = hyperConvert3d(r, h, w, 1);
figure; imagesc(abs(r)); title('CEM Detector Results'); axis image;
    colorbar;
hyperSaveFigure(gcf, sprintf('%s\\cem detector.png', resultsDir));

% Adaptive Cosine Estimator (ACE)
r = hyperAce(M, target);
r = hyperConvert3d(r, h, w, 1);
figure; imagesc(r); title('ACE Detector Results'); axis image;
    colorbar;
hyperSaveFigure(gcf, sprintf('%s\\ace detector.png', resultsDir));

% Signed Adaptive Cosine Estimator (S-ACE)
r = hyperSignedAce(M, target);
r = hyperConvert3d(r, h, w, 1);
figure; imagesc(r); title('Signed ACE Detector Results'); axis image;
    colorbar;
hyperSaveFigure(gcf, sprintf('%s\\signed ace detector.png', resultsDir));

% Matched Filter
r = hyperMatchedFilter(M, target);
r = hyperConvert3d(r, h, w, 1);
figure; imagesc(r); title('MF Detector Results'); axis image;
    colorbar;
hyperSaveFigure(gcf, sprintf('%s\\mf detector.png', resultsDir));

% Generalized Likehood Ratio Test (GLRT) detector
r = hyperGlrt(M, target);
r = hyperConvert3d(r, h, w, 1);
figure; imagesc(r); title('GLRT Detector Results'); axis image;
    colorbar;
hyperSaveFigure(gcf, sprintf('%s\\cem detector.png', resultsDir));

% Estimate background endmembers
U = hyperAtgp(M, 5);

% Hybrid Unstructured Detector (HUD)
r = hyperHud(M, U, target);
r = hyperConvert3d(r, h, w, 1);
figure; imagesc(abs(r)); title('HUD Detector Results'); axis image;
    colorbar;
hyperSaveFigure(gcf, sprintf('%s\\hud detector.png', resultsDir));

% Adaptive Matched Subspace Detector (AMSD)
r = hyperAmsd(M, U, target);
r = hyperConvert3d(r, h, w, 1);
figure; imagesc(abs(r)); title('AMSD Detector Results'); axis image;
    colorbar;
hyperSaveFigure(gcf, sprintf('%s\\amsd detector.png', resultsDir));
figure; mesh(r); title('AMSD Detector Results');

% Orthogonal Subspace Projection (OSP)
r = hyperOsp(M, U, target);
r = hyperConvert3d(r, h, w, 1);
figure; imagesc(abs(r)); title('OSP Detector Results'); axis image;
    colorbar;
hyperSaveFigure(gcf, sprintf('%s\\osp detector.png', resultsDir));


function obj_val = test_binary_classifier(objective_function,classf, ...
				  prior,system,input_data)
% Returns the result of the objective function evaluated on the
% scores.
%
% Inputs:
%   objective_function: a function handle to the objective function
%                       to feed the scores into
%   classf: length T vector where T is the number of trials with entries +1 for target scores; -1
%           for non-target scores
%   prior: the prior (given to the system that produced the scores)
%   system: a function handle to the system to be run
%   input_data: the data to run the system on (to produce scores)
%
% Outputs
%   obj_val: the value returned by the objective function

if nargin==0
    test_this();
    return;
end

scores = system(input_data);
obj_val = evaluate_objective(objective_function,scores,classf,prior);

end

function test_this()

num_trials = 100;
input_data = randn(20,num_trials);

prior = 0.5;
maxiters = 1000;
classf = [ones(1,num_trials/2),-ones(1,num_trials/2)];
tar = input_data(:,1:num_trials/2);
non = input_data(:,num_trials/2+1:end);
[sys,run_sys,w0] = linear_fusion_factory(tar,non);

w = train_binary_classifier(@cllr_obj,classf,sys,[],w0,[],maxiters,[],prior,[],true);

system = @(data) run_sys(w,data);
test_binary_classifier(@cllr_obj,classf,prior,system,input_data)

end


classdef CSMaster < handle
    %CS_LAB superclass
    %
    % (c) Thomas Kuestner
    % ---------------------------------------------------------------------

    properties
        type % reconstruction type
        kSpace % save kSpace in class to reduce memory overhead
        iNOUTER % outer loop iterations
        iNINNER % inner loop iterations
        p % L-p norm for reweighted minimum norm
        lambda % lagrange multiplier for noise stability
        lambdaCalib % lagrange multiplier for calibration
        lambdaTV % lagrange multiplier for TV
        lambdaESPReSSo % lagrange multiplier for ESPReSSo constraint
        lambdaMC % lagrange multiplier for motion correction
%         oversampling % oversampling along space directions
%         dimension % 2D (x-y or x-y-t), 3D (x,y,z) or 4D (x,y,z,t) problem
%         dim % nPha x nFreq x nZ x nTime x nCha
        measPara % struct containing all measurement parameters (dim, dimension, ...)
        kernelSize % kernel window dimension for convolution with calibration data
        calibSize % k-space center needed for initial estimate of W or GRAPPA calibration
        fullMask % mask of acquired data points
        currSlice % current slice, just needed for 2D with several slices
        currCha % current channel, just needed for 2D/3D with several channels
        trafo % struct of (tight frame) transformation in sparse basis
        espresso % espresso parameters
        window % cut out window options
        lSense % apply sense mask for multicoil reconstruction
        dRecontime % reconstruction time
    end

    methods
        function obj = CSMaster(iNOUTER, iNINNER, p, lambda, measPara)
            % constructor
            obj.iNOUTER = iNOUTER;
            obj.iNINNER = iNINNER;
            obj.p       = p;
            obj.lambda  = lambda;
            obj.measPara = measPara;
        end
    end

end


function varargout=bss_decomp_filt(varargin)

% decompose an estimated source into target/interference/noise/artefacts components, assuming the admissible distortion is a pure time-invariant filter.
% components, assuming the admissible distortion is a pure time-invariant
% filter.
%
% Usage:
%
% [s_target,e_interf[,e_noise],e_artif]=bss_decomp_filt(se,index,S[,N],L)
%
% Input:
%   - se: row vector of length T containing the estimated source,
%   - index: points which component of S se has to be compared to,
%   - S: n x T matrix containing the original sources,
%   - N: m x T matrix containing the noise on the observations (if any).
%   - L: the number of lags of the allowed filter
%
% Output:
%   - s_target: row vector of length T containing the target source(s)
%   contribution,
%   - e_interf: row vector of length T containing the interferences
%   contribution,
%   - e_noise: row vector of length T containing the noise contribution (if
%   any),
%   - e_artif: row vector of length T containing the artifacts
%   contribution.
%
% Developers:  - Cedric Fevotte (fevotte@tsi.enst.fr) - Emmanuel Vincent
% (emmanuel.vincent@irisa.fr) - Remi Gribonval (remi.gribonval@irisa.fr)

se=varargin{1}; index=varargin{2}; S=varargin{3};

switch nargin
    case 4
    N=[];
    L=varargin{4};
    case 5
    N=varargin{4};
    L=varargin{5};
    otherwise
    disp('Wrong number of arguments.')
end

[ne,Te]=size(se);
[n,T]=size(S);

%%%%%%%%%% WARNINGS %%%%%%%%%%%%%
switch isempty(N)
    case 1
        if n>T | ne>Te, disp('Watch out: signals must be in rows.'), return; end
        if ne~=1, disp('Watch out: se must contain only one row.'), return; end
        if T~=Te, disp('Watch out: se and S have different lengths.'), return; end
    case 0
        [m,Tm]=size(N);
        if n>T | ne>Te | m>Tm, disp('Watch out: signals must be in rows.'), return; end
        if ne~=1, disp('Watch out: se must contain only one row.'), return; end
        if T~=Te, disp('Watch out: S and Se have different lengths.'), return; end
        if T~=Tm, disp('Watch out: N, S and Se have different lengths.'), return; end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Create the space of target source(s)
target_space=bss_make_lags(S(index,:),L);
% Create the space of sources
sources_space=bss_make_lags(S,L);
% Create the noise space
noise_space=bss_make_lags(N,L);

s_target=zeros(1,T);
e_interf=zeros(1,T);
e_artif=zeros(1,T);
if isempty(noise_space)==0, e_noise=zeros(1,T); end

%%% Target source(s) contribution %%%
s_target = bss_proj(se,target_space);

%%% Interferences contribution %%%
P_S_se = bss_proj(se,sources_space);
e_interf = P_S_se - s_target;

switch isempty(noise_space)
    case 1 % No noise
        %%% Artifacts contribution %%%
        e_artif= se - P_S_se;

        %%% Output %%%
        varargout{1}=s_target;
        varargout{2}=e_interf;
        varargout{3}=e_artif;

    case 0 % Noise
        %%% Noise contribution %%%
        P_SN_se= bss_proj(se,[sources_space;noise_space]);
        e_noise=P_SN_se-P_S_se;

        %%% Artifacts contribution %%%
        e_artif=se-P_SN_se;

        %%% Output %%%
        varargout{1}=s_target;
        varargout{2}=e_interf;
        varargout{3}=e_noise;
        varargout{4}=e_artif;
end


function [BG, FG, L_hat, S_hat, T_hat, t_hat, ...
    P_track_full, T_calc]= ...
    MEDRoP(M1, P_init, mu, ev_thresh, alpha, K)
%%%This is the MEDRoP algorithm. This is the main function for the real video Background-
%%%Foreground separation problem.

%This folder contains the code accompanying pre-print.

%%%                          Inputs                         %%%
%%%     M - measurement matrix                              %%%
%%%     ev_thres - threshold for subspace change detection  %%%
%%%     P_init - an initial estimate of the subspace        %%%
%%%     t_train - the dimension of the training data        %%%

%%%                       Algorithm parameters              %%%
%%%     alpha - frame length                                %%%
%%%     mu - column-averages of data                        %%%
%%%     K - number of projection PCA steps                  %%%
%%%     omega - threshold for non-zero value in S           %%%

%%%                          Outputs                        %%%
%%%     BG - Estimated Background                           %%%
%%%     FG - Estimated Foreground                           %%%
%%%     L_hat - estimate of the low rank matrix             %%%
%%%     P_hat - estimate of the subspace in which L lies    %%%
%%%     S_hat - estimate of the sparse signal               %%%
%%%     t_hat - estimate of subspace change times           %%%

%% Initializations

P_hat_old = P_init;
[~, r] = size(P_init);
P_hat = P_hat_old;

[n, t_max] = size(M1);
T_hat = zeros(n, t_max);
S_hat = zeros(n, t_max);
L_hat = zeros(n, t_max);

FG = zeros(n, t_max);
BG = zeros(n, t_max);

t_hat = [];
M = M1 - repmat(mu, 1, t_max);
k = 0;
cnt = 1;
ph = 0;     %ph - 0 => detect, 1 => ppca
opts.delta = 0.4;

phi_t = speye(n) - P_hat * P_hat';
%% Main Algorithm Loop
for ii = 2 : t_max
        %% Estimate support
        Atf.times = @(x) x - (P_hat * (P_hat' * x));
        Atf.trans = @(y) y - (P_hat * (P_hat' * y));
        phi.times = @(x) x - (P_hat_old * (P_hat_old' * x));
        y_t = Atf.times(M(:, ii));
        opts.tol   = 1e-3;
        opts.print = 0;

        opts.delta = norm(Atf.times(L_hat(:, ii - 1)));

        x_t_hat_cs = yall1(Atf, y_t, opts);
        omega = sqrt(M(:, ii)' * M(:, ii) / n);

        t_hat_temp = find(abs(x_t_hat_cs) > omega);
        T_hat(t_hat_temp, ii) = 255;

        LS.times = @(x) phi(:, t_hat_temp) * x;
        LS.trans = @(y) phi(:, t_hat_temp)' * x;

        %% Estimate signal components

        S_hat(t_hat_temp, ii) = cgls(phi_t(:, t_hat_temp), y_t, 0, 1e-3);
        L_hat(:, ii) = M(:, ii) - S_hat(:, ii);
        FG(t_hat_temp, ii) = M1(t_hat_temp, ii);
        BG(:, ii) = L_hat(:, ii) + mu;
        %% Subspace update
        if(~mod(ii - 1 , alpha))
            MM = (1 / sqrt(alpha)) * phi.times(L_hat(:, ii - alpha + 1 : ii));

            if(~ph)     %%detect phase
                aa = svds(MM, 1);
                if(aa >= sqrt(ev_thresh))
                    ph = 1;
                    t_hat = [t_hat, ii];
                    k = 0;
                end
            else        %%ppca phase
                P_hat = simpleEVD((L_hat(:, ii - alpha + 1 : ii)), r);
                phi_t = speye(n) - P_hat * P_hat';
                k = k + 1;

                if(k==K + 1)
                    P_hat_old = P_hat;
                    k = 1;
                    ph = 0;
                    phi_t = speye(n) - P_hat * P_hat';
                end
            end
        end

    %% Return subspace estimates
    if((ii == 0) || (~(mod(ii - 1, alpha))))
        P_track_full{cnt} = P_hat;
        %P_track_new{cnt} = P_hat_new;
        T_calc(cnt) = ii;
        cnt = cnt + 1;
    end
end
%L_hat = L_hat + repmat(mu, n, 1);
end


function ActiveImportanceSampling = activeimportancesampling_vbmc(vp,gp,acqfun,acqinfo,options)
%ACTIVEIMPORTANCESAMPLING_VBMC Setup importance sampling acquisition functions

% This function samples from the base importance sampling (IS) density in
% three steps:
% 1) Use importance sampling-resampling (ISR) to sample from the
%    base IS density based on a proposal distribuion which is a mixture of
%    a smoothed variational posterior and on box-uniform mixture centered
%    around the current training points
% 2) Optionally use MCMC initialized with the ISR samples to sample from
%    the base IS density
% 3) Compute IS statistics and book-keeping

% Does the importance sampling step use the variational posterior?
isamplevp_flag = isfield(acqinfo,'importance_sampling_vp') ...
    && acqinfo.importance_sampling_vp;

% Do we simply sample from the variational posterior?
onlyvp_flag = isfield(acqinfo,'variational_importance_sampling') ...
    && acqinfo.variational_importance_sampling;

D = size(gp.X,2);

Ns_gp = numel(gp.post); % # GP hyperparameter samples

% Input space bounds and typical scales (for MCMC only)
widths = std(gp.X,[],1);
MaxBnd = 0.5;
diam = max(gp.X) - min(gp.X);
LB = min(gp.X) - MaxBnd*diam;
UB = max(gp.X) + MaxBnd*diam;

ActiveImportanceSampling.lnw = [];
ActiveImportanceSampling.Xa = [];
ActiveImportanceSampling.fs2a = [];

if onlyvp_flag
    %% Step 0: Simply sample from the variational posterior

    if ischar(options.ActiveImportanceSamplingMCMCSamples)
        K = vp.K;
        nvars = D;
        Na = ceil(eval(options.ActiveImportanceSamplingMCMCSamples));
    elseif isscalar(options.ActiveImportanceSamplingMCMCSamples)
        Na = ceil(options.ActiveImportanceSamplingMCMCSamples);
    else
        Na = 0;
    end

    if ~isfinite(Na) || ~isscalar(Na) || Na <= 0
        error('OPTIONS.ActiveImportanceSamplingMCMCSamples should be (or evaluate to) a positive integer.');
    end

    Xa = vbmc_rnd(vp,Na,0);
    [~,~,fmu,fs2] = gplite_pred(gp,Xa,[],[],1,0);

    if isfield(acqinfo,'mcmc_importance_sampling') && acqinfo.mcmc_importance_sampling

        % Compute fractional effective sample size (ESS)
        fESS = fess_vbmc(vp,fmu,Xa);

        % If fESS is less than thresh ==> major mismatch, do MCMC
        if fESS < options.ActiveImportanceSamplingfESSThresh
            Xa_old = Xa;

            if isamplevp_flag
                logpfun = @(x) log_isbasefun(x,acqfun,gp,vp);
            else
                logpfun = @(x) log_isbasefun(x,acqfun,gp,[]);
            end

            % Get MCMC options
            Nmcmc_samples = Na*options.ActiveImportanceSamplingMCMCThin;
            thin = 1;
            burnin = 0;
            sampleopts = get_mcmcopts([],thin,burnin);
            logPfuns = logpfun;
            W = Na;    % # walkers

            % Perform a single MCMC step for all samples
            Xa = eissample_lite(logPfuns,Xa,Nmcmc_samples,W,widths,LB,UB,sampleopts);
            Xa = Xa(end-Na+1:end,:);
            [~,~,fmu,fs2] = gplite_pred(gp,Xa,[],[],1,0);

            if 0
                hold off;
                scatter(Xa_old(:,1),Xa_old(:,2),'b'); hold on;
                scatter(Xa(:,1),Xa(:,2),'k'); hold on;
                drawnow;
            end
        end
    end

    if isamplevp_flag
        vlnpdf = max(vbmc_pdf(vp,Xa,0,1),log(realmin));
        lny = acqfun('islogf1',vlnpdf,[],[],fmu,fs2);
    else
        lny = acqfun('islogf1',[],[],[],fmu,fs2);
    end

    ActiveImportanceSampling.fs2a = fs2;
    ActiveImportanceSampling.lnw = lny';
    ActiveImportanceSampling.Xa = Xa;

else
    %% Step 1: Importance sampling-resampling

    Nvp_samples = options.ActiveImportanceSamplingVPSamples;
    Nbox_samples = options.ActiveImportanceSamplingBoxSamples;
    w_vp = Nvp_samples/(Nvp_samples + Nbox_samples);

    rect_delta = 2*std(gp.X);

    % Smoothed posterior for importance sampling-resampling
    if Nvp_samples > 0
        scale_vec = [0.05,0.2,1];
        %scale = sqrt(mean(var(gp.X,[],1)));

        vp_is = vp;
        for ii = 1:numel(scale_vec)
            vp_is.K = vp_is.K + vp.K;
            vp_is.w = [vp_is.w, vp.w];
            vp_is.mu = [vp_is.mu, vp.mu];
            vp_is.sigma = [vp_is.sigma, sqrt(vp.sigma.^2 + scale_vec(ii)^2)];
        end
        vp_is.w = vp_is.w/sum(vp_is.w);

        % Sample from smoothed posterior
        Xa_vp = vbmc_rnd(vp_is,Nvp_samples,0,0);
        [lnw,fs2a_vp] = activesample_proposalpdf(Xa_vp,gp,vp_is,w_vp,rect_delta,acqfun,vp,isamplevp_flag);
        ActiveImportanceSampling.lnw = [ActiveImportanceSampling.lnw, lnw'];
        ActiveImportanceSampling.Xa = [ActiveImportanceSampling.Xa; Xa_vp];
        ActiveImportanceSampling.fs2a = [ActiveImportanceSampling.fs2a; fs2a_vp];
    else
        vp_is = [];
    end

    % Box-uniform sampling around training inputs
    if Nbox_samples > 0
        jj = randi(size(gp.X,1),[1,Nbox_samples]);
        Xa_box = gp.X(jj,:) + bsxfun(@times,2*rand(numel(jj),D)-1,rect_delta);
        [lnw,fs2a_box] = activesample_proposalpdf(Xa_box,gp,vp_is,w_vp,rect_delta,acqfun,vp,isamplevp_flag);
        ActiveImportanceSampling.lnw = [ActiveImportanceSampling.lnw, lnw'];
        ActiveImportanceSampling.Xa = [ActiveImportanceSampling.Xa; Xa_box];
        ActiveImportanceSampling.fs2a = [ActiveImportanceSampling.fs2a; fs2a_box];
    end

    ActiveImportanceSampling.lnw(~isfinite(ActiveImportanceSampling.lnw)) = -Inf;
    % optimState.w = exp(optimState.lnw - max(optimState.lnw))';
    % optimState.w = optimState.w / sum(optimState.w);
    % 1./sum(optimState.w.^2)

    %% Step 2 (optional): MCMC sample

    Nmcmc_samples = options.ActiveImportanceSamplingMCMCSamples;

    if Nmcmc_samples > 0

        ActiveImportanceSampling_old = ActiveImportanceSampling;

        ActiveImportanceSampling.lnw = zeros(Ns_gp,Nmcmc_samples);
        ActiveImportanceSampling.Xa = zeros(Nmcmc_samples,D,Ns_gp);
        ActiveImportanceSampling.fs2a = zeros(Nmcmc_samples,Ns_gp);

        gp1 = gp;   % Consider only one GP sample at a time

        for s = 1:Ns_gp

            gp1.post = [];  % Assign current GP sample
            gp1.post = gp.post(s);

            if D == 2 && 0
                % We could use a quasi-random grid for D <= 2, but not implemented
                Xa = rand(Na,D).*(UB - LB) + LB;
                [~,~,fmu,optimState.fs2a] = gplite_pred(gp1,Xa,[],[],1,0);
                optimState.lnw = mean(fmu,2)';
                optimState.Xa = Xa;
            else
                if isamplevp_flag
                    logpfun = @(x) log_isbasefun(x,acqfun,gp1,vp);
                else
                    logpfun = @(x) log_isbasefun(x,acqfun,gp1,[]);
                end

                % Get MCMC options
                thin = options.ActiveImportanceSamplingMCMCThin;
                burnin = ceil(thin*Nmcmc_samples/2);
                sampleopts = get_mcmcopts(Nmcmc_samples,thin,burnin);

                logPfuns = logpfun;
                % sampleopts.TransitionOperators = {'transSliceSampleRD'};
                W = 2*(D+1);    % # walkers

                if 0
                    % Take starting points from high posterior density region
                    hpd_frac = 0.5;
                    N = numel(gp1.y);
                    N_hpd = min(N,max(W,round(hpd_frac*N)));
                    [~,ord] = sort(gp1.y,'descend');
                    X_hpd = gp1.X(ord(1:N_hpd),:);
                    x0 = X_hpd(randperm(N_hpd,min(W,N_hpd)),:);
                    x0 = bsxfun(@min,bsxfun(@max,x0,LB),UB);
                else
                    % Use importance sampling-resampling
                    [~,~,fmu,fs2] = gplite_pred(gp1,ActiveImportanceSampling_old.Xa,[],[],1,0);
                    lnw = ActiveImportanceSampling_old.lnw(s,:) + acqfun('islogf2',[],[],[],fmu,fs2)';
                    w = exp(bsxfun(@minus,lnw,max(lnw,[],2)));
                    x0 = zeros(W,D);
                    for ii = 1:W
                        idx = catrnd(w,1);
                        w(idx) = 0;
                        x0(ii,:) = ActiveImportanceSampling_old.Xa(idx,:);
                    end
                end

                [Xa,logp] = eissample_lite(logPfuns,x0,Nmcmc_samples,W,widths,LB,UB,sampleopts);
                [~,~,fmu,fs2] = gplite_pred(gp1,Xa,[],[],1,0);

                % Fixed log weight for importance sampling (log fixed integrand)
                if isamplevp_flag
                    vlnpdf = max(vbmc_pdf(vp,Xa,0,1),log(realmin));
                    lny = acqfun('islogf1',vlnpdf,[],[],fmu,fs2);
                else
                    lny = acqfun('islogf1',[],[],[],fmu,fs2);
                end
                % lny = lny - warpvars_vbmc(Xa,'logp',vp.trinfo);

                ActiveImportanceSampling.fs2a(:,s) = fs2;
                ActiveImportanceSampling.lnw(s,:) = bsxfun(@minus,lny',logp');
                ActiveImportanceSampling.Xa(:,:,s) = Xa;
            end

        end
    end
end

if 0
    hold off;
    scatter(ActiveImportanceSampling.Xa(:,1),ActiveImportanceSampling.Xa(:,2)); hold on;
    % scatter(x0(:,1),x0(:,2),'ro','MarkerFaceColor','r')
    xlim([-2,2]);
    ylim([-2,2]);
    drawnow;
end

%% Step 3: Pre-compute quantities for importance sampling calculations

% Precompute cross-kernel matrix on importance points
Kax_mat = zeros(size(ActiveImportanceSampling.Xa,1),size(gp.X,1),Ns_gp);
Ctmp_mat = zeros(size(gp.X,1),size(ActiveImportanceSampling.Xa,1),Ns_gp);
for s = 1:Ns_gp
    if size(ActiveImportanceSampling.Xa,3) == 1
        Xa = ActiveImportanceSampling.Xa;
    else
        Xa(:,:) = ActiveImportanceSampling.Xa(:,:,s);
    end
    hyp = gp.post(s).hyp;
    L = gp.post(s).L;
    Lchol = gp.post(s).Lchol;
    sn2_eff = 1/gp.post(s).sW(1)^2;
    if gp.covfun(1) == 1    % Hard-coded SE-ard for speed
        ell = exp(hyp(1:D))';
        sf2 = exp(2*hyp(D+1));
        Kax_tmp = sq_dist(Xa*diag(1./ell),gp.X*diag(1./ell));
        Kax_mat(:,:,s) = sf2 * exp(-Kax_tmp/2);
    else
        error('Other covariance functions not supported yet.');
    end

    if Lchol
        Ctmp_mat(:,:,s) = (L\(L'\Kax_mat(:,:,s)'))/sn2_eff;
    else
        Ctmp_mat(:,:,s) = (L*Kax_mat(:,:,s)');
    end
end
ActiveImportanceSampling.Kax_mat = Kax_mat;
ActiveImportanceSampling.Ctmp_mat = Ctmp_mat;

% Precompute integrated mean basis function on importance points
if isfield(gp,'intmeanfun') && gp.intmeanfun > 0
    plus_idx = gp.intmeanfun_var > 0;
    if size(ActiveImportanceSampling.Xa,3) == 1
        Ha = gplite_intmeanfun(ActiveImportanceSampling.Xa,gp.intmeanfun);
        ActiveImportanceSampling.Ha = Ha(plus_idx,:);
    else
        for s = 1:Ns_gp
            Ha = gplite_intmeanfun(ActiveImportanceSampling.Xa(:,:,s),gp.intmeanfun);
            ActiveImportanceSampling.Ha(:,:,s) = Ha(plus_idx,:);
        end
    end
end

end

%--------------------------------------------------------------------------
function [lnw,fs2] = activesample_proposalpdf(Xa,gp,vp_is,w_vp,rect_delta,acqfun,vp,isamplevp_flag)
%ACTIVESAMPLE_PROPOSALPDF Compute importance weights for proposal pdf

[N,D] = size(gp.X);
Na = size(Xa,1);

[~,~,fmu,fs2] = gplite_pred(gp,Xa,[],[],1,0);

Ntot = 1 + N; % Total number of mixture elements

if w_vp < 1; templpdf = zeros(Na,Ntot); end

% Mixture of variational posteriors
if w_vp > 0
    logflag = true;
    templpdf(:,1) = vbmc_pdf(vp_is,Xa,0,logflag) + log(w_vp);
else
    templpdf(:,1) = -Inf;
end

% Fixed log weight for importance sampling (log fixed integrand)
if isamplevp_flag
    vlnpdf = max(vbmc_pdf(vp,Xa,0,1),log(realmin));
    lny = acqfun('islogf1',vlnpdf,[],[],fmu,fs2);
else
    lny = acqfun('islogf1',[],[],[],fmu,fs2);
end
% lny = lny - warpvars_vbmc(Xa,'logp',vp.trinfo);

% Mixture of box-uniforms
if w_vp < 1
    VV = prod(2*rect_delta);

    for ii = 1:N
        templpdf(:,ii+1) = log(all(abs(bsxfun(@minus,Xa,gp.X(ii,:))) < rect_delta,2) / VV / N * (1 - w_vp));
    end

    mmax = max(templpdf,[],2);
    lpdf = log(sum(exp(templpdf - mmax),2));
    lnw = bsxfun(@minus,lny,lpdf + mmax);
else
    lnw = bsxfun(@minus,lny,templpdf);
end

end

%--------------------------------------------------------------------------
function y = log_isbasefun(x,acqfun,gp,vp)
%LOG_ISBASEFUN Base importance sampling proposal log pdf

[fmu,fs2] = gplite_pred(gp,x);
if isempty(vp)
    y = acqfun('islogf',[],[],[],fmu,fs2);
else
    vlnpdf = max(vbmc_pdf(vp,x,0,1),log(realmin));
    y = acqfun('islogf',vlnpdf,[],[],fmu,fs2);
end

end

%--------------------------------------------------------------------------
%SQ_DIST Compute matrix of all pairwise squared distances between two sets
% of vectors, stored in the columns of the two matrices, a (of size n-by-D)
% and b (of size m-by-D).
function C = sq_dist(a,b)

n = size(a,1);
m = size(b,1);
mu = (m/(n+m))*mean(b,1) + (n/(n+m))*mean(a,1);
a = bsxfun(@minus,a,mu); b = bsxfun(@minus,b,mu);
C = bsxfun(@plus,sum(a.*a,2),bsxfun(@minus,sum(b.*b,2)',2*a*b'));
C = max(C,0);

end

%--------------------------------------------------------------------------
function sampleopts = get_mcmcopts(Ns,thin,burnin)
%GET_MCMCOPTS Get standard MCMC options.

if nargin < 2 || isempty(thin); thin = 1; end
if nargin < 3; burnin = []; end

sampleopts.Thin = thin;
if isempty(burnin)
    sampleopts.Burnin = ceil(sampleopts.Thin*Ns/2);
else
    sampleopts.Burnin = burnin;
end
sampleopts.Display = 'off';
sampleopts.Diagnostics = false;
sampleopts.VarTransform = false;
sampleopts.InversionSample = false;
sampleopts.FitGMM = false;

end

%--------------------------------------------------------------------------
function x = catrnd(p,n)
%CATRND Sample from categorical distribution.

maxel = 1e6;
Nel = n*numel(p);
stride = ceil(maxel/numel(p));

cdf(1,:) = cumsum(p);
u = rand(n,1)*cdf(end);

% Split for memory reasons
if Nel <= maxel
    x = sum(bsxfun(@lt, cdf, u),2) + 1;
else
    x = zeros(n,1);
    idx_min = 1;
    while idx_min <= n
        idx_max = min(idx_min+stride-1,n);
        idx = idx_min:idx_max;
        x(idx) = sum(bsxfun(@lt, cdf, u(idx)),2) + 1;
        idx_min = idx_max+1;
    end
end

end


function [V, converged, i] = newtonpf_I_cart(Ybus, Sbus, V0, ref, pv, pq, mpopt)
%NEWTONPF_I_CART  Solves power flow using full Newton's method (current/cartesian)
%   [V, CONVERGED, I] = NEWTONPF_I_CART(YBUS, SBUS, V0, REF, PV, PQ, MPOPT)
%
%   Solves for bus voltages using a full Newton-Raphson method, using nodal
%   current balance equations and cartesian coordinate representation of
%   voltages, given the following inputs:
%       YBUS  - full system admittance matrix (for all buses)
%       SBUS  - handle to function that returns the complex bus power
%               injection vector (for all buses), given the bus voltage
%               magnitude vector (for all buses)
%       V0    - initial vector of complex bus voltages
%       REF   - bus index of reference bus (voltage ang reference & gen slack)
%       PV    - vector of bus indices for PV buses
%       PQ    - vector of bus indices for PQ buses
%       MPOPT - (optional) MATPOWER option struct, used to set the
%               termination tolerance, maximum number of iterations, and
%               output options (see MPOPTION for details).
%
%   The bus voltage vector contains the set point for generator
%   (including ref bus) buses, and the reference angle of the swing
%   bus, as well as an initial guess for remaining magnitudes and
%   angles.
%
%   Returns the final complex voltages, a flag which indicates whether it
%   converged or not, and the number of iterations performed.
%
%   See also RUNPF, NEWTONPF, NEWTONPF_S_CART, NEWTONPF_I_POLAR.

%   MATPOWER
%   Copyright (c) 1996-2019, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%   and Baljinnyam Sereeter, Delft University of Technology
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% default arguments
if nargin < 7
    mpopt = mpoption;
end

%% options
tol         = mpopt.pf.tol;
max_it      = mpopt.pf.nr.max_it;
lin_solver  = mpopt.pf.nr.lin_solver;

%% initialize
converged = 0;
i = 0;
V = V0;
Vm = abs(V);
Vmpv = Vm(pv);
n = length(V0);

%% set up indexing for updating V
npv = length(pv);
npq = length(pq);
j1 = 1;         j2 = npv;           %% j1:j2 - Q of pv buses
j3 = j2 + 1;    j4 = j2 + npq;      %% j3:j4 - Vr of pq buses
j5 = j4 + 1;    j6 = j4 + npv;      %% j5:j6 - Vr of pv buses
j7 = j6 + 1;    j8 = j6 + npq;      %% j7:j8 - Vi of pq buses
j9 = j8 + 1;    j10= j8 + npv;      %% j9:j10- Vi of pv buses

%% evaluate F(x0)
Sb = Sbus(Vm);
Sb(pv) = real(Sb(pv)) + 1j * imag(V(pv) .* conj(Ybus(pv, :) * V));
mis = Ybus * V - conj(Sb ./ V);
F = [   real(mis([pv; pq]));
        imag(mis([pv; pq]));
        V(pv) .* conj(V(pv)) - Vmpv.^2  ];

%% check tolerance
normF = norm(F, inf);
if mpopt.verbose > 1
    fprintf('\n it   max Ir & Ii mismatch (p.u.)');
    fprintf('\n----  ---------------------------');
    fprintf('\n%3d        %10.3e', i, normF);
end
if normF < tol
    converged = 1;
    if mpopt.verbose > 1
        fprintf('\nConverged!\n');
    end
end

%% attempt to pick fastest linear solver, if not specified
if isempty(lin_solver)
    nx = length(F);
    if nx <= 10 || have_feature('octave')
        lin_solver = '\';       %% default \ operator
    else    %% MATLAB and nx > 10 or Octave and nx > 2000
        lin_solver = 'LU3';     %% LU decomp with 3 output args, AMD ordering
    end
end

%% do Newton iterations
while (~converged && i < max_it)
    %% update iteration counter
    i = i + 1;

    %% evaluate Jacobian
    dImis_dQ = sparse(pv, pv, 1j./conj(V(pv)), n, n);
    dV2_dVr = sparse(1:npv, npq+(1:npv), 2*real(V(pv)), npv, npv+npq);
    dV2_dVi = sparse(1:npv, npq+(1:npv), 2*imag(V(pv)), npv, npv+npq);
    [dImis_dVr, dImis_dVi] = dImis_dV(Sb, Ybus, V, 1);

    %% handling of derivatives for voltage dependent loads
    %% (not yet implemented) goes here

    j11 = real(dImis_dQ([pv; pq], pv));
    j12 = real(dImis_dVr([pv; pq], [pq; pv]));
    j13 = real(dImis_dVi([pv; pq], [pq; pv]));
    j21 = imag(dImis_dQ([pv; pq], pv));
    j22 = imag(dImis_dVr([pv; pq], [pq; pv]));
    j23 = imag(dImis_dVi([pv; pq], [pq; pv]));
    j31 = sparse(npv, npv);
    j32 = dV2_dVr;
    j33 = dV2_dVi;

    J = [   j11 j12 j13;
            j21 j22 j23;
            j31 j32 j33;    ];

    %% compute update step
    dx = mplinsolve(J, -F, lin_solver);

    %% update voltage
    if npv
        V(pv) = V(pv) + dx(j5:j6) + 1j * dx(j9:j10);
        Sb(pv) = real(Sb(pv)) + 1j * (imag(Sb(pv)) + dx(j1:j2));
    end
    if npq
        V(pq) = V(pq) + dx(j3:j4) + 1j * dx(j7:j8);
    end

    %% evalute F(x)
    mis = Ybus * V - conj(Sb ./ V);
    F = [   real(mis([pv; pq]));
            imag(mis([pv; pq]));
            V(pv) .* conj(V(pv)) - Vmpv.^2  ];

    %% check for convergence
    normF = norm(F, inf);
    if mpopt.verbose > 1
        fprintf('\n%3d        %10.3e', i, normF);
    end
    if normF < tol
        converged = 1;
        if mpopt.verbose
            fprintf('\nNewton''s method power flow (current balance, cartesian) converged in %d iterations.\n', i);
        end
    end
end

if mpopt.verbose
    if ~converged
        fprintf('\nNewton''s method power flow (current balance, cartesian) did not converge in %d iterations.\n', i);
    end
end


% This is a test script which demonstrates the usage of the "MofN_TrackInitiatorX" class.
% =========================================================================>

% Load the ground truth data
load('multiple-robot-tracking.mat');

% Plot settings
ShowPlots = 1;              % Set to 0 to hide plots
numTrueTracks = 3;

% Model parameter shortcuts
lambdaV = 10; % Expected number of clutter measurements over entire surveillance region
V = 10^2;     % Volume of surveillance region (10x10 2D-grid)
V_bounds = [0 10 0 10]; % [x_min x_max y_min y_max]
P_D = 0.9;    % Probability of detection
timestep_duration = duration(0,0,1);

%% Models
transition_model = ConstantVelocityX('VelocityErrVariance', 0.0001,...
                                     'NumDims', 2,...
                                     'TimestepDuration', timestep_duration);
measurement_model = LinearGaussianX('NumMeasDims', 2,...
                                    'NumStateDims', 4,...
                                    'MeasurementErrVariance', 2,...
                                    'Mapping', [1 3]);
clutter_model = PoissonRateUniformPositionX('ClutterRate',lambdaV,...
                                            'Limits',[V_bounds(1:2);...
                                                      V_bounds(3:4)]);
detection_model = ConstantDetectionProbabilityX('DetectionProbability',P_D);

% Compile the State-Space model
model = StateSpaceModelX(transition_model,measurement_model,'Clutter',clutter_model, 'Detection', detection_model);

%% Generate DataList
meas_simulator = MultiTargetMeasurementSimulatorX('Model',model);
% meas_simulator.DetectionProbability = 1;
DataList = meas_simulator.simulate(GroundTruthStateSequence);
N = numel(DataList);

%% Base Filter
obs_covar= measurement_model.covar();
PriorState = GaussianStateX(zeros(4,1), transition_model.covar() + blkdiag(obs_covar(1,1), 0, obs_covar(2,2),0));
base_filter = KalmanFilterX('Model', model, 'StatePrior', PriorState);

%% Data Associator
config.ClutterModel = clutter_model;
config.Clusterer = NaiveClustererX();
config.Gater = EllipsoidalGaterX(2,'GateLevel',10)';
config.DetectionModel = detection_model;
jpdaf = JointProbabilisticDataAssocX(config);

%% Track Initiator

% Initiate Data Associator
config_pdaf.ClutterModel = clutter_model;
config_pdaf.DetectionModel = detection_model;
config_pdaf.Gater = EllipsoidalGaterX(2,'GateLevel',5);
config_pdaf.DetectionProbability = P_D;
pdaf = ProbabilisticDataAssocX(config_pdaf);

% Initiate Tag Generator
tag_gen = RandSampleTagGeneratorX(1:10000);

% Prepare initiator parameters
config_ti.TagGenerator = tag_gen;
config_ti.InitFilter = base_filter;
config_ti.DataAssociator = pdaf;
CovarThreshold = 4*PriorState.Covar;
config_ti.CustomDeleteConditionFcn = ...
    @(x,t) t.Filter.StatePosterior.Covar(1,1)>CovarThreshold(1,1) ...
           || t.Filter.StatePosterior.Covar(3,3)>CovarThreshold(3,3);

% Create the track initiator
myti = LLR_TrackInitiatorX(config_ti);

%% Metric Generators
ospa = OSPAX('CutOffThreshold',1,'Order',2);
gospa = GOSPAX('CutOffThreshold',1,'Order',2);

%% START OF SIMULATION
%  ===================>

% Create figure windows
if(ShowPlots)
    img = imread('maze.png');

    % set the range of the axes
    % The image will be stretched to this.
    min_x = 0;
    max_x = 10;
    min_y = 0;
    max_y = 10;

    % make data to plot - just a line.
    x = min_x:max_x;
    y = (6/8)*x;

    figure('units','normalized','outerposition',[0 0 .5 1])
    ax(1) = gca;
end

TrackList = [];
gospa_vals= zeros(N,4);
for k=2:N
    fprintf('Iteration = %d/%d\n================>\n',k,N);

    %% Extract DataList at time k
    MeasurementList = DataList(k);
    timestamp_km1 = DataList(k-1).Timestamp;
    timestamp_k = MeasurementList.Timestamp;
    dt = timestamp_k - timestamp_km1;
    transition_model.TimestepDuration = dt;
    fprintf('Timestamp = %s\n================>\n',timestamp_k);

    %% Process JPDAF
    jpdaf.MeasurementList = MeasurementList;
    jpdaf.TrackList = TrackList;
    jpdaf.predictTracks();
    jpdaf.associate();
    jpdaf.updateTracks();

    %% Perform Track initiation
    [TrackList, TentativeTrackList] = myti.initiateTracks(jpdaf.TrackList, MeasurementList, jpdaf.AssocWeightsMatrix, jpdaf.LikelihoodMatrix);

    [gospa_vals(k,1), gospa_vals(k,2), gospa_vals(k,3), gospa_vals(k,4)]= ...
        gospa.evaluate(GroundTruthStateSequence{k},jpdaf.TrackList);
    %% Plot update step results
    if(ShowPlots)

        cla(ax(1));
        %imagesc(ax(1),[min_x max_x], [min_y max_y], flipud(img));
        hold on;
        if(exist('data_plot','var'))
            delete(data_plot);
        end
        data_inv = measurement_model.finv(MeasurementList.Vectors);
        data_plot = plot(ax(1), data_inv(1,:), data_inv(3,:),'k*','MarkerSize', 10);

        % Plot confirmed tracks
        for j=1:numel(TrackList)
            means = [TrackList{j}.Trajectory.Mean];
            h2 = plot(ax(1), means(1,:),means(3,:),'-','LineWidth',1);
            h2 = plotgaussellipse(TrackList{j}.Filter.StatePosterior.Mean([1 3]),...
                                  TrackList{j}.Filter.StatePosterior.Covar([1 3],[1 3]),...
                                  'Color','r',...
                                  'Axis',ax(1));
        end

        % Plot tentative tracks
        for j=1:numel(TentativeTrackList)
            h2 = plot(ax(1), TentativeTrackList{j}.Filter.StatePosterior.Mean(1),TentativeTrackList{j}.Filter.StatePosterior.Mean(3),'.','LineWidth',1);
            if j==2
                set(get(get(h2,'Annotation'),'LegendInformation'),'IconDisplayStyle','off');
            end
            h2 = plotgaussellipse(TentativeTrackList{j}.Filter.StatePosterior.Mean([1 3]),...
                                  TentativeTrackList{j}.Filter.StatePosterior.Covar([1 3],[1 3]),...
                                  'Color','g',...
                                  'Axis',ax(1));
        end

        % set the y-axis back to normal.
        set(ax(1),'ydir','normal');
        str = sprintf('Robot positions (Update)');
        title(ax(1),str)
        xlabel('X position (m)')
        ylabel('Y position (m)')
        axis(ax(1),V_bounds)
        pause(0.01)
    end
end

figure
title("GOSPA");
subplot(2,3,[1 2 3]), plot(1:k,gospa_vals(1:k,1)), title("GOSPA Metric");
subplot(2,3,4), plot(1:k,gospa_vals(1:k,2)), title("GOSPA Localisation");
subplot(2,3,5), plot(1:k,gospa_vals(1:k,3)), title("GOSPA Missed");
subplot(2,3,6), plot(1:k,gospa_vals(1:k,4)), title("GOSPA False");


% Zhou, Zongwei, et al. "Fine-tuning convolutional neural networks for biomedical image analysis: actively and incrementally."
% IEEE conference on computer vision and pattern recognition, Hawaii. 2017.

% Author: [Zongwei Zhou](http://www.zongweiz.com)
% Email: zongweiz@asu.edu
% Last modified: Jan.26.2017

%% Table 1. examples of seven different patterns
% Relationships among seven prediction patterns and six AIFT methods in active candidate selection. We assume that
% a candidate has 11 patches, and their probabilities predicted by the current CNN are listed in Column 2. AIFT Entropyα,
% Diversityα, and (Entropy+Diversity)α operate on the top or bottom α percent of the candidate’s patches based on the majority
% prediction as described in Sec. 3.3. In this illustration, we choose α to be 1/4, meaning that the selection criterion (Eq. 3) is
% computed based on 3 patches within each candidate. The first choice of each method is highlighted in yellow and the second
% choice is in light yellow.

function Alg_Analysis()

close all; clear; clc;

% A, B, C, D, E, F represents outputs from CNN after Softmax Layer belong to one candidate.
A = [0.4 0.4 0.4 0.5 0.5 0.5 0.5 0.5 0.5 0.6 0.6];
B = [0.0 0.1 0.2 0.3 0.4 0.4 0.6 0.7 0.8 1.0 1.0];
C = [0.0 0.0 0.0 0.1 0.1 0.9 0.9 1.0 1.0 1.0 1.0];
D = [1.0 1.0 1.0 1.0 1.0 1.0 1.0 0.9 0.9 0.9 0.9];
E = [0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.1 0.1 0.1 0.1];
F  = [0.0 0.0 0.1 0.1 0.1 0.1 0.2 0.2 0.3 0.9 1.0];
G  = [0.0 0.1 0.7 0.8 0.8 0.9 0.9 0.9 0.9 1.0 1.0];

%% Produce values in Table 1 by row pattern A
disp('Table 1. pattern A');
disp(['Entropy_A = ', num2str(roundn(compute_matrix(A, 1, 0, 1), -2))]);
disp(['Entropy^1/4_A = ', num2str(roundn(compute_matrix(A, 1, 0, 0.25), -2))]);
disp(['Diversity_A = ', num2str(roundn(compute_matrix(A, 0, 1, 1), -2))]);
disp(['Diversity^1/4_A = ', num2str(roundn(compute_matrix(A, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_A = ', num2str(roundn(compute_matrix(A, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_A = ', num2str(roundn(compute_matrix(A, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern B
disp('Table 1. pattern B');
disp(['Entropy_B = ', num2str(roundn(compute_matrix(B, 1, 0, 1), -2))]);
disp(['Entropy^1/4_B = ', num2str(roundn(compute_matrix(B, 1, 0, 0.25), -2))]);
disp(['Diversity_B = ', num2str(roundn(compute_matrix(B, 0, 1, 1), -2))]);
disp(['Diversity^1/4_B = ', num2str(roundn(compute_matrix(B, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_B = ', num2str(roundn(compute_matrix(B, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_B = ', num2str(roundn(compute_matrix(B, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern C
disp('Table 1. pattern C');
disp(['Entropy_C = ', num2str(roundn(compute_matrix(C, 1, 0, 1), -2))]);
disp(['Entropy^1/4_C = ', num2str(roundn(compute_matrix(C, 1, 0, 0.25), -2))]);
disp(['Diversity_C = ', num2str(roundn(compute_matrix(C, 0, 1, 1), -2))]);
disp(['Diversity^1/4_C = ', num2str(roundn(compute_matrix(C, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_C = ', num2str(roundn(compute_matrix(C, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_C = ', num2str(roundn(compute_matrix(C, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern D
disp('Table 1. pattern D');
disp(['Entropy_D = ', num2str(roundn(compute_matrix(D, 1, 0, 1), -2))]);
disp(['Entropy^1/4_D = ', num2str(roundn(compute_matrix(D, 1, 0, 0.25), -2))]);
disp(['Diversity_D = ', num2str(roundn(compute_matrix(D, 0, 1, 1), -2))]);
disp(['Diversity^1/4_D = ', num2str(roundn(compute_matrix(D, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_D = ', num2str(roundn(compute_matrix(D, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_D = ', num2str(roundn(compute_matrix(D, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern E
disp('Table 1. pattern E');
disp(['Entropy_E = ', num2str(roundn(compute_matrix(E, 1, 0, 1), -2))]);
disp(['Entropy^1/4_E = ', num2str(roundn(compute_matrix(E, 1, 0, 0.25), -2))]);
disp(['Diversity_E = ', num2str(roundn(compute_matrix(E, 0, 1, 1), -2))]);
disp(['Diversity^1/4_E = ', num2str(roundn(compute_matrix(E, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_E = ', num2str(roundn(compute_matrix(E, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_E = ', num2str(roundn(compute_matrix(E, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern F
disp('Table 1. pattern F');
disp(['Entropy_F = ', num2str(roundn(compute_matrix(F, 1, 0, 1), -2))]);
disp(['Entropy^1/4_F = ', num2str(roundn(compute_matrix(F, 1, 0, 0.25), -2))]);
disp(['Diversity_F = ', num2str(roundn(compute_matrix(F, 0, 1, 1), -2))]);
disp(['Diversity^1/4_F = ', num2str(roundn(compute_matrix(F, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_F = ', num2str(roundn(compute_matrix(F, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_F = ', num2str(roundn(compute_matrix(F, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern G
disp('Table 1. pattern G');
disp(['Entropy_G = ', num2str(roundn(compute_matrix(G, 1, 0, 1), -2))]);
disp(['Entropy^1/4_G = ', num2str(roundn(compute_matrix(G, 1, 0, 0.25), -2))]);
disp(['Diversity_G = ', num2str(roundn(compute_matrix(G, 0, 1, 1), -2))]);
disp(['Diversity^1/4_G = ', num2str(roundn(compute_matrix(G, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_G = ', num2str(roundn(compute_matrix(G, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_G = ', num2str(roundn(compute_matrix(G, 1, 1, 0.25), -2))]);
disp(' ');

% %% Visualize the diversity equation
% x = 0:0.01:1;
% y = 0:0.01:1;
% [x,y] = meshgrid(x,y);
% div = (x-y).*log(x./(y+eps))+(y-x).*log((1-x)./(1-y+eps));
% mesh(x,y,div)

end

%% Compute matrix R (Eq. 3 in the paper)
function [S] = compute_matrix(p, lamda_1, lamda_2, alpha)

% lamda_1 and lamda_2 are trade-offs between entropy and diversity
% lamda_1: turn on/off entropy (1 or 0)
% lamda_2: turn on/off diversity (1 or 0)

% Handling noisy labels via majority selection (see Sec 3.3)
top = round(length(p) * alpha);
p = Majority(p,top);

% Initialize the R matrix
R = zeros(length(p));
for i = 1:length(p)
    for j = 1:i
        if i == j % entropy computed for diagonal of R
            R(i,i) = - lamda_1 * (p(i)*log(p(i)+eps)+(1-p(i))*log(1-p(i)+eps));
        else % diversity computed for rest of R
            R(i,j) = lamda_2 * ((p(i)-p(j)) * log((p(i)+eps)/(p(j)+eps)) + ...
                     (1-p(i)-1+p(j)) * log((1-p(i)+eps)/(1-p(j)+eps)));
            R(j,i) = R(i,j);
        end
    end
end

% Compute active selecting value from matrix R for this candidate
% by numerical sum of R (see Alg 1 line 11)
S = sum(R(:));

end

%% Sec 3.3. Handling noisy labels via majority selection
% eq. 4, calculate the mean and sort based on dominate predictions.
function [p] = Majority(p,top)

m = mean(p);
if m > 0.5
    p = sort(p,'descend');
else
    p = sort(p,'ascend');
end
p = p(1:top);

end


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

% display montage
function MU_dispMontage(Temp,Event,Disp_Matrix,handles)

        % Montage slice selection checking
        if str2num(get(handles.Sli_from,'String'))>str2num(get(handles.Sli_to,'String'))
                errordlg('No slice is chosen for creating montage !');
                return;
        end
        if str2num(get(handles.Sli_from,'String'))<1
                set(handles.Sli_from,'String','1')
                return;
        end
        if str2num(get(handles.Sli_to,'String'))>handles.V.Layer
                set(handles.Sli_to,'String',num2str(handles.V.Layer))
                return;
        end

        % Montage layout initialization
        [Row,Column,Layer]=size(Disp_Matrix);
        ind=0;
        b_flag=0;
        for i=1:str2num(get(handles.Mont_row,'String'))
                for j=1:str2num(get(handles.Mont_col,'String'))
                        DMatrix((i-1)*Row+1:i*Row,(j-1)*Column+1:j*Column)=Disp_Matrix(:,:,str2num(get(handles.Sli_from,'String'))+ind);
                        ind=ind+1;
                        if ind>str2num(get(handles.Sli_to,'String'))-str2num(get(handles.Sli_from,'String'))
                                b_flag=1;
                                break;
                        end
                end
                if b_flag==1
                        break;
                end
        end

        %-------------------------Windows Contrast Change
        Color_map=handles.V.Color_map;
        Contrast_Low=handles.V.C_lower;
        Contrast_High=handles.V.C_upper;
        Contrast_Change=0;
        point=[0 0];
        point2=[0 0];
        Contrast_Interval=(Contrast_High-Contrast_Low)/100;
        %-------------------------End

        delete(handles.Cre_Mont);
        figure ('KeyReleaseFcn',@WindowKeyRelease,'KeyPressFcn',@WindowKeyPress, 'WindowScrollWheelFcn',@MouseScrollWheel,...
                'WindowButtonUpFcn',@MouseUp,'WindowButtonDownFcn', @MouseClick,'Name',['Display ' handles.V.Current_matrix ' Montage']);
        imagesc(DMatrix,[Contrast_Low Contrast_High]);
        colormap(Color_map);
        colorbar;

        function MouseScrollWheel (Temp, Event)
                if Contrast_Change==-1
                    Contrast_Low=Contrast_Low + Event.VerticalScrollCount*Contrast_Interval;
                end
                if Contrast_Change==1
                    Contrast_High=Contrast_High + Event.VerticalScrollCount*Contrast_Interval;
                end

                imagesc (DMatrix,[Contrast_Low Contrast_High]);
                colormap(Color_map);
                colorbar;

        end

        function MouseClick(Temp,Event)
                tpoint=get(gca,'currentpoint');
                point=[round(tpoint(1)),round(tpoint(3))];

        end

        function MouseUp(Temp,Event)
                tpoint2=get(gca,'currentpoint');
                point2=[round(tpoint2(1)),round(tpoint2(3))];

                if sum(abs(point2-point))~=0
                    rectangle('Position',[point(1),point(2),point2(1)-point(1),point2(2)-point(2)],'EdgeColor',[0 1 0]);
                    DispS=DMatrix(point(2):point2(2),point(1):point2(1));
                    DispS=DispS(:);
                    tmean=mean(double(DispS));
                    tstd=std(double(DispS));
                    xlabel(['ROI mean:' num2str(tmean) '  ROI std:' num2str(tstd) '  RSD(%):' num2str(abs(tstd./tmean)*100)],'FontSize',18);
                end
        end

        function WindowKeyPress(Temp,Event)
                %------------------------------Windows Contrast Change
                if Event.Character=='l'
                    Contrast_Change=-1;
                elseif Event.Character=='u'
                    Contrast_Change=1;
                end
                %------------------------------End
        end

        function WindowKeyRelease(Temp,Event)
                Contrast_Change=0;
        end

end


function plot_large_point_cloud(P,fig,ms,rel)

% Plots a random subset of a large point cloud. The user specifies the
% relative size of the subset (input "rel" given as in percentage points).
%
% Inputs:
% P     Point cloud
% fig   Figure number
% ms    Marker size
% rel   Subset size in percentage points (%).
%           E.g. if rel = 12, then about 12 % poinst are plotted

rel = 0.5/(1-rel/100); % Compute a coeffiecient

I = logical(round(rel*rand(size(P,1),1)));
plot_point_cloud(P(I,:),fig,ms)


function rs = minus(s1,s2)

%tstoolbox/@signal/minus
%   Syntax:
%     * rs=minus(s, offset)
%     * rs=minus(s1,s2)
%
%   Input arguments:
%     * s, s1, s2 - signal object
%     * offset - scalar value
%
%   Calculate difference of signals s1 and s2 or substract a scalar value
%   from s.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(2,2);

if isa(s2, 'signal')
	c = s1.core - s2.core;
%	d = merge(s1.description, s2.description);
	rs = signal(c, s1);
%	rs.description = d;
	rs = addhistory(rs,  ['Subtracted second signal from first signal']);
	rs = addcommandlines(rs, 's = minus(s, s2');
elseif isa(s2, 'double')
	if size(s2)==[1 1]
		c = core(data(s1)-s2);
		rs = signal(c, s1);
		rs = addhistory(rs,  ['Subtracted ' num2str(s2) ' from signal']);
		rs = addcommandlines(rs, 's = minus(s', s2);
	else
	 	error('second argument must be a signal or a scalar');
	end
else
	error('second argument must be a signal or a scalar');
end


function arrMean = slarrmean(data, arrsiz, n, varargin)
%SLARRMEAN Computes the mean of a set of arrays
%
% $ Syntax $
%   - slarrmean(arrs, arrsiz, n, ...)
%   - slarrmean(fns, arrsiz, n, ...)
%
% $ Arguments $
%   - arrs:         the super-array consisting of all arrays
%   - fns:          the file paths of all array files
%   - arrsiz:       the size of each array unit. (for a column vector, it
%                   is the length of the vector)
%   - n:            the total number of array units
%
% $ Description $
%   - slarrmean(arrs, arrsiz, n, ...) computes the mean of all array units
%     with the size of each unit specified by arrsiz. If there are more
%     than one array unit, the size of arrs should be [arrsiz, n].
%
%   - slarrmean(fns, arrsiz, n, ...) computes the mean of all array units
%     stored in the array files given in fns. Each array file stores
%     an super-array of a set of array units.
%
%   - You can specify additional properties.
%       \t      The properties of slarrmean
%       \h      name       &        description
%              'weights'   &  The weights of each array unit, default = []
%
% $ History $
%   - Created by Dahua Lin on Jul 27th, 2006
%

%% parse and verify input arguments

if nargin < 3
    raise_lackinput('slarrmean', 3);
end

if isnumeric(data)
    isdirect = true;
    arrs = data;
    arrsiz = arrsiz(:)';
    if ~isequal(size(arrs), [arrsiz, n])
        error('sltoolbox:sizmismatch', ...
            'The size of arrs (data) is invalid');
    end

elseif iscell(data)
    isdirect = false;
    fns = data;
    arrsiz = arrsiz(:)';
    nfiles = numel(fns);

else
    error('sltoolbox:invalidarg', ...
        'The first argument for slarrmean should be an numeric array or a cell array of file names');
end

opts.weights = [];

opts = slparseprops(opts, varargin{:});
hasweights = ~isempty(opts.weights);

if (hasweights)
    opts.weights = opts.weights(:);
    if length(opts.weights) ~= n
        error('sltoolbox:invalidarg', ...
            'The length of weights is inconsistent with the number of units');
    end
end

%% Main skeleton

if isdirect
    arrMean = compute_array_sum(arrs, arrsiz, n, opts.weights);
else
    arrMean = zeros(arrsiz);
    c = 0;
    for i = 1 : nfiles
        curarrs = slreadarray(fns{i});
        curn = size(curarrs, length(arrsiz) + 1);
        if ~hasweights
            arrMean = arrMean + compute_array_sum(curarrs, arrsiz, curn, []);
        else
            arrMean = arrMean + compute_array_sum(curarrs, arrsiz, curn, opts.weights(c+1:c+curn));
        end
        c = c + curn;

        if c > n
            error('sltoolbox:sizmismatch', ...
                'The total number of units in the set of array files is not n');
        end
    end

    if c ~= n
        error('sltoolbox:sizmismatch', ...
            'The total number of units in the set of array files is not n');
    end

end

if ~hasweights
    arrMean = arrMean / n;
else
    arrMean = arrMean / sum(opts.weights);
end

%% Compute function

function S = compute_array_sum(arrs, arrsiz, n, w)

if ~isequal(size(arrs), [arrsiz, n])
    error('sltoolbox:sizmismatch', ...
        'The size of array is not consistent as specified');
end

d = length(arrsiz);
if isempty(w)
    S = sum(arrs, d+1);
else
    S = reshape(arrs, [prod(arrsiz), n]) * w;
    if d == 1
        S = reshape(S, [arrsiz, 1]);
    else
        S = reshape(S, arrsiz);
    end
end


function [codebook,scheme] = code_OneVsAll(m)
% Generate the codebook for multiclass classification with One-Versus-All encoding.
%
% codebook = code_OneVsAll(m)
%
%  see also:
%    code, codedist_hamming

% (c) SCD-KULeuven, rights & help @ http://www.esat.kuleuven.be/sista/lssvmlab

codebook = eye(m).*2-1;
scheme = []; for i=1:m, scheme = [scheme 'b']; end


function [Y,index] = Shuffle(X)
% [Y,index] = Shuffle(X)
%
% Randomly sorts X.
% If X is a vector, sorts all of X, so Y = X(index).
% If X is an m-by-n matrix, sorts each column of X, so
%	for j=1:n, Y(:,j)=X(index(:,j),j).
%
% Also see SORT, Sample, Randi, and RandSample.
%
% xx/xx/92  dhb  Wrote it.
% 10/25/93  dhb  Return index.
% 5/25/96   dgp  Made consistent with sort and "for i=Shuffle(1:10)"
% 6/29/96	dgp	 Edited comments above.
%
% Probably from PTB.  I think this is Pelli and Brainard.

% I guess rand() never returns two identical numbers.
[null,index] = sort(rand(size(X)));
Y = X(index);

return


function testvlfeatsift()
 dir='\\File.ceegs.ohio-state.edu\SPIN\UAV\August 08, 2013\Nikon D800\109ND800\';
fn1=[dir,'DSC_1023.jpg'];
fn2=[dir,'DSC_1024.jpg'];
im1=imread(fn1);
im2=imread(fn2);
Nimg1 = gaussdownsample(im1,4,[5 5],1.6);
Nimg2 = gaussdownsample(im2,4,[5 5],1.6);

figure; imshow(Nimg1);

sift_mosaic(Nimg1, Nimg2);

function Nimg = gaussdownsample(img,levels,gsize,sigma);
%
%  Function to generate a gaussian-pyramid for the given input image
% image is resize to width/2^(levels-1)*height/2^(levels-1)
% Input:
%        img: input image-matrix grayscale
%        levels:  number of levels of the pyramid, we only return the level
%        of the least size
%        gsize: size of the gaussian kernel [w h] ([5 5] normally provides a smooth output)
%        sigma:  sigma for gaussian kernel
% Output:
%        Nimg:  the smoothed image of the smallest size
% Usage:
%      im = imread('cameraman.tif');
%      Nimg = Gscale(im,3,[5 5],1.6);
%      figure; imshow(Nimg);
Nimg=img;
if(levels==1)
    return;
end
%guassian filter  with a sigma=1.6
g = fspecial('gaussian',gsize,sigma);
%pyramid
for i = 2:levels
        %perform guassian filtering
        im = imfilter(Nimg,g,'conv');
        %perform downsampling (horizontal)
        im1 = im(:,1:2:size(Nimg,2),:);
        %vertical
        im2 = im1(1:2:size(Nimg,1),:,:);
        %store it in a struct format
        Nimg = im2;
end

%End


function v = null(N, prefs, nullity)
%NULL   Null space of a linear CHEBOP.
%   Z = NULL(N) returns a CHEBMATRIX with orthonormal columns which span the
%   null space of the linear CHEBOP N. That is, N(Z) has negligible elements,
%   SIZE(Z, 2) is the nullity of N, and Z'*Z = I. N may contain linear boundary
%   conditions, but they will be treated as homogeneous. The nullity is
%   determined by comparing the differential order of the system and the number
%   of supplied boundary conditions.
%
%   NULL(N, PREFS) allows additional preferences to be passed via the
%   CHEBOPPREF, PREFS.
%
%   NULL(n, PREFS, K) or NULL(N, K) attempts to find K null vectors. If the
%   nullity of N is determined to be less than K then a warning is thrown. This
%   is useful in situations where the nullity is known in advance and the
%   algorithm struggles to determine it automatically.
%
%   Systems of equations are not yet supported.
%
% Example 1:
%   N = chebop(@(u) diff(u), [0, pi]);
%   V = null(N);
%   norm(N(V))
%
% Example 2:
%   N = chebop(@(x, u) 0.2*diff(u, 3) - sin(3*x).*diff(u));
%   N.rbc = 1;
%   V = null(N)
%   norm(N(V))
%   plot(V)
%
% See also CHEBOP/SVDS, CHEBOP/EIGS, NULL.

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

if ( nargin < 3 )
    nullity = [];
end
if ( nargin < 2 )
    prefs = cheboppref();
elseif ( ~isa(prefs, 'cheboppref') )
    nullity = prefs;
    prefs = cheboppref();
end

% Linearize and check whether the CHEBOP is linear:
[L, ignored, fail] = linop(N); %#ok<ASGLU>
if ( fail )
    error('CHEBFUN:CHEBOP:null:nonlinear', ...
        ['The operator appears to be nonlinear.\n', ...
         'NULL() supports only linear CHEBOP instances.']);
end

% Determine the discretization:
prefs = determineDiscretization(N, L, prefs);

% Call LINOP/NULL:
v = null(L, prefs, nullity);

% Return a CHEBFUN rather than a CHEBMATRIX for scalar problems:
if ( isa(v, 'chebmatrix') && all(size(v, 1) == 1) )
    v = chebfun(v);
end

end


load('table_T.mat')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Params for UAV
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% UAV == Zagi
%     p_drone.mass = 1.56;
%     p_drone.Jx   = 0.1147;
%     p_drone.Jy   = 0.0576;
%     p_drone.Jz   = 0.1712;
%     p_drone.Jxz  = 0.0015;

%%% UAV == Aerosonde
p_drone.mass = 13.5;
p_drone.Jx   = 0.8244;
p_drone.Jy   = 1.135;
p_drone.Jz   = 1.759;
p_drone.Jxz  = .1204;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute gammas
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

p_drone.gamma  = p_drone.Jx*p_drone.Jz - p_drone.Jxz^2;
p_drone.gamma1 = p_drone.Jxz*(p_drone.Jx-p_drone.Jy+p_drone.Jz)/p_drone.gamma;
p_drone.gamma2 = (p_drone.Jz*(p_drone.Jz-p_drone.Jy)+p_drone.Jxz^2)/p_drone.gamma;
p_drone.gamma3 = p_drone.Jz/p_drone.gamma;
p_drone.gamma4 = p_drone.Jxz/p_drone.gamma;
p_drone.gamma5 = (p_drone.Jz-p_drone.Jx)/p_drone.Jy;
p_drone.gamma6 = p_drone.Jxz/p_drone.Jy;
p_drone.gamma7 = ((p_drone.Jx-p_drone.Jy)*p_drone.Jx+p_drone.Jxz^2)/p_drone.gamma;
p_drone.gamma8 = p_drone.Jx/p_drone.gamma;

% Aerodynamic coefficients
p_drone.S_wing          = 0.55;     % Surface of the plane
p_drone.b              = 2.8956;
p_drone.c              = 0.18994;
p_drone.S_prop         = 0.2027;    % Surface of the propeller
p_drone.k_motor        = 80;        % k motor
p_drone.k_TP           = 0;
p_drone.k_omega        = 0;
p_drone.e              = 0.9;

p_drone.kb = @(Va) 0.5*p_drone.b/Va;
p_drone.kc = @(Va) 0.5*p_drone.c/Va;

% Longitudinal dynamics:
%   lift and drag (F_L, F_D)
p_drone.CL0            = 0.28;     % C_L_0
p_drone.CL_alpha       = 3.45;     % C_L_alpha
p_drone.CL_q           = 0.0;      % C_L_q
p_drone.CL_de          = -0.36;    % C_L_delta_e
p_drone.CD0            = 0.03;     % C_D_0
p_drone.CD_alpha       = 0.30;     % C_D_alpha
p_drone.CD_p           = 0.0437;   % C_D_p
p_drone.CD_q           = 0.0;      % C_D_q
p_drone.CD_de          = 0.0;      % C_D_delta_e

%   moment around y (m)
p_drone.Cm0            = -0.02338; % C_m_0
p_drone.Cm_alpha       = -0.38;    % C_m_alpha
p_drone.Cm_q           = -3.6;     % C_m_q
p_drone.Cm_de          = -0.5;     % C_m_delta_e

% Lateral dynamics
%   F_y
p_drone.CY0            = 0.0;
p_drone.CY_beta        = -0.98;
p_drone.CY_p           = 0.0;
p_drone.CY_r           = 0.0;      % C_Y_r
p_drone.CY_da          = 0.0;      % C_Y_delta_a
p_drone.CY_dr          = -0.17;    % C_Y_delta_r

%   moment around x (l) and z (n)
p_drone.Cl0            = 0.0;      % C_l_0
p_drone.Cl_beta        = -0.12;    % Cl_beta
p_drone.Cl_p           = -0.26;    % C_l_p
p_drone.Cl_r           = 0.14;     % C_l_r
p_drone.Cl_da          = 0.08;     % C_l_delta_aileron
p_drone.Cl_dr          = 0.105;    % C_l_delta_rudder
p_drone.Cn0            = 0.0;      % C_n_0
p_drone.Cn_beta        = 0.25;     % C_n_beta
p_drone.Cn_p           = 0.022;    % C_n_p
p_drone.Cn_r           = -0.35;    % C_n_r
p_drone.Cn_da          = 0.06;     % C_n_delta_a
p_drone.Cn_dr          = -0.032;   % C_n_delta_r

p_drone.C_prop         = 1.0;      % propeller proportional coeff fx=Sprop*Cprop*dp
p_drone.M              = 50;
p_drone.epsilon        = 0.1592;
p_drone.alpha0         = 0.4712;

p_drone.Cp0     = p_drone.gamma3 * p_drone.Cl0     + p_drone.gamma4 * p_drone.Cn0;
p_drone.Cp_beta = p_drone.gamma3 * p_drone.Cl_beta + p_drone.gamma4 * p_drone.Cn_beta;
p_drone.Cp_p    = p_drone.gamma3 * p_drone.Cl_p    + p_drone.gamma4 * p_drone.Cn_p;
p_drone.Cp_r    = p_drone.gamma3 * p_drone.Cl_r    + p_drone.gamma4 * p_drone.Cn_r;
p_drone.Cp_da   = p_drone.gamma3 * p_drone.Cl_da   + p_drone.gamma4 * p_drone.Cn_da;
p_drone.Cp_dr   = p_drone.gamma3 * p_drone.Cl_dr   + p_drone.gamma4 * p_drone.Cn_dr;
p_drone.Cr0     = p_drone.gamma4 * p_drone.Cl0     + p_drone.gamma8 * p_drone.Cn0;
p_drone.Cr_b    = p_drone.gamma4 * p_drone.Cl_beta + p_drone.gamma8 * p_drone.Cn_beta;
p_drone.Cr_p    = p_drone.gamma4 * p_drone.Cl_p    + p_drone.gamma8 * p_drone.Cn_p;
p_drone.Cr_r    = p_drone.gamma4 * p_drone.Cl_r    + p_drone.gamma8 * p_drone.Cn_r;
p_drone.Cr_da   = p_drone.gamma4 * p_drone.Cl_da   + p_drone.gamma8 * p_drone.Cn_da;
p_drone.Cr_dr   = p_drone.gamma4 * p_drone.Cl_dr   + p_drone.gamma8 * p_drone.Cn_dr;

% first cut at initial conditions
% p_drone.pn_T    = xyu_T(1);  % initial North position
% p_drone.pe_T    = xyu_T(2);  % initial East position
% p_drone.pd_T    = xyu_T(3);  % initial Down position (negative altitude)
% p_drone.vx_T    = xyu_T(4); % initial velocity along body x-axis
% p_drone.vy_T    = xyu_T(5);  % initial velocity along body y-axis
% p_drone.vz_T    = xyu_T(6);  % initial velocity along body z-axis
% p_drone.phi_T   = xyu_T(7);  % initial roll angle
% p_drone.theta_T = xyu_T(8);  % initial pitch angle
% p_drone.psi_T   = xyu_T(9);  % initial yaw angle
% p_drone.p_T     = xyu_T(10);  % initial body frame roll rate
% p_drone.q_T     = xyu_T(11);  % initial body frame pitch rate
% p_drone.r_T     = xyu_T(12);  % initial body frame yaw rate

% p_drone.Va_T = xyu_T(13);
% p_drone.alpha_T = xyu_T(14);
% p_drone.beta_T = xyu_T(15);

% p_drone.de_T = xyu_T(16);
% p_drone.da_T = xyu_T(17);
% p_drone.dr_T = xyu_T(18);
% p_drone.dt_T = xyu_T(19);

% Values for the Aerosonde UAV
p_drone.Va0 = 35;        % m/s (~85 mph)

% Trim conditions used for static PID tuning
Va_Td = 35;
gamma_Td = deg2rad(0);
R_Td = Inf;

Td = [Va_Td, gamma_Td, R_Td];

% Compute PID control
% p_drone.C = compute_pid_controller(Td, P)
[Va_idx, gamma_idx, R_idx]  = Td2idx(Td);
xyu_T = table_T(:,Va_idx, gamma_idx, R_idx);
p_drone.phi_T   = 0;  % initial roll angle
p_drone.theta_T = 0;  % initial pitch angle
p_drone.psi_T   = 0;  % initial yaw angle
p_drone.Va_T = 35;
[a,TF] = compute_tf_model(xyu_T, p_drone, p_physics);
% PVa_T = xyu_T(13);  % Extract trimmed state

% Altitude state machine parameters
p_drone.altitude_take_off_zone = 40;
p_drone.h_theta_hold_zone      = 10;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Tuning PID loops

% Lateral dynamics
% Roll_aileron-hold
ephi_max = deg2rad(15);
p_drone.kp_phi = p_drone.da_max / ephi_max;
ksi_phi = KSI_OPT;
wn_phi = sqrt(p_drone.kp_phi * a.phi2);
p_drone.kd_phi = (2*ksi_phi*wn_phi - a.phi1) / a.phi2;

p_drone.tau_phi = 0.05; % [seconds] <=> 1 autopilot loop

% Course_roll-hold
W_chi_phi = 10; % Bandwith separation factor
% Between 5 and 10. Safety corresponds to 10.
wn_chi = wn_phi / W_chi_phi; %[rad/sec]
ksi_chi = KSI_OPT;
p_drone.kp_chi = 2*ksi_chi*wn_chi*p_drone.Va_T/p_physics.gravity;
p_drone.ki_chi = wn_chi^2*p_drone.Va_T/p_physics.gravity;

% Sideslip_rudder-hold
%TODO

% Longitudinal dynamics
% Pitch_elevator-hold
etheta_max  = deg2rad(10);
p_drone.kp_theta  = p_drone.de_max / etheta_max * sign(a.theta3);
wn_theta = sqrt(a.theta2 + p_drone.kp_theta*a.theta3);
ksi_theta = KSI_OPT;
if isfinite(a.theta3)
    p_drone.kd_theta  = (2*ksi_theta*wn_theta - a.theta1) / a.theta3;
else
    p_drone.kd_theta  = 0;
    disp("kd_theta is null!");
end
p_drone.tau_theta = 0.1; % [seconds] <=> 10 autopilot loops

% Altitude_pitch-hold
% Scale factor between theta_c and theta
K_theta_DC = p_drone.kp_theta*a.theta3/(a.theta2+p_drone.kp_theta*a.theta3);
W_h_theta = 10; % Between 5 and 15. Safety corresponds to 15.
wn_h = wn_theta / W_h_theta; % [rad/sec]
ksi_h = 1;
p_drone.kp_h  = 2*ksi_h*wn_h/(K_theta_DC*p_drone.Va_T);
p_drone.ki_h  = wn_h^2 / (K_theta_DC*p_drone.Va_T);

% Airspeed_thrust-hold
wn_va_dt = 10; % [rad/sec] Need to be tuned.
ksi_va_dt = 1;
p_drone.kp_va_dt = (2*ksi_va_dt*wn_va_dt - a.va1)/a.va2;
p_drone.ki_va_dt = wn_va_dt^2/a.va2;

% Airspeed_pitch-hold
W_va_theta = 7; % [rad/sec] 10 by default, can be lower.
wn_va_theta = wn_theta / W_va_theta;
ksi_va_theta = KSI_OPT;
p_drone.kp_va_theta = (a.va1 - 2*ksi_va_theta*wn_va_theta)/(K_theta_DC*p_physics.gravity);
p_drone.ki_va_theta = -wn_va_theta^2/(K_theta_DC*p_physics.gravity);

% first cut at initial conditions
p_drone.pn_T    = xyu_T(1);  % initial North position
p_drone.pe_T    = xyu_T(2);  % initial East position
p_drone.pd_T    = xyu_T(3)-200;  % initial Down position (negative altitude)
p_drone.vx_T    = xyu_T(4); % initial velocity along body x-axis
p_drone.vy_T    = xyu_T(5);  % initial velocity along body y-axis
p_drone.vz_T    = xyu_T(6);  % initial velocity along body z-axis
p_drone.phi_T   = xyu_T(7);  % initial roll angle
p_drone.theta_T = xyu_T(8);  % initial pitch angle
p_drone.psi_T   = xyu_T(9);  % initial yaw angle
p_drone.p_T     = xyu_T(10);  % initial body frame roll rate
p_drone.q_T     = xyu_T(11);  % initial body frame pitch rate
p_drone.r_T     = xyu_T(12);  % initial body frame yaw rate

p_drone.Va_T    = xyu_T(13);
p_drone.alpha_T = xyu_T(14);
p_drone.beta_T  = xyu_T(15);

p_drone.de_T    = xyu_T(16);
p_drone.da_T    = xyu_T(17);
p_drone.dr_T    = xyu_T(18);
p_drone.dt_T    = xyu_T(19);

run('param_sensors')


function rhs = globNedRHSVIFE3D(fun, mesh, fem, femI, dind, vind)

%% USAGE: generate global load vector on a tetrahedral mesh
%
% INPUTS:
% fun --- the load function from PDE (pde.f)
% mesh --- a struct data contains mesh information.
% fem --- global DoF for test function space
% dind --- derivative info for test function
%            d = [0,0,0]: function value
%            d = [1,0,0]: Dx value
%            d = [0,1,0]: Dy value
%            d = [0,0,1]: Dz value
%
% OUTPUTS:
% rhs --- global rhs vector

% Last Modified: 08/07/2020 by Xu Zhang

%% 1. RHS on non-interface elements
dof = size(fem.g2ldof,2);  nloc = dof;
ntID = find(mesh.tLoc > 0); ntN = length(ntID);
AN = fem.area(ntID); gw = fem.gw;
gxN = fem.gx(ntID,:); gyN = fem.gy(ntID,:); gzN = fem.gz(ntID,:);
X = zeros(nloc*ntN, 1);

feEvalBas = @EvalNed1Bas3D;
IfeEvalBas = @evalNed1IFEBas3D;

fN = feval(fun,gxN,gyN,gzN);
ind = 0;
I = reshape(femI.g2ldof(ntID,1:6),nloc*ntN,1);
for i = 1:dof
    ibas = feEvalBas(fem.bas, ntID, gxN, gyN, gzN, i, dind, vind);
    X(ind+1:ind+ntN) = AN.*sum((ibas.*fN).*gw',2).*fem.t_e_orit(ntID,i);
    ind = ind + ntN;
end
rhsN = sparse(I,1,X,length(femI.gdof),1);

%% 2. RHS on interface elements
AI1 = femI.area1; gw = femI.gw; gxI1 = femI.gx1; gyI1 = femI.gy1; gzI1 = femI.gz1;
ntI1 = size(femI.tType1,1); % not number of interface element, but quadrature element
nloc1 = 12; X1 = zeros(nloc1*ntI1, 1);
fI1 = feval(fun,gxI1,gyI1,gzI1);
ind = 0;
I1 = reshape(femI.tType1,nloc1*ntI1,1);
for i = 1:nloc1
    ibas = femI.basUType1(:,vind,i);
    X1(ind+1:ind+ntI1) = AI1.*sum((ibas.*fI1).*gw',2);
    ind = ind + ntI1;
end
rhsI1 = sparse(I1,1,X1,size(femI.gdof,1),1);

AI2 = femI.area2; gw = femI.gw; gxI2 = femI.gx2; gyI2 = femI.gy2; gzI2 = femI.gz2;
ntI2 = size(femI.tType2,1); % not number of interface element, but quadrature element
nloc2 = 14; X2 = zeros(nloc2*ntI2, 1);
fI2 = feval(fun,gxI2,gyI2,gzI2);
ind = 0;
I2 = reshape(femI.tType2,nloc2*ntI2,1);
for i = 1:nloc2
    ibas = femI.basUType2(:,vind,i);
    X2(ind+1:ind+ntI2) = AI2.*sum((ibas.*fI2).*gw',2);
    ind = ind + ntI2;
end
rhsI2 = sparse(I2,1,X2,size(femI.gdof,1),1);

rhs = rhsN + rhsI1 + rhsI2;


function outsig = signal_from_spectrum(amplitude,phase,f,dim,conf)
%SIGNAL_FROM_SPECTRUM time signal from single-sided spectrum
%
%   Usage: outsig = signal_from_spectrum(amplitude,phase,f,[dim],conf)
%
%   Input parameters:
%       amplitude   - the single-sided amplitude spectrum
%       phase       - the single-sided phase spectrum / rad
%       f           - the corresponding frequency vector
%       dim         - dimension along which the ifft is performed
%       conf        - configuration struct (see SFS_config)
%
%   Output parameters:
%       outsig      - one channel audio (time) signal
%
%   SIGNAL_FROM_SPECTRUM(amplitude,phase,f,conf) generates the time signal from
%   single-sided amplitude and phase spectra using ifft. It is the counterpart
%   of SPECTRUM_FROM_SIGNAL and not interchangeable with calling ifft.
%
%   See also: spectrum_from_signal, ifft

%*****************************************************************************
% 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 *
%*****************************************************************************

%% ===== Checking input arguments ========================================
nargmin = 4;
nargmax = 5;
narginchk(nargmin,nargmax);
if nargin == nargmin
   conf = dim;
   dim = find(size(amplitude)~=1, 1);  % find first non-singleton dimension
else
   isargpositivescalar(dim);
end
isargstruct(conf);

Ndims = ndims(amplitude);
dim = min(dim,Ndims);
amplitude = permute(amplitude, [dim:Ndims, 1:dim-1]);  % move dim to first dimension
phase = permute(phase, [dim:Ndims, 1:dim-1]);  % move dim to first dimension
s = size(amplitude);
Nx = s(1);
amplitude = reshape(amplitude, Nx, []);  % squeeze all other dimensions
phase = reshape(phase, Nx, []);  % squeeze all other dimensions

%% ===== Configuration ===================================================
fs = conf.fs;

%% ===== Regenerating wave form from spectrum ============================
% Provided number of frequency bins
bins = length(f);
if bins ~= Nx
   error('%s: size of spectrum does not match length of frequency vector',...
       upper(mfilename));
end

if f(end) == fs/2  % -> even time signal length
    % Length of the signal to generate
    samples = 2 * (bins-1);
    % Rescaling (see spectrum_from_signal())
    amplitude = [amplitude(1,:); amplitude(2:end-1,:)/2; amplitude(end,:)] ...
        * samples;
    % Mirror the amplitude spectrum ( 2*pi periodic [0, fs[ )
    amplitude = [amplitude; amplitude(end-1:-1:2,:)];
    % Mirror the phase spectrum and build the inverse (complex conjugate)
    phase = [phase; -1 * phase(end-1:-1:2,:)];

else  % -> odd time signal length
    % Length of the signal to generate
    samples = 2*bins - 1;
    % Rescaling (see signal_from_spectrum)
    amplitude = [amplitude(1,:); amplitude(2:end,:)/2] * samples;
    % Mirror the amplitude spectrum ( 2*pi periodic [0, fs-bin] )
    amplitude = [amplitude; amplitude(end:-1:2,:)];
    % Mirror the phase spectrum and build the inverse (complex conjugate)
    phase = [phase; -1*phase(end:-1:2,:)];
end

% Convert to complex spectrum
compspec = amplitude .* exp(1i*phase);

% Build the inverse fft and assume spectrum is conjugate symmetric
outsig = real(ifft(compspec));

%% ===== Output ==========================================================
% undo reshape and permute
outsig = reshape(outsig, [samples, s(2:end)]);
outsig = permute(outsig, [Ndims-dim+2:Ndims, 1:Ndims-dim+1]);


function [ar,arp,aru,g]=v_lpcrf2ar(rf)
%V_LPCRF2AR Convert reflection coefs to autoregressive coefs [AR,ARP,ARU,G]=(RF)
%
% Input:  RF(:,p+1) gives reflection coefficients of one or more p-section lossless tubes
% Ouputs: G is the gain of the all-pole AR filter
%         AR/G is the transfer function from U_in to the glottal input wave, U_g.
%               AR(:,1)=1 always.
%         ARP*K is the transfer function from U_in to the pressure just after the glottis
%               where K = rho*c/Alips: rho = air density 1.23 kg/m^3, c=sound speed 340 m/s,
%               Alips = effective area of free space beyond the lips.
%         ARU is the transfer function from U_in to the total volume velocity through the glottis
%
%              where U_in=z^(p/2)*U_lips is the time-advanced volume velocity at the lips
%
%         Energy into the vcal tract is equal to K*filter(ARP,1,Ulips).*filter(ARU,1,Ulips)
%              reverse glottal flows divided by 1-r0 where r0 is the glottal reflection coefficient.
%              The scale factor is included to avoid a zero answer when the glottis is closed giving r0=1.
%
% The transfer functions have ar(:,1)=art(:,1)=1
% They should both be multiplied by z^(p/2)/prod(1+rf) to correct the absolute
% gain and to compensate for the delay of p/2 samples along the length of the tube.
%
% The energy into the vocal tract is given by ars(speech) * are(speech)
%
% Ref: D. M. Brookes and H. P. Loke. "Modelling energy flow in the vocal tract with
%           applications to glottal closure and opening detection." In Proc ICASSP'99, pages 213-216, Mar 1999.

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: v_lpcrf2ar.m 10865 2018-09-21 17:22:45Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p1]=size(rf);
p2=p1+1;
p=p1-1;
pm=p-1;
arf=[ones(nf,1) zeros(nf,p)];
arr=[zeros(nf,p) rf(:,p1)];
cr=zeros(nf,p);
for k=1:p-1
  rk=rf(:,(p1-k)*ones(1,k));
  cr(:,1:k)=arr(:,p2-k:p1);
  arr(:,p1-k:p)=arr(:,p1-k:p)+rk.*arf(:,1:k);
  arf(:,2:k+1)=arf(:,2:k+1)+rk.*cr(:,1:k);
end
r1=rf(:,1);
ar=arf+r1(:,ones(1,p1)).*arr;
if nargout>1
   kp=prod(1-rf(:,2:p1),2);
   arp=(arf-arr)./kp(:,ones(1,p1));
   if nargout>2
      g=prod(1+rf(:,2:p1),2);
      aru=(arf+arr)./g(:,ones(1,p1));
      if nargout>3
         g=g.*(1+rf(:,1));
      end
   end
end


function [b, dev, stat] = glm_multilevel(D, Yvarname, Xvarnames, wh_keep)
% Predict Y from X using GLM
%
% :Usage:
% ::
%
%    [b, dev, stat] = glm_multilevel(D, Yvarname, Xvarnames, wh_keep)
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2013 Tor Wager
%
%     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/>.
% ..
%
% :Inputs:
%
%   **D:**
%        a canlab_dataset object
%
%   **Yvarname:**
%        the name of a variable to predict. must be event level
%
%   **Xvarnames:**
%        the name(s) of predictor variables. if multiple, put in
%        cell array. must be event level
%
%   **wh_keep:**
%        a logical vector of 1/0 values
%
% :Outputs:
%   **b:**
%       a vector of coefficient estimates (same as for glmfit())
%
%   **dev:**
%       the deviance of the fit (same as for glmfit())
%
%   **stat:**
%       structure containing stats fields (see glmfit() documentation)
%

[Y, ~, levelY] = get_var(D, Yvarname, wh_keep);
Y = Y';

%% MUST IMPLEMENT GET_VAR FOR MULTIPLE VARS AT AN EVENT LEVEL.  RETURNS A CELL ARRAY FOR EACH PERSON.  DAT BECOMES WARNING STRING, DATCELL IS OF INTEREST.

[X, ~, levelX] = get_var(D, Xvarnames, wh_keep);

if levelY ~= 2 || levelX ~= 2, error('Vars must be event level'); end

n=size(Y,2);
X1 = cell(1,n);
X2 = mean(Y)'; % matrix of 2nd level preds

%{
if isstruct(varargin{1}) % the "alternative format" described above
    xstruct = varargin{1};
    fields = varargin{2};
    for i = 1:n % each subject
        clear myX
        for j = 1:length(fields) %all the fields
            myX(:, j) = xstruct.(fields{j}){i};
            X1{i} = myX;
        end
    getvif(X1{i})
    end

else
%}
    for i = 1:n % each subject
        clear myX
        for j = 1:length(varargin)
            myX(:, j) = varargin{j}{i};
            X1{i} = myX;
        end
    end
%end

stats = glmfit_multilevel(Y, X1, scale(X2, 1), 'weighted', 'noplots', ...
    'names', {'Intrcpt' names{2:end}}, 'beta_names', {'Avg within-ss relationship' ['Effect of Avg. ' names{1}]});

end


function [pvec, pstruct] = tapas_ph_binary_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2015 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

pvec    = NaN(1,length(ptrans));
pstruct = struct;

pvec(1)       = tapas_sgm(ptrans(1),1); % v_0
pstruct.v_0   = pvec(1);
pvec(2)       = tapas_sgm(ptrans(2),1); % al_0
pstruct.al_0  = pvec(2);
pvec(3)       = exp(ptrans(3));         % S
pstruct.S     = pvec(3);

return;


function [b,R,t,s] = FitEdges( im,xp,landmarks,shapePC,shapeMU,shapeEV,Ef,Ev,tri,ndims,w_prior, w_edges, w_landmarks,niter )
%FITEDGES Perform morphable model fitting using edges
%   This function initialises by fitting to a sparse set of landmarks, then
%   iteratively fits to edges by finding nearest neighbour between
%   projected model edges and image edges
%
% Inputs:
%   im        - Image to fit to
%   xp        - 2 by nlandmarks matrix containing locations of 2D landmarks
%   landmarks - Indices of vertices corresponding to points in xp
%   shapePC   - 3DMM principal components
%   shapeMU   - 3DMM average shape
%   shapeEV   - standard deviations of each 3DMM dimension
%   Ef        - nedges by 2 matrix storing faces adjacent to each edge
%   Ev        - nedges by 2 matrix storing vertices adjacent to each edge
%   tri       - Face structure of 3DMM mesh
%   ndims     - Number of model dimensions to use
%   numsd     - Hard limit on number of standard deviations each parameter
%               may deviate
%
% Outputs:
%   b         - Estimated shape parameters
%   R,t,s     - Estimated rotation, translation and scale of face

FV.faces = tri;
% PARAMETERS
% Number of model dimensions used for initial landmark fit
%ndims = 40;
% Number of edge fitting iterations (may want to replace with a convergence
% test)
%niter = 1;
% Proportion of nearest-neighbour edge matches used at each iteration, may
% be more sensible to threshold on distance to nearest neighbour
percentile = 1;

% Perform initial landmark-only fit
%w_prior = 0.7;
[b,R,t,s] = FitSingleSOP( xp,shapePC,shapeMU,shapeEV,ndims,landmarks,w_prior );
%b=zeros(20,1);
FV.vertices = reshape(shapePC(:,1:ndims)*b+shapeMU,3,size(shapePC,1)/3)';

% Display input image
%figure; imshow(im);

% Extract image edges
edges = edge(rgb2gray(im),'canny',0.15);
%figure; imshow(edges);

% Store pixel locations of edges
[r,c]=find(edges);
r = size(edges,1)+1-r;
%bold=b;
for iter=1:niter
    % Compute vertices lying on occluding boundary
    [ occludingVertices ] = occludingBoundaryVertices( FV,Ef,Ev,R );

    % Project occluding boundary vertices
    x2 = R*FV.vertices(occludingVertices,:)';
    x2 = x2(1:2,:);
    x2(1,:) = x2(1,:)+t(1);
    x2(2,:) = x2(2,:)+t(2);
    x2 = x2.*s;

    % Find edge correspondences
    [idx,d] = knnsearch([c r],x2'); %its correct, searching based on columns dont change!

    % Filter edge matches - probably want to do something better here
    sortedd=sort(d);
    threshold = sortedd(round(percentile*length(sortedd)));
    idx = idx(d<threshold);
    occludingVertices = occludingVertices(d<threshold);

    [b,R,t,s] = FitSingleSOP( [c(idx)' xp(1,:); r(idx)' xp(2,:)],shapePC,shapeMU,shapeEV,ndims,[occludingVertices; landmarks],w_edges,w_landmarks,length(landmarks));
    % Note: this is completely refitting from scratch. We could just
    % re-start the nonlinear optimisation using previous estimates as
    % initialisation but this doesn't seem to work well.
    %disp(num2str(norm(b-bold)));
    %differb(iter)=norm(b-bold);
    FV.vertices = reshape(shapePC(:,1:ndims)*b+shapeMU,3,size(shapePC,1)/3)';
    %bold=b;
end

end


%	Bloch simulator using mex file
%   [mx,my,mz] = blochCim(b1,gr,tp,T1,T2,freq,pr,mode,sens,mx0,my0,mz0);
%
%	Bloch simulation of rotations due to B1, gradient and
%	off-resonance, including relaxation effects.  At each time
%	point, the rotation matrix and decay matrix are calculated.
%	Simulation can simulate the steady-state if the sequence
%	is applied repeatedly.This code can also simulate the parallel transmit.
%
%   INPUT:
%   b1 = (ntime * ncoil) RF pulse, complex number in (Gauss).
%   gr = (ntime * 3) gradient in (Gauss/cm).
%   tp = (ntime * 1) time duration of each b1 and gr point, in (sec),
%				or (1 * 1) time step if constant for all points
%				or monotonically INCREASING endtime of each
%				interval..
%   T1, T2 = (1 * 1) relaxation time in (sec)
%   freq = (nx * ny * nz) or (npos * 1) off-resonance freq. in (Hz)
%   pr = (npos * 3) array of spatial positions in (cm)
%   mode= Bitmask simulation mode: (default 0)
%		Bit 0:  0-Simulate from start, 1-Steady State
%		Bit 1:  1-Record m at time points.  0-just end time.%
%       Bit 2:  0-display simulation information. 1-only print out warnings.
%
%
%   (OPTIONAL)
%   sens = (ncoils * npos or ncoils * nx*ny*nz) sensitivities matrix.
%   default is 1 for all position. Set to default if you put 1 here.
%   mx0,my0,mz0 = (npos * 1) initial magnetization
%   default is (0,0,1) for all position
%
%	OUTPUT:
%		mx,my,mz = npos x 1 arrays of the resulting magnetization
%				components at each position.
%
%	The code is originally downloaded from Brian Hargreaves's website.
%
%   Hao Sun made the following main changes:
%   (1)match field map with position
%   (2)correct the minus sign error in the 'b1imag' in the blochsimfz sub
%   function in Brian's code
%   (3)add parallel simulation function
%
%
%   Hao Sun, the University of Michigan, Jul 22,2012


function [cg,theta] = cgmo(im,radius,norient,varargin)
% function [cg] = cgmo(im,radius,norient,...)
%
% Compute the color gradient at a single scale and multiple
% orientations.
%
% INPUT
%	im		Grayscale or RGB image, values in [0,1].
%	radius		Radius of disc for cg.
%	norient		Number of orientations for cg.
%	'nbins'		Number of bins; should be > 1/sigmaSim.
%	'sigmaSim'	For color similarity function.
%	'gamma'		Gamma correction for LAB [2.5].
%	'smooth'	Smoothing method, one of
%			{'gaussian','savgol','none'}, default 'none'.
%	'sigmaSmo'	Sigma for smoothing, default to radius.
%
% OUTPUT
%	cg		Size [h,w,d,norient] array of cg images,
%			where d is the dimensionality of the image.
%
% The input parameters {radius,nbins,sigmaSim,sigmaSmo} should be
% scalars when the input image is grayscale, and can be either scalars
% or 3-element vectors when the image is RGB.
%
% See also cgmo.
%
% David R. Martin <dmartin@eecs.berkeley.edu>
% April 2003

% process options
nbins = 32;
sigmaSim = 0.1;
gamma = 2.5;
smooth = 'none';
sigmaSmo = radius;
for i = 1:2:numel(varargin),
  opt = varargin{i};
  if ~ischar(opt), error('option names not a string'); end
  if i==numel(varargin), error(sprintf('option ''%s'' has no value',opt)); end
  val = varargin{i+1};
  switch opt,
   case 'nbins', nbins=val;
   case 'sigmaSim', sigmaSim=val;
   case 'gamma', gamma=val;
   case 'smooth',
    switch val,
     case {'none','gaussian','savgol'}, smooth=val;
     otherwise, error(sprintf('invalid option smooth=''%s''',val));
    end
   case 'sigmaSmo', sigmaSmo=val;
   otherwise, error(sprintf('invalid option ''%s''',opt));
  end
end

% check arguments
if ndims(im)==2, % grayscale image
  if numel(radius)~=1, error('radius should have 1 element'); end
  if numel(nbins)~=1, error('nbins should have 1 element'); end
  if numel(sigmaSim)~=1, error('sigmaSim should have 1 element'); end
  if numel(sigmaSmo)~=1, error('sigmaSim should have 1 element'); end
elseif ndims(im)==3, % RGB image
  if numel(radius)==1, radius = radius*ones(3,1); end
  if numel(nbins)==1, nbins = nbins*ones(3,1); end
  if numel(sigmaSim)==1, sigmaSim = sigmaSim*ones(3,1); end
  if numel(sigmaSmo)==1, sigmaSmo = sigmaSmo*ones(3,1); end
  if numel(radius)~=3, error('radius should have 1 or 3 elements'); end
  if numel(nbins)~=3, error('nbins should have 1 or 3 elements'); end
  if numel(sigmaSim)~=3, error('sigmaSim should have 1 or 3 elements'); end
  if numel(sigmaSmo)~=3, error('sigmaSmo should have 1 or 3 elements'); end
  radius = radius(:);
  nbins = nbins(:);
  sigmaSim = sigmaSim(:);
  sigmaSmo = sigmaSmo(:);
else
  error('image not of valid dimension');
end
norient = max(1,norient);
nbins = max(1,nbins);

% min and max values for a,b channels of LAB
% used to scale values into the unit interval
abmin = -73;
abmax = 95;

% make sure nbins is large enough with respect to sigmaSim
if any( nbins < 1./sigmaSim ),
  warning('nbins < 1/sigmaSim is suspect');
end

% check pixel valies
if min(im(:)) < 0 | max(im(:))>1,
  error('pixel values out of range [0,1]');
end

if ndims(im)==2, % grayscale image

  % compute cg from gray values
  cmap = max(1,ceil(im*nbins));
  csim = colorsim(nbins,sigmaSim);
  [cg,theta] = tgmo(...
      cmap,nbins,radius,norient,...
      'tsim',csim,'smooth',smooth,'sigma',sigmaSmo);

else, % RGB image

  % convert gamma-corrected image to LAB and scale values into [0,1]
  lab = RGB2Lab(im.^gamma);
  lab(:,:,1) = lab(:,:,1) ./ 100;
  lab(:,:,2) = (lab(:,:,2) - abmin) ./ (abmax-abmin);
  lab(:,:,3) = (lab(:,:,3) - abmin) ./ (abmax-abmin);
  lab(:,:,2) = max(0,min(1,lab(:,:,2)));
  lab(:,:,3) = max(0,min(1,lab(:,:,3)));

  % compute cg from LAB values
  cg = zeros([size(im) norient]);
  for i = 1:3,
    cmap = max(1,ceil(lab(:,:,i)*nbins(i)));
    csim = colorsim(nbins(i),sigmaSim(i));
    [cg(:,:,i,:),theta] = tgmo(...
        cmap,nbins(i),radius(i),norient,...
        'tsim',csim,'smooth',smooth,'sigma',sigmaSmo(i));
  end

end

% compute color similarity matrix assuming colors are in [0,1]
function m = colorsim(nbins,sigma)
bc = ((1:nbins)-0.5)/nbins; % bin centers
[x,y] = meshgrid(bc,bc);
m = 1.0 - exp(-abs(x-y).^2./(2*sigma.^2));