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

// essential.hpp — Essential 行列推定 + 分解
//
// 新規実装:
//   - 8 点法 (正規化)
//   - Essential 行列から R, t を分解
//   - RANSAC によるロバスト推定

#ifndef CALX_IMAGE_CV_ESSENTIAL_HPP
#define CALX_IMAGE_CV_ESSENTIAL_HPP

#include <math/core/matrix.hpp>
#include <math/linalg/decomposition.hpp>
#include <image/cv/ransac.hpp>
#include <image/cv/camera.hpp>
#include <array>
#include <cmath>
#include <vector>

namespace calx::cv {

    /// 正規化カメラ座標での対応点ペア
    struct NormalizedMatch {
        double x1, y1;  // 画像1 の正規化座標
        double x2, y2;  // 画像2 の正規化座標
    };

    /// 8 点法で Fundamental/Essential 行列を推定
    /// @param matches  8 点以上の正規化対応点
    /// @return         3x3 Essential 行列
    inline calx::Matrix<double> estimateEssential8pt(const std::vector<NormalizedMatch>& matches) {
        std::size_t n = matches.size();

        // A * e = 0 の構築 (n × 9)
        calx::Matrix<double> A(n, 9, 0.0);
        for (std::size_t i = 0; i < n; ++i) {
            double x1 = matches[i].x1, y1 = matches[i].y1;
            double x2 = matches[i].x2, y2 = matches[i].y2;

            A(i, 0) = x2 * x1; A(i, 1) = x2 * y1; A(i, 2) = x2;
            A(i, 3) = y2 * x1; A(i, 4) = y2 * y1; A(i, 5) = y2;
            A(i, 6) = x1;      A(i, 7) = y1;      A(i, 8) = 1.0;
        }

        // SVD
        auto [U, S, Vt] = calx::algorithms::svd_decomposition(A);

        // 最小特異値に対応するベクトル
        calx::Matrix<double> E(3, 3);
        for (int i = 0; i < 9; ++i) {
            E(i / 3, i % 3) = Vt(8, i);
        }

        // Essential 行列の制約: 特異値が (σ, σ, 0) になるよう射影
        auto [Ue, Se, Vte] = calx::algorithms::svd_decomposition(E);
        double sigma = (Se[0] + Se[1]) / 2.0;

        calx::Matrix<double> diagS(3, 3, 0.0);
        diagS(0, 0) = sigma;
        diagS(1, 1) = sigma;
        // diagS(2, 2) = 0.0

        return Ue * diagS * Vte;
    }

    /// Essential 行列から R, t の 4 つの候補を分解
    /// @param E  Essential 行列
    /// @return   4 つの (R, t) 候補
    inline std::vector<std::pair<calx::Matrix<double>, calx::Vector<double>>>
    decomposeEssential(const calx::Matrix<double>& E) {
        auto [U, S, Vt] = calx::algorithms::svd_decomposition(E);

        // W 行列 (90° 回転)
        calx::Matrix<double> W(3, 3, 0.0);
        W(0, 1) = -1.0; W(1, 0) = 1.0; W(2, 2) = 1.0;

        calx::Matrix<double> Wt(3, 3, 0.0);
        Wt(0, 1) = 1.0; Wt(1, 0) = -1.0; Wt(2, 2) = 1.0;

        // R の候補
        calx::Matrix<double> R1 = U * W * Vt;
        calx::Matrix<double> R2 = U * Wt * Vt;

        // det(R) = +1 を保証
        auto ensureDetPositive = [](calx::Matrix<double>& R) {
            double det = R(0,0)*(R(1,1)*R(2,2)-R(1,2)*R(2,1))
                       - R(0,1)*(R(1,0)*R(2,2)-R(1,2)*R(2,0))
                       + R(0,2)*(R(1,0)*R(2,1)-R(1,1)*R(2,0));
            if (det < 0) {
                for (int i = 0; i < 3; ++i)
                    for (int j = 0; j < 3; ++j)
                        R(i, j) = -R(i, j);
            }
        };
        ensureDetPositive(R1);
        ensureDetPositive(R2);

        // t の候補 (U の第 3 列)
        calx::Vector<double> t(3);
        for (int i = 0; i < 3; ++i) t[i] = U(i, 2);

        // 正規化
        double tn = std::sqrt(t[0]*t[0] + t[1]*t[1] + t[2]*t[2]);
        if (tn > 1e-12) { t[0] /= tn; t[1] /= tn; t[2] /= tn; }

        calx::Vector<double> neg_t(3);
        neg_t[0] = -t[0]; neg_t[1] = -t[1]; neg_t[2] = -t[2];

        return {{R1, t}, {R1, neg_t}, {R2, t}, {R2, neg_t}};
    }

