#include "loop_detector.h"
#include <utility>

const float PI = 3.1415927;

LoopDetector::
LoopDetector(const vector<vector<glm::vec3>> &s_evaluation_, const vector<vector<glm::vec3>> &f_evaluation_,
             const vector<vector<glm::vec3>> &s_tangent_u_, const vector<vector<glm::vec3>> &f_tangent_u_,
             const vector<vector<glm::vec3>> &s_tangent_v_, const vector<vector<glm::vec3>> &f_tangent_v_,
             const vector<vector<glm::vec3>> &s_normal_, const vector<vector<glm::vec3>> &f_normal_,
             const tinynurbs::RationalSurface<float> &s_)
        : s_evaluation(s_evaluation_), f_evaluation(f_evaluation_),
          s_tangent_u(s_tangent_u_), s_tangent_v(s_tangent_v_),
          f_tangent_u(f_tangent_u_), f_tangent_v(f_tangent_v_),
          s_normal(s_normal_), f_normal(f_normal_), s(s_) {}

// old version
void LoopDetector::initOrientedDistance() {

    selectedPointsIdx = vector<vector<pair<int, int>>>(s_subPatchEdgeSampleCnt_u,
                                                       vector<pair<int, int>>(s_subPatchEdgeSampleCnt_v));
//    distance = vector<vector<glm::vec3>>(s_subPatchEdgeSampleCnt_u, vector<glm::vec3>(s_subPatchEdgeSampleCnt_v));

    for (int i = 0; i < s_subPatchEdgeSampleCnt_u; i++) {
        for (int j = 0; j < s_subPatchEdgeSampleCnt_v; j++) {
            float minDis = FLT_MAX;
            int minDisFIdx_u = -1, minDisFIdx_v = -1;
            for (int k = 0; k < f_subPatchEdgeSampleCnt_u; k++) {
                for (int l = 0; l < f_subPatchEdgeSampleCnt_v; l++) {
                    auto dis = glm::distance(
                            s_evaluation[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j],
                            f_evaluation[f_subPatchIdxRange_u.first + k][f_subPatchIdxRange_v.first + l]);
                    // 确定f上的对应点时，依据的最小欧氏距离，而不是有向距离
                    if (dis < minDis) {
                        minDis = dis;
                        minDisFIdx_u = k;
                        minDisFIdx_v = l;
                    }
                }
            }
//            auto N = glm::normalize(s_evaluation[i][j] - f_evaluation[minDisFIdx_u][minDisFIdx_v]);
//            // 直接normalize得到的单位向量总是与SF同向，但实际上N应当总是与f的一侧法向量成锐角
//            auto testAngle = N * glm::cross(f_tangent_u[minDisFIdx_u][minDisFIdx_v], f_tangent_v[minDisFIdx_u][minDisFIdx_v]);
//            N = testAngle.x + testAngle.y + testAngle.z > 0 ? N : -N;
//            distance[i][j] = N * (s_evaluation[i][j] - f_evaluation[minDisFIdx_u][minDisFIdx_v]);
            selectedPointsIdx[i][j] = {minDisFIdx_u, minDisFIdx_v};
        }
    }
}

