close all;
clear all;

data = loadData('2speeds');

x_acc = data.accel(:,1);
figure;
plot(x_acc);

sec1 = mean(x_acc(1:14350))
sec2 = mean(x_acc(15500:26500))
sec3 = mean(x_acc(27200:32500))
sec4 = mean(x_acc(33300:38400))
sec5 = mean(x_acc(39400:54000))

means = [sec1 sec2 sec3 sec4 sec5];
figure;
plot(means,'-+');

slope1 = (sec3 - sec1)/(26500-15500);
slope2 = (sec5 - sec3)/(39400-33300);

offset1 = sec1 + slope1*(0:11000);
offset2 = sec3 + slope2*(0:6100);

acc1 = x_acc(15500:26500);
acc2 = x_acc(33300:39400);
sec2Offset = mean(acc1-offset1');
sec4Offset = mean(acc2-offset2');

newMeans = [sec1 sec2Offset sec3 sec4Offset sec5]
figure;
plot(newMeans);

r = .0127; %m
radToRPM = 60/(2*pi);

sec2RPM = sqrt(sec2/r)*radToRPM
sec4RPM = sqrt(sec4/r)*radToRPM
sec2OffsetRPM = sqrt(sec2Offset/r)*radToRPM
sec4OffsetRPM = sqrt(sec4Offset/r)*radToRPM

figure;
plot(acc1);
hold on;
plot(offset1);