function [beat_label,num_beat_label]=get_beat_ecg_mode2(signal_ecg,signal_abp,samp_rate,ecg_abp_shift_time)
ecg_abp_shift=round(samp_rate*ecg_abp_shift_time);
align_margin=round(samp_rate*0.3);
thres_align=30;
abp_mode_flag=1;
show_flag=0;

[B_ecg,A_ecg] = butter(12,40/250);

signal_ecg=filtfilt(B_ecg,A_ecg,signal_ecg);
siglen=length(signal_ecg);
ecg_peak_array=get_peak_array5(signal_ecg,siglen);
ecg_peak_span_array=mark_peak_array_span(ecg_peak_array,size(ecg_peak_array,1),2*samp_rate);
ecg_stg_peak_index=get_strong_peak(ecg_peak_array,length(ecg_peak_array),ecg_peak_span_array,5,0.5);
ecg_stg_peak=ecg_peak_array(ecg_stg_peak_index,1);

if abp_mode_flag
    signal_abp=filtfilt(B_ecg,A_ecg,signal_abp);
    grad_abp=get_gradient(signal_abp,siglen);
    grad_abp=filtfilt(B_ecg,A_ecg,grad_abp);
    abp_peak_array=get_peak_array5(grad_abp,siglen);
    abp_peak_span_array=mark_peak_array_span(abp_peak_array,size(abp_peak_array,1),2*samp_rate);
    abp_stg_peak_index=get_strong_peak(abp_peak_array,length(abp_peak_array),abp_peak_span_array,5,0.5);
    abp_stg_peak=abp_peak_array(abp_stg_peak_index,1);
end;

if show_flag
    ecg_stg_peak_mark=zeros(siglen,1);
    ecg_stg_peak_mark(ecg_stg_peak)=ecg_peak_array(ecg_stg_peak_index,5);
    if abp_mode_flag
        abp_stg_peak_mark=zeros(siglen,1);
        abp_stg_peak_mark(abp_stg_peak)=abp_peak_array(abp_stg_peak_index,5);
    end;
end;


%% ECG ABP alignment
if abp_mode_flag
    %(dist)(shift)(map_index)(select_flag)
    [e_a_map,a_e_map,shift_hist_e_a_1]=align_peak(ecg_stg_peak,abp_stg_peak,ecg_abp_shift,align_margin);
    [hv,hi]=max(shift_hist_e_a_1);
    shift_hist_adjust_e_a_1=(hi-align_margin-1)*-1;
    ecg_abp_shift=ecg_abp_shift+shift_hist_adjust_e_a_1;
    
    [e_a_map2,a_e_map2,shift_hist_e_a_2]=align_peak(ecg_stg_peak,abp_stg_peak,ecg_abp_shift,align_margin);
    [hv,hi]=max(shift_hist_e_a_2);
    shift_hist_adjust_e_a_2=(hi-align_margin-1)*-1;
    ecg_abp_shift=ecg_abp_shift+shift_hist_adjust_e_a_2;
    
    if show_flag
        index=find(e_a_map2(:,1)<10);
        ecg_stg_peak_mark(ecg_stg_peak(index))=signal_ecg(ecg_stg_peak(index));
        index=find(a_e_map2(:,1)<10);
        abp_stg_peak_mark(abp_stg_peak(index,1))=grad_abp(abp_stg_peak(index));
    end;
    
    
    index=(e_a_map2(:,1)>=thres_align);
    e_a_map2(index,:)=nan;
    index=(a_e_map2(:,1)>=thres_align);
    a_e_map2(index,:)=nan;
    
    siglen_new_a=siglen-ecg_abp_shift;
    signal_ecg_align_a=zeros(siglen_new_a,1);
    signal_abp_align_a=zeros(siglen_new_a,1);
    signal_ecg_align_a(1:siglen_new_a)=signal_ecg(1:siglen-ecg_abp_shift);
    signal_abp_align_a(1:siglen_new_a)=grad_abp(ecg_abp_shift+1:siglen);
end;


%% CEPS PROCESSING

windurmin=0.2;
lagmin=windurmin/2;

%step size and segment size
stepdur_seg=0.25;
stepdur_seg=floor(samp_rate*stepdur_seg)/samp_rate;%make it integer
stepsize=round(stepdur_seg*samp_rate);
windurmax_seg=4;%segment of 4 seconds

lagmax_seg=windurmax_seg/2;
show_lagsize=round(lagmax_seg*samp_rate);%considered lag range
cw_seg=MSCM_Get_CropWin(250,64,lagmin,lagmax_seg);

if abp_mode_flag
    %ecg-abp
    cepsgram_ecg_a = challenge_get_ceps(signal_ecg_align_a, samp_rate, windurmax_seg, stepdur_seg, cw_seg);
    cepsgram_abp_a = challenge_get_ceps(signal_abp_align_a, samp_rate, windurmax_seg, stepdur_seg, cw_seg);
    
    nstep_a=size(cepsgram_ecg_a,1);
    thres_lagdist=10;%use log thres
    [cepspeak_ecg_a,cepspeak_abp_a]=get_ceps_lag_diff_by_mode_direct(cepsgram_ecg_a,cepsgram_abp_a,nstep_a,show_lagsize,thres_lagdist);
