% -------------------------------------------------------------------------------------------------
% QrsAvgCancelTestClP_SC.m: Test QRS cancellation
%                           Analysis of the cases with atrial fibrillation from
%                           the CinC 2004 challenge   
%
% Copyright (C) 2004 Maurizio Varanini, Clinical Physiology Institute, CNR, Pisa, Italy
%
% 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 "as is" and "as available" in the hope that it will be useful,
% but WITHOUT ANY WARRANTY of any kind; 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.
%
% For any comment or bug report, please send e-mail to: maurizio.varanini@ifc.cnr.it
% -------------------------------------------------------------------------------------------------

function QrsAvgCancelTest(mainPath_in, casiDir, cname, mainPath)
%nargin=0
echo off
% clc
close all
debug=0;
debugQRS=0;
graf=0
savedat=1;


mainPath_out=fullfile(mainPath, 'DatiQc');
if ~exist(mainPath_out,'dir') mkdir(mainPath, 'DatiQc'); end
filepath_out= fullfile(mainPath_out, casiDir);
if ~exist(filepath_out,'dir') mkdir(mainPath_out, casiDir); end
fpname_out= fullfile(filepath_out, [cname, '.dat']);


dataType='short';
ns=2;
anfilt=0;

% dataType='double';
% ns=4;
% anfilt=1;

extension='.dat';
freq=128;

[progpath, progname] = fileparts(which(mfilename));

fprintf('Nome del caso: %s\n', cname);

% fpname= strcat(filepath, cname, extension);
fpname_in = fullfile(mainPath_in, casiDir, [cname extension]);
fprintf('Lettura del file: %s\n', fpname_in);
fid  = fopen(fpname_in,'r')
[datap, count] = fread(fid,[ns,inf],dataType);
fclose(fid);

% case type: 1->N Normale,   2->T (sotto Terapia)
tipoC=cname(1);
%-------------------------------------------------------------
% cNameMod=['_Avg'];
% if debug cNameMod=[cNameMod 'P']; end
% dirOut=['Canc' cNameMod];
% fprintf('Directory per i risultati = %s\n', dirOut);
% mainPath_out=mainPath_in;
% filepath_out= fullfile(mainPath_out, dirOut);
% if ~exist(filepath_out,'dir') mkdir(mainPath_out, dirOut); end

% eliminate the last second because it is an artefact generated by the ANFILT program
if(anfilt) data=datap(:,1:end-freq); else data=datap; end
% registration duration in seconds
[ns ndt]=size(data);
lendts=ndt/freq;
fprintf('Frequenza di campionamento = %g\n', freq);
fprintf('Durata totale della registrazione in campioni= %g,  in secondi = %g\n', ndt, lendts);

if(graf)
    PlotSgnMrkN([data(1,:);data(2,:)], [], freq, cname);
end

