%% Analyse data from ultrasound sensors

close all; clear all; clc;

%% H=170; V=10; H=4; Robot in the middle; Object in the center; 2Way

[time,s1,s2,s3] = textread('170_10_4_c_co_2way','%d %f %f %f','headerlines',1);

s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;

windowSize = 10;

tmax=max(time);
s1=filter(ones(1,windowSize)/windowSize,1,s1);
s2=filter(ones(1,windowSize)/windowSize,1,s2);
s3=filter(ones(1,windowSize)/windowSize,1,s3);

subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance');
legend('LEFT')
axis([0 tmax 0 270]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('H=170; V=10; H=4; Robot in the middle; Object in the center');
xlabel('time');
ylabel('distance');
legend('CENTER')
axis([0 tmax 0 270]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance');
legend('RIGHT')
axis([0 tmax 0 270]);


% % subplot(1,3,1); 
% figure(1); plot(time, s1, 'b');
% % subplot(1,3,2); 
% figure(2); plot(time, s2, 'r');
% % subplot(1,3,3); 
% figure(3); plot(time, s3, 'g');
% %plot(time, s1, 'b'); hold on; plot(time, s2, 'r');plot(time, s3, 'g');

%% H=170; V=10; H=4; Robot in the right side; Object in the right; 2Way

[time,s1,s2,s3] = textread('170_10_4_r_ro','%d %f %f %f','headerlines',1);

s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;

windowSize = 10;

tmax=max(time);
s1=filter(ones(1,windowSize)/windowSize,1,s1);
s2=filter(ones(1,windowSize)/windowSize,1,s2);
s3=filter(ones(1,windowSize)/windowSize,1,s3);


subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance');
legend('LEFT')
axis([0 tmax 0 270]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('H=170; V=10; H=4; Robot in the right side; Object in the right');
xlabel('time');
ylabel('distance');
legend('CENTER')
axis([0 tmax 0 270]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance');
legend('RIGHT')
axis([0 tmax 0 270]);


% % subplot(1,3,1); 
% figure(1); plot(time, s1, 'b');
% % subplot(1,3,2); 
% figure(2); plot(time, s2, 'r');
% % subplot(1,3,3); 
% figure(3); plot(time, s3, 'g');
% %plot(time, s1, 'b'); hold on; plot(time, s2, 'r');plot(time, s3, 'g');


%% H=170; V=10; H=4; Robot in the right side; Object in the left; 2Way

[time,s1,s2,s3] = textread('170_10_4_r_lo','%d %f %f %f','headerlines',1);


s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;

windowSize = 10;

tmax=max(time);
s1=filter(ones(1,windowSize)/windowSize,1,s1);
s2=filter(ones(1,windowSize)/windowSize,1,s2);
s3=filter(ones(1,windowSize)/windowSize,1,s3);


subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance');
legend('LEFT')
axis([0 tmax 0 270]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('H=170; V=10; H=4; Robot in the right side; Object in the left');
xlabel('time');
ylabel('distance');
legend('CENTER')
axis([0 tmax 0 270]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance');
legend('RIGHT')
axis([0 tmax 0 270]);


% % subplot(1,3,1); 
% figure(1); plot(time, s1, 'b');
% % subplot(1,3,2); 
% figure(2); plot(time, s2, 'r');
% % subplot(1,3,3); 
% figure(3); plot(time, s3, 'g');
% %plot(time, s1, 'b'); hold on; plot(time, s2, 'r');plot(time, s3, 'g');


%% H=170; V=10; H=4; Robot in the left side; Object in the left; 2Way

[time,s1,s2,s3] = textread('170_10_4_l_lo','%d %f %f %f','headerlines',1);


s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;

windowSize = 10;

tmax=max(time);
s1=filter(ones(1,windowSize)/windowSize,1,s1);
s2=filter(ones(1,windowSize)/windowSize,1,s2);
s3=filter(ones(1,windowSize)/windowSize,1,s3);

subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance');
legend('LEFT')
axis([0 tmax 0 270]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('H=170; V=10; H=4; Robot in the left side; Object in the left');
xlabel('time');
ylabel('distance');
legend('CENTER')
axis([0 tmax 0 270]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance');
legend('RIGHT')
axis([0 tmax 0 270]);


% % subplot(1,3,1); 
% figure(1); plot(time, s1, 'b');
% % subplot(1,3,2); 
% figure(2); plot(time, s2, 'r');
% % subplot(1,3,3); 
% figure(3); plot(time, s3, 'g');
% %plot(time, s1, 'b'); hold on; plot(time, s2, 'r');plot(time, s3, 'g');


%% H=170; V=10; H=4; Robot in the left side; Object in the right; 2Way

[time,s1,s2,s3] = textread('170_10_4_l_ro','%d %f %f %f','headerlines',1);

s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;

windowSize = 10;

tmax=max(time);
s1=filter(ones(1,windowSize)/windowSize,1,s1);
s2=filter(ones(1,windowSize)/windowSize,1,s2);
s3=filter(ones(1,windowSize)/windowSize,1,s3);


subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance');
legend('LEFT')
axis([0 tmax 0 270]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('H=170; V=10; H=4; Robot in the left side; Object in the right');
xlabel('time');
ylabel('distance');
legend('CENTER')
axis([0 tmax 0 270]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance');
legend('RIGHT')
axis([0 tmax 0 270]);


% % subplot(1,3,1); 
% figure(1); plot(time, s1, 'b');
% % subplot(1,3,2); 
% figure(2); plot(time, s2, 'r');
% % subplot(1,3,3); 
% figure(3); plot(time, s3, 'g');
% %plot(time, s1, 'b'); hold on; plot(time, s2, 'r');plot(time, s3, 'g');

