clc; clear all; close all; format longG
%% data NGTR
car="UFR";
numberOfStints=3;
fuel=81/3*2; %in [%]
tank=60; %in [l]
wr="1:46.68";
wrUFR=laptime_to_seconds(wr);
laps=99;
tracklength=5.1;

[UFRrace, UFRpit, UFRpara, UFRcov] = estimatedRaceTime(car,laps,wr,numberOfStints,fuel,tank)

car="XFR";
% numberOfStints=2;
fuel=90/3*2; %in [%]
tank=45; %in [l]
wr="1:46.69";
wrXFR=laptime_to_seconds(wr);
% laps=57;

[XFRrace, XFRpit, XFRpara, XFRcov] = estimatedRaceTime(car,laps,wr,numberOfStints,fuel,tank)

dNGTrace=UFRrace-XFRrace
dNGTpit=UFRpit-XFRpit
dNGTtotal=dNGTrace+dNGTpit

%% data LRF
car="LX6";
% numberOfStints=3;
fuel=85; %in [%]
tank=40; %in [l]
wr="1:52.90";
wrLX6=laptime_to_seconds(wr);
% laps=57;

[LX6race, LX6pit, LX6para, LX6cov] = estimatedRaceTime(car,laps,wr,numberOfStints,fuel,tank)
ERTLX6=LX6race+LX6pit

car="RAC"
% numberOfStints=3;
fuel=87; %in [%]
tank=42; %in [l]
wr="1:53.18";
wrRAC=laptime_to_seconds(wr);
% laps=57;

[RACrace, RACpit, RACpara, RACcov] = estimatedRaceTime(car,laps,wr,numberOfStints,fuel,tank)
ERTRAC=RACrace+RACpit
dRAC=ERTRAC-ERTLX6

car="FZ5"
% numberOfStints=3;
fuel=80; %in [%]
tank=90; %in [l]
wr="1:52.50";
wrFZ5=laptime_to_seconds(wr);
% laps=57;

[FZ5race, FZ5pit, FZ5para, FZ5cov] = estimatedRaceTime(car,laps,wr,numberOfStints,fuel,tank)
ERTFZ5=FZ5race+FZ5pit
dFZ5=ERTFZ5-ERTLX6


%% plot
varPlotX=0:round(laps/3);
varPlotX=varPlotX*tracklength;
varPlotUFR=UFRpara(1)*varPlotX.^2+UFRpara(2)*varPlotX+UFRpara(3);
varPlotUFR=varPlotUFR*wrUFR;
varPlotXFR=XFRpara(1)*varPlotX.^2+XFRpara(2)*varPlotX+XFRpara(3);
varPlotXFR=varPlotXFR*wrXFR;
figure
hold all
varPlotX=varPlotX/tracklength;
plot(varPlotX,varPlotUFR,"DisplayName",'UFR')
plot(varPlotX,varPlotXFR,"DisplayName",'XFR')
varPlotX=0:round(laps/3);
varPlotX=varPlotX*tracklength;
varPlotLX6=LX6para(1)*varPlotX.^2+LX6para(2)*varPlotX+LX6para(3);
varPlotLX6=varPlotLX6*wrLX6;
varPlotRAC=RACpara(1)*varPlotX.^2+RACpara(2)*varPlotX+RACpara(3);
varPlotRAC=varPlotRAC*wrRAC;
varPlotFZ5=FZ5para(1)*varPlotX.^2+FZ5para(2)*varPlotX+FZ5para(3);
varPlotFZ5=varPlotFZ5*wrFZ5;
varPlotX=varPlotX/tracklength;
plot(varPlotX,varPlotLX6,"DisplayName",'LX6')
plot(varPlotX,varPlotRAC,"DisplayName",'RAC')
plot(varPlotX,varPlotFZ5,"DisplayName",'FZ5')
title('model lap time comparison KY2r')
grid on
xlabel('laps')
ylabel('laptime [s]')
legend
hold off

%% plot 2
varPlotX=1:laps;
varPlotX=varPlotX*tracklength;

