%
% 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.


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 [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


function [k, sk, skIC] = heatKernDiagCompute(heatKern, x)

% HEATKERNDIAGCOMPUTE Diagonal of a kernel matrix for a HEAT kernel.
% DESC computes the diagonal of the kernel matrix for the HEAT kernel
% given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : a vector containing the diagonal of the kernel matrix
% computed at the given points.
% RETURN sk : unscaled version of the diagonal.
% RETURN sk : unscaled version of the diagonal for the kernel matrix of the
% initial conditions.
%
% SEEALSO : heatKernParamInit, kernDiagCompute, kernCreate, heatKernCompute
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if size(x, 2) ~= 2
    error('Input can only have two columns');
end

option = 'cos';

if strcmp(option, 'cos')
    [k, sk] = heatXheatKernCompute(heatKern, heatKern, x, x);
    k = diag(k);
    sk = diag(sk);
else
    % Split the domain into time domain and spatial domain and account for
    % missing values. If there are no missing values the computation of the
    % kernel is a pointwise prodruct, otherwise it is a kronecker product.
    t = x(x(:,1)~=Inf,1);
    s = x(x(:,2)~=Inf,2);

    if (length(t) == length(s))
        ut = unique(t);
        us = unique(s);
        if (length(ut)*length(us) == length(t))
            t = ut;
            s = us;
            isPointwise = false;
            sk = zeros(length(t)*length(s), 1);
            if heatKern.includeIC
                skIC = zeros(length(t)*length(s), 1);
            end
        else
            isPointwise = true;
            sk = zeros(length(t), 1);
            if heatKern.includeIC
                skIC = zeros(length(t), 1);
            end
        end
    else
        isPointwise = false;
        sk = zeros(length(t)*length(s), 1);
        if heatKern.includeIC
            skIC = zeros(length(t)*length(s), 1);
        end
    end

    % Although this is done in heatKernExpandParam.m, we do it here again as a
    % precaution.

    heatKern.sim.inverseWidth = heatKern.inverseWidthTime;

    sigmax  = sqrt(2/heatKern.inverseWidthSpace);
    lengthX = heatKern.lengthX;
    nterms  = heatKern.nTerms;
    decay   = heatKern.decay;
    diff    = heatKern.diffusion;

    % Precompute some terms
    w = ((1:nterms)*(pi/lengthX))';
    gamma = sqrt(-1)*w;
    beta = decay + diff*(w.^2);
    z1 = sigmax*gamma/2;
    z2 = lengthX/sigmax + z1;
    wz1 = wofzPoppe(sqrt(-1)*z1);
    wz2 = wofzPoppe(sqrt(-1)*z2);
    cK = 4/(lengthX^2);

    simLocal = heatKern.sim;

    if heatKern.includeIC
        sigmah = sqrt(2/heatKern.inverseWidthSpaceIC);
        z1h = sigmah*gamma/2;
        z2h = lengthX/sigmah + z1h;
        wz1h = wofzPoppe(sqrt(-1)*z1h);
        wz2h = wofzPoppe(sqrt(-1)*z2h);
        if isPointwise
            for i=1:nterms
                heatKern.sim.decay = beta(i);
                kt = simKernDiagCompute(heatKern.sim, t);
                ks = sheatKernDiagCompute(sigmax, lengthX, s, w, gamma, wz1, wz2, i, i);
                ktIC = exp(-2*beta(i)*t);
                ksIC = sheatKernDiagCompute(sigmah, lengthX, s, w, gamma, wz1h, wz2h, i, i);
                skIC = skIC + ktIC.*ksIC;
                sk = sk + kt.*ks;
                for j=1:i-1
                    if (mod(i+j,2)==0)
                        heatKern.sim.decay = beta(i);
                        simLocal.decay = beta(j);
                        %kt = diag(simXsimKernCompute(heatKern.sim, simLocal, t, t));
                        kt = simXsimKernDiagCompute(heatKern.sim, simLocal, t);
                        ks = sheatKernDiagCompute(sigmax, lengthX, s, w, gamma, wz1, wz2, i, j);
                        ktIC = exp(-(beta(i)+beta(j))*t);
                        ksIC = sheatKernDiagCompute(sigmah, lengthX, s, w, gamma, wz1h, wz2h, i, j);
                        skIC = skIC + 2*ktIC.*ksIC;
                        sk = sk + 2*kt.*ks;
                    end
                end
            end
        else
            for i=1:nterms
                heatKern.sim.decay = beta(i);
                kt = simKernDiagCompute(heatKern.sim, t);
                ks = sheatKernDiagCompute(sigmax, lengthX, s, w, gamma, wz1, wz2, i, i);
                ktIC = exp(-2*beta(i)*t);
                ksIC = sheatKernDiagCompute(sigmah, lengthX, s, w, gamma, wz1h, wz2h, i, i);
                skIC = skIC + kron(ktIC, ksIC);
                sk = sk + kron(kt, ks);
                for j=1:i-1
                    if (mod(i+j,2)==0)
                        heatKern.sim.decay = beta(i);
                        simLocal.decay = beta(j);
                        %kt = diag(simXsimKernCompute(heatKern.sim, simLocal, t, t));
                        kt = simXsimKernDiagCompute(heatKern.sim, simLocal, t);
                        ks = sheatKernDiagCompute(sigmax, lengthX, s, w, gamma, wz1, wz2, i, j);
                        ktIC = exp(-(beta(i)+beta(j))*t);
                        ksIC = sheatKernDiagCompute(sigmah, lengthX, s, w, gamma, wz1h, wz2h, i, j);
                        skIC = skIC + 2*kron(ktIC, ksIC);
                        sk = sk + 2*kron(kt, ks);
                    end
                end
            end
        end
        sk = cK*sk;
        skIC = cK*skIC;
        k = (heatKern.sensitivity^2)*sk + (heatKern.sensitivityIC^2)*skIC;
    else
        if isPointwise
            for i=1:nterms
                heatKern.sim.decay = beta(i);
                kt = simKernDiagCompute(heatKern.sim, t);
                ks = sheatKernDiagCompute(sigmax, lengthX, s, w, gamma, wz1, wz2, i, i);
                sk = sk + kt.*ks;
                for j=1:i-1
                    if (mod(i+j,2)==0)
                        heatKern.sim.decay = beta(i);
                        simLocal.decay = beta(j);
                        %kt = diag(simXsimKernCompute(heatKern.sim, simLocal, t, t));
                        kt = simXsimKernDiagCompute(heatKern.sim, simLocal, t);
                        ks = sheatKernDiagCompute(sigmax, lengthX, s, w, gamma, wz1, wz2, i, j);
                        sk = sk + 2*kt.*ks;
                    end
                end
            end
        else
            for i=1:nterms
                heatKern.sim.decay = beta(i);
                kt = simKernDiagCompute(heatKern.sim, t);
                ks = sheatKernDiagCompute(sigmax, lengthX, s, w, gamma, wz1, wz2, i, i);
                sk = sk + kron(kt, ks);
                for j=1:i-1
                    if (mod(i+j,2)==0)
                        heatKern.sim.decay = beta(i);
                        simLocal.decay = beta(j);
                        %kt = diag(simXsimKernCompute(heatKern.sim, simLocal, t, t));
                        kt = simXsimKernDiagCompute(heatKern.sim, simLocal, t);
                        ks = sheatKernDiagCompute(sigmax, lengthX, s, w, gamma, wz1, wz2, i, j);
                        sk = sk + kron(2*kt, ks);
                    end
                end
            end
        end
        sk = cK*sk;
        k = (heatKern.sensitivity^2)*sk;
        if nargin > 2
            skIC = 0;
        end
    end
end

k = real(k);
sk = real(sk);


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
% function P = nodal2center(y,m,mex)
%
% transfers a nodal grid to a cell-centered grid
% if nargin==1, builds P explicitely, else return results of P(y), matrix free; endif
% depending on numel(Y), the matrix free version also handles P'(Y)
%
% Input:
%   y        input points,  nodal or cell-centered
%   m        number of discretization points
%   mex      bool (use mex code for matrix-free implementation)
%
% Output:
%   P        the projection matrix P, if nargin == 2
%            P*y,                     if y is nodal
%            P'*y,                    if y is cell-centered
%==============================================================================

function P = nodal2center(y,m,mex)

if nargin == 0 % help and minimal example
  help(mfilename);
  runMinimalExample;
  P = 'endMinimalExample';
  return
end

Js = {[1 2 3], [2 1 3], [3 1 2]}; % different permutations of dimensions

if nargin==1, m = y; end
dim = length(m);

% Here starts the matrix-based code
if nargin==1
  % ----------------------------------
  % build the matrix P and return it
  % ----------------------------------
  av = @(i) spdiags(ones(m(i),1)*[ 1,1],[0,1],m(i),m(i)+1)/2;
  zero = sparse(prod(m),prod(m+1));
  switch dim
    case 2
      A = kron(av(2), av(1));
      P = sparse([A zero; zero A]);
    case 3
      A = kron(av(3), kron(av(2), av(1)));
      P = sparse([A zero zero; zero A zero; zero zero A]);
    otherwise
      error('Dimension must be either 2 or 3.')
  end
  return; % end of story
end

% Here starts the MEX based code
if (dim > 1) && exist(fullfile(FAIRpath,'kernel',[mfilename,'C.',mexext])) == 3,
  if numel(y) == length(m)*prod(m)
    % cell-centered ->  nodal
    status = true;
  else
    % nodal -> cell-centered
    status = false;
  end;
  P = nodal2centerC(y, m, dim, status);
  return;
end;

% Here starts the matrix-free code

if numel(y) == length(m)*prod(m)
  % -----------------------
  % cell-centered ->  nodal
  % -----------------------
  y = reshape(y,prod(m),[]);
  Z = zeros(prod(m+1),size(y,2));         % allocate memory
  for i=1:size(y,2)                       % run over all components of Y
    yi = reshape(y(:,i),m);
    for j=1:dim                           % run over all dimensions in yi
      % J = [j,setdiff(1:dim,j)];         % make j-th dimension first
      J = Js{j};                          % make j-th dimension first
      yi = permute(yi,J);

      zi = yi([1 1:end],:,:);
      zi(2:end-1,:,:) = zi(2:end-1,:,:) + yi(2:end,:,:);

      yi = .5* ipermute(zi,J);            % undo permutation
    end
    Z(:,i) = yi(:);                       % store i-th component
  end
else
  % -----------------------
  % nodal -> cell-centered
  % -----------------------
  % MATLAB implementation
  y = reshape(y,prod(m+1),[]);
  Z = zeros(prod(m),size(y,2));           % allocate memory
  for i=1:size(y,2)                       % run over all components of Y
    if dim==1                             % 1D case is simplr and does not admit oermutations
      P = 0.5*(y(1:end-1)+y(2:end));
      return
    else
      yi = reshape(y(:,i),m+1);           % reorganize Y
    end
    for j=1:dim                           % run over all dimensions in yi
      % J = [j,setdiff(1:dim,j)];         % make j-th dimension first
      J = Js{j};                          % make j-th dimension first
      yi = permute(yi,J);
      % average nodal to center
      yi = 0.5*(yi(1:end-1,:,:)+yi(2:end,:,:));
      yi = ipermute(yi,J);                % undo permutation
    end
    Z(:,i) = reshape(yi,[],1);            % store i-th component
  end

end
P = reshape(Z,[],1);                      % reshape
%------------------------------------------------------------------------------

function runMinimalExample

omega = [0 2 0 1];
m     = [8,7];
yn    = getNodalGrid(omega,m);
xc    = nodal2center(yn,m);
xc    = reshape(xc,[],2);

FAIRfigure(1); clf;
subplot(2,1,1); spy(nodal2center([4,5,6])); title('spy(nodal2gcenter operator)');
subplot(2,1,2); plotGrid(yn,omega,m,'color','b'); hold on; plot(xc(:,1),xc(:,2),'rx');
title(mfilename);

%==============================================================================


function facenb=faceneighbors(t,opt)
%
% facenb=faceneighbors(t,opt)
%
% to find 4 face-neighboring elements of a tetrahedron
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%     t: tetrahedron element list, 4 columns of integers
%     opt: if opt='surface', return boundary triangle list
%          (should be the same as the face output from v2m)
%          if opt='rowmajor', same as 'surface', except the
%          order of the triangles are in the row-major order
%%
%          otherwise, return the element list for each element:
%          each row contains 4 numbers, representing the element
%          indices sharing triangular faces [1 2 3],[1 2 4],[1 3 4]
%          and [2 3 4] in order, where 1~4 is the node local index.
%          if the index is 0, indicating the face has no neighbor
%          (i.e. a boundary face)
%
% output:
%     facenb: see opt
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

faces=[t(:,[1,2,3]);
       t(:,[1,2,4]);
       t(:,[1,3,4]);
       t(:,[2,3,4])];

faces=sort(faces,2);
[foo,ix,jx]=unique(faces,'rows');
if(isoctavemesh)
        u=unique(jx);
        qx=u(hist(jx,u)==2);
else
        vec=histc(jx,1:max(jx));
        qx=find(vec==2);
end

nn=max(t(:));
ne=size(t,1);
facenb=zeros(size(t));

% now I need to find all repeatitive elements
% that share a face, to do this, unique('first')
% will give me the 1st element, and 'last' will
% give me the second. There will be no more than 2

% doing this is 60 times faster than doing find(jx==qx(i))
% inside a loop

if(isoctavemesh || datenum(version('-date'))>datenum('January 27 2006')) % compare to matlab 7.2
	[ujx,ii]=unique(jx,'first');
	[ujx,ii2]=unique(jx,'last');
else
	ujx=unique(jx);
	[t1,ii2]=ismember(ujx,jx);
	[t1,ii]=ismember(ujx,flipwd(jx(:)));
	ii=length(jx)-ii+1;
end

% iddup is the list of all pairs that share a common face

iddup=[ii(qx) ii2(qx)];
faceid=ceil(iddup/ne);
eid=mod(iddup,ne);
eid(eid==0)=ne;

% now rearrange this list into an element format

for i=1:length(qx)
	facenb(eid(i,1),faceid(i,1))=eid(i,2);
	facenb(eid(i,2),faceid(i,2))=eid(i,1);
end

% facenb may contain 0s, that just means the corresponding
% face is a boundary face and has no neighbor.

% if the second option is 'surface', I am going to find
% and return surface patches only

if(nargin==2)
  if(strcmp(opt,'surface'))
	facenb=faces(find(facenb==0),:);
  elseif(strcmp(opt,'rowmajor'))
	index=[1:length(faces)];
	index=(reshape(index,[],4))';
	faces=faces(index(:),:);
	facenb=faces(find(facenb'==0),:);
  else
        error(['supplied option "' opt '" is not supported.']);
  end
end


function [Model, Info] = linear_sparse_stepwise(X,Y,Model,parm)
%  Estimate linear weight matrix for input-output mapping
%     Automatic Relevance Prior for each input dimension
%     is imposed to get sparse weight matrix
%
% Notice !!
%     Delay embedding is done in this module,
%     then input vector should be original input data without embedding
%
%   [Model, Info] = linear_sparse_stepwise(X,Y,Model,parm)
%     Scalar iteration version
% --- Input
%  Y  : Output data ( N x T x Ntrial )
%  X  : Input data  ( M x (T + (Dtau-1)*Tau)  x Ntrial)
%  N  =  # of output
%  M  =  # of input (original input space dimension)
%  T  =  # of time sample
%
%  Estimate the following model
%    Y(t) = W * [X(:, t + (Dtau-1)*Tau ); ...; X(:, t)]
%
%  Model : Structure for estimated model
%  if Model is empty, initialization is done before training
%  if Model is previous training result, re-training us done
%
%  parm  : Structure for learning parameter
%  parm.Npre_train :  # of VB-update in initial training
%  parm.Ntrain :  # of training
%  parm.Nskip  :  skip # for print
%  parm.a_min  :  Min value for pruning small variance component
%  parm.Prune  :  = 1 : Prune small variance & irrelevant input dimension
%
%  parm.Tau       = Lag time
%  parm.Dtau      = Number of embedding dimension
%    Total input dimension in embedding space is (M * parm.Dtau)
% --- Output
%  Model : Structure for estimated model
%  Model.SY  :  Noise variance         ( 1 x 1 )
%  Model.W   :  Weight matrix          ( N x M*D ) , D = parm.Dtau
%  Model.A   :  Prior weight variance  ( 1 x M*D )
%  Model.ix_act : Active index for W after pruning
%
%  Info  : Structure for learning process history
%  Info.FE  = LP + H : Free energy
%  Info.LP  = Log likelihood
%  Info.H   = - Model entropy
%
% 2009-10-18 Made by M. Sato

MINVAL = 1.0e-15;

fprintf('linear_sparse_stepwise start\n')

% Dimension
[N ,T  ,Ntrial]  = size(Y); % N = # of output
[M ,Tx ,Ntrialx] = size(X); % M = # of input without embedding

% Number of samples for each trial
if isfield(parm,'Trial'),
	Trial = parm.Trial;
	if length(Trial) ~= Ntrial, error('Trial sample seting is wrong');end
	if max(Trial) > T, error('Trial sample seting is wrong');end
else
	Trial = repmat(T, [Ntrial 1]);
end
% Set valid sample flag for each trial
trial_sw = zeros(N,T,Ntrial);
for n = 1:Ntrial
	trial_sw(:,1:Trial(n),n) = 1;
end

Tall = sum(Trial);

if Ntrial~=Ntrialx, error('# of trial is different in X and Y'); end;

Nskip  = 100;   % skip steps for display info
a_min = 1e-14;	% Minimum value for weight pruning
Fdiff = 1e-12; % Threshold for convergence
Ncheck = 100;   % Minimum number of training iteration
Fstep  = 5;     % Free energy convergence check step
Prune  = 1;     % Prune mode
space_ARD=1;	% if space_ARD=1, ARD is done only for space dimension

if isfield(parm,'Nskip'), Nskip  = parm.Nskip; end;
if isfield(parm,'Fdiff'), Fdiff   = parm.Fdiff; end;
if isfield(parm,'a_min'), a_min   = parm.a_min ; end;
if isfield(parm,'Prune'), Prune = parm.Prune; end;
if isfield(parm,'Ncheck'), Ncheck = parm.Ncheck; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end
if isfield(parm,'space_ARD'),space_ARD  = parm.space_ARD ; end

% # of embedding dimension
if isfield(parm,'Dtau')
	D    = parm.Dtau;
	tau  = parm.Tau;
else
	D    = 1;
	tau  = 1;
end
% Parameters
Ntrain = parm.Ntrain;

if Ntrain < 1, Info = []; return; end;

if isfield(parm,'Npre_train')
	Npre_train = parm.Npre_train;
else
%	Npre_train = Ntrain;
	if Tall >= 2*M*D
		Npre_train = 0;
	elseif Tall >= M*D
		Npre_train = fix(Ntrain/2);
	else
		Npre_train = Ntrain;
	end
end

if Npre_train > Ntrain, Npre_train = Ntrain; end;

if isfield(parm,'Nupdate')
	Nupdate = parm.Nupdate;
else
	Nupdate = 1;
end

fprintf('--- Output Dimension  = %d\n',N)
fprintf('--- Input  Dimension  = %d\n',M)
fprintf('--- Embedding  Dimension  = %d\n',D)
fprintf('--- Number of trials  = %d\n',Ntrial)
fprintf('--- Number of training sample = %d\n',Tall)
fprintf('--- Total update iteration    = %d (%d)\n',Ntrain,Npre_train)

% original input dimension
Xdim  = M;
M = Xdim*D;
M_ALL = M;

%
% --- Initialization
%

% Input/Output variance
sx = mean((X(:) - mean(X(:))).^2);
sy = mean((Y(:) - mean(Y(:))).^2);

SY0 = mean(sy);
A0  = 1./mean(sx);

if isfield(parm,'Ta0') && parm.Ta0 > 0,
	Ta0 = parm.Ta0;
	a0  = parm.a0 * A0;
else
	Ta0 = 0;
	a0  = 1;
end

% Input covariance
Trial = Trial+(D-1)*tau;

XX = zeros(Xdim,1);
for n=1:Ntrial
	XX = XX + sum(X(:,1:Trial(n), n).^2 , 2);
end
XX  = XX / sum(Trial);
%XX  = sum(sum(X.^2,3),2)/(Tx*Ntrial);
XX  = repmat(XX', [1 D]);% 1 x M

if isfield(Model,'ix_act')
	SY  = mean(Model.SY);  % 1 x 1

	% Active index
	ix_act = Model.ix_act;
	A = zeros(1,M_ALL);
	W = zeros(N,M_ALL);

	A(ix_act)   = sum(Model.A,1);
	W(:,ix_act) = Model.W;

	if M_ALL ~= Xdim*D,
		fprintf('M_ALL=%d,Xdim=%d,D=%d\n',M_ALL ,Xdim ,D)
		error('M_ALL ~= Xdim*D')
	end

	% Active index in input space without embedding
	IX_dim = find( sum(reshape(A,[Xdim,D]),2) > 0);

	IX_act = repmat( (0:(D-1))* Xdim ,[length(IX_dim) 1]) ...
	       + repmat(IX_dim, [1 D]);
	IX_act = IX_act(:);

	W  = W(:,IX_act) ;  % N x (M*D)
	A  = A(IX_act) ;  % 1 x (M*D)
	X  = X(IX_dim,:,:);
	XX = XX(IX_act);

	M     = length(IX_act);
	Xdim  = length(IX_dim);
else
	A = repmat(A0, [1,M_ALL]);
	W = zeros(N,M_ALL);
	SY  = SY0;
	% Save original input for pruning
	IX_act  = 1:M_ALL;
	IX_dim  = 1:Xdim;
end

ix_act  = 1:M;
ix_dim  = 1:Xdim;
M_all   = M;

% Delay embedding index for W
Wid = [(0:(D-1))'* Xdim + 1 , (1:D)'* Xdim];

% Working variable
G_A = zeros(N,M);       % N x M

if size(A,1)==Xdim && size(A,2)==1
	A   = repmat(A',[1 D]);
elseif size(A,1)==1 && size(A,2)==Xdim
	A   = repmat(A,[1 D]);
end

SW = SY./( Tall*XX + SY./A );

fprintf('a_min = %g\n', a_min)
fprintf('SY0 = %g\n', SY0)
fprintf('SY  = %g\n', SY)

% Free energy histry
FE  = zeros(Ntrain,1);
LP  = zeros(Ntrain,1);
H   = zeros(Ntrain,1);
Err = zeros(Ntrain,1);
Mhist = zeros(Ntrain,1);

% ARD hyper param. history
if isfield(parm,'Debug') && ~isempty(parm.Debug) && parm.Debug > 0
	Debug = 1;
	A_tmp = zeros(N*M_ALL, ceil(Ntrain/Nskip));
else
	Debug = 0;
end

% recover all component
A_all  = zeros(1,M_all);       % N x M

dY = error_delay_time_sw(X,Y,W,tau,Trial);
dY = dY .* trial_sw;                           % mask invalid samples

k_save  = 0;

%%%%%% Learning Loop %%%%%%
for k=1:Ntrain
	% Ainv = alpha , A = 1/alpha
	% (Y-W*X)^2/SY + W^2/A
	% ( (Y-W*X)^2 + W^2*Ainv )/SY
	Ainv    = SY./A;

    W   = weight_update_embed_sw(X, dY, W, Tall*XX, Ainv, tau, Trial);
	dY  = error_delay_time_sw(X, Y, W, tau, Trial);
	dY  = dY .* trial_sw;                           % mask invalid samples

    dYY = sum(dY(:).^2)/(N*Tall);
	WW  = sum(W.^2,1);

    % Noise variance update
    SY  = dYY + sum(WW .* Ainv)/(N*Tall);
    % Prevent zero variance
    SY  = max( SY, MINVAL);

    % Log variance
    SWA     = max( SW .* Ainv , MINVAL);
    log_sw  = N*(sum( log(SWA) - SWA + 1 ));
    log_sy  = N*( log(SY) );
    log_a   = Ta0*(sum(log(Ainv) - a0.*Ainv + 1));

    % Free energy
    LP(k)  = - (0.5*Tall) * log_sy ;
    H(k)   = 0.5*( log_sw + log_a - sum(WW .*Ainv) );
    FE(k)  = LP(k) + H(k);
    Err(k) = sum(dYY)/(N*SY0);

    % Weight variance

    SW = SY./( Tall*XX + Ainv );

    % Hyper parameter for weight variance (ARD)
	if mod(k,Nupdate) == 0,
	    % A = Alpha^-1 * SY
		% Ainv = SY./A;
	    % SW = SY./( Tall*XX + Ainv );

	    % G_A = 1 - SW./A;
	    %     = (Tall*XX + Ainv - SY/A) * SW/SY
	    G_A = Tall.*XX.*SW./SY;
	    G_A = max((G_A), MINVAL);

		% N*A = N * (Alpha * SY) =  (W.^2) + N * SW  ;
		if k <= Npre_train,
			% VB update rule (Stable)
			%  ARD for each weight
			A  = (WW + N*SW + 2*Ta0*a0)/( N + 2*Ta0 );
		else
			% Accelerated update rule (Unstable)
		    A  = sqrt( A.* WW./(G_A * N));
		    %A  = (WW + 2*Ta0*a0)./(G_A * N + 2*Ta0);
		end

		if space_ARD==1
			Am = mean(reshape(A,[M/D,D]),2);
			A  = repmat(Am', 1,D);
		end

	    % Prune small variance
	    if Prune == 1
		    % Find active input dimension
		    ix_act_old = ix_act;
		    ix_dim_old = ix_dim;

		    % recover all component
			A_all  = zeros(1,M_all);       % 1 x M
		    %  A_all(:,ix_act)  = A;
		    A_all(ix_act) = WW/max(WW);

		    % find active input dimension (absolute index)
		    ix_dim = find( sum(reshape(A_all,[Xdim,D]),2) > a_min );
		    ix_act = repmat(ix_dim, [1 D]) ...
		           + repmat([0:D-1]*Xdim, [length(ix_dim) 1]);
		    ix_act = ix_act(:);

		    Mnew   = length(ix_act);  		% # of effective input

		    if Mnew < M,
			    % convert to relative index
			    jx_act = trans_index(ix_act,ix_act_old,M_all);
			    jx_dim = trans_index(ix_dim,ix_dim_old,Xdim);

			    M   = Mnew;
			    A   = A(jx_act) ;  % N x M
			    W   = W(:,jx_act) ;  % N x M
			    SW  = SW(jx_act);  % N x M

			    X	 = X(jx_dim,:,:);  	 	% M x T
			    XX	 = XX(jx_act);  	 	% M x T
			end
	    end
	    % END of if Prune == 1

		A = max(A,MINVAL);

	end
	% END of if mod(k,Nupdate) == 0

	Mhist(k) = M;
	SW  = SW /SY;

    if mod(k, Nskip)==0
        % Save history
		if Debug == 1
        	k_save = k_save + 1;
        	A_tmp(:,k_save) = A_all(:);
		end

        fprintf('Iter = %4d, M = %4d, err = %g, F = %g, SY= %g, H = %g\n', ...
               k, length(ix_act), Err(k), FE(k), SY, - 2*H(k)/log(T));
    end

    % Convergence check
	if k > Ncheck,
		Fdif = (FE(k) - FE(k-Fstep))/(abs(FE(k))+eps);
	else
		Fdif = Fdiff + 1;
	end

	if (Fdiff > abs(Fdif)),
		fprintf('Converged : Free energy change = %g\n',Fdif)
		break;
	end;
end

ix_act = IX_act(ix_act);

% Active index
Model.ix_act = ix_act;
Model.M_all  = M_ALL ;

% Save output variable
Model.A    = A ;
Model.W    = W ;
Model.SY   = SY;
Model.SW   = SW; % = 1./(Tall*XX + diag(Ainv))

Model.method = 'linear_sparse_stepwise';
Model.mode   = 'scalar';
Model.sparse = 'sparse';

% Save history
Info.FE  = FE(1:k);
Info.LP  = LP(1:k);
Info.H   = H(1:k) ;
Info.Err = Err(1:k);
Info.M   = Mhist(1:k);

if exist('A_tmp','var')
	Info.A   = A_tmp(:,1:k_save) ;
end

% recover all component
%W_all   = zeros(N,M_all);
%SW_all  = zeros(N,M_all);
%
%A_all(:,ix_act)  = A;
%W_all(:,ix_act)  = W ;  % N x M
%SW_all(:,ix_act) = SW;  % N x M

%%% ---- Index transformation from old active_index to current active_index
function	jx = trans_index(ix,ix_old,M)

N = length(ix_old);
Itrans = zeros(M,1);
Itrans(ix_old) = 1:N;

jx = Itrans(ix);


% sph2topo() - Convert from a 3-column headplot file in spherical coordinates
%              to 3-column topoplot() locs file in polar (not cylindrical) coords.
%              Used for topoplot() and other 2-D topographic plotting programs.
%              Assumes a spherical coordinate system in which horizontal angles
%              have a range [-180,180] deg,  with zero pointing to the right ear.
%              In the output polar coordinate system, zero points to the nose.
%              See  >> help readlocs
% Usage:
%          >> [chan_num,angle,radius] = sph2topo(input,shrink_factor,method);
%
% Inputs:
%   input         = [channo,az,horiz] = chan_number, azumith (deg), horiz. angle (deg)
%                   When az>0, horiz=0 -> right ear, 90 -> nose
%                   When az<0, horiz=0 -> left ear, -90 -> nose
%   shrink_factor = arc_length shrinking factor>=1 (deprecated).
%                   1 -> plot edge is 90 deg azimuth {default};
%                   1.5 -> plot edge is +/-135 deg azimuth See
%                   >> help topoplot().
%   method        = [1|2], optional. 1 is for Besa compatibility, 2 is for
%                   compatibility with Matlab function cart2sph(). Default is 2
%
% Outputs:
%   channo  = channel number (as in input)
%   angle   = horizontal angle (0 -> nose; 90 -> right ear; -90 -> left ear)
%   radius  = arc_lengrh from vertex (Note: 90 deg az -> 0.5/shrink_factor);
%             By topoplot() convention, radius=0.5 is the nasion-ear_canal plane.
%             Use topoplot() 'plotrad' to plot chans with abs(az) > 90 deg.
%
% Author: Scott Makeig & Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 6/12/98
%
% See also: cart2topo(), topo2sph()

% Copyright (C) 6/12/98 Scott Makeig, SCCN/INC/UCSD, scott@sccn.ucsd.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% corrected left/right orientation mismatch, Blair Hicks 6/20/98
% changed name sph2pol() -> sph2topo() for compatibility -sm
% 01-25-02 reformated help & license -ad
% 01-25-02 changed computation so that it works with sph2topo -ad

function [channo,angle,radius] = sph2topo(input,factor, method)

chans = size(input,1);
angle = zeros(chans,1);
radius = zeros(chans,1);

if nargin < 1
   help sph2topo
   return
end

if nargin< 2
  factor = 0;
end
if factor==0
  factor = 1;
end
if factor < 1
  help sph2topo
  return
end

if size(input,2) ~= 3
   help sph2topo
   return
end

channo = input(:,1);
az = input(:,2);
horiz = input(:,3);

if exist('method')== 1 & method == 1
  radius = abs(az/180)/factor;
  i = find(az>=0);
  angle(i) = 90-horiz(i);
  i = find(az<0);
  angle(i) = -90-horiz(i);
else
  angle  = -horiz;
  radius = 0.5 - az/180;
end;


% -------------------------------------------------------------------------------------------------------------------------
function [init_sz, final_sz] = ideal_size(net, max_sz)
%   Luca Bertinetto, Jack Valmadre, Joao F. Henriques, 2016
% -------------------------------------------------------------------------------------------------------------------------
final_sz = forward(net, max_sz);
init_sz = backward(net, final_sz);
while ~all(init_sz <= max_sz)
    final_sz = final_sz - 1;
    init_sz = backward(net, final_sz);
end

end

function n = forward(net, n)
    for i = 1:numel(net.layers)
        l = net.layers{i};
        switch l.type
        case 'conv'
            m = [size(l.weights{1}, 1), size(l.weights{1}, 2)];
            n = filter(n, l.pad, m, l.stride);
        case 'pool'
            n = filter(n, l.pad, l.pool, l.stride);
        end
    end
end

function n = backward(net, n)
    for i = numel(net.layers):-1:1
        l = net.layers{i};
        switch l.type
        case 'conv'
            m = [size(l.weights{1}, 1), size(l.weights{1}, 2)];
            n = unfilter(n, l.pad, m, l.stride);
        case 'pool'
            n = unfilter(n, l.pad, l.pool, l.stride);
        end
    end
end

function n = filter(n, pad, m, k)
assert(numel(pad) == 1);
n = floor((n + 2*pad - m) / k) + 1;
end

function n = unfilter(n, pad, m, k)
assert(numel(pad) == 1);
n = k*(n - 1) + m - 2*pad;
end