#!/usr/bin/octave

# /* LICENSE: */


				# Process logged gsensor data to train
				# decision trees for stuck state
				# detection.
				# Edit the filenames below to load
				# gsensor log files (as text) into
				# Octave. Run this script to make
				# training examples. Then run
				# build_data.sh to get *.txt which are
				# data examples ready for C4.5

clear;

				# We need to skip a certain amount of
				# data from the start and end of each
				# run. This is because it took some time
				# to unpause the robot and for it to
				# write the log.
start_skip = 25*30;
end_skip = 25*15;

load hooked.txt;
load playing.txt;
load wall.txt;
load goal_corner.txt;

				# Switch to old file names
front_leg_hooked_gsensor = hooked;
playing_gsensor = playing;
standing_gsensor = goal_corner;
walking_into_wall_gsensor = wall;

				# Clear data that was loaded from files
				# as we used these names for variables
				# below.
clear hooked;
clear playing;
clear  wall;
clear goal_corner;


				# Convert timestamps to seconds
front_leg_hooked_gsensor(:,2) = front_leg_hooked_gsensor(:,2) ./ 1000000;
playing_gsensor(:,2) = playing_gsensor(:,2) ./ 1000000;
standing_gsensor(:,2) = standing_gsensor(:,2) ./ 1000000;
walking_into_wall_gsensor(:,2) = walking_into_wall_gsensor(:,2) ./ \
    1000000;


				# While long, descriptive filenames are
				# great, these variable names are going
				# to kill me. Let's come up with some
				# nice, short aliases for different
				# fields.
[y x] = size(front_leg_hooked_gsensor);
end_index = y - end_skip;
hooked_t = front_leg_hooked_gsensor(start_skip:end_index,2);
hooked_x = front_leg_hooked_gsensor(start_skip:end_index,3);
hooked_y = front_leg_hooked_gsensor(start_skip:end_index,4);
hooked_z = front_leg_hooked_gsensor(start_skip:end_index,5);
hooked(:,1) = hooked_t;
hooked(:,2) = hooked_x;
hooked(:,3) = hooked_y;
hooked(:,4) = hooked_z;

[y x] = size(playing_gsensor);
end_index = y - end_skip;
playing_t = playing_gsensor(start_skip:end_index,2);
playing_x = playing_gsensor(start_skip:end_index,3);
playing_y = playing_gsensor(start_skip:end_index,4);
playing_z = playing_gsensor(start_skip:end_index,5);
playing(:,1) = playing_t;
playing(:,2) = playing_x;
playing(:,3) = playing_y;
playing(:,4) = playing_z;

[y x] = size(standing_gsensor);
end_index = y - end_skip;
standing_t = standing_gsensor(start_skip:end_index,2);
standing_x = standing_gsensor(start_skip:end_index,3);
standing_y = standing_gsensor(start_skip:end_index,4);
standing_z = standing_gsensor(start_skip:end_index,5);
standing(:,1) = standing_t;
standing(:,2) = standing_x;
standing(:,3) = standing_y;
standing(:,4) = standing_z;

[y x] = size(walking_into_wall_gsensor);
end_index = y - end_skip;
wall_t = walking_into_wall_gsensor(start_skip:end_index,2);
wall_x = walking_into_wall_gsensor(start_skip:end_index,3);
wall_y = walking_into_wall_gsensor(start_skip:end_index,4);
wall_z = walking_into_wall_gsensor(start_skip:end_index,5);
wall(:,1) = wall_t;
wall(:,2) = wall_x;
wall(:,3) = wall_y;
wall(:,4) = wall_z;

window_size = 125;

				# We need to calculate features for
				# learning. The most obvious thing to
				# look at is mean magnitude over various
				# time windows. Let's look at the past
				# 5, 10, 15, 20, and 25 frames.
				#We approximate that the mean of each
				#window is the "zero" of the wave and
				#subtract it off in order to find the
				#magnitude at each point.

[y x] = size(wall);

data = zeros(window_size, 3);

history_mean = zeros(1, 3);
history_sum = zeros(1, 3);
history_xx_sum = 0;
history_yy_sum = 0;
history_zz_sum = 0;
history_xy_sum = 0;
history_xz_sum = 0;
history_yz_sum = 0;

data_index = 1;


for i = 1:1:y
  accel = wall(i, 2:4);
				# Update the mean of our window
  
  history_mean = history_mean .- (data(data_index,:) / window_size);
  history_mean = history_mean .+ (accel / window_size);
  
