

close all;
clear all;
clc;






%%colocar aqui parametros:

% tempo de espera para saber se já começou a manobra
%time_wait_half=1;
time_wait_end=1.2;



%txt_open='salvado_6.txt';
 
%txt_open='andre_6.txt';
 

txt_open='parking_andre_6.txt';
%txt_open='parking_salvado_2.txt'; %descartar talhada_1


 fid = fopen(txt_open,'rt');
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% inicio das variaveis %%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% disp(txt_open)

%contem o numero maximo de linhas do ficheiro
 nlines =0;
while (fgets(fid) ~= -1),
  nlines = nlines+1;
end
%nLines
 fclose(fid);
 

%ciclo para construir os vectores a tratar
for nt=1:nlines
    
    [names, bag_time, ignition, light_left, light_right, throtle, brake, clutch, velocity, steering, rpm] = textread(txt_open,'%s\t%f\t%d\t%d\t%d\t%d\t%d\t%d\t%f\t%f\t%f', nt) ;

    
end

for n=1:nlines
    if(velocity(n)>0 && velocity(n)<0.25)
        velocity(n)=0;
    end
    
    if(steering(n)==0)
        steering(n)=mean([steering(n-1) steering(n)]);  
    end
    
    if(rpm(n)==0)
        rpm(n)=mean([rpm(n-1) rpm(n)]);  
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%inicio de tratamento de dados %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%definição da contagem do tempo desde zero até ao final do ficheiro
set_time_zero=bag_time(1);
for n=1:nlines
    time_sec(n)=bag_time(n)-set_time_zero;
end




%calculo do tempo final da manobra
init_man=0;
half_man=0;
half_end_man=0;

man_state=0;


state_1=0;
state_2=0;
state_3=0;
state_4=0;
state_5=0;

sit_1=0;
sit_2=0;
sit_3=0;
sit_4=0;


line_end=0;
last_time_move=0;
for hi=1:nlines
    
    %calculo do tempo em que o carro começa a estacionar, inicio de manobra.
    
    %se for menor que zero epera um certo numero de valores para confirmar
    
    %que começou a manobra
    if(velocity(hi)<0)
       init_man=init_man+1; 
    
       if (init_man==1)
           begin_man_time=time_sec(hi);
           sit_1=hi
           state_2=1; %variavel que diz se a manobra começou ou não
        end
    end
   
    
    
    
    % meio da manobra
    if(state_2==1 && state_3==0)
        
        if(velocity(hi)==0)
           half_man=half_man+1;
       

            if (half_man==1) %numero de vezes para confirmar que parou!depois de ter andado marcha a tras
               hold_man_time=time_sec(hi);
               sit_2=hi
               state_3=1; %variavel que diz se a manobra começou ou não
            end
        end
    end
    
    
    
     if(state_3==1 && state_4==0 && velocity(hi)~=0)
          sit_3=line_stop
          state_4=1;
    end
     
    
    
    
    % Verificação de que o carro está parado desde um certo tempo.
    
     if(state_4==1 && velocity(hi)==0 && state_5==0) %Se está parado depois de ter iniciado a manobra?
            sit_4=line_move
            state_5=1; 
     end
         
    
    
    
    if(velocity(hi)==0 ) %só entra neste ciclo para saber quando o carro começou a estar parado
        last_time_stop=time_sec(hi);
        line_stop=hi;
    end

    if(velocity(hi)~=0 ) %só entra neste ciclo para saber quando o carro começou a estar parado
        half_end_man=half_end_man+1;
        last_time_move=time_sec(hi);
        line_move=hi;
    end
    
end





%

% calculo da velocidade média,mediana e tempo, entre cada percurso


clc;


Tempo_1=time_sec(sit_2)-time_sec(sit_1);
Tempo_2=time_sec(sit_3)-time_sec(sit_2);
Tempo_3=time_sec(sit_4)-time_sec(sit_3);
Tempo_total=Tempo_1+Tempo_2+Tempo_3;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%% Velocidades %%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% média das velocidades
vel_1_2=mean(velocity(sit_1:sit_2));
vel_3_4=mean(velocity(sit_3:sit_4));

% mediana das velocidades
vel_1_2_med=median(velocity(sit_1:sit_2));
min_vel_1_2=min(velocity(sit_1:sit_2));


vel_3_4_med=median(velocity(sit_3:sit_4));
max_vel_3_4=max(velocity(sit_3:sit_4));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%% Angulos %%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ang_med_1_2=median(steering(sit_1:sit_2));
ang_med_2_3=median(steering(sit_2:sit_3));
ang_med_3_4=median(steering(sit_3:sit_4));

max_ang_1_2=max(steering(sit_1:sit_2));
max_ang_2_3=max(steering(sit_2:sit_3));
max_ang_3_4=max(steering(sit_3:sit_4));

