Most of the code for pathfinding

Algorithm doesn't work and I don't understand why