// old version
void LoopDetector::initOrientedDisAndVecFields(const vector<pair<pair<int, int>, pair<int, int>>> &intersectBoxPairs) {
    selectedPointsIdx = vector<vector<pair<int, int>>>(s_subPatchEdgeSampleCnt_u,
                                                       vector<pair<int, int>>(s_subPatchEdgeSampleCnt_v, {-1, -1}));
    vectorFields = vector<vector<glm::vec2>>(s_subPatchEdgeSampleCnt_u, vector<glm::vec2>(s_subPatchEdgeSampleCnt_v));
    int neighborIdxes[4][2] = {{1,  0},
                               {0,  1},
                               {-1, 0},
                               {0,  -1}};
    for (const auto &boxPair: intersectBoxPairs) {
        float minDis = FLT_MAX;
        int minDisFIdx_u = -1, minDisFIdx_v = -1;
        for (int i = 0; i < 2; i++) {
            for (int j = 0; j < 2; j++) {
                for (auto neighborIdx: neighborIdxes) {
                    int idxI = boxPair.first.first + i + neighborIdx[0];
                    int idxJ = boxPair.first.second + j + neighborIdx[1];
                    if (idxI - s_subPatchIdxRange_u.first < 0 ||
                        idxI - s_subPatchIdxRange_u.first >= s_subPatchEdgeSampleCnt_u ||
                        idxJ - s_subPatchIdxRange_v.first < 0 ||
                        idxJ - s_subPatchIdxRange_v.first >= s_subPatchEdgeSampleCnt_v ||
                        selectedPointsIdx[idxI - s_subPatchIdxRange_u.first][idxJ - s_subPatchIdxRange_v.first] !=
                        pair<int, int>(-1, -1)) {
                        // 不重复计算
                        continue;
                    }
                    for (int k = 0; k < f_subPatchEdgeSampleCnt_u; k++) {
                        for (int l = 0; l < f_subPatchEdgeSampleCnt_v; l++) {
                            auto dis = glm::distance(
                                    s_evaluation[idxI][idxJ],
                                    f_evaluation[f_subPatchIdxRange_u.first + k][f_subPatchIdxRange_v.first + l]);
                            // 确定f上的对应点时，依据的最小欧氏距离，而不是有向距离
                            if (dis < minDis) {
                                minDis = dis;
                                minDisFIdx_u = k;
                                minDisFIdx_v = l;
                            }
                        }
                    }

                    selectedPointsIdx[idxI - s_subPatchIdxRange_u.first][idxJ - s_subPatchIdxRange_v.first]
                            = {minDisFIdx_u, minDisFIdx_v};
                    // 计算vector fields
                    auto fPtRelatedIdx = pair<int, int>(minDisFIdx_u, minDisFIdx_v);
                    auto n2 = f_normal[f_subPatchIdxRange_u.first + fPtRelatedIdx.first][f_subPatchIdxRange_v.first +
                                                                                         fPtRelatedIdx.second];
                    auto ps_pu = s_tangent_u[idxI][idxJ];
                    auto ps_pv = s_tangent_v[idxI][idxJ];
                    vectorFields[idxI - s_subPatchIdxRange_u.first][idxJ - s_subPatchIdxRange_v.first]
                            = glm::vec2(glm::dot(n2, ps_pu), glm::dot(n2, ps_pv));
                }
            }
        }
    }
}