end;

%% BEAT MAP vs CEPS
% e_p_map2 vs cepspeak_ecg_p
thres_li=0.05;

if abp_mode_flag
    [beat_label_e_a,beat_label_a_e]=label_beat_dups(e_a_map2,ecg_stg_peak,cepspeak_ecg_a,a_e_map2,abp_stg_peak,cepspeak_abp_a,ecg_abp_shift,lagmax_seg,samp_rate,stepsize,nstep_a,thres_li);
    [ecg_norm_a,np3]=get_peak_norm(beat_label_e_a,ecg_peak_array,ecg_stg_peak_index);
    [abp_norm_a,np4]=get_peak_norm(beat_label_a_e,abp_peak_array,abp_stg_peak_index);
    [beat_chunk_e_a,nchunk_e_a]=get_chunks(beat_label_e_a);
    npeak_abp=size(abp_peak_array,1);
end;

npeak_ecg=size(ecg_peak_array,1);
% nstg_peak_ecg=size(ecg_stg_peak_index,1);

%% ABP chunks and gaps
if abp_mode_flag
    nbeat_final_abp=0;
    
    chunk_peak_e_a(nchunk_e_a).nvp=0;
    chunk_peak_e_a(nchunk_e_a).vp=[];
    for i=1:nchunk_e_a
        nvp_loose=beat_chunk_e_a(i,2)-beat_chunk_e_a(i,1)+1;
        vp_loose=ecg_stg_peak_index(beat_chunk_e_a(i,1):beat_chunk_e_a(i,2))';
        beat_label=sum(beat_label_e_a(beat_chunk_e_a(i,1):beat_chunk_e_a(i,2),:),2);
        skip_index=(beat_label==0);
        vp_loose(skip_index)=[];
        chunk_peak_e_a(i).nvp=nvp_loose-length(find(skip_index));
        chunk_peak_e_a(i).vp=vp_loose;
        nbeat_final_abp=nbeat_final_abp+chunk_peak_e_a(i).nvp;
    end;
    
    ngap_e_a=nchunk_e_a+1;
    gap_peak_e_a(ngap_e_a).mode=0;
    gap_peak_e_a(ngap_e_a).nvp=0;
    gap_peak_e_a(ngap_e_a).vp=[];
    
    for i=1:ngap_e_a
        if i==1
            gap_flag=1;
            beat_s_e_a=1;
            beat_e_e_a=ecg_stg_peak_index(beat_chunk_e_a(i,1));
            beat_s_a_e=1;
            beat_e_a_e=abp_stg_peak_index(e_a_map2(beat_chunk_e_a(i,1),3));
        elseif i==ngap_e_a
            gap_flag=-1;
            beat_s_e_a=ecg_stg_peak_index(beat_chunk_e_a(i-1,2));
            beat_e_e_a=npeak_ecg;
            beat_s_a_e=abp_stg_peak_index(e_a_map2(beat_chunk_e_a(i-1,2),3));
            beat_e_a_e=npeak_abp;
        else
            gap_flag=0;
            beat_s_e_a=ecg_stg_peak_index(beat_chunk_e_a(i-1,2));
            beat_e_e_a=ecg_stg_peak_index(beat_chunk_e_a(i,1));
            beat_s_a_e=abp_stg_peak_index(e_a_map2(beat_chunk_e_a(i-1,2),3));
            beat_e_a_e=abp_stg_peak_index(e_a_map2(beat_chunk_e_a(i,1),3));
        end;
        
        [ecg_peak_mono_a,np_e_a]=get_mono_peak(ecg_peak_array,beat_s_e_a,beat_e_e_a,ecg_norm_a,1);
        [abp_peak_mono_a,np_a_e]=get_mono_peak(abp_peak_array,beat_s_a_e,beat_e_a_e,abp_norm_a,2);
        [valid_ecg_peak_mono_a,nvp_e_a]=validate_mono_peak(ecg_peak_mono_a,np_e_a,ecg_peak_array,cepspeak_ecg_a,0,lagmax_seg,samp_rate,stepsize,nstep_a,thres_li,gap_flag,1);
        [valid_abp_peak_mono_a,nvp_a_e]=validate_mono_peak(abp_peak_mono_a,np_a_e,abp_peak_array,cepspeak_abp_a,ecg_abp_shift,lagmax_seg,samp_rate,stepsize,nstep_a,thres_li,gap_flag,2);
        
        if nvp_e_a>nvp_a_e
            gap_peak_e_a(i).mode=1;
            gap_peak_e_a(i).nvp=nvp_e_a;
            gap_peak_e_a(i).vp=valid_ecg_peak_mono_a;
        elseif nvp_e_a<nvp_a_e
            gap_peak_e_a(i).mode=2;
            gap_peak_e_a(i).nvp=nvp_a_e;
            gap_peak_e_a(i).vp=valid_abp_peak_mono_a;
        else
            if sum(ecg_peak_mono_a(:,2))<=sum(abp_peak_mono_a(:,2))
                gap_peak_e_a(i).mode=1;
                gap_peak_e_a(i).nvp=nvp_e_a;
                gap_peak_e_a(i).vp=valid_ecg_peak_mono_a;
            else
                gap_peak_e_a(i).mode=2;
                gap_peak_e_a(i).nvp=nvp_a_e;
                gap_peak_e_a(i).vp=valid_abp_peak_mono_a;
            end;
        end;
        
        nbeat_final_abp=nbeat_final_abp+gap_peak_e_a(i).nvp;
        
        ok=1;
        %     ecg_peak_array
        %     abp_peak_array
        %     ecg_abp_shift
    end;
