A* algorithm almost working, Node class reworked