void matchBoundingBoxes(std::vector<cv::DMatch> &matches, std::map<int, int> &bbBestMatches, DataFrame &prevFrame, DataFrame &currFrame)
{

    for (const auto& prevBox : prevFrame.boundingBoxes)
    {
        int maxMatchCount = 0;
        int bestMatchBoxId = -1;

        for (const auto& currBox : currFrame.boundingBoxes)
        {
            int matchCount = 0;

            for (const auto& match : matches)
            {
                int prevKeyPointIdx = match.queryIdx;
                int currKeyPointIdx = match.trainIdx;

                if (prevBox.roi.contains(prevFrame.keypoints[prevKeyPointIdx].pt) &&
                    currBox.roi.contains(currFrame.keypoints[currKeyPointIdx].pt))
                {
                    matchCount++;
                }
            }

            if (matchCount > maxMatchCount)
            {
                maxMatchCount = matchCount;
                bestMatchBoxId = currBox.boxID;
            }
        }

        bbBestMatches[prevBox.boxID] = bestMatchBoxId;
    }

}

void computeTTCLidar(std::vector<LidarPoint> &lidarPointsPrev,
                     std::vector<LidarPoint> &lidarPointsCurr, double frameRate, double &TTC)
{
    
    double dT = 1.0 / frameRate;
    double laneWidth = 1.2;     // width of the preceding lidar area
    
    double yEdge = (laneWidth - 0.2) / 2;

    auto checkFunc = [&yEdge](const LidarPoint &lp) { return std::abs(lp.y) < yEdge; };

    // Filter out Lidar points outside the ego lane
    lidarPointsPrev.erase(std::remove_if(lidarPointsPrev.begin(), lidarPointsPrev.end(), checkFunc), lidarPointsPrev.end());
    lidarPointsCurr.erase(std::remove_if(lidarPointsCurr.begin(), lidarPointsCurr.end(), checkFunc), lidarPointsCurr.end());

    // Sort the Lidar points based on x-coordinate
    std::sort(lidarPointsPrev.begin(), lidarPointsPrev.end(), [](const LidarPoint &lp1, const LidarPoint &lp2) { return lp1.x < lp2.x; });
    std::sort(lidarPointsCurr.begin(), lidarPointsCurr.end(), [](const LidarPoint &lp1, const LidarPoint &lp2) { return lp1.x < lp2.x; });

    // Calculate the median x-coordinate for the Lidar points
    double prevXMedian = lidarPointsPrev[lidarPointsPrev.size() / 2].x;
    double currXMedian = lidarPointsCurr[lidarPointsCurr.size() / 2].x;
    
    TTC = currXMedian * dT / (prevXMedian - currXMedian);

}

void clusterKptMatchesWithROI(BoundingBox &boundingBox_c, BoundingBox &boundingBox_p, std::vector<cv::KeyPoint> &kptsPrev, std::vector<cv::KeyPoint> &kptsCurr, std::vector<cv::DMatch> &kptMatches)
{
    float shrinkFactor = 0.12;
    cv::Rect smallerBox_c(
        boundingBox_c.roi.x + shrinkFactor * boundingBox_c.roi.width / 2.0,
        boundingBox_c.roi.y + shrinkFactor * boundingBox_c.roi.height / 2.0,
        boundingBox_c.roi.width * (1 - shrinkFactor),
        boundingBox_c.roi.height * (1 - shrinkFactor)
    );

    cv::Rect smallerBox_p(
        boundingBox_p.roi.x + shrinkFactor * boundingBox_p.roi.width / 2.0,
        boundingBox_p.roi.y + shrinkFactor * boundingBox_p.roi.height / 2.0,
        boundingBox_p.roi.width * (1 - shrinkFactor),
        boundingBox_p.roi.height * (1 - shrinkFactor)
    );

    std::vector<cv::DMatch> kptMatches_roi;

    for (const auto& match : kptMatches)
    {
        cv::KeyPoint train = kptsCurr[match.trainIdx];
        cv::KeyPoint query = kptsPrev[match.queryIdx];

        auto train_pt = cv::Point(train.pt.x, train.pt.y);
        auto query_pt = cv::Point(query.pt.x, query.pt.y);

        if (smallerBox_c.contains(train_pt) && smallerBox_p.contains(query_pt))
            kptMatches_roi.push_back(match);
    }

    double dist_mean = 0.0;
    for (const auto& match : kptMatches_roi)
    {
        cv::KeyPoint train = kptsCurr[match.trainIdx];
        cv::KeyPoint query = kptsPrev[match.queryIdx];

        auto train_pt = cv::Point(train.pt.x, train.pt.y);
        auto query_pt = cv::Point(query.pt.x, query.pt.y);

        dist_mean += cv::norm(train_pt - query_pt);
    }

    dist_mean /= kptMatches_roi.size();

    for (const auto& match : kptMatches_roi)
    {
        cv::KeyPoint train = kptsCurr[match.trainIdx];
        cv::KeyPoint query = kptsPrev[match.queryIdx];

        auto train_pt = cv::Point(train.pt.x, train.pt.y);
        auto query_pt = cv::Point(query.pt.x, query.pt.y);

        if (cv::norm(train_pt - query_pt) < dist_mean * 1.5)
            boundingBox_c.kptMatches.push_back(match);
    }

    std::cout << "curr_bbx_matches_size: " << boundingBox_c.kptMatches.size() << std::endl;
}

void computeTTCCamera(std::vector<cv::KeyPoint> &kptsPrev, std::vector<cv::KeyPoint> &kptsCurr, 
                      std::vector<cv::DMatch> kptMatches, double frameRate, double &TTC, cv::Mat *visImg)
{
    std::vector<double> distRatios;
    
    for (size_t i = 0; i < kptMatches.size() - 1; ++i)
    {
        const cv::KeyPoint& kpOuterCurr = kptsCurr[kptMatches[i].trainIdx];
        const cv::KeyPoint& kpOuterPrev = kptsPrev[kptMatches[i].queryIdx];

        for (size_t j = i + 1; j < kptMatches.size(); ++j)
        {
            const cv::KeyPoint& kpInnerCurr = kptsCurr[kptMatches[j].trainIdx];
            const cv::KeyPoint& kpInnerPrev = kptsPrev[kptMatches[j].queryIdx];

            double distCurr = cv::norm(kpOuterCurr.pt - kpInnerCurr.pt);
            double distPrev = cv::norm(kpOuterPrev.pt - kpInnerPrev.pt);

            if (distPrev > std::numeric_limits<double>::epsilon() && distCurr >= 100.0)
            {
                double distRatio = distCurr / distPrev;
                distRatios.push_back(distRatio);
            }
        }
    }

    if (distRatios.empty())
    {
        TTC = NAN;
        return;
    }

    std::sort(distRatios.begin(), distRatios.end());

    size_t medIndex = distRatios.size() / 2;
    double medDistRatio = (distRatios.size() % 2 == 0)
        ? (distRatios[medIndex - 1] + distRatios[medIndex]) / 2.0
        : distRatios[medIndex];

    double dT = 1.0 / frameRate;
    TTC = -dT / (1.0 - medDistRatio);


}