end;
ok=1;

%% FINAL BEAT ARRAY
%ABP Final beats
if abp_mode_flag
    beat_final_abp=zeros(nbeat_final_abp,1);
    bc=0;
    for i=1:nchunk_e_a
        nb=gap_peak_e_a(i).nvp;
        if gap_peak_e_a(i).mode==1
            beat_final_abp(bc+1:bc+nb)=ecg_peak_array(gap_peak_e_a(i).vp(1:nb),1);
        else
            beat_final_abp(bc+1:bc+nb)=abp_peak_array(gap_peak_e_a(i).vp(1:nb),1)-ecg_abp_shift;
        end;
        bc=bc+nb;
        nb=chunk_peak_e_a(i).nvp;
        beat_final_abp(bc+1:bc+nb)=ecg_peak_array(chunk_peak_e_a(i).vp(1:nb),1);
        bc=bc+nb;
    end;
    i=ngap_e_a;
    nb=gap_peak_e_a(i).nvp;
    if gap_peak_e_a(i).mode==1
        beat_final_abp(bc+1:bc+nb)=ecg_peak_array(gap_peak_e_a(i).vp(1:nb),1);
    else
        beat_final_abp(bc+1:bc+nb)=abp_peak_array(gap_peak_e_a(i).vp(1:nb),1)-ecg_abp_shift;
    end;
end;

beat_label=beat_final_abp;
num_beat_label=nbeat_final_abp;

if show_flag
figure
subplot(211);
subplot(212);
bar(beat_final_abp(2:end)-beat_final_abp(1:end-1));


figure
subplot(211);
bar(beat_label_e_a(:,1));hold on;
bar(beat_label_e_a(:,2)*-1);hold off;

subplot(212);
bar(beat_label_a_e(:,1));hold on;
bar(beat_label_a_e(:,2)*-1);hold off;

ok=1;
% figure
% subplot(311);
% plot(signal_ecg);hold on;
% bar(ecg_stg_peak_mark,'r'); hold off;
% subplot(312);
% plot(grad_ppg);hold on;
% bar(ppg_stg_peak_mark,'r'); hold off;
% subplot(313);
% bar(ecg_stg_peak_mark); hold on;
% bar(ppg_stg_peak_mark*10,'r'); hold off;


figure
subplot(211);
bar(shift_hist_e_a_1);
subplot(212);
bar(shift_hist_e_a_2);


figure
subplot(211);
plot(signal_ecg(1:end-ecg_abp_shift));hold on;
plot(grad_abp(ecg_abp_shift+1:end)*10,'r');hold off;
subplot(212);
stem(ecg_stg_peak_mark(1:end-ecg_abp_shift)); hold on;
stem(abp_stg_peak_mark(ecg_abp_shift+1:end)*10,'r'); hold off;


figure;%ecg, grad of abp/ppg
subplot(211);
plot((signal_abp-mean(signal_abp))/100,'r'); title('r: abp, g: ppg, b: ecg');
hold on;
% plot((signal_ppg-mean(signal_ppg)),'g'); 
plot((signal_ecg-mean(signal_ecg)),'b'); 
hold off;
subplot(212);
plot((grad_abp-mean(grad_abp))/100,'r'); title('r: abp grad, g: ppg grad, b: ecg');
hold on;
% plot((grad_ppg-mean(grad_ppg)),'g');
plot((signal_ecg-mean(signal_ecg))/10,'b'); 
hold off;

end;

end

function [valid_peak_mono,nvp]=validate_mono_peak(peak_mono,np,peak_array,cepspeak,shift,lagmax_seg,samp_rate,stepsize,nstep,thres_li,gap_flag,mode_flag)
valid_peak_mono=[];
nvp=0;
beat_label=zeros(np,1);

if np<2; return; end;
for i=2:np
    beat1=peak_array(peak_mono(i-1,1),1);
    beat2=peak_array(peak_mono(i,1),1);
    beat_center=round((beat1+beat2)/2);
    ceps_step=round((beat_center-shift-lagmax_seg*samp_rate)/stepsize)+1;
    if ceps_step<1; ceps_step=1; end;
    if ceps_step>nstep; ceps_step=nstep; end;
    bbi=beat2-beat1;
