(svn r2635) Fix: [ntp/misc] Improve the old pathfinder. Changed it to A* instead of Dijkstra.

  - Benchmark shows that NTP is now around 10x faster than NPF.
  - Made IsTunnelTile macro to determine if a tile is a tunnel.
  - Added some useful debugging functions for making tiles red / getting accurate timestamps.
  - Remove old depot finding algorithm.
  - Disable warning for signed/unsigned comparisons.

13 files changed, 333 insertions, 279 deletions

diff --git a/debug.c b/debug.c
index a02e750fa..9987c2822 100644
--- a/debug.c
+++ b/debug.c
@@ -15,6 +15,7 @@ int _debug_net_level;
 int _debug_spritecache_level;
 int _debug_oldloader_level;
 int _debug_pbs_level;
+int _debug_ntp_level;
 #ifdef GPMI
 int _debug_gpmi_level;
 #endif /* GPMI */
@@ -49,6 +50,7 @@ typedef struct DebugLevel {
 	DEBUG_LEVEL(spritecache),
 	DEBUG_LEVEL(oldloader),
 	DEBUG_LEVEL(pbs),
+	DEBUG_LEVEL(ntp),
 #ifdef GPMI
 	DEBUG_LEVEL(gpmi),
 #endif
diff --git a/debug.h b/debug.h
index 1bfe990ba..d67b54aa0 100644
--- a/debug.h
+++ b/debug.h
@@ -15,6 +15,7 @@
 	extern int _debug_spritecache_level;
 	extern int _debug_oldloader_level;
 	extern int _debug_pbs_level;
+	extern int _debug_ntp_level;
 #ifdef GPMI
 	extern int _debug_gpmi_level;
 #endif /* GPMI */
diff --git a/functions.h b/functions.h
index 74d6980f4..ea755ab76 100644
--- a/functions.h
+++ b/functions.h
@@ -128,8 +128,8 @@ uint InteractiveRandomRange(uint max);
 
 
 // Used for profiling
-#define TIC() { extern uint32 rdtsc(void); uint32 _xxx_ = rdtsc();
-#define TOC(s) 	_xxx_ = rdtsc() - _xxx_; printf("%s: %d\n", s, _xxx_); }
+#define TIC() { extern uint32 rdtsc(void); uint32 _xxx_ = rdtsc(); static float __avg__;
+#define TOC(s) 	_xxx_ = rdtsc() - _xxx_; __avg__=__avg__*0.99+_xxx_*0.01; printf("%s: %8d %f\n", s, _xxx_,__avg__); }
 
 
 void SetDate(uint date);
diff --git a/pathfind.c b/pathfind.c
index d2d1fd250..5290b86f1 100644
--- a/pathfind.c
+++ b/pathfind.c
@@ -45,7 +45,6 @@ static bool TPFSetTileBit(TrackPathFinder *tpf, TileIndex tile, int dir)
 			/* allocate a link. if out of links, handle this by returning
 			 * that a tile was already visisted. */
 			if (tpf->num_links_left == 0) {
-				DEBUG(misc, 4) ("[NTP] no links left\n");
 				return false;
 			}
 			tpf->num_links_left--;
@@ -84,7 +83,6 @@ static bool TPFSetTileBit(TrackPathFinder *tpf, TileIndex tile, int dir)
 	/* get here if we need to add a new link to link,
 	 * first, allocate a new link, in the same way as before */
 	if (tpf->num_links_left == 0) {
-			DEBUG(misc, 4)("[NTP] no links left\n");
 			return false;
 	}
 	tpf->num_links_left--;
@@ -125,11 +123,6 @@ static const byte _otherdir_mask[4] = {
 	0x2A,
 };
 
-#ifdef DEBUG_TILE_PUSH
-extern void dbg_push_tile(TileIndex tile, int track);
-extern void dbg_pop_tile();
-#endif
-
 static void TPFMode2(TrackPathFinder *tpf, TileIndex tile, int direction)
 {
 	uint bits;
@@ -198,15 +191,9 @@ static void TPFMode2(TrackPathFinder *tpf, TileIndex tile, int direction)
 continue_here:;
 		tpf->the_dir = HASBIT(_otherdir_mask[direction],i) ? (i+8) : i;
 
-#ifdef DEBUG_TILE_PUSH
-		dbg_push_tile(tile, tpf->the_dir);
-#endif
 		if (!tpf->enum_proc(tile, tpf->userdata, tpf->the_dir, tpf->rd.cur_length, NULL)) {
 			TPFMode2(tpf, tile, _tpf_new_direction[tpf->the_dir]);
 		}
-#ifdef DEBUG_TILE_PUSH
-		dbg_pop_tile();
-#endif
 
 		tpf->rd = rd;
 	} while (++i, bits>>=1);
@@ -327,16 +314,10 @@ static void TPFMode1(TrackPathFinder *tpf, TileIndex tile, int direction)
 				tpf->the_dir = (_otherdir_mask[direction] & (byte)(1 << i)) ? (i+8) : i;
 				rd = tpf->rd;
 
-#ifdef DEBUG_TILE_PUSH
-		dbg_push_tile(tile, tpf->the_dir);
-#endif
 				if (TPFSetTileBit(tpf, tile, tpf->the_dir) &&
 						!tpf->enum_proc(tile, tpf->userdata, tpf->the_dir, tpf->rd.cur_length, &tpf->rd.pft_var6) ) {
 					TPFMode1(tpf, tile, _tpf_new_direction[tpf->the_dir]);
 				}
-#ifdef DEBUG_TILE_PUSH
-		dbg_pop_tile();
-#endif
 				tpf->rd = rd;
 			} while (bits != 0);
 		}
