function y = perform_waveatoms_transform(x,dir, options)

% perform_waveatoms_transform - interface to WaveAtom transform
%
%   y = perform_waveatoms_transform(x,dir, options);
%
%   The waveatom toolbox can be downloaded from
%       http://www.waveatom.org/
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
issym = getoptions(options, 'issym', 0);
force_real = getoptions(options, 'force_real', 1);

if dir==1
    d = nb_dims(x);
else
    if iscell(x)
        d = nb_dims(x{1});
    else
        d = nb_dims(x);
    end
end

pat = 'p'; tp = 'ortho';
if d==1
    if dir==1
        if issym
            y = fatom1sym(x,pat,[1 1]);
        else
            y = fwa1(x,pat,tp);
        end
    else
        if issym
            y = iatom1sym(x,pat,[1 1]);
        else
            y = iwa1(x,pat,tp);
        end
    end
else
    if dir==1
        if issym
            y = fwa2sym(x,pat,[1 1]);
        else
            y = fwa2(x,pat,tp);
        end
    else
        if issym
            y = iwa2sym(x,pat,[1 1]);
        else
            y = iwa2(x,pat,tp);
        end
    end
end

if force_real && dir==-1
    y = real(y);
end

function c = fwa2(x,pat,tp)
% fwa2 - 2D forward wave atom transform
% -----------------
% INPUT
% --
% x is a real N-by-N matrix. N is a power of 2.
% --
% pat specifies the type of frequency partition which satsifies
% parabolic scaling relationship. pat can either be 'p' or 'q'.
% --
% tp is the type of tranform.
% 	'ortho': orthobasis
% 	'directional': real-valued frame with single oscillation direction
% 	'complex': complex-valued frame
% -----------------
% OUTPUT
% --
% c is a cell array which contains the wave atom coefficients. If
% tp=='ortho', then c{j}{m1,m2}(n1,n2) is the coefficient at scale j,
% frequency index (m1,m2) and spatial index (n1,n2). If
% tp=='directional', then c{j,d}{m1,m2}(n1,n2) with d=1,2 are the
% coefficients at scale j, frequency index (m1,m2) and spatial index
% (n1,n2). If tp=='complex', then c{j,d}{m1,m2)(n1,n2) with d=1,2,3,4
% are the coefficients at scale j, frequency index (m1,m2) and spatial
% index (n1,n2).
% -----------------
% Written by Lexing Ying and Laurent Demanet, 2007

if( ismember(tp, {'ortho','directional','complex'})==0 | ismember(pat, {'p','q','u'})==0 )    error('wrong');  end