%     lag=cepspeak(ceps_step,4);
    lag=cepspeak(ceps_step,1);
    li=abs(log(lag/bbi));
    if (ceps_step==1) || (ceps_step==nstep)
        li=li/2;
        if (cepspeak(ceps_step,2)<0.015) && mode_flag==1
            li=0;
        end;
    end
    if (li<thres_li)
        beat_label(i-1)=1;
        beat_label(i)=1;
    end;
end
if gap_flag==0
    beat_label(1)=0;
    beat_label(np)=0;
elseif gap_flag==1
    beat_label(np)=0;
else
    beat_label(1)=0;
end;
index=find(beat_label);
nvp=length(index);
valid_peak_mono(1:nvp)=peak_mono(index,1);
end

function [ecg_peak_mono_p,np]=get_mono_peak(ecg_peak_array,beat_s_e_p,beat_e_e_p,ecg_norm_p,mode_flag)
if mode_flag==1%wider range for ecg
    nsd3a=3;
    nsd4a=3;
else
    nsd3a=1.5;
    nsd4a=1.5;
end;

np0=beat_e_e_p-beat_s_e_p+1;
ecg_peak_mono_p=zeros(np0,2);
np=0;
for i=beat_s_e_p:beat_e_e_p
    count=0;
    if ecg_peak_array(i,2)<(ecg_norm_p(1,1)-ecg_norm_p(1,2)*2)
        count=count+1;
        if count>=2; continue; end;
    end;
    if ecg_peak_array(i,2)>(ecg_norm_p(1,1)+ecg_norm_p(1,2)*2)
        count=count+1;
        if count>=2; continue; end;
    end;
    if ecg_peak_array(i,3)<(ecg_norm_p(2,1)-ecg_norm_p(2,2)*2)
        count=count+1;
        if count>=2; continue; end;
    end;
    if ecg_peak_array(i,3)>(ecg_norm_p(2,1)+ecg_norm_p(2,2)*2)
        count=count+1;
        if count>=2; continue; end;
    end;
    if ecg_peak_array(i,4)<(ecg_norm_p(3,1)-ecg_norm_p(3,2)*nsd3a)
        count=count+1;
        if count>=2; continue; end;
    end;
    if ecg_peak_array(i,4)>(ecg_norm_p(3,1)+ecg_norm_p(3,2)*4)
        count=count+1;
        if count>=2; continue; end;
    end;
    if ecg_peak_array(i,5)<(ecg_norm_p(4,1)-ecg_norm_p(4,2)*nsd4a)
        count=count+1;
        if count>=2; continue; end;
    end;
    if ecg_peak_array(i,5)>(ecg_norm_p(4,1)+ecg_norm_p(4,2)*4)
        count=count+1;
        if count>=2; continue; end;
    end;
    np=np+1;
    ecg_peak_mono_p(np,1)=i;
    ecg_peak_mono_p(np,2)=count;
end;
if np<np0
    ecg_peak_mono_p(np+1:np0,:)=[];
end;
end
function [beat_chunk,nchunk]=get_chunks(beat_label)
plen=size(beat_label,1);
beat_chunk=zeros(16,2);
nchunk=0;
chunk_stat=0;
chunk_s=0;
chunk_e=0;
for i=1:plen
    if chunk_stat==1
        if hop1>1
            hop1=hop1-1;
            continue;
        end;
    end;
    
    if (beat_label(i,1)>0)
        hop1=beat_label(i,1);
        hop3=beat_label(i+hop1,2);
        if hop1==hop3
            if chunk_stat==0
                chunk_stat=1;
                nchunk=nchunk+1;
                chunk_s=i;
                chunk_e=i;
            else
                chunk_e=i;
            end;
        end;
    elseif (beat_label(i,2)>0)
        if chunk_stat==1
            chunk_e=i;
            beat_chunk(nchunk,1)=chunk_s;
            beat_chunk(nchunk,2)=chunk_e;
            chunk_stat=0;
        end;
    else
        if chunk_stat==1
            beat_chunk(nchunk,1)=chunk_s;
            beat_chunk(nchunk,2)=chunk_e;
            chunk_stat=0;
        end;
    end;
end;

if nchunk<16
    beat_chunk(nchunk+1:16,:)=[];
end;

end

function [norm_p,np]=get_peak_norm(beat_label_e_p,ecg_peak_array,ecg_stg_peak_index)
plen=size(beat_label_e_p,1);
norm_p_array=zeros(plen,4);
np=0;
for i=1:plen
    if (beat_label_e_p(i,1)||beat_label_e_p(i,2))
        np=np+1;
        norm_p_array(np,1:4)=ecg_peak_array(ecg_stg_peak_index(i),2:5);
    end;
end;
norm_p=zeros(4,1);

for i=1:4
    norm_p(i,1)=mean(norm_p_array(1:np,i));
    norm_p(i,2)=std(norm_p_array(1:np,i));
end;
end

