Index: source/simulation2/components/tests/test_Pathfinder.h
===================================================================
--- source/simulation2/components/tests/test_Pathfinder.h
+++ source/simulation2/components/tests/test_Pathfinder.h
@@ -17,8 +17,11 @@
 
 #include "simulation2/system/ComponentTest.h"
 
+#define TEST
+
 #include "simulation2/components/ICmpObstructionManager.h"
 #include "simulation2/components/ICmpPathfinder.h"
+#include "simulation2/components/CCmpPathfinder_Common.h"
 
 #include "graphics/MapReader.h"
 #include "graphics/Terrain.h"
@@ -64,6 +67,133 @@
 		TS_ASSERT_EQUALS((Pathfinding::NAVCELL_SIZE >> 1).ToInt_RoundToZero(), Pathfinding::NAVCELL_SIZE_LOG2);
 	}
 
+	void hierarchical_globalRegions_testmap(std::wstring map)
+	{
+		CTerrain terrain;
+
+		CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain);
+		sim2.LoadDefaultScripts();
+		sim2.ResetState();
+
+		CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself
+
+		LDR_BeginRegistering();
+		mapReader->LoadMap(map,
+						   sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue,
+						   &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
+						   &sim2, &sim2.GetSimContext(), -1, false);
+		LDR_EndRegistering();
+		TS_ASSERT_OK(LDR_NonprogressiveLoad());
+
+		sim2.Update(0);
+
+		CmpPtr<ICmpPathfinder> cmpPathfinder(sim2, SYSTEM_ENTITY);
+
+		pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default");
+		HierarchicalPathfinder& hier = ((CCmpPathfinder*)cmpPathfinder.operator->())->m_LongPathfinder.GetHierarchicalPathfinder();
+
+		std::map<HierarchicalPathfinder::RegionID, HierarchicalPathfinder::GlobalRegionID> globalRegions = hier.m_GlobalRegions[obstructionsMask];
+
+#ifdef DEBUG
+		// speed things up a little for debug mode otherwise it'll take ages.
+		for (u8 cj = 4; cj < hier.m_ChunksH; cj += 16)
+			for (u8 ci = 4; ci < hier.m_ChunksW; ci += 16)
+#else
+		for (u8 cj = 0; cj < hier.m_ChunksH; cj += 2)
+			for (u8 ci = 0; ci < hier.m_ChunksW; ci += 2)
+#endif
+				for(u16 i : hier.GetChunk(ci, cj, obstructionsMask).m_RegionsID)
+				{
+					std::set<HierarchicalPathfinder::RegionID> reachables;
+					hier.FindReachableRegions(HierarchicalPathfinder::RegionID{ci, cj, i}, reachables, obstructionsMask);
+					HierarchicalPathfinder::GlobalRegionID ID = globalRegions[HierarchicalPathfinder::RegionID{ci, cj, i}];
+					for (HierarchicalPathfinder::RegionID region : reachables)
+						TS_ASSERT_EQUALS(ID, globalRegions[region]);
+				}
+	}
+
+	void test_hierarchical_globalRegions()
+	{
+		// This test validates that the hierarchical's pathfinder global regions are in accordance with its regions
+		// IE it asserts that, for any two regions A and B of the hierarchical pathfinder, if one can find a path from A to B
+		// then A and B have the same global region.
+		std::vector<std::wstring> maps = { L"maps/scenarios/Peloponnese.pmp", L"maps/skirmishes/Corinthian Isthmus (2).pmp", L"maps/skirmishes/Greek Acropolis (2).pmp" };
+
+		for (std::wstring t : maps)
+			hierarchical_globalRegions_testmap(t);
+	}
+
+	void hierarchical_update_testmap(std::wstring map)
+	{
+		CTerrain terrain;
+
+		CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain);
+		sim2.LoadDefaultScripts();
+		sim2.ResetState();
+
+		CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself
+
+		LDR_BeginRegistering();
+		mapReader->LoadMap(map,
+						   sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue,
+						   &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
+						   &sim2, &sim2.GetSimContext(), -1, false);
+		LDR_EndRegistering();
+		TS_ASSERT_OK(LDR_NonprogressiveLoad());
+
+		sim2.Update(0);
+
+		CmpPtr<ICmpPathfinder> cmpPathfinder(sim2, SYSTEM_ENTITY);
+
+		pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default");
+		HierarchicalPathfinder& hier = ((CCmpPathfinder*)cmpPathfinder.operator->())->m_LongPathfinder.GetHierarchicalPathfinder();
+
+		// make copies
+		const auto pristine_GR = hier.m_GlobalRegions;
+		const auto pristine_Chunks = hier.m_Chunks;
+		const HierarchicalPathfinder::EdgesMap pristine_Edges = hier.m_Edges.at(obstructionsMask);
+
+		Grid<NavcellData>* pathfinderGrid = ((CCmpPathfinder*)cmpPathfinder.operator->())->m_LongPathfinder.m_Grid;
+
+		Grid<u8> dirtyGrid(hier.m_ChunksW * HierarchicalPathfinder::CHUNK_SIZE,hier.m_ChunksH * HierarchicalPathfinder::CHUNK_SIZE);
+		srand(1234);
+
+#ifdef DEBUG
+		size_t tries = 2;
+#else
+		size_t tries = 20;
+#endif
+		for (size_t i = 0; i < tries; ++i)
+		{
+			// Dirty a random one
+			dirtyGrid.reset();
+			u8 ci = rand() % (hier.m_ChunksW-10) + 8;
+			u8 cj = rand() % (hier.m_ChunksH-10) + 8;
+			dirtyGrid.set(ci * HierarchicalPathfinder::CHUNK_SIZE + 4, cj * HierarchicalPathfinder::CHUNK_SIZE + 4, 1);
+
+			hier.Update(pathfinderGrid, dirtyGrid);
+
+			// Formally speaking we should rather validate that regions exist with the same pixels, but so far
+			// re-initing regions will keep the same IDs for the same pixels so this is OK.
+			TS_ASSERT_EQUALS(hier.m_Chunks.at(obstructionsMask), pristine_Chunks.at(obstructionsMask));
+			// same here
+			TS_ASSERT_EQUALS(pristine_Edges, hier.m_Edges.at(obstructionsMask));
+
+			// TODO: ought to test global regions, but those should be OK if the connections are OK
+			// and glboal regions ID can change on update making it annoying.
+		}
+	}
+
+	void test_hierarchical_update()
+	{
+		// This test validates that the "Update" function of the hierarchical pathfinder
+		// ends up in a correct state (by comparing it with the clean, "Recompute"-d state).
+		std::vector<std::wstring> maps = { L"maps/scenarios/Peloponnese.pmp", L"maps/skirmishes/Corinthian Isthmus (2).pmp", L"maps/skirmishes/Greek Acropolis (2).pmp" };
+
+		for (std::wstring t : maps)
+			hierarchical_update_testmap(t);
+	}
+
 	void test_performance_DISABLED()
 	{
 		CTerrain terrain;
@@ -285,6 +415,367 @@
 		stream << "</svg>\n";
 	}
 
