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


%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])


clear;

%Sensors
%%Acc set1
A1=diag([0.999987092709754 1]);
B1=diag([3.5596949329112e-006 1e-7]);
C1=[1 1];
sR1=(8.55478077790315e-007)^0.5;
sP1=diag([(4.9087e-007)^0.5 1e-3]);
[A1, Q1, R1]=dc2dc_v000(A1,B1*B1',sR1*sR1',1/100,2,1);
B1=Q1^0.5;
sR1=R1^0.5;

A2=A1;A3=A1;A4=A1;A5=A1;
B2=B1;B3=B1;B4=B1;B5=B1;
C2=C1;C3=C1;C4=C1;C5=C1;
sR2=sR1;sR3=sR1;sR4=sR1;sR5=sR1;
sP2=sP1;sP3=sP1;sP4=sP1;sP5=sP1;

%%Acc set2
A11=1;
B11=1e-7;%5.35244311625171e-008;
C11=1;
sR11=(8.55478077790315e-007)^0.5;
sP11=(6.8307e-007*20)^0.5;
[A11, Q11, R11]=dc2dc_v000(A11,B11*B11',sR11*sR11',1/100,2,1);
B11=Q11^0.5;
sR11=R11^0.5;

A12=A11;A13=A11;A14=A11;A15=A11;
B12=B11;B13=B11;B14=B11;B15=B11;
C12=C11;C13=C11;C14=C11;C15=C11;
sR12=sR11;sR13=sR11;sR14=sR11;sR15=sR11;
sP12=sP11;sP13=sP11;sP14=sP11;sP15=sP11;

%gyros
A6=0.999954464990211;
B6=7.50002159071526e-007*10;
C6=1;
sR6=(4.40958675010914e-008)^0.5;
vr_a=markov1st_v000(A6 ,B6,[], 2,100);
sP6=(vr_a(3))^0.5;
[A6, Q6, R6]=dc2dc_v000(A6,B6*B6',sR6*sR6',1/100,2,1);
B6=Q6^0.5;
sR6=R6^0.5;

A7=A6;B7=B6;C7=C6;sR7=sR6;sP7=sP6;

% A7=0.999977286299473;
% B7=2.06907835784507e-006;
% C7=1;
% sR7=(3.74167215805145e-007)^0.5;
% sP7=(9.4241e-008)^0.5;
% [A7, Q7, R7]=dc2dc_v000(A7,B7*B7',sR7*sR7',1/100,2,1);
% B7=Q7^0.5;
% sR7=R7^0.5;

MA1=[cos(0) sin(0) 0;
    cos(0) sin(0) 0;
    cos(-pi/5) sin(-pi/5) 0;
    cos(-pi/2) sin(-pi/2) 0;
    cos(-pi/2) sin(-pi/2) 0];

MG1=[0 0 1;0 0 1];

MA2=[cos(pi/10) sin(pi/10) 0;
    cos(pi/8) sin(pi/8) 0;
    cos(pi/6) sin(pi/6) 0;
    cos(pi/4) sin(pi/4) 0;
    cos(pi/2) sin(pi/2) 0];

%%% Main model
%M=[MA1;MG];
M=[MA1;MG1;MA2];

