%%Clean
clc, clear all, close all;

global qplo;
%%Read file
M= csvread('shoe_left'); 

%%stp: número de passos
%%on_off: 0 = all steps; 1 = 1 step; 2= intrevalo de steps
stp=18; on_off= 2; whs=10; s_1 = 4; s_2 = 7; 


%%Data size
n= size(M,1);

%%Calibration Var left shoe
a0 = 0.0005; a1 = 0.0003731; a2 = 0.0005; a3= 0.0005;
a4 = 1.0963; a5 = 1.0958; a6 = 1.0845; a7 = 1.0809;
b0 = -0.00004; b1 = 0.000049; b2 = 0.0001; b3 = 0.00001;
b4 = 65.881; b5 = 69.702; b6 = 66.179; b7 = 63.568;

%%Calibration Var right shoe
% a0 = 0.1716; a1 = 0.1888; a2 = 0.1991 ; a3= 0.2211;
% a4 = 1.1869 ; a5 = 1.4093; a6 = 1.1358; a7 = 1.1358 ;
% b0 = 580 ; b1 =507.5 ; b2 = 507.5; b3 =474 ;
% b4 = 12.377; b5 = 8.8946 ; b6 = 33.055 ; b7 = 33.055 ;

%%Data extraction

for i=1:n
    F2(i)=M(i,5);
    F4(i)=M(i,8);
    F6(i)=M(i,2)-M(1,2);
    F7(i)=M(i,6)-M(1,6);
    F3(i)=M(i,7);
    F1(i)=M(i,3);
    F5(i)=M(i,4)-M(1,4);
    F8(i)=M(i,9)-M(1,9);
   
    r = M(end,10)-M(1,10);
    az(i) = M(i,1);
end

%rate
rate = (n)/r;
imp_aux= ['Rate:  ' ,num2str(rate), ' Hz'];
disp(imp_aux); 

%dados perdidos
dados_perdidos = ((az(n)-n)/n)*100;
dados_per_aux = [ 'Erro[%]: ', num2str(dados_perdidos)];
disp(dados_per_aux);

%%Time
%Time_send = ((Time_send - Time_send(1))/1000000);
%Time_send(1,end) = Time_send(1,end-1);
nn=size(F1,2);
Time_send1 = 0:(1/100):(nn/100);
Time_send = Time_send1(1,1:end-1);

%%Force calculation
Vout0 = (F1+100) * (5.0 / 1023.0); Vout1 = F2 * (5.0 / 1023.0); Vout2 = F3 * (5.0 / 1023.0); Vout3 = F4 * (5.0 / 1023.0);
for k=1:nn
 Rfsr0(k) = (6000/Vout0(k))-1200;
 Rfsr1(k) = (6000/Vout1(k))- 1200;
Rfsr2(k) = (6000/Vout2(k))- 1200;
  Rfsr3(k)= (6000/Vout3(k))- 1200;
end
for h=1:nn
 F0a(h) = ((1/Rfsr0(h)) - b0) *(4.44822162825/a0);
 F1a(h) = ((1/Rfsr1(h)) - b1) *(4.44822162825/a1);
F2a(h) = ((1/Rfsr2(h)) - b2) *(4.44822162825/a2);
 F3a(h) = ((1/Rfsr3(h)) - b3) *(4.44822162825/a3); 
end
F4a = ((F5-b4)/a4)/100; F5a = ((F6-b5)/a5)/100; F6a = ((F7-b6)/a6)/100; F7a = ((F8-b7)/a7)/100;

%if right foot

%F0a = ((F1-b0)/a0)/100; F1a = ((F2-b1)/a1)/100; F2a = ((F3-b2)/a2)/100; F3a = ((F4-b3)/a3)/100;
%F4a = ((F5-b4)/a4)/100; F5a = ((F6-b5)/a5)/100; F6a = ((F7-b6)/a6)/100; F7a = ((F8-b7)/a7)/100;

%%fn, fhmov
fn=(F0a+F1a+F2a+F3a);
fhmov=+F4a*0.7071+F5a*0.7071+F6a*0.7071+F7a*0.7071;

