%% Data from ultrasound sensors

close all; clear all; clc;


%% No filter 74cm

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

s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;
time=linspace(1, length(s1), length(s1));
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);

x_min=0;
x_max=tmax;
y_min=0;
y_max=100;

subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor1')
axis([x_min x_max y_min y_max]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('No filter - object at 74cm');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor2')
axis([x_min x_max y_min y_max]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor3')
axis([x_min x_max y_min y_max]);

%% Filter 74cm

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

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

s1=s1(1:1500);
s2=s2(1:1500);
s3=s3(1:1500);

time=linspace(1, length(s1), length(s1));
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);

x_min=0;
x_max=tmax;
y_min=40;
y_max=100;

subplot(1,3,1); plot(time, s1, 'b');
xlabel('Iterações');
ylabel('Distância (cm)');
legend('Sensor1')
axis([x_min x_max y_min y_max]);
subplot(1,3,2); plot(time, s2, 'r');
%TITLE('Filter - object at 74cm');
TITLE('Obstáculo a 74cm');
xlabel('Iterações');
ylabel('Distância (cm)');
legend('Sensor2')
axis([x_min x_max y_min y_max]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('Iterações');
ylabel('Distância (cm)');
legend('Sensor3')
axis([x_min x_max y_min y_max]);


%% No filter 134cm

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

s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;
time=linspace(1, length(s1), length(s1));
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);

x_min=0;
x_max=tmax;
y_min=0;
y_max=150;

subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor1')
axis([x_min x_max y_min y_max]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('No filter - object at 134cm');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor2')
axis([x_min x_max y_min y_max]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor3')
axis([x_min x_max y_min y_max]);
%% Filter 134cm

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

s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;
time=linspace(1, length(s1), length(s1));
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);

x_min=0;
x_max=tmax;
y_min=0;
y_max=150;

subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor1')
axis([x_min x_max y_min y_max]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('Filter - object at 134cm');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor2')
axis([x_min x_max y_min y_max]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor3')
axis([x_min x_max y_min y_max]);

%% No filter 194cm

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

s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;
time=linspace(1, length(s1), length(s1));
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);

x_min=0;
x_max=tmax;
y_min=0;
y_max=250;

subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor1')
axis([x_min x_max y_min y_max]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('No filter - object at 194cm');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor2')
axis([x_min x_max y_min y_max]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor3')
axis([x_min x_max y_min y_max]);

%% Filter 194cm

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

s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;
time=linspace(1, length(s1), length(s1));
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);

x_min=0;
x_max=tmax;
y_min=0;
y_max=250;

subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor1')
axis([x_min x_max y_min y_max]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('Filter - object at 194cm');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor2')
axis([x_min x_max y_min y_max]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor3')
axis([x_min x_max y_min y_max]);

%% No filter movement

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

s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;
time=linspace(1, length(s1), length(s1));
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);

x_min=0;
x_max=tmax;
y_min=0;
y_max=300;

subplot(1,3,1); plot(time, s1, 'b');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor1')
axis([x_min x_max y_min y_max]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('No filter - object moving');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor2')
axis([x_min x_max y_min y_max]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('time');
ylabel('distance (cm)');
legend('Sensor3')
axis([x_min x_max y_min y_max]);

%% Filter movement

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

s1(s1==0)=NaN;
s2(s2==0)=NaN;
s3(s3==0)=NaN;
time=linspace(1, length(s1), length(s1));
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);

% s1=s1(1000:3700);
% s2=s2(1000:3700);
% s3=s3(1000:3700);



x_min=0;
x_max=tmax;
y_min=60;
y_max=220;

subplot(1,3,1); plot(time, s1, 'b');
xlabel('Iterações');
ylabel('Distância (cm)');
legend('Sensor1')
axis([x_min x_max y_min y_max]);
subplot(1,3,2); plot(time, s2, 'r');
TITLE('Obstáculo em movimento');
xlabel('Iterações');
ylabel('Distância (cm)');
legend('Sensor2')
axis([x_min x_max y_min y_max]);
subplot(1,3,3); plot(time, s3, 'g');
xlabel('Iterações');
ylabel('Distância (cm)');
legend('Sensor3')
axis([x_min x_max y_min y_max]);
