%DepthData=[];
%d=((((AllData(:,9))*2-0.5)*70/4)-1)*10;
%DepthData=[DepthData;AllData(:,1) d];
close all
clear all
load DepthData
%%
DepthData(isnan(DepthData(:,2)),:)=[];
DepthData(isnan(DepthData(:,1)),:)=[];

figure;
plot(DepthData(:,1),DepthData(:,2))
datetick('x','HH:MM','keepticks','keeplimits')
title('Raw BED Depth Data');
xlabel('Time');
ylabel('Depth (m)');

%ThalDepth=data(:,3).*-1;
load Thalwag;
figure; plot(ThalDepth);
title('Thal Depth Records');
ylabel('Depth (m)');


%ThalDist=data(:,6);
%figure;plot(ThalDist);
close all;
Thaltime= ThalDist/3.3;     %3.3m/sec is average speed

EpochSecs=(DepthData(:,1)-719529)*86400;
BedsElapsedTime=zeros(size(EpochSecs));
for i=1:size(EpochSecs,1),
    BedsElapsedTime(i,1)=(EpochSecs(i,1)-EpochSecs(1,1))/60/60;
end

figure; plot(Thaltime/60, ThalDepth)
hold on;
plot(BedsElapsedTime.*60,DepthData(:,2),'r')
plot(Thaltime/60,ThalDepth+7.6845,'g');
legend('Thalwag','BED','Thal+offset','Location','Best');xlabel('Elapsed Min');ylabel('Depth (dBar)');

%%
[B,I,J] = unique(round(DepthData(:,2)),'stable');
BedsRounded=[BedsElapsedTime(I),B];

figure; plot(BedsElapsedTime.*60,DepthData(:,2),'b');hold on;
plot(BedsRounded(:,1).*60,BedsRounded(:,2),'g')
xlabel('Elapsed Time(min)');ylabel('Depth (dbar)');
legend('Raw Data', 'Rounded Data','Location','Best');

[C,H,K]=unique(round(ThalDepth),'stable');

figure; plot(Thaltime/60, ThalDepth)
hold on;
plot(Thaltime(H)/60,C,'r--');
xlabel('Elapsed Time(min)');ylabel('Depth (dbar)');
legend('Thal depth',' Rounded Thal depth','Location','Best');
ThalRound=[C, ThalDist(H)];


[C,IA,IB] = intersect(BedsRounded(:,2),ThalRound(:,1),'rows');

BedsDist=ones(size(BedsRounded,1),1);
BedsDist(IA)=ThalRound(IB,2);
[i,j]=find(BedsDist==1);
BedsDist(i)=nan;

BedsRounded(isnan(BedsDist),:)=[];
BedsDist(isnan(BedsDist))=[];

diffDist=diff(BedsDist);
diffTime=diff(BedsRounded(:,1)*3600);
BedsTime=BedsRounded(:,1)*3600;

[i,j]=find(diffTime<0);
diffDist(i)=[];
diffTime(i,:)=[];
BedsTime(i,:)=[];

[i,j]=find(diffDist<0);
diffDist(i)=[];
diffTime(i,:)=[];
BedsTime(i,:)=[];

figure;subplot(2,1,1);bar(diffDist);ylabel('Delta Dist (m)');
subplot(2,1,2);bar(diffTime);ylabel('Delta Time (sec)');

deltas=[diffTime diffDist BedsTime(1:end-1)];
deltas(isnan(deltas(:,1)),:)=[];
deltas(isnan(deltas(:,2)),:)=[];
figure;subplot(2,1,1);bar(deltas(:,2));ylabel('Delta Dist (m)');
subplot(2,1,2);bar(deltas(:,1));ylabel('Delta Time (sec)');



EstVelocity=deltas(:,2)./deltas(:,1);


figure; bar(EstVelocity,'b');
ylabel('Velocity (m/sec)');
%figure;plot(deltas(:,3),EstVelocity);


figure;subplot(2,1,1);
plot(diff(BedsRounded(:,1)).*3600);
ylabel('seconds')
subplot(2,1,2);
plot(diff(BedsDist));
ylabel('meters')


%%%% try griding data and filtering
% X = 0:10; V = sin(X); Xq = 0:.25:10;
% Vq = interp1(X,V,Xq); plot(X,V,'o',Xq,Vq)

X=BedsElapsedTime.*3600;    %time in elapsed sec
V=DepthData(:,2);
Xq=linspace(0,X(end),size(X,1));

Vq=interp1(X,V,Xq);
figure;plot(X,V,'o',Xq,Vq)

Vfilt=medfilt1(Vq,10);

figure;
plot(X/60,V,'b',Xq/60,Vq,'g',Xq/60,Vfilt,'r'); hold on;
legend('Raw Data','Regrid Data','Filtered Data','Location','Best');
xlabel('Elapsed Min');
ylabel('Depth (dBar)');

figure;
plot(Thaltime/60, ThalDepth);hold on; plot(Xq/60,Vfilt,'r');
legend('Thal Const vel','Filtered Data','Location','Best');
xlabel('Elapsed Min');
ylabel('Depth (dBar)');

