%**************************************************************************
% Clear and close everything
%**************************************************************************
clc;
clear all;
close all;

%**************************************************************************
% Load input data
%**************************************************************************
[sig, f_s] = audioread('mcg_01.wav');

%**************************************************************************
% Detrending - Method 1: Subtraction of constant (mean)
%**************************************************************************
sig_detr_const = sig - mean(sig);

%**************************************************************************
% Detrending - Method 2: Highpass
%**************************************************************************

% Parameters **************************************************************
N_hp    = 2*f_s;

% Initializations *********************************************************
mean_start      = mean(sig);
sig_detr_hp     = zeros(size(sig));
est_mean_sig_hp = zeros(size(sig));
sig_mem         = zeros(N_hp,1) + mean_start;
ptr_sig         = 0;
mean_rec        = mean_start;
mean_iter       = 0;
k_iter          = 0;
N_hp_inv        = 1 / N_hp;

%**************************************************************************
% Main loop 
%**************************************************************************
for k = 1:length(sig)
    
    %**********************************************************************
    % Get new input signal
    %**********************************************************************
    sig_entering = sig(k);
    
    %**********************************************************************
    % Update ring buffer
    %**********************************************************************
    % Update of the pointer (modulo N_hp) *********************************
    ptr_sig = ptr_sig + 1;
    if (ptr_sig > N_hp)
        ptr_sig = 1;
    end
    
    % Store leaving signal in variable ************************************
    sig_leaving      = sig_mem(ptr_sig);
    
    % Add new input to signal memory **************************************
    sig_mem(ptr_sig) = sig_entering;
    
    %**********************************************************************
    % Update mean recursively
    %**********************************************************************
    mean_rec = mean_rec + (sig_entering - sig_leaving) * N_hp_inv;
    
    %**********************************************************************
    % Iteratively computed mean and correction of rec. mean
    %**********************************************************************
    k_iter    = k_iter + 1;
    mean_iter = mean_iter + sig_entering; 
    if (k_iter == N_hp)
        mean_rec  = mean_iter * N_hp_inv;
        mean_iter = 0;
        k_iter    = 0;
    end
    
    %**********************************************************************
    % Store estimated mean (for analysis purposes)
    %**********************************************************************
    est_mean_sig_hp(k) = mean_rec;
    
    %**********************************************************************
    % Output = input - mean
    %**********************************************************************
    sig_detr_hp(k) = sig_entering - mean_rec;    
end

%**************************************************************************
% Detrending - Method 3: New method (no name found yet)
%**************************************************************************

% Parameters **************************************************************
Cell_dur       = round(0.1 * f_s);           % Cell duration
N_mem_des      = round(1.0 * f_s);           % Total memory duration
N_cells        = round(N_mem_des/Cell_dur);  % Number of cells
mean_start     = mean(sig);
beta_sm        = 0.98; 

% Initializations *********************************************************
detr_counter              = 0;
Cell_dur_inv              = 1 / Cell_dur;   
mean_curr_cell            = 0; 
vec_cell_means            = zeros(N_cells,1)   + mean_start;
vec_cell_means_sorted     = zeros(N_cells,1)   + mean_start;
vec_cell_means_sorted_ext = zeros(N_cells+1,1) + mean_start;
pointer_vec_cell_means    = 0;
global_mean_est           = mean_start;   
sig_detr_new              = zeros(size(sig));
est_mean_sig_new          = zeros(size(sig));
global_mean_est_sm        = mean_start;