@@ -422,7 +403,8 @@ void FollowTrack(TileIndex tile, uint16 flags, byte direction, TPFEnumProc *enum
 
 typedef struct {
 	TileIndex tile;
-	uint16 cur_length;
+	uint16 cur_length; // This is the current length to this tile.
+	uint16 priority; // This is the current length + estimated length to the goal.
 	byte track;
 	byte depth;
 	byte state;
@@ -445,8 +427,9 @@ typedef struct HashLink {
 } HashLink;
 
 typedef struct {
-	TPFEnumProc *enum_proc;
+	NTPEnumProc *enum_proc;
 	void *userdata;
+	TileIndex dest;
 
 	byte tracktype;
 	uint maxlength;
@@ -457,7 +440,7 @@ typedef struct {
 	uint nstack;
 	StackedItem stack[256]; // priority queue of stacked items
 
-	uint16 hash_head[0x400]; // hash heads. 0 means unused. 0xFFC0 = length, 0x3F = type
+	uint16 hash_head[0x400]; // hash heads. 0 means unused. 0xFFFC = length, 0x3 = dir
 	TileIndex hash_tile[0x400]; // tiles. or links.
 
 	HashLink links[0x400]; // hash links
@@ -475,7 +458,7 @@ static inline void HeapifyUp(NewTrackPathFinder *tpf)
 	StackedItem si;
 	int i = ++tpf->nstack;
 
-	while (i != 1 && ARR(i).cur_length < ARR(i>>1).cur_length) {
+	while (i != 1 && ARR(i).priority < ARR(i>>1).priority) {
 		// the child element is larger than the parent item.
 		// swap the child item and the parent item.
 		si = ARR(i); ARR(i) = ARR(i>>1); ARR(i>>1) = si;
@@ -500,11 +483,11 @@ static inline void HeapifyDown(NewTrackPathFinder *tpf)
 
 	while ((j=i*2) <= n) {
 		// figure out which is smaller of the children.
-		if (j != n && ARR(j).cur_length > ARR(j+1).cur_length)
+		if (j != n && ARR(j).priority > ARR(j+1).priority)
 			j++; // right item is smaller
 
 		assert(i <= n && j <= n);
-		if (ARR(i).cur_length <= ARR(j).cur_length)
+		if (ARR(i).priority <= ARR(j).priority)
 			break; // base elem smaller than smallest, done!
 
 		// swap parent with the child
@@ -544,8 +527,11 @@ static bool NtpVisit(NewTrackPathFinder *tpf, TileIndex tile, uint dir, uint len
 		// two tiles with the same hash, need to make a link
 		// allocate a link. if out of links, handle this by returning
 		// that a tile was already visisted.
-		if (tpf->num_links_left == 0)
+		if (tpf->num_links_left == 0) {
+			DEBUG(ntp, 1) ("[NTP] no links left");
 			return false;
+		}
+
 		tpf->num_links_left--;
 		link = tpf->new_link++;
 
@@ -575,8 +561,10 @@ static bool NtpVisit(NewTrackPathFinder *tpf, TileIndex tile, uint dir, uint len
 
 	/* get here if we need to add a new link to link,
 	 * first, allocate a new link, in the same way as before */
-	if (tpf->num_links_left == 0)
-			return false;
+	if (tpf->num_links_left == 0) {
+		DEBUG(ntp, 1) ("[NTP] no links left");
+		return false;
+	}
 	tpf->num_links_left--;
 	new_link = tpf->new_link++;
 
@@ -638,155 +626,239 @@ static const uint16 _is_upwards_slope[15] = {
 };
 
 
+#define DIAG_FACTOR 3
+#define STR_FACTOR 2
+
+
+static uint DistanceMoo(TileIndex t0, TileIndex t1)
+{
+	const uint dx = abs(TileX(t0) - TileX(t1));
+	const uint dy = abs(TileY(t0) - TileY(t1));
+
+	const uint straightTracks = 2 * min(dx, dy); /* The number of straight (not full length) tracks */
+	/* OPTIMISATION:
+	 * Original: diagTracks = max(dx, dy) - min(dx,dy);
+	 * Proof:
+	 * (dx-dy) - straightTracks  == (min + max) - straightTracks = min + // max - 2 * min = max - min */
+	const uint diagTracks = dx + dy - straightTracks; /* The number of diagonal (full tile length) tracks. */
+
+	return diagTracks*DIAG_FACTOR + straightTracks*STR_FACTOR;
+}
+
+// These has to be small cause the max length of a track
+// is currently limited to 16384
+
+static const byte _length_of_track[16] = {
+	DIAG_FACTOR,DIAG_FACTOR,STR_FACTOR,STR_FACTOR,STR_FACTOR,STR_FACTOR,0,0,
+	DIAG_FACTOR,DIAG_FACTOR,STR_FACTOR,STR_FACTOR,STR_FACTOR,STR_FACTOR,0,0
+};
+
 // new more optimized pathfinder for trains...
+// Tile is the tile the train is at.
+// direction is the tile the train is moving towards.
+
 static void NTPEnum(NewTrackPathFinder *tpf, TileIndex tile, uint direction)
 {
-	uint bits, tile_org;
-	int i;
+	uint bits, tile_org, track;
 	StackedItem si;
 	FindLengthOfTunnelResult flotr;
+	int estimation;
 
-	si.cur_length = 0;
+	// Need to have a special case for the start.
+	// We shouldn't call the callback for the current tile.
+	si.cur_length = 1; // Need to start at 1 cause 0 is a reserved value.
 	si.depth = 0;
 	si.state = 0;
+	goto start_at;
 
-restart:
-	if (IsTileType(tile, MP_TUNNELBRIDGE) && (_m[tile].m5 & 0xF0) == 0) {
-		/* This is a tunnel tile */
-		if ( (uint)(_m[tile].m5 & 3) != (direction ^ 2)) { /* ^ 2 is reversing the direction */
-			/* We are not just driving out of the tunnel */
-			if ( (uint)(_m[tile].m5 & 3) != direction || ((_m[tile].m5>>1)&6) != tpf->tracktype)
-				/* We are not driving into the tunnel, or it
-				 * is an invalid tunnel */
-				goto stop_search;
-			flotr = FindLengthOfTunnel(tile, direction);
-			si.cur_length += flotr.length;
-			tile = flotr.tile;
-		}
-	}
+	for(;;) {
+		// Get the next item to search from from the priority queue
+		do {
+			if (tpf->nstack == 0)
+				return; // nothing left? then we're done!
+			si = tpf->stack[0];
+			tile = si.tile;
 
-	// remember the start tile so we know if we're in an inf loop.
-	tile_org = tile;
+			HeapifyDown(tpf);
+			// Make sure we havn't already visited this tile.
+		} while (!NtpCheck(tpf, tile, _tpf_prev_direction[si.track], si.cur_length));
 
-	for(;;) {
-		int track;
+		// Add the length of this track.
+		si.cur_length += _length_of_track[si.track];
 
-		tile += TileOffsByDir(direction);
+callback_and_continue:
+		if (tpf->enum_proc(tile, tpf->userdata, si.first_track, si.cur_length))
+			return;
 
-		// too long search length? bail out.
-		if (++si.cur_length >= tpf->maxlength) {
-			DEBUG(misc,4) ("[NTP] cur_length too big\n");
-			goto stop_search;
+		direction = _tpf_new_direction[si.track];
+
+start_at:
+		// If the tile is the entry tile of a tunnel, and we're not going out of the tunnel,
+		//   need to find the exit of the tunnel.
+		if (IsTileType(tile, MP_TUNNELBRIDGE)) {
+			if ((_m[tile].m5 & 0xF0) == 0 &&
+				(uint)(_m[tile].m5 & 3) != (direction ^ 2)) {
+				/* This is a tunnel tile */
+				/* We are not just driving out of the tunnel */
+				if ( (uint)(_m[tile].m5 & 3) != direction || ((_m[tile].m5>>1)&6) != tpf->tracktype)
+					/* We are not driving into the tunnel, or it
+					 * is an invalid tunnel */
+					continue;
+				flotr = FindLengthOfTunnel(tile, direction);
+				si.cur_length += flotr.length * DIAG_FACTOR;
+				tile = flotr.tile;
+				// tile now points to the exit tile of the tunnel
+			}
 		}
 
-		// Not a regular rail tile?
-		// Then we can't use the code below, but revert to more general code.
-		if (!IsTileType(tile, MP_RAILWAY) || !IsPlainRailTile(tile)) {
-			bits = GetTileTrackStatus(tile, TRANSPORT_RAIL) & _tpfmode1_and[direction];
-			bits = (bits | (bits >> 8)) & 0x3F;
-			if (bits == 0) goto stop_search;
-			break;
-		}
+		// This is a special loop used to go through
+		// a rail net and find the first intersection
+		tile_org = tile;
+		for(;;) {
+			tile += TileOffsByDir(direction);
 
-		// regular rail tile, determine the tracks that are actually reachable.
-		bits = _m[tile].m5 & _bits_mask[direction];
-		if (bits == 0) goto stop_search; // no tracks there? stop searching.
+			// too long search length? bail out.
+			if (si.cur_length >= tpf->maxlength) {
+				DEBUG(ntp,1) ("[NTP] cur_length too big");
+				bits = 0;
+				break;
+			}
 
-		// intersection? then we need to branch the search space,
-		// can't handle that from here.
-		if (KILL_FIRST_BIT(bits) != 0) break;
+			// Not a regular rail tile?
+			// Then we can't use the code below, but revert to more general code.
+			if (!IsTileType(tile, MP_RAILWAY) || !IsPlainRailTile(tile)) {
+				// We found a tile which is not a normal railway tile.
+				// Determine which tracks that exist on this tile.
+				bits = GetTileTrackStatus(tile, TRANSPORT_RAIL) & _tpfmode1_and[direction];
+				bits = (bits | (bits >> 8)) & 0x3F;
 
-		track = _new_track[FIND_FIRST_BIT(bits)][direction];
+				// Check that the tile contains exactly one track
+				if (bits == 0 || KILL_FIRST_BIT(bits) != 0)
+					break;
 
-		// Check if this rail is an upwards slope. If it is, then add a penalty.
-		// Small optimization here.. if (track&7)>1 then it can't be a slope so we avoid calling GetTileSlope
-		if ((track & 7) <= 1 && (_is_upwards_slope[GetTileSlope(tile, NULL)] & (1 << track)) ) {
-			// upwards slope. add some penalty.
-			si.cur_length += 2;
-		}
+				///////////////////
+				// If we reach here, the tile has exactly one track.
+				//   tile - index to a tile that is not rail tile, but still straight (with optional signals)
+				//   bits - bitmask of which track that exist on the tile (exactly one bit is set)
+				//   direction - which direction are we moving in?
+				///////////////////
+				si.track = _new_track[FIND_FIRST_BIT(bits)][direction];
+				si.cur_length += _length_of_track[si.track];
+				goto callback_and_continue;
+			}
 
-		// railway tile with signals..?
-		if (HasSignals(tile)) {
-			byte m3;
-
-			m3 = _m[tile].m3;
-			if (!(m3 & SignalAlongTrackdir(track))) {
-				// if one way signal not pointing towards us, stop going in this direction.
-				if (m3 & SignalAgainstTrackdir(track))
-					goto stop_search;
-			} else if (_m[tile].m2 & SignalAlongTrackdir(track)) {
-				// green signal in our direction. either one way or two way.
-				si.state |= 1;
-			} else {
-				// reached a red signal.
-				if (m3 & SignalAgainstTrackdir(track)) {
-					// two way red signal. unless we passed another green signal on the way,
-					// stop going in this direction.
-					// this is to prevent us from going into a full platform.
-					if (!(si.state&1))
-						goto stop_search;
-				}
-				// add some penalty since this path has a red signal on it.
-				// only add this penalty max 2 times.
-				if ((si.state & 6) != 4) {
-					si.cur_length += 10;
-					si.state += 2; // remember that we added penalty.
-				}
+			// Regular rail tile, determine which tracks exist.
+			bits = _m[tile].m5 & 0x3F;
+			if (bits == 0)
+				break; // None at all?
+
+			// Make sure that the tile contains exactly ONE track
+			if (KILL_FIRST_BIT(bits) != 0) {
+				// It contained many tracks,
+				// but first, mask out the tracks that are not reachable
+				bits &= _bits_mask[direction];
+				break;
 			}
 
-			if (tpf->enum_proc(tile, tpf->userdata, track, si.cur_length, &si.state))
-				goto stop_search; // we should stop searching in this direction
-		}
+			track = _new_track[FIND_FIRST_BIT(bits)][direction];
+			si.cur_length += _length_of_track[track];
 
-		// continue with the next track
-		direction = _tpf_new_direction[track];
-		assert(direction != 0xFF);
+			// Check if this rail is an upwards slope. If it is, then add a penalty.
+			// Small optimization here.. if (track&7)>1 then it can't be a slope so we avoid calling GetTileSlope
+			if ((track & 7) <= 1 && (_is_upwards_slope[GetTileSlope(tile, NULL)] & (1 << track)) ) {
+				// upwards slope. add some penalty.
+				si.cur_length += 4*DIAG_FACTOR;
+			}
 
-		// check if we're running around chasing our tail... (infinite loop)
-		if (tile == tile_org)
-			goto stop_search;
-	}
+			// railway tile with signals..?
+			if (HasSignals(tile)) {
+				byte m3;
+
+				m3 = _m[tile].m3;
+				if (!(m3 & SignalAlongTrackdir(track))) {
+					// if one way signal not pointing towards us, stop going in this direction.
+					if (m3 & SignalAgainstTrackdir(track)) {
+						bits = 0;
+						break;
+					}
+				} else if (_m[tile].m2 & SignalAlongTrackdir(track)) {
+					// green signal in our direction. either one way or two way.
+					si.state |= 3;
+				} else {
+					// reached a red signal.
+					if (m3 & SignalAgainstTrackdir(track)) {
+						// two way red signal. unless we passed another green signal on the way,
+						// stop going in this direction.
+						// this is to prevent us from going into a full platform.
+						if (!(si.state&1)) {
+							bits = 0;
+							break;
+						}
+					}
+					if (!(si.state & 2)) {
+						// Is this the first signal we see? And it's red... add penalty
+						si.cur_length += 10*DIAG_FACTOR;
+						si.state += 2; // remember that we added penalty.
+						// Because we added a penalty, we can't just continue as usual.
+						// Need to get out and let A* do it's job with
+						// possibly finding an even shorter path.
+						break;
+					}
+				}
 
-	// if only one possible track to choose from, just continue
-	if (KILL_FIRST_BIT(bits) == 0) {
-		i = _new_track[FIND_FIRST_BIT(bits)][direction];
-		// call the callback to check if we've reached the destination
-		if (tpf->enum_proc(tile, tpf->userdata, i, si.cur_length, &si.state))
-			goto stop_search; // we should stop searching in this direction.
+				if (tpf->enum_proc(tile, tpf->userdata, si.first_track, si.cur_length))
+					return;
+			}
 
-		// we should continue searching. determine new direction.
-		direction = _tpf_new_direction[i];
-		goto restart; // use tail recursion optimization.
-	}
+			// continue with the next track
+			direction = _tpf_new_direction[track];
+			assert(direction != 0xFF);
 
-	////////////////
-	// We got multiple tracks to choose between (intersection).
-	// Branch the search space into several branches.
-	// Push each alternative on the stack.
-	////////////////
-
-	// Increase recursion depth counter, and
-	// Check so the depth is not too big.. to prevent enourmous slowdown.
-	if (si.depth >= _patches.pf_maxdepth) {
-		DEBUG(misc, 4) ("[NTP] depth too big\n");
-		goto stop_search;
-	}
-	si.depth++;
+			// safety check if we're running around chasing our tail... (infinite loop)
+			if (tile == tile_org) {
+				bits = 0;
+				break;
+			}
+		}
+
+		// There are no tracks to choose between.
+		// Stop searching in this direction
+		if (bits == 0)
+			continue;
+
+		////////////////
+		// We got multiple tracks to choose between (intersection).
+		// Branch the search space into several branches.
+		////////////////
+
+		// Check if we've already visited this intersection.
+		// If we've already visited it with a better length, then
+		// there's no point in visiting it again.
+		if (!NtpVisit(tpf, tile, direction, si.cur_length))
+			continue;
 
-	// Check if we've already visited this intersection.
-	// If we've already visited it with a better length, then
-	// there's no point in visiting it again.
-	if (NtpVisit(tpf, tile, direction, si.cur_length)) {
 		// Push all possible alternatives that we can reach from here
 		// onto the priority heap.
+		// 'bits' contains the tracks that we can choose between.
+
+		// First compute the estimated distance to the target.
+		// This is used to implement A*
+		estimation = 0;
+		if (tpf->dest != 0)
+			estimation = DistanceMoo(tile, tpf->dest);
+
+		si.depth++;
 		si.tile = tile;
 		do {
 			si.track = _new_track[FIND_FIRST_BIT(bits)][direction];
+			si.priority = si.cur_length + estimation;
+
 			// out of stack items, bail out?
 			if (tpf->nstack >= lengthof(tpf->stack)) {
-				DEBUG(misc, 4) ("[NTP] out of stack\n");
+				DEBUG(ntp, 1) ("[NTP] out of stack");
 				break;
 			}
+
 			tpf->stack[tpf->nstack] = si;
 			HeapifyUp(tpf);
 		} while ((bits = KILL_FIRST_BIT(bits)) != 0);
@@ -795,54 +867,36 @@ restart:
 		// so the code outside knows which path is better.
 		// also randomize the order in which we search through them.
 		if (si.depth == 1) {
-			uint32 r = Random();
-			assert(tpf->nstack == 2 || tpf->nstack == 3);
-			if (r&1) swap_byte(&tpf->stack[0].track, &tpf->stack[1].track);
-			if (tpf->nstack != 2) {
-				byte t = tpf->stack[2].track;
-				if (r&2) swap_byte(&tpf->stack[0].track, &t);
-				if (r&4) swap_byte(&tpf->stack[1].track, &t);
-				tpf->stack[2].first_track = tpf->stack[2].track = t;
+			assert(tpf->nstack == 1 || tpf->nstack == 2 || tpf->nstack == 3);
+			if (tpf->nstack != 1) {
+				uint32 r = Random();
+				if (r&1) swap_byte(&tpf->stack[0].track, &tpf->stack[1].track);
+				if (tpf->nstack != 2) {
+					byte t = tpf->stack[2].track;
+					if (r&2) swap_byte(&tpf->stack[0].track, &t);
+					if (r&4) swap_byte(&tpf->stack[1].track, &t);
+					tpf->stack[2].first_track = tpf->stack[2].track = t;
+				}
+				tpf->stack[0].first_track = tpf->stack[0].track;
+				tpf->stack[1].first_track = tpf->stack[1].track;
 			}
-			tpf->stack[0].first_track = tpf->stack[0].track;
-			tpf->stack[1].first_track = tpf->stack[1].track;
 		}
-	}
 
-
-stop_search:
-	// Now continue searching from the intersection that has the lowest
-	// cost.
-	// Pop the lowest cost item from the priority heap.
-	do {
-		if (tpf->nstack == 0)
-			return; // nothing left? then we're done!
-		si = tpf->stack[0];
-		tile = si.tile;
-		HeapifyDown(tpf);
-
-	// First check if we've already visited the tile we're just about to continue at.
-	// If we've already visited it, no point in continuing from there.
-	// Then call the callback.
-	} while (
-		!NtpCheck(tpf, tile, _tpf_prev_direction[si.track], si.cur_length) || // already have better path to that tile?
-		tpf->enum_proc(tile, tpf->userdata, si.track, si.cur_length, &si.state)
-	);
-
-	direction = _tpf_new_direction[si.track];
-	goto restart;
+		// Continue with the next from the queue...
+	}
 }
 
 
 // new pathfinder for trains. better and faster.
-void NewTrainPathfind(TileIndex tile, byte direction, TPFEnumProc *enum_proc, void *data, byte *cache)
+void NewTrainPathfind(TileIndex tile, TileIndex dest, byte direction, NTPEnumProc *enum_proc, void *data)
 {
 	NewTrackPathFinder tpf;
 
+	tpf.dest = dest;
 	tpf.userdata = data;
 	tpf.enum_proc = enum_proc;
 	tpf.tracktype = 0;
-	tpf.maxlength = _patches.pf_maxlength;
+	tpf.maxlength = min(_patches.pf_maxlength * 3, 10000);
 	tpf.nstack = 0;
 	tpf.new_link = tpf.links;
 	tpf.num_links_left = lengthof(tpf.links);
diff --git a/pathfind.h b/pathfind.h
index 0543df461..583836693 100644
--- a/pathfind.h
+++ b/pathfind.h
@@ -8,6 +8,7 @@ typedef struct TrackPathFinder TrackPathFinder;
 typedef bool TPFEnumProc(TileIndex tile, void *data, int track, uint length, byte *state);
 typedef void TPFAfterProc(TrackPathFinder *tpf);
 
+typedef bool NTPEnumProc(TileIndex tile, void *data, int track, uint length);
 
 #define PATHFIND_GET_LINK_OFFS(tpf, link) ((byte*)(link) - (byte*)tpf->links)
 #define PATHFIND_GET_LINK_PTR(tpf, link_offs) (TrackPathFinderLink*)((byte*)tpf->links + (link_offs))
@@ -63,6 +64,6 @@ typedef struct {
 } FindLengthOfTunnelResult;
 FindLengthOfTunnelResult FindLengthOfTunnel(TileIndex tile, int direction);
 
-void NewTrainPathfind(TileIndex tile, byte direction, TPFEnumProc *enum_proc, void *data, byte *cache);
+void NewTrainPathfind(TileIndex tile, TileIndex dest, byte direction, NTPEnumProc *enum_proc, void *data);
 
 #endif /* PATHFIND_H */
diff --git a/settings.c b/settings.c
index 0433bc9e1..44d6287d5 100644
--- a/settings.c
+++ b/settings.c
@@ -872,7 +872,6 @@ const SettingDesc patch_settings[] = {
 	{"snow_line_height",		SDT_UINT8,	(void*)7,			&_patches.snow_line_height,			NULL},
 
 	{"bribe",								SDT_BOOL,		(void*)true,	&_patches.bribe,								NULL},
-	{"new_depot_finding",		SDT_BOOL,		(void*)false,	&_patches.new_depot_finding,		NULL},
 
 	{"nonuniform_stations",	SDT_BOOL,		(void*)true,	&_patches.nonuniform_stations,	NULL},
 	{"always_small_airport",SDT_BOOL,		(void*)false,	&_patches.always_small_airport,	NULL},
diff --git a/settings_gui.c b/settings_gui.c
index 5833635e1..fc6c00462 100644
--- a/settings_gui.c
+++ b/settings_gui.c
@@ -679,7 +679,6 @@ static const PatchEntry _patches_vehicles[] = {
 	{PE_BOOL,		0, STR_CONFIG_PATCHES_MAMMOTHTRAINS,		"mammoth_trains", 	&_patches.mammoth_trains,						0,  0,  0, NULL},
 	{PE_BOOL,		0, STR_CONFIG_PATCHES_GOTODEPOT,				"goto_depot", 			&_patches.gotodepot,								0,  0,  0, NULL},
 	{PE_BOOL,		0, STR_CONFIG_PATCHES_ROADVEH_QUEUE,		"roadveh_queue", 		&_patches.roadveh_queue,						0,  0,  0, NULL},
-	{PE_BOOL,		0, STR_CONFIG_PATCHES_NEW_DEPOT_FINDING,"depot_finding", 		&_patches.new_depot_finding,				0,  0,  0, NULL},
 	{PE_BOOL,		0, STR_CONFIG_PATCHES_NEW_PATHFINDING_ALL, "new_pathfinding_all", &_patches.new_pathfinding_all,		0,  0,  0, NULL},
 
 	{PE_BOOL,		PF_PLAYERBASED, STR_CONFIG_PATCHES_WARN_INCOME_LESS, "train_income_warn", &_patches.train_income_warn,				0,  0,  0, NULL},
diff --git a/stdafx.h b/stdafx.h
index e37d2dac8..414e06ea9 100644
--- a/stdafx.h
+++ b/stdafx.h
@@ -9,7 +9,7 @@
 #pragma warning(disable: 4244) // conversion
 #pragma warning(disable: 4245) // conversion
 #pragma warning(disable: 4305) // 'initializing' : truncation from 'const int ' to 'char '
-//#pragma warning(disable: 4018) // warning C4018: '==' : signed/unsigned mismatch
+#pragma warning(disable: 4018) // warning C4018: '==' : signed/unsigned mismatch
 #pragma warning(disable: 4201) // nameless union
 #pragma warning(disable: 4514) // removed unref inline
 #pragma warning(disable: 4127) // constant conditional expression
diff --git a/tile.h b/tile.h
index 9074f8601..4f67746bc 100644
--- a/tile.h
+++ b/tile.h
@@ -15,7 +15,7 @@ typedef enum TileTypes {
 	MP_VOID, // invisible tiles at the SW and SE border
 	MP_INDUSTRY,
 	MP_TUNNELBRIDGE,
-	MP_UNMOVABLE
+	MP_UNMOVABLE,
 } TileType;
 
 /* Direction as commonly used in v->direction, 8 way. */
@@ -95,6 +95,11 @@ static inline bool IsTileType(TileIndex tile, TileType type)
 	return GetTileType(tile) == type;
 }
 
+static inline bool IsTunnelTile(TileIndex tile)
+{
+	return IsTileType(tile, MP_TUNNELBRIDGE) && (_m[tile].m5 & 0xF0) == 0;
+}
+
 static inline Owner GetTileOwner(TileIndex tile)
 {
 	assert(tile < MapSize());
diff --git a/train_cmd.c b/train_cmd.c
index 5fe2b0a6d..6508eaa58 100644
--- a/train_cmd.c
+++ b/train_cmd.c
@@ -666,11 +666,6 @@ int32 CmdBuildRailVehicle(int x, int y, uint32 flags, uint32 p1, uint32 p2)
 	return value;
 }
 
-static bool IsTunnelTile(TileIndex tile)
-{
-	return IsTileType(tile, MP_TUNNELBRIDGE) && (_m[tile].m5 & 0x80) == 0;
-}
-
 
 /* Check if all the wagons of the given train are in a depot, returns the
  * number of cars (including loco) then. If not, sets the error message to
@@ -1307,9 +1302,7 @@ TileIndex GetVehicleTileOutOfTunnel(const Vehicle *v, bool reverse)
 		return v->tile;
 
 	for (tile = v->tile;; tile += delta) {
-		if (IsTileType(tile, MP_TUNNELBRIDGE) &&
-				(_m[tile].m5 & 0xF3) != (direction) &&
-				GetTileZ(tile) == v->z_pos)
+		if (IsTunnelTile(tile) && (_m[tile].m5 & 0x3) != (direction) && GetTileZ(tile) == v->z_pos)
  			break;
  	}
  	return tile;
@@ -1569,26 +1562,17 @@ typedef struct TrainFindDepotData {
 	bool reverse;
 } TrainFindDepotData;
 
-static bool TrainFindDepotEnumProc(TileIndex tile, TrainFindDepotData *tfdd, int track, uint length, byte *state)
+static bool NtpCallbFindDepot(TileIndex tile, TrainFindDepotData *tfdd, int track, uint length)
 {
 	if (IsTileType(tile, MP_RAILWAY) && IsTileOwner(tile, tfdd->owner)) {
 		if ((_m[tile].m5 & ~0x3) == 0xC0) {
-			if (length < tfdd->best_length) {
-				tfdd->best_length = length;
-				tfdd->tile = tile;
-			}
+			tfdd->best_length = length;
+			tfdd->tile = tile;
 			return true;
 		}
-
-		// make sure the train doesn't run against a oneway signal
-		if ((_m[tile].m5 & 0xC0) == 0x40) {
-			if (!(_m[tile].m3 & SignalAlongTrackdir(track)) && _m[tile].m3 & SignalAgainstTrackdir(track))
-				return true;
-		}
 	}
 
-	// stop  searching if we've found a destination that is closer already.
-	return length >= tfdd->best_length;
+	return false;
 }
 
 // returns the tile of a depot to goto to. The given vehicle must not be
@@ -1632,21 +1616,17 @@ static TrainFindDepotData FindClosestTrainDepot(Vehicle *v)
 			if (NPFGetFlag(&ftd.node, NPF_FLAG_REVERSE))
 				tfdd.reverse = true;
 		}
-	} else if (!_patches.new_depot_finding) {
-		// search in all directions
-		for(i=0; i!=4; i++)
-			NewTrainPathfind(tile, i, (TPFEnumProc*)TrainFindDepotEnumProc, &tfdd, NULL);
 	} else {
 		// search in the forward direction first.
 		i = v->direction >> 1;
 		if (!(v->direction & 1) && v->u.rail.track != _state_dir_table[i]) { i = (i - 1) & 3; }
-		NewTrainPathfind(tile, i, (TPFEnumProc*)TrainFindDepotEnumProc, &tfdd, NULL);
+		NewTrainPathfind(tile, 0, i, (NTPEnumProc*)NtpCallbFindDepot, &tfdd);
 		if (tfdd.best_length == (uint)-1){
 			tfdd.reverse = true;
 			// search in backwards direction
 			i = (v->direction^4) >> 1;
 			if (!(v->direction & 1) && v->u.rail.track != _state_dir_table[i]) { i = (i - 1) & 3; }
-			NewTrainPathfind(tile, i, (TPFEnumProc*)TrainFindDepotEnumProc, &tfdd, NULL);
+			NewTrainPathfind(tile, 0, i, (NTPEnumProc*)NtpCallbFindDepot, &tfdd);
 		}
 	}
 
@@ -1887,7 +1867,7 @@ typedef struct TrainTrackFollowerData {
 	byte best_track;
 } TrainTrackFollowerData;
 
-static bool TrainTrackFollower(TileIndex tile, TrainTrackFollowerData *ttfd, int track, uint length, byte *state)
+static bool NtpCallbFindStation(TileIndex tile, TrainTrackFollowerData *ttfd, int track, uint length)
 {
 	// heading for nowhere?
 	if (ttfd->dest_coords == 0)
@@ -1900,24 +1880,16 @@ static bool TrainTrackFollower(TileIndex tile, TrainTrackFollowerData *ttfd, int
 		 * because in that case the dest_coords are just an
 		 * approximation of where the station is */
 		// found station
-		ttfd->best_bird_dist = 0;
-		if (length < ttfd->best_track_dist) {
-			ttfd->best_track_dist = length;
-			ttfd->best_track = state[1];
-		}
+		ttfd->best_track = track;
 		return true;
 	} else {
 		uint dist;
 
-		// we've actually found the destination already. no point searching in directions longer than this.
-		if (ttfd->best_track_dist != (uint)-1)
-			return length >= ttfd->best_track_dist;
-
-		// didn't find station
+		// didn't find station, keep track of the best path so far.
 		dist = DistanceManhattan(tile, ttfd->dest_coords);
 		if (dist < ttfd->best_bird_dist) {
 			ttfd->best_bird_dist = dist;
-			ttfd->best_track = state[1];
+			ttfd->best_track = track;
 		}
 		return false;
 	}
@@ -2046,7 +2018,8 @@ static byte ChooseTrainTrack(Vehicle *v, TileIndex tile, int enterdir, TrackdirB
 		fd.best_track_dist = (uint)-1;
 		fd.best_track = 0xFF;
 
-		NewTrainPathfind(tile - TileOffsByDir(enterdir), enterdir, (TPFEnumProc*)TrainTrackFollower, &fd, NULL);
+		NewTrainPathfind(tile - TileOffsByDir(enterdir), v->dest_tile,
+			enterdir, (NTPEnumProc*)NtpCallbFindStation, &fd);
 
 		if (fd.best_track == 0xff) {
 			// blaha
@@ -2118,7 +2091,7 @@ static bool CheckReverseTrain(Vehicle *v)
 			fd.best_bird_dist = (uint)-1;
 			fd.best_track_dist = (uint)-1;
 
-			NewTrainPathfind(v->tile, reverse ^ i, (TPFEnumProc*)TrainTrackFollower, &fd, NULL);
+			NewTrainPathfind(v->tile, v->dest_tile, reverse ^ i, (NTPEnumProc*)NtpCallbFindStation, &fd);
 
 			if (best_track != -1) {
 				if (best_bird_dist != 0) {
@@ -2861,18 +2834,15 @@ green_light:
 			/* in tunnel */
 			GetNewVehiclePos(v, &gp);
 
-			if (IsTileType(gp.new_tile, MP_TUNNELBRIDGE) &&
-					!(_m[gp.new_tile].m5 & 0xF0)) {
-				r = VehicleEnterTile(v, gp.new_tile, gp.x, gp.y);
-				if (r & 0x4) goto common;
+			// Check if to exit the tunnel...
+			if (!IsTunnelTile(gp.new_tile) ||
+					!(VehicleEnterTile(v, gp.new_tile, gp.x, gp.y)&0x4) ) {
+				v->x_pos = gp.x;
+				v->y_pos = gp.y;
+				VehiclePositionChanged(v);
+				continue;
 			}
-
-			v->x_pos = gp.x;
-			v->y_pos = gp.y;
-			VehiclePositionChanged(v);
-			continue;
 		}
-common:;
 
 		/* update image of train, as well as delta XY */
 		newdir = GetNewVehicleDirection(v, gp.x, gp.y);
@@ -3125,8 +3095,7 @@ static bool TrainCheckIfLineEnds(Vehicle *v)
 	tile = v->tile;
 
 	// tunnel entrance?
-	if (IsTileType(tile, MP_TUNNELBRIDGE) &&
-			(_m[tile].m5 & 0xF0) == 0 && (byte)((_m[tile].m5 & 3)*2+1) == v->direction)
+	if (IsTunnelTile(tile) && (byte)((_m[tile].m5 & 3)*2+1) == v->direction)
 				return true;
 
 	// depot?
diff --git a/variables.h b/variables.h
index cef95f6f5..c8d13e3fb 100644
--- a/variables.h
+++ b/variables.h
@@ -130,7 +130,6 @@ typedef struct Patches {
 	byte errmsg_duration;		// duration of error message
 	byte snow_line_height;	// a number 0-15 that configured snow line height
 	bool bribe;							// enable bribing the local authority
-	bool new_depot_finding;	// use new algorithm to find a depot.
 	bool nonuniform_stations;// allow nonuniform train stations
 	bool always_small_airport; // always allow small airports
 	bool realistic_acceleration; // realistic acceleration for trains
diff --git a/viewport.c b/viewport.c
index f9657ff6f..32dd8637d 100644
--- a/viewport.c
+++ b/viewport.c
@@ -540,60 +540,42 @@ void *AddStringToDraw(int x, int y, StringID string, uint32 params_1, uint32 par
 	return ss;
 }
 
-/* Debugging code */
 
-#ifdef DEBUG_TILE_PUSH
-static uint _num_push;
-static TileIndex _pushed_tile[200];
-static int _pushed_track[200];
+#ifdef DEBUG_HILIGHT_MARKED_TILES
 
-static TileIndex _stored_tile[200];
-static int _stored_track[200];
-static uint _num_stored;
-
-void dbg_store_path(void)
+static void DrawHighlighedTile(const TileInfo *ti)
 {
-	memcpy(_stored_tile, _pushed_tile, sizeof(_stored_tile));
-	memcpy(_stored_track, _pushed_track, sizeof(_stored_tile));
-	_num_stored = _num_push;
-	MarkWholeScreenDirty();
+	if (_m[ti->tile].extra & 0x80) {
+		DrawSelectionSprite(PALETTE_TILE_RED_PULSATING | (SPR_SELECT_TILE + _tileh_to_sprite[ti->tileh]), ti);
+	}
 }
 
-void dbg_push_tile(TileIndex tile, int track)
-{
-	_pushed_tile[_num_push] = tile;
-	_pushed_track[_num_push++] = track;
-	dbg_store_path();
-}
+int _debug_marked_tiles, _debug_red_tiles;
 
-void dbg_pop_tile(void)
-{
-	assert(_num_push > 0)
-	_num_push--;
+// Helper functions that allow you mark a tile as red.
+void DebugMarkTile(TileIndex tile) {
+	_debug_marked_tiles++;
+	if (_m[tile].extra & 0x80)
+		return;
+	_debug_red_tiles++;
+	MarkTileDirtyByTile(tile);
+	_m[tile].extra = (_m[tile].extra & ~0xE0) | 0x80;
 }
 
-static const uint16 _dbg_track_sprite[] = {
-	0x3F4,
-	0x3F3,
-	0x3F5,
-	0x3F6,
-	0x3F8,
-	0x3F7,
-};
-
-static int dbg_draw_pushed(const TileInfo *ti)
+void DebugClearMarkedTiles()
 {
-	uint i;
-
-	if (ti->tile == 0) return 0;
-	for (i = 0; i != _num_stored; i++) {
-		if (_stored_tile[i] == ti->tile) {
-			DrawGroundSpriteAt(_dbg_track_sprite[_stored_track[i]&7], ti->x, ti->y, ti->z);
+	uint size = MapSize(), i;
+	for(i=0; i!=size; i++) {
+		if (_m[i].extra & 0x80) {
+			_m[i].extra &= ~0x80;
+			MarkTileDirtyByTile(i);
 		}
 	}
-	return -1;
+	_debug_red_tiles = 0;
+	_debug_red_tiles = 0;
 }
 
+
 #endif
 
 static void DrawSelectionSprite(uint32 image, const TileInfo *ti)
@@ -641,8 +623,8 @@ static void DrawTileSelection(const TileInfo *ti)
 {
 	uint32 image;
 
-#ifdef DEBUG_TILE_PUSH
-	dbg_draw_pushed(ti);
+#ifdef DEBUG_HILIGHT_MARKED_TILES
+	DrawHighlighedTile(ti);
 #endif
 
 	// Draw a red error square?
diff --git a/win32.c b/win32.c
index 9fb8ca0a5..72a94b158 100644
--- a/win32.c
+++ b/win32.c
@@ -181,6 +181,34 @@ static void ClientSizeChanged(int w, int h)
 
 extern void DoExitSave(void);
 
+#ifdef _DEBUG
+// Keep this function here..
+// It allows you to redraw the screen from within the MSVC debugger
+int RedrawScreenDebug()
+{
+	HDC dc,dc2;
+	static int _fooctr;
+	HBITMAP old_bmp;
+	HPALETTE old_palette;
+
+	_screen.dst_ptr = _wnd.buffer_bits;
+	UpdateWindows();
+
+	dc = GetDC(_wnd.main_wnd);
+	dc2 = CreateCompatibleDC(dc);
+
+	old_bmp = SelectObject(dc2, _wnd.dib_sect);
+	old_palette = SelectPalette(dc, _wnd.gdi_palette, FALSE);
+	BitBlt(dc, 0, 0, _wnd.width, _wnd.height, dc2, 0, 0, SRCCOPY);
+	SelectPalette(dc, old_palette, TRUE);
+	SelectObject(dc2, old_bmp);
+	DeleteDC(dc2);
+	ReleaseDC(_wnd.main_wnd, dc);
+
+	return _fooctr++;
+}
+#endif
+
 static LRESULT CALLBACK WndProcGdi(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam)
 {
 	switch (msg) {
@@ -2267,3 +2295,18 @@ void CSleep(int milliseconds)
 {
 	Sleep(milliseconds);
 }
+
+
+// Utility function to get the current timestamp in milliseconds
+// Useful for profiling
+int64 GetTS()
+{
+	static double freq;
+	__int64 value;
+	if (!freq) {
+		QueryPerformanceFrequency((LARGE_INTEGER*)&value);
+		freq = (double)1000000 / value;
+	}
+	QueryPerformanceCounter((LARGE_INTEGER*)&value);
+	return (__int64)(value * freq);
+}
\ No newline at end of file