/*
	This file is part of Warzone 2100.
	Copyright (C) 1999-2004  Eidos Interactive
	Copyright (C) 2005-2020  Warzone 2100 Project

	Warzone 2100 is free software; you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation; either version 2 of the License, or
	(at your option) any later version.

	Warzone 2100 is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with Warzone 2100; if not, write to the Free Software
	Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/** @file
 *  A* based path finding
 *  See http://en.wikipedia.org/wiki/A*_search_algorithm for more information.
 *  How this works:
 *  * First time (in a given tick)  that some droid  wants to pathfind  to a particular
 *    destination,  the A*  algorithm from source to  destination is used.  The desired
 *    destination,  and the nearest  reachable point  to the  destination is saved in a
 *    Context.
 *  * Second time (in a given tick)  that some droid wants to  pathfind to a particular
 *    destination,  the appropriate  Context is found,  and the A* algorithm is used to
 *    find a path from the nearest reachable point to the destination  (which was saved
 *    earlier), to the source.
 *  * Subsequent times  (in a given tick) that some droid wants to pathfind to a parti-
 *    cular destination,  the path is looked up in appropriate Context.  If the path is
 *    not already known,  the A* weights are adjusted, and the previous A*  pathfinding
 *    is continued until the new source is reached.  If the new source is  not reached,
 *    the droid is  on a  different island than the previous droid,  and pathfinding is
 *    restarted from the first step.
 *  Up to 30 pathfinding maps from A* are cached, in a LRU list. The PathNode heap con-
 *  tains the  priority-heap-sorted  nodes which are to be explored.  The path back  is
 *  stored in the PathExploredTile 2D array of tiles.
 */

#ifndef WZ_TESTING
#include "lib/framework/frame.h"

#include "astar.h"
#include "map.h"
#endif

#include <list>
#include <vector>
#include <algorithm>
#include <memory>
#include <iterator>
#include <cstddef>

#include "lib/netplay/sync_debug.h"

/// A coordinate.
struct PathCoord
{
	PathCoord(): x(0), y(0) {}
	PathCoord(int16_t x_, int16_t y_) : x(x_), y(y_) {}
	bool operator ==(PathCoord const &z) const
	{
		return x == z.x && y == z.y;
	}
	bool operator !=(PathCoord const &z) const
	{
		return !(*this == z);
	}

	int16_t x, y;
};

/** The structure to store a node of the route in node table
 *
 *  @ingroup pathfinding
 */
struct PathNode
{
	bool operator <(PathNode const &z) const
	{
		// Sort descending est, fallback to ascending dist, fallback to sorting by position.
		if (est  != z.est)
		{
			return est  > z.est;
		}
		if (dist != z.dist)
		{
			return dist < z.dist;
		}
		if (p.x  != z.p.x)
		{
			return p.x  < z.p.x;
		}
		return p.y  < z.p.y;
	}

	PathCoord p;                    // Map coords.
	unsigned  dist, est;            // Distance so far and estimate to end.
};
struct PathExploredTile
{
	PathExploredTile() : iteration(0xFFFF), dx(0), dy(0), dist(0), visited(false) {}

	uint16_t iteration;
	int8_t   dx, dy;                // Offset from previous point in the route.
	unsigned dist;                  // Shortest known distance to tile.
	bool     visited;
};

struct PathBlockingType
{
	uint32_t gameTime;

	PROPULSION_TYPE propulsion;
	int owner;
	FPATH_MOVETYPE moveType;
};
/// Pathfinding blocking map
struct PathBlockingMap
{
	bool operator ==(PathBlockingType const &z) const
	{
		return type.gameTime == z.gameTime &&
		       fpathIsEquivalentBlocking(type.propulsion, type.owner, type.moveType,
		                                 z.propulsion,    z.owner,    z.moveType);
	}

	PathBlockingType type;
	std::vector<bool> map;
	std::vector<bool> dangerMap;	// using threatBits
};

