Finished updating the pathfinding code, i think

Signed-off-by: Exikle <dixondcunha@hotmail.com>