size_t propagateIncrements(linearpart<float>& flowDir, SparsePartition<short>& inc, std::vector<node>& queue) {
    size_t numInc = 0;
    int st = 1;
    
    std::vector<node> newFlats;
    while (!queue.empty()) {
        for(node flat : queue) {
            // Duplicate. already set
            if (inc.getData(flat.x, flat.y) > 0)
                continue;

            for (int k = 1; k <= 8; k++) {
                if (dontCross(k, flat.x, flat.y, flowDir) == 0) {
                    int in = flat.x + d1[k];
                    int jn = flat.y + d2[k];

                    if (!flowDir.isInPartition(in, jn)) 
                        continue;

                    float flow = flowDir.getData(in, jn);

                    if (flow == -1 && inc.getData(in, jn) == 0) {
                        newFlats.push_back(node(in, jn));
                        inc.setData(in, jn, -1);
                    }
                }
            }

            numInc++;
            inc.setData(flat.x, flat.y, st);
        }

        queue.clear();
        queue.swap(newFlats);
        st++;
    }

    return numInc;
}
void flowTowardsLower(T& elev, linearpart<float>& flowDir, std::vector<std::vector<node>>&islands, SparsePartition<short>& inc) 
{
    long nx = flowDir.getnx();
    long ny = flowDir.getny();

    std::vector<node> lowBoundaries;

    // Find low boundaries. 
    for(auto& island : islands) {
        for(node flat : island) {
            float flatElev = elev.getData(flat.x, flat.y);

            for (int k = 1; k <= 8; k++) {
                if (dontCross(k, flat.x, flat.y, flowDir) == 0) {
                    int in = flat.x + d1[k];
                    int jn = flat.y + d2[k];

                    if (!flowDir.hasAccess(in, jn))
                        continue;

                    auto elevDiff = flatElev - elev.getData(in,jn);
                    float flow = flowDir.getData(in, jn);

                    bool edgeDrain = flowDir.isNodata(in, jn);

                    // Adjacent cell drains and is equal or lower in elevation so this is a low boundary
                    if ((elevDiff >= 0 && flow >= 0.0) || edgeDrain) {
                        lowBoundaries.push_back(flat);
                        inc.setData(flat.x, flat.y, -1);

                        // No need to check the other neighbors
                        break;
                    } 
                }
            }
        }
    }

    size_t numInc = propagateIncrements(flowDir, inc, lowBoundaries);

    // Not all grid cells were resolved - pits remain
    // Remaining grid cells are unresolvable pits
    if (numInc > 0)          
    {
        markPits(elev, flowDir, islands, inc);
    }
}
void flowFromHigher(T& elev, linearpart<float>& flowDir, std::vector<std::vector<node>>&islands, SparsePartition<short>& inc) 
{
    long nx = flowDir.getnx();
    long ny = flowDir.getny();

    // Find high boundaries
    for (auto& island : islands) {
        std::vector<node> highBoundaries;

        for (node flat : island) {
            float flatElev = elev.getData(flat.x, flat.y);
            bool highBoundary = false;

            for (int k = 1; k <= 8; k++) {
                if (dontCross(k, flat.x, flat.y, flowDir) == 0) {
                    int in = flat.x + d1[k];
                    int jn = flat.y + d2[k];

                    if (!flowDir.hasAccess(in, jn))
                        continue;

                    auto elevDiff = flatElev - elev.getData(in, jn);
                    
                    if (elevDiff < 0) {
                        // Adjacent cell has higher elevation so this is a high boundary
                        highBoundary = true;
                        break;
                    }
                }
            }

            if (highBoundary) {
                inc.setData(flat.x, flat.y, -1);
                highBoundaries.push_back(flat);
            }
        }

        propagateIncrements(flowDir, inc, highBoundaries);
    }
}
size_t propagateBorderIncrements(linearpart<float>& flowDir, SparsePartition<short>& inc) 
{
    int nx = flowDir.getnx();
    int ny = flowDir.getny();

    struct pnode {
        int x;
        int y;
        int inc;

        bool operator<(const struct pnode& b) const {
            return inc < b.inc;
        }
    };
    
    std::vector<pnode> queue;

    // Find the starting nodes at the edge of the raster
    //
    // FIXME: oob access
    for (auto y : {-1, ny}) {
        for(int x = 0; x < nx; x++) {
            int st = inc.getData(x, y);

            if (st == 0)
                continue;

            auto jn = y == -1 ? 0 : ny - 1;

            for (auto in : {x-1, x, x+1}) {
                if (!flowDir.isInPartition(in, jn))
                    continue;

                float flow = flowDir.getData(in, jn);
                auto neighSt = inc.getData(in, jn);

                if (flow == -1 && (neighSt == 0 || neighSt > st + 1 || -neighSt > st + 1)) {
                    queue.push_back({in, jn, st + 1});

                    // Here we set a negative increment if it's still pending
                    //
                    // Another flat might be neighboring the same cell with a lower increment,
                    // which has to override the higher increment (that hasn't been set yet but is in the queue).
                    inc.setData(in, jn, -(st + 1));
                }
            }
        }
    }

    size_t numChanged = 0;

    // Sort queue by lowest increment
    std::sort(queue.begin(), queue.end());
    std::vector<pnode> newFlats;

    while (!queue.empty()) {
        for(pnode flat : queue) {
            // Skip if the increment was already set and it is lower 
            auto st = inc.getData(flat.x, flat.y);
            if (st > 0 && st <= flat.inc) {
                continue;
            }

            for (int k = 1; k <= 8; k++) {
                if (dontCross(k, flat.x, flat.y, flowDir) == 0) {
                    int in = flat.x + d1[k];
                    int jn = flat.y + d2[k];

                    if (!flowDir.isInPartition(in, jn)) 
                        continue;

                    float flow = flowDir.getData(in, jn);
                    auto neighInc = inc.getData(in, jn);

                    if (flow == -1 && (neighInc == 0 || neighInc > flat.inc + 1 || -neighInc > flat.inc + 1)) {
                        newFlats.push_back({in, jn, flat.inc + 1});
                        inc.setData(in, jn, -(flat.inc + 1));
                    }
                }
            }

            inc.setData(flat.x, flat.y, flat.inc);
            numChanged++;
        }

        std::sort(newFlats.begin(), newFlats.end());
        queue.clear();
        queue.swap(newFlats);
    }

    return numChanged;
}