function [beat_label_e_p,beat_label_p_e]=label_beat_dups(e_p_map,ecg_stg_peak,cepspeak_ecg_p,p_e_map,ppg_stg_peak,cepspeak_ppg_p,ecg_ppg_shift,lagmax_seg,samp_rate,stepsize,nstep_p,thres_li)
nbeat_ecg=size(e_p_map,1);
beat_label_e_p=zeros(nbeat_ecg,2);%1:direct_dup;2:indirect_dup
nbeat_ppg=size(p_e_map,1);
beat_label_p_e=zeros(nbeat_ppg,2);

for i=1:nbeat_ecg-1
    if (~isnan(e_p_map(i,4)) && ~isnan(e_p_map(i+1,4))) && (e_p_map(i,4) && e_p_map(i+1,4))
        ecg_beat1=ecg_stg_peak(i);
        ecg_beat2=ecg_stg_peak(i+1);
        ppg_beat1=ppg_stg_peak(e_p_map(i,3))-ecg_ppg_shift;
        ppg_beat2=ppg_stg_peak(e_p_map(i+1,3))-ecg_ppg_shift;
        ecg_beat_center=round((ecg_beat1+ecg_beat2)/2);
        ppg_beat_center=round((ppg_beat1+ppg_beat2)/2);
        ecg_ceps_step=round((ecg_beat_center-lagmax_seg*samp_rate)/stepsize)+1;
        ppg_ceps_step=round((ppg_beat_center-lagmax_seg*samp_rate)/stepsize)+1;
        if ecg_ceps_step<1; ecg_ceps_step=1; end;
        if ppg_ceps_step<1; ppg_ceps_step=1; end;
        if ecg_ceps_step>nstep_p; ecg_ceps_step=nstep_p; end;
        if ppg_ceps_step>nstep_p; ppg_ceps_step=nstep_p; end;
        ecg_bbi=ecg_beat2-ecg_beat1;
        ppg_bbi=ppg_beat2-ppg_beat1;
        ecg_lag=cepspeak_ecg_p(ecg_ceps_step,4);
        ppg_lag=cepspeak_ppg_p(ppg_ceps_step,4);
        ecg_li=abs(log(ecg_lag/ecg_bbi));
        ppg_li=abs(log(ppg_lag/ppg_bbi));
        
        if (ecg_li<thres_li)
            beat_label_e_p(i,1)=1;
            beat_label_e_p(i+1,2)=1;
        else
            ok=1;
        end;            
        if (ppg_li<thres_li)
            hop=e_p_map(i+1,3)-e_p_map(i,3);
            beat_label_p_e(e_p_map(i,3),1)=hop;
            beat_label_p_e(e_p_map(i+1,3),2)=hop;
        else
            ok=1;
        end;            
        ok=1;
        
    end;
    
end;

for i=1:nbeat_ppg-1
    if ( ~isnan(p_e_map(i,4)) && ~isnan(p_e_map(i+1,4))) && ( p_e_map(i,4) && p_e_map(i+1,4)) && ((~beat_label_p_e(i,1)) && (~beat_label_p_e(i+1,2)) )
        ppg_beat1=ppg_stg_peak(i)-ecg_ppg_shift;
        ppg_beat2=ppg_stg_peak(i+1)-ecg_ppg_shift;
        ecg_beat1=ecg_stg_peak(p_e_map(i,3));
        ecg_beat2=ecg_stg_peak(p_e_map(i+1,3));
        ppg_beat_center=round((ppg_beat1+ppg_beat2)/2);
        ecg_beat_center=round((ecg_beat1+ecg_beat2)/2);
        ppg_ceps_step=round((ppg_beat_center-lagmax_seg*samp_rate)/stepsize)+1;
        ecg_ceps_step=round((ecg_beat_center-lagmax_seg*samp_rate)/stepsize)+1;
        if ppg_ceps_step<1; ppg_ceps_step=1; end;
        if ecg_ceps_step<1; ecg_ceps_step=1; end;
        if ppg_ceps_step>nstep_p; ppg_ceps_step=nstep_p; end;
        if ecg_ceps_step>nstep_p; ecg_ceps_step=nstep_p; end;
        ppg_bbi=ppg_beat2-ppg_beat1;
        ecg_bbi=ecg_beat2-ecg_beat1;
        ppg_lag=cepspeak_ppg_p(ppg_ceps_step,4);
        ecg_lag=cepspeak_ecg_p(ecg_ceps_step,4);
        ppg_li=abs(log(ppg_lag/ppg_bbi));
        ecg_li=abs(log(ecg_lag/ecg_bbi));
        
        if (ppg_li<thres_li)
            beat_label_p_e(i,1)=1;
            beat_label_p_e(i+1,2)=1;
        else
            ok=1;
        end;            
        if (ecg_li<thres_li)
            hop=p_e_map(i+1,3)-p_e_map(i,3);
            beat_label_e_p(p_e_map(i,3),1)=hop;
            beat_label_e_p(p_e_map(i+1,3),2)=hop;
        else
            ok=1;
        end;            
        ok=1;
        
    end;
    
end;

end

