Fixed pathfinding

Fixes #62, #65, #66, #75, #84, #86, #87