Pathfinding Updates

A* functional but buggy