void LoopDetector::initOrientedDisAndVecFields(const map<pair<int, int>, set<pair<int, int>>> &intersectBoxPairs) {

    selectedPointsIdx = vector<vector<pair<int, int>>>(s_subPatchEdgeSampleCnt_u,
                                                       vector<pair<int, int>>(s_subPatchEdgeSampleCnt_v, {-1, -1}));
    vectorFields = vector<vector<glm::vec2>>(s_subPatchEdgeSampleCnt_u, vector<glm::vec2>(s_subPatchEdgeSampleCnt_v));
    int neighborIdxes[4][2] = {{1,  0},
                               {0,  1},
                               {-1, 0},
                               {0,  -1}};
    int dirs[2][2][2] = {{{-1, 1},  {1, 1}},
                         {{-1, -1}, {1, -1}}}; // 必须是左上，右上，左下，右下的顺序
    for (const auto &boxPair: intersectBoxPairs) {
        for (int i = 0; i < 2; i++) {
            for (int j = 0; j < 2; j++) {
                for (auto neighborIdx: neighborIdxes) {
                    float minDis = FLT_MAX;
                    int minDisFIdx_u = -1, minDisFIdx_v = -1;
                    int idxI = boxPair.first.first + i + neighborIdx[0];
                    int idxJ = boxPair.first.second + j + neighborIdx[1];
                    if (idxI - s_subPatchIdxRange_u.first < 0 ||
                        idxI - s_subPatchIdxRange_u.first >= s_subPatchEdgeSampleCnt_u ||
                        idxJ - s_subPatchIdxRange_v.first < 0 ||
                        idxJ - s_subPatchIdxRange_v.first >= s_subPatchEdgeSampleCnt_v ||
                        selectedPointsIdx[idxI - s_subPatchIdxRange_u.first][idxJ - s_subPatchIdxRange_v.first] !=
                        pair<int, int>(-1, -1)) {
                        // 不重复计算
                        continue;
                    }
                    for (auto relatedBox: boxPair.second) {
                        for (int k = 0; k < 2; k++) {
                            for (int l = 0; l < 2; l++) {
                                for (int step_u = 0; step_u <= 2; step_u++) {
                                    for (int step_v = 0; step_v + step_u <= 2; step_v++) {

                                        int fIdx_u = relatedBox.first + k + step_u * dirs[k][l][0];
                                        int fIdx_v = relatedBox.second + l + step_v * dirs[k][l][1];
                                        if (fIdx_u < 0 || fIdx_u >= f_evaluation.size() ||
                                            fIdx_v < 0 || fIdx_v >= f_evaluation[0].size())
                                            continue;
                                        auto dis = glm::distance(s_evaluation[idxI][idxJ],
                                                                 f_evaluation[fIdx_u][fIdx_v]);
                                        if (dis < minDis) {
                                            minDis = dis;
                                            minDisFIdx_u = fIdx_u;
                                            minDisFIdx_v = fIdx_v;
                                        }
                                    }
                                }
                            }
                        }
                    }
                    selectedPointsIdx[idxI - s_subPatchIdxRange_u.first][idxJ - s_subPatchIdxRange_v.first]
                            = {minDisFIdx_u - f_subPatchIdxRange_u.first, minDisFIdx_v - f_subPatchIdxRange_v.first};
                    auto n2 = f_normal[minDisFIdx_u][minDisFIdx_v];
                    auto ps_pu = s_tangent_u[idxI][idxJ];
                    auto ps_pv = s_tangent_v[idxI][idxJ];
                    vectorFields[idxI - s_subPatchIdxRange_u.first][idxJ - s_subPatchIdxRange_v.first]
                            = glm::vec2(glm::dot(n2, ps_pu), glm::dot(n2, ps_pv));
                }
            }
        }
    }
}

// Yawei Ma的文章中的做法
// old version
void LoopDetector::initVectorField0() {
    vectorFields = vector<vector<glm::vec2>>(s_subPatchEdgeSampleCnt_u, vector<glm::vec2>(s_subPatchEdgeSampleCnt_v));
    for (int i = 0; i < s_subPatchEdgeSampleCnt_u; i++) {
        for (int j = 0; j < s_subPatchEdgeSampleCnt_v; j++) {
            auto fPointIdx = selectedPointsIdx[i][j];
            auto N = glm::normalize(s_evaluation[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j] -
                                    f_evaluation[f_subPatchIdxRange_u.first + fPointIdx.first]
                                    [f_subPatchIdxRange_v.first + fPointIdx.second]);
            if (isnan(N.x) || isnan(N.y) || isnan(N.z)) {
                // 绝了，曲面s和f的交线正好经过采样点，此时就无法定义向量N
                // 这里取两个曲面在交点的法向量的平均做近似
//                auto tmp1 = glm::cross(s_tangent_u[i][j], s_tangent_v[i][j]);
//                auto tmp2 = glm::cross(f_tangent_u[fPointIdx.first][fPointIdx.second],
//                                       f_tangent_v[fPointIdx.first][fPointIdx.second]);
                N = glm::normalize(s_normal[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j] +
                                   f_normal[f_subPatchIdxRange_u.first + fPointIdx.first]
                                   [f_subPatchIdxRange_v.first + fPointIdx.second]);
            }
            // u1、u2两个向量构成和N垂直的曲面
            glm::vec3 u1(1, 1, 1);
            if (N.z != 0)u1.z = (-N.x - N.y) / N.z;
            else if (N.y != 0)u1.y = (-N.x - N.z) / N.y;
            else u1.x = (-N.y - N.z) / N.x;
            u1 = glm::normalize(u1);
            auto u2 = glm::normalize(glm::cross(N, u1));
            // s，f曲面在两个方向上的偏导
            auto ps_pu = s_tangent_u[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j]
            , ps_pv = s_tangent_v[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j];
            auto pf_pp = f_tangent_u[f_subPatchIdxRange_u.first + fPointIdx.first]
            [f_subPatchIdxRange_v.first + fPointIdx.second], pf_pq = f_tangent_v[f_subPatchIdxRange_u.first +
                                                                                 fPointIdx.first]
            [f_subPatchIdxRange_v.first + fPointIdx.second];
            // 构造Aij矩阵，见<Detection of loops and singularities of surface intersections> APPENDIX (A7)
            auto inverseAij = glm::inverse(
                    glm::mat2x2(glm::dot(u1, pf_pp), glm::dot(u1, pf_pq), glm::dot(u2, pf_pp), glm::dot(u2, pf_pq))
            );
            auto mBmn = glm::mat2x2(glm::dot(u1, ps_pu), glm::dot(u1, ps_pv), glm::dot(u2, ps_pu), glm::dot(u2, ps_pv));
            auto tmp = glm::vec2(glm::dot(N, pf_pp), glm::dot(N, pf_pq));
            auto vNfpNfq_inverseAij = glm::vec2(glm::dot(N, pf_pp), glm::dot(N, pf_pq)) * inverseAij;
            auto pd_pu = glm::dot(N, ps_pu) -
                         glm::dot(vNfpNfq_inverseAij, glm::vec2(glm::dot(u1, ps_pu), glm::dot(u2, ps_pu)));
            auto pd_pv = glm::dot(N, ps_pv) -
                         glm::dot(vNfpNfq_inverseAij, glm::vec2(glm::dot(u1, ps_pv), glm::dot(u2, ps_pv)));
            vectorFields[i][j] = glm::vec2(pd_pu, pd_pv);
        }
    }
    int a = 1;
    int b = 2;
}

