std::vector<std::vector<float>> BlockMatching::getOpticalFlowGrayBM(cv::Mat frame1, cv::Mat frame2, std::vector<cv::Point2f> sparsePoints, float thres)
{
    cv::Size blockSize(16, 16);
    cv::Size searchSize(48, 48);
    cv::Mat result;

    std::vector<std::vector<float>> opticalFlow;
    cv::Mat scaledFrame1, scaledFrame2;
    cv::cvtColor(frame1, scaledFrame1, CV_BGR2GRAY);
    cv::cvtColor(frame2, scaledFrame2, CV_BGR2GRAY);
    int width = frame1.cols;
    int height = frame1.rows;
    for (unsigned i = 0; i < sparsePoints.size(); i++) {
        cv::Point2i upperBlock, lowerBlock;
        upperBlock.x = std::max(0, (int)sparsePoints[i].x - blockSize.width / 2);
        upperBlock.y = std::max(0, (int)sparsePoints[i].y - blockSize.height / 2);

        lowerBlock.x = std::min(width, (int)sparsePoints[i].x + blockSize.width / 2);
        lowerBlock.y = std::min(height, (int)sparsePoints[i].y + blockSize.height / 2);
        cv::Rect blockRect(upperBlock, lowerBlock);

        cv::Point2i upperSearch, lowerSearch;
        upperSearch.x = std::max(0, (int)sparsePoints[i].x - searchSize.width / 2);
        upperSearch.y = std::max(0, (int)sparsePoints[i].y - searchSize.height / 2);

        lowerSearch.x = std::min(width, (int)sparsePoints[i].x + searchSize.width / 2);
        lowerSearch.y = std::min(height, (int)sparsePoints[i].y + searchSize.height / 2);
        cv::Rect searchRect(upperSearch, lowerSearch);

        cv::Mat block, search;
        scaledFrame1(blockRect).copyTo(block);
        scaledFrame2(searchRect).copyTo(search);

        int resultCols = search.cols - block.cols + 1;
        int resultRows = search.rows - block.rows + 1;

        result.create(resultRows, resultCols, CV_32FC1);
        cv::matchTemplate(search, block, result, CV_TM_SQDIFF);
        cv::normalize(result, result, 0, 1, cv::NORM_MINMAX, -1, cv::Mat());

        double minVal; double maxVal; cv::Point minLoc; cv::Point maxLoc;
        cv::Point matchLoc;

        minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat());
        matchLoc = minLoc;

        // std::cout << minVal << "," << minLoc << std::endl;

        if (minVal < 0.01) {
        // if ((minVal > 0) && (minVal < thres)) {
            std::vector<float> opticalFlowPair;
            opticalFlowPair.resize(4);

            int u = matchLoc.x - (resultCols - 1) / 2;
            int v = matchLoc.y - (resultRows - 1) / 2;

            opticalFlowPair[0] = sparsePoints[i].x + u;     // j
            opticalFlowPair[1] = sparsePoints[i].y + v;     // i
            opticalFlowPair[2] = u; // u
            opticalFlowPair[3] = v; // v

            opticalFlow.push_back(opticalFlowPair);
            //std::cout << u << "," << v << " : " << minVal << std::endl;
            // cv::rectangle(frame2, cv::Point(opticalFlowPair[0] - block.cols / 2, opticalFlowPair[1] - block.rows / 2),
            //  cv::Point(opticalFlowPair[0] + block.cols / 2, opticalFlowPair[1] + block.rows / 2), cv::Scalar(0,255,0), 2, 8, 0);
            // cv::rectangle(frame2, cv::Point(sparsePoints[i].x - block.cols / 2, sparsePoints[i].y - block.rows / 2),
            //  cv::Point(sparsePoints[i].x + block.cols / 2, sparsePoints[i].y + block.rows / 2), cv::Scalar(0,255,0), 2, 8, 0);
            // cv::rectangle(frame2, upperBlock, lowerBlock, cv::Scalar(0,0,255), 2, 8, 0);
            // cv::rectangle(frame2, upperSearch, lowerSearch, cv::Scalar(0,0,255), 1, 8, 0);
        }
    }

    cv::imshow("frame2", frame2);
    // cv::waitKey(0);

    return opticalFlow;
}