load data_angles.gg;
t = data_angles(:,1);
rHipT = data_angles(:,2);
rHipB = data_angles(:,3);
rKnee = data_angles(:,4);
rAnkle = data_angles(:,5);
torso = data_angles(:,6);
lAnkle = data_angles(:,7);
lKnee = data_angles(:,8);
lHipB = data_angles(:,9);
lHipT = data_angles(:,10);

for i = 1:length(t) 
  t1(i) = sum(t(2:i)); 
end

load data;
numAcc = 2;
numPlots = 8;
smooth = 20;
sideTilt = data(:,2);
frontTilt = data(:,1);
rFI = data(:,(numAcc+1)+4);
rFO = data(:,(numAcc+1)+5);
rBI = data(:,(numAcc+1)+6);
rBO = data(:,(numAcc+1)+7);
lFI = data(:,(numAcc+1)+1);
lFO = data(:,(numAcc+1)+0);
lBI = data(:,(numAcc+1)+3);
lBO = data(:,(numAcc+1)+2);
t = (0:12:(length(sideTilt) - 1) * 12);


figure(1);
grid on, title('Side Tilt');
hold on;
delay = 0;
for i = 1:length(sideTilt)
  delay = delay + sideTilt(i);
  if i > smooth
    delay = delay - sideTilt(i - smooth);
    sideTiltD(i) = delay/smooth;
   else sideTiltD(i) = delay/i;
  end
end
plot(t,(sideTiltD - 1023/2)/(1023/2),'r');
plot(t1, (sideTiltC)/(1023/2), 'g');
rFoot = rFI|rFO|rBI|rBO;
lFoot = lFI|lFO|lBI|lBO;
plot(t, -0.1*rFoot + 0.8, 'g');
plot(t, -0.1*lFoot + 0.6, 'k');
plot(t1, rHipB/180 - 0.8, 'c');
plot(t1, lHipB/180 - 0.8, 'c--');

figure(2);
grid on, title('Front Tilt');
hold on;
delay = 0;
for i = 1:length(frontTilt)
  delay = delay + frontTilt(i);
  if i > smooth
    delay = delay - frontTilt(i - smooth);
    frontTiltD(i) = delay/smooth;
   else frontTiltD(i) = delay/i;
  end
end
plot(t,(frontTiltD - 1023/2)/(1023/2),'r');
plot(t1, (frontTiltC)/(1023/2), 'g');
plot(t, -0.1*rFoot + 0.8, 'g');
plot(t, -0.1*lFoot + 0.6, 'k');
plot(t1, rHipB/180 - 0.8, 'c');
plot(t1, lHipB/180 - 0.8, 'c--');