// G A Kriezis的文章中的做法
// old version
void LoopDetector::initVectorField() {
    vectorFields = vector<vector<glm::vec2>>(s_subPatchEdgeSampleCnt_u, vector<glm::vec2>(s_subPatchEdgeSampleCnt_v));
    for (int i = 0; i < s_subPatchEdgeSampleCnt_u; i++) {
        for (int j = 0; j < s_subPatchEdgeSampleCnt_v; j++) {
            auto fPtRelatedIdx = selectedPointsIdx[i][j];
            auto n2 = f_normal[f_subPatchIdxRange_u.first + fPtRelatedIdx.first][f_subPatchIdxRange_v.first +
                                                                                 fPtRelatedIdx.second];
//            glm::vec3 n2(1.,0.,0.);
            auto ps_pu = s_tangent_u[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j];
            auto ps_pv = s_tangent_v[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j];
            vectorFields[i][j] = glm::vec2(glm::dot(n2, ps_pu), glm::dot(n2, ps_pv));
        }
    }
}

// old version
void LoopDetector::getRotationNumber() {
    // vectorFields的参数域是与s（第一个曲面）一致的
    auto edgeSampleCnt = s_evaluation.size();
    auto uParamCellSize = (*(s.knots_u.end() - 1) - *s.knots_u.begin()) / (float) edgeSampleCnt;
    auto vParamCellSize = (*(s.knots_v.end() - 1) - *s.knots_v.begin()) / (float) edgeSampleCnt;
//    vector<vector<float>> pu_pt = {{0.,  1.},
//                                   {-1., 0.}};
//    vector<vector<float>> pv_pt = {{1., 0.},
//                                   {0., -1.}};
    rotationNumbers = vector<vector<int>>(s_subPatchEdgeSampleCnt_u,
                                          vector<int>(s_subPatchEdgeSampleCnt_v));

    // 以小格子为单位遍历
    for (int i = 0; i < s_subPatchEdgeSampleCnt_u - 1; i++) {
        for (int j = 0; j < s_subPatchEdgeSampleCnt_v - 1; j++) {
            float F[2][2], G[2][2];
            for (int biasI = 0; biasI < 2; biasI++) {
                for (int biasJ = 0; biasJ < 2; biasJ++) {
                    auto idxI = i + biasI;
                    auto idxJ = j + biasJ;
                    auto v = vectorFields[idxI][idxJ];
                    auto vSquareSum = v.x * v.x + v.y * v.y;
                    auto pTheta_pChi = -v.y / vSquareSum;
                    auto pTheta_pPhi = v.x / vSquareSum;
//                    auto pChi_pu =
//                            (vectorFields[idxI + 1][idxJ].x - vectorFields[idxI - 1][idxJ].x) / (2 * uParamCellSize);
//                    auto pChi_pv =
//                            (vectorFields[idxI][idxJ + 1].x - vectorFields[idxI][idxJ - 1].x) / (2 * vParamCellSize);
                    glm::vec2 pV_pu;
                    glm::vec2 pV_pv;
                    if (idxI == 0)
                        pV_pu = (vectorFields[idxI + 1][idxJ] - vectorFields[idxI][idxJ]) / uParamCellSize;
                    else if (idxI == s_subPatchEdgeSampleCnt_u - 1)
                        pV_pu = (vectorFields[idxI][idxJ] - vectorFields[idxI - 1][idxJ]) / uParamCellSize;
                    else
                        pV_pu = (vectorFields[idxI + 1][idxJ] - vectorFields[idxI - 1][idxJ]) / (2 * uParamCellSize);
                    if (idxJ == 0)
                        pV_pv = (vectorFields[idxI][idxJ + 1] - vectorFields[idxI][idxJ]) / vParamCellSize;
                    else if (idxJ == s_subPatchEdgeSampleCnt_v - 1)
                        pV_pv = (vectorFields[idxI][idxJ] - vectorFields[idxI][idxJ - 1]) / vParamCellSize;
                    else
                        pV_pv = (vectorFields[idxI][idxJ + 1] - vectorFields[idxI][idxJ - 1]) / (2 * vParamCellSize);
                    F[biasI][biasJ] = pTheta_pChi * pV_pu.x + pTheta_pPhi * pV_pu.y;
                    G[biasI][biasJ] = pTheta_pChi * pV_pv.x + pTheta_pPhi * pV_pv.y;
                }
            }
            rotationNumbers[i][j] = int(
                    round(((F[0][1] + F[1][1] - F[0][0] - F[1][0]) * uParamCellSize +
                           (G[0][0] + G[0][1] - G[1][0] - G[1][1]) * vParamCellSize)) / 2.);
        }
    }
//    int a = 3;
//    int b = 1;
}