# We also track the sum of each variable.
  history_sum = history_sum .- data(data_index, :);
  history_sum = history_sum .+ accel;

# And we track the sum of a bunch of products.
  history_xx_sum = history_xx_sum - data(data_index, 1)*data(data_index, 1);
  history_yy_sum = history_yy_sum - data(data_index, 2)*data(data_index, 2);
  history_zz_sum = history_zz_sum - data(data_index, 3)*data(data_index, 3);
  history_xy_sum = history_xy_sum - data(data_index, 1)*data(data_index, 2);
  history_xz_sum = history_xz_sum - data(data_index, 1)*data(data_index, 3);
  history_yz_sum = history_yz_sum - data(data_index, 2)*data(data_index, 3);

  history_xx_sum = history_xx_sum + accel(1)*accel(1);
  history_yy_sum = history_yy_sum + accel(2)*accel(2);
  history_zz_sum = history_zz_sum + accel(3)*accel(3);
  history_xy_sum = history_xy_sum + accel(1)*accel(2);
  history_xz_sum = history_xz_sum + accel(1)*accel(3);
  history_yz_sum = history_yz_sum + accel(2)*accel(3);

				# calculate variance
# var = sum((x - xmean)^2)/n
# = sum(x^2 - 2*x*xmean + xmean^2)/n
# = (sum(x^2) - 2*xmean*sum(x) + n*xmean^2)/n

  accel_var(1,1) = history_xx_sum;
  accel_var(1,2) = history_yy_sum;
  accel_var(1,3) = history_zz_sum;
  accel_var = accel_var - 2 .* history_mean .* history_sum;
  accel_var = accel_var + window_size .* history_mean .* history_mean;
  accel_var = accel_var / (window_size - 1);
  
# Correlation:
# c = sum((x - xmean)(y - ymean))/(sqrt(xvar*yvar)*n)
# The denominator is easy to calculate here since we've got variances
# numerator = sum((x - xmean)(y - ymean))
# = sum(xy - y*xmean - x*ymean + xmean*ymean)
# = sum(xy) - xmean*sum(y) - ymean*sum(x) + n*xmean*ymean

				# XY corr
  denom = (window_size - 1)*sqrt(accel_var(1) * accel_var(2));

  if denom > 0.0000001
    xy_cor = history_xy_sum;
    xy_cor = xy_cor - history_mean(1)*history_sum(2);
    xy_cor = xy_cor - history_mean(2)*history_sum(1);
    xy_cor = xy_cor + history_mean(1)*history_mean(2)*window_size;
    xy_cor = xy_cor/denom;
  else
    xy_cor = 0;
  end

				# XZ corr
  denom = (window_size - 1)*sqrt(accel_var(1) * accel_var(3));

  if denom > 0.0000001
    xz_cor = history_xz_sum;
    xz_cor = xz_cor - history_mean(1)*history_sum(3);
    xz_cor = xz_cor - history_mean(3)*history_sum(1);
    xz_cor = xz_cor + history_mean(1)*history_mean(3)*window_size;
    xz_cor = xz_cor/denom;
  else
    xz_cor = 0;
  end

				# YZ corr
  denom = (window_size - 1)*sqrt(accel_var(2) * accel_var(3));

  if denom > 0.0000001
    yz_cor = history_yz_sum;
    yz_cor = yz_cor - history_mean(2)*history_sum(3);
    yz_cor = yz_cor - history_mean(3)*history_sum(2);
    yz_cor = yz_cor + history_mean(2)*history_mean(3)*window_size;
    yz_cor = yz_cor/denom;
  else
    yz_cor = 0;
  end

				# Write to data arrays
  data(data_index, :) = accel;

  data_index = data_index + 1;
  if data_index > window_size
    data_index = 1;
  end

				# We should save our feature vector
  wall_out(i,1:3) = accel_var;
  wall_out(i,4) = xy_cor;
  wall_out(i,5) = xz_cor;
  wall_out(i,6) = yz_cor;
  
end

[y x] = size(standing);

data = zeros(window_size, 3);

history_mean = zeros(1, 3);
history_sum = zeros(1, 3);
history_xx_sum = 0;
history_yy_sum = 0;
history_zz_sum = 0;
history_xy_sum = 0;
history_xz_sum = 0;
history_yz_sum = 0;

data_index = 1;


