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

// warp.hpp — 幾何変換 (アフィン / 射影)
//
// warpAffine:          2x3 アフィン変換
// warpPerspective:     3x3 射影変換
// getRotationMatrix2D: 回転行列の生成

#ifndef CALX_IMAGE_IMGPROC_WARP_HPP
#define CALX_IMAGE_IMGPROC_WARP_HPP

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

namespace calx::image {

    /// 2x3 アフィン変換
    /// M = [[a00, a01, a02],
    ///      [a10, a11, a12]]
    /// dst(y,x) = src(M^{-1} * [x, y, 1]^T) (bilinear 補間)
    template<typename T>
    Image<T> warpAffine(const Image<T>& src, const Matrix<T>& M,
                        size_t dstH, size_t dstW) {
        assert(M.rows() == 2 && M.cols() == 3);

        // M の逆変換を計算: A = M[:,:2], t = M[:,2]
        // A^{-1} = 1/det * [[a11, -a01], [-a10, a00]]
        T a00 = M(0, 0), a01 = M(0, 1), a02 = M(0, 2);
        T a10 = M(1, 0), a11 = M(1, 1), a12 = M(1, 2);
        T det = a00 * a11 - a01 * a10;
        assert(std::abs(det) > T{1e-12});
        T invDet = T{1} / det;

        // 逆行列の係数
        T ia00 =  a11 * invDet, ia01 = -a01 * invDet;
        T ia10 = -a10 * invDet, ia11 =  a00 * invDet;
        T it0 = -(ia00 * a02 + ia01 * a12);
        T it1 = -(ia10 * a02 + ia11 * a12);

        int srcH = static_cast<int>(src.height());
        int srcW = static_cast<int>(src.width());
        int nch = src.numChannels();
        Image<T> dst(dstH, dstW, nch, T{0});

        for (size_t dy = 0; dy < dstH; ++dy) {
            for (size_t dx = 0; dx < dstW; ++dx) {
                T dxf = static_cast<T>(dx);
                T dyf = static_cast<T>(dy);

                // 逆変換: ソース座標
                T sx = ia00 * dxf + ia01 * dyf + it0;
                T sy = ia10 * dxf + ia11 * dyf + it1;

                // バイリニア補間
                int x0 = static_cast<int>(std::floor(sx));
                int y0 = static_cast<int>(std::floor(sy));
                T fx = sx - static_cast<T>(x0);
                T fy = sy - static_cast<T>(y0);

                if (x0 >= 0 && x0 + 1 < srcW && y0 >= 0 && y0 + 1 < srcH) {
                    for (int c = 0; c < nch; ++c) {
                        T v00 = src.channel(c)(y0,     x0);
                        T v01 = src.channel(c)(y0,     x0 + 1);
                        T v10 = src.channel(c)(y0 + 1, x0);
                        T v11 = src.channel(c)(y0 + 1, x0 + 1);
                        T val = (T{1} - fy) * ((T{1} - fx) * v00 + fx * v01)
                              +        fy  * ((T{1} - fx) * v10 + fx * v11);
                        dst.channel(c)(dy, dx) = val;
                    }
                }
            }
        }
        return dst;
    }