struct PathNonblockingArea
{
	PathNonblockingArea() {}
	PathNonblockingArea(StructureBounds const &st) : x1(st.map.x), x2(st.map.x + st.size.x), y1(st.map.y), y2(st.map.y + st.size.y) {}
	bool operator ==(PathNonblockingArea const &z) const
	{
		return x1 == z.x1 && x2 == z.x2 && y1 == z.y1 && y2 == z.y2;
	}
	bool operator !=(PathNonblockingArea const &z) const
	{
		return !(*this == z);
	}
	bool isNonblocking(int x, int y) const
	{
		return x >= x1 && x < x2 && y >= y1 && y < y2;
	}

	int16_t x1 = 0;
	int16_t x2 = 0;
	int16_t y1 = 0;
	int16_t y2 = 0;
};

// Data structures used for pathfinding, can contain cached results.
struct PathfindContext
{
	PathfindContext() : myGameTime(0), iteration(0), blockingMap(nullptr) {}
	bool isBlocked(int x, int y) const
	{
		if (dstIgnore.isNonblocking(x, y))
		{
			return false;  // The path is actually blocked here by a structure, but ignore it since it's where we want to go (or where we came from).
		}
		// Not sure whether the out-of-bounds check is needed, can only happen if pathfinding is started on a blocking tile (or off the map).
		return x < 0 || y < 0 || x >= mapWidth || y >= mapHeight || blockingMap->map[x + y * mapWidth];
	}
	bool isDangerous(int x, int y) const
	{
		return !blockingMap->dangerMap.empty() && blockingMap->dangerMap[x + y * mapWidth];
	}
	bool matches(const std::shared_ptr<const PathBlockingMap> &blockingMap_, PathCoord tileS_, PathNonblockingArea dstIgnore_) const
	{
		// Must check myGameTime == blockingMap_->type.gameTime, otherwise blockingMap could be a deleted pointer which coincidentally compares equal to the valid pointer blockingMap_.
		return myGameTime == blockingMap_->type.gameTime && blockingMap == blockingMap_ && tileS == tileS_ && dstIgnore == dstIgnore_;
	}
	void assign(const std::shared_ptr<const PathBlockingMap> &blockingMap_, PathCoord tileS_, PathNonblockingArea dstIgnore_)
	{
		blockingMap = blockingMap_;
		tileS = tileS_;
		dstIgnore = dstIgnore_;
		myGameTime = blockingMap->type.gameTime;
		nodes.clear();

		// Make the iteration not match any value of iteration in map.
		if (++iteration == 0xFFFF)
		{
			map.clear();  // There are no values of iteration guaranteed not to exist in map, so clear the map.
			iteration = 0;
		}
		map.resize(static_cast<size_t>(mapWidth) * static_cast<size_t>(mapHeight));  // Allocate space for map, if needed.
	}

	PathCoord       tileS;                // Start tile for pathfinding. (May be either source or target tile.)
	uint32_t        myGameTime;

	PathCoord       nearestCoord;         // Nearest reachable tile to destination.

	/** Counter to implement lazy deletion from map.
	 *
	 *  @see fpathTableReset
	 */
	uint16_t        iteration;

	std::vector<PathNode> nodes;        ///< Edge of explored region of the map.
	std::vector<PathExploredTile> map;  ///< Map, with paths leading back to tileS.
	std::shared_ptr<const PathBlockingMap> blockingMap; ///< Map of blocking tiles for the type of object which needs a path.
	PathNonblockingArea dstIgnore;      ///< Area of structure at destination which should be considered nonblocking.
};

/// Lists of blocking maps from current tick.
static std::vector<std::shared_ptr<PathBlockingMap>> fpathBlockingMaps;
/// Game time for all blocking maps in fpathBlockingMaps.
static uint32_t fpathCurrentGameTime;

