// Copyright (C) 2026 Kiyotsugu Arai
// SPDX-License-Identifier: LGPL-3.0-or-later

// ransac.hpp — 汎用 RANSAC フレームワーク
//
// 新規実装:
//   - テンプレートベースの汎用 RANSAC
//   - ホモグラフィ、Essential 行列等のロバスト推定に使用

#ifndef CALX_IMAGE_CV_RANSAC_HPP
#define CALX_IMAGE_CV_RANSAC_HPP

#include <algorithm>
#include <cmath>
#include <functional>
#include <numeric>
#include <random>
#include <vector>

namespace calx::cv {

    /// RANSAC の結果
    template<typename Model>
    struct RansacResult {
        Model bestModel;
        std::vector<int> inlierIndices;
        int numInliers = 0;
    };

    /// 汎用 RANSAC
    /// @tparam Model       モデルの型
    /// @tparam Datum        データ点の型
    /// @param data          全データ点
    /// @param sampleSize    モデル推定に必要な最小サンプル数
    /// @param maxIter       最大反復回数
    /// @param threshold     インライア判定の閾値
    /// @param fitModel      sampleSize 個の点からモデルを推定する関数
    /// @param calcError     1 点のモデルからの誤差を計算する関数
    /// @return              最良モデルとインライア
    template<typename Model, typename Datum>
    RansacResult<Model> ransac(
        const std::vector<Datum>& data,
        int sampleSize,
        int maxIter,
        double threshold,
        std::function<Model(const std::vector<Datum>&)> fitModel,
        std::function<double(const Model&, const Datum&)> calcError) {

        int n = static_cast<int>(data.size());
        if (n < sampleSize) return {};

        RansacResult<Model> best;
        std::mt19937 rng(12345);

        for (int iter = 0; iter < maxIter; ++iter) {
            // ランダムサンプリング
            std::vector<int> indices(n);
            std::iota(indices.begin(), indices.end(), 0);
            for (int i = 0; i < sampleSize; ++i) {
                std::uniform_int_distribution<int> dist(i, n - 1);
                std::swap(indices[i], indices[dist(rng)]);
            }

            std::vector<Datum> sample(sampleSize);
            for (int i = 0; i < sampleSize; ++i) {
                sample[i] = data[indices[i]];
            }

            // モデル推定
            Model model;
            try {
                model = fitModel(sample);
            } catch (...) {
                continue;
            }

            // インライア数のカウント
            std::vector<int> inliers;
            for (int i = 0; i < n; ++i) {
                double err = calcError(model, data[i]);
                if (err < threshold) {
                    inliers.push_back(i);
                }
            }

            if (static_cast<int>(inliers.size()) > best.numInliers) {
                best.bestModel = model;
                best.inlierIndices = inliers;
                best.numInliers = static_cast<int>(inliers.size());
            }
        }

        return best;
    }

} // namespace calx::cv

#endif // CALX_IMAGE_CV_RANSAC_HPP