%imu observation structure
Rimu=sR1*sR1';
Rimu=diagmat_v000(sR2*sR2',Rimu,1);
Rimu=diagmat_v000(sR3*sR3',Rimu,1);
Rimu=diagmat_v000(sR4*sR4',Rimu,1);
Rimu=diagmat_v000(sR5*sR5',Rimu,1);
Rimu=diagmat_v000(sR6*sR6',Rimu,1);
Rimu=diagmat_v000(sR7*sR7',Rimu,1);
Rimu=diagmat_v000(sR11*sR11',Rimu,1);
Rimu=diagmat_v000(sR12*sR12',Rimu,1);
Rimu=diagmat_v000(sR13*sR13',Rimu,1);
Rimu=diagmat_v000(sR14*sR14',Rimu,1);
Rimu=diagmat_v000(sR15*sR15',Rimu,1);

Cimu=C1;
Cimu=diagmat_v000(C2,Cimu,1);
Cimu=diagmat_v000(C3,Cimu,1);
Cimu=diagmat_v000(C4,Cimu,1);
Cimu=diagmat_v000(C5,Cimu,1);
Cimu=diagmat_v000(C6,Cimu,1);
Cimu=diagmat_v000(C7,Cimu,1);
Cimu=diagmat_v000(C11,Cimu,1);
Cimu=diagmat_v000(C12,Cimu,1);
Cimu=diagmat_v000(C13,Cimu,1);
Cimu=diagmat_v000(C14,Cimu,1);
Cimu=diagmat_v000(C15,Cimu,1);

[T Mls]=cp_Tparam_v000(M,Rimu);
Cimu_obs=[T*Cimu];
Rimu_obs=T*Rimu*T';
Rimu_inp=Mls*Rimu*Mls';

A=A1;
A=diagmat_v000(A2,A,1);
A=diagmat_v000(A3,A,1);
A=diagmat_v000(A4,A,1);
A=diagmat_v000(A5,A,1);
A=diagmat_v000(A6,A,1);
A=diagmat_v000(A7,A,1);
A=diagmat_v000(A11,A,1);
A=diagmat_v000(A12,A,1);
A=diagmat_v000(A13,A,1);
A=diagmat_v000(A14,A,1);
A=diagmat_v000(A15,A,1);

Q=B1*B1';
Q=diagmat_v000(B2*B2',Q,1);
Q=diagmat_v000(B3*B3',Q,1);
Q=diagmat_v000(B4*B4',Q,1);
Q=diagmat_v000(B5*B5',Q,1);
Q=diagmat_v000(B6*B6',Q,1);
Q=diagmat_v000(B7*B7',Q,1);
Q=diagmat_v000(B11*B11',Q,1);
Q=diagmat_v000(B12*B12',Q,1);
Q=diagmat_v000(B13*B13',Q,1);
Q=diagmat_v000(B14*B14',Q,1);
Q=diagmat_v000(B15*B15',Q,1);

P=sP1*sP1';
P=diagmat_v000(sP2*sP2',P,1);
P=diagmat_v000(sP3*sP3',P,1);
P=diagmat_v000(sP4*sP4',P,1);
P=diagmat_v000(sP5*sP5',P,1);
P=diagmat_v000(sP6*sP6',P,1);
P=diagmat_v000(sP7*sP7',P,1);
P=diagmat_v000(sP11*sP11',P,1);
P=diagmat_v000(sP12*sP12',P,1);
P=diagmat_v000(sP13*sP13',P,1);
P=diagmat_v000(sP14*sP14',P,1);
P=diagmat_v000(sP15*sP15',P,1);

Pwls=P;

% %%%model for the equivalent systems
%Equivalent models for identical systems
[Mls_e1, Ae1, Be1, Ce1, sRe1, sPe1]=cp_redsys_v000(MA1(:,1:2), A1, B1, C1, sR1, sP1);
[Mls_e2, Ae2, Be2, Ce2, sRe2, sPe2]=cp_redsys_v000(MA2(:,1:2), A11, B11, C11, sR11, sP11);
[Mls_e3, Ae3, Be3, Ce3, sRe3, sPe3]=cp_redsys_v000(MG1(:,3), A6, B6, C6, sR6, sP6);

%imu observation structure
Rimue=sRe1*sRe1';
Rimue=diagmat_v000(sR6*sR6',Rimue,1);
Rimue=diagmat_v000(sR7*sR7',Rimue,1);
Rimue=diagmat_v000(sRe2*sRe2',Rimue,1);

Cimue=Ce1;
Cimue=diagmat_v000(C6,Cimue,1);
Cimue=diagmat_v000(C7,Cimue,1);
Cimue=diagmat_v000(Ce2,Cimue,1);

Me=[1 0 0;0 1 0;MG1;1 0 0;0 1 0];
[Te Mlse]=cp_Tparam_v000(Me,Rimue);
Cimu_obse=[Te*Cimue];
Rimu_obse=Te*Rimue*Te';
Rimu_inpe=Mlse*Rimue*Mlse';

%single system model
Ae=Ae1;
Ae=diagmat_v000(A6,Ae,1);
Ae=diagmat_v000(A7,Ae,1);
Ae=diagmat_v000(Ae2,Ae,1);

Qe=Be1*Be1';
Qe=diagmat_v000(B6*B6',Qe,1);
Qe=diagmat_v000(B7*B7',Qe,1);
Qe=diagmat_v000(Be2*Be2',Qe,1);

Pe=sPe1*sPe1';
Pe=diagmat_v000(sP6*sP6',Pe,1);
Pe=diagmat_v000(sP7*sP7',Pe,1);
Pe=diagmat_v000(sPe2*sPe2',Pe,1);

%%%%%%%%%%%%%%%%%%
nit=60*100;
nst=size(A,1);
nste=size(Ae,1);

x=(P^0.5)*randn(nst,1);
xest=zeros(size(A,1),1);
xeste=zeros(size(Ae,1),1);

sRimu=Rimu^0.5;
nsen=size(sRimu,1);
sQ=Q^0.5;

%debug variables
debctr=0;
debx=zeros(nst,nit);
debp=zeros(nst,nit);
debxe=zeros(nste,nit);
debpe=zeros(nste,nit);
debuopt=zeros(3,nit);
debuopte=zeros(3,nit);
debuwls=zeros(3,nit);
debpopt=zeros(3,nit);
debpopte=zeros(3,nit);
debpwls=zeros(3,nit);
debeq=zeros(5,nit);

% debctr=debctr+1;
% debx(:,debctr)=xest;
% debp(:,debctr)=diag(P).^0.5;
% debxe(:,debctr)=xeste;
% debpe(:,debctr)=diag(Pe).^0.5;

%Start main loop
for in=1:nit
    %observations
    u=[0;0;0];

    y=M*u+Cimu*x+sRimu*randn(nsen,1);
    imu_inp=Mls*y;
    imu_obs=T*y;

    ye=[Mls_e1*y(1:5);y(6:7);Mls_e2*y(8:end)];
    imu_inpe=Mlse*ye;
    imu_obse=Te*ye;

    %Kalman
    K=P*Cimu_obs'*inv(Cimu_obs*P*Cimu_obs'+Rimu_obs);
    P=P-K*Cimu_obs*P;
    dz=K*(imu_obs-Cimu_obs*xest);
    xest=xest+dz;

    Ke=Pe*Cimu_obse'*inv(Cimu_obse*Pe*Cimu_obse'+Rimu_obse);
    Pe=Pe-Ke*Cimu_obse*Pe;
    dze=Ke*(imu_obse-Cimu_obse*xeste);
    xeste=xeste+dze;

    mx_a=Mls*Cimu*P*Cimu'*Mls';
    mx_b=Mlse*Cimue*Pe*Cimue'*Mlse';
    mx_c=Mls*Cimu*Pwls*Cimu'*Mls';

    debctr=debctr+1;
    debx(:,debctr)=xest;
    debp(:,debctr)=diag(P).^0.5;
    debxe(:,debctr)=xeste;
    debpe(:,debctr)=diag(Pe).^0.5;
    debuopt(:,debctr)=Mls*y-Mls*Cimu*xest;
    debuopte(:,debctr)=Mlse*ye-Mlse*Cimue*xeste;
    debuwls(:,debctr)=Mls*(y);
    debpopt(:,debctr)=diag(mx_a+Rimu_inp).^0.5;
    debpopte(:,debctr)=diag(mx_b+Rimu_inpe).^0.5;
    debpwls(:,debctr)=diag(mx_c+Rimu_inp).^0.5;
    debeq(1:2,debctr)=Mls_e1*y(1:5);
    debeq(3,debctr)=Mls_e3*y(6:7);
    debeq(4:5,debctr)=Mls_e2*y(8:end);

    %Compute the states of next cycle
    x=A*x+sQ*randn(nst,1);

    %prediction for the next states
    xest=A*xest;
    P=A*P*A'+Q;
    Pwls=A*Pwls*A'+Q;

    xeste=Ae*xeste;
    Pe=Ae*Pe*Ae'+Qe;

end

figure(10);
yy=1;
tt=[1:10:debctr]/60;
plot(tt,debuopte(yy,1:10:debctr));
hold on;
plot(tt,debuopt(yy,1:10:debctr));
plot(tt,debuwls(yy,1:10:debctr),'k');
plot(tt,debeq(0+yy,1:10:debctr),'r');
plot(tt,debeq(3+yy,1:10:debctr),'m');

plot(tt,debpopte(yy,1:10:debctr),'--');
plot(tt,debpopt(yy,1:10:debctr),'--');
plot(tt,debpwls(yy,1:10:debctr),'--k');
grid;
xlabel('Time (min)');
ylabel('X-Acceleration Error(m/sec^2)');
%legend('Opt. S. (u^{opt})', 'Red. Opt. S. (u^{red})','WLS S. (u^{wls})','Set1 Opt. (u^{A1})','Set2 Opt. (u^{A2})','Opt. SD.', 'Red. Opt. SD.','WLS SD');
legend('Opt. S. (u^{opt})', 'Red. Opt. S. (u^{red})','WLS S. (u^{wls})','Set1 Opt. (u^{A1})','Set2 Opt. (u^{A2})');

figure(11);
yy=2;
tt=[1:10:debctr]/60;
plot(tt,debuopte(yy,1:10:debctr));
hold on;
plot(tt,debuopt(yy,1:10:debctr));
plot(tt,debuwls(yy,1:10:debctr),'k');
plot(tt,debeq(0+yy,1:10:debctr),'r');
plot(tt,debeq(3+yy,1:10:debctr),'m');

plot(tt,debpopte(yy,1:10:debctr),'--');
plot(tt,debpopt(yy,1:10:debctr),'--');
plot(tt,debpwls(yy,1:10:debctr),'--k');
grid;
xlabel('Time (min)');
ylabel('Y-Acceleration Error(m/sec^2)');
%legend('Opt. S. (u^{opt})', 'Red. Opt. S. (u^{red})','WLS S. (u^{wls})','Set1 Opt. (u^{A1})','Set2 Opt. (u^{A2})','Opt. SD.', 'Red. Opt. SD.','WLS SD');
legend('Opt. S. (u^{opt})', 'Red. Opt. S. (u^{red})','WLS S. (u^{wls})','Set1 Opt. (u^{A1})','Set2 Opt. (u^{A2})');

return;


function varargout=minDist(varargin)

% function [D1,minIND]=minDist(V1,V2,maxVarSize,selfAvoid,numFreeBytes)

%% Parse input
if nargin<2
    error('Insufficient input arguments');
end

V1=varargin{1};
V2=varargin{2};
switch nargin
    case 2
        maxVarSize=[]; %Empty will force calcucation below
        selfAvoid=0;
        numFreeBytes=[];
    case 3
        maxVarSize=varargin{3};
        selfAvoid=0;
        numFreeBytes=[];
    case 4
        maxVarSize=varargin{3};
        selfAvoid=varargin{4};
        numFreeBytes=[];
    case 5
        maxVarSize=varargin{3};
        selfAvoid=varargin{4};
        numFreeBytes=varargin{5};
end

%Get free memory
if isempty(numFreeBytes)
    [numFreeBytes]=freeMemory;
end

%Get max variable size available
if isempty(maxVarSize)
    maxVarSize=numFreeBytes/2;
end

if isnan(maxVarSize)
    numSteps=1;
else
    %Derive class dependent variable size
    [~,b1]=maxnumel(V1(1),numFreeBytes);
    [~,b2]=maxnumel(V2(1),numFreeBytes);
    b=max([b1 b2]);
    numelVar=numel(V1)*numel(V2);
    varSize=numelVar*b;

    numSteps=ceil(varSize/maxVarSize);
    indSteps=round(linspace(0,size(V1,1),numSteps));
    indSteps=sort(unique(indSteps));
    numSteps=numel(indSteps);
end

if numSteps>1 %In steps
    D1=zeros(size(V1,1),1);
    minIND=zeros(size(V1,1),1);
    for q=1:1:numSteps-1
        v1=V1(indSteps(q)+1:indSteps(q+1),:);
        try
            d=dist(v1,V2'); %dist from Neural network toolbox
        catch
            d=distND(v1,V2); %GIBBON's dist function
        end
        if selfAvoid
            %Set "diagonal" to something too large so self is avoided in
            %minimum (could use NaN and nanmin but the latter is a toolbox
            %function)
            I=1:size(v1,1);
            J=indSteps(q)+1:indSteps(q+1);
            ind=sub2ind(size(d),I,J); %Indices of selfies
            d(ind)=1+max(d(:)); %Overide selfies
        end

        [min_d,min_ind]=min(d,[],2);
        D1(indSteps(q)+1:indSteps(q+1))=min_d;
        minIND(indSteps(q)+1:indSteps(q+1))=min_ind;
    end
else %In one go
    try
        D=dist(V1,V2'); %dist from Neural network toolbox
    catch
        D=distND(V1,V2); %GIBBON's dist function
    end
    if selfAvoid
        %Set "diagonal" to something too large so self is avoided in
        %minimum (could use NaN and nanmin but the latter is a toolbox
        %function)
        L=eye(size(D))>0;
        D(L)=1+max(D(:));
    end
    [D1,minIND]=min(D,[],2);
    D1=D1(:);
    minIND=minIND(:);
end

switch nargout
    case 1
        varargout{1}=D1;
    case 2
        varargout{1}=D1;
        varargout{2}=minIND;
    otherwise
        error('wrong number of output arguments');
end

%%
% _*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) 2018  Kevin Mattheus Moerman
%
% 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/>.


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

% Cleaning
clear all
close all
clc

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

% Parameters
L     = [5 4 3]
% Xsrc  = [3.9 2.8 1.7]
% Xmes  = [2.1 2.2 1.3]
Xsrc  = [4 2 1.7];
Xmes  = [2 2 1.7];
Nray  = 1e5
rad   = 0.2

% Read mesh
mesh = msh('cube1e1.mesh');

% Mesh reshape to feat dimensions in L
mesh.vtx = 0.5 .* (1 + mesh.vtx);
mesh.vtx = (ones(size(mesh.vtx,1),1)*L) .* mesh.vtx;

% Initialize ray
ray = ray(mesh,Xsrc,Nray);

% Graphical sphere
[X,Y,Z] = sphere(50);
X = Xmes(1) + rad*X; Y = Xmes(2) + rad*Y; Z = Xmes(3) + rad*Z;

% Graphical representation
figure
plot(mesh)
hold on
% plot(ray)
surf(X,Y,Z,ones(size(X)))
axis equal;
xlabel('X');   ylabel('Y');   zlabel('Z');
alpha(0.3)
colorbar

% Material properties (http://www.odeon.dk/material-manufactures)
ray.msh.col(mesh.col==1) = 10;    % X = 1
ray.msh.col(mesh.col==2) = 20;    % Y = 1
ray.msh.col(mesh.col==3) = 30;    % X = 0
ray.msh.col(mesh.col==4) = 40;    % Y = 0
ray.msh.col(mesh.col==5) = 50;    % Z = 0
ray.msh.col(mesh.col==6) = 60;    % Z = 1
% ray.msh.col(:) = 60;

% Maximum distances
r1000 = rad/2 * sqrt(Nray/1000);
r100  = rad/2 * sqrt(Nray/100);
r10   = rad/2 * sqrt(Nray/10);
rMax  = rad/2 * sqrt(Nray/2);

% Ray-tracing
tic
ray = ray.tracer(100,rMax);
toc
% plot(ray)

% Images sources
tic
[img,nrg] = ray.image(Xmes,rad,rMax);
toc

% Graphical representation for images
n   = size(img,1);
tmp = ones(n,1)*Xmes + img;
tmp = msh(tmp,(1:n)');
plot(tmp,10*log10(nrg(:,1)));
axis equal
colorbar

% Analytical solution
load('odeon.mat')
num = [30 ; % X = 0
    10 ;    % X = 1
    40 ;    % Y = 0
    20 ;    % Y = 1
    50 ;    % Z = 0
    60];    % Z = 1
% num = 60 * ones(6,1);
mat = odeon.mat(num,:);
rhm = 30;
air = 5.5 * (50/rhm) .* (odeon.frq/1000).^1.7 * 1e-4;
tic
[imgRef,nrgRef] = rayCubeAnalytic(L,mat,air,Xsrc,Xmes,rMax);
toc

% Graphical representation
plot3(imgRef(:,1)+Xmes(1),imgRef(:,2)+Xmes(2),imgRef(:,3)+Xmes(3),'ok')

% Data
r    = sqrt(sum(img.^2,2));
rRef = sqrt(sum(imgRef.^2,2));
sol  = mean(nrg,2);
ref  = mean(nrgRef,2);

% Energy error in dB
figure
plot(rRef,10*log10(ref),'or',r,10*log10(sol),'+b')
hold on
plot([r1000 r1000],[-60,0],'k--')
text(r1000,-55,' n = 1000')
plot([r100 r100],[-60,0],'k--')
text(r100,-57,' n = 100')
plot([r10 r10],[-60,0],'k--')
text(r10,-55,' n = 10')
plot([rMax rMax],[-60,0],'k--')
text(rMax,-55,' n = 1')
xlabel('Distance source mesure')
ylabel('Energie mesuree (dB)')
title('Energie mesuree selon la distance de mesure')
legend({'Analytique','Statistique'})
grid on

% Audio file
[audio,fs] = audioread('anechoicVoice.wav');

% Fir from images
T = floor(r/340*fs) + 1;
for i = 1:size(nrg,2)
    fir8(:,i) = accumarray(T,nrg(:,i),[max(T) 1]);
end

% Bank filtering
dir8 = ray.bank(256,fs);
rir  = 0;
abs(sum(sum(dir8,2)) - 1)
for i = 1:size(dir8,2)
    rir = rir + fftfilt(dir8(:,i),fir8(:,i));
%     figure
%     freqz(dir8(:,i),1,2*256)
end

% Graphical representation
figure
plot(rir)

% Audio rendering (5 seconds)
out = fftfilt(rir,audio(1:5*fs));
% sound(out,fs)

disp('~~> Michto gypsilab !')


function soln = directCollocation(problem)
% soln = directCollocation(problem)
%
% OptimTraj utility function
%
% This function is designed to be called by either "trapezoid" or
% "hermiteSimpson". It actually calls FMINCON to solve the trajectory
% optimization problem.
%
% Analytic gradients are supported.
%
% NOTES:
%
%   If analytic gradients are used, then the sparsity pattern is returned
%   in the struct: soln.info.sparsityPattern. View it using spy().
%

%To make code more readable
G = problem.guess;
B = problem.bounds;
F = problem.func;
Opt = problem.options;

nGrid = length(F.weights);

flagGradObj = strcmp(Opt.nlpOpt.GradObj,'on');
flagGradCst = strcmp(Opt.nlpOpt.GradConstr,'on');

% Print out notice about analytic gradients
if Opt.verbose > 0
    if flagGradObj
        fprintf('      - using analytic gradients of objective function\n');
    end
    if flagGradCst
        fprintf('      - using analytic gradients of constraint function\n');
    end
    fprintf('\n');
end

% Interpolate the guess at the grid-points for transcription:
guess.tSpan = G.time([1,end]);
guess.time = linspace(guess.tSpan(1), guess.tSpan(2), nGrid);
guess.state = interp1(G.time', G.state', guess.time')';
guess.control = interp1(G.time', G.control', guess.time')';

[zGuess, pack] = packDecVar(guess.time, guess.state, guess.control);

if flagGradCst || flagGradObj
    gradInfo = grad_computeInfo(pack);
end

% Unpack all bounds:
tLow = linspace(B.initialTime.low, B.finalTime.low, nGrid);
xLow = [B.initialState.low, B.state.low*ones(1,nGrid-2), B.finalState.low];
uLow = B.control.low*ones(1,nGrid);
zLow = packDecVar(tLow,xLow,uLow);

tUpp = linspace(B.initialTime.upp, B.finalTime.upp, nGrid);
xUpp = [B.initialState.upp, B.state.upp*ones(1,nGrid-2), B.finalState.upp];
uUpp = B.control.upp*ones(1,nGrid);
zUpp = packDecVar(tUpp,xUpp,uUpp);

%%%% Set up problem for fmincon:
if flagGradObj
    P.objective = @(z)( ...
        myObjGrad(z, pack, F.pathObj, F.bndObj, F.weights, gradInfo) );   %Analytic gradients
    [~, objGradInit] = P.objective(zGuess);
    sparsityPattern.objective = (objGradInit~=0)';  % Only used for visualization!
else
    P.objective = @(z)( ...
        myObjective(z, pack, F.pathObj, F.bndObj, F.weights) );   %Numerical gradients
end
if flagGradCst
    P.nonlcon = @(z)( ...
        myCstGrad(z, pack, F.dynamics, F.pathCst, F.bndCst, F.defectCst, gradInfo) ); %Analytic gradients
    [~,~,cstIneqInit,cstEqInit] = P.nonlcon(zGuess);
    sparsityPattern.equalityConstraint = (cstEqInit~=0)';  % Only used for visualization!
    sparsityPattern.inequalityConstraint = (cstIneqInit~=0)';  % Only used for visualization!
else
    P.nonlcon = @(z)( ...
        myConstraint(z, pack, F.dynamics, F.pathCst, F.bndCst, F.defectCst) ); %Numerical gradients
end

P.x0 = zGuess;
P.lb = zLow;
P.ub = zUpp;
P.Aineq = []; P.bineq = [];
P.Aeq = []; P.beq = [];
P.options = Opt.nlpOpt;
P.solver = 'fmincon';

%%%% Call fmincon to solve the non-linear program (NLP)
tic;
[zSoln, objVal,exitFlag,output] = fmincon(P);
[tSoln,xSoln,uSoln] = unPackDecVar(zSoln,pack);
nlpTime = toc;

%%%% Store the results:

soln.grid.time = tSoln;
soln.grid.state = xSoln;
soln.grid.control = uSoln;

soln.interp.state = @(t)( interp1(tSoln',xSoln',t','linear',nan)' );
soln.interp.control = @(t)( interp1(tSoln',uSoln',t','linear',nan)' );

soln.info = output;
soln.info.nlpTime = nlpTime;
soln.info.exitFlag = exitFlag;
soln.info.objVal = objVal;

if flagGradCst || flagGradObj  % Then return sparsity pattern for visualization
    if flagGradObj
        [~, objGradInit] = P.objective(zSoln);
        sparsityPattern.objective = (objGradInit~=0)';
    end
    if flagGradCst
        [~,~,cstIneqInit,cstEqInit] = P.nonlcon(zSoln);
        sparsityPattern.equalityConstraint = (cstEqInit~=0)';
        sparsityPattern.inequalityConstraint = (cstIneqInit~=0)';
    end
    soln.info.sparsityPattern = sparsityPattern;
end

soln.problem = problem;  % Return the fully detailed problem struct

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%
%%%%                          SUB FUNCTIONS                            %%%%
%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [z,pack] = packDecVar(t,x,u)
%
% This function collapses the time (t), state (x)
% and control (u) matricies into a single vector
%
% INPUTS:
%   t = [1, nTime] = time vector (grid points)
%   x = [nState, nTime] = state vector at each grid point
%   u = [nControl, nTime] = control vector at each grid point
%
% OUTPUTS:
%   z = column vector of 2 + nTime*(nState+nControl) decision variables
%   pack = details about how to convert z back into t,x, and u
%       .nTime
%       .nState
%       .nControl
%

nTime = length(t);
nState = size(x,1);
nControl = size(u,1);

tSpan = [t(1); t(end)];
xCol = reshape(x, nState*nTime, 1);
uCol = reshape(u, nControl*nTime, 1);

indz = reshape(2+(1:numel(u)+numel(x)),nState+nControl,nTime);

% index of time, state, control variables in the decVar vector
tIdx = 1:2;
xIdx = indz(1:nState,:);
uIdx = indz(nState+(1:nControl),:);

% decision variables
% variables are indexed so that the defects gradients appear as a banded
% matrix
z = zeros(2+numel(indz),1);
z(tIdx(:),1) = tSpan;
z(xIdx(:),1) = xCol;
z(uIdx(:),1) = uCol;

pack.nTime = nTime;
pack.nState = nState;
pack.nControl = nControl;
pack.tIdx = tIdx;
pack.xIdx = xIdx;
pack.uIdx = uIdx;

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [t,x,u] = unPackDecVar(z,pack)
%
% This function unpacks the decision variables for
% trajectory optimization into the time (t),
% state (x), and control (u) matricies
%
% INPUTS:
%   z = column vector of 2 + nTime*(nState+nControl) decision variables
%   pack = details about how to convert z back into t,x, and u
%       .nTime
%       .nState
%       .nControl
%
% OUTPUTS:
%   t = [1, nTime] = time vector (grid points)
%   x = [nState, nTime] = state vector at each grid point
%   u = [nControl, nTime] = control vector at each grid point
%

nTime = pack.nTime;
nState = pack.nState;
nControl = pack.nControl;

t = linspace(z(1),z(2),nTime);

x = z(pack.xIdx);
u = z(pack.uIdx);

% make sure x and u are returned as vectors, [nState,nTime] and
% [nControl,nTime]
x = reshape(x,nState,nTime);
u = reshape(u,nControl,nTime);

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function cost = myObjective(z,pack,pathObj,bndObj,weights)
%
% This function unpacks the decision variables, sends them to the
% user-defined objective functions, and then returns the final cost
%
% INPUTS:
%   z = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   pathObj = user-defined integral objective function
%   endObj = user-defined end-point objective function
%
% OUTPUTS:
%   cost = scale cost for this set of decision variables
%

[t,x,u] = unPackDecVar(z,pack);

% Compute the cost integral along trajectory
if isempty(pathObj)
    integralCost = 0;
else
    dt = (t(end)-t(1))/(pack.nTime-1);
    integrand = pathObj(t,x,u);  %Calculate the integrand of the cost function
    integralCost = dt*integrand*weights;  %Trapazoidal integration
end

% Compute the cost at the boundaries of the trajectory
if isempty(bndObj)
    bndCost = 0;
else
    t0 = t(1);
    tF = t(end);
    x0 = x(:,1);
    xF = x(:,end);
    bndCost = bndObj(t0,x0,tF,xF);
end

cost = bndCost + integralCost;

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [c, ceq] = myConstraint(z,pack,dynFun, pathCst, bndCst, defectCst)
%
% This function unpacks the decision variables, computes the defects along
% the trajectory, and then evaluates the user-defined constraint functions.
%
% INPUTS:
%   z = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   dynFun = user-defined dynamics function
%   pathCst = user-defined constraints along the path
%   endCst = user-defined constraints at the boundaries
%
% OUTPUTS:
%   c = inequality constraints to be passed to fmincon
%   ceq = equality constraints to be passed to fmincon
%

[t,x,u] = unPackDecVar(z,pack);

%%%% Compute defects along the trajectory:
dt = (t(end)-t(1))/(length(t)-1);
f = dynFun(t,x,u);
defects = defectCst(dt,x,f);

%%%% Call user-defined constraints and pack up:
[c, ceq] = collectConstraints(t,x,u,defects, pathCst, bndCst);

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%
%%%%            Additional Sub-Functions for Gradients                 %%%%
%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function gradInfo = grad_computeInfo(pack)
%
% This function computes the matrix dimensions and indicies that are used
% to map the gradients from the user functions to the gradients needed by
% fmincon. The key difference is that the gradients in the user functions
% are with respect to their input (t,x,u) or (t0,x0,tF,xF), while the
% gradients for fmincon are with respect to all decision variables.
%
% INPUTS:
%   nDeVar = number of decision variables
%   pack = details about packing and unpacking the decision variables
%       .nTime
%       .nState
%       .nControl
%
% OUTPUTS:
%   gradInfo = details about how to transform gradients
%

nTime = pack.nTime;
nState = pack.nState;
nControl = pack.nControl;
nDecVar = 2 + nState*nTime + nControl*nTime;

zIdx = 1:nDecVar;
gradInfo.nDecVar = nDecVar;
[tIdx, xIdx, uIdx] = unPackDecVar(zIdx,pack);
gradInfo.tIdx = tIdx([1,end]);
gradInfo.xuIdx = [xIdx;uIdx];

%%%% Compute gradients of time:
% alpha = (0..N-1)/(N-1)
% t = alpha*tUpp + (1-alpha)*tLow
alpha = (0:(nTime-1))/(nTime-1);
gradInfo.alpha = [1-alpha; alpha];

if (gradInfo.tIdx(1)~=1 || gradInfo.tIdx(end)~=2)
    error('The first two decision variables must be the initial and final time')
end
gradInfo.dtGrad = [-1; 1]/(nTime-1);

%%%% Compute gradients of state
gradInfo.xGrad = zeros(nState,nTime,nDecVar);
for iTime=1:nTime
    for iState=1:nState
        gradInfo.xGrad(iState,iTime,xIdx(iState,iTime)) = 1;
    end
end

%%%% For unpacking the boundary constraints and objective:
gradInfo.bndIdxMap = [tIdx(1); xIdx(:,1); tIdx(end); xIdx(:,end)];

end

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function [c, ceq, cGrad, ceqGrad] = grad_collectConstraints(t,x,u,defects, defectsGrad, pathCst, bndCst, gradInfo)
% [c, ceq, cGrad, ceqGrad] = grad_collectConstraints(t,x,u,defects, defectsGrad, pathCst, bndCst, gradInfo)
%
% OptimTraj utility function.
%
% Collects the defects, calls user-defined constraints, and then packs
% everything up into a form that is good for fmincon. Additionally, it
% reshapes and packs up the gradients of these constraints.
%
% INPUTS:
%   t = time vector
%   x = state matrix
%   u = control matrix
%   defects = defects matrix
%   pathCst = user-defined path constraint function
%   bndCst = user-defined boundary constraint function
%
% OUTPUTS:
%   c = inequality constraint for fmincon
%   ceq = equality constraint for fmincon
%

ceq_dyn = reshape(defects,numel(defects),1);
ceq_dynGrad = grad_flattenPathCst(defectsGrad);

%%%% Compute the user-defined constraints:
if isempty(pathCst)
    c_path = [];
    ceq_path = [];
    c_pathGrad = [];
    ceq_pathGrad = [];
else
    [c_pathRaw, ceq_pathRaw, c_pathGradRaw, ceq_pathGradRaw] = pathCst(t,x,u);
    c_path = reshape(c_pathRaw,numel(c_pathRaw),1);
    ceq_path = reshape(ceq_pathRaw,numel(ceq_pathRaw),1);
    c_pathGrad = grad_flattenPathCst(grad_reshapeContinuous(c_pathGradRaw,gradInfo));
    ceq_pathGrad = grad_flattenPathCst(grad_reshapeContinuous(ceq_pathGradRaw,gradInfo));
end
if isempty(bndCst)
    c_bnd = [];
    ceq_bnd = [];
    c_bndGrad = [];
    ceq_bndGrad = [];
else
    t0 = t(1);
    tF = t(end);
    x0 = x(:,1);
    xF = x(:,end);
    [c_bnd, ceq_bnd, c_bndGradRaw, ceq_bndGradRaw] = bndCst(t0,x0,tF,xF);
    c_bndGrad = grad_reshapeBoundary(c_bndGradRaw,gradInfo);
    ceq_bndGrad = grad_reshapeBoundary(ceq_bndGradRaw,gradInfo);
end

%%%% Pack everything up:
c = [c_path;c_bnd];
ceq = [ceq_dyn; ceq_path; ceq_bnd];

cGrad = [c_pathGrad;c_bndGrad]';
ceqGrad = [ceq_dynGrad; ceq_pathGrad; ceq_bndGrad]';

end

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function C = grad_flattenPathCst(CC)
%
% This function takes a path constraint and reshapes the first two
% dimensions so that it can be passed to fmincon
%
if isempty(CC)
    C = [];
else
    [n1,n2,n3] = size(CC);
    C = reshape(CC,n1*n2,n3);
end

end

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function CC = grad_reshapeBoundary(C,gradInfo)
%
% This function takes a boundary constraint or objective from the user
% and expands it to match the full set of decision variables
%

CC = zeros(size(C,1),gradInfo.nDecVar);
CC(:,gradInfo.bndIdxMap) = C;

end

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function grad = grad_reshapeContinuous(gradRaw,gradInfo)
% grad = grad_reshapeContinuous(gradRaw,gradInfo)
%
% OptimTraj utility function.
%
% This function converts the raw gradients from the user function into
% gradients with respect to the decision variables.
%
% INPUTS:
%   stateRaw = [nOutput,nInput,nTime]
%
% OUTPUTS:
%   grad = [nOutput,nTime,nDecVar]
%

if isempty(gradRaw)
    grad = [];
else
    [nOutput, ~, nTime] = size(gradRaw);

    grad = zeros(nOutput,nTime,gradInfo.nDecVar);

    % First, loop through and deal with time.
    timeGrad = gradRaw(:,1,:); timeGrad = permute(timeGrad,[1,3,2]);
    for iOutput=1:nOutput
        A = ([1;1]*timeGrad(iOutput,:)).*gradInfo.alpha;
        grad(iOutput,:,gradInfo.tIdx) = permute(A,[3,2,1]);
    end

    % Now deal with state and control:
    for iOutput=1:nOutput
        for iTime=1:nTime
            B = gradRaw(iOutput,2:end,iTime);
            grad(iOutput,iTime,gradInfo.xuIdx(:,iTime)) = permute(B,[3,1,2]);
        end
    end
end

end

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function [cost, costGrad] = myObjGrad(z,pack,pathObj,bndObj,weights,gradInfo)
%
% This function unpacks the decision variables, sends them to the
% user-defined objective functions, and then returns the final cost
%
% INPUTS:
%   z = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   pathObj = user-defined integral objective function
%   endObj = user-defined end-point objective function
%
% OUTPUTS:
%   cost = scale cost for this set of decision variables
%

%Unpack the decision variables:
[t,x,u] = unPackDecVar(z,pack);

% Time step for integration:
dt = (t(end)-t(1))/(length(t)-1);
dtGrad = gradInfo.dtGrad;
nTime = length(t);
nState = size(x,1);
nControl = size(u,1);
nDecVar = length(z);

% Compute the cost integral along the trajectory
if isempty(pathObj)
    integralCost = 0;
    integralCostGrad = zeros(nState+nControl,1);
else

    % Objective function integrand and gradients:
    [obj, objGradRaw] = pathObj(t,x,u);
    nInput = size(objGradRaw,1);
    objGradRaw = reshape(objGradRaw,1,nInput,nTime);
    objGrad = grad_reshapeContinuous(objGradRaw,gradInfo);

    % integral objective function
    unScaledIntegral = obj*weights;
    integralCost = dt*unScaledIntegral;

    % Gradient of integral objective function
    dtGradTerm = zeros(1,nDecVar);
    dtGradTerm(1) = dtGrad(1)*unScaledIntegral;
    dtGradTerm(2) = dtGrad(2)*unScaledIntegral;
    objGrad = reshape(objGrad,nTime,nDecVar);
    integralCostGrad = ...
        dtGradTerm + ...
        dt*sum(objGrad.*(weights*ones(1,nDecVar)),1);
end

% Compute the cost at the boundaries of the trajectory
if isempty(bndObj)
    bndCost = 0;
    bndCostGrad = zeros(1,nDecVar);
else
    t0 = t(1);
    tF = t(end);
    x0 = x(:,1);
    xF = x(:,end);
    [bndCost, bndCostGradRaw] = bndObj(t0,x0,tF,xF);
    bndCostGrad = grad_reshapeBoundary(bndCostGradRaw,gradInfo);
end

% Cost function
cost = bndCost + integralCost;

% Gradients
costGrad = bndCostGrad + integralCostGrad;

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [c, ceq, cGrad, ceqGrad] = myCstGrad(z,pack,dynFun, pathCst, bndCst, defectCst, gradInfo)
%
% This function unpacks the decision variables, computes the defects along
% the trajectory, and then evaluates the user-defined constraint functions.
%
% INPUTS:
%   z = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   dynFun = user-defined dynamics function
%   pathCst = user-defined constraints along the path
%   endCst = user-defined constraints at the boundaries
%
% OUTPUTS:
%   c = inequality constraints to be passed to fmincon
%   ceq = equality constraints to be passed to fmincon
%

%Unpack the decision variables:
[t,x,u] = unPackDecVar(z,pack);

% Time step for integration:
dt = (t(end)-t(1))/(length(t)-1);
dtGrad = gradInfo.dtGrad;

% Gradient of the state with respect to decision variables
xGrad = gradInfo.xGrad;

%%%% Compute defects along the trajectory:
[f, fGradRaw] = dynFun(t,x,u);
fGrad = grad_reshapeContinuous(fGradRaw,gradInfo);

[defects, defectsGrad] = defectCst(dt,x,f,...
    dtGrad, xGrad, fGrad);

% Compute gradients of the user-defined constraints and then pack up:
[c, ceq, cGrad, ceqGrad] = grad_collectConstraints(t,x,u,...
    defects, defectsGrad, pathCst, bndCst, gradInfo);

end


function [g1, g2] = disimXdisimKernGradient(disimKern1, disimKern2, t1, t2, covGrad)

% DISIMXDISIMKERNGRADIENT Compute a cross gradient between two DISIM kernels.
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel
% between two disim kernels for the multiple output kernel.
% ARG disimKern1 : the kernel structure associated with the first DISIM
% kernel.
% ARG disimKern2 : the kernel structure associated with the second DISIM
% kernel.
% ARG t : inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see disimKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see disimKernExtractParam.
%
% FORMAT
% DESC computes cross kernel terms between two DISIM kernels for
% the multiple output kernel.
% ARG disimKern1 : the kernel structure associated with the first DISIM
% kernel.
% ARG disimKern2 : the kernel structure associated with the second DISIM
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see disimKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see disimKernExtractParam.
%
% SEEALSO : multiKernParamInit, multiKernCompute, disimKernParamInit, disimKernExtractParam
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% COPYRIGHT : Antti Honkela, 2007-2009

% KERN

arg{1}=t1;
if nargin < 5
  covGrad = t2;
  t2 = t1;
else
  arg{2}=t2;
end
if size(t1, 2) > 1 || size(t2, 2) > 1
  error('Input can only have one column');
end
if disimKern1.inverseWidth ~= disimKern2.inverseWidth
  error('Kernels cannot be cross combined if they have different inverse widths.')
end
if disimKern1.di_decay ~= disimKern2.di_decay
  error('Kernels cannot be cross combined if they have different driving input decays.');
end
if disimKern1.di_variance ~= disimKern2.di_variance
  error('Kernels cannot be cross combined if they have different driving input variances.');
end
if disimKern1.rbf_variance ~= disimKern2.rbf_variance
  error('Kernels cannot be cross combined if they have different RBF variances.');
end

delta = disimKern1.di_decay;
D1 = disimKern1.decay;
D2 = disimKern2.decay;

l = sqrt(2/disimKern1.inverseWidth);
[h1, dh1_ddelta, dh1_dD1, dh1_dD2, dh1_dl] = disimComputeH(t1, t2, delta, D1, D2, l);
[hp1, dhp1_ddelta, dhp1_dD1, dhp1_dD2, dhp1_dl] = disimComputeHPrime(t1, t2, delta, D1, D2, l);

% Avoid making the expensive call twice with the same arguments
if ((length(t1) == length(t2)) && all(t1 == t2) && ...
    (D1 == D2)),
  h2 = h1;
  dh2_ddelta = dh1_ddelta;
  dh2_dD2 = dh1_dD1;
  dh2_dD1 = dh1_dD2;
  dh2_dl = dh1_dl;

  hp2 = hp1;
  dhp2_ddelta = dhp1_ddelta;
  dhp2_dD2 = dhp1_dD1;
  dhp2_dD1 = dhp1_dD2;
  dhp2_dl = dhp1_dl;
else,
  [h2, dh2_ddelta, dh2_dD2, dh2_dD1, dh2_dl] = disimComputeH(t2, t1, delta, D2, D1, l);
  [hp2, dhp2_ddelta, dhp2_dD2, dhp2_dD1, dhp2_dl] = disimComputeHPrime(t2, t1, delta, D2, D1, l);
end

dK_ddelta = dh1_ddelta + dh2_ddelta' + dhp1_ddelta + dhp2_ddelta';
dK_dD1 = dh1_dD1 + dh2_dD1' + dhp1_dD1 + dhp2_dD1';
dK_dD2 = dh1_dD2 + dh2_dD2' + dhp1_dD2 + dhp2_dD2';
dK_dl = dh1_dl + dh2_dl' + dhp1_dl + dhp2_dl';

C0 = disimKern1.di_variance;
C1 = sqrt(disimKern1.variance);
C2 = sqrt(disimKern2.variance);
C3 = disimKern1.rbf_variance;
K = h1 + h2' + hp1 + hp2';
K = 0.5*K*sqrt(pi);
var2 = C0*C1*C2*C3;
dk_ddelta = (sum(sum(covGrad.*dK_ddelta)))*0.5*sqrt(pi)*l*var2;
dk_dD1 = (sum(sum(covGrad.*dK_dD1)))*0.5*sqrt(pi)*l*var2;
dk_dD2 = (sum(sum(covGrad.*dK_dD2)))*0.5*sqrt(pi)*l*var2;
dk_dl = sum(sum(covGrad.*(dK_dl*0.5*sqrt(pi)*l + K)))*var2;
K = l*K;
dk_dC0 = C1*C2*C3*sum(sum(covGrad.*K));
dk_dC1 = C0*C2*C3*sum(sum(covGrad.*K));
dk_dC2 = C0*C1*C3*sum(sum(covGrad.*K));
dk_dC3 = C0*C1*C2*sum(sum(covGrad.*K));

dk_dDIVariance = dk_dC0;
dk_dDisim1Variance = dk_dC1*0.5/C1;
dk_dDisim2Variance = dk_dC2*0.5/C2;
dk_dRBFVariance = dk_dC3;

dk_dinvWidth = -0.5*sqrt(2)/(disimKern1.inverseWidth* ...
                             sqrt(disimKern1.inverseWidth))*dk_dl;

K = var2*K;

if isfield(disimKern1, 'gaussianInitial') && disimKern1.gaussianInitial && ...
  isfield(disimKern2, 'gaussianInitial') && disimKern2.gaussianInitial,
  if disimKern1.initialVariance ~= disimKern2.initialVariance
    error('Kernels cannot be cross combined if they have different initial variances.');
  end

  dim1 = size(t1, 1);
  dim2 = size(t2, 1);
  t1Mat = t1(:, ones(1, dim2));
  t2Mat = t2(:, ones(1, dim1))';

  the_rest = (exp(- delta * t1Mat) - exp(- D1 * t1Mat)) ./ (D1 - delta) .* ...
      (exp(- delta * t2Mat) - exp(- D2 * t2Mat)) ./ (D2 - delta);

  dk_dinitVariance = sum(sum((sqrt(disimKern1.variance) * ...
			      sqrt(disimKern2.variance) * ...
			      the_rest) .* covGrad));
  dk_dDisim1Variance = dk_dDisim1Variance + ...
      sum(sum((.5 ./ sqrt(disimKern1.variance) * ...
	       disimKern1.initialVariance * sqrt(disimKern2.variance) * the_rest) .* covGrad));
  dk_dDisim2Variance = dk_dDisim2Variance + ...
      sum(sum((.5 ./ sqrt(disimKern2.variance) * ...
	       disimKern1.initialVariance * sqrt(disimKern1.variance) * the_rest) .* covGrad));

  dk_dD1 = dk_dD1 + ...
	   sum(sum((disimKern1.initialVariance * ...
		    sqrt(disimKern1.variance) * sqrt(disimKern2.variance) * ...
		    (t1Mat * (D1 - delta).*exp(-D1*t1Mat) - exp(-delta*t1Mat) + exp(-D1*t1Mat)) ./ (D1-delta).^2 .* ...
		    (exp(- delta * t2Mat) - exp(- D2 * t2Mat)) ./ (D2 - delta)).*covGrad));

  dk_dD2 = dk_dD2 + ...
	   sum(sum((disimKern1.initialVariance * ...
		    sqrt(disimKern1.variance) * sqrt(disimKern2.variance) * ...
		    (t2Mat * (D2 - delta).*exp(-D2*t2Mat) - exp(-delta*t2Mat) + exp(-D2*t2Mat)) ./ (D2-delta).^2 .* ...
		    (exp(- delta * t1Mat) - exp(-D1 * t1Mat)) ./ (D1 - delta)).*covGrad));

  dk_ddelta = dk_ddelta + ...
      sum(sum((disimKern1.initialVariance * ...
	       sqrt(disimKern1.variance) * sqrt(disimKern2.variance) * ...
	       ((-t2Mat * (D2 - delta).*exp(-delta*t2Mat) + exp(-delta*t2Mat) - exp(-D2*t2Mat)) ./ (D2-delta).^2 .* ...
	       (exp(- delta * t1Mat) - exp(-D1 * t1Mat)) ./ (D1 - delta) + ...
		(-t1Mat * (D1 - delta).*exp(-delta*t1Mat) + exp(-delta*t1Mat) - exp(-D1*t1Mat)) ./ (D1-delta).^2 .* ...
		(exp(- delta * t2Mat) - exp(-D2 * t2Mat)) ./ (D2 - delta))).*covGrad));

  g1 = [dk_ddelta dk_dinvWidth dk_dDIVariance dk_dD1 dk_dDisim1Variance dk_dRBFVariance dk_dinitVariance];
  g2 = [0 0 0 dk_dD2 dk_dDisim2Variance 0 0];
else
  % only pass the gradient with respect to the inverse width to one
  % of the gradient vectors ... otherwise it is counted twice.
  g1 = [dk_ddelta dk_dinvWidth dk_dDIVariance dk_dD1 dk_dDisim1Variance dk_dRBFVariance];
  g2 = [0 0 0 dk_dD2 dk_dDisim2Variance 0];
end