Path Finding Stabilization

Mostly tweaks and bug fixes.