// Convert a direction into an offset
// dir 0 => x = 0, y = -1
static const Vector2i aDirOffset[] =
{
	Vector2i(0, 1),
	Vector2i(-1, 1),
	Vector2i(-1, 0),
	Vector2i(-1, -1),
	Vector2i(0, -1),
	Vector2i(1, -1),
	Vector2i(1, 0),
	Vector2i(1, 1),
};

void fpathHardTableReset()
{
	fpathBlockingMaps.clear();
}

/** Get the nearest entry in the open list
 */
/// Takes the current best node, and removes from the node heap.
static inline PathNode fpathTakeNode(std::vector<PathNode> &nodes)
{
	// find the node with the lowest distance
	// if equal totals, give preference to node closer to target
	PathNode ret = nodes.front();

	// remove the node from the list
	std::pop_heap(nodes.begin(), nodes.end());  // Move the best node from the front of nodes to the back of nodes, preserving the heap properties, setting the front to the next best node.
	nodes.pop_back();                           // Pop the best node (which we will be returning).

	return ret;
}

/** Estimate the distance to the target point
 */
static inline unsigned WZ_DECL_PURE fpathEstimate(PathCoord s, PathCoord f)
{
	// Cost of moving horizontal/vertical = 70*2, cost of moving diagonal = 99*2, 99/70 = 1.41428571... ≈ √2 = 1.41421356...
	unsigned xDelta = abs(s.x - f.x), yDelta = abs(s.y - f.y);
	return std::min(xDelta, yDelta) * (198 - 140) + std::max(xDelta, yDelta) * 140;
}
static inline unsigned WZ_DECL_PURE fpathGoodEstimate(PathCoord s, PathCoord f)
{
	// Cost of moving horizontal/vertical = 70*2, cost of moving diagonal = 99*2, 99/70 = 1.41428571... ≈ √2 = 1.41421356...
	return iHypot((s.x - f.x) * 140, (s.y - f.y) * 140);
}

/** Generate a new node
 */
static inline void fpathNewNode(PathfindContext &context, PathCoord dest, PathCoord pos, unsigned prevDist, PathCoord prevPos)
{
	ASSERT_OR_RETURN(, (unsigned)pos.x < (unsigned)mapWidth && (unsigned)pos.y < (unsigned)mapHeight, "X (%d) or Y (%d) coordinate for path finding node is out of range!", pos.x, pos.y);

	// Create the node.
	PathNode node;
	unsigned costFactor = context.isDangerous(pos.x, pos.y) ? 5 : 1;
	node.p = pos;
	node.dist = prevDist + fpathEstimate(prevPos, pos) * costFactor;
	node.est = node.dist + fpathGoodEstimate(pos, dest);

	Vector2i delta = Vector2i(pos.x - prevPos.x, pos.y - prevPos.y) * 64;
	bool isDiagonal = delta.x && delta.y;

	PathExploredTile &expl = context.map[pos.x + pos.y * mapWidth];
	if (expl.iteration == context.iteration)
	{
		if (expl.visited)
		{
			return;  // Already visited this tile. Do nothing.
		}
		Vector2i deltaA = delta;
		Vector2i deltaB = Vector2i(expl.dx, expl.dy);
		Vector2i deltaDelta = deltaA - deltaB;  // Vector pointing from current considered source tile leading to pos, to the previously considered source tile leading to pos.
		if (abs(deltaDelta.x) + abs(deltaDelta.y) == 64)
		{
			// prevPos is tile A or B, and pos is tile P. We were previously called with prevPos being tile B or A, and pos tile P.
			// We want to find the distance to tile P, taking into account that the actual shortest path involves coming from somewhere between tile A and tile B.
			// +---+---+
			// |   | P |
			// +---+---+
			// | A | B |
			// +---+---+
			unsigned distA = node.dist - (isDiagonal ? 198 : 140) * costFactor; // If isDiagonal, node is A and expl is B.
			unsigned distB = expl.dist - (isDiagonal ? 140 : 198) * costFactor;
			if (!isDiagonal)
			{
				std::swap(distA, distB);
				std::swap(deltaA, deltaB);
			}
			int gradientX = int(distB - distA) / costFactor;
			if (gradientX > 0 && gradientX <= 98)  // 98 = floor(140/√2), so gradientX <= 98 is needed so that gradientX < gradientY.
			{
				// The distance gradient is now known to be somewhere between the direction from A to P and the direction from B to P.
				static const uint8_t gradYLookup[99] = {140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 139, 139, 139, 139, 139, 139, 139, 139, 139, 138, 138, 138, 138, 138, 138, 137, 137, 137, 137, 137, 136, 136, 136, 136, 135, 135, 135, 134, 134, 134, 134, 133, 133, 133, 132, 132, 132, 131, 131, 130, 130, 130, 129, 129, 128, 128, 127, 127, 126, 126, 126, 125, 125, 124, 123, 123, 122, 122, 121, 121, 120, 119, 119, 118, 118, 117, 116, 116, 115, 114, 113, 113, 112, 111, 110, 110, 109, 108, 107, 106, 106, 105, 104, 103, 102, 101, 100};
				int gradientY = gradYLookup[gradientX];  // = sqrt(140² -  gradientX²), rounded to nearest integer
				unsigned distP = gradientY * costFactor + distB;
				node.est -= node.dist - distP;
				node.dist = distP;
				delta = (deltaA * gradientX + deltaB * (gradientY - gradientX)) / gradientY;
			}
		}
		if (expl.dist <= node.dist)
		{
			return;  // A different path to this tile is shorter.
		}
	}

	// Remember where we have been, and remember the way back.
	expl.iteration = context.iteration;
	expl.dx = delta.x;
	expl.dy = delta.y;
	expl.dist = node.dist;
	expl.visited = false;

	// Add the node to the node heap.
	context.nodes.push_back(node);                               // Add the new node to nodes.
	std::push_heap(context.nodes.begin(), context.nodes.end());  // Move the new node to the right place in the heap.
}