%%which plot qplo = fn or fhmov
qplo = fn;

subplot(2,2,1); plot(Time_send, qplo); hold on; plot(Time_send, qplo, '.'); 

%%Extract steps
a=1; x=-1; j=1; z=0; b=0; indzero=find(fn(a:end)<1 & fn(a:end)>-1); a1=indzero(1);
for g=1:1:stp
    while x==-1
        x=indzero(1,j)-indzero(1,j+1); j=j+1; z=z+1; %n de zeros (pontos - pé no ar)
    end
    y=indzero(1,j-1)-indzero(1,j); a=a1+b+z; c=a-y-1;
    if g ~=1 %step index
        I(1,g)=a+1; %start step
        F(1,g)=c; %end step
    end
    b=c; z=0; x=-1; a1=0;
end

%%Remove ZEROS
bb=1;
for hhh=1:1:size(I,2)
    if F(1,hhh)-I(1,hhh) > 0 
        II(1,bb) = I(1,hhh); FF(1,bb) = F(1,hhh); bb=bb+1;    
    end
end

%%check 'steps' lenght
jg=size(FF,2);
for kh=1:1:jg;
gj(kh)=FF(kh)-II(kh);
end

%%Look for steps with lenght between xx-xx %%%
baba=1;
for jl=1:1:jg
    if (gj(jl) == 84 || gj(jl) == 85 )
        III(1,baba) = II(1,jl); FFF(1,baba) = FF(1,jl); baba=baba+1;
    end
end

%%Make all steps ht same lenght
jg11=FFF-III; mini=min(FFF-III);
for kh=1:1:size(jg11,2)
    if(jg11(kh) == mini+1 )
        III(kh) = III(kh)+1;
    end
end

%%steps indexs 
%all steps xx=1:1:size(III,2)
%which step -. ws=N;

if(on_off == 1) ws= whs; end
if(on_off == 0) ws=1:1:size(III,2); end
if(on_off == 2) ws= s_1:1:s_2; end
for xx=ws;
    Y=fn(III(1,xx):FFF(1,xx));
    an(:,:,xx)=Y;
end

%%assembly steps vari: assem
cll=size(an,2)*size(an,3); assem(1:cll)=an(1,:,:);

%%plot all steps mpl=1:1:size(III,2)
%%plot 1 step ws=N
for mpl=ws;
subplot(2,2,2); plot(Time_send(III(mpl):FFF(mpl)), qplo(III(mpl):FFF(mpl))); hold on; 
end

%%fft
L=(mini+1)*size(an,3); %Signal lenght
Fs=mini+1; %sampling frequency
bbb = detrend(assem); %remove 1st peak
res = fft(bbb); %fft
P1=abs(res/L); 
f=Fs*(0:(L-1))/L;
subplot(2,2,3); plot(f,P1); hold on; 


%%step fftmax
[maxmax, Indd]=max(P1); %MAX
[ndmax, inddnd] = max(P1(P1<max(P1))); %2ND MAX
[ndmax1, inddnd1] = max(P1(P1<ndmax));%3rd max
[ndmax2, inddnd2] = max(P1(P1<ndmax1)); %4th max
%%correlation coef between steps, FFT each step

for xx=1:1:size(III,2)
    %%subplot(2,2,4);  plot(Time_send(II(1,xx):FF(1,xx)),fn(II(1,xx):FF(1,xx)));hold on;; hold on; 
    Y=fft(fn(III(1,xx):FFF(1,xx)));
    en(:,:,xx)=Y;
end

for cc0=1:1:size(III,2)-1;
    for cc1=1:1:size(III,2)-1;
    relat(cc0,cc1)=corr2(abs(en(:,:,cc0)),abs(en(:,:,cc1)));
    end
end

%%phase angle
X_phase = angle(res);
X_phase(inddnd+1);

subplot(2,2,4); plot(f,X_phase); hold on; 

%%Disp ans
disp('Max: '); disp(maxmax);
disp('2nd_Max: '); disp(ndmax);
disp('3rd_Max: '); disp(ndmax1);
disp('4th_Max: '); disp(ndmax2);
disp('angle_Max: '); disp(X_phase(Indd));