if(strcmp(tp, 'ortho')==1)
    %---------------------------------------------------------
    N = size(x,1);
    H = N/2;
    lst = freq_pat(H,pat);
    %------------------
    f = fft2(x) / sqrt(prod(size(x)));
    A = N;
    c = cell(length(lst),1);
    %------------------
    for s=1:length(lst)
        nw = length(lst{s});
        c{s} = cell(nw,nw);
        for I=0:nw-1
            for J=0:nw-1
                if(lst{s}(I+1)==0 & lst{s}(J+1)==0)
                    c{s}{I+1,J+1} = [];
                else
                    B = 2^(s-1);
                    D = 2*B;
                    Ict = I*B;      Jct = J*B; %starting position in freq
                    if(mod(I,2)==0)
                        Ifm = Ict-2/3*B;        Ito = Ict+4/3*B;
                    else
                        Ifm = Ict-1/3*B;        Ito = Ict+5/3*B;
                    end
                    if(mod(J,2)==0)
                        Jfm = Jct-2/3*B;        Jto = Jct+4/3*B;
                    else
                        Jfm = Jct-1/3*B;        Jto = Jct+5/3*B;
                    end
                    res = zeros(D,D);
                    for id=0:1
                        if(id==0)
                            Idx = [ceil(Ifm):floor(Ito)];      Icf = kf_rt(Idx/B*pi, I);
                        else
                            Idx = [ceil(-Ito):floor(-Ifm)];      Icf = kf_lf(Idx/B*pi, I);
                        end
                        for jd=0:1
                            if(jd==0)
                                Jdx = [ceil(Jfm):floor(Jto)];      Jcf = kf_rt(Jdx/B*pi, J);
                            else
                                Jdx = [ceil(-Jto):floor(-Jfm)];      Jcf = kf_lf(Jdx/B*pi, J);
                            end
                            res(mod(Idx,D)+1,mod(Jdx,D)+1) = res(mod(Idx,D)+1,mod(Jdx,D)+1) + conj( Icf.'*Jcf ) .* f(mod(Idx,A)+1,mod(Jdx,A)+1);
                        end
                    end
                    c{s}{I+1,J+1} = ifft2(res) * sqrt(prod(size(res)));
                end
            end
        end
    end

elseif(strcmp(tp, 'directional')==1)
    %---------------------------------------------------------
    N = size(x,1);
    H = N/2;
    lst = freq_pat(H,pat);
    %------------------
    f = fft2(x) / sqrt(prod(size(x)));
    A = N;
    c1 = cell(length(lst),1);
    c2 = cell(length(lst),1);
    %------------------
    for s=1:length(lst)
        nw = length(lst{s});
        c1{s} = cell(nw,nw);
        c2{s} = cell(nw,nw);
        for I=0:nw-1
            for J=0:nw-1
                if(lst{s}(I+1)==0 & lst{s}(J+1)==0)
                    c1{s}{I+1,J+1} = [];
                    c2{s}{I+1,J+1} = [];
                else
                    B = 2^(s-1);
                    D = 2*B;
                    res = zeros(D,D);
                    Ict = I*B;      Jct = J*B; %starting position in freq
                    if(mod(I,2)==0)
                        Ifm = Ict-2/3*B;        Ito = Ict+4/3*B;
                    else
                        Ifm = Ict-1/3*B;        Ito = Ict+5/3*B;
                    end
                    if(mod(J,2)==0)
                        Jfm = Jct-2/3*B;        Jto = Jct+4/3*B;
                    else
                        Jfm = Jct-1/3*B;        Jto = Jct+5/3*B;
                    end

                    res = zeros(D,D);
                    Idx = [ceil(Ifm):floor(Ito)];      Icf = kf_rt(Idx/B*pi, I);
                    Jdx = [ceil(Jfm):floor(Jto)];      Jcf = kf_rt(Jdx/B*pi, J);
                    res(mod(Idx,D)+1,mod(Jdx,D)+1) = res(mod(Idx,D)+1,mod(Jdx,D)+1) + conj( Icf.'*Jcf ) .* f(mod(Idx,A)+1,mod(Jdx,A)+1);
                    Idx = [ceil(-Ito):floor(-Ifm)];      Icf = kf_lf(Idx/B*pi, I);
                    Jdx = [ceil(-Jto):floor(-Jfm)];      Jcf = kf_lf(Jdx/B*pi, J);
                    res(mod(Idx,D)+1,mod(Jdx,D)+1) = res(mod(Idx,D)+1,mod(Jdx,D)+1) + conj( Icf.'*Jcf ) .* f(mod(Idx,A)+1,mod(Jdx,A)+1);
                    c1{s}{I+1,J+1} = ifft2(res) * sqrt(prod(size(res)));

                    res = zeros(D,D);
                    Idx = [ceil(Ifm):floor(Ito)];      Icf = kf_rt(Idx/B*pi, I);
                    Jdx = [ceil(-Jto):floor(-Jfm)];      Jcf = kf_lf(Jdx/B*pi, J);
                    res(mod(Idx,D)+1,mod(Jdx,D)+1) = res(mod(Idx,D)+1,mod(Jdx,D)+1) + conj( Icf.'*Jcf ) .* f(mod(Idx,A)+1,mod(Jdx,A)+1);
                    Idx = [ceil(-Ito):floor(-Ifm)];      Icf = kf_lf(Idx/B*pi, I);
                    Jdx = [ceil(Jfm):floor(Jto)];      Jcf = kf_rt(Jdx/B*pi, J);
                    res(mod(Idx,D)+1,mod(Jdx,D)+1) = res(mod(Idx,D)+1,mod(Jdx,D)+1) + conj( Icf.'*Jcf ) .* f(mod(Idx,A)+1,mod(Jdx,A)+1);
                    c2{s}{I+1,J+1} = ifft2(res) * sqrt(prod(size(res)));
                end
            end
        end
    end
    c = [c1 c2];

elseif(strcmp(tp, 'complex')==1)
    %---------------------------------------------------------
    N = size(x,1);
    H = N/2;
    lst = freq_pat(H,pat);
    %------------------
    f = fft2(x) / sqrt(prod(size(x)));
    A = N;
    c1 = cell(length(lst),1);
    c2 = cell(length(lst),1);
    c3 = cell(length(lst),1);
    c4 = cell(length(lst),1);
    %------------------
    for s=1:length(lst)
        nw = length(lst{s});
        c1{s} = cell(nw,nw);
        c2{s} = cell(nw,nw);
        c3{s} = cell(nw,nw);
        c4{s} = cell(nw,nw);
        for I=0:nw-1
            for J=0:nw-1
                if(lst{s}(I+1)==0 & lst{s}(J+1)==0)
                    c1{s}{I+1,J+1} = [];
                    c2{s}{I+1,J+1} = [];
                    c3{s}{I+1,J+1} = [];
                    c4{s}{I+1,J+1} = [];
                else
                    B = 2^(s-1);
                    D = 2*B;
                    res = zeros(D,D);
                    Ict = I*B;      Jct = J*B; %starting position in freq
                    if(mod(I,2)==0)
                        Ifm = Ict-2/3*B;        Ito = Ict+4/3*B;
                    else
                        Ifm = Ict-1/3*B;        Ito = Ict+5/3*B;
                    end
                    if(mod(J,2)==0)
                        Jfm = Jct-2/3*B;        Jto = Jct+4/3*B;
                    else
                        Jfm = Jct-1/3*B;        Jto = Jct+5/3*B;
                    end

                    res = zeros(D,D);
                    Idx = [ceil(Ifm):floor(Ito)];      Icf = kf_rt(Idx/B*pi, I);
                    Jdx = [ceil(Jfm):floor(Jto)];      Jcf = kf_rt(Jdx/B*pi, J);
                    res(mod(Idx,D)+1,mod(Jdx,D)+1) = res(mod(Idx,D)+1,mod(Jdx,D)+1) + conj( Icf.'*Jcf ) .* f(mod(Idx,A)+1,mod(Jdx,A)+1);
                    c1{s}{I+1,J+1} = ifft2(res) * sqrt(prod(size(res)));

                    res = zeros(D,D);
                    Idx = [ceil(-Ito):floor(-Ifm)];      Icf = kf_lf(Idx/B*pi, I);
                    Jdx = [ceil(-Jto):floor(-Jfm)];      Jcf = kf_lf(Jdx/B*pi, J);
                    res(mod(Idx,D)+1,mod(Jdx,D)+1) = res(mod(Idx,D)+1,mod(Jdx,D)+1) + conj( Icf.'*Jcf ) .* f(mod(Idx,A)+1,mod(Jdx,A)+1);
                    c2{s}{I+1,J+1} = ifft2(res) * sqrt(prod(size(res)));

                    res = zeros(D,D);
                    Idx = [ceil(Ifm):floor(Ito)];      Icf = kf_rt(Idx/B*pi, I);
                    Jdx = [ceil(-Jto):floor(-Jfm)];      Jcf = kf_lf(Jdx/B*pi, J);
                    res(mod(Idx,D)+1,mod(Jdx,D)+1) = res(mod(Idx,D)+1,mod(Jdx,D)+1) + conj( Icf.'*Jcf ) .* f(mod(Idx,A)+1,mod(Jdx,A)+1);
                    c3{s}{I+1,J+1} = ifft2(res) * sqrt(prod(size(res)));

                    res = zeros(D,D);
                    Idx = [ceil(-Ito):floor(-Ifm)];      Icf = kf_lf(Idx/B*pi, I);
                    Jdx = [ceil(Jfm):floor(Jto)];      Jcf = kf_rt(Jdx/B*pi, J);
                    res(mod(Idx,D)+1,mod(Jdx,D)+1) = res(mod(Idx,D)+1,mod(Jdx,D)+1) + conj( Icf.'*Jcf ) .* f(mod(Idx,A)+1,mod(Jdx,A)+1);
                    c4{s}{I+1,J+1} = ifft2(res) * sqrt(prod(size(res)));
                end
            end
        end
    end
    c = [c1 c2 c3 c4];
end

function x = iwa2(c,pat,tp)
% iwa2 - 2D inverse wave atom transform
% -----------------
% INPUT
% --
% c is a cell array which contains the wave atom coefficients. If
% tp=='ortho', then c{j}{m1,m2}(n1,n2) is the coefficient at scale j,
% frequency index (m1,m2) and spatial index (n1,n2). If
% tp=='directional', then c{j,d}{m1,m2}(n1,n2) with d=1,2 are the
% coefficients at scale j, frequency index (m1,m2) and spatial index
% (n1,n2). If tp=='complex', then c{j,d}{m1,m2)(n1,n2) with d=1,2,3,4
% are the coefficients at scale j, frequency index (m1,m2) and spatial
% index (n1,n2).
% --
% pat specifies the type of frequency partition which satsifies
% parabolic scaling relationship. pat can either be 'p' or 'q'.
% --
% tp is the type of tranform.
% 	'ortho': orthobasis
% 	'directional': real-valued frame with single oscillation direction
% 	'complex': complex-valued frame
% -----------------
% OUTPUT
% --
% x is a real N-by-N matrix. N is a power of 2.
% -----------------
% Written by Lexing Ying and Laurent Demanet, 2007

if( ismember(tp, {'ortho','directional','complex'})==0 | ismember(pat, {'p','q','u'})==0 )    error('wrong');  end

if(strcmp(tp, 'ortho')==1)
    %---------------------------------------------------------
    T = 0;
    for s=1:length(c)
        nw = length(c{s});
        for I=1:nw
            for J=1:nw
                T = T + prod(size(c{s}{I,J}));
            end
        end
    end
    N = sqrt(T);
    H = N/2;
    lst = freq_pat(H,pat);
    A = N;
    f = zeros(A,A);
    %------------------
    for s=1:length(lst)
        nw = length(lst{s});
        for I=0:nw-1
            for J=0:nw-1
                if(~isempty(c{s}{I+1,J+1}))
                    B = 2^(s-1);
                    D = 2*B;
                    Ict = I*B;      Jct = J*B; %starting position in freq
                    if(mod(I,2)==0)
                        Ifm = Ict-2/3*B;        Ito = Ict+4/3*B;
                    else
                        Ifm = Ict-1/3*B;        Ito = Ict+5/3*B;
                    end
                    if(mod(J,2)==0)
                        Jfm = Jct-2/3*B;        Jto = Jct+4/3*B;
                    else
                        Jfm = Jct-1/3*B;        Jto = Jct+5/3*B;
                    end
                    res = fft2(c{s}{I+1,J+1}) / sqrt(prod(size(c{s}{I+1,J+1}))); %res = zeros(D,D);
                    for id=0:1
                        if(id==0)
                            Idx = [ceil(Ifm):floor(Ito)];      Icf = kf_rt(Idx/B*pi, I);
                        else
                            Idx = [ceil(-Ito):floor(-Ifm)];      Icf = kf_lf(Idx/B*pi, I);
                        end
                        for jd=0:1
                            if(jd==0)
                                Jdx = [ceil(Jfm):floor(Jto)];      Jcf = kf_rt(Jdx/B*pi, J);
                            else
                                Jdx = [ceil(-Jto):floor(-Jfm)];      Jcf = kf_lf(Jdx/B*pi, J);
                            end
                            f(mod(Idx,A)+1,mod(Jdx,A)+1) = f(mod(Idx,A)+1,mod(Jdx,A)+1) + ( Icf.'*Jcf ) .* res(mod(Idx,D)+1,mod(Jdx,D)+1);
                        end
                    end
                end
            end
        end
    end
    %------------------
    x = ifft2(f) * sqrt(prod(size(f)));

elseif(strcmp(tp, 'directional')==1)
    %---------------------------------------------------------
    c1 = c(:,1);
    c2 = c(:,2);

    T = 0;
    for s=1:length(c1)
        nw = length(c1{s});
        for I=1:nw
            for J=1:nw
                T = T + prod(size(c1{s}{I,J}));
            end
        end
    end
    N = sqrt(T);
    H = N/2;
    lst = freq_pat(H,pat);
    A = N;
    f = zeros(A,A);
    %------------------
    for s=1:length(lst)
        nw = length(lst{s});
        for I=0:nw-1
            for J=0:nw-1
                if(~isempty(c1{s}{I+1,J+1}))
                    B = 2^(s-1);
                    D = 2*B;
                    Ict = I*B;      Jct = J*B; %starting position in freq
                    if(mod(I,2)==0)
                        Ifm = Ict-2/3*B;        Ito = Ict+4/3*B;
                    else
                        Ifm = Ict-1/3*B;        Ito = Ict+5/3*B;
                    end
                    if(mod(J,2)==0)
                        Jfm = Jct-2/3*B;        Jto = Jct+4/3*B;
                    else
                        Jfm = Jct-1/3*B;        Jto = Jct+5/3*B;
                    end

                    res = fft2(c1{s}{I+1,J+1}) / sqrt(prod(size(c1{s}{I+1,J+1}))); %res = zeros(D,D);
                    Idx = [ceil(Ifm):floor(Ito)];      Icf = kf_rt(Idx/B*pi, I);
                    Jdx = [ceil(Jfm):floor(Jto)];      Jcf = kf_rt(Jdx/B*pi, J);
                    f(mod(Idx,A)+1,mod(Jdx,A)+1) = f(mod(Idx,A)+1,mod(Jdx,A)+1) + ( Icf.'*Jcf ) .* res(mod(Idx,D)+1,mod(Jdx,D)+1);
                    Idx = [ceil(-Ito):floor(-Ifm)];      Icf = kf_lf(Idx/B*pi, I);
                    Jdx = [ceil(-Jto):floor(-Jfm)];      Jcf = kf_lf(Jdx/B*pi, J);
                    f(mod(Idx,A)+1,mod(Jdx,A)+1) = f(mod(Idx,A)+1,mod(Jdx,A)+1) + ( Icf.'*Jcf ) .* res(mod(Idx,D)+1,mod(Jdx,D)+1);

                    res = fft2(c2{s}{I+1,J+1}) / sqrt(prod(size(c2{s}{I+1,J+1}))); %res = zeros(D,D);
                    Idx = [ceil(Ifm):floor(Ito)];      Icf = kf_rt(Idx/B*pi, I);
                    Jdx = [ceil(-Jto):floor(-Jfm)];      Jcf = kf_lf(Jdx/B*pi, J);
                    f(mod(Idx,A)+1,mod(Jdx,A)+1) = f(mod(Idx,A)+1,mod(Jdx,A)+1) + ( Icf.'*Jcf ) .* res(mod(Idx,D)+1,mod(Jdx,D)+1);
                    Idx = [ceil(-Ito):floor(-Ifm)];      Icf = kf_lf(Idx/B*pi, I);
                    Jdx = [ceil(Jfm):floor(Jto)];      Jcf = kf_rt(Jdx/B*pi, J);
                    f(mod(Idx,A)+1,mod(Jdx,A)+1) = f(mod(Idx,A)+1,mod(Jdx,A)+1) + ( Icf.'*Jcf ) .* res(mod(Idx,D)+1,mod(Jdx,D)+1);
                end
            end
        end
    end
    x = ifft2(f) * sqrt(prod(size(f)));

elseif(strcmp(tp, 'complex')==1)
    %---------------------------------------------------------
    c1 = c(:,1);
    c2 = c(:,2);
    c3 = c(:,3);
    c4 = c(:,4);

    T = 0;
    for s=1:length(c1)
        nw = length(c1{s});
        for I=1:nw
            for J=1:nw
                T = T + prod(size(c1{s}{I,J}));
            end
        end
    end
    N = sqrt(T);
    H = N/2;
    lst = freq_pat(H,pat);
    A = N;
    f = zeros(A,A);
    %------------------
    for s=1:length(lst)
        nw = length(lst{s});
        for I=0:nw-1
            for J=0:nw-1
                if(~isempty(c1{s}{I+1,J+1}))
                    B = 2^(s-1);
                    D = 2*B;
                    Ict = I*B;      Jct = J*B; %starting position in freq
                    if(mod(I,2)==0)
                        Ifm = Ict-2/3*B;        Ito = Ict+4/3*B;
                    else
                        Ifm = Ict-1/3*B;        Ito = Ict+5/3*B;
                    end
                    if(mod(J,2)==0)
                        Jfm = Jct-2/3*B;        Jto = Jct+4/3*B;
                    else
                        Jfm = Jct-1/3*B;        Jto = Jct+5/3*B;
                    end

                    res = fft2(c1{s}{I+1,J+1}) / sqrt(prod(size(c1{s}{I+1,J+1}))); %res = zeros(D,D);
                    Idx = [ceil(Ifm):floor(Ito)];      Icf = kf_rt(Idx/B*pi, I);
                    Jdx = [ceil(Jfm):floor(Jto)];      Jcf = kf_rt(Jdx/B*pi, J);
                    f(mod(Idx,A)+1,mod(Jdx,A)+1) = f(mod(Idx,A)+1,mod(Jdx,A)+1) + ( Icf.'*Jcf ) .* res(mod(Idx,D)+1,mod(Jdx,D)+1);

                    res = fft2(c2{s}{I+1,J+1}) / sqrt(prod(size(c2{s}{I+1,J+1}))); %res = zeros(D,D);
                    Idx = [ceil(-Ito):floor(-Ifm)];      Icf = kf_lf(Idx/B*pi, I);
                    Jdx = [ceil(-Jto):floor(-Jfm)];      Jcf = kf_lf(Jdx/B*pi, J);
                    f(mod(Idx,A)+1,mod(Jdx,A)+1) = f(mod(Idx,A)+1,mod(Jdx,A)+1) + ( Icf.'*Jcf ) .* res(mod(Idx,D)+1,mod(Jdx,D)+1);

                    res = fft2(c3{s}{I+1,J+1}) / sqrt(prod(size(c3{s}{I+1,J+1}))); %res = zeros(D,D);
                    Idx = [ceil(Ifm):floor(Ito)];      Icf = kf_rt(Idx/B*pi, I);
                    Jdx = [ceil(-Jto):floor(-Jfm)];      Jcf = kf_lf(Jdx/B*pi, J);
                    f(mod(Idx,A)+1,mod(Jdx,A)+1) = f(mod(Idx,A)+1,mod(Jdx,A)+1) + ( Icf.'*Jcf ) .* res(mod(Idx,D)+1,mod(Jdx,D)+1);

                    res = fft2(c4{s}{I+1,J+1}) / sqrt(prod(size(c4{s}{I+1,J+1}))); %res = zeros(D,D);
                    Idx = [ceil(-Ito):floor(-Ifm)];      Icf = kf_lf(Idx/B*pi, I);
                    Jdx = [ceil(Jfm):floor(Jto)];      Jcf = kf_rt(Jdx/B*pi, J);
                    f(mod(Idx,A)+1,mod(Jdx,A)+1) = f(mod(Idx,A)+1,mod(Jdx,A)+1) + ( Icf.'*Jcf ) .* res(mod(Idx,D)+1,mod(Jdx,D)+1);
                end
            end
        end
    end
    x = ifft2(f) * sqrt(prod(size(f)));
end

function lst = freq_pat(H,pat)
% freq_pat.m - generate frequency partition
%
% input:
%   H:    half length of the frequency span, H needs to be greater than 16
%   pat:  type of frequency partition which satsifies parabolic scaling relationship
%         equal to 'p' or 'q'
%
% output:
%   lst:  data representing the partition of frequency
%
% Written by Lexing Ying and Laurent Demanet, 2006

if(H<16)    error('H needs to be at least 16');  end

if(    pat=='p') %parabolic scaling by lexing
    len = ceil(log2(H)/2)+1;
    lst = cell(len,1);
    lst{1} = [1 1];    lst{2} = [0 1];    lst{3} = [0 1 1 1];
    cnt = 16;
    idx = 3;    rad = 4;
    while(cnt<H)
        old = cnt;
        lst{idx} = [lst{idx} 1 1];
        cnt = cnt + 2*rad;
        idx = idx+1;
        rad = 2*rad;
        trg = min(4*old,H);
        lst{idx} = [zeros(1,cnt/rad), ones(1,(trg-cnt)/rad)];
        cnt = trg;
    end
elseif(pat=='q') %parabolic scaling by laurent
    len = floor(log2(H)/2)+1;
    lst = cell(len,1);
    lst{1} = [1 1];    lst{2} = [0 1 1 1];
    cnt = 8;
    idx = 2;      rad = 2;
    while(cnt<H)
        old = cnt;
        lst{idx} = [lst{idx} 1 1];
        cnt = cnt + 2*rad;
        idx = idx+1;
        rad = 2*rad;
        trg = min(4*old,H);
        lst{idx} = [zeros(1,cnt/rad), ones(1,(trg-cnt)/rad)];
        cnt = trg;
    end
elseif(pat=='u')
    len = ceil(log2(H)/2)+1; %uniform partitioning
    lst = cell(len,1);
    B = 2^(len-1);
    lst{end} = ones(1,H/B);
else
    error('wrong pat');
end

function r = kf_rt(w,n)
% kf_rt.m - right bump
%
% Written by Lexing Ying and Laurent Demanet, 2006

r = zeros(size(w));
an = pi/2*(n+1/2);
en = (-1)^n;
en1 = (-1)^(n+1);

r = exp(-i*w/2) .* ( exp(i*an)*g(en*(w-pi*(n+1/2))) );

function r = g(w)
% g.m - 'g' function Villemoes's construction
%
% Written by Lexing Ying and Laurent Demanet, 2006
r = zeros(size(w));
gd = w<5*pi/6 & w>-7*pi/6;
r(gd) = abs(sf(w(gd)-3*pi/2));

%----------------------------------------------------------------------
function r = sf(w)
r = zeros(size(w));

aw = abs(w);
r(aw<=2*pi/3) = 0;

gd = aw>=2*pi/3 & aw<=4*pi/3;
r(gd) = 1/sqrt(2) * hf(w(gd)/2+pi);

gd = aw>=4*pi/3 & aw<=8*pi/3;
r(gd) = 1/sqrt(2) * hf(w(gd)/4);

r(aw>8*pi/2) = 0;

function r = hf(w)
w = mod(w+pi,2*pi) - pi;
r = zeros(size(w));
w = abs(w);

r = sqrt(2) * cos(pi/2 * beta(3*w/pi-1));
r(w<=pi/3) = sqrt(2);
r(w>=2*pi/3) = 0;

function r = beta(x)
r = x.^4 .*(35-84*x + 70*x.^2 -20*x.^3);

function r = kf_lf(w,n)
% kf_lf.m - left bump
%
% Written by Lexing Ying and Laurent Demanet, 2006

r = zeros(size(w));
an = pi/2*(n+1/2);
en = (-1)^n;
en1 = (-1)^(n+1);

r = exp(-i*w/2) .* ( exp(-i*an)*g(en1*(w+pi*(n+1/2))) );

function res = fwa2sym(x,pat,tp)
% fwa2sym - 2D forward wave atom transform (symmetric version)
% -----------------
% INPUT
% --
% x is a real N-by-N matrix. N is a power of 2.
% --
% pat specifies the type of frequency partition which satsifies
% parabolic scaling relationship. pat can either be 'p' or 'q'.
% --
% tp is the type of tranform.
% 	'ortho': orthobasis
% 	'directional': real-valued frame with single oscillation direction
% 	'complex': complex-valued frame
% -----------------
% OUTPUT
% --
% res is an array containing all the wave atom coefficients. If
% tp=='ortho', then res is of size N-by-N. If tp=='
% If tp=='directional', then res is of size N-by-N-by-2. If tp=='complex',
% then res is of size N-by-N-by-4.
% -----------------
% Written by Lexing Ying and Laurent Demanet, 2007

if( ismember(tp, {'ortho','directional','complex'})==0 | ismember(pat, {'p','q','u'})==0 )    error('wrong');  end

call = fwa2(x,pat,tp);
N = size(x,1);
res = zeros(N,N,size(call,2));

for i=1:size(call,2)
    c = call(:,i);
    y = zeros(N,N);
    for s=1:length(c)
        D = 2^s;
        nw = length(c{s});
        for I=0:nw-1
            for J=0:nw-1
                if(~isempty(c{s}{I+1,J+1}))
                    y( I*D+[1:D], J*D+[1:D] ) = c{s}{I+1,J+1};
                end
            end
        end
    end
    res(:,:,i) = y;
end

function x = iwa2sym(res,pat,tp)
% iwa2sym - 2D inverse wave atom transform (symmetric version)
% -----------------
% INPUT
% --
% res is an array containing all the wave atom coefficients. If
% tp=='ortho', then res is of size N-by-N. If tp=='
% If tp=='directional', then res is of size N-by-N-by-2. If tp=='complex',
% then res is of size N-by-N-by-4.
% --
% pat specifies the type of frequency partition which satsifies
% parabolic scaling relationship. pat can either be 'p' or 'q'.
% --
% tp is the type of tranform.
% 	'ortho': orthobasis
% 	'directional': real-valued frame with single oscillation direction
% 	'complex': complex-valued frame
% -----------------
% OUTPUT
% --
% x is a real N-by-N matrix. N is a power of 2.
% -----------------
% Written by Lexing Ying and Laurent Demanet, 2007

if( ismember(tp, {'ortho','directional','complex'})==0 | ismember(pat, {'p','q','u'})==0 )    error('wrong');  end

N = size(res,1);
H = N/2;
lst = freq_pat(H,pat);
red = size(res,3);

call = cell(length(lst),red);
for i=1:red
    y = res(:,:,i);
    c = cell(length(lst),1);
    for s=1:length(lst)
        B = 2^(s-1);    D = 2*B;
        nw = length(lst{s});
        c{s} = cell(nw,nw);
        for I=0:nw-1
            for J=0:nw-1
                if(lst{s}(I+1)==0 & lst{s}(J+1)==0)
                    c{s}{I+1,J+1} = [];
                else
                    c{s}{I+1,J+1} = y( I*D+[1:D], J*D+[1:D] );
                end
            end
        end
    end
    call(:,i) = c;
end
x = iwa2(call,pat,tp);


function [in] = dm2in(dm)
% Convert length from decimeters to inches.
% I've still never used a decimeter.  Who uses this unit?
% Chad A. Greene 2012
in = dm/.254;


function X = cell2mat_ov_sum(I,xx_s,xx_f,yy_s,yy_f,zz_s,zz_f,overlap,sz,Bs)

% converts a cell array to a matrix when the cell elements overlap
% INPUTS:
% I:            cell array
% grid_size:    true size of each element
% overlap:      amount of overlap in each direction
% d1:           number of rows of matrix
% d2:           number of columns of matrix

% OUTPUT:
% X:            output matrix

% Written by Eftychios A. Pnevmatikakis, Simons Foundation, 2016

if nargin < 10 || isempty(Bs)
    Bs = cellfun(@(x) ones(size(x)), I,'un',0);
end
X = zeros([sz,size(I{1,1},length(sz)+1)]);
B = zeros(size(X));
if length(sz) == 2; sz(3) = 1; end

for i = 1:length(xx_f)
    for j = 1:length(yy_f)
        for k = 1:length(zz_f)
            extended_grid = [max(xx_s(i)-overlap(1),1),min(xx_f(i)+overlap(1),sz(1)),max(yy_s(j)-overlap(2),1),min(yy_f(j)+overlap(2),sz(2)),max(zz_s(k)-overlap(3),1),min(zz_f(k)+overlap(3),sz(3))];
            %W = construct_weights([xx_s(i),xx_f(i),yy_s(j),yy_f(j),zz_s(k),zz_f(k)],extended_grid)';
            Xtemp = zeros(size(I{i,j,k}));
            Btemp = Xtemp;
            ind = ~isnan(I{i,j,k});
            Xtemp(ind) = Bs{i,j,k}(ind).*I{i,j,k}(ind);
            Btemp(ind) = Bs{i,j,k}(ind);
            %X(extended_grid(1):extended_grid(2),extended_grid(3):extended_grid(4),extended_grid(5):extended_grid(6)) = X(extended_grid(1):extended_grid(2),extended_grid(3):extended_grid(4),extended_grid(5):extended_grid(6)) + Bs{i,j,k}.*I{i,j,k};
            %B(extended_grid(1):extended_grid(2),extended_grid(3):extended_grid(4),extended_grid(5):extended_grid(6)) = B(extended_grid(1):extended_grid(2),extended_grid(3):extended_grid(4),extended_grid(5):extended_grid(6)) + Bs{i,j,k}.*(I{i,j,k}~=0);
            X(extended_grid(1):extended_grid(2),extended_grid(3):extended_grid(4),extended_grid(5):extended_grid(6)) = X(extended_grid(1):extended_grid(2),extended_grid(3):extended_grid(4),extended_grid(5):extended_grid(6)) + Xtemp;
            B(extended_grid(1):extended_grid(2),extended_grid(3):extended_grid(4),extended_grid(5):extended_grid(6)) = B(extended_grid(1):extended_grid(2),extended_grid(3):extended_grid(4),extended_grid(5):extended_grid(6)) + Btemp;
        end
    end
end

X = X./B;
%X(isnan(X))=0;


function femIF = genNedIFEM3DFace(mesh,fem,femI)
%% Usage: Generate Quadrature Information on Interface Faces Used in PPIFEM
%         Each interface face (triangle) is cut into 3 small triangles
%            A1                A1
%           D  E      or      E  D         equivalent
%          A2  A3            A3   A2
%
%          p = [A1;D;E;A2;A3];
%          t = [1 2 3; 2 3 4; 3 4 5]
%
% femIF.tL --- global index of left element
% femIF.tR --- global index of right element
% femIF.basL --- 3*nfi-4-4 matrix basis functions of left elem on small tri
% femIF.basR --- 3*nfi-4-4 matrix basis functions of right elem on small tri
% femIF.gx --- Gaussian x nodes on small tri
% femIF.gy --- Gaussian y nodes on small tri
% femIF.gz --- Gaussian z nodes on small tri
% femIF.gw --- Gaussian weights on small tri (3 Gaussian pt for linear IFEM)
% femIF.area --- Areas of all small tri
% femIF.normal --- unit normal vector of each small tri

% Last Modified by Xu Zhang 08/07/2020
%%
nfI = -min(mesh.fLoc); % number of interface faces
intFID = find(mesh.fLoc<0);
nfIB = size(find(mesh.f_t(intFID,2)==0),1); % # of interface faces on Boundary
nfII = nfI - nfIB; % # of internal interface faces: two tetra share it L&R
tL = zeros(3*nfII,6); tR = zeros(3*nfII,6);
tIntfLID = zeros(3*nfII,1); tIntfRID = zeros(3*nfII,1);
basL = zeros(3*nfII,6,6); basR = zeros(3*nfII,6,6);
gx = zeros(3*nfII,3); gy = zeros(3*nfII,3); gz = zeros(3*nfII,3);
A = zeros(3*nfII,1); normal = zeros(3*nfII,3);

tB = zeros(3*nfIB,6); basB = zeros(3*nfIB,6,6);
gxB = zeros(3*nfIB,3); gyB = zeros(3*nfIB,3); gzB = zeros(3*nfIB,3);
AB = zeros(3*nfIB,1); normalB = zeros(3*nfIB,3);

id = 0; idB = 0;
for i = 1:nfI
    %% Form small triangular partition on the interface face
    fID = intFID(i); % face index
    f_e = mesh.f_e(fID,:); % three surrounding edge index
    idE = find(mesh.eLoc(f_e)<0); % find index of two interface edges
    tmp = [mesh.e(f_e(idE(1)),:), mesh.e(f_e(idE(2)),:)];
    nd1 = sum(tmp) - sum(unique(tmp)); % the node of two interface edges
    nd2 = sum(mesh.e(f_e(idE(1)),:)) - nd1;
    nd3 = sum(mesh.e(f_e(idE(2)),:)) - nd1;
    p = [mesh.p(nd1,:); mesh.eIntP(-mesh.eLoc(f_e(idE(1))),:); ...
        mesh.eIntP(-mesh.eLoc(f_e(idE(2))),:); mesh.p(nd2,:); mesh.p(nd3,:)];
    t = [1 2 3; 2 3 4; 3 4 5];

    %% gx gy gz on a triangle with 3 internal point, accurate upto pd = 2
    X1 = p(t(:,1),:);  X2 = p(t(:,2),:);   X3 = p(t(:,3),:);
    G = zeros(3,9);  w1 = 2/3;  w2 = 1/6; % see gaussPtri.m
    G(:,[1,4,7]) = w1*X1 + w2*(X2+X3);
    G(:,[2,5,8]) = w1*X2 + w2*(X1+X3);
    G(:,[3,6,9]) = w1*X3 + w2*(X1+X2);

    %% triangle area on three-dimension.
    x1 = X1(:,1);  y1 = X1(:,2);   z1 = X1(:,3);
    x2 = X2(:,1);  y2 = X2(:,2);   z2 = X2(:,3);
    x3 = X3(:,1);  y3 = X3(:,2);   z3 = X3(:,3);
    AT = 1/2*(((x1-x3).*(y2-y1) - (x1-x2).*(y3-y1)).^2 + ...
        ((y1-y3).*(z2-z1) - (y1-y2).*(z3-z1)).^2 + ...
        ((z1-z3).*(x2-x1) - (z1-z2).*(x3-x1)).^2).^(1/2);

    %% Left and Right Element
    tIDL = mesh.f_t(fID,1); % element index of left element
    tIDR = mesh.f_t(fID,2); % element index of right element
    if tIDR > 0 % Internal Face
        tIDLi = -mesh.tLoc(tIDL); % intf elem index of left element
        tIDRi = -mesh.tLoc(tIDR); % intf elem index of right element

        %% Determine piece
        nd1ID = mesh.pLoc(nd1); tLp = femI.plusPC(tIDLi); tRp = femI.plusPC(tIDRi);
        if (nd1ID < 0 && tLp == 1) || (nd1ID > 0 && tLp == 2)
            basL(id+1,:,:) = femI.bas2(tIDLi,:,:);
            basL(id+2,:,:) = femI.bas1(tIDLi,:,:);
            basL(id+3,:,:) = femI.bas1(tIDLi,:,:);
        elseif (nd1ID < 0 && tLp == 2) || (nd1ID > 0 && tLp == 1)
            basL(id+1,:,:) = femI.bas1(tIDLi,:,:);
            basL(id+2,:,:) = femI.bas2(tIDLi,:,:);
            basL(id+3,:,:) = femI.bas2(tIDLi,:,:);
        end
        if (nd1ID < 0 && tRp == 1) || (nd1ID > 0 && tRp == 2)
            basR(id+1,:,:) = femI.bas2(tIDRi,:,:);
            basR(id+2,:,:) = femI.bas1(tIDRi,:,:);
            basR(id+3,:,:) = femI.bas1(tIDRi,:,:);
        elseif (nd1ID < 0 && tRp == 2) || (nd1ID > 0 && tRp == 1)
            basR(id+1,:,:) = femI.bas1(tIDRi,:,:);
            basR(id+2,:,:) = femI.bas2(tIDRi,:,:);
            basR(id+3,:,:) = femI.bas2(tIDRi,:,:);
        end

        gx(id+1:id+3,:) = G(:,1:3);
        gy(id+1:id+3,:) = G(:,4:6);
        gz(id+1:id+3,:) = G(:,7:9);

        A(id+1:id+3,:) = AT;
        normal(id+1:id+3,:) = repmat(mesh.f_norm(fID,:),3,1);

        %% tL and tR, use locID, b/c index on interface cell is different
        temp = fem.g2ldof(tIDL,:);
        %temp1 = temp(femI.elocIDvec(tIDLi,:));
        tL(id+1:id+3,:) = repmat(temp,3,1);
        tIntfLID(id+1:id+3,:) = tIDL;

        temp = fem.g2ldof(tIDR,:);
        %temp2 = temp(femI.elocIDvec(tIDRi,:));
        tR(id+1:id+3,:) = repmat(temp,3,1);
        tIntfRID(id+1:id+3,:) = tIDR;
        id = id+3;

    elseif tIDR == 0 % Boundary Face
        tIDLi = -mesh.tLoc(tIDL); % intf elem index of left element

        %% Determine piece: only one element.
        nd1ID = mesh.pLoc(nd1); tLp = femI.plusPC(tIDLi);
        if (nd1ID < 0 && tLp == 1) || (nd1ID > 0 && tLp == 2)
            basB(idB+1,:,:) = femI.bas2(tIDLi,:,:);
            basB(idB+2,:,:) = femI.bas1(tIDLi,:,:);
            basB(idB+3,:,:) = femI.bas1(tIDLi,:,:);
        elseif (nd1ID < 0 && tLp == 2) || (nd1ID > 0 && tLp == 1)
            basB(idB+1,:,:) = femI.bas1(tIDLi,:,:);
            basB(idB+2,:,:) = femI.bas2(tIDLi,:,:);
            basB(idB+3,:,:) = femI.bas2(tIDLi,:,:);
        end

        gxB(idB+1:idB+3,:) = G(:,1:3);
        gyB(idB+1:idB+3,:) = G(:,4:6);
        gzB(idB+1:idB+3,:) = G(:,7:9);

        AB(idB+1:idB+3,:) = AT;
        normalB(idB+1:idB+3,:) = repmat(mesh.f_norm(fID,:),3,1);

        %% tB use locID, b/c index on interface cell is different
        temp = fem.t(tIDL,:);
        temp1 = temp(femI.locID(tIDLi,:));
        tB(idB+1:idB+3,:) = repmat(temp1,3,1);
        idB = idB+3;
    end
end

femIF = struct('tL',tL,'tR',tR,'tB',tB,'basL',basL,'basR',basR,'basB',basB, ...
    'gx',gx,'gy',gy,'gz',gz,'gxB',gxB,'gyB',gyB,'gzB',gzB,'area',A,'areaB',AB,...
    'gw',[1/3;1/3;1/3],'normal',normal,'normalB',normalB,'tIntfLID',tIntfLID,...
    'tIntfRID',tIntfRID);


function  [yout, nfe] = mmid1(y, dy, xs, htot, nstep, odefun, la, kb, kk0)
% mmid.m is the algoritm for a 'modified midpoint' step in the Bulirsch-Stoer method
% for solving ode's, see W Press ea.in "Numerical Recipes" (1994) p723. This version
% is for parallel processing, see odeintp.m and mmid.m
%
% inputs : at xs(1..np) the dependent variable vector y(1..nv,1..np) and its derivative dy(1..nv,1..np)
% are input, also the total step htot(1..np) to be made  and the number of substeps used (nstep),
% a client function for calculating the derivatives (odefun) with some additional
% parameters la, kb(1..np), kk0 that are used only by odefun.
%
% output : the estimated vector for the next step yout(1..nv, 1..np) and number of function evaluations (nfe)

% D Vangheluwe 8 mrt 2005, revised 2 may 2005
% remark 1 : we did not take nv = number of variables as input argument as this can also
%   be obtained from the length of vector y or dy.

nv = size(y,1);
nfe = 0;
h = htot/nstep;
ym = y;
hm = repmat(h, nv, 1);
yn = y + hm .* dy;
x = xs + h;
yout = feval(odefun, x, y, la, kb, kk0);
nfe = nfe + 1;
h2 = 2 * hm;
for i = 2: nstep
   swap = ym + h2 .* yout;
   ym = yn;
   yn = swap;
   x = x + h;
   yout = feval(odefun, x, yn, la, kb, kk0);
   nfe = nfe + 1;
end
yout = 0.5 * (ym + yn + hm .* yout);
return


function [node,elem,bdFlag,HB,tree] = bisect(node,elem,markedElem,bdFlag)
%% BISECT bisect a 2-D triangulation.
%
% [node,elem] = BISECT(node,elem,markedElem) refine the current
% triangulation by bisecting marked elements and minimal neighboring
% elements to get a conforming and shape regular triangulation. Newest
% vertex bisection is implemented. markedElem is a vector containing the
% indices of elements to be bisected. It could be a logical vector of
% length size(elem,1).
%
% [node,elem,bdFlag] = BISECT(node,elem,markedElem,bdFlag) returns the
% updated bdFlag after the bisection. It will be used for PDEs with mixed
% boundary conditions.
%
% [node,elem,bdFlag,HB,tree] = BISECT(node,elem,markedElem,bdFlag)
% returns HB and tree arrays.
%
% - HB(:,1:3) is a hierarchical basis structure for nodes, where
%   HB(:,1) is the global index of new added nodes, and HB(:,2:3) the
%   global indices of two parent nodes of new added nodes. HB is usful
%   for the interpolation between two grids; see also nodeinterpolate.
%
% - tree(:,1:3) stores the binary tree of the refinement. tree(:,1) is the
%   index of the parent element in the coarse mesh and tree(:,2:3) are two
%   children indices in the fine mesh. tree is useful for the interpolation
%   of elementwise function; see eleminterpolate.
%
% Example
%
%      node = [0,0; 1,0; 1,1; 0,1];
%      elem = [2,3,1; 4,1,3];
%      figure(1); subplot(1,3,1); showmesh(node,elem);
%      [node,elem] = bisect(node,elem,'all');
%      figure(1); subplot(1,3,2); showmesh(node,elem);
%      bdFlag = setboundary(node,elem,'Dirichlet','all','Neumann','y==1');
%      [node,elem,bdFlag] = bisect(node,elem,[1 4],bdFlag);
%      figure(1); subplot(1,3,3); showmesh(node,elem);
%
% See also bisect3, coarsen, coarsen3, nodeinterpolate, eleminterpolate.
%
% Reference page in Help browser
% ifem meshdoc
% ifem bisectdoc

% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Set up
HB = []; tree = [];
if ~exist('bdFlag','var'), bdFlag = []; end
if ~exist('markedElem','var'), markedElem = (1:size(elem,1))'; end
if isempty(markedElem), return; end
if strcmp(markedElem,'all'), markedElem = (1:size(elem,1))'; end
if islogical(markedElem), markedElem = find(markedElem); end

%% Construct auxiliary data structure
T = auxstructure(elem);
neighbor = T.neighbor; elem2edge = T.elem2edge; edge = T.edge;
clear T;
%[neighbor,elem2edge,edge] = auxstructurec(int32(elem));
N = size(node,1); NT = size(elem,1); NE = size(edge,1);

%% Add new nodes
isCutEdge = false(NE,1);
while sum(markedElem)>0
    isCutEdge(elem2edge(markedElem,1)) = true;
    refineNeighbor = neighbor(markedElem,1);
    markedElem = refineNeighbor(~isCutEdge(elem2edge(refineNeighbor,1)));
end
edge2newNode = zeros(NE,1,'uint32');
edge2newNode(isCutEdge) = N+1:N+sum(isCutEdge);
HB = zeros(sum(isCutEdge),3,'uint32');
HB(:,1) = edge2newNode(isCutEdge);
HB(:,[2 3]) = edge(isCutEdge,[1 2]);
node(HB(:,1),:) = (node(HB(:,2),:) + node(HB(:,3),:))/2;

%% Refine marked elements
Nb = 0; tree = zeros(3*NT,3,'uint32');
for k = 1:2
    t = find(edge2newNode(elem2edge(:,1))>0);
    newNT = length(t);
    if (newNT == 0), break; end
    L = t;
    R = NT+1:NT+newNT;
    p1 = elem(t,1); p2 = elem(t,2); p3 = elem(t,3);
    p4 = edge2newNode(elem2edge(t,1));
    elem(L,:) = [p4, p1, p2];
    elem(R,:) = [p4, p3, p1];
	if nargin==4 && ~isempty(bdFlag) % Refine boundary edges
   		bdFlag(R,[1 3]) = bdFlag(t,[2 1]);
   		bdFlag(L,[1 2]) = bdFlag(t,[3 1]);
        bdFlag(L,3) = 0;
    else
        bdFlag = [];
	end
    tree(Nb+1:Nb+newNT,1) = L;
    tree(Nb+1:Nb+newNT,2) = L;
    tree(Nb+1:Nb+newNT,3) = R;
    elem2edge(L,1) = elem2edge(t,3);
    elem2edge(R,1) = elem2edge(t,2);
    NT = NT + newNT;
    Nb = Nb + newNT;
end
tree = tree(1:Nb,:);


function m = marginal_family(engine, n, add_ev)
% MARGINAL_FAMILY Compute the marginal on i's family (loopy)
% m = marginal_family(engine, n, add_ev)

if nargin < 3, add_ev = 0; end

bnet = bnet_from_engine(engine);
ns = bnet.node_sizes;
ps = parents(bnet.dag, n);
dom = [ps n];
CPD = bnet.CPD{bnet.equiv_class(n)};

switch engine.msg_type
  case 'd',
   % The method is similar to the following HMM equation:
   % xi(i,j,t) = normalise( alpha(i,t) * transmat(i,j) * obsmat(j,t+1) * beta(j,t+1) )
   % where xi(i,j,t) = Pr(Q(t)=i, Q(t+1)=j | y(1:T))
   % beta == lambda, alpha == pi, alpha from each parent = pi msg
   % In general, if A,B are parents of C,
   % P(A,B,C) = P(C|A,B) pi_msg(A->C) pi_msg(B->C) lambda(C)
   % where lambda(C) = P(ev below and including C|C) = prod incoming lamba_msg(children->C)
   % and pi_msg(X->C) = P(X|ev above) etc

   T = dpot(dom, ns(dom), CPD_to_CPT(CPD));
   for j=1:length(ps)
     p = ps(j);
     pi_msg = dpot(p, ns(p), engine.msg{n}.pi_from_parent{j});
     T = multiply_by_pot(T, pi_msg);
   end
   lambda = dpot(n, ns(n), engine.msg{n}.lambda);
   T = multiply_by_pot(T, lambda);
   T = normalize_pot(T);
   m = pot_to_marginal(T);
   if ~add_ev
     m.T = shrink_obs_dims_in_table(m.T, dom, engine.evidence);
   end
 case 'g',
  if engine.disconnected_nodes_bitv(n)
    m.T = 1;
    m.domain = dom;
    if add_ev
      m = add_ev_to_dmarginal(m, engine.evidence, ns)
    end
    return;
  end

  [m, C, W] = gaussian_CPD_params_given_dps(CPD, dom, engine.evidence);
  cdom = myintersect(dom, bnet.cnodes);
  pot = linear_gaussian_to_cpot(m, C, W, dom, ns, cdom, engine.evidence);
  % linear_gaussian_to_cpot will set the effective size of observed nodes to 0,
  % so we need to do this explicitely for the messages, too,
  % so they are all the same size.
  obs_bitv = ~isemptycell(engine.evidence);
  ps = parents(engine.msg_dag, n);
  for j=1:length(ps)
    p = ps(j);
    msg = engine.msg{n}.pi_from_parent{j};
    if obs_bitv(p)
      pi_msg = mpot(p, 0);
    else
      pi_msg = mpot(p, ns(p), 0, msg.mu, msg.Sigma);
    end
    pot = multiply_by_pot(pot, mpot_to_cpot(pi_msg));
  end
  msg = engine.msg{n}.lambda;
  if obs_bitv(n)
    lambda = cpot(n, 0);
  else
    lambda = cpot(n, ns(n), 0, msg.info_state, msg.precision);
  end
  pot = multiply_by_pot(pot, lambda);
  m = pot_to_marginal(pot);
  if add_ev
    m = add_evidence_to_gmarginal(m, engine.evidence, bnet.node_sizes, bnet.cnodes);
  end
end


function [m,v,w]=gaussmixt(m1,v1,w1,m2,v2,w2)
%GAUSSMIXT Multiply two GMM pdfs
%
% Inputs: Input mixtures: k1,k2 mixtures, p dimensions
%
%   M(k1,p) = mixture means for mixture 1
%   V(k1,p) or V(p,p,k1) variances (diagonal or full)
%   W(k1,1) = mixture weights
%   M(k2,p) = mixture means for mixture 2
%   V(k2,p) or V(p,p,k2) variances (diagonal or full)
%   W(k2,1) = mixture weights
%
% Outputs:
%
%   M(k1*k2,p) = mixture means
%   V(k1*k2,p) or V(p,p,k1*k2) if p>1 and at least one input has full covariance matrix
%   W(k1*k2,1) = mixture weights
%
% See also: gaussmix, gaussmixg, gaussmixp, randvec

%      Copyright (C) Mike Brookes 2000-2012
%      Version: $Id: gaussmixt.m 5453 2014-11-19 13:10:51Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent r13 r21 r22 r31 r312 r112 r1223 r321 ch1h r122 r124
if isempty(r21)
    r13=[1 3];
    r21=[2 1];
    r22=[2 2];
    r31=[3 1];
    r112=[1 1 2];
    r122=[1 2 2];
    r124=[1 2 4];
    r312=[3 1 2];
    r321=[3 2 1];
    r1223=[1 2 2 3];
    ch1h=[-0.5; 1; -0.5];
end
[k1,p]=size(m1);
[k2,p2]=size(m2);
f1=ndims(v1)>2 || size(v1,1)>k1; % full covariance matrix is supplied
f2=ndims(v2)>2 || size(v2,1)>k2; % full covariance matrix is supplied
% ff=f1+2*f2;
if p~=p2
    error('mixtures must have the same vector dimension');
end
k=k1*k2;
j1=repmat((1:k1)',k2,1);
j2=reshape(repmat(1:k2,k1,1),k,1);
if p==1
    % display('1D vectors');
    p1=1./v1(:);
    p2=1./v2(:);
    v=1./(p1(j1)+p2(j2));
    s1=p1.*m1;
    s2=p2.*m2;
    m=v.*(s1(j1)+s2(j2));
    v12=v1(j1)+v2(j2);
    wx=-0.5*(m1(j1)-m2(j2)).^2./v12(:);
    wx=wx-max(wx); % normalize to avoid underflow
    w=w1(j1).*w2(j2).*exp(wx)./sqrt(v12(:));
    w=w/sum(w);
else
    if ~f1 && ~f2 % both diagonal covariances
        % display('both diagonal');
        p1=1./v1;
        p2=1./v2;
        v=1./(p1(j1,:)+p2(j2,:));
        s1=p1.*m1;
        s2=p2.*m2;
        m=v.*(s1(j1,:)+s2(j2,:));
        v12=v1(j1,:)+v2(j2,:);
        wx=-0.5*sum((m1(j1,:)-m2(j2,:)).^2./v12,2);
        wx=wx-max(wx); % normalize to avoid underflow
        w=w1(j1).*w2(j2).*exp(wx)./sqrt(prod(v12,2));
        w=w/sum(w);
    else % at least one full covariances
        m=zeros(k,p);
        v=zeros(p,p,k);
        w=zeros(k,1);
        wx=w;
        idp=1:p+1:p*p; % diagonal elements of p x p matrix
        if p==2                 % special code for 2D vectors
            if ~f2  % GMM 2 is diagonal
                % display('2D GMM 2 diagonal');
                p2=1./v2;
                pm2=p2.*m2;
                vx1=permute(v1,r312);
                vx1=vx1(:,r124);
                px1=vx1./repmat((vx1(:,1).*vx1(:,3)-vx1(:,2).^2),1,3); % [a b; b c] -> [c -b a]
                pm1=m1.*px1(:,r31)-m1(:,r21).*px1(:,r22);
                px=px1(j1,:);
                px(:,r31)=px(:,r31)+p2(j2,:);  % add onto diagonal elements
                vijx=vx1(j1,:);
                vijx(:,r13)=vijx(:,r13)+v2(j2,:);  % add onto diagonal elements
            elseif ~f1 % GMM 1 is diagonal
                % display('2D GMM 1 diagonal');
                p1=1./v1;
                pm1=p1.*m1;
                vx2=permute(v2,r312);
                vx2=vx2(:,r124);
                px2=vx2./repmat((vx2(:,1).*vx2(:,3)-vx2(:,2).^2),1,3); % [a b; b c] -> [c -b a]
                pm2=m2.*px2(:,r31)-m2(:,r21).*px2(:,r22);
                px=px2(j2,:);
                px(:,r31)=px(:,r31)+p1(j1,:);  % add onto diagonal elements
                vijx=vx2(j2,:);
                vijx(:,r13)=vijx(:,r13)+v1(j1,:);  % add onto diagonal elements
            else % both full covariances
                % display('2D both full');
                vx1=permute(v1,r312);
                vx1=vx1(:,r124); % make each 2 x 2 matrix into a row [a b; b c] -> [a b c]
                px1=vx1./repmat((vx1(:,1).*vx1(:,3)-vx1(:,2).^2),1,3); % [a b; b c] -> [c -b a]
                vx2=permute(v2,r312);
                vx2=vx2(:,r124);
                px2=vx2./repmat((vx2(:,1).*vx2(:,3)-vx2(:,2).^2),1,3); % [a b; b c] -> [c -b a]
                pm1=m1.*px1(:,r31)-m1(:,r21).*px1(:,r22);
                pm2=m2.*px2(:,r31)-m2(:,r21).*px2(:,r22);
                px=px1(j1,:)+px2(j2,:);
                vijx=vx1(j1,:)+vx2(j2,:);
            end
            vx=px./repmat((px(:,1).*px(:,3)-px(:,2).^2),1,3);   % divide by determinant to get inverse
            m=pm1(j1,:)+pm2(j2,:);
            m=m.*vx(:,r13)+m(:,r21).*vx(:,r22);                 % multiple by 2 x 2 matrix vx
            v=reshape(vx(:,r1223)',[2 2 k]);                    % convert vx to a 3D array of 2 x 2 matrices
            m12=m1(j1,:)-m2(j2,:);                              % subtract means to calculate weight exponent
            dij=vijx(:,1).*vijx(:,3)-vijx(:,2).^2;              % determinant of V1+V2
            wx=m12(:,r112).*m12(:,r122).*vijx(:,r321)*ch1h./dij;% exponent of weight
            w=w1(j1).*w2(j2)./sqrt(dij);                        % weight is w*exp(wx)
        else
            if ~f2  % GMM 2 is diagonal
                % display('GMM 2 diagonal');
                p2=1./v2;
                pm2=p2.*m2;
                for i=1:k1
                    v1i=v1(:,:,i);
                    p1i=inv(v1i);
                    m1i=m1(i,:);
                    pm1i=m1i*p1i;
                    w1i=w1(i);
                    ix=i;
                    for j=1:k2
                        pij=p1i;
                        pij(idp)=pij(idp)+p2(j,:);
                        vix=inv(pij);
                        vij=v1i;
                        vij(idp)=vij(idp)+v2(j,:);
                        v(:,:,ix)=vix;
                        m(ix,:)=(pm2(j,:)+pm1i)*vix;
                        m12=m2(j,:)-m1i;
                        wx(ix)=-0.5*m12/vij*m12';           % exponent of weight
                        w(ix)=w2(j)*w1i/sqrt(det(vij));     % weight is w*exp(wx)
                        ix=ix+k1;
                    end
                end
            elseif ~f1 % GMM 1 is diagonal
                % display('GMM 1 diagonal');
                p1=1./v1;
                pm1=p1.*m1;
                ix=1;
                for j=1:k2
                    v2j=v2(:,:,j);
                    p2j=inv(v2j);
                    m2j=m2(j,:);
                    pm2j=m2j*p2j;
                    w2j=w2(j);
                    for i=1:k1
                        pij=p2j;
                        pij(idp)=pij(idp)+p1(i,:);
                        vix=inv(pij);
                        vij=v2j;
                        vij(idp)=vij(idp)+v1(i,:);
                        v(:,:,ix)=vix;
                        m(ix,:)=(pm1(i,:)+pm2j)*vix;
                        m12=m1(i,:)-m2j;
                        wx(ix)=-0.5*m12/vij*m12';           % exponent of weight
                        w(ix)=w1(i)*w2j/sqrt(det(vij));     % weight is w*exp(wx)
                        ix=ix+1;
                    end
                end
            else % both full covariances
                % display('both full');
                p1=zeros(p,p,k1);
                pm1=zeros(k1,p);
                for i=1:k1
                    p1i=inv(v1(:,:,i));
                    p1(:,:,i)=p1i;
                    pm1(i,:)=m1(i,:)*p1i;
                end
                ix=1;
                for j=1:k2
                    v2j=v2(:,:,j);
                    p2j=inv(v2j);
                    m2j=m2(j,:);
                    pm2j=m2j*p2j;
                    w2j=w2(j);
                    for i=1:k1
                        pij=p1(:,:,i)+p2j;
                        vix=inv(pij);
                        v(:,:,ix)=vix;
                        vij=v1(:,:,i)+v2j;
                        m(ix,:)=(pm1(i,:)+pm2j)*vix;
                        m12=m1(i,:)-m2j;
                        wx(ix)=-0.5*m12/vij*m12';           % exponent of weight
                        w(ix)=w1(i)*w2j/sqrt(det(vij));     % weight is w*exp(wx)
                        ix=ix+1;
                    end
                end
            end

        end
        wx=wx-max(wx);              % adjust exponents to avoid underflow
        w=w.*exp(wx);               % calculate weights
        w=w/sum(w);                 % normalize weights to sum to unity
        if k==1
            v=reshape(v,size(v,1),size(v,2)); % squeeze last dimension of v if possible
        end
    end
end
if ~nargout
    if p==1
        nxx=256; % number of points to plot
        nsd=3; % number of std deviations
        sd=sqrt([v1(:);v2(:);v]);
        ma=[m1;m2;m];
        xax=linspace(min(ma-nsd*sd),max(ma+nsd*sd),nxx);
        plot(xax,gaussmixp(xax(:),m1,v1,w1),'--b');
        hold on
        plot(xax,gaussmixp(xax(:),m2,v2,w2),':r');
        plot(xax,gaussmixp(xax(:),m,v,w),'-k');
        hold off
        ylabel('Log probability density');
        legend('Mix 1','Mix 2','Product','location','best');
        axisenlarge([-1 -1 -1 -1.05]);
    elseif p==2
        nxx=128; % number of points to plot
        nsd=3;
        if f1
            s1=sqrt([v1(1:4:end)' v1(4:4:end)']); % extract diagonal elements only
        else
            s1=sqrt(v1);
        end
        if f2
            s2=sqrt([v2(1:4:end)' v2(4:4:end)']); % extract diagonal elements only
        else
            s2=sqrt(v2);
        end
        if ndims(v)>2 || size(v,1)>k
            s3=sqrt([v(1:4:end)' v(4:4:end)']); % extract diagonal elements only
        else
            s3=sqrt(v);
        end
        mal=[m1;m2;m];
        sal=[s1;s2;s3];
        xax=linspace(min(mal(:,1)-nsd*sal(:,1)),max(mal(:,1)+nsd*sal(:,1)),nxx);
        yax=linspace(min(mal(:,2)-nsd*sal(:,2)),max(mal(:,2)+nsd*sal(:,2)),nxx);
        xx(:,:,1)=repmat(xax',1,nxx);
        xx(:,:,2)=repmat(yax,nxx,1);
        xx=reshape(xx,nxx^2,2);
        subplot(2,2,1);
        imagesc(xax,yax,reshape(gaussmixp(xx,m1,v1,w1),nxx,nxx)');
        axis 'xy';
        title('Input Mix 1');
        subplot(2,2,2);
        imagesc(xax,yax,reshape(gaussmixp(xx,m2,v2,w2),nxx,nxx)');
        axis 'xy';
        title('Input Mix 2');
        subplot(2,2,3);
        imagesc(xax,yax,reshape(gaussmixp(xx,m,v,w),nxx,nxx)');
        axis 'xy';
        title('Product GMM');
    end
end


function [result_tab, error_code, nb_warm_start, z]=SparseLP_old(c,polyhedron, zeps, max_warm_start)

% Compute the sparsest solution of a linear program (LP)
% min c'x
% s.t.  Sx=b
%       lb <= x <= ub
%

 n=size(c,1);error_code=0;

% Compute the optimal value of the LP
% [x0, err_init]=LPSolve(c,Aeq,beq,Ain,bin,lb,ub);
 [x0, err_init]=LPSolve(c,polyhedron);
 err_init
if err_init~=1
 init_result=length(find(abs(x0)>zeps));
 optvalue=x0'*c;

% Heuristic to get 0 if optimal value is 0.
 if optvalue==0 && max(polyhedron.Aeq*zeros(n,1)-polyhedron.beq)==0 && max(polyhedron.Ain*zeros(n,1)-polyhedron.bin)<=0 && max(polyhedron.lb-zeros(n,1))<=0 && max(zeros(n,1)-polyhedron.ub)<=0
	z=zeros(n,1);result_tab=[0]; error_code=0; nb_warm_start=0;
 else

% Generate final polyhedron with optimal value as a constraint.
 polyhedron=struct('Aeq',[polyhedron.Aeq;c'],'beq',[polyhedron.beq;optvalue],'Ain',polyhedron.Ain,'bin',polyhedron.bin,'lb',polyhedron.lb,'ub',polyhedron.ub);

%#################################################################################
% MAIN : Algorithm call with warm start if we didn't improve l1-norm solution or if we have precision problem

 nb_warm_start=-1;tic;
 [x, err_l1]=L1Solve(polyhedron); l1_result=length(find(abs(x)>zeps));
 err_l1
 if err_l1~=1
	result_tab=[init_result, l1_result];
 else
	result_tab=[init_result];
 end

% Parameters
 nb_itemax_r=50; nb_itemax_sla=2; update_param=0.5;rstop=sqrt(eps); move=1;

 while err_l1~=1 && nb_warm_start<max_warm_start && init_result>0 && ( nb_warm_start==-1 || nnz_z<=min(result_tab)) && move

  [z, nnz_z, error_code] = ThetaL0(polyhedron, @theta1, @dtheta1, nb_itemax_r, rstop, update_param, nb_itemax_sla, zeps, x);
% Update
	nb_warm_start=nb_warm_start+1;
	if nnz_z<min(result_tab)
		result_tab=[result_tab, nnz_z];x=z;
	elseif z==x
		move=0;
	end

 end
 if err_l1==1
	z=x0;nnz_z=-1;error_code=1;
 end
%################################################################################
 toc
 end
else
 result_tab=[]; error_code=1;nb_warm_start=0;z=x0;
end
end

%################################################################################
%# Declaration of functions thetas and their derivatives :

function y = theta1(t,r)
	y=abs(t)./(abs(t)+r);
end

function y = dtheta1(t,r)
	y=sign(t).*r./((abs(t)+r).^2);
end

function y = theta2(t,r)
	y=1-exp(-abs(t)/r);
end

function y = dtheta2(t,r)
	y=sign(t).*exp(-abs(t)./r)./r;
end

%######################################################################################################################
%######################################################################################################################
function [x, nnz_x, error_code] = ThetaL0(polyhedron, theta_func, dtheta_func, nb_itemax_r, rstop, update_param, nb_itemax_sla, zero_precision, init_point)

% Citation :  A Smoothing Method for Sparse Optimization over Polyhedral Sets, Modelling, Computation and Optimization in Information Systems and Management Sciences Advances in Intelligent Systems and Computing Volume 359, 2015, pp 369-379   Tangi Migot, Mounir Haddou, 2015

% error_code :
% -2 : maximum iteration in r
% -1 : maximum iteration in SLA
% 0 : success
% 1 : unbounded
% 2 : precision

 error_code=0;
% code d'erreur SLA : (init in 1 : in case no SLA are done)
 err_code=1;

% for debug :
minx=[];rtab=[];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialization

% if no initial point we take the solution of l1-norm problem
 if isempty(init_point)
	init_point=L1Solve(polyhedron);
 end

 sol=init_point;
 nnz_current=length(find(abs(sol)>zero_precision));nnzero=[];soltab=[];minx=[min(sol(sol>0))];
% initialization of parameter r
 i=1;r0=1;rold=r0;r=rold;rtab=[r0];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%  Main Loop

 while i<nb_itemax_r && theta_func(min(sol(sol>zero_precision)),rold)<nnz_current/(nnz_current+1) && r>rstop
% Scaling to put variables in [0,1] box
	scal=max(abs(sol))/2;z=sol/scal;

	spolyhedron=struct('Aeq',polyhedron.Aeq,'beq',polyhedron.beq/scal,'Ain',polyhedron.Ain,'bin',polyhedron.bin/scal,'lb',polyhedron.lb/scal,'ub',polyhedron.ub/scal);
% Concave program
	[z, err_code] = ThetaSLA(spolyhedron, z,r, nb_itemax_sla, theta_func, dtheta_func);

	    if err_code==1
	     error_code=1;i=nb_itemax_r; % unbounded
	    else
	     	sol=z*scal;soltab=[soltab;sol'];minx=[minx;min(sol(sol>0))];rtab=[rtab,r];
	     	nnzk=length(find(abs(sol)>zero_precision));nnzero=[nnzero, nnzk];
% Update
	     	i=i+1;rold=r;r=r*update_param;
	    end
	    if err_code==-1
	     error_code=-1; % max iteration SLA
	    end

 end
%%%%% End Main Loop
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Best solution find :
if err_code ~=1
 nnz_x=min(nnzero);index=find(nnzero==nnz_x);x=soltab(index(1),:)';
else
 nnz_x=length(find(abs(sol)>zero_precision));x=init_point;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Error code :
 if i==nb_itemax_r && error_code~=1
	error_code=-2; % max iteration in r
 end

 if ( (~isempty(polyhedron.Aeq) && norm(polyhedron.Aeq*x-polyhedron.beq,Inf)>sqrt(eps)) || (~isempty(polyhedron.Ain) && max(polyhedron.Ain*x-polyhedron.bin)>sqrt(eps)) || (~isempty(polyhedron.lb) && max(polyhedron.lb-x)>sqrt(eps)) || (~isempty(polyhedron.ub) && max(x-polyhedron.ub)>sqrt(eps))) && error_code~=1
	error_code=2; % precision
 end

end
%######################################################################################################################
%
% Solve concave program of minimization Theta with Successiv Linearization Algorithm
%
%######################################################################################################################
function [x, error_code] =ThetaSLA(polyhedron,init_point,r_param,ite_max, theta_func, dtheta_func)

% err_code :
% -1 : maximum iteration
%  0 : success
%  1 : unbounded linear program
%  2 : precision
 error_code=0;

 j=1;y=init_point;zero_precision=sqrt(eps);

 while j<ite_max && (j==1 || abs(c'*yold-c'*x)>zero_precision)
	% Objectiv theta function with scaling :

%	c=dtheta_func(y,r_param)/(theta_func(1,r_param))
	c=dtheta_func(y,r_param)*r_param;

	[x,bool_LP]=LPSolve(c,polyhedron);
	% update :
	yold=y;y=x;j=j+1;

	if bool_LP==1
		error_code=1;j=ite_max;
	end
 end

 if error_code~=1
	 if j==ite_max
		error_code=-1;
	 end

	 if (~isempty(polyhedron.Aeq) && norm(polyhedron.Aeq*x-polyhedron.beq,Inf)>zero_precision) || (~isempty(polyhedron.Ain) && max(polyhedron.Ain*x-polyhedron.bin)>zero_precision) || (~isempty(polyhedron.lb) &&max(polyhedron.lb-x)>sqrt(eps)) || (~isempty(polyhedron.ub) &&max(x-polyhedron.ub)>sqrt(eps))
		error_code=2;
	 end
 end
end
%######################################################################################################################
%
% Solve convex program of minimization l1-norm
%
%######################################################################################################################
function [x,error_code]=L1Solve(polyhedron)
%{
% here we use SeDuMi from CVX Matlab package
 n=size(polyhedron.lb,1);error_code=0;
% min c'x
% s.t.  Aeq x == beq
%       Ain x <= bin
%       lb <= x <= ub

 cvx_begin quiet
     cvx_precision best
%     cvx_solver sdpt3
     variable x(n)
     minimize(norm(x,1))
     subject to
         polyhedron.Aeq*x == polyhedron.beq;
         polyhedron.Ain*x <= polyhedron.bin;
         x >= polyhedron.lb;
         x <= polyhedron.ub;
 cvx_end

% error 1 : unbounded
 if cvx_optval<=-Inf || cvx_optval>=Inf
	error_code=1;
 end

% error 2 : precision
 if norm(polyhedron.Aeq*x-polyhedron.beq,Inf)>sqrt(eps) || max(polyhedron.Ain*x-polyhedron.bin)>sqrt(eps) || max(polyhedron.lb-x)>sqrt(eps) || max(x-polyhedron.ub)>sqrt(eps)
	error_code=2;
 end
%}
%######################################################################################################################
% Solveur GUROBI

% solver with Gurobi (reformulation comme un LP)
if isempty(polyhedron.Aeq)
	%On a pas besoin des égalités
	AEQ=[];
else
	m1=size(polyhedron.beq,1);n=size(polyhedron.Aeq,2);AEQ=[polyhedron.Aeq, zeros(m1,n)];
end
if isempty(polyhedron.Ain)
	n=size(polyhedron.lb,1);
	AIN=[eye(n), -eye(n); -eye(n), -eye(n)];
	BIN=[zeros(n,1); zeros(n,1)];
else
	m2=size(polyhedron.bin,1);n=size(polyhedron.Ain,2);
	AIN=[polyhedron.Ain, zeros(m2,n); eye(n), -eye(n); -eye(n), -eye(n)];
	BIN=[polyhedron.bin;zeros(n,1); zeros(n,1)];
end
if isempty(polyhedron.lb)
	LB=[];
else
	n=size(polyhedron.lb,1);LB=[polyhedron.lb; zeros(n,1)];
end
if isempty(polyhedron.ub)
	UB=[];
else
	n=size(polyhedron.ub,1);temp=max(abs(polyhedron.lb),abs(polyhedron.ub));UB=[polyhedron.ub; temp];
end

polyhedron_l1=struct('Aeq',AEQ,'beq',polyhedron.beq,'Ain',AIN,'bin',BIN,'lb',LB,'ub',UB);
c=[zeros(n,1);ones(n,1)];

[x,error_code]=LPSolve(c,polyhedron_l1);
x=x(1:n);

end
%######################################################################################################################
%
% Solve linear Program
%
%######################################################################################################################
function [x,error_code]=LPSolve(c,polyhedron)
%{
% Solveur SEDUMI
 n=size(c,1);error_code=0;
% min c'x
% s.t.  Aeq x == beq
%       Ain x <= bin
%       lb <= x <= ub

 cvx_begin quiet
    cvx_precision best
    cvx_solver sedumi
    variable x(n)
    minimize(c'*x)
    subject to
	polyhedron.Aeq*x == polyhedron.beq;
	polyhedron.Ain*x <= polyhedron.bin;
	x >= polyhedron.lb;
	x <= polyhedron.ub;
 cvx_end

% error 1 : unbounded
 if cvx_optval<=-Inf || cvx_optval>=Inf
	error_code=1;
 end

% error 2 : precision
 if norm(polyhedron.Aeq*x-polyhedron.beq,Inf)>sqrt(eps) || max(polyhedron.Ain*x-polyhedron.bin)>sqrt(eps) || max(polyhedron.lb-x)>sqrt(eps) || max(x-polyhedron.ub)>sqrt(eps)
	error_code=2;
 end
%}
%######################################################################################################################
% Solveur GUROBI

[x, fval, exitflag] = linprog_gurobi(c, polyhedron.Ain, polyhedron.bin, polyhedron.Aeq, polyhedron.beq, polyhedron.lb, polyhedron.ub);

if exitflag==1 || exitflag==0
 error_code=0;
elseif exitflag==-2 || exitflag==-3
 error_code=1;
end

% error 2 : precision
 if (~isempty(polyhedron.Aeq) && norm(polyhedron.Aeq*x-polyhedron.beq,Inf)>sqrt(eps)) || (~isempty(polyhedron.Ain) && max(polyhedron.Ain*x-polyhedron.bin)>sqrt(eps)) || (~isempty(polyhedron.lb) &&max(polyhedron.lb-x)>sqrt(eps)) || (~isempty(polyhedron.ub) &&max(x-polyhedron.ub)>sqrt(eps))
	error_code=2;
 end

end
function [x, fval, exitflag] = linprog_gurobi(f, A, b, Aeq, beq, lb, ub)
%LINPROG A linear programming example using the Gurobi MATLAB interface
%
%   This example is based on the linprog interface defined in the
%   MATLAB Optimization Toolbox. The Optimization Toolbox
%   is a registered trademark of The MathWorks, Inc.
%
%   x = LINPROG(f,A,b) solves the linear programming problem:
%
%   minimize     f'*x
%   subject to   A*x <= b
%
%
%   x = LINPROG(f,A,b,Aeq,beq) solves the problem:
%
%   minimize     f'*x
%   subject to     A*x <= b,
%                Aeq*x == beq.
%
%   x = LINPROG(f,A,b,Aeq,beq,lb,ub) solves the problem:
%
%   minimize     f'*x
%   subject to     A*x <= b,
%                Aeq*x == beq,
%          lb <=     x <= ub.
%
%   You can set lb(j) = -inf, if x(j) has no lower bound,
%   and ub(j) = inf, if x(j) has no upper bound.
%
%   [x, fval] = LINPROG(f, A, b) returns the objective value
%   at the solution. That is, fval = f'*x.
%
%   [x, fval, exitflag] = LINPROG(f, A, b) returns an exitflag
%   containing the status of the optimization. The values for
%   exitflag and corresponding status codes are:
%      1 - OPTIMAL,
%      0 - ITERATION_LIMIT,
%     -2 - INFEASIBLE,
%     -3 - UNBOUNDED.
%

if nargin < 3
    error('linprog(f, A, b)')
end

if nargin > 7
    error('linprog(f, A, b, Aeq, beq, lb, ub)');
end

if ~isempty(A)
    n = size(A, 2);
elseif nargin > 4 && ~isempty(Aeq)
    n = size(Aeq, 2);
else
    error('No linear constraints specified')
end

if ~issparse(A)
    A = sparse(A);
end

if nargin > 3 && ~issparse(Aeq)
    Aeq = sparse(Aeq);
end

model.obj = f;

if nargin < 4
    model.A = A;
    model.rhs = b;
    model.sense = '<';
else
    model.A = [A; Aeq];
    model.rhs = [b; beq];
    model.sense = [repmat('<', size(A,1), 1); repmat('=', size(Aeq,1), 1)];
end

if nargin < 6
    model.lb = -inf(n,1);
else
    model.lb = lb;
end

if nargin == 7
   model.ub = ub;
end

params.outputflag = 0;
result = gurobi(model, params);

if strcmp(result.status, 'OPTIMAL')
    exitflag = 1;
elseif strcmp(result.status, 'ITERATION_LIMIT')
    exitflag = 0;
elseif strcmp(result.status, 'INF_OR_UNBD')
    params.dualreductions = 0;
    result = gurobi(model, params);
    if strcmp(result.status, 'INFEASIBLE')
        exitflag = -2;
    elseif strcmp(result.status, 'UNBOUNDED')
        exitflag = -3;
    else
        exitflag = nan;
    end
elseif strcmp(result.status, 'INFEASIBLE')
    exitflag = -2;
elseif strcmp(result.status, 'UNBOUNDED')
    exitflag = -3;
else
    exitflag = nan;
end

if isfield(result, 'x')
    x = result.x;
else
    x = nan(n,1);
end

if isfield(result, 'objval')
    fval = result.objval;
else
    fval = nan;
end
end


% This file is part of TREEQSM.
%
% TREEQSM 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.
%
% TREEQSM 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 TREEQSM.  If not, see <http://www.gnu.org/licenses/>.

function [R,dR1,dR2] = form_rotation_matrices(theta)

% --------------------------------------------------------------------------
% FORM_ROTATION_MATRICES.M      Forms rotation matrices R = R2*R1 and its
%                                   derivatives
%
% Input
% theta    Plane rotation angles (t1, t2)
%
% Output
% R        Rotation matrix
% R1       Plane rotation [1 0 0; 0 c1 -s1; 0 s1 c1]
% R2       Plane rotation [c2 0 s2; 0 1 0; -s2 0 c2]

c = cos(theta);
s = sin(theta);

R1 = [1 0 0; 0 c(1) -s(1); 0 s(1) c(1)];
R = R1;

R2 = [c(2) 0 s(2); 0 1 0; -s(2) 0 c(2)];
R = R2*R;

if nargout > 1
    dR1 = [0 0 0; 0 -R1(3,2) -R1(2,2); 0 R1(2,2) -R1(3,2)];
end

if nargout > 2
    dR2 = [-R2(1,3) 0 R2(1,1); 0 0 0; -R2(1,1) 0 -R2(1,3)];
end


%% PIVOT AND UNPIVOT: a user's guide
% The scope of these two functions is to organize a _flat_ dataset into a PIVOT table (and eventually undo it).
% This guide will illustrate how to use them.

%% CONCEPT: flat dataset
% A _flat_ dataset usually contains a column of values and "n" columns of features. So, each row
% hs its value with its features.
%
% *1. A _flat_ dataset with two features (Region and Year)*
load Demodata.mat
display([{'Year','Region','People'}; Demodata.ex1])
%% CONCEPT: pivot table
% A pivot table as intended by the T-SQL command _pivot_ or as implemented by spreadsheet softwares is a data
% visualization tool which groups a _flat_ dataset by two chosen dimension.
%
% *2. Format Demodata.ex1 into a _pivot_ table*
warning off warnPivot:funGroup      % we'll come back later on the warning
display(Pivot(Demodata.ex1))
%% PIVOT input constraints
% $$Pivot(In, Fun, noHeaders, Pad)$$
%
% Not every type of input is supported by PIVOT. Only a _flat_ dataset with max three columns is supported. The first
% column should be the grouping dimension which will appear as the column  header and the second column the dimension
% that will appear as the row header. The last column should contain the values that will be "intersected" and grouped.
%
% Here are listed all the constraints to the inputs (please mail me if something is left out):
%
    %%
    %
    % * IN should have 3 columns
    % * IN can be numeric or a cell array of scalars or strings if headers (no mixing)
    % * Nested cells are not allowed
    % * NaNs or empty cells in the first two columns of IN are not allowed
    % * If IN is a cell the column with the values must be a cell array of scalars
    % * FUN must be a function handle
    % * FUN applied to a vector must return a scalar

try Pivot([1,3;2,4]); catch; err = lasterror; display(err.message); end %#ok
try Pivot('try'); catch; err = lasterror; display(err.message); end %#ok
try Pivot({'1999',1,2;1999,1,2}); catch; err = lasterror; display(err.message); end %#ok
try Pivot({1,2,3;1,{2},3}); catch; err = lasterror; display(err.message); end %#ok
try Pivot({NaN,1,9;1,3,4}); catch; err = lasterror; display(err.message); end %#ok
try Pivot([1,NaN,9;1,3,4]); catch; err = lasterror; display(err.message); end %#ok
try Pivot({[],23,9;1,3,4}); catch; err = lasterror; display(err.message); end %#ok
try Pivot({23,[],9;1,3,4}); catch; err = lasterror; display(err.message); end %#ok
try Pivot({23,1,9;1,3,'9'}); catch; err = lasterror; display(err.message); end %#ok
try Pivot(Demodata.ex1(2:end,:),'sum'); catch; err = lasterror; display(err.message); end %#ok
try Pivot(Demodata.ex1(2:end,:),@diff); catch; err = lasterror; display(err.message); end %#ok
%% PIVOT examples
% How to use this function:
%
% *1. IN numeric or cellarray of scalars. No grouping involved. Padded by default with NaN*
disp(Pivot(Demodata.ex2))
disp(Pivot(num2cell(Demodata.ex2))) % OUT will be automatically converted to numeric
%%
% *2. Grouping involved using default FUN (@sum) or @min*
disp(Pivot(Demodata.ex3))
disp(Pivot(Demodata.ex3,@min))
%%
% *3. OUT without headers*
disp(Pivot(Demodata.ex3,[],true))
%%
% *4. Pad with zeros or with '#' and no headers (OUT will be cell).*
disp(Pivot(Demodata.ex3,[],false,0))
disp(Pivot(Demodata.ex3,[],1,'#'))
%%
% *5. The same syntaxes could be used with IN headers as cellarrays of strings*
disp(Pivot(Demodata.ex4))
disp(Pivot(Demodata.ex4,@max))
disp(Pivot(Demodata.ex4,@max,1))
disp(Pivot(Demodata.ex4,@max,1,'???'))
%%
% *6. Full output*
[Out,colHeader,rowHeader,Settings] = Pivot(Demodata.ex4,@max,1,'???');
disp([{'colHeader','rowHeader'};[colHeader,rowHeader]])
disp('    Settings')
disp(Settings)
%% PIVOT warning
% We set *warning off warnPivot:funGroup* before. This was meant to avoid the following message
disp(lastwarn)
    %%
    % being thrown each time PIVOT is applied to a dataset which has multiple values on the
    % same intersection between the two grouping dimensions. The warning is meant
    % to remember the user that some values are being grouped
%% UNPIVOT input constraints
% $$unPivot(In, dim, rmPad)$$
%
% Only _pivot_ tables are accepted as inputs. The first elements of IN should be NaN. The first row and the
% first column (except the IN(1) element) are the headers/features that will define the values in
% IN(2:end,2:end).
%
% Here are listed all the constraints to the inputs (please mail me if something is left out):
%
    %%
    %
    % * IN should be a _pivot_ table
    % * IN(1) must be NaN
    % * Nested cells are not allowed
    % * DIM can be 1 or 2 or empty
    % * RMPAD can be a char or a scalar
try unPivot([1,3]); catch; err = lasterror; display(err.message); end %#ok
try unPivot({'G1',1999;'G2',24}); catch; err = lasterror; display(err.message); end %#ok
try unPivot({NaN,1999;'G2',{24}}); catch; err = lasterror; display(err.message); end %#ok
try unPivot(Demodata.ex5,3); catch; err = lasterror; display(err.message); end %#ok
try unPivot(Demodata.ex5,'1'); catch; err = lasterror; display(err.message); end %#ok
try unPivot(Demodata.ex5,[],{23}); catch; err = lasterror; display(err.message); end %#ok
%% UNPIVOT examples
% How to use this function:
%
% *1. Simplest case*
    %%
    % IN numeric or cellarray of scalars. Removes by default the NaNs. Sorts by default
    % according the column headers (second column in the _unPivoted_ table)
A = unPivot(Demodata.ex5); disp(A)
B = unPivot(Demodata.ex5,[]);
C = unPivot(Demodata.ex5,1);
if isequal(A,B,C); fprintf('A, B and C are equivalent'); else error('something''s wrong'); end
%%
% *2. Sort by row header (first column in the _unPivoted_ table)*
disp(unPivot(Demodata.ex5,2))
%%
% *3. Remove padded value other than NaN*
disp(unPivot(Demodata.ex5,1,3174))
%%
% *4. Same syntaxes can be applied to a cell array. Empty cells removed by default.*
disp(unPivot(Demodata.ex6))
disp(unPivot(Demodata.ex6,2))
disp(unPivot(Demodata.ex6,[],20))
disp(unPivot(Demodata.ex6,2,'???'))
%% Performance issues
% Lets test the performances of PIVOT and UNPIVOT.
    %%
    % First we create our Test input (which isn't included in Demodata since its about 35 MB)
    Nums1 = ceil(rand(100,1)*100);
    Nums2 = num2str(ceil(rand(100,1)*100));
    Test = [cellstr(strcat('Year', num2str(Nums1+1900))),...
            cellstr(strcat('Group', Nums2)),...
            num2cell(Nums1)];
    Test = repmat(Test,1000,1);
    %%
    % Then using the profile...
    profile on
    Testpivoted = Pivot(Test,[],[],' ');
    Testback = unPivot(Testpivoted,1,' ');
    profile off
    stats = profile('info');
    stats = [{stats.FunctionTable.FunctionName}',{stats.FunctionTable.TotalTime}'];
    disp(stats([strmatch('Pivot',stats(:,1)); strmatch('unPivot',stats(:,1))],:))
    %%
    % While UNPIVOT is fast enough PIVOT is slowed down by cell2mat like behaviour.
    % Here is implemented cell2floatmod which is a compact version of the cell2float by Jos(10584)
    %%
    % Also notice that the Test that was originally created is different from the unpivoted
    % version of the same Test because of the grouping.
    fprintf('Are they equal: ''%d''', isequalwithequalnans(Test, Testback))
%% Other Pivot-like functions
% *Statistics Toolbox owners:*
%%
% pivoting could be accomplished on a dataset obj by the functions _stack_ and _unstack_.
% These functions use a similar engine approach to Pivot and unPivot and the performances
% lie on the same scale but the syntax is different.
%%
% *FEX submission 20963-reshape-a-matrix by Dimitri Shvorob (ID:17777):*
%%
% The functions are called _wide2tall_ and _tall2wide_ and are probably the foundations for the
% statistics TB functions because of the syntax and the names. Has drawbacks on performance.


function P_matrix = Initialize_LC_P_matrix(LC_KF_config)
%Initialize_LC_P_matrix - Initializes the loosely coupled INS/GNSS KF
%error covariance matrix
%
% Software for use with "Principles of GNSS, Inertial, and Multisensor
% Integrated Navigation Systems," Second Edition.
%
% This function created 12/4/2012 by Paul Groves
%
% Inputs:
%   TC_KF_config
%     .init_att_unc           Initial attitude uncertainty per axis (rad)
%     .init_vel_unc           Initial velocity uncertainty per axis (m/s)
%     .init_pos_unc           Initial position uncertainty per axis (m)
%     .init_b_a_unc           Initial accel. bias uncertainty (m/s^2)
%     .init_b_g_unc           Initial gyro. bias uncertainty (rad/s)
%
% Outputs:
%   P_matrix              state estimation error covariance matrix

% Copyright 2012, Paul Groves
% License: BSD; see license.txt for details

% Begins

% Initialize error covariance matrix
P_matrix =  zeros(15);
P_matrix(1:3,1:3) = eye(3) * LC_KF_config.init_att_unc^2;
P_matrix(4:6,4:6) = eye(3) * LC_KF_config.init_vel_unc^2;
P_matrix(7:9,7:9) = eye(3) * LC_KF_config.init_pos_unc^2;
P_matrix(10:12,10:12) = eye(3) * LC_KF_config.init_b_a_unc^2;
P_matrix(13:15,13:15) = eye(3) * LC_KF_config.init_b_g_unc^2;

% Ends


function [datafit,Amps,freqs,Fval,sig]=fitlinesc(data,params,p,plt,f0,fscanbw)
% fits significant sine waves to data (continuous data).
%
% Usage: [datafit,Amps,freqs,Fval,sig]=fitlinesc(data,params,p,plt,f0)
%
%  Inputs:
% Note that units of Fs, fpass have to be consistent.
%       data        (data in [N,C] i.e. time x channels/trials or a single
%       vector) - required.
%       params      structure containing parameters - params has the
%       following fields: tapers, Fs, fpass, pad
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                   (1) A numeric vector [TW K] where TW is the
%                       time-bandwidth product and K is the number of
%                       tapers to be used (less than or equal to
%                       2TW-1).
%                   (2) A numeric vector [W T p] where W is the
%                       bandwidth, T is the duration of the data and p
%                       is an integer such that 2TW-p tapers are used. In
%                       this form there is no default i.e. to specify
%                       the bandwidth, you have to specify T and p as
%                       well. Note that the units of W and T have to be
%                       consistent: if W is in Hz, T must be in seconds
%                       and vice versa. Note that these units must also
%                       be consistent with the units of params.Fs: W can
%                       be in Hz if and only if params.Fs is in Hz.
%                       The default is to use form 1 with TW=3 and K=5
%
%	        Fs 	        (sampling frequency) -- optional. Defaults to 1.
%               fpass       (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%	    p		    (P-value to calculate error bars for) - optional.
%                           Defaults to 0.05/N where N is data length.
%       plt         (y/n for plot and no plot respectively) - plots the
%       Fratio at all frequencies if y
%       f0          frequencies at which you want to remove the
%                   lines - if unspecified the program
%                   will compute the significant lines
%       fscanbw     bandwidth centered on ea. f0 to scan for significant
%                   lines (TM)
%
%  Outputs:
%       datafit        (linear superposition of fitted sine waves)
%       Amps           (amplitudes at significant frequencies)
%       freqs          (significant frequencies)
%       Fval           (Fstatistic at all frequencies)
%       sig            (significance level for F distribution p value of p)
data=change_row_to_column(data);
[N,C]=size(data);
if nargin < 2 || isempty(params); params=[]; end;
[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear pad fpass err trialave;
if nargin < 3 || isempty(p);p=0.05/N;end;
if nargin < 4 || isempty(plt); plt='n'; end;
if nargin < 5; f0=[]; end;
if nargin < 6; fscanbw = []; end

params.tapers=dpsschk(tapers,N,Fs); % calculate the tapers
[Fval,A,f,sig] = ftestc(data,params,p,plt);
if isempty(f0) && isempty(fscanbw)
    fmax=chron_findpeaks(Fval,sig);
    freqs=cell(1,C);
    Amps=cell(1,C);
    datafit=data;
    for ch=1:C;
        fsig=f(fmax(ch).loc);
        freqs{ch}=fsig;
        Amps{ch}=A(fmax(ch).loc,ch);
        Nf=length(fsig);
        %       fprintf('The significant lines for channel %d and the amplitudes are \n',ch);
        %        for nf=1:Nf;
        %            fprintf('%12.8f\n',fsig(nf));
        %            fprintf('%12.8f\n',real(A(fmax(ch).loc(nf),ch)));
        %            fprintf('%12.8f\n',imag(A(fmax(ch).loc(nf),ch)));
        %            fprintf('\n');
        %        end;
        datafit(:,ch)=exp(i*2*pi*(0:N-1)'*fsig/Fs)*A(fmax(ch).loc,ch)+exp(-i*2*pi*(0:N-1)'*fsig/Fs)*conj(A(fmax(ch).loc,ch));
    end;
else

    fsig = [];

    if ~isempty(fscanbw)

        % for each line f0(n), scan f0+-BW/2 for largest significant peak of Fval

        k = 1;
        for n=1:length(f0)
            % extract scan range around f0 ( f0 +- fscanbw/2 )
            [dummy ridx(1)] = min(abs(f-(f0(n)-fscanbw/2)));
            [dummy ridx(2)] = min(abs(f-(f0(n)+fscanbw/2)));

            Fvalscan = Fval(ridx(1):ridx(2));
            Fvalscan(Fvalscan<sig)=0;
            if any(Fvalscan)
                % if there's a significant line, pull the max one
                [dummy rmaxidx] = max(Fvalscan);
                indx(k) = ridx(1)+rmaxidx-1;
                fsig(k) = f(indx(k));
                k = k+1;
            end

        end
    else

        % remove exact lines

        indx = zeros( size(f0) );
        for n=1:length(f0);
            [fsig,indx(n)]=min(abs(f-f0(n)));
        end;
        fsig=f(indx);
    end

    if isempty(fsig)
        % no significant lines
        datafit = zeros(N,C);
        Amps  = cell(1,C);
        freqs = cell(1,C);
    else
        for ch=1:C;
            freqs{ch}=fsig;
            Amps{ch}=A(indx,ch);
            Nf=length(fsig);
            %        fprintf('For channel %d the amplitudes and the Fstatistic at f=%f are \n',ch,f0);
            %        fprintf('Fstatistic = %12.8f Fthreshold = %12.8f\n',Fval(indx),sig);
            %        fprintf('Real part of amplitude = %12.8f\n',real(A(indx,ch)));
            %        fprintf('Imaginary part of amplitude = %12.8f\n',imag(A(indx,ch)));
            datafit(:,ch)=exp(i*2*pi*(0:N-1)'*fsig/Fs)*A(indx,ch)+exp(-i*2*pi*(0:N-1)'*fsig/Fs)*conj(A(indx,ch));
        end;
    end
end;


classdef Kernel
    % Kernel
    % Represents the kernel G(x)
    properties (SetAccess = protected, GetAccess = public)
        func, % anonymous function representing G(x)
        der, % anonymous function representing G'(x)
        scalFunc, % anonymous function,
        % scalFunc(a,b,rho) =2 \pi C(rho) \int_{a}^b rG'(r) J_0(\rho r)
        % where C(rho) is the normalization constant, si Cp.m in folder
        % radialQuad.
        normFunc,  % anonymous function,
        % normFunc(a,b) = \sqrt{2\pi \int_{a}^b r G'(r)^2 dr}
        startFreq = 0, % real number
        % frequency where the greatest Bessel coefficients are located.
        gamma_est, % Estimator used to predict the number of coefficients in
        % the Bessel series
        customRadialQuad = [],
        % If non-empty, contains the method to compute the radial
        % quadrature optimized for this kernel.
        singular = true;
        % Set to false if x G'(x)^2 is integrable near 0.
        lim0;
    end

    methods
        % Class constructor
        function[kernel] = Kernel(func,der)
            if nargin == 0
                func = @(x)(0*x);
                der = @(x)(0*x);
            end
            kernel.func = func;
            kernel.der = der;
            fun1 = @(rho)(@(x)((x.*der(x)).*...
                -rho(:)'.*Cp(rho(:)').*besselj(1,rho(:)'*x)));
            % H10 scalar product
            fun2 = @(x)(x.*der(x).^2);
            % H10 norm
            kernel.scalFunc = @(a,b,rho)(2*pi*integral(fun1(rho),...
                a,b,'ArrayValued',true)'); % -2\pi \int_{a}^b rf'(r)ep'(r)dr
            kernel.normFunc = @(a,b)(sqrt(2*pi*integral(fun2,a,b)));
            kernel.startFreq = 0; % We don't know a priori where the energy
            % is located in the spectrum.
            kernel.gamma_est = @(tol)deal(0,7); % No fine tuning of gamma
            kernel.customRadialQuad = [];
            if ~ or(isinf(func(0)),isnan(func(0)))
                kernel.singular = false;
                kernel.gamma_est = @(tol)deal(0,3); % No fine tuning of gamma
                kernel.lim0 = func(0);
            else
                kernel.lim0 = 0;
            end
        end
    end

    methods (Access = public)

        % Efficient Bessel Decomposition for the kernel.
        function[onlineEBD,rq,loc] = offlineEBD(this,X,Y,a,tol)

            % Rescaling
            rMax = rMaxCalc(X,Y); % diameter
            x = X/rMax; % in B(0,1)
            y = Y/rMax; % in B(0,1)
            k1 = this.dilatation(rMax); % k1 must be approximated on {a < r < 1}

            % Radial quadrature (Bessel decomposition in 1D)
            rq = k1.radialQuadKernel(a,tol);

            % Circular quadrature
            q2d = Quad2D(rq);

            % Local correction
            loc = localCorrections(x,y,a,k1,rq,tol,false);
            % We are ready to compute fast convolutions.
            onlineEBD = @(v)(q2d.conv(x,y,v) + loc*v);
        end
        function[MVx,MVy,rq,locx,locy] = offline_dEBD(this,X,Y,a,tol)

            % Rescaling
            rMax = rMaxCalc(X,Y); % diameter
            x = X/rMax; % in B(0,1)
            y = Y/rMax; % in B(0,1)
            k1 = this.dilatation(rMax); % k1 must be approximated on {a < r < 1}
            rq = k1.radialQuadKernel(a,tol*rMax,'grad',true);
            % the tolerance is set to tol*rMax because G'(rMax*u) = 1/rMax
            % k1'(u).
            q2d = Quad2D(rq);
            [q2dx,q2dy] = grad(q2d);
            loc = localCorrections(x,y,a,k1,rq,tol,true);
            locx = loc{1}; locy = loc{2};
            MVx = @(v)(-(q2dx.conv(x,y,v))/rMax + locx*v/rMax);
            MVy = @(v)(-(q2dy.conv(x,y,v))/rMax + locy*v/rMax);
        end

        % Display
        function[] = disp(this)
            fprintf('Kernel : function %s \n',func2str(this.func));
        end

        function[out] = eval(this,x)
            fun = this.func;
            out = fun(x);
            out(abs(x) < 1e-12) = this.lim0;
        end
        function[out] = evalDer(this,x)
            fun = this.der;
            out = fun(x);
            out(abs(x) < 1e-12) = 0;
        end
        % Setters
        function[this] = setScalFunc(this,f)
            this.scalFunc = f;
        end
        function[this] = setNormFunc(this,g)
            this.normFunc = g;
        end
        function[this] = setStartFreq(this,k)
            this.startFreq = k;
        end
        % Addition of two kernels
        function[C] = plus(k1,k2)
            C = Kernel(@(x)(k1.func(x) + k2.func(x)),@(x)(k1.der(x) + k2.der(x)));
            sf1 = k1.scalFunc;
            sf2 = k2.scalFunc;
            C.scalFunc =  @(a,b,rho)(sf1(a,b,rho) + sf2(a,b,rho));
            C.customRadialQuad = @(a,tol,varargin)(k1.radialQuadKernel(a,tol/2,varargin{:}) ...
                + k2.radialQuadKernel(a,tol/2,varargin{:}));
            C.singular = or(k1.singular,k2.singular);
            C.lim0 = k1.lim0 + k2.lim0;
        end
        % Multiplication by a constant
        function[c] = mtimes(lambda,this)
            if and(isa(lambda,'double'),isscalar(lambda))
                assert(isa(this,'Kernel'));
                c = this;
                c.func = @(x)(lambda*this.func(x));
                c.der = @(x)(lambda*this.der(x));
                c.scalFunc = @(a,b,rho)(lambda*this.scalFunc(a,b,rho));
                c.normFunc = @(a,b)(abs(lambda)*this.normFunc(a,b));
                c.lim0 = lambda*this.lim0;
                c.customRadialQuad = @(a,tol,varargin)(lambda*this.radialQuadKernel(a,tol/lambda,varargin{:}));
            else
                assert(and(isa(this,'double'),isscalar(this)))
                c = times(this,lambda);
            end
        end
    end
    methods (Access = public)
        % Wrapper
        function[rq] = radialQuadKernel(this,a,tol,varargin)
            if ~ isempty(this.customRadialQuad)
                rq = this.customRadialQuad(a,tol,varargin{:});
            else
                rq = RadialQuadrature(a,this,tol,varargin{:});
            end
        end
        % Dilatation of the kernel : G(x) -> G(R*x).
        function[this] = dilatation(old,lambda)
            oldFunc = old.func;
            oldDer = old.der;
            ffunc = @(x)(oldFunc(lambda*x));
            dder = @(x)(lambda*oldDer(lambda*x));
            this = Kernel(ffunc,dder);
            this = this.setStartFreq(lambda*old.startFreq);
            this.lim0 = old.lim0;
        end

    end
end


function [strikb,dipb,rakeb,dipdib,ierr] = focal_pl2pl(strika,dipa,rakea)

    % compute strike, dip and rake of a nodal plane from strike, dip and rake of the other one
    %
    %
    %     usage:
    %     call pl2pl(strika,dipa,rakea,strikb,dipb,rakeb,dipdib,ierr)
    %
    %     arguments:
    %     strika         strike angle in degrees of the first nodal plane (INPUT)
    %     dipa           dip angle in degrees of the first nodal plane (INPUT)
    %     rakea          rake angle in degrees of the first nodal plane (INPUT)
    %     strikb         strike angle in degrees of the second nodal plane (OUTPUT)
    %     dipb           dip angle in degrees of the second nodal plane (OUTPUT)
    %     rakeb          rake angle in degrees of the second nodal plane (OUTPUT)
    %     dipdib         dip direction in degrees of the second nodal plane (OUTPUT)
    %     ierr           error indicator (OUTPUT)
    %
    %     errors:
    %     1              input STRIKE angle out of range
    %     2              input DIP angle out of range
    %     4              input RAKE angle out of range
    %     3              1+2
    %     5              1+4
    %     7              1+2+4
    %     8              internal error
    %
    %      implicit none
    %-------------------------------------------------------------------------------
    %%       integer io
    %%       real amistr,amastr,amidip,amadip,amirak,amarak,amitre,amatre
    %%      1,amiplu,amaplu,orttol,ovrtol,tentol,dtor,c360,c90,c0,c1,c2,c3
    %%       common /fpscom/amistr,amastr,amidip,amadip,amirak,amarak,amitre
    %%      1,amatre,amiplu,amaplu,orttol,ovrtol,tentol,dtor,c360,c90,c0,c1,c2
    %%      2,c3,io
    %-------------------------------------------------------------------------------
    %%       real strika,dipa,rakea,anx,any,anz,dx,dy,dz,strikb,dipb,rakeb,
    %%      1dipdib
    %%       integer ierr
    %
    %%      call fpsset
    amistr=-360.;
    amastr=360.;
    amidip=0.;
    amadip=90.;
    amirak=-360.;
    amarak=360.;
    amitre=-360.;
    amatre=360.;
    amiplu=0.;
    amaplu=90.;
    orttol=2.;
    ovrtol=0.001;
    tentol=0.0001;
    dtor=0.017453292519943296;
    c360=360.;
    c90=90.;
    c0=0.;
    c1=1.;
    c2=2.;
    c3=3.;
    io=6;
    ifl=1;

    [anx,any,anz,dx,dy,dz,ierr] = focal_pl2nd(strika,dipa,rakea);
    if (ierr ~= 0)
        disp(['PL2PL: ierr = ' num2str(ierr)]);
        return;
    end
    [strikb,dipb,rakeb,dipdib,ierr] = focal_nd2pl(dx,dy,dz,anx,any,anz);
    if (ierr ~= 0)
        ierr = 8;
        disp(['PL2PL: ierr = ' num2str(ierr)]);
    end
end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal, Marc Bakry (c) 2017-2019.                 |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             marc.bakry@polytechnique.edu                               |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtHmxVibroSlab2d.m                           |
%|    #    |   VERSION    : 0.55                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Marc Bakry                  |
%|  ( # )  |   CREATION   : 14.03.2019                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   :                                               |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Accuracy
tol = 1e-3

% Width of the slab
e = 0.5

% Frequency
f = 900:500:4000

% Incident direction (from bottom)
X0  = [0 1 0];

% Exterior domain (water)
rho0 = 1000;         % density      (kg.m3)
c0   = 1500;         % celerity     (m.s-1)
k0   = 2*pi/c0.*f;   % wave-number  (m-1)
lam0 = c0./f;        % wave-length  (m)

% Interior domain (different from water)
rhoS = 2*rho0;                                 % density                             (kg.m3)
cL   = 2*c0;                                   % celerity of longitudinal waves      (m.s-1)
kL   = 2*pi.*f./cL;                            % wave-number of longitudinal waves   (m-1)
lamL = real(cL)./f;                            % wavelength of longitudinal waves    (m)
cT   = 0;                                      % celerity of transverse waves        (m.s-1)
kT   = 2*pi.*f./cT;                            % wave-number of transverse waves     (m-1)
lamT = real(cT)./f;                            % wavelength of transverse waves      (m)

% Solution (pressure)
sol = zeros(2,length(f));

% Loop for each frequency
for i = 1:length(f)
    % Minimum wavelength
    tmp  = [lam0(i),lamL(i),lamT(i)];
    lmin = min(tmp(tmp>0));

    % Slab mesh
    L    = 60 * lmin;            % 60 wavelength to simulate infinite slab
    nx   = ceil(L/lmin * 6)+1;   % 6 node per wavelength for L
    ny   = ceil(e/lmin * 12)+1;  % 12 node per wavelength for e
    N    = nx * ny;              % Total number of nodes
    mesh = mshSquare(N,[L e])

    % Radiative mesh (fixed number of nodes)
    radiat = mshSquare(1e3,[L L]);

    % Boundary
    bound = swap(mesh.bnd)

    % Measurement points for trans and refl coeff (1 wavelenth from bound)
    Xmes = [0 -e/2-lmin 0 ; 0 e/2+lmin 0];

    % Cut-off function (50% full, 10% decrease)
    cutoff = vibsCutoff(1,L/5,L/10);

    % Green kernel function
    Gxy         = @(X,Y) femGreenKernel(X,Y,'[H0(kr)]',k0(i));
    gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]1',k0(i));
    gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]2',k0(i));
    gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]3',k0(i));

    % Plane wave function
    PW         = @(X) exp(1i*k0(i)*X*X0') .* cutoff(X);
    gradxPW{1} = @(X) 1i*k0(i)*X0(1) .* PW(X);
    gradxPW{2} = @(X) 1i*k0(i)*X0(2) .* PW(X);
    gradxPW{3} = @(X) 1i*k0(i)*X0(3) .* PW(X);

    % Coupling coeff for Brackage-Werner simulation
    beta = 1i*k0(i);

    % Quadrature and finite elements (volumn)
    omega = dom(mesh,3);
    U     = fem(mesh,'P1');

    % Quadrature and finite elements (boundary)
    sigma = dom(bound,3);
    u     = fem(bound,'P1');

    % Left-hand side
    tic
    [A,B,C,D] = vibsHmxBlockOperator(omega,U,sigma,u,cL,cT,rhoS,c0,rho0,f(i),tol);
    toc

    % Add dirichlet condition to x unknows (penalization)
    A(sub2ind(size(A),1:length(U),1:length(U))) = 1e15;

    % Right-hand side
    V    = cell(3,1);
    V{1} = - integral(sigma,ntimes(U,1),PW);
    V{2} = - integral(sigma,ntimes(U,2),PW);
    V{3} = integral(sigma,ntimes(u),gradxPW);

    % Resolution with Schur complement
    Fa     = decomposition(A);
    LHS    = @(V) D*V - C*(Fa \ (B.Ml*(B.Mr*V)) );
    RHS    = V{end} - C*(Fa \ cell2mat(V(1:end-1)) );
    mu     = mgcr(LHS,RHS,[],tol,100);
    lambda = beta*mu;

    % Measure of refexive and transmitted coeff
    tic
    Pmes = 1i/4 .* integral(Xmes,sigma,Gxy,u)*lambda - ...
        1i/4 .* integral(Xmes,sigma,gradyGxy,ntimes(u))*mu;
    Pmes(2) = Pmes(2) + PW(Xmes(2,:));
    toc

    % Save solution
    sol(:,i) = Pmes;
end

% Analytical solution
tic
c     = ones(length(f),1) * [c0 cL c0];
rho   = [rho0 rhoS rho0];
[R,T] = slabVibro(f,rho,c,e);
toc

% Comparison (db)
ref = 20*log10(abs([R ; T]));
sol = 20*log10(abs(sol));

norm(ref-sol)/norm(ref)

% Graphical representation
figure(100)
subplot(1,2,1)
plot(f,ref(1,:),'r',f,sol(1,:),'b+')
grid on
title('Reflexion coeffiscient')
legend({'Analytical','Numerical'})
xlabel('Frequency (Hz)')
ylabel('Amplitude (dB)')

subplot(1,2,2)
plot(f,ref(2,:),'r',f,sol(2,:),'b+')
grid on
title('Transmission coeffiscient')
legend({'Analytical','Numerical'})
xlabel('Frequency (Hz)')
ylabel('Amplitude (dB)')

disp('~~> Michto gypsilab !')