Finished pathfinding

Pathfinding written out
Bugs still need fixing