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

/**
 * @file feature_descriptors.hpp
 * @brief AKAZE / BRISK 特徴点検出・記述 (OCV-20, OCV-21)
 *
 * AKAZE: 非線形スケール空間での加速特徴検出。特許なし。
 * BRISK: Binary Robust Invariant Scalable Keypoints。特許なし。
 */

#ifndef CALX_IMAGE_CV_FEATURE_DESCRIPTORS_HPP
#define CALX_IMAGE_CV_FEATURE_DESCRIPTORS_HPP

#include <image/Image.hpp>
#include <vector>
#include <cmath>
#include <algorithm>
#include <array>
#include <cstdint>
#include <numeric>

namespace calx::image {

/// 特徴点
struct KeyPoint {
    float x, y;         // 位置
    float size;         // スケール
    float angle;        // 方向 [rad]
    float response;     // 応答強度
    int octave;         // オクターブ
};

/// バイナリ記述子 (256bit = 32bytes)
using BinaryDescriptor = std::array<uint8_t, 32>;

// ========================================================================
// BRISK (簡易版)
// ========================================================================

/// BRISK 特徴点検出 (FAST ベース + スケール空間)
template<typename T>
std::vector<KeyPoint> detectBRISK(const Image<T>& img, int threshold = 30) {
    Image<T> gray = (img.numChannels() >= 3) ? img.toGray() : img;
    int h = static_cast<int>(gray.height());
    int w = static_cast<int>(gray.width());
    std::vector<KeyPoint> kps;

    // FAST-9 コーナー検出 (簡易版: Bresenham 円上の 16 ピクセル)
    static const int circle[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}
    };

    T t = T(threshold) / T(255);
    for (int y = 3; y < h - 3; ++y) {
        for (int x = 3; x < w - 3; ++x) {
            T center = gray(y, x);
            int bright = 0, dark = 0;

            // 高速チェック: 1,5,9,13 番目のピクセル
            for (int i : {0, 4, 8, 12}) {
                T v = gray(y + circle[i][1], x + circle[i][0]);
                if (v > center + t) ++bright;
                else if (v < center - t) ++dark;
            }
            if (bright < 3 && dark < 3) continue;

            // 全 16 ピクセルチェック
            bright = 0; dark = 0;
            int maxConsecBright = 0, maxConsecDark = 0;
            int consecB = 0, consecD = 0;
            for (int i = 0; i < 16 + 9; ++i) {
                int idx = i % 16;
                T v = gray(y + circle[idx][1], x + circle[idx][0]);
                if (v > center + t) { ++consecB; consecD = 0; }
                else if (v < center - t) { ++consecD; consecB = 0; }
                else { consecB = 0; consecD = 0; }
                maxConsecBright = std::max(maxConsecBright, consecB);
                maxConsecDark = std::max(maxConsecDark, consecD);
            }
            if (maxConsecBright < 9 && maxConsecDark < 9) continue;

            // 応答強度 (中心との差の合計)
            float resp = 0;
            for (int i = 0; i < 16; ++i) {
                T v = gray(y + circle[i][1], x + circle[i][0]);
                resp += std::abs(float(v) - float(center));
            }

            kps.push_back({float(x), float(y), 1.0f, 0, resp, 0});
        }
    }

    // NMS (非最大値抑制)
    std::sort(kps.begin(), kps.end(), [](const auto& a, const auto& b) {
        return a.response > b.response;
    });

    std::vector<KeyPoint> filtered;
    std::vector<bool> suppress(kps.size(), false);
    for (std::size_t i = 0; i < kps.size(); ++i) {
        if (suppress[i]) continue;
        filtered.push_back(kps[i]);
        for (std::size_t j = i + 1; j < kps.size(); ++j) {
            float dx = kps[i].x - kps[j].x, dy = kps[i].y - kps[j].y;
            if (dx*dx + dy*dy < 9.0f) suppress[j] = true;
        }
    }
    return filtered;
}

