Fix logic errors
Merge branch 'pathfinder'