Graph reimplemented, Pathfinding ported, just need to debug