1 files changed, 237 insertions, 132 deletions

diff --git a/src/yapf/yapf_costrail.hpp b/src/yapf/yapf_costrail.hpp
index 9dc2cc701..db6fb8d1a 100644
--- a/src/yapf/yapf_costrail.hpp
+++ b/src/yapf/yapf_costrail.hpp
@@ -19,8 +19,43 @@ public:
 	typedef typename Node::CachedData CachedData;
 
 protected:
+
+	/* Structure used inside PfCalcCost() to keep basic tile information. */
+	struct TILE {
+		TileIndex   tile;
+		Trackdir    td;
+		TileType    tile_type;
+		RailType    rail_type;
+
+		TILE()
+		{
+			tile = INVALID_TILE;
+			td = INVALID_TRACKDIR;
+			tile_type = MP_VOID;
+			rail_type = INVALID_RAILTYPE;
+		}
+
+		TILE(TileIndex tile, Trackdir td)
+		{
+			this->tile = tile;
+			this->td = td;
+			this->tile_type = GetTileType(tile);
+			this->rail_type = GetTileRailType(tile);
+		}
+
+		TILE(const TILE &src)
+		{
+			tile = src.tile;
+			td = src.td;
+			tile_type = src.tile_type;
+			rail_type = src.rail_type;
+		}
+	};
+
+protected:
 	int           m_max_cost;
 	CBlobT<int>   m_sig_look_ahead_costs;
+
 public:
 	bool          m_stopped_on_first_two_way_signal;
 protected:
@@ -65,8 +100,8 @@ public:
 		return cost;
 	}
 
