#include <cmath>
#include <cstdint>
#include <vector>
#include <algorithm>

namespace ipm
{
    // =========================
    // 기초 데이터 구조
    // =========================

    struct Vec3
    {
        double x;
        double y;
        double z;
    };

    struct GroundPoint
    {
        double X;     // 세계 좌표 X (좌우)
        double Y;     // 세계 좌표 Y (전후)
        bool valid;   // 지면과 유효한 교점이 있는지 여부
    };

    struct CameraParam
    {
        // 초점 거리 (픽셀 단위)
        // d 하나만 있다면 fx = fy = d로 설정 가능
        double fx;
        double fy;

        // 주점 (보통 이미지 중심)
        double cx;
        double cy;

        // 카메라 높이, 단위 예: cm
        double H;

        // 카메라 하향 앙각 (라디안)
        double pitch;
    };

    struct IPMParam
    {
        // 출력 조감도 크기
        int outWidth;
        int outHeight;

        // 세계 좌표 범위 (H와 동일한 단위, 예: cm)
        // X: 좌우 범위
        // Y: 전후 범위
        double minX;
        double maxX;
        double minY;
        double maxY;
    };

    // =========================
    // 유틸리티 함수
    // =========================

    inline double clampDouble(double v, double lo, double hi)
    {
        return (v < lo) ? lo : ((v > hi) ? hi : v);
    }

    inline uint8_t clampToByte(double v)
    {
        if (v < 0.0) return 0;
        if (v > 255.0) return 255;
        return static_cast<uint8_t>(v + 0.5);
    }

    // X축 회전: 카메라 좌표계의 방향을 세계 좌표계로 변환
    // 여기서는 다음과 같이 가정
    // - 세계 Z축은 상향
    // - 카메라 광축은 기본적으로 세계 Y 양의 방향
    // - pitch > 0은 카메라가 아래를 내려다보는 것을 의미
    //
    // 이미지 좌표(v가 아래쪽)와 일치시키기 위해 엔지니어링에서 일반적으로 사용하는 매핑 구성:
    //
    // 카메라계 광선 rc = [x, y, 1]
    // 먼저 "피치가 없는" 세계 방향으로 매핑:
    //   x -> Xw
    //   y -> -Zw
    //   z -> Yw
    //
    // 그런 다음 세계 X축을 중심으로 pitch만큼 회전
    //
    inline Vec3 cameraRayToWorldRay(const Vec3& rc, double pitch)
    {
        // 피치가 없는 세계 방향
        // 카메라 오른쪽 -> 세계 오른쪽
        // 카메라 아래 -> 세계 위쪽의 반대
        // 카메라 앞 -> 세계 앞
        const double X0 = rc.x;
        const double Y0 = rc.z;
        const double Z0 = -rc.y;

        const double c = std::cos(pitch);
        const double s = std::sin(pitch);

        // X축을 중심으로 회전
        Vec3 rw;
        rw.x = X0;
        rw.y = c * Y0 - s * Z0;
        rw.z = s * Y0 + c * Z0;

        return rw;
    }

    // =========================
    // 픽셀 점 -> 지면 세계 좌표
    // =========================
    //
    // 입력 픽셀 (u, v)에 대해 지면 Z=0에서의 세계 점 (X, Y) 계산
    //
    // 참고:
    // 1. 광선이 위를 향하거나 지면과 평행하면 유효하지 않음
    // 2. fx, fy는 픽셀 단위 사용
    // 3. H의 단위가 출력 세계 좌표 단위를 결정
    //
    inline GroundPoint imagePixelToGround(
        double u,
        double v,
        const CameraParam& cam)
    {
        // 1) 픽셀 좌표 -> 카메라 정규화 좌표
        Vec3 rc;
        rc.x = (u - cam.cx) / cam.fx;
        rc.y = (v - cam.cy) / cam.fy;
        rc.z = 1.0;

        // 2) 카메라 광선 -> 세계 광선
        Vec3 rw = cameraRayToWorldRay(rc, cam.pitch);

        // 3) 카메라 중심의 세계 좌표 위치
        // Cw = (0, 0, H)
        // 광선 방정식: P(t) = Cw + t * rw
        //
        // 지면 Zw = 0과의 교점:
        // H + t * rw.z = 0  =>  t = -H / rw.z
        //
        GroundPoint gp{};
        gp.valid = false;

        // 광선이 지면을 가리키지 않거나 거의 평행한 경우
        if (std::abs(rw.z) < 1e-12)
            return gp;

        const double t = -cam.H / rw.z;

        // "앞쪽" 교점만 허용
        if (t <= 0.0)
            return gp;

        gp.X = t * rw.x;
        gp.Y = t * rw.y;
        gp.valid = true;
        return gp;
    }

