A* Pathfinding Algorithm in C find path with lowest sum through a matrix of numbers