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

// HoughTransform.hpp — Hough 変換 (直線検出 + 円検出)
//
// 移植元: lib++20 画像処理/パターン検出/Hough変換.h
// 改良点:
//   - calx Matrix<T> ベース
//   - 英語 API
//   - ピーク検出・NMS 統合
//   - 円 Hough 変換追加

#ifndef CALX_IMAGE_DETECTION_HOUGH_HPP
#define CALX_IMAGE_DETECTION_HOUGH_HPP

#include <math/core/matrix.hpp>
#include <algorithm>
#include <cassert>
#include <cmath>
#include <vector>

namespace calx::image {

    /// Hough 変換で検出された直線パラメータ
    template<typename T>
    struct HoughPeak {
        std::size_t thetaIdx;   // θ 方向のインデックス
        std::size_t rhoIdx;     // ρ 方向のインデックス
        T theta;                // 法線角度 [rad]
        T rho;                  // 原点からの距離 [pixel]
        T votes;                // 投票数
    };

    /// 直線 Hough 変換
    /// @param image     入力画像 (2 値またはエッジ強度; 0 = 背景)
    /// @param thetaMin  θ の最小値 [rad]
    /// @param thetaMax  θ の最大値 [rad]
    /// @param nTheta    θ の分割数
    /// @param rhoMin    ρ の最小値 [pixel]
    /// @param rhoMax    ρ の最大値 [pixel]
    /// @param nRho      ρ の分割数
    /// @return          (nRho × nTheta) のアキュムレータ行列
    template<typename T>
    Matrix<T> houghTransform(const Matrix<T>& image,
                             T thetaMin, T thetaMax, int nTheta,
                             T rhoMin, T rhoMax, int nRho) {
        assert(nTheta > 1 && nRho > 1);

        T dTheta = (thetaMax - thetaMin) / (nTheta - 1);
        T dRho = (rhoMax - rhoMin) / (nRho - 1);

        // cos, sin テーブルを事前計算
        std::vector<T> cosTable(nTheta), sinTable(nTheta);
        for (int i = 0; i < nTheta; ++i) {
            T theta = thetaMin + dTheta * i;
            cosTable[i] = std::cos(theta);
            sinTable[i] = std::sin(theta);
        }

        std::size_t H = image.rows();
        std::size_t W = image.cols();
        Matrix<T> acc(nRho, nTheta, T{0});

        for (std::size_t y = 0; y < H; ++y) {
            for (std::size_t x = 0; x < W; ++x) {
                T a = image(y, x);
                if (a == T{0}) continue;

                for (int iTheta = 0; iTheta < nTheta; ++iTheta) {
                    T rho = (x + T{0.5}) * cosTable[iTheta]
                          + (y + T{0.5}) * sinTable[iTheta];
                    int iRho = static_cast<int>((rho - rhoMin) / dRho + T{0.5});
                    if (iRho >= 0 && iRho < nRho) {
                        acc(iRho, iTheta) += a;
                    }
                }
            }
        }
        return acc;
    }

    /// アキュムレータからピークを検出 (Non-Maximum Suppression)
    /// @param acc           アキュムレータ行列 (nRho × nTheta)
    /// @param thetaMin      θ の最小値
    /// @param dTheta        θ の刻み幅
    /// @param rhoMin        ρ の最小値
    /// @param dRho          ρ の刻み幅
    /// @param threshold     ピーク閾値 (最大投票数に対する比率, 0-1)
    /// @return              検出されたピークのリスト
    template<typename T>
    std::vector<HoughPeak<T>> findPeaks(const Matrix<T>& acc,
                                         T thetaMin, T dTheta,
                                         T rhoMin, T dRho,
                                         T threshold = T{0.5}) {
        std::size_t nRho = acc.rows();
        std::size_t nTheta = acc.cols();

        // 最大値を求める
        T maxVal = T{0};
        for (std::size_t i = 0; i < nRho; ++i)
            for (std::size_t j = 0; j < nTheta; ++j)
                if (acc(i, j) > maxVal) maxVal = acc(i, j);

        T thresh = maxVal * threshold;
        std::vector<HoughPeak<T>> peaks;

        for (std::size_t y = 0; y < nRho; ++y) {
            for (std::size_t x = 0; x < nTheta; ++x) {
                if (acc(y, x) <= thresh) continue;

                // 3×3 近傍で局所最大か判定
                bool isMax = true;
                for (int dy = -1; dy <= 1 && isMax; ++dy) {
                    for (int dx = -1; dx <= 1 && isMax; ++dx) {
                        if (dy == 0 && dx == 0) continue;
                        int ny = static_cast<int>(y) + dy;
                        int nx = static_cast<int>(x) + dx;
                        if (ny >= 0 && ny < static_cast<int>(nRho)
                            && nx >= 0 && nx < static_cast<int>(nTheta)) {
                            if (acc(ny, nx) > acc(y, x)) isMax = false;
                        }
                    }
                }

                if (isMax) {
                    peaks.push_back({x, y,
                                     thetaMin + dTheta * x,
                                     rhoMin + dRho * y,
                                     acc(y, x)});
                }
            }
        }

        // 投票数で降順ソート
        std::sort(peaks.begin(), peaks.end(),
                  [](const HoughPeak<T>& a, const HoughPeak<T>& b) {
                      return a.votes > b.votes;
                  });

        return peaks;
    }

    /// 円 Hough 変換
    /// @param image     入力画像 (エッジ強度)
    /// @param rMin      検出する半径の最小値
    /// @param rMax      検出する半径の最大値
    /// @param nR        半径の分割数
    /// @return          (nR × H × W) の 3D アキュムレータ (Matrix のベクトルとして返す)
    template<typename T>
    std::vector<Matrix<T>> circleHoughTransform(const Matrix<T>& image,
                                                 T rMin, T rMax, int nR) {
        assert(nR > 0);

        std::size_t H = image.rows();
        std::size_t W = image.cols();
        T dr = (nR > 1) ? (rMax - rMin) / (nR - 1) : T{0};

        std::vector<Matrix<T>> acc(nR, Matrix<T>(H, W, T{0}));

        // 角度テーブル (360 分割)
        constexpr int nAngle = 360;
        std::vector<T> cosA(nAngle), sinA(nAngle);
        for (int i = 0; i < nAngle; ++i) {
            T angle = T{2} * T{3.14159265358979323846} * i / nAngle;
            cosA[i] = std::cos(angle);
            sinA[i] = std::sin(angle);
        }

        for (std::size_t y = 0; y < H; ++y) {
            for (std::size_t x = 0; x < W; ++x) {
                T a = image(y, x);
                if (a == T{0}) continue;

                for (int ir = 0; ir < nR; ++ir) {
                    T r = rMin + dr * ir;
                    for (int ia = 0; ia < nAngle; ++ia) {
                        int cx = static_cast<int>(x - r * cosA[ia] + T{0.5});
                        int cy = static_cast<int>(y - r * sinA[ia] + T{0.5});
                        if (cx >= 0 && cx < static_cast<int>(W)
                            && cy >= 0 && cy < static_cast<int>(H)) {
                            acc[ir](cy, cx) += a;
                        }
                    }
                }
            }
        }
        return acc;
    }

} // namespace calx::image

#endif // CALX_IMAGE_DETECTION_HOUGH_HPP
