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

/**
 * @file camshift.hpp
 * @brief CamShift / MeanShift 追跡 (OCV-15)
 */

#ifndef CALX_IMAGE_CV_CAMSHIFT_HPP
#define CALX_IMAGE_CV_CAMSHIFT_HPP

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

namespace calx::image {

struct TrackRect {
    int x, y, width, height;
    double angle;  // CamShift の回転角 (MeanShift では 0)
};

/// MeanShift 追跡
/// backProj: バックプロジェクション画像 (確率マップ)
/// window: 初期探索窓 (更新される)
/// maxIter: 最大反復回数, eps: 収束判定 (移動量)
template<typename T>
int meanShift(const Image<T>& backProj, TrackRect& window,
               int maxIter = 10, double eps = 1.0) {
    int iter = 0;
    for (; iter < maxIter; ++iter) {
        // 窓内のモーメント計算
        double m00 = 0, m10 = 0, m01 = 0;
        int x0 = std::max(0, window.x);
        int y0 = std::max(0, window.y);
        int x1 = std::min(static_cast<int>(backProj.width()) - 1, window.x + window.width - 1);
        int y1 = std::min(static_cast<int>(backProj.height()) - 1, window.y + window.height - 1);

        for (int y = y0; y <= y1; ++y)
            for (int x = x0; x <= x1; ++x) {
                double v = double(backProj(y, x));
                m00 += v;
                m10 += x * v;
                m01 += y * v;
            }

        if (m00 < 1e-6) break;

        int newX = static_cast<int>(m10 / m00 - window.width * 0.5);
        int newY = static_cast<int>(m01 / m00 - window.height * 0.5);
        double dx = newX - window.x, dy = newY - window.y;
        window.x = newX;
        window.y = newY;

        if (dx * dx + dy * dy < eps * eps) break;
    }
    return iter;
}

/// CamShift 追跡 (MeanShift + 窓サイズ・回転の適応)
template<typename T>
int camShift(const Image<T>& backProj, TrackRect& window,
              int maxIter = 10, double eps = 1.0) {
    int iter = meanShift(backProj, window, maxIter, eps);

    // 2次モーメントから回転角とサイズを推定
    double m00 = 0, m10 = 0, m01 = 0, m20 = 0, m11 = 0, m02 = 0;
    int x0 = std::max(0, window.x);
    int y0 = std::max(0, window.y);
    int x1 = std::min(static_cast<int>(backProj.width()) - 1, window.x + window.width - 1);
    int y1 = std::min(static_cast<int>(backProj.height()) - 1, window.y + window.height - 1);

    for (int y = y0; y <= y1; ++y)
        for (int x = x0; x <= x1; ++x) {
            double v = double(backProj(y, x));
            m00 += v; m10 += x*v; m01 += y*v;
            m20 += x*x*v; m11 += x*y*v; m02 += y*y*v;
        }

    if (m00 > 1e-6) {
        double cx = m10/m00, cy = m01/m00;
        double mu20 = m20/m00 - cx*cx;
        double mu11 = m11/m00 - cx*cy;
        double mu02 = m02/m00 - cy*cy;

        window.angle = 0.5 * std::atan2(2*mu11, mu20 - mu02);

        double cs = std::cos(window.angle), sn = std::sin(window.angle);
        double a = mu20*cs*cs + 2*mu11*cs*sn + mu02*sn*sn;
        double b = mu20*sn*sn - 2*mu11*cs*sn + mu02*cs*cs;
        window.width = static_cast<int>(2 * std::sqrt(std::max(a, 0.0)) * 2 + 0.5);
        window.height = static_cast<int>(2 * std::sqrt(std::max(b, 0.0)) * 2 + 0.5);
        window.width = std::max(window.width, 1);
        window.height = std::max(window.height, 1);
    }
    return iter;
}

} // namespace calx::image

#endif
