// Computes all variables that form a connected component in the
// adjacenceMatrix.
// The connected component chosen is arbitrary - it takes the first it finds.
bool BruteForceOptMatching::getConnectedComponent(
    const McDMatrix<int>& adjacenceMatrix,
    McDMatrix<int>& adjacenceMatrixWithoutConnctedComponent,
    McDArray<int>& connComp) {
    adjacenceMatrixWithoutConnctedComponent.resize(adjacenceMatrix.nRows(),
                                                   adjacenceMatrix.nCols());
    memcpy(adjacenceMatrixWithoutConnctedComponent.dataPtr(),
           adjacenceMatrix.dataPtr(),
           sizeof(int) * adjacenceMatrix.nRows() * adjacenceMatrix.nCols());

    // find first a startpoint
    int start = -1;
    connComp.resize(0);
    for (int i = 0; i < adjacenceMatrix.nRows(); i++) {
        for (int j = i; j < adjacenceMatrix.nCols(); j++) {
            if (adjacenceMatrix[i][j] == 1) {
                start = i;
                break;
            }
        }
    }
    if (start == -1)
        return false;

    McDArray<int> queue;
    queue.append(start);
    connComp.clear();
    while (queue.size() > 0) {
        int cur = queue.last();
        connComp.append(cur);
        queue.pop_back();
        for (int i = 0; i < adjacenceMatrixWithoutConnctedComponent.nCols();
             i++) {
            if (adjacenceMatrixWithoutConnctedComponent[cur][i] == 1) {
                queue.push(i);
                adjacenceMatrixWithoutConnctedComponent[cur][i] = 0;
            }
        }
    }
    // remove duplicates
    connComp.sort(&mcStandardCompare);
    int cur = connComp.last();
    for (int i = connComp.size() - 2; i >= 0; i--) {
        if (cur == connComp[i])
            connComp.remove(i, 1);
        else
            cur = connComp[i];
    }
    return true;
}
float BruteForceOptMatching::getMedianZ(const McDArray<McVec3f>& vertices) {

    if (!vertices.size())
        return -1 * FLT_MAX;
    McDArray<float> zs;
    float mean = 0.0;

    for (int i = 0; i < vertices.size(); i++) {
        zs.append(vertices[i].z);
        mean += vertices[i].z;
    }
    zs.sort(&mcStandardCompare);
    int medianIdx = zs.size() / 2.0;
    // cout <<"MeanZ: "<<mean/vertices.size();
    // return mean/vertices.size();
    return zs[medianIdx];
}