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

/**
 * @file stereo.hpp
 * @brief ステレオ校正・平行化・視差マップ (OCV-17, OCV-18)
 */

#ifndef CALX_IMAGE_CV_STEREO_HPP
#define CALX_IMAGE_CV_STEREO_HPP

#include <image/Image.hpp>
#include <math/core/matrix.hpp>
#include <vector>
#include <cmath>
#include <algorithm>
#include <array>

namespace calx::image {

/// ステレオマッチング (Block Matching)
/// 左右画像から視差マップ (disparity map) を計算
template<typename T>
Image<T> stereoBlockMatch(const Image<T>& left, const Image<T>& right,
                           int blockSize = 15, int numDisparities = 64) {
    Image<T> leftG = (left.numChannels() >= 3) ? left.toGray() : left;
    Image<T> rightG = (right.numChannels() >= 3) ? right.toGray() : right;

    int h = static_cast<int>(leftG.height());
    int w = static_cast<int>(leftG.width());
    int half = blockSize / 2;

    Image<T> disparity(h, w, 1, T(0));

    for (int y = half; y < h - half; ++y) {
        for (int x = numDisparities + half; x < w - half; ++x) {
            T bestCost = T(1e9);
            int bestD = 0;

            for (int d = 0; d < numDisparities; ++d) {
                T cost = T(0);
                for (int by = -half; by <= half; ++by)
                    for (int bx = -half; bx <= half; ++bx) {
                        T diff = leftG(y + by, x + bx) - rightG(y + by, x + bx - d);
                        cost += diff * diff;
                    }
                if (cost < bestCost) { bestCost = cost; bestD = d; }
            }
            disparity(y, x) = T(bestD) / T(numDisparities);
        }
    }
    return disparity;
}

/// Fisheye カメラモデル (OCV-18)
/// 等距離射影: r = f * θ (θ = 入射角)
struct FisheyeParams {
    double fx, fy;       // 焦点距離
    double cx, cy;       // 主点
    double k1, k2, k3, k4;  // 歪み係数
};

/// Fisheye 歪み補正 (undistort)
template<typename T>
Image<T> fisheyeUndistort(const Image<T>& img, const FisheyeParams& params) {
    int h = static_cast<int>(img.height());
    int w = static_cast<int>(img.width());
    int ch = img.numChannels();
    Image<T> result(h, w, ch, T(0));

    for (int y = 0; y < h; ++y)
        for (int x = 0; x < w; ++x) {
            // 正規化座標
            double xn = (x - params.cx) / params.fx;
            double yn = (y - params.cy) / params.fy;
            double r = std::sqrt(xn*xn + yn*yn);
            double theta = std::atan(r);

            // 歪みモデル: θ_d = θ(1 + k1*θ² + k2*θ⁴ + k3*θ⁶ + k4*θ⁸)
            double t2 = theta*theta;
            double td = theta * (1 + params.k1*t2 + params.k2*t2*t2
                                   + params.k3*t2*t2*t2 + params.k4*t2*t2*t2*t2);

            double scale = (r > 1e-8) ? td / r : 1.0;
            int sx = static_cast<int>(params.cx + xn * scale * params.fx + 0.5);
            int sy = static_cast<int>(params.cy + yn * scale * params.fy + 0.5);

            if (sx >= 0 && sx < w && sy >= 0 && sy < h)
                for (int c = 0; c < ch; ++c)
                    result.at(y, x, c) = img.at(sy, sx, c);
        }
    return result;
}

} // namespace calx::image

#endif
