clear all
close all
clc
set(0,'defaultTextInterpreter','latex');


%% FILE DESCRIPTION

% This file is used to simulate the sOC parameters, treated as random
% variables over a given population of missions. It also compute the
% distributions for the composite random variables used to classify an OC
% according to the GTA and UFD descriptions.

% REFERENCES

% [1] L Romano, M Godio, P Johannesson, F Bruzelius, T Ghandriz, and B
% Jacobson. "Development of an operating cycle for long-haul heavy-duty
% trucks: theory, analysis and applications". Submitted for publication.

% Please, cite the above references when using this script for purposes of
% academic or industrial research


%% INITIALISATION

% Define the size for the population of road missions
dim = 10000;


%% STOCHASTIC VARIABLES
% 1 = Urban, 2 = Rural, 3 = Highway

% Model for road type probability matrix

% First row
pd_alpha12 = makedist('Gamma','a',0.85,'b',1);
pd_alpha13 = makedist('Gamma','a',0.1,'b',1);
% Second row
pd_alpha21 = makedist('Gamma','a',0.59,'b',1);
pd_alpha23 = makedist('Gamma','a',0.48,'b',1);
% Third row
pd_alpha31 = makedist('Gamma','a',0.14,'b',1);
pd_alpha32 = makedist('Gamma','a',1.18,'b',1);


% Model for road type length L_R
pd_L_R1 = makedist('Lognormal','mu',0.15,'sigma',0.59);
pd_L_R2 = makedist('Lognormal','mu',1.48,'sigma',0.93);
pd_L_R3 = makedist('Lognormal','mu',1.18,'sigma',1.30);

% Model for topography variance
pd_Sigma_Y1 = makedist('Lognormal','mu',0.74,'sigma',0.43);
pd_Sigma_Y2 = makedist('Lognormal','mu',0.50,'sigma',0.36);
pd_Sigma_Y3 = makedist('Lognormal','mu',0.34,'sigma',0.49);

% Model for mean hill lenghts L_h
pd_L_h1 = makedist('Lognormal','mu',5.63,'sigma',0.3);
pd_L_h2 = makedist('Lognormal','mu',6.63,'sigma',0.37);
pd_L_h3 = makedist('Lognormal','mu',6.47,'sigma',0.52);

% Model for curve intensity lambda_C
pd_lambda_C1 = makedist('Lognormal','mu',1.76,'sigma',0.5);
pd_lambda_C2 = makedist('Lognormal','mu',0.29,'sigma',0.85);
pd_lambda_C3 = makedist('Lognormal','mu',-0.64,'sigma',1.02);

% Model for curve log-mean radius mu_C
pd_mu_C1 = makedist('Normal','mu',4.27,'sigma',0.39);
pd_mu_C2 = makedist('Normal','mu',4.86,'sigma',0.3);
pd_mu_C3 = makedist('Normal','mu',5.07,'sigma',0.39);

% Model for log-std radius sigma_C
pd_sigma_C1 = makedist('Lognormal','mu',0.02,'sigma',0.14);
pd_sigma_C2 = makedist('Lognormal','mu',-0.11,'sigma',0.22);
pd_sigma_C3 = makedist('Lognormal','mu',-0.29,'sigma',0.43);

% Model for curve log-mean length mu_L
pd_mu_L1 = makedist('Normal','mu',3.92,'sigma',0.20);
pd_mu_L2 = makedist('Normal','mu',3.91,'sigma',0.22);
pd_mu_L3 = makedist('Normal','mu',3.79,'sigma',0.22);

% Model for log-std length sigma_L
pd_sigma_L1 = makedist('Lognormal','mu',-0.45,'sigma',0.22);
pd_sigma_L2 = makedist('Lognormal','mu',-0.57,'sigma',0.29);
pd_sigma_L3 = makedist('Lognormal','mu',-0.74,'sigma',0.48);

% Model for stop signs
% Note: the waiting times are not needed for classification since only the
% mean number of stops is considered
pd_lambda_s1 = makedist('Lognormal','mu',-0.09,'sigma',0.72);
pd_lambda_s2 = makedist('Lognormal','mu',-2.94,'sigma',1.14);
pd_lambda_s3 = makedist('Lognormal','mu',-3.71,'sigma',1.36);

T_min1_pdf = makedist('Gamma','a',0.631812025519393,'b',33.491216377703300);
T_min2_pdf = makedist('Gamma','a',0.790387960260550,'b',24.826543755612303);
T_min3_pdf = makedist('Gamma','a',0.692105951942917,'b',46.528663547188440);

Delta_T1_pdf = makedist('Gamma','a',0.642369309651950,'b',4.039812330670097e+02);
Delta_T2_pdf = makedist('Gamma','a',0.431644010142896,'b',1.759906975636135e+02);
Delta_T3_pdf = makedist('Gamma','a',0.529482437536209,'b',57.262776919060556);

% Speed signs urban first row
pd_alpha12_R1 = makedist('Gamma','a',0.39,'b',1);
pd_alpha13_R1 = makedist('Gamma','a',0.42,'b',1);
% Speed signs rural second row
pd_alpha21_R1 = makedist('Gamma','a',0.36,'b',1);
pd_alpha23_R1 = makedist('Gamma','a',0.37,'b',1);
% Speed signs urban third row
pd_alpha31_R1 = makedist('Gamma','a',0.30,'b',1);
pd_alpha32_R1 = makedist('Gamma','a',0.36,'b',1);