for i = 1:1:y
  accel = standing(i, 2:4);
				# Update the mean of our window
  
  history_mean = history_mean .- (data(data_index,:) / window_size);
  history_mean = history_mean .+ (accel / window_size);
  
# We also track the sum of each variable.
  history_sum = history_sum .- data(data_index, :);
  history_sum = history_sum .+ accel;

# And we track the sum of a bunch of products.
  history_xx_sum = history_xx_sum - data(data_index, 1)*data(data_index, 1);
  history_yy_sum = history_yy_sum - data(data_index, 2)*data(data_index, 2);
  history_zz_sum = history_zz_sum - data(data_index, 3)*data(data_index, 3);
  history_xy_sum = history_xy_sum - data(data_index, 1)*data(data_index, 2);
  history_xz_sum = history_xz_sum - data(data_index, 1)*data(data_index, 3);
  history_yz_sum = history_yz_sum - data(data_index, 2)*data(data_index, 3);

  history_xx_sum = history_xx_sum + accel(1)*accel(1);
  history_yy_sum = history_yy_sum + accel(2)*accel(2);
  history_zz_sum = history_zz_sum + accel(3)*accel(3);
  history_xy_sum = history_xy_sum + accel(1)*accel(2);
  history_xz_sum = history_xz_sum + accel(1)*accel(3);
  history_yz_sum = history_yz_sum + accel(2)*accel(3);

				# calculate variance
# var = sum((x - xmean)^2)/n
# = sum(x^2 - 2*x*xmean + xmean^2)/n
# = (sum(x^2) - 2*xmean*sum(x) + n*xmean^2)/n

  accel_var(1, 1) = history_xx_sum;
  accel_var(1, 2) = history_yy_sum;
  accel_var(1, 3) = history_zz_sum;
  accel_var = accel_var - 2 .* history_mean .* history_sum;
  accel_var = accel_var + window_size .* history_mean .* history_mean;
  accel_var = accel_var / (window_size - 1);
  
# Correlation:
# c = sum((x - xmean)(y - ymean))/(sqrt(xvar*yvar)*n)
# The denominator is easy to calculate here since we've got variances
# numerator = sum((x - xmean)(y - ymean))
# = sum(xy - y*xmean - x*ymean + xmean*ymean)
# = sum(xy) - xmean*sum(y) - ymean*sum(x) + n*xmean*ymean

				# XY corr
  denom = (window_size - 1)*sqrt(accel_var(1) * accel_var(2));

  if denom > 0.0000001
    xy_cor = history_xy_sum;
    xy_cor = xy_cor - history_mean(1)*history_sum(2);
    xy_cor = xy_cor - history_mean(2)*history_sum(1);
    xy_cor = xy_cor + history_mean(1)*history_mean(2)*window_size;
    xy_cor = xy_cor/denom;
  else
    xy_cor = 0;
  end

				# XZ corr
  denom = (window_size - 1)*sqrt(accel_var(1) * accel_var(3));

  if denom > 0.0000001
    xz_cor = history_xz_sum;
    xz_cor = xz_cor - history_mean(1)*history_sum(3);
    xz_cor = xz_cor - history_mean(3)*history_sum(1);
    xz_cor = xz_cor + history_mean(1)*history_mean(3)*window_size;
    xz_cor = xz_cor/denom;
  else
    xz_cor = 0;
  end

				# YZ corr
  denom = (window_size - 1)*sqrt(accel_var(2) * accel_var(3));

  if denom > 0.0000001
    yz_cor = history_yz_sum;
    yz_cor = yz_cor - history_mean(2)*history_sum(3);
    yz_cor = yz_cor - history_mean(3)*history_sum(2);
    yz_cor = yz_cor + history_mean(2)*history_mean(3)*window_size;
    yz_cor = yz_cor/denom;
  else
    yz_cor = 0;
  end

				# Write to data arrays
  data(data_index, :) = accel;

  data_index = data_index + 1;
  if data_index > window_size
    data_index = 1;
  end

				# We should save our feature vector
  standing_out(i,1:3) = accel_var;
  standing_out(i,4) = xy_cor;
  standing_out(i,5) = xz_cor;
  standing_out(i,6) = yz_cor;
  
end

[y x] = size(playing);

data = zeros(window_size, 3);

history_mean = zeros(1, 3);
history_sum = zeros(1, 3);
history_xx_sum = 0;
history_yy_sum = 0;
history_zz_sum = 0;
history_xy_sum = 0;
history_xz_sum = 0;
history_yz_sum = 0;

data_index = 1;


