#include <cuda_runtime.h>
#include "Arith_Utils.h"
#include "Arith_SysStruct.h"
#include "StitchStruct.h"

__device__ Pole device_getPoleFromFPan(POINT32S pt, FPanInfo _panPara)
{
    Pole _pole = { 0 };
    _pole.beta = _panPara.centerGeo.fAz + (pt.x - _panPara.m_pan_width / 2) * _panPara.fAglRes;
    _pole.alpha = _panPara.centerGeo.fPt + (_panPara.m_pan_height / 2 - pt.y) * _panPara.fAglRes;
    return _pole;
}

__device__ PointXYZ device_getXYZFromPole(Pole targetPole)
{
    // 距离未知时，异原点坐标系之间不能转换
    // 但由于同车坐标系的原点差一般远小于目标距离，故距离较大时误差较小，误差量级可以用线偏差/距离获得的弧度进行估计
    // 如线偏差1米，目标3000米，则转换误差为(1/1000-1/3000)约为0.04°

    // 吊舱无源定位允许最低高度10米
    if (targetPole.distance < 10)
    {
        targetPole.distance = 10000;
    }

    PointXYZ result;
    double aa, bb, cc;
    //double ac = sqrt(targetPole.distance*targetPole.distance-bb*bb);    
    bb = sin(RADIAN(targetPole.alpha)) * targetPole.distance;
    double ac = sqrt(targetPole.distance * targetPole.distance - bb * bb);
    aa = cos(RADIAN(targetPole.beta)) * ac;
    cc = sin(RADIAN(targetPole.beta)) * ac;

    result.X = aa;
    result.Y = bb;
    result.Z = cc;

    return result;

}


__device__ POINT32F device_getImgPosFromPole(double* H_inv, Pole _pole, float dep)
{
    PointXYZ virPt = device_getXYZFromPole(_pole);

    //虚拟点投影到地面
    float ratio = -dep / virPt.Y;
    PointXYZ realPt = { 0 };
    realPt.X = virPt.X * ratio;
    realPt.Y = virPt.Y * ratio;
    realPt.Z = virPt.Z * ratio;


    // 转东北地
    PointXYZ realPtGeo = { 0 };
    realPtGeo.X = realPt.Z;
    realPtGeo.Y = realPt.X;
    realPtGeo.Z = -realPt.Y;

    // 投影回像方
    POINT32F dstP = { 0 };
    dstP.x = (H_inv[0] * realPtGeo.X + H_inv[1] * realPtGeo.Y + H_inv[2]) / (H_inv[6] * realPtGeo.X + H_inv[7] * realPtGeo.Y + H_inv[8]);
    dstP.y = (H_inv[3] * realPtGeo.X + H_inv[4] * realPtGeo.Y + H_inv[5]) / (H_inv[6] * realPtGeo.X + H_inv[7] * realPtGeo.Y + H_inv[8]);

    return dstP;
}



