function x = Advanced_SC_decoder(llr, frozen_bits, node_type_matrix, lambda_offset_)
%Theoretically, this decoder has much lower latency compared with
%traditional SC decoder because 9 kinds of specific constituent nodes are
%identified and decoded with alternative methods instead of successive
%cancellation. However, due to complex logical that is employed to
%distingguish above nodes, the decoding speed is even slightly lower than traditional SC in terms of MATLAB implementation.
N = length(llr);
m = log2(N);
C = zeros(2 * N - 1, 2);
P = zeros(2 * N - 1, 1);
P(end - N + 1 : end) = llr;
node_type_matrix_cnt = 1;
phi = 0;
while(phi < N)
    if (phi + 1) == node_type_matrix(node_type_matrix_cnt, 1)
        switch node_type_matrix(node_type_matrix_cnt, 3)
            case -1%RATE 0
                M = node_type_matrix(node_type_matrix_cnt, 2);
                reduced_layer = log2(M); %This reduced layer denotes where to stop.
                psi = phi;%This psi leads you to a known (higher level with already obtained LLRs) layer.
                psi_ = phi;%This psi_ is used for bits recursion
                for i = 1 : reduced_layer
                    psi_ = floor(psi_/2);
                end
                layer = 0;%This layer denotes where to start.

                if phi ~= 0
                    while(mod(psi, 2) == 0)
                        psi = floor(psi/2);
                        layer = layer + 1;
                    end
                end

                if phi == 0
                    for i_layer = m - 1 : -1 : reduced_layer%NOT FULL LAYER = 0
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        for beta = 0 : index_1 - 1
                            P(beta + index_1) = sign(P(2*beta + index_2)) *  sign(P(2*beta + 1 + index_2)) * ...
                                min(abs(P(2*beta + index_2)), abs(P(2*beta + 1 + index_2)));
                        end
                    end
                else
                    for i_layer = layer: -1 : reduced_layer
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        if i_layer == layer
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = (1 - 2*C(beta + index_1, 1)) * P(2 * beta + index_2) + P(2 * beta + 1 + index_2);
                            end
                        else
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = sign(P(2 * beta + index_2)) *  sign(P(2 * beta + 1 + index_2)) * ...
                                    min(abs(P(2 * beta + index_2)), abs(P(2 * beta + 1 + index_2)));
                            end
                        end
                    end
                end

                if mod(psi_, 2) == 0
                    C(M : 2 * M - 1, 1) = zeros(M, 1);
                else
                    C(M : 2 * M - 1, 2) = zeros(M, 1);
                    while(mod(psi_, 2) == 1)
                        psi_ = floor(psi_/2);
                        index_1 = lambda_offset_(reduced_layer + 1);
                        index_2 = lambda_offset_(reduced_layer + 2);
                        for beta = 0 : index_1 - 1
                            C(2 * beta + index_2, 1 + mod(psi_, 2)) = mod(C(beta + index_1, 1) + C(beta + index_1, 2), 2);
                            C(2 * beta + 1 + index_2, 1 + mod(psi_, 2)) = C(beta + index_1, 2);
                        end
                        reduced_layer = reduced_layer + 1;
                    end
                end
                phi = phi + M;

                %
                %                 M = node_type_matrix(node_type_matrix_cnt, 2);
                %                 reduced_layer = log2(M);
                %                 psi = phi;
                %                 for i = 1 : reduced_layer
                %                     psi = floor(psi/2);
                %                 end
                %                 P = recursive_calc_llr(m - reduced_layer, psi, P, C(: , 1), m, lambda_offset);
                %
                %                 if mod(psi, 2) == 0
                %                     C(M : 2 * M - 1, 1) = zeros(M, 1);
                %                 else
                %                     C(M : 2 * M - 1, 2) = zeros(M, 1);
                %                     [C(:, 1), C(:, 2)] = recursive_calc_bits(m - reduced_layer, psi, C(:, 1), C(:, 2), m, lambda_offset);
                %                 end
                %                 phi = phi + M;
            case 1%RATE 1
                M = node_type_matrix(node_type_matrix_cnt, 2);
                reduced_layer = log2(M); %This reduced layer denotes where to stop.
                psi = phi;
                psi_ = phi;%This psi_ is used for bits recursion
                for i = 1 : reduced_layer
                    psi_ = floor(psi_/2);
                end
                layer = 0;%This layer denotes where to start.

                if phi ~= 0
                    while(mod(psi, 2) == 0)
                        psi = floor(psi/2);
                        layer = layer + 1;
                    end
                end

                if phi == 0
                    for i_layer = m - 1 : -1 : reduced_layer%NOT FULL LAYER = 0
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        for beta = 0 : index_1 - 1
                            P(beta + index_1) = sign(P(2*beta + index_2)) *  sign(P(2*beta + 1 + index_2)) * ...
                                min(abs(P(2*beta + index_2)), abs(P(2*beta + 1 + index_2)));
                        end
                    end
                else
                    for i_layer = layer: -1 : reduced_layer
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        if i_layer == layer
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = (1 - 2*C(beta + index_1, 1)) * P(2 * beta + index_2) + P(2 * beta + 1 + index_2);
                            end
                        else
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = sign(P(2 * beta + index_2)) *  sign(P(2 * beta + 1 + index_2)) * ...
                                    min(abs(P(2 * beta + index_2)), abs(P(2 * beta + 1 + index_2)));
                            end
                        end
                    end
                end
                x_r1 = P(M : 2 * M - 1) < 0;
                if mod(psi_, 2) == 0
                    C(M : 2 * M - 1, 1) = x_r1;
                else
                    C(M : 2 * M - 1, 2) = x_r1;
                    while(mod(psi_, 2) == 1)
                        psi_ = floor(psi_/2);
                        index_1 = lambda_offset_(reduced_layer + 1);
                        index_2 = lambda_offset_(reduced_layer + 2);
                        for beta = 0 : index_1 - 1
                            C(2 * beta + index_2, 1 + mod(psi_, 2)) = mod(C(beta + index_1, 1) + C(beta + index_1, 2), 2);
                            C(2 * beta + 1 + index_2, 1 + mod(psi_, 2)) = C(beta + index_1, 2);
                        end
                        reduced_layer = reduced_layer + 1;
                    end
                end
                phi = phi + M;

                %                 M = node_type_matrix(node_type_matrix_cnt, 2);
                %                 reduced_layer = log2(M);
                %                 psi = phi;
                %                 for i = 1 : reduced_layer
                %                     psi = floor(psi/2);
                %                 end
                %                 P = recursive_calc_llr(m - reduced_layer, psi, P, C(: , 1), m, lambda_offset);
                %                 x_r1 = P(M : 2 * M - 1) < 0;
                %                 if mod(psi, 2) == 0
                %                     C(M : 2 * M - 1, 1) = x_r1;
                %                 else
                %                     C(M : 2 * M - 1, 2) = x_r1;
                %                     [C(:, 1), C(:, 2)] = recursive_calc_bits(m - reduced_layer, psi, C(:, 1), C(:, 2), m, lambda_offset);
                %                 end
                %                 phi = phi + M;
            case 2%REP
                M = node_type_matrix(node_type_matrix_cnt, 2);
                reduced_layer = log2(M); %This reduced layer denotes where to stop.
                psi = phi;
                psi_ = phi;%This psi_ is used for bits recursion
                for i = 1 : reduced_layer
                    psi_ = floor(psi_/2);
                end
                layer = 0;%This layer denotes where to start.

                if phi ~= 0
                    while(mod(psi, 2) == 0)
                        psi = floor(psi/2);
                        layer = layer + 1;
                    end
                end

                if phi == 0
                    for i_layer = m - 1 : -1 : reduced_layer%NOT FULL LAYER = 0
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        for beta = 0 : index_1 - 1
                            P(beta + index_1) = sign(P(2*beta + index_2)) *  sign(P(2*beta + 1 + index_2)) * ...
                                min(abs(P(2*beta + index_2)), abs(P(2*beta + 1 + index_2)));
                        end
                    end
                else
                    for i_layer = layer: -1 : reduced_layer
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        if i_layer == layer
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = (1 - 2*C(beta + index_1, 1)) * P(2 * beta + index_2) + P(2 * beta + 1 + index_2);
                            end
                        else
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = sign(P(2 * beta + index_2)) *  sign(P(2 * beta + 1 + index_2)) * ...
                                    min(abs(P(2 * beta + index_2)), abs(P(2 * beta + 1 + index_2)));
                            end
                        end
                    end
                end
                x_rep = (sum(P(M : 2 * M - 1)) < 0) * ones(M , 1);
                if mod(psi_, 2) == 0
                    C(M : 2 * M - 1, 1) = x_rep;
                else
                    C(M : 2 * M - 1, 2) = x_rep;
                    while(mod(psi_, 2) == 1)
                        psi_ = floor(psi_/2);
                        index_1 = lambda_offset_(reduced_layer + 1);
                        index_2 = lambda_offset_(reduced_layer + 2);
                        for beta = 0 : index_1 - 1
                            C(2 * beta + index_2, 1 + mod(psi_, 2)) = mod(C(beta + index_1, 1) + C(beta + index_1, 2), 2);
                            C(2 * beta + 1 + index_2, 1 + mod(psi_, 2)) = C(beta + index_1, 2);
                        end
                        reduced_layer = reduced_layer + 1;
                    end
                end
                phi = phi + M;

                %                 M = node_type_matrix(node_type_matrix_cnt, 2);
                %                 reduced_layer = log2(M);
                %                 psi = phi;
                %                 for i = 1 : reduced_layer
                %                     psi = floor(psi/2);
                %                 end
                %                 P = recursive_calc_llr(m - reduced_layer, psi, P, C(: , 1), m, lambda_offset);
                %                 x_rep = (sum(P(M : 2 * M - 1)) < 0) * ones(M , 1);
                %                 if mod(psi, 2) == 0
                %                     C(M : 2 * M - 1, 1) = x_rep;
                %                 else
                %                     C(M : 2 * M - 1, 2) = x_rep;
                %                     [C(:, 1), C(:, 2)] = recursive_calc_bits(m - reduced_layer, psi, C(:, 1), C(:, 2), m, lambda_offset);
                %                 end
                %                 phi = phi + M;
            case 3%spc
                M = node_type_matrix(node_type_matrix_cnt, 2);
                reduced_layer = log2(M); %This reduced layer denotes where to stop.
                psi = phi;
                psi_ = phi;%This psi_ is used for bits recursion
                for i = 1 : reduced_layer
                    psi_ = floor(psi_/2);
                end
                layer = 0;%This layer denotes where to start.

                if phi ~= 0
                    while(mod(psi, 2) == 0)
                        psi = floor(psi/2);
                        layer = layer + 1;
                    end
                end

                if phi == 0
                    for i_layer = m - 1 : -1 : reduced_layer%NOT FULL LAYER = 0
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        for beta = 0 : index_1 - 1
                            P(beta + index_1) = sign(P(2*beta + index_2)) *  sign(P(2*beta + 1 + index_2)) * ...
                                min(abs(P(2*beta + index_2)), abs(P(2*beta + 1 + index_2)));
                        end
                    end
                else
                    for i_layer = layer: -1 : reduced_layer
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        if i_layer == layer
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = (1 - 2*C(beta + index_1, 1)) * P(2 * beta + index_2) + P(2 * beta + 1 + index_2);
                            end
                        else
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = sign(P(2 * beta + index_2)) *  sign(P(2 * beta + 1 + index_2)) * ...
                                    min(abs(P(2 * beta + index_2)), abs(P(2 * beta + 1 + index_2)));
                            end
                        end
                    end
                end
                x_spc = SPC(P(M : 2 * M - 1));
                if mod(psi_, 2) == 0
                    C(M : 2 * M - 1, 1) = x_spc;
                else
                    C(M : 2 * M - 1, 2) = x_spc;
                    while(mod(psi_, 2) == 1)
                        psi_ = floor(psi_/2);
                        index_1 = lambda_offset_(reduced_layer + 1);
                        index_2 = lambda_offset_(reduced_layer + 2);
                        for beta = 0 : index_1 - 1
                            C(2 * beta + index_2, 1 + mod(psi_, 2)) = mod(C(beta + index_1, 1) + C(beta + index_1, 2), 2);
                            C(2 * beta + 1 + index_2, 1 + mod(psi_, 2)) = C(beta + index_1, 2);
                        end
                        reduced_layer = reduced_layer + 1;
                    end
                end
                phi = phi + M;

                %                 M = node_type_matrix(node_type_matrix_cnt, 2);
                %                 reduced_layer = log2(M);
                %                 psi = phi;
                %                 for i = 1 : reduced_layer
                %                     psi = floor(psi/2);
                %                 end
                %                 P = recursive_calc_llr(m - reduced_layer, psi, P, C(: , 1), m, lambda_offset);
                %                 x_spc = SPC(P(M : 2 * M - 1));
                %                 if mod(psi, 2) == 0
                %                     C(M : 2 * M - 1, 1) = x_spc;
                %                 else
                %                     C(M : 2 * M - 1, 2) = x_spc;
                %                     [C(:, 1), C(:, 2)] = recursive_calc_bits(m - reduced_layer, psi, C(:, 1), C(:, 2), m, lambda_offset);
                %                 end
                %                 phi = phi + M;
            case 4% I type
                M = node_type_matrix(node_type_matrix_cnt, 2);
                reduced_layer = log2(M); %This reduced layer denotes where to stop.
                psi = phi;
                psi_ = phi;%This psi_ is used for bits recursion
                for i = 1 : reduced_layer
                    psi_ = floor(psi_/2);
                end
                layer = 0;%This layer denotes where to start.

                if phi ~= 0
                    while(mod(psi, 2) == 0)
                        psi = floor(psi/2);
                        layer = layer + 1;
                    end
                end

                if phi == 0
                    for i_layer = m - 1 : -1 : reduced_layer%NOT FULL LAYER = 0
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        for beta = 0 : index_1 - 1
                            P(beta + index_1) = sign(P(2*beta + index_2)) *  sign(P(2*beta + 1 + index_2)) * ...
                                min(abs(P(2*beta + index_2)), abs(P(2*beta + 1 + index_2)));
                        end
                    end
                else
                    for i_layer = layer: -1 : reduced_layer
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        if i_layer == layer
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = (1 - 2*C(beta + index_1, 1)) * P(2 * beta + index_2) + P(2 * beta + 1 + index_2);
                            end
                        else
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = sign(P(2 * beta + index_2)) *  sign(P(2 * beta + 1 + index_2)) * ...
                                    min(abs(P(2 * beta + index_2)), abs(P(2 * beta + 1 + index_2)));
                            end
                        end
                    end
                end
                x_typeI = typeI(P(M : 2 * M - 1));
                if mod(psi_, 2) == 0
                    C(M : 2 * M - 1, 1) = x_typeI;
                else
                    C(M : 2 * M - 1, 2) = x_typeI;
                    while(mod(psi_, 2) == 1)
                        psi_ = floor(psi_/2);
                        index_1 = lambda_offset_(reduced_layer + 1);
                        index_2 = lambda_offset_(reduced_layer + 2);
                        for beta = 0 : index_1 - 1
                            C(2 * beta + index_2, 1 + mod(psi_, 2)) = mod(C(beta + index_1, 1) + C(beta + index_1, 2), 2);
                            C(2 * beta + 1 + index_2, 1 + mod(psi_, 2)) = C(beta + index_1, 2);
                        end
                        reduced_layer = reduced_layer + 1;
                    end
                end
                phi = phi + M;

                %                 M = node_type_matrix(node_type_matrix_cnt, 2);
                %                 reduced_layer = log2(M);
                %                 psi = phi;
                %                 for i = 1 : reduced_layer
                %                     psi = floor(psi/2);
                %                 end
                %                 P = recursive_calc_llr(m - reduced_layer, psi, P, C(: , 1), m, lambda_offset);
                %                 x_typeI = typeI(P(M : 2 * M - 1));
                %                 if mod(psi, 2) == 0
                %                     C(M : 2 * M - 1, 1) = x_typeI;
                %                 else
                %                     C(M : 2 * M - 1, 2) = x_typeI;
                %                     [C(:, 1), C(:, 2)] = recursive_calc_bits(m - reduced_layer, psi, C(:, 1), C(:, 2), m, lambda_offset);
                %                 end
                %                 phi = phi + M;
            case 5% II type

                M = node_type_matrix(node_type_matrix_cnt, 2);
                reduced_layer = log2(M); %This reduced layer denotes where to stop.
                psi = phi;
                psi_ = phi;%This psi_ is used for bits recursion
                for i = 1 : reduced_layer
                    psi_ = floor(psi_/2);
                end
                layer = 0;%This layer denotes where to start.

                if phi ~= 0
                    while(mod(psi, 2) == 0)
                        psi = floor(psi/2);
                        layer = layer + 1;
                    end
                end

                if phi == 0
                    for i_layer = m - 1 : -1 : reduced_layer%NOT FULL LAYER = 0
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        for beta = 0 : index_1 - 1
                            P(beta + index_1) = sign(P(2*beta + index_2)) *  sign(P(2*beta + 1 + index_2)) * ...
                                min(abs(P(2*beta + index_2)), abs(P(2*beta + 1 + index_2)));
                        end
                    end
                else
                    for i_layer = layer: -1 : reduced_layer
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        if i_layer == layer
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = (1 - 2*C(beta + index_1, 1)) * P(2 * beta + index_2) + P(2 * beta + 1 + index_2);
                            end
                        else
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = sign(P(2 * beta + index_2)) *  sign(P(2 * beta + 1 + index_2)) * ...
                                    min(abs(P(2 * beta + index_2)), abs(P(2 * beta + 1 + index_2)));
                            end
                        end
                    end
                end
                x_typeII = typeII(P(M : 2 * M - 1));
                if mod(psi_, 2) == 0
                    C(M : 2 * M - 1, 1) = x_typeII;
                else
                    C(M : 2 * M - 1, 2) = x_typeII;
                    while(mod(psi_, 2) == 1)
                        psi_ = floor(psi_/2);
                        index_1 = lambda_offset_(reduced_layer + 1);
                        index_2 = lambda_offset_(reduced_layer + 2);
                        for beta = 0 : index_1 - 1
                            C(2 * beta + index_2, 1 + mod(psi_, 2)) = mod(C(beta + index_1, 1) + C(beta + index_1, 2), 2);
                            C(2 * beta + 1 + index_2, 1 + mod(psi_, 2)) = C(beta + index_1, 2);
                        end
                        reduced_layer = reduced_layer + 1;
                    end
                end
                phi = phi + M;

                %                 M = node_type_matrix(node_type_matrix_cnt, 2);
                %                 reduced_layer = log2(M);
                %                 psi = phi;
                %                 for i = 1 : reduced_layer
                %                     psi = floor(psi/2);
                %                 end
                %                 P = recursive_calc_llr(m - reduced_layer, psi, P, C(: , 1), m, lambda_offset);
                %                 x_typeII = typeII(P(M : 2 * M - 1));
                %                 if mod(psi, 2) == 0
                %                     C(M : 2 * M - 1, 1) = x_typeII;
                %                 else
                %                     C(M : 2 * M - 1, 2) = x_typeII;
                %                     [C(:, 1), C(:, 2)] = recursive_calc_bits(m - reduced_layer, psi, C(:, 1), C(:, 2), m, lambda_offset);
                %                 end
                %                 phi = phi + M;
            case 6% III type
                M = node_type_matrix(node_type_matrix_cnt, 2);
                reduced_layer = log2(M); %This reduced layer denotes where to stop.
                psi = phi;
                psi_ = phi;%This psi_ is used for bits recursion
                for i = 1 : reduced_layer
                    psi_ = floor(psi_/2);
                end
                layer = 0;%This layer denotes where to start.

                if phi ~= 0
                    while(mod(psi, 2) == 0)
                        psi = floor(psi/2);
                        layer = layer + 1;
                    end
                end

                if phi == 0
                    for i_layer = m - 1 : -1 : reduced_layer%NOT FULL LAYER = 0
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        for beta = 0 : index_1 - 1
                            P(beta + index_1) = sign(P(2*beta + index_2)) *  sign(P(2*beta + 1 + index_2)) * ...
                                min(abs(P(2*beta + index_2)), abs(P(2*beta + 1 + index_2)));
                        end
                    end
                else
                    for i_layer = layer: -1 : reduced_layer
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        if i_layer == layer
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = (1 - 2*C(beta + index_1, 1)) * P(2 * beta + index_2) + P(2 * beta + 1 + index_2);
                            end
                        else
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = sign(P(2 * beta + index_2)) *  sign(P(2 * beta + 1 + index_2)) * ...
                                    min(abs(P(2 * beta + index_2)), abs(P(2 * beta + 1 + index_2)));
                            end
                        end
                    end
                end
                x_typeIII = typeIII(P(M : 2 * M - 1));
                if mod(psi_, 2) == 0
                    C(M : 2 * M - 1, 1) = x_typeIII;
                else
                    C(M : 2 * M - 1, 2) = x_typeIII;
                    while(mod(psi_, 2) == 1)
                        psi_ = floor(psi_/2);
                        index_1 = lambda_offset_(reduced_layer + 1);
                        index_2 = lambda_offset_(reduced_layer + 2);
                        for beta = 0 : index_1 - 1
                            C(2 * beta + index_2, 1 + mod(psi_, 2)) = mod(C(beta + index_1, 1) + C(beta + index_1, 2), 2);
                            C(2 * beta + 1 + index_2, 1 + mod(psi_, 2)) = C(beta + index_1, 2);
                        end
                        reduced_layer = reduced_layer + 1;
                    end
                end
                phi = phi + M;

                %                 M = node_type_matrix(node_type_matrix_cnt, 2);
                %                 reduced_layer = log2(M);
                %                 psi = phi;
                %                 for i = 1 : reduced_layer
                %                     psi = floor(psi/2);
                %                 end
                %                 P = recursive_calc_llr(m - reduced_layer, psi, P, C(: , 1), m, lambda_offset);
                %                 x_typeIII = typeIII(P(M : 2 * M - 1));
                %                 if mod(psi, 2) == 0
                %                     C(M : 2 * M - 1, 1) = x_typeIII;
                %                 else
                %                     C(M : 2 * M - 1, 2) = x_typeIII;
                %                     [C(:, 1), C(:, 2)] = recursive_calc_bits(m - reduced_layer, psi, C(:, 1), C(:, 2), m, lambda_offset);
                %                 end
                %                 phi = phi + M;
            case 7% IV type
                M = node_type_matrix(node_type_matrix_cnt, 2);
                reduced_layer = log2(M); %This reduced layer denotes where to stop.
                psi = phi;
                psi_ = phi;%This psi_ is used for bits recursion
                for i = 1 : reduced_layer
                    psi_ = floor(psi_/2);
                end
                layer = 0;%This layer denotes where to start.

                if phi ~= 0
                    while(mod(psi, 2) == 0)
                        psi = floor(psi/2);
                        layer = layer + 1;
                    end
                end

                if phi == 0
                    for i_layer = m - 1 : -1 : reduced_layer%NOT FULL LAYER = 0
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        for beta = 0 : index_1 - 1
                            P(beta + index_1) = sign(P(2*beta + index_2)) *  sign(P(2*beta + 1 + index_2)) * ...
                                min(abs(P(2*beta + index_2)), abs(P(2*beta + 1 + index_2)));
                        end
                    end
                else
                    for i_layer = layer: -1 : reduced_layer
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        if i_layer == layer
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = (1 - 2*C(beta + index_1, 1)) * P(2 * beta + index_2) + P(2 * beta + 1 + index_2);
                            end
                        else
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = sign(P(2 * beta + index_2)) *  sign(P(2 * beta + 1 + index_2)) * ...
                                    min(abs(P(2 * beta + index_2)), abs(P(2 * beta + 1 + index_2)));
                            end
                        end
                    end
                end
                x_typeIV = typeIV(P(M : 2 * M - 1));
                if mod(psi_, 2) == 0
                    C(M : 2 * M - 1, 1) = x_typeIV;
                else
                    C(M : 2 * M - 1, 2) = x_typeIV;
                    while(mod(psi_, 2) == 1)
                        psi_ = floor(psi_/2);
                        index_1 = lambda_offset_(reduced_layer + 1);
                        index_2 = lambda_offset_(reduced_layer + 2);
                        for beta = 0 : index_1 - 1
                            C(2 * beta + index_2, 1 + mod(psi_, 2)) = mod(C(beta + index_1, 1) + C(beta + index_1, 2), 2);
                            C(2 * beta + 1 + index_2, 1 + mod(psi_, 2)) = C(beta + index_1, 2);
                        end
                        reduced_layer = reduced_layer + 1;
                    end
                end
                phi = phi + M;

                %                 M = node_type_matrix(node_type_matrix_cnt, 2);
                %                 reduced_layer = log2(M);
                %                 psi = phi;
                %                 for i = 1 : reduced_layer
                %                     psi = floor(psi/2);
                %                 end
                %                 P = recursive_calc_llr(m - reduced_layer, psi, P, C(: , 1), m, lambda_offset);
                %                 x_typeIV = typeIV(P(M : 2 * M - 1));
                %                 if mod(psi, 2) == 0
                %                     C(M : 2 * M - 1, 1) = x_typeIV;
                %                 else
                %                     C(M : 2 * M - 1, 2) = x_typeIV;
                %                     [C(:, 1), C(:, 2)] = recursive_calc_bits(m - reduced_layer, psi, C(:, 1), C(:, 2), m, lambda_offset);
                %                 end
                %                 phi = phi + M;
            case 8% V type
                M = node_type_matrix(node_type_matrix_cnt, 2);
                reduced_layer = log2(M); %This reduced layer denotes where to stop.
                psi = phi;
                psi_ = phi;%This psi_ is used for bits recursion
                for i = 1 : reduced_layer
                    psi_ = floor(psi_/2);
                end
                layer = 0;%This layer denotes where to start.

                if phi ~= 0
                    while(mod(psi, 2) == 0)
                        psi = floor(psi/2);
                        layer = layer + 1;
                    end
                end

                if phi == 0
                    for i_layer = m - 1 : -1 : reduced_layer%NOT FULL LAYER = 0
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        for beta = 0 : index_1 - 1
                            P(beta + index_1) = sign(P(2*beta + index_2)) *  sign(P(2*beta + 1 + index_2)) * ...
                                min(abs(P(2*beta + index_2)), abs(P(2*beta + 1 + index_2)));
                        end
                    end
                else
                    for i_layer = layer: -1 : reduced_layer
                        index_1 = lambda_offset_(i_layer + 1);
                        index_2 = lambda_offset_(i_layer + 2);
                        if i_layer == layer
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = (1 - 2*C(beta + index_1, 1)) * P(2 * beta + index_2) + P(2 * beta + 1 + index_2);
                            end
                        else
                            for beta = 0 : index_1 - 1
                                P(beta + index_1) = sign(P(2 * beta + index_2)) *  sign(P(2 * beta + 1 + index_2)) * ...
                                    min(abs(P(2 * beta + index_2)), abs(P(2 * beta + 1 + index_2)));
                            end
                        end
                    end
                end
                x_typev = typeV(P(M : 2 * M - 1));
                if mod(psi_, 2) == 0
                    C(M : 2 * M - 1, 1) = x_typev;
                else
                    C(M : 2 * M - 1, 2) = x_typev;
                    while(mod(psi_, 2) == 1)
                        psi_ = floor(psi_/2);
                        index_1 = lambda_offset_(reduced_layer + 1);
                        index_2 = lambda_offset_(reduced_layer + 2);
                        for beta = 0 : index_1 - 1
                            C(2 * beta + index_2, 1 + mod(psi_, 2)) = mod(C(beta + index_1, 1) + C(beta + index_1, 2), 2);
                            C(2 * beta + 1 + index_2, 1 + mod(psi_, 2)) = C(beta + index_1, 2);
                        end
                        reduced_layer = reduced_layer + 1;
                    end
                end
                phi = phi + M;
                %                 M = node_type_matrix(node_type_matrix_cnt, 2);
                %                 reduced_layer = log2(M);
                %                 psi = phi;
                %                 for i = 1 : reduced_layer
                %                     psi = floor(psi/2);
                %                 end
                %                 P = recursive_calc_llr(m - reduced_layer, psi, P, C(: , 1), m, lambda_offset);
                %                 x_typeV = typeV(P(M : 2 * M - 1));
                %                 if mod(psi, 2) == 0
                %                     C(M : 2 * M - 1, 1) = x_typeV;
                %                 else
                %                     C(M : 2 * M - 1, 2) = x_typeV;
                %                     [C(:, 1), C(:, 2)] = recursive_calc_bits(m - reduced_layer, psi, C(:, 1), C(:, 2), m, lambda_offset);
                %                 end
                %                 phi = phi + M;
        end
        node_type_matrix_cnt = node_type_matrix_cnt + 1;
        if node_type_matrix_cnt == size(node_type_matrix, 1)
            node_type_matrix_cnt = 1;
        end
    else
        layer = 0;
        if phi == 0
            layer = m - 1;
        else
            psi = phi;
            while(mod(psi, 2) == 0)
                psi = floor(psi/2);
                layer = layer + 1;
            end
        end

        if phi == 0
            for i_layer = m - 1 : -1 : 0
                index_1 = lambda_offset_(i_layer + 1);
                index_2 = lambda_offset_(i_layer + 2);
                for beta = 0 : index_1 - 1
                    P(beta + index_1) = sign(P(2*beta + index_2)) *  sign(P(2*beta + 1 + index_2)) * ...
                        min(abs(P(2*beta + index_2)), abs(P(2*beta + 1 + index_2)));
                end
            end
        else
            for i_layer = layer: -1 : 0
                index_1 = lambda_offset_(i_layer + 1);
                index_2 = lambda_offset_(i_layer + 2);
                if i_layer == layer
                    for beta = 0 : index_1 - 1
                        P(beta + index_1) = (1 - 2*C(beta + index_1, 1)) * P(2 * beta + index_2) + P(2 * beta + 1 + index_2);
                    end
                else
                    for beta = 0 : index_1 - 1
                        P(beta + index_1) = sign(P(2 * beta + index_2)) *  sign(P(2 * beta + 1 + index_2)) * ...
                            min(abs(P(2 * beta + index_2)), abs(P(2 * beta + 1 + index_2)));
                    end
                end
            end
        end

        if frozen_bits(phi + 1) == 1
            C(1, 1 + mod(phi, 2)) = 0;
        else
            C(1, 1 + mod(phi, 2)) = P(1) < 0;
        end

        if mod(phi, 2) == 1
            layer = 0;
            psi = phi;
            while(mod(psi, 2) == 1)
                psi = floor(psi/2);
                index_1 = lambda_offset_(layer + 1);
                index_2 = lambda_offset_(layer + 2);
                for beta = 0 : index_1 - 1
                    C(2 * beta + index_2, 1 + mod(psi, 2)) = mod(C(beta + index_1, 1) + C(beta + index_1, 2), 2);
                    C(2 * beta + 1 + index_2, 1 + mod(psi, 2)) = C(beta + index_1, 2);
                end
                layer = layer + 1;
            end
        end

        %         P = recursive_calc_llr(m, phi, P, C(: , 1), m, lambda_offset);
        %         if frozen_bits(phi + 1) == 1
        %             C(1, mod(phi, 2) + 1) = 0;
        %         else
        %             C(1, mod(phi, 2) + 1) = P(1) < 0;
        %         end
        %
        %         if mod(phi, 2) == 1
        %             [C(:, 1), C(:, 2)] = recursive_calc_bits(m, phi, C(:, 1), C(:, 2), m, lambda_offset);
        %         end

        phi = phi + 1;
    end