Vround=round(Vfilt)';
Tround=round(ThalDepth);
[C,IA,IB] = intersect(Vround,Tround,'rows');

figure;plot(Vround(IA),'o');hold on;plot(Tround(IB),'xr');
legend('Beds','Thalwag','Location','Best');
title('Intersect points');
xlabel('Data point');
ylabel('Depth (dBar)');

VelEst=[];
VelEst(:,1)=Xq(IA);
VelEst(:,2)=Vround(IA);
VelEst(:,3)=ThalDist(IB);

figure;plot(VelEst(:,1)/60,VelEst(:,2));hold on; plot(X/60,V,'r');
legend('Velocity Data Set','Raw Data','Location','Best');
ylabel('Depth (dBar)');
xlabel('Elapsed Min');

figure;subplot(2,1,1);
bar(diff(VelEst(:,1)));
ylabel('seconds');
title('Delta time');

subplot(2,1,2);
bar(diff(VelEst(:,3)));
ylabel('meters')
title('Delta Dist');

a=diff(VelEst(:,3));
b=diff(VelEst(:,1));
Velocity2=diff(VelEst(:,3))./diff(VelEst(:,1));


figure; bar(Velocity2,'b');
ylabel('Velocity (m/sec)');
hold on;
bar(EstVelocity,'r');
legend('Filtered Est', 'Rounded Est','Location','Best');

%BedsRounded=[BedsElapsedTime(I),B];