/// Recalculates estimates to new tileF tile.
static void fpathAStarReestimate(PathfindContext &context, PathCoord tileF)
{
	for (auto &node : context.nodes)
	{
		node.est = node.dist + fpathGoodEstimate(node.p, tileF);
	}

	// Changing the estimates breaks the heap ordering. Fix the heap ordering.
	std::make_heap(context.nodes.begin(), context.nodes.end());
}

/// Returns nearest explored tile to tileF.
static PathCoord fpathAStarExplore(PathfindContext &context, PathCoord tileF)
{
	PathCoord       nearestCoord(0, 0);
	unsigned        nearestDist = 0xFFFFFFFF;

	// search for a route
	bool foundIt = false;
	while (!context.nodes.empty() && !foundIt)
	{
		PathNode node = fpathTakeNode(context.nodes);
		if (context.map[node.p.x + node.p.y * mapWidth].visited)
		{
			continue;  // Already been here.
		}
		context.map[node.p.x + node.p.y * mapWidth].visited = true;

		// note the nearest node to the target so far
		if (node.est - node.dist < nearestDist)
		{
			nearestCoord = node.p;
			nearestDist = node.est - node.dist;
		}

		if (node.p == tileF)
		{
			// reached the target
			nearestCoord = node.p;
			foundIt = true;  // Break out of loop, but not before inserting neighbour nodes, since the neighbours may be important if the context gets reused.
		}

		// loop through possible moves in 8 directions to find a valid move
		for (unsigned dir = 0; dir < ARRAY_SIZE(aDirOffset); ++dir)
		{
			// Try a new location
			int x = node.p.x + aDirOffset[dir].x;
			int y = node.p.y + aDirOffset[dir].y;

			/*
			   5  6  7
			     \|/
			   4 -I- 0
			     /|\
			   3  2  1
			   odd:orthogonal-adjacent tiles even:non-orthogonal-adjacent tiles
			*/
			if (dir % 2 != 0 && !context.dstIgnore.isNonblocking(node.p.x, node.p.y) && !context.dstIgnore.isNonblocking(x, y))
			{
				int x2, y2;

				// We cannot cut corners
				x2 = node.p.x + aDirOffset[(dir + 1) % 8].x;
				y2 = node.p.y + aDirOffset[(dir + 1) % 8].y;
				if (context.isBlocked(x2, y2))
				{
					continue;
				}
				x2 = node.p.x + aDirOffset[(dir + 7) % 8].x;
				y2 = node.p.y + aDirOffset[(dir + 7) % 8].y;
				if (context.isBlocked(x2, y2))
				{
					continue;
				}
			}

			// See if the node is a blocking tile
			if (context.isBlocked(x, y))
			{
				// tile is blocked, skip it
				continue;
			}

			// Now insert the point into the appropriate list, if not already visited.
			fpathNewNode(context, tileF, PathCoord(x, y), node.dist, node.p);
		}
	}

	return nearestCoord;
}

