/// <summary>
/// Get the ending of road state
/// </summary>
/// <param name="roadState">Road state</param>
/// <param name="timeState">Time state</param>
/// <returns>Ending of road</returns>
RoadEnding GetEndingOfRoad(List<CarState> roadState, CollideTimeState timeState)
{
RoadEnding ending = new RoadEnding();
CollideTimeState aState = this.GetCollideTimeState(roadState, timeState.Time, timeState.CarAIndex);
CollideTimeState bState = this.GetCollideTimeState(roadState, timeState.Time, timeState.CarBIndex);
CollideTimeState maxState = null;
bool isAStateBetter = true;
if (this.CompareNegativeAsInfinity(aState.Time - timeState.Time, bState.Time - timeState.Time) > 0) // aState is better
{
maxState = aState;
isAStateBetter = true;
}
else
{
maxState = bState;
isAStateBetter = false;
}
// t > 0, should recursive call to solve
// t = 0, it's going to crash
// t < 0, cheers!
if (maxState.Time - timeState.Time > 0)
{
if (isAStateBetter && timeState.CarAIndex >= 0)
{
roadState[timeState.CarAIndex].ChangeLane();
}
else if(timeState.CarBIndex >= 0)
{
roadState[timeState.CarBIndex].ChangeLane();
}
// Recursive call
ending = this.GetEndingOfRoad(
roadState,
maxState
);
}
else if (maxState.Time - timeState.Time == 0)
{
// BAD END
ending.CanSafeDriveForever = false;
ending.TimeBeforeCollide = timeState.Time;
}
else
{
// GOOD END
ending.CanSafeDriveForever = true;
ending.TimeBeforeCollide = -1;
}
return ending;
}
/// <summary>
/// Get the max time before collide
/// </summary>
/// <param name="roadState">Road state</param>
/// <param name="time">Time passed</param>
/// <param name="carIndexChangingLane">Index of car that changing lane</param>
/// <returns>Time state</returns>
CollideTimeState GetCollideTimeState(List<CarState> roadState, double time, Int32 carIndexChangingLane)
{
CollideTimeState maxTimeState = new CollideTimeState(time, -1, -1);
double?[] minTimeBeforeCollide = new double?[2];
Int32[][] carIndex = new Int32[2][];
carIndex[0] = new Int32[2];
carIndex[1] = new Int32[2];
// If index not in road, treat as bad end
// But if index < 0, that means do-not-change-lane-of-any-car
// Currently this only happend at the begainning of process
if (carIndexChangingLane < roadState.Count)
{
// Try change lane
if (carIndexChangingLane >= 0)
{
roadState[carIndexChangingLane].ChangeLane();
}
List<List<CarState>> carsOnLanes = new List<List<CarState>>(2)
{
(from car in roadState
where car.Lane == CarState.LaneSide.Left
orderby car.PositionAt(time)
select car)
.ToList(),
(from car in roadState
where car.Lane == CarState.LaneSide.Right
orderby car.PositionAt(time)
select car)
.ToList()
};
for (int i = 0; i < carsOnLanes.Count; i++)
{
for (int j = 0; j < carsOnLanes[i].Count; j++)
{
if (j == carsOnLanes[i].Count - 1) // The last car
{
minTimeBeforeCollide[i] = (minTimeBeforeCollide[i] < 0 || !minTimeBeforeCollide[i].HasValue) ?
-1 :
minTimeBeforeCollide[i];
}
else
{
if (carsOnLanes[i][j].Speed > carsOnLanes[i][j + 1].Speed)
{
double distance = carsOnLanes[i][j + 1].PositionAt(time) > carsOnLanes[i][j].PositionAt(time) + carsOnLanes[i][j].Length ?
carsOnLanes[i][j + 1].PositionAt(time) - (carsOnLanes[i][j].PositionAt(time) + carsOnLanes[i][j].Length)
: 0;
double timeBeforeCollide = distance / (carsOnLanes[i][j].Speed - carsOnLanes[i][j + 1].Speed);
if (minTimeBeforeCollide[i].HasValue)
{
// Only store the minimum collide time
if (this.CompareNegativeAsInfinity(timeBeforeCollide, minTimeBeforeCollide[i].Value) < 0)
{
minTimeBeforeCollide[i] = timeBeforeCollide;
carIndex[i][0] = carsOnLanes[i][j].ID;
carIndex[i][1] = carsOnLanes[i][j + 1].ID;
}
}
else
{
minTimeBeforeCollide[i] = timeBeforeCollide;
carIndex[i][0] = carsOnLanes[i][j].ID;
carIndex[i][1] = carsOnLanes[i][j + 1].ID;
}
}
else
{
double distance = carsOnLanes[i][j + 1].PositionAt(time) - (carsOnLanes[i][j].PositionAt(time) + carsOnLanes[i][j].Length);
if (distance >= 0)
{
minTimeBeforeCollide[i] = (minTimeBeforeCollide[i] < 0 || !minTimeBeforeCollide[i].HasValue) ?
-1
: minTimeBeforeCollide[i];
}
else
{
minTimeBeforeCollide[i] = 0;
}
}
}
}
}
// Change back
if (carIndexChangingLane >= 0)
{
roadState[carIndexChangingLane].ChangeLane();
}
}
bool leftLaneIsMoreDangerous = false;
bool atLeastOneLaneIsDangerous = false;
if (minTimeBeforeCollide[0].HasValue
&& minTimeBeforeCollide[1].HasValue)
{
atLeastOneLaneIsDangerous = true;
if (this.CompareNegativeAsInfinity(minTimeBeforeCollide[0].Value, minTimeBeforeCollide[1].Value) < 0)
{
leftLaneIsMoreDangerous = true;
}
else
{
leftLaneIsMoreDangerous = false;
}
}
else if(minTimeBeforeCollide[0].HasValue || minTimeBeforeCollide[1].HasValue)
{
atLeastOneLaneIsDangerous = true;
if (minTimeBeforeCollide[0].HasValue)
{
leftLaneIsMoreDangerous = true;
}
else
{
leftLaneIsMoreDangerous = false;
}
}
if (atLeastOneLaneIsDangerous)
{
double minTime = 0;
if (leftLaneIsMoreDangerous)
{
minTime = minTimeBeforeCollide[0].Value;
maxTimeState.CarAIndex = carIndex[0][0];
maxTimeState.CarBIndex = carIndex[0][1];
}
else
{
minTime = minTimeBeforeCollide[1].Value;
maxTimeState.CarAIndex = carIndex[1][0];
maxTimeState.CarBIndex = carIndex[1][1];
}
if (minTime < 0)
{
maxTimeState.Time += -1;
}
else
{
maxTimeState.Time += minTime;
}
}
return maxTimeState;
}
