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);
t = (0:12:(length(sideTilt) - 1) * 12);
t1 = t1 * t(length(t))/t1(length(t1));
PD = 1/1000;

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;
plot(t1, (sideTiltC)/(1023/2), 'g');

figure(2);
subplot(3,1,1);
rFoot = rFI|rFO|rBI|rBO;
lFoot = lFI|lFO|lBI|lBO;

figure(3);
subplot(2,1,1);
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--');


figure(4);
%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;
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), 'k');
%plot(t, (frontAve - 1023/2)/(1023/2), 'g');
%plot(t, (frontE)/(1023/2), 'c');
plot(t, ((rHipBE - 128)*(100/(-133)))/180 - 0.8, 'r');
plot(t, ((lHipBE - 128)*(100/(133)))/180 - 0.8, 'r--');

figure(3);
hold off;
subplot(2,1,1);
plot(t1*PD,rHipB,'b');
hold on;
plot(t1*PD,lHipB,'r--');
grid on;
title('Hip Bend');
xlabel('Time (s)');
ylabel('degrees (^\circ)');
legend('right hip','left hip',2);

figure(4);
hold off;
subplot(2,1,1);
plot(t1*PD,rKnee,'b');
hold on;
plot(t1*PD,lKnee,'r--');
grid on;
title('Knee');
xlabel('Time (s)');
ylabel('degrees (^\circ)');
legend('right knee','left knee',2);

figure(5);
hold off;
subplot(2,1,1);
plot(t1*PD,rAnkle,'b');
hold on;
plot(t1*PD,lAnkle,'r--');
grid on;
title('Ankle');
xlabel('Time (s)');
ylabel('degrees (^\circ)');
legend('right ankle','left ankle',2);

figure(6);
hold off;
subplot(2,1,1);
plot(t1*PD,torso,'b');
grid on;
title('Torso');
xlabel('Time (s)');
ylabel('degrees (^\circ)');



figure(7);
subplot(numPlots, 1, 1), plot(t,rFI), grid, title('Right Front Inner');
subplot(numPlots, 1, 2), plot(t,rFO), grid, title('Right Front Outer');
subplot(numPlots, 1, 3), plot(t,rBI), grid, title('Right Back Inner');
subplot(numPlots, 1, 4), plot(t,rBO), grid, title('Right Back Outer');
subplot(numPlots, 1, 5), plot(t,lFI), grid, title('Left Front Inner');
subplot(numPlots, 1, 6), plot(t,lFO), grid, title('Left Front Outer');
subplot(numPlots, 1, 7), plot(t,lBI), grid, title('Left Front Inner');
subplot(numPlots, 1, 8), plot(t,lBO), grid, title('Left Front Outer');