static void fpathInitContext(PathfindContext &context, const std::shared_ptr<const PathBlockingMap> &blockingMap, PathCoord tileS, PathCoord tileRealS, PathCoord tileF, PathNonblockingArea dstIgnore)
{
	context.assign(blockingMap, tileS, dstIgnore);

	// Add the start point to the open list
	fpathNewNode(context, tileF, tileRealS, 0, tileRealS);
	ASSERT(!context.nodes.empty(), "fpathNewNode failed to add node.");
}

class PathfindContextList
{
public:
	struct Iterator
	{
		using iterator_category = std::random_access_iterator_tag;
		using difference_type = std::ptrdiff_t;
		using value_type = PathfindContext;
		using pointer = value_type*;
		using reference = value_type&;

		Iterator(PathfindContextList& list, size_t idx) : m_list(list), m_idx(idx)
		{}

		Iterator(const Iterator& other)
		: m_list(other.m_list)
		, m_idx(other.m_idx)
		{}

		Iterator& operator=(const Iterator& other)
		{
			m_list = other.m_list;
			m_idx = other.m_idx;
			return *this;
		}

		reference operator*() const { return m_list.contexts[m_list.orderedIndexes[m_idx]]; }
		pointer operator->() const { return &m_list.contexts[m_list.orderedIndexes[m_idx]]; }

		Iterator& operator++() { ++m_idx; return *this; }
		Iterator operator++(int) { Iterator tmp = *this; ++(*this); return tmp; }

		Iterator& operator--() { --m_idx; return *this; }
		Iterator operator--(int) { Iterator tmp = *this; --(*this); return tmp; }

		Iterator operator+(difference_type n) const { return Iterator(m_list, m_idx + n); }
		Iterator operator-(difference_type n) const { return Iterator(m_list, m_idx - n); }
		difference_type operator-(const Iterator& other) const { return m_idx - other.m_idx; }
		Iterator& operator+=(difference_type n) { m_idx += n; return *this; }
		Iterator& operator-=(difference_type n) { m_idx -= n; return *this; }

		reference operator[](difference_type n) const { return m_list.contexts[m_list.orderedIndexes[n]]; }

		bool operator== (const Iterator& other) { return m_idx == other.m_idx; }
		bool operator!= (const Iterator& other) { return m_idx != other.m_idx; }
		bool operator< (const Iterator& other) const { return m_idx < other.m_idx; }
		bool operator> (const Iterator& other) const { return m_idx > other.m_idx; }
		bool operator<= (const Iterator& other) const { return m_idx <= other.m_idx; }
		bool operator>= (const Iterator& other) const { return m_idx >= other.m_idx; }

	private:
		friend class PathfindContextList;

		PathfindContextList& m_list;
		size_t m_idx;
	};

public:

	Iterator push_back(PathfindContext& ctx);
	Iterator push_back(PathfindContext&& ctx);