// old version
void LoopDetector::getRotationNumber(const vector<pair<pair<int, int>, pair<int, int>>> &intersectBoxPairs) {
    // vectorFields的参数域是与s（第一个曲面）一致的
    auto edgeSampleCnt = s_evaluation.size();
    auto uParamCellSize = (*(s.knots_u.end() - 1) - *s.knots_u.begin()) / (float) edgeSampleCnt;
    auto vParamCellSize = (*(s.knots_v.end() - 1) - *s.knots_v.begin()) / (float) edgeSampleCnt;
    rotationNumbers = vector<vector<int>>(s_subPatchEdgeSampleCnt_u,
                                          vector<int>(s_subPatchEdgeSampleCnt_v));
    // 以小格子为单位遍历
    for (const auto &boxPair: intersectBoxPairs) {
        float F[2][2], G[2][2];
        for (int biasI = 0; biasI < 2; biasI++) {
            for (int biasJ = 0; biasJ < 2; biasJ++) {
                auto idxI = boxPair.first.first + biasI - s_subPatchIdxRange_u.first;
                auto idxJ = boxPair.first.second + biasJ - s_subPatchIdxRange_v.first;
                auto v = vectorFields[idxI][idxJ];
                auto vSquareSum = v.x * v.x + v.y * v.y;
                auto pTheta_pChi = -v.y / vSquareSum;
                auto pTheta_pPhi = v.x / vSquareSum;
//                    auto pChi_pu =
//                            (vectorFields[idxI + 1][idxJ].x - vectorFields[idxI - 1][idxJ].x) / (2 * uParamCellSize);
//                    auto pChi_pv =
//                            (vectorFields[idxI][idxJ + 1].x - vectorFields[idxI][idxJ - 1].x) / (2 * vParamCellSize);
                glm::vec2 pV_pu;
                glm::vec2 pV_pv;
                if (idxI == 0)
                    pV_pu = (vectorFields[idxI + 1][idxJ] - vectorFields[idxI][idxJ]) / uParamCellSize;
                else if (idxI == s_subPatchEdgeSampleCnt_u - 1)
                    pV_pu = (vectorFields[idxI][idxJ] - vectorFields[idxI - 1][idxJ]) / uParamCellSize;
                else
                    pV_pu = (vectorFields[idxI + 1][idxJ] - vectorFields[idxI - 1][idxJ]) / (2 * uParamCellSize);
                if (idxJ == 0)
                    pV_pv = (vectorFields[idxI][idxJ + 1] - vectorFields[idxI][idxJ]) / vParamCellSize;
                else if (idxJ == s_subPatchEdgeSampleCnt_v - 1)
                    pV_pv = (vectorFields[idxI][idxJ] - vectorFields[idxI][idxJ - 1]) / vParamCellSize;
                else
                    pV_pv = (vectorFields[idxI][idxJ + 1] - vectorFields[idxI][idxJ - 1]) / (2 * vParamCellSize);
                F[biasI][biasJ] = pTheta_pChi * pV_pu.x + pTheta_pPhi * pV_pu.y;
                G[biasI][biasJ] = pTheta_pChi * pV_pv.x + pTheta_pPhi * pV_pv.y;
            }
        }
        rotationNumbers[boxPair.first.first - s_subPatchIdxRange_u.first][boxPair.first.second -
                                                                          s_subPatchIdxRange_v.first] = int(
                round(((F[0][1] + F[1][1] - F[0][0] - F[1][0]) * uParamCellSize +
                       (G[0][0] + G[0][1] - G[1][0] - G[1][1]) * vParamCellSize)) / 2.);
    }
}

