% Detecta o impacto total do pé no solo

function detect_impact(t,pressure,current)

% Cálculo da média
press=sum(pressure')/size(pressure,2);
press=press';

% Cálculo do número de amostras para cálculo da variação
% (0.2s definido para uma velocidade de 100 -> periodo=2 segundos)
delta_t=t(end)/length(press);
n_eval=floor(0.2/delta_t);

% Variação da pressão
press_var=zeros(size(press));
for i=n_eval+1:length(press)
    press_var(i)=press(i)-press(i-n_eval);
end

% Sinal esperado (-m*x^2+b)
peak_duration=0.4;
peak_amplitude=30;

% Cálculo dos parametros do sinal esperado
b=peak_amplitude;
m=b/(peak_duration/2)^2;
h=-m*([0:delta_t:peak_duration]-peak_duration/2).^2+b;
% figure(1)
% plot(h,'.');
% title('Sinal esperado');

% Cálculo da convolução
convolution=zeros(size(press_var));
for i=1:length(press_var)
    cnv=0;
    for j=1:length(h)
        if (i-j+1<=0), continue, end
        cnv=cnv+h(j)*press_var(i-j+1);
    end
    convolution(i)=cnv;
end

% subplot(2,2,1)
% plot(t,pressure,'.');
% axis([0 t(end) 100 220]);
% legend('sensor 1','sensor 2','sensor 3','sensor 4');
% title('Sensores de força individuais');
% grid on

%subplot(2,2,2);
plot(t,press_var,'.');
axis([0 t(end) -30 30])
title('Variação da média dos sensores de força');
grid on

% subplot(2,2,3)
% plot(t,press,'.');
% axis([0 t(end) 100 220]);
% title('Média dos sensores de força');
% grid on
% 
% subplot(2,2,4)
% plot(convolution);
% axis([0 length(convolution) -1000 14000])
% title('Matched filter com o sinal esperado (-x^2+b)');
% grid on

% subplot(4,1,1)
% plot(t,pressure,'.');
% axis([0 t(end) 100 220]);
% legend('sensor 1','sensor 2','sensor 3','sensor 4');
% title('Sensores de força individuais');
% grid on
% 
% subplot(4,1,2)
% plot(t,press,'.');
% axis([0 t(end) 100 220]);
% title('Média dos sensores de força');
% grid on
% 
% subplot(4,1,3)
% plot(t,current(:,:,1),'s');
% axis([0 t(end) 0 100]);
% legend('Motor ortogonal ao pe','Motor paralelo ao pe','Motor do Joelho')
% title('Pulso de corrente do 1º Microcontrolador');
% grid on
% 
% subplot(4,1,4)
% plot(t,current(:,:,2),'s');
% axis([0 t(end) 0 100]);
% legend('Motor ortogonal ao pe','Motor paralelo ao pe','Motor do Joelho')
% title('Pulso de corrente do 2º Microcontrolador');
% grid on