min_ang_1_2=min(steering(sit_1:sit_2));
min_ang_2_3=min(steering(sit_2:sit_3));
min_ang_3_4=min(steering(sit_3:sit_4));


ang_var_1_2=var(steering(sit_1:sit_2));
ang_var_2_3=var(steering(sit_2:sit_3));
ang_var_3_4=var(steering(sit_3:sit_4));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% RPM %%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rpm_med_1_2=median(rpm(sit_1:sit_2));
rpm_med_2_3=median(rpm(sit_2:sit_3));
rpm_med_3_4=median(rpm(sit_3:sit_4));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%% parametro vel*ang %%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


ang_vel_1_2=steering(sit_1:sit_2).*velocity(sit_1:sit_2);
ang_vel_3_4=steering(sit_3:sit_4).*velocity(sit_3:sit_4);


ang_vel_med_1_2=median(ang_vel_1_2);
ang_vel_med_3_4=median(ang_vel_3_4);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%% Pedais %%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

c_throtle_l=0;
c_throtle_l_m=0;
c_throtle_m_h=0;
c_throtle_h=0;

count_clutch=0;

c_brake_l=0;
c_brake_m=0;
c_brake_h=0;



for hut=sit_1:sit_4
    
     % pedal dio acelerador
     if (throtle(hut)>30 && throtle(hut)<100)
         c_throtle_l=c_throtle_l+1;
     end
     
     if (throtle(hut)>=100 && throtle(hut)<200)
         c_throtle_l_m=c_throtle_l_m+1;
     end
     
     
     if (throtle(hut)>=200 && throtle(hut)<300)
         c_throtle_m_h=c_throtle_m_h+1;
     end
     
     if (throtle(hut)>=300 )
         c_throtle_h=c_throtle_h+1;
     end
     
     % pedal da embreagem
     if (clutch(hut)>500)
         count_clutch=count_clutch+1;
     end
     
     
     
     %pedal do travão
     if (brake(hut)>360 && brake(hut)<450)
         c_brake_l=c_brake_l+1;
     end
     
     
     if (brake(hut)>=450 && brake(hut)<550)
         c_brake_m=c_brake_m+1;
     end
     
     if (brake(hut)>=550 )
         c_brake_h=c_brake_h+1;
     end
     
 end


c_throtle_l=c_throtle_l/Tempo_total
c_throtle_l_m=c_throtle_l_m/Tempo_total
c_throtle_m_h=c_throtle_m_h/Tempo_total
c_throtle_h=c_throtle_h/Tempo_total
 
count_clutch=count_clutch/Tempo_total
 
c_brake_l=c_brake_l/Tempo_total
c_brake_m=c_brake_m/Tempo_total
c_brake_h=c_brake_h/Tempo_total







%time with lights on
blinking_time=0;
last_blink=0;
for hu=sit_1:sit_2
    
    if(light_right(hu)==1)
       blinking_time=blinking_time+1;
       last_blink=hu;
    end
end

blink_right_time=0;
if(last_blink~=0)
    blink_right_time= blinking_time/Tempo_1;
end




% gráficos!!

%ficheiro com a informação dos bags
txt_base= strcat( txt_open(1:(length(txt_open)-5)),'base.txt');

 fid2 = fopen(txt_base,'a+');
 
 n2lines = 0;
while (fgets(fid2) ~= -1),
  n2lines = n2lines+1;
end
n2lines;


%  if(n2lines==0)
%      fprintf(fid2,' Tempo_total\t Tempo_1\t Tempo_2\t Tempo_3\t vel_1_2_med\t vel_3_4_med\t rpm_med_1_2\t rpm_med_2_3\t rpm_med_3_4\t ang_vel_med_1_2\t ang_vel_med_3_4\t ang_med_2_3\t ang_var_2_3\t blink_right_time \n');
%  end
                                                                          %
 fprintf(fid2,'\n%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f', Tempo_total,Tempo_1,Tempo_2,Tempo_3,vel_1_2_med,   vel_3_4_med,rpm_med_1_2,rpm_med_2_3,rpm_med_3_4,ang_vel_med_1_2,    ang_vel_med_3_4,ang_med_2_3,ang_var_2_3,blink_right_time,      c_throtle_l,c_throtle_l_m,c_throtle_m_h,c_throtle_h,count_clutch,c_brake_l,c_brake_m,c_brake_h );

 fclose(fid2);


%%

x = mvnrnd([0;0], [1 .9;.9 1], 100);
       y = [1 1;1 -1;-1 1;-1 -1];
       MahalDist = mahal(y,x)
       sqEuclidDist = sum((y - repmat(mean(x),4,1)).^2, 2)
       plot(x(:,1),x(:,2),'b.',y(:,1),y(:,2),'ro')


