close all
A = importdata('poses.txt', ' ', 1);

% Options:
max_acc=60
max_jump_filtered=0.02

t = A.data(:,1)-A.data(1,1);
x = A.data(:,2);
y = A.data(:,3);
z = A.data(:,4);

##pkg load quaternion
##Q = quaternion(A.data(:,8), A.data(:,5), A.data(:,6), A.data(:,7));
##dangular=zeros(length(Q)-1, 1);
##warning('off', 'q2rot:normalizing')
##for i=1:length(Q)-1
##  q1 = Q(i) * quaternion(0,1,0,0) * Q(i)';
##  q2 = Q(i+1) * quaternion(0,1,0,0) * Q(i+1)';
##  v1 = [q1.x, q1.y, q1.z];
##  v2 = [q2.x, q2.y, q2.z];
##  
##  % Acompute angle between vectors
##  rad = dot(v1,v2) / sqrt (norm(v1)^2 * norm(v2)^2);
##  if (rad < -1.0)
##    rad = -1.0;
##  endif
##  if (rad >  1.0)
##    rad = 1.0;
##  endif
##  dangular(i) = acos (rad);
##  
##  if (mod(i, 1000) == 0)
##    disp(num2str(i));
##  endif
##endfor
##dangular = rad2deg(dangular);

dt=t(2:end) - t(1:end-1);
dx=x(2:end) - x(1:end-1);
dy=y(2:end) - y(1:end-1);
dz=z(2:end) - z(1:end-1);
dlinear=sqrt(dx.*dx+dy.*dy+dz.*dz);
vx=dx./dt;
vy=dy./dt;
vz=dz./dt;
velocity=dlinear./dt;
##angvelocity=dangular./dt;

ax=(vx(2:end) - vx(1:end-1))./dt(1:end-1);
ay=(vy(2:end) - vy(1:end-1))./dt(1:end-1);
az=(vz(2:end) - vz(1:end-1))./dt(1:end-1);
acc=sqrt(ax.^2+ay.^2+az.^2);

offset = [0,0,0]
for i=3:length(x)
  x(i) = x(i) + offset(1);
  y(i) = y(i) + offset(2);
  z(i) = z(i) + offset(3);
  max_jump = max([abs(x(i-1)-x(i)), abs(y(i-1)-y(i)), abs(z(i-1)-z(i))]);
  if acc(i-2) > max_acc && max_jump > max_jump_filtered
    new_val = x(i-1)+vx(i-2)*dt(i-1);
    offset(1) = offset(1)+(new_val-x(i));
    printf("i=%d t=%f acc=%f vx(i-2)=%f prev_x=%f x=%f dt=%f new_x=%f offset=%f jump=%f\n", 
       i, t(i), acc(i-2), vx(i-2), x(i-1), x(i), dt(i-1), new_val, offset(1), max_jump)
    x(i) = new_val;
    vx(i-1) = (x(i) - x(i-1))/dt(i-1);
    
    new_val = y(i-1)+vy(i-2)*dt(i-1);
    offset(2) = offset(2)+(new_val-y(i));
    y(i) = new_val;
    vy(i-1) = (y(i) - y(i-1))/dt(i-1);
    
    new_val = z(i-1)+vz(i-2)*dt(i-1);
    offset(3) = offset(3)+(new_val-z(i));
    z(i) = new_val;
    vz(i-1) = (z(i) - z(i-1))/dt(i-1);
  endif
endfor

% recompute velocity data
dx=x(2:end) - x(1:end-1);
dy=y(2:end) - y(1:end-1);
dz=z(2:end) - z(1:end-1);
dlinear=sqrt(dx.*dx+dy.*dy+dz.*dz);
vx=dx./dt;
vy=dy./dt;
vz=dz./dt;
velocity=dlinear./dt;

ax=(vx(2:end) - vx(1:end-1))./dt(1:end-1);
ay=(vy(2:end) - vy(1:end-1))./dt(1:end-1);
az=(vz(2:end) - vz(1:end-1))./dt(1:end-1);
acc=sqrt(ax.^2+ay.^2+az.^2);
##angacc=(angvelocity(2:end) - angvelocity(1:end-1))./dt(1:end-1);

figure
plot(t(1:end-1), dt, '.-')
hold on
plot(t(1:end-1), dlinear, '.-')
legend('dt', 'translation (m)')
title('Abs translation vs dt')

figure
plot(t,x, '.-')
hold on
plot(t,y, '.-')
plot(t,z, '.-')
legend('x', 'y', 'z')
title('Position (m)')

figure
plot(t(1:end-1), velocity, '.-')
hold on
plot(t(1:end-1),vx, '.-')
plot(t(1:end-1),vy, '.-')
plot(t(1:end-1),vz, '.-')
legend('vt','vx', 'vy', 'vz')
title('Linear Velocity (m/s)')

##figure
##plot(t(1:end-1), angvelocity, '.-')
##title('Angular Velocity (deg/s)')

figure
plot(t(1:end-2),acc, '.-')
hold on
plot(t(1:end-2),ax, '.-')
plot(t(1:end-2),ay, '.-')
plot(t(1:end-2),az, '.-')
legend('at','ax', 'ay', 'az')
title('Linear Acceleration m/s^2')