% This function is the reference implementation of the SCCM
% - Step Classification and Characterization Method.
% The SCCM uses two dedicated step detectors/estimators: the SLHEM and 
% the SLHEP. The SLHEM (PATV-based Step Location and Height Estimator
% - Magnitude) is devoted to detect and estimate the location and 
% height (depth) of a magnitude step in an AC signal.
% The SLHEP (PATV-based Step Location and Height Estimator - Phase) is
% devoted to detect and estimate the location and height (depth) of a phase 
% step in an AC signal. The outputs of the SLHEM and SLHEP are then used to
% classify the type of step among 3 types: 0 (no step present); 
% 1 (magnitude step present); 3 (phase step present). The SCCM outputs
% the step type and, depending on the that, the related step
% characterization, in terms of step location and depth. The SCCM
% also outputs (as a bonus) estimates of the phasor magnitude, frequency
% and initial phase (the later is not realiable).

% The function also implements the SLHEM and SLHEP, which require the 
% patv_MM.m function as distributed by Selesnik.
% https://eeweb.engineering.nyu.edu/iselesni/patv/

% Tested for Matlab 2012b and R2023a.

% Script written by Paulo Esquef: pesquef@lncc.br
% Last modified: May 3rd, 2024

function [tau_est,type_step,hm_est,hp_est,Xm_est,phi_0_est,f1_est]=sccm_ref(xn,dm,dp,lambda_m,lambda_p,lambda_v,Nit,Lm,Lp)

% INPUTS
% xn: Noisy AC signal with a step
% dm: polynomial order PATV for the SLHEM
% dp: polynomial order PATV for the SLHEP
% lambda_m: lambda of PATV for the SLHEM
% lambda_p: lambda of PATV for the SLHEP - step location estimation
% lambda_v: lambda of PATV for the SLHEP - step height estimation
% Nit: number of iterations of PATV (same for SLHEM and SLHEP)
% Lm: detection threshold  for the SLHEM
% Lp: detection threshold  for the SLHEP

% OUTPUTS

% tau_est: step location estimate
% type_step: index of the step type
% hm_est: estimated depth of the magnitude step  
% hp_est: estimated depth of the phase step 
% Xm_est: estimate of the underlying phasor magnitude
% phi_0_est: estimate of the underlying initial phase of the phasor (not realiable)
% f1_est: estimate of the underlying frequency of the phasor

% first, we run the SLHEM and the SLHEP
[tau_est_m,gamma_m,hm_est,Xm_est]=slhem(xn,dm,lambda_m,Nit,Lm);
[tau_est_p,gamma_p,hp_est,phi_0_est,f1_est]=slhep(xn,dp,lambda_p,lambda_v,Nit,Lp);

% If a magnitude step is not detected by SLHEM, it returns tau_est_m=NaN
% and hm_est=0. 
% If a phase step is not detected by SLHEP, it returns tau_est_p=NaN
% and hp_est=0. 
% gamma_m is the maximum value of the detection function (SLHEM)
% gamma_p is the maximum value of the detection function (SLHEP)

% From the outputs of the SLHEM and the SLHEP we run the step type classifier

% Runs step type classifier
[type_step]=step_classifier(tau_est_m,gamma_m,Lm,tau_est_p,gamma_p,Lp);

% Runs step characterization method after classifying the step type
[tau_est,hm_est,hp_est]=step_characterization(type_step,tau_est_m,hm_est,tau_est_p,hp_est);


% Implements the SLHEM - PATV-based Step Location Estimator based on instantaneous
% Magnitude. The SLHEM extends the SLEM_PATV to estimate the
% step height besides its location

function [tau_est,gamma_m,hm_est,Xm_est]=slhem(xn,dm,lambda_m,Nit,Lm)

% xn: Noisy AC signal with a step
% dm: polynomial order PATV
% lambda_m: lambda of PATV
% Nit: number of iterations of PATV
% Lm: detection threshold

% Outputs
% tau_est: step location estimate
% gamma_m: peak of the detection function Dm
% hm_est: step height estimate
% Xm_est: an estimate of the phasor magnitude

a_m=abs(hilbert(xn)); % obtains the instantaneous magnitude via Hilbert transform
[c_m, s] = patv_MM(a_m, dm, lambda_m, Nit);  % runs PATV filtering of a_m   

% s is the polynomial component
% c is the piecewise constant function (may represent the step function)

