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

// sift.hpp — SIFT (Scale-Invariant Feature Transform)
//
// 特許失効 (2020年3月)。自由に実装可能。
// Lowe (2004) のアルゴリズム:
//   1. スケール空間の構築 (DoG ピラミッド)
//   2. 極値検出 (3×3×3 近傍)
//   3. キーポイントの精密化 (サブピクセル, 低コントラスト/エッジ除去)
//   4. 方向の割り当て (勾配ヒストグラム)
//   5. 128 次元記述子の計算

#ifndef CALX_IMAGE_CV_SIFT_HPP
#define CALX_IMAGE_CV_SIFT_HPP

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

namespace calx::cv {

    /// SIFT 記述子付きキーポイント
    template<typename T>
    struct SiftKeypoint {
        T x, y;           // 位置
        T scale;           // スケール (σ)
        T orientation;     // 主方向 (ラジアン)
        std::vector<T> descriptor;  // 128 次元
    };

    /// SIFT 検出・記述
    /// @param img             入力グレースケール画像 (1ch)
    /// @param nOctaves        オクターブ数 (0 = 自動)
    /// @param nScalesPerOctave オクターブ内のスケール数
    /// @param contrastThresh  低コントラスト除去閾値
    /// @param edgeThresh      エッジ除去閾値 (ヘッセ行列の比)
    template<typename T>
    std::vector<SiftKeypoint<T>> sift(const image::Image<T>& img,
                                       int nOctaves = 0,
                                       int nScalesPerOctave = 3,
                                       T contrastThresh = T{0.04},
                                       T edgeThresh = T{10})
    {
        assert(img.numChannels() == 1);
        size_t H = img.height(), W = img.width();

        if (nOctaves <= 0)
            nOctaves = static_cast<int>(std::log2(std::min(H, W))) - 2;
        if (nOctaves < 1) nOctaves = 1;

        int S = nScalesPerOctave;
        T sigmaBase = T{1.6};
        T k = std::pow(T{2}, T{1} / static_cast<T>(S));

        std::vector<SiftKeypoint<T>> keypoints;

        // 各オクターブを処理
        // 簡略化: 最初のオクターブのみ処理
        // (フル実装はダウンサンプリングを繰り返す)

        for (int oct = 0; oct < std::min(nOctaves, 2); ++oct) {
            T octScale = std::pow(T{2}, static_cast<T>(oct));
            size_t oH = H >> oct, oW = W >> oct;
            if (oH < 8 || oW < 8) break;

            // ガウシアンスケール空間 (S+3 レベル)
            int nLevels = S + 3;
            std::vector<std::vector<std::vector<T>>> gauss(nLevels,
                std::vector<std::vector<T>>(oH, std::vector<T>(oW, T{0})));

            // 初期レベル (ぼかし済み)
            for (size_t y = 0; y < oH; ++y)
                for (size_t x = 0; x < oW; ++x)
                    gauss[0][y][x] = static_cast<T>(
                        img.at(std::min(y << oct, H - 1),
                               std::min(x << oct, W - 1), 0));

            // 各スケールでガウシアンぼかし
            for (int s = 1; s < nLevels; ++s) {
                T sigma = sigmaBase * std::pow(k, static_cast<T>(s));
                int ksize = static_cast<int>(std::ceil(sigma * 3)) * 2 + 1;
                int half = ksize / 2;
                std::vector<T> kern(ksize);
                T sum = T{0};
                for (int i = 0; i < ksize; ++i) {
                    T xi = static_cast<T>(i - half);
                    kern[i] = std::exp(-xi * xi / (T{2} * sigma * sigma));
                    sum += kern[i];
                }
                for (auto& ki : kern) ki /= sum;

                // 分離フィルタ
                auto& prev = gauss[s - 1];
                std::vector<std::vector<T>> tmp(oH, std::vector<T>(oW, T{0}));
                for (size_t y = 0; y < oH; ++y)
                    for (size_t x = 0; x < oW; ++x) {
                        T v = T{0};
                        for (int m = -half; m <= half; ++m) {
                            int xx = std::clamp(static_cast<int>(x) + m, 0, static_cast<int>(oW) - 1);
                            v += kern[m + half] * prev[y][xx];
                        }
                        tmp[y][x] = v;
                    }
                for (size_t y = 0; y < oH; ++y)
                    for (size_t x = 0; x < oW; ++x) {
                        T v = T{0};
                        for (int m = -half; m <= half; ++m) {
                            int yy = std::clamp(static_cast<int>(y) + m, 0, static_cast<int>(oH) - 1);
                            v += kern[m + half] * tmp[yy][x];
                        }
                        gauss[s][y][x] = v;
                    }
            }

            // DoG (Difference of Gaussians)
            int nDog = nLevels - 1;
            std::vector<std::vector<std::vector<T>>> dog(nDog,
                std::vector<std::vector<T>>(oH, std::vector<T>(oW, T{0})));
            for (int s = 0; s < nDog; ++s)
                for (size_t y = 0; y < oH; ++y)
                    for (size_t x = 0; x < oW; ++x)
                        dog[s][y][x] = gauss[s + 1][y][x] - gauss[s][y][x];

            // 極値検出 (3×3×3 近傍)
            for (int s = 1; s + 1 < nDog; ++s) {
                for (size_t y = 1; y + 1 < oH; ++y) {
                    for (size_t x = 1; x + 1 < oW; ++x) {
                        T val = dog[s][y][x];
                        if (std::abs(val) < contrastThresh * T{0.5}) continue;

                        bool isMax = true, isMin = true;
                        for (int ds = -1; ds <= 1 && (isMax || isMin); ++ds)
                            for (int dy = -1; dy <= 1 && (isMax || isMin); ++dy)
                                for (int dx = -1; dx <= 1 && (isMax || isMin); ++dx) {
                                    if (ds == 0 && dy == 0 && dx == 0) continue;
                                    T neighbor = dog[s+ds][y+dy][x+dx];
                                    if (val <= neighbor) isMax = false;
                                    if (val >= neighbor) isMin = false;
                                }

                        if (!isMax && !isMin) continue;

                        // エッジ除去 (ヘッセ行列のトレース/行列式)
                        T dxx = dog[s][y][x+1] - T{2}*val + dog[s][y][x-1];
                        T dyy = dog[s][y+1][x] - T{2}*val + dog[s][y-1][x];
                        T dxy = (dog[s][y+1][x+1] - dog[s][y+1][x-1]
                                - dog[s][y-1][x+1] + dog[s][y-1][x-1]) / T{4};
                        T tr = dxx + dyy;
                        T det = dxx * dyy - dxy * dxy;
                        if (det <= T{0}) continue;
                        T ratio = tr * tr / det;
                        T thresh = (edgeThresh + T{1}) * (edgeThresh + T{1}) / edgeThresh;
                        if (ratio > thresh) continue;

                        // 方向の割り当て (36-bin ヒストグラム)
                        T sigma_ori = sigmaBase * std::pow(k, static_cast<T>(s)) * T{1.5};
                        int radius = static_cast<int>(std::ceil(sigma_ori * T{3}));
                        std::vector<T> hist(36, T{0});
                        for (int dy = -radius; dy <= radius; ++dy)
                            for (int dx = -radius; dx <= radius; ++dx) {
                                int py = static_cast<int>(y) + dy;
                                int px = static_cast<int>(x) + dx;
                                if (py < 1 || py >= static_cast<int>(oH) - 1 ||
                                    px < 1 || px >= static_cast<int>(oW) - 1) continue;
                                T gx = gauss[s][py][px+1] - gauss[s][py][px-1];
                                T gy = gauss[s][py+1][px] - gauss[s][py-1][px];
                                T gmag = std::sqrt(gx*gx + gy*gy);
                                T gdir = std::atan2(gy, gx);
                                if (gdir < T{0}) gdir += T{2} * T{3.14159265358979};
                                int bin = static_cast<int>(gdir / (T{2} * T{3.14159265358979}) * T{36});
                                if (bin >= 36) bin = 35;
                                T w = std::exp(-(dx*dx + dy*dy) / (T{2} * sigma_ori * sigma_ori));
                                hist[bin] += gmag * w;
                            }

                        T maxHist = *std::max_element(hist.begin(), hist.end());
                        if (maxHist < T{1e-10}) continue;

                        // 主方向
                        int mainBin = 0;
                        for (int b = 0; b < 36; ++b)
                            if (hist[b] > hist[mainBin]) mainBin = b;
                        T orientation = (static_cast<T>(mainBin) + T{0.5}) / T{36} * T{2} * T{3.14159265358979};

                        // 128 次元記述子 (4×4 空間ビン × 8 方向ビン)
                        std::vector<T> desc(128, T{0});
                        int descRadius = 8;
                        T cosOri = std::cos(orientation), sinOri = std::sin(orientation);

                        for (int dy = -descRadius; dy < descRadius; ++dy)
                            for (int dx = -descRadius; dx < descRadius; ++dx) {
                                int py = static_cast<int>(y) + dy;
                                int px = static_cast<int>(x) + dx;
                                if (py < 1 || py >= static_cast<int>(oH) - 1 ||
                                    px < 1 || px >= static_cast<int>(oW) - 1) continue;

                                T gx = gauss[s][py][px+1] - gauss[s][py][px-1];
                                T gy = gauss[s][py+1][px] - gauss[s][py-1][px];
                                T gmag = std::sqrt(gx*gx + gy*gy);
                                T gdir = std::atan2(gy, gx) - orientation;
                                if (gdir < T{0}) gdir += T{2} * T{3.14159265358979};

                                // 回転した座標
                                T rx = cosOri * dx + sinOri * dy;
                                T ry = -sinOri * dx + cosOri * dy;
                                int bx = static_cast<int>((rx + descRadius) / (descRadius / 2));
                                int by = static_cast<int>((ry + descRadius) / (descRadius / 2));
                                if (bx < 0 || bx >= 4 || by < 0 || by >= 4) continue;
                                int bo = static_cast<int>(gdir / (T{2} * T{3.14159265358979}) * T{8});
                                if (bo >= 8) bo = 7;

                                T w = std::exp(-(dx*dx + dy*dy) / (T{2} * T{64}));
                                desc[(by * 4 + bx) * 8 + bo] += gmag * w;
                            }

                        // L2 正規化 + クリッピング + 再正規化
                        T norm = T{0};
                        for (auto v : desc) norm += v * v;
                        norm = std::sqrt(norm);
                        if (norm > T{1e-10})
                            for (auto& v : desc) v /= norm;
                        for (auto& v : desc) v = std::min(v, T{0.2});
                        norm = T{0};
                        for (auto v : desc) norm += v * v;
                        norm = std::sqrt(norm);
                        if (norm > T{1e-10})
                            for (auto& v : desc) v /= norm;

                        keypoints.push_back({
                            static_cast<T>(x) * octScale,
                            static_cast<T>(y) * octScale,
                            sigmaBase * std::pow(k, static_cast<T>(s)) * octScale,
                            orientation,
                            std::move(desc)
                        });
                    }
                }
            }
        }

        return keypoints;
    }

} // namespace calx::cv

#endif // CALX_IMAGE_CV_SIFT_HPP