    /// 3x3 射影変換 (ホモグラフィ)
    /// dst(y,x) = src(H^{-1} * [x, y, 1]^T) (bilinear 補間)
    template<typename T>
    Image<T> warpPerspective(const Image<T>& src, const Matrix<T>& M,
                             size_t dstH, size_t dstW) {
        assert(M.rows() == 3 && M.cols() == 3);

        // 3x3 逆行列 (Cramer)
        auto m = [&](int r, int c) { return M(r, c); };
        T det = m(0,0) * (m(1,1)*m(2,2) - m(1,2)*m(2,1))
              - m(0,1) * (m(1,0)*m(2,2) - m(1,2)*m(2,0))
              + m(0,2) * (m(1,0)*m(2,1) - m(1,1)*m(2,0));
        assert(std::abs(det) > T{1e-12});
        T invDet = T{1} / det;

        // 余因子行列の転置 (逆行列)
        T inv[3][3];
        inv[0][0] = (m(1,1)*m(2,2) - m(1,2)*m(2,1)) * invDet;
        inv[0][1] = (m(0,2)*m(2,1) - m(0,1)*m(2,2)) * invDet;
        inv[0][2] = (m(0,1)*m(1,2) - m(0,2)*m(1,1)) * invDet;
        inv[1][0] = (m(1,2)*m(2,0) - m(1,0)*m(2,2)) * invDet;
        inv[1][1] = (m(0,0)*m(2,2) - m(0,2)*m(2,0)) * invDet;
        inv[1][2] = (m(0,2)*m(1,0) - m(0,0)*m(1,2)) * invDet;
        inv[2][0] = (m(1,0)*m(2,1) - m(1,1)*m(2,0)) * invDet;
        inv[2][1] = (m(0,1)*m(2,0) - m(0,0)*m(2,1)) * invDet;
        inv[2][2] = (m(0,0)*m(1,1) - m(0,1)*m(1,0)) * invDet;

        int srcH = static_cast<int>(src.height());
        int srcW = static_cast<int>(src.width());
        int nch = src.numChannels();
        Image<T> dst(dstH, dstW, nch, T{0});

        for (size_t dy = 0; dy < dstH; ++dy) {
            for (size_t dx = 0; dx < dstW; ++dx) {
                T dxf = static_cast<T>(dx);
                T dyf = static_cast<T>(dy);

                // 逆射影変換
                T w = inv[2][0] * dxf + inv[2][1] * dyf + inv[2][2];
                if (std::abs(w) < T{1e-12}) continue;
                T sx = (inv[0][0] * dxf + inv[0][1] * dyf + inv[0][2]) / w;
                T sy = (inv[1][0] * dxf + inv[1][1] * dyf + inv[1][2]) / w;

                // バイリニア補間
                int x0 = static_cast<int>(std::floor(sx));
                int y0 = static_cast<int>(std::floor(sy));
                T fx = sx - static_cast<T>(x0);
                T fy = sy - static_cast<T>(y0);

                if (x0 >= 0 && x0 + 1 < srcW && y0 >= 0 && y0 + 1 < srcH) {
                    for (int c = 0; c < nch; ++c) {
                        T v00 = src.channel(c)(y0,     x0);
                        T v01 = src.channel(c)(y0,     x0 + 1);
                        T v10 = src.channel(c)(y0 + 1, x0);
                        T v11 = src.channel(c)(y0 + 1, x0 + 1);
                        T val = (T{1} - fy) * ((T{1} - fx) * v00 + fx * v01)
                              +        fy  * ((T{1} - fx) * v10 + fx * v11);
                        dst.channel(c)(dy, dx) = val;
                    }
                }
            }
        }
        return dst;
    }

    /// 回転行列を生成 (2x3)
    /// @param cx, cy  回転中心
    /// @param angle   回転角 (ラジアン)
    /// @param scale   スケール
    /// @return        2x3 アフィン変換行列
    template<typename T>
    Matrix<T> getRotationMatrix2D(T cx, T cy, T angle, T scale = T{1}) {
        T cosA = std::cos(angle) * scale;
        T sinA = std::sin(angle) * scale;

        Matrix<T> M(2, 3, T{0});
        M(0, 0) =  cosA;
        M(0, 1) = -sinA;
        M(0, 2) = (T{1} - cosA) * cx + sinA * cy;
        M(1, 0) =  sinA;
        M(1, 1) =  cosA;
        M(1, 2) = -sinA * cx + (T{1} - cosA) * cy;
        return M;
    }

} // namespace calx::image

#endif // CALX_IMAGE_IMGPROC_WARP_HPP