+	static const size_t scale = 1;
+
+	void MakeGoalReachable_testIteration(CStr& map, u16 sx, u16 sz, u16 gx, u16 gz)
+	{
+		int colors[26][3] = {
+			{ 255, 0, 0 },
+			{ 0, 255, 0 },
+			{ 0, 0, 255 },
+			{ 255, 255, 0 },
+			{ 255, 0, 255 },
+			{ 0, 255, 255 },
+			{ 255, 255, 255 },
+
+			{ 127, 0, 0 },
+			{ 0, 127, 0 },
+			{ 0, 0, 127 },
+			{ 127, 127, 0 },
+			{ 127, 0, 127 },
+			{ 0, 127, 127 },
+			{ 127, 127, 127},
+
+			{ 255, 127, 0 },
+			{ 127, 255, 0 },
+			{ 255, 0, 127 },
+			{ 127, 0, 255},
+			{ 0, 255, 127 },
+			{ 0, 127, 255},
+			{ 255, 127, 127},
+			{ 127, 255, 127},
+			{ 127, 127, 255},
+
+			{ 127, 255, 255 },
+			{ 255, 127, 255 },
+			{ 255, 255, 127 },
+		};
+
+		// Load up a map, dump hierarchical regions
+		// From a few positions test making a few positions reachable.
+		// Check performance and output results as svg files so user can verify sanity.
+
+		CTerrain terrain;
+
+		CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain);
+		sim2.LoadDefaultScripts();
+		sim2.ResetState();
+
+		CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself
+
+		LDR_BeginRegistering();
+		mapReader->LoadMap(map.FromUTF8(),
+						   sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue,
+						   &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
+						   &sim2, &sim2.GetSimContext(), -1, false);
+		LDR_EndRegistering();
+		TS_ASSERT_OK(LDR_NonprogressiveLoad());
+
+		sim2.Update(0);
+
+		map.Replace(".pmp",""); map.Replace("/","");
+		CStr path("MGR_" + map + "_"  + CStr::FromUInt(sx) + "_"   + CStr::FromUInt(sz) + "_" + CStr::FromUInt(gx) + "_" + CStr::FromUInt(gz) + ".html");
+		std::cout << path << std::endl;
+		std::ofstream stream(OsString(path).c_str(), std::ofstream::out | std::ofstream::trunc);
+
+		CmpPtr<ICmpObstructionManager> cmpObstructionManager(sim2, SYSTEM_ENTITY);
+		CmpPtr<ICmpPathfinder> cmpPathfinder(sim2, SYSTEM_ENTITY);
+
+		pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default");
+		const Grid<NavcellData>& obstructions = cmpPathfinder->GetPassabilityGrid();
+
+		// Dump as canvas. This is terrible code but who cares.
+		stream << "<!DOCTYPE html>\n";
+		stream << "<head>\n"
+					"<style>\n"
+						"canvas { position:absolute; left:0px; top: 0px; }"
+						"canvas #hier { opacity:0.5; }"
+						"input[type=\"checkbox\"] { height: 50px; width:50px; background:red; -webkit-appearance:none; }input[type=\"checkbox\"]:checked { background:blue; }"
+						"p.pixel { z-index:20000; display:block; position:absolute; width: " << scale << "px; height: " << scale << "px;}"
+					"</style>\n"
+				  "</head>\n"
+				  "<body>";
+
+		stream << "<p style='z-index: 200; background: white; position: absolute;transform:scale(2); transform-origin:0 0;'>\n";
+		stream << "<input type='checkbox' checked='true' id='gridcb' onchange=\"var checked = document.getElementById('gridcb').checked; document.getElementById('grid').style.display = checked ? 'block' :' none';\">Display Grid</input>";
+		stream << "<input type='checkbox' checked='true' id='hiercb' onchange=\"var checked = document.getElementById('hiercb').checked; document.getElementById('hier').style.display = checked ? 'block' :' none';\">Display Hierarchical grid </input>";
+		stream << "<input type='checkbox' checked='true' id='hier2cb' onchange=\"var checked = document.getElementById('hier2cb').checked; document.getElementById('hier2').style.display = checked ? 'block' :' none';\">Display Global Regions </input>";
+		stream << "<input type='checkbox' checked='true' id='pathcb' onchange=\"var checked = document.getElementById('pathcb').checked; document.getElementById('path').style.display = checked ? 'block' :' none';\">Display Path search </input>";
+		stream << "<input type='checkbox' checked='true' id='path2cb' onchange=\"var checked = document.getElementById('path2cb').checked; document.getElementById('path2').style.display = checked ? 'block' :' none';\">Display path lookups </input>";
+		stream << "<input id='scaleinput' type='number' value='1' onchange=\"var nscale = +document.getElementById('scaleinput').value; scale=nscale; printGrid(scale);printHier(scale);\">";
+		stream << "<input id='totoro' type='range' min='0' max='100' value='1' oninput=\"var nscale = +document.getElementById('totoro').value; printPath(scale, nscale);\">\n";
+		stream << "</p>";
+
+		stream << "<canvas id='grid' width='" << obstructions.m_W*scale << "' height='" << obstructions.m_H*scale << "'></canvas>";
+
+		// set up grid
+		stream << "<script>"
+					"var scale = " << scale << ";\n"
+					"function printGrid(scale) {\n"
+					"var grid = document.getElementById('grid');\n"
+					"grid.width = scale * " << obstructions.m_W << ";"
+					"grid.height = scale * " << obstructions.m_H << ";"
+					"var context = grid.getContext('2d');"
+					"context.clearRect(0, 0, grid.width, grid.height);"
+					"context.fillStyle = '#000000';\n";
+		for (u16 j = 0; j < obstructions.m_H; ++j)
+			for (u16 i = 0; i < obstructions.m_W; i++)
+				if (obstructions.get(i, j) & obstructionsMask)
+				{
+					u16 i1 = i;
+					for (; i1 < obstructions.m_W; i1++)
+						if (!(obstructions.get(i1, j) & obstructionsMask))
+							break;
+					stream << "context.fillRect(" << i << " * scale," << j << " * scale," << (i1 - i) << " * scale, scale);\n";
+					i = i1 - 1;
+				}
+		stream << "};";
+		stream << "printGrid(scale)";
+		stream << "</script>\n";
+
+		// Dump hierarchical regions on another one.
+		stream << "<canvas id='hier' width='" << obstructions.m_W*scale << "' height='" << obstructions.m_H*scale << "'></canvas>";
+		stream << "<canvas id='hier2' width='" << obstructions.m_W*scale << "' height='" << obstructions.m_H*scale << "'></canvas>";
+
+		HierarchicalPathfinder& hier = ((CCmpPathfinder*)cmpPathfinder.operator->())->m_LongPathfinder.GetHierarchicalPathfinder();
+
+		stream << "<script>"
+		"function printHier(scale) {\n"
+		"var hier = document.getElementById('hier');\n"
+		"hier.width = scale * " << obstructions.m_W << ";"
+		"hier.height = scale * " << obstructions.m_H << ";"
+		"var context = hier.getContext('2d');"
+		"context.clearRect(0, 0, hier.width, hier.height);"
+		"context.fillStyle = '#000000';\n";
+		for (u16 j = 0; j < obstructions.m_H; ++j)
+			for (u16 i = 0; i < obstructions.m_W; i++)
+			{
+				u16 st = hier.Get(i, j, obstructionsMask).r;
+				u16 ci = i / hier.CHUNK_SIZE;
+				u16 cj = j / hier.CHUNK_SIZE;
+				u16 i1 = i;
+				for (; i1 < obstructions.m_W; ++i1)
+					if (hier.Get(i1, j, obstructionsMask).r != st || i1 / hier.CHUNK_SIZE != ci)
+						break;
+
+				if (st == 0)
+					stream << "context.fillStyle = 'rgba(0,0,0,1.0)';\n";
+				else if (st == 0xFFFF)
+					stream << "context.fillStyle = 'rgba(255,0,255,0.5)';\n";
+				else
+				{
+					size_t c = (st + ci*5 + cj*7) % ARRAY_SIZE(colors);
+					stream << "context.fillStyle = 'rgba("<<colors[c][0]<<","<<colors[c][1]<<","<<colors[c][2]<<",1.0)';\n";
+				}
+				stream << "context.fillRect(" << i << " * scale," << j << " * scale," << (i1 - i) << " * scale, scale);\n";
+				i = i1 - 1;
+			}
+		stream << "};";
+		stream << "printHier(scale);\n"
+		"function printHier2(scale) {\n"
+		"var hier2 = document.getElementById('hier2');\n"
+		"hier2.width = scale * " << obstructions.m_W << ";"
+		"hier2.height = scale * " << obstructions.m_H << ";"
+		"var context = hier2.getContext('2d');"
+		"context.clearRect(0, 0, hier2.width, hier2.height);"
+		"context.fillStyle = '#000000';\n";
+		for (u16 j = 0; j < obstructions.m_H; ++j)
+			for (u16 i = 0; i < obstructions.m_W; i++)
+			{
+				u32 globalID = hier.GetGlobalRegion(i,j,obstructionsMask);
+				u16 i1 = i;
+				for (; i1 < obstructions.m_W; ++i1)
+					if (hier.GetGlobalRegion(i1,j,obstructionsMask) != globalID)
+						break;
+
+				if (globalID == 0)
+					stream << "context.fillStyle = 'rgba(0,0,0,1.0)';\n";
+				else
+				{
+					size_t c = globalID % ARRAY_SIZE(colors);
+					stream << "context.fillStyle = 'rgba("<<colors[c][0]<<","<<colors[c][1]<<","<<colors[c][2]<<",1.0)';\n";
+				}
+				stream << "context.fillRect(" << i << " * scale," << j << " * scale," << (i1 - i) << " * scale, scale);\n";
+				i = i1 - 1;
+			}
+		stream << "};";
+		stream << "printHier2(scale)";
+		stream << "</script>\n";
+
+		// Ok let's check out MakeGoalReachable
+		// pick a point
+		fixed X,Z;
+		X = fixed::FromInt(sx);
+		Z = fixed::FromInt(sz);
+		u16 gridSize = obstructions.m_W;
+		// Convert the start coordinates to tile indexes
+		u16 i0, j0;
+		Pathfinding::NearestNavcell(X, Z, i0, j0, gridSize, gridSize);
+
+		// Dump as HTML so that it's on top and add fancy shadows so it's easy to see.
+		stream << "<p class='pixel' style='z-index:500000; border-radius:100%;box-shadow: 0 0 0 10px green, 0 0 0 12px black; left:" << i0 * scale << "px; top:" << j0 * scale << "px;'></p>";
+
+		hier.FindNearestPassableNavcell(i0, j0, obstructionsMask);
+		stream << "<p class='pixel' style='border-radius:100%;box-shadow: 0 0 0 20px blue, 0 0 0 22px red; left:" << i0 * scale << "px; top:" << j0 * scale << "px;'></p>";
+
+		// Make the goal reachable. This includes shortening the path if the goal is in a non-passable
+		// region, transforming non-point goals to reachable point goals, etc.
+
+		PathGoal goal;
+		goal.type = PathGoal::POINT;
+		goal.x = fixed::FromInt(gx);
+		goal.z = fixed::FromInt(gz);
+		goal.u = CFixedVector2D(fixed::FromInt(1), fixed::Zero());
+		goal.v = CFixedVector2D(fixed::Zero(),fixed::FromInt(1));
+		goal.hh = fixed::FromInt(0);
+		goal.hw = fixed::FromInt(0);
+
+		u16 i1, j1;
+		Pathfinding::NearestNavcell(goal.x, goal.z, i1, j1, gridSize, gridSize);
+		stream << "<p class='pixel' style='z-index:500000; border-radius:100%;box-shadow: 0 0 0 10px red, 0 0 0 12px white; left:" << i1 * scale << "px; top:" << j1 * scale << "px;'></p>";
+
+		PathGoal goalCopy = goal;
+		hier.MakeGoalReachable(i0, j0, goalCopy, obstructionsMask);
+
+		Pathfinding::NearestNavcell(goalCopy.x, goalCopy.z, i1, j1, gridSize, gridSize);
+		stream << "<p class='pixel' style='border-radius:100%;box-shadow: 0 0 0 20px blue, 0 0 0 25px red; left:" << i1 * scale << "px; top:" << j1 * scale << "px;'></p>";
+
+
+		stream << "<canvas id='path' width='" << obstructions.m_W*scale << "' height='" << obstructions.m_H*scale << "'></canvas>";
+		stream << "<canvas id='path2' width='" << obstructions.m_W*scale << "' height='" << obstructions.m_H*scale << "'></canvas>";
+
+		stream << "<script>"
+		"var maxStep = 0;"
+		"function printPath(scale, step) {\n"
+		"var path = document.getElementById('path');\n"
+		"path.width = scale * " << obstructions.m_W << ";"
+		"path.height = scale * " << obstructions.m_H << ";"
+		"var context = path.getContext('2d');"
+		"context.clearRect(0, 0, path.width, path.height);"
+		"var path2 = document.getElementById('path2');\n"
+		"path2.width = scale * " << obstructions.m_W << ";"
+		"path2.height = scale * " << obstructions.m_H << ";"
+		"var context2 = path2.getContext('2d');"
+		"context2.clearRect(0, 0, path2.width, path2.height);";
+
+		goalCopy = goal;
+		hier.MakeGoalReachable_Astar(i0, j0, goalCopy, obstructionsMask);//, stream);
+
+		stream << "};";
+		stream << "printPath(scale,10000);";
+		stream << "document.getElementById('totoro').max = maxStep";
+		stream << "</script>\n";
+
+		Pathfinding::NearestNavcell(goalCopy.x, goalCopy.z, i1, j1, gridSize, gridSize);
+		stream << "<p class='pixel' style='border-radius:100%;box-shadow: 0 0 0 20px green, 0 0 0 25px red; left:" << i1 * scale << "px; top:" << j1 * scale << "px;'></p>";
+
+		// Perf test. This is a little primitive, but should work well enough to give an idea of the algo.
+
+		double t = timer_Time();
+
+		srand(1234);
+		for (size_t j = 0; j < 10000; ++j)
+		{
+			PathGoal oldGoal = goal;
+			hier.MakeGoalReachable(i0, j0, goal, obstructionsMask);
+			goal = oldGoal;
+		}
+
+		t = timer_Time() - t;
+		printf("\nPoint Only  Goal old:  [%f]\n", t);
+
+		std::ofstream ostr(OsString("out.html").c_str(), std::ofstream::out | std::ofstream::trunc);
+
+		t = timer_Time();
+		for (size_t j = 0; j < 10000; ++j)
+		{
+			PathGoal oldGoal = goal;
+			hier.MakeGoalReachable_Astar(i0, j0, goal, obstructionsMask);//, ostr);
+			goal = oldGoal;
+		}
+
+		t = timer_Time() - t;
+		printf("\nPoint Only  Goal new:  [%f]\n", t);
+
+
+		// Replace the point with a small circle: this prevents the flood fill from early-exiting
+		// whereas A* can perform about the same.
+		goal.type = PathGoal::CIRCLE;
+		goal.hh = fixed::FromInt(1);
+		goal.hw = fixed::FromInt(1);
+
+		t = timer_Time();
+
+		srand(1234);
+		for (size_t j = 0; j < 10000; ++j)
+		{
+			PathGoal oldGoal = goal;
+			hier.MakeGoalReachable(i0, j0, goal, obstructionsMask);
+			goal = oldGoal;
+		}
+
+		t = timer_Time() - t;
+		printf("\nSmall Circle Goal old:  [%f]\n", t);
+
+		t = timer_Time();
+		for (size_t j = 0; j < 10000; ++j)
+		{
+			PathGoal oldGoal = goal;
+			hier.MakeGoalReachable_Astar(i0, j0, goal, obstructionsMask);//, ostr);
+			goal = oldGoal;
+		}
+
+		t = timer_Time() - t;
+		printf("\nSmall Circle Goal new:  [%f]\n\n\n", t);
+
+		stream << "</body>\n";
+	}
+
+	void test_MakeGoalReachable_performance_DISABLED()
+	{
+		struct test
+		{
+			CStr map;
+			u16 sx;
+			u16 sz;
+			u16 gx;
+			u16 gz;
+		};
+		/*
+		 * Compare performance on a few cases:
+		 * - short path, good case for the flood fill (it finds immediately the point/circle and stops)
+		 * - short path, bad case for the flood fill (it will not find the correct region right away, so it's literally about 100x slower than the former)
+		 * - long path around the bend, close to worst-case for A*
+		 * - Long unreachable path, but the "closest point" is reachable in almost a straight direction.
+		 * - Inverse of the former (the region to fill is much smaller)
+		 * - large island, A* still has a lot to fill here
+		 * - straight with obstructions
+		 * - straight, fewer obstructions
+		 * - bad case (U shape around the start containing A*)
+		 * - bad case: U shape + unreachable. We need to return something reasonably close, not in the first U
+		 * - bad calse: two U shapes tripping A*
+		 */
+		std::vector<test> maps = {
+			{ "maps/scenarios/Peloponnese.pmp", 600, 800, 800, 800 },
+			{ "maps/scenarios/Peloponnese.pmp", 600, 800, 600, 900 },
+			{ "maps/scenarios/Peloponnese.pmp", 600, 800, 770, 1400 },
+			{ "maps/scenarios/Peloponnese.pmp", 1000, 300, 1500, 1450 },
+			{ "maps/scenarios/Peloponnese.pmp", 1500, 1450, 1000, 300 },
+			{ "maps/skirmishes/Corsica and Sardinia (4).pmp", 300, 1300, 1300, 300 },
+			{ "maps/skirmishes/Alpine_Mountains_(3).pmp", 200, 200, 800, 800 },
+			{ "maps/skirmishes/Corinthian Isthmus (2).pmp", 200, 200, 800, 800 },
+			{ "maps/skirmishes/Mediterranean Cove (2).pmp", 200, 200, 800, 800 },
+			{ "maps/skirmishes/Dueling Cliffs (3v3).pmp", 200, 200, 800, 800 },
+			{ "maps/skirmishes/Dueling Cliffs (3v3).pmp", 350, 200, 900, 900 },
+			{ "maps/skirmishes/Dueling Cliffs (3v3).pmp", 200, 200, 950, 950 },
+		};
+
+		for (auto t : maps)
+		{
+			MakeGoalReachable_testIteration(t.map, t.sx, t.sz, t.gx, t.gz);
+		}
+	}
+
 	void DumpPath(std::ostream& stream, int i0, int j0, int i1, int j1, CmpPtr<ICmpPathfinder>& cmpPathfinder)
 	{
 		entity_pos_t x0 = entity_pos_t::FromInt(i0);
Index: source/simulation2/helpers/HierarchicalPathfinder.h
===================================================================
--- source/simulation2/helpers/HierarchicalPathfinder.h
+++ source/simulation2/helpers/HierarchicalPathfinder.h
@@ -24,6 +24,9 @@
 #include "Render.h"
 #include "graphics/SColor.h"
 
+#include <boost/container/flat_map.hpp>
+#include <boost/container/flat_set.hpp>
+
 /**
  * Hierarchical pathfinder.
  *
@@ -42,11 +45,20 @@
  * region and then doing a relatively small graph search.
  */
 
+#ifdef TEST
+class TestCmpPathfinder;
+#endif
+
 class HierarchicalOverlay;
 
 class HierarchicalPathfinder
 {
+#ifdef TEST
+	friend class TestCmpPathfinder;
+#endif
 public:
+	typedef u32 GlobalRegionID;
+
 	struct RegionID
 	{
 		u8 ci, cj; // chunk ID
@@ -54,7 +66,7 @@
 
 		RegionID(u8 ci, u8 cj, u16 r) : ci(ci), cj(cj), r(r) { }
 
-		bool operator<(RegionID b) const
+		bool operator<(const RegionID& b) const
 		{
 			// Sort by chunk ID, then by per-chunk region ID
 			if (ci < b.ci)
@@ -68,7 +80,7 @@
 			return r < b.r;
 		}
 
-		bool operator==(RegionID b) const
+		bool operator==(const RegionID& b) const
 		{
 			return ((ci == b.ci) && (cj == b.cj) && (r == b.r));
 		}
@@ -89,6 +101,7 @@
 	bool IsChunkDirty(int ci, int cj, const Grid<u8>& dirtinessGrid) const;
 
 	RegionID Get(u16 i, u16 j, pass_class_t passClass);
+	GlobalRegionID GetGlobalRegion(u16 i, u16 j, pass_class_t passClass);
 
 	/**
 	 * Updates @p goal so that it's guaranteed to be reachable from the navcell
@@ -101,6 +114,8 @@
 	 * at the reachable navcell of the goal which is nearest to the starting navcell.
 	 */
 	void MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass);
+	// TODO: remove this and rename it MakeGoalReachable
+	void MakeGoalReachable_Astar(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass);
 
 	/**
 	 * Updates @p i, @p j (which is assumed to be an impassable navcell)
@@ -130,7 +145,7 @@
 	struct Chunk
 	{
 		u8 m_ChunkI, m_ChunkJ; // chunk ID
-		u16 m_NumRegions; // number of local region IDs (starting from 1)
+		std::vector<u16> m_RegionsID; // IDs of local region, without 0
 		u16 m_Regions[CHUNK_SIZE][CHUNK_SIZE]; // local region ID per navcell
 
 		cassert(CHUNK_SIZE*CHUNK_SIZE/2 < 65536); // otherwise we could overflow m_NumRegions with a checkerboard pattern
@@ -144,16 +159,42 @@
 		void RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const;
 
 		bool RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const;
+
+#ifdef TEST
+		bool operator==(const Chunk& b) const
+		{
+			return (m_ChunkI == b.m_ChunkI && m_ChunkJ == b.m_ChunkJ && m_RegionsID == b.m_RegionsID && memcmp(&m_Regions, &b.m_Regions, sizeof(u16) * CHUNK_SIZE * CHUNK_SIZE) == 0);
+		}
+#endif
 	};
 
 	typedef std::map<RegionID, std::set<RegionID> > EdgesMap;
 
-	void FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges);
+	void RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges);
+	void UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges);
 
 	void FindReachableRegions(RegionID from, std::set<RegionID>& reachable, pass_class_t passClass);
 
 	void FindPassableRegions(std::set<RegionID>& regions, pass_class_t passClass);
 
+	void FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set<RegionID>& regions, pass_class_t passClass);
+
+	/*
+	 * Implementations of A* to make a goal reachable. Called by MakeGoalReachable
+	 * There are two cases: if the goal is known to be reachable, and if the goal is known to be unreachable (but we can try to get close).
+	 * We reuse flat_XX containers so as to have good cache locality and avoid the cost of allocating memory. Flat_XX implementa  map/set as a sorted vector
+	 */
+	boost::container::flat_map<RegionID, HierarchicalPathfinder::RegionID> m_Astar_Predecessor;
+	boost::container::flat_map<RegionID, int> m_Astar_GScore;
+	boost::container::flat_map<RegionID, int> m_Astar_FScore;
+	boost::container::flat_set<RegionID> m_Astar_ClosedNodes;
+	boost::container::flat_set<RegionID> m_Astar_OpenNodes;
+
+	inline int DistBetween(const RegionID& a, const RegionID& b)
+	{
+		return (abs(a.ci - b.ci) + abs(a.cj - b.cj)) * CHUNK_SIZE - 30;
+	};
+
 	/**
 	 * Updates @p iGoal and @p jGoal to the navcell that is the nearest to the
 	 * initial goal coordinates, in one of the given @p regions.
@@ -174,6 +215,9 @@
 
 	std::map<pass_class_t, EdgesMap> m_Edges;
 
+	std::map<pass_class_t, std::map<RegionID, GlobalRegionID>> m_GlobalRegions;
+	std::vector<GlobalRegionID> m_AvailableGlobalRegionIDs;
+
 	// Passability classes for which grids will be updated when calling Update
 	std::map<std::string, pass_class_t> m_PassClassMasks;
 
Index: source/simulation2/helpers/HierarchicalPathfinder.cpp
===================================================================
--- source/simulation2/helpers/HierarchicalPathfinder.cpp
+++ source/simulation2/helpers/HierarchicalPathfinder.cpp
@@ -111,13 +111,12 @@
 	}
 
 	// Directly point the root ID
-	m_NumRegions = 0;
 	for (u16 i = 1; i < regionID+1; ++i)
 	{
-		if (connect[i] == i)
-			++m_NumRegions;
-		else
+		if (connect[i] != i)
 			connect[i] = RootID(i, connect);
+		if (std::find(m_RegionsID.begin(),m_RegionsID.end(), connect[i]) == m_RegionsID.end())
+			m_RegionsID.push_back(connect[i]);
 	}
 
 	// Replace IDs by the root ID
@@ -224,6 +223,8 @@
 	{
 	case PathGoal::POINT:
 	{
+		if (gi/CHUNK_SIZE != m_ChunkI || gj/CHUNK_SIZE != m_ChunkJ)
+			return false;
 		if (m_Regions[gj-m_ChunkJ * CHUNK_SIZE][gi-m_ChunkI * CHUNK_SIZE] == r)
 		{
 			iOut = gi;
@@ -366,6 +367,10 @@
 	m_Chunks.clear();
 	m_Edges.clear();
 
+	// reset global regions
+	m_AvailableGlobalRegionIDs.clear();
+	m_AvailableGlobalRegionIDs.push_back(1);
+
 	for (auto& passClassMask : allPassClasses)
 	{
 		pass_class_t passClass = passClassMask.second;
@@ -381,16 +386,35 @@
 		}
 
 		// Construct the search graph over the regions
-
 		EdgesMap& edges = m_Edges[passClass];
+		RecomputeAllEdges(passClass, edges);
 
-		for (int cj = 0; cj < m_ChunksH; ++cj)
-		{
-			for (int ci = 0; ci < m_ChunksW; ++ci)
-			{
-				FindEdges(ci, cj, passClass, edges);
-			}
-		}
+		// Spread global regions.
+		std::map<RegionID, GlobalRegionID>& globalRegion = m_GlobalRegions[passClass];
+		globalRegion.clear();
+		for (u8 cj = 0; cj < m_ChunksH; ++cj)
+			for (u8 ci = 0; ci < m_ChunksW; ++ci)
+				for (u16 rid : GetChunk(ci, cj, passClass).m_RegionsID)
+				{
+					RegionID reg{ci,cj,rid};
+					if (globalRegion.find(reg) == globalRegion.end())
+					{
+						GlobalRegionID ID = m_AvailableGlobalRegionIDs.back();
+						m_AvailableGlobalRegionIDs.pop_back();
+						if (m_AvailableGlobalRegionIDs.empty())
+							m_AvailableGlobalRegionIDs.push_back(ID+1);
+
+						globalRegion.insert({ reg, ID });
+						// avoid creating an empty link if possible, FindReachableRegions uses [] which calls the default constructor
+						if (edges.find(reg) != edges.end())
+						{
+							std::set<RegionID> reachable;
+							FindReachableRegions(reg, reachable, passClass);
+							for (const RegionID& region : reachable)
+								globalRegion.insert({ region, ID });
+						}
+					}
+				}
 	}
 
 	if (m_DebugOverlay)
@@ -405,9 +429,10 @@
 {
 	PROFILE3("Hierarchical Update");
 
-	for (int cj = 0; cj <  m_ChunksH; ++cj)
+	std::vector<RegionID> updated;
+	for (u8 cj = 0; cj <  m_ChunksH; ++cj)
 	{
-		for (int ci = 0; ci < m_ChunksW; ++ci)
+		for (u8 ci = 0; ci < m_ChunksW; ++ci)
 		{
 			if (!IsChunkDirty(ci, cj, dirtinessGrid))
 				continue;
@@ -415,25 +440,79 @@
 			{
 				pass_class_t passClass = passClassMask.second;
 				Chunk& a = m_Chunks[passClass].at(ci + cj*m_ChunksW);
+
+				// Clean up edges and global region ID
+				EdgesMap& edgeMap = m_Edges[passClass];
+				for (u16 i : a.m_RegionsID)
+				{
+					RegionID reg{ci, cj, i};
+					m_GlobalRegions[passClass].erase(reg);
+					for (const RegionID& neighbor : edgeMap[reg])
+					{
+						edgeMap[neighbor].erase(reg);
+						if (edgeMap[neighbor].empty())
+							edgeMap.erase(neighbor);
+					}
+					edgeMap.erase(reg);
+				}
+
+				// recompute
 				a.InitRegions(ci, cj, grid, passClass);
+
+				for (u16 i : a.m_RegionsID)
+					updated.push_back(RegionID{ci, cj, i});
+
+				// add back edges
+				UpdateEdges(ci, cj, passClass, edgeMap);
 			}
 		}
 	}
 
-	// TODO: Also be clever with edges
-	m_Edges.clear();
-	for (const std::pair<std::string, pass_class_t>& passClassMask : m_PassClassMasks)
-	{
-		pass_class_t passClass = passClassMask.second;
-		EdgesMap& edges = m_Edges[passClass];
+	// Add back global region ID
+	// To try and be clever we'll run a custom flood-fill that stops as soon as it runs into something we know,
+	// and if nothing then we'll create a new global region.
+	// It also keeps track of all connected regions with no IDs in case of contiguous dirtiness (likely) to be faster if possible.
+	// This probably scales poorly with a large enough update?
 
-		for (int cj = 0; cj < m_ChunksH; ++cj)
+	for (const RegionID& reg : updated)
+		for (const std::pair<std::string, pass_class_t>& passClassMask : m_PassClassMasks)
 		{
-			for (int ci = 0; ci < m_ChunksW; ++ci)
+			std::set<RegionID> visited;
+			std::vector<RegionID> open;
+			std::vector<RegionID> updating = { reg };
+			open.push_back(reg);
+
+			GlobalRegionID ID = 0;
+			std::map<RegionID, GlobalRegionID>& globalRegion = m_GlobalRegions[passClassMask.second];
+			EdgesMap& edgeMap = m_Edges[passClassMask.second];
+			// avoid creating empty edges.
+			bool unlinked = edgeMap.find(reg) == edgeMap.end();
+			while (!open.empty() && ID == 0 && !unlinked)
 			{
-				FindEdges(ci, cj, passClass, edges);
+				RegionID curr = open.back();
+				open.pop_back();
+				for (const RegionID& region : edgeMap[curr])
+					if (visited.insert(region).second)
+					{
+						open.push_back(region);
+						if (globalRegion.find(region) != globalRegion.end())
+						{
+							ID = globalRegion.at(region);
+							break;
+						}
+						else
+							updating.push_back(region);
+					}
 			}
-		}
+			if (ID == 0)
+			{
+				ID = m_AvailableGlobalRegionIDs.back();
+				m_AvailableGlobalRegionIDs.pop_back();
+				if (m_AvailableGlobalRegionIDs.empty())
+					m_AvailableGlobalRegionIDs.push_back(ID+1);
+			}
+			for (const RegionID& reg : updating)
+				globalRegion[reg] = ID;
 	}
 
 	if (m_DebugOverlay)
@@ -462,22 +541,15 @@
 }
 
 /**
- * Find edges between regions in this chunk and the adjacent below/left chunks.
+ * Connect a chunk's regions to their neighbors. Not optimised for global recomputing.
+ * TODO: reduce code duplication with below
  */
-void HierarchicalPathfinder::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges)
+void HierarchicalPathfinder::UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges)
 {
 	std::vector<Chunk>& chunks = m_Chunks[passClass];
 
 	Chunk& a = chunks.at(cj*m_ChunksW + ci);
 
-	// For each edge between chunks, we loop over every adjacent pair of
-	// navcells in the two chunks. If they are both in valid regions
-	// (i.e. are passable navcells) then add a graph edge between those regions.
-	// (We don't need to test for duplicates since EdgesMap already uses a
-	// std::set which will drop duplicate entries.)
-	// But as set.insert can be quite slow on large collection, and that we usually
-	// try to insert the same values, we cache the previous one for a fast test.
-
 	if (ci > 0)
 	{
 		Chunk& b = chunks.at(cj*m_ChunksW + (ci-1));
@@ -499,6 +571,27 @@
 		}
 	}
 
