Reduce pathfinding array size

Signed-off-by: Ross Allan <rallanpcl@gmail.com>
Improved profiling

Signed-off-by: Ross Allan <rallanpcl@gmail.com>