% Model for mean speed length (urban)
pd_L_V1_R1 = makedist('Lognormal','mu',-0.79,'sigma',0.60);
pd_L_V2_R1 = makedist('Lognormal','mu',-0.82,'sigma',0.45);
pd_L_V3_R1 = makedist('Lognormal','mu',-0.44,'sigma',0.39);

% Speed signs urban first row
pd_alpha12_R2 = makedist('Gamma','a',0.33,'b',1);
pd_alpha13_R2 = makedist('Gamma','a',0.28,'b',1);
% Speed signs rural second row
pd_alpha21_R2 = makedist('Gamma','a',0.48,'b',1);
pd_alpha23_R2 = makedist('Gamma','a',1.03,'b',1);
% Speed signs urban third row
pd_alpha31_R2 = makedist('Gamma','a',0.35,'b',1);
pd_alpha32_R2 = makedist('Gamma','a',1.22,'b',1);


% Model for mean speed length (rural)
pd_L_V1_R2 = makedist('Lognormal','mu',-0.14,'sigma',0.46);
pd_L_V2_R2 = makedist('Lognormal','mu',0.20,'sigma',0.59);
pd_L_V3_R2 = makedist('Lognormal','mu',1.06,'sigma',0.91);

% Model for mean speed length (highway)
pd_L_V1_R3 = makedist('Lognormal','mu',1.14,'sigma',1.27);
pd_L_V2_R3 = makedist('Lognormal','mu',0.38,'sigma',0.67);

% Model for mission length
pd_Lm = makedist('Gamma','a',1.31,'b',62.5);


% Volvo GTA
y_max1 = 3;
y_max2 = 6;
y_max3 = 9;
% SCANIA UFD
y_max = 2;


kappa = 0.008;
r_turn = 12.5;


%% ROAD TYPES

% Each row of the matrix P_R behaves as a Dirichlet random variable. To
% generate Dirichlet distributions, we first generate Gamma distributed
% variables, and then we use these to compute the Dirichlet variables
y_12 = random(pd_alpha12,dim,1);
y_13 = random(pd_alpha13,dim,1);
p_R12 = y_12./(y_12+y_13);
p_R13 = y_13./(y_12+y_13);

y_21 = random(pd_alpha21,dim,1);
y_23 = random(pd_alpha23,dim,1);
p_R21 = y_21./(y_21+y_23);
p_R23 = y_23./(y_21+y_23);

y_31 = random(pd_alpha31,dim,1);
y_32 = random(pd_alpha32,dim,1);
p_R31 = y_31./(y_31+y_32);
p_R32 = y_32./(y_31+y_32);

% Generate the stochastic mean road lengths
% Note: I am multiplying by 1000 to express the lengths in m, but this is
% ininfluent on the values of Pi_Ri, i = 1,...,3.
L_R1 = random(pd_L_R1,dim,1)*1000;
L_R2 = random(pd_L_R2,dim,1)*1000;
L_R3 = random(pd_L_R3,dim,1)*1000;

% Compute the stochastic stationary distributions (Eqs. (29) in paper)
Pi_R1 = L_R1.*(p_R21.*p_R32+p_R31)./(L_R1.*(p_R21.*p_R32+p_R31) + ...
    L_R2.*(p_R12.*p_R31 + p_R32) + L_R3.*(1-p_R12.*p_R21));

Pi_R2 = L_R2.*(p_R12.*p_R31 + p_R32)./(L_R1.*(p_R21.*p_R32+p_R31) + ...
    L_R2.*(p_R12.*p_R31 + p_R32) + L_R3.*(1-p_R12.*p_R21));

Pi_R3 = L_R3.*(1-p_R12.*p_R21)./(L_R1.*(p_R21.*p_R32+p_R31) + ...
    L_R2.*(p_R12.*p_R31 + p_R32) + L_R3.*(1-p_R12.*p_R21));


%% TOPOGRAPHY 

% Generate topography standard deviations
Sigma_Y1 = random(pd_Sigma_Y1,dim,1);
Sigma_Y2 = random(pd_Sigma_Y2,dim,1);
Sigma_Y3 = random(pd_Sigma_Y3,dim,1);

P_y1_max3 = 2*normcdf(y_max1./Sigma_Y1)-1;
P_y2_max3 = 2*normcdf(y_max1./Sigma_Y2)-1;
P_y3_max3 = 2*normcdf(y_max1./Sigma_Y3)-1;

P_y1_max6 = 2*normcdf(y_max2./Sigma_Y1)-1;
P_y2_max6 = 2*normcdf(y_max2./Sigma_Y2)-1;
P_y3_max6 = 2*normcdf(y_max2./Sigma_Y3)-1;

P_y1_max9 = 2*normcdf(y_max3./Sigma_Y1)-1;
P_y2_max9 = 2*normcdf(y_max3./Sigma_Y2)-1;
P_y3_max9 = 2*normcdf(y_max3./Sigma_Y3)-1;

P_y1_max2 = 2*normcdf(y_max./Sigma_Y1)-1;
P_y2_max2 = 2*normcdf(y_max./Sigma_Y2)-1;
P_y3_max2 = 2*normcdf(y_max./Sigma_Y3)-1;

% Volvo GTA
P_y_max3 = P_y1_max3.*Pi_R1 + P_y2_max3.*Pi_R2 + P_y3_max3.*Pi_R3;
P_y_max6 = P_y1_max6.*Pi_R1 + P_y2_max6.*Pi_R2 + P_y3_max6.*Pi_R3;
P_y_max9 = P_y1_max9.*Pi_R1 + P_y2_max9.*Pi_R2 + P_y3_max9.*Pi_R3;
mean_ymax3 = mean(P_y_max3);
mean_ymax6 = mean(P_y_max6);
mean_ymax9 = mean(P_y_max9);
% Scania UFD
P_y_max2 = P_y1_max2.*Pi_R1 + P_y2_max2.*Pi_R2 + P_y3_max2.*Pi_R3;
mean_ymax2 = mean(P_y_max2);

