function [OperatingCycle,varargout] = generate_dOC_Road(StatisticalOC, ...
    cargoWeight, plotResult, auxiliaryParameters, givenRoadType, generateSpeedSeparately, ...
    useDirectAssignment, curveLengthUpperBound, doSaveRawData, doUseCurveClustering, ...
    minimumCurveLength)

%% DESCRIPTION
% This function generates a dOC of given input length.
% If the various stochastic parameters are not given, default values are
% assigned.
% Note that the toolboxes (wafo, toporoll, poissim) must be initialised
% prior to calling the function.
%
% The difference between GenerateStatisticalOC_v2 and
% GenerateStatisticalOC (_v1) is that v2 uses stat-OC version 1.02, while
% the old used stat_OC version 1.01.
%
%   Input
%
%   StatisticalOC: struct containing the stat-OC (v1.02), at least with
%       TC, Topo, Curv, Bump, Stop and Speed.
%   cargoWeight: misson payload in kg.
%   plotResult: flag to enable plotting of various signals
%   auxiliaryParameters: additional parameters to modify the mapping
%       between stat-OC and det-OC.
%   givenRoadType: an array of road types, instead of generating a new 
%       sequence. If left empty, then the road type is generated too
%   generateSpeedSeparately: flag that specifies whether speed signs should
%       be generated or if the bounds on the road type are used.
%   useDirectAssignment: specifies which algorithm to use to for the speed
%       generation
%   curveLengthUpperBound: an upper bound on the curve length. If left
%       empty, then no boundary is used. Default is no boundary (m).
%   doSaveRawData: T/F-flag whether to store raw data or not. Default is
%       false.
%   doUseCurveClustering: T/F-flag whether to use 'stupid' curve clustering
%       or not
%   minimumCurveLength: option to included minimum curve length (m).
%   Default is 0 m (disabled).
%   
%   Output
%
%   OperatingCycle: mission description on det-OC-format
%   ocVersion: version information for the det-OC. This script generates to
%       version 1.02
%   FigHandle: Struct that contains the handles of all figures plotted from
%       this script. Only relevant if plotResult was given as TRUE.


%% INITIALISATION

n = nargin;
if(n < 11)
    minimumCurveLength = [];
end
if(n < 10)
    doUseCurveClustering = [];
end
if(n < 9)
    doSaveRawData = [];
end
if(n < 8)
    curveLengthUpperBound = [];
end
if(n < 7)
    useDirectAssignment = [];
end
if(n < 6)
    generateSpeedSeparately = [];
end
if(n < 5)
    givenRoadType = [];
end
if(n < 4)
    auxiliaryParameters = [];
end
if(n < 3)
    plotResult = [];
end
if(n < 2)
    error('Too few arguments given, a stat-OC and a cargo weight must be specified')
end
if(isempty(useDirectAssignment))
    useDirectAssignment = true;
end
if(isempty(generateSpeedSeparately))
    generateSpeedSeparately = false;
end
if(isempty(auxiliaryParameters))
    missionStopStandstillTime = 5;
    speedBumpParameterDimension = 4;    %pos, length, height, angle
    defaultBumpHeight = 0.1;
    defaultBumpLength = 2.0;
    calibrationSpeedOnBump = 15/3.6;
    bumpCalibrationAngle = 7;
    angleSpeedFactor = calibrationSpeedOnBump*bumpCalibrationAngle/...
        (90-bumpCalibrationAngle);   %constant C_1 in format-paper, C_2 = 0
else
    error('Auxiliary parameter assignment not implemented yet');
end
if(isempty(plotResult))
    plotResult = false;
    FigHandle = [];
else
    FigHandle = struct('Speed',[],'Topography',[],'Curvature',[],...
        'StopAndBump',[]);
end
if(isempty(doSaveRawData))
    doSaveRawData = false;
end
if(isempty(doUseCurveClustering))
    doUseCurveClustering = false;
end
if(isempty(minimumCurveLength))
    minimumCurveLength = 0;