-	/** return one tile cost. If tile is a tunnel entry, it is moved to the end of tunnel */
-	FORCEINLINE int OneTileCost(TileIndex prev_tile, TileIndex& tile, Trackdir trackdir)
+	/** Return one tile cost (base cost + level crossing penalty). */
+	FORCEINLINE int OneTileCost(TileIndex& tile, Trackdir trackdir)
 	{
 		int cost = 0;
 		// set base cost
@@ -99,7 +134,7 @@ public:
 			bool has_signal_along = HasSignalOnTrackdir(tile, trackdir);
 			if (has_signal_against && !has_signal_along) {
 				// one-way signal in opposite direction
-				n.m_segment->flags_u.flags_s.m_end_of_line = true;
+				n.m_segment->m_end_segment_reason |= ESRB_DEAD_END;
 			} else if (has_signal_along) {
 				SignalState sig_state = GetSignalStateByTrackdir(tile, trackdir);
 				// cache the look-ahead polynomial constant only if we didn't pass more signals than the look-ahead limit is
@@ -117,7 +152,7 @@ public:
 					// was it first signal which is two-way?
 					if (Yapf().TreatFirstRedTwoWaySignalAsEOL() && n.flags_u.flags_s.m_choice_seen && has_signal_against && n.m_num_signals_passed == 0) {
 						// yes, the first signal is two-way red signal => DEAD END
-						n.m_segment->flags_u.flags_s.m_end_of_line = true;
+						n.m_segment->m_end_segment_reason |= ESRB_DEAD_END;
 						Yapf().m_stopped_on_first_two_way_signal = true;
 						return -1;
 					}
@@ -170,182 +205,241 @@ public:
 public:
 	FORCEINLINE void SetMaxCost(int max_cost) {m_max_cost = max_cost;}
 
+
+
 	/** Called by YAPF to calculate the cost from the origin to the given node.
 	 *  Calculates only the cost of given node, adds it to the parent node cost
 	 *  and stores the result into Node::m_cost member */
-	FORCEINLINE bool PfCalcCost(Node &n, const TrackFollower &tf)
+	FORCEINLINE bool PfCalcCost(Node &n, const TrackFollower *tf)
 	{
 		assert(!n.flags_u.flags_s.m_targed_seen);
+		assert(tf->m_new_tile == n.m_key.m_tile);
+		assert((TrackdirToTrackdirBits(n.m_key.m_td) & tf->m_new_td_bits) != TRACKDIR_BIT_NONE);
+
 		CPerfStart perf_cost(Yapf().m_perf_cost);
-		int parent_cost = (n.m_parent != NULL) ? n.m_parent->m_cost : 0;
-		int first_tile_cost = 0;
-		int segment_cost = 0;
+
+		/* Does the node have some parent node? */
+		bool has_parent = (n.m_parent != NULL);
+
+		/* Do we already have a cached segment? */
+		CachedData &segment = *n.m_segment;
+		bool is_cached_segment = (segment.m_cost >= 0);
+
+		int parent_cost = has_parent ? n.m_parent->m_cost : 0;
+
+		/* Each node cost contains 2 or 3 main components:
+		 *  1. Transition cost - cost of the move from previous node (tile):
+		 *    - curve cost (or zero for straight move)
+		 *  2. Tile cost:
+		 *    - base tile cost
+		 *      - YAPF_TILE_LENGTH for diagonal tiles
+		 *      - YAPF_TILE_CORNER_LENGTH for non-diagonal tiles
+		 *    - tile penalties
+		 *      - tile slope penalty (upward slopes)
+		 *      - red signal penalty
+		 *      - level crossing penalty
+		 *      - speed-limit penalty (bridges)
+		 *      - station platform penalty
+		 *      - penalty for reversing in the depot
+		 *      - etc.
+		 *  3. Extra cost (applies to the last node only)
+		 *    - last red signal penalty
+		 *    - penalty for too long or too short platform on the destination station
+		 */
+		int transition_cost = 0;
+		int tile_cost = 0;
 		int extra_cost = 0;
+
+		/* Segment: one or more tiles connected by contiguous tracks of the same type.
+		 * Each segment cost includes 'Tile cost' for all its tiles (including the first
+		 * and last), and the 'Transition cost' between its tiles. The first transition
+		 * cost of segment entry (move from the 'parent' node) is not included!
+		 */
+		int segment_entry_cost = 0;
+		int segment_cost = 0;
+
 		const Vehicle* v = Yapf().GetVehicle();
 
 		// start at n.m_key.m_tile / n.m_key.m_td and walk to the end of segment
-		TileIndex prev_tile      = (n.m_parent != NULL) ? n.m_parent->GetLastTile() : INVALID_TILE;
-		Trackdir  prev_trackdir  = (n.m_parent != NULL) ? n.m_parent->GetLastTrackdir() : INVALID_TRACKDIR;
-		TileType  prev_tile_type = (n.m_parent != NULL) ? GetTileType(n.m_parent->GetLastTile()) : MP_VOID;
+		TILE cur(n.m_key.m_tile, n.m_key.m_td);
 
-		TileIndex tile = n.m_key.m_tile;
-		Trackdir trackdir = n.m_key.m_td;
-		TileType tile_type = GetTileType(tile);
+		// the previous tile will be needed for transition cost calculations
+		TILE prev = has_parent ? TILE() : TILE(n.m_parent->GetLastTile(), n.m_parent->GetLastTrackdir());
 
-		RailType rail_type = GetTileRailType(tile);
+		EndSegmentReasonBits end_segment_reason = ESRB_NONE;
 
-		bool target_seen = Yapf().PfDetectDestination(tile, trackdir);
-		bool end_by_target_seen = false;
+		TrackFollower tf_local(v, &Yapf().m_perf_ts_cost);
 
-		if (tf.m_is_station) {
-			// station tiles have an extra penalty
-			segment_cost += Yapf().PfGetSettings().rail_station_penalty * (tf.m_tiles_skipped + 1);
+		if (!has_parent) {
+			/* We will jump to the middle of the cost calculator assuming that segment cache is not used. */
+			assert(!is_cached_segment);
+			/* Skip the first transition cost calculation. */
+			goto no_entry_cost;
 		}
 
-		while (true) {
-			segment_cost += Yapf().OneTileCost(prev_tile, tile, trackdir);
-			segment_cost += Yapf().CurveCost(prev_trackdir, trackdir);
-			segment_cost += Yapf().SlopeCost(tile, trackdir);
-			segment_cost += Yapf().SignalCost(n, tile, trackdir);
-			if (n.m_segment->flags_u.flags_s.m_end_of_line) {
-				break;
-			}
-
-			// finish if we have reached the destination
-			if (target_seen) {
-				end_by_target_seen = true;
-				break;
-			}
-
-			// finish on first station tile - segment should end here to avoid target skipping
-			// when cached segments are used
-			if (tile_type == MP_STATION && prev_tile_type != MP_STATION) {
-				break;
+		for (;;) {
+			/* Transition cost (cost of the move from previous tile) */
+			transition_cost = Yapf().CurveCost(prev.td, cur.td);
+
+			/* First transition cost counts against segment entry cost, other transitions
+			 * inside segment will come to segment cost (and will be cached) */
+			if (segment_cost == 0) {
+				/* We just entered the loop. First transition cost goes to segment entry cost)*/
+				segment_entry_cost = transition_cost;
+				transition_cost = 0;
+
+				/* It is the right time now to look if we can reuse the cached segment cost. */
+				if (is_cached_segment) {
+					/* Yes, we already know the segment cost. */
+					segment_cost = segment.m_cost;
+					/* We know also the reason why the segment ends. */
+					end_segment_reason = segment.m_end_segment_reason;
+					/* No further calculation needed. */
+					cur = TILE(n.GetLastTile(), n.GetLastTrackdir());
+					break;
+				}
+			} else {
+				/* Other than first transition cost count as the regular segment cost. */
+				segment_cost += transition_cost;
 			}
 
-			// finish also on waypoint - same workaround as for first station tile
-			if (tile_type == MP_RAILWAY && IsRailWaypoint(tile)) {
-				break;
-			}
+no_entry_cost: // jump here at the beginning if the node has no parent (it is the first node)
 
-			// if there are no reachable trackdirs on the next tile, we have end of road
-			TrackFollower F(v, &Yapf().m_perf_ts_cost);
-			if (!F.Follow(tile, trackdir)) {
-				// we can't continue?
-				// n.m_segment->flags_u.flags_s.m_end_of_line = true;
-				break;
-			}
+			/* All other tile costs will be calculated here. */
+			segment_cost += Yapf().OneTileCost(cur.tile, cur.td);
 
-			// if there are more trackdirs available & reachable, we are at the end of segment
-			if (KillFirstBit2x64(F.m_new_td_bits) != 0) {
-				break;
-			}
+			/* If we skipped some tunnel/bridge/station tiles, add their base cost */
+			segment_cost += YAPF_TILE_LENGTH * tf->m_tiles_skipped;
 
-			Trackdir new_td = (Trackdir)FindFirstBit2x64(F.m_new_td_bits);
+			/* Slope cost. */
+			segment_cost += Yapf().SlopeCost(cur.tile, cur.td);
 
-			{
-				// end segment if train is about to enter simple loop with no junctions
-				// so next time it should stop on the next if
-				if (segment_cost > s_max_segment_cost && IsTileType(F.m_new_tile, MP_RAILWAY))
-					break;
+			/* Signal cost (routine can modify segment data). */
+			segment_cost += Yapf().SignalCost(n, cur.tile, cur.td);
+			end_segment_reason = segment.m_end_segment_reason;
 
-				// stop if train is on simple loop with no junctions
-				if (F.m_new_tile == n.m_key.m_tile && new_td == n.m_key.m_td)
-					return false;
+			/* Tests for 'potential target' reasons to close the segment. */
+			if (cur.tile == prev.tile) {
+				/* Penalty for reversing in a depot. */
+				assert(IsRailDepot(cur.tile));
+				segment_cost += Yapf().PfGetSettings().rail_depot_reverse_penalty;
+				/* We will end in this pass (depot is possible target) */
+				end_segment_reason |= ESRB_DEPOT;
+
+			} else if (tf->m_is_station) {
+				/* Station penalties. */
+				uint platform_length = tf->m_tiles_skipped + 1;
+				/* We don't know yet if the station is our target or not. Act like
+				 * if it is pass-through station (not our destination). */
+				segment_cost += Yapf().PfGetSettings().rail_station_penalty * platform_length;
+				/* We will end in this pass (station is possible target) */
+				end_segment_reason |= ESRB_STATION;
+
+			} else if (cur.tile_type == MP_RAILWAY && IsRailWaypoint(cur.tile)) {
+				/* Waypoint is also a good reason to finish. */
+				end_segment_reason |= ESRB_WAYPOINT;
 			}
 
-			// if tail type changes, finish segment (cached segment can't contain more rail types)
+			/* Apply min/max speed penalties only when inside the look-ahead radius. Otherwise
+			 * it would cause desync in MP. */
+			if (n.m_num_signals_passed < m_sig_look_ahead_costs.Size())
 			{
-				RailType new_rail_type = GetTileRailType(F.m_new_tile);
-				if (new_rail_type != rail_type) {
-					break;
-				}
-				rail_type = new_rail_type;
+				int min_speed = 0;
+				int max_speed = tf->GetSpeedLimit(&min_speed);
+				if (max_speed < v->max_speed)
+					extra_cost += YAPF_TILE_LENGTH * (v->max_speed - max_speed) * (4 + tf->m_tiles_skipped) / v->max_speed;
+				if (min_speed > v->max_speed)
+					extra_cost += YAPF_TILE_LENGTH * (min_speed - v->max_speed);
 			}
 
-			// move to the next tile
-			prev_tile = tile;
-			prev_trackdir = trackdir;
-			prev_tile_type = tile_type;
+			/* Finish if we already exceeded the maximum path cost (i.e. when
+			 * searching for the nearest depot). */
+			if (m_max_cost > 0 && (parent_cost + segment_entry_cost + segment_cost) > m_max_cost) {
+				end_segment_reason |= ESRB_PATH_TOO_LONG;
+			}
 
-			tile = F.m_new_tile;
-			trackdir = new_td;
-			tile_type = GetTileType(tile);
+			/* Move to the next tile/trackdir. */
+			tf = &tf_local;
+			tf_local.Init(v, &Yapf().m_perf_ts_cost);
 
-			target_seen = Yapf().PfDetectDestination(tile, trackdir);
+			if (!tf_local.Follow(cur.tile, cur.td)) {
+				/* Can't move to the next tile (EOL?). */
+				end_segment_reason |= ESRB_DEAD_END;
+				break;
+			}
 
-			// reversing in depot penalty
-			if (tile == prev_tile) {
-				segment_cost += Yapf().PfGetSettings().rail_depot_reverse_penalty;
+			/* Check if the next tile is not a choice. */
+			if (KillFirstBit2x64(tf_local.m_new_td_bits) != 0) {
+				/* More than one segment will follow. Close this one. */
+				end_segment_reason |= ESRB_CHOICE_FOLLOWS;
 				break;
 			}
 
-			// if we skipped some tunnel tiles, add their cost
-			segment_cost += YAPF_TILE_LENGTH * F.m_tiles_skipped;
+			/* Gather the next tile/trackdir/tile_type/rail_type. */
+			TILE next(tf_local.m_new_tile, (Trackdir)FindFirstBit2x64(tf_local.m_new_td_bits));
 
-			// add penalty for skipped station tiles
-			if (F.m_is_station)
-			{
-				uint platform_length = F.m_tiles_skipped + 1;
-				if (target_seen) {
-					// it is our destination station
-					segment_cost += PlatformLengthPenalty(platform_length);
-				} else {
-					// station is not our destination station, apply penalty for skipped platform tiles
-					segment_cost += Yapf().PfGetSettings().rail_station_penalty * platform_length;
-				}
+			/* Check the next tile for the rail type. */
+			if (next.rail_type != cur.rail_type) {
+				/* Segment must consist from the same rail_type tiles. */
+				end_segment_reason |= ESRB_RAIL_TYPE;
+				break;
 			}
 
-			// add min/max speed penalties
-			int min_speed = 0;
-			int max_speed = F.GetSpeedLimit(&min_speed);
-			if (max_speed < v->max_speed)
-				segment_cost += YAPF_TILE_LENGTH * (v->max_speed - max_speed) / v->max_speed;
-			if (min_speed > v->max_speed)
-				segment_cost += YAPF_TILE_LENGTH * (min_speed - v->max_speed);
-
-			// finish if we already exceeded the maximum cost
-			if (m_max_cost > 0 && (parent_cost + first_tile_cost + segment_cost) > m_max_cost) {
-				return false;
+			/* Avoid infinite looping. */
+			if (next.tile == n.m_key.m_tile && next.td == n.m_key.m_td) {
+				end_segment_reason |= ESRB_INFINITE_LOOP;
+				break;
 			}
 
-			if (first_tile_cost == 0) {
-				// we just have done first tile
-				first_tile_cost = segment_cost;
-				segment_cost = 0;
-
-				// look if we can reuse existing (cached) segment cost
-				if (n.m_segment->m_cost >= 0) {
-					// reuse the cached segment cost
+			if (segment_cost > s_max_segment_cost) {
+				/* Potentially in the infinite loop (or only very long segment?). We should
+				 * not force it to finish prematurely unless we are on a regular tile. */
+				if (IsTileType(tf->m_new_tile, MP_RAILWAY)) {
+					end_segment_reason |= ESRB_SEGMENT_TOO_LONG;
 					break;
 				}
 			}
-			// segment cost was not filled yes, we have not cached it yet
-			n.SetLastTileTrackdir(tile, trackdir);
 
-		} // while (true)
+			/* Any other reason bit set? */
+			if (end_segment_reason != ESRB_NONE) {
+				break;
+			}
+
+			/* For the next loop set new prev and cur tile info. */
+			prev = cur;
+			cur = next;
+
+		} // for (;;)
 
-		if (first_tile_cost == 0) {
-			// we have just finished first tile
-			first_tile_cost = segment_cost;
-			segment_cost = 0;
+		bool target_seen = false;
+		if ((end_segment_reason & ESRB_POSSIBLE_TARGET) != ESRB_NONE) {
+			/* Depot, station or waypoint. */
+			if (Yapf().PfDetectDestination(cur.tile, cur.td)) {
+				/* Destination found. */
+				target_seen = true;
+			}
 		}
 
-		// do we have cached segment cost?
-		if (n.m_segment->m_cost >= 0) {
-			// reuse the cached segment cost
-			segment_cost = n.m_segment->m_cost;
-		} else {
-			// save segment cost
-			n.m_segment->m_cost = segment_cost;
+		/* Update the segment if needed. */
+		if (!is_cached_segment) {
+			/* Write back the segment information so it can be reused the next time. */
+			segment.m_cost = segment_cost;
+			segment.m_end_segment_reason = end_segment_reason & ESRB_CACHED_MASK;
+			assert(segment.m_end_segment_reason != ESRB_NONE);
+			/* Save end of segment back to the node. */
+			n.SetLastTileTrackdir(cur.tile, cur.td);
+		}
 
-			// save end of segment back to the node
-			n.SetLastTileTrackdir(tile, trackdir);
+		/* Do we have an excuse why not to continue pathfinding in this direction? */
+		if (!target_seen && (end_segment_reason & ESRB_ABORT_PF_MASK) != ESRB_NONE) {
+			/* Reason to not continue. Stop this PF branch. */
+			return false;
 		}
 
-		// special costs for the case we have reached our target
-		if (end_by_target_seen) {
+		/* Special costs for the case we have reached our target. */
+		if (target_seen) {
 			n.flags_u.flags_s.m_targed_seen = true;
+			/* Last-red and last-red-exit penalties. */
 			if (n.flags_u.flags_s.m_last_signal_was_red) {
 				if (n.m_last_red_signal_type == SIGTYPE_EXIT) {
 					// last signal was red pre-signal-exit
@@ -355,12 +449,23 @@ public:
 					extra_cost += Yapf().PfGetSettings().rail_lastred_penalty;
 				}
 			}
+
+			/* Station platform-length penalty. */
+			if ((end_segment_reason & ESRB_STATION) != ESRB_NONE) {
+				Station *st = GetStationByTile(n.GetLastTile());
+				assert(st != NULL);
+				uint platform_length = st->GetPlatformLength(n.GetLastTile(), ReverseDiagDir(TrackdirToExitdir(n.GetLastTrackdir())));
+				/* Reduce the extra cost caused by passing-station penalty (each station receives it in the segment cost). */
+				extra_cost -= Yapf().PfGetSettings().rail_station_penalty * platform_length;
+				/* Add penalty for the inappropriate platform length. */
+				extra_cost += PlatformLengthPenalty(platform_length);
+			}
 		}
 
 		// total node cost
-		n.m_cost = parent_cost + first_tile_cost + segment_cost + extra_cost;
+		n.m_cost = parent_cost + segment_entry_cost + segment_cost + extra_cost;
 
-		return !n.m_segment->flags_u.flags_s.m_end_of_line || end_by_target_seen;
+		return true;
 	}
 
 	FORCEINLINE bool CanUseGlobalCache(Node& n) const