x1=detrend(data(1,:)');
x2=detrend(data(2,:)');

fmind=0.2;
fmaxd=40;
Wn = [fmind fmaxd]/(freq/2);  % The cut-off frequency Wn must be between 0 < Wn < 1.0, with 1.0 corresponding to half the sample rate
delay=1000;
ncf=delay*2+1;
B = FIR1(ncf,Wn); % default -> Hamming
x1 = filter(B,1,[x1; zeros(delay,1)]);   x1= x1(delay+1:end);
x2 = filter(B,1,[x2; zeros(delay,1)]);   x2=x2(delay+1:end);

if(graf)
    PlotSgnMrkN([x1,x2], [], freq, [cname, '  filtered']);
end

% interpolation
fs=1024;
x1 = InterpFFT(x1, freq, fs);
x2 = InterpFFT(x2, freq, fs);
freq=fs
xtime=0:length(x1)-1; xtime=xtime/freq;
B250=[1; 1; 0; -1; -1];
B1000=[1;1;1;1;1;1;1-1; 0.5; 0;0;0; -0.5; -1;-1;-1;-1;-1; ];
B=B1000;
delay=floor(length(B)/2);
% evaluate the derivative and correct the delay
dx1=filter(B,1,[x1; zeros(delay,1)]);   dx1=dx1(delay+1:end);
dx2=filter(B,1,[x2; zeros(delay,1)]);   dx2=dx2(delay+1:end);
adx1=abs(dx1);
adx2=abs(dx2);
vadx=adx1+adx2;
if(graf & debugQRS)
    % plot derivative signals and spatial velocity
    figure;
    subplot(3,1,1), plot(xtime,x1,'b',xtime,dx1,'g');
    subplot(3,1,2), plot(xtime,x2,'b',xtime,dx2,'g');
    subplot(3,1,3), plot(xtime,vadx,'g');
end
wl=fix(freq*0.08);


w02=fix(0.2*freq);
mD02=meanMaxSc(vadx, w02, 1,1);

w2=fix(2*freq);
mD2=meanMaxSc(vadx, w2, 1,1);

%pth=0.45;     % 
pth=0.35;     % lowered pth because we lost QRS in case a29
%mvadx=madx(:,2);
mvadx=vadx;
qrsM=QRSdetector(mvadx,freq,pth);

nQRS=size(qrsM,2);
fprintf('Number of QRSs= %d\n', nQRS);

RRc= diff(qrsM(1,:));
RRs= RRc/freq;
RRmean=meansc(RRs,4,4);
%RRmean= mean(RRs);
RRstd= std(RRs);
fprintf('RR mean= %d,  stdev=%d\n', RRmean, RRstd);

if(graf & debugQRS)
    figure
    plot(RRs);
    figure
    plot(vadx,'b');
    DrawVertMarker(qrsM(1,:)','r',':','none');
    DrawVertMarker(qrsM(2,:)','b',':','none');
    DrawVertMarker(qrsM(3,:)','r',':','none');
end    
if(graf) PlotSgnMrkN([x1,x2], qrsM(1,:), freq, cname);  end

x1=(x1-mean(x1))/std(x1);
QRSw=meansc(qrsM(3,:)-qrsM(1,:),2,2);
m=QRSw+2;
npp=fix(0.1*freq); 
npd=fix(min(0.5, RRmean-0.1)*freq);
npt=1+npp+npd;
wigp=10;
wigd=10;
% use as a reference the 1st sup of the derivative
if(qrsM(1,1)-npp -wigp < 1) qi=2; else qi=1; end 
if(qrsM(1,nQRS)+npd +wigd > length(x1)) qf=nQRS-1; else qf=nQRS; end 
nq= qf-qi+1;

% built matrix of QRS templates
iw=qrsM(1,qi:qf)-npp;
fw=qrsM(1,qi:qf)+npd;
ifw=find(iw>0 & fw<size(x1,1));
iw=iw(ifw);
fw=fw(ifw);
nqe=size(iw,2);
for i=1:nqe;
    x1M(:,i)= x1(iw(i):fw(i));
    x2M(:,i)= x2(iw(i):fw(i));
end
if(graf)
    figure
    plot(x1M);
    title('ecg 1: all QRS template')
    figure
    plot(x2M);
    title('ecg 2: all QRS template')
end

beatGroup1= beatGroupingSC(x1, iw, npt);
vq{1}=beatGroup1.vq;
vij{1}=beatGroup1.vij;
vih{1}=beatGroup1.vih;
vqcc{1}=beatGroup1.vqcc;
avgC1Q=beatGroup1.avgC1Q;
nc=size(avgC1Q,2);

beatGroup2= beatGroupingSC(x2, iw, npt);
vq{2}=beatGroup2.vq;
vij{2}=beatGroup2.vij;
vih{2}=beatGroup2.vih;
vqcc{2}=beatGroup2.vqcc;
avgC2Q=beatGroup2.avgC1Q;
nc=size(avgC2Q,2);

% beatGroup= beatGrouping(x1, x2, iw, npt);
% vq=beatGroup.vq;
% vij=beatGroup.vij;
% vih=beatGroup.vih;
% vqcc=beatGroup.vqcc;
% avgC1Q=beatGroup.avgC1Q;
% avgC2Q=beatGroup.avgC2Q;
% nc=size(avgC1Q,2);

%fprintf('min e max shift of template= %f,  %f\n', min(iw-vij), max(iw-vij))

% Plot the QRS template of each class
if(graf)
    figure, hold on
    subplot(2,1,1), plot(avgC1Q);
    title('ecg 1: QRS template classes')
    subplot(2,1,2), plot(avgC2Q);
    title('ecg 2: QRS template classes')
    hold off
end
% For each class, plot all QRS belonging to it
if(graf)
    figure, hold on
    for ic=1:nc
        subplot(2,nc,ic), plot(x1M(:,vq{1}{ic}));
        title(['ecg 1, class=', num2str(ic), ': QRS '])
        subplot(2,nc,nc+ic), plot(x2M(:,vq{2}{ic}));
        title(['ecg 2, class=', num2str(ic), ': QRS '])
    end
    hold off
end
% building signal by the class centroid
sq1=zeros(length(x1),1);
sq2=sq1;
vp1=0; vp2=0;
jp1=1; jp2=1;
for iq=1:nqe
    ic1=vqcc{1}(iq);  
    ij1=vij{1}(iq);  
    fj1=ij1+npt-1;   
    ih1=vih{1}(iq);
    ic2=vqcc{2}(iq);  ij2=vij{2}(iq);  fj2=ij2+npt-1;   ih2=vih{2}(iq);
    va1=avgC1Q(1,ic1)+ih1;  dv1=va1-vp1;  
    va2=avgC2Q(1,ic2)+ih2;  dv2=va2-vp2;  
    if(ij1>jp1)
        pv1=dv1/(ij1-jp1);
        sq1(jp1+1:ij1-1)=vp1+pv1*(1:ij1-jp1-1);
    end
    if(ij2>jp2)
        pv2=dv2/(ij2-jp2);
        sq2(jp2+1:ij2-1)=vp2+pv2*(1:ij2-jp2-1);
    end

    sq1(ij1:fj1)= avgC1Q(:,ic1)+ih1;
    sq2(ij2:fj2)= avgC2Q(:,ic2)+ih2;
    vp1= avgC1Q(end,ic1)+ih1;  
    vp2= avgC2Q(end,ic2)+ih2;
    jp1=fj1; 
    jp2=fj2;
end
ij1=qrsM(1,end)-npp;
ij2=qrsM(2,end)-npp;
va1=x1(ij1);  dv1=va1-vp1;  pv1=dv1/(ij1-jp1);
sq1(jp1+1:ij1-1)=vp1+pv1:pv1:va1-pv1;
sq1(ij1:end)=x1(ij1:end);
va2=x2(ij2);  dv2=va2-vp2;  pv2=dv2/(ij2-jp2);
sq2(jp2+1:ij2-1)=vp2+pv2:pv2:va2-pv2;
sq2(ij2:end)=x2(ij2:end);

e1=x1-sq1;
e2=x2-sq2;

% % subtraction of the class centroid 
% e1=x1;
% e2=x2;
% for iq=1:nqe
%     ic=vqcc(iq);
%     ij=vij(iq);
%     fj=ij+npt-1;
%     e1(ij:fj)= e1(ij:fj)-avgC1Q(:,ic);
%     e2(ij:fj)= e2(ij:fj)-avgC2Q(:,ic);
% end
% 
% sq1=x1-e1;
% sq2=x2-e2;

if(graf)
    % canale 1
    PlotSgnMrkN([x1,sq1,e1], qrsM(1,:), freq, [cname '_c1: x,y,e']);
    % canale 2
    PlotSgnMrkN([x2,sq2,e2], qrsM(1,:), freq, [cname '_c2: x,y,e']);
end

w02=fix(0.2*freq);  % mean value of the maxs on windows with 0.2s width
mD02=meanMaxSc(e1, w02, 1,1);
w2=fix(2*freq);   % mean value of the maxs on windows with 2s width
mD2=meanMaxSc(e1, w2, 1,1);
rq1=mD2/mD02;

w02=fix(0.2*freq);  % mean value of the maxs on windows with 0.2s width
mD02=meanMaxSc(e2, w02, 1,1);
w2=fix(2*freq);   % mean value of the maxs on windows with 2s width
mD2=meanMaxSc(e2, w2, 1,1);
rq2=mD2/mD02;
ic= 1+ (rq2<rq1);
fprintf('rq1=%f; rq2=%f; ic=%d\n', rq1, rq2, ic);

% [miy, may] = mimaxsc(e1,5,5);
[miy, may] = mimaxsc(e1,5,5);
e1n=e1/(may-miy);
[miy, may] = mimaxsc(e2,5,5);
e2n=e2/(may-miy);
if(graf)
    PlotSgnMrk(e1n, qrsM(1,:), freq, [cname, ':  e1n']);
    PlotSgnMrk(e2n, qrsM(1,:), freq, [cname, ':  e2n']);
end
% clipping
mima=[-1, 1];
e1n=boundV(e1n,mima);
e2n=boundV(e2n,mima);

crr=e1n'*e2n;
fprintf('crr=%f\n', crr);
if(graf)
    PlotSgnMrk([e1n,e2n], qrsM(1,:), freq, [cname, ':  e1n, e2n']);
end

% clipping
% mima=[-0.3, 0.3];
mima=[-0.5, 0.5];
e1nb=boundV(e1n,mima);
e2nb=boundV(e2n,mima);
crr=e1nb'*e2nb;
fprintf('crr=%f\n', crr);
fmind=3;
fmaxd=8;
Wn = [fmind fmaxd]/(freq/2);  % The cut-off frequency Wn must be between 0 < Wn < 1.0, with 1.0 
                              % corresponding to half the sample rate
B = FIR1(401,Wn); % default -> Hamming
e1f = filter(B,1,[e1nb; zeros(delay,1)]);    e1f=e1nb(delay+1:end);
e2f = filter(B,1,[e2nb; zeros(delay,1)]);    e2f=e2nb(delay+1:end);
if(graf)
    PlotSgnMrk([e1f,e2f], qrsM(1,:), freq, cname);
end
pn1=e1n'*e1n;
pn2=e2n'*e2n;

cr=e1f'*e2f;
pf1=e1f'*e1f;
pf2=e2f'*e2f;
crr=cr/sqrt(pf1*pf2);
fprintf('cr=%f, pf1=%f, pf2=%f, crr=%f\n', cr, pf1, pf2, crr);
rp1fn=pf1/pn1;
rp2fn=pf2/pn2;
fprintf('rp1fn=%f, rp2fn=%f\n', rp1fn, rp2fn);
%
% sgncr=sign(crr); % tried to use the signal e1n+sgncr*e2n with bad results! If we use datiF!!
% % better results if we always use e1n+e2n
% e1s = e1n-sgncr*e2n;
% e2s = e1n+sgncr*e2n;
yd=e1nb-e2nb;
ys=e1nb+e2nb;
if(graf)
    PlotSgnMrk(yd, qrsM(1,:), freq, [cname, ':  e1nb-e2nb']);
    PlotSgnMrk(ys, qrsM(1,:), freq, [cname, ':  e1nb+e2nb']);
end

if(abs(crr) < 0.1) 
    if(rp2fn > rp1fn) e1s=e1nb; e2s=e2nb; fprintf('Canale 2\n'); else e1s=e2nb; e2s=e1nb; fprintf('Canale 1\n');end
else
    if(crr > 0) e1s= yd; e2s=ys; fprintf('Somma\n'); else e1s= ys; e2s=yd;  fprintf('Differenza\n');end
end

%e1s=e1nb; 
%e2s=e2nb;
Somma = e1nb + e2nb;
Differenza = e1nb - e2nb;


if savedat
    sgn_out=[x1, x2, e1s, e2s, e1nb, e2nb, Somma, Differenza];
%    mainPath_out
    % write the right results on disk
    fprintf('Scrittura del file: %s\n', fpname_out);
    fid  = fopen(fpname_out,'wb');
    count = fwrite(fid, sgn_out', 'double');
    fclose(fid);
    
    fpnamePar_out= fullfile(filepath_out, [cname,'.mat']);
    fprintf('Scrittura parametri nel file: %s\n', fpnamePar_out);
    save(fpnamePar_out, 'RRs', 'RRmean', 'RRstd', 'nc', 'crr', 'rp1fn', 'rp2fn');
end
%QRSclass_out= [mainPath_out,'\',cname,'_QRSclassA.mat'];
%save (QRSclass_out,'beatGroup1','beatGroup2');