IA/ Cautarea cost uniform

// CAUTAREA CU COST UNIFORM

    noduri[0] = oras_Start;
    nr_noduri++;
    vizitat[oras_Start] = 1;
    cost[oras_Start] = 0;
    int gasit = 0;
    int nod;//nodul curent pe care il initializam
    while (gasit == 0 && (nr_noduri != 0)) {

        nod = noduri[0];//scoatem primul element din lista de noduri si il retinem in noduri
        for (int i = 0; i < nr_noduri - 1; i++) {
            noduri[i] = noduri[i + 1];
        }
        nr_noduri--;

        if (nod == oras_Destinatie) {
            gasit = 1;
        }
        else {
            for (int i = 0; i < 20; i++) {
                if (a[nod][i] != 0 && ((vizitat[i] == 0) || ((vizitat[i] == 1) && (cost[i]>cost[nod] + a[nod][i])))) {//orasele conectate de nod && nevizitate
                                                                                                                      // costul vechi e mai mare decat noul cost
                                                                                                                      //  orasele conectate de nod SI (fie sunt nevizitate , fie au fost vizitate) dar la o adancime mai mare decat adancimea PE care ar primi-o acum
                                                                                                                      //e prea mare programul sa-ti dai seama cv =.="                                                                                             /
                    cost[i] = cost[nod] + a[nod][i];
                    int j = 0;
                    while (cost[i] > cost[noduri[j]] && j < nr_noduri) {
                        j++;// j va fi pozitia PE care inserez in lista de noduri
                    }
                    for (int i = nr_noduri; i > j; i--) { // Adaug acel i astfel incat lista sa fie ordonata crescator dupa cost
                        noduri[i] = noduri[i - 1]; // noduri[i-1] si asta in loc de a mai pune i-- la for
                    }
                    noduri[j] = i; // aceste 3 linii se modifica la cautarea in latime
                    nr_noduri++;
                    vizitat[i] = 1;
                    parinte[i] = nod;
                    adancime[i] = adancime[nod] + 1;
                }
            }
        }
    }