private void swap(int currentPos, int parent) {
    Location tmp = this.locations[parent];
    this.locations[parent] = this.locations[currentPos];
    this.locations[currentPos] = tmp;
    this.locations[parent].setPositionHeap(parent);
    this.locations[currentPos].setPositionHeap(currentPos);
}

public Location removeMin(){
    if (nbLocations == 0) return null;
    Location res = this.locations[0];
    this.locations[0] = this.locations[nbLocations - 1];
    this.locations[0].setPositionHeap(0);
    nbLocations -= 1;
    int currentPos = 0;
    percolateDown(currentPos);
    return res;
}

private void percolateDown(int currentPos) {
    while (currentPos < nbLocations){
        int left = 2 * currentPos + 1;
        int right = 2 * currentPos + 2;
        if (left >= nbLocations && right >= nbLocations){
            break;
        }
        int minLocation = left;
        if (right < nbLocations && this.locations[right].getDistance() < this.locations[left].getDistance()){
            minLocation = right;
        }
        if (this.locations[currentPos].getDistance() > this.locations[minLocation].getDistance()){
            swap(minLocation, currentPos);
            currentPos = minLocation;
        } else {
            break;
        }
    }
}