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

// hog.hpp — HOG 特徴量 (Histogram of Oriented Gradients)
//
// 新規実装:
//   - 勾配方向ヒストグラム
//   - セル・ブロック正規化
//   - 検出ウィンドウ用記述子

#ifndef CALX_IMAGE_CV_HOG_HPP
#define CALX_IMAGE_CV_HOG_HPP

#include <image/Image.hpp>
#include <cmath>
#include <vector>

namespace calx::cv {

    /// HOG 特徴量計算器
    class HOGDescriptor {
    public:
        /// @param cellSize    セルのサイズ (ピクセル)
        /// @param blockSize   ブロック内のセル数
        /// @param numBins     方向のビン数
        HOGDescriptor(int cellSize = 8, int blockSize = 2, int numBins = 9)
            : cellSize_(cellSize), blockSize_(blockSize), numBins_(numBins) {}

        /// HOG 特徴量を計算
        /// @param img  入力画像 (1ch, グレースケール)
        /// @return     HOG 記述子ベクトル
        template<typename T>
        std::vector<double> compute(const image::Image<T>& img) const {
            int H = static_cast<int>(img.height());
            int W = static_cast<int>(img.width());

            // 勾配計算
            std::vector<double> magnitude(H * W, 0.0);
            std::vector<double> orientation(H * W, 0.0);

            for (int y = 1; y < H - 1; ++y) {
                for (int x = 1; x < W - 1; ++x) {
                    double gx = static_cast<double>(img(y, x + 1)) - static_cast<double>(img(y, x - 1));
                    double gy = static_cast<double>(img(y + 1, x)) - static_cast<double>(img(y - 1, x));
                    magnitude[y * W + x] = std::sqrt(gx * gx + gy * gy);
                    double angle = std::atan2(gy, gx) * 180.0 / 3.14159265358979323846;
                    if (angle < 0) angle += 180.0;  // [0, 180) に正規化
                    orientation[y * W + x] = angle;
                }
            }

            // セル内ヒストグラム
            int cellsH = H / cellSize_;
            int cellsW = W / cellSize_;
            if (cellsH < 1 || cellsW < 1) return {};

            std::vector<std::vector<double>> cellHist(cellsH * cellsW, std::vector<double>(numBins_, 0.0));

            double binWidth = 180.0 / numBins_;
            for (int cy = 0; cy < cellsH; ++cy) {
                for (int cx = 0; cx < cellsW; ++cx) {
                    auto& hist = cellHist[cy * cellsW + cx];
                    for (int dy = 0; dy < cellSize_; ++dy) {
                        for (int dx = 0; dx < cellSize_; ++dx) {
                            int py = cy * cellSize_ + dy;
                            int px = cx * cellSize_ + dx;
                            if (py >= H || px >= W) continue;

                            double mag = magnitude[py * W + px];
                            double ori = orientation[py * W + px];
                            int bin = static_cast<int>(ori / binWidth) % numBins_;
                            hist[bin] += mag;
                        }
                    }
                }
            }

            // ブロック正規化 (L2-norm)
            std::vector<double> descriptor;
            int blocksH = cellsH - blockSize_ + 1;
            int blocksW = cellsW - blockSize_ + 1;

            for (int by = 0; by < blocksH; ++by) {
                for (int bx = 0; bx < blocksW; ++bx) {
                    // ブロック内の全セルを連結
                    std::vector<double> blockVec;
                    for (int cy = by; cy < by + blockSize_; ++cy) {
                        for (int cx = bx; cx < bx + blockSize_; ++cx) {
                            const auto& hist = cellHist[cy * cellsW + cx];
                            blockVec.insert(blockVec.end(), hist.begin(), hist.end());
                        }
                    }

                    // L2 正規化
                    double norm = 0;
                    for (double v : blockVec) norm += v * v;
                    norm = std::sqrt(norm + 1e-6);
                    for (double& v : blockVec) v /= norm;

                    descriptor.insert(descriptor.end(), blockVec.begin(), blockVec.end());
                }
            }

            return descriptor;
        }

        /// HOG を可視化 (勾配方向を線分で描画)
        template<typename T>
        image::Image<T> visualize(const image::Image<T>& img) const {
            int H = static_cast<int>(img.height());
            int W = static_cast<int>(img.width());
            auto canvas = img.toRgb();

            int cellsH = H / cellSize_;
            int cellsW = W / cellSize_;

            for (int cy = 0; cy < cellsH; ++cy) {
                for (int cx = 0; cx < cellsW; ++cx) {
                    // セルの勾配を計算
                    double totalGx = 0, totalGy = 0;
                    for (int dy = 0; dy < cellSize_; ++dy) {
                        for (int dx = 0; dx < cellSize_; ++dx) {
                            int py = cy * cellSize_ + dy;
                            int px = cx * cellSize_ + dx;
                            if (py < 1 || py >= H - 1 || px < 1 || px >= W - 1) continue;
                            totalGx += static_cast<double>(img(py, px + 1)) - static_cast<double>(img(py, px - 1));
                            totalGy += static_cast<double>(img(py + 1, px)) - static_cast<double>(img(py - 1, px));
                        }
                    }

                    double mag = std::sqrt(totalGx * totalGx + totalGy * totalGy);
                    if (mag < 0.01) continue;

                    int ccx = cx * cellSize_ + cellSize_ / 2;
                    int ccy = cy * cellSize_ + cellSize_ / 2;
                    int len = cellSize_ / 2;
                    int ex = ccx + static_cast<int>(len * totalGx / mag);
                    int ey = ccy + static_cast<int>(len * totalGy / mag);

                    T maxVal = std::is_floating_point_v<T> ? T{1} : T{255};
                    canvas.drawLine(ccx, ccy, ex, ey, {T{0}, maxVal, T{0}}, 1);
                }
            }

            return canvas;
        }

    private:
        int cellSize_, blockSize_, numBins_;
    };

} // namespace calx::cv

#endif // CALX_IMAGE_CV_HOG_HPP
