function [IR, VAR] = VARir(VAR,VARopt)
% =========================================================================
% Compute impulse responses (IRs) for a VAR model estimated with the
% VARmodel.m function. Four identification schemes can be specified:
% zero contemporaneous restrictions, zero long-run restrictions, sign
% restrictions, and external instrumenmts.
% =========================================================================
% [IRF, VAR] = VARir(VAR,VARopt)
% -------------------------------------------------------------------------
% INPUT
%   - VAR: structure, result of VARmodel.m
%   - VARopt: options of the VAR (result of VARmodel.m)
% -------------------------------------------------------------------------
% OUTPUT
%   - IR(:,:,:) : matrix with IRF (H horizons, N variables, N shocks)
%   - VAR: structure including VAR estimation results. Note here that the
%       structure VAR is an output of VARmodel, too. This fucntion adds to
%       VAR some additional results, e.g. VAR.B is the structural impact
%       matrix
% =======================================================================
% VAR Toolbox 3.0
% Ambrogio Cesa-Bianchi
% ambrogiocesabianchi@gmail.com
% March 2012. Updated November 2020
% -----------------------------------------------------------------------

%% Check inputs
%==========================================================================
if ~exist('VAR','var')
    error('You need to provide VAR structure, result of VARmodel');
end
IV = VAR.IV;
if strcmp(VARopt.ident,'iv')
    if isempty(IV)
        error('You need to provide the data for the instrument in VAR (IV)');
    end
end

%% Retrieve and initialize variables
%==========================================================================
nsteps = VARopt.nsteps;
impact = VARopt.impact;
shut   = VARopt.shut;
recurs = VARopt.recurs;
Fcomp  = VAR.Fcomp;
nvar   = VAR.nvar;
nlag   = VAR.nlag;
sigma  = VAR.sigma;
IR     = nan(nsteps,nvar,nvar);

%% Compute Wold representation
%==========================================================================
% Initialize Wold multipliers
PSI = zeros(nvar,nvar,nsteps);
% Re-write F matrix to compute multipliers
VAR.Fp = zeros(nvar,nvar,nlag);
I = VAR.const+1;
for ii=1:nsteps
    if ii<=nlag
        VAR.Fp(:,:,ii) = VAR.F(:,I:I+nvar-1);
    else
        VAR.Fp(:,:,ii) = zeros(nvar,nvar);
    end
    I = I + nvar;
end
% Compute multipliers
PSI(:,:,1) = eye(nvar);
for ii=2:nsteps
    jj=1;
    aux = 0;
    while jj<ii
        aux = aux + PSI(:,:,ii-jj)*VAR.Fp(:,:,jj);
        jj=jj+1;
    end
    PSI(:,:,ii) = aux;
end
% Update VAR with Wold multipliers
VAR.PSI = PSI;

%% Identification: Recover B matrix
%==========================================================================
% B matrix is recovered with Cholesky decomposition
if strcmp(VARopt.ident,'short')
    [out, chol_flag] = chol(sigma);
    if chol_flag~=0; error('VCV is not positive definite'); end
    B = out';