function [ecg_ppg_comb1,ecg_ppg_comb2]=get_ceps_lag_diff_by_mode(cepspeak_ecg1,cepspeak_ppg1,nstep1,step_shift_ecg_ppg,thres_lagdist)
ecg_ppg_comb1=zeros(nstep1,4);%ecg (lag)(mag)(lag_diff)(lag_clean)
ecg_ppg_comb2=zeros(nstep1,4);%ppg (lag)(mag)(lag_diff)(lag_clean)
ecg_ppg_comb1(:,:)=nan;
ecg_ppg_comb2(:,:)=nan;

% max_lagdist=round(samp_rate*0.08);
% lagdist_hist=zeros(max_lagdist+1,1);

for i=1:nstep1-step_shift_ecg_ppg
    ecg_ppg_comb1(i,1)=cepspeak_ecg1(i,4);
    ecg_ppg_comb1(i,2)=cepspeak_ecg1(i,2);
    ecg_ppg_comb2(i+step_shift_ecg_ppg,1)=cepspeak_ppg1(i+step_shift_ecg_ppg,4);
    ecg_ppg_comb2(i+step_shift_ecg_ppg,2)=cepspeak_ppg1(i+step_shift_ecg_ppg,2);
    
    lagdist=abs(cepspeak_ecg1(i,4)-cepspeak_ppg1(i+step_shift_ecg_ppg,4));%could be nan
    ecg_ppg_comb1(i,3)=lagdist;
    ecg_ppg_comb2(i+step_shift_ecg_ppg,3)=lagdist;
    
%     if lagdist<=max_lagdist
%         lagdist_hist(lagdist+1)=lagdist_hist(lagdist+1)+1;
%     end
end;
index1=ecg_ppg_comb1(:,3)<=thres_lagdist;
ecg_ppg_comb1(index1,4)=ecg_ppg_comb1(index1,1);
index2=ecg_ppg_comb2(:,3)<=thres_lagdist;
ecg_ppg_comb2(index2,4)=ecg_ppg_comb2(index2,1);
end

function [cepspeak_1,cepspeak_2]=get_ceps_lag_diff_by_mode_direct(cepsgram_1,cepsgram_2,nstep,show_lagsize,thres_lagdist)
cepspeak_1=zeros(nstep,4);%(lag)(mag)(lag_diff)(lag_clean)
cepspeak_2=zeros(nstep,4);%(lag)(mag)(lag_diff)(lag_clean)
cepspeak_1(:,:)=nan;
cepspeak_2(:,:)=nan;

for i=1:nstep
    [mv1,mi1]=max(cepsgram_1(i,1:show_lagsize));
    cepspeak_1(i,1)=mi1;
    cepspeak_1(i,2)=mv1;
    [mv2,mi2]=max(cepsgram_2(i,1:show_lagsize));
    cepspeak_2(i,1)=mi2;
    cepspeak_2(i,2)=mv2;
    lagdiff=mi1-mi2;
    lagdist=abs(lagdiff);
    cepspeak_1(i,3)=lagdist;
    cepspeak_2(i,3)=lagdist;
end;

index1=cepspeak_1(:,3)<=thres_lagdist;
cepspeak_1(index1,4)=cepspeak_1(index1,1);
index2=cepspeak_2(:,3)<=thres_lagdist;
cepspeak_2(index2,4)=cepspeak_2(index2,1);

%forward recovery
lag_prev1=250;
lag_prev2=250;
for i=1:nstep
   if ~isnan(cepspeak_1(i,4)) 
       lag_prev1=cepspeak_1(i,4);
       lag_prev2=cepspeak_2(i,4);
       break;
   end;
end;
for i=1:nstep
   if ~isnan(cepspeak_1(i,4)) 
       lag_prev1=cepspeak_1(i,4);
       lag_prev2=cepspeak_2(i,4);
   end;
   if isnan(cepspeak_1(i,4))
       lag_lb=round(lag_prev1/sqrt(2));
       lag_hb=round(lag_prev1*sqrt(2));
       if lag_lb<1; lag_lb=1; end;
       if lag_hb>show_lagsize; lag_hb=show_lagsize; end;
       [mv1,mi1]=max(cepsgram_1(i,lag_lb:lag_hb));
       mi1=mi1+lag_lb-1;
       cepspeak_1(i,1)=mi1;
       cepspeak_1(i,2)=mv1;
   end;
   if isnan(cepspeak_2(i,4))
       lag_lb=round(lag_prev2/sqrt(2));
       lag_hb=round(lag_prev2*sqrt(2));
       if lag_lb<1; lag_lb=1; end;
       if lag_hb>show_lagsize; lag_hb=show_lagsize; end;
       [mv2,mi2]=max(cepsgram_2(i,lag_lb:lag_hb));
       mi2=mi2+lag_lb-1;
       cepspeak_2(i,1)=mi2;
       cepspeak_2(i,2)=mv2;
   end;
   lagdiff=mi1-mi2;
   lagdist=abs(lagdiff);
   cepspeak_1(i,3)=lagdist;
   cepspeak_2(i,3)=lagdist;
   if lagdist<=thres_lagdist
       lag_prev1=mi1;
       lag_prev2=mi2;
   end;
    
