clear SBE41Pressure SBE41LPFPressure SBE41DiffVelocity SBE41LPFVelocity

PFilterCoef = [ 8.81131298055604e-06,	0.00211708177469335,	0.0240149216743935,	0.102119533086314,	0.225973798612221, ...
    0.291531707078795,	0.225973798612221,	0.102119533086314,	0.0240149216743935,	0.00211708177469335, ...
    8.81131298055604e-06];
numPCoef = length(PFilterCoef);


d = fdesign.differentiator('N,Fp,Fst', 8, .1, .4);
Hd = design(d, 'equiripple');
VFilterCoef = Hd.Numerator
numVCoef = length(VFilterCoef);

%First lpf the pressure data
pressureSample = rawPressure;
PFilterDelayLine = ones(1, numPCoef) * pressureSample(1);

for i=1:length(pressureSample)-numPCoef
    if(i < 200)
        ctdSamplePeriod = 3.0;
    else
        ctdSamplePeriod = 1.0;
    end
    
    accum = 0.0;
    PFilterDelayLine(1) = pressureSample(i);
    for j = 1:numPCoef
        accum = accum + (PFilterCoef(j) * PFilterDelayLine(j));
    end
    SBE41Pressure(i) = PFilterDelayLine(1);
    SBE41LPFPressure(i) = accum;
    SBE41DiffVelocity(i) = (PFilterDelayLine(2) - PFilterDelayLine(1)) / ctdSamplePeriod;
    
    %Shift delay line 1 sample
    for j= numPCoef - 1:-1:1
        PFilterDelayLine(j + 1) = PFilterDelayLine(j);
    end
end

%Now run velocity filter on lpf pressure data
velocitySample = SBE41LPFPressure;
VFilterDelayLine = ones(1, numVCoef) * velocitySample(1);

for i=1:length(velocitySample)-numVCoef
    if(i < 200)
        ctdSamplePeriod = 3.0;
    else
        ctdSamplePeriod = 1.0;
    end
        
    accum = 0.0;
    VFilterDelayLine(1) = velocitySample(i);
    for j=1:numVCoef
        accum = accum + (VFilterCoef(j) * VFilterDelayLine(j));
    end
    SBE41LPFVelocity(i) = -1.0 * accum / ctdSamplePeriod;        %-1.0 because pressure increases positively down but velocity in the down direction needs to be negative
    
    %Shift delay line 1 sample
    for j= numVCoef - 1:-1:1
        VFilterDelayLine(j + 1) = VFilterDelayLine(j);
    end
end
