C++ - zadatak MINPATH - BFS

#include <iostream>
#include <cstdlib>
#include <queue>
#include <set>

using namespace std;

// dimenzije ploce
int r,s;

// igraca ploca
int a[30][30];

// uzorak kretanja
int pattern[4][2] = {{-1,0},{0,-1},{0,1},{1,0}};

struct pozicija {

    int x,y;

    pozicija(int _x = 0, int _y = 0) {
        x = _x;
        y = _y;
    }

    // da li je pozicija ispravna (unutar matrice, 0 ili iduci zeton) ?
    bool ispravna(int vrijednost = 0) {
        return (    x >= 0 && x < r
                 && y >= 0 && y < s
                 && (a[x][y] == 0 || a[x][y] == vrijednost)
               );
    }

    bool jednaka(pozicija p){ return (p.x == x) && (p.y == y); }

    pair<int,int> uPar() { return make_pair(x,y); }

};

struct zeton {
    pozicija pos;
    int val;

    zeton (int _x = 0, int _y = 0, int _val = 0) {
        pos = pozicija( _x , _y );
        val = _val;
    }
};

struct cvor {
    pozicija pos;
    int korak;

    cvor(pozicija _pos, int _korak = 0) {
        pos = _pos;
        korak = _korak;
    }
};

////////////////////////////

bool usporedi_zetone(zeton a, zeton b) {
    return a.val < b.val;
}

int bfs(pozicija pocetak, pozicija kraj, int vrijednost){

    // cvorovi s pozicijama i udaljenostima, koje posjecujemo
    queue< cvor > q;

    // vidjene pozicije
    set < pair<int,int> > vidjeno;

    // postavi pocetnu poziciju
    q.push(cvor(pocetak,0));
    vidjeno.insert(pocetak.uPar());

    do {
        cvor trenutni = q.front(); q.pop();

        if (trenutni.pos.jednaka(kraj)) {
            return trenutni.korak;
        }

        for (int i = 0 ; i < 4; i++) {
            pozicija iduca = pozicija(
                  trenutni.pos.x + pattern[i][0]
                , trenutni.pos.y + pattern[i][1]
            );

            if (
                iduca.ispravna(vrijednost)
                && ! vidjeno.count( iduca.uPar() )
            ) {
                vidjeno.insert( iduca.uPar() );
                cvor iduci = cvor( iduca, trenutni.korak + 1 );
                q.push( iduci  );
            }
        }

    } while ( !q.empty() );

    return 0;

}

////////////////////////////


int main() {



    vector < zeton > v;

    cin >> r >> s;

    // unesi ploce i zabiljezi zetone
    for (int i = 0 ; i < r; i++ ) {
        for (int j = 0 ; j < s; j++) {
            cin >> a[i][j];
            if (a[i][j] > 0) {
                v.push_back(zeton(i,j,a[i][j]));
            }
        }
    }

    // sortiraj zetone
    sort(v.begin(),v.end(),usporedi_zetone);

    // ukupna duljina
    int ukupna = 0;

    for (int i = 0; i < v.size()-1; i++) {
        int duljina = bfs( v[i].pos , v[i+1].pos, v[i+1].val );
        if (duljina > 0) {
            // makni zeton
            a[v[i].pos.x][v[i].pos.y] = 0;

            // sumiraj duljinu
            ukupna += duljina;
        } else {
            ukupna = 0;
            break;
        }
    }

    cout << ukupna << endl;

    system("pause");
    return 0;
}