close all
clear
clc

format long

senVal = csvread('senvall.csv',0,8,[0,8,5267,8]);

x=0:1:size(senVal,1)-1;

hold on
plot(x,senVal)

Fs=1400;         %sample frequency
t=0:1/Fs:4;      %time instants

% ESTES SAO OS MELHORES VALORES!!!
order=5;      %filter order
cutf=10;      %cut off freq (Hz)
%cutatten=10;  %cut off attenuation (dB)
%ripple=3;     %allowed ripple in the passband (dB)

% IIR with other values
order2=5;      %filter order
cutf2=11;      %cut off freq (Hz)

% IIR with other values
order3=5;      %filter order
cutf3=9;      %cut off freq (Hz)


% IIR - non linear phase. Can be unstable: adapt order and cutf
[B1,A1] = butter(order,cutf/(Fs/2),'low');
y1=filter(B1,A1,senVal);

[B2,A2] = butter(order2,cutf2/(Fs/2),'low');
y2=filter(B2,A2,senVal);  

[B3,A3] = butter(order3,cutf3/(Fs/2),'low');
y3=filter(B3,A3,senVal); 


%FIR
order4=30;      %filter order
cutf4=10;      %cut off freq (Hz)
%FIR -stable minimum phase
[B4] = fir1(order4,cutf4/(Fs/2),'low');
y4=filter(B4,1,senVal);

plot(x,y1)
plot(x,y2)
plot(x,y3)
%plot(x,y4)

legend('Raw data',['IIR order=' num2str(order) ' cutoff=' num2str(cutf)], ['IIR order=' num2str(order2) ' cutoff=' num2str(cutf2)] ,['IIR order=' num2str(order3) ' cutoff=' num2str(cutf3)])%,['FIR order=' num2str(order4) ' cutoff=' num2str(cutf4)]) 
%legend('Raw data','IIR order=5 cutoff=10','FIR order=30 cutoff=10')
xlabel('Sample')
ylabel('Force [N]')

b1=0.052958197653552e-7;   
b2=0.264790988267760e-7;
b3=0.529581976535520e-7;
b4=0.529581976535520e-7;
b5=0.264790988267760e-7;
b6=0.052958197653552e-7;

a1=1.000000000000000;
a2=-4.854769163220606;
a3=9.429566799621668;
a4=-9.159620169686651;
a5=4.449629234379628;
a6=-0.864806531627807;

realvalues=[0 0 0 0 0 0];
filtervalues=[0 0 0 0 0 0];
%Y=[0 0 0 0 0];

for n=6:size(senVal)
     realvalues=[realvalues(2) realvalues(3) realvalues(4) realvalues(5) realvalues(6) senVal(n)];
    
     newvalue=b1*realvalues(6) + b2*realvalues(5) + b3*realvalues(4) + b4*realvalues(3) + b5*realvalues(2) + b6*realvalues(1) - a2*filtervalues(6) - a3*filtervalues(5) -a4*filtervalues(4) - a5*filtervalues(3) - a6*filtervalues(2);
    
     %Y=[Y newvalue];
     
     filtervalues=[filtervalues(2) filtervalues(3) filtervalues(4) filtervalues(5) filtervalues(6) newvalue];
end

figure
hold on
plot(x,senVal)
plot(x,y1)
plot(x,Y)
legend('Raw data','Matlab filter','Implemented filter')
xlabel('Sample')
ylabel('Force [N]')