void LoopDetector::getRotationNumber(const map<pair<int, int>, set<pair<int, int>>> &intersectBoxPairs) {
    // vectorFields的参数域是与s（第一个曲面）一致的
    auto edgeSampleCnt = s_evaluation.size();
    auto uParamCellSize = (*(s.knots_u.end() - 1) - *s.knots_u.begin()) / (float) edgeSampleCnt;
    auto vParamCellSize = (*(s.knots_v.end() - 1) - *s.knots_v.begin()) / (float) edgeSampleCnt;
    rotationNumbers = vector<vector<int>>(s_subPatchEdgeSampleCnt_u,
                                          vector<int>(s_subPatchEdgeSampleCnt_v));
    targetCells = {};
    // 以小格子为单位遍历
    for (const auto &boxPair: intersectBoxPairs) {
        if (boxPair.first.first == 127 && boxPair.first.second == 0) {
            int haha = 1;
        }
        float F[2][2], G[2][2];
        for (int biasI = 0; biasI < 2; biasI++) {
            for (int biasJ = 0; biasJ < 2; biasJ++) {
                auto idxI = boxPair.first.first + biasI - s_subPatchIdxRange_u.first;
                auto idxJ = boxPair.first.second + biasJ - s_subPatchIdxRange_v.first;
                auto v = vectorFields[idxI][idxJ];
                auto vSquareSum = v.x * v.x + v.y * v.y;
                auto pTheta_pChi = -v.y / vSquareSum;
                auto pTheta_pPhi = v.x / vSquareSum;
//                    auto pChi_pu =
//                            (vectorFields[idxI + 1][idxJ].x - vectorFields[idxI - 1][idxJ].x) / (2 * uParamCellSize);
//                    auto pChi_pv =
//                            (vectorFields[idxI][idxJ + 1].x - vectorFields[idxI][idxJ - 1].x) / (2 * vParamCellSize);
                glm::vec2 pV_pu;
                glm::vec2 pV_pv;
                if (idxI == 0)
                    pV_pu = (vectorFields[idxI + 1][idxJ] - vectorFields[idxI][idxJ]) / uParamCellSize;
                else if (idxI == s_subPatchEdgeSampleCnt_u - 1)
                    pV_pu = (vectorFields[idxI][idxJ] - vectorFields[idxI - 1][idxJ]) / uParamCellSize;
                else
                    pV_pu = (vectorFields[idxI + 1][idxJ] - vectorFields[idxI - 1][idxJ]) / (2 * uParamCellSize);
                if (idxJ == 0)
                    pV_pv = (vectorFields[idxI][idxJ + 1] - vectorFields[idxI][idxJ]) / vParamCellSize;
                else if (idxJ == s_subPatchEdgeSampleCnt_v - 1)
                    pV_pv = (vectorFields[idxI][idxJ] - vectorFields[idxI][idxJ - 1]) / vParamCellSize;
                else
                    pV_pv = (vectorFields[idxI][idxJ + 1] - vectorFields[idxI][idxJ - 1]) / (2 * vParamCellSize);
                F[biasI][biasJ] = pTheta_pChi * pV_pu.x + pTheta_pPhi * pV_pu.y;
                G[biasI][biasJ] = pTheta_pChi * pV_pv.x + pTheta_pPhi * pV_pv.y;
            }
        }

        int rotationNumber = int(round(((F[0][1] + F[1][1] - F[0][0] - F[1][0]) * uParamCellSize +
                                        (G[0][0] + G[0][1] - G[1][0] - G[1][1]) * vParamCellSize)) / 2.);
        rotationNumbers[boxPair.first.first - s_subPatchIdxRange_u.first][boxPair.first.second -
                                                                          s_subPatchIdxRange_v.first] = rotationNumber;
        if (rotationNumber != 0) {
            targetCells.insert({{boxPair.first.first, boxPair.first.second}, rotationNumber});
        }

    }
}