	void moveToFront(Iterator it); // invalidates any iterators

	Iterator begin() { return Iterator(*this, 0); }
	Iterator end() { return Iterator(*this, orderedIndexes.size()); }

	void clear();

	bool empty() const { return contexts.empty(); }
	PathfindContext& front() { return contexts[orderedIndexes.front()]; }

	size_t size() const { return contexts.size(); }

private:
	std::vector<PathfindContext> contexts;
	std::vector<size_t> orderedIndexes;
};

PathfindContextList::Iterator PathfindContextList::push_back(PathfindContext& ctx)
{
	contexts.push_back(ctx);
	orderedIndexes.push_back(contexts.size() - 1);
	return Iterator(*this, orderedIndexes.size() - 1);
}

PathfindContextList::Iterator PathfindContextList::push_back(PathfindContext&& ctx)
{
	contexts.push_back(std::move(ctx));
	orderedIndexes.push_back(contexts.size() - 1);
	return Iterator(*this, orderedIndexes.size() - 1);
}

void PathfindContextList::moveToFront(Iterator it)
{
	auto oIt = orderedIndexes.begin() + it.m_idx;
	std::rotate(orderedIndexes.begin(), oIt, oIt + 1);
}

void PathfindContextList::clear()
{
	contexts.clear();
	orderedIndexes.clear();
}

class FPathExecuteContextImpl : public FPathExecuteContext
{
public:
	virtual ~FPathExecuteContextImpl();

	void resetForNewGameTimeIfNeeded(const PATHJOB& job);
public:
	/// Last recently used list of contexts.
	PathfindContextList fpathContexts;
	/// Used to avoid extra allocations in fpathAStarRoute
	std::vector<Vector2i> pathBuffer;
};

FPathExecuteContext::~FPathExecuteContext()
{ }

FPathExecuteContextImpl::~FPathExecuteContextImpl()
{ }

void FPathExecuteContextImpl::resetForNewGameTimeIfNeeded(const PATHJOB& job)
{
	if (!fpathContexts.empty() && job.blockingMap->type.gameTime != fpathContexts.front().myGameTime)
	{
		fpathContexts.clear();
	}
}

std::shared_ptr<FPathExecuteContext> makeFPathExecuteContext()
{
	return std::make_shared<FPathExecuteContextImpl>();
}