end

% TC  = transport cycle?)
TC = StatisticalOC.TC;
Topo = StatisticalOC.Topo;
Curv = StatisticalOC.Curv;
Bump = StatisticalOC.Bump;
Stop = StatisticalOC.Stop;


%% SIMULATE TRANSPORT CYCLE

% Construct Markov Chain for transport cycle
pp = 1-TC.L./TC.LL;
% P = diag(pp) + diag(1-pp)*mat2tmat(TC.PP);
P = diag(pp) + diag(1-pp)*mat2tmat(TC.PP);

Nsim = ceil(TC.Ltot/TC.L); %4e3
if(isempty(givenRoadType))
% Simulate MC for TC
    zsim = mcsim(P,Nsim,3);
else % the road type is given
    zsim = interp1(givenRoadType(:,1),givenRoadType(:,2),((0:(Nsim-1))*TC.L)',...
        'previous','extrap');
end

% Plot simulated TC
if(plotResult)
    fhdlRoadType = pfig;
    x = (1:(Nsim))*TC.L/1e3;
    hmmplot(zsim,zsim,x)
end


%% SIMULATE LEGAL SPEED

if(generateSpeedSeparately)
    if(isfield(StatisticalOC,'Speed'))
        Speed = StatisticalOC.Speed;
    else
        error(['The flag generateSpeedSeparately is TRUE but there is no statistical information on' ...
            ' the speed in the given stat-OC (no Speed-field is present in the struct): ' ...
            'unable to generate v_sim']);
    end
    
    nRoadTypes = length(Speed);
    vMatrix = zeros(Nsim,nRoadTypes);
    
    for i = 1:nRoadTypes
        iTransitionMatrix = Speed{i}.PP;
        iMeanLength = Speed{i}.LL;
        iSpeedState = Speed{i}.Speed(:);
        if(sum(isnan(iMeanLength)) > 0)
            warning(['Speed states are degenerate: exception has not been implemented, '...
                'it is recommended to set generateSpeedSeparately = false']);
        end
        if(length(iSpeedState) == 1)
            iv_sim = ones(Nsim,1)*iSpeedState;
        else
            ipp = 1-TC.L./iMeanLength;
            iP = diag(ipp) + diag(1-ipp)*mat2tmat(iTransitionMatrix);
            iState = mcsim(iP,Nsim);
            iv_sim = iSpeedState(iState);
        end
        vMatrix(:,i) = iv_sim;
    end
    if(useDirectAssignment)
        tmp  = (1:Nsim)';
        idVMatrixLinear = sub2ind([Nsim,nRoadTypes], tmp, zsim);    % Here linear indexing is used for reassignment
        v_sim = vMatrix(idVMatrixLinear);
    else
        v_sim = zeros(Nsim,1);
        tracker = (1:Nsim)';
        for i = 1:nRoadTypes
            isThisType = (zsim == i);
            nEntries = sum(isThisType);
            iIndex = tracker(isThisType);
            v_sim(iIndex) = vMatrix(1:nEntries,i);
        end
    end
    
%     if(debug)
%         % direct assignment output
%         tmp  = (1:Nsim)';
%         idVMatrixLinear = sub2ind([Nsim,nRoadTypes], tmp, zsim);    % here linear indexing is used for reassignment
%         vDirect = vMatrix(idVMatrixLinear);
%     
%         %sequential assignment output
%         vSequential = zeros(Nsim,1);
%         tracker = (1:Nsim)';
%         for i = 1:nRoadTypes
%             isThisType = (zsim == i);
%             nEntries = sum(isThisType);
%             iIndex = tracker(isThisType);
%             vSequential(iIndex) = vMatrix(1:nEntries,i);
%         end
%         x = (1:(Nsim))*TC.L/1e3;
%           
%         figure
%         stairs(x,vDirect*3.6);
%         hold on
%         stairs(x,vSequential*3.6,'--');
%         legend('Direct','Sequential');   
%     end
else
    v_sim = TC.Speed(zsim)';