for i = 1:1:y
  accel = playing(i, 2:4);
				# Update the mean of our window
  
  history_mean = history_mean .- (data(data_index,:) / window_size);
  history_mean = history_mean .+ (accel / window_size);
  
# We also track the sum of each variable.
  history_sum = history_sum .- data(data_index, :);
  history_sum = history_sum .+ accel;

# And we track the sum of a bunch of products.
  history_xx_sum = history_xx_sum - data(data_index, 1)*data(data_index, 1);
  history_yy_sum = history_yy_sum - data(data_index, 2)*data(data_index, 2);
  history_zz_sum = history_zz_sum - data(data_index, 3)*data(data_index, 3);
  history_xy_sum = history_xy_sum - data(data_index, 1)*data(data_index, 2);
  history_xz_sum = history_xz_sum - data(data_index, 1)*data(data_index, 3);
  history_yz_sum = history_yz_sum - data(data_index, 2)*data(data_index, 3);

  history_xx_sum = history_xx_sum + accel(1)*accel(1);
  history_yy_sum = history_yy_sum + accel(2)*accel(2);
  history_zz_sum = history_zz_sum + accel(3)*accel(3);
  history_xy_sum = history_xy_sum + accel(1)*accel(2);
  history_xz_sum = history_xz_sum + accel(1)*accel(3);
  history_yz_sum = history_yz_sum + accel(2)*accel(3);

				# calculate variance
# var = sum((x - xmean)^2)/n
# = sum(x^2 - 2*x*xmean + xmean^2)/n
# = (sum(x^2) - 2*xmean*sum(x) + n*xmean^2)/n

  accel_var(1, 1) = history_xx_sum;
  accel_var(1, 2) = history_yy_sum;
  accel_var(1, 3) = history_zz_sum;
  accel_var = accel_var - 2 .* history_mean .* history_sum;
  accel_var = accel_var + window_size .* history_mean .* history_mean;
  accel_var = accel_var / (window_size - 1);
  
# Correlation:
# c = sum((x - xmean)(y - ymean))/(sqrt(xvar*yvar)*n)
# The denominator is easy to calculate here since we've got variances
# numerator = sum((x - xmean)(y - ymean))
# = sum(xy - y*xmean - x*ymean + xmean*ymean)
# = sum(xy) - xmean*sum(y) - ymean*sum(x) + n*xmean*ymean

				# XY corr
  denom = (window_size - 1)*sqrt(accel_var(1) * accel_var(2));

  if denom > 0.0000001
    xy_cor = history_xy_sum;
    xy_cor = xy_cor - history_mean(1)*history_sum(2);
    xy_cor = xy_cor - history_mean(2)*history_sum(1);
    xy_cor = xy_cor + history_mean(1)*history_mean(2)*window_size;
    xy_cor = xy_cor/denom;
  else
    xy_cor = 0;
  end

				# XZ corr
  denom = (window_size - 1)*sqrt(accel_var(1) * accel_var(3));

  if denom > 0.0000001
    xz_cor = history_xz_sum;
    xz_cor = xz_cor - history_mean(1)*history_sum(3);
    xz_cor = xz_cor - history_mean(3)*history_sum(1);
    xz_cor = xz_cor + history_mean(1)*history_mean(3)*window_size;
    xz_cor = xz_cor/denom;
  else
    xz_cor = 0;
  end

				# YZ corr
  denom = (window_size - 1)*sqrt(accel_var(2) * accel_var(3));

  if denom > 0.0000001
    yz_cor = history_yz_sum;
    yz_cor = yz_cor - history_mean(2)*history_sum(3);
    yz_cor = yz_cor - history_mean(3)*history_sum(2);
    yz_cor = yz_cor + history_mean(2)*history_mean(3)*window_size;
    yz_cor = yz_cor/denom;
  else
    yz_cor = 0;
  end

				# Write to data arrays
  data(data_index, :) = accel;

  data_index = data_index + 1;
  if data_index > window_size
    data_index = 1;
  end

				# We should save our feature vector
  playing_out(i,1:3) = accel_var;
  playing_out(i,4) = xy_cor;
  playing_out(i,5) = xz_cor;
  playing_out(i,6) = yz_cor;
  
end

[y x] = size(hooked);

data = zeros(window_size, 3);

history_mean = zeros(1, 3);
history_sum = zeros(1, 3);
history_xx_sum = 0;
history_yy_sum = 0;
history_zz_sum = 0;
history_xy_sum = 0;
history_xz_sum = 0;
history_yz_sum = 0;

