function Show_EWT(ewt,f,rec)

% ====================================================================
% function Show_EWT(f,ewt)
%
% This function plots the successive filtered components (low scale
% first and then wavelets scales). The original and
% reconstructed signals are plotted on a different graph.
% If f and rec are provided, it also plot the original and reconstructed
% signals on a separate figure
%
% Inputs:
%   -ewt: EWT components
%   -f: input signal  (OPTIONNAL)
%   -rec: reconstructed signal (OPTIONNAL)
%
% Author: Jerome Gilles
% Institution: UCLA - Department of Mathematics
% Year: 2012
% Version: 1.0
% =====================================================================

%These lines plot the EWT components
figure;
x=0:1/length(ewt{1}):(length(ewt{1})-1)/length(ewt{1});
l=1;
if length(ewt)>6
    lm=6;
else
    lm=length(ewt);
end

for k=1:length(ewt)
   hold on; subplot(lm,1,l); plot(x,ewt{k});
   if mod(k,6) == 0
        figure;
        l=1;
   else
    l=l+1;
   end
end

%These lines plot f and its reconstruction
if nargin>1
    figure;
    subplot(2,1,1);plot(x,f);
    subplot(2,1,2);plot(x,rec);
end


function test_suite = test_triangleArea3d
%TEST_TRIANGLEAREA3D  Test case for the file triangleArea3d
%
%   Test case for the file triangleArea3d

%   Example
%   test_triangleArea3d
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-08-23,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

function test_Simple(testCase) %#ok<*DEFNU>

% rectangle triangle with sides 20 and 30 -> area is 300.
p1 = [10 20 30];
p2 = [30 20 30];
p3 = [10 50 30];

exp = 300;

area = triangleArea3d(p1, p2, p3);
testCase.assertEqual(exp, area);
area = triangleArea3d(p2, p3, p1);
testCase.assertEqual(exp, area);
area = triangleArea3d(p3, p1, p2);
testCase.assertEqual(exp, area);

area = triangleArea3d(p3, p2, p1);
testCase.assertEqual(exp, area);
area = triangleArea3d(p1, p3, p2);
testCase.assertEqual(exp, area);
area = triangleArea3d(p2, p1, p3);
testCase.assertEqual(exp, area);

area = triangleArea3d([p1; p2; p3]);
testCase.assertEqual(exp, area);

function test_XZ(testCase)

% rectangle triangle with sides 20 and 30 -> area is 300.
p1 = [10 30 20];
p2 = [30 30 20];
p3 = [10 30 50];
exp = 300;

area = triangleArea3d(p1, p2, p3);
testCase.assertEqual(exp, area);

area = triangleArea3d([p1; p2; p3]);
testCase.assertEqual(exp, area);

function test_YZ(testCase)

% rectangle triangle with sides 20 and 30 -> area is 300.
p1 = [30 10 20];
p2 = [30 30 20];
p3 = [30 10 50];
exp = 300;

area = triangleArea3d(p1, p2, p3);
testCase.assertEqual(exp, area);

area = triangleArea3d([p1; p2; p3]);
testCase.assertEqual(exp, area);


 function y = ir_fftshift2(x)
%function y = ir_fftshift2(x)
%|
%| 2D fft shift of 2D+T object

if nargin < 1, ir_usage, end

y = fftshift(fftshift(x, 1), 2);


function [sourcemodel] = patchsvd(cfg, sourcemodel)

% This function does something like Limpiti et al.
% IEEE trans biomed eng 2006;53(9);1740-54

% Copyright (c) 2006, Jan-Mathijs Schoffelen & Robert Oostenveld, F.C. Donders Centre
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% set the defaults
if ~isfield(cfg, 'patchsvd'),    cfg.patchsvd    = 3;         end
if ~isfield(cfg, 'patchsvdnum'), cfg.patchsvdnum = 5;         end
if ~isfield(cfg, 'feedback'),    cfg.feedback    = 'textbar'; end

if isnumeric(cfg.patchsvd),
  Ndipoles = size(sourcemodel.pos,1);
else
  Ndipoles = 1;
end
Ninside  = length(sourcemodel.inside);
Nchans   = size(sourcemodel.leadfield{sourcemodel.inside(1)}, 1);
lfall    = cell(1,Ndipoles);
coeff    = cell(1,Ndipoles);
nghbr    = cell(1,Ndipoles);

