Started implementation in C of the Pathfinding algorithm