Basic pathfinding using A* - C (CLI)