Xm_est=s(1);  % estimate of the magnitude of the underlying phasor.
% note that s[n] is constant and c_m(1)=0, by construction.

a_mpatv=c_m+s; a_mpatv=a_mpatv(:); % PATV-based step component with offset.
% a_mpatv is a clean downsized approximation of the magnitude step
% component

%
Dm=abs(gradient(a_mpatv)); % detection function       
[gamma_m,nhat]=max(Dm);    % local maximum of Dm    
if gamma_m > Lm,   % compares gamma_m against threshold Lm
    tau_est=nhat;   % tau_est takes the instant of gamma_m if gamma_m>Lm
else
    tau_est=NaN;  % tau_est gets an invalid identifier
end

if isnan(tau_est)  % no magnitude step detected if tau_est=NaN
    hm_est=0;  % sets step height to zero
else  % a magnitude step has been detected. Estimates its height
    % Extension to estimate the step height
    % First, run the procedure to correct the scale of a_mpatv
    mam=mean(a_m); % mean value of a_m
    % Signal attributions as in the paper
    w=a_m-mam;  % zero-mean a_m; 
    c=c_m-mean(c_m);
    g=(c'*w)/(c'*c);  % finds scale gain g
    r=g*c+mam;     % r is a_mpatv at the correct scale

    if 0  % enable to plot a figure that compares a_m, am_patv and r
    plot(a_m); hold on; plot(a_mpatv,'k','linewidth',3);
    plot(r,'r','linewidth',3); 
    set(gca,'fontsize',14)
    xlabel('$n$ (samples)','fontsize',17,'interpreter','latex')
    ylabel('Amplitude')
    fff=legend('$\hat{a}_m[n]$','$\hat{a}_{m,\textrm{patv}}[n]$',...
        '$r[n]=gc[n]+\mu_a$');
    set(fff,'interpreter','latex')
    axis tight
    pause; close all
    end
    
    % Step height estimation from r[n]
    
    qq=round(0.05*length(xn));  % number of samples to be discarded around the 
    % located step tau_est
    Lr=mean(r(tau_est+qq:end)); % height after estimated step location
    Ll=mean(r(1:tau_est-qq));% height before estimated step location
    hm_est=Lr-Ll;  % magnitude step height estimate
end 


%%%%%%%%%%%%%%%%%%%

% Implements the SLHEP

function [tau_est,gamma_p,hp_est,phi_0_est,f1_est]=slhep(xn,dp,lambda_p,lambda_v,Nit,Lp)

% xn: Noisy AC signal with a step
% dp: polynomial order PATV
% lambda_p: lambda of PATV to compute c_p[n]
% lambda_v: lambda of second PATV to estimate the linear trend in Psi[n]
% Nit: number of iterations of PATV
% Lp: detection threshold

% tau_est: estimate of tau_n (phase step location)
% gamma_p: peak of the detection function
% hp_est: estimate of the height of the phase step
% phi_0_est: estimate of the initial phase of the phasor (not reliable) 
% f1_est: estimate of the frequency of the underlying phasor (in rad/sample)

z=hilbert(xn);  % obtains Hilbert's analytic signal
Psi = phase(z);  % estimates the instantaneous phase (noisy)

% Runs PATV decomposition of Psi (instantaneous phase)
[c_p, s] = patv_MM(Psi, dp, lambda_p, Nit);  % PATV decomposition: c_p is a 
% downsized clean approximation of the phase step
% c_p starts at zero by property

Dp=abs(gradient(c_p)); Dp(1)=[]; % detection function

[gamma_p,nhat]=max(Dp);  % gamma_p is the max value or local maximum of Dp
% nhat is the corresponding index (first maximum)
        
if gamma_p > Lp,
    tau_est=nhat;    
else
    tau_est=NaN;
end

[aux, s] = patv_MM(Psi, dp, lambda_v, 5);  % second PATV filtering of Psi:

if isnan(tau_est)  % no phase step detected
    hp_est=0; % sets phase step height to zero
    
    % estimates of the underlying phasor parameters
    phi_0_est=s(1); % estimate of the initial phase in rad
    f1_est=s(2)-s(1); % estimate of the phasor frequency in rad/sample

    