ASR_RETVAL fpathAStarRoute(const std::shared_ptr<FPathExecuteContext>& ctx, MOVE_CONTROL *psMove, PATHJOB *psJob)
{
	ASR_RETVAL      retval = ASR_OK;

	bool            mustReverse = true;

	auto ctxImpl = std::static_pointer_cast<FPathExecuteContextImpl>(ctx);
	ctxImpl->resetForNewGameTimeIfNeeded(*psJob);

	auto& fpathContexts = ctxImpl->fpathContexts;
	const PathCoord tileOrig(map_coord(psJob->origX), map_coord(psJob->origY));
	const PathCoord tileDest(map_coord(psJob->destX), map_coord(psJob->destY));
	const PathNonblockingArea dstIgnore(psJob->dstStructure);

	PathCoord endCoord;  // Either nearest coord (mustReverse = true) or orig (mustReverse = false).

	auto contextIterator = fpathContexts.begin();
	for (; contextIterator != fpathContexts.end(); ++contextIterator)
	{
		if (!contextIterator->matches(psJob->blockingMap, tileDest, dstIgnore))
		{
			// This context is not for the same droid type and same destination.
			continue;
		}

		// We have tried going to tileDest before.

		if (contextIterator->map[tileOrig.x + tileOrig.y * mapWidth].iteration == contextIterator->iteration
		    && contextIterator->map[tileOrig.x + tileOrig.y * mapWidth].visited)
		{
			// Already know the path from orig to dest.
			endCoord = tileOrig;
		}
		else
		{
			// Need to find the path from orig to dest, continue previous exploration.
			fpathAStarReestimate(*contextIterator, tileOrig);
			endCoord = fpathAStarExplore(*contextIterator, tileOrig);
		}

		if (endCoord != tileOrig)
		{
			// orig turned out to be on a different island than what this context was used for, so can't use this context data after all.
			continue;
		}

		mustReverse = false;  // We have the path from the nearest reachable tile to dest, to orig.
		break;  // Found the path! Don't search more contexts.
	}

	if (contextIterator == fpathContexts.end())
	{
		// We did not find an appropriate context. Make one.
		contextIterator = fpathContexts.push_back(PathfindContext());

		// Init a new context, overwriting the oldest one if we are caching too many.
		// We will be searching from orig to dest, since we don't know where the nearest reachable tile to dest is.
		fpathInitContext(*contextIterator, psJob->blockingMap, tileOrig, tileOrig, tileDest, dstIgnore);
		endCoord = fpathAStarExplore(*contextIterator, tileDest);
		contextIterator->nearestCoord = endCoord;
	}

	PathfindContext &context = *contextIterator;

	// return the nearest route if no actual route was found
	if (context.nearestCoord != tileDest)
	{
		retval = ASR_NEAREST;
	}

	// Get route, in reverse order.
	std::vector<Vector2i>& path = ctxImpl->pathBuffer;
	path.clear();

	Vector2i newP(0, 0);
	for (Vector2i p(world_coord(endCoord.x) + TILE_UNITS / 2, world_coord(endCoord.y) + TILE_UNITS / 2); true; p = newP)
	{
		ASSERT_OR_RETURN(ASR_FAILED, worldOnMap(p.x, p.y), "Assigned XY coordinates (%d, %d) not on map!", (int)p.x, (int)p.y);
		ASSERT_OR_RETURN(ASR_FAILED, path.size() < (static_cast<size_t>(mapWidth) * static_cast<size_t>(mapHeight)), "Pathfinding got in a loop.");

		path.push_back(p);

		PathExploredTile &tile = context.map[map_coord(p.x) + map_coord(p.y) * mapWidth];
		newP = p - Vector2i(tile.dx, tile.dy) * (TILE_UNITS / 64);
		Vector2i mapP = map_coord(newP);
		int xSide = newP.x - world_coord(mapP.x) > TILE_UNITS / 2 ? 1 : -1; // 1 if newP is on right-hand side of the tile, or -1 if newP is on the left-hand side of the tile.
		int ySide = newP.y - world_coord(mapP.y) > TILE_UNITS / 2 ? 1 : -1; // 1 if newP is on bottom side of the tile, or -1 if newP is on the top side of the tile.
		if (context.isBlocked(mapP.x + xSide, mapP.y))
		{
			newP.x = world_coord(mapP.x) + TILE_UNITS / 2; // Point too close to a blocking tile on left or right side, so move the point to the middle.
		}
		if (context.isBlocked(mapP.x, mapP.y + ySide))
		{
			newP.y = world_coord(mapP.y) + TILE_UNITS / 2; // Point too close to a blocking tile on rop or bottom side, so move the point to the middle.
		}
		if (map_coord(p) == Vector2i(context.tileS.x, context.tileS.y) || p == newP)
		{
			break;  // We stopped moving, because we reached the destination or the closest reachable tile to context.tileS. Give up now.
		}
	}
	if (retval == ASR_OK)
	{
		// Found exact path, so use exact coordinates for last point, no reason to lose precision
		Vector2i v(psJob->destX, psJob->destY);
		if (mustReverse)
		{
			path.front() = v;
		}
		else
		{
			path.back() = v;
		}
	}

	// Allocate memory
	psMove->asPath.resize(path.size());

	// get the route in the correct order
	// If as I suspect this is to reverse the list, then it's my suspicion that
	// we could route from destination to source as opposed to source to
	// destination. We could then save the reversal. to risky to try now...Alex M
	//
	// The idea is impractical, because you can't guarentee that the target is
	// reachable. As I see it, this is the reason why psNearest got introduced.
	// -- Dennis L.
	//
	// If many droids are heading towards the same destination, then destination
	// to source would be faster if reusing the information in nodeArray. --Cyp
	if (mustReverse)
	{
		// Copy the list, in reverse.
		std::copy(path.rbegin(), path.rend(), psMove->asPath.data());

		if (!context.isBlocked(tileOrig.x, tileOrig.y))  // If blocked, searching from tileDest to tileOrig wouldn't find the tileOrig tile.
		{
			// Next time, search starting from nearest reachable tile to the destination.
			fpathInitContext(context, psJob->blockingMap, tileDest, context.nearestCoord, tileOrig, dstIgnore);
		}
	}
	else
	{
		// Copy the list.
		std::copy(path.begin(), path.end(), psMove->asPath.data());
	}

	// Move context to beginning of last recently used list.
	if (contextIterator != fpathContexts.begin())  // Not sure whether or not the splice is a safe noop, if equal.
	{
		fpathContexts.moveToFront(contextIterator);
	}

	psMove->destination = psMove->asPath[path.size() - 1];

	return retval;
}

