clear
close all;

Fs = 0.5;
dt = 1 / Fs;
time = 0:1/Fs:1000;
speed = 0.1 * ones(1, length(time));
speed(300:350) = 2 * speed(300:350);
noiseAmp = 10.0;
depthNoNoise = speed .* time;
noise = noiseAmp .* rand(1,length(depthNoNoise));
depth = depthNoNoise + noise;
plot(time,depthNoNoise,time,depth);

amp = 0.1;
P = zeros(1,50);
P = [P amp*ones(1,50)];
P=P';

P = depth';

h1 = fdesign.differentiator('N,Fp,Fst,Ast', 6, .4, .45, 20);
H1 = design(h1);
h2 = fdesign.differentiator('N,Fp,Fst',20,.25,.45);
H2 = design(h2);
h3 = fdesign.differentiator('N,Fp,Fst',6,.4,.45);
H3 = design(h3);

vDiff = diff(P)/dt;
aDiff = diff(vDiff)/dt;
vDiff = [0; vDiff];
aDiff = [0; 0; aDiff];

D1 = mean(grpdelay(H1)); % filter delay
%vFilt1 = filter(H1,[P; zeros(D1,1)]);
vFilt1 = filter(H1, P);
%vFilt1 = vFilt1(D1+1:end);
%aFilt1 = filter(H1,[vFilt1; zeros(D1,1)]);
aFilt1 = filter(H1,vFilt1);
%aFilt1 = aFilt1(D1+1:end);
vFilt1 = vFilt1/dt;
aFilt1 = aFilt1/dt^2;

D2 = mean(grpdelay(H2)); % filter delay
%vFilt2 = filter(H2,[P; zeros(D2,1)]);
vFilt2 = filter(H2, P);
%vFilt2 = vFilt2(D2+1:end);
%aFilt2 = filter(H2,[vFilt2; zeros(D2,1)]);
aFilt2 = filter(H2, vFilt2);
%aFilt2 = aFilt2(D2+1:end);
vFilt2 = vFilt2/dt;
aFilt2 = aFilt2/dt^2;

D3 = mean(grpdelay(H3)); % filter delay
%vFilt3 = filter(H3,[P; zeros(D3,1)]);
vFilt3 = filter(H3, P);
%vFilt3 = vFilt3(D3+1:end);
%aFilt3 = filter(H3,[vFilt3; zeros(D3,1)]);
aFilt3 = filter(H3, vFilt3);
%aFilt3 = aFilt3(D3+1:end);
vFilt3 = vFilt3/dt;
aFilt3 = aFilt3/dt^2;

time = (0:length(P)-1)* dt;
vOffset = 40;
plot(time, P, time, vDiff + vOffset, time, vFilt1  + vOffset, time, vFilt2 + vOffset, time, vFilt3 + vOffset);
legend('Pressure', 'vDiff', 'V1', 'V2', 'V3');

rms(vDiff)
rms(vFilt1)
rms(vFilt2)
rms(vFilt3)