% Generate mean hill lengths
L_h1 = random(pd_L_h1,dim,1);
L_h2 = random(pd_L_h2,dim,1);
L_h3 = random(pd_L_h3,dim,1);

L_h = L_h1.*Pi_R1 + L_h2.*Pi_R2 + L_h3.*Pi_R3;

mean_Lh = mean(L_h);


%% CURVINESS

% Generate curve intensities 
Lambda_C1 = random(pd_lambda_C1,dim,1);
Lambda_C2 = random(pd_lambda_C2,dim,1);
Lambda_C3 = random(pd_lambda_C3,dim,1);

% Generate log-mean radius
M_C1 = random(pd_mu_C1,dim,1);
M_C2 = random(pd_mu_C2,dim,1);
M_C3 = random(pd_mu_C3,dim,1);

% Generate log-std radius
Sigma_C1 = random(pd_sigma_C1,dim,1);
Sigma_C2 = random(pd_sigma_C2,dim,1);
Sigma_C3 = random(pd_sigma_C3,dim,1);

% Generate log-mean length
M_L1 = random(pd_mu_L1,dim,1);
M_L2 = random(pd_mu_L2,dim,1);
M_L3 = random(pd_mu_L3,dim,1);

% Generate log-std length
Sigma_L1 = random(pd_sigma_L1,dim,1);
Sigma_L2 = random(pd_sigma_L2,dim,1);
Sigma_L3 = random(pd_sigma_L3,dim,1);

% Scania UFD
N_C1 = Lambda_C1.*(1+erf((log(1/kappa-r_turn)-M_C1)./(sqrt(2)*Sigma_C1)))/2;
N_C2 = Lambda_C1.*(1+erf((log(1/kappa-r_turn)-M_C2)./(sqrt(2)*Sigma_C2)))/2;
N_C3 = Lambda_C1.*(1+erf((log(1/kappa-r_turn)-M_C3)./(sqrt(2)*Sigma_C3)))/2;

N_C = N_C1.*Pi_R1 + N_C2.*Pi_R2+N_C3.*Pi_R3;

% Volvo GTA
P_kappa1 = N_C1.*exp(M_L1+Sigma_L1.^2/2)/1000; % Here we need to divide by 1000 to express everything in m
P_kappa2 = N_C2.*exp(M_L2+Sigma_L2.^2/2)/1000;
P_kappa3 = N_C3.*exp(M_L3+Sigma_L3.^2/2)/1000;

P_kappa = 1-P_kappa1.*Pi_R1 - P_kappa2.*Pi_R2-P_kappa3.*Pi_R3;
mean_Nc = mean(N_C);
mean_Pkappa = mean(P_kappa);


%% STOP SIGNS

Lambda_s1 = random(pd_lambda_s1,dim,1);
Lambda_s2 = random(pd_lambda_s2,dim,1);
Lambda_s3 = random(pd_lambda_s3,dim,1);

Lambda_s = Lambda_s1.*Pi_R1+Lambda_s2.*Pi_R2+Lambda_s3.*Pi_R3;

mean_Lambdas = mean(Lambda_s);

T_min1 = random(T_min1_pdf,dim,1);
T_min2 = random(T_min2_pdf,dim,1);
T_min3 = random(T_min3_pdf,dim,1);
Delta_T1 = random(Delta_T1_pdf,dim,1);
Delta_T2 = random(Delta_T2_pdf,dim,1);
Delta_T3 = random(Delta_T3_pdf,dim,1);

%% SPEED SIGNS

% Each row of the matrix P_R behaves as a Dirichlet random variable. To
% generate Dirichlet distributions, we first generate Gamma distributed
% variables, and then we use these to compute the Dirichlet variables
y_12 = random(pd_alpha12_R1,dim,1);
y_13 = random(pd_alpha13_R1,dim,1);
p_V12_R1 = y_12./(y_12+y_13);
p_V13_R1 = y_13./(y_12+y_13);

y_21 = random(pd_alpha21_R1,dim,1);
y_23 = random(pd_alpha23_R1,dim,1);
p_V21_R1 = y_21./(y_21+y_23);
p_V23_R1 = y_23./(y_21+y_23);

y_31 = random(pd_alpha31_R1,dim,1);
y_32 = random(pd_alpha32_R1,dim,1);
p_V31_R1 = y_31./(y_31+y_32);
p_V32_R1 = y_32./(y_31+y_32);

% Generate the stochastic mean road lengths
% Note: I am multiplying by 1000 to express the lengths in m, but this is
% ininfluent on the values of Pi_Ri, i = 1,...,3.
L_V1_R1 = random(pd_L_V1_R1,dim,1)*1000;
L_V2_R1 = random(pd_L_V2_R1,dim,1)*1000;
L_V3_R1 = random(pd_L_V3_R1,dim,1)*1000;

% Compute the stochastic stationary distributions (Eqs. (33) in paper)
Pi_V1_R1 = L_V1_R1.*(p_V21_R1.*p_V32_R1+p_V31_R1)./(L_V1_R1.*(p_V21_R1.*p_V32_R1+p_V31_R1) + ...
    L_V2_R1.*(p_V12_R1.*p_V31_R1 + p_V32_R1) + L_V3_R1.*(1-p_V12_R1.*p_V21_R1));

