classdef FFE_A2TrackedLevels < handle
    %FFE_A2TrackedLevels FFE with A2 tracked-level decision thresholds.
    %
    % This class is intentionally narrow: it contains the normal adaptive FFE
    % flow plus the A2 tracked-level decision logic. The output sample y stays
    % unchanged; only the decision constellation is moved by the tracked
    % level-wise offsets.

    properties
        sps
        order
        e
        e_tr
        error

        len_tr
        mu_tr
        epochs_tr

        adaption_technique
        dd_mode
        mu_dd
        epochs_dd
        dd_len_fraction

        % A2 tracked-level decision
        dc_level_avg_bufferlength_a2
        dc_level_update_blocklength_a2
        dc_smoothing_a2
        dc_level_weights_a2

        P
        constellation

        decide

        save_debug = 0
        debug_struct
    end

    methods
        function obj = FFE_A2TrackedLevels(options)
            arguments(Input)
                options.sps = 2
                options.order = 15

                options.len_tr = 4096
                options.mu_tr = 0
                options.epochs_tr = 5

                options.adaption_technique adaption_method = adaption_method.lms
                options.dd_mode = 1
                options.mu_dd = 1e-5
                options.epochs_dd = 5
                options.dd_len_fraction = 1

                options.dc_level_avg_bufferlength_a2 = 0
                options.dc_level_update_blocklength_a2 = 0
                options.dc_smoothing_a2 = 0
                options.dc_level_weights_a2 = 0

                options.decide = false
                options.save_debug = 0
            end

            fn = fieldnames(options);
            for n = 1:numel(fn)
                obj.(fn{n}) = options.(fn{n});
            end

            assert(obj.dc_level_avg_bufferlength_a2 >= 0);
            assert(obj.dc_level_update_blocklength_a2 >= 0);

            obj.e = zeros(obj.order,1);
            obj.error = 0;
            obj.dc_level_avg_bufferlength_a2 = floor(obj.dc_level_avg_bufferlength_a2);
            obj.dc_level_update_blocklength_a2 = floor(obj.dc_level_update_blocklength_a2);
            obj.dc_smoothing_a2 = min(max(obj.dc_smoothing_a2,0),1);
        end

        function [X,Noi] = process(obj,X,D)
            X = X.normalize("mode","rms");

            obj.constellation = unique(D.signal);

            delta = 0.05;
            obj.P = (1/delta) * eye(obj.order);

            training = true;
            showviz = false;
            obj.equalize(X.signal,D.signal,obj.mu_tr,obj.epochs_tr,obj.len_tr,training,showviz);
            obj.e_tr = obj.e;

            n = X.length;
            training = false;
            if obj.dd_mode
                [signal,decision] = obj.equalize(X.signal,D.signal,obj.mu_dd,obj.epochs_dd,n,training,showviz);
            else
                [signal,decision] = obj.equalize(X.signal,D.signal,0,1,n,training,showviz);
            end

            if obj.decide
                X.signal = decision;
            else
                X.signal = signal;
            end

            X.fs = D.fs;
            X = X.logbookentry([num2str(obj.order),' tap FFE A2 tracked levels']);

            Noi = X - D;
        end

        function [y,d_hat] = equalize(obj,x,d,mu,epochs,N,training,showviz)
            arguments
                obj
                x
                d
                mu
                epochs
                N
                training
                showviz %#ok<INUSA>
            end

            x = [zeros(floor(obj.order/2),1); x; zeros(obj.order,1)];
            lambda = mu;
            n_symbols = ceil(N / obj.sps);
            y = zeros(n_symbols,1);
            d_hat = zeros(n_symbols,1);
            err = zeros(n_symbols,1);
            constellation = obj.constellation;

            adaption_code = obj.adaptionCode();
            adaption_is_rls = adaption_code == 3;

            mask = ones(obj.order,1);
            maincursor_pos = ceil(length(obj.e)/2);
            grad = 0;
            weight = 0;
            update = 0;

            if mu == 0 || (~obj.dd_mode && ~training)
                epochs = 1;
            end

            dc_level_enabled = obj.dc_level_avg_bufferlength_a2 > 1 && any(obj.dc_level_weights_a2(:) ~= 0);
            if dc_level_enabled
                if isempty(constellation)
                    builtin("error","FFE_A2TrackedLevels:MissingConstellation", ...
                        "A2 tracked-level decision requires obj.constellation to be set.");
                end

                n_levels = numel(constellation);
                if isscalar(obj.dc_level_weights_a2)
                    dc_level_weight_by_level = repmat(obj.dc_level_weights_a2,n_levels,1);
                elseif numel(obj.dc_level_weights_a2) == n_levels
                    dc_level_weight_by_level = obj.dc_level_weights_a2(:);
                else
                    builtin("error","FFE_A2TrackedLevels:InvalidDCLevelWeights", ...
                        "dc_level_weights_a2 must be scalar or have one entry per constellation level.");
                end

                dc_level_buffer_len = obj.dc_level_avg_bufferlength_a2;
                dc_level_buffer = NaN(n_levels,dc_level_buffer_len);
                dc_level_buffer_pos_by_level = zeros(n_levels,1);
                dc_level_sum_by_level = zeros(n_levels,1);
                dc_level_buffer_valid_count_by_level = zeros(n_levels,1);
                dc_level_offset_by_level = zeros(n_levels,1);
                dc_level_valid_count_by_level = zeros(n_levels,1);
                dc_level_update_blocklength = obj.dc_level_update_blocklength_a2;
                if dc_level_update_blocklength <= 0
                    dc_level_update_blocklength = dc_level_buffer_len;
                end
                dc_level_update_blocklength = max(1,floor(dc_level_update_blocklength));
                dc_level_window_future_fraction = obj.dc_smoothing_a2; %#ok<NASGU>
            end

            debug_enabled = obj.save_debug;
            if debug_enabled
                obj.debug_struct.error = NaN(1,n_symbols);
                obj.debug_struct.error_first_epoch = NaN(1,n_symbols);
                obj.debug_struct.main_cursor = NaN(1,n_symbols);
                obj.debug_struct.mu_nlms = NaN(1,n_symbols);
                obj.debug_struct.update_gradient = NaN(1,n_symbols);
                obj.debug_struct.dc_level_mpi_est = NaN(1,n_symbols);
                obj.debug_struct.dc_level_weight = NaN(1,n_symbols);
                obj.debug_struct.dc_level_valid_count = NaN(1,n_symbols);
                obj.debug_struct.dc_level_symbol_idx = NaN(1,n_symbols);
                obj.debug_struct.dc_level_decision_level = NaN(1,n_symbols);
                obj.debug_struct.dc_level_y_raw = NaN(1,n_symbols);

                if training
                    obj.debug_struct.error_tr = NaN(1,n_symbols);
                    obj.debug_struct.update_tr = NaN(1,n_symbols);
                else
                    obj.debug_struct.error_dd = NaN(1,n_symbols);
                    obj.debug_struct.update = NaN(1,n_symbols);
                end
            end

            for epoch = 1:epochs
                symbol = 0;
                for sample = 1:obj.sps:N
                    symbol = symbol + 1;
                    dc_level_offset = 0;
                    dc_level_weight = 0;
                    dc_level_valid_count = 0;

                    U = x(obj.order+sample-1:-1:sample);
                    y(symbol,1) = (obj.e.*mask).' * U;
                    y_raw = y(symbol);

                    if training
                        d_hat(symbol,1) = d(symbol);
                        [~,symbol_idx] = min(abs(d_hat(symbol) - constellation));
                        dc_level_decision_level = constellation(symbol_idx);
                    else
                        if dc_level_enabled
                            dc_level_ramp_by_level = min(dc_level_valid_count_by_level / dc_level_buffer_len,1);
                            dc_level_decision_constellation = constellation + ...
                                dc_level_weight_by_level .* dc_level_ramp_by_level .* dc_level_offset_by_level;
                            [~,symbol_idx] = min(abs(y_raw - dc_level_decision_constellation));
                            dc_level_decision_level = dc_level_decision_constellation(symbol_idx);
                        else
                            [~,symbol_idx] = min(abs(y_raw - constellation));
                            dc_level_decision_level = constellation(symbol_idx);
                        end
                        d_hat(symbol,1) = constellation(symbol_idx);
                    end

                    dc_level_symbol_idx = symbol_idx;
                    if dc_level_enabled
                        dc_level_offset = dc_level_offset_by_level(symbol_idx);
                        dc_level_valid_count = dc_level_valid_count_by_level(symbol_idx);
                        dc_level_weight = dc_level_weight_by_level(symbol_idx) * ...
                            min(dc_level_valid_count / dc_level_buffer_len,1);

                        [dc_level_buffer,dc_level_buffer_pos_by_level,dc_level_sum_by_level, ...
                            dc_level_buffer_valid_count_by_level,dc_level_offset_by_level, ...
                            dc_level_valid_count_by_level] = obj.updateTrackedLevelBuffer( ...
                            dc_level_buffer,dc_level_buffer_pos_by_level,dc_level_sum_by_level, ...
                            dc_level_buffer_valid_count_by_level,dc_level_offset_by_level, ...
                            dc_level_valid_count_by_level,y_raw,symbol_idx,symbol, ...
                            dc_level_update_blocklength,constellation);
                    end

                    err(symbol) = d_hat(symbol) - y(symbol);

                    if training || obj.dd_mode
                        switch adaption_code
                            case 1
                                normU = (U.'*U) + eps;
                                weight = mu / normU;
                                grad = err(symbol) * U;
                                update = grad * weight;
                                obj.e = obj.e + update;

                            case 2
                                weight = mu;
                                grad = err(symbol) * U;
                                update = grad * weight;
                                obj.e = obj.e + update;

                            case 3
                                denom = lambda + U.' * obj.P * U;
                                k = (obj.P * U) / denom;
                                update = k * err(symbol);
                                obj.e = obj.e + update;
                                obj.P = (1/lambda) * (obj.P - k * (U.' * obj.P));
                        end
                    end

                    if debug_enabled && epoch == 1
                        obj.debug_struct.error_first_epoch(1,symbol) = err(symbol) * err(symbol)';
                    end

                    if debug_enabled && epoch == epochs
                        error_power = err(symbol) * err(symbol)';
                        update_power = update.'*update ./ (sqrt((obj.e.'*obj.e) / obj.order) + eps);
                        obj.debug_struct.error(1,symbol) = error_power;
                        obj.debug_struct.main_cursor(1,symbol) = abs(obj.e(maincursor_pos));
                        obj.debug_struct.mu_nlms(1,symbol) = weight;
                        obj.debug_struct.update_gradient(1,symbol) = grad.'*grad;
                        obj.debug_struct.dc_level_mpi_est(1,symbol) = dc_level_offset;
                        obj.debug_struct.dc_level_weight(1,symbol) = dc_level_weight;
                        obj.debug_struct.dc_level_valid_count(1,symbol) = dc_level_valid_count;
                        obj.debug_struct.dc_level_symbol_idx(1,symbol) = dc_level_symbol_idx;
                        obj.debug_struct.dc_level_decision_level(1,symbol) = dc_level_decision_level;
                        obj.debug_struct.dc_level_y_raw(1,symbol) = y_raw;

                        if training
                            obj.debug_struct.error_tr(1,symbol) = error_power;
                            obj.debug_struct.update_tr(1,symbol) = update_power;
                        else
                            obj.debug_struct.error_dd(1,symbol) = error_power;
                            obj.debug_struct.update(1,symbol) = update_power;
                        end
                    end
                end
            end

            if ~adaption_is_rls
                obj.P = [];
            end
        end
    end

    methods (Access = private)
        function adaption_code = adaptionCode(obj)
            if obj.adaption_technique == adaption_method.nlms
                adaption_code = 1;
            elseif obj.adaption_technique == adaption_method.lms
                adaption_code = 2;
            elseif obj.adaption_technique == adaption_method.rls
                adaption_code = 3;
            else
                builtin("error","FFE_A2TrackedLevels:InvalidAdaptionTechnique", ...
                    "Unsupported FFE adaption technique.");
            end
        end
    end

    methods (Static, Access = private)
        function [buffer,buffer_pos_by_level,level_sum_by_level,buffer_valid_count_by_level, ...
                offset_by_level,valid_count_by_level] = updateTrackedLevelBuffer( ...
                buffer,buffer_pos_by_level,level_sum_by_level,buffer_valid_count_by_level, ...
                offset_by_level,valid_count_by_level,new_value,symbol_idx,symbol, ...
                update_blocklength,constellation)

            buffer_pos = buffer_pos_by_level(symbol_idx) + 1;
            if buffer_pos > size(buffer,2)
                buffer_pos = 1;
            end
            buffer_pos_by_level(symbol_idx) = buffer_pos;

            old_value = buffer(symbol_idx,buffer_pos);
            if isfinite(old_value)
                level_sum_by_level(symbol_idx) = level_sum_by_level(symbol_idx) - old_value;
                buffer_valid_count_by_level(symbol_idx) = buffer_valid_count_by_level(symbol_idx) - 1;
            end

            if isfinite(new_value)
                buffer(symbol_idx,buffer_pos) = new_value;
                level_sum_by_level(symbol_idx) = level_sum_by_level(symbol_idx) + new_value;
                buffer_valid_count_by_level(symbol_idx) = buffer_valid_count_by_level(symbol_idx) + 1;
            else
                buffer(symbol_idx,buffer_pos) = NaN;
            end

            if mod(symbol,update_blocklength) == 0
                valid_count_by_level = buffer_valid_count_by_level;
                has_valid = valid_count_by_level > 0;
                offset_by_level(:) = 0;
                offset_by_level(has_valid) = ...
                    level_sum_by_level(has_valid) ./ valid_count_by_level(has_valid) - ...
                    constellation(has_valid);
            end
        end
    end
end