// old version
vector<pair<int, int>> LoopDetector::detect(pair<int, int> _s_subPatchIdxRange_u, pair<int, int> _s_subPatchIdxRange_v,
                                            pair<int, int> _f_subPatchIdxRange_u,
                                            pair<int, int> _f_subPatchIdxRange_v) {
    s_subPatchIdxRange_u = _s_subPatchIdxRange_u;
    s_subPatchIdxRange_v = _s_subPatchIdxRange_v;
    f_subPatchIdxRange_u = _f_subPatchIdxRange_u;
    f_subPatchIdxRange_v = _f_subPatchIdxRange_v;

    // subPatch每条边上的采样点个数。边上的格子个数=range.second-range.first+1，采样点个数=格子个数+1
    s_subPatchEdgeSampleCnt_u = s_subPatchIdxRange_u.second - s_subPatchIdxRange_u.first + 2;
    s_subPatchEdgeSampleCnt_v = s_subPatchIdxRange_v.second - s_subPatchIdxRange_v.first + 2;
    f_subPatchEdgeSampleCnt_u = f_subPatchIdxRange_u.second - f_subPatchIdxRange_u.first + 2;
    f_subPatchEdgeSampleCnt_v = f_subPatchIdxRange_v.second - f_subPatchIdxRange_v.first + 2;

    initOrientedDistance();
    initVectorField();
    getRotationNumber();
    return {};
}

vector<pair<int, int>> LoopDetector::detect(const map<pair<int, int>, set<pair<int, int>>> &intersectBoxPairs,
                                            pair<int, int> _s_subPatchIdxRange_u, pair<int, int> _s_subPatchIdxRange_v,
                                            pair<int, int> _f_subPatchIdxRange_u,
                                            pair<int, int> _f_subPatchIdxRange_v) {
    s_subPatchIdxRange_u = _s_subPatchIdxRange_u;
    s_subPatchIdxRange_v = _s_subPatchIdxRange_v;
    f_subPatchIdxRange_u = _f_subPatchIdxRange_u;
    f_subPatchIdxRange_v = _f_subPatchIdxRange_v;

    // subPatch每条边上的采样点个数。边上的格子个数=range.second-range.first+1，采样点个数=格子个数+1
    s_subPatchEdgeSampleCnt_u = s_subPatchIdxRange_u.second - s_subPatchIdxRange_u.first + 2;
    s_subPatchEdgeSampleCnt_v = s_subPatchIdxRange_v.second - s_subPatchIdxRange_v.first + 2;
    f_subPatchEdgeSampleCnt_u = f_subPatchIdxRange_u.second - f_subPatchIdxRange_u.first + 2;
    f_subPatchEdgeSampleCnt_v = f_subPatchIdxRange_v.second - f_subPatchIdxRange_v.first + 2;

    initOrientedDisAndVecFields(intersectBoxPairs);
    getRotationNumber(intersectBoxPairs);
    return {};
}