Pathfinding algorithm

Still working on pathfinding, almost done.