Pi_V2_R1 = L_V2_R1.*(p_V12_R1.*p_V31_R1 + p_V32_R1)./(L_V1_R1.*(p_V21_R1.*p_V32_R1+p_V31_R1) + ...
    L_V2_R1.*(p_V12_R1.*p_V31_R1 + p_V32_R1) + L_V3_R1.*(1-p_V12_R1.*p_V21_R1));

Pi_V3_R1 = L_V3_R1.*(1-p_V12_R1.*p_V21_R1)./(L_V1_R1.*(p_V21_R1.*p_V32_R1+p_V31_R1) + ...
    L_V2_R1.*(p_V12_R1.*p_V31_R1 + p_V32_R1) + L_V3_R1.*(1-p_V12_R1.*p_V21_R1));

% Each row of the matrix P_R behaves as a Dirichlet random variable. To
% generate Dirichlet distributions, we first generate Gamma distributed
% variables, and then we use these to compute the Dirichlet variables
y_12 = random(pd_alpha12_R2,dim,1);
y_13 = random(pd_alpha13_R2,dim,1);
p_V12_R2 = y_12./(y_12+y_13);
p_V13_R2 = y_13./(y_12+y_13);

y_21 = random(pd_alpha21_R2,dim,1);
y_23 = random(pd_alpha23_R2,dim,1);
p_V21_R2 = y_21./(y_21+y_23);
p_V23_R2 = y_23./(y_21+y_23);

y_31 = random(pd_alpha31_R2,dim,1);
y_32 = random(pd_alpha32_R2,dim,1);
p_V31_R2 = y_31./(y_31+y_32);
p_V32_R2 = y_32./(y_31+y_32);

% Generate the stochastic mean road lengths
% Note: I am multiplying by 1000 to express the lengths in m, but this is
% ininfluent on the values of Pi_Ri, i = 1,...,3.
L_V1_R2 = random(pd_L_V1_R2,dim,1)*1000;
L_V2_R2 = random(pd_L_V2_R2,dim,1)*1000;
L_V3_R2 = random(pd_L_V3_R2,dim,1)*1000;

% Compute the stochastic stationary distributions (Eq. (33) in paper)
Pi_V1_R2 = L_V1_R2.*(p_V21_R2.*p_V32_R2+p_V31_R2)./(L_V1_R2.*(p_V21_R2.*p_V32_R2+p_V31_R2) + ...
    L_V2_R2.*(p_V12_R2.*p_V31_R2 + p_V32_R2) + L_V3_R2.*(1-p_V12_R2.*p_V21_R2));

Pi_V2_R2 = L_V2_R2.*(p_V12_R2.*p_V31_R2 + p_V32_R2)./(L_V1_R2.*(p_V21_R2.*p_V32_R2+p_V31_R2) + ...
    L_V2_R2.*(p_V12_R2.*p_V31_R2 + p_V32_R2) + L_V3_R2.*(1-p_V12_R2.*p_V21_R2));

Pi_V3_R2 = L_V3_R2.*(1-p_V12_R2.*p_V21_R2)./(L_V1_R2.*(p_V21_R2.*p_V32_R2+p_V31_R2) + ...
    L_V2_R2.*(p_V12_R2.*p_V31_R2 + p_V32_R2) + L_V3_R2.*(1-p_V12_R2.*p_V21_R2));

L_V1_R3 = random(pd_L_V1_R3,dim,1)*1000;
L_V2_R3 = random(pd_L_V2_R3,dim,1)*1000;

Pi_V1_R3 = L_V1_R3./(L_V1_R3+L_V2_R3);
Pi_V2_R3 = L_V2_R3./(L_V1_R3+L_V2_R3);

V_hat1 = 30*Pi_V1_R1+40*Pi_V2_R1+50*Pi_V3_R1;
V_hat2 = 60*Pi_V1_R2+70*Pi_V2_R2+80*Pi_V3_R2;
V_hat3 = 90*Pi_V1_R3+100*Pi_V2_R3;

V_hat = V_hat1.*Pi_R1+V_hat2.*Pi_R2+V_hat3.*Pi_R3;

meanSpeed = mean(V_hat);


%% CALCULATE MEAN OF MODEL PARAMETERS

% First row
mean_p_R12 = 0.85/0.95;
mean_p_R13 = 0.1/0.95;
% Second row
mean_p_R21 = 0.59/(0.59+0.48);
mean_p_R23 = 0.48/(0.59+0.48);
% Third row
mean_p_R31 = 0.14/(0.14+1.18); 
mean_p_R32 = 1.18/(0.14+1.18);

mean_L_R1 = exp(pd_L_R1.mu + pd_L_R1.sigma^2/2);
mean_L_R2 = exp(pd_L_R2.mu + pd_L_R2.sigma^2/2);
mean_L_R3 = exp(pd_L_R3.mu + pd_L_R3.sigma^2/2);

approx_Pi_R1 = mean_L_R1.*(mean_p_R21.*mean_p_R32+mean_p_R31)./(mean_L_R1.*(mean_p_R21.*mean_p_R32+mean_p_R31) + ...
    mean_L_R2.*(mean_p_R12.*mean_p_R31 + mean_p_R32) + mean_L_R3.*(1-mean_p_R12.*mean_p_R21));