__device__ PXL_VAL device_Interpolation_RGB24(UBYTE8* pSrcImage, int iWidth, int iHeight, float x, float y)
{
    PXL_VAL VAL = { 0 };
    int nStride = iWidth * 3;

    // 四个最临近象素的坐标(i1, j1), (i2, j1), (i1, j2), (i2, j2)
    int	i1, i2;
    int	j1, j2;

    // 四个最临近象素值
    UBYTE8 f1[3], f2[3], f3[3], f4[3];

    // 计算四个最临近象素的坐标
    i1 = MAX((int)x, 0);
    i2 = MIN(i1 + 1, iWidth - 1);
    j1 = MAX((int)y, 0);
    j2 = MIN(j1 + 1, iHeight - 1);

    float t, s;		//t = x - [x], s = y- [y];

    t = x - i1;
    s = y - j1;

    //四个最临近象素点的值的加权值
    float uv0, uv1, uv2, uv3;  //权重之和为1：uv0+uv1+uv2+uv3 = 1
    uv0 = (1 - s) * (1 - t);
    uv1 = (1 - s) * t;
    uv2 = (1 - t) * s;
    uv3 = s * t;

    //根据不同情况分别处理
    if (x - iWidth + 1 > 0)
    {
        // 要计算的点在图像右边缘上
        if (y - iHeight + 1 < 0)
        {
            // 要计算的点正好是图像最右/左下角那一个象素，直接返回该点象素值
            VAL.R = *(pSrcImage + nStride * j1 + i1 * 3);
            VAL.G = *(pSrcImage + nStride * j1 + i1 * 3 + 1);
            VAL.B = *(pSrcImage + nStride * j1 + i1 * 3 + 2);
            return VAL;
        }
        else
        {
            // 在图像右/左边缘上且不是最后一点，直接一次插值即可
            f1[0] = *(pSrcImage + nStride * j1 + i1 * 3);
            f1[1] = *(pSrcImage + nStride * j1 + i1 * 3 + 1);
            f1[2] = *(pSrcImage + nStride * j1 + i1 * 3 + 2);


            f3[0] = *(pSrcImage + nStride * j2 + i1 * 3 + 0);
            f3[1] = *(pSrcImage + nStride * j2 + i1 * 3 + 1);
            f3[2] = *(pSrcImage + nStride * j2 + i1 * 3 + 2);

            VAL.R = f1[0] + s * (f3[0] - f1[0]);
            VAL.G = f1[1] + s * (f3[1] - f1[1]);
            VAL.B = f1[2] + s * (f3[2] - f1[2]);

            // 返回插值结果
            return VAL;
        }
    }
    else if (y - iHeight + 1 > 0)
    {
        // 要计算的点在图像下边缘上且不是最后一点，直接一次插值即可
        //f1 = *(pSrcImage + iWidth * j1 + i1);
        //f2 = *(pSrcImage + iWidth * j1 + i2);

        f1[0] = *(pSrcImage + nStride * j1 + i1 * 3);
        f1[1] = *(pSrcImage + nStride * j1 + i1 * 3 + 1);
        f1[2] = *(pSrcImage + nStride * j1 + i1 * 3 + 2);


        f2[0] = *(pSrcImage + nStride * j1 + i2 * 3 + 0);
        f2[1] = *(pSrcImage + nStride * j1 + i2 * 3 + 1);
        f2[2] = *(pSrcImage + nStride * j1 + i2 * 3 + 2);

        VAL.R = f1[0] + t * (f2[0] - f1[0]);
        VAL.G = f1[1] + t * (f2[1] - f1[1]);
        VAL.B = f1[2] + t * (f2[2] - f1[2]);

        // 返回插值结果
        return VAL;
    }
    else
    {
        // 计算四个最临近象素值
        f1[0] = *(pSrcImage + nStride * j1 + i1 * 3);
        f1[1] = *(pSrcImage + nStride * j1 + i1 * 3 + 1);
        f1[2] = *(pSrcImage + nStride * j1 + i1 * 3 + 2);

        f2[0] = *(pSrcImage + nStride * j1 + i2 * 3);
        f2[1] = *(pSrcImage + nStride * j1 + i2 * 3 + 1);
        f2[2] = *(pSrcImage + nStride * j1 + i2 * 3 + 2);

        f3[0] = *(pSrcImage + nStride * j2 + i1 * 3);
        f3[1] = *(pSrcImage + nStride * j2 + i1 * 3 + 1);
        f3[2] = *(pSrcImage + nStride * j2 + i1 * 3 + 2);

        f4[0] = *(pSrcImage + nStride * j2 + i2 * 3);
        f4[1] = *(pSrcImage + nStride * j2 + i2 * 3 + 1);
        f4[2] = *(pSrcImage + nStride * j2 + i2 * 3 + 2);

        VAL.R = uv0 * f1[0] + uv1 * f2[0] + uv2 * f3[0] + uv3 * f4[0];
        VAL.G = uv0 * f1[1] + uv1 * f2[1] + uv2 * f3[1] + uv3 * f4[1];
        VAL.B = uv0 * f1[2] + uv1 * f2[2] + uv2 * f3[2] + uv3 * f4[2];

        return VAL;
    }



    return VAL;
}


__global__ void projKernel(unsigned char* framePtr, SIZE32S size, unsigned char* panSubPtr, 
    RECT32S roi, double* h_inv, FPanInfo _panPara, float dep, unsigned char* mask)
{
    int j = blockIdx.x * blockDim.x + threadIdx.x;
    int i = blockIdx.y * blockDim.y + threadIdx.y;

    if (j > roi.w - 1 || i > roi.h -1)
    {
        return;
    }

    POINT32F p_img = device_getImgPosFromPole(h_inv, device_getPoleFromFPan(POINT32S{ j + roi.x, i + roi.y }, _panPara), dep);

    //if (j == 0 && i == 200)
    //{
    //    printf("size:%d,%d\n", size.w, size.h);
    //    auto rgbVal = device_Interpolation_RGB24(framePtr, size.w, size.h, p_img.x, p_img.y);

    //    printf("%d:%d-%d,RGB:%d-%d-%d\n", j, p_img.x, p_img.y, rgbVal.R, rgbVal.G, rgbVal.B);
    //}

    if (p_img.x >= 200 && p_img.y >= 200 && p_img.x < size.w - 200 && p_img.y < size.h - 200)
    {
       auto rgbVal = device_Interpolation_RGB24(framePtr, size.w, size.h, p_img.x, p_img.y);
       panSubPtr[(i * (roi.w) + j) * 3 + 0] = rgbVal.R;
       panSubPtr[(i * (roi.w) + j) * 3 + 1] = rgbVal.G;
       panSubPtr[(i * (roi.w) + j) * 3 + 2] = rgbVal.B;

       mask[i * roi.w + j] = 255;
    }
}


