A* implementado, falta un bucle y mirar el tema de los costes acumulados pero la idea parece estar Ok