    // =========================
    // 세계 좌표 -> 조감도 픽셀
    // =========================
    //
    // 지면 점 (X, Y)을 출력 조감도의 (bx, by)로 매핑
    //
    // 출력 이미지 규칙:
    // - 왼쪽은 minX, 오른쪽은 maxX
    // - 위쪽은 maxY (더 먼 곳)
    // - 아래쪽은 minY (더 가까운 곳)
    //
    inline bool groundToBirdPixel(
        double X, double Y,
        const IPMParam& ipmParam,
        double& bx, double& by)
    {
        if (X < ipmParam.minX || X > ipmParam.maxX ||
            Y < ipmParam.minY || Y > ipmParam.maxY)
        {
            return false;
        }

        const double xRatio =
            (X - ipmParam.minX) / (ipmParam.maxX - ipmParam.minX);

        const double yRatio =
            (Y - ipmParam.minY) / (ipmParam.maxY - ipmParam.minY);

        // X는 왼쪽에서 오른쪽으로
        bx = xRatio * (ipmParam.outWidth - 1);

        // "먼 곳이 이미지 위쪽에 오도록"
        by = (1.0 - yRatio) * (ipmParam.outHeight - 1);

        return true;
    }

    // =========================
    // 이중 선형 샘플링 (그레이스케일)
    // =========================
    inline uint8_t bilinearSampleGray(
        const uint8_t* src,
        int width,
        int height,
        int stride,
        double u,
        double v)
    {
        if (u < 0.0 || v < 0.0 || u > width - 1.0 || v > height - 1.0)
            return 0;

        const int x0 = static_cast<int>(std::floor(u));
        const int y0 = static_cast<int>(std::floor(v));
        const int x1 = std::min(x0 + 1, width - 1);
        const int y1 = std::min(y0 + 1, height - 1);

        const double dx = u - x0;
        const double dy = v - y0;

        const double p00 = src[y0 * stride + x0];
        const double p10 = src[y0 * stride + x1];
        const double p01 = src[y1 * stride + x0];
        const double p11 = src[y1 * stride + x1];

        const double v0 = p00 * (1.0 - dx) + p10 * dx;
        const double v1 = p01 * (1.0 - dx) + p11 * dx;
        const double val = v0 * (1.0 - dy) + v1 * dy;

        return clampToByte(val);
    }

    // =========================
    // 이중 선형 샘플링 (RGB 3채널)
    // 픽셀당 3바이트, RGBRGB...
    // =========================
    inline void bilinearSampleRGB(
        const uint8_t* src,
        int width,
        int height,
        int stride,
        double u,
        double v,
        uint8_t outRGB[3])
    {
        if (u < 0.0 || v < 0.0 || u > width - 1.0 || v > height - 1.0)
        {
            outRGB[0] = outRGB[1] = outRGB[2] = 0;
            return;
        }

        const int x0 = static_cast<int>(std::floor(u));
        const int y0 = static_cast<int>(std::floor(v));
        const int x1 = std::min(x0 + 1, width - 1);
        const int y1 = std::min(y0 + 1, height - 1);

        const double dx = u - x0;
        const double dy = v - y0;

        const uint8_t* p00 = src + y0 * stride + x0 * 3;
        const uint8_t* p10 = src + y0 * stride + x1 * 3;
        const uint8_t* p01 = src + y1 * stride + x0 * 3;
        const uint8_t* p11 = src + y1 * stride + x1 * 3;

        for (int c = 0; c < 3; ++c)
        {
            const double v0 = p00[c] * (1.0 - dx) + p10[c] * dx;
            const double v1 = p01[c] * (1.0 - dx) + p11[c] * dx;
            const double val = v0 * (1.0 - dy) + v1 * dy;
            outRGB[c] = clampToByte(val);
        }
    }

    // =========================
    // 조감도 픽셀 -> 세계 좌표
    // =========================
    //
    // "역매핑"의 핵심:
    // 출력 조감도의 각 픽셀에 대해 먼저 세계 지면의 점을 구하고,
    // 그런 다음 원본 이미지에서의 위치를 역산하여 원본 이미지에서 샘플링합니다.
    //
    inline void birdPixelToGround(
        double bx,
        double by,
        const IPMParam& ipmParam,
        double& X,
        double& Y)
    {
        const double xRatio = bx / (ipmParam.outWidth - 1);
        const double yRatio = 1.0 - by / (ipmParam.outHeight - 1);

        X = ipmParam.minX + xRatio * (ipmParam.maxX - ipmParam.minX);
        Y = ipmParam.minY + yRatio * (ipmParam.maxY - ipmParam.minY);
    }

