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

/**
 * @file line_segment_detector.hpp
 * @brief LSD 線分検出 (OCV-32: Line Segment Detector)
 *
 * 勾配方向に基づく線分検出。Grompone von Gioi et al. (2010)
 * Hough 変換より高精度で、パラメータチューニングが不要。
 */

#ifndef CALX_IMAGE_IMGPROC_LINE_SEGMENT_DETECTOR_HPP
#define CALX_IMAGE_IMGPROC_LINE_SEGMENT_DETECTOR_HPP

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

namespace calx::image {

struct LineSegment {
    double x1, y1, x2, y2;
    double width;   // 線分の幅
    double angle;   // 方向角 [rad]
    double nfa;     // NFA (Number of False Alarms)
};

/// LSD 線分検出 (簡易版)
template<typename T>
std::vector<LineSegment> detectLineSegments(
    const Image<T>& img, double scale = 0.8, double angleTolerance = 22.5)
{
    int h = static_cast<int>(img.height());
    int w = static_cast<int>(img.width());
    constexpr double pi = 3.14159265358979323846;
    double tolRad = angleTolerance * pi / 180.0;

    // 1. ガウシアンぼかし + 縮小 (scale < 1 の場合)
    Image<T> gray = (img.numChannels() >= 3) ? img.toGray() : img;

    // 2. 勾配の大きさと方向
    std::vector<double> mag(h * w), angle(h * w);
    std::vector<bool> used(h * w, false);

    for (int y = 1; y < h - 1; ++y)
        for (int x = 1; x < w - 1; ++x) {
            double gx = double(gray(y, x+1)) - double(gray(y, x-1));
            double gy = double(gray(y+1, x)) - double(gray(y-1, x));
            mag[y * w + x] = std::sqrt(gx * gx + gy * gy);
            angle[y * w + x] = std::atan2(gy, gx);
        }

    // 3. 勾配強度でソート (降順)
    std::vector<int> order(h * w);
    std::iota(order.begin(), order.end(), 0);
    std::sort(order.begin(), order.end(), [&](int a, int b) {
        return mag[a] > mag[b];
    });

    std::vector<LineSegment> result;

    // 4. 領域成長法で線分を検出
    double minMag = 0.01;  // 最小勾配閾値

    for (int idx : order) {
        if (mag[idx] < minMag) break;
        if (used[idx]) continue;

        int sy = idx / w, sx = idx % w;
        if (sy <= 0 || sy >= h-1 || sx <= 0 || sx >= w-1) continue;

        double seedAngle = angle[idx];

        // 領域成長: シードから同方向のピクセルを収集
        std::vector<std::pair<int,int>> region;
        std::vector<std::pair<int,int>> stack = {{sy, sx}};

        while (!stack.empty()) {
            auto [cy, cx] = stack.back(); stack.pop_back();
            if (cy < 0 || cy >= h || cx < 0 || cx >= w) continue;
            int ci = cy * w + cx;
            if (used[ci] || mag[ci] < minMag) continue;

            // 方向の一致チェック
            double da = angle[ci] - seedAngle;
            while (da > pi) da -= 2*pi;
            while (da < -pi) da += 2*pi;
            if (std::abs(da) > tolRad && std::abs(da - pi) > tolRad && std::abs(da + pi) > tolRad)
                continue;

            used[ci] = true;
            region.push_back({cy, cx});

            // 4近傍に拡張
            stack.push_back({cy-1, cx}); stack.push_back({cy+1, cx});
            stack.push_back({cy, cx-1}); stack.push_back({cy, cx+1});
        }

        if (region.size() < 5) continue;  // 短すぎる

        // 領域から線分パラメータを推定 (PCA)
        double mx = 0, my = 0;
        for (auto [ry, rx] : region) { mx += rx; my += ry; }
        mx /= region.size(); my /= region.size();

        double cxx = 0, cyy = 0, cxy = 0;
        for (auto [ry, rx] : region) {
            double dx = rx - mx, dy = ry - my;
            cxx += dx*dx; cyy += dy*dy; cxy += dx*dy;
        }

        double theta = 0.5 * std::atan2(2 * cxy, cxx - cyy);
        double cosT = std::cos(theta), sinT = std::sin(theta);

        // 投影して端点を求める
        double tMin = 1e9, tMax = -1e9;
        for (auto [ry, rx] : region) {
            double t = (rx - mx) * cosT + (ry - my) * sinT;
            tMin = std::min(tMin, t);
            tMax = std::max(tMax, t);
        }

        LineSegment seg;
        seg.x1 = mx + tMin * cosT;
        seg.y1 = my + tMin * sinT;
        seg.x2 = mx + tMax * cosT;
        seg.y2 = my + tMax * sinT;
        seg.angle = theta;
        seg.width = 1.0;
        seg.nfa = 0;
        result.push_back(seg);
    }

    return result;
}

} // namespace calx::image

#endif