for k=1:laps
    
    %UFR
    varPlotUFR=UFRpara(1)*varPlotX(mod(k,29)+1).^2+...
        UFRpara(2)*varPlotX(mod(k,29)+1)+UFRpara(3);
    varPlotUFR=varPlotUFR*wrUFR;
    if k==1
        RaceGraphUFR(k)=varPlotUFR;
    elseif k==round(laps/3) || k==round(laps*2/3)
        RaceGraphUFR(k)=RaceGraphUFR(k-1)+varPlotUFR+39/2+21/2;
    else
        RaceGraphUFR(k)=RaceGraphUFR(k-1)+varPlotUFR;
    end
   
        
    %XFR
    varPlotXFR=XFRpara(1)*varPlotX(mod(k,29)+1).^2+...
        XFRpara(2)*varPlotX(mod(k,29)+1)+XFRpara(3);
    varPlotXFR=varPlotXFR*wrXFR;
    if k==1
        RaceGraphXFR(k)=varPlotXFR;
    elseif k==round(laps/3) || k==round(laps*2/3)
        RaceGraphXFR(k)=RaceGraphXFR(k-1)+varPlotXFR+37/2;
    else
        RaceGraphXFR(k)=RaceGraphXFR(k-1)+varPlotXFR;
    end
    
    %LX6
    varPlotLX6=LX6para(1)*varPlotX(mod(k,19)+1).^2+...
        LX6para(2)*varPlotX(mod(k,19)+1)+LX6para(3);
    varPlotLX6=varPlotLX6*wrLX6;
    if k==1
        RaceGraphLX6(k)=varPlotLX6;
    elseif k==round(laps/3) || k==round(laps*2/3)
        RaceGraphLX6(k)=RaceGraphLX6(k-1)+varPlotLX6+40/2+65/2;
    else
        RaceGraphLX6(k)=RaceGraphLX6(k-1)+varPlotLX6;
    end
    
    %RAC
    varPlotRAC=RACpara(1)*varPlotX(mod(k,19)+1).^2+...
        RACpara(2)*varPlotX(mod(k,19)+1)+RACpara(3);
    varPlotRAC=varPlotRAC*wrRAC;
    if k==1
        RaceGraphRAC(k)=varPlotRAC;
    elseif k==round(laps/3) || k==round(laps*2/3)
        RaceGraphRAC(k)=RaceGraphRAC(k-1)+varPlotRAC+41/2;
    else
        RaceGraphRAC(k)=RaceGraphRAC(k-1)+varPlotRAC;
    end
    
    %FZ5
    varPlotFZ5=FZ5para(1)*varPlotX(mod(k,19)+1).^2+...
        FZ5para(2)*varPlotX(mod(k,19)+1)+FZ5para(3);
    varPlotFZ5=varPlotFZ5*wrFZ5;
    if k==1
        RaceGraphFZ5(k)=varPlotFZ5;
    elseif k==round(laps/3) || k==round(laps*2/3)
        RaceGraphFZ5(k)=RaceGraphFZ5(k-1)+varPlotFZ5+55/2+12/2;
    else
        RaceGraphFZ5(k)=RaceGraphFZ5(k-1)+varPlotFZ5;
    end
end

varPlotX=varPlotX/tracklength;

figure
subplot(2,1,1)
hold all
plot(varPlotX,RaceGraphUFR-RaceGraphUFR,"DisplayName",'UFR')
plot(varPlotX,RaceGraphXFR-RaceGraphUFR,"DisplayName",'XFR')
grid on
legend
suptitle('race graph of the model laptimes with pitstop balancing')
title('NGTR class')
xlabel('laps')
ylabel('gap to UFR [s]')
hold off
subplot(2,1,2)
hold all
plot(varPlotX,RaceGraphLX6-RaceGraphLX6,"DisplayName",'LX6')
plot(varPlotX,RaceGraphRAC-RaceGraphLX6,"DisplayName",'RAC')
plot(varPlotX,RaceGraphFZ5-RaceGraphLX6,"DisplayName",'FZ5')
grid on
legend
title('LRF class')
xlabel('laps')
ylabel('gap to LX6 [s]')
hold off