end

% Plot speed
if(plotResult)
    figure(fhdlRoadType),clf
    x = (1:(Nsim))*TC.L/1e3;
    hmmplot(v_sim*3.6,zsim,x,[1 4],'Simulated speed','Signed speed (km/h)',1)
    xlabel('Distance (km)')
end


%% SIMULATE TOPOGRAPHY

PAR=Topo;

% Find change points between segments
K = [1; find(diff(zsim)~=0)+1; length(zsim)+1];
ysim=zsim;
for k=1:(length(K)-1)
    zk=zsim(K(k));
    Park= PAR{zk};
    Park.Lar=Park.L*(K(k+1)-K(k));
    ysimk = topoarsim(Park);
    I = K(k):(K(k+1)-1);
    ysim(I)=ysimk.y;   
end

% Plot topography
if(plotResult)
    fhdlTopography = pfig;
    x = (1:(Nsim))*TC.L/1e3;   % problem if Topo has different segment length (-s) than TC
    hmmplot(ysim,zsim,x)
    hmmplot(ysim,zsim,x,[],'Simulated topography','Slope (%)',1)
    xlabel('Distance (km)')
end


%% SIMULATE CURVES

PAR=Curv;
INT = [PAR{1}.int PAR{2}.int PAR{3}.int]*1000;  % multiplied by 1000 to convert into 1/km 
csim = poissim(zsim,TC.L,INT);                  % which is what's required by poissim

if isempty(csim)
    csim.x = [];
    csim.R = [];
    csim.L = [];
    csim.z = [];
elseif(doUseCurveClustering)
    clusterSize = [PAR{1}.cs, PAR{2}.cs, PAR{3}.cs];
    nCsim = length(csim.x);
    maxClusterSize = max(clusterSize);
    xExtended = nan*ones(1,maxClusterSize*nCsim);
    xExtended(1:maxClusterSize:end) = csim.x;
    tmp = repmat(csim.z,maxClusterSize,1);
    zExtended = tmp(:)';
    rExtended = nan*ones(1,maxClusterSize*nCsim);
    lExtended = nan*ones(1,maxClusterSize*nCsim);

    for k = 1:nCsim
        zk = csim.z(k);
        Park = PAR{zk};
        kClusterProbability = Park.p_cc;
        aRandomNumber = rand(1);
        
        if(kClusterProbability > aRandomNumber)
            kClusterSize = Park.cs;
            individualRadius = zeros(1,kClusterSize);
            individualLength = zeros(1,kClusterSize);
            for ii = 1:kClusterSize
                Y = exp(rndnorm(Park.mC,Park.sC^2));
                RR = Y+Park.r_turn;
                Cdir = sign(randn);
                individualRadius(ii) = Cdir*RR; 
                curveLength = exp(rndnorm(Park.mL,Park.sL^2));
                individualLength(ii) = curveLength + Park.l_turn;                
            end
            idToWrite = (1:maxClusterSize) + maxClusterSize*(k-1);
            rExtended(idToWrite) = individualRadius;
            lExtended(idToWrite) = individualLength;
            consecutivePosition = csim.x(k) + cumsum([0, individualLength(1:end-1)]);
            xExtended(idToWrite) = consecutivePosition;
        else
            idToWrite = maxClusterSize*(k-1) + 1;
            Y = exp(rndnorm(Park.mC,Park.sC^2));
            RR = Y+Park.r_turn;
            Cdir = sign(randn);
            curveLength = exp(rndnorm(Park.mL,Park.sL^2));
            rExtended(idToWrite)= Cdir*RR; 
            lExtended(idToWrite) = curveLength + Park.l_turn;
            xExtended(idToWrite) = csim.x(k);
        end
    end
    isFinite = isfinite(xExtended);
    xExtended = xExtended(isFinite);
    rExtended = rExtended(isFinite);
    lExtended = lExtended(isFinite);
    zExtended = zExtended(isFinite);
    csimWithOverlap.x = xExtended;
    csimWithOverlap.R = rExtended;
    csimWithOverlap.L = lExtended;
    csimWithOverlap.z = zExtended;
    
