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

// triangulation.hpp — 三角測量 (2 視点から 3D 点を復元)
//
// 新規実装:
//   - 線形三角測量 (DLT)
//   - 非線形精密化 (オプション)

#ifndef CALX_IMAGE_CV_TRIANGULATION_HPP
#define CALX_IMAGE_CV_TRIANGULATION_HPP

#include <math/core/matrix.hpp>
#include <math/linalg/decomposition.hpp>
#include <array>
#include <cmath>
#include <vector>

namespace calx::cv {

    /// 線形三角測量 (DLT)
    /// @param P1  カメラ 1 の射影行列 (3x4)
    /// @param P2  カメラ 2 の射影行列 (3x4)
    /// @param x1, y1  画像 1 の正規化座標
    /// @param x2, y2  画像 2 の正規化座標
    /// @return  3D 点 (X, Y, Z)
    inline std::array<double, 3> triangulatePoint(
        const calx::Matrix<double>& P1, const calx::Matrix<double>& P2,
        double x1, double y1, double x2, double y2) {

        // A * X = 0 (4x4)
        calx::Matrix<double> A(4, 4, 0.0);
        for (int j = 0; j < 4; ++j) {
            A(0, j) = x1 * P1(2, j) - P1(0, j);
            A(1, j) = y1 * P1(2, j) - P1(1, j);
            A(2, j) = x2 * P2(2, j) - P2(0, j);
            A(3, j) = y2 * P2(2, j) - P2(1, j);
        }

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

        // 最後の行 (最小特異値に対応)
        double w = Vt(3, 3);
        if (std::abs(w) < 1e-15) w = 1e-15;

        return {Vt(3, 0) / w, Vt(3, 1) / w, Vt(3, 2) / w};
    }

    /// 複数点の三角測量
    /// @param P1, P2  射影行列
    /// @param pts1    画像 1 の正規化座標
    /// @param pts2    画像 2 の正規化座標
    /// @return        3D 点のリスト
    inline std::vector<std::array<double, 3>> triangulatePoints(
        const calx::Matrix<double>& P1, const calx::Matrix<double>& P2,
        const std::vector<std::array<double, 2>>& pts1,
        const std::vector<std::array<double, 2>>& pts2) {

        std::size_t n = std::min(pts1.size(), pts2.size());
        std::vector<std::array<double, 3>> points3d;
        points3d.reserve(n);

        for (std::size_t i = 0; i < n; ++i) {
            points3d.push_back(triangulatePoint(
                P1, P2, pts1[i][0], pts1[i][1], pts2[i][0], pts2[i][1]));
        }

        return points3d;
    }

    /// P = K * [R | t] を構築
    inline calx::Matrix<double> buildProjectionMatrix(
        const calx::Matrix<double>& K,
        const calx::Matrix<double>& R,
        const calx::Vector<double>& t) {

        calx::Matrix<double> Rt(3, 4, 0.0);
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) Rt(i, j) = R(i, j);
            Rt(i, 3) = t[i];
        }
        return K * Rt;
    }

} // namespace calx::cv

#endif // CALX_IMAGE_CV_TRIANGULATION_HPP
