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

// fast.hpp — FAST 特徴点検出 (Features from Accelerated Segment Test)
//
// 新規実装:
//   - 16 画素 Bresenham 円上の輝度比較
//   - 高速判定 (4点テスト → 12点テスト)
//   - NMS + 応答値計算

#ifndef CALX_IMAGE_CV_FAST_HPP
#define CALX_IMAGE_CV_FAST_HPP

#include <image/Image.hpp>
#include <image/cv/harris.hpp>  // KeyPoint
#include <algorithm>
#include <cmath>
#include <vector>

namespace calx::cv {

    /// FAST 特徴点検出
    /// @param img       入力画像 (1ch, グレースケール)
    /// @param threshold 輝度差の閾値
    /// @param nPoints   連続して明るい/暗い画素の数 (9, 11, 12 が一般的)
    /// @param doNms     Non-Maximum Suppression を行うか
    /// @return          検出されたキーポイント
    template<typename T>
    std::vector<KeyPoint> fastDetect(const image::Image<T>& img,
                                     T threshold = T{20},
                                     int nPoints = 9,
                                     bool doNms = true) {
        // uint8_t 以外の型で threshold を正規化
        T thresh = threshold;
        if constexpr (std::is_floating_point_v<T>) {
            // [0,1] の場合はそのまま
        } else {
            // 整数型の場合はそのまま
        }

        int H = static_cast<int>(img.height());
        int W = static_cast<int>(img.width());

        // Bresenham 円 (半径 3) の 16 画素のオフセット
        static const int circle16[16][2] = {
            { 0, -3}, { 1, -3}, { 2, -2}, { 3, -1},
            { 3,  0}, { 3,  1}, { 2,  2}, { 1,  3},
            { 0,  3}, {-1,  3}, {-2,  2}, {-3,  1},
            {-3,  0}, {-3, -1}, {-2, -2}, {-1, -3}
        };

        const auto& ch = img.channel(0);

        // スコア行列 (NMS 用)
        calx::Matrix<T> score(H, W, T{0});

        std::vector<KeyPoint> keypoints;

        for (int y = 3; y < H - 3; ++y) {
            for (int x = 3; x < W - 3; ++x) {
                T center = ch(y, x);
                T upper = center + thresh;
                T lower = center - thresh;

                // 高速リジェクト: 1, 5, 9, 13 番目 (四隅) をチェック
                int nBright = 0, nDark = 0;
                for (int idx : {0, 4, 8, 12}) {
                    T p = ch(y + circle16[idx][1], x + circle16[idx][0]);
                    if (p > upper) ++nBright;
                    else if (p < lower) ++nDark;
                }
                if (nBright < 3 && nDark < 3) continue;

                // 全 16 画素を分類
                int bright[16] = {}, dark[16] = {};
                T sumDiff = T{0};
                for (int i = 0; i < 16; ++i) {
                    T p = ch(y + circle16[i][1], x + circle16[i][0]);
                    if (p > upper) {
                        bright[i] = 1;
                        sumDiff += std::abs(p - center);
                    } else if (p < lower) {
                        dark[i] = 1;
                        sumDiff += std::abs(p - center);
                    }
                }

                // nPoints 連続チェック (円形なので mod 16)
                bool isBright = false, isDark = false;

                auto checkConsecutive = [&](const int* arr) -> bool {
                    int maxRun = 0, run = 0;
                    // 2 周分チェック (円の折り返し)
                    for (int i = 0; i < 32; ++i) {
                        if (arr[i % 16]) {
                            ++run;
                            if (run >= nPoints) return true;
                        } else {
                            run = 0;
                        }
                    }
                    return false;
                };

                isBright = checkConsecutive(bright);
                isDark = checkConsecutive(dark);

                if (isBright || isDark) {
                    score(y, x) = sumDiff;
                    keypoints.emplace_back(
                        static_cast<double>(x), static_cast<double>(y),
                        static_cast<double>(sumDiff));
                }
            }
        }

        // NMS
        if (doNms && !keypoints.empty()) {
            std::vector<KeyPoint> filtered;
            filtered.reserve(keypoints.size());

            for (const auto& kp : keypoints) {
                int px = static_cast<int>(kp.x);
                int py = static_cast<int>(kp.y);
                T s = score(py, px);
                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 = py + dy, nx = px + dx;
                        if (ny >= 0 && ny < H && nx >= 0 && nx < W) {
                            if (score(ny, nx) > s) isMax = false;
                        }
                    }
                }
                if (isMax) filtered.push_back(kp);
            }
            return filtered;
        }

        return keypoints;
    }

} // namespace calx::cv

#endif // CALX_IMAGE_CV_FAST_HPP