end
x = C(end - N + 1 : end, 1);
end


function [outBoxes idsGood]= FilterBoxesWidth(inBoxes, minLen)
% [outBoxes idsGood]= FilterBoxesWidth(inBoxes, minLen)
%
% Filters out small boxes. Boxes have to have a width and height
% larger than minLen
%
% inBoxes:       M x 4 array of boxes
% minLen:   Minimum width and height of boxes
%
% outBoxes:      N x 4 array of boxes, N < M
% idsGood:       M x 1 logical array denoting boxes kept
%
%     Jasper Uijlings - 2013

[nr nc] = BoxSize(inBoxes);

idsGood = (nr >= minLen) & (nc >= minLen);
outBoxes = inBoxes(idsGood,:);


function D = gamma_dist()

    D.gamma_expectation = @gamma_expectation;
    function ratio = gamma_expectation(alpha, beta)

        ratio = alpha ./ beta;

    end

    D.gamma_expectation_log = @gamma_expectation_log;
    function val = gamma_expectation_log(alpha, beta)

        val = psi(alpha) - log(beta);

    end

end


%DAGNN.UNIFORMSCALINGGRIDGENERATOR  Generate an iso-tropic scaling + translation
%   grid for bilinear resampling.
%   This layer maps 1 x 1 x 3 x N transformation parameters corresponding to
%   the scale and translation y and x respectively, to 2 x Ho x Wo x N
%   sampling grids compatible with dagnn.BlilinearSampler.