if isnumeric(cfg.patchsvd) && ~isfield(sourcemodel, 'patchindx'),
  fprintf('computing patches in 3D, not taking topology into account\n');
  progress('init', cfg.feedback, 'computing patchsvd');
  for dipindx=1:Ninside
    % renumber the loop-index variable to make it easier to print the progress bar
    i = sourcemodel.inside(dipindx);

    % compute the distance from this dipole to each other dipole
    dist = sqrt(sum((sourcemodel.pos-repmat(sourcemodel.pos(i,:), [Ndipoles 1])).^2, 2));

    % define the region of interest around this dipole
    sel  = find(dist<=cfg.patchsvd);
    sel  = intersect(sel, sourcemodel.inside);
    Nsel = length(sel);

    progress(dipindx/Ninside, 'computing patchsvd %d/%d, Nsel=%d\n', dipindx, Ninside, Nsel);
    % concatenate the leadfield of all dipoles that are inside the ROI into one matrix
    lfr     = cell2mat(sourcemodel.leadfield(sel(:)'));
    % svd of leadfields of dipoles inside the ROI
    [U,S,V] = svd(lfr);

    if cfg.patchsvdnum < 1,
      % Limpiti et al 2006 formula 12
      s          = diag(S).^2;
      s          = cumsum(s)./sum(s);
      n(dipindx) = find(s - cfg.patchsvdnum > 0, 1);
    else
      n(dipindx) = cfg.patchsvdnum;
    end
    lfall{i} = U(:,1:n(dipindx)); %*S(1:n(dipindx),1:n(dipindx)); %klopt dit?
    nghbr{i} = sel;
    coeff{i} = V(1:n(dipindx), :);
  end
  progress('close');
elseif isnumeric(cfg.patchsvd) && isfield(sourcemodel, 'patchindx'),
  fprintf('computing patches in 2D, taking surface topology into account\n');
  progress('init', cfg.feedback, 'computing patchsvd');
  for dipindx=1:Ninside
    % renumber the loop-index variable to make it easier to print the progress bar
    i = sourcemodel.inside(dipindx);

    % define the region of interest around this dipole
    sel  = nearest(sourcemodel.patchsize(i,:), cfg.patchsvd);
    sel  = sourcemodel.patchindx(i,1:sel);
    Nsel = length(sel);

    progress(dipindx/Ninside, 'computing patchsvd %d/%d, Nsel=%d\n', dipindx, Ninside, Nsel);
    % concatenate the leadfield of all dipoles that are inside the ROI into one matrix
    lfr     = cell2mat(sourcemodel.leadfield(sel(:)'));
    % svd of leadfields of dipoles inside the ROI
    [U,S,V] = svd(lfr);

    if cfg.patchsvdnum < 1,
      % Limpiti et al 2006 formula 12
      s          = diag(S).^2;
      s          = cumsum(s)./sum(s);
      n(dipindx) = find(s - cfg.patchsvdnum > 0, 1);
    else
      n(dipindx) = min(cfg.patchsvdnum, size(lfr,2));
    end
    lfall{i} = U(:,1:n(dipindx)); %*S(1:n(dipindx),1:n(dipindx)); %klopt dit?
    nghbr{i} = sel;
    coeff{i} = V(1:n(dipindx), :);
    sv{i}    = s;
  end
  progress('close');
elseif strcmp(cfg.patchsvd, 'all'),
  lfr     = cell2mat(sourcemodel.leadfield(sourcemodel.inside(:)'));
  [U,S,V] = svd(lfr);

  if cfg.patchsvdnum < 1,
    % Limpiti et al 2006 formula 12
    s = diag(S).^2;
    s = cumsum(s)./sum(s);
    n = find(s - cfg.patchsvdnum > 0, 1);
  else
    n = cfg.patchsvdnum;
  end
  lfall{1} = U(:,1:n); %*S(1:n(dipindx),1:n(dipindx)); %klopt dit?
  nghbr{1} = sourcemodel.inside;
  coeff{1} = V(1:n, :);

  %---change output
  sourcemodel.pos    = mean(sourcemodel.pos(sourcemodel.inside,:),1);
  sourcemodel.inside = 1;
  sourcemodel.dim    = [1 1 1];
  sourcemodel.xsourcemodel  = sourcemodel.pos(1);
  sourcemodel.ysourcemodel  = sourcemodel.pos(2);
  sourcemodel.zsourcemodel  = sourcemodel.pos(3);
else
  %do nothing
end

if ~all(n==n(1)),
  nmax = max(n);
  for dipindx = 1:Ninside
    i = sourcemodel.inside(dipindx);
    if n(dipindx)<nmax,
      lfall{i} = [lfall{i} zeros(Nchans, nmax-n(dipindx))];
    end
  end
end

% update the leadfields
sourcemodel.leadfield = lfall;
%sourcemodel.expcoeff  = coeff;
sourcemodel.neighbours= nghbr;


% use EKF to estimate gyro and accel biases, for
% MEMS IMU grade close to microstrain 3dm gx3-35
% s0 frame of the sensor coincides a earth-fixed n-frame(NED) at epoch t0, n0.

% The state
% \f$ p_S^N \f$ position of the IMU in n0-frame
% \f$ v_N \f$ velocity of the IMU in n0-frame
% \f$ q_N^S \f$ quaternion representing the rotation from n0-frame to IMU frame
% \f$ b_a \f$ accelerometer bias
% \f$ b_g \f$ gyroscope bias

% The error state
% \f$ \delta p_S^N = \hat{p}_S^N - p_S^N \f$
% \f$ \delta v^N = \hat{v}^N - v^N \f$
% \f$ \psi^E \f$ Error in the rotation from n0-frame to IMU frame $C_N^S$
% defined as \f$ C_N^S = (I + \psi \times) \hat{C}_N^S \f$
% \f$ \delta b_a = b_a - \hat{b}_a \f$ accelerometer bias
% \f$ \delta b_g = b_g - \hat{b}_g \f$ gyro bias

classdef EKF_filter_s0frame_bias < handle
    properties (Hidden)
        type = 'ekf';
        tag  = 'EKF_GPS_IMU_GRAVITY';  % ID tag
    end
    % The following properties can be set only by class methods
    properties (SetAccess = private)
        stateTime; % state timestamp
        invalidateIMUerrors; % reset IMU errors when they goes beyond expectation
        imuType; % the type of IMU, e.g., MEMS 3DX GM 3-35, Steval inems, etc.
        imuErrorModel=3; % the IMU error model corresponding to biases
        dt; % IMU sampling interval, unit sec
        rqs02e; % position of s0 in the e frame, qs02e, constant
        rvqs0; % position of current s(k) frame in s0 frame, velocity in s0 frame, q s0 to s(k)
        imuErrors; %ba, bg
        imuOrientSIP=7; % imu orientation covariance start index in s-frame
        imuBiasDriftSIP=10; %imu bias error covariance start index
        covDim=15; % the dimension of P matrix
        velNoiseStd; % noise density driving horizontal velocity
        p_k_k; % the covariance of the entire state vector
    end
    methods
        function filter = EKF_filter_s0frame_bias(options)
            filter.stateTime = options.startTime;
            filter.invalidateIMUerrors=options.InvalidateIMUerrors;
            filter.imuType=options.imutype;
            filter.imuErrorModel=options.imuErrorModel;
            filter.dt=options.dt;

            inillh=options.inillh_ant;
            Ce2n0=llh2dcm_v000(inillh(1:2),[0;1]);
            filter.rqs02e=[ecef2geo_v000(options.inillh_ant,1); rotro2qr(Ce2n0')];
            % assume the IMU is very close to antenna
            filter.rvqs0=zeros(10,1);

            qs2n=quatmult_v001(options.qb2n,rotro2qr(options.Cb2imu),2);
            filter.rvqs0(7:10)=[qs2n(1);-qs2n(2:4)];
            filter.rvqs0(4:6)=options.Vn;
            filter.rvqs0(1:3) = quatrot_v000(qs2n, -options.imu_p_ant, 0);

            filter.imuErrors=options.imuErrors;
            filter.velNoiseStd=options.velNoiseStd;
            filter.p_k_k=zeros(filter.covDim);    % in s0 frame, position, velocity, attitude are of very small variance

            filter.p_k_k(1:3,1:3)=diag([0.2, 0.2, 0.5].^2); %position errors in meter
            filter.p_k_k(4:6,4:6)=diag([0.1, 0.1, 0.2].^2); %vel error in m/s
            filter.p_k_k(filter.imuOrientSIP+(0:2),filter.imuOrientSIP+(0:2))=...
                diag(([options.initAttVar,options.initAttVar,options.initAttVar*3]).^2);
            IMU_ERRDEF=imu_err_defs_v000(options.imutype);
            filter.p_k_k(filter.imuBiasDriftSIP+(0:2),filter.imuBiasDriftSIP+(0:2))=4*eye(3)*IMU_ERRDEF.acc_bias_var;% enlarge initial std by 2
            filter.p_k_k(filter.imuBiasDriftSIP+(3:5),filter.imuBiasDriftSIP+(3:5))=4*eye(3)*IMU_ERRDEF.gyro_bias_var;

            filter.p_k_k=sparse (filter.p_k_k);
        end
        %===============================================================================================
        %-- State transition function
        % propogate state with accelerometer and gyro input at time k-1 to predict state at k,
        % i.e., X(k|k-1), from state at k-1. U1 contains IMU measurement
        % acc, and gyro angular rate, or delta v, and delta theta
        % and 7th row is the previous epoch(k-1) and 8th is the current
        % epoch(k). Optionally, 9-11th rows are preset gravity in s0 frame,
        % 12-14 th row is $\omega_{ie}_{s_0}$, angular rate of local s0-frame w.r.t i-frame
        % represented in s0-frame. We also call s0 frame as w-frame
        % imuaccum records the accumulated delta v and delta angle for two speed
        % covariance update
        function imuaccum= ffun_state(filter, imuaccum, U1, isDelta, isConstantVel)
            if(isDelta)
                dt1=U1(8,end)-U1(7,1);
                gyroinc=sum(U1(4:6,:),2);
                accinc=sum(U1(1:3,:),2);

                %reset the imu errors if estimates diverges
                IMU_ERRDEF=imu_err_defs_v000(filter.imuType);

                if(filter.invalidateIMUerrors)
                    spuracc=find(abs(filter.imuErrors(1:3))>2*IMU_ERRDEF.initacc_bias_err,1);
                    spurgyro=find(abs(filter.imuErrors(4:6))>2*IMU_ERRDEF.initgyro_bias_err,1);
                    if(~isempty(spuracc)||~isempty(spurgyro))
                        disp(['IMU errors diverge at ' num2str(U1(8,end)) '!']);
                        filter.imuErrors=zeros(6,1);
                    end
                end
                angleinc=gyroinc-filter.imuErrors(4:6)*dt1;
                velinc=accinc-filter.imuErrors(1:3)*dt1;
                %imu data accumulator for the covariance update
                imuaccum=imuaccum+[velinc;angleinc];
                gyroinc=angleinc/dt1;
                accinc=velinc/dt1;
            else
                dt1=U1(8,end)-U1(7,1);
                gyroinc=mean(U1(4:6,:),2);
                accinc=mean(U1(1:3,:),2);
                %reset the imu errors if estimates diverges
                IMU_ERRDEF=imu_err_defs_v000(filter.imuType);
                if(filter.invalidateIMUerrors)
                    spuracc=find(abs(filter.imuErrors(1:3))>2*IMU_ERRDEF.initacc_bias_err,1);
                    spurgyro=find(abs(filter.imuErrors(4:6))>2*IMU_ERRDEF.initgyro_bias_err,1);
                    if(~isempty(spuracc)||~isempty(spurgyro))
                        disp(['IMU errors diverge at ' num2str(U1(8,end)) '!']);
                        filter.imuErrors=zeros(6,1);
                    end
                end
                gyroinc=gyroinc-filter.imuErrors(4:6);
                accinc=accinc-filter.imuErrors(1:3);
                %imu data accumulator for the covariance update
                imuaccum=imuaccum+[accinc;gyroinc]*dt1;
            end
            if(size(U1,1)>8)
                gwomegaw=U1(9:end,:);
            else
                %Gravity (most time consuming part of ecef implementations)
                xyz_imu=filter.rqs02e(1:3)+quatrot_v000(filter.rqs02e(4:7),filter.rvqs0(1:3),0);
                Llh=ecef2geo_v000(xyz_imu,0);
                Cn2e=pos2Cne_v000(Llh(1), Llh(2));
                [Rn, Re, gn, sL, cL, WIE_E]=geoparam_v000(Llh);
                gwomegaw=[quatrot_v000(filter.rqs02e(4:7), Cn2e*[0;0;gn], 1);quatrot_v000(filter.rqs02e(4:7),[0;0;WIE_E],1)];
            end
            if(~isConstantVel)
                filter.rvqs0=strapdown_local_quat_bias(filter.rvqs0, filter.rqs02e, accinc, gyroinc, dt1, gwomegaw);
            else
                filter.rvqs0=strapdown_local_quat_constvel(filter.rvqs0, filter.rqs02e, accinc, gyroinc, dt1, gwomegaw);
            end
            filter.stateTime = U1(8);
        end
        function ffun_covariance(filter, imuaccum, covupt_time, curimutime, isConstantVel )
            % propagate the covariance in the local s0 frame
            % the covariance corresponds to states,
            % rs in s0, v s in s0, q s0 2s, ba, bg
            covdt=curimutime-covupt_time;
            if(~isConstantVel)
            [STM, Qd]=sys_local_dcm_bias(filter.rqs02e, filter.rvqs0,...
                imuaccum(1:3)/covdt,imuaccum(4:6)/covdt, covdt,filter.imuType, filter.imuErrorModel);
            else
                [STM, Qd]=sys_local_dcm_constvel(filter.rqs02e, filter.rvqs0,...
                imuaccum(1:3)/covdt,imuaccum(4:6)/covdt, covdt,filter.imuType, filter.velNoiseStd);
            end

            Pvf = STM*filter.p_k_k*STM'+Qd;  % the covariance of the navigation states and imu error terms
            filter.p_k_k=sparse(Pvf);
        end
        %==============================================================================================
        function applied = correctstates(filter, predict, measure, H, R, measurementTime, gatingtest)
            % Suppose the prediction function is h, and the measurement is
            % z, then z = h(p, v, q, ba, bg). Formally,
            % \f$ H_p = - \frac{\partial h(p \oplus \delta)}{\partial \delta} \f$
            % \f$ H_\psi = - \frac{\partial h(C(q) \oplus \psi)}{\partial
            % \psi} \f$. Note in this update function,
            % \f$ p \oplus \delta = p - \delta \f$, and
            % \f$ C(q) \oplus \psi = \exp(\psi) C(q) \f$.

            if nargin < 7
                gatingtest = true;
            end
            if nargin < 6
                measurementTime = filter.stateTime;
            else
                if (measurementTime > filter.stateTime)
                    fprintf('Warn: Measurement time %.6f  >= state time %.6f\n', measurementTime, filter.stateTime);
                end
            end
            p_km1_k=filter.p_k_k;
            inno=predict-measure;
            S=H*p_km1_k*H'+R;
            if gatingtest
                gamma=inno'/S*inno;
                tol = chi2inv(0.99, 3);
                if gamma > tol
                    fprintf(['Discard measurement at %.6f with large ', ...
                             'gamma %.4f > tol %.4f.\n'], measurementTime, gamma, tol);
                    applied = false;
                    return;
                end
            end
            K=p_km1_k*H'/S;
            deltaX=K*inno;

            filter.p_k_k=(eye(size(p_km1_k,1))-K*H)*p_km1_k*(eye(size(p_km1_k,1))-K*H)'+K*R*K';
            % compute updated states
            filter.rvqs0(1:6)=filter.rvqs0(1:6)-deltaX(1:6); % position and velocity
            qst=rvec2quat_v000(deltaX(7:9));
            filter.rvqs0(7:10)=quatmult_v000(qst, filter.rvqs0(7:10));
            filter.imuErrors = filter.imuErrors + deltaX(filter.imuBiasDriftSIP:end);
            applied = true;
        end

        function SaveToFile(filter, ~, preimutime, ffilres)
            formatString = ['%.8f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.8f,%.8f,%.8f,%.8f,', ...
                '%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.8f,%.8f,%.8f\n'];
            fprintf(ffilres, formatString, [preimutime;filter.rvqs0(1:6); ...
                [-filter.rvqs0(8:10); filter.rvqs0(7)];...
                full(sqrt(diag(filter.p_k_k(1:9,1:9))))]);
        end
        function mag=GetVelocityMag(filter)
            mag=norm(filter.rvqs0(4:6),2);
        end
    end
end


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

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

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


function [g1, g2] = lfmwhiteXwhiteKernGradient(lfmKern, whiteKern, t1, varargin)

% LFMWHITEXWHITEKERNGRADIENT Compute gradient between the LFM-WHITE and
% WHITE kernels.
% FORMAT
% DESC computes the gradient of an objective function with respect
% to cross kernel terms between LFM-WHITE and WHITE kernels for
% the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFM-WHITE
% kernel.
% ARG whiteKern : the kernel structure associated with the WHITE
% kernel.
% ARG t1 : inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of objective function with respect to kernel
% parameters of LFM-WHITE kernel.
% RETURN g2 : gradient of objective function with respect to kernel
% parameters of WHITE kernel.
%
% FORMAT
% DESC computes the gradient of an objective function with respect
% to cross kernel terms between LFM-WHITE and WHITE kernels for
% the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFM-WHITE
% kernel.
% ARG whiteKern : the kernel structure associated with the WHITE
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of objective function with respect to kernel
% parameters of LFM-WHITE kernel.
% RETURN g2 : gradient of objective function with respect to kernel
% parameters of WHITE kernel.
%
% SEEALSO : multiKernParamInit, multiKernCompute, lfmwhiteKernParamInit,
% whiteKernParamInit
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 5
    t2 = t1;
else
    t2 = varargin{1};
end
covGrad = varargin{end};

if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end
if lfmKern.variance ~= whiteKern.variance
  error('Kernels cannot be cross combined if they have different variances.')
end

g1 = zeros(1,3);
g2 = 0; % The only parameter of the WHITE kernel (its variance) is already
        % accounted for in g1

mass = lfmKern.mass;
spring = lfmKern.spring;
damper = lfmKern.damper;
variance = lfmKern.variance;
sensitivity = lfmKern.sensitivity;
alpha = lfmKern.alpha;
omega = lfmKern.omega;

T1 = repmat(t1, 1, size(t2, 1));
T2 = repmat(t2.', size(t1, 1), 1);
deltaT = T1-T2;
indT = (T1 >= T2);
c = 1 / (mass * omega);
K0 = c * exp(-alpha*deltaT) .* indT;
K1 = K0 .* sin(omega*deltaT);
K2 = K0 .* cos(omega*deltaT);

gradMass = [1 0 0];
gradAlpha = [-damper/(2*mass^2) 1/(2*mass) 0];
c2 = sqrt(4*mass*spring-damper^2);
gradOmega = [(damper^2-2*mass*spring)/(2*c2*mass^2) -damper/(2*c2*mass) 1/c2];

% Gradient w.r.t. m_q
g1(1) = variance * sensitivity * sum(sum((gradOmega(1) * (T1-T2) .* K2 ...
    - gradAlpha(1) * (T1-T2) .* K1 ...
    - (gradMass(1)/mass + gradOmega(1)/omega) * K1) .* covGrad));

% Gradient w.r.t. D_q
g1(2) = variance * sensitivity * sum(sum((gradOmega(3) * (T1-T2) .* K2 ...
    - gradAlpha(3) * (T1-T2) .* K1 ...
    - (gradMass(3)/mass + gradOmega(3)/omega) * K1) .* covGrad));

% Gradient w.r.t. C_q
g1(3) = variance * sensitivity * sum(sum((gradOmega(2) * (T1-T2) .* K2 ...
    - gradAlpha(2) * (T1-T2) .* K1 ...
    - (gradMass(2)/mass + gradOmega(2)/omega) * K1) .* covGrad));

% Gradient w.r.t. sigma_r^2
g1(4) = sensitivity * sum(sum(K1 .* covGrad));

% Gradient w.r.t. S_{qr}
g1(5) = variance * sum(sum(K1 .* covGrad));

% Ensuring that g1 is real
g1 = real(g1);


function [x,fval,exitflag,info] = opti_csdp(f,A,b,lb,ub,sdcone,x0,opts)
%OPTI_CSDP Solve a SDP using CSDP [Primal Form]
%
%   min f'*x      subject to:     A*x <= b
%    x                            lb <= x <= ub
%                                 X = F1*x1 + F2*x2 + ... + Fn*xn - F0
%                                 X >= 0 [positive semidefinite]
%
%
%
%   x = opti_csdp(f,A,b,lb,ub) solves a LP where f is the objective
%   vector, A,b are the linear inequality constraints, and lb,ub are the
%   bounds.
%
%   x = opti_csdp(f,...,ub,sdcone) uses the cell array sdcone to pass
%   semidefinite constraints to the solver. Each cell is a sparse matrix of
%   the form [F0(:) F1(:) F2(:)...] (or in alternative notation, [C A0 A1 A2..])
%   where each F matrix has been converted to a column vector and concatenated.
%
%   x = opti_csdp(f,...,x0,opts) uses opts to pass optiset options to the
%   solver.
%
%   [x,fval,exitflag,info] = opti_csdp(...) returns the objective value at
%   the solution, together with the solver exitflag, and an information
%   structure.
%
%   THIS IS A WRAPPER FOR CSDP USING THE MEX INTERFACE
%   See supplied Eclipse Public License

%   Copyright (C) 2013 Jonathan Currie (I2C2)

t = tic;

% Handle missing arguments
if nargin < 8, opts = optiset; end
if nargin < 7, x0 = []; end
if nargin < 6, sdcone = []; end
if nargin < 5, ub = []; end
if nargin < 4, lb = []; end
if nargin < 3, error('You must supply at least 3 arguments to opti_csdp'); end

warn = strcmpi(opts.warnings,'all');

%Check sparsity
if(~isempty(A) && ~issparse(A))
    if(warn), optiwarn('opti:sparse','The A matrix should be sparse, correcting: [sparse(A)]'); end
    A = sparse(A);
end
if(~isempty(sdcone))
    ok = 1;
    if(iscell(sdcone))
        for i = 1:length(sdcone)
            if(~issparse(sdcone{i}))
                sdcone{i} = sparse(sdcone{i});
                ok = 0;
            end
        end
    elseif(~issparse(sdcone))
        sdcone = sparse(sdcone);
        ok = 0;
    end
    if(~ok && warn)
        optiwarn('opti:sparse','The Semidefinite Constraints should be sparse, correcting: [sparse(sdcone)]');
    end
end

%Addin csdp settings if specified
if(isfield(opts,'solverOpts') && ~isempty(opts.solverOpts))
    copts = csdpset(opts.solverOpts);
else
    copts = [];
end
%Add OPTI Options
copts.maxtime = opts.maxtime;
copts.maxiter = opts.maxiter;
copts.display = dispLevel(opts.display);

%MEX contains error checking
[x,fvals,exitflag,stats,X] = csdp(f, A, b, lb, ub, sdcone, x0, copts);

%Assign primal fval
fval = fvals.pval;

%Assign Outputs
info.Iterations = stats.iter;
info.Time = toc(t);
info.Algorithm = 'CSDP: Predictor-Corrector Primal-Dual SDP Solver';

switch(exitflag)
    case 0
        info.Status = 'CSDP Converged';
        exitflag = 1;
    case 1
        info.Status = 'Primal Infeasible';
        exitflag = -1;
    case 2
        info.Status = 'Dual Infeasible';
        exitflag = -1;
    case 3
        info.Status = 'Partial Success (Full Accuracy Not Achieved)';
        exitflag = 3;
    case 4
        info.Status = 'Exceeded Maximum Iterations';
        exitflag = 0;
    case 5
        info.Status = 'Stuck at Edge of Primal Feasibility';
        exitflag = -2;
    case 6
        info.Status = 'Stuck at Edge of Dual Infeasibility';
        exitflag = -2;
    case 7
        info.Status = 'Lack of Progress';
        exitflag = -3;
    case 8
        info.Status = 'X, Z or O was singular';
        exitflag = -3;
    case 9
        info.Status = 'Detected NaN or Inf';
        exitflag = -3;
    case 10
        info.Status = 'General easy_sdp() failure';
        exitflag = -3;
    case 11
        info.Status = 'Failed check on C (check symmetry)';
        exitflag = -3;
    case 12
        info.Status = 'Failed constraint check';
        exitflag = -3;
    case -27
        info.Status = 'Exceeded Maximum Time';
        exitflag = 0;
    case -50
        info.Status = 'User Exited';
        exitflag = -5;
    otherwise
        info.Status = [];
end

info.DualObjective = fvals.dval;
info.X = X;


classdef BayesianfastF0NLS< handle

%     usuage

% obj= BayesianfastF0NLS(N, L, pitchBounds, A_var,voicingProb,varargin)
%  N is the frame length
%  L is the maximum allowed harmonic order
%  pitchBounds denotes the pitch range e.g.,[f_min/fs, f_max/fs], where fs is
%  the sampling frequency
%  A_var denotes the frequncy transition variance.
%  voicingProb denotes the initial voicing probability

    % Can only be set in the constructor
    properties (SetAccess=immutable)
        N
        L
        F
        pitchBoundsOuter = [0.0 0.5] % The outer bounds that are acceptable

        % default values
        epsilon = 0.0
        dcIsIncluded = false
        epsilon_ref = 0.0

        % Precomputed quantities
        crossCorrelationVectors
        Gamma1
        Gamma2
        fftShiftVector
    end

    properties (SetAccess=private)
        pitchBounds  % The current active bounds
        fullPitchGrid
        validFftIndices
        defaultRefinementTol
        refinementTol
        gPriorParam = 3
        logPitchPdfs % the pitch pdf (column) for every candidate model order
        logModelPmf % an (L+1)x1 vector
         logpi
        A
        scaled_alpha_buffer
        scaled_alpha_buffer2
        unvoicing_scaled_alpha_buffer
        logModelPrior;
        %% varSpeech is a regularization parameter and it should be chosen according to the input SNR
        % In high SNR, a larger value leads to better performance from our
        % experience.
        varSpeech=5e-6;
        %%
        B
        C
        norml_factor=0;
        cnt=1;
        prew_state
    end

    methods

        function obj = BayesianfastF0NLS(N, L, pitchBounds, A_var,voicingProb,varargin)
            switch nargin
                case 0
                     N=0.03*16000;
                     L=10;
                     pitchBounds=[50/16000,400/16000];
                     A_var=[2/16000];
                     voicingProb=0.7;
                case 1
                     L=10;
                     pitchBounds=[50/16000,400/16000];
                     A_var=[2/16000];
                     voicingProb=0.7;
                case 2
                    pitchBounds=[50/16000,400/16000];
                     A_var=[2/16000];
                     voicingProb=0.7;
                case 3
                     A_var=[2/16000];
                     voicingProb=0.7;
                case 4
                     voicingProb=0.7;
            end

            % validate input
            if length(varargin) >= 1
                if islogical(varargin{1})
                    obj.dcIsIncluded = varargin{1};
                else
                    error('Argument 4 is not of type logical (true/false)')
                end
            end
            if length(varargin) >= 2
                if isscalar(varargin{2})
                    obj.F = varargin{2};
                else
                    error('Argument 5 is not a scalar')
                end
            else
                obj.F = 2^14;
            end
            obj.defaultRefinementTol = 1/obj.F;

            if length(varargin) >= 3
                if isscalar(varargin{3})
                    obj.epsilon = varargin{3};
                else
                    error('Argument 6 is not a scalar')
                end
            end

            if length(varargin) > 4
                error('Too many input arguments')
            end

            if ~isscalar(N)
                error('Input argument N is not a scalar')
            else
                obj.N = N;
            end
            if ~isscalar(L)
                error('Input argument L is not a scalar')
            else
                obj.L = L;
            end

            if ~ (isvector(pitchBounds) && length(pitchBounds) == 2)
                error(['Input argument pitchBound must be a 2-' ...
                       'vector'])
            elseif pitchBounds(1) < obj.pitchBoundsOuter(1) || ...
                pitchBounds(2) > obj.pitchBoundsOuter(2)

                error(['Input argument pitchBounds must be within the ' ...
                       'bounds specified in the constructor (at ' ...
                       'least [0.0 0.5])'])
            else
                obj.pitchBounds = pitchBounds;
            end

            if pitchBounds(1) < 1/N
                warning(['The lower pitch bound is set lower than one '...
                    ' period/segment. Inaccurate results might be '...
                    'produced - especially if you do not set the'...
                    ' regularisation parameter.']);
            end

            % Init
            F = obj.F;
            minFftIndex = ceil(F*pitchBounds(1));
            maxFftIndex = floor(F*pitchBounds(2));
            obj.validFftIndices = (minFftIndex:maxFftIndex)';
            obj.fullPitchGrid = obj.validFftIndices/F;
            nPitches = length(obj.fullPitchGrid);
            obj.logModelPrior=[voicingProb,1-voicingProb];

            % cross-correlation vectors
            obj.crossCorrelationVectors = ...
                [N*ones(1, nPitches)/2 + N*obj.epsilon;...
                sin(pi*(1:2*L)'*obj.fullPitchGrid'*N)./...
                (2*sin(pi*(1:2*L)'*obj.fullPitchGrid'))];

            obj.fftShiftVector = ...
                exp(1i*2*pi*(0:ceil(F/2)-1)'*(N-1)/(2*F));

            % Compute Gamma for the T+H and T-H systems
            [obj.Gamma1, obj.Gamma2] = computeGamma(L, F, pitchBounds, ...
                obj.crossCorrelationVectors, nPitches, ...
                obj.validFftIndices, ...
                obj.dcIsIncluded);
            %%          added by Liming Shi
            obj.scaled_alpha_buffer=nan;
            grid_width=diff(obj.fullPitchGrid(1:2));
%             obj.logpi=(log(1/(nPitches-1))-log(grid_width))*ones(nPitches,1);
            obj.logpi=log((voicingProb)/(nPitches*L))*ones(nPitches,1);
            obj.A=zeros(nPitches,nPitches);

            for jj=1:nPitches
%                 f0=obj.fullPitchGrid(jj);
%                  obj.A(jj,:)= abs(sin(pi*(obj.fullPitchGrid-f0)/f0)./pi./(obj.fullPitchGrid-f0)*f0)+.1;
                 obj.A(jj,:)=(normpdf(obj.fullPitchGrid,obj.fullPitchGrid(jj),A_var));
%                  var_value=1e-1;
%                  obj.logA(jj,:)=log(lognpdf(obj.fullPitchGrid,log(obj.fullPitchGrid(jj)*exp(var_value)),var_value));
%                    obj.logA(jj,:)=log(normpdf(log(obj.fullPitchGrid),log(obj.fullPitchGrid(jj)),.1));
            end

            obj.A=obj.A./repmat(sum(obj.A,2),1,nPitches);

            obj.B=zeros(obj.L,obj.L);

            for jj=1:obj.L
                 obj.B(jj,:)=normpdf(jj,[1:obj.L],1);
            end

            obj.B=obj.B./repmat(sum(obj.B,2),1,obj.L);

            obj.C=[.7 .3;.4,.6];

            obj.scaled_alpha_buffer2=1/obj.L/nPitches*ones(nPitches,obj.L);

        end

        function varargout = computeCostFunctions(obj, x)
            % Compute and returns the cost functions for l=1,...,L

            % validate input
            if ~isvector(x) && length(x) == obj.N
                error(['First argument x must be vector of ' ...
                               'length N=', num2str(obj.N)]);
            end
            x = reshape(x, obj.N, 1); % make sure its a column
                                      % vector

            varargout{1} = computeAllCostFunctions(x, obj.L, ...
            	obj.fullPitchGrid, obj.fftShiftVector, ...
                obj.crossCorrelationVectors, obj.Gamma1, obj.Gamma2, ...
                obj.dcIsIncluded);

            if nargout == 2
                varargout{2} = obj.fullPitchGrid;
            end

        end

        function varargout = estimate(obj, x, flag,tinc,varargin)
            % Estimate fundamental frequency and order of the signal x

            % validate input
            if ~isvector(x) && length(x) == obj.N
                error(['First argument x must be vector of ' ...
                               'length N=', num2str(obj.N)]);
            end
            x = reshape(x, obj.N, 1); % make sure its a column
                                      % vector

            if length(varargin) == 1
                if isscalar(varargin{1})
                    obj.refinementTol = varargin{1};
                else
                    error('Argument 2 is not a scalar')
                end
            else
                obj.refinementTol = obj.defaultRefinementTol;
            end
            if flag==1
                spec_pow_all=(abs(fft(x,2048)).^2)';
                if isempty(obj.prew_state)
                    [psdouput,obj.prew_state]=estnoiseg_orig(spec_pow_all,tinc);
                else
                    [psdouput,obj.prew_state]=estnoiseg_orig(spec_pow_all,obj.prew_state);
                end
                mmse_covariance_all = (real(ifft(psdouput')));
                mmse_lpc_all= levinson(mmse_covariance_all,30);
                [xp] = filter(mmse_lpc_all,1,...
                    x);
                  x=xp/norm(xp)*norm(x);
            end

            % if the data consists of all zeros then return null model
            if  x'*x/obj.N<1e-10
                estimatedPitch = nan;
                estimatedOrder = 0;
                unvoicing_scaled_alpha=log(1);
            else
                % Step 2: compute the profile log-likelihood function
                % over only the fundamental frequency (the linear
                % parameters, noise variance and g have been integrated
                % out)
%                 x=x/norm(x);
                costs = obj.computeCostFunctions(x);

%                 for ii=1:size(costs,1)
%                     regvalue=1e-5;
%                     tempt=costs(ii,:)/(2*regvalue);
%                     pitchLogLikelihood(ii,:)=tempt+log(tempt)*(-3/2-ii+1)+...
%                         log(gammainc(costs(ii,:),3/2+ii-1));
%                 end

                cod = costs*(1/(x'*x+obj.varSpeech*obj.N));
% %                  cod = (costs)*(1/(x'*x));
% %                   cod = costs*(1/(x'*x));
                [~, pitchLogLikelihood] = ...
                    computePitchLogLikelihood(cod, obj.N, obj.gPriorParam);
% %                 pitchLogLikelihood=pitchLogLikelihood;
                null_modellike=1;

                    %% added/modified by Liming Shi (implements the hidden markov chain)
                if isnan(obj.scaled_alpha_buffer)
                    inx=isnan(pitchLogLikelihood);
                    pitchLogLikelihood(inx)=-inf;
                    bar_alpha=obj.logpi+pitchLogLikelihood';

                    unvoicing_bar_alpha=log(obj.logModelPrior(2)*null_modellike);
                    log_scale=log_sumsum_exp_ls([[unvoicing_bar_alpha,-inf*ones(1,obj.L-1)];bar_alpha]);
                    scaled_alpha=bar_alpha-log_scale;
                    unvoicing_scaled_alpha=unvoicing_bar_alpha-log_scale;

                else
                    inx=isnan(pitchLogLikelihood);
                    pitchLogLikelihood(inx)=-inf;
                    state_prior=obj.C(1,1)*obj.A'*obj.scaled_alpha_buffer*obj.B;

                    state_prior=state_prior+obj.scaled_alpha_buffer2*obj.C(2,1)*obj.unvoicing_scaled_alpha_buffer;

                    bar_alpha=log(state_prior)+pitchLogLikelihood';

                    temp=[1-obj.unvoicing_scaled_alpha_buffer;obj.unvoicing_scaled_alpha_buffer];

                    unvoicing_bar_alpha=log(obj.C(:,2)'*temp*null_modellike);
                    log_scale=log_sumsum_exp_ls([[unvoicing_bar_alpha,-inf*ones(1,obj.L-1)];bar_alpha]);
                    scaled_alpha=bar_alpha-log_scale;
                    unvoicing_scaled_alpha=unvoicing_bar_alpha-log_scale;

                end

                    [value, inx] = ...
                        max(scaled_alpha);
                    [~,inx_2]=max(value);
                    pitchIndex=inx(inx_2);
                    estimatedOrder=inx_2;
                    estimatedPitch = obj.fullPitchGrid(pitchIndex(1));
             %% added by Liming Shi (buffering the previous meaningful estimates of the posterior states)

                if unvoicing_scaled_alpha < log(0.5)
                    obj.scaled_alpha_buffer2=exp(scaled_alpha-log(1-exp(unvoicing_scaled_alpha)));
                else
%                     estimatedOrder=nan;
                end
                obj.scaled_alpha_buffer=exp(scaled_alpha);
                obj.unvoicing_scaled_alpha_buffer=exp(unvoicing_scaled_alpha);
            end

            %%

            if nargout >= 1
                varargout{1} = estimatedPitch;
            end
            if nargout >= 2
                varargout{2} = estimatedOrder;
            end
            if nargout >= 3
%                 [~, alpha] = objFunction(estimatedPitch, x, ...
%                     estimatedOrder, obj.dcIsIncluded);
                varargout{3} = 1-exp(unvoicing_scaled_alpha);
            end

        end
        function obj=destroy(obj)
        end
    end
end
function y=log_sumsum_exp_ls(x)
max_temp=max(max(x));
inf_ind=isinf(max_temp);
y=log(sum(sum(exp(x-max_temp))))+max_temp;
y(inf_ind)=-inf;
end


function y = tt_blockwise(arr)
%% TT-Toolbox 2.2.1, 2009-2015
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel

%for questions on this particular code email Alexey Boyko, Skolkovo Institute of Science and Technology,
% a.boyko@skoltech.ru or alexey.boyko@skolkovotech.ru
%Moscow, Russia, 2016

%%
%INPUTS
% arr {2d cell array} of <tt_matrix>  variables
%OUTPUTS
% y <tt_matrix>
%
%this function assembles a block matrix in tt-format from a given 2d cell array
%of blocks in tt-format

    [n,m]=size(arr);

    block=zeros(n,m); block(1,1)=1;
    %since current version of TT-Toolbox doesn't support
    %init like M=0; M=M+<tt_matrix>, one should construct
    %a valid tt_matrix of consistent dimensions as an initial variable.

    y=tkron(arr{1,1},tt_matrix(block));

    for i=1:n
        for j=1:m
%             full(a{i,j})
            if (i~=1)||(j~=1)
                block=zeros(n,m);
                block(i,j)=1;
                y=y+tkron(arr{i,j},tt_matrix(block));
            end
        end
    end
return
end


classdef ParadigmSpecRCSP < ParadigmDataflowSimplified
    % Advanced paradigm for oscillatory processes via the Spectrally weighted regularized CSP algorithm.
    %
    % This method is a combination of the features of the Spec-CSP [1] and the RCSP [7] methods.
    %
    % The Spec-CSP paradigm [1] is a more advanced variant of CSP, developed for the Berlin
    % Brain-Computer Interface (BBCI); the primary focus was motor imagery BCI, but the algorithm was
    % designed from the outset to be applicable for a wider range of applications. The implementation
    % closely follows the TR [2].
    %
    % The RCSP (Regularized Common Spatial Patterns) method [7] is a recently-developed regularized
    % extension of the CSP method, which combines multiple regularization terms (of which we here
    % implemented covariance shrinkage and Tikhonov regularization); these terms are themselves
    % derived from earlier regularized CSP methods, such as TRCSP (Tikhonov Regularized CSP).
    %
    % In addition to the aforementioned papers this method also use multi-taper spectral estimation
    % instead of the Welch method to regularize the spectral estimates, unlike the Spec-CSP method.
    %
    % The paradigm is applicable to the majority of oscillatory processes, and is the most advanced
    % spatio-spectrally adaptive method that is currently provided in the toolbox. Whenever the exact
    % frequency and location of some (conjectured) oscillatory process is not known exactly, Spec-RCSP
    % can be used, and typically gives better results than CSP with an appropriately unrestricted (e.g.,
    % broad-band) spectral filter. Several other methods exist to adapt the spectrum to a process of
    % interest, among others Common Spatio-Spectral Patterns [3], Common Sparse Spectral Spatial Pattern
    % [4], r^2-based heuristics [5], automated parameter search, and manual selection based on visual
    % inspection. Several of these methods have been shown to give approx. comparable results [2]. An
    % alternative and competitive method, especially when there complex interactions between frequency
    % bands and time periods are to be modeled is the Dual-Augmented Lagrange paradigm
    % (para_dal/para_dal_hf).
    %
    % The method iteratively optimizes spatial and spectral filters in alternation and extracts
    % log-variance features from the resulting (filtered) signal. These features are subsequently
    % processed by a (typically simple) machine learning component, by default LDA. Learning is
    % therefore significantly slower than CSP. An option is to impose custom prior knowledge on the
    % relevant data spectrum, for example by placing a focus on the alpha rhythm, without ruling out
    % other frequencies. Note that there are parameters which constrain the spectrum: one is the
    % frequency prior and the other is the spectral filter that is applied before running the alorithm;
    % both need to be adapted when the considered spectrum shall be extended (e.g. to high-gamma
    % oscillations). Other parameters which are frequently adapted are the time window of interest and
    % the learner component (e.g., logistic regression is a good alternative choice).
    %
    % Some application areas include detection of major brain rhythm modulations (e.g. theta, alpha,
    % beta, gamma), for example related to relaxation/stress, aspects of workload, emotion,
    % sensori-motor imagery, and in general cortical idle oscillations in various modalities.
    %
    % Example: Consider the goal of predicting the emotion felt by a person at a given time. A possible
    % calibration data set for this task would contain a sequence of blocks in each of which the subject
    % is one out of several possible emotions, indicated by events 'e1','e2','e3','e4' covering these
    % blocks at regular rate. The data might for example be induced via guided imagery [6].
    %
    %   calib = io_loadset('data sets/bruce/emotions.eeg')
    %   myapproach = {'SpecRCSP' 'SignalProcessing',{'EpochExtraction',[-2.5 2.5]}};
    %   [loss,model,stats] = bci_train('Data',calib,'Approach',myapproach, 'TargetMarkers',{'e1','e2','e3','e4'});
    %
    %
    % References:
    %  [1] Tomioka, R., Dornhege, G., Aihara, K., and Mueller, K.-R. "An iterative algorithm for spatio-temporal filter optimization."
    %      In Proceedings of the 3rd International Brain-Computer Interface Workshop and Training Course 2006.
    %  [2] Ryota Tomioka, Guido Dornhege, Guido Nolte, Benjamin Blankertz, Kazuyuki Aihara, and Klaus-Robert Mueller
    %      "Spectrally Weighted Common Spatial Pattern Algorithm for Single Trial EEG Classification",
    %      Mathematical Engineering Technical Reports (METR-2006-40), July 2006.
    %  [3] Steven Lemm, Benjamin Blankertz, Gabriel Curio, and Klaus-Robert M?ller.
    %      "Spatio-spectral filters for improving classification of single trial EEG."
    %      IEEE Trans Biomed Eng, 52(9):1541-1548, 2005.
    %  [4] G. Dornhege, B. Blankertz, M. Krauledat, F. Losch, G. Curio, and K.-R. M?ller,
    %      "Combined optimization of spatial and temporal filters for improving brain-computer interfacing,"
    %      IEEE Transactions on Biomedical Engineering, vol. 53, no. 11, pp. 2274?2281, 2006.
    %  [5] Benjamin Blankertz, Ryota Tomioka, Steven Lemm, Motoaki Kawanabe, and Klaus-Robert Mueller.
    %      "Optimizing spatial filters for robust EEG single-trial analysis."
    %      IEEE Signal Process Mag, 25(1):41-56, January 2008
    %  [6] Onton J & Makeig S. "Broadband high-frequency EEG dynamics during emotion imagination."
    %      Frontiers in Human Neuroscience, 2009.
    %  [7] Lotte, F., Guan, G., "Regularizing Common Spatial Patterns to Improve BCI Designs: Unified Theory and New Algorithms."
    %      IEEE Trans Biomed Eng 58, 2 (2011), 355-362.
    %
    % Name:
    %   Spectrally Weighted RCSP
    %
    %                           Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
    %                           2015-11-05

    methods

        function defaults = preprocessing_defaults(self) %#ok<MANU>
            defaults = {'IIRFilter',{'Frequencies',[0.5 5],'Mode','highpass'}, 'EpochExtraction',[0.5 3.5], 'Resampling',100};
        end

        function model = feature_adapt(self,varargin) %#ok<INUSL>
            args = arg_define(varargin, ...
                arg_norep('signal'), ...
                ... % standard CSP parameters
                arg({'patterns','PatternPairs'},3,uint32([1 1 64 1000]),'Number of CSP patterns (times two).','cat','Feature Extraction'), ...
                ... % multi-taper spectral estimation
                arg({'freqwnd','FrequencyRange','SpectralPrior'},[7 30],[0 Inf],'Frequency range of interest. This is the overall frequency range within which to compute the cross spectrum.'), ...
                arg({'bandwidth','TimeBandwidth'},5,[],'Spectral smoothing. Controls the bias vs. variance of the spectral estimation. Reasonable values are 1 to 3 (1 being fairly noisy, and 3 being fairly smooth but 5x slower)'), ...
                arg({'tapers','Tapers'},[],[],'Number of tapers. Should be an integer smaller than 2*TimeBandwith; default 2*TimeBandwidth-1'), ...
                arg({'subsample_spectrum','SubsampleSpectrum'},1,[1 Inf],'Sub-sample the spectrum. This is the sub-sampling factor.'), ...
                arg({'robust_spectrum','RobustSpectrum'},false,[],'Robust spectral statistics. Can help when some trials are noisy.'), ...
                arg({'normalize_spectrum','NormalizeSpectrum'},false,[],'Normalize spectrum. This corrects for the 1/f characteristic of the spectrum, which is useful when employing the power-based spec-csp prior to low frequencies.'), ...
                ... % RCSP regularization parameters
                arg({'cov_shrinkage','CovarianceShrinkage','covariance_shrinkage'},0,[0 1],'Covariance shrinkage. This is the shrinkage parameter gamma that blends between the empirical covariance (if 0) and the identity matrix (if 1). A good search range is: search(0:0.1:0.9)'), ...
                arg({'tikhonov_reg','TikhonovRegularization','tikhonov_regularization'},0,[0 Inf],'Tikhonov regularization. This parameter regularizes the CSP objective function. A good search range is: search(10.^(-10:-1))'), ...
                arg({'robust_cov','RobustCovariance','robust_covariance'},false,[],'Robust covariance estimation. Can help when some trials are noisy.'), ...
                ... % Spec-CSP specific parameters (rarely used)
                arg({'pp','ParameterP'},0,[-1 1],'Regularization parameter p''. Can be searched over -1:0.5:1.','cat','Feature Extraction','guru',true), ...
                arg({'qp','ParameterQ'},1,[0 4],'Regularization parameter q''. Can be searched over 0:0.5:4.','cat','Feature Extraction','guru',true), ...
                arg({'steps','MaxIterations'},3,uint32([1 3 10 50]),'Number of iterations. A step is spatial optimization, followed by spectral optimization.','cat','Feature Extraction'));

            [signal,n_of,pp,qp,steps] = deal(args.signal,args.patterns,args.pp,args.qp,args.steps);
            if signal.nbchan == 1
                error('Spec-RCSP does intrinsically not support single-channel data (it is a spatial filter).'); end
            if signal.nbchan < args.patterns
                error('Spec-RCSP prefers to work on at least as many channels as you request output patterns. Please reduce the number of pattern pairs.'); end
            p = pp+qp; q = qp;  % re-parameterize the hyper-parameters p' and q' into p and q
            % number of nC=Channels, nS=Samples, nT=trials, nF=frequency bins
            [nC,nS,nT] = size(signal.data); %#ok<ASGLU>
            for c=2:-1:1
                % Extension: compute the per-class multi-taper cross-spectrum across trials
                X{c} = exp_eval_optimized(set_picktrials(signal,'rank',c));
                F{c} = 2*utl_calc_crossspec('Signal',X{c},'FrequencyRange',args.freqwnd,'TimeBandwidth',args.bandwidth, ...
                    'Tapers',args.tapers,'SubsampleSpectrum',args.subsample_spectrum, 'Measure','real',...
                    'FilteredSubsampling',true,'FeatureFilters',false);
                F{c} = permute(F{c},[2 3 1 4]);
                % perform 1/f normalization
                if args.normalize_spectrum
                    F{c} = bsxfun(@times,F{c},reshape((1:size(F{c},3))/size(F{c},3),1,1,[])); end
                % compute the cross-spectrum V as an average over trials
                if args.robust_cov && X{c}.trials > 1
                    V{c} = reshape(geometric_median(reshape(F{c},[],X{c}.trials)')',nC,nC,[]);
                else
                    V{c} = mean(F{c},4);
                end
            end
            nF = size(F{1},3);
            % 1. initialize the filter set alpha and the number of filters J
            J = 1; alpha{J}(1:nF) = 1;
            % 2. for each step
            for step=1:steps
                % 3. for each set of spectral coefficients alpha{j} (j=1,...,J)
                for j=1:J
                    % 4. calculate sensor covariance matrices for each class from alpha{j}
                    for c=2:-1:1
                        Sigma{c} = zeros(nC); %#ok<*AGROW>
                        for b=1:nF
                            Sigma{c} = Sigma{c} + alpha{j}(b)*V{c}(:,:,b); end
                        % Extension: optionally implement shrinkage towards identity
                        if args.cov_shrinkage > 0
                            Sigma{c} = (1-args.cov_shrinkage)*Sigma{c} + args.cov_shrinkage*eye(nC); end
                    end

                    if args.tikhonov_reg == 0
                        % 5. solve the generalized eigenvalue problem Eq. (2) in Spec-CSP
                        [VV,DD] = eig(Sigma{1},Sigma{1}+Sigma{2});
                        iVV = inv(VV)';
                        % and retain n_of top eigenvectors at both ends of the eigenvalue spectrum...
                        W{j} = {VV(:,1:n_of), VV(:,end-n_of+1:end)};
                        P{j} = {iVV(:,1:n_of), iVV(:,end-n_of+1:end)};
                        % as well as the top eigenvalue for each class
                        lambda(j,:) = [DD(1), DD(end)];
                    else
                        [VV,DD,iVV] = deal({});
                        % Extension: use Tikhonov regularization as in TRCSP
                        for c=2:-1:1
                            M{c} = Sigma{c}/(Sigma{3-c} + args.tikhonov_reg*eye(nC));
                            try
                                [VV{c}, DD{c}] = eig(M{c});
                                [DD{c},order] = sort(diag(DD{c}),'descend');
                                VV{c} = VV{c}(:,order);
                                iVV{c} = inv(VV{c});
                                if ~all(isfinite(iVV{c}(:)))
                                    error('Divergence.'); end
                            catch
                                fprintf('ParadigmSpecRCSP: encountered a divergence.\n');
                                % keep going, results like this will be weeded out by a parameter
                                % search
                                VV{c} = randn(size(M{c}));
                                DD{c} = randn(size(M{c}));
                                iVV{c} = randn(size(M{c}));
                            end
                        end
                        % wrap up (note that the roles of patterns and filters are confused in this
                        % formulation, and that we are using the largest EVs from each matrix)
                        W{j} = {iVV{2}(1:n_of,:)',iVV{1}(1:n_of,:)'};
                        P{j} = {VV{2}(:,1:n_of), VV{1}(:,1:n_of)};
                        % ... however, the outer loop finds the minimal first lambda, so we invert it
                        lambda(j,:) = [1/DD{2}(1), DD{1}(1)];
                    end
                end

                % 7. set W{c} from all W{j}{c} such that lambda(j,c) is minimal/maximal over j
                W = {W{argmin(lambda(:,1))}{1}, W{argmax(lambda(:,2))}{2}};
                P = {P{argmin(lambda(:,1))}{1}, P{argmax(lambda(:,2))}{2}};
                % 8. for each projection w in the concatenated [W{1},W{2}]...
                Wcat = [W{1} W{2}]; J = 2*n_of;
                Pcat = [P{1} P{2}];
                for j=1:J
                    w = Wcat(:,j);
                    % 9. calcualate (across trials within each class) mean and variance of the w-projected cross-spectrum components
                    for c=2:-1:1
                        % part of Eq. (3)
                        s{c} = zeros(size(F{c},4),nF);
                        for k=1:nF
                            for t = 1:size(s{c},1)
                                s{c}(t,k) = w'*F{c}(:,:,k,t)*w; end
                        end
                        if args.robust_spectrum
                            mu_s{c} = median(s{c});
                            var_s{c} = median(abs(bsxfun(@minus,s{c},mu_s{c})))*1.4826;
                        else
                            mu_s{c} = mean(s{c});
                            var_s{c} = var(s{c});
                        end
                    end
                    % 10. update alpha{j} according to Eqs. (4) and (5)
                    for c=2:-1:1
                        for k=1:nF
                            % Eq. (4)
                            alpha_opt{c}(k) = max(0, (mu_s{c}(k)-mu_s{3-c}(k)) / (var_s{1}(k) + var_s{2}(k)) );
                            % Eq. (5), with prior from Eq. (6)
                            alpha_tmp{c}(k) = alpha_opt{c}(k).^q * ((mu_s{1}(k) + mu_s{2}(k))/2).^p;
                        end
                    end
                    % ... as the maximum for both classes
                    alpha{j} = max(alpha_tmp{1},alpha_tmp{2});
                    % and normalize alpha{j} so that it sums to unity
                    alpha{j} = alpha{j} / sum(alpha{j});
                end
            end
            alpha = vertcat(alpha{:})';
            model = struct('W',{Wcat},'P',{Pcat},'nF',{nF},'nC',{nC},'alpha',{alpha},'chanlocs',{signal.chanlocs}, ...
                'freqwnd',{args.freqwnd},'bandwidth',{args.bandwidth},'tapers',{args.tapers}, ...
                'subsample_spectrum',{args.subsample_spectrum},'nTrials',[X{1}.trials,X{2}.trials]);
        end

        function features = feature_extract(self,signal,mdl) %#ok<*INUSL>
            spec = 2*utl_calc_crossspec('Signal',signal,'FrequencyRange',mdl.freqwnd,'TimeBandwidth',mdl.bandwidth, ...
                'Tapers',mdl.tapers,'SubsampleSpectrum',mdl.subsample_spectrum, 'Measure','real',...
                'FilteredSubsampling',true,'FeatureFilters',false);
            features = zeros(size(signal.data,3),size(mdl.W,2));
            [W, alpha] = deal(mdl.W, mdl.alpha);
            for f=1:size(W,2)
                C = permute(sum(bsxfun(@times,spec,alpha(:,f)),1),[2,3,4,1]);
                for t=1:size(signal.data,3)
                        features(t,f) = log(W(:,f)'*C(:,:,t)*W(:,f)); end
            end
        end

        function visualize_model(self,varargin) %#ok<*INUSD>
            args = arg_define([0 3],varargin, ...
                arg_norep({'myparent','Parent'},[],[],'Parent figure.'), ...
                arg_norep({'featuremodel','FeatureModel'},[],[],'Feature model. This is the part of the model that describes the feature extraction.'), ...
                arg_norep({'predictivemodel','PredictiveModel'},[],[],'Predictive model. This is the part of the model that describes the predictive mapping.'), ...
                arg({'patterns','PlotPatterns'},true,[],'Plot patterns instead of filters. Whether to plot spatial patterns (forward projections) rather than spatial filters.'), ...
                arg({'paper','PaperFigure'},false,[],'Use paper-style font sizes. Whether to generate a plot with font sizes etc. adjusted for paper.'), ...
                arg_nogui({'nosedir_override','NoseDirectionOverride'},'',{'','+X','+Y','-X','-Y'},'Override nose direction.'));
            arg_toworkspace(args);

            % no parent: create new figure
            if isempty(myparent)
                myparent = figure('Name','Common Spatial Patterns'); end
            % determine nose direction for EEGLAB graphics
            try
                nosedir = args.fmodel.signal.info.chaninfo.nosedir;
            catch
                disp_once('Nose direction for plotting not store in model; assuming +X');
                nosedir = '+X';
            end
            if ~isempty(nosedir_override)
                nosedir = nosedir_override; end
            % number of pairs, and index of pattern per subplot
            np = size(featuremodel.W,2)/2; idxp = [1:np np+(2*np:-1:np+1)]; idxf = [np+(1:np) 2*np+(2*np:-1:np+1)];
            % for each CSP pattern...
            for p=1:np*2
                subplot(4,np,idxp(p),'Parent',myparent);
                if args.patterns
                    plotdata = featuremodel.P(:,p);
                else
                    plotdata = featuremodel.W(:,p);
                end
                topoplot(plotdata,featuremodel.chanlocs,'nosedir',nosedir);
                subplot(4,np,idxf(p),'Parent',myparent);
                alpha = featuremodel.alpha(:,p);
                range = 1:max(find(alpha)); %#ok<MXFND>
                if ~isfield(featuremodel,'freqs')
                    featuremodel.freqs = linspace(featuremodel.freqwnd(1),featuremodel.freqwnd(2),length(range)); end
                pl=plot(featuremodel.freqs(range),featuremodel.alpha(range,p));
                xlim([min(featuremodel.freqs(range)) max(featuremodel.freqs(range))]);
                l1 = xlabel('Frequency in Hz');
                l2 = ylabel('Weight');
                t=title(['Spec-CSP Pattern ' num2str(p)]);
                if args.paper
                    set([gca,t,l1,l2],'FontUnits','normalized');
                    set([gca,t,l1,l2],'FontSize',0.2);
                    set(pl,'LineWidth',2);
                end
            end
            try set(gcf,'Color',[1 1 1]); end
        end

        function layout = dialog_layout_defaults(self)
            layout = {'SignalProcessing.Resampling.SamplingRate', 'SignalProcessing.FIRFilter.Frequencies', ...
                'SignalProcessing.FIRFilter.Type', 'SignalProcessing.EpochExtraction', '', ...
                'Prediction.FeatureExtraction', '', ...
                'Prediction.MachineLearning.Learner'};
        end

        function tf = needs_voting(self)
            tf = true;
        end

    end
end


function [m, pot] = marginal_nodes(engine, query)
% MARGINAL_NODES Compute the marginal on the specified set of nodes (global_joint)
% [m, pot] = marginal_nodes(engine, query)

pot = marginalize_pot(engine.jpot, query);
m = pot_to_marginal(pot);
%[m.T, lik] = normalize(m.T);
%loglik = log(lik);


function one_pixel_result=seg_eva_one_img(predict_mask, gt_mask, class_info)

    exclude_class_idxes=class_info.void_class_idxes;
    class_num=class_info.class_num;

    assert(all(size(gt_mask)==size(predict_mask)));

    gt_mask_vector=gt_mask(:);
    predict_mask_vector=predict_mask(:);

    if ~isempty(exclude_class_idxes)
        valid_pixel_sel=~ismember(gt_mask, exclude_class_idxes);
        gt_mask_vector=gt_mask_vector(valid_pixel_sel);
        predict_mask_vector=predict_mask_vector(valid_pixel_sel);
    end

    [tmp_con_mat, label_vs]=confusionmat(gt_mask_vector, predict_mask_vector);
    confusion_mat=zeros(class_num, class_num);
    confusion_mat(label_vs,label_vs)=tmp_con_mat;

    one_pixel_result=seg_eva_gen_result_from_con_mat(confusion_mat, exclude_class_idxes);

end


classdef GeographicElementSet < AbstractElementSet
    %GeographicElementSet Summary of this class goes here
    %   Detailed explanation goes here

    properties
        lat%(1,1) double %rad
        long%(1,1) double %rad
        alt%(1,1) double %km
        velAz%(1,1) double %rad NEZ frame
        velEl%(1,1) double %rad NEZ frame
        velMag%(1,1) double %km/s NEZ frame

        optVar GeographicElementSetVariable
    end

    properties(Constant)
        typeEnum = ElementSetEnum.GeographicElements;
    end

    methods
        function obj = GeographicElementSet(time, lat, long, alt, velAz, velEl, velMag, frame)
            if(nargin > 0)
                obj.time = time;
                obj.lat = lat;
                obj.long = long;
                obj.alt = alt;
                obj.velAz = velAz;
                obj.velEl = velEl;
                obj.velMag = velMag;
                obj.frame = frame;
            end
        end

        %vectorized
        function cartElemSet = convertToCartesianElementSet(obj)
            radius = NaN(1, length(obj));
            for(i=1:length(obj))
                radius(i) = obj(i).frame.getOriginBody().radius;
            end
            r = radius + [obj.alt];

            x = r.*cos([obj.lat]).*cos([obj.long]);
            y = r.*cos([obj.lat]).*sin([obj.long]);
            z = r.*sin([obj.lat]);

            rVect = [x;y;z];

            sezVVectAz = pi - [obj.velAz];
            sezVVectEl = [obj.velEl];
            sezVVectMag = [obj.velMag];

            [x,y,z] = sph2cart(sezVVectAz, sezVVectEl, sezVVectMag);
            vectorSez = [x;y;z];
            vVect = rotSEZVectToECEFCoords(rVect, vectorSez); %not necessarily ECEF coords here but the transform holds

%             cartElemSet = CartesianElementSet(obj.time, rVect, vVect, obj.frame);
%             cartElemSet = repmat(CartesianElementSet.getDefaultElements(), size(obj));
%             for(i=1:length(obj))
%                 cartElemSet(i) = CartesianElementSet(obj(i).time, rVect(:,i), vVect(:,i), obj(i).frame);
%             end

            obj = obj(:)';
            cartElemSet = CartesianElementSet([obj.time], rVect, vVect, [obj.frame]);
        end

        %vectorized
        function kepElemSet = convertToKeplerianElementSet(obj)
            kepElemSet = convertToKeplerianElementSet(convertToCartesianElementSet(obj));
        end

        %vectorized
        function geoElemSet = convertToGeographicElementSet(obj)
            geoElemSet = obj;
        end

        %vectorized
        function univElemSet = convertToUniversalElementSet(obj)
            univElemSet = convertToUniversalElementSet(convertToKeplerianElementSet(obj));
        end

        function elemVect = getElementVector(obj)
            elemVect = [rad2deg(obj.lat),rad2deg(obj.long),obj.alt,rad2deg(obj.velAz),rad2deg(obj.velEl),obj.velMag];
        end

        function newElemSet = copyWithoutOptVar(obj)
            newElemSet = GeographicElementSet(obj.time, obj.lat, obj.long, obj.alt, obj.velAz, obj.velEl, obj.velMag, obj.frame);
        end
    end

    methods(Access=protected)
        function displayScalarObject(obj)
            fprintf('Geographic State \n\tTime: %0.3f sec UT \n\tLatitude: %0.3f deg \n\tLongitude: %0.3f deg \n\tAltitude: %0.3f km \n\tVel Az (NEZ): %0.3f deg \n\tVel El (NEZ): %0.3f deg \n\tVel Mag (NEZ): %0.3f km/s \n\tFrame: %s\n', ...
                    obj.time, ...
                    rad2deg(obj.lat), ...
                    rad2deg(obj.long), ...
                    obj.alt, ...
                    rad2deg(obj.velAz), ...
                    rad2deg(obj.velEl), ...
                    obj.velMag, ...
                    obj.frame.getNameStr());
        end
    end

    methods(Static)
        function elemSet = getDefaultElements()
            elemSet = GeographicElementSet();
        end

        function errMsg = validateInputOrbit(errMsg, hLat, hLong, hAlt, hVelAz, hVelEl, hVelMag, bodyInfo, bndStr, checkElement)
            if(isempty(bndStr))
                bndStr = '';
            else
                bndStr = sprintf(' (%s Bound)', bndStr);
            end

            if(checkElement(1))
                lat = str2double(get(hLat,'String'));
                enteredStr = get(hLat,'String');
                numberName = ['Latitude', bndStr];
                lb = -90;
                ub = 90;
                isInt = false;
                errMsg = validateNumber(lat, numberName, lb, ub, isInt, errMsg, enteredStr);
            end

            if(checkElement(2))
                long = str2double(get(hLong,'String'));
                enteredStr = get(hLong,'String');
                numberName = ['Longitude', bndStr];
                lb = -360;
                ub = 360;
                isInt = false;
                errMsg = validateNumber(long, numberName, lb, ub, isInt, errMsg, enteredStr);
            end

            if(checkElement(3))
                alt = str2double(get(hAlt,'String'));
                enteredStr = get(hAlt,'String');
                numberName = ['Altitude', bndStr];
                lb = -bodyInfo.radius;
                ub = Inf;
                isInt = false;
                errMsg = validateNumber(alt, numberName, lb, ub, isInt, errMsg, enteredStr);
            end

            if(checkElement(4))
                bfVx = str2double(get(hVelAz,'String'));
                enteredStr = get(hVelAz,'String');
                numberName = ['Body-Fixed Velocity Azimuth', bndStr];
                lb = -360;
                ub = 360;
                isInt = false;
                errMsg = validateNumber(bfVx, numberName, lb, ub, isInt, errMsg, enteredStr);
            end

            if(checkElement(5))
                bfVy = str2double(get(hVelEl,'String'));
                enteredStr = get(hVelEl,'String');
                numberName = ['Body-Fixed Velocity Elevation', bndStr];
                lb = -90;
                ub = 90;
                isInt = false;
                errMsg = validateNumber(bfVy, numberName, lb, ub, isInt, errMsg, enteredStr);
            end

            if(checkElement(6))
                bfVz = str2double(get(hVelMag,'String'));
                enteredStr = get(hVelMag,'String');
                numberName = ['Body-Fixed Velocity Magnitude', bndStr];
                lb = 0;
                ub = Inf;
                isInt = false;
                errMsg = validateNumber(bfVz, numberName, lb, ub, isInt, errMsg, enteredStr);
            end
        end
    end
end


% Evaluate fitting performance on different datasets.
%
% Author: Luca Vezzaro (elvezzaro@gmail.com)

clear all;
close all;

addpath('..');

datasets = { 'child', 'Cootes', 'mother', 'movie1', 'movie2'};
convergence_thresholds = {2, 7, 3, 5.5, 5.5};
test = {[1 2 3 4 5 10 13], [8 10 12 22 26 ], [2 3 14], [7 12 5 21], [1 3 22 31 ]};

% Selected dataset
sel = 3;
blur_training_set = 1;

base_path = sprintf('performance/%s', datasets{sel});

% Available landmark files
landmark_files = dir(sprintf('%s/training/*.mat', base_path));
ns = numel(landmark_files);

for j=1:ns
	% Prepare appearance data
	app = double(imread(sprintf('%s/training/%s', base_path, landmark_files(j).name(1:end-4)))) / 255;

	if blur_training_set
		app = convn(convn(app, exp(-(-5:5).^2)./sum(exp(-(-5:5).^2)), 'same'), exp(-(-5:5).^2)./sum(exp(-(-5:5).^2))', 'same');
	end

	appearances(:,:,:,j) = app;
end

for j=1:ns
	% Load associated landmarks
	load(sprintf('%s/training/%s', base_path, landmark_files(j).name));
	shapes(:,:,j) = xy2ij(annotations, size(app,1));
end

load(sprintf('%s/triangulation.mat', base_path));

np = size(shapes, 1);

% Sum of fitting times
tempi = 0;
% Number of fitting times considered
ntempi = 0;

% Number of times the fitting is evaluated
ntries = zeros(10,1);
% Number of times the fitting successfully produced a result (ie. the warp didn't diverge)
nfitted = zeros(10,1);
% Number of times the pt.pt error was below the convergence threshold
nconverged = zeros(10,1);
% Sum of ptpt errors for a given value of 'k'. The number of values considered here is given by 'nfitted'.
ptpt = zeros(10,1);
% Minimum pt.pt. error for a given value of 'k'
ptpt_min = ones(10,1) * inf;
% Maximum pt.pt. error for a given value of 'k'
ptpt_max = zeros(10,1);

% Carry out leave-one-out testing
for jj=1:numel(test{sel})
	j = test{sel}(jj);

	fprintf('\n\n\n****************************************************************************\n');
	fprintf('Carrying out %d/%d leave-one-out try on dataset: %s\n', jj, numel(test{sel}), datasets{sel});
	fprintf('****************************************************************************\n');

	one_out = [1:j-1,j+1:ns];
	AAM = build_model_2d(shapes(:,:,one_out), appearances(:,:,:,one_out), 'triangulation', triangulation);

	for k=1:10
		fprintf('%d/10\n', k);

		d1 = [rand() rand()] - [0.5 0.5];
		d1 = 3*d1 / norm(d1);

		d2 = [rand() rand()] - [0.5 0.5];
		d2 = 3*d2 / norm(d2);

		% dx dy
		%displacements = k*0.6*[0 0; 0 5; 5 0; 0 -5; -5 0; -3.5355 3.5355; 3.5355 3.5355; 3.5355 -3.5355; -3.5355 -3.5355];
		displacements = k*0.6*[0 5; 5 0; 0 -5; -5 0];
		%displacements = k*[d1; d2];
		rotations = k*[0 pi/80 -pi/80 ];
		scales = [1 1 1 ] + k*[0 0.02 -0.02];

		cog = repmat(mean(shapes(:,:,j), 1), [np 1]);

		for ti=1:size(displacements, 1)
			for rot=1:numel(rotations)
				for sc=1:numel(scales)
					ntries(k) = ntries(k) + 1;
					theta = rotations(rot);

					A = [cos(theta) sin(theta); -sin(theta) cos(theta)] * scales(sc);
					d = repmat(displacements(ti,:), [np, 1]);

					init_shape = (shapes(:,:,j) - cog) * A + d + cog;
					%init_shape = shapes(:,:,j) + d;

					%ptpt_init = mean(sqrt(sum((init_shape - shapes(:,:,j)).^2, 2)));
					%rms_init = sqrt(sum(sum((init_shape - shapes(:,:,j)).^2)) / (2*np));

					diverged = 0;
					try
						tic;
						[ fitted_shape ] = fit_2d(AAM, init_shape, appearances(:,:,:,j), 20);
						dt = toc;
						nfitted(k) = nfitted(k) + 1;
					catch
						dt = toc;
						diverged = 1;
					end

					if diverged == 0
						ptpt_err = mean(sqrt(sum((fitted_shape - shapes(:,:,j)).^2, 2)))
						%rms_err = sqrt(sum(sum((fitted_shape - shapes(:,:,j)).^2)) / (2*np));

						ptpt(k) =  ptpt(k) + ptpt_err;

						if ptpt_err < ptpt_min(k)
							ptpt_min(k) = ptpt_err;
						end

						if ptpt_err > ptpt_max(k)
							ptpt_max(k) = ptpt_err;
						end

						tempi = tempi + dt;
						ntempi = ntempi + 1;

						% Check if error is acceptable
						if ptpt_err < convergence_thresholds{sel}
							nconverged(k) = nconverged(k) + 1;
						end
					end

					%hold off;
					%imshow(appearances(:,:,:,j));
					%hold on;
					%triplot(triangulation, init_shape(:,2), init_shape(:,1), 'b', 'LineWidth', 2);
					%if diverged == 0
					%	triplot(triangulation, fitted_shape(:,2), fitted_shape(:,1), 'w', 'LineWidth', 2);
					%end
					%pause;
				end
			end
		end
	end
end

% Convergence rate as a function of 'k'
results.conv_rate = nconverged ./ ntries;
% Mean fitting time
results.time_mean = tempi / ntempi;
% Mean pt.pt error as a function of 'k'
results.ptpt_mean = ptpt ./ nfitted;

results.ptpt_min = ptpt_min;
results.ptpt_max = ptpt_max;

results.nfitted = nfitted;
results.ntries = ntries;
results.nconverged = nconverged;

disp 'Plotting convergence ratio as a function of k...'
figure;
plot(1:numel(nconverged), results.conv_rate);

result_path = sprintf('%s/results.mat', base_path);
save(result_path, 'results');
fprintf('\For the full results of the performance evaluation see: ./%s. See that file for more details.\n', result_path);

clear all;


function days = month_length_persian ( y, m )

%*****************************************************************************80
%
%% MONTH_LENGTH_PERSIAN returns the number of days in a Persian month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer DAYS, the number of days
%    in the month.
%
  mdays = [ 31, 31, 31, 31, 31, 31, 30, 30, 30, 30, 30, 29 ];
%
%  Copy the input.
%
  m2 = m;
  y2 = y;
%
%  Get the number of days in the month.
%
  days = mdays(m2);
%
%  If necessary, add 1 day for a leap year.
%
  if ( m2 == 12 && year_is_leap_persian ( y2 ) )
    days = days + 1;
  end

  return
end


function lp = create_lp_solve_model(A,b,f,xint,LB,UB,e,options);

[m,n] = size(A);
lp = lp_solve('make_lp', m, n);

lp_solve('set_mat', lp, A);
lp_solve('set_rh_vec', lp, b);
lp_solve('set_obj_fn', lp, f);
lp_solve('set_maxim', lp); % default is solving minimum lp.
for i = 1:length(e)
    if e(i) < 0
        con_type = 1;
    elseif e(i) == 0
        con_type = 3;
    else
        con_type = 2;
    end
    lp_solve('set_constr_type', lp, i, con_type);
end
for i = 1:length(LB)
    %if ~isinf(LB(i))
        lp_solve('set_lowbo', lp, i, LB(i));
    %end
end
for i = 1:length(UB)
   if ~isinf(UB(i))
        lp_solve('set_upbo', lp, i, UB(i));
   end
end
for i = 1:length(xint)
    lp_solve('set_int', lp, xint(i), 1);
end

if options.lpsolve.scalemode~=0
    lp_solve('set_scaling', lp, scalemode);
end

% for i = 1:length(sos)
%     lp_solve('add_SOS', lp, ['dummy' num2str(i)], 1, i, sos{i}, 1:length(sos{i}));
% end

switch options.verbose
    case 0
        lp_solve('set_verbose', lp, 0);%options.verbose)
    case 1
        lp_solve('set_verbose', lp, 4);%options.verbose)
    case 2
        lp_solve('set_verbose', lp, 5);%options.verbose)
    otherwise
        lp_solve('set_verbose', lp, 6);%options.verbose)
end


function [SetupStruc] = ISM_setup()
%%%%%%   the environment coefficients
SetupStruc.T60 = 0;      %T60, Options: 0, 0.3, 0.6, 0.9
SetupStruc.c = 340;
SetupStruc.Sign_compared = -1;   % choosing the standard signal, non-negtive corresponding to the signal in the select T60, or the original signal
%%%%%%    the microphone coefficients
SetupStruc.Size = 4.35*0.01;
SetupStruc.Channel_Num = 7;      %the number of mics
SetupStruc.Sign_Mid = 1;         %the exist of the center mic
SetupStruc.AddNoiseFlag = 0;
SetupStruc.NoiseSNR = 30;
SetupStruc.Height = 0; %set 0 use the sameheight data, set 1 use the differentheight data


function psat_values_test ( )

%*****************************************************************************80
%
%% PSAT_VALUES_TEST demonstrates the use of PSAT_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2005
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PSAT_VALUES_TEST:\n' );
  fprintf ( 1, '  PSAT_VALUES stores values of\n' );
  fprintf ( 1, '  the saturation pressure of water\n' );
  fprintf ( 1, '  as a function of temperature.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      T            PSAT(T)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, tc, psat ] = psat_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', tc, psat );

  end

  return
end


function [ f,resL2,qualMeasOut] = AwPCSD(proj,geo,angles,maxiter,varargin)
%AwPCSD solves the reconstruction problem using adaptive-weighted
%projection-controlled steepest descent method
%
%   AwPCSD(PROJ,GEO,ALPHA,NITER) solves the reconstruction problem using
%   the projection data PROJ taken over ALPHA angles, corresponding to the
%   geometry described in GEO, using NITER iterations.
%
%   AwPCSD(PROJ,GEO,ALPHA,NITER,OPT,VAL,...) uses options and values for solving. The
%   possible options in OPT are:
%
%
%   'lambda':      Sets the value of the hyperparameter for the SART iterations.
%                  Default is 1
%
%   'lambdared':   Reduction of lambda.Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
%       'init':    Describes diferent initialization techniques.
%                   •  'none'     : Initializes the image to zeros(default)

%                   •  'FDK'      : intializes image to FDK reconstrucition
%
%   'TViter':      Defines the amount of TV iterations performed per SART
%                  iteration. Default is 20
%
%   'maxL2err'     Maximum L2 error to accept an image as valid. This
%                  parameter is crucial for the algorithm, determines at
%                  what point an image should not be updated further.
%                  Default is 20% of the FDK L2 norm.
%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.
%   'delta'        Defines Parameter to control the amount of smoothing
%                  for pixels at the edges. A large 'delta' is not able to
%                  differentiate image gradients at different pixels. A
%                  small 'delta' give low weights to almost every pixels,
%                  making the algorithm inefficient in removing noise or
%                  straking artifacts. Default is -0.00055
% 'redundancy_weighting': true or false. Default is true. Applies data
%                         redundancy weighting to projections in the update step
%                         (relevant for offset detector geometry)
%  'groundTruth'  an image as grounf truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri and Manasavee Lohvithee
%--------------------------------------------------------------------------
%% parse inputs
[beta,beta_red,f,ng,verbose,epsilon,delta,QualMeasOpts,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,varargin);
measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<3 && measurequality
    warning("Image metrics requested but none catched as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);

if nargout>1
    computeL2=true;
else
    computeL2=false;
end
resL2=zeros(1,niter);

% [alphablocks,orig_index]=order_subsets(angles,blocksize,OrderStrategy);
%
% angles=cell2mat(alphablocks);
% index_angles=cell2mat(orig_index);

% does detector rotation exists?
if ~isfield(geo,'rotDetector')
    geo.rotDetector=[0;0;0];
end

%% Create weigthing matrices for the SART step
% the reason we do this, instead of calling the SART fucntion is not to
% recompute the weigths every AwASD-POCS iteration, thus effectively doubling
% the computational time
% Projection weigth, W
W=computeW(geo,angles,gpuids);

% Back-Projection weigth, V
V=computeV(geo,angles,num2cell(angles),num2cell(1:length(angles)),'gpuids',gpuids);

if redundancy_weights
    % Data redundancy weighting, W_r implemented using Wang weighting
    % reference: https://iopscience.iop.org/article/10.1088/1361-6560/ac16bc

    num_frames = size(proj,3);
    W_r = redundancy_weighting(geo);
    W_r = repmat(W_r,[1,1,num_frames]);
    % disp('Size of redundancy weighting matrix');
    % disp(size(W_r));
    W = W.*W_r; % include redundancy weighting in W
end
%Initialize image.
%f=zeros(geo.nVoxel','single');

iter=0;
offOrigin=geo.offOrigin;
offDetector=geo.offDetector;
rotDetector=geo.rotDetector;
stop_criteria=0;
DSD=geo.DSD;
DSO=geo.DSO;
while ~stop_criteria %POCS
    % If quality is going to be measured, then we need to save previous image
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = f; % only store if necesary
    end
    f0=f;
    if (iter==0 && verbose==1);tic;end
    iter=iter+1;

    %Estimation error in the projection domain
    est_proj=Ax(f,geo,angles,'interpolated');
    delta_p=im3Dnorm(est_proj-proj,'L2');

    %Enforcing ART along all projections if squared delta_p > epsilon
    if (delta_p^2)>epsilon
        for jj=1:size(angles,2)
            if size(offOrigin,2)==size(angles,2)
                geo.offOrigin=offOrigin(:,jj);
            end
            if size(offDetector,2)==size(angles,2)
                geo.offDetector=offDetector(:,jj);
            end
            if size(rotDetector,2)==size(angles,2)
                geo.rotDetector=rotDetector(:,jj);
            end
            if size(DSD,2)==size(angles,2)
                geo.DSD=DSD(jj);
            end
            if size(DSO,2)==size(angles,2)
                geo.DSO=DSO(jj);
            end

            f=f+beta* bsxfun(@times,1./sum(V(:,:,jj),3),Atb(W(:,:,jj).*(proj(:,:,jj)-Ax(f,geo,angles(:,jj),'gpuids',gpuids)),geo,angles(:,jj),'gpuids',gpuids));

        end
    end

    %Non-negativity projection on all pixels
    if nonneg
        f=max(f,0);
    end

    geo.offDetector=offDetector;
    geo.offOrigin=offOrigin;

    if measurequality
        qualMeasOut(:,iter)=Measure_Quality(res_prev,f,QualMeasOpts);
    end

    % Compute L2 error of actual image. Ax-b
    dd=im3Dnorm(Ax(f,geo,angles,'gpuids',gpuids)-proj,'L2');

    % Compute change in the image after last SART iteration
    dp_vec=(f-f0);

    if iter==1
        step=1;
    else
        step=delta_p/delta_p_first;
    end
    f0=f;
    %  TV MINIMIZATION
    % =========================================================================
    %  Call GPU to minimize AwTV
    f=minimizeAwTV(f0,step,ng,delta,'gpuids',gpuids);    %   This is the MATLAB CODE, the functions are sill in the library, but CUDA is used nowadays
    %                                             for ii=1:ng
    %                                                 %delta=-0.00038 for thorax phantom
    %                                                 df=weighted_gradientTVnorm(f,delta);
    %                                                 df=df./im3Dnorm(df,'L2');
    %                                                 f=f-(step.*df);
    %                                             end
    % Compute change by TV min
    dg_vec=(f-f0);

    if iter==1
        delta_p_first=im3Dnorm((Ax(f0,geo,angles,'interpolated','gpuids',gpuids))-proj,'L2');
    end

    % Reduce SART step
    beta=beta*beta_red;

    % Check convergence criteria
    % ==========================================================================

    %Define c_alpha as in equation 21 in the journal
    c=dot(dg_vec(:),dp_vec(:))/(norm(dg_vec(:),2)*norm(dp_vec(:),2));
    %This c is examined to see if it is close to -1.0

    if (c<-0.99 && dd<=epsilon) || beta<0.005|| iter>maxiter
        if verbose
            disp(['Stopping criteria met']);
            disp(['   c    = ' num2str(c)]);
            disp(['   beta = ' num2str(beta)]);
            disp(['   iter = ' num2str(iter)]);
        end
        stop_criteria=true;
    end

    if computeL2
        geo.offOrigin=offOrigin;
        geo.offDetector=offDetector;
        errornow=im3Dnorm(proj-Ax(f,geo,angles,'gpuids',gpuids),'L2');                       % Compute error norm2 of b-Ax
        % If the error is not minimized.
        if  iter~=1 && errornow>errorL2(end)
            if verbose
                disp(['Convergence criteria met, exiting on iteration number:', num2str(iter)]);
            end
            return;
        end
        errorL2=[errorL2 errornow];
    end

    if (iter==1 && verbose==1)
        expected_time=toc*maxiter;
        disp('AwPCSD');
        disp(['Expected duration  :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end

end
end

function [beta,beta_red,f0,ng,verbose,epsilon,delta,QualMeasOpts,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,argin)
opts=     {'lambda','lambda_red','init','tviter','verbose','maxl2err','delta','qualmeas','nonneg','gpuids','redundancy_weighting','groundtruth'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('TIGRE:AwPCSD:InvalidInput','Invalid number of inputs')
end

% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('TIGRE:AwPCSD:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option isnot default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('TIGRE:AwPCSD:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end
    % parse inputs
    switch opt
        % Verbose
        %  =========================================================================
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('TIGRE:Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
            % Lambda
            %  =========================================================================
        case 'lambda'
            if default
                beta=1;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:AwPCSD:InvalidInput','Invalid lambda')
                end
                beta=val;
            end
            % Lambda reduction
            %  =========================================================================
        case 'lambda_red'
            if default
                beta_red=0.99;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:AwPCSD:InvalidInput','Invalid lambda')
                end
                beta_red=val;
            end
            % Initial image
            %  =========================================================================
        case 'init'
            if default || strcmp(val,'none')
                f0=zeros(geo.nVoxel','single');

            else
                if strcmp(val,'FDK')
                    f0=FDK(proj, geo, angles);
                else
                    error('TIGRE:AwPCSD:InvalidInput','Invalid init')
                end
            end
            % Number of iterations of TV
            %  =========================================================================
        case 'tviter'
            if default
                ng=20;
            else
                ng=val;
            end
            %  Maximum L2 error to have a "good image"
            %  =========================================================================
        case 'maxl2err'
            if default
                epsilon=im3Dnorm(FDK(proj,geo,angles))*0.2; %heuristic
            else
                epsilon=val;
            end
            %Parameter to control the amount of smoothing for pixels at the
            %edges
            %  =========================================================================
        case 'delta'
            if default
                delta=-0.005;
            else
                delta=val;
            end
            % Image Quality Measure
            %  =========================================================================
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('TIGRE:AwPCSD:InvalidInput','Invalid quality measurement parameters');
                end
            end
            %  Non negative
            %  =========================================================================
        case 'nonneg'
            if default
                nonneg=true;
            else
                nonneg=val;
            end
            %  GPU Ids
            %  =========================================================================
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'redundancy_weighting'
            if default
                redundancy_weights = true;
            else
                redundancy_weights = val;
            end
                case 'groundtruth'
            if default
                gt=nan;
            else
                gt=val;
            end
        otherwise
            error('TIGRE:AwPCSD:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in AwPCSD()']);

    end
end

end


%% Copyright (C) 2014, 2016 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defop  Method   @@sym le {(@var{a}, @var{b})}
%% @defopx Operator @@sym {@var{a} <= @var{b}} {}
%% Test/define symbolic inequality, less than or equal to.
%%
%% Examples:
%% @example
%% @group
%% sym(1) <= sym(pi)
%%   @result{} (sym) True
%%
%% syms x
%% x <= 10
%%   @result{} (sym) x ≤ 10
%% @end group
%% @end example
%%
%% @seealso{@@sym/lt, @@sym/gt, @@sym/ge, @@sym/eq, @@sym/ne,
%%          @@sym/logical, @@sym/isAlways}
%% @end defop

function t = le(x, y)

  if (nargin ~= 2)
    print_usage ();
  end

  t = ineq_helper('<=', 'Le', sym(x), sym(y));

end

%!test
%! % simple
%! x = sym(1); y = sym(1); e = x <= y;
%! assert (logical (e))
%! x = sym(1); y = sym(2); e = x <= y;
%! assert (logical (e))

%!test
%! % array -- array
%! syms x
%! a = sym([1 3 3 2*x]);
%! b = sym([2 x 3 10]);
%! e = a <= b;
%! assert (isa (e, 'sym'))
%! assert (logical (e(1)))
%! assert (isa (e(2), 'sym'))
%! assert (isequal (e(2), 3 <= x))
%! assert (logical (e(3)))
%! assert (isa (e(4), 'sym'))
%! assert (isequal (e(4), 2*x <= 10))


function isol = get_isolated(st0, id0, Mask, nt1)

% nt1 = 41;

imin = 1;
icurrent = 1;
imax = 1;
nspbatch = numel(st0);

isol = true(nspbatch, 1);

nt0 = (size(Mask,1)+1)/2;
while(icurrent<=nspbatch)
    while (st0(imax) - st0(icurrent)) <= nt1-1 && imax<nspbatch
        imax = imax+1;
    end

    while (st0(icurrent) - st0(imin)) >= nt1-1
        imin = imin+1;
    end
    for i = [imin:icurrent-1 icurrent+1:imax-1]
        if (Mask(nt0 + (st0(i) - st0(icurrent)), id0(icurrent), id0(i)))
            isol(icurrent) = false;
            break;
        end
    end

    icurrent = icurrent + 1;
end


function ListFacesInHullROI = MakeGeodesicOuterROI (mesh_outer, iV, facesInROI);

FacesByLines=[];
FacesByLines=mesh_outer.faces(facesInROI,:);

[index,value]=find(FacesByLines==iV);
index=index';
t=FacesByLines(index,:);

more off
l=unique(t(:));
l2=l;
oldsize=1;
while size(l,1)>oldsize
        for r=1:size(l2,1)
        [index2,value2]=find(FacesByLines==l2(r));
        index2=index2';
        index=[index index2];
        t2=FacesByLines(index2,:);
        t=[t; t2];
        end
     t=unique(t,'rows');
     oldsize=size(l);
     oldl=l;
     l=unique(t(:));
     l2=setdiff(l,oldl);
     indexListHull=unique(index);
end

ListFacesInHullROI=t;

if size(facesInROI,2) ~= size(indexListHull,2)
  disp([' non geodesic regions removed from the patch '])
end


function [errKN,errKI] = getErrInfIFE3D(uh, fun, mesh, femI, fem, dind)

%% USAGE: generate u-uh on all Gaussian points
%
% INPUTS:
% fun --- coefficient function
% mesh --- a struct data contains very rich mesh information.
% fem --- global DoF for test function space
% femI --- global DoF for test function space
% dind --- derivative info for test function
%            dind = [0,0,0]: function value
%            dind = [1,0,0]: Dx value
%            dind = [0,1,0]: Dy value
%            dind = [0,0,1]: Dz value
%
% OUTPUTS:
% matrix --- global (mass, stiffness, ...) matrix.

% Last Modified: 07/11/2020 by Xu Zhang
%% 0. Initializaiton
if strcmp(fem.type,'P1')||strcmp(fem.type,'DGP1')||strcmp(fem.type,'CR')
    feEvalBas = @evalP1Bas3D;
elseif strcmp(fem1.type,'P2')||strcmp(fem.type,'DGP2')
    feEvalBas = @evalP2Bas3D;
end
%% 1. Error on noninterface elements
dof = fem.ldof;
gx = fem.gx; gy = fem.gy; gz = fem.gz;
ntID = find(mesh.tLoc > 0);
gxN = gx(ntID,:); gyN = gy(ntID,:); gzN = gz(ntID,:);
uhK = uh(fem.t); uhKN = uhK(ntID,:);
tuN = feval(fun, gxN, gyN, gzN);
uKN = 0;
for i = 1:dof
    BAS = fem.bas(ntID,:,i);
    uKN = uKN + uhKN(:,i).*feEvalBas(BAS, gxN, gyN, gzN, dind);
end
errKN = norm(tuN-uKN,'inf');

%% 2. Error on interface elements
gxI = femI.gx; gyI = femI.gy; gzI = femI.gz;
itID = find(mesh.tLoc < 0); ntI = length(itID);
uhK = uh(fem.t); uhKitmp = uhK(itID,:); uhKI = zeros(length(femI.t),4);
id = 0;
for i = 1:ntI
    tmp = uhKitmp(i,femI.locID(i,:));
    uhKI(id+1:id+femI.quadT(i),:) = repmat(tmp,femI.quadT(i),1);
    id = id + femI.quadT(i);
end
tuI = feval(fun,gxI,gyI,gzI);
uKI = 0;
for i = 1:dof
    BAS = femI.bas(:,:,i);
    uKI = uKI + uhKI(:,i).*feEvalBas(BAS, gxI, gyI, gzI, dind);
end
errKI = norm(tuI-uKI,'inf');


function C = xor(A,B)
%XOR Logical XOR for sptensors.
%
%   See also SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Observations for sparse matrix case.
% The result of xor(a,5) is dense!
% The result of xor(a,0) is dense!
% The result of xor(a,full(a)) is dense!
% The result of xor(a,b) is sparse.

%% Case 1: One argument is a scalar
if isscalar(B) || isa(B,'tensor')
    C = xor(full(A),B);
    return;
end
if isscalar(A)
    C = xor(A,full(B));
    return;
end

%% Case 2: Both x and y are tensors of some sort
if ~isequal(size(A),size(B))
    error('Must be tensors of the same size');
end

if isa(A,'sptensor') && isa(B,'sptensor')
    C = sptensor([A.subs; B.subs], 1, size(A), @(x) length(x) == 1);
    return;
end

if isa(B,'tensor')
    Bsubs = find(B ~= 0);
    C = sptensor([A.subs; Bsubs], 1, size(A), @(x) length(x) == 1);
    return;
end

%% Otherwise
error('The arguments must be two sptensors or an sptensor and a scalar.');