% B matrix is recovered with Cholesky on cumulative IR to infinity
elseif strcmp(VARopt.ident,'long')
    Finf_big = inv(eye(length(Fcomp))-Fcomp);
    Finf = Finf_big(1:nvar,1:nvar);
    D  = chol(Finf*sigma*Finf')';
    B = Finf\D;
% B matrix is recovered with SR.m
elseif strcmp(VARopt.ident,'sign')
    if isempty(VAR.B)
        error('You need to provide the B matrix with SR.m and/or SignRestrictions.m')
    else
        B = VAR.B;
    end
% B matrix is recovered with external instrument IV
elseif strcmp(VARopt.ident,'iv')
    % Recover residuals (first variable is the one to be instrumented - order matters!)
    up = VAR.resid(:,1);     % residuals to be instrumented
    uq = VAR.resid(:,2:end); % residulas for second stage

    % Make sample of IV comparable with up and uq
    [aux, fo, lo] = CommonSample([up IV(VAR.nlag+1:end,:)]);
    p = aux(:,1);
    q = uq(end-length(p)+1:end,:); pq = [p q];
    Z = aux(:,2:end);

    % Run first stage regression and fitted
    FirstStage = OLSmodel(p,Z);
    p_hat = FirstStage.yhat;

    % Recover first column of B matrix with second stage regressions
    Biv(1,1) = 1;  % Start with impact IR normalized to 1
    sqsp = zeros(size(q,2),1);
    for ii=2:nvar
        SecondStage = OLSmodel(q(:,ii-1),p_hat);
        Biv(ii,1) = SecondStage.beta(2);
        sqsp(ii-1) = SecondStage.beta(2);
    end
    % Update size of the shock (ftn 4 of Gertler and Karadi (2015))
    sigma_b = (1/(length(pq)-VAR.ntotcoeff))*...
        (pq-repmat(mean(pq),size(pq,1),1))'*...
        (pq-repmat(mean(pq),size(pq,1),1));
    s21s11 = sqsp;
    S11 = sigma_b(1,1);
    S21 = sigma_b(2:end,1);
    S22 = sigma_b(2:end,2:end);
    Q = s21s11*S11*s21s11'-(S21*s21s11'+s21s11*S21')+S22;
    sp = sqrt(S11-(S21-s21s11*S11)'*(Q\(S21-s21s11*S11)));
    % Rescale Biv vector
    Biv = Biv*sp;
    B = zeros(nvar,nvar);
    B(:,1) = Biv;
% If none of the above, you've done somerthing wrong :)
else
    disp('---------------------------------------------')
    disp('Identification incorrectly specified.')
    disp('Choose one of the following options:');
    disp('- short: zero contemporaneous restrictions');
    disp('- long:  zero long-run restrictions');
    disp('- sign:  sign restrictions');
    disp('- iv:  external instrument');
    disp('---------------------------------------------')
    error('ERROR. See details above');
end

%% Compute the impulse response
%==========================================================================
for mm=1:nvar
    % Set to zero a row of the companion matrix if "shut" is selected
    if shut~=0
        Fcomp(shut,:) = 0;
    end
    % Initialize the impulse response vector
    response = zeros(nvar, nsteps);
    % Create the impulse vector
    impulse = zeros(nvar,1);
    % Set the size of the shock
    if impact==0
        impulse(mm,1) = 1; % one stdev shock
    elseif impact==1
        impulse(mm,1) = 1/B(mm,mm); % unitary shock
    else
        error('Impact must be either 0 or 1');
    end
    % First period impulse response (=impulse vector)
    response(:,1) = B*impulse;
    % Shut down the response if "shut" is selected
    if shut~=0
        response(shut,1) = 0;
    end
    % Recursive computation of impulse response
    if strcmp(recurs,'wold')
        for kk = 2:nsteps
            response(:,kk) = PSI(:,:,kk)*B*impulse;
        end
    elseif strcmp(recurs,'comp')
        for kk = 2:nsteps
            FcompN = Fcomp^(kk-1);
            response(:,kk) = FcompN(1:nvar,1:nvar)*B*impulse;
        end
    end
    IR(:,:,mm) = response';
end
% Update VAR with structural impact matrix
VAR.B = B;
if strcmp(VARopt.ident,'iv')
    VAR.FirstStage = FirstStage;
    VAR.sigma_b = sigma_b;
    VAR.Biv = Biv;
end


function [y,nbr_bits] = perform_spiht_coding(x,options)

% perform_spiht_coding - SPIHT coding of wavelet coefficients
%
% Coding :
%   options.Jmin = ??;      % minimum scale of the transform
%   options.nb_bits = ??;   % target number of bits
%   [stream,nbr_bits] = perform_spiht_coding(MW,options);
% Decoding :
%   MW = perform_spiht_coding(stream);
%
% This is a simple wrapper of the code of Jing Tian
% <scuteejtian at hotmail.com>

options.null = 0;

if isfield(options, 'arithmetic_coding')
    arithmetic_coding = options.arithmetic_coding;
else
    arithmetic_coding = 1;
end

if size(x,1)>1 && size(x,2)>1
    if isfield(options, 'Jmin')
        Jmin = options.Jmin;
    else
        warning('You should provide options.Jmin');
        Jmin = 4;
    end
    if isfield(options, 'nb_bits')
        nb_bits = options.nb_bits;
    else
        nb_bits = floor( prod(size(x))*0.1 );
    end
    %-----------   Coding   ----------------
    Jmax = log2(size(x,1))-1;
    level = Jmax-Jmin+1;
    y = func_SPIHT_Enc(x, nb_bits, level); y = y(:);
    if arithmetic_coding
        % remove trailing 2
        I = find(y==2); y(I) = [];
        % the 3 first entry are [size nb_bitsplanes level]
        [z,nbr_bits] = perform_arithmetic_coding(y(4:end), +1, options);
        y = [y(1:3); z];
        nbr_bits = nbr_bits + 16; % approx 16 bits
    else
        nbr_bits = nb_bits;
    end
else
    %-----------   Decoding   ----------------
    if arithmetic_coding
        x = x(:);
        % the 3 first entry are [size nb_bitsplanes level]
        z = perform_arithmetic_coding(x(4:end), -1, options);
        x = [x(1:3); z];
        nbr_bits = -1;
    end
    y = func_SPIHT_Dec(x(:)');
end

function out = func_SPIHT_Enc(m, max_bits, level)
% Matlab implementation of SPIHT (without Arithmatic coding stage)
%
% Encoder
%
% input:    m : input image in wavelet domain
%           max_bits : maximum bits can be used
%           level : wavelet decomposition level
%
% output:   out : bit stream
%
% Jing Tian
% Contact me : scuteejtian@hotmail.com
% This program is part of my undergraduate project in GuangZhou, P. R. China.
% April - July 1999

%-----------   Initialization  -----------------
bitctr = 0;
out = 2*ones(1,max_bits - 14);
n_max = floor(log2(abs(max(max(m)'))));
Bits_Header = 0;
Bits_LSP = 0;
Bits_LIP = 0;
Bits_LIS = 0;

%-----------   output bit stream header   ----------------
% image size, number of bit plane, wavelet decomposition level should be
% written as bit stream header.
out(1,[1 2 3]) = [size(m,1) n_max level]; bitctr = bitctr + 24;
index = 4;
Bits_Header = Bits_Header + 24;

%-----------   Initialize LIP, LSP, LIS   ----------------
temp = [];
bandsize = 2.^(log2(size(m, 1)) - level + 1);
temp1 = 1 : bandsize;
for i = 1 : bandsize
    temp = [temp; temp1];
end
LIP(:, 1) = temp(:);
temp = temp';
LIP(:, 2) = temp(:);
LIS(:, 1) = LIP(:, 1);
LIS(:, 2) = LIP(:, 2);
LIS(:, 3) = zeros(length(LIP(:, 1)), 1);
pstart = 1;
pend = bandsize / 2;
for i = 1 : bandsize / 2
    LIS(pstart : pend, :) = [];
    pdel = pend - pstart + 1;
    pstart = pstart + bandsize - pdel;
    pend = pend + bandsize - pdel;
end
LSP = [];

n = n_max;

%-----------   coding   ----------------
while(bitctr < max_bits)

    % Sorting Pass
    LIPtemp = LIP; temp = 0;
    for i = 1:size(LIPtemp,1)
        temp = temp+1;
        if (bitctr + 1) >= max_bits
            if (bitctr < max_bits)
                out(length(out))=[];
            end
            return
        end
        if abs(m(LIPtemp(i,1),LIPtemp(i,2))) >= 2^n % 1: positive; 0: negative
            out(index) = 1; bitctr = bitctr + 1;
            index = index +1; Bits_LIP = Bits_LIP + 1;
            sgn = m(LIPtemp(i,1),LIPtemp(i,2))>=0;
            out(index) = sgn; bitctr = bitctr + 1;
            index = index +1; Bits_LIP = Bits_LIP + 1;
            LSP = [LSP; LIPtemp(i,:)];
            LIP(temp,:) = []; temp = temp - 1;
        else
            out(index) = 0; bitctr = bitctr + 1;
            index = index +1;
            Bits_LIP = Bits_LIP + 1;
        end
    end

    LIStemp = LIS; temp = 0; i = 1;
    while ( i <= size(LIStemp,1))
        temp = temp + 1;
        if LIStemp(i,3) == 0
            if bitctr >= max_bits
                return
            end
            max_d = func_MyDescendant(LIStemp(i,1),LIStemp(i,2),LIStemp(i,3),m);
            if max_d >= 2^n
                out(index) = 1; bitctr = bitctr + 1;
                index = index +1; Bits_LIS = Bits_LIS + 1;
                x = LIStemp(i,1); y = LIStemp(i,2);

                if (bitctr + 1) >= max_bits
                    if (bitctr < max_bits)
                        out(length(out))=[];
                    end
                    return
                end
                if abs(m(2*x-1,2*y-1)) >= 2^n
                    LSP = [LSP; 2*x-1 2*y-1];
                    out(index) = 1; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    sgn = m(2*x-1,2*y-1)>=0;
                    out(index) = sgn; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                else
                    out(index) = 0; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    LIP = [LIP; 2*x-1 2*y-1];
                end

                if (bitctr + 1) >= max_bits
                    if (bitctr < max_bits)
                        out(length(out))=[];
                    end
                    return
                end
                if abs(m(2*x-1,2*y)) >= 2^n
                    LSP = [LSP; 2*x-1 2*y];
                    out(index) = 1; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    sgn = m(2*x-1,2*y)>=0;
                    out(index) = sgn; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                else
                    out(index) = 0; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    LIP = [LIP; 2*x-1 2*y];
                end

                if (bitctr + 1) >= max_bits
                    if (bitctr < max_bits)
                        out(length(out))=[];
                    end
                    return
                end
                if abs(m(2*x,2*y-1)) >= 2^n
                    LSP = [LSP; 2*x 2*y-1];
                    out(index) = 1; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    sgn = m(2*x,2*y-1)>=0;
                    out(index) = sgn; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                else
                    out(index) = 0; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    LIP = [LIP; 2*x 2*y-1];
                end

                if (bitctr + 1) >= max_bits
                    if (bitctr < max_bits)
                        out(length(out))=[];
                    end
                    return
                end
                if abs(m(2*x,2*y)) >= 2^n
                    LSP = [LSP; 2*x 2*y];
                    out(index) = 1; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    sgn = m(2*x,2*y)>=0;
                    out(index) = sgn; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                else
                    out(index) = 0; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    LIP = [LIP; 2*x 2*y];
                end

                if ((2*(2*x)-1) < size(m) & (2*(2*y)-1) < size(m))
                    LIS = [LIS; LIStemp(i,1) LIStemp(i,2) 1];
                    LIStemp = [LIStemp; LIStemp(i,1) LIStemp(i,2) 1];
                end
                LIS(temp,:) = []; temp = temp-1;

            else
                out(index) = 0; bitctr = bitctr + 1;
                index = index +1; Bits_LIS = Bits_LIS + 1;
            end
        else
            if bitctr >= max_bits
                return
            end
            max_d = func_MyDescendant(LIStemp(i,1),LIStemp(i,2),LIStemp(i,3),m);
            if max_d >= 2^n
                out(index) = 1; bitctr = bitctr + 1;
                index = index +1;
                x = LIStemp(i,1); y = LIStemp(i,2);
                LIS = [LIS; 2*x-1 2*y-1 0; 2*x-1 2*y 0; 2*x 2*y-1 0; 2*x 2*y 0];
                LIStemp = [LIStemp; 2*x-1 2*y-1 0; 2*x-1 2*y 0; 2*x 2*y-1 0; 2*x 2*y 0];
                LIS(temp,:) = []; temp = temp - 1;
            else
                out(index) = 0; bitctr = bitctr + 1;
                index = index +1; Bits_LIS = Bits_LIS + 1;
            end
        end
        i = i+1;
    end

    % Refinement Pass
    temp = 1;
    value = floor(abs(2^(n_max-n+1)*m(LSP(temp,1),LSP(temp,2))));
    while (value >= 2^(n_max+2) & (temp <= size(LSP,1)))
        if bitctr >= max_bits
            return
        end
        s = bitget(value,n_max+2);
        out(index) = s; bitctr = bitctr + 1;
        index = index +1; Bits_LSP = Bits_LSP + 1;
        temp = temp + 1;
        if temp <= size(LSP,1)
            value = floor(abs(2^(n_max-n+1)*m(LSP(temp,1),LSP(temp,2))));
        end
    end

    n = n - 1;
end

function m = func_SPIHT_Dec(in)
% Matlab implementation of SPIHT (without Arithmatic coding stage)
%
% Decoder
%
% input:    in : bit stream
%
% output:   m : reconstructed image in wavelet domain
%
% Jing Tian
% Contact me : scuteejtian@hotmail.com
% This program is part of my undergraduate project in GuangZhou, P. R. China.
% April - July 1999

%-----------   Initialization  -----------------
% image size, number of bit plane, wavelet decomposition level should be
% written as bit stream header.
m = zeros(in(1,1));
n_max = in(1,2);
level = in(1,3);
ctr = 4;

%-----------   Initialize LIP, LSP, LIS   ----------------
temp = [];
bandsize = 2.^(log2(in(1,1)) - level + 1);
temp1 = 1 : bandsize;
for i = 1 : bandsize
    temp = [temp; temp1];
end
LIP(:, 1) = temp(:);
temp = temp';
LIP(:, 2) = temp(:);

LIS(:, 1) = LIP(:, 1);
LIS(:, 2) = LIP(:, 2);
LIS(:, 3) = zeros(length(LIP(:, 1)), 1);
pstart = 1;
pend = bandsize / 2;
for i = 1 : bandsize / 2
    LIS(pstart : pend, :) = [];
    pdel = pend - pstart + 1;
    pstart = pstart + bandsize - pdel;
    pend = pend + bandsize - pdel;
end
LSP = [];

%-----------   coding   ----------------
n = n_max;
while (ctr <= size(in,2))

    %Sorting Pass
    LIPtemp = LIP; temp = 0;
    for i = 1:size(LIPtemp,1)
        temp = temp+1;
        if ctr > size(in,2)
            return
        end
        if in(1,ctr) == 1
            ctr = ctr + 1;
            if in(1,ctr) > 0
                m(LIPtemp(i,1),LIPtemp(i,2)) = 2^n + 2^(n-1);
            else
                m(LIPtemp(i,1),LIPtemp(i,2)) = -2^n  - 2^(n-1);
            end
            LSP = [LSP; LIPtemp(i,:)];
            LIP(temp,:) = []; temp = temp - 1;
        end
        ctr = ctr + 1;
    end

    LIStemp = LIS; temp = 0; i = 1;
    while ( i <= size(LIStemp,1))
        temp = temp + 1;
        if ctr > size(in,2)
            return
        end
        if LIStemp(i,3) == 0
            if in(1,ctr) == 1
                ctr = ctr + 1;
                x = LIStemp(i,1); y = LIStemp(i,2);

                if ctr > size(in,2)
                    return
                end
                if in(1,ctr) == 1
                    LSP = [LSP; 2*x-1 2*y-1];
                    ctr = ctr + 1;
                    if in(1,ctr) == 1
                        m(2*x-1,2*y-1) = 2^n + 2^(n-1);
                    else
                        m(2*x-1,2*y-1) = -2^n  - 2^(n-1);
                    end
                    ctr = ctr + 1;
                else
                    LIP = [LIP; 2*x-1 2*y-1];
                    ctr = ctr + 1;
                end

                if ctr > size(in,2)
                    return
                end
                if in(1,ctr) == 1
                    ctr = ctr + 1;
                    LSP = [LSP; 2*x-1 2*y];
                    if in(1,ctr) == 1;
                        m(2*x-1,2*y) = 2^n + 2^(n-1);
                    else
                        m(2*x-1,2*y) = -2^n  - 2^(n-1);
                    end
                    ctr = ctr + 1;
                else
                    LIP = [LIP; 2*x-1 2*y];
                    ctr = ctr + 1;
                end

                if ctr > size(in,2)
                    return
                end
                if in(1,ctr) == 1
                    ctr = ctr + 1;
                    LSP = [LSP; 2*x 2*y-1];
                    if in(1,ctr) == 1
                        m(2*x,2*y-1) = 2^n + 2^(n-1);
                    else
                        m(2*x,2*y-1) = -2^n  - 2^(n-1);
                    end
                    ctr = ctr + 1;
                else
                    LIP = [LIP; 2*x 2*y-1];
                    ctr = ctr + 1;
                end

                if ctr > size(in,2)
                    return
                end
                if in(1,ctr) == 1
                    ctr = ctr + 1;
                    LSP = [LSP; 2*x 2*y];
                    if in(1,ctr) == 1
                        m(2*x,2*y) = 2^n + 2^(n-1);
                    else
                        m(2*x,2*y) = -2^n  - 2^(n-1);
                    end
                    ctr = ctr + 1;
                else
                    LIP = [LIP; 2*x 2*y];
                    ctr = ctr + 1;
                end

                if ((2*(2*x)-1) < size(m) & (2*(2*y)-1) < size(m))
                    LIS = [LIS; LIStemp(i,1) LIStemp(i,2) 1];
                    LIStemp = [LIStemp; LIStemp(i,1) LIStemp(i,2) 1];
                end
                LIS(temp,:) = []; temp = temp-1;

            else
                ctr = ctr + 1;
            end
        else
            if in(1,ctr) == 1
                x = LIStemp(i,1); y = LIStemp(i,2);
                LIS = [LIS; 2*x-1 2*y-1 0; 2*x-1 2*y 0; 2*x 2*y-1 0; 2*x 2*y 0];
                LIStemp = [LIStemp; 2*x-1 2*y-1 0; 2*x-1 2*y 0; 2*x 2*y-1 0; 2*x 2*y 0];
                LIS(temp,:) = []; temp = temp - 1;
            end
            ctr = ctr + 1;
        end
        i = i+1;
    end

    % Refinement Pass
    temp = 1;
    value = m(LSP(temp,1), LSP(temp,2));
    while (abs(value) >= 2^(n+1) & (temp <= size(LSP,1)))
        if ctr > size(in,2)
            return
        end

        value = value + ((-1)^(in(1,ctr) + 1)) * (2^(n-1))*sign(m(LSP(temp,1),LSP(temp,2)));
        m(LSP(temp,1),LSP(temp,2)) = value;
        ctr = ctr + 1;
        temp = temp + 1;
        if temp <= size(LSP,1)
            value = m(LSP(temp,1),LSP(temp,2));
        end
    end

    n = n-1;
end

function value = func_MyDescendant(i, j, type, m)
% Matlab implementation of SPIHT (without Arithmatic coding stage)
%
% Find the descendant with largest absolute value of pixel (i,j)
%
% input:    i : row coordinate
%           j : column coordinate
%           type : type of descendant
%           m : whole image
%
% output:   value : largest absolute value
%
% Jing Tian
% Contact me : scuteejtian@hotmail.com
% This program is part of my undergraduate project in GuangZhou, P. R. China.
% April - July 1999

s = size(m,1);

S = [];

index = 0; a = 0; b = 0;

while ((2*i-1)<s & (2*j-1)<s)
    a = i-1; b = j-1;

    mind = [2*(a+1)-1:2*(a+2^index)];
    nind = [2*(b+1)-1:2*(b+2^index)];

    chk = mind <= s;
    len = sum(chk);
    if len < length(mind)
        mind(len+1:length(mind)) = [];
    end

    chk = nind <= s;
    len = sum(chk);
    if len < length(nind)
        nind(len+1:length(nind)) = [];
    end

    S = [S reshape(m(mind,nind),1,[])];

    index = index + 1;
    i = 2*a+1; j = 2*b+1;
end

if type == 1
    S(:,1:4) = [];;
end

value = max(abs(S));


classdef MappingComputer < handle

    properties (Access = private)
        LHS
        RHS
    end

    properties (Access = private)
        meshDisc
        mesh
        orientation
        interp
        interpolator
    end

    methods (Access = public)

        function obj = MappingComputer(cParams)
            obj.init(cParams);
        end

        function uF = compute(obj)
            obj.computeLHS();
            obj.computeRHS();
            uC = obj.solveSystem();
            In = obj.interpolator;
            u  = In*uC;
            uV(1,:,:) = reshape(u,obj.mesh.nnodeElem,[]);
            s.mesh    = obj.mesh;
            s.fValues = uV;
            uF = P1DiscontinuousFunction(s);
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.mesh        = cParams.mesh;
            obj.orientation  = cParams.orientation;
            obj.interpolator = cParams.interpolator;
            obj.meshDisc     = obj.mesh.createDiscontinuousMesh();
        end

        function computeLHS(obj)
            K = obj.computeStiffnessMatrix();
            In = obj.interpolator;
            Kn = In'*K*In;
            obj.LHS = Kn;
        end

        function K = computeStiffnessMatrix(obj)
            s.mesh = obj.mesh;
            s.type = 'StiffnessMatrix';
            s.fun  = P1DiscontinuousFunction.create(obj.mesh,1);
            lhs2 = LHSintegrator.create(s);
            K = lhs2.compute();
        end

        function computeRHS(obj)
            q = Quadrature.set(obj.mesh.type);
            q.computeQuadrature('QUADRATIC');
%             fG = obj.orientation.evaluate(q.posgp);
            s.mesh  = obj.meshDisc;
            s.type = 'ShapeDerivative';
            s.quadratureOrder = q.order;
            rhs  = RHSintegrator.create(s);
            rhsF = rhs.compute(obj.orientation);
            In = obj.interpolator;
            rhsV = In'*rhsF.fValues;
            obj.RHS = rhsV;
        end

        function u = solveSystem(obj)
            a.type = 'DIRECT';
            s = Solver.create(a);
            u = s.solve(obj.LHS,obj.RHS);
            u = u(1:end);
        end

    end

end


function [cIX_,gIX_,num] = ArtifactAnalysis(cIX,gIX,CellXYZ,absIX)
U = unique(gIX);
cIX_abs = absIX(cIX);
%%
% Mean = zeros(length(U),3);
% Median = zeros(length(U),3);
% Midpoint = zeros(length(U),3);
Std = zeros(length(U),3);
% TF_clus = zeros(length(U),1);
TF = zeros(length(gIX),1);
for i = 1:length(U),
   IX = gIX==U(i);
   YXZ = CellXYZ(cIX_abs(IX),:);
   for j = 1:3,
%        Mean(i,j) = mean(YXZ(:,j));
%        Median(i,j) = median(YXZ(:,j));
%        Midpoint(i,j) = mean([max(YXZ(:,j)),min(YXZ(:,j))]);
       Std(i,j) = std(YXZ(:,j));
   end
   tf1 = Std(i,1)<0.5;
   tf2 = Std(i,2)<0.5;
   tf3 = Std(i,3)<0.5;
   tf = tf1 || tf2 || tf3;
%    TF_clus(i) = tf;
   TF(IX) = tf;
end
%%
cIX_ = cIX(find(TF));
gIX_ = gIX(find(TF));
num = length(unique(gIX_));
end


function [G_D2,MSE]=GenerateGD2Table(numiter,N,alphaNT,G_D2init,Range_prior,Range_post)
%function [G_D2,MSE]=GenerateGD2Table(numiter,N,alphaNT,Range_prior,Range_post);
% Generates gain table for MMSE noise power estimation. The recursive
% nature of the estimation problem is taken into account by an iterative
% scheme. For more details see:
%
% J.S. Erkelens and R. Heusdens, "Tracking of nonstationary noise based on
% data-driven recursive noise power estimation", IEEE Trans. Audio,
% Speech & Lang. Proc., vol. 16, no. 6, pp. 1112-1123, 2008.
%
% J.S. Erkelens and R. Heusdens, "Fast noise tracking based on recursive
% smoothing of MMSE noise power estimates", ICASSP 2008, pp. 4873-4876.
%
% INPUT VARIABLES:
% numiter : number of optimization iterations.
% N : frame length.
% alphaNT : smoothing parameter in prior SNR parameter for noise tracking.
% Range_prior : range of the prior SNR parameter, e.g, [-19 40] (as used
% in the papers) means that the prior SNR is quantized between -19 dB and
% 40 dB in steps of 1 dB.
% Range_post: range of the posterior SNR. In the papers [-30 40] is used,
% meaning that the gain table G_D2 has dimensions 60-by-71.
% G_D2init : Initial gain table for MMSE estimation of noise power. May be
% found with the basic data-driven method. Simply initializing with
% ones(60,71) should also work (for Range_prior=[-19 40] and
% Range_post=[-30 40]).
%
% OUTPUT VARIABLES:
% G_D2 : gain table for MMSE estimation of noise power.
% MSE : normalized mean-square error per iteration (NOTE: since we are
% initializing with the noisy powers, this MSE may INCREASE with each
% iteration.
% If we apply the gain functions to the training data RECURSIVELY, however,
% the mse decreases with each iteration step. Both MSEs (on the traindata
% and when applied recursively) do converge to the same end value. See the
% 2nd paper mentioned above for a more thorough explanation and
% illustration.
%
% Copyright 2008: Delft University of Technology, Information and
% Communication Theory Group. The software is free for non-commercial use.
% This program comes WITHOUT ANY WARRANTY.
%
% Last modified: 17-3-2008.

global NoiseSpectrum
load NoiseSpectrum

Gd2new=G_D2init;
% Intialize MSE
MSE=zeros(1,numiter);
% SNRs (dB scale)
SNRs=[-12.5:5:27.5];
% Directory with training data
DIR='C:/data/Timit_TBW/Matrices/';
% List of training data files
fid=fopen('C:/data/Timit_TBW/trainlist.txt','r');
% read the first filename
s=fgetl(fid);
for k1=1:numiter
    % Initialize statistics needed to compute the optimal gain values
    sumD2R2=zeros(Range_prior(2)-Range_prior(1)+1,Range_post(2)-Range_post(1)+1);
    sumR4=sumD2R2;sumD4=sumD2R2;
    while s~=-1 % while not EOF
        % Use 2 Timit directories for training
        if strcmpi(s(16:18),'dr4') | strcmpi(s(16:18),'dr5')
            % Strip some path info and convert to lowercase
            s=lower(s(16:end-3));
            % Some difference between Windows and Linux
            I=find(s=='\');s(I)='/';
            % Collect statistics for all SNRs
            for snrnr=1:length(SNRs)
                % strip path info, retain file name
                filestr=s(11:end-1);
                % directory name for arrays of frame numbers with
                % sufficient speech energy.
                dirstr=[DIR s(1:10)];
                % load indices of frames with sufficient speech energy.
                % Only those are used for optimization of the gain.
                eval(['load ' dirstr filestr '_IsufE'])
                % directory name for the data at current SNR
                dirstr=[DIR s(1:10) 'Snr' num2str(snrnr) '/'];
                % load noise powers (needed to compute the statistics
                % for gain optimization).
                iterstr='Init';matstrD='Dmatrix';
                filestrD=[dirstr filestr '_' matstrD 'Snr' num2str(snrnr) iterstr];
                eval(['load ' filestrD ' ' matstrD])
                % "Dclean" means the "true" noise powers. The final
                % estimates will of course not be "clean" but contaminated
                % by some speech power.
                Dclean=Dmatrix;
                % Load noisy amplitudes (Rmatrix)
                filestrR=[dirstr filestr '_Rmatrix' 'Snr' num2str(snrnr) 'Init'];
                eval(['load ' filestrR ' Rmatrix'])
                % Load estimates from the previous iteration (Dmatrix) ...
                iterstr='Iter';
                filestrD=[dirstr filestr '_' matstrD 'Snr' num2str(snrnr) iterstr];
                eval(['load ' filestrD ' ' matstrD])
                % ... but initialize with 'Rmatrix', i.e., noisy amplitudes
                % in the first iteration.
                if k1==1,Dmatrix=Rmatrix;end
                % Collect statistics and calculate new data
                [sumd4,sumd2r2,sumr4,Dmatrix]=collectdata(N,alphaNT,IsufE,Dclean,Dmatrix,Rmatrix,Gd2new,Range_prior,Range_post,SNRs(snrnr));
                sumD2R2=sumD2R2+sumd2r2;sumR4=sumR4+sumr4;sumD4=sumD4+sumd4;
                % Save updated Dmatrix
                iterstr='Iter';
                filestrD=[dirstr filestr '_' matstrD 'Snr' num2str(snrnr) iterstr];
                eval(['save ' filestrD ' ' matstrD])
            end % for snrnr=1:length(SNRs)
        end % strcmpi(s(16:18),'dr4') | strcmpi(s(16:18),'dr5')
        % read the next file name
        s=fgetl(fid);
    end % while s~=-1
    % Update gain function and calculate MSE
    sumR41=sumR4;
    sumR41(sumR41==0)=1;% avoid dividing by 0
    Gd2new=sumD2R2./sumR41;
    % Calculate MSE
    MSEd=sumD4-2*Gd2new.*sumD2R2+(Gd2new.^2).*sumR4;
    % Normalize
    MSEd=sum(sum(MSEd))/sum(sum(sumD4));
    MSE(k1)=MSEd;
    % Save some stuff
    alphap=0.1;alphad=0.85;T=4;% These values are used below in the function collectdata
    eval(['save ' DIR2 'GainTable_iteration' num2str(k1) ' Gd2new MSE alphaNT alphad alphap N T']);
    % display number of iterations to go and MSE
    disp(numiter-k1),disp(MSEd)
    % Go through all the data again in the next iteration
    frewind(fid);s=fgetl(fid);
end % for k1=1:numiter
fclose(fid);
G_D2=Gd2new;

function [sumd4,sumd2r2,sumr4,Dnew]=collectdata(N,alphaNT,IsufE,Dclean,Dmatrix,Rmatrix,Gd2new,Range_prior,Range_post,dBsnr)
global NoiseSpectrum
% Initialization of statistics for computing the gain table
sumd2r2=zeros(Range_prior(2)-Range_prior(1)+1,Range_post(2)-Range_post(1)+1);
sumr4=sumd2r2;
sumd4=sumd2r2;
alpha2=0.85*ones(1,N/2+1)';
alphap=0.1;alphad=alpha2(1);
T=4;
% Frequency bins used to collect the statistics from
freqbins=10:110;
% speech presence probability
Pspi=zeros(1,N/2+1)';Pspii=Pspi;
% speech presence index
Isp=zeros(1,N/2+1)';
% minimum value prior SNR
ksi_min=10^(-1.9);% -19 dB
% Initialization of noise variance for iteration i and i+1 (denoted by ii)
labda_di=NoiseSpectrum/(10^(dBsnr/10));%True noise variance.
% Appropriate when the speech is normalized as in GenerateTraindata.m
% Note that statistics are collected for bins in the passband only.
labda_dii=labda_di;
S=size(Dmatrix);
Dnew=zeros(S);Dnew(:,1)=Dmatrix(:,1);
% frame length
N=2*S(1)-2;
S=S(2);
% length of a column of the gain matrix
Gsize=size(Gd2new);Gsize=Gsize(1);
for m=2:S
    R=Rmatrix(:,m);D=Dclean(:,m);
    % Update prior SNR (priorNT) and posterior SNR (postSNR) using data
    % from the last iteration
    postSNR=R.^2./labda_di;
    B0=Rmatrix(:,m-1).^2;
    priorNT=max(alphaNT*B0./labda_di+(1-alphaNT)*postSNR,ksi_min);
    dBprior=10*log10(priorNT);
    dBpost=10*log10(postSNR+eps);
    % Indices in gain table
    Iprior=min(max(round(dBprior),Range_prior(1)),Range_prior(2))-Range_prior(1)+1;
    Ipost=min(max(round(dBpost),Range_post(1)),Range_post(2))-Range_post(1)+1;
    % Update labda_di
    D20=Dmatrix(:,m).^2;
    Mpost=freqsmooth(postSNR,1);Isp=(Mpost>T);
    Pspi=alphap.*Pspi+(1-alphap).*Isp;
    alpha2i=alphad+(1-alphad).*Pspi;
    labda_di=alpha2i.*labda_di+(1-alpha2i).*D20;
    % Calculate new estimated noise powers
    D2=GainR2(Gd2new,Iprior,Ipost,R,N);
    Dnew(:,m)=sqrt(D2);
    % Calculate new priorNT en postSNR
    postSNR=R.^2./labda_dii;
    priorNT=max(alphaNT*B0./labda_dii+(1-alphaNT)*postSNR,ksi_min);
    dBprior=10*log10(priorNT);
    dBpost=10*log10(postSNR+eps);
    Iprior=min(max(round(dBprior),Range_prior(1)),Range_prior(2))-Range_prior(1)+1;
    Ipost=min(max(round(dBpost),Range_post(1)),Range_post(2))-Range_post(1)+1;
    % Collect data from bins 10 t/m 110 (because telephone bandwidth speech
    % is limited to about 300-3400 Hz) for frames with sufficient speech
    % energy.
    if ~isempty(intersect(m,IsufE))
        %CHECK FOR MULTIPLE ELEMENTS
        index2=Iprior(freqbins)+Gsize*(Ipost(freqbins)-1);
        if length(unique(index2))==length(index2)
            sumd4(index2)=sumd4(index2)+D(freqbins).^4;
            sumr4(index2)=sumr4(index2)+R(freqbins).^4;
            sumd2r2(index2)=sumd2r2(index2)+(R(freqbins).*D(freqbins)).^2;
        else
            for k1=1:length(freqbins)
                index2=Iprior(freqbins(k1))+Gsize*(Ipost(freqbins(k1))-1);
                sumd4(index2)=sumd4(index2)+D(freqbins(k1)).^4;
                sumr4(index2)=sumr4(index2)+R(freqbins(k1)).^4;
                sumd2r2(index2)=sumd2r2(index2)+(R(freqbins(k1)).*D(freqbins(k1))).^2;
            end
        end
    end
    % Update labda_d with new estimate of D^2
    D20=Dnew(:,m).^2;
    Mpost=freqsmooth(postSNR,1);Isp=(Mpost>T);
    Pspii=alphap.*Pspii+(1-alphap).*Isp;
    alpha2ii=alphad+(1-alphad).*Pspii;
    labda_dii=alpha2ii.*labda_dii+(1-alpha2ii).*D20;
end

function Amplitude2=GainR2(Gmatrix,Iprior,Ipost,R,N)
L=size(Gmatrix);L=L(1);
index=1:N/2+1;index2=Iprior(index)+L*(Ipost(index)-1);
Amplitude2=Gmatrix(index2).*(R.^2);

function mx=freqsmooth(x,w)
L=length(x);mx=zeros(size(x));
for k=1:L
    i1=max(k-w,1);i2=min(k+w,L);
    mx(k)=sum(x(i1:i2))/(i2-i1+1);
end


function plot_average_frequency(w)
    figure
    sp=spectralobject(1024,924,100,[]);
    % Plot a spectrogram with superposed frequency metrics
    [Tcell,meanF,peakF]=spectrogram(w,'spectralobject',sp,'plot_metrics',1);
    % Plot the frequency metrics on their own
    for c=1:numel(w)
        subplot(numel(w),1,c)
        plot(Tcell{c},smooth(peakF{c}),'g')
        hold on
        plot(Tcell{c},meanF{c},'k')
        datetick('x')
        ylabel('Hz')
        sta= get(w(c),'station');
        chan= get(w(c),'channel');
        th=text(0.1,0.9, sprintf('%s %s.%s',datestr(Tcell{c}(1),30),sta,chan),'Units','normalized')
    end

end


function hmm_out = logisticMarginaliseHMM(hmm_in,iY)
% This function takes a multiple logistic strcuture HMM and returns the
% equivalent HMM with only the marginalised component iY of the
% logisticYdim components.

Xfulldim = hmm_in.train.ndim;
Xdim = Xfulldim - hmm_in.train.logisticYdim;
Xfulldim_new=Xdim+1;
select_vec = [1:Xdim,Xdim+iY];
hmm_out=hmm_in;
hmm_out.train.origlogisticYdim = hmm_in.train.logisticYdim;

%update S and options:
hmm_out.train.S=hmm_out.train.S(1:Xfulldim_new,1:Xfulldim_new);
hmm_out.train.Sind=hmm_out.train.Sind(1:Xfulldim_new,1:Xfulldim_new);
hmm_out.train.logisticYdim = 1;
hmm_out.train.ndim = Xfulldim_new;

%update W:
for st=1:hmm_in.train.K
    hmm_out.state(st).W.Mu_W = hmm_out.state(st).W.Mu_W(select_vec,select_vec);
    hmm_out.state(st).W.S_W = hmm_out.state(st).W.S_W(select_vec,select_vec,select_vec);
    hmm_out.state(st).W.iS_W = hmm_out.state(st).W.iS_W(select_vec,select_vec,select_vec);
end

%update alpha and sigma
for st=1:hmm_out.train.K
    %recall that for logistic setups, alpha has dimension Xdim x logisticYdim
    hmm_out.state(st).alpha.Gam_rate = hmm_out.state(st).alpha.Gam_rate(:,iY);
end

%update psi:
if isfield(hmm_in,'psi')
    if size(hmm_in.psi,2)==hmm_in.train.logisticYdim
        %has psi been calculated specific to this variable:
        hmm_out.psi = hmm_in.psi(:,iY);
    else
        hmm_out = rmfield(hmm_out,'psi');
    end
end

end


function y = HighPassModulationFilter(x, para)

[nFr, dim] = size(x);

switch para.filter_type
    case 'CMN'
        y = CMN(x);

    case 'CMN_online'
        y = CMN_online(x);

    case 'IIR'
        % Hd = FilterDesignChebchev();
        Hd = FilterDesignButterworth2_1();
        % Hd = FilterDesignCLS0_5();

        y = filter(Hd, [x;x;x;x]);
        y= y(end-nFr+1:end,:);

    otherwise
        fprintf('Unknown filter type\n');
end

end


function [oceanloadcorr] = ocean_loading_correction(time, XR, XS)

% SYNTAX:
%   [oceanloadcorr] = ocean_loading_correction(time, XR, XS);
%
% INPUT:
%   time = GPS time
%   XR   = receiver position  (X,Y,Z)
%   XS   = satellite position (X,Y,Z)
%
% OUTPUT:
%   oceanloadcorr = ocean loading correction terms (along the satellite-receiver line-of-sight)
%
% DESCRIPTION:
%   Computation of the ocean loading displacement terms.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

%ocean loading displacements matrix, station-dependent (see http://holt.oso.chalmers.se/loading/)
global ol_disp zero_time

oceanloadcorr = zeros(size(XS,1),1);
if (isempty(ol_disp))
    return
end

%terms depending on the longitude of the lunar node (see Kouba and Heroux, 2001)
fj = 1; %(at 1-3 mm precision)
uj = 0; %(at 1-3 mm precision)

%ref: http://202.127.29.4/cddisa/data_base/IERS/Convensions/Convension_2003/SUBROUTINES/ARG.f
tidal_waves = [1.40519E-4, 2.0,-2.0, 0.0, 0.00; ... % M2  - semidiurnal
               1.45444E-4, 0.0, 0.0, 0.0, 0.00; ... % S2  - semidiurnal
               1.37880E-4, 2.0,-3.0, 1.0, 0.00; ... % N2  - semidiurnal
               1.45842E-4, 2.0, 0.0, 0.0, 0.00; ... % K2  - semidiurnal
               0.72921E-4, 1.0, 0.0, 0.0, 0.25; ... % K1  - diurnal
               0.67598E-4, 1.0,-2.0, 0.0,-0.25; ... % O1  - diurnal
               0.72523E-4,-1.0, 0.0, 0.0,-0.25; ... % P1  - diurnal
               0.64959E-4, 1.0,-3.0, 1.0,-0.25; ... % Q1  - diurnal
               0.53234E-5, 0.0, 2.0, 0.0, 0.00; ... % Mf  - long-period
               0.26392E-5, 0.0, 1.0,-1.0, 0.00; ... % Mm  - long-period
               0.03982E-5, 2.0, 0.0, 0.0, 0.00];    % Ssa - long-period

refdate = datenum([1975 1 1 0 0 0]);

[week, sow] = time2weektow(zero_time + time);
dateUTC = datevec(gps2utc(datenum(gps2date(week, sow))));

%separate the fractional part of day in seconds
fday = dateUTC(4)*3600 + dateUTC(5)*60 + dateUTC(6);
dateUTC(4:end) = 0;

%number of days since reference date (1 Jan 1975)
days = (datenum(dateUTC) - refdate);

capt = (27392.500528 + 1.000000035*days)/36525;

%mean longitude of the Sun at the beginning of day
H0 = (279.69668 + (36000.768930485 + 3.03e-4*capt)*capt)*pi/180;

%mean longitude of the Moon at the beginning of day
S0 = (((1.9e-6*capt - 0.001133)*capt + 481267.88314137)*capt + 270.434358)*pi/180;

%mean longitude of the lunar perigee at the beginning of day
P0 = (((-1.2e-5*capt - 0.010325)*capt + 4069.0340329577)*capt + 334.329653)*pi/180;

corr = zeros(3,1);
for k = 1 : 11
    angle = tidal_waves(k,1)*fday + tidal_waves(k,2)*H0 + tidal_waves(k,3)*S0 + tidal_waves(k,4)*P0 + tidal_waves(k,5)*2*pi;
    corr  = corr + fj*ol_disp(1).matrix(1:3,k).*cos(angle + uj - ol_disp(1).matrix(4:6,k)*pi/180);
end
corrENU(1,1) = -corr(2,1); %east
corrENU(2,1) = -corr(3,1); %north
corrENU(3,1) =  corr(1,1); %up

%displacement along the receiver-satellite line-of-sight
XRcorr = local2globalPos(corrENU, XR);
corrXYZ = XRcorr - XR;
for s = 1 : size(XS,1)
    LOS  = XR - XS(s,:)';
    LOSu = LOS / norm(LOS);
    % oceanloadcorr(s,1) = dot(corrXYZ,LOSu);
    oceanloadcorr(s,1) = sum(conj(corrXYZ).*LOSu);
end


function rot = byEuler(varargin)
% define rotentations by Euler angles
%
% Syntax
%   rot = rotation.byEuler(phi1,Phi,phi2)          % Bunge convention
%   rot = rotation.byEuler(alpha,beta,gamma,'ZYZ') % Matthies convention
%   rot = rotation.byEuler(phi1,Phi,phi2,'Kocks')  % Kocks convention
%
% Input
%  phi1, Phi, phi2 - Euler angles in radiant
%
% Output
%  rot - @rotation
%
% Flags
%  Bunge, ZXZ -
%  ABG, Matthies, ZYZ -
%  Roe -
%  Kocks -
%  Canova -
%
% See also
% rotentation/rotentation rotentation/byMiller rotentation/byAxisAngle
% rotation/map

q = euler2quat(varargin{:});

rot = rotation(q);


function [res,exact] = AccDot(varargin)
%AccDot       Accurate dot product with faithful, to nearest or K-fold rounding
%
%   res = AccDot(A1,B1,...,An,Bn,K)
%
%res is sum(Ai*Bi)
%   K optional, default K=1 for faithfully rounded result
%   K   =0    rounded to nearest result
%       >1    result in cell array of length K of non-overlapping sequences,
%               sum(res{i})=sum(Ai,Bi) with K-fold accuracy
%       =inf  result in cell array of length K of non-overlapping sequences of
%               sufficient length s.t. sum(res{i})=sum(Ai,Bi) exactly
%       []    interval inclusion of the result; inclusion is best possible if no
%               underflow occurs.
%
%output exact (optional), array of 0/1 of size of result with entry equal to 1
%  iff corresponding dot product exact.
%
%All factors may be real non-interval.
%  The size of every product Ai*Bi must be the same.
%
%One of the factors Ai,Bi may be a cell array. If Ai is a cell array, say,
%  then Ai*Bi is interpreted as sum(Ai{k})*Bi. All entries Ai{k} must be of
%  the same size. This gives a convenient way to compute an approximate inverse
%  of an extremely ill-conditioned matrix only using double precision and our
%  K-fold accurate dot product AccDot. For details and examples with cond(A)~1e65
%  and more, see the INTLAB homepage.
%
%Interval input makes only sense, if all intervals are degenerated (point intervals);
%  therefore omitted.
%
%All factors are first stored into one array to be summed up; summation of
%  components by loops. For matrix input Ai,Bi, there may be interpretation overhead.
%For speed and to fight interpretation overhead, 2 extra matrices are necessary; for
%  the product of to nxn matrices this would be n^3 memory, therefore this is not allowed.
%
%   [res,exact] = AccDot(A1,B1,...,An,Bn,K)
%
%output exact (optional), array of 0/1 of size of result with entry equal to 1
%  iff corresponding dot product exact.
%
%Result is set to NaN if overflow occurs. For simplicity, factors Ai,Bi and
%  products Ai*Bi are limited in absolute value to less or equal 1e300. Small
%  numbers and underflow is treated correctly, rounding to nearest might be off
%  one bit if underflow occurs.
%
%A simple way to compute an inclusion of sum(Ai*Bi) of K-fold accuracy is
%
%   [res,exact] = AccDot(A1,B1,...,An,Bn,K);
%   ResK = intval(res{K});
%   ResK(~exact) = infsup(pred(ResK(~exact)),succ(ResK(~exact)));
%   res{K} = ResK;
%
%This uses faithfully rounded result. Note that the interval is represented by
%   sum(res(1:K-1)) + ResK;
%This interval is generally 2 bits wide unless res(mu,nu)=sum(Ai*Bi)(mu,nu).
%
%A best possible interval including sum(Ai*Bi) is computed by
%
%   res = AccDot(A1,B1,...,An,Bn,[]);
%
%The inclusion is best possible, however, more expensive than the previous approach.
%The gain is at most one bit.
%Maximum number of nonzero elements per sum is limited to 67108862, which
%seems sufficient for Matlab. More elements can be treated, see our paper (Huge length).
%
%Uses various algorithms in
%  S.M. Rump, T. Ogita, S. Oishi: Accurate Floating-point Summation I:
%    Faithful Rounding, SIAM J. Sci. Comput., 31(1):189-224, 2008.
%  S.M. Rump, T. Ogita, S. Oishi: Accurate Floating-point Summation II:
%    Sign, K-fold Faithful and Rounding to Nearest, Siam J. Sci. Comput.,
%    31(2):1269-1302, 2008.
%
%CAUTION: !!! THIS IMPLEMENTATION SUFFERS SEVERELY FROM INTERPRETATION OVERHEAD !!!
%!!! IT IS INCLUDED TO SHOW THE PRINCIPLES OF THE NEW METHODS !!!
%!!! DO NOT USE FOR LARGE DIMENSIONS !!!
%

% written  12/12/05     S.M. Rump
% modified 02/13/06     S.M. Rump  check for complex operands
% modified 08/26/12     S.M. Rump  rounding
% modified 12/23/12     S.M. Rump  typo
%

  e = 1e-30;
  if 1+e==1-e                       % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  % detect parameters
  len = length(varargin);
  if even(len)                          % default accuracy
    K = 1;
  else
    K = varargin{len};
  end

  % check K
  if ~(K>=0) & ~isempty(K)
    error('invalid value K for accuracy in AccDot')
  end

  % detect dimension of result
  Ai = varargin{1};
  if iscell(Ai)
    Ai = Ai{1};
  end
  Bi = varargin{2};
  if iscell(Bi)
    Bi = Bi{1};
  end
  if prod(size(Ai))==1
    [m n] = size(Bi);
  elseif prod(size(Bi))==1
    [m n] = size(Ai);
  else
    m = size(Ai,1);                     % m or n must be 1
    n = size(Bi,2);
  end
  sizeres = [m n];

  % Convert input into A*B+C, where A*B and C is m x n
  % C covers products Ai*Bi where one factor is +/-1, stored as index set indexC
  % index is negative if cofactor is cell array
  A = [];
  B = [];
  indexC = [];                          % summands, cofactor +/- 1
  indexD = [];                          % cell array summands, cofactor +/- 1
  isfull = 1;                           % all factors full

  for i=1:floor(nargin/2)
    % next product Ai*Bi
    Ai = varargin{2*i-1};
    Bi = varargin{2*i};
    if iscell(Ai) & iscell(Bi)
      error('at most factor can be cell array in AccDot')
    end
    if iscell(Ai)
      isfull = isfull & ( ~issparse(Ai{1}) & ~issparse(Bi) );
      sAi = size(Ai{1});
      for k=2:length(Ai)
        if ~isequal(sAi,size(Ai{k}))
          error('sizes in cell array factors in AccDot must coincide')
        end
      end
      sBi = size(Bi);
    elseif iscell(Bi)
      isfull = isfull & ( ~issparse(Ai) & ~issparse(Bi{1}) );
      sAi = size(Ai);
      sBi = size(Bi{1});
      for k=2:length(Bi)
        if ~isequal(sBi,size(Bi{k}))
          error('sizes in cell array factors in AccDot must coincide')
        end
      end
    else
      isfull = isfull & ( ~issparse(Ai) & ~issparse(Bi) );
      sAi = size(Ai);
      sBi = size(Bi);
    end
    % check dimensions
    if prod(sAi)==1
      if ~isequal(sBi,sizeres)
      error('dimensions in AccDot do not match')
      end
    elseif  prod(sBi)==1
      if ~isequal(sAi,sizeres)
        error('dimensions in AccDot do not match')
      end
    elseif sAi(2)~=sBi(1)
      error('dimensions in AccDot do not match')
    end

    % sAi size of factor Ai,  sBi size of factor Bi
    % determine factors
    if iscell(Ai)                         % first factor Ai cell array
      if prod(sBi)==1                     % second factor Bi scalar
        if abs(Bi)==1                     % Bi = +/- 1
          indexD = [ indexD Bi*(2*i-1) ];
        else                              % Bi scalar, but not +/- 1
          if isfull
            Bi_ = sparse(1:n,1:n,Bi);
          else
            Bi_ = Bi*eye(n);
          end
          for k=1:length(Ai)
            A = [ A Ai{k} ];
            B = [ B ; Bi_ ];
          end
        end
      else                                % second factor Bi non-scalar
        if ( prod(sAi)==1 ) & ( ~isequal(sizeres,[1 1]) )
          error('cell array factor cannot be sequence of scalars')
        end
        for k=1:length(Ai)
          A = [ A Ai{k} ];
          B = [ B ; Bi ];
        end
      end
    elseif iscell(Bi)                     % second factor Bi cell array
      if prod(sAi)==1                     % first factor Ai scalar
        if abs(Ai)==1                     % Ai = +/- 1
          indexD = [ indexD Ai*(2*i) ];
        else                              % Ai scalar, but not +/- 1
          if isfull
            Ai_ = sparse(1:m,1:m,Ai);
          else
            Ai_ = Ai*eye(m);
          end
          for k=1:length(Bi)
            A = [ A Ai_ ];
            B = [ B ; Bi{k} ];
          end
        end
      else                                % first factor Ai non-scalar
        if ( prod(sBi)==1 ) & ( ~isequal(sizeres,[1 1]) )
          error('cell array factor cannot be sequence of scalars')
        end
        for k=1:length(Bi)
          A = [ A Ai ];
          B = [ B ; Bi{k} ];
        end
      end
    else                                  % none of Ai,Bi cell array
      if prod(sAi)==1                     % first factor Ai scalar
        if abs(Ai)==1                     % Ai = +/- 1
          indexC = [ indexC Ai*(2*i) ];
        else
          if isfull
            A = [ A Ai*eye(m) ];
          else
            A = [ A sparse(1:m,1:m,Ai) ]; % Ai scalar, but not +/- 1
          end
          B = [ B ; Bi ];
        end
      elseif prod(sBi)==1                 % second factor Bi scalar
        if abs(Bi)==1                     % Bi = +/- 1
          indexC = [ indexC Bi*(2*i-1) ];
        else
          A = [ A Ai ];                   % Bi scalar, but not +/- 1
          if isfull
            B = [ B ; sparse(1:n,1:n,Bi) ];
          else
            B = [ B ; Bi*eye(n) ];
          end
        end
      else                                % both factors Ai,Bi non-scalar
        A = [ A Ai ];
        B = [ B ; Bi ];
      end
    end
  end

  % check interval input
  if isa(A,'intval') | isa(B,'intval')
    error('input of AccDot must not be of type intval')
  end

  % check for complex input
  if ( ~isreal(A) ) | ( ~isreal(B) )
    error('input of AccDot must be real')
  end

  % check huge input
  if any(any(abs(A)>1e300)) | any(any(abs(B)>1e300))
    error('for simplicity all entries and intermediate products limited to 1e300')
  end
  if ~isempty(indexC)
    for k=1:length(indexC)
      if any(any(varargin{abs(indexC(k))}>1e300))
        error('for simplicity all entries and intermediate products limited to 1e300')
      end
    end
  end
  kD = 0;
  if ~isempty(indexD)
    for k=1:length(indexD)
      DD = varargin{abs(indexD(k))};
      kD = kD + length(DD);
      for kk=1:length(DD)
        if any(any(DD{kk}>1e300))
          error('for simplicity all entries and intermediate products limited to 1e300')
        end
      end
    end
  end

  % transpose A:  result A.'*B
  A = A.';

  factor = 134217729;               % splitting factor 2^27+1
  % [A1,A2] = Split(A)
  C = factor*A;
  A1 = C - ( C - A );               % upper part of A
  A2 = A - A1;                      % A = A1+A2 exact splitting
  % [B1,B2] = Split(B)
  C = factor*B;
  B1 = C - ( C - B );               % upper part of B
  B2 = B - B1;                      % B = B1+B2 exact splitting

  % initialize result
  if isempty(K)
    rescell = 0;
  else
    rescell = ( K>1 );
  end
  if isfull
    if rescell
      res{1} = zeros(m,n);
    else
      res = zeros(m,n);
    end
  else
    if rescell
      res{1} = sparse([],[],[],m,n);
    else
      res = sparse([],[],[],m,n);
    end
  end
  if ( K>1 ) & ~isinf(K)
    for k=2:K
      res{k} = res{1};
    end
  end
  if isempty(K)
    res = intval(res);
  end
  if nargout==2
    if isfull
      exact = zeros(m,n);
    else
      exact = sparse([],[],[],m,n);
    end
  end

  % compute result
  sA = size(A,1);
  sC = length(indexC);
  sD = length(indexD);
  summands = ( sC+sD~=0 );
  % underflow constants
  phi = 2^643;                                % factor sqrt(eps^-4 eta^-1) for Ai,Bi
  Fbound = 2^(-968);                          % lower bound eps^-2 eta for factors
  for i=1:m
    for j=1:n
      if sA~=0
        x1 = A1(:,i);
        x2 = A2(:,i);
        y1 = B1(:,j);
        y2 = B2(:,j);
        AiBip = A(:,i).*B(:,j);
        r = x2.*y2 - ((( AiBip - x1.*y1 ) - x2.*y1 ) - x1.*y2 );
        if any(abs(AiBip)>1e300)              % AiBip is vector
          error('for simplicity all entries and intermediate products limited to 1e300')
        end
      else
        AiBip = [];
        r = [];
      end
      if summands
        Ci = zeros(sC+kD,1);
        for k=1:sC
          if indexC(k)>0
            Ci(k) = varargin{indexC(k)}(i,j);
          else
            Ci(k) = -varargin{-indexC(k)}(i,j);
          end
        end
        kkk = 0;
        for k=1:sD
          DD = varargin{abs(indexD(k))};
          if indexD(k)>0
            for kk=1:length(DD)
              kkk = kkk+1;
              Ci(sC+kkk) = DD{kk}(i,j);
            end
          else
            for kk=1:length(DD)
              kkk = kkk+1;
              Ci(sC+kkk) = -DD{kk}(i,j);
            end
          end
        end
      else
        Ci = [];
      end
      p = [ AiBip ; Ci ];
      % execute one ExtractVector
      np = nnz(p);                          % initialization
      mu = full(max(abs(p)));               % abs(p_i) <= mu; full: avoid matlab bug
      if ( np==0 ) | ( mu==0 )              % no or only zero summands
        R = 0;                              % result exactly zero in any rounding
        p = [];
      else
        Ms = 2^nextpow2(np+2);              % np+2 <= 2^M
        if Ms^2*eps>1
          error('vector length n too large for AccDot; Huge n not implemented')
        end
        sigma = Ms*2^nextpow2(mu);          % first extraction unit
        if isinf(sigma) | isnan(sigma)      % overflow, could be avoided with scaling
          if iscell(res)
            res{1}(i,j) = NaN;
          else
            res(i,j) = NaN;
          end
          exact(i,j) = 0;
          continue
        else
          q = ( sigma + p ) - sigma;        % [R,p] = ExtractVector(sigma,p);
          R = sum(q);                       % sum of leading terms
          p = p - q;                        % remaining terms
        end
      end
      if K<=1                               % nearest of faithful rounding
        [res(i,j),ext,R,p] = AccSum( [ p ; r ] , K , 0 , R );
        if res(i,j)==realmin                % take care of underflow, original sum = R + sum(p)
          index = find( abs(AiBip)<Fbound );      % small parts of AiBip
          p = [ p ; -AiBip(index) ; -r(index) ];  % subtract small parts
          [res(i,j),ext] = ...
            AccSum( scaleinput(A(index,i),B(index,j),p,phi) , K , 0 , phi*(phi*R) );
          if ( nargout==2 ) & ext
            exact(i,j) = diam( (intval(res(i,j))/phi) / phi == 0 );
          end
          res(i,j) = (res(i,j)/phi) / phi;  % avoid overflow
        end
      elseif isempty(K)                       % interval result
        [rr,ext,R,p] = AccSum( [ p ; r ] , 0 , -1 , R );
        if rr==realmin                        % take care of underflow, original sum = R + sum(p)
          index = find( abs(AiBip)<Fbound );  % small parts of AiBip
          p = [ p ; -AiBip(index) ; -r(index) ];  % subtract small parts
          [rr,ext] = AccSum( scaleinput(A(index,i),B(index,j),p,phi) , 0 , -1 , phi*(phi*R) );
          if ext
            res(i,j) = intval(rr);
          else
            res(i,j) = infsup(rr,succ(rr));
          end
          res(i,j) = ( res(i,j)/phi ) / phi;
        else
          res(i,j) = infsup(rr,succ(rr));
        end
      else                                          % K-fold accuracy
        p = [ p ; r ];
        scale = 0;
        cont = 1;
        k = 0;
        ext(i,j) = 0;
        while ( k<K ) & cont
          k = k+1;
          [rr,ext,R,p] = AccSum( p , 1 , 0 , R );
          if ~scale & ( rr==realmin )               % take care of underflow, original sum = R + sum(p)
            scale = 1;                              % only one scaling
            index = find( abs(AiBip)<Fbound );      % small parts of AiBip
            p = [ p ; -AiBip(index) ; -r(index) ];  % subtract small parts
            [rr,ext,R,p] = ...                      % scaled result
              AccSum( scaleinput(A(index,i),B(index,j),p,phi) , 1 , 0 , phi*(phi*R) );
          end
          rrr = rr;
          if scale
            rr = ( rr/phi )/phi;
          end
          if isinf(K) & ( k>length(res) ) & ( rr~=0 )
            if isfull
              res{k} = zeros(m,n);
            else
              res{k} = sparse([],[],[],m,n);
            end
          end
          if rr~=0
            res{k}(i,j) = rr;
          end
          if abs(rr)<=realmin                       % result exact
            cont = 0;
          end
          if isnan(rr)
            res{k}(i,j) = NaN;
            cont = 0;
          end
          if ( nargout==2 ) & ext
            exact(i,j) = ( diam( (intval(rrr)/phi) / phi )==0 );
          end
        end
      end
    end
  end

  if rndold
    setround(rndold)
  end

function [res,exact,R,p] = AccSum(p,K,rnd,rho)
%Simplified version of AccSum :
%  rnd only valid for K==0
%  no overflow can occur
%  indices for possible underflow stored
%  vector length not too large
%  stops if sigma too small
%  ~(K>1)
%

  res = 0;
  exact = 1;
  R = 0;
  if isempty(p)
    return
  end

  % input real, compute sum(p)
  % rounding to nearest
  if K==0                             % rnd maybe -1 or 0 or +1
    % let S:=sum(p)
    [res,dummy,R,p] = AccSum(p,1,0,rho);   % S = res + R + sum(p)  for new p
    if res==realmin                   % take care of underflow
      return
    end
    [delta,dummy,R,p] = AccSum(p,1,0,R);   % delta + R + sum(p) = S - res  for new R,p
    if delta==realmin                 % scale; otherwise sign(delta) = sign(S-res)
      return
    end
    if delta==0                       % result exact: res=S in any rounding
      return
    end
    exact = 0;                        % result not exact

    if rnd~=0                         % rounding downwards or upwards
      if sign(delta)==rnd             % res on wrong side
        if rnd==1
          res = succ(res);
        else
          res = pred(res);
        end
      end
      return
    end

    % sign(delta) = sign(S-res)
    % compute nearest neighbor resp in direction sign(delta), rnd=0
    if delta>0
      resp = succ(res);
    else
      resp = pred(res);
    end
    mu = (resp-res)/2;
    if abs(delta)>abs(mu)
      res = resp;
    elseif abs(delta)==abs(mu)
      delta = AccSum(p,1,0,R);
      if delta==0
        res = res + mu;
      elseif sign(delta)==sign(mu)
        res = resp;
      end
    end
    return
  end

  % the standard case: real vector input, K=1
  if issparse(p)
    n = nnz(p);                         % initialization
  else
    n = length(p);
  end
  tau1 = 0;
  tau2 = 0;
  mu = full(max(abs(p)));               % abs(p_i) <= mu
  if ( n==0 ) | ( mu==0 )               % no or only zero summands
    res = rho;                          % result exactly zero in any rounding
    return
  end
  Ms = 2^nextpow2(n+2);                 % n+2 <= 2^M
  sigma = Ms*2^nextpow2(mu);            % first extraction unit
  phi = 2^(-53)*Ms;                     % factor to decrease sigma
  factor = 2*phi*Ms;                    % factor for sigma check

  % underflow constant
  sigmamin = 2^(-968);                  % lower bound eps^-2 eta for sigma
  t = rho;
  while 1
    if sigma<=sigmamin                  % sigma too small, but t+tau exact
      res = realmin;                    % indicates underflow
      R = t;                            % original sum = R + sum(p)
      return
    end
    q = ( sigma + p ) - sigma;          % [tau,p] = ExtractVector(sigma,p);
    tau = sum(q);                       % sum of leading terms
    p = p - q;                          % remaining terms
    tau1 = t + tau;                     % new approximation
    if abs(tau1)>=factor*sigma
      tau2 = tau - ( tau1 - t );        % [tau1,tau2] = FastTwoSum(t,tau)
      res = tau1 + ( tau2 + sum(p) );   % faithfully rounded final result
      R = tau2 - ( res - tau1 );        % only for K-fold result
      if nargout>=2
        exact = ( R==0 ) & ~any(p);
      end
      return
    end
    t = tau1;                           % sum t+tau exact
    if t==0                             % accelerate case sum(p)=0
      [res,exact,R,p] = AccSum(p(p~=0),K,0,0);  % recursive call, zeros eliminated
      return
    end
    sigma = phi*sigma;                  % new extraction unit
  end

function p = scaleinput(Ai,Bi,Ci,phi)
% scale  p = phi*Ai * phi*Bi  for column vector input

  factor = 134217729;               % splitting factor 2^27+1
  % [A1,A2] = Split(phi*Ai)
  Ai = phi*Ai;
  C = factor*Ai;
  A1 = C - ( C - Ai );            % upper part of Ai
  A2 = Ai - A1;                   % Ai = A1+A2 exact splitting
  % [B1,B2] = Split(phi*Bi)
  Bi = phi*Bi;
  C = factor*Bi;
  B1 = C - ( C - Bi );            % upper part of Bi
  B2 = Bi - B1;                   % Bi = B1+B2 exact splitting
  % phi^2*A(:,i)*B(:,j) = p+r, no underflow possible
  p = Ai.*Bi;
  p = [ p ; A2.*B2 - ((( p - A1.*B1 ) - A2.*B1 ) - A1.*B2 ) ; phi*(phi*Ci) ];


function output = ConvertPowerToDecibel(x)
output = 10 * log10(x);
end


function out = isSubset(A, B, tol)
%ISSUBSET   Test if A is a subset of B.
%   OUT = ISSUBSET(A, B, TOL) returns logical true if A is a subset of B up
%   to the tolerance TOL, where A and B are domains of a chebfun, chebfun2
%   or chebfun3 (i.e., vectors of size 1x2, 1x4 or 1x6 with A(1) <= A(2),
%   A(3) <= A(4), A(5) <= A(6)).

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

if ( isempty(A) )
    out = 1;
    return
end

if ( length(A) ~= length(B) )
    error('CHEBFUN:isSubset:size', ...
        'Domains must have the same number of entries.')
end

if ( ( A(1) < B(1) - tol ) || ( B(2) + tol < A(2) ) )
    out = 0;
    return
else
    % Recurse:
    out = isSubset(A(3:end), B(3:end), tol);
end

end


function [ y, m, d, f, ierror ] = ymdf_check_republican ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_CHECK_REPUBLICAN checks a Republican YMDF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, D, real F, the
%    YMDF date, which may be corrected if necessary and possible.
%
%    Output, integer IERROR, is 0 if the date is legal.
%
  ierror = 0;

  [ y, m, d, ierror ] = ymd_check_republican ( y, m, d );

  if ( ierror ~= 0 )
    return
  end

  [ y, m, d, f ] = frac_borrow_republican ( y, m, d, f );

  [ y, m, d, f ] = frac_carry_republican ( y, m, d, f );

  return
end


function [ x, know ] = p03_sol ( m, know )

%*****************************************************************************80
%
%% P03_SOL returns known solutions for problem 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Harald Niederreiter, Kevin McCurley,
%    Optimization of functions by quasi-random search methods,
%    Computing,
%    Volume 22, Number 2, 1979, pages 119-123.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input/output, integer KNOW.
%    On input, KNOW is 0, or the index of the previously returned solution.
%    On output, KNOW is 0 if there are no more solutions, or it is the
%    index of the next solution.
%
%    Output, real X(M), the solution.
%
  if ( know == 0 )
    know = 1;
    x(1:m,1) = [ ...
      0.999980569087140, ...
      0.500000721280566, ...
      0.333341891834645, ...
      0.249997266604697 ]';
  else
    know = 0;
    x = zeros ( m, 1 );
  end

  return
end


function [points,weights] = get_spherical_grid(number,conf)
%GET_SPHERICAL_GRID spherical grid points and weights
%
%   Usage: [points,weights] = get_spherical_grid(number,conf)
%
%   Input parameters:
%       number  - number of grid points
%       conf    - configuration struct (see SFS_config)
%
%   Output parameters:
%       points  - grid points
%       weights - integration weights for the grid points
%
%   GET_SPHERICAL_GRID(number,conf) returns the points and weights for a grid on
%   a sphere. The type of grid is specified by conf.secondary_sources.grid.
%   For available grids, have a look at https://github.com/sfstoolbox/data.
%   It expects the grid files at SFS_basepath/data/spherical_grids. If the
%   desired file is not available on the hard disk, the function tries to
%   download it directly from github.
%   For conf.secondary_sources.grid='gauss' the grid positions are calculated
%   after Ahrens (2012), p. 121 (see also: Rafaely (2015), p. 64)
%
%   See also: secondary_source_positions,
%       weights_for_points_on_a_sphere_rectangle
%
%   References:
%       Ahrens (2012) - "Analytic Methods of Sound Field Synthesis", Springer,
%       ISBN 978-3-642-25743-8
%
%       Rafaely (2015) - "Fundamentals of Spherical Array Processing", Springer,
%       ISBN 978-3-662-45664-4

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

%% ===== Checking input parameters =======================================
nargmin = 2;
nargmax = 2;
narginchk(nargmin,nargmax);
isargpositivescalar(number);
isargstruct(conf);

%% ===== Configuration ===================================================
spherical_grid = conf.secondary_sources.grid;

%% ===== Main ============================================================
filename = sprintf('%06.0fpoints.mat',number);
basepath = get_sfs_path();

if strcmp('equally_spaced_points',spherical_grid)
    % Check if we have a squared number of points, because only for those values
    % are equally spaced points grids available.
    if mod(number,sqrt(number))~=0
        error('%s: number has to be a squared number.',upper(mfilename));
    end
    file = [basepath '/data/spherical_grids/equally_spaced_points/' filename];
    url = ['https://raw.githubusercontent.com/sfstoolbox/data/master/' ...
           'spherical_grids/equally_spaced_points/' filename];
    % Download file if not present
    if ~exist(file,'file')
        download_file(url,file);
    end
    tmp = load(file,'-ascii');
    points = tmp(:,1:3);
    weights = tmp(:,4);
elseif strcmp('fabian',spherical_grid)
    % Here we have only one number of secondary sources available
    if number~=11345
        error('%s: this grid is only available for 11345 sources.', ...
            upper(mfilename));
    end
    file = [basepath '/data/spherical_grids/fabian/' filename];
    url = ['https://raw.githubusercontent.com/sfstoolbox/data/master/' ...
           'spherical_grids/fabian/' filename];
    % Download file if not present
    if ~exist(file,'file')
        download_file(url,file);
    end
    tmp = load(file,'-ascii');
    points = tmp(:,1:3);
    weights = tmp(:,4);
elseif strcmp('gauss',spherical_grid)
    % The number of secondary sources needs to be 2,8,18,32, ... ,
    % see Ahrens (2012)
    if mod(number,sqrt(number/2))~=0
        error(['%s: the number of secondary sources needs to be ', ...
            '2*n^2 for a gauss grid.'],upper(mfilename));
    end
    number = sqrt(number/2);
    % Get gauss points and weights
    [p,w] = legpts(number);
    % Sampling points along azimuth
    PHI = linspace(0,2*pi,2*number+1);
    % Remove the last one, because phi=0 and phi=2pi are the same
    PHI = PHI(1:end-1);
    % Sampling points along elevation
    THETA = acos(p)-pi/2;
    % Get grid points
    [phi,theta] = meshgrid(PHI,THETA);
    [~,weights] = meshgrid(PHI,w);
    r = ones(size(phi));
    % Convert to cartesian
    [points(:,1) points(:,2) points(:,3)] = sph2cart(phi(:),theta(:),r(:));
    % Normalize integration weights
    weights = weights(:)*pi/number;
else
    error(['%s: the given spherical grid is not available, have a look at ' ...
        'https://github.com/sfstoolbox/data for avialable grids.'], ...
        upper(mfilename));
end


classdef InertialAeroAnglesPolySteeringModel < AbstractAnglePolySteeringModel
    %InertialAeroAnglesPolySteeringModel Summary of this class goes here
    %   Detailed explanation goes here

    properties
        bankModel(1,1) PolynominalModel = PolynominalModel(0,0,0,0);
        aoAModel(1,1) PolynominalModel = PolynominalModel(0,0,0,0);
        slipModel(1,1) PolynominalModel = PolynominalModel(0,0,0,0);

        bankContinuity(1,1) logical = true;
        aoAContinuity(1,1) logical = true;
        slipContinuity(1,1) logical = true;
    end

    methods
        function R_body_2_inertial = getBody2InertialDcmAtTime(obj, ut, rVect, vVect, bodyInfo)
            bankAng = obj.bankModel.getValueAtTime(ut);
            angOfAttack = obj.aoAModel.getValueAtTime(ut);
            angOfSideslip = obj.slipModel.getValueAtTime(ut);

            baseFrame = bodyInfo.getBodyCenteredInertialFrame();
            [~, ~, ~, R_body_2_inertial] = computeInertialBodyAxesFromFrameAeroAngles(ut, rVect, vVect, bodyInfo, bankAng, angOfAttack, angOfSideslip, baseFrame);
        end

        function [angleModel, continuity] = getAngleNModel(obj, n)
            angleModel = PolynominalModel.empty(1,0);

            switch n
                case 1
                    angleModel = obj.bankModel;
                    continuity = obj.bankContinuity;
                case 2
                    angleModel = obj.aoAModel;
                    continuity = obj.aoAContinuity;
                case 3
                    angleModel = obj.slipModel;
                    continuity = obj.slipContinuity;
            end
        end

        function t0 = getT0(obj)
            t0 = obj.slipModel.t0;
        end

        function setT0(obj, newT0)
            obj.bankModel.t0 = newT0;
            obj.aoAModel.t0 = newT0;
            obj.slipModel.t0 = newT0;
        end

        function setConstTerms(obj, bankConst, aoaConst, slipConst)
            obj.bankModel.constTerm = bankConst;
            obj.aoAModel.constTerm = aoaConst;
            obj.slipModel.constTerm = slipConst;
        end

        function setLinearTerms(obj, bank, aoa, slip)
            obj.bankModel.linearTerm = bank;
            obj.aoAModel.linearTerm = aoa;
            obj.slipModel.linearTerm = slip;
        end

        function setAccelTerms(obj, bank, aoa, slip)
            obj.bankModel.accelTerm = bank;
            obj.aoAModel.accelTerm = aoa;
            obj.slipModel.accelTerm = slip;
        end

        function setTimeOffsets(obj, timeOffset)
            obj.bankModel.tOffset = timeOffset;
            obj.aoAModel.tOffset = timeOffset;
            obj.slipModel.tOffset = timeOffset;
        end

        function [angle1Cont, angle2Cont, angle3Cont] = getContinuityTerms(obj)
            angle1Cont = obj.bankContinuity;
            angle2Cont = obj.aoAContinuity;
            angle3Cont = obj.slipContinuity;
        end

        function setContinuityTerms(obj, angle1Cont, angle2Cont, angle3Cont)
            obj.bankContinuity = angle1Cont;
            obj.aoAContinuity = angle2Cont;
            obj.slipContinuity = angle3Cont;
        end

        function setConstsFromDcmAndContinuitySettings(obj, dcm, ut, rVect, vVect, bodyInfo)
            if(obj.bankContinuity || obj.aoAContinuity || obj.slipContinuity)
                [bankAng,angOfAttack,angOfSideslip] = computeInertialAeroAnglesFromBodyAxes(ut, rVect, vVect, bodyInfo, dcm(:,1), dcm(:,2), dcm(:,3));

                if(obj.bankContinuity)
                    obj.bankModel.constTerm = bankAng;
                end

                if(obj.aoAContinuity)
                    obj.aoAModel.constTerm = angOfAttack;
                end

                if(obj.slipContinuity)
                    obj.slipModel.constTerm = angOfSideslip;
                end
            end
        end

        function setInitialAttitudeFromState(obj, stateLogEntry, tOffsetDelta)
            t0 = stateLogEntry.time;
            obj.setT0(t0);

            obj.bankModel.tOffset = obj.bankModel.tOffset + tOffsetDelta;
            obj.aoAModel.tOffset = obj.aoAModel.tOffset + tOffsetDelta;
            obj.slipModel.tOffset = obj.slipModel.tOffset + tOffsetDelta;
        end

        function [angle1Name, angle2Name, angle3Name] = getAngleNames(~)
            angle1Name = 'Bank Angle';
            angle2Name = 'Angle of Attack';
            angle3Name = 'Side Slip Angle';
        end

        function newSteeringModel = deepCopy(obj)
            newSteeringModel = InertialAeroAnglesPolySteeringModel(obj.bankModel.deepCopy(), obj.aoAModel.deepCopy(), obj.slipModel.deepCopy());
            newSteeringModel.bankContinuity = obj.bankContinuity;
            newSteeringModel.aoAContinuity = obj.aoAContinuity;
            newSteeringModel.slipContinuity = obj.slipContinuity;
        end

        function optVar = getNewOptVar(obj)
            optVar = SetAeroSteeringModelActionOptimVar(obj);
        end

        function optVar = getExistingOptVar(obj)
            optVar = obj.optVar;
        end

        function [addActionTf, steeringModel] = openEditSteeringModelUI(obj, lv, useContinuity)
            output = AppDesignerGUIOutput({false, obj});
            lvd_EditActionSetSteeringModelGUI_App(obj, lv, useContinuity, output);
            addActionTf = output.output{1};
            steeringModel = output.output{2};
        end
    end

    methods(Access=private)
        function obj = InertialAeroAnglesPolySteeringModel(bankModel, aoAModel, slipModel)
            obj.bankModel = bankModel;
            obj.aoAModel = aoAModel;
            obj.slipModel = slipModel;
        end
    end

    methods(Static)
        function model = getDefaultSteeringModel()
            bankModel = PolynominalModel(0,0,0,0);
            aoAModel = PolynominalModel(0,0,0,0);
            slipModel = PolynominalModel(0,0,0,0);

            model = InertialAeroAnglesPolySteeringModel(bankModel, aoAModel, slipModel);
        end

        function typeStr = getTypeNameStr()
            typeStr = SteeringModelEnum.InertialAeroAnglesPoly.nameStr;
        end
    end
end


% reader struct
%    array: specify whether input is array or single channel
%    multiArrayFiles: if input is array, specify whether each channel is
%    one file.
%    useChannel: if input is array, specify which channel(s) to use.
%    if not defined, use all channels.
%
function feat = Reader_wavfile2spectrum(files, reader, useGPU, precision)
if nargin<4
    precision = 'single';
end
for i=1:length(files)
    [wav, fs] = Reader_waveform(files(i), reader);
    if strcmpi(precision, 'single')
        wav = single(wav{1});
    else
        wav = double(wav{1});
    end

    reader = SetDefaultValue(reader, 'frame_len', fs*0.025);
    reader = SetDefaultValue(reader, 'frame_shift', fs*0.01);
    reader = SetDefaultValue(reader, 'window_type', 'hamming');
    reader = SetDefaultValue(reader, 'removeDC', 0);
    reader = SetDefaultValue(reader, 'useGPU', 0);
    reader = SetDefaultValue(reader, 'doDithering', 0);

    FFT_length = 2^nextpow2(reader.frame_len);
    nCh = size(wav',2);
    tmp = sfft_multi(wav', reader.frame_len, reader.frame_shift, FFT_length, reader.window_type, reader.removeDC, reader.useGPU);
    % take the first half of the Fourier coefficients
    tmp = tmp(1:FFT_length/2+1,:,:);
    % reshape multi-channel Fourier coefficients into a vector
    tmp_feat = reshape(tmp, nCh*(FFT_length/2+1), size(tmp,3));

    if strcmpi(precision, 'single')
        feat{i} = single(gather(tmp_feat));
    else
        feat{i} = double(gather(tmp_feat));
    end
end
end


clc
close all
clear
% This file tests feasibility between MPC-DCBF and NMPC-DCBF.

%% System setup

timestep = 0.1;
system_param.A = [[1 ,timestep, 0]; [0, 1, timestep]; [0, 0, 1]];
system_param.B = [0; 0; timestep];
system_param.ul = -1;
system_param.uu = 1;
system_param.timestep = timestep;
x0 = [0;0;0]; % this value will be overrided
t0 = 0.0;

gammalist = [0.05, 0.1, 0.15, 0.2];
for gammaindex = 1:length(gammalist)
    %% Problem setup with different hyperparameters

    % MPC-DCBF simulator
    simulator_mpcdcbf = CBFDT(system_param, x0, t0);
    param_mpcdcbf = ParamMPCDCBF(8, gammalist(gammaindex), 10.0*eye(3), 10.0*eye(3), 1.0);
    simulator_mpcdcbf.setOpt('mpcdcbf', param_mpcdcbf);
    % NMPC-DCBF simulator
    simulator_nmpcdcbf = CBFDT(system_param, x0, t0);
    param_nmpcdcbf = ParamNMPCDCBF(8, 8, gammalist(gammaindex), 10.0*eye(3), 10.0*eye(3), 1.0, 10.0);
    simulator_nmpcdcbf.setOpt('nmpcdcbf', param_nmpcdcbf);
    % iterate over states
    x1list = linspace(-2,0,11);
    x2list = linspace(0,2,11);
    x3list = linspace(0,2,11);
    % data collection
    feas_points_mpcdcbf = [];
    feas_points_nmpcdcbf = [];
    infeas_points_all = [];

    %% Test feasibility with sampling states among controllers

    for k1 = 1:length(x1list)
        for k2 = 1:length(x2list)
            for k3 = 1:length(x3list)
                xfeas = [x1list(k1); x2list(k2); x3list(k3)];
                % solve the problem with MPC-DCBF
                simulator_mpcdcbf.xcurr = xfeas;
                [feas_mpcdcbf, ~, ~, ~] = simulator_mpcdcbf.solve;
                % solve the problem with NMPC-DCBF
                simulator_nmpcdcbf.xcurr = xfeas;
                [feas_nmpcdcbf, ~, ~, ~] = simulator_nmpcdcbf.solve;
                if feas_mpcdcbf == 1
                    feas_points_mpcdcbf = [feas_points_mpcdcbf, xfeas];
                end
                if feas_nmpcdcbf == 1
                    feas_points_nmpcdcbf = [feas_points_nmpcdcbf, xfeas];
                end
                if feas_mpcdcbf == 0 && feas_nmpcdcbf == 0
                    infeas_points_all = [infeas_points_all, xfeas];
                end
            end
        end
    end

    %% Plotting
    close all
    outer_color = [0.3010, 0.7450, 0.9330];
    inner_color = [1, 0, 0];
    figure('Renderer', 'painters', 'Position', [0 0 400 400]);
    set(gca,'LooseInset',get(gca,'TightInset'));
    hold on;
    scatter3(feas_points_mpcdcbf(1,:),feas_points_mpcdcbf(2,:),feas_points_mpcdcbf(3,:), 5, 'o','LineWidth',1.0, 'MarkerEdgeColor',inner_color,'MarkerFaceColor',inner_color);
    scatter3(feas_points_nmpcdcbf(1,:),feas_points_nmpcdcbf(2,:),feas_points_nmpcdcbf(3,:), 20, 'o','LineWidth',1.2, 'MarkerEdgeColor',outer_color);
    axis equal
    if gammaindex == 1
        h=get(gca,'Children');
        h_legend = legend(h([end, end-1]),...
            {'MPC-DCBF', 'NMPC-DCBF'}, 'Location', 'NorthEast');
        set(h_legend, 'Interpreter','latex');
    end
    set(gca,'LineWidth', 1.0, 'FontSize', 15);
    xlabel('$x (m)$','interpreter','latex','FontSize',20);
    ylabel('$v (m/s)$','interpreter','latex','FontSize',20);
    zlabel('$a (m/s^2)$','interpreter','latex','FontSize',20);
    xlim([-2, 0]);
    ylim([0, 2]);
    zlim([0, 2]);
    view(70, 4);
    grid on;
    figurename_eps = "feasibility-mpcdcbf" + gammaindex + ".eps";
    figurename_png = "feasibility-mpcdcbf" + gammaindex + ".png";
    dataname = "feasibility-mpcdcbf" + gammaindex + ".mat";
    % save data and generate figures
    print(gcf,strcat('figures/',figurename_eps), '-depsc');
    print(gcf,strcat('figures/',figurename_png), '-dpng', '-r800');
    save(strcat('data/',dataname));
end


function [kPa] = hPa2kPa(hPa)
% Convert pressure from hectopascals to kilopascals.
% Chad Greene 2012
kPa = hPa/10;


function Rlabel=RegionMerging(varargin)
% post-processing over segment Image based RAG region merging
% input:
% I by M x N gray Image
% label: initial segment result
% Treshold: interation temination condion default is 1000
% RegionNum: the Region Number after Mereging defalut is 2(forground and
% background
% output:
% Rlable: merging result
% reference:
% writed by radishgiant

[I,Rlabel,Treshold,RegionNumber] = parse_inputs(varargin{:});
minsim=Treshold-1;
K=length(unique(Rlabel));
[matrixSize(1),matrixSize(2),channel]=size(I);
X=reshape(I,matrixSize(1)*matrixSize(2),channel);
Vset=cell(K,1);
Pset=cell(K,1);
delnode=[];
labelstart=min(Rlabel(:));
if labelstart==0
    Rlabel=Rlabel+1;
elseif labelstart>1
    Rlabel=Rlabel-labelstart+1;
end
for i=1:K
    [r,c]=find(Rlabel==i);
    Vset{i}=[r,c];
    Pset{i}=X(sub2ind(matrixSize,r,c),:);
end

while length(unique(Rlabel))>RegionNumber || minsim(1)<=Treshold
Adj=imRAG(Rlabel);
sim=partionsimliar(Pset,Adj,delnode);
[minsim,simInd]=sort(sim,1,'ascend');
node1=Adj(simInd(1),1);
node2=Adj(simInd(1),2);
node=min(node1,node2);
nodem=max(node1,node2);
Rlabel(Rlabel==node1|Rlabel==node2)=node;
Pset{node}=[Pset{node1};Pset{node2}];
Pset{nodem}=[];
Vset{node}=[Vset{node1};Vset{node2}];
Vset{nodem}=[];
delnode=[delnode;nodem];
end

end

function [I,label,Treshold,RegionNumber] = parse_inputs(varargin)

narginchk(2,4);
I = varargin{1};
label=varargin{2};
validateattributes(I,{'numeric'}, {'2d' '3d'}, ...
    mfilename, 'I', 1);
validateattributes(label,{'numeric'}, {'2d'}, ...
    mfilename, 'label', 2);
if nargin<3
    Treshold=1000;
    RegionNumber=2;
elseif nargin<4
    Treshold=varargin{3};
    RegionNumber=2;
else
    Treshold=varargin{3};
    RegionNumber=varargin{4};
validateattributes(Treshold,{'numeric'}, {'scalar','real'}, ...
    mfilename, 'Treshold', 3);
validateattributes(RegionNumber,{'numeric'}, {'scalar','real','>=',2}, ...
    mfilename, 'RegionNumber', 4);
end

end


function [w,s]=fram2wav(x,tt,mode)
%FRAM2WAV  converts frame values to a continuous waveform [W]=(X,TT,MODE)
%  Inputs:
%          x(nf,p)      is the input signal: one row per frame
%	       tt(nf,3)     specifies the frames. Each row has the form [start_sample end_sample flag]
%                       where flag = 1 for the start of a new spurt.
%                       If tt(:,3) is omitted, a new spurt will be started whenever there is a gap
%                       of more than one between the end of one frame and the beginning or the next.
%                       A new spurt is automatically started if x() = NaN.
%          mode         consists of one or more of the following letters:
%                          z for zero-order hold interpolation (i.e. constant within each frame)
%                          l for linear interpolation within each spurt [default]
%
% Outputs:
%          w(n,p)       contains the interpolated waveforms. Their length is n = tt(nf,2)
%          s(ns,2)      gives the starting and ending sample numbers of each spurt (excluding NaN spurts)
%
%    This routine converts frame-based values to continuous waveforms by performing
%    a chosen method of interpolation. Interpolation is restarted at the beginning of each spurt.

%    Bugs/Suggestions
%      (1)   Additional mode option for cubic interpolation
%      (2)   Additional mode option for interpolation in log domain
%      (3)   Additional mode option for x values being
%            frame averages rather than mid-frame values.

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: fram2wav.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3
    mode='l';
end
[nf,m]=size(x);
n=round(tt(end,2));
w=repmat(NaN,n,m);
nt=size(tt,2);
ix1=ceil(tt(:,1)); % start of frame sample
ix2=floor(tt(:,2)); % end of frame sample

% determine the start and end of spurts

if nt>2
    ty=tt(:,3)>0;   % frame type set by user
else
    ty=zeros(nf,1);
    ty(2:end)=ix1(2:end)>ix2(1:end-1)+1;    % new spurt whenever a gap
end
ty(1)=1;           % first frame always starts a spurt
ty(isnan(x))=1;    % NaN always ends previous spurt
ty(1+find(isnan(x(1:end-1))))=1; % NaN always forces a new spurt
ty=double(ty);
ty(1:end-1)=ty(1:end-1)+2*ty(2:end);
ty(end)=ty(end)+2;   % last frame always ends a spurtw=repmat(NaN,n,m);  % initialize output to all NaN
nx=ix2-ix1+1;

if any(mode=='z')   % zero-order hold
    for i=1:nf
        if nx(i)
            w(ix1(i):ix2(i),:)=repmat(x(i,:),nx(i),1);
        end
    end
else   % linear interpolation is the default
    ttm=(tt(:,1)+tt(:,2))/2;    % mid point of frame
    ixm=floor(ttm); % end of first half of frame
    for i=1:nf
        if i==176
            i
        end
        if nx(i)
            tyi=ty(i);
            if tyi==3    % use a zero order hold
                w(ix1(i):ix2(i),:)=repmat(x(i,:),nx(i),1);
            else
                nxm=ixm(i)-ix1(i)+1;
                if nxm
                    if tyi==1
                        grad=(x(i+1,:)-x(i,:))/(ttm(i+1)-ttm(i));
                    else
                        grad=(x(i,:)-x(i-1,:))/(ttm(i)-ttm(i-1));
                    end
                    w(ix1(i):ixm(i),:)=repmat(x(i,:),nxm,1)+((ix1(i):ixm(i))'-ttm(i))*grad;
                end
                if nx(i)>nxm
                    if tyi==2
                        grad=(x(i,:)-x(i-1,:))/(ttm(i)-ttm(i-1));
                    else
                        grad=(x(i+1,:)-x(i,:))/(ttm(i+1)-ttm(i));
                    end
                    w(ixm(i)+1:ix2(i),:)=repmat(x(i,:),ix2(i)-ixm(i),1)+((ixm(i)+1:ix2(i))'-ttm(i))*grad;
                end
            end
        end
    end
end

% now sort out the start and end spurt positions

ty(isnan(x))=0;    % Don't count NaN spurts
s=repmat(ix1(bitand(ty,1)>0),1,2);
s(:,2)=ix2(bitand(ty,2)>0);
if ~nargout
    tw=(1:n)';
    for i=size(s,1):-1:2
        j=s(i,1);   % start of new spurt
        tw=[tw(1:j-1); tw(j); tw(j:end)];
        w=[w(1:j-1); NaN; w(j:end)];        % insert a NaN to force a plotting break
    end
    plot(tt(:,1:2)',repmat(x(:)',2,1),'r-+',tw,w,'b-');
end


% Test Tx DMA data output
amplitude = 2^15; frequency = 20e6;
swv1 = dsp.SineWave(amplitude, frequency);
swv1.ComplexOutput = true;
swv1.SamplesPerFrame = 2^20;
swv1.SampleRate = 100e6;
y = swv1();

uri = 'ip:analog';
fc = 1e9;

%% Tx set up
tx = adi.AD9371.Tx('uri',uri);
tx.CenterFrequency = fc;
tx.EnableCustomProfile = true;
tx.CustomProfileFileName = 'profile_TxBW100_ORxBW100_RxBW100.txt';
tx.DataSource = 'DMA';
tx.EnableCyclicBuffers = true;
tx.AttenuationChannel0 = -10;
tx(y);

%% Rx set up
rx = adi.AD9371.Rx('uri',uri);
rx.CenterFrequency = fc;

%% Run
for k=1:20
    valid = false;
    while ~valid
        [out, valid] = rx();
    end
end
rx.release();
tx.release();

%% Plot
nSamp = length(out);
fs = tx.SamplingRate;
FFTRxData  = fftshift(10*log10(abs(fft(out))));
df = fs/nSamp;  freqRangeRx = (-fs/2:df:fs/2-df).'/1000;
plot(freqRangeRx, FFTRxData);
xlabel('Frequency (kHz)');ylabel('Amplitude (dB)');grid on;


classdef GradientCirclePerimeterExperiment < handle

    properties (Access = private)
        radius
        nameCase
        inputFile
        scale
        imesh

        backgroundMesh
        boundaryMesh
        unfittedMesh

        domainLength
        levelSet

        regularizedPerimeter

        xNodesToPlot
        gradientInNodesToPlot

        outputFolder

    end

    methods (Access = public)

        function obj = GradientCirclePerimeterExperiment()
            obj.init();

            for imesh = 1:numel(obj.inputFile)
                obj.imesh = imesh;
                obj.createBackgroundAndBoundaryMesh();
                obj.computeDomainLength();
                obj.createLevelSet();
                obj.computeRegularizedPerimeters();
                obj.computeGradientSurfaces();
            end

        end

    end

    methods (Access = private)

        function init(obj)
            obj.radius = 0.2499999;
            obj.nameCase = 'GradientCirclePerimeterExperiment';
            obj.inputFile = {'SquareMacroTriangle'; ...
                'SquareMacroTriangleFine';...
                'SquareMacroTriangleFineFine'};
            obj.scale     = 'MACRO';
            obj.outputFolder = '/home/alex/Dropbox/Perimeter/';
        end

        function createBackgroundAndBoundaryMesh(obj)
            s.inputFile = obj.inputFile{obj.imesh};
            s.isBackgroundMeshRectangularBox = true;
            mCreator = BackgroundAndBoundaryMeshCreatorFromInputFile(s);
            obj.backgroundMesh = mCreator.backgroundMesh;
            obj.boundaryMesh   = mCreator.boundaryMesh;
        end

        function computeDomainLength(obj)
            x = obj.backgroundMesh.coord;
            d = max(x(:,1)) - min(x(:,1));
            obj.domainLength = d;
        end

        function createLevelSet(obj)
            s.type = 'circleInclusion';
            halfSide = obj.domainLength/2;
            s.fracRadius = obj.radius/halfSide;
            s.coord      = obj.backgroundMesh.coord;
            s.ndim       = obj.backgroundMesh.ndim;
            lsCreator = LevelSetCreator.create(s);
            obj.levelSet = lsCreator.getValue();
        end

        function computeGradientSurfaces(obj)
            s.outPutFolder = obj.outputFolder;
            s.mesh         = obj.backgroundMesh;
            s.rPerimeter   = obj.regularizedPerimeter;
            s.iMesh        = obj.imesh;
            s.domainLength = obj.domainLength;
            gComputer = GradientSurfPerimeterComputer(s);
            gComputer.compute();
        end

    end

end


function CI = calcCI(resp,time,censoring)
% resp: Hazards ratio for Cox regression
% Y: Time-to-event (the higher, the better)

% FLIP THE RESPONSE. If Resp1 > resp2 && Y1 > Y2, add + 1. IF Y1<Y2 and Y1
% is censored, discard.
% ________________________________________________________________________

% INITIALIZATION
nInst = numel(time);
resp = - resp; % Flipping the hazards for easier comparison

% GETTING ALL PAIRS OF RESPONSES
pairs = combnk(1:nInst,2);
resp = resp(pairs);
time = time(pairs);
censoring = censoring(pairs);

% REMOVING UNUSABLE DATA

% 1. Both patients lived: unusable data
sumCens = censoring(:,1) + censoring(:,2);
out = find(sumCens == 2);
resp(out,:) = []; time(out,:) = []; censoring(out,:) = [];

% 2. One patient died, but the follow-up of the other is not long enough (did not outlive the first patient): unusable data
valCheck = (censoring(:,2) - censoring(:,1)) .* (time(:,2) - time(:,1));
out = find(valCheck < 0);
resp(out,:) = []; time(out,:) = []; censoring(out,:) = [];

% 3. Both patients died at the same time: unusable data
valCheck1 = time(:,2) - time(:,1); valCheck2 = censoring(:,2) - censoring(:,1); % (all 1,1 censored pairs have been removed by now, leaving only 0,1 or 0,0)
out = find(valCheck1 == 0 & valCheck2 == 0);
resp(out,:) = []; time(out,:) = []; censoring(out,:) = [];

% COMPUTING TIES IN RESPONSES
diffResp = resp(:,2) - resp(:,1);
indTies = find(diffResp == 0);
nTies = numel(indTies);
resp(indTies,:) = []; time(indTies,:) = []; censoring(indTies,:) = [];

% CALCULATING CI
valCheck = (time(:,2) - time(:,1)) .* (resp(:,2) - resp(:,1)); nCheck = numel(valCheck);
indGood = find(valCheck > 0); nGood = numel(indGood);
CI = (nGood + 0.5*nTies)/(nCheck + nTies);
if isnan(CI)
    CI = 0.5;
end

end


function d = month_to_nones_roman ( m )

%*****************************************************************************80
%
%% MONTH_TO_NONES_ROMAN returns the day of the nones of a Roman month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the month index.
%
%    Output, integer D, the day of the nones of the month.
%
  nones = [ ...
    5, 5, 7, 5, 7, 5, 7, 5, 5, 7, 5, 5 ];

  if ( m < 1 || 12 < m )
    d = -1;
  else
    d = nones(m);
  end

  return
end