+	if (ci < m_ChunksW-1)
+	{
+		Chunk& b = chunks.at(cj*m_ChunksW + (ci+1));
+		RegionID raPrev(0,0,0);
+		RegionID rbPrev(0,0,0);
+		for (int j = 0; j < CHUNK_SIZE; ++j)
+		{
+			RegionID ra = a.Get(CHUNK_SIZE-1, j);
+			RegionID rb = b.Get(0, j);
+			if (ra.r && rb.r)
+			{
+				if (ra == raPrev && rb == rbPrev)
+					continue;
+				edges[ra].insert(rb);
+				edges[rb].insert(ra);
+				raPrev = ra;
+				rbPrev = rb;
+			}
+		}
+	}
+
 	if (cj > 0)
 	{
 		Chunk& b = chunks.at((cj-1)*m_ChunksW + ci);
@@ -520,6 +613,94 @@
 		}
 	}
 
+	if (cj < m_ChunksH - 1)
+	{
+		Chunk& b = chunks.at((cj+1)*m_ChunksW + ci);
+		RegionID raPrev(0,0,0);
+		RegionID rbPrev(0,0,0);
+		for (int i = 0; i < CHUNK_SIZE; ++i)
+		{
+			RegionID ra = a.Get(i, CHUNK_SIZE-1);
+			RegionID rb = b.Get(i, 0);
+			if (ra.r && rb.r)
+			{
+				if (ra == raPrev && rb == rbPrev)
+					continue;
+				edges[ra].insert(rb);
+				edges[rb].insert(ra);
+				raPrev = ra;
+				rbPrev = rb;
+			}
+		}
+	}
+}
+
+/**
+ * Find edges between regions in all chunks, in an optimised manner (only look at top/left)
+ */
+void HierarchicalPathfinder::RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges)
+{
+	std::vector<Chunk>& chunks = m_Chunks[passClass];
+
+	edges.clear();
+
+	for (int cj = 0; cj < m_ChunksH; ++cj)
+	{
+		for (int ci = 0; ci < m_ChunksW; ++ci)
+		{
+			Chunk& a = chunks.at(cj*m_ChunksW + ci);
+
+			// For each edge between chunks, we loop over every adjacent pair of
+			// navcells in the two chunks. If they are both in valid regions
+			// (i.e. are passable navcells) then add a graph edge between those regions.
+			// (We don't need to test for duplicates since EdgesMap already uses a
+			// std::set which will drop duplicate entries.)
+			// But as set.insert can be quite slow on large collection, and that we usually
+			// try to insert the same values, we cache the previous one for a fast test.
+
+			if (ci > 0)
+			{
+				Chunk& b = chunks.at(cj*m_ChunksW + (ci-1));
+				RegionID raPrev(0,0,0);
+				RegionID rbPrev(0,0,0);
+				for (int j = 0; j < CHUNK_SIZE; ++j)
+				{
+					RegionID ra = a.Get(0, j);
+					RegionID rb = b.Get(CHUNK_SIZE-1, j);
+					if (ra.r && rb.r)
+					{
+						if (ra == raPrev && rb == rbPrev)
+							continue;
+						edges[ra].insert(rb);
+						edges[rb].insert(ra);
+						raPrev = ra;
+						rbPrev = rb;
+					}
+				}
+			}
+
+			if (cj > 0)
+			{
+				Chunk& b = chunks.at((cj-1)*m_ChunksW + ci);
+				RegionID raPrev(0,0,0);
+				RegionID rbPrev(0,0,0);
+				for (int i = 0; i < CHUNK_SIZE; ++i)
+				{
+					RegionID ra = a.Get(i, 0);
+					RegionID rb = b.Get(i, CHUNK_SIZE-1);
+					if (ra.r && rb.r)
+					{
+						if (ra == raPrev && rb == rbPrev)
+							continue;
+						edges[ra].insert(rb);
+						edges[rb].insert(ra);
+						raPrev = ra;
+						rbPrev = rb;
+					}
+				}
+			}
+		}
+	}
 }
 
 /**
@@ -557,7 +738,7 @@
 			xz.push_back(b.Y.ToFloat());
 
 			m_DebugOverlayLines.emplace_back();
-			m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0);
+			m_DebugOverlayLines.back().m_Color = CColor(1.0, 0.0, 0.0, 1.0);
 			SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true);
 		}
 	}
@@ -571,6 +752,14 @@
 	return m_Chunks[passClass][cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE);
 }
 
+HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(u16 i, u16 j, pass_class_t passClass)
+{
+	RegionID region = Get(i, j, passClass);
+	if (region.r == 0)
+		return (GlobalRegionID)0;
+	return m_GlobalRegions[passClass][region];
+}
+
 void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass)
 {
 	PROFILE2("MakeGoalReachable");
@@ -578,20 +767,40 @@
 
 	// Find everywhere that's reachable
 	std::set<RegionID> reachableRegions;
-	FindReachableRegions(source, reachableRegions, passClass);
 
-	// Check whether any reachable region contains the goal
-	// And get the navcell that's the closest to the point
+	// Flood-fill the region graph, starting at 'from',
+	// collecting all the regions that are reachable via edges
+	std::vector<RegionID> open;
+	open.reserve(64);
+	open.push_back(source);
+	reachableRegions.insert(source);
+
 
 	u16 bestI = 0;
 	u16 bestJ = 0;
 	u32 dist2Best = std::numeric_limits<u32>::max();
 
-	for (const RegionID& region : reachableRegions)
+	u16 iGoal, jGoal;
+	Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);
+
+	std::set<RegionID> possRegs;
+	FindGoalRegions(iGoal, jGoal, goal, possRegs, passClass);
+
+	EdgesMap& edgeMap = m_Edges[passClass];
+	while (!open.empty())
 	{
+		RegionID curr = open.back();
+		open.pop_back();
+
+		for (const RegionID& region : edgeMap[curr])
+			// Add to the reachable set; if this is the first time we added
+			// it then also add it to the open list
+			if (reachableRegions.insert(region).second)
+				open.push_back(region);
+
 		// Skip region if its chunk doesn't contain the goal area
-		entity_pos_t x0 = Pathfinding::NAVCELL_SIZE * (region.ci * CHUNK_SIZE);
-		entity_pos_t z0 = Pathfinding::NAVCELL_SIZE * (region.cj * CHUNK_SIZE);
+		entity_pos_t x0 = Pathfinding::NAVCELL_SIZE * (curr.ci * CHUNK_SIZE);
+		entity_pos_t z0 = Pathfinding::NAVCELL_SIZE * (curr.cj * CHUNK_SIZE);
 		entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE;
 		entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE;
 		if (!goal.RectContainsGoal(x0, z0, x1, z1))
@@ -602,7 +811,7 @@
 
 		// If the region contains the goal area, the goal is reachable
 		// Remember the best point for optimization.
-		if (GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, i, j, dist2))
+		if (GetChunk(curr.ci, curr.cj, passClass).RegionNearestNavcellInGoal(curr.r, i0, j0, goal, i, j, dist2))
 		{
 			// If it's a point, no need to move it, we're done
 			if (goal.type == PathGoal::POINT)
@@ -613,16 +822,18 @@
 				bestJ = j;
 				dist2Best = dist2;
 			}
+			possRegs.erase(curr);
+			// We ran out of potential regions so we're done
+			if (possRegs.empty())
+				return;
 		}
+
 	}
 
 	// If the goal area wasn't reachable,
 	// find the navcell that's nearest to the goal's center
 	if (dist2Best == std::numeric_limits<u32>::max())
 	{
-		u16 iGoal, jGoal;
-		Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);
-
 		FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass);
 
 		// Construct a new point goal at the nearest reachable navcell
@@ -640,6 +851,233 @@
 	goal = newGoal;
 }
 
+#define OUTPUT 0
+
+#if OUTPUT
+	#include <fstream>
+#endif
+
+#if OUTPUT
+// also change the header
+void HierarchicalPathfinder::MakeGoalReachable_Astar(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass, std::ofstream& stream)
+#else
+void HierarchicalPathfinder::MakeGoalReachable_Astar(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass)
+#endif
+{
+	/*
+	 * Relatively straightforward implementation of A* on the hierarchical pathfinder graph.
+	 * Since this isn't a grid, we cannot use JPS (though I'm fairly sure it could sort of be extended to work, but it's probably not trivial/worth it)
+	 * Uses flat_set and flat_map over std::set and std::map since testing proves that reusing the memory ends up being more efficient
+	 * The main optimisations are:
+	 *	- picking the best item directly from the open list when we can be sure we know which one it is (see fasttrack)
+	 *	- checking whether the goal is reachable or not, and if it isn't stopping early to avoid slowly flood-filling everything
+	 * The path isn't used for now but can reconstructed using predecessor.
+	 * NB: strictly speaking, from a reachability POV, we can skip A* entirely if the goal has the same global region ID (see below).
+	 * Haven't done so yet and having the path may be interesting but it this function ends up being too slow it would be an idea.
+	 */
+	RegionID source = Get(i0, j0, passClass);
+
+	u16 gi, gj;
+	Pathfinding::NearestNavcell(goal.x, goal.z, gi, gj, m_W, m_H);
+
+	// determine if we will be able to reach the goal.
+	// If not, we can stop A* earlier by being clever.
+	std::set<RegionID> goalRegions;
+	FindGoalRegions(gi, gj, goal, goalRegions, passClass);
+
+	GlobalRegionID startID = GetGlobalRegion(i0, j0, passClass);
+	bool reachable = false;
+	for (const RegionID& r : goalRegions)
+		if (m_GlobalRegions[passClass][r] == startID)
+		{
+			reachable = true;
+			break;
+		}
+
+#if OUTPUT
+	stream << "context.fillStyle = 'rgba(1,0,1,1)';\n";
+	for (const RegionID& r : goalRegions)
+	{
+		entity_pos_t x0 = Pathfinding::NAVCELL_SIZE * (r.ci * CHUNK_SIZE);
+		entity_pos_t z0 = Pathfinding::NAVCELL_SIZE * (r.cj * CHUNK_SIZE);
+		stream << "context2.fillRect(" << x0.ToInt_RoundToZero() << " * scale," << z0.ToInt_RoundToZero() << " * scale," << (int)CHUNK_SIZE << " * scale," << (int)CHUNK_SIZE << " * scale);\n";
+	}
+#endif
+
+	// In general, our maps are relatively open, so it's usually a better bet to be biaised towards minimal distance over path length.
+	int (*DistEstimate)(const RegionID&, u16, u16) = [](const RegionID& source, u16 gi, u16 gj) -> int { return (source.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi)*(source.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi) + (source.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj)*(source.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj); };
+	// However, run unbiaised euclidian if we know the goal is unreachable, since we want to get as close as possible efficiently.
+	if (!reachable)
+		DistEstimate = [](const RegionID& source, u16 gi, u16 gj) -> int {
+			return isqrt64((source.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi)*(source.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi) + (source.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj)*(source.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj));
+		};
+
+	m_Astar_ClosedNodes.clear();
+	m_Astar_OpenNodes.clear();
+	m_Astar_OpenNodes.insert(source);
+
+	m_Astar_Predecessor.clear();
+
+	m_Astar_GScore.clear();
+	m_Astar_GScore[source] = 0;
+
+	m_Astar_FScore.clear();
+	m_Astar_FScore[source] = DistEstimate(source, gi, gj);
+
+	RegionID current {0,0,0};
+
+	u16 bestI, bestJ;
+	u32 dist2;
+
+	u32 timeSinceLastFScoreImprovement = 0;
+#if OUTPUT
+	int step = 0;
+#endif
+
+	RegionID fastTrack = source;
+	int currentFScore = m_Astar_FScore[source];
+	int secondBestFScore = currentFScore;
+	int globalBestFScore = currentFScore;
+
+	EdgesMap& edgeMap = m_Edges[passClass];
+	while (!m_Astar_OpenNodes.empty())
+	{
+		// Since we are not using a fancy open list, we have to go through all nodes each time
+		// So when we are sure that we know the best node (because the last run gave us a node better than us, which was already the best
+		// we can fast-track and not sort but just pick that one instead.
+		// In cases where the obvious path is the best, we hardly ever sort and it's a little faster
+		if (fastTrack.r)
+		{
+			current = fastTrack;
+			currentFScore = m_Astar_FScore[current];
+			secondBestFScore = currentFScore;
+		}
+		else
+		{
+			auto iter = m_Astar_OpenNodes.begin();
+			current = *iter;
+			currentFScore = m_Astar_FScore[current];
+			secondBestFScore = currentFScore;
+			while (++iter != m_Astar_OpenNodes.end())
+			{
+				int score = m_Astar_FScore[*iter];
+				if (score < currentFScore)
+				{
+					current = *iter;
+					secondBestFScore = currentFScore;
+					currentFScore = score;
+				}
+			}
+		}
+
+		// Stop heuristic in case we know we cannot reach the goal.
+		// Indeed this would cause A* to become an inacceptably slow flood fill.
+		// We keep track of our best fScore, we'll early-exit if we're too far from it
+		// or we haven't found a better path in a while.
+		// This will cause us to return largely suboptimal paths now and then,
+		// but then again those should be rare and the player can just re-order a move.
+		if (!reachable)
+		{
+			if (currentFScore < globalBestFScore)
+			{
+				globalBestFScore = currentFScore;
+				timeSinceLastFScoreImprovement = 0;
+			}
+			else if (currentFScore > globalBestFScore * 2 || ++timeSinceLastFScoreImprovement > m_ChunksW)
+				break;
+		}
+
+		entity_pos_t x0 = Pathfinding::NAVCELL_SIZE * (current.ci * CHUNK_SIZE);
+		entity_pos_t z0 = Pathfinding::NAVCELL_SIZE * (current.cj * CHUNK_SIZE);
+		entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE;
+		entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE;
+
+#if OUTPUT
+		stream << "context.fillStyle = 'rgba(" <<DistEstimate(current, gi, gj) / 10000 << ",255,"<< (fastTrack.r > 0 ? 255 : 0) <<",0.8)';\n maxStep = " << step+1 << ";\n";
+		stream << "if (step >= " << step << ") context.fillRect(" << x0.ToInt_RoundToZero() << " * scale," << z0.ToInt_RoundToZero() << " * scale," << (int)CHUNK_SIZE << " * scale," << (int)CHUNK_SIZE << " * scale);\n";
+#endif
+
+		fastTrack = RegionID{0,0,0};
+
+		// TODO: we should get those first and then validate here, instead of recomputing for each.
+		if (goal.RectContainsGoal(x0, z0, x1, z1))
+			if (GetChunk(current.ci, current.cj, passClass).RegionNearestNavcellInGoal(current.r, i0, j0, goal, bestI, bestJ, dist2))
+				break;
+
+		m_Astar_OpenNodes.erase(current);
+		m_Astar_ClosedNodes.emplace(current);
+		if (reachable)
+			m_Astar_FScore.erase(current);
+		m_Astar_GScore.erase(current);
+
+		int currScore = m_Astar_GScore[current];
+		for (const RegionID& neighbor : edgeMap[current])
+		{
+			if (m_Astar_ClosedNodes.find(neighbor) != m_Astar_ClosedNodes.end())
+				continue;
+			int temp_m_Astar_GScore = currScore + DistBetween(neighbor, current);
+			auto iter = m_Astar_OpenNodes.emplace(neighbor);
+			if (!iter.second && temp_m_Astar_GScore >= m_Astar_GScore[neighbor])
+				continue;
+#if OUTPUT
+			x0 = Pathfinding::NAVCELL_SIZE * (neighbor.ci * CHUNK_SIZE);
+			z0 = Pathfinding::NAVCELL_SIZE * (neighbor.cj * CHUNK_SIZE);
+			stream << "context2.fillStyle = 'rgba(255,255,0,0.3)';\n";
+			stream << "if (step >= " << step << ") context2.fillRect(" << x0.ToInt_RoundToZero() << " * scale," << z0.ToInt_RoundToZero() << " * scale," << (int)CHUNK_SIZE << " * scale," << (int)CHUNK_SIZE << " * scale);\n";
+#endif
+
+			m_Astar_GScore[neighbor] = temp_m_Astar_GScore;
+			// no default constructor so we'll use this hack in the meantime
+			auto alreadyThere = m_Astar_Predecessor.emplace( boost::container::flat_map<RegionID, RegionID>::value_type{ neighbor, current });
+			alreadyThere.first->second = current;
+			int score = temp_m_Astar_GScore + DistEstimate(neighbor, gi, gj);
+			if (score < secondBestFScore)
+			{
+				secondBestFScore = score;
+				fastTrack = neighbor;
+			}
+			m_Astar_FScore[neighbor] = score;
+		}
+#if OUTPUT
+		step++;
+#endif
+	}
+
+	// TODO: arrive from predecessor, not origin.
+	bool exactFound = GetChunk(current.ci, current.cj, passClass).RegionNearestNavcellInGoal(current.r, i0, j0, goal, bestI, bestJ, dist2);
+
+#if OUTPUT
+	fixed x0 = Pathfinding::NAVCELL_SIZE * (current.ci * CHUNK_SIZE);
+	fixed z0 = Pathfinding::NAVCELL_SIZE * (current.cj * CHUNK_SIZE);
+	stream << "context.fillStyle = 'rgba(255,0,0,0.6)';\n";
+	stream << "if (step >= " << step << ") context.fillRect(" << x0.ToInt_RoundToZero() << " * scale," << z0.ToInt_RoundToZero() << " * scale," << (int)CHUNK_SIZE << " * scale," << (int)CHUNK_SIZE << " * scale);\n";
+
+	// sanity check
+	if (reachable)
+		ENSURE (exactFound);
+	else
+		ENSURE (!exactFound);
+#endif
+
+	if (!exactFound)
+	{
+		// Pick best and roll with that.
+		current = *std::min_element(m_Astar_ClosedNodes.begin(), m_Astar_ClosedNodes.end(),
+									[this](const RegionID& a, const RegionID& b) -> bool { return m_Astar_FScore[a] < m_Astar_FScore[b]; });
+
+		std::set<RegionID> set = { current };
+		Pathfinding::NearestNavcell(goal.x, goal.z, bestI, bestJ, m_W, m_H);
+
+		FindNearestNavcellInRegions(set, bestI, bestJ, passClass);
+	}
+
+	PathGoal newGoal;
+	newGoal.type = PathGoal::POINT;
+	Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z);
+	goal = newGoal;
+}
+
+
 void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass)
 {
 	std::set<RegionID> regions;
@@ -710,15 +1148,16 @@
 	// collecting all the regions that are reachable via edges
 
 	std::vector<RegionID> open;
+	open.reserve(64);
 	open.push_back(from);
 	reachable.insert(from);
 
+	EdgesMap& edgeMap = m_Edges[passClass];
 	while (!open.empty())
 	{
 		RegionID curr = open.back();
 		open.pop_back();
-
-		for (const RegionID& region : m_Edges[passClass][curr])
+		for (const RegionID& region : edgeMap[curr])
 			// Add to the reachable set; if this is the first time we added
 			// it then also add it to the open list
 			if (reachable.insert(region).second)
@@ -731,12 +1170,34 @@
 	// Construct a set of all regions of all chunks for this pass class
 	for (const Chunk& chunk : m_Chunks[passClass])
 	{
-		// region 0 is impassable tiles
-		for (int r = 1; r <= chunk.m_NumRegions; ++r)
+		for (u16 r : chunk.m_RegionsID)
 			regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r));
 	}
 }
 