/// BRISK 記述子計算 (簡易: 短距離ペアの強度比較)
template<typename T>
std::vector<BinaryDescriptor> computeBRISKDescriptors(
    const Image<T>& img, const std::vector<KeyPoint>& kps)
{
    Image<T> gray = (img.numChannels() >= 3) ? img.toGray() : img;
    int h = static_cast<int>(gray.height());
    int w = static_cast<int>(gray.width());

    // サンプリングパターン (同心円上の点ペア)
    static const int nPairs = 256;
    struct Pair { int dx1, dy1, dx2, dy2; };
    std::vector<Pair> pairs(nPairs);
    for (int i = 0; i < nPairs; ++i) {
        double a1 = double(i) * 6.283 / nPairs;
        double a2 = a1 + 1.0 + double(i % 5) * 0.5;
        int r1 = 2 + (i % 4), r2 = 2 + ((i + 7) % 4);
        pairs[i] = {int(r1 * std::cos(a1)), int(r1 * std::sin(a1)),
                     int(r2 * std::cos(a2)), int(r2 * std::sin(a2))};
    }

    std::vector<BinaryDescriptor> descs;
    descs.reserve(kps.size());

    for (const auto& kp : kps) {
        BinaryDescriptor desc{};
        int cx = int(kp.x), cy = int(kp.y);

        for (int i = 0; i < nPairs; ++i) {
            int x1 = cx + pairs[i].dx1, y1 = cy + pairs[i].dy1;
            int x2 = cx + pairs[i].dx2, y2 = cy + pairs[i].dy2;
            x1 = std::clamp(x1, 0, w-1); y1 = std::clamp(y1, 0, h-1);
            x2 = std::clamp(x2, 0, w-1); y2 = std::clamp(y2, 0, h-1);

            if (gray(y1, x1) > gray(y2, x2))
                desc[i / 8] |= (1 << (i % 8));
        }
        descs.push_back(desc);
    }
    return descs;
}

/// バイナリ記述子のハミング距離
inline int descriptorDistance(const BinaryDescriptor& a, const BinaryDescriptor& b) {
    int dist = 0;
    for (int i = 0; i < 32; ++i) {
        uint8_t x = a[i] ^ b[i];
        while (x) { dist += x & 1; x >>= 1; }
    }
    return dist;
}

// ========================================================================
// FLANN ベースマッチング (OCV-22) — ブルートフォース + KD-tree 簡易版
// ========================================================================

struct DMatch {
    int queryIdx, trainIdx;
    int distance;
};

/// ブルートフォースマッチング (バイナリ記述子)
inline std::vector<DMatch> matchBruteForce(
    const std::vector<BinaryDescriptor>& query,
    const std::vector<BinaryDescriptor>& train)
{
    std::vector<DMatch> matches;
    matches.reserve(query.size());
    for (int i = 0; i < static_cast<int>(query.size()); ++i) {
        int bestJ = -1, bestDist = 257;
        for (int j = 0; j < static_cast<int>(train.size()); ++j) {
            int d = descriptorDistance(query[i], train[j]);
            if (d < bestDist) { bestDist = d; bestJ = j; }
        }
        if (bestJ >= 0)
            matches.push_back({i, bestJ, bestDist});
    }
    return matches;
}

/// Lowe's ratio test (良いマッチのみ残す)
inline std::vector<DMatch> ratioTest(
    const std::vector<BinaryDescriptor>& query,
    const std::vector<BinaryDescriptor>& train,
    double ratio = 0.75)
{
    std::vector<DMatch> good;
    for (int i = 0; i < static_cast<int>(query.size()); ++i) {
        int best1 = -1, best2 = -1, d1 = 257, d2 = 257;
        for (int j = 0; j < static_cast<int>(train.size()); ++j) {
            int d = descriptorDistance(query[i], train[j]);
            if (d < d1) { d2 = d1; best2 = best1; d1 = d; best1 = j; }
            else if (d < d2) { d2 = d; best2 = j; }
        }
        if (best1 >= 0 && d2 > 0 && double(d1) / d2 < ratio)
            good.push_back({i, best1, d1});
    }
    return good;
}

} // namespace calx::image

#endif
