[ Foro de C ]

Necesito ayuda con un problema de metaheuristica...

Tengo este codigo de aqui el cual por alguna razon siempre arroja "Camino no encontrado". Si alguien puede ayudarme seria genial:
int AStar(Laberinto *lab, Coordenada inicio, Coordenada fin, Coordenada *ruta) {
   Coordenada openSet[MAX_COORDENADAS];
   bool visitado[MAX_FILAS][MAX_COLUMNAS] = {false};
   int openCount = 0;

   // Inicializa la celda de inicio
   openSet[openCount] = inicio;
   openCount++;
   lab->mapa[inicio.fila][inicio.columna].g = 0;
   lab->mapa[inicio.fila][inicio.columna].h = calcularHeuristica(inicio, fin);
   lab->mapa[inicio.fila][inicio.columna].f = lab->mapa[inicio.fila][inicio.columna].h;
   
   // Algoritmo principal
   while (openCount > 0) {
    printf("AAA");
   // Encuentra la celda con el menor costo 'f' en openSet
   int mejorIndice = 0;
   
   for (int i = 1; i < openCount; i++) {
       if (lab->mapa[openSet[i].fila][openSet[i].columna].f <
           lab->mapa[openSet[mejorIndice].fila][openSet[mejorIndice].columna].f) {
           mejorIndice = i;
       }
   }

   // Seleccionamos la celda actual y la eliminamos del openSet
   Coordenada actual = openSet[mejorIndice];
   openSet[mejorIndice] = openSet[--openCount];

   // Si hemos llegado a la meta
   if (actual.fila == fin.fila && actual.columna == fin.columna) {
       // Reconstruimos la ruta y devolvemos éxito
       int indiceRuta = 0;
       while (!(actual.fila == inicio.fila && actual.columna == inicio.columna)) {
           ruta[indiceRuta++] = actual;
           actual = lab->mapa[actual.fila][actual.columna].padre;
       }
       ruta[indiceRuta] = inicio;
       return 1; // Camino encontrado
   }


   // Añadir la celda actual al conjunto de celdas visitadas
   visitado[actual.fila][actual.columna] = true;
   // Mover del openSet al conjunto visitado
   openSet[mejorIndice] = openSet[--openCount];
       
       
       
   // Examinar los vecinos
Coordenada vecinos[4] = {
  {actual.fila - 1, actual.columna}, // Norte
    {actual.fila + 1, actual.columna}, // Sur
    {actual.fila, actual.columna - 1}, // Oeste
    {actual.fila, actual.columna + 1}  // Este
};

for (int i = 0; i < 4; i++) {
    Coordenada vecino = vecinos[i];

    // Verifica si el vecino está dentro del laberinto y es caminable
    if (vecino.fila < 0 || vecino.fila >= lab->altura ||
        vecino.columna < 0 || vecino.columna >= lab->anchura ||
        lab->mapa[vecino.fila][vecino.columna].valor == '#' ||
        visitado[vecino.fila][vecino.columna]) {
        continue; // No es un vecino válido
    }

    // Calculamos el nuevo costo acumulado para llegar al vecino
    float gNuevo = lab->mapa[actual.fila][actual.columna].g + 1;

    // Si encontramos un mejor camino hacia el vecino
    if (gNuevo < lab->mapa[vecino.fila][vecino.columna].g ||
        lab->mapa[vecino.fila][vecino.columna].g == 0) {
        lab->mapa[vecino.fila][vecino.columna].padre = actual;
        lab->mapa[vecino.fila][vecino.columna].g = gNuevo;
        lab->mapa[vecino.fila][vecino.columna].h = calcularHeuristica(vecino, fin);
        lab->mapa[vecino.fila][vecino.columna].f =
            lab->mapa[vecino.fila][vecino.columna].g + lab->mapa[vecino.fila][vecino.columna].h;

        // Añade el vecino al openSet si aún no ha sido procesado
        openSet[openCount++] = vecino;
    }
}
   }
   
   return 0; // No hay solución
}


MENU:
int MenuPrincipal()
{
int opcion;
do
{
system("cls");
printf(BLANCO_T AMARILLO_F ">>>>GESTOR DE LABERINTOS<<<<" RESET_COLOR "\n");
printf(BLANCO_T AMARILLO_F "****************************" RESET_COLOR "\n");
printf(BLANCO_T AMARILLO_F "* 1. Generar laberinto     *" RESET_COLOR "\n");
printf(BLANCO_T AMARILLO_F "* 2. Editar laberinto      *" RESET_COLOR "\n");
printf(BLANCO_T AMARILLO_F "* 3. Guardar laberinto     *" RESET_COLOR "\n");
printf(BLANCO_T AMARILLO_F "* 4. Cargar laberinto      *" RESET_COLOR "\n");
printf(BLANCO_T AMARILLO_F "* 5. Tiene solucion?       *" RESET_COLOR "\n");
printf(BLANCO_T AMARILLO_F "* 6. Solucionar            *" RESET_COLOR "\n");
printf(BLANCO_T AMARILLO_F "* 7. Resolver con A*       *" RESET_COLOR "\n");
printf(BLANCO_T AMARILLO_F "* 0. Salir                 *" RESET_COLOR "\n");
printf(BLANCO_T AMARILLO_F "****************************" RESET_COLOR "\n>");
fflush(stdin);
scanf("%d", &opcion);
}while(opcion < 0 || opcion > 7);

return opcion;
}


MAIN:
case 7: {// Resolver el laberinto con A*
    system("cls");

    Coordenada inicio, fin;

    // Buscar la entrada (E) y salida (S) en el mapa
    for (int i = 0; i < lab.altura; i++) {
        for (int j = 0; j < lab.anchura; j++) {
            if (lab.mapa[i][j].valor == 'E') {
                inicio = (Coordenada){i, j};
            }
            if (lab.mapa[i][j].valor == 'S') {
                fin = (Coordenada){i, j};
            }
        }
    }

    // Llamar a la función AStar
    Coordenada ruta[MAX_COORDENADAS];
    int resultado = AStar(&lab, inicio, fin, ruta);

    if (resultado) {
        printf("Camino encontrado:\n");
        for (int i = MAX_COORDENADAS - 1; i >= 0; i--) {
            if (ruta[i].fila == -1) break;
            printf("(%d, %d)\n", ruta[i].fila, ruta[i].columna);
        }
    } else {
        printf("No se encontró un camino.\n");
    }
    system("pause");
    break;
}