+void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set<HierarchicalPathfinder::RegionID>& regions, pass_class_t passClass)
+{
+	if (goal.type == PathGoal::POINT)
+	{
+		regions.insert(Get(gi, gj, passClass));
+		return;
+	}
+
+	// Find bounds
+	int size = (std::max(goal.hh, goal.hw) * 3 / 2).ToInt_RoundToInfinity();
+
+	u16 a,b; u32 c; // unused params
+
+	for (u8 sz = (gj - size) / CHUNK_SIZE; sz <= (gj + size) / CHUNK_SIZE; ++sz)
+		for (u8 sx = (gi - size) / CHUNK_SIZE; sx <= (gi + size) / CHUNK_SIZE; ++sx)
+		{
+			Chunk& chunk = GetChunk(sx, sz, passClass);
+			for (u16 i : chunk.m_RegionsID)
+				if (chunk.RegionNearestNavcellInGoal(i, 0, 0, goal, a, b, c))
+					regions.insert(RegionID{sx, sz, i});
+		}
+}
+
 void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid<u16>& grid)
 {
 	ENSURE(grid.m_W == m_W && grid.m_H == m_H);
Index: source/simulation2/helpers/LongPathfinder.h
===================================================================
--- source/simulation2/helpers/LongPathfinder.h
+++ source/simulation2/helpers/LongPathfinder.h
@@ -164,6 +164,10 @@
 	LongPathfinder();
 	~LongPathfinder();
 
+#ifdef TEST
+	HierarchicalPathfinder& GetHierarchicalPathfinder() { return m_PathfinderHier; }
+#endif
+
 	void SetDebugOverlay(bool enabled);
 
 	void SetHierDebugOverlay(bool enabled, const CSimContext *simContext)
Index: source/simulation2/helpers/LongPathfinder.cpp
===================================================================
--- source/simulation2/helpers/LongPathfinder.cpp
+++ source/simulation2/helpers/LongPathfinder.cpp
@@ -813,7 +813,7 @@
 
 	// Make the goal reachable. This includes shortening the path if the goal is in a non-passable
 	// region, transforming non-point goals to reachable point goals, etc.
-	m_PathfinderHier.MakeGoalReachable(i0, j0, state.goal, passClass);
+	m_PathfinderHier.MakeGoalReachable_Astar(i0, j0, state.goal, passClass);
 
 	// If we're already at the goal tile, then move directly to the exact goal coordinates
 	if (state.goal.NavcellContainsGoal(i0, j0))