Refactoring: Pathfinder

Rewrite pathfinder algorithm and start to integrate into battle