﻿clear all;
clc;

raw=[
0.990722656
0.986816406
0.99609375
0.99609375
0.997070313
0.992675781
0.993164063
0.986328125
0.984375
1.00390625
1.001464844
0.987304688
1.005371094
0.979003906
1.008300781
0.983398438
1.006347656
0.998046875
0.976074219
0.950683594
0.812988281
0.719726563
0.745605469
1.500976563
1.014160156
1.199707031
1.053222656
0.897949219
1.268066406
0.891601563
0.9375
0.875488281
1.129394531
0.989257813
0.745117188
1.935546875
0.452148438
1.102539063
1.321289063
0.335449219
1.375976563
0.954101563
0.888183594
0.977539063
0.990722656
0.875
1.021484375
0.984863281
1.012695313
0.95703125
1.006835938
1.00390625
0.9921875
0.998535156
0.996582031
0.99609375
0.983398438
1.010253906
0.979492188
0.998046875
1.00390625
1.005371094
1.031738281
0.955078125
1.016113281
1.009765625
0.965332031
1.037109375
0.971679688
0.890625
0.974121094
0.757324219
1.001953125
0.425292969
1.473144531
1.243652344
1.282226563
1.108398438
0.895507813
0.94140625
0.942382813
0.998535156
0.887207031
1.246582031
0.944824219
0.760742188
2.067382813
0.245117188
1.262207031
0.977539063
0.78515625
1.124511719
1.075195313
0.743652344
1.100097656
0.878417969
1.015136719
0.911621094
1.01953125
1.001953125
0.983886719
0.987304688
0.999023438
0.994628906
0.990234375
0.989257813
1.012695313
0.982421875
0.987304688
0.983398438
1.005371094
1.007324219
0.995605469
1.002929688
0.982910156
1.001464844
0.991210938
0.982421875
1.004394531
0.992675781
0.90625
0.92578125
0.802246094
0.703125
1.075195313
1.25
1.229492188
1.09765625
0.669433594
1.895019531
0.411132813
0.971191406
0.937011719
0.838378906
1.380859375
0.956542969
0.758789063
1.792480469
0.444335938
1.424804688
0.842773438
0.701660156
1.235839844
0.916992188
0.799316406
1.064941406
0.944824219
0.921875
1.026855469
0.971679688
0.98828125
0.991210938
0.985839844
0.985351563
1.01171875
0.987792969
1
0.997070313
0.983398438
0.985351563
0.986816406
1.011230469
1.00390625
0.98828125
1.026367188
0.953125
1.011230469
0.978515625
0.970214844
1.064941406
0.978027344
0.854492188
0.958496094
0.678222656
0.903808594
0.820800781
1.268554688
1.249511719
1.152832031
0.958007813
1.557617188
0.557617188
0.864746094
0.965820313
1.018554688
1.189941406
1.061523438
1.16796875
0.712890625
0.849121094
1.706542969
0.317871094
1.19140625
1.049316406
0.932128906
0.952148438
0.950195313
0.944335938
0.959960938
1.008789063
0.971191406
0.984375
0.994628906
0.984375
1.003417969
0.972167969
0.995117188
0.988769531
0.979980469
0.991699219
0.992675781
0.986328125
0.986816406
1.000976563
0.979980469
0.993652344
0.991210938
0.982910156
0.993652344
0.987792969
0.987792969
0.988769531
0.993652344
0.984863281
0.982910156
0.994628906
0.987304688
0.990234375
0.990722656
0.990234375
0.990234375
0.988769531
0.987792969
0.993652344
0.987792969
0.989257813
0.986328125
0.994628906
0.989257813
0.983886719
0.993164063
0.990722656
0.985839844
0.991699219
0.989746094
0.989746094
0.989257813
0.991210938
0.990234375
0.988769531
0.9921875
0.989257813
0.991210938
0.991699219
0.987792969
0.990234375
0.990234375
0.989257813
0.990234375
0.990234375
0.98828125
];

step = 0.009347; %=107 Hz
speed = 100/3.6; %m/s
M = round((2/speed) / step);

raw = raw' * 9.81;

%Low pass filter signal
N = 50;
_raw = [raw(1) * ones(1, N) raw raw(length(raw)) * ones(1, N)];
b = fir1(N, 60/107, 'low');
filtered = filter(b, 1, _raw)(1.5*N+1:end-0.5*N);

t = (0:1:((length(filtered)-1))) * step;

%Detrending signal
acc = detrend(filtered, 2);
%acc = filtered - mean(filtered);

%Identify silence periods
accsquared = acc .* acc;
sa = movmean(accsquared, M);
samax = max(sa);

function retval = silence(sa, max)
    if (sa < 0.02 * max)
        retval = 1;
    else
        retval = NaN;
    endif
endfunction

sbbp = arrayfun(@(x) silence(x, samax), sa);

figure(1)
plot(t, acc, "r", t, sa, "b");

figure(2)
plot(t, acc, "r", t, sbbp, "b");

%Calculate displacement from acceleration by double integration using trapezoidal rule
v = cumtrapz(t, acc); 
v = v - mean(v);
disp = cumtrapz(t, v);

%Calculating correction baseline to be subtracted from displacement
disp_acc = disp.*sbbp;
x = 1:length(disp_acc);

disp_base_spline = disp_acc;
disp_base_spline(isnan(disp_acc)) = interp1(x(~isnan(disp_acc)),disp_acc(~isnan(disp_acc)),x(isnan(disp_acc)), 'pchip') ;

base = movmean(disp_base_spline, 5);

figure(4)
plot(t, base, 'r', t, disp_acc, 'b', t, disp, 'g');

disp_cleaned = disp - base;

figure(7)
plot(t, disp_cleaned, 'r');

%Maximum displacement in meters
max_displacement = min(disp_cleaned)