data_index = 1;

for i = 1:1:y
  accel = hooked(i, 2:4);
				# Update the mean of our window
  
  history_mean = history_mean .- (data(data_index,:) / window_size);
  history_mean = history_mean .+ (accel / window_size);
  
# We also track the sum of each variable.
  history_sum = history_sum .- data(data_index, :);
  history_sum = history_sum .+ accel;

# And we track the sum of a bunch of products.
  history_xx_sum = history_xx_sum - data(data_index, 1)*data(data_index, 1);
  history_yy_sum = history_yy_sum - data(data_index, 2)*data(data_index, 2);
  history_zz_sum = history_zz_sum - data(data_index, 3)*data(data_index, 3);
  history_xy_sum = history_xy_sum - data(data_index, 1)*data(data_index, 2);
  history_xz_sum = history_xz_sum - data(data_index, 1)*data(data_index, 3);
  history_yz_sum = history_yz_sum - data(data_index, 2)*data(data_index, 3);

  history_xx_sum = history_xx_sum + accel(1)*accel(1);
  history_yy_sum = history_yy_sum + accel(2)*accel(2);
  history_zz_sum = history_zz_sum + accel(3)*accel(3);
  history_xy_sum = history_xy_sum + accel(1)*accel(2);
  history_xz_sum = history_xz_sum + accel(1)*accel(3);
  history_yz_sum = history_yz_sum + accel(2)*accel(3);

				# calculate variance
# var = sum((x - xmean)^2)/n
# = sum(x^2 - 2*x*xmean + xmean^2)/n
# = (sum(x^2) - 2*xmean*sum(x) + n*xmean^2)/n

  accel_var(1, 1) = history_xx_sum;
  accel_var(1, 2) = history_yy_sum;
  accel_var(1, 3) = history_zz_sum;
  accel_var = accel_var - 2 .* history_mean .* history_sum;
  accel_var = accel_var + window_size .* history_mean .* history_mean;
  accel_var = accel_var / (window_size - 1);
  
# Correlation:
# c = sum((x - xmean)(y - ymean))/(sqrt(xvar*yvar)*n)
# The denominator is easy to calculate here since we've got variances
# numerator = sum((x - xmean)(y - ymean))
# = sum(xy - y*xmean - x*ymean + xmean*ymean)
# = sum(xy) - xmean*sum(y) - ymean*sum(x) + n*xmean*ymean

				# XY corr
  denom = (window_size - 1)*sqrt(accel_var(1) * accel_var(2));

  if denom > 0.0000001
    xy_cor = history_xy_sum;
    xy_cor = xy_cor - history_mean(1)*history_sum(2);
    xy_cor = xy_cor - history_mean(2)*history_sum(1);
    xy_cor = xy_cor + history_mean(1)*history_mean(2)*window_size;
    xy_cor = xy_cor/denom;
  else
    xy_cor = 0;
  end

				# XZ corr
  denom = (window_size - 1)*sqrt(accel_var(1) * accel_var(3));

  if denom > 0.0000001
    xz_cor = history_xz_sum;
    xz_cor = xz_cor - history_mean(1)*history_sum(3);
    xz_cor = xz_cor - history_mean(3)*history_sum(1);
    xz_cor = xz_cor + history_mean(1)*history_mean(3)*window_size;
    xz_cor = xz_cor/denom;
  else
    xz_cor = 0;
  end

				# YZ corr
  denom = (window_size - 1)*sqrt(accel_var(2) * accel_var(3));

  if denom > 0.0000001
    yz_cor = history_yz_sum;
    yz_cor = yz_cor - history_mean(2)*history_sum(3);
    yz_cor = yz_cor - history_mean(3)*history_sum(2);
    yz_cor = yz_cor + history_mean(2)*history_mean(3)*window_size;
    yz_cor = yz_cor/denom;
  else
    yz_cor = 0;
  end

				# Write to data arrays
  data(data_index, :) = accel;

  data_index = data_index + 1;
  if data_index > window_size
    data_index = 1;
  end

				# We should save our feature vector
  hooked_out(i,1:3) = accel_var;
  hooked_out(i,4) = xy_cor;
  hooked_out(i,5) = xz_cor;
  hooked_out(i,6) = yz_cor;
  
end

save wall_out.txt wall_out;
save hooked_out.txt hooked_out;
save playing_out.txt playing_out;
save standing_out.txt standing_out;
