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);
sideTiltC = data_angles(:,14);
frontTiltC = data_angles(:,15);

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);
front = data(:,11);
frontAve = data(:,12);
frontE = data(:,13);
rHipBE = data(:,14);
lHipBE = data(:,15);
PD = 1/1000;
t = (0:12:(length(sideTilt) - 1) * 12)*PD;
t1 = t1 * t(length(t))/t1(length(t1));

figure(1);
%plot(t,(sideTilt - 1023/2)/(1023/2),'b'), 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
subplot(2,1,1);
plot(t,(sideTiltD - 1023/2)/(1023/2),'r');
hold on; grid on;
plot(t1, (sideTiltC)/(1023/2), 'g');
ylabel('acceleration (g)');
legend('filtered actual','desired',3);
title('Side acceleration');

figure(2);
subplot(4,1,1);
rFoot = rFI|rFO|rBI|rBO;
lFoot = lFI|lFO|lBI|lBO;
plot(t, -0.6*rFoot + 0.8, 'b');
hold on;grid on;
plot(t, -0.6*lFoot -0.2, 'b--');
ylabel('position (up/down)');
legend('right foot','left foot',2);
title('Foot-ground contact');

figure(3);
subplot(2,1,1);
plot(t1, rHipB, 'b');
hold on;grid on;
plot(t1, lHipB, 'b--');
plot(t, ((rHipBE - 128)*(100/(-133))), 'r');
plot(t, ((lHipBE - 128)*(100/(133))), 'r--');
xlabel('time (s)');
ylabel('angle (^\circ)');
legend('right hip','left hip','right hip corrected','left hip corrected',2);
title('Hip bend');

figure(4);
subplot(2,1,1);
%plot(t,(frontTilt- 1023/2)/(1023/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');
hold on;grid on;
%plot(t, -0.1*rFoot + 0.8, 'g');
%plot(t, -0.1*lFoot + 0.6, 'g--');
%plot(t1, rHipB/180 - 0.8, 'c');
%plot(t1, lHipB/180 - 0.8, 'c--');
plot(t, (front - 1023/2)/(1023/2), 'g');
%plot(t, (frontAve - 1023/2)/(1023/2), 'g');
%plot(t, (frontE)/(1023/2), 'c');
ylabel('acceleration (g)');
legend('filtered actual','desired',2);
title('Front acceleration');