end;

index1=cepspeak_1(:,3)<=thres_lagdist;
cepspeak_1(index1,4)=cepspeak_1(index1,1);
index2=cepspeak_2(:,3)<=thres_lagdist;
cepspeak_2(index2,4)=cepspeak_2(index2,1);

end

function [ cepsgram ] = challenge_get_ceps(signal, samp_rate, windurmax, stepdur, cw)
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here

siglen=length(signal);

% windurmax=3.00;
% windurmin=0.80;
windurmin=0.8;
% stepdur=0.3;

% fcut_ceps=128;%64;%32;%16;%128;%64;%Hz
lagmin=windurmin/2;
lagmax=windurmax/2;
lagcap=lagmax;

% get cropwin stack (cropwin of diff sizes)
% cw=MSCM_Get_CropWin(samp_rate,fcut_ceps,lagmin,lagcap);

lagsizemin=round(lagmin*samp_rate);
lagsizemax=round(lagmax*samp_rate);
lagsizecap=round(lagcap*samp_rate);
winsizemin=lagsizemin*2;
winsizemax=lagsizemax*2;
winsizecap=lagsizecap*2;

% get time win of diff sizes
% for iLag=lagsizemin:lagsizecap
%     hw.HanWin{iLag}=hanning(iLag*2);
% end;

hw.HanWin{lagsizecap}=hanning(lagsizecap*2);

stepsize=round(stepdur*samp_rate);

nstep=floor((siglen-winsizecap)/stepsize)+1;

cepsgram=zeros(nstep,lagsizemax);%ceps

for iStep=1:nstep
    %get datawin for ecg and bp
    wincenter_l=(iStep-1)*stepsize+lagsizecap;
    wincenter_r=wincenter_l+1;
    
    widx_start=wincenter_l-lagsizecap+1;
    widx_end=wincenter_r+lagsizecap-1;
    datawin=signal(widx_start:widx_end);
    
    %get ceps for ecg and bp
    y1=datawin(1:lagsizecap*2);
    y1=y1-mean(y1);
    y2=abs(fft(y1.*hw.HanWin{lagsizecap}));
    y3=log10(y2);
    y4=real(ifft(y3.*cw.CropWin{lagsizecap}));
    
    cepsgram(iStep,lagsizemin:lagsizecap)=y4(lagsizemin+1:lagsizecap+1);
end;

end

function [ cw ] = MSCM_Get_CropWin( F_samp, F_cut, Lag_min, Lag_max )
%MSCM_Get_CropWin 
%   F_samp (Hz)
%   F_cut (Hz)
%   Lag_min (s) 
%   Lag_max (s)

F_nq=F_samp/2;
CropRatio=F_nq/F_cut;
HalfWinSize_min=round(Lag_min*F_samp);
HalfWinSize_max=round(Lag_max*F_samp);

cw.Fs=F_samp;
cw.Fcut=F_cut;
cw.HWSmin=HalfWinSize_min;
cw.HWSmax=HalfWinSize_max;

for hws=HalfWinSize_min:HalfWinSize_max
    winsize=hws*2;
    halfcropsize=round(hws/CropRatio);
    cropsize=halfcropsize*2;

    cropwin=zeros(winsize,1);
    cropwin(1)=1;%DC
    if cropsize<=winsize
        tempwin=hanning(cropsize+1);%single center point
        cropwin(1+1:1+halfcropsize)=tempwin(halfcropsize+2:cropsize+1);%positive freq
        cropwin(winsize-halfcropsize+1:winsize)=tempwin(1:halfcropsize);%negative freq
    elseif cropsize>winsize
        tempwin=hanning(cropsize+1);%single center point
        cropwin(1+1:1+hws)=tempwin(halfcropsize+2:halfcropsize+hws+1);%positive freq
        cropwin(winsize-hws+1:winsize)=tempwin(halfcropsize-hws+1:halfcropsize);
    end;
    
    cw.CropWin{hws}=cropwin;
            
end;

end


function [e_p_map,p_e_map,shift_hist]=align_peak(ecg_stg_peak,ppg_stg_peak,ecg_ppg_shift,align_margin)
eplen=length(ecg_stg_peak);
pplen=length(ppg_stg_peak);
ppg_stg_peak=ppg_stg_peak-ecg_ppg_shift;
%(dist)(shift)(map_index)(select_flag)
e_p_map=zeros(eplen,4);
e_p_map(:,:)=nan;
p_e_map=zeros(pplen,4);
p_e_map(:,:)=nan;


i=1;
j=1;