%     -- for debug and development --
%     figure
%     plot((diff(xExtended) < 0),'x');
%     set(gca,'ylim', [-0.1, 1.1]);
%     -- --
    
    csim = RemoveOverlappingCurvesInCurviness(csimWithOverlap);
% Here going with the sorting approach 
%     [xSorted,idSorted] = sort(xExtended);
%     rSorted = rExtended(idSorted);
%     lSorted = lExtended(idSorted);
%     zSorted = zExtended(idSorted);
%     csim.x = xSorted;
%     csim.R = rSorted;
%     csim.L = lSorted;
%     csim.z = zSorted;
else
    for k=1:length(csim.x)
        zk=csim.z(k);              % Road type
        Park = PAR{zk};             % Parameters for road section
        Y = exp(rndnorm(Park.mC,Park.sC^2)); % Help variable for curve radius
        RR = Y+Park.r_turn;         % Curve radius
        Cdir = sign(randn);         % Direction of curve (right/left)
        csim.R(k)= Cdir*RR;        % Curve radius (with sign)
        % previous curve length generation
        % csim.L(k)= Park.mL;        % Curve length

        % new curve length generation
        curveLength = exp(rndnorm(Park.mL,Park.sL^2));
        csim.L(k) = curveLength + Park.l_turn;
    end
end

if(~isempty(curveLengthUpperBound))
    csim.L(csim.L > curveLengthUpperBound) = curveLengthUpperBound;
end

% Plot Curves
if(plotResult)
    figure(fhdlRoadType),clf
    x = (1:(Nsim))*TC.L/1e3;
    hmmplot(v_sim*3.6,zsim,x,[1 4],'Simulated speed','Signed speed (km/h)',1)
    xlabel('Distance (km)')
    col = get(gca,'ColorOrder');
    for k=1:length(csim.z)
        plot(csim.x(k)/1e3,4,'+','Color',col(csim.z(k),:)), hold on
    end
    hold off
    
    figure(fhdlTopography),clf
    x = (1:(Nsim))*TC.L/1e3;
    hmmplot(ysim,zsim,x,[1 4],'Simulated topography','Slope (%)',1)
    xlabel('Distance (km)')
    %hold on, plot(csim.x/1e3,4,'k+'), hold off
    col = get(gca,'ColorOrder');
    for k=1:length(csim.z)
        plot(csim.x(k)/1e3,4,'+','Color',col(csim.z(k),:)), hold on
    end
    hold off

    fhdlCurvature = pfig;
    col = get(gca,'ColorOrder');
    for k=1:length(csim.z)
        stem(csim.x(k)/1e3,csim.R(k),'Color',col(csim.z(k),:)), hold on
    end
    hold off
    title('Curve position and radius')
    xlabel('Distance (km)')
    ylabel('Curve radius (m)')

end


%% SIMULATE SPEED BUMPS

PAR=Bump;
INT = [PAR{1}.int PAR{2}.int PAR{3}.int]*1000; % multiplied by 1000 to convert into 1/km 
yy = poissim(zsim,TC.L,INT);

if(isempty(yy))
    yy.v = [];
    yy.z = [];
    yy.x = [];
else
    for k=1:length(yy.x)
        zk=yy.z(k);              % Road type
        Park = PAR{zk};             % Parameters for road section
        vv = Park.v;
        yy.v(k) = rand*(vv(2)-vv(1))+vv(1);
    end
end
bump_sim=yy;


%% SIMULATE STOP SIGNS

PAR=Stop;
INT = [PAR{1}.int PAR{2}.int PAR{3}.int]*1000;  % multiplied by 1000 to convert into 1/km 
yy = poissim(zsim,TC.L,INT);

if(isempty(yy))
    yy.t = [];
    yy.z = [];
    yy.x = [];