[D,L,M]=unique(Vround,'stable');
UniqFiltBeds=[Xq(L)',D];
figure; plot(X,V,'b');hold on;
plot(UniqFiltBeds(:,1),UniqFiltBeds(:,2),'gx')
xlabel('Elapsed Time(min)');ylabel('Depth (dbar)');
legend('Raw Data', 'Rounded Data','Location','Best');

[C,IA,IB] = intersect(UniqFiltBeds(:,2),ThalRound(:,1),'rows');

figure;plot(UniqFiltBeds(IA,2),'o');hold on;plot(ThalRound(IB),'xr');
legend('Beds','Thalwag','Location','Best');
title('Intersect points');
xlabel('Data point');
ylabel('Depth (dBar)');

Vel3=[];
Vel3(:,1)=UniqFiltBeds(IA,1);
Vel3(:,2)=UniqFiltBeds(IA,2);
Vel3(:,3)=ThalRound(IB,2);

figure; plot(X/60,V);hold on;plot(Vel3(:,1)/60,Vel3(:,2),'r');
xlabel('Elapsed Time(min)');ylabel('Depth (dbar)');
legend('Raw Data', 'Matched Data','Location','Best');

VelEst3=diff(Vel3(:,3))./diff(Vel3(:,1));

figure; bar(VelEst3);
[i,j]=find(VelEst3>6);
VelWhacked=VelEst3;
TimeWhacked=Vel3(2:end,1);
TimeWhacked(i)=[];
VelWhacked(i)=[];
figure;bar(VelWhacked);
figure;plot(TimeWhacked,VelWhacked);

figure;subplot(2,1,1);
bar(diff(Vel3(:,1)));
ylabel('seconds');
title('Delta time');

subplot(2,1,2);
bar(diff(Vel3(:,3)));
ylabel('meters')
title('Delta Dist');

figure; [AX,H1,H2]=plotyy(X,V,Vel3(2:end,1),VelEst3);
%%
ThalLinTime=linspace(0,BedsElapsedTime(end).*3600, size(ThalDepth,1));

X=ThalLinTime;    %time in elapsed sec
V=ThalDepth+7.6845;
Xq=linspace(0,X(end),size(BedsElapsedTime,1));

Vq=interp1(X,V,Xq);

figure;plot(X,V,'b',Xq,Vq,'r--')

XX=BedsElapsedTime.*3600;    %time in elapsed sec
VV=DepthData(:,2);
XXq=linspace(0,XX(end),size(XX,1));
VVq=interp1(XX,VV,XXq);
%figure; plot(XX,VV,'b',XXq,VVq,'r--')
figure; plot(XXq,VVq,'b',Xq,Vq,'r');
legend('BEDs data', 'Thalwag');
xlabel('Elapsed time(sec)');
ylabel('Depth (dBar)');
% Example:
%   Measure the delay between two correlated signals.
%     load sensorData s1 s2
%     [acor,lag] = xcorr(s2,s1);
%     [~,I] = max(abs(acor));
%     timeDiff = lag(I)         % sensor 2 leads sensor 1 by 350 samples
%     subplot(311); plot(s1); title('s1');
%     subplot(312); plot(s2); title('s2');
%     subplot(313); plot(lag,acor);
%     title('Cross-correlation between s1 and s2')

[c, lags] = xcorr(VV(1:100),Vq(1:100));


% [c, lags] = xcorr(Vq(1:N),lagThal(1:N));
% [~,I] = max(abs(c));
% timeDiff = lags(I)
% figure; plot(Xq(1:N),Vq(1:N),'b',Xq(1:N),lagThal(1:N),'r')
% 
% figure; plot(c);
% figure; plot(Xq(1:N),Vq(1:N),'b');hold on; plot(Xq(lg:N-1),lagThal(1:N-lg),'r--')
% figure;[amis corrs] = ami([lagThal(1:100) Vq(1:100) ],[15 15],100);

lg=25;
N=500;
lagThal=zeros(size(Vq));
lagThal(1:size(Vq,2)-lg+1)=Vq(lg:end);

err=[];
j=0;
timeLags=[];
x=Vq;
y=VV';
x=Vq(1:N);
y=lagThal(1:N);

x=Vq(N:2*N);
y=Vq(N-lg:2*N-lg);

figure; plot(x,'b');hold on;plot(y,'r'); legend('Thalwag','Lag Thalwag');


[c, lags] = xcorr(x,y);
[~,I] = max(abs(c));
timeDiff = lags(I)
figure;plot(lags,c); 
figure;[amis corrs] = ami([x;y],[20 20],100);
[i,j]=max(corrs)

binSize=250;
for k=1:floor(size(x,2)/binSize),
    err=[];
    startIndex = 1+(k-1)*binSize;
    endIndex = k*binSize;
    for lag=0:100,
        
        %err=[err;std(y(1:end-lag) - x(lag+1:end)),std(x(1:end-lag) - y(lag+1:end))];
        err=[err;std(y(startIndex:endIndex-lag) - x(lag+startIndex:endIndex)),std(x(startIndex:endIndex-lag) - y(lag+startIndex:endIndex))];
    end
    figure;plot(err);title(strcat('Error',num2str(1+(k-1)*binSize)))
    if min(err(:,1))<min(err(:,2)),
        [i,j]=min(err(:,1));
        MinErr1=min(err(:,1))
        j
        figure;
        plot(y(startIndex:endIndex-j),'b');
        hold on;plot(x(startIndex:endIndex-j),'r');
        plot(x(j-1+startIndex:j-1+endIndex),'g');
        legend('Thal','Bed','Thal+lag','Location','Best');
        timeLags=[timeLags;1+(k-1)*binSize,j,MinErr1,0];
        
    else
        [i,j]=min(err(:,2));
        MinErr2=min(err(:,2))
        j
        figure;plot(x(startIndex:endIndex-j),'b');
        hold on;plot(y(startIndex:endIndex-j),'r');plot(y(j-1+startIndex:endIndex),'g--');
        legend('Thal','Bed','Bed+lag','Location','Best');
         timeLags=[timeLags;1+(k-1)*binSize,j,0,MinErr2];
    end
    
end

%%correlation 
x=Vq;   %interpolated Thalwas
y=VV';  %BED data

% binSize=100;
% nBins=[20 20];
% nLags=90;
BinMeans=[];
BinRange=[25,50,100,200,500, 750];
for n=2: size(BinRange,2),
    binSize=BinRange(n);
    nBins=[20 20];
    nLags=floor(0.95*BinRange(n));
timeLags=[];
for k=1:floor(size(x,2)/binSize),
    startIndex = 1+(k-1)*binSize;
    endIndex = k*binSize;
    
    figure;
    [amis corrsX] = ami([x(startIndex:endIndex);y(startIndex:endIndex)],nBins,nLags);
    [i,j]=max(corrsX);close;
    figure;
    [amis corrsY] = ami([y(startIndex:endIndex);x(startIndex:endIndex)],nBins,nLags);
    [m,l]=max(corrsY);close
   
    
    if corrsY(l)< corrsX(j),
        
        figure;
        plot(y(startIndex:endIndex),'b');
        hold on;plot(x(startIndex:endIndex),'r');
        plot(y(startIndex+j-1:endIndex+j-1),'g--');
        legend('Bed','Thal','Bed+lag','Location','Best');
        %   starting index, last index, lag (number of samples,
        %   correlation coeff, index 
        timeLags=[timeLags;startIndex,endIndex,j,corrsX(j),0];
        
    else
       
        figure;plot(x(startIndex:endIndex),'r');
        hold on;plot(y(startIndex:endIndex),'b');plot(x(l-1+startIndex:l-1+endIndex),'g--');
        legend('Thal','Bed','Thal+lag','Location','Best');
        timeLags=[timeLags;startIndex,endIndex,l,corrsY(l),1];
    end
    
end
close all;
    BinMeans=[BinMeans;BinRange(n),mean(timeLags(:,4))];
end

[i,j]=find(timeLags(:,5)==1)
ThalLags=timeLags(:,3);
ThalLags(i)=ThalLags(i).*-1;
figure; plotyy(timeLags(:,1),ThalLags,timeLags(:,1),timeLags(:,4))

%%
%Filtering attempts
% Ts=0.02;
% fcutoff = 2;   %Hz
% fNorm = fcutoff*Ts*2;
% [b,a] = butter(5, fNorm, 'low');
% filtVV = filtfilt(b,a,VV);
% Vfilt=medfilt1(VV,10);
% figure;plot(XX(1:100),filtVV(1:100),'b');hold on; plot(Xq(1:100),Vq(1:100),'r');
% plot(Xq(1:100),Vfilt(1:100),'g')