    // =========================
    // 세계 지면 점 -> 원본 이미지 픽셀
    // =========================
    //
    // 세계 점 (X, Y, 0)을 입력 이미지로 역투영하여 역매핑 샘플링에 사용합니다.
    //
    inline bool groundToImagePixel(
        double X,
        double Y,
        const CameraParam& cam,
        double& u,
        double& v)
    {
        // 세계 점 Pw = (X, Y, 0)
        // 카메라 중심 Cw = (0, 0, H)
        // 세계 방향 벡터 d_w = Pw - Cw = (X, Y, -H)
        const double dwx = X;
        const double dwy = Y;
        const double dwz = -cam.H;

        // 세계 방향을 카메라 방향으로 되돌려야 함
        // cameraRayToWorldRay에서는 Rw = Rx(pitch) * base를 사용
        // 따라서 여기서는 역회전: Rx(-pitch)
        const double c = std::cos(cam.pitch);
        const double s = std::sin(cam.pitch);

        // 먼저 역회전하여 피치가 없는 상태로
        const double X0 = dwx;
        const double Y0 = c * dwy + s * dwz;
        const double Z0 = -s * dwy + c * dwz;

        // 다시 카메라 좌표로 매핑
        // base: [X0, Y0, Z0] = [xc, zc, -yc]
        const double xc = X0;
        const double yc = -Z0;
        const double zc = Y0;

        // 카메라 뒤쪽에 있으면 유효하지 않음
        if (zc <= 1e-12)
            return false;

        u = cam.fx * (xc / zc) + cam.cx;
        v = cam.fy * (yc / zc) + cam.cy;
        return true;
    }

    // =========================
    // 그레이스케일 IPM
    // =========================
    //
    // src: 입력 그레이스케일 이미지
    // dst: 출력 그레이스케일 이미지, 외부에서 outHeight * dstStride 바이트 할당 필요
    //
    inline void warpIPMGray(
        const uint8_t* src,
        int srcWidth,
        int srcHeight,
        int srcStride,
        uint8_t* dst,
        int dstStride,
        const CameraParam& cam,
        const IPMParam& ipmParam)
    {
        for (int by = 0; by < ipmParam.outHeight; ++by)
        {
            uint8_t* dstRow = dst + by * dstStride;

            for (int bx = 0; bx < ipmParam.outWidth; ++bx)
            {
                // 1) 출력 조감도 픽셀 -> 세계 지면 점
                double X, Y;
                birdPixelToGround(static_cast<double>(bx),
                                  static_cast<double>(by),
                                  ipmParam, X, Y);

                // 2) 세계 지면 점 -> 원본 이미지 픽셀
                double u, v;
                if (!groundToImagePixel(X, Y, cam, u, v))
                {
                    dstRow[bx] = 0;
                    continue;
                }

                // 3) 이중 선형 샘플링
                dstRow[bx] = bilinearSampleGray(src, srcWidth, srcHeight, srcStride, u, v);
            }
        }
    }

    // =========================
    // RGB IPM
    // =========================
    //
    // src: 입력 RGB 이미지, RGBRGB... 순서
    // dst: 출력 RGB 이미지, RGBRGB... 순서
    //
    inline void warpIPMRGB(
        const uint8_t* src,
        int srcWidth,
        int srcHeight,
        int srcStride,
        uint8_t* dst,
        int dstStride,
        const CameraParam& cam,
        const IPMParam& ipmParam)
    {
        for (int by = 0; by < ipmParam.outHeight; ++by)
        {
            uint8_t* dstRow = dst + by * dstStride;

            for (int bx = 0; bx < ipmParam.outWidth; ++bx)
            {
                double X, Y;
                birdPixelToGround(static_cast<double>(bx),
                                  static_cast<double>(by),
                                  ipmParam, X, Y);

                double u, v;
                if (!groundToImagePixel(X, Y, cam, u, v))
                {
                    uint8_t* p = dstRow + bx * 3;
                    p[0] = p[1] = p[2] = 0;
                    continue;
                }

                uint8_t rgb[3];
                bilinearSampleRGB(src, srcWidth, srcHeight, srcStride, u, v, rgb);

                uint8_t* p = dstRow + bx * 3;
                p[0] = rgb[0];
                p[1] = rgb[1];
                p[2] = rgb[2];
            }
        }
    }

}