% (c) 2016 Ankush Gupta
classdef UniformScalingGridGenerator < dagnn.Layer

 properties
     Ho = 0;
     Wo = 0;
 end

  properties (Transient)
    % the grid --> this is cached
    % has the size: [2 x HoWo]
    xxyy ;
  end

  methods

    function outputs = forward(obj, inputs, ~)
      % input is a 1x1x3xN TENSOR corresponding to:
      % [  s 0 ty ]
      % [  0 s tx ]
      %
      % OUTPUT is a 2xHoxWoxN grid

      % reshape the tfm params into matrices:
      T = inputs{1};
      % check shape:
      sz_T = size(T);
      assert(all(sz_T(1:3) == [1 1 3]), 'transforms have incorrect shape');
      nbatch = size(T,4);
      S = reshape(T(1,1,1,:), 1,1,nbatch); % x,y scaling
      t = reshape(T(1,1,2:3,:), 2,1,nbatch); % translation
      % generate the grid coordinates:
      useGPU = isa(T, 'gpuArray');
      if isempty(obj.xxyy)
        obj.initGrid(useGPU);
      end
      % transform the grid:
      g = bsxfun(@times, obj.xxyy, S); % scale
      g = bsxfun(@plus, g, t); % translate
      g = reshape(g, 2,obj.Ho,obj.Wo,nbatch);
      outputs = {g};
    end

    function [derInputs, derParams] = backward(obj, inputs, ~, derOutputs)
      dY = derOutputs{1};
      useGPU = isa(dY, 'gpuArray');
      nbatch = size(dY,4);

      % create the gradient buffer:
      dA = zeros([1,1,3,nbatch], 'single');
      if useGPU, dA = gpuArray(dA); end

      dY  = reshape(dY, 2,obj.Ho*obj.Wo,nbatch);
      % gradient wrt the linear part:
      dA(1,1,1,:) = reshape(obj.xxyy,1,[]) * reshape(dY, [],nbatch);
      % gradient wrt translation (or bias):
      dA(1,1,2:3,:) = reshape(sum(dY,2),1,1,2,[]);

      derInputs = {dA};
      derParams = {};
    end

    function outputSizes = getOutputSizes(obj, inputSizes)
      nBatch = inputSizes{1}(4);
      outputSizes = {[2, obj.Ho, obj.Wo, nBatch]};
    end

    function obj = UniformScalingGridGenerator(varargin)
      obj.load(varargin);
      % get the output sizes:
      obj.Ho = obj.Ho;
      obj.Wo = obj.Wo;
      obj.xxyy = [];
    end

    function obj = reset(obj)
      reset@dagnn.Layer(obj) ;
      obj.xxyy = [] ;
    end

    function initGrid(obj, useGPU)
      % initialize the grid:
      % this is a constant
      xi = linspace(-1, 1, obj.Ho);
      yi = linspace(-1, 1, obj.Wo);
      [yy,xx] = meshgrid(yi,xi);
      xxyy = [xx(:), yy(:)]' ; % 2xM
      if useGPU
        xxyy = gpuArray(xxyy);
      end
      obj.xxyy = xxyy ;
    end

  end
