#include "Arith_FeaMatch.h"
#include "Arith_GeoSolver.h"
using std::vector;
using cv::KeyPoint;

// 构造函数
FeatureMatcher::FeatureMatcher(DetectorType detectorType, MatcherType matcherType)
    : detectorType_(detectorType), matcherType_(matcherType)
{
    initDetector();
    initMatcher();
}

// 提取特征点和描述符
void FeatureMatcher::extractFeatures(cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)
{
    detector_->detectAndCompute(image, cv::noArray(), keypoints, descriptors);
}

// 匹配特征点
void FeatureMatcher::matchFeatures(cv::Mat& descriptors1, cv::Mat& descriptors2, std::vector<cv::DMatch>& matches)
{
    if (matcherType_ == FLANN)
    {
        flannMatcher_->match(descriptors1, descriptors2, matches);
    }
    else
    {
        bfMatcher_->match(descriptors1, descriptors2, matches);
    }
}

void FeatureMatcher::matchFeatures_WithH(vector<KeyPoint> keypoints1, cv::Mat& descriptors1, vector<KeyPoint> keypoints2, cv::Mat& descriptors2, cv::Mat H1, cv::Mat H2, std::vector<cv::DMatch>& matches)
{
    std::vector<cv::DMatch> _matches;
    if (matcherType_ == FLANN)
    {
        flannMatcher_->match(descriptors1, descriptors2, _matches);
    }
    else
    {
        bfMatcher_->match(descriptors1, descriptors2, _matches);
    }

    int i, j;
    double minimaDsit = 99999;
    for (i = 0; i < _matches.size(); i++)
    {
        double dist = _matches[i].distance;
        if (dist < minimaDsit)
        {
            minimaDsit = dist;
        }
    }
    double fitedThreshold = minimaDsit * 10;

    for (size_t i = 0; i < _matches.size(); i++)
    {
        cv::DMatch mc = _matches[i];

        if (mc.distance > fitedThreshold)
        {
            continue;
        }

        auto warp_pt1 = warpPointWithH(H1, keypoints1[mc.queryIdx].pt);
        auto warp_pt2 = warpPointWithH(H2, keypoints2[mc.trainIdx].pt);

        if (fabs(warp_pt1.x - warp_pt2.x) + fabs(warp_pt1.y - warp_pt2.y) < 15)
        {
            matches.push_back(mc);
        }
    }
}

// 计算单应性矩阵
cv::Mat FeatureMatcher::computeHomography(std::vector<cv::KeyPoint>& keypoints1, std::vector<cv::KeyPoint>& keypoints2,
    std::vector<cv::DMatch>& matches, double ransacReprojThreshold)
{
    std::vector<cv::Point2f> points1, points2;
    for (const auto& match : matches)
    {
        points1.push_back(keypoints1[match.queryIdx].pt);
        points2.push_back(keypoints2[match.trainIdx].pt);
    }
    return cv::findHomography(points1, points2, cv::RANSAC, ransacReprojThreshold);
}

// 初始化特征检测器
void FeatureMatcher::initDetector()
{
    switch (detectorType_)
    {
    case SIFT:
        detector_ = cv::SIFT::create(FEA_NUM_MAX);
        break;
    case SURF:
        //detector_ = cv::xfeatures2d::SURF::create();
#ifdef HAVE_OPENCV_XFEATURES2D
       detector_ = cv::xfeatures2d::SURF::create();
#else
        throw std::runtime_error("SURF is not supported in this OpenCV build.");
#endif
        break;
    case ORB:
        detector_ = cv::ORB::create(FEA_NUM_MAX);
        break;
    default:
        throw std::invalid_argument("Unsupported feature detector type");
    }
}

// 初始化匹配器
void FeatureMatcher::initMatcher()
{
    if (matcherType_ == FLANN)
    {
        if (detectorType_ == SIFT || detectorType_ == SURF)
        {
            flannMatcher_ = cv::FlannBasedMatcher::create();
        }
        else
        {
            flannMatcher_ = cv::makePtr<cv::FlannBasedMatcher>(cv::FlannBasedMatcher(cv::makePtr<cv::flann::LshIndexParams>(12, 20, 2)));
        }
    }
    else
    {
        if (detectorType_ == SIFT || detectorType_ == SURF)
        {
            bfMatcher_ = cv::BFMatcher::create(cv::NORM_L2);
        }
        else
        {
            bfMatcher_ = cv::BFMatcher::create(cv::NORM_HAMMING);
        }
    }
}