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

// harris.hpp — Harris コーナー検出
//
// 新規実装:
//   - 画像勾配 (Sobel) → 構造テンソル → コーナー応答 R → NMS
//   - Image<T> ベース

#ifndef CALX_IMAGE_CV_HARRIS_HPP
#define CALX_IMAGE_CV_HARRIS_HPP

#include <image/Image.hpp>
#include <image/filters/ndimage.hpp>
#include <algorithm>
#include <cmath>
#include <vector>

namespace calx::cv {

    /// 検出されたキーポイント
    struct KeyPoint {
        double x, y;        // 位置 (サブピクセル)
        double response;    // 応答値
        double angle;       // 方向 [rad] (-1 = 未計算)
        int octave;         // ピラミッドレベル
        double size;        // 特徴点のサイズ

        KeyPoint() : x(0), y(0), response(0), angle(-1), octave(0), size(1) {}
        KeyPoint(double x, double y, double resp = 0, double angle = -1, int oct = 0, double sz = 1)
            : x(x), y(y), response(resp), angle(angle), octave(oct), size(sz) {}
    };

    /// Sobel 勾配を計算 (Ix, Iy)
    template<typename T>
    std::pair<calx::Matrix<T>, calx::Matrix<T>> sobelGradient(const calx::Matrix<T>& img) {
        // Sobel カーネル
        calx::Matrix<T> kx(3, 3);
        kx(0,0) = T{-1}; kx(0,1) = T{0}; kx(0,2) = T{1};
        kx(1,0) = T{-2}; kx(1,1) = T{0}; kx(1,2) = T{2};
        kx(2,0) = T{-1}; kx(2,1) = T{0}; kx(2,2) = T{1};

        calx::Matrix<T> ky(3, 3);
        ky(0,0) = T{-1}; ky(0,1) = T{-2}; ky(0,2) = T{-1};
        ky(1,0) = T{0};  ky(1,1) = T{0};  ky(1,2) = T{0};
        ky(2,0) = T{1};  ky(2,1) = T{2};  ky(2,2) = T{1};

        auto Ix = image::convolve(img, kx);
        auto Iy = image::convolve(img, ky);
        return {Ix, Iy};
    }

    /// Harris コーナー検出
    /// @param img       入力画像 (1ch, グレースケール)
    /// @param threshold コーナー応答の閾値
    /// @param k         Harris パラメータ (通常 0.04-0.06)
    /// @param blockSize 構造テンソルの平均化ウィンドウサイズ
    /// @return          検出されたキーポイント
    template<typename T>
    std::vector<KeyPoint> harrisCorners(const image::Image<T>& img,
                                        T threshold = T{0.01},
                                        T k = T{0.04},
                                        int blockSize = 3) {
        auto [Ix, Iy] = sobelGradient(img.channel(0));
        int H = static_cast<int>(img.height());
        int W = static_cast<int>(img.width());

        // 構造テンソルの要素
        calx::Matrix<T> Ixx(H, W), Ixy(H, W), Iyy(H, W);
        for (int y = 0; y < H; ++y) {
            for (int x = 0; x < W; ++x) {
                Ixx(y, x) = Ix(y, x) * Ix(y, x);
                Ixy(y, x) = Ix(y, x) * Iy(y, x);
                Iyy(y, x) = Iy(y, x) * Iy(y, x);
            }
        }

        // ガウシアンぼかしで平均化
        auto kernel = image::gaussianKernel<T>(blockSize, T{1.0});
        Ixx = image::convolve(Ixx, kernel);
        Ixy = image::convolve(Ixy, kernel);
        Iyy = image::convolve(Iyy, kernel);

        // コーナー応答 R = det(M) - k * trace(M)^2
        calx::Matrix<T> R(H, W);
        T maxR = T{0};
        for (int y = 0; y < H; ++y) {
            for (int x = 0; x < W; ++x) {
                T det = Ixx(y, x) * Iyy(y, x) - Ixy(y, x) * Ixy(y, x);
                T trace = Ixx(y, x) + Iyy(y, x);
                R(y, x) = det - k * trace * trace;
                if (R(y, x) > maxR) maxR = R(y, x);
            }
        }

        // NMS (Non-Maximum Suppression) + 閾値
        // maxR が非常に小さい場合は検出なしとする
        if (maxR < T{1e-10}) return {};

        T absThreshold = threshold * maxR;
        int half = blockSize / 2;
        if (half < 1) half = 1;

        std::vector<KeyPoint> keypoints;
        for (int y = half; y < H - half; ++y) {
            for (int x = half; x < W - half; ++x) {
                if (R(y, x) <= absThreshold) continue;

                bool isMax = true;
                for (int dy = -half; dy <= half && isMax; ++dy) {
                    for (int dx = -half; dx <= half && isMax; ++dx) {
                        if (dy == 0 && dx == 0) continue;
                        if (R(y + dy, x + dx) >= R(y, x)) isMax = false;
                    }
                }
                if (isMax) {
                    keypoints.emplace_back(
                        static_cast<double>(x), static_cast<double>(y),
                        static_cast<double>(R(y, x)));
                }
            }
        }

        // 応答値で降順ソート
        std::sort(keypoints.begin(), keypoints.end(),
                  [](const KeyPoint& a, const KeyPoint& b) { return a.response > b.response; });

        return keypoints;
    }

} // namespace calx::cv

#endif // CALX_IMAGE_CV_HARRIS_HPP
