Refactor pathfinding function
Add impassable terrain
Extend map