    /// 正しい (R, t) を選択 (三角測量によるキラリティテスト)
    /// @param candidates  4 つの (R, t) 候補
    /// @param matches     対応点
    /// @return            正しい (R, t)
    inline std::pair<calx::Matrix<double>, calx::Vector<double>>
    selectCorrectPose(const std::vector<std::pair<calx::Matrix<double>, calx::Vector<double>>>& candidates,
                      const std::vector<NormalizedMatch>& matches) {
        int bestIdx = 0;
        int bestCount = 0;

        for (int c = 0; c < static_cast<int>(candidates.size()); ++c) {
            const auto& [R, t] = candidates[c];
            int count = 0;

            for (const auto& m : matches) {
                // 三角測量 (簡易: 線形法)
                // P1 = [I | 0], P2 = [R | t]
                // 各点の Z > 0 チェック (キラリティ)
                calx::Matrix<double> A2(4, 4, 0.0);
                A2(0, 0) = m.x1; A2(0, 1) = -1.0; // x1 * p3 - p1 (for P1=[I|0])
                A2(1, 0) = m.y1; A2(1, 2) = -1.0;

                // P2 = [R | t]
                double p2_row0 = R(0,0)*m.x2 + R(0,1)*m.y2 + R(0,2);
                double p2_row1 = R(1,0)*m.x2 + R(1,1)*m.y2 + R(1,2);

                // 簡易チェック: カメラ1 の前方にあるか
                // Z1 = depth in camera 1
                double denom = 1.0 - p2_row0 * m.x1 - p2_row1 * m.y1;
                if (std::abs(denom) < 1e-10) continue;

                double Z1 = (t[0] * m.x1 + t[1] * m.y1 + t[2]) / denom;

                // Z2 = depth in camera 2
                double X = Z1 * m.x1;
                double Y = Z1 * m.y1;
                double Z2 = R(2,0)*X + R(2,1)*Y + R(2,2)*Z1 + t[2];

                if (Z1 > 0 && Z2 > 0) ++count;
            }

            if (count > bestCount) {
                bestCount = count;
                bestIdx = c;
            }
        }

        return candidates[bestIdx];
    }

    /// RANSAC で Essential 行列をロバスト推定
    /// @param matches     正規化対応点
    /// @param maxIter     RANSAC 反復回数
    /// @param threshold   インライア判定閾値
    /// @return            (Essential 行列, インライアインデックス)
    inline std::pair<calx::Matrix<double>, std::vector<int>>
    findEssentialRansac(const std::vector<NormalizedMatch>& matches,
                        int maxIter = 500,
                        double threshold = 0.01) {
        auto fitFn = [](const std::vector<NormalizedMatch>& sample) -> calx::Matrix<double> {
            return estimateEssential8pt(sample);
        };

        auto errorFn = [](const calx::Matrix<double>& E, const NormalizedMatch& m) -> double {
            // Sampson 誤差の簡易近似
            // x2^T E x1 (エピポーラ拘束)
            double ex1_0 = E(0,0)*m.x1 + E(0,1)*m.y1 + E(0,2);
            double ex1_1 = E(1,0)*m.x1 + E(1,1)*m.y1 + E(1,2);
            double ex1_2 = E(2,0)*m.x1 + E(2,1)*m.y1 + E(2,2);

            double err = m.x2 * ex1_0 + m.y2 * ex1_1 + ex1_2;

            double etx2_0 = E(0,0)*m.x2 + E(1,0)*m.y2 + E(2,0);
            double etx2_1 = E(0,1)*m.x2 + E(1,1)*m.y2 + E(2,1);

            double denom = ex1_0*ex1_0 + ex1_1*ex1_1 + etx2_0*etx2_0 + etx2_1*etx2_1;
            if (denom < 1e-15) return 1e10;

            return (err * err) / denom;
        };

        auto result = ransac<calx::Matrix<double>, NormalizedMatch>(
            matches, 8, maxIter, threshold, fitFn, errorFn);

        return {result.bestModel, result.inlierIndices};
    }

} // namespace calx::cv

#endif // CALX_IMAGE_CV_ESSENTIAL_HPP