else
    for k=1:length(yy.x)
        zk=yy.z(k);              % Road type
        Park = PAR{zk};          % Parameters for road section
        t = Park.t;
        yy.t(k) = rand*(t(2)-t(1)) + t(1);
    end
end

stop_sim = yy;


%% PLOT DETAILS, IF REQUESTED

if(plotResult)
    figure(fhdlRoadType),clf
    x = (1:(Nsim))*TC.L/1e3;
    hmmplot(v_sim*3.6,zsim,x,[1 6],'Simulated speed','Signed speed (km\,h^{-1})',1)
    xlabel('Distance / km')
    col = get(gca,'ColorOrder');
    % Plot curves
    for k=1:length(csim.z)
        plot(csim.x(k)/1e3,4,'+','Color',col(csim.z(k),:)), hold on
    end
    % Plot speed bumps
    for k=1:length(bump_sim.z)
        plot(bump_sim.x(k)/1e3,5,'x','Color',col(bump_sim.z(k),:)), hold on
    end
    % Plot stops
    for k=1:length(stop_sim.z)
        plot(stop_sim.x(k)/1e3,6,'o','Color',col(stop_sim.z(k),:)), hold on
    end
    hold off

    % Plot simulated OC
    figure(fhdlTopography),clf
    x = (1:(Nsim))*TC.L/1e3;
    hmmplot(ysim,zsim,x,[1 6],'Simulated topography','Road grade (\%)',1)
    xlabel('Distance / km')
    col = get(gca,'ColorOrder');
    % Plot curves
    for k=1:length(csim.z)
        plot(csim.x(k)/1e3,4,'+','Color',col(csim.z(k),:)), hold on
    end
    % Plot speed bumps
    for k=1:length(bump_sim.z)
        plot(bump_sim.x(k)/1e3,5,'x','Color',col(bump_sim.z(k),:)), hold on
    end
    % Plot stops
    for k=1:length(stop_sim.z)
        plot(stop_sim.x(k)/1e3,6,'o','Color',col(stop_sim.z(k),:)), hold on
    end

    % Plot Curves
%     figure(102), clf
%     col = get(gca,'ColorOrder');
%     for k=1:length(csim.z)
%         stem(csim.x(k)/1e3,csim.R(k),'Color',col(csim.z(k),:)), hold on
%     end
%     hold off
%     title('OC signal: Curve radius'), xlabel('Distance / km')

    % Plot Seed bumps and Stops
    fhdlStopAndBump = pfig;
    col = get(gca,'ColorOrder');
    for k=1:length(bump_sim.z)
        bmpHdl = stem(bump_sim.x(k)/1e3,bump_sim.v(k),'Color',col(bump_sim.z(k),:));
        hold on;
    end
    title('Simulated speed bumps and stop signs'), ...
        ylabel('Speed, recommended (km/h)')
    for k=1:length(stop_sim.z)
        stpHdl = plot(stop_sim.x(k)/1e3,-.1,'*','Color',col(stop_sim.z(k),:));
        hold on;
    end
    xlabel('Distance (km)')
%     legend([bmpHdl(1),stpHdl(1)],{'Speed bump','Stop sign'});
    axis([0 100 -5 45])
    
    FigHandle.Speed = fhdlRoadType;
    FigHandle.Topography = fhdlTopography;
    FigHandle.Curvature = fhdlCurvature;
    FigHandle.StopAndBump = fhdlStopAndBump;
end


%% CONVERT TIME SERIES TO dOC FORMAT
allowDynamicRiseLength = true;

% Create empty OC
[OperatingCycle,ocVersion] = CreateEmptyOC();

% Assign legal speed
nbrPts = size(zsim,1);
ds = TC.L;
position = ds*(0:nbrPts)';
reducedPosition = position(1:end-1);
speedMagnitude = v_sim(:);
isChanged = [true; abs(diff(speedMagnitude)) > 0];
positionAtChange = reducedPosition(isChanged); 
endPosition = reducedPosition(end) + ds;
endSpeed = speedMagnitude(end);
positionLegalSpeed = [positionAtChange; endPosition];
magnitudeLegalSpeed = [speedMagnitude(isChanged); endSpeed];
legalSpeed = [positionLegalSpeed, magnitudeLegalSpeed];
OperatingCycle.Road.LegalSpeed.Data = legalSpeed;