//#define DYNAMIC_MEM

void UpdatePan_CUDA(cv::Mat rgbFrame, cv::Mat pan, cv::Rect2d roi, cv::Mat h_inv, FPanInfo _panPara, float dep, cuda_Mem cuda_res)
{
    //// check roi
    if (roi.width <= 0 || roi.height <= 0)
    {
        return;
    }

    unsigned char* cuda_Frame, * cuda_Pan, *cuda_pan_mask;

    double* cuda_H_inv_data;

    int pan_sub_w = roi.width;
    int pan_sub_h = roi.height;
#ifdef DYNAMIC_MEM
    cudaMalloc((void**)&cuda_Frame, rgbFrame.cols * rgbFrame.rows * 3);//rgb
    cudaMalloc((void**)&cuda_Pan, pan_sub_w * pan_sub_h * 3);//rgb
    cudaMalloc((void**)&cuda_H_inv_data, 9 * sizeof(double));//
    cudaMalloc((void**)&cuda_pan_mask, pan_sub_w * pan_sub_h * 1);//gray
#else
    cuda_Frame = cuda_res.global_cuda_Frame;
    cuda_Pan = cuda_res.global_cuda_Pan;
    cuda_H_inv_data = cuda_res.global_cuda_H_inv_data;
    cuda_pan_mask = cuda_res.global_cuda_pan_mask;
#endif
    // 拷贝帧数据到设备
    cudaMemcpy(cuda_Frame, rgbFrame.data, rgbFrame.cols * rgbFrame.rows * 3, cudaMemcpyHostToDevice);
    cudaMemcpy(cuda_H_inv_data, (double*)h_inv.data,9 * sizeof(double), cudaMemcpyHostToDevice);

    // 定义线程块和网格大小
    dim3 blockSize(8, 8);
    dim3 gridSize((pan_sub_w + blockSize.x - 1) / blockSize.x, (pan_sub_h + blockSize.y - 1) / blockSize.y);//向上取整，确保线程覆盖全区域

    SIZE32S FrameSize = { rgbFrame.cols, rgbFrame.rows };

    RECT32S RoiRect = { roi.x,roi.y,roi.width,roi.height };


    cudaMemset(cuda_Pan, 0, roi.width * roi.height * 3);
    cudaMemset(cuda_pan_mask, 0, roi.width * roi.height);

    // 启动CUDA核函数
    projKernel << <gridSize, blockSize >> > (cuda_Frame, FrameSize, cuda_Pan, RoiRect, cuda_H_inv_data, _panPara, dep, cuda_pan_mask);

    cudaError_t err = cudaGetLastError();
    if (err != cudaSuccess) {
        printf("CUDA error: %s\n", cudaGetErrorString(err));
    }
    //cudaDeviceSynchronize();

    // 拷贝结果回主机
    cv::Mat pan_sub = pan(roi);

    cv::Mat pan_sub_clone = cv::Mat::zeros(pan_sub.size(), CV_8UC3);
    cv::Mat pan_sub_mask = cv::Mat::zeros(pan_sub.size(), CV_8UC1);

    cudaMemcpy(pan_sub_clone.data, cuda_Pan, pan_sub_w * pan_sub_h * 3, cudaMemcpyDeviceToHost);
    cudaMemcpy(pan_sub_mask.data, cuda_pan_mask, pan_sub_w * pan_sub_h * 1, cudaMemcpyDeviceToHost);



    pan_sub_clone.copyTo(pan_sub, pan_sub_mask);

    //cv::Mat dst;
    //cv::resize(pan_sub_clone, dst, cv::Size(pan_sub_w / 4, pan_sub_h / 4));
    //imshow("pan_sub_clone", dst);
    //cv::waitKey(1);
    
#ifdef DYNAMIC_MEM
    // 释放设备内存
    cudaFree(cuda_Frame);
    cudaFree(cuda_Pan);
    cudaFree(cuda_H_inv_data);
    cudaFree(cuda_pan_mask);
#endif
}