approx_Pi_R2 = mean_L_R2.*(mean_p_R12.*mean_p_R31 + mean_p_R32)./(mean_L_R1.*(mean_p_R21.*mean_p_R32+mean_p_R31) + ...
    mean_L_R2.*(mean_p_R12.*mean_p_R31 + mean_p_R32) + mean_L_R3.*(1-mean_p_R12.*mean_p_R21));

approx_Pi_R3 = mean_L_R3.*(1-mean_p_R12.*mean_p_R21)./(mean_L_R1.*(mean_p_R21.*mean_p_R32+mean_p_R31) + ...
    mean_L_R2.*(mean_p_R12.*mean_p_R31 + mean_p_R32) + mean_L_R3.*(1-mean_p_R12.*mean_p_R21));

mean_Sigma_Y1 = exp(pd_Sigma_Y1.mu + pd_Sigma_Y1.sigma^2/2);
mean_Sigma_Y2 = exp(pd_Sigma_Y2.mu + pd_Sigma_Y2.sigma^2/2);
mean_Sigma_Y3 = exp(pd_Sigma_Y3.mu + pd_Sigma_Y3.sigma^2/2);
approx_P_y1_max2 = 2*normcdf(y_max./mean_Sigma_Y1)-1;
approx_P_y2_max2 = 2*normcdf(y_max./mean_Sigma_Y2)-1;
approx_P_y3_max2 = 2*normcdf(y_max./mean_Sigma_Y3)-1;
approx_P_y_max2 = approx_P_y1_max2.*approx_Pi_R1 ...
    + approx_P_y2_max2.*approx_Pi_R2 + approx_P_y3_max2.*approx_Pi_R3;
mean_ymax2 = mean(P_y_max2);

approx_P_y1_max3 = 2*normcdf(3./mean_Sigma_Y1)-1;
approx_P_y2_max3 = 2*normcdf(3./mean_Sigma_Y3)-1;
approx_P_y3_max3 = 2*normcdf(3./mean_Sigma_Y1)-1;
approx_P_y_max3 = approx_P_y1_max3.*approx_Pi_R1 ...
    + approx_P_y2_max3.*approx_Pi_R2 + approx_P_y3_max3.*approx_Pi_R3;

approx_P_y1_max6 = 2*normcdf(6./mean_Sigma_Y1)-1;
approx_P_y2_max6 = 2*normcdf(6./mean_Sigma_Y3)-1;
approx_P_y3_max6 = 2*normcdf(6./mean_Sigma_Y1)-1;
approx_P_y_max6 = approx_P_y1_max6.*approx_Pi_R1 ...
    + approx_P_y2_max6.*approx_Pi_R2 + approx_P_y3_max6.*approx_Pi_R3;

approx_P_y1_max9 = 2*normcdf(9./mean_Sigma_Y1)-1;
approx_P_y2_max9 = 2*normcdf(9./mean_Sigma_Y3)-1;
approx_P_y3_max9 = 2*normcdf(9./mean_Sigma_Y1)-1;
approx_P_y_max9 = approx_P_y1_max9.*approx_Pi_R1 ...
    + approx_P_y2_max9.*approx_Pi_R2 + approx_P_y3_max9.*approx_Pi_R3;

mean_L_h1 = exp(pd_L_h1.mu + pd_L_h1.sigma^2/2);
mean_L_h2 = exp(pd_L_h2.mu + pd_L_h2.sigma^2/2);
mean_L_h3 = exp(pd_L_h3.mu + pd_L_h3.sigma^2/2);
approx_L_h = mean_L_h1.*approx_Pi_R1 + mean_L_h2.*approx_Pi_R2 ...
    + mean_L_h3.*approx_Pi_R3;

mean_Lambda_C1 = exp(pd_lambda_C1.mu + pd_lambda_C1.sigma^2/2);
mean_Lambda_C2 = exp(pd_lambda_C2.mu + pd_lambda_C2.sigma^2/2);
mean_Lambda_C3 = exp(pd_lambda_C3.mu + pd_lambda_C3.sigma^2/2);

mean_M_C1 = pd_mu_C1.mu;
mean_M_C2 = pd_mu_C2.mu;
mean_M_C3 = pd_mu_C3.mu;

mean_Sigma_C1 = exp(pd_sigma_C1.mu + pd_sigma_C1.sigma^2/2);
mean_Sigma_C2 = exp(pd_sigma_C2.mu + pd_sigma_C2.sigma^2/2);
mean_Sigma_C3 = exp(pd_sigma_C3.mu + pd_sigma_C3.sigma^2/2);

mean_M_L1 = pd_mu_L1.mu;
mean_M_L2 = pd_mu_L2.mu;
mean_M_L3 = pd_mu_L3.mu;

mean_Sigma_L1 = exp(pd_sigma_L1.mu + pd_sigma_L1.sigma^2/2);
mean_Sigma_L2 = exp(pd_sigma_L2.mu + pd_sigma_L2.sigma^2/2);
mean_Sigma_L3 = exp(pd_sigma_L3.mu + pd_sigma_L3.sigma^2/2);

mean_Lambda_s1 = exp(pd_lambda_s1.mu + pd_lambda_s1.sigma^2/2);
mean_Lambda_s2 = exp(pd_lambda_s2.mu + pd_lambda_s2.sigma^2/2);
mean_Lambda_s3 = exp(pd_lambda_s3.mu + pd_lambda_s3.sigma^2/2);
approx_Lambda_s = mean_Lambda_s1.*approx_Pi_R1 + mean_Lambda_s2.*approx_Pi_R2 ...
    + mean_Lambda_s3.*approx_Pi_R3;