void fpathSetBlockingMap(PATHJOB *psJob)
{
	if (fpathCurrentGameTime != gameTime)
	{
		// New tick, remove maps which are no longer needed.
		fpathCurrentGameTime = gameTime;
		fpathBlockingMaps.clear();
	}

	// Figure out which map we are looking for.
	PathBlockingType type;
	type.gameTime = gameTime;
	type.propulsion = psJob->propulsion;
	type.owner = psJob->owner;
	type.moveType = psJob->moveType;

	// Find the map.
	auto i = std::find_if(fpathBlockingMaps.begin(), fpathBlockingMaps.end(), [&](std::shared_ptr<PathBlockingMap> const &ptr) {
		return *ptr == type;
	});
	if (i == fpathBlockingMaps.end())
	{
		// Didn't find the map, so i does not point to a map.
		auto blockMap = std::make_shared<PathBlockingMap>();
		fpathBlockingMaps.push_back(blockMap);

		// blockMap now points to an empty map with no data. Fill the map.
		blockMap->type = type;
		std::vector<bool> &map = blockMap->map;
		map.resize(static_cast<size_t>(mapWidth) * static_cast<size_t>(mapHeight));
		uint32_t checksumMap = 0, checksumDangerMap = 0, factor = 0;
		for (int y = 0; y < mapHeight; ++y)
			for (int x = 0; x < mapWidth; ++x)
			{
				map[x + y * mapWidth] = fpathBaseBlockingTile(x, y, type.propulsion, type.owner, type.moveType);
				checksumMap ^= map[x + y * mapWidth] * (factor = 3 * factor + 1);
			}
		if (!isHumanPlayer(type.owner) && type.moveType == FMT_MOVE)
		{
			std::vector<bool> &dangerMap = blockMap->dangerMap;
			dangerMap.resize(static_cast<size_t>(mapWidth) * static_cast<size_t>(mapHeight));
			for (int y = 0; y < mapHeight; ++y)
				for (int x = 0; x < mapWidth; ++x)
				{
					dangerMap[x + y * mapWidth] = auxTile(x, y, type.owner) & AUXBITS_THREAT;
					checksumDangerMap ^= dangerMap[x + y * mapWidth] * (factor = 3 * factor + 1);
				}
		}
		syncDebug("blockingMap(%d,%d,%d,%d) = %08X %08X", gameTime, psJob->propulsion, psJob->owner, psJob->moveType, checksumMap, checksumDangerMap);

		psJob->blockingMap = blockMap;
	}
	else
	{
		syncDebug("blockingMap(%d,%d,%d,%d) = cached", gameTime, psJob->propulsion, psJob->owner, psJob->moveType);

		psJob->blockingMap = *i;
	}
}