end


function [data,units] = compute_wing_arear_mm(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  data{i} = trx(fly).wing_arear ./ trx.pxpermm^2;

end
units = parseunits('mm^2');


function [ y, j, ierror ] = yj_check_julian ( y, j )

%*****************************************************************************80
%
%% YJ_CHECK_JULIAN checks a Julian YJ date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, J, the YJ date.
%
%    Output, integer IERROR, is 0 if no serious error was found in
%    the date, and 1 otherwise.
%

%
%  Check the year.
%
  [ y, ierror ] = y_check_julian ( y );

  if ( ierror ~= 0 )
    return
  end
%
%  Make sure J is not too small or too big.
%
  [ y, j ] = j_borrow_julian ( y, j );

  [ y, j ] = j_carry_julian ( y, j );

  return
end


function write_tsurf_to_gltf(filename, tsh)
  drawnow;
  % Check that plot is set up for per-vertex data
  assert(strcmp(tsh.FaceColor,'interp'));
  % Even though you may pass it  with 'CData' seems it gets moved to
  % 'FaceVertexCData'
  assert(size(tsh.FaceVertexCData,2) == 1);
  assert(size(tsh.FaceVertexCData,1) == size(tsh.Vertices,1));

  % get color map
  CM = tsh.Parent.Colormap;
  CA = caxis(tsh.Parent);
  % pad colormap to power of 2
  pad = 2^ceil(log2(size(CM,1))) - size(CM,1);
  CA = [CA(1) CA(1) + (CA(2)-CA(1)) * (size(CM,1)+pad)/size(CM,1)];
  CM(end+(1:pad),:) = repmat(CM(end,:),pad,1);
  im = permute(flipud(CM),[1 3 2]);

  D = (tsh.FaceVertexCData-CA(1))/(CA(2)-CA(1));
  R = axisangle2matrix([1 0 0],pi/2);
  VN = -tsh.VertexNormals;
  if ~isempty(VN);
   VN = VN*R;
  end

  writeGLTF_and_validate( ...
    filename, ...
    tsh.Vertices * R , ...
    tsh.Faces, ...
    'Normals',VN, ...
    'MagFilter','NEAREST', ...
    'MinFilter','NEAREST', ...
    'TextureCoordinates',[D D], ...
    'TextureImage',im);
end


%--------------------------------------------------------------------------
%   [dataout] = d2h(datain,N_bit)
%--------------------------------------------------------------------------
%   功能:
%   转换10进制数到16进制模式(常用于FPGA测试中)
%--------------------------------------------------------------------------
%   input:
%           datain                  输入数据
%           N_bit                   转换位数
%   output:
%           dataout                 输出数据
%--------------------------------------------------------------------------
%   Examples:
%   d2h(13,5)
%   ans =
%       "D"
%   d2b(-13,5)
%   ans =
%       "10011"
%--------------------------------------------------------------------------
function dataout = d2h(datain,N_bit)
[X,Y,Z] = size(datain);
datain(datain<0) = datain(datain<0)+2^N_bit;
h = dec2hex(datain);
temp = string([]);
for index = 1:X*Y*Z
    temp(index)= string(h(index,:));
end
dataout = reshape(temp,[X Y Z]);
end


function varargout = gridmeshToQuadmesh(x, y, varargin)
%GRIDMESHTOQUADMESH Create a quad mesh from a grid mesh
%
%   Deprecated: replaced by surfToMesh (2012.10.25)
%
%   [V F] = gridmeshToQuadmesh(X, Y)
%   [V F] = gridmeshToQuadmesh(X, Y, Z)
%   Converts the surface grid given by two or three coordinate arrays into
%   a face-vertex quad mesh.
%
%   Example
%     % transform a surface into a mesh
%     [X,Y] = meshgrid(-2:.2:2, -2:.2:2);
%     Z = X .* exp(-X.^2 - Y.^2);
%     [V F] = gridmeshToQuadmesh(X, Y, Z);
%     figure;
%     drawMesh(V, F);
%
%     % Transform surface of a cylinder as a mesh
%     [x y z] = cylinder(5*ones(1, 10));
%     [v f] = gridmeshToQuadmesh(x, y, z);
%     figure;
%     drawMesh(v, f);
%     view(3); axis equal;
%
%   See also
%     meshes3d, meshgrid, drawMesh
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-12-18,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

warning('MatGeom:deprecated', ...
    'function "gridmeshToQuadmesh" is deprecated, and was replaced by "surfToMesh".');

% number of vertices
dim = size(x);
nv = prod(dim);

% vertex indices in grid
inds = reshape(1:nv, dim);

% create vertices
if isempty(varargin)
    vertices = [x(:) y(:)];
else
    z = varargin{1};
    vertices = [x(:) y(:) z(:)];
end

% create faces
v1 = inds(1:end-1, 1:end-1);
v2 = inds(1:end-1, 2:end);
v3 = inds(2:end, 2:end);
v4 = inds(2:end, 1:end-1);

faces = [v1(:) v2(:) v3(:) v4(:)];

% format output
if nargout <= 1
    % concatenate into one structure
    mesh.vertices = vertices;
    mesh.edges = edges;
    mesh.faces = faces;
    varargout = {mesh};

elseif nargout == 2
    % returns as separate arguments
    varargout = {vertices, faces};

elseif nargout == 3
    % also return vertex indices
    varargout = {vertices, faces, inds};

end


function [X,I,U,P,H] = variablesUnpack(V, N, nx, ni, nu, np)

[X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.indizes(N, nx, ni, nu, np);

X = reshape(V(X_indizes), nx, N+1);
I = reshape(V(I_indizes), ni, N);
U = reshape(V(U_indizes), nu, N);
P = reshape(V(P_indizes), np, N+1);
H = reshape(V(H_indizes), 1 , N);


function Animate_System(States,Params,Trajectory,Goal_Idx)

Fig_Num = 2;

N_Frames = Params.Sim.Frames_Per_Second * diff(Params.Sim.Tspan) / Params.Sim.Speed_Up_Video;
Frames = linspace(1,Params.Sim.Nsteps,N_Frames);
Frames = round(Frames);

Time_Target = linspace(1,diff(Params.Sim.Tspan)/Params.Sim.Speed_Up_Video,N_Frames);
TimeStep=0;

Npts = Params.Traj.Npts;

x = States(1,:);
y = States(2,:);
th = States(3,:);
phi = States(4,:);

Lt = Params.Dyn.Lt;
Lc = Params.Dyn.Lc;

%[dStates, A, B, Positions] = Derive_EoM()

Pa = [...   %Position of the back of the trailer
    x;
    y];

Pb = [...    %Position of the back of the cab
 x - Lt*sin(th);
 y + Lt*cos(th)];

Pc = [...   %Position of the front of the cab
 x - Lc*(cos(phi).*sin(th) + cos(th).*sin(phi)) - Lt*sin(th);
 y + Lc*(cos(phi).*cos(th) - sin(phi).*sin(th)) + Lt*cos(th)];

if strcmp(Params.Sim.Direction,'reverse')
    Dir = 2;
else
    Dir = 1;
end
Road = zeros(2,Npts);
for i=1:Npts
    Road(:,i) = Trajectory.States{Dir,i}(1:2);
end

All_Points = [Pa,Pb,Pc,Road];

Xmin = min(All_Points(1,:));
Xmax = max(All_Points(1,:));
Ymin = min(All_Points(2,:));
Ymax = max(All_Points(2,:));

figure(Fig_Num)
tic;
for i=Frames
    clf(Fig_Num);
    hold on
    axis([Xmin,Xmax,Ymin,Ymax])
    axis equal
    Trailer_X = [Pa(1,i); Pb(1,i)];
    Trailer_Y = [Pa(2,i); Pb(2,i)];
    Cab_X = [Pb(1,i); Pc(1,i)];
    Cab_Y = [Pb(2,i); Pc(2,i)];

    Idx = Goal_Idx(i);
    if Idx < 1
        Idx = Params.Traj.Npts;
        %At the end of the road
    end

    Target = Road(:,Idx);

    %Plot road
        plot(Road(1,:),Road(2,:),'k','LineWidth',Params.Sim.RoadWidth)
    %Plot the traces:
        plot(Pa(1,1:i),Pa(2,1:i),'b','LineWidth',4)
        plot(Pc(1,1:i),Pc(2,1:i),'g','LineWidth',4)
    %Plot truck
        plot(Trailer_X,Trailer_Y,'b','LineWidth',20)
        plot(Cab_X, Cab_Y,'g','LineWidth',17)
    %Plot the control target
        plot(Target(1),Target(2),'ro','MarkerSize',18,'LineWidth',3)
    title('Simulation of the Tractor Trailer Truck')
    axis equal

    TimeStep = TimeStep + 1;
    Time_Error = Time_Target(TimeStep)-toc;
    if Time_Error > 0
        pause(Time_Error)
    else
        pause(0.001)
    end
end

end


%% hexMeshCylinder
% Below is a demonstration of the features of the |hexMeshCylinder| function

%%

clear; close all; clc;

%%
% Plot settings
fontSize=15;
faceAlpha1=1;
edgeColor=0.25*ones(1,3);
edgeWidth=2;

%% Creating a solid hexahedral mesh sphere
% Creating a solid hexahedral mesh sphere

pointSpacing=0.5;
cylRadius=1;
cylLength=2;

[meshStruct]=hexMeshCylinder(cylRadius,cylLength,pointSpacing);

% Access model element and patch data
Fb=meshStruct.facesBoundary;
Cb=meshStruct.boundaryMarker;
V=meshStruct.nodes;
E=meshStruct.elements;

%%
% Visualize mesh

hFig=cFigure;
subplot(1,2,1); hold on;
title('Boundary surfaces','FontSize',fontSize);
gpatch(Fb,V,Cb,'k',0.5,2);
% patchNormPlot(Fb,V);
axisGeom(gca,fontSize);
colormap(gjet); icolorbar;
camlight headlight;

hs=subplot(1,2,2); hold on;
title('Cut view of solid mesh','FontSize',fontSize);
optionStruct.hFig=[hFig hs];
gpatch(Fb,V,'kw','none',0.25);
meshView(meshStruct,optionStruct);
axisGeom(gca,fontSize);
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


% Dasslc test problem
% requires files: dydt1.m

t0  = 0.0;        % initial value for independent variable
tf  = 1.0;        % final value for independent variable
y0  = [3 6 5]';   % initial state variables

[t,y]=dasslc('dydt1',[t0 tf],y0);
plot(t,y);


function plotCustom(v,pcmd,varargin)
%
% Syntax
%   plotcustom(v,@(x,y) drawCommand(x,y))  %
%
% Input
%  v  - @vector3d
%  s  - string
%
% Options
%
% Output
%
% See also

% initialize spherical plot
sP = newSphericalPlot(v,varargin{:});

for j = 1:numel(sP)

  % project data
  [x,y] = project(sP(j).proj,v,varargin{:});

  % plot custom
  for i = 1:length(x), pcmd{1}(sP(j).hgt,x(i),y(i)); end
end


function output = callquadprog(interfacedata)

options = interfacedata.options;
model = yalmip2quadprog(interfacedata);

if options.savedebug
    save quadprogdebug model
end

if options.showprogress;showprogress(['Calling ' interfacedata.solver.tag],options.showprogress);end
solvertime = tic;
solveroutput = callsolver(model,options);
solvertime = toc(solvertime);
solution = quadprogsol2yalmipsol(solveroutput,model);

% Save all data sent to solver?
if ~options.savesolverinput
    model = [];
end

% Save all data from the solver?
if ~options.savesolveroutput
    solveroutput = [];
end

% Standard interface
Primal      = solution.x(:);
Dual        = solution.D_struc;
problem     = solution.problem;
infostr     = yalmiperror(solution.problem,interfacedata.solver.tag);
if ~options.savesolverinput
    solverinput = [];
else
    solverinput = model;
end
if ~options.savesolveroutput
    solveroutput = [];
else
    solveroutput = solveroutput;
end
% Standard interface
output = createOutputStructure(Primal,Dual,[],problem,infostr,solverinput,solveroutput,solvertime);

function solveroutput = callsolver(model,options)
x = [];
fmin = [];
flag = [];
output = [];
lambda = [];
if nnz(model.Q) == 0
    % BUG in LIN/QUADPROG, computation of lambda crashes in some rare
    % cases. To avoid seeing this when we don't want the lambdas anyway, we
    % don't ask for it
    if options.saveduals
        [x,fmin,flag,output,lambda] = linprog(model.c, model.A, model.b, model.Aeq, model.beq, model.lb, model.ub, model.x0,model.ops);
    else
        lambda = [];
        [x,fmin,flag,output] = linprog(model.c, model.A, model.b, model.Aeq, model.beq, model.lb, model.ub, model.x0,model.ops);
    end
else
    if options.saveduals
        [x,fmin,flag,output,lambda] = quadprog(model.Q, model.c, model.A, model.b, model.Aeq, model.beq, model.lb, model.ub, model.x0,model.ops);
    else
        lambda = [];
        if options.savesolveroutput
            [x,fmin,flag,output] = quadprog(model.Q, model.c, model.A, model.b, model.Aeq, model.beq, model.lb, model.ub, model.x0,model.ops);
        else
            [x,fmin,flag] = quadprog(model.Q, model.c, model.A, model.b, model.Aeq, model.beq, model.lb, model.ub, model.x0,model.ops);
        end
    end
    if flag==5
        [x,fmin,flag,output,lambda] = quadprog(model.Q, model.c, model.A, model.b, model.Aeq, model.beq, model.lb, model.ub, [],model.ops);
    end
end
if isempty(x)
    x = nan(length(model.c),1);
end
solveroutput.x = x;
solveroutput.fmin = fmin;
solveroutput.flag = flag;
solveroutput.output = output;
solveroutput.lambda = lambda;

function solution = quadprogsol2yalmipsol(solveroutput,model)

solution.x = solveroutput.x(:);
% Internal format for duals
if ~isempty(solveroutput.lambda)
    solution.D_struc = [solveroutput.lambda.eqlin;solveroutput.lambda.ineqlin];
else
    solution.D_struc = [];
end

% Check, currently not exhaustive...
problem = 0;
if solveroutput.flag==0
    solution.problem = 3;
else
    if solveroutput.flag==-3
        solution.problem = 2;
    elseif solveroutput.flag==-2
        solution.problem = 1;
    else
        if solveroutput.flag>0
            solution.problem = 0;
        else
            if isempty(solveroutput.x)
                solution.x = repmat(nan,length(model.f),1);
            end
            if any((model.A*solveroutput.x-model.b)>sqrt(eps)) | any( abs(model.Aeq*solveroutput.x-model.beq)>sqrt(eps))
                solution.problem = 1; % Likely to be infeasible
            else
                if model.f'*solveroutput.x<-1e10 % Likely unbounded
                    solution.problem = 2;
                else          % Probably convergence issues
                    solution.problem = 5;
                end
            end
        end
    end
end


function get_seed_test ( )

%*****************************************************************************80
%
%% GET_SEED_TEST tests GET_SEED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 June 2006
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GET_SEED_TEST\n' );
  fprintf ( 1, '  GET_SEED picks an initial seed value for R8_UNIFORM_01.\n' );
  fprintf ( 1, '  The value chosen should vary over time, because\n' );
  fprintf ( 1, '  the seed is based on reading the clock.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  This is just the "calendar" clock, which does\n' );
  fprintf ( 1, '  not change very fast, so calling GET_SEED several\n' );
  fprintf ( 1, '  times in a row may result in the same value.\n' );

  seed = 12345678;
  seed_old = seed;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial seed is %12d\n', seed );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Next 3 values of R8_UNIFORM_01:\n' );
  fprintf ( 1, '\n' );

  for j = 1 : 3
    [ r, seed ] = r8_uniform_01 ( seed );
    fprintf ( 1, '  %f\n', r );
  end

  for i = 1 : 4

    while ( 1 )

      seed = get_seed ( );

      if ( seed ~= seed_old )
        seed_old = seed;
        break;
      end

    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  New seed from GET_SEED is %12d\n', seed );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Next 3 values of R8_UNIFORM_01:\n' );
    fprintf ( 1, '\n' );

    for j = 1 : 3
      [ r, seed ] = r8_uniform_01 ( seed );
      fprintf ( 1, '  %f\n', r );
    end

  end

  return