% Assign topography
dz = ds*(ysim(:)/100)./sqrt(1+(ysim(:)/100).^2);
altitude = [0; cumsum(dz)];
OperatingCycle.Road.Topography.Data = [position,altitude];

% Assign stop signs
% nbrStopSign = size(stop_sim.x,1);
tmp = OperatingCycle.Road.StopSign.Data;
% OperatingCycle.Road.StopSign.Data = [tmp; ...
%     [stop_sim.x(:),stopSignStandstillTime*ones(nbrStopSign,1)]];
OperatingCycle.Road.StopSign.Data = [tmp; ...
    [stop_sim.x(:),round(stop_sim.t(:))]];

% Assign curvature
if(isempty(csim.x))
    OperatingCycle.Road.Curvature.Data = [0,0];
else
    whichRoadCurvatureAlgorithm = 2;
    csimTruncated = CutAwayCurveOutsideMission(csim,TC.Ltot);
    generatedCurvature = GetRoadCurvature(csimTruncated, TC.Ltot, allowDynamicRiseLength, ...
        whichRoadCurvatureAlgorithm, minimumCurveLength);
    isBelowMissionLength = (generatedCurvature(:,1) <= TC.Ltot);
    generatedCurvature = generatedCurvature(isBelowMissionLength,:);
    doAddFinalEntry = (generatedCurvature(end,1) < TC.Ltot);
    tmp = [TC.Ltot,0];
    OperatingCycle.Road.Curvature.Data = ...
        [generatedCurvature; tmp(doAddFinalEntry,:)];
%     OperatingCycle.Road.Curvature.Data = GetRoadCurvature(csim);
    % -- debugg starts --
    tmp = diff(OperatingCycle.Road.Curvature.Data(:,1));
    isNonMonotonic = sum(tmp <=0);
    if(isNonMonotonic)
       save('..\data\debug_nonMonotonic','csim','OperatingCycle','TC',...
           'allowDynamicRiseLength');
       error('Curvature signal is not monotonic, debug file added (data/debug_nonMonotonic')
    end
    % -- deubg ends --
end

% Assign speed bumps
bumpAngle = round(angleSpeedFactor*90./((bump_sim.v(:) + ...
    angleSpeedFactor)),1); % 90 because degrees instead of radians

speedBumpArray = zeros(size(bumpAngle,1),...
    speedBumpParameterDimension);
speedBumpArray(:,1) = bump_sim.x(:);
speedBumpArray(:,2) = defaultBumpLength;
speedBumpArray(:,3) = defaultBumpHeight;
speedBumpArray(:,4) = bumpAngle;
if(~isempty(speedBumpArray))
    OperatingCycle.Road.SpeedBump.Data = speedBumpArray;
end % Default value already specified

% Assign mission field
tmp = OperatingCycle.Mission.MissionMatrix;
tmp(1,end) = missionStopStandstillTime;
missionMatrix = [tmp; tmp];
missionMatrix(end,1) = TC.Ltot;
missionMatrix(:,3) = cargoWeight;
OperatingCycle.Mission = DistributeMissionParameters(missionMatrix);

varargout{1} = ocVersion;
varargout{2} = FigHandle;
varargout{3} = zsim;
varargout{4} = csim;
varargout{5} = ysim;

if(doSaveRawData)
    dateToday = char(datetime('today','Format','yyyyMMdd'));
    pathname = '..\data\';
    savename = ['DataForFiguresInSection4RawSequences_' dateToday];
    save([pathname savename], 'zsim','v_sim','ysim','csim','bump_sim',...
        'stop_sim','Nsim','TC','StatisticalOC'); 
end

end