else  % a phase step has been detected
    % Extension of the SLEP_PATV to estimate the height of the phase step
    qq=round(0.05*length(Psi)); % number of samples to be discarded 
    % in the beginning and end of the window and around the 
    % located phase step. 
    %[aux, s] = patv_MM(Psi, dp, lambda_v, 5);  % second PATV filtering of Psi:
    %with way lower value of lambda
    % s will be used to estimate the initial phase and the frequency of the
    % phasor
    phi_0_est=s(1); % estimate of the initial phase in rad
    f1_est=s(2)-s(1); % estimate of the phasor frequency in rad/sample
    
        
    % s will be used to remove more accurately the linear trend from Psi
    % using the notation defined in the paper
    v=Psi-s+s(1); % this is noisy phase step component 
    mv=mean(v);
    % signal atributions as in the paper: scale correction
    w=v-mv; % zero-mean noisy phase step
    c=c_p-mean(c_p);   % downsized clean phase step
    rg=qq:length(Psi)-qq;

    g=(c(rg)'*w(rg))/(c(rg)'*c(rg));  % here, for calculating g
    % we trim the sequences

    r=g*c+mv;  % r is the clean approximation of the 
    %phase step at the correct scale

    if 0   % enable or disable to plot the instantaneous functions
    plot(v,'linewidth',3); hold on; plot(r,'r','linewidth',3); 
    set(gca,'fontsize',14)
    ylabel('Phase (rad)');
    xlabel('$n$ (samples)','interpreter','latex');
    hhh=legend('$v[n]$ (noisy phase step)','$r[n]$ (clean phase step)');
    set(hhh,'interpreter','latex');
    end
    
    % Estimation of the height of the phase step from r[n]
    Lr=mean(r(tau_est+qq:end)); % level after estimated step location
    Ll=mean(r(1:tau_est-qq));% level before estimated step location
    hp_est=Lr-Ll;   % phase step height estimate
end

%%%%%%%%%%%%%%%%%%%


function [type_step]=step_classifier(tau_est_m,gamma_m,Lm,tau_est_p,gamma_p,Lp)

% Classifies the type of step based on information output by the SLHEM and
% SLHEP
% INPUTs:
% tau_est_m: step location estimate given by the SHLEM
% gamma_m: maximum of the detection function in the SLHEM
% Lm: detection threshold in the SLHEM
% tau_est_p: step location estimate given by the SHLEP
% gamma_p: maximum of the detection function in the SLHEP
% Lp: detection threshold in the SLHEP

% OUTPUT
% type_step: step type. 
% type_step=0 => step absent
% type_step=1 => magnitude step present
% type_step=2 => phase step present

if isnan(tau_est_m)&&isnan(tau_est_p), % no step detected, 
    %either in magnitude or phase
    type_step=0;  % no step present in the AC signal
% Criterion for deciding whether a step happened in magnitude or in phase
elseif gamma_p/Lp >  gamma_m/Lm, % a phase step is more likely to have happened
    type_step=2;  % phase step present in the AC signal
else % a magnitude step is more likely to have happened
    type_step=1;  % magnitude step present in the AC signal
end

function [tau_est,hm_est,hp_est]=step_characterization(type_step,tau_est_m,hm_est,tau_est_p,hp_est) 

% Characterizes the classified step type (1 or 2) providing estimates for 
% the step location and height 

% INPUTs
% type_step: classified step type provided by function step_classifier
% tau_est_m: step location estimate given by the SHLEM
% hm_est: step height provided by the SLHEM
% tau_est_p: step location estimate given by the SHLEP
% hp_est: step height provided by the SLHEP

% OUTPUTs
% tau_est: final estimate of the step location (either in magnitude or phase)
% hm_est: estimate of the step height in the magnitude component
% hp_est: estimate of the step height in the phase component


if type_step==0, % no step detected
    tau_est=NaN;  % invalid step location
    hm_est=0;   % depth of the magnitude step is null
    hp_est=0;  % depth of the phase step is null
elseif type_step==1, % step classified as magnitude step
    tau_est=tau_est_m;  % step location as estimated by the SLHEM
    hm_est=hm_est;   % depth of the magnitude step that estimated by the SLHEM
    hp_est=0;  % depth of the phase step is null
elseif type_step==2, % step classified as phase step
    tau_est=tau_est_p;  % step location as estimated by the SLHEP
    hm_est=0;   % depth of the magnitude step is null
    hp_est=hp_est;   % depth of the phase step that estimated by the SLHEP
end
    