end


function dat  =  testing(algo,dat)

%  if isempty(algo.child.dat) % maybe used a custom ker to train and then changed!
%    kerMaTemp=get_kernel(algo.child,dat,algo.xSV);
%  else
%    kerMaTemp=test(algo.child,dat);
%    kerMaTemp=kerMaTemp.X;
%  end

  %% <<---To avoid large kernel matrices, we test in batches--->
  sz=get_dim(algo.Xsv);   %% <---500x500 point are the maximum for one batch
  if sz==0 sz=1; end;
  batch_size=round((500^2)/sz);

  yEst=[];
  for i=[1:batch_size:get_dim(dat)]
    take= [i:min(i+batch_size-1,get_dim(dat))];
    if ~isempty(algo.alpha)
     kerMaTemp=get_kernel(algo.child,get(dat,take),algo.Xsv);
     yEst=[yEst; ((algo.alpha'* kerMaTemp)+algo.b0)'];
    else
        yEst=[yEst ; algo.b0*ones(length(take),1)];
    end
  end

  if algo.algorithm.use_signed_output==1
    yEst=sign(yEst);
  end

  dat=set_x(dat,yEst);
  dat=set_name(dat,[get_name(dat) ' -> ' get_name(algo)]);


%klas_bayes 'Performs Bayesian maximum likelihood classification '
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros las_bayes.pane file
%
% Parameters:
% InputFile: i 'Data object ', required: 'Input data object'
% InputFile: p 'Pre-Classification', required: 'File containing input predetermined classification'
% InputFile: ppf 'Prior Probabilities File (ASCII)', optional: 'Prior probabilities files (ASCII)'
% OutputFile: o 'Classified data object', required: 'Resulting output classified data object'
% OutputFile: mlo 'Maximum Likelihood object', optional: 'output maximum likelihood object'
% OutputFile: cso 'Chi-Square object', optional: 'output Chi-Square tail-rejection areas'
%
% Example: [o, mlo, cso] = klas_bayes({i, p, ppf}, {'i','';'p','';'ppf','';'o','';'mlo','';'cso',''})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% las_bayes - Performs Bayesian maximum likelihood classification
%
%  DESCRIPTION
%
%
%
%  EXAMPLES
%
%  "SEE ALSO"
% llas_bayes
%
%  RESTRICTIONS
%
%  REFERENCES
% This program is modeled after the Land Analysis System (LAS)
% program "bayes".
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc.  All rights reserved.
%

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

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


%% init java, run at begin, once
% necessary to  load java files
% clear java
% javaclasspath(pwd)
%% steps:
% - load maps
% - choose algorithm (DSL: D*Lite; FDS: Focussed D*, AS: A*)
% - do initial search
% -(optional) resolve path
% - update map
% - search in updated map
% - resolve path

%% load maps
mapsNames = {'a', 'e', 'b', 'c','map01'};
maps = {};
%scalling should be 1(original map) or 4(downscalled map for better performance)
scalling = 4;
for i=1:1: length(mapsNames)
        tmp = LoadMap(  strcat(mapsNames{i}, '.png'), scalling);
        start = tmp.start';
        goal = tmp.goal';
    maps{end+1} = tmp.map;
end

%% initial search
DSL = 0;
FDS = 1;
AS = 0;
bool = 0;
clc
close all
tic
    if DSL
        state = DSLInit(start, goal, maps{1}, scalling);
        state = DSLComputePath(state);
    elseif FDS
        state = FDSInit(start, goal, maps{1}, scalling);
        state = FDSComputePath(state);
    elseif AS
        state = ASInit(start, goal, maps{1}, scalling);
        state = ASComputePath(state);
    end
    bool = 1;
toc
gah = state;
%% update map, search in update map, run twice
state.kM = 50;
close all
tic
    if DSL
        if bool
            bool = 0;
            state = DSLUpdateMap(state, maps{2});
        else
            state = DSLComputePath(state);
        end
    elseif FDS
        if bool
            bool = 0;
            state = FDSUpdateMap(state, maps{2});
            state.kM = 50;
        else
            state = FDSComputePath(state);
        end
    end
toc
%% resolve path, insert map name here if you want to get image in original size when map was downscalled
    state.path = ResolvePath(state);
    resp = PlotPath(state, scalling, 'e');

%% show A-star graph
a = state.graph(:,:,1);
a(a(:,:)==-1) = 0; % unvisited
a(state.map(:,:)==0) = 0.2; % obstacles
a(a(:,:)==inf) = 0;
a(a(:,:)>=1) = 0.3;
imshow(a)
%% show D-star graph
% figure
% obstacles 0.2
% visited : 0.3
% unavailble, unvisited: 0
% in queue: 1
% in queue and g-val == inf: 0.7
a = state.graph(:,:,1);
a(a(:,:)~=inf) = 0.3;
a(a(:,:)==inf) = 0;
a(state.map(:,:)==0) = 0.2;
b = state.graph(:,:,3);
b( state.map(:,:)==0 ) = 0;
b( state.graph(:,:,2)==inf ) = 0;
b = b*0.7;
imshow(b+a, 'Border', 'tight')


%%*****************************************************************************
%% checkdepconst: compute AAt to determine if the
%%             constraint matrices Ak are linearly independent.
%%
%% [At,b,y,idxB,neardepconstr,feasible,AAt] = checkdepconstr(blk,At,b,y,rmdepconstr);
%%
%% rmdepconstr = 1, if want to remove dependent columns in At
%%             = 0, otherwise.
%%
%% idxB = indices of linearly independent columns of original At.
%% neardepconstr = 1 if there is nearly dependent columns in At
%%               = 0, otherwise.
%% Note: the definition of "nearly dependent" is dependent on the
%%       threshold used to determine the small diagonal elements in
%%       the LDLt factorization of A*At.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************************

   function  [At,b,y,idxB,neardepconstr,feasible,AAt] = ...
              checkdepconstr(blk,At,b,y,rmdepconstr)

%    global existlowrank printlevel
%    global nnzmatold
    printlevel = 0;
%%
%% compute AAt
%%
   m = length(b);
   AAt = sparse(m,m);  numdencol = 0; UU = [];
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      if strcmp(pblk{1},'s');
         m1 = size(At{p,1},2); m2 = m - m1;
         if (m2 > 0)
            if (m2 > 0.3*m); AAt = full(AAt); end
            dd = At{p,3};
            lenn = size(dd,1);
            idxD = [0; find(diff(dd(:,1))); lenn];
            ss = [0, cumsum(pblk{3})];
            if (existlowrank)
               AAt(1:m1,1:m1) = AAt(1:m1,1:m1) + At{p,1}'*At{p,1};
               for k = 1:m2
                  idx = [ss(k)+1 : ss(k+1)];
                  idx2 = [idxD(k)+1: idxD(k+1)];
                  ii = dd(idx2,2)-ss(k); %% undo cumulative indexing
                  jj = dd(idx2,3)-ss(k);
                  len = pblk{3}(k);
                  Dk = spconvert([ii,jj,dd(idx2,4); len,len,0]);
                  tmp = svec(pblk,At{p,2}(:,idx)*Dk*At{p,2}(:,idx)');
                  tmp2 = AAt(1:m1,m1+k) + (tmp'*At{p,1})';
                  AAt(1:m1,m1+k) = tmp2;
                  AAt(m1+k,1:m1) = tmp2';
               end
            end
            DD = spconvert([dd(:,2:4); sum(pblk{3}),sum(pblk{3}),0]);
            VtVD = (At{p,2}'*At{p,2})*DD;
            VtVD2 = VtVD'.*VtVD;
            for k = 1:m2
               idx0 = [ss(k)+1 : ss(k+1)];
               %%tmp = VtVD(idx0,:)' .* VtVD(:,idx0);
               tmp = VtVD2(:,idx0);
               tmp = tmp*ones(length(idx0),1);
               tmp3 = AAt(m1+[1:m2],m1+k) + mexqops(pblk{3},tmp,ones(length(tmp),1),1);
               AAt(m1+[1:m2],m1+k) = tmp3;
            end
         else
            AAt = AAt + (At{p,1})'*(At{p,1});
         end
      else
         decolidx = checkdense(At{p,1}');
         %% checkdense punts if the matrix has too many dense columns
         %% in this case we make the whole matrix dense
         if ~isempty(decolidx);
            n2 = size(At{p,1},1);
            dd = ones(n2,1);
            len= length(decolidx);
            if ((len < 0.5*sum(pblk{2})) || (sum(pblk{2}) >= 20)) ...
               && (len < 0.5*m && m > 500) %%2020-Apr-20
               dd(decolidx) = zeros(len,1);
               AAt = AAt + (abs(At{p,1})' *spdiags(dd,0,n2,n2)) *abs(At{p,1});
               tmp = At{p,1}(decolidx,:)';
               UU = [UU, tmp];
               numdencol = numdencol + len;
            end
         else
            if (nnz(At{p,1})>0.2*numel(At{p,1}))
               Atp=full(abs(At{p,1}));
            else
               Atp=abs(At{p,1});
            end
            AAt = AAt + Atp'*Atp;
         end
      end
   end
   if (numdencol > 0) & (printlevel)
      fprintf('\n number of dense column in A = %d',numdencol);
   end
   numdencol = size(UU,2);
%%
%%
%%
   feasible = 1; neardepconstr = 0;
   if ~issparse(AAt); AAt = sparse(AAt); end
   nnzmatold = mexnnz(AAt);
   rho = 1e-15;
   diagAAt = diag(AAt);
   %%mexschurfun(AAt,rho*max(diagAAt,1)); %% does not work for 2015b
   AAt = mexschurfun(AAt,rho*max(diagAAt,1));
   [L.R,indef,L.perm] = chol(AAt,'vector');
   L.d = full(diag(L.R)).^2;
   if (indef)
      msg = 'AAt is not pos. def.';
      idxB = [1:m];
      neardepconstr = 1;
      if (printlevel); fprintf('\n checkdepconstr: %s',msg); end
      return;
   end
%%
%% find independent rows of A
%%
   dd = zeros(m,1);
   idxB = [1:m]';
   dd(L.perm) = abs(L.d);
   idxN = find(dd < 1e-13*mean(L.d));
   ddB = dd(setdiff([1:m],idxN));
   ddN = dd(idxN);
   if ~isempty(ddN) & ~isempty(ddB) & (min(ddB)/max(ddN) < 10)
      %% no clear separation of elements in dd
      %% do not label constraints as dependent
      idxN = [];
   end
   if ~isempty(idxN)
      neardepconstr = 1;
      if (printlevel)
         fprintf('\n number of nearly dependent constraints = %1.0f',length(idxN));
      end
      if (numdencol==0)
         if (rmdepconstr)
            idxB = setdiff([1:m]',idxN);
            if (printlevel)
               fprintf('\n checkdepconstr: removing dependent constraints...');
            end
            [W,resnorm] = findcoeffsub(blk,At,idxB,idxN);
   	        tol = 1e-8;
            if (resnorm > sqrt(tol))
               idxB = [1:m]';
               neardepconstr = 0;
               if (printlevel)
                  fprintf('\n checkdepconstr: basis rows cannot be reliably identified,');
                  fprintf(' abort removing nearly dependent constraints');
               end
               return;
            end
            tmp = W'*b(idxB) - b(idxN);
            nnorm = norm(tmp)/max(1,norm(b));
            if (nnorm > tol)
               feasible = 0;
               if (printlevel)
                  fprintf('\n checkdepconstr: inconsistent constraints exist,');
                  fprintf(' problem is infeasible.');
               end
            else
               feasible = 1;
               for p = 1:size(blk,1)
                  At{p,1} = At{p,1}(:,idxB);
               end
               b = b(idxB);
               y = y(idxB);
               AAt = AAt(idxB,idxB);
            end
	     else
            if (printlevel)
               fprintf('\n To remove these constraints,');
               fprintf(' re-run sqlp.m with OPTIONS.rmdepconstr = 1.');
            end
         end
      else
         if (printlevel)
            fprintf('\n warning: the sparse part of AAt may be nearly singular.');
         end
      end
   end
%%*****************************************************************************
%% findcoeffsub:
%%
%% [W,resnorm] = findcoeffsub(blk,At,idXB,idXN);
%%
%% idXB = indices of independent columns of At.
%% idxN = indices of   dependent columns of At.
%%
%% AB = At(:,idxB); AN = At(:,idxN) = AB*W
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%*****************************************************************************

   function [W,resnorm] = findcoeffsub(blk,At,idxB,idxN)

   AB = []; AN = [];
   for p = 1:size(blk,1)
      AB = [AB; At{p,1}(:,idxB)];
      AN = [AN; At{p,1}(:,idxN)];
   end
   [m,n] = size(AB);
%%
%%-----------------------------------------
%% find W so that AN = AB*W
%%-----------------------------------------
%%
   [L,U,P,Q] = lu(sparse(AB));
   rhs  = P*AN;
   Lhat = L(1:n,:);
   W = Q*( U \ (Lhat \ rhs(1:n,:)));
   resnorm = norm(AN-AB*W,'fro')/max(1,norm(AN,'fro'));
%%*****************************************************************************


%EVALEXPR Evaluates an expression in specified points.
%
%   [ VEVAL ] = EVALEXPR( S_EXPR, XP, PROB, SOLNUM, EVAL_TYPE, TOL )
%   Evaluates the expression S_EXPR in the points specified in XP (in
%   global x/y/z-coordinates). PROB is a valid finite element problem
%   struct. The EVAL_TYPE flag (default 1) toggles vectorized
%   evaluation for dof variables with P0-P2 fem shape functions, or
%   allows faster evalution via intermediate nodal linear
%   interpolation. TOL is a tolerance used by ismembertol/deduplicate
%   to determined which if any evaluation points in XP are aligned
%   with grid points.
%
%       Input       Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       s_expr      string                 String expression to evaluate
%       xp          [n_sdim,n_xp]          Coordinates of evaluation points
%       prob        struct                 Problem definition struct
%       solnum      scalar  {n_sols}       Solution number/time to evaluate
%       eval_type   scalar  {1}            Evaluation type
%                                             0 - Standard/full evaluation
%                                             1 - Vectorized evaluation for dep. variables
%                                             2 - Linear interpolation via nodes (fast)
%                                             3 - Like 2 and allow invalid (nan) values
%       tol         scalar  {1e-5}         Tolerance for evalation/grid points
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       vEval       [n_xp,1]               Output vector of evaluated values

% Copyright 2013-2022 Precise Simulation, Ltd.


function output = calloptidsdp(interfacedata)

% Retrieve needed data
options = interfacedata.options;
F_struc = interfacedata.F_struc;
c       = interfacedata.c;
K       = interfacedata.K;
x0      = interfacedata.x0;
ub      = interfacedata.ub;
lb      = interfacedata.lb;

model = yalmip2optidsdp(interfacedata);
if options.showprogress;showprogress(['Calling ' interfacedata.solver.tag],options.showprogress);end

if options.savedebug
    save dsdpdebug model
end

solvertime = tic;
[y,fvals,exitflag,stats,X] = dsdp(model.f,model.A,model.b,model.lb,model.ub,model.sdcone,model.y0,model.ops);
solvertime = toc(solvertime);

% Create dual variable in internal format
if options.saveduals
    if any(K.l)
        top = 1;
        D_struc = X{1};
    else
        top = 0;
        D_struc = [];
    end
    if any(K.s)
        for j = 1:length(K.s)
            n = K.s(j);
            Z = triu(ones(n));
            i = find(Z);
            Z(i) = X{j+top};
            Z = Z + Z' - diag(diag(Z));
            D_struc = [D_struc;Z(:)];
        end
    end
else
    D_struc = [];
end

x = y;  % Our notation do not coincide ...
switch exitflag
    case 1
        switch stats.pdflag
            case 1
                problem = 0;
            case 3
                problem = 2;
            case 4
                problem = 1;
            otherwise
                problem = 4;
        end
    case -6
        problem = 6;
    case {-2,-8,-9}
        problem = 4;
    case 7
        problem = 16;
    case {-3,-27}
        problem = 3;
    otherwise
        problem = -1;
end

if options.savesolveroutput
	solveroutput.y = y;
    solveroutput.fvals = fvals;
    solveroutput.exitflag = exitflag;
    solveroutput.stats = stats;
    solveroutput.X = X;
else
	solveroutput = [];
end

if options.savesolverinput
	solverinput.model = model;
else
	solverinput = [];
end

% Standard interface
output = createOutputStructure(x,D_struc,[],problem,interfacedata.solver.tag,solverinput,solveroutput,solvertime);


%DXform Distance transform navigation class
%
% A concrete subclass of the abstract Navigation class that implements the distance
% transform navigation algorithm which computes minimum distance paths.
%
% Methods::
%  DXform       Constructor
%  plan         Compute the cost map given a goal and map
%  query        Find a path
%  plot         Display the distance function and obstacle map
%  plot3d       Display the distance function as a surface
%  display      Print the parameters in human readable form
%  char         Convert to string
%
% Properties (read only)::
%  distancemap   Distance from each point to the goal.
%  metric        The distance metric, can be 'euclidean' (default) or 'cityblock'
%
% Example::
%
%        load map1           % load map
%        goal = [50,30];     % goal point
%        start = [20, 10];   % start point
%        dx = DXform(map);   % create navigation object
%        dx.plan(goal)       % create plan for specified goal
%        dx.query(start)     % animate path from this start location
%
% Notes::
% - Obstacles are represented by NaN in the distancemap.
% - The value of each element in the distancemap is the shortest distance from the
%   corresponding point in the map to the current goal.
%
% References::
% -  Robotics, Vision & Control, Sec 5.2.1,
%    Peter Corke, Springer, 2011.
%
% See also Navigation, Dstar, PRM, distancexform.

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

classdef DXform < Navigation

    properties
        metric;     % distance metric
        distancemap;   % distance transform results
    end

    methods

        function dx = DXform(world, varargin)
            %DXform.DXform Distance transform constructor
            %
            % DX = DXform(MAP, OPTIONS) is a distance transform navigation object,
            % and MAP is an occupancy grid, a representation of a planar
            % world as a matrix whose elements are 0 (free space) or 1
            % (occupied).
            %
            % Options::
            % 'goal',G      Specify the goal point (2x1)
            % 'metric',M    Specify the distance metric as 'euclidean' (default)
            %               or 'cityblock'.
            % 'inflate',K   Inflate all obstacles by K cells.
            %
            % Other options are supported by the Navigation superclass.
            %
            % See also Navigation.Navigation.

            % TODO NEEDS PROPER ARG HANDLER

            % invoke the superclass constructor
            dx = dx@Navigation(world, varargin{:});

            opt.metric = {'euclidean', 'cityblock'};
            [opt,args] = tb_optparse(opt, varargin);
            dx.metric = opt.metric;
        end

        function s = char(dx)
            %DXform.char Convert to string
            %
            % DX.char() is a string representing the state of the object in
            % human-readable form.
            %
            % See also DXform.display, Navigation.char

            % most of the work is done by the superclass
            s = char@Navigation(dx);

            % dxform specific stuff
            s = char(s, sprintf('  distance metric: %s', dx.metric));
            if ~isempty(dx.distancemap)
                s = char(s, sprintf('  distancemap: computed:'));
            else
                s = char(s, sprintf('  distancemap: empty:'));
            end
        end

        % invoked by superclass on a change of goal, mark the distancemap
        % as invalid
        function goal_change(dx, goal)

            dx.distancemap = [];
            if dx.verbose
                disp('Goal changed -> distancemap cleared');
            end
        end

        function plan(dx, varargin)
            %DXform.plan Plan path to goal
            %
            % DX.plan(GOAL, OPTIONS) plans a path to the goal given to the constructor,
            % updates the internal distancemap where the value of each element is the
            % minimum distance from the corresponding point to the goal.
            %
            % DX.plan(GOAL, OPTIONS) as above but goal is specified explicitly
            %
            % Options::
            % 'animate'    Plot the distance transform as it evolves
            %
            % Notes::
            % - This may take many seconds.
            %
            % See also Navigation.path.

            opt.animate = false;

            [opt,args] = tb_optparse(opt, varargin);

            if opt.animate
                show = 0.05;
            else
                show = 0;
            end

            if ~isempty(args) && isvec(args{1},2)
                dx.setgoal(args{1});
            end

            assert(~isempty(dx.goal), 'RTB:DXform:plan', 'no goal specified here or in constructor');

            dx.distancemap = distancexform(dx.occgridnav, dx.goal, dx.metric, show);
        end

        function plot(dx, varargin)
            %DXform.plot Visualize navigation environment
            %
            % DX.plot(OPTIONS) displays the occupancy grid and the goal distance
            % in a new figure.  The goal distance is shown by intensity which
            % increases with distance from the goal.  Obstacles are overlaid
            % and shown in red.
            %
            % DX.plot(P, OPTIONS) as above but also overlays a path given by the set
            % of points P (Mx2).
            %
            % Notes::
            % - See Navigation.plot for options.
            %
            % See also Navigation.plot.

            plot@Navigation(dx, varargin{:}, 'distance', dx.distancemap);

        end

        function n = next(dx, robot)
            if isempty(dx.distancemap)
                error('No distancemap computed, you need to plan');
            end

            % list of all possible directions to move from current cell
            directions = [
                -1 -1
                0 -1
                1 -1
                -1 0
                0 0
                1 0
                -1 1
                0 1
                1 1];

            x = robot(1); y = robot(2);

            % find the neighbouring cell that has the smallest distance
            mindist = Inf;
            mindir = [];
            for d=directions'
                % use exceptions to catch attempt to move outside the map
                try
                    if dx.distancemap(y+d(1), x+d(2)) < mindist
                        mindir = d;
                        mindist = dx.distancemap(y+d(1), x+d(2));
                    end
                catch
                end
            end

            x = x + mindir(2);
            y = y + mindir(1);

            if all([x;y] == dx.goal)
                n = [];     % indicate we are at the goal
            else
                n = [x; y];  % else return the next closest point to the goal
            end
        end % next

        function plot3d(dx, p, varargin)
            %DXform.plot3d 3D costmap view
            %
            % DX.plot3d() displays the distance function as a 3D surface with
            % distance from goal as the vertical axis.  Obstacles are "cut out"
            % from the surface.
            %
            % DX.plot3d(P) as above but also overlays a path given by the set
            % of points P (Mx2).
            %
            % DX.plot3d(P, LS) as above but plot the line with the MATLAB linestyle LS.
            %
            % See also Navigation.plot.
            surf(dx.distancemap);
            shading interp

            if nargin > 1
                % plot path if provided
                k = sub2ind(size(dx.distancemap), p(:,2), p(:,1));
                height = dx.distancemap(k);
                hold on
                if isempty(varargin)
                    varargin{1} = 'k.';
                end
                plot3(p(:,1), p(:,2), height, varargin{:})
                hold off
            end
        end
    end % methods
end % classdef


function ADEM_occlusion
% Slow pursuit and occlusion under active inference:
%__________________________________________________________________________
% This demo illustrates slow pursuit in the context of visual occlusion. We
% start with a simulation of canonical slow pursuit of a visual target
% with sine wave motion. Crucially, the generative model is equipped with
% a simple empirical prior encoding the hidden motion of the target (using
% a linear oscillator, whose frequency is determined by a hidden cause).
% We then examine failures of tracking and anticipation during occlusion
% and when the target re-emerges from behind the occluder. We look at a
% simulation in which the precision of the oscillator dynamics modelling
% long-term behaviour of the target is reduced (cf., neuromodulatory
% deficits in cortical areas encoding biological motion). This has a
% an effect of producing a failure of pursuit, resulting in a catch-up
% saccade on target reappearance. The suppression of prior precision can
% however have beneficial effects when motion is itself unpredicted
% (as shown with differential pursuit performance under a reversal of
% the trajectory towards the end of motion). Finally, we look at how prior
% beliefs are acquired during exposure to the target - in terms of
% cumulative inference on the hidden causes encoding the frequency of
% periodic motion. This can be regarded as a high order form of evidence
% accumulation. Importantly, this (experience-dependent) inference is
% markedly impaired by the simulated lesion to precision above. In other
% words, a single failure of inference in modelling the motion of hidden
% states can have secondary consequences - such as a failure to even
% register and remember regularities. All these simulations are based upon
% active inference; with the prior belief that the centre of gaze is
% attracted to the same point responsible for target motion.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: ADEM_occlusion.m 7679 2019-10-24 15:54:07Z spm $

% hidden causes and states
%==========================================================================
% x    - hidden states:
%   x.o(1) - oculomotor angle
%   x.o(2) - oculomotor velocity
%   x.x(1) - target angle - extrinsic coordinates
%
% v    - causal states: force on target
%
% g    - sensations:
%   g(1) - oculomotor angle (proprioception)
%   g(2) - oculomotor velocity
%   g(:) - visual input - intrinsic coordinates
%--------------------------------------------------------------------------

% Set-up
%==========================================================================
M(1).E.s = 1/2;                               % smoothness
M(1).E.n = 4;                                 % order of
M(1).E.d = 1;                                 % generalised motion

% angular frequency of target motion
%--------------------------------------------------------------------------
w  = 2*pi/32;

% sensory mappings with and without occlusion
%--------------------------------------------------------------------------
g  = '[x.o; exp(-([-8:8]'' - x.x + x.o(1)).^2)*(x.x < 1/2)]';
h  = '[x.o; exp(-([-8:8]'' - x.x + x.o(1)).^2)]';

% oculomotor latencies (sinusoidal movement)
%==========================================================================
% Endow the model with internal dynamics (a simple oscillator) so that is
% recognises and remembers the trajectory to anticipate jumps in rectified
% sinusoidal motion. First, demonstrate canonical pursuit under occlusion:

% slow pursuit following with (second order) generative model
%--------------------------------------------------------------------------
x.o = [0;0];                                  % motor angle & velocity
x.x = 0;                                      % target location

% level 1: Displacement dynamics and mapping to sensory/proprioception
%--------------------------------------------------------------------------
M(1).f = '[x.o(2); (v - x.o(1))/4 - x.o(2)/2; v - x.x]';
M(1).g = g;
M(1).x = x;                                   % hidden states
M(1).V = exp(4);                              % error precision
M(1).W = exp(4);                              % error precision

% level 2: With hidden (memory) states
%--------------------------------------------------------------------------
M(2).f  = '[x(2); -x(1)]*v/8';
M(2).g  = 'x(1)';
M(2).x  = [0; 0];                             % hidden states
M(2).V  = exp(4);                             % error precision
M(2).W  = exp(4);                             % error precision

% level 3: Encoding frequency of memory states (U)
%--------------------------------------------------------------------------
M(3).v = 0;
M(3).V = exp(4);

% generative model
%==========================================================================

% first level
%--------------------------------------------------------------------------
G(1).f = '[x.o(2); a/4 - x.o(2)/8; v - x.x]';
G(1).g = g;
G(1).x = x;                                  % hidden states
G(1).V = exp(16);                            % error precision (errors)
G(1).W = exp(16);                            % error precision (motion)
G(1).U = sparse(1,[1 2],[1 1],1,19)*exp(4);  % motor gain

% second level
%--------------------------------------------------------------------------
G(2).v = 0;                                  % exogenous force
G(2).a = 0;                                  % action force
G(2).V = exp(16);

% Sine wave cause
%--------------------------------------------------------------------------
N      = 64;                                 % length of data sequence
dt     = 16;                                 % time step (ms)
t      = (1:N)*dt;                           % PST

DEM.M  = M;
DEM.G  = G;
DEM.C  = sin((1:N)*w).*((1:N) > 16);         % sinusoidal target motion
DEM.U  = zeros(1,N) + w*8;                   % prior beliefs
DEM    = spm_ADEM(DEM);

spm_figure('GetWin','Figure 1');
spm_DEM_qU(DEM.qU,DEM.pU)
subplot(3,2,1), title({'Slow pursuit:', 'prediction and error'},'FontSize',16)
subplot(3,2,2), title({'Occluded motion:', 'hidden states'},'FontSize',16)

% create movie in extrinsic and intrinsic coordinates
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2'); clf
spm_dem_occlusion_movie(DEM)

% repeat with simulated lesion to precision
%==========================================================================
SEM        = DEM;
SEM.M(2).W = exp(0);
SEM        = spm_ADEM(SEM);
spm_DEM_qU(SEM.qU,SEM.pU)

spm_figure('GetWin','Figure 3'); clf
spm_dem_occlusion_movie(SEM)
subplot(2,2,3), title({'Angular position:','reduced precision'},'FontSize',16)
subplot(2,2,4), title({'Angular velocity:','and catch-up saccade'},'FontSize',16)

% show improved tracking of unexpected trajectories under reduced precision
%==========================================================================

% remove occlusion and switch target trajectory after one cycle
%--------------------------------------------------------------------------
i          = 50;
DEM.M(1).g = h;
DEM.G(1).g = h;
DEM.C(i:N) = -DEM.C(i:N);

% reduce precisions and integrate
%--------------------------------------------------------------------------
SEM        = DEM;
SEM.M(2).W = exp(0);

DEM        = spm_ADEM(DEM);
SEM        = spm_ADEM(SEM);

spm_figure('GetWin','Figure 4'); clf
spm_dem_occlusion_movie(DEM)
subplot(2,2,3), hold on, subplot(2,2,4), hold on
spm_dem_occlusion_movie(SEM)
subplot(2,2,3), hold off, subplot(2,2,4), hold off

% illustrate inference on hidden cause (motion of target)
%==========================================================================

% allow for uncertainty about hidden causes (frequency of motion)
%--------------------------------------------------------------------------
DEM.M(3).V = exp(-4);

% remove occlusion
%--------------------------------------------------------------------------
DEM.M(1).g = h;
DEM.G(1).g = h;

% create a longer stimulus and reduce prior expectation
%--------------------------------------------------------------------------
N      = 128;
DEM.C  = sin((1:N)*w).*((1:N) > 16);
DEM.U  = zeros(1,N) + w/8;
DEM    = spm_ADEM(DEM);

spm_figure('GetWin','Figure 5');
spm_DEM_qU(DEM.qU,DEM.pU)
subplot(3,2,5), hold on, plot([1 N],[w w]*8,'-.k','LineWidth',4), hold off

% repeat with simulated lesion to precision
%==========================================================================
SEM        = DEM;
SEM.M(2).W = exp(0);
SEM        = spm_ADEM(SEM);
spm_DEM_qU(SEM.qU,SEM.pU)

spm_figure('GetWin','Figure 6'); clf
spm_DEM_qU(SEM.qU,SEM.pU)
subplot(3,2,4), title({'hidden states','with reduced precision'},'FontSize',16)
subplot(3,2,5), hold on, plot([1 N],[w w]*8,'-.k','LineWidth',4), hold off
axis([1 N -1/2 2])


function [U2] = mle_lam0(phi2,PP,PnP)
%
%
% this function calculates the maximum likelihood function U2
% (lambda) with 0 Fourier harmonics
%
%
% zero harmonic
%
Np=sum(sum(PnP));
sumPP=sum(sum(PP));
U2=Np*log(phi2(1))-sumPP*phi2(1);