%**************************************************************************
% Main loop 
%**************************************************************************
for k = 1:length(sig)

    %**********************************************************************
    % Get new input signal
    %**********************************************************************
    sig_entering = sig(k);
    
    %**********************************************************************
    % Increment main counter
    %**********************************************************************
    detr_counter = detr_counter + 1;
    
    if (detr_counter > Cell_dur - 1)  
    
        % Reset counter ***************************************************
        detr_counter = 0;
        
        % Use current sample **********************************************
        mean_curr_cell = mean_curr_cell + sig(k);
        
        % Finalize estimation of mean of current cell *********************
        mean_curr_cell = mean_curr_cell * Cell_dur_inv;
        
        % Update vector containing cell means *****************************
        pointer_vec_cell_means = pointer_vec_cell_means + 1;
        
        if (pointer_vec_cell_means > N_cells)
            pointer_vec_cell_means = 1;
        end
        
        vec_cell_means_leaving = vec_cell_means(pointer_vec_cell_means);
        vec_cell_means(pointer_vec_cell_means) = mean_curr_cell;

        % Insert new mean into sorted list ********************************
        index_offset = 0;
        
        for k_sort = 1:N_cells
            
            if ( (mean_curr_cell > vec_cell_means_sorted(k_sort)) && ...
                 (index_offset == 0) )
                index_offset = 1;
                vec_cell_means_sorted_ext(k_sort)   = mean_curr_cell;
            end
            
            vec_cell_means_sorted_ext(k_sort+index_offset) = vec_cell_means_sorted(k_sort);                        
        end
        
        if (index_offset == 0)
            vec_cell_means_sorted_ext(N_cells+1) = mean_curr_cell;
        end
            
        % Remove leaving mean from sorted list ****************************
        index_offset = 0;
        
        for k_sort = 1:N_cells
            
            if ( (vec_cell_means_leaving == vec_cell_means_sorted_ext(k_sort)) && ...
                 (index_offset == 0) )
                index_offset = 1;
            end
            
            vec_cell_means_sorted(k_sort) = vec_cell_means_sorted_ext(k_sort+index_offset);                        
        end      

        % Update mean by taking from the middle of the sorted list ********
        global_mean_est = vec_cell_means_sorted(round(N_cells/2));
        
        % Reset current mean estimation ***********************************
        mean_curr_cell = 0;            
    else
        % Update estimation of mean of current cell ***********************
        mean_curr_cell = mean_curr_cell + sig_entering;
    end
    
    %**********************************************************************
    % Smoothing of the estimated mean
    %**********************************************************************
    global_mean_est_sm = beta_sm * global_mean_est_sm + ...
                         (1 - beta_sm) * global_mean_est;
    est_mean_sig_new(k) = global_mean_est_sm;
    
    %**********************************************************************
    % Detrend input signal
    %**********************************************************************
    sig_detr_new(k) = sig_entering - global_mean_est;
end

%**************************************************************************
% Analyses
%**************************************************************************
t_h = (0*f_s+1:30*f_s-1);
t   = (t_h-1)/f_s;
lw = 1.5;

% Time-domain analyses ***************************************************
fig = figure;
set(fig,'Units','Normalized');
set(fig,'Position',[0.1 0.1 0.8 0.8]);

sb_td_detr(1) = subplot('Position',[0.08 0.68 0.84 0.28]);
plot(t,sig_detr_const(t_h),'b','LineWidth',lw);
grid on;
set(gca,'XTickLabel','');
legend('Detrended signal (const subtraction)');

sb_td_detr(2) = subplot('Position',[0.08 0.38 0.84 0.28]);
plot(t,sig_detr_hp(t_h),'b','LineWidth',lw);
grid on;
set(gca,'XTickLabel','');
legend('Detrended signal (highpass)');

sb_td_detr(3) = subplot('Position',[0.08 0.08 0.84 0.28]);
plot(t,sig_detr_new(t_h),'b','LineWidth',lw);
grid on;
xlabel('Time in seconds');
legend('Detrended signal (new method)');

linkaxes(sb_td_detr,'xy');

% Time-domain analyses ***************************************************
fig = figure;
set(fig,'Units','Normalized');
set(fig,'Position',[0.1 0.1 0.8 0.8]);

sb_td_detr(1) = subplot('Position',[0.08 0.68 0.84 0.28]);
plot(t,sig(t_h),'b', ...
     t,sig(t_h)*0+mean(sig),'r','LineWidth',lw);
grid on;
set(gca,'XTickLabel','');
legend('Input signal','Estimated mean (const subtraction)');

sb_td_detr(2) = subplot('Position',[0.08 0.38 0.84 0.28]);
plot(t,sig(t_h),'b', ...
     t,est_mean_sig_hp(t_h),'r','LineWidth',lw);
grid on;
set(gca,'XTickLabel','');
legend('Input signal ','Estimated mean (highpass)');

sb_td_detr(3) = subplot('Position',[0.08 0.08 0.84 0.28]);
plot(t,sig(t_h),'b', ...
     t,est_mean_sig_new(t_h),'r','LineWidth',lw);
grid on;
xlabel('Time in seconds');
legend('Input signal ','Estimated mean (new method)');

linkaxes(sb_td_detr,'xy');