while 1
    shift=ecg_stg_peak(i)-ppg_stg_peak(j);
    dist=abs(shift);
    if dist<=align_margin
        if isnan(e_p_map(i,1))
            e_p_map(i,1)=dist;
            e_p_map(i,2)=shift;
            e_p_map(i,3)=j;
        else
            if dist<e_p_map(i,1)
                e_p_map(i,1)=dist;
                e_p_map(i,2)=shift;
                e_p_map(i,3)=j;
            end;
        end;

        if isnan(p_e_map(j,1))
            p_e_map(j,1)=dist;
            p_e_map(j,2)=shift;
            p_e_map(j,3)=i;
        else
            if dist<p_e_map(j,1)
                p_e_map(j,1)=dist;
                p_e_map(j,2)=shift;
                p_e_map(j,3)=i;
            end;
        end;
    end;
    
    if (i<eplen)
        if (j<pplen)
            if ecg_stg_peak(i+1)<=ppg_stg_peak(j+1)
                i=i+1;
            else
                j=j+1;
            end;
        else
            i=i+1;
        end;
    else
        if (j<pplen)
            j=j+1;
        else
            break;
        end;
    end;
end;

shift_hist_size=align_margin*2+1;
shift_hist=zeros(shift_hist_size,1);
for i=1:eplen
    if ~isnan(e_p_map(i,3))
        j=e_p_map(i,3);
        if ~isnan(p_e_map(j,3));
            if i==p_e_map(j,3)
                e_p_map(i,4)=1;
                p_e_map(j,4)=1;
                shift=e_p_map(i,2);
                shift_hist(shift+align_margin+1)=shift_hist(shift+align_margin+1)+1;
            end;
        end;
    end;
end;

end

function ecg_stg_peak_index=get_strong_peak(peak_array,plen,peak_span_array,v_index,v_ratio)
ecg_stg_peak_index=[];
value_array(1:plen)=peak_array(1:plen,v_index);

for i=2:plen
    if value_array(i)>0
        if ((i==1)&&(value_array(i)>value_array(i+1))) || ((i==plen)&&(value_array(i)>value_array(i-1))) || ((value_array(i)>value_array(i-1))&&(value_array(i)>value_array(i+1)))
            v=value_array(i);
            idx=find(value_array(peak_span_array(i,1):peak_span_array(i,2))<v*v_ratio);
            value_array(idx+peak_span_array(i,1)-1)=0;
        end;
    end;
end;

ecg_stg_peak_index=find(value_array(:));


end



function peak_span_array=mark_peak_array_span(peak_array,plen,t_span)
peak_span_array=zeros(plen,2);

s_index=1;
e_index=1;
for i=1:plen
    if (peak_array(i,1)-peak_array(e_index,1))>t_span
        j=s_index;
        while (peak_span_array(j,1)==0)
            peak_span_array(j,1)=e_index;
            j=j-1;
            if j<=0; break; end;
        end;

        j=e_index;
        while (peak_array(i,1)-peak_array(j,1))>t_span
            %mark right span 
            peak_span_array(j,2)=s_index;
            j=j+1;
        end;
        s_index=i;
        e_index=j;
    else
        s_index=i;
        
    end;
end;

j=s_index;
while (peak_span_array(j,1)==0)
    peak_span_array(j,1)=e_index;
    j=j-1;
    if j<=0; break; end;
end;

j=e_index;
while (j<=s_index)
    %mark right span
    peak_span_array(j,2)=s_index;
    j=j+1;
end;

end

function peak_array=get_peak_array5(signal,siglen)


%1(flag:1/-1)
%2(left oppo index)3(right oppo index)
%4(left relative value)5(right relative value)


s1=zeros(siglen,1);
s2=zeros(siglen,1);
s1(2:siglen)=signal(2:siglen)-signal(1:siglen-1);%left gradient
s2(1:siglen-1)=-1*s1(2:siglen);%right gradient
index1=(s1>0);
index2=(s2>0);
index3=index1.*index2;%index for peaks
index1=(s1<0);
index2=(s2<0);
index4=index1.*index2;%index for troughs

index5=find(index3);%list of peaks_index
index6=find(index4);%list of troughs_index

plen=length(index5);
if plen<2; peak_array=[]; return; end;
%remove trough at front and back
if index5(1)<index6(1)
    index5(1)=[];
    plen=plen-1;
end;
if index5(end)>index6(end)
    index5(end)=[];
    plen=plen-1;
end;

peak_array=zeros(plen,5);
peak_array(1:plen,1)=index5(1:plen);%flag for peak
peak_array(1:plen,2)=index5(1:plen)-index6(1:plen);
peak_array(1:plen,3)=index6(2:plen+1)-index5(1:plen);
peak_array(1:plen,4)=signal(index5)-signal(index6(1:plen));
peak_array(1:plen,5)=signal(index5)-signal(index6(2:plen+1));

% vleft=zeros(plen,1);
% vright=zeros(plen,1);
% vlr=vleft-vright;
% idx=(vlr<0);
% vleft(idx)=vright(idx);
% peak_array(index5,4)=vleft(1:plen);



end

function sig_grad=get_gradient(sig,siglen)
sig_grad=zeros(siglen,1);
sig_grad(1)=(sig(2)-sig(1))*2;
sig_grad(siglen)=(sig(siglen)-sig(siglen-1))*2;
sig_grad(2:siglen-1)=sig(3:siglen)-sig(1:siglen-2);
end
