clear;

ambient100Hz = loadData('ambient100Hz');
figure(1);
plotXYZ(ambient100Hz)

%Remove DC offset?
ambient100Hz.linAccBody(:,1) = ambient100Hz.linAccBody(:,1) + 0.0033

t_mins = 0:(1/6000):(length(ambient100Hz.linAccBody)-1)/6000.0;
t_secs = 0:.01:(length(ambient100Hz.linAccBody)-1)/100.0;

vel = cumtrapz(t_secs,ambient100Hz.linAccBody(:,1))*100;
figure(2);
plot(t_mins, vel);
xlabel('Time (min)');
ylabel('X velocity (cm/s)');

pos = cumtrapz(t_secs,vel/100);
figure(3);
plot(t_mins, pos)
xlabel('Time (min)');
ylabel('X position (m)');

disp(mean(ambient100Hz.linAccBody(:,1)))

% TODO: Block averaging
xAcc = ambient100Hz.linAccBody(:,1);
%100 second block size
blkSize = 100*100;
for n = 1:(length(xAcc)/blkSize)
    xAcc_blocks(((n-1)*blkSize+1):n*blkSize) = mean(xAcc(1:n*blkSize));
end
%Truncated for convenience - need to figure out how to deal with last part
%of data ^
xAcc_blocks = transpose(xAcc_blocks);
xAcc_corr = xAcc(1:370000)-xAcc_blocks;
t = t_secs(1:370000).';
figure(4);
plot(t, xAcc_corr);
xlabel('Time (min)');
ylabel('Acceleration (m/s^2)');

vel_corr = cumtrapz(t,xAcc_corr)*100;
figure(5);
plot(t, vel_corr);
xlabel('Time (min)');
ylabel('X velocity (corrected) (cm/s)');
