%tento soubor lze spustit
fis=readfis("FuzzyTempomat.fis"); %nacteni fuzzy systemu

n=400; % počet kroků (40 sekund)
t=0.1; % délka kroku 0,1 sekundy
v0=120 % pocatecni rychlost vozidla s tempomatem
vzd0=80 % pocatecni vzdalenost vozidel

vp=zeros(n,1) % rychlost - predchozi vozidlo
vp(1)=120 % pocatecni rychlost predchoziho vozidlo
for j=2:(n/2)    % v dalsich dvou cyklech lze upravit prubeh rychlosti predchoziho vozidla
    if(vp(j-1)<135)
        vp(j)=vp(j-1);
    else
        vp(j)=vp(j-1);
    end
end
for j=(n/2)+1:n
    if(vp(j-1)<135)
        vp(j)=vp(j-1);
    else
        vp(j)=vp(j-1);
    end
end

sklon=zeros(n,1); % sklon vozovky
sklon(1)=0 % pocatecni sklon vozovky
for j=2:n
    sklon(j)=sklon(j-1);
end
theta=zeros(n,1)
for j=1:n
    theta(j)=atand(sklon(j)/100);
end

v=zeros(n,1)
v(1)=v0;
vzd=zeros(n,1)
vzd(1)=vzd0;
m=1400; % uvazovana hmotnost vozidla
Frr=206; % uvazovany valivy odpor
g=9.81 % gravitacni zrychleni
output=zeros(n,1) % pole reakcí fuzzy systému
for i=1:n 
    input=[v(i) vp(i) vzd(i)]; 
    output(i) = evalfis(fis,input)/100; % vystup systemu pro dalsi ucely delime 100
    
    Fg=m*g*sind(theta(i)); % slozka gravitacni sila - zavisi na sklonu
    Fair=0.5*(v(i)/3.6)^2; % odpor vzduchu - zavisi na rychlosti
    if(output(i)<=0) % zaporny vystup -> brzdeni
        F=output(i)*7000-Frr-Fair-Fg; %prepocitame vystup na brzdnou silu
    else %zrychlovani
        F=output(i)*4900-Frr-Fair-Fg; %prepocitame vystup na pohonnou silu
    end
    a=F/m; %zrychleni k urceni rychlosti v dalsim kroku
    
    v(i+1)=(v(i)/3.6)+(a*t); % rychlost v dalsim kroku
    if(v(i+1)<0)
        v(i+1)=0;
    end
    v(i+1)=v(i+1)*3.6;
    
    vzd(i+1)=vzd(i)+(vp(i)/3.6)*t-((((v(i+1)+v(i))/3.6)/2)*t); %vzdalenost vozidel v dalsim kroku  
end
% vykresleni grafu
bezpvzd=zeros(n,1);
bezpvzd=(v./3.6)*2;
bezpvzd2=zeros(n,1);
bezpvzd2=(v./3.6)*5;
figure(1)
plot(v, 'DisplayName','rychlost - fuzzy adaptivní tempomat')
hold on
plot(vp, 'DisplayName','rychlost předchozího')
xlabel('čas (krok 0,1 sekundy)')
ylabel('rychlost [km/h]')
legend('location', 'best');
ylim([50 150])
figure(2)
plot(vzd, 'DisplayName','vzdálenost')
ylim([0 100])
hold on
plot(bezpvzd, 'DisplayName','vzdálenost - odstup 2 sekundy')
xlabel('čas (krok 0,1 sekundy)')
ylabel('vzdálenost [m]') 
legend('location', 'best');
figure(3)
plot(output)
xlabel('čas (krok 0,1 sekundy)')
ylabel('reakce fuzzy adaptivního tempomatu')
