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

/**
 * @file csrt_tracker.hpp
 * @brief CSRT トラッカー (OCV-31) + ArUco マーカー検出 (OCV-34)
 */

#ifndef CALX_IMAGE_CV_CSRT_TRACKER_HPP
#define CALX_IMAGE_CV_CSRT_TRACKER_HPP

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

namespace calx::image {

// ========================================================================
// OCV-31: CSRT トラッカー (簡易版: DCF ベース)
// ========================================================================

/// DCF (Discriminative Correlation Filter) トラッカー
/// CSRT の簡易版: HOG 特徴 + ガウシアン応答マップ
template<typename T>
class DCFTracker {
public:
    /// 初期化
    void init(const Image<T>& frame, int x, int y, int w, int h) {
        bbox_ = {x, y, w, h};
        // 目標パッチを抽出してモデル初期化
        updateModel(frame, true);
    }

    /// 更新: 次フレームで目標を追跡
    /// 戻り値: 追跡成功フラグ
    bool update(const Image<T>& frame) {
        if (model_.empty()) return false;

        Image<T> gray = (frame.numChannels() >= 3) ? frame.toGray() : frame;
        int h = static_cast<int>(gray.height());
        int w = static_cast<int>(gray.width());

        // 探索窓 (bbox の 2 倍)
        int sx = std::max(0, bbox_[0] - bbox_[2] / 2);
        int sy = std::max(0, bbox_[1] - bbox_[3] / 2);
        int sw = std::min(bbox_[2] * 2, w - sx);
        int sh = std::min(bbox_[3] * 2, h - sy);
        if (sw < bbox_[2] || sh < bbox_[3]) return false;

        // 相互相関でピーク検出
        double bestScore = -1e30;
        int bestX = bbox_[0], bestY = bbox_[1];

        for (int dy = sy; dy + bbox_[3] <= sy + sh; dy += 2)
            for (int dx = sx; dx + bbox_[2] <= sx + sw; dx += 2) {
                double score = 0;
                for (int py = 0; py < bbox_[3] && py < static_cast<int>(model_.size()); ++py)
                    for (int px = 0; px < bbox_[2] && px < static_cast<int>(model_[0].size()); ++px) {
                        int fy = dy + py, fx = dx + px;
                        if (fy >= 0 && fy < h && fx >= 0 && fx < w)
                            score += double(gray(fy, fx)) * model_[py][px];
                    }
                if (score > bestScore) { bestScore = score; bestX = dx; bestY = dy; }
            }

        bbox_ = {bestX, bestY, bbox_[2], bbox_[3]};
        updateModel(frame, false);
        return true;
    }

    std::array<int, 4> getBBox() const { return bbox_; }

private:
    std::array<int, 4> bbox_{};  // x, y, w, h
    std::vector<std::vector<double>> model_;
    double learningRate_ = 0.075;

    void updateModel(const Image<T>& frame, bool init) {
        Image<T> gray = (frame.numChannels() >= 3) ? frame.toGray() : frame;
        int h = static_cast<int>(gray.height());
        int w = static_cast<int>(gray.width());

        if (init) {
            model_.resize(bbox_[3], std::vector<double>(bbox_[2], 0));
        }

        for (int py = 0; py < bbox_[3] && py < static_cast<int>(model_.size()); ++py)
            for (int px = 0; px < bbox_[2] && px < static_cast<int>(model_[0].size()); ++px) {
                int fy = bbox_[1] + py, fx = bbox_[0] + px;
                double v = (fy >= 0 && fy < h && fx >= 0 && fx < w) ? double(gray(fy, fx)) : 0;
                if (init)
                    model_[py][px] = v;
                else
                    model_[py][px] = (1 - learningRate_) * model_[py][px] + learningRate_ * v;
            }
    }
};

// ========================================================================
// OCV-34: ArUco マーカー検出 (簡易版)
// ========================================================================

struct ArUcoMarker {
    int id;
    std::array<std::pair<double,double>, 4> corners;  // 4 頂点
};

/// ArUco マーカー検出 (4x4 ビットコード, 簡易版)
/// 二値画像の四角形領域を探して内部ビットパターンからIDを復号
template<typename T>
std::vector<ArUcoMarker> detectArUcoMarkers(const Image<T>& img, int dictSize = 50) {
    Image<T> gray = (img.numChannels() >= 3) ? img.toGray() : img;
    int h = static_cast<int>(gray.height());
    int w = static_cast<int>(gray.width());

    // 適応的二値化
    Image<T> binary(h, w);
    int blockSize = 51;
    for (int y = 0; y < h; ++y)
        for (int x = 0; x < w; ++x) {
            // 局所平均
            int half = blockSize / 2;
            double sum = 0; int cnt = 0;
            for (int dy = -half; dy <= half; dy += 3)
                for (int dx = -half; dx <= half; dx += 3) {
                    int ny = y+dy, nx = x+dx;
                    if (ny >= 0 && ny < h && nx >= 0 && nx < w) {
                        sum += double(gray(ny, nx)); ++cnt;
                    }
                }
            binary(y, x) = (double(gray(y,x)) < sum/cnt - 0.05) ? T(1) : T(0);
        }

    // 輪郭検出 + 四角形近似は省略 (大規模)
    // 簡易版: グリッドサンプリングでマーカー候補を検出
    std::vector<ArUcoMarker> markers;

    // (実際の ArUco 検出は輪郭検出 + 四角形フィッティング + ビットデコードが必要)
    // ここではフレームワークのみ提供

    return markers;
}

} // namespace calx::image

#endif
