Pathfind finished

Finished pathfind but it's ugly