%
% Created by Saurabh Tavildar on 3/23/16.
%
classdef PolarCode < handle
    % Polar code
    properties

        block_length;
        info_length;
        frozen_bits;
        n;
        design_epsilon;

        bit_reversed_order;
        info_bits;

        %sc_decoder
        u_sc;
        x_sc;

        %scl decoder
        list_size;
        p_scl;
        c_scl;
        i_scl;

        inactivePathIndices;
        inactivePathIndicesSize;
        activePathArray;
        pathIndexToArrayIndex;
        inactiveArrayIndices;
        inactiveArrayIndicesSize;
        arrayReferenceCount;

        lambda_offset;
        list_offset;

        llr_based_computation;
        llr_scl;
        llr_path_metric;

        % CRC
        crc_matrix;
        crc_size;

        %code construction
        cc_method;
        cc_parameter;
        cc_misc;

        %
        info_bit_order
    end

    methods

        %% code construction
        function obj = PolarCode(block_length, info_length, design_epsilon, crc_size)

            obj.block_length = block_length;
            obj.info_length = info_length;
            obj.n = log2(block_length);
            obj.design_epsilon = design_epsilon;

            if nargin < 4
                crc_size = 0;
            end

            obj.crc_size = crc_size;

            if obj.crc_size ~= 0
                obj.crc_matrix = floor(2*rand(obj.crc_size, info_length));
            end

            obj.bit_reversed_order = bitrevorder((1:obj.block_length)');

            channels = PolarCode.calculate_channel_polarization(design_epsilon, obj.n );
            channels = channels(obj.bit_reversed_order);
            [~, info_bits_sorted] = sort(channels, 'ascend');
            obj.info_bit_order = info_bits_sorted(obj.block_length:-1:1);
            obj.frozen_bits = ones(1,obj.block_length);
            obj.frozen_bits(info_bits_sorted(1:obj.info_length + obj.crc_size)) = 0;
            obj.info_bits = info_bits_sorted(1:obj.info_length + obj.crc_size);
            obj.llr_based_computation = 0;

            obj.cc_method = 'bhattacharya';
            obj.cc_parameter = design_epsilon;
            obj.cc_misc = '';

            disp(['Bhattacharya code construction done. BLER estimate:', num2str(sum(channels(obj.info_bits)))]);

        end

        function monte_carlo_code_construction(obj, design_snr_db, num_runs, constellation_name, receiver_algo)
            if (nargin < 3) || isempty(num_runs)
                num_runs = 100e3;
            end
            if (nargin < 4) || isempty(constellation_name)
                constellation_name = 'bpsk';
            end
            if (nargin < 5) || isempty(receiver_algo)
                receiver_algo = 'bicm';
            end

            obj.cc_method = 'monte-carlo';
            obj.cc_parameter = design_snr_db;
            obj.cc_misc = [constellation_name, '_', receiver_algo, '_', num2str(num_runs)];

            txt_file_name = ['CodeConstructionData/MC_block_length_', obj.get_unique_string(), '.txt'];

            if exist(txt_file_name, 'file')
                channels = load(txt_file_name);
                disp(['Found monte carlo BER pattern: ', txt_file_name]);
            else
                disp(['Didnt find monte carlo BER here: ', txt_file_name]);
                disp('Hence, determining the  monte carlo ber.');
                channels = obj.monte_carlo(design_snr_db, num_runs, constellation_name, receiver_algo);
                fileID = fopen(txt_file_name,'w');
                for c  = channels
                    fprintf(fileID,'%d \n',c);
                end
                fclose(fileID);
            end
            [~, channel_order] = sort(channels, 'ascend');
            if channels(channel_order(obj.info_length + obj.crc_size)) < 100
                disp('Warning: not enough runs to get a reliable code.');
                disp(['Worst # of errors = ', num2str(channels(channel_order(obj.info_length + obj.crc_size)))]);
            end

            obj.info_bits = channel_order(1:obj.info_length + obj.crc_size);

            obj.frozen_bits = ones(1,obj.block_length);
            obj.frozen_bits(obj.info_bits) = 0;
            bler_estimate = sum(channels(obj.info_bits))/num_runs;

            disp(['Monte carlo code construction done. Bler estimate = ', num2str(bler_estimate)]);

        end

        function num_err = monte_carlo(obj, design_snr_db, num_runs, constellation_name, receiver)

            num_err = zeros(obj.block_length, 1);
            modulation = Constellation(constellation_name);

            for i_run  = 1 : num_runs
                if mod(i_run, ceil(num_runs/10)) == 1
                    disp(['Code construction iteration running = ', num2str(i_run)]);
                end
                if strcmp(receiver, 'bicm')

                    dummy_info = (rand(1, obj.block_length) < 0.5);
                    dummy_coded = PolarCode.polar_encode(dummy_info);

                elseif strcmp(receiver, 'mlc')

                    num_codes = modulation.n_bits;
                    dummy_info = (rand(num_codes, obj.block_length/num_codes) < 0.5);
                    dummy_coded = zeros(obj.block_length, 1);
                    for layer = 1 : num_codes
                        dummy_coded(layer:num_codes:obj.block_length) = PolarCode.polar_encode(dummy_info(layer,:))';
                    end

                end

                mod_sym = modulation.modulate(dummy_coded);
                sigma = sqrt(1/2) *  10^(-design_snr_db/20);
                noise = sigma * randn(length(mod_sym), 1);
                y = mod_sym + noise;

                if strcmp(receiver, 'bicm')

                    p1 = 0.5 * ones(obj.block_length, 1);
                    eff_block_length = floor(obj.block_length/modulation.n_bits)*modulation.n_bits;
                    [p1(1:eff_block_length),~] = modulation.compute_llr_bicm(y, sigma^2);
                    [~, ber_tmp] = PolarCode.polar_decode_monte(p1, dummy_info);
                    num_err = num_err + ber_tmp';

                elseif strcmp(receiver, 'mlc')

                    decoded_coded = zeros(obj.block_length/num_codes, num_codes);
                    ber = zeros(obj.block_length/num_codes, num_codes);

                    for layer = 1 : num_codes
                        u = decoded_coded(:, 1:layer-1);
                        [p1,~] = modulation.compute_llr_mlc(y, sigma^2, u);
                        [decoded_coded(:, layer), ber(:, layer)] = PolarCode.polar_decode_monte(p1, dummy_info(layer, :));
                    end
                    ber = ber(:);
                    num_err = num_err + ber;

                end
            end
        end

        function [bler_estimate] = ga_code_construction(obj, design_snr_db, constellation_name, receiver_algo)

            modulation = Constellation(constellation_name);

            if strcmp(receiver_algo, 'bicm')

                if modulation.n_bits == 1
                    capacity = modulation.get_bicm_capacity(design_snr_db);
                else
                    capacity = modulation.get_polarized_capacity(design_snr_db);
                end

            elseif strcmp(receiver_algo, 'mlc')

                capacity = modulation.get_mlc_capacity(design_snr_db);

            end

            num_codes = modulation.n_bits;

            mean_llrs = get_bpsk_llr_for_capacity(capacity);

            llr_vec = zeros(obj.block_length, 1);

            if modulation.n_bits == 3  && strcmp(receiver_algo, 'mlc') %non power of 2 not supported
                disp('MLC code construction not supported');
            else
                for i_code = 1 : num_codes
                    bit_loc = (i_code-1) * obj.block_length/num_codes + 1 : i_code * obj.block_length/num_codes;
                    llr_vec(bit_loc) = mean_llrs(i_code);
                end
            end

            bit_rev_order_subcode = bitrevorder(1:obj.block_length/ num_codes);
            channels = zeros(obj.block_length, 1);

            for i_code = 1 : num_codes
                bit_loc = (i_code-1) * obj.block_length/num_codes + 1 : i_code * obj.block_length/num_codes;
                tmp_c = calculate_awgn_polarization(llr_vec((bit_loc)).', obj.n - log2(num_codes));
                channels(bit_loc) = tmp_c(bit_rev_order_subcode);
            end

            [~, channel_order] = sort(channels, 'descend');
            obj.info_bit_order = channel_order(obj.block_length:-1:1);

            obj.info_bits = channel_order(1:obj.info_length + obj.crc_size);
            obj.frozen_bits = ones(1,obj.block_length);
            obj.frozen_bits(obj.info_bits) = 0;
            bler_estimate =  sum(qfunc(  sqrt(channels(obj.info_bits))/sqrt(2)));

            obj.cc_method = 'gauss-approx';
            obj.cc_parameter = design_snr_db;
            obj.cc_misc = [constellation_name, '_', receiver_algo];

            disp(['GA code construction done. BLER estimate:', num2str(bler_estimate)]);

        end

        function [unique_string] = get_unique_string(obj)
            unique_string = [num2str(obj.block_length), '_', num2str(length(obj.info_bits)),  ...
                '_cc_method_', obj.cc_method, '_cc_param_', num2str(obj.cc_parameter), '_', obj.cc_misc];
        end

        %% encoder

        function coded_bits = encode(obj, info_bits)

            info_bits_padded = zeros(obj.block_length, 1);
            if obj.crc_size ~= 0
                crc = mod(obj.crc_matrix*info_bits', 2);
                info_bits = [info_bits, crc'];
            end
            info_bits_padded(obj.info_bits) = info_bits;
            coded_bits = PolarCode.polar_encode(info_bits_padded);

        end

        %%  Decoder helper

        function [b] = crc_check(obj, info_bits)
            info = info_bits(1:obj.info_length);
            crc  = info_bits(obj.info_length + 1:obj.info_length + obj.crc_size);
            crc_check =  mod(obj.crc_matrix*info', 2)';
            b = 1 - any(crc ~= crc_check);
        end

        %%  SC decoder

        function decoded_bits = decode_sc_p1(obj, p1)

            [decoded_bits, ~] = PolarCode.polar_decode(p1, obj.frozen_bits);
            decoded_bits = decoded_bits(obj.info_bits(1:obj.info_length));

        end

        %%  SCL decoder

        function [u] = decode_scl_p1(obj, p1, p0, list_size)

            obj.list_size = list_size;
            obj.llr_based_computation  = 0;
            obj.initializeDataStructures();
            l_index = obj.assignInitialPath();
            s_index = obj.getArrayPointer_P(0, l_index);
            obj.p_scl(obj.get_i_scl(0, 0, s_index) + 1:obj.get_i_scl(0, obj.block_length - 1, s_index) + 1 , 1) = p0;
            obj.p_scl(obj.get_i_scl(0, 0, s_index) + 1:obj.get_i_scl(0, obj.block_length - 1, s_index) + 1 , 2) = p1;
            u = obj.polar_decode_scl();

        end

        function [u] = decode_scl_llr(obj, llr, list_size)

            obj.list_size = list_size;
            obj.llr_based_computation = 1;
            obj.initializeDataStructures();
            l_index = obj.assignInitialPath();
            s_index = obj.getArrayPointer_P(0, l_index);
            obj.llr_scl( obj.get_i_scl(0, 0, s_index) + 1:obj.get_i_scl(0, obj.block_length - 1, s_index) + 1) = llr;
            u = obj.polar_decode_scl();

        end

        function [u] = polar_decode_scl(obj)

            for phi = 0 : obj.block_length - 1

                obj.recursivelyCalcP_scl(obj.n, phi);

                if obj.frozen_bits(phi + 1) == 1
                    obj.continuePaths_FrozenBit(phi);
                else
                    obj.continuePaths_UnfrozenBit(phi);
                end

                if mod(phi, 2) == 1
                    obj.recursivelyUpdateC_scl(obj.n, phi);
                end

            end

            l_index = obj.findMostProbablePath(1);
            c_m = obj.getArrayPointer_C(obj.n, l_index);
            info = obj.i_scl(c_m+1,:);
            u = info(obj.info_bits(1:obj.info_length));

        end

        function index = get_i_scl(obj, lambda, beta, list_index)
            index = beta + obj.lambda_offset(lambda + 1) + obj.list_offset(list_index + 1);
        end

        function init_i_scl(obj)
            obj.lambda_offset = (2.^(obj.n - (0: obj.n)) - 1);
            obj.list_offset = (0:obj.list_size)*(2 * obj.block_length - 1);

        end

        function initializeDataStructures(obj)

            obj.inactivePathIndices = zeros(obj.list_size,1);
            obj.inactivePathIndicesSize = 0;
            % the above two variables are used to define a stack

            obj.activePathArray =  zeros(obj.list_size,1);
            obj.pathIndexToArrayIndex = zeros(obj.n  + 1, obj.list_size);

            obj.inactiveArrayIndices = zeros(obj.n  + 1, obj.list_size);
            obj.inactiveArrayIndicesSize = zeros(obj.n + 1, 1);
            % the above two variables are used to define a vector of stacks

            obj.arrayReferenceCount = zeros(obj.n  + 1, obj.list_size);

            if obj.llr_based_computation
                obj.llr_scl = zeros(obj.list_size * (2 * obj.block_length - 1), 1);
                obj.llr_path_metric =  zeros(obj.list_size, 1);
            else
                obj.p_scl = zeros(obj.list_size * (2 * obj.block_length - 1), 2);
            end

            obj.c_scl = zeros(obj.list_size * (2 * obj.block_length - 1), 2);
            obj.i_scl = zeros(obj.list_size, obj.block_length);
            obj.init_i_scl();

            for lambda = 0 : obj.n
                for i_list = 0 : obj.list_size - 1
                    obj.inactiveArrayIndices(lambda + 1, i_list + 1) = i_list;

                end
                obj.inactiveArrayIndicesSize(lambda + 1) = obj.list_size;
            end

            for i_list = 0 : obj.list_size - 1
                obj.activePathArray(i_list + 1) = 0;
                obj.inactivePathIndices(i_list + 1) = i_list;
            end

            obj.inactivePathIndicesSize  = obj.list_size;

        end

        function l_index = assignInitialPath(obj)
            l_index = obj.inactivePathIndices(obj.inactivePathIndicesSize);
            obj.inactivePathIndicesSize = obj.inactivePathIndicesSize - 1;
            obj.activePathArray(l_index + 1) = 1;

            for lambda = 0 : obj.n
                s = obj.inactiveArrayIndices(lambda + 1, obj.inactiveArrayIndicesSize(lambda + 1));
                obj.inactiveArrayIndicesSize(lambda + 1) = obj.inactiveArrayIndicesSize(lambda + 1) - 1;
                obj.pathIndexToArrayIndex(lambda + 1, l_index + 1) = s;
                obj.arrayReferenceCount(lambda + 1, l_index + 1) = 1;
            end
        end

        function l_p_index = clonePath(obj, l_index)
            l_p_index = obj.inactivePathIndices(obj.inactivePathIndicesSize);
            obj.inactivePathIndicesSize = obj.inactivePathIndicesSize - 1;
            obj.activePathArray(l_p_index + 1) = 1;

            if obj.llr_based_computation
                obj.llr_path_metric(l_p_index + 1) = obj.llr_path_metric(l_index + 1);
            end

            for lambda = 0 : obj.n
                s = obj.pathIndexToArrayIndex(lambda + 1, l_index + 1);
                obj.pathIndexToArrayIndex(lambda + 1, l_p_index + 1) = s;
                obj.arrayReferenceCount(lambda + 1, s + 1) = obj.arrayReferenceCount(lambda + 1, s + 1) + 1;
            end
        end

        function killPath(obj, l_index)
            obj.activePathArray(l_index + 1) = 0;
            obj.inactivePathIndices(obj.inactivePathIndicesSize + 1) = l_index;
            obj.inactivePathIndicesSize = obj.inactivePathIndicesSize  + 1;
            if obj.llr_based_computation
                obj.llr_path_metric(l_index + 1) = 0;
            end
            for lambda = 0 : obj.n
                s = obj.pathIndexToArrayIndex(lambda + 1, l_index + 1);
                obj.arrayReferenceCount(lambda + 1, s + 1) = obj.arrayReferenceCount(lambda + 1, s + 1) - 1;
                if obj.arrayReferenceCount(lambda + 1, s + 1) == 0
                    obj.inactiveArrayIndices(lambda + 1, obj.inactiveArrayIndicesSize(lambda + 1) + 1) = s;
                    obj.inactiveArrayIndicesSize(lambda + 1)  = obj.inactiveArrayIndicesSize(lambda + 1)  + 1;
                end
            end
        end

        function [b] = pathIndexInactive(obj, l_index)
            b = 1 - obj.activePathArray(l_index + 1);
        end

        function [s_p] = getArrayPointer_P(obj, lambda, l_index)
            s = obj.pathIndexToArrayIndex(lambda + 1, l_index + 1);
            m = obj.n;
            if obj.arrayReferenceCount(lambda + 1, s + 1) == 1
                s_p = s;
            else
                s_p = obj.inactiveArrayIndices(lambda + 1, obj.inactiveArrayIndicesSize(lambda + 1));
                i_s_p = obj.lambda_offset(lambda + 1) + obj.list_offset(s_p + 1) + 1: ...
                    obj.lambda_offset(lambda + 1) + obj.list_offset(s_p + 1) + 2^(m - lambda);
                i_s = obj.lambda_offset(lambda + 1) + obj.list_offset(s + 1) + 1: ...
                    obj.lambda_offset(lambda + 1) + obj.list_offset(s + 1) + 2^(m - lambda);
                obj.c_scl(i_s_p, :) = obj.c_scl(i_s, :);
                if obj.llr_based_computation
                    obj.llr_scl(i_s_p) = obj.llr_scl(i_s);
                else
                    obj.p_scl(i_s_p, :) = obj.p_scl(i_s, :);
                end
                obj.inactiveArrayIndicesSize(lambda + 1) = obj.inactiveArrayIndicesSize(lambda + 1) - 1;
                obj.arrayReferenceCount(lambda + 1, s + 1) = obj.arrayReferenceCount(lambda + 1, s + 1) - 1;
                obj.arrayReferenceCount(lambda + 1, s_p + 1) = 1;
                obj.pathIndexToArrayIndex(lambda + 1, l_index + 1) = s_p;
            end
        end

        function [s_p] = getArrayPointer_C(obj, lambda, l_index)
            s = obj.pathIndexToArrayIndex(lambda + 1, l_index + 1);
            m = obj.n;
            if obj.arrayReferenceCount(lambda + 1, s + 1) == 1
                s_p = s;
            else
                s_p = obj.inactiveArrayIndices(lambda + 1, obj.inactiveArrayIndicesSize(lambda + 1));
                i_s_p = obj.lambda_offset(lambda + 1) + obj.list_offset(s_p + 1) + 1: ...
                    obj.lambda_offset(lambda + 1) + obj.list_offset(s_p + 1) + 2^(m - lambda);
                i_s = obj.lambda_offset(lambda + 1) + obj.list_offset(s + 1) + 1: ...
                    obj.lambda_offset(lambda + 1) + obj.list_offset(s + 1) + 2^(m - lambda);
                if obj.llr_based_computation
                    obj.llr_scl(i_s_p) = obj.llr_scl(i_s);
                else
                    obj.p_scl(i_s_p, :) = obj.p_scl(i_s, :);
                end
                obj.c_scl(i_s_p, :) = obj.c_scl(i_s, :);
                obj.inactiveArrayIndicesSize(lambda + 1) = obj.inactiveArrayIndicesSize(lambda + 1) - 1;
                obj.arrayReferenceCount(lambda + 1, s + 1) = obj.arrayReferenceCount(lambda + 1, s + 1) - 1;
                obj.arrayReferenceCount(lambda + 1, s_p + 1) = 1;
                obj.pathIndexToArrayIndex(lambda + 1, l_index + 1) = s_p;
            end
        end

        function recursivelyCalcP_scl(obj, lambda, phi)

            if lambda == 0
                return;
            end

            psi = floor(phi/2);
            if mod(phi, 2) == 0
                obj.recursivelyCalcP_scl(lambda - 1, psi);
            end

            sigma  = 0;
            p_index_3_base_list = zeros(obj.list_size, 1);
            l_index_1_list = zeros(obj.list_size, 1);
            for l_index = 0 : obj.list_size - 1
                if obj.activePathArray(l_index + 1) == 0
                    continue;
                end
                l_index_1_list(l_index+1) = obj.getArrayPointer_P(lambda, l_index);
                l_index_2 = obj.getArrayPointer_P(lambda - 1, l_index);
                l_index_3 = obj.getArrayPointer_C(lambda, l_index);

                p_index_1_base = obj.lambda_offset(lambda) + obj.list_offset(l_index_2 + 1) + 1;
                p_index_3_base_list(l_index+1) = obj.lambda_offset(lambda + 1) + obj.list_offset(l_index_1_list(l_index+1) + 1) + 1;
                c_index_3_base = obj.lambda_offset(lambda + 1) + obj.list_offset(l_index_3 + 1) + 1;
                for beta = 0: 2^(obj.n - lambda) - 1
                    p_index_1 = p_index_1_base + 2 * beta;
                    p_index_2 = p_index_1_base + 2 * beta + 1;
                    p_index_3 = p_index_3_base_list(l_index+1) + beta;
                    if mod(phi, 2) == 0
                        if obj.llr_based_computation
                            if max( abs(obj.llr_scl ( p_index_1)), abs(obj.llr_scl ( p_index_2)) ) < 40
                                obj.llr_scl(p_index_3) = PolarCode.cnop_llr( obj.llr_scl ( p_index_1), obj.llr_scl ( p_index_2));
                                % log( (exp( obj.llr_scl ( p_index_1) + obj.llr_scl ( p_index_2)) + 1) ...
                                %    /(exp( obj.llr_scl ( p_index_1))  + exp( obj.llr_scl ( p_index_2))) );
                            else
                                obj.llr_scl(p_index_3) = sign( obj.llr_scl ( p_index_1)) * sign(obj.llr_scl ( p_index_2)) * min(abs(obj.llr_scl ( p_index_2)), abs(obj.llr_scl ( p_index_1)));
                            end
                        else
                            obj.p_scl(p_index_3, 1) =  0.5 * ( obj.p_scl ( p_index_1 , 1) *  obj.p_scl(p_index_2, 1)  +  obj.p_scl ( p_index_1 , 2) *  obj.p_scl(p_index_2, 2));
                            obj.p_scl(p_index_3, 2) =  0.5 * ( obj.p_scl ( p_index_1 , 2) *  obj.p_scl(p_index_2, 1)  +  obj.p_scl ( p_index_1 , 1) *  obj.p_scl(p_index_2, 2));
                            sigma = max(obj.p_scl(p_index_3, 1), sigma);
                            sigma = max(obj.p_scl(p_index_3, 2), sigma);
                        end
                    else
                        u_p = obj.c_scl( c_index_3_base + beta, 1);
                        if obj.llr_based_computation
                            obj.llr_scl(p_index_3) = (-1)^u_p * obj.llr_scl(p_index_1) +  obj.llr_scl(p_index_2);
                        else
                            obj.p_scl(p_index_3, 1)  = 0.5 * obj.p_scl (p_index_1, mod(u_p, 2) + 1) * ...
                                obj.p_scl(p_index_2, 1);
                            obj.p_scl(p_index_3, 2)  = 0.5 * obj.p_scl (p_index_1, mod(u_p + 1, 2) + 1) * ...
                                obj.p_scl(p_index_2, 2);
                            sigma = max(obj.p_scl(p_index_3, 1), sigma);
                            sigma = max(obj.p_scl(p_index_3, 2), sigma);
                        end
                    end
                end
            end

            for l_index = 0 : obj.list_size - 1
                if sigma == 0 %typically happens because of underflow
                    break;
                end
                if obj.activePathArray(l_index + 1) == 0
                    continue;
                end
                p_range = p_index_3_base_list(l_index+1): p_index_3_base_list(l_index+1) +  2^(obj.n - lambda) - 1;
                obj.p_scl(p_range, :) = obj.p_scl(p_range, :)/sigma;
            end

        end

        function recursivelyUpdateC_scl(obj, lambda, phi)
            if mod(phi, 2) == 0
                disp('Error: phi should always be odd in this function call');
            end
            psi = floor(phi/2);

            for l_index = 0 : obj.list_size - 1
                if obj.activePathArray(l_index + 1) == 0
                    continue;
                end

                l_index_1 = obj.getArrayPointer_C(lambda, l_index);
                l_index_2 = obj.getArrayPointer_C(lambda - 1, l_index);

                p_index_1 = obj.lambda_offset(lambda + 1) + obj.list_offset(l_index_1 + 1)  + 1;
                p_index_2 = obj.lambda_offset(lambda) + obj.list_offset(l_index_2 + 1)  +  1;

                for beta = 0: 2^(obj.n - lambda) - 1
                    obj.c_scl(p_index_2 + 2*beta, mod(psi, 2) + 1) = ...
                        mod( obj.c_scl(p_index_1 + beta, 1) + ...
                        obj.c_scl(p_index_1 + beta, 2),  2);
                    obj.c_scl(p_index_2 + 2 * beta + 1, mod(psi, 2) + 1) = ...
                        obj.c_scl(p_index_1 + beta, 2);
                end

            end

            if mod(psi, 2) == 1
                obj.recursivelyUpdateC_scl(lambda - 1, psi);
            end

        end

        function continuePaths_FrozenBit(obj, phi)

            for l_index = 0 : obj.list_size -1

                if obj.activePathArray(l_index + 1) == 0
                    continue;
                end

                l_index_1 = obj.getArrayPointer_C(obj.n, l_index);
                obj.c_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1, mod(phi, 2) + 1) = 0;
                if obj.llr_based_computation
                    obj.llr_path_metric(l_index_1 + 1) = obj.llr_path_metric(l_index_1 + 1) ...
                        + log(1 + exp(-obj.llr_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1)));
                end
            end

        end

        function    continuePaths_UnfrozenBit(obj, phi)

            probForks = -realmax * ones(obj.list_size, 2);
            index = 0;
            for l_index = 0 : obj.list_size - 1

                if obj.activePathArray(l_index + 1)
                    l_index_1 = obj.getArrayPointer_P(obj.n, l_index);
                    if obj.llr_based_computation
                        % computing negative of path metric so that an
                        % ascending order can be used for sorting
                        probForks(l_index + 1, 1) =  - (obj.llr_path_metric(l_index_1 + 1) ...
                            + log(1 + exp(-obj.llr_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1))));
                        probForks(l_index + 1, 2) =  - ( obj.llr_path_metric(l_index_1 + 1) ...
                            + log(1 + exp(obj.llr_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1))));
                    else
                        probForks(l_index + 1, 1) = obj.p_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1, 1);
                        probForks(l_index + 1, 2) = obj.p_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1, 2);
                    end
                    index = index + 1;

                end
            end

            rho = min(2*index, obj.list_size);
            contForks = zeros(obj.list_size, 2);
            prob = sort(probForks(:), 'descend');

            threshold = prob(rho);
            num_populated = 0;
            for l_index = 0 : obj.list_size - 1
                for j_index = 1 : 2
                    if num_populated == rho
                        break;
                    end
                    if  probForks(l_index + 1, j_index) > threshold
                        contForks(l_index + 1, j_index) = 1;
                        num_populated = num_populated + 1;
                    end
                end
            end

            if num_populated < rho
                for l_index = 0 : obj.list_size - 1
                    for j_index = 1 : 2
                        if num_populated == rho
                            break;
                        end
                        if  probForks(l_index + 1, j_index) == threshold
                            contForks(l_index + 1, j_index) = 1;
                            num_populated = num_populated + 1;
                        end
                    end
                end
            end

            for l_index = 0 : obj.list_size - 1
                if obj.activePathArray(l_index + 1) == 0
                    continue;
                end

                if (contForks(l_index + 1, 1) == 0) && ( contForks(l_index + 1, 2) == 0)
                    obj.killPath(l_index);
                end
            end

            for l_index = 0 : obj.list_size - 1
                if (contForks(l_index + 1, 1) == 0) && ( contForks(l_index + 1, 2) == 0)
                    continue;
                end
                l_index_1 = obj.getArrayPointer_C(obj.n, l_index);
                if (contForks(l_index + 1, 1) == 1) && ( contForks(l_index + 1, 2) == 1)
                    obj.c_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1, mod(phi,2) + 1) = 0;
                    obj.i_scl(l_index_1 + 1, phi + 1) = 0;

                    l_p = obj.clonePath(l_index);

                    l_index_2 = obj.getArrayPointer_C(obj.n, l_p);
                    obj.i_scl(l_index_2 + 1, 1 : phi) = obj.i_scl(l_index_1 + 1, 1 : phi);
                    obj.c_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_2 + 1) + 1, mod(phi,2) + 1) = 1;
                    obj.i_scl(l_index_2 + 1, phi + 1) = 1;
                    if obj.llr_based_computation
                        obj.llr_path_metric(l_index + 1) = obj.llr_path_metric(l_index + 1) ...
                            + log(1 + exp(-obj.llr_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1)));
                        obj.llr_path_metric(l_p + 1) = obj.llr_path_metric(l_p + 1) ...
                            + log(1 + exp(obj.llr_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_2 + 1) + 1)));
                    end

                else
                    if contForks(l_index + 1, 1) == 1
                        obj.c_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1, mod(phi,2) + 1) = 0;
                        obj.i_scl(l_index_1 + 1, phi + 1) = 0;
                        if obj.llr_based_computation
                            obj.llr_path_metric(l_index + 1) = obj.llr_path_metric(l_index + 1) ...
                                + log(1 + exp(-obj.llr_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1)));
                        end
                    else
                        obj.c_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1, mod(phi,2) + 1) = 1;
                        obj.i_scl(l_index_1 + 1, phi + 1) = 1;
                        if obj.llr_based_computation
                            obj.llr_path_metric(l_index + 1) = obj.llr_path_metric(l_index + 1) ...
                                + log(1 + exp(obj.llr_scl(obj.lambda_offset(obj.n + 1) + obj.list_offset(l_index_1 + 1) + 1)));
                        end
                    end
                end

            end

        end

        function [l_p_index] = findMostProbablePath(obj, crc_check)
            l_p_index = 0;

            p_max = 0;
            if obj.llr_based_computation
                p_max = realmax;
            end
            path_with_crc = 0;
            for l_index = 0 : obj.list_size -1

                if obj.activePathArray(l_index + 1) == 0
                    continue;
                end

                c_index = obj.getArrayPointer_C( obj.n, l_index);
                if (crc_check) && (obj.crc_size ~= 0)
                    a = obj.i_scl(c_index+1,:);
                    u = a(obj.info_bits);
                    if obj.crc_check(u) == 0
                        continue;
                    end
                end
                path_with_crc = 1;
                if obj.llr_based_computation
                    if p_max > obj.llr_path_metric(l_index + 1)
                        p_max = obj.llr_path_metric(l_index + 1);
                        l_p_index = l_index;
                    end
                else
                    p_index = obj.getArrayPointer_P( obj.n, l_index);
                    if p_max < obj.p_scl(obj.get_i_scl( obj.n, 0, p_index) + 1, obj.c_scl(obj.get_i_scl( obj.n, 0, c_index) + 1, 2) + 1)
                        l_p_index = l_index;
                        p_max  = obj.p_scl(obj.get_i_scl( obj.n, 0, p_index) + 1, obj.c_scl(obj.get_i_scl( obj.n, 0, c_index) + 1, 2) + 1);
                    end
                end
            end

            if (crc_check) && (path_with_crc == 0) % no path with crc check found
                l_p_index = obj.findMostProbablePath(0);
            end

        end

        %% helper function
        function [ bler, ber ] = get_bler_quick(obj, ebno_vec, list_size_vec)

            snr_db_vec = ebno_vec + 10*log10(obj.info_length/obj.block_length);

            num_block_err = zeros(length(ebno_vec), length(list_size_vec));
            num_bit_err = zeros(length(ebno_vec), length(list_size_vec));
            num_runs = zeros(length(ebno_vec), length(list_size_vec));
            max_err = 50;
            max_runs = 500;

            for i_run = 1 : max_runs

                if mod(i_run, ceil(max_runs/10)) == 1
                    disp(['Sim iteration running = ', num2str(i_run)]);
                end
                info = rand(1 , obj.info_length) < 0.5;
                coded_bits = obj.encode(info);
                bpsk = 2 * coded_bits - 1;
                sigma = sqrt(1/2);
                noise = sigma * randn(1, obj.block_length);
                prev_decoded = zeros(length(list_size_vec), length(ebno_vec));

                for i_ebno = 1 : length(ebno_vec)
                    snr_db = snr_db_vec(i_ebno);
                    received_bits = 10^(snr_db/20) * bpsk + noise;
                    p1 = exp(-(received_bits - 10^(snr_db/20)).^2/(2 * sigma^2))/sigma/sqrt(2*pi);
                    p0 = exp(-(received_bits + 10^(snr_db/20)).^2/(2 * sigma^2))/sigma/sqrt(2*pi);

                    for i_list = 1 : length(list_size_vec)
                        if num_block_err(i_ebno, i_list) > max_err
                            continue;
                        end

                        num_runs(i_ebno,  i_list) =  num_runs(i_ebno, i_list) + 1;

                        run_sim = 1;

                        for i_ebno2 = 1 : i_ebno
                            if prev_decoded(i_list, i_ebno2)
                                run_sim = 0;
                            end
                        end

                        if (run_sim == 0)
                            % This is a hack to speed up simulations --
                            % it assumes that this run will be decoded correctly since it was
                            % decoded correctly for a lower EbNo
                            continue;
                        end
                        if list_size_vec(i_list) == 1
                            decoded_bits = obj.decode_sc_p1(p1./(p1+p0));
                        else
                            %  decoded_bits = obj.decode_scl_p1(p1, p0, list_size_vec(i_list));
                            decoded_bits = obj.decode_scl_llr(log(p0./p1), list_size_vec(i_list));
                        end
                        err = any(info ~= decoded_bits);
                        if err
                            num_block_err(i_ebno, i_list) =  num_block_err(i_ebno, i_list) + 1;
                            num_bit_err(i_ebno, i_list) = num_bit_err(i_ebno, i_list) + sum(info ~= decoded_bits);
                        else
                            prev_decoded(i_list, i_ebno) = 1;
                        end
                    end
                end

            end
            bler = num_block_err./num_runs;
            ber = num_bit_err./num_runs;

        end

    end

    methods (Static)
        % the next four functions are modified version of code in:
        % http://pfister.ee.duke.edu/courses/ecen655/polar.pdf
        function x = polar_encode(info_bits_padded)

            N = length(info_bits_padded);
            if (N == 1)
                x = info_bits_padded;
            else
                u1u2 = mod(info_bits_padded(1:2:end) + info_bits_padded(2:2:end) , 2);
                u2 = info_bits_padded(2:2:end);
                x = [PolarCode.polar_encode(u1u2) PolarCode.polar_encode(u2)];
            end

        end

        function [u, x] = polar_decode(y,f)
            N = length(y);
            if (N==1)
                if (f == 0)  % info bit
                    x = (1-sign(1-2*y))/2;
                else  % frozen bit -- assumed to be zero
                    x = 0;
                end
                u = x;
            else
                u1est = PolarCode.cnop(y(1:2:end),y(2:2:end));
                [uhat1,u1hardprev] = PolarCode.polar_decode(u1est,f(1:N/2));
                u2est = PolarCode.vnop(PolarCode.cnop(u1hardprev,y(1:2:end)),y(2:2:end));
                [uhat2,u2hardprev] = PolarCode.polar_decode(u2est,f(N/2+1:end));
                u = [uhat1 uhat2];
                x = reshape([PolarCode.cnop(u1hardprev,u2hardprev); u2hardprev],1,[]);
            end
        end

        function z = cnop(w1,w2) % notation = probability of bit = 1
            z = w1.*(1-w2) + w2.*(1-w1);
        end

        function z = vnop(w1,w2) % notation = probability of bit = 1
            z = w1.*w2 ./ (w1.*w2 + (1-w1).*(1-w2));
        end

        function [x, ber] = polar_decode_monte(y, dummy_info)
            N = length(y);
            if (N==1)
                if (y > 0.5 && dummy_info == 1) || (y <= 0.5 && dummy_info == 0)
                    ber = 0;
                else
                    ber = 1;
                end
                x = dummy_info;
            else
                u1est = PolarCode.cnop(y(1:2:end),y(2:2:end));
                [u1hardprev, ber1] = PolarCode.polar_decode_monte(u1est,dummy_info(1:N/2));
                u2est = PolarCode.vnop(PolarCode.cnop(u1hardprev',y(1:2:end)),y(2:2:end));
                [u2hardprev, ber2] = PolarCode.polar_decode_monte(u2est,dummy_info(N/2+1:end));
                ber = [ber1 ber2];
                x = reshape([PolarCode.cnop(u1hardprev,u2hardprev); u2hardprev],1,[]);
            end
        end

        function [x, u] = polar_decode_capacity(y, dummy_info)
            N = length(y);
            if (N==1)
                u = y;
                x = dummy_info;
            else
                u1est = PolarCode.cnop(y(1:2:end),y(2:2:end));
                [u1hardprev, u1] = PolarCode.polar_decode_capacity(u1est,dummy_info(1:N/2));
                u2est = PolarCode.vnop(PolarCode.cnop(u1hardprev',y(1:2:end)),y(2:2:end));
                [u2hardprev, u2] = PolarCode.polar_decode_capacity(u2est,dummy_info(N/2+1:end));
                u = [u1 u2];
                x = reshape([PolarCode.cnop(u1hardprev,u2hardprev); u2hardprev],1,[]);
            end
        end

        function [x, u] = polar_decode_capacity_llr(y, dummy_info)
            N = length(y);
            if (N==1)
                u = y;
                x = dummy_info;
            else
                u1est = PolarCode.cnop_llr(y(1:2:end),y(2:2:end));
                [u1hardprev, u1] = PolarCode.polar_decode_capacity_llr(u1est,dummy_info(1:N/2));
                u2est = PolarCode.vnop_llr((1 - 2*u1hardprev').*y(1:2:end),y(2:2:end));
                [u2hardprev, u2] = PolarCode.polar_decode_capacity_llr(u2est,dummy_info(N/2+1:end));
                u = [u1 u2];
                x = reshape([PolarCode.cnop(u1hardprev,u2hardprev); u2hardprev],1,[]);
            end
        end

        function l = cnop_llr(l1,l2)
            l = 2 * atanh(tanh(l1/2).*tanh(l2/2));
        end

        function l = vnop_llr(l1,l2)
            l = l1 + l2;
        end

        function channels  = calculate_channel_polarization( epsilon, n)

            N  = 2^n;
            log_domain_enabled = 1;
            if log_domain_enabled
                epsilon = log(epsilon);
            end
            if length(epsilon) == 1
                channels = epsilon*ones(1, N);
            elseif length(epsilon) == N
                channels = epsilon;
            end

            for i = 1:n
                c1 = channels(1:2:N);
                c2 = channels(2:2:N);
                if log_domain_enabled
                    channels = [log(exp(c1) + exp(c2) - exp(c1+c2)), c1 + c2];
                else
                    channels = [c1 + c2 - c1.*c2, c1.*c2];
                end
            end

            if log_domain_enabled
                channels = exp(channels);
            end

        end

    end

end


function [MSLPoints,coeffData]=ellips2MSLHelmert(points,useNGAApprox,modelType,coeffData)
%%ELLIPS2MSLHELMERT  Given points in WGS-84 ellipsoidal coordinates,
%                    convert the ellipsoidal height components of the
%                    points into heights above mean-sea level (MSL)
%                    using Helmert's projection method with the EGM2008
%                    gravitational (and terrain correction) models. This
%                    differs from the more precise but seldom-used
%                    Pizzetti's projection method in that the curvature of
%                    the plumb line is completely ignored.
%
%INPUTS: points One or more points given in WGS-84 geodetic latitude and
%               longitude, in radians, and height, in meters for which the
%               corresponding MSL heights are desired. To convert N points,
%               points is a 3XN matrix with each column having the format
%               [latitude;longitude; height].
%  useNGAApprox If one wishes the intermediate tide-free geoid computation
%               to match the results of the National Geospatial
%               Intelligence Agency's code to three digits after the
%               decimal point, then this should be true. Setting this to
%               false might produce results that are marginally more
%               accurate. The default is false if this parameter is
%               omitted.
%     modelType An optional parameter specifying coefficient model to
%               load if coeffData is not provided. Possible values are
%               0 (The default if omitted) Load the EGM2008 model.
%               1 Load the EGM96 model.
%     coeffData A set of pre-loaded coefficients that can speed up the
%               computation by eliminating the need to compute them on the
%               fly. coeffData is as defined for the coeffData return value
%               of getEGMGeoidHeight.
%
%OUTPUTS: MSLPoints A 3XN array of the converted points, where each vector
%                   contains [latitude;longitude;MSLHeight]; The latitude
%                   and longitude are unchanged from the input data, but
%                   the heights have been converted from WGS-84 ellipsoidal
%                   heights to MSL heights.
%         coeffData The coeffData coefficients that can be passed to
%                   another call of ellips2MSLHelmert or MSL2EllipseHelmert
%                   to make it faster.
%
%Note that this function will be very slow if one hasn't called
%CompileCLibraries to compile the spherical harmonic synthesis functions.
%
%As described in Chapter 5.5 of [1], the MSL height using Helmert's
%projection is just the ellipsoidal height minus the geoid height. This
%function calls getEGMGeoidHeight to get the geoidal height and
%subtracts it off.
%
%Helmert's projection calculated the height using a line from the point to
%the reference ellipsoid, subtracting off the distance from the point to
%the geoid along the line. Pizzetti's projection follows the curved plumb
%line down to the geoid. Thus, the latitude and longitude of the projected
%point on the geoid are not the same as that of the point being projected.
%The MSL height obtained is also slightly different. However, the
%difference is small and Pizzetti's projection is difficult to use, so
%Helmert's projection is almost always used.
%
%The inverse of this function is MSL2EllipseHelmert.
%
%REFERENCES:
%[1] B. Hofmann-Wellenhof and H. Moritz, Physical Geodesy, 2nd ed.
%    SpringerWienNewYork, 2006.
%
%January 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(useNGAApprox))
    useNGAApprox=false;
end

if(nargin<3||isempty(modelType))
    modelType=0;
end

if(nargin>3)
    [geoidHeight,coeffData]=getEGMGeoidHeight(points(1:2,:),1,useNGAApprox,modelType,coeffData);
else
    [geoidHeight,coeffData]=getEGMGeoidHeight(points(1:2,:),1,useNGAApprox,modelType);
end

MSLHeight=points(3,:)-geoidHeight';
numPoints=size(points,2);

MSLPoints=zeros(3,numPoints);
MSLPoints(1:2,:)=points(1:2,:);
MSLPoints(3,:)=MSLHeight;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


%SF_DISC1 Linear discontinuous shape function (P1).
%
%   [ VBASE, NLDOF, XLDOF, SFUN ] = SF_DISC1( I_EVAL, N_SDIM, N_VERT, I_DOF, XI, AINVJAC, VBASE )
%   Evaluates discontinuous linear shape functions with with values
%   defined by a constant and derivatives in the cell interior.
%
%       Input       Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       i_eval      scalar:  1             Evaluate function values
%                           >1             Evaluate values of derivatives
%       n_sdim      scalar: 1-3            Number of space dimensions
%       n_vert      scalar: 2-8            Number of vertices per cell
%       i_dof       scalar: 1-n_ldof       Local basis function to evaluate
%       xi          [n_sdim(+1)]           Local coordinates of evaluation point
%       aInvJac     [n,n_sdim(+1)*n_sdim]  Inverse of transformation Jacobian
%       vBase       [n]                    Preallocated output vector
%                                                                                .
%       Output      Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       vBase       [n]                    Evaluated function values
%       nLDof       [4]                    Number of local degrees of freedom on
%                                          vertices, edges, faces, and cell interiors
%       xLDof       [n_sdim,n_ldof]        Local coordinates of local dofs
%       sfun        string                 Function name of called shape function
%
%   See also SF_DISC0

% Copyright 2013-2022 Precise Simulation, Ltd.

% Check for evaluation on simplicies.


 % evaluate simulated data - ICA and NMF
% V ~ WH
% V -> N_pix x N_t
% W -> N_pix x N_comp - xth pixel of the ith components
% H -> N_copm x N_t - contribution of the i-th component in the time t

function separNMFICA(sep0, offset0, path_data, path_res, prename, niter, savethis, sep_how)

if ~exist('initval', 'var')
    initval = 0;
end

if ~exist('sep_how', 'var')
    sep_how = 'in';
end
ncomp = 3; %number of components to be separated + background

for rr = 1: length(offset0)
    for ll=1 : length(sep0)
        namedir = [prename num2str(100*sep0(ll)) 'offset_' num2str(offset0(rr))];
        cd ([path_data namedir])
        for mm=1:niter
            %reads the first...
            namefile = [namedir '-iter_' num2str(mm)];
            load ([namefile '.mat'])
            %cat the folowing,,,
            p.catitervec=[1];
            [dpixc, dveccr, blinkmat, p] = catsimul(namedir, p.catitervec);
            if sum(sep_how == 'i')>0 %ICA
                [icasig{mm}, A{mm}, W{mm}] = fastica (dveccr, 'numOfIC', ncomp, 'g', 'tanh');
                icapixICA{mm} = reshape(A{mm},p.nx, p.ny, ncomp);
            end

            if sum(sep_how == 'n')>0 %NMF
                % background estimation:
                %[out, bg(mm), bg_im]=backgroundoffset(dpixc);
                [out, bg(mm), bg_im]=backgroundoffset(dpixc, 'no', 5, 20, 8); %empirical values...

                dvec_bg = ones(p.nx*p.ny, 1);
                %                     dvec_bg = p.offset*ones(1, p.nx*p.ny); %changed for
                %                     offset 10...

                dvec_ind = squeeze(reshape(double(array2im(dpixc_ind)), p.nx*p.ny, 1, 2)); % vectors of resized images
                %                     sum_dvec_ind = sum(dvec_ind, 1);
                %                     dvec_ind = dvec_ind./repmat(sum_dvec_ind, p.nx*p.ny,1); %normlaized

                %                     winit = [f*dvec_ind'; dvec_bg];       %original 'true' points + background
                % % %                     winittmp = [dvec_ind, dvec_bg];
                winittmp = [rand(size(dvec_ind)), dvec_bg];
                sumw = sum(winittmp,1);
                winit = winittmp./repmat(sumw, p.nx*p.ny, 1); %normalized to 1
                f = mean(dveccr(:)-bg(mm))/mean(mean(winit(:, 1:2))); %ration of the data/psf
                %                     winit = [rand(ncomp, p.nx*p.ny); dvec_bg];
                blinkmatrand = rand(ncomp-1, p.Nt); %uniform random;
                hinit = [f*blinkmatrand; bg(mm)*sumw(ncomp)*ones(1, p.Nt)];             %random weights will be assigned to firts two and bg fixed
                % % %                     hinit = [f*blinkmat./repmat(mean(blinkmat,2),1,size(blinkmat,2)); bg(mm)*sumw(ncomp)*ones(1, p.Nt)];             %random weights will be assigned to firts two and bg fixed

                %                    [w{mm},h{mm}, wtrace{mm},wtrace{mm}]=nmf_test(double(dveccr'),ncomp+1,1,hinit,winit, [3], [3]);
                %                     [w{mm},h{mm}, wtrace,htrace,ddiv{mm}]=nmf_testconvD(double(dveccr'),ncomp+1,1,hinit,winit, [3], [3]);

                dvec_bg = ones(p.nx*p.ny, 1);
                winittmp = [dvec_ind, dvec_bg];
                sumw = sum(winittmp,1);
                winit = winittmp./repmat(sumw, p.nx*p.ny, 1); %normalized to 1
                f = mean(dveccr(:)-bg(mm))/mean(mean(winit(:, 1:2))); %ration of the data/psf

                p.Nt=1;
                p.meanblinkmat=mean(mean(blinkmat,2));
                p.htrue=[0.3; 0.7];
                htruef=p.meanblinkmat*p.htrue;

                %hinit = [p.meanblinkmat*rand(2,1); p.offset*sumw(ncomp)*ones(1, p.Nt)];

                    hinit = [p.meanblinkmat*rand(2,1); p.offset*sumw(ncomp)*ones(1, p.Nt)];
%                     hinit = [htruef; p.offset*sumw(ncomp)*ones(1, p.Nt)];
                    dveccr=dvec_ind*htruef+p.offset;
                    dpixcd = dip_image(reshape(dveccr,p.nx,p.ny));
                    dpixcdn = noise(dpixcd,'poisson');
                    dveccr=reshape(double(dpixcdn),p.nx*p.ny,1);
                    %[c,w{mm},h{mm}, X1,X2, dhr, minXr, miXvalr, mhdr, htrace, p]
                    [c,w{mm},h{mm}, X1,X2, dh, minX, miXval, mhd, htr, p]=nmf_S43(double(dveccr),ncomp,1,winit,hinit, [3], [3],p);
                    res=struct('c',c,'w',w,'h',h,'X1',X1,'X2',X2,'dh',dh,'minX',minX,'miXval',miXval, 'mhd',mhd, 'htr',htr, 'p',p);

                %hinit = [f*blinkmat./repmat(mean(blinkmat,2),1,size(blinkmat,2)); bg(mm)*sumw(ncomp)*ones(1, p.Nt)];             %random weights will be assigned to firts two and bg fixed
                %[c,w{mm},h{mm}, X1,X2, dht, minXt, miXvalt, mhdt, htrace,p]=nmf_S41(double(dveccr),ncomp,1,winit,hinit, [3], [3],p);
                qqq=[];
                icapixNMF{mm} = reshape(w{mm},p.nx,p.ny,ncomp);
            end

        end

        p.path_data = path_data;
        p.path_res = path_res;

        if savethis == 1
            fprintf('saving data \n');
            if ~(strcmp(path_res, path_data)) %not identical
                mkdir ([path_res namedir]);
                cd ([path_res namedir]);
            end
            %             save (p.namedir)
            %save ([namedir '_separMulti'],save ([namedir '_separMulti'],'p', 'X1','X2','dhr','minXr', 'miXvalr','dht'))
            save ([namedir '_DdviMap'],'res')
            writedata([],[],p,[namedir '_param'])
        end
    end
end

fprintf('\n')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%	SurfBox-MATLAB (c)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%	Yue Lu and Minh N. Do
%%
%%	Department of Electrical and Computer Engineering
%%	Coordinated Science Laboratory
%%	University of Illinois at Urbana-Champaign
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%	IRCrecF.m
%%
%%	First created: 08-14-05
%%	Last modified: 04-13-06
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function subband =IRCrecF(subband, K, level, Hflts)

%   Iteratively Resampled Checkerboard Filter Bank Reconstruction (Fourier
%   Domain Implementation)
%
%   *****  INPUT:  *****
%
%   subband: N-dimensional (N >= 2) input signal in the FREQUENCY domain.
%
%   K: channel index, 1 <= K <= N
%
%   level: a row vector specifying the decomposition levels for each pair
%   of dimensions. Note: only level(m) for m ~= k will be used.
%
%   Hflts: A cell array containing two decomposition checkerboard filters
%   {H0, c0, H1, c1}
%
%   *****  OUTPUT:  *****
%
%   subband: N-D output signal in the FREQUENCY domain.
%
%   See also IRCdecF.m
%

%% dimension of the problem
N = ndims(subband);

%% check parameter validity
if (length(level) ~= N) | (K > length(level)) ...
        | (sum(level < 0) ~= 1) | (level(K) >= 0)
    error('Input parameter LEVEL is not valid.');
end

%% prepration
szS = size(subband);
szH0 = size(Hflts{1}); szH1 = size(Hflts{3});
szFlt = max(szH0, szH1);

%% Subscript cell array
subary = repmat({':'}, [2 N]);

%% preallocate the memory for a temp variable
subtmp = repmat(complex(0), szS);

%% start working
for m = N : -1 : 1  %% N is a small number like 2,3, or 4.

    %% We work on pairs of dimensions: (K,1), (K,2), ... (K, N)
    if m == K   %% except on dimensions (K, K)
        continue;
    end

    %% Size of the 2-D slice
    ftSpat = zeros((szS(K)-1) * 2, szS(m) / 2^(level(m)-1)); %% get the full size

    %% This one is ``thinner'' in the m dimension. This will reduce
    %% the number of filter resampling (shearing) operations.
    ftSpatSml = zeros((szS(K)-1) * 2, szFlt(2));

    indm = [1: szS(m)];

    %% This variable is what we actually use to multiply with the subband
    ftSlice = repmat(complex(0), [szS(K), szS(m)]);

    %% IRCdec on dimensions (K, m)
    for le = level(m) : -1 : 1 %% Another small number, like 0, 1, 2, 3

        if any(size(ftSpat) < szFlt)
            error('Image size is smaller than the filter size. Try using more compact filters.');
        end

        %% select the right column indices
        selcol = mod(floor( (indm - 1) / (szS(m) / 2^le) ), 2) == 0;
        subary{1,m} = indm(selcol);
        subary{2,m} = indm(~selcol);

        for chan = 0 : 1    %% Two channels

            %% Upsampling by 2 along dimension m
            subdouble = subband;
            subdouble(subary{2-chan,:}) = subband(subary{1+chan,:});

            H = Hflts{2*chan + 1};  %% Get the decomposition filter
            ctr = Hflts{2*chan + 2};
            szH = size(H);

            %% To get back to the original K-m dimension, since ftSlice
            %% might have been transposed.
            ftSlice = reshape(ftSlice, [szS(K), szS(m)]);

            if le == 1
                %% Get the FFT of filters
                ftSpat(:) = 0;
                ftSpat(1:szH(1), 1:szH(2)) = H;
                ftFreq = fft2(circshift(ftSpat, 1 - ctr));
                 %% Remove the complex conjugate symmetric part
                ftSlice = ftFreq(1:szS(K), :);
            else
                %% We will resample the checkerboard filters before filtering.
                %% This has the advantage of carrying out the resampling
                %% operation on 2-D filters instead of on N-D signals. Also by
                %% doing this, we avoid having to backsample at the end.
                %% Resampling in the frequency domain is generally MUCH
                %% harder than what one might think at first glance,
                %% especially when the data is of rectangular sizes, i.e.,
                %% when size(x, 1) ~= size(x, 2).

                nsubs = 2^(le - 1); %% number of subbands
                tmp_sub = [1 : szS(m) / nsubs]; %% a set of subscripts

                for n = 0 : nsubs - 1
                    %% Get the shearing factor (SF)
                    sf = nsubs - 1 - 2 * n;

                    %% After shearing, the filter H becomes ``taller''. We need
                    %% to make sure that we can still handle this filter
                    %% size.
                    if size(ftSpatSml, 1) < (szH(1) + abs(sf)* (szH(2)-1))
                        error('Image size is smaller than the filter size (from shearing). Try using more compact filters.');
                    end

                    %% load the filter
                    ftSpatSml(:) = 0;
                    ftSpat(:) = 0;
                    if sf > 0
                        ftSpatSml(1:szH(1), 1: szH(2)) = H;
                        newctr = [ctr(1) + sf * (ctr(2)-1), ctr(2)];
                    else
                        ftSpatSml(end-szH(1)+1:end, 1: szH(2)) = H;
                        newctr = [size(ftSpatSml, 1) + ctr(1) - szH(1) + sf * (ctr(2)-1), ctr(2)];
                    end

                    %% resample the filter in the spatial domain
                    ftSpatSml = resampc(ftSpatSml, (3 + sign(sf))/2, abs(sf), 'per');

                    ftSpat(:, 1:size(ftSpatSml, 2)) = ftSpatSml;
                    ftFreq = fft2(circshift(ftSpat, 1 - newctr));
                    ftSlice(:, tmp_sub) = ftFreq(1:szS(K), :);

                    tmp_sub = tmp_sub + szS(m) / nsubs;
                end

            end

            if K > m
                ftSlice = ftSlice .';
            end

            %% Pointwise multiplication of Filters and Input Data
            sz = ones(1, N);
            sz([K,m]) = szS([K,m]);

            ftSlice = reshape(ftSlice, sz);
            if chan == 0
                subtmp = subdouble .* repmat(ftSlice, szS ./ sz);
            else
                subband = subtmp + subdouble .* repmat(ftSlice, szS ./ sz);
            end
            clear subdouble;

        end  %% chan = 0:1

       %% Double the size of the 2-D slice along dimension m
       ftSpat = zeros(size(ftSpat) .* [1 2]);

    end %% le = 1 : level(m)

    %% We will work on the next dimension.
    subary{1, m} = ':'; subary{2,m} = ':';
end

%%	This software is provided "as-is", without any express or implied
%%	warranty. In no event will the authors be held liable for any
%%	damages arising from the use of this software.


function M = permutation(P)
% PERMUTATION
%
% M = permutation(X)
%
% Creates the model [sum(P,1)==1,sum(P,2)==1,binary(P)]

M = [sum(P,1)==1,sum(P,2)==1,binary(P)];


function [track_info,pos_info_val] = bz_olypherInfo(data,round_to,smoothing)
% USAGE
%        [track_info,pos_info_val] = bz_olypherInfo(data,round_to,smoothing)
%
% INPUTS
%         data - matrix (M x N x D) where M is the number of cells to analyze,
%                N is the number of trials for each cell, and D is the number
%                of time bins
%         round_to - integer value that data is discritized to. A value of
%                    2 means all data will be rounded to nearest 2 (i.e.
%                    2,4,8...)
%         smoothing - 0 no smoothing, else smooth with N bins
% OUTPUTS
%         track_info - matrix (M x D-2) of information scores across all
%                      trials or behavior windows (N)
%         pos_info_val - matrix (M x N x D) of all information values
%                        that are calculated
%
% this function calculates the information carried in the firing rate of single
% neurons per spatial/temporal bin
%
%  Written by David Tingley
%  UCSD Cognitive Neuroscience
%  1/15/12

%% TODO
% - convert to varargin with input parser
% - add 'exclude' input to remove 0's from info calculation
% -

M = size(data,1);
N = size(data,2);
D = size(data,3);

if M == 0
    M = 1
end
if N == 0
    N = 1
end
if D == 0
    D = 1
end
pos_info_val = zeros(M,N,D);
a = N*D;

%% Rounding
if smoothing ~= 0
    for i = 1 : M
        for k = 1:N
            data(i,k,:) = smooth(squeeze(data(i,k,:)),smoothing).*smoothing;
        end
    end
end
data = round(data./round_to)*round_to;

%% Info Analysis

for i = 1 : M
      for x = 1 : D
         for k = 1:N

                q = data(i,k,x);

                pKx = ((length(find(data(i,:,x) == q)))/N);

                pK = ((length(find(data(i,:,:) == q)))/a);

                if pK == 0 || pKx == 0 || pKx < pK
                    pos_info_val(i,k,x) = pos_info_val(i,k,x);
                else
                    pos_info_val(i,k,x) = pos_info_val(i,k,x) + (pKx*log2(pKx/pK));
                end

         end
     end

end

for i = 1:M
track_info(i,:) = sum(pos_info_val(i,:,2:end-1),2);
end


function [Xpoints1, Ypoints1, Xpoints2, Ypoints2, tms, yc] = ProcessVideo(videoFilePath)
if nargin < 1
    videoFilePath = fullfile(pwd, 'video/video.avi');
end
% clc; clear all; close all;
% videoFilePath = fullfile(pwd, 'video/video.avi');
time_start = cputime;
[pathstr, name, ext] = fileparts(videoFilePath);
foldername = fullfile(pwd, sprintf('%s_images', name));
T = 1;
P = 5;
W1 = [75 95];
L1 = [360 17];
W2 = [55 55];
L2 = [35 1565];
Xpoints1 = [];
Ypoints1 = [];
Xpoints2 = [];
Ypoints2 = [];
Xpointst1 = [];
Ypointst1 = [];
Xpointst2 = [];
Ypointst2 = [];
figure('Position', get(0, 'ScreenSize'));
hg1 = subplot(1, 2, 1);
hg2 = subplot(1, 2, 2);
for frame = 1:146
    filename = fullfile(foldername, sprintf('%04d.jpg', frame));
    R = imread(filename);
    Imi = R;
    xc1 = 0;
    yc1 = 0;
    xc2 = 0;
    yc2 = 0;
    if frame > 72
        I = rgb2hsv(Imi);
        I = I(:,:,1);
        I = roicolor(I, 0.1, 0.17);
        MeanConverging1 = 1;
        while MeanConverging1
            M00 = 0.0;
            for i = L1(1)-P : (L1(1)+W1(1)+P)
                for j = L1(2)-P : (L1(2)+W1(2)+P)
                    if i > size(I,1) || j > size(I,2) || i < 1 || j < 1
                        continue;
                    end
                    M00 = M00 + double(I(i,j));
                end
            end
            M10 = 0.0;
            for i = L1(1)-P : (L1(1)+W1(1)+P)
                for j = L1(2)-P : (L1(2)+W1(2)+P)
                    if i > size(I,1) || j > size(I,2) || i < 1 || j < 1
                        continue;
                    end
                    M10 = M10 + i * double(I(i,j));
                end
            end
            M01 = 0.0;
            for i = L1(1)-P : (L1(1)+W1(1)+P)
                for j = L1(2)-P : (L1(2)+W1(2)+P)
                    if i > size(I,1) || j > size(I,2)|| i < 1 || j < 1
                        continue;
                    end
                    M01 = M01 + j * double(I(i,j));
                end
            end
            xc1 = round(M10 / M00);
            yc1 = round(M01 / M00);
            oldL = L1;
            L1 = [floor(xc1 - (W1(1)/2)) floor(yc1 - (W1(2)/2))];
            if abs(oldL(1)-L1(1)) < T || abs(oldL(2)-L1(2)) < T
                MeanConverging1 = 0;
            end
        end
        s = round(1.1 * sqrt(M00));
        W1 = [ s floor(1.2*s) ];
        L1 = [floor(xc1 - (W1(1)/2)) floor(yc1 - (W1(2)/2))];
        Xpoints1 = [Xpoints1 xc1];
        Ypoints1 = [Ypoints1 yc1];
        yc1t = yc1+randi(2,1,1)*25;
        xc1t = xc1+randi(2,1,1)*25;
        Xpointst1 = [Xpointst1 xc1t];
        Ypointst1 = [Ypointst1 yc1t];
    else
        Xpoints1 = [Xpoints1 NaN];
        Ypoints1 = [Ypoints1 NaN];
        Xpointst1 = [Xpointst1 NaN];
        Ypointst1 = [Ypointst1 NaN];
    end
    if frame > 90 && frame < 146
        R = Imi;
        I = rgb2ycbcr(R);
        I = I(:,:,1);
        I = mat2gray(I);
        I = roicolor(I, 0.05, 0.3);
        MeanConverging2 = 1;
        while MeanConverging2
            M00 = 0.0;
            M00 = 0.0;
            for i = L2(1)-P : (L2(1)+W2(1)+P),
                for j = L2(2)-P : (L2(2)+W2(2)+P),
                    if i > size(I,1) || j > size(I,2) || i < 1 || j < 1
                        continue;
                    end
                    M00 = M00 + double(I(i,j));
                end
            end
            M10 = 0.0;
            for i = L2(1)-P : (L2(1)+W2(1)+P),
                for j = L2(2)-P : (L2(2)+W2(2)+P),
                    if i > size(I,1) || j > size(I,2) || i < 1 || j < 1
                        continue;
                    end
                    M10 = M10 + i * double(I(i,j));
                end
            end
            M01 = 0.0;
            for i = L2(1)-P : (L2(1)+W2(1)+P),
                for j = L2(2)-P : (L2(2)+W2(2)+P),
                    if i > size(I,1) || j > size(I,2)|| i < 1 || j < 1
                        continue;
                    end
                    M01 = M01 + j * double(I(i,j));
                end
            end
            xc2 = round(M10 / M00);
            yc2 = round(M01 / M00);
            oldL = L2;
            L2 = [floor(xc2 - (W2(1)/2)) floor(yc2 - (W2(2)/2))];
            if abs(oldL(1)-L2(1)) < T || abs(oldL(2)-L2(2)) < T
                MeanConverging2 = 0;
            end
        end
        s = round(1.1 * sqrt(M00));
        W2 = [ s      floor(1.2*s) ];
        L2 = [floor(xc2 - (W2(1)/2)) floor(yc2 - (W2(2)/2))];
        Xpoints2 = [Xpoints2 xc2];
        Ypoints2 = [Ypoints2 yc2];
        yc2t = yc2+randi(2,1,1)*25;
        xc2t = xc2+randi(2,1,1)*25;
        Xpointst2 = [Xpointst2 xc2t];
        Ypointst2 = [Ypointst2 yc2t];
    else
        Xpoints2 = [Xpoints2 NaN];
        Ypoints2 = [Ypoints2 NaN];
        Xpointst2 = [Xpointst2 NaN];
        Ypointst2 = [Ypointst2 NaN];
    end
    axes(hg1); cla;
    imshow(Imi, []); hold on;
    if xc1 > 0 && yc1 > 0
        plot(yc1, xc1, 'go', 'MarkerFaceColor', 'g');
        plot(yc1t, xc1t, 'g+', 'MarkerFaceColor', 'g');
    end
    if xc2 > 0 && yc2 > 0
        plot(yc2, xc2, 'bo', 'MarkerFaceColor', 'b');
        plot(yc2t, xc2t, 'b+', 'MarkerFaceColor', 'b');
    end
    hold off; title(sprintf('%04d֡', frame));
    bg = true(size(Imi,1), size(Imi,2));
    axes(hg2); cla; imshow(bg);
    hold on; box on;
    plot(Ypoints1, Xpoints1, 'go-', 'MarkerFaceColor', 'g');
    plot(Ypoints2, Xpoints2, 'bo-', 'MarkerFaceColor', 'b');
    hold off; title(sprintf('%04d֡', frame));
    pause(0.001);
end
time_end = cputime;
tms = time_end - time_start;
yc.Xpointst1 = Xpointst1;
yc.Ypointst1 = Ypointst1;
yc.Xpointst2 = Xpointst2;
yc.Ypointst2 = Ypointst2;


function  jitterLags = realized_kernel_jitter_lag_length(noiseEstimate,iqEstimate,kernel,N)
% Computes the optimal amount of end point jitter given a kernel, noise estimate, integrated
% quarticity estimate and number of observations.
%
% USAGE:
%   [JITTERLAGS] = realized_kernel_weights(NOISEESTIMATE,IQESTIMATE,KERNEL,N)
%
% INPUTS:
%   NOISEESTIMATE - Estimated variance of the noise present in a high freuqency asset price (see
%                     realized_kernel_select_lag_length)
%   IQESTIMATE    - Estimated integrated quarticity.  For the purposes of estimating the number of
%                     points to jitter it is often reasonable to use the square of a low frequence
%                     IV measure.
%   KERNEL        - String containing one of the supported kernel types:
%                     Non-flat-top, weakly positive, n^(1/5) rate:
%                     - 'nonflatparzen' [RECOMMENDED] Parzen kernel applied to non-flat-top case
%                     - 'qs' Quadratic Spectral
%                     - 'fejer' Fejer kernel
%                     - 'thinf' Tukey-Hanning kernel with infinite lag order
%                     - 'bnhls' Kernel proposed by Barndorf-Neilsen, Hansen, Lunde and Shephard
%                     Flat-top, n^(1/4) rate:
%                     - 'parzen' Parzen's kernel
%                     - 'th1' Tukey Hanning kernel with power 1
%                     - 'th2' Tukey Hanning kernel with power 2
%                     - 'th5' Tukey Hanning kernel with power 5
%                     - 'th16' Tukey Hanning kernel with power 16
%                     - 'cubic','multiscale' 3rd order (cubic) kernel, which
%                         is asymptotically equivalent to the multiscale estimator
%                     - '5thorder' 5th order Kernel
%                     - '6thorder' 6th order Kernel
%                     - '7thorder' 7th order Kernel
%                     - '8thorder' 8th order Kernel
%                     Flat-top, n^(1/6) rate:
%                     - 'bartlett','twoscale' Bartlett kernel, which is
%                         asymptotically equivalent to the two-scale estimator
%                     - '2ndorder' 2nd order (quadratic) kernel
%                     - 'epanechnikov' Epanechnikov kernel
%   N                 - Number of observation of the price
%
% OUTPUTS:
%   JITTERLAGS    - Estimated optimal number of lags to jitter the end points of a kernel.  If 1
%                     then no pre-averaging is needindicated
%
% COMMENTS:
%   This is a helper function for REALIZED_KERNEL. See Barndorf-Nielsen,
%   Hansen, Lunde and Shephard (2008a, 2008b) for details about the kernel and
%   their properties.
%
%  See also REALIZED_KERNEL_SELECT_LAG_LENGTH, REALIZED_VARIANCE, REALIZED_KERNEL,
%  REALIZED_PRICE_FILTER, REALIZED_KERNEL_WEIGHTS

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin~=4
    error('Four inputs required.')
end
if length(noiseEstimate)>1 || noiseEstimate<0
    error('NOISEESTIMATE must be a non-negative scalar.');
end
if length(iqEstimate)>1 || iqEstimate<0
    error('IQESTIMATE must be a non-negative scalar.');
end
if ~ismember(kernel,{'bartlett','twoscale','2ndorder','epanechnikov','cubic','multiscale','5thorder','6thorder','7thorder','8thorder','parzen','th1','th2','th5','th16','nonflatparzen','qs','fejer','thinf','bnhls'})
    error('KERNEL must be one of the listed types.')
end

if ~isscalar(N) || N<1 || floor(N)~=N
    error('N must be an integer greater than 1.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Look up table
switch lower(kernel)
    case {'bartlett','twoscale'}
        cStar = 2.28;
        k00 = 1/3;
        kernelType = 2;
    case '2ndorder'
        cStar = 3.42;
        k00 = 1/5;
        kernelType = 2;
    case 'epanechnikov'
        cStar = 2.46;
        k00 = 8/15;
        kernelType = 2;
    case {'cubic','multiscale'}
        cStar = 3.68;
        k00 = 0.371;
        k11 = 1.20;
        k22 = 12.0;
        kernelType = 1;
    case '5thorder'
        k00 = 0.391;
        k11 = 1.42;
        k22 = 17.1;
        cStar = 3.70;
        kernelType = 1;
    case '6thorder'
        k00 = 0.471;
        k11 = 1.55;
        k22 = 22.8;
        cStar = 3.97;
        kernelType = 1;
    case '7thorder'
        k00 = 0.533;
        k11 = 1.71;
        k22 = 31.8;
        cStar = 4.11;
        kernelType = 1;
    case '8thorder'
        k00 = 0.582;
        k11 = 1.87;
        k22 = 43.8;
        cStar = 4.31;
        kernelType = 1;
    case 'parzen'
        k00 = 0.269;
        k11 = 1.50;
        k22 = 24.0;
        cStar = 4.77;
        kernelType = 1;
    case 'th1'
        k00 = 0.375;
        k11 = 1.23;
        k22 = 12.1;
        cStar = 3.70;
        kernelType = 1;
    case 'th2'
        k00 = 0.219;
        k11 = 1.71;
        k22 = 41.7;
        cStar = 5.74;
        kernelType = 1;
    case 'th5'
        k00 = 0.097;
        k11 = 3.50;
        k22 = 489.0;
        cStar = 8.07;
        kernelType = 1;
    case 'th16'
        k00 = 0.032;
        k11 = 10.26;
        k22 = 14374.0;
        cStar = 39.16;
        kernelType = 1;
    case 'nonflatparzen'
        cStar = ((12)^2/0.269)^(1/5);
        k00 = .269;
        kernelType = 3;
    case 'qs'
        cStar = ((1/5)^2/(3*pi/5))^(1/5);
        k00 = 3*pi/5;
        kernelType = 3;
    case 'fejer'
        cStar = ((2/3)^2/(pi/3))^(1/5);
        k00 = pi/3;
        kernelType = 3;
    case 'thinf'
        cStar = ((pi^2/2)^2/(.52))^(1/5);
        k00 = 0.52;
        kernelType = 3;
    case 'bnhls'
        cStar = (1^2/(5/4))^(1/5);
        k00 = 5/4;
        kernelType = 3;
    otherwise
        error('KERNEL must be one of the listed types.')
end

% Compute the quantities necessary
if kernelType ==1
    % n^1/4 flat top
    % Approximate at the constant variance solution
    d=k00*k22/k11^2;
    fd = sqrt(1+sqrt(1+3*d));
    g = sqrt(k00*k11)*(1/fd + fd);
    avar = 16/3 * g * noiseEstimate * iqEstimate^(3/4);
    power = 1/4;
elseif kernelType == 2
    % n^1/6 flat top
    avar = 6 * cStar * k00 * noiseEstimate^(4/3) * iqEstimate^(2/3);
    power = 1/6;
elseif kernelType == 3
    % n^1/5 non flat top
    avar = 5 * cStar * k00 * noiseEstimate^(4/5) * iqEstimate^(4/5);
    power = 1/5;
end

% Problem is to min 8*noise^2*jitterLag^(-2) + avar * (N-jitterLag)^(-2*power)
% jittering over m data points, N observations in total
MSE = inf*ones(N-1,1);
for m=1:(N-1)
    MSE(m) = 8 * noiseEstimate^2 * m^(-2) + avar * (N-m)^(-2*power);
end

% Find the minimum, which may be 1
[temp,jitterLags] = min(MSE);


%% projection onto T_X(M_r)
%   X=U*diag(S)*V'
%   cf. [Van12]

function P=proj_fr(Z,U,V,sp)

if nargin<4, sp='t'; end

switch sp
    case 't'
        UtZ=U'*Z;
        P=U*UtZ+(Z*V)*V'-(U*(UtZ*V))*V';

    case 'n'
        UtZ=U'*Z;
        P=Z-(U*UtZ+(Z*V)*V'-(U*(UtZ*V))*V');
end

%% old version

% Pu=U*U';
% Pv=V*V';
%
% switch sp
%     case 't'
%         P=Pu*Z+Z*Pv-Pu*Z*Pv;
%
%     case 'n'
%         P=Z-Pu*Z-Z*Pv+Pu*Z*Pv;
% end


function [ Y_hat, W_hat] = MCWNNM_ADMM1_Estimation( NL_mat, Sigma_arr, CurPat, Par )

Y_hat = zeros(size(CurPat));
W_hat    = zeros(size(CurPat));
for  i      =  1 : length(Par.SelfIndex) % For each keypatch group
    Y    =   CurPat(:, NL_mat(1:Par.nlsp,i)); % Non-local similar patches to the keypatch
    mY  =   repmat(mean( Y, 2 ),1,Par.nlsp);
    Y    =   Y-mY;
    X 	=   MCWNNM_ADMM1( Y, Sigma_arr(:, Par.SelfIndex(i)), Par); % WNNM Estimation
    Y_hat(:,NL_mat(1:Par.nlsp,i))  = Y_hat(:,NL_mat(1:Par.nlsp,i))+X+mY;
    W_hat(:,NL_mat(1:Par.nlsp,i))     = W_hat(:,NL_mat(1:Par.nlsp,i))+ones(Par.ps2ch, Par.nlsp);
end
end


% DEMUSPSVARGPLVM1 Demonstrate variational GPLVM on USPS data.
% Copyright: Michalis Titsias, Neil Lawrence, Andreas Damianou, 2010 - 2014
% VARGPLVM

% Fix seeds
rng(1e5, 'v4');

dataSetName = 'usps';
experimentNo = 1;
printDiagram = 1;

% load data
[YTrain, lblsTrain, YTest, lblsTest] = lvmLoadData(dataSetName);

nClasses = size(lblsTrain,2);
% Set up model
options = vargplvmOptions('dtcvar');
options.kern = {'rbfard2', 'white'};
options.numActive = 50;
%options.tieParam = 'tied';

options.optimiser = 'scg';
latentDim = 10;
d = size(YTrain, 2);

capName = dataSetName;
capName(1) = upper(capName(1));
modelType = 'vargplvm'; % varmodel{1}.type;
modelType(1) = upper(modelType(1));
modelFile = ['dem' capName modelType num2str(experimentNo) '.mat'];

if exist(modelFile, 'file') % Training data exists, just load it.
    load(modelFile);
else % Do the training:
    iters = 1000;
    display = 1;

    varmodel = cell(nClasses,1);
    % create a separate vargplvm for each digit
    for i=1:nClasses
        %
        fprintf('\n\n# Training for Class # %d\n\n',i)
        Y = YTrain(lblsTrain(:,i)==1,:);

        model = vargplvmCreate(latentDim, d, Y, options);
        %
        model = vargplvmParamInit(model, model.m, model.X);

        model = vargplvmOptimise(model, display, iters);

        varmodel{i} = model;
        %
    end
    save(modelFile, 'varmodel');
end

iters = 100;
display = 0;

% measure performance on test data
prob = zeros(size(YTest,1),nClasses);
TestError = 0;

% New and faster way: give the whole test matrix at once, rather than using
% a for loop through each test element. This is almost 10 times faster then
% the old way (using 2 Matlab workers on a 4-core machine).

% Compute the approximate class conditional density for each digit
tic
for i=1:nClasses
    sprintf('--------\nComputing test probability for class=%d\n', i);
    prob(:,i) = vargplvmProbabilityCompute(varmodel{i}, YTest, 0, iters);
end
[maxP C] = max(prob,[],2);
for n=1:size(YTest,1)
    TestError = TestError + ~lblsTest(n,C(n));
end
fprintf('TestError=%d,\tTime=%fseconds.\n', TestError, toc);
%
save(['dem' capName modelType num2str(experimentNo) '.mat'], 'varmodel', 'prob', 'TestError');

%% Nearest Neighbour baseline
dists = dist2(YTrain,YTest);
[~,positions] = min(dists);
Ypred_NN = lblsTrain(positions,:);
NNError = 0;
for n=1:size(YTest,1)
    pp = find(Ypred_NN(n,:));
    NNError = NNError + ~lblsTest(n, pp);
end

%% Training of logistic regression classifier
labelsTrain = transformLabels(lblsTrain)';
labelsTest = transformLabels(lblsTest)';
Nstar = size(YTest,1);

for i=1:nClasses
    fprintf('\n # LogReg training for class # %d\n', i)
    lb = zeros(size(YTrain,1),1);
    lb(labelsTrain == i) = 1;
    B{i} = glmfit(YTrain, lb,'binomial','logit'); % Logistic regression
end

% Prediction of each binary classifier
Ypred_logReg = zeros(size(lblsTest));
for i=1:nClasses
    Ypred_logReg(:,i) = glmval(B{i},YTest,'logit')';
end

% Replace predictions with maximum probability (ie, make a decision)
[~,ind]=max(Ypred_logReg');
LogRegError = 0;
for i=1:size(YTest,1)
    LogRegError = LogRegError + (ind(i) ~= labelsTest(i));
end

% Print all results
fprintf('Var-GPLVM = %d (%f %%) \n', TestError, TestError*100/Nstar);
fprintf('NN        = %d mistakes (%f %% misclassified) \n', NNError, NNError*100/Nstar);
fprintf('Log. Reg. = %d mistakes (%f %% misclassified) \n', LogRegError, LogRegError*100/Nstar);


function [traj, infStates] = tapas_hgf_binary_pu(r, p, varargin)
% Calculates the trajectories of the agent's representations under the HGF
%
% This function can be called in two ways:
%
% (1) tapas_hgf_binary_pu(r, p)
%
%     where r is the structure generated by tapas_fitModel and p is the parameter vector in native space;
%
% (2) tapas_hgf_binary_pu(r, ptrans, 'trans')
%
%     where r is the structure generated by tapas_fitModel, ptrans is the parameter vector in
%     transformed space, and 'trans' is a flag indicating this.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2015 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform paramaters back to their native space if needed
if ~isempty(varargin) && strcmp(varargin{1},'trans');
    p = tapas_hgf_binary_pu_transp(r, p);
end

% Number of levels
try
    l = r.c_prc.n_levels;
catch
    l = (length(p)+1)/5;

    if l ~= floor(l)
        error('tapas:hgf:UndetNumLevels', 'Cannot determine number of levels');
    end
end

% Unpack parameters
mu_0 = p(1:l);
sa_0 = p(l+1:2*l);
rho  = p(2*l+1:3*l);
ka   = p(3*l+1:4*l-1);
om   = p(4*l:5*l-2);
th   = exp(p(5*l-1));
al   = p(5*l);
eta0 = p(5*l+1);
eta1 = p(5*l+2);

% Add dummy "zeroth" trial
u = [0; r.u(:,1)];

% Number of trials (including prior)
n = length(u);

% Assume that if u has more than one column, the last contains t
try
    if r.c_prc.irregular_intervals
        if size(u,2) > 1
            t = [0; r.u(:,end)];
        else
            error('tapas:hgf:InputSingleColumn', 'Input matrix must contain more than one column if irregular_intervals is set to true.');
        end
    else
        t = ones(n,1);
    end
catch
    if size(u,2) > 1
        t = [0; r.u(:,end)];
    else
        t = ones(n,1);
    end
end

% Initialize updated quantities

% Representations
mu = NaN(n,l);
pi = NaN(n,l);

% Other quantities
muhat = NaN(n,l);
pihat = NaN(n,l);
v     = NaN(n,l);
w     = NaN(n,l-1);
da    = NaN(n,l);

% Representation priors
% Note: first entries of the other quantities remain
% NaN because they are undefined and are thrown away
% at the end; their presence simply leads to consistent
% trial indices.
mu(1,1) = tapas_sgm(mu_0(1), 1);
pi(1,1) = Inf;
mu(1,2:end) = mu_0(2:end);
pi(1,2:end) = 1./sa_0(2:end);

% Pass through representation update loop
for k = 2:1:n
    if not(ismember(k-1, r.ign))

        %%%%%%%%%%%%%%%%%%%%%%
        % Effect of input u(k)
        %%%%%%%%%%%%%%%%%%%%%%

        % 2nd level prediction
        muhat(k,2) = mu(k-1,2) +t(k) *rho(2);

        % 1st level
        % ~~~~~~~~~
        % Prediction
        muhat(k,1) = tapas_sgm(ka(1) *muhat(k,2), 1);

        % Precision of prediction
        pihat(k,1) = 1/(muhat(k,1)*(1 -muhat(k,1)));

        % Mean update
        mu(k,1) = u(k);
        und1 = exp(-(u(k) -eta1)^2/(2*al));
        und0 = exp(-(u(k) -eta0)^2/(2*al));
        mu(k,1) = muhat(k,1) *und1 /(muhat(k,1) *und1 +(1 -muhat(k,1)) *und0);

        % Prediction error
        da(k,1) = mu(k,1) -muhat(k,1);

        % 2nd level
        % ~~~~~~~~~
        % Prediction: see above

        % Precision of prediction
        pihat(k,2) = 1/(1/pi(k-1,2) +exp(ka(2) *mu(k-1,3) +om(2)));

        % Updates
        pi(k,2) = pihat(k,2) +ka(1)^2/pihat(k,1);
        mu(k,2) = muhat(k,2) +ka(1)/pi(k,2) *da(k,1);

        % Implied posterior precision at first level
        sgmmu2 = tapas_sgm(ka(1) *mu(k,2), 1);
        pi(k,1) = pi(k,2)/(sgmmu2*(1-sgmmu2));

        % Volatility prediction error
        da(k,2) = (1/pi(k,2) +(mu(k,2) -muhat(k,2))^2) *pihat(k,2) -1;

        if l > 3
            % Pass through higher levels
            % ~~~~~~~~~~~~~~~~~~~~~~~~~~
            for j = 3:l-1
                % Prediction
                muhat(k,j) = mu(k-1,j) +t(k) *rho(j);

                % Precision of prediction
                pihat(k,j) = 1/(1/pi(k-1,j) +t(k) *exp(ka(j) *mu(k-1,j+1) +om(j)));

                % Weighting factor
                v(k,j-1) = t(k) *exp(ka(j-1) *mu(k-1,j) +om(j-1));
                w(k,j-1) = v(k,j-1) *pihat(k,j-1);

                % Updates
                pi(k,j) = pihat(k,j) +1/2 *ka(j-1)^2 *w(k,j-1) *(w(k,j-1) +(2 *w(k,j-1) -1) *da(k,j-1));

                if pi(k,j) <= 0
                    error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
                end

                mu(k,j) = muhat(k,j) +1/2 *1/pi(k,j) *ka(j-1) *w(k,j-1) *da(k,j-1);

                % Volatility prediction error
                da(k,j) = (1/pi(k,j) +(mu(k,j) -muhat(k,j))^2) *pihat(k,j) -1;
            end
        end

        % Last level
        % ~~~~~~~~~~
        % Prediction
        muhat(k,l) = mu(k-1,l) +t(k) *rho(l);

        % Precision of prediction
        pihat(k,l) = 1/(1/pi(k-1,l) +t(k) *th);

        % Weighting factor
        v(k,l)   = t(k) *th;
        v(k,l-1) = t(k) *exp(ka(l-1) *mu(k-1,l) +om(l-1));
        w(k,l-1) = v(k,l-1) *pihat(k,l-1);

        % Updates
        pi(k,l) = pihat(k,l) +1/2 *ka(l-1)^2 *w(k,l-1) *(w(k,l-1) +(2 *w(k,l-1) -1) *da(k,l-1));

        if pi(k,l) <= 0
            error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
        end

        mu(k,l) = muhat(k,l) +1/2 *1/pi(k,l) *ka(l-1) *w(k,l-1) *da(k,l-1);

        % Volatility prediction error
        da(k,l) = (1/pi(k,l) +(mu(k,l) -muhat(k,l))^2) *pihat(k,l) -1;
    else

        mu(k,:) = mu(k-1,:);
        pi(k,:) = pi(k-1,:);

        muhat(k,:) = muhat(k-1,:);
        pihat(k,:) = pihat(k-1,:);

        v(k,:)  = v(k-1,:);
        w(k,:)  = w(k-1,:);
        da(k,:) = da(k-1,:);

    end
end

% Implied learning rate at the first level
sgmmu2 = tapas_sgm(ka(1) *mu(:,2), 1);
lr1    = diff(sgmmu2)./da(2:n,1);
lr1(da(2:n,1)==0) = 0;

% Remove representation priors
mu(1,:)  = [];
pi(1,:)  = [];

% Check validity of trajectories
if any(isnan(mu(:))) || any(isnan(pi(:)))
    error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
else
    % Check for implausible jumps in trajectories
    dmu = diff(mu(:,2:end));
    dpi = diff(pi(:,2:end));
    rmdmu = repmat(sqrt(mean(dmu.^2)),length(dmu),1);
    rmdpi = repmat(sqrt(mean(dpi.^2)),length(dpi),1);

    jumpTol = 16;
    if any(abs(dmu(:)) > jumpTol*rmdmu(:)) || any(abs(dpi(:)) > jumpTol*rmdpi(:))
        error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
    end
end

% Remove other dummy initial values
muhat(1,:) = [];
pihat(1,:) = [];
v(1,:)     = [];
w(1,:)     = [];
da(1,:)    = [];

% Create result data structure
traj = struct;

traj.mu     = mu;
traj.sa     = 1./pi;

traj.muhat  = muhat;
traj.sahat  = 1./pihat;

traj.v      = v;
traj.w      = w;
traj.da     = da;

% Updates with respect to prediction
traj.ud = mu -muhat;

% Psi (precision weights on prediction errors)
psi        = NaN(n-1,l);
psi(:,2)   = 1./pi(:,2);
psi(:,3:l) = pihat(:,2:l-1)./pi(:,3:l);
traj.psi   = psi;

% Epsilons (precision-weighted prediction errors)
epsi        = NaN(n-1,l);
epsi(:,2:l) = psi(:,2:l) .*da(:,1:l-1);
traj.epsi   = epsi;

% Full learning rate (full weights on prediction errors)
wt        = NaN(n-1,l);
wt(:,1)   = lr1;
wt(:,2)   = psi(:,2);
wt(:,3:l) = 1/2 *(v(:,2:l-1) *diag(ka(2:l-1))) .*psi(:,3:l);
traj.wt   = wt;

% Create matrices for use by the observation model
infStates = NaN(n-1,l,4);
infStates(:,:,1) = traj.muhat;
infStates(:,:,2) = traj.sahat;
infStates(:,:,3) = traj.mu;
infStates(:,:,4) = traj.sa;

return;


function [density, sound_speed] = hounsfield2density(ct_data, plot_fitting)
%HOUNSFIELD2DENSITY   Convert Hounsfield units to density.
%
% DESCRIPTION:
%       hounsfield2density converts Hounsfield units to units of density
%       [kg/m^3] based on the experimental data given by Schneider et al.
%       The conversion is made using a piece-wise linear fit to the data.
%       For soft-tissue, the approximate sound speed can also be returned
%       using the empirical relationship given by Mast.
%
% USAGE:
%       density = hounsfield2density(ct_data)
%       density = hounsfield2density(ct_data, plot_fitting)
%       [density, sound_speed] = hounsfield2density(ct_data)
%       [density, sound_speed] = hounsfield2density(ct_data, plot_fitting)
%
% INPUTS:
%       ct_data      - CT data in Hounsfield units to convert to density
%
% OPTIONAL INPUTS:
%       plot_fitting - Boolean controlling whether the original data points
%                      and fitting is plotted (default = false)
%
% OUTPUTS:
%       density      - density in kg/m^3
%       sound_speed  - sound speed in m/s
%
% ABOUT:
%       author       - Bradley Treeby
%       date         - 9th January 2012
%       last update  - 10th January 2012
%
% REFERENCES:
%       Schneider, U., Pedroni, E., and Lomax A., "The calibration of CT
%       Hounsfield units for radiotherapy treatment planning," Phys. Med.
%       Biol., 41, pp. 111-124 (1996).
%
%       Mast, T. D., "Empirical relationships between acoustic parameters
%       in human soft tissues," Acoust. Res. Lett. Online, 1(2), pp. 37-42
%       (2000).
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave 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 Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% create empty density matrix
density = zeros(size(ct_data));

% apply conversion in several parts using linear fits to the data
% Part 1: Less than 930 Hounsfield Units
density(ct_data < 930) = polyval([1.025793065681423  -5.680404011488714], ct_data(ct_data < 930));

% Part 2: Between 930 and 1098 (soft tissue region)
density(ct_data >= 930 & ct_data <= 1098) = polyval([0.9082709691264   103.6151457847139], ct_data(ct_data >= 930 & ct_data <= 1098));

% Part 3: Between 1098 and 1260 (between soft tissue and bone)
density(ct_data > 1098 & ct_data < 1260) = polyval([0.5108369316599   539.9977189228704], ct_data(ct_data > 1098 & ct_data < 1260));

% Part 4: Greater than 1260 (bone region)
density(ct_data >= 1260) = polyval([0.6625370912451   348.8555178455294], ct_data(ct_data >= 1260));

% calculate corresponding sound speed values if required using soft tissue
% relationship
if nargout == 2
    sound_speed = (density + 349)./0.893;
end

% plot original data and fitted curves if required
if nargin == 2 && plot_fitting

    % soft tissue values excluding spongiosa
    density_soft_tissue = [0.95, 1.06, 1.04, 1.02, 1.00, 1.07, 1.03, 1.06, 1.05, 1.06, 1.05, 1.03, 1.05, 1.05, 1.04, 1.10, 1.03, 0.98, 1.09, 1.06, 1.04, 1.05]*1000;
    hounsfd_soft_tissue = [ 930, 1055, 1037, 1003, 1003, 1050, 1023, 1055, 1043, 1053, 1044, 1028, 1042, 1045, 1032, 1098, 1014,  958, 1075, 1054, 1032, 1040];

    % bone values
    density_bone = [1.92, 1.61, 1.33, 1.46, 1.68, 1.41, 1.52, 1.29, 1.18, 1.42, 1.33]*1000;
    hounsfd_bone = [2376, 1903, 1499, 1683, 2006, 1595, 1763, 1413, 1260, 1609, 1477];

    % filled lung values
    density_lung = 0.26*1000;
    hounsfd_lung = 259;

    % find linear fit for soft tissue data points
    h_axis_soft_tissue = min(hounsfd_soft_tissue(:)):max(hounsfd_soft_tissue(:));
    p_soft_tissue = polyfit(hounsfd_soft_tissue, density_soft_tissue, 1);

    % find linear fit for bone data points
    h_axis_bone = min(hounsfd_bone(:)):max(hounsfd_bone(:));
    p_bone = polyfit(hounsfd_bone, density_bone, 1);

    % find linear fit from soft tissue to bone region
    h_axis_tissue_to_bone = [h_axis_soft_tissue(end), h_axis_bone(1)];
    density_tissue_to_bone = [polyval(p_soft_tissue, h_axis_soft_tissue(end)), polyval(p_bone, h_axis_bone(1))];
    p_tissue_to_bone = polyfit(h_axis_tissue_to_bone, density_tissue_to_bone, 1);

    % find linear fit from filled lung to soft tissue region
    h_axis_lung_to_tissue = [hounsfd_lung, h_axis_soft_tissue(1)];
    density_lung_to_tissue = [density_lung, polyval(p_soft_tissue, h_axis_soft_tissue(1))];
    p_lung_to_tissue = polyfit(h_axis_lung_to_tissue, density_lung_to_tissue, 1);

    % plot original data points
    figure;
    hold on;
    plot(hounsfd_soft_tissue, density_soft_tissue, 'r.');
    plot(hounsfd_bone , density_bone , 'b.');
    plot(hounsfd_lung, density_lung, 'g.');

    % plot fitting data
    plot(h_axis_soft_tissue, polyval(p_soft_tissue, h_axis_soft_tissue), 'r-');
    plot(h_axis_bone, polyval(p_bone, h_axis_bone), 'b-');
    plot(h_axis_tissue_to_bone, polyval(p_tissue_to_bone, h_axis_tissue_to_bone), 'k-');
    plot(h_axis_lung_to_tissue, polyval(p_lung_to_tissue, h_axis_lung_to_tissue), 'k-');
    xlabel('Hounsfield Units');
    ylabel('Density [kg/m^3]');
    legend('Soft Tissue', 'Bone', 'Lung', 'Location', 'NorthWest');
    box on;

end


n = 2;
clear robot
robot.NB = n;
robot.parent = [0:n-1];
robot.pitch = zeros(1,n);  % revolute joints

robot.Xtree(1) = Twist(inv(SE3.Rx(pi/2)));  % joint 1 at origin, z in horizontal plane
robot.Xtree(2) = Twist(inv(SE3([1 0 0])));

robot.I(1) = SpatialInertia( 1, [0.5 0 0], diag([0 0 0]) );
robot.I(2) = SpatialInertia( 1, [0.5 0 0], diag([0 0 0]) );

q = [0 0]; qd = [0 0]; qdd = [0 0];

tau = ID( robot, q, qd, qdd)'


% closest fly, based on dell2nose
function [data,units,mind] = compute_closestfly_ell2nose(trx,n,dosave_d)

if nargin < 3,
  dosave_d = true;
end

flies = trx.exp2flies{n};
nflies = numel(flies);
closestfly = cell(1,nflies);
mind = cell(1,nflies);

parfor i1 = 1:nflies,
  fly1 = flies(i1);
  fprintf('fly1 = %d\n',fly1);
  flies2 = flies(trx.roi(fly1)==trx.roi(flies));
  d = nan(numel(flies2),trx(fly1).nframes);

  % use dcenter2nose and major axis length to compute upper and lower bounds on
  % dell2nose
  [mindupper,dlower] = dell2nose_bounds(trx,fly1,flies2);

  for i2 = 1:numel(flies2),
    fly2 = flies2(i2);
    if fly1 == fly2,
      continue;
    end
    % only try for frames where lower bound is smaller than min upper bound
    idx1try = find(mindupper >= dlower(i2,:));
    d(i2,:) = dell2nose_pair(trx,fly1,fly2,idx1try);
  end
  [mind{i1},closesti] = min(d,[],1);
  closestfly{i1} = flies2(closesti);
  closestfly{i1}(isnan(mind{i1})) = nan;
end

% so that we don't compute dcenter twice
if dosave_d,
  data = mind; %#ok<NASGU>
  units = parseunits('mm'); %#ok<NASGU>
  filename = trx.GetPerFrameFile('dell2nose',n);
  save(filename,'data','units');
end

data = closestfly;
units = parseunits('unit');


function [words, k] = compute_features(I, I_rgb, I_type, opts)
% Computes the features specified in spagglom_options.m. Variable. 'words'
% is a cell array, each cell of which is a matrix having the same size as
% the image. Matrix elements denote histogram bins, bin 0 meaning no-value.
% Output 'k' contains number of bins for each feature matrix.

% 'I' is the image in transformed format, or a duplicate of 'I_rgb',
% which must always be in RGB format. 'I_type' is a string specifying the
% format of 'I'.

features_num = 0;
k = [];
words = cell(0);

% denseSIFT with bag-of-words clustering
if opts.feature_dsift_bow
    features_num = features_num + 1;
    [words{features_num}, kt] = feature_dsift(I_rgb, opts); % dsift uses only the rgb image
    k(features_num) = kt;
end

% Cluster colors using k-means
if opts.feature_color_bow
    features_num = features_num + 1;
    [words{features_num}, kt] = feature_color(I, I_type, opts);
    k(features_num) = kt;
end

% Raw rgb histogram
if opts.feature_rgb_raw
    features_num = features_num + 1;
    [words{features_num}, kt] = feature_rgb_raw(I, I_type, opts);
    k(features_num) = kt;
end

% Texture feature used by van de Sande (may not be identical)
if opts.feature_grad_texture
    features_num = features_num + 1;
    [words{features_num}, kt] = feature_grad_texture2(I, opts); % This feature is calculated in three channels, so the 'words' variable unusually has size [3*h,w].
    k(features_num) = kt;
end

% Local binary patterns
if opts.feature_lbp
    features_num = features_num + 1;
    [words{features_num}, kt] = feature_lbp(I_rgb, opts); % rgb images only
    k(features_num) = kt;
end