approx_N_C1 = mean_Lambda_C1.*(1+erf((log(1/kappa-r_turn)-mean_M_C1)./(sqrt(2)*mean_Sigma_C1)))/2;
approx_N_C2 = mean_Lambda_C1.*(1+erf((log(1/kappa-r_turn)-mean_M_C2)./(sqrt(2)*mean_Sigma_C2)))/2;
approx_N_C3 = mean_Lambda_C1.*(1+erf((log(1/kappa-r_turn)-mean_M_C3)./(sqrt(2)*mean_Sigma_C3)))/2;
approx_N_C = approx_N_C1.*approx_Pi_R1 + approx_N_C2.*approx_Pi_R2 ...
    +approx_N_C3.*approx_Pi_R3;

approx_P_kappa1 = approx_N_C1.*exp(mean_M_L1+mean_Sigma_L1.^2/2)/1000; % Here we need to divide by 1000 to express everything in m
approx_P_kappa2 = approx_N_C2.*exp(mean_M_L2+mean_Sigma_L2.^2/2)/1000;
approx_P_kappa3 = approx_N_C3.*exp(mean_M_L3+mean_Sigma_L3.^2/2)/1000;
approx_P_kappa = 1-(approx_P_kappa1.*approx_Pi_R1 ...
    + approx_P_kappa2.*approx_Pi_R2 + approx_P_kappa3.*approx_Pi_R3);


% Calculate mean single-transition probabilities for speed signs
mean_p_V12_R1 = pd_alpha12_R1.a/(pd_alpha12_R1.a+pd_alpha13_R1.a);
mean_p_V13_R1 = pd_alpha13_R1.a/(pd_alpha12_R1.a+pd_alpha13_R1.a);

mean_p_V21_R1 =  pd_alpha21_R1.a/(pd_alpha21_R1.a+pd_alpha23_R1.a);
mean_p_V23_R1 = pd_alpha23_R1.a/(pd_alpha21_R1.a+pd_alpha23_R1.a);

mean_p_V31_R1 =  pd_alpha31_R1.a/(pd_alpha31_R1.a+pd_alpha32_R1.a);
mean_p_V32_R1 = pd_alpha32_R1.a/(pd_alpha31_R1.a+pd_alpha32_R1.a);

% Calculate the mean values for mean road lengths
% Note: I am multiplying by 1000 to express the lengths in m, but this is
% ininfluent on the values of Pi_Ri, i = 1,...,3.
mean_L_V1_R1 = exp(pd_L_V1_R1.mu+pd_L_V1_R1.sigma^2/2);
mean_L_V2_R1 = exp(pd_L_V2_R1.mu+pd_L_V2_R1.sigma^2/2);
mean_L_V3_R1 = exp(pd_L_V3_R1.mu+pd_L_V3_R1.sigma^2/2);

% Compute the mean stochastic stationary distributions 
approx_Pi_V1_R1 = mean_L_V1_R1.*(mean_p_V21_R1.*mean_p_V32_R1+mean_p_V31_R1)./(mean_L_V1_R1.*(mean_p_V21_R1.*mean_p_V32_R1+mean_p_V31_R1) + ...
    mean_L_V2_R1.*(mean_p_V12_R1.*mean_p_V31_R1 + mean_p_V32_R1) + mean_L_V3_R1.*(1-mean_p_V12_R1.*mean_p_V21_R1));

approx_Pi_V2_R1 = mean_L_V2_R1.*(mean_p_V12_R1.*mean_p_V31_R1 + mean_p_V32_R1)./(mean_L_V1_R1.*(mean_p_V21_R1.*mean_p_V32_R1+mean_p_V31_R1) + ...
    mean_L_V2_R1.*(mean_p_V12_R1.*mean_p_V31_R1 + mean_p_V32_R1) + mean_L_V3_R1.*(1-mean_p_V12_R1.*mean_p_V21_R1));

approx_Pi_V3_R1 = mean_L_V3_R1.*(1-mean_p_V12_R1.*mean_p_V21_R1)./(mean_L_V1_R1.*(mean_p_V21_R1.*mean_p_V32_R1+mean_p_V31_R1) + ...
    mean_L_V2_R1.*(mean_p_V12_R1.*mean_p_V31_R1 + mean_p_V32_R1) + mean_L_V3_R1.*(1-mean_p_V12_R1.*mean_p_V21_R1));

approx_V_R1 = 30*approx_Pi_V1_R1 + 40*approx_Pi_V2_R1+50*approx_Pi_V3_R1;

mean_p_V12_R2 = pd_alpha12_R2.a/(pd_alpha12_R2.a+pd_alpha13_R2.a);
mean_p_V13_R2 = pd_alpha13_R2.a/(pd_alpha12_R2.a+pd_alpha13_R2.a);

mean_p_V21_R2 =  pd_alpha21_R2.a/(pd_alpha21_R2.a+pd_alpha23_R2.a);
mean_p_V23_R2 = pd_alpha23_R2.a/(pd_alpha21_R2.a+pd_alpha23_R2.a);

mean_p_V31_R2 =  pd_alpha31_R2.a/(pd_alpha31_R2.a+pd_alpha32_R2.a);
mean_p_V32_R2 = pd_alpha32_R2.a/(pd_alpha31_R2.a+pd_alpha32_R2.a);

% Calculate the mean values for mean road lengths
% Note: I am multiplying by 1000 to express the lengths in m, but this is
% ininfluent on the values of Pi_Ri, i = 1,...,3.
mean_L_V1_R2 = exp(pd_L_V1_R2.mu+pd_L_V1_R2.sigma^2/2);
mean_L_V2_R2 = exp(pd_L_V2_R2.mu+pd_L_V2_R2.sigma^2/2);
mean_L_V3_R2 = exp(pd_L_V3_R2.mu+pd_L_V3_R2.sigma^2/2);

% Compute the mean stochastic stationary distributions 
approx_Pi_V1_R2 = mean_L_V1_R2.*(mean_p_V21_R2.*mean_p_V32_R2+mean_p_V31_R2)./(mean_L_V1_R2.*(mean_p_V21_R2.*mean_p_V32_R2+mean_p_V31_R2) + ...
    mean_L_V2_R2.*(mean_p_V12_R2.*mean_p_V31_R2 + mean_p_V32_R2) + mean_L_V3_R2.*(1-mean_p_V12_R2.*mean_p_V21_R2));

approx_Pi_V2_R2 = mean_L_V2_R2.*(mean_p_V12_R2.*mean_p_V31_R2 + mean_p_V32_R2)./(mean_L_V1_R2.*(mean_p_V21_R2.*mean_p_V32_R2+mean_p_V31_R2) + ...
    mean_L_V2_R2.*(mean_p_V12_R2.*mean_p_V31_R2 + mean_p_V32_R2) + mean_L_V3_R2.*(1-mean_p_V12_R2.*mean_p_V21_R2));

approx_Pi_V3_R2 = mean_L_V3_R2.*(1-mean_p_V12_R2.*mean_p_V21_R2)./(mean_L_V1_R2.*(mean_p_V21_R2.*mean_p_V32_R2+mean_p_V31_R2) + ...
    mean_L_V2_R2.*(mean_p_V12_R2.*mean_p_V31_R2 + mean_p_V32_R2) + mean_L_V3_R2.*(1-mean_p_V12_R2.*mean_p_V21_R2));

approx_V_R2 = 60*approx_Pi_V1_R2 + 70*approx_Pi_V2_R2+80*approx_Pi_V3_R2;

mean_L_V1_R3 = exp(pd_L_V1_R3.mu+pd_L_V1_R3.sigma^2/2);
mean_L_V2_R3 = exp(pd_L_V2_R3.mu+pd_L_V2_R3.sigma^2/2);

approx_Pi_V1_R3 = mean_L_V1_R3./(mean_L_V1_R3+mean_L_V2_R3);
approx_Pi_V2_R3 = mean_L_V2_R3./(mean_L_V1_R3+mean_L_V2_R3);

approx_V_R3 = 90*approx_Pi_V1_R3 + 100*approx_Pi_V2_R3;

approx_V_hat = approx_V_R1.*approx_Pi_R1+approx_V_R2.*approx_Pi_R2 ...
    +approx_V_R3.*approx_Pi_R3;


%% MISSION LENGTH

Lm = random(pd_Lm,dim,1);


%% FIGURES

figure(1)
subplot(3,2,1)
histogram(P_y_max2,50,'Normalization','pdf', 'FaceColor', ...
    [0.00,0.45,0.74])
hold on
xline(mean_ymax2, 'LineWidth', 1.5)
hold on
xline(approx_P_y_max2, '--', 'LineWidth', 1.5, 'Color', [0.93,0.69,0.13])
set(gca,'TickLabelInterpreter','latex')
axis([0 1 0 inf])
xlabel('Road grade length ratio $p_y$ (-)')
ylabel('Probability density')
% legend('Measured', 'Model', 'Interpreter', 'latex')
title('\textbf{(a)}')



subplot(3,2,2)
histogram(L_h,100,'Normalization','pdf', 'FaceColor', ...
    [0.00,0.45,0.74])
hold on
xline(mean_Lh, 'LineWidth', 1.5)
hold on
xline(approx_L_h, '--', 'LineWidth', 1.5, 'Color', [0.93,0.69,0.13])
set(gca,'TickLabelInterpreter','latex')
axis([0 2500 0 inf])
xlabel('Mean hill length $L_{\mathrm{h}}$ (m)')
ylabel('Probability density')
% legend('Measured', 'Model', 'Interpreter', 'latex')
title('\textbf{(b)}')


subplot(3,2,3)
histogram(N_C,80,'Normalization','pdf', 'FaceColor', ...
    [0.00,0.45,0.74])
hold on
xline(mean_Nc, 'LineWidth', 1.5)
hold on
xline(approx_N_C, '--', 'LineWidth', 1.5, 'Color', [0.93,0.69,0.13])
set(gca,'TickLabelInterpreter','latex')
axis([0 20 0 inf])
xlabel('Expected number of curves $\bar{n}_C^\prime$ ($\mathrm{km}^{-1}$)')
ylabel('Probability density')
% legend('Measured', 'Model', 'Interpreter', 'latex')
title('\textbf{(c)}')


subplot(3,2,4)
histogram(P_kappa,50,'Normalization','pdf', 'FaceColor', ...
    [0.00,0.45,0.74])
hold on
xline(mean_Pkappa, 'LineWidth', 1.5)
hold on
xline(approx_P_kappa, '--', 'LineWidth', 1.5, 'Color', [0.93,0.69,0.13])
set(gca,'TickLabelInterpreter','latex')
axis([0 1 0 inf])
xlabel('Curviness length ratio $p_\kappa$ (-)')
ylabel('Probability density')
% legend('Measured', 'Model', 'Interpreter', 'latex')
title('\textbf{(d)}')

subplot(3,2,5)
histogram(Lambda_s,300,'Normalization','pdf', 'FaceColor', ...
    [0.00,0.45,0.74])
hold on
xline(mean_Lambdas, 'LineWidth', 1.5)
hold on
xline(approx_Lambda_s, '--', 'LineWidth', 1.5, 'Color', [0.93,0.69,0.13])
set(gca,'TickLabelInterpreter','latex')
axis([0 2 0 inf])
xlabel('Expected number of stops $\bar{n}_{\mathrm{s}}$ ($\mathrm{km}^{-1}$)')
ylabel('Probability density')
% legend('Measured', 'Model', 'Interpreter', 'latex')
title('\textbf{(e)}')


subplot(3,2,6)
histogram(V_hat,50,'Normalization','pdf', 'FaceColor', ...
    [0.00,0.45,0.74])
hold on
xline(meanSpeed, 'LineWidth', 1.5)
hold on
xline(approx_V_hat, '--', 'LineWidth', 1.5, 'Color', [0.93,0.69,0.13])
set(gca,'TickLabelInterpreter','latex')
axis([30 100 0 inf])
xlabel('Mean legal speed $\hat{v}$ ($\mathrm{km\,h^{-1}}$)')
ylabel('Probability density')
% legend('Measured', 'Model', 'Interpreter', 'latex')
title('\textbf{(f)}')

%%
figure(2)
histogram(P_y_max2,50,'Normalization','pdf', 'FaceColor', ...
    [0.00,0.45,0.74])
hold on
xline(mean_ymax3, 'LineWidth', 1.5)
hold on
xline(approx_P_y_max3, '--', 'LineWidth', 1.5, 'Color', [0.93,0.69,0.13])
set(gca,'TickLabelInterpreter','latex')
axis([0.4 1 0 inf])
xlabel('Road grade length ratio $p_y$ (-)')
ylabel('Probability density')
legend('Distribution', 'Numerical expectation', ...
    'Approximated expectation', 'Interpreter', 'latex')
%% SAVE GENERATED VARIABLES

% designationDir = 'C:\Users\luigir\Desktop\sOC for TRD';
% save([designationDir '\distributionsGenerated_forSOC.mat'], 'L_h1', 'L_h2', 'L_h3', 'L_R1', ...
%     'L_R2', 'L_R3', 'L_V1_R1', 'L_V1_R2','L_V1_R3', 'L_V2_R1', 'L_V2_R2', ...
%     'L_V2_R3', 'L_V3_R1','L_V3_R2','Lambda_C1','Lambda_C2','Lambda_C3', ...
%     'Lambda_s1','Lambda_s2','Lambda_s3','M_C1','M_C2','M_C3','M_L1', ...
%     'M_L2','M_L3','p_R12','p_R13','p_R21','p_R23','p_R31','p_R32', ...
%     'p_V12_R1', 'p_V12_R2','p_V13_R1','p_V13_R2','p_V21_R1','p_V21_R2', ...
%     'p_V23_R1','p_V23_R2','p_V31_R1','p_V31_R2','p_V32_R1','p_V32_R2', ...
%     'Sigma_C1','Sigma_C2','Sigma_C3','Sigma_L1','Sigma_L2','Sigma_L3', ...
%     'Sigma_Y1','Sigma_Y2', 'Sigma_Y3', 'V_hat', 'P_y_max2', 'P_y_max3', ...
%     'P_y_max6', 'P_y_max9', 'L_h', 'P_kappa', 'N_C', 'Lambda_s', 'T_min1', ...
%     'T_min2', 'T_min3', 'Delta_T1', 'Delta_T2', 'Delta_T3', 'Lm')

%% FIGURE
figure(3)

x = 0:.01:8;
pdf_Sy_x1 = pdf(pd_Sigma_Y1,x);
pdf_Sy_x2 = pdf(pd_Sigma_Y2,x);
pdf_Sy_x3 = pdf(pd_Sigma_Y3,x);

subplot(1,3,1)
histogram(Sigma_Y1,50,'Normalization','pdf', 'FaceColor', ...
    [0.00,0.45,0.74])
hold on
plot(x,pdf_Sy_x1,'LineWidth',1, 'color', [0.40,0.40,0.40])
xlabel('Topography std $\sigma_{Y|r_1}$ (-)')
ylabel('Probability density')
title('\textbf{(a)}')
set(gca,'TickLabelInterpreter','latex')
legend('Simulated', 'Analytical PDF', 'Interpreter', 'latex')
axis([0 8 0 0.8])
subplot(1,3,2)
histogram(Sigma_Y2,50,'Normalization','pdf', 'FaceColor', ...
    [0.85,0.33,0.10])
hold on
plot(x,pdf_Sy_x2,'LineWidth',1, 'color', [0.40,0.40,0.40])
xlabel('Topography std $\sigma_{Y|r_2}$ (-)')
ylabel('Probability density')
title('\textbf{(b)}')
set(gca,'TickLabelInterpreter','latex')
legend('Simulated', 'Analytical PDF', 'Interpreter', 'latex')
axis([0 8 0 0.8])
subplot(1,3,3)
histogram(Sigma_Y3,50,'Normalization','pdf', 'FaceColor', ...
    [0.93,0.69,0.13])
hold on
plot(x,pdf_Sy_x3,'LineWidth',1, 'color', [0.40,0.40,0.40])
xlabel('Topography std $\sigma_{Y|r_3}$ (-)')
ylabel('Probability density')
title('\textbf{(c)}')
set(gca,'TickLabelInterpreter','latex')
legend('Simulated', 'Analytical PDF', 'Interpreter', 'latex')
axis([0 8 0 0.8])
