#include "API_FrontStitch.h"
#include "Arith_FrontStitch.h"
#include "Arith_Utils.h"
#include "Arith_CoordModule.h"
#include "Arith_FeaMatch.h"
#include <opencv2/opencv.hpp>
#include <omp.h>
#include <cuda_runtime.h>

using namespace std;
using namespace cv;

API_FrontStitch* API_FrontStitch::Create(SINT32 nWidth, SINT32 nHeight)
{
    return new FrontStitch(nWidth, nHeight);
}


void API_FrontStitch::Destroy(API_FrontStitch* obj)
{
    delete obj;
}

FrontStitch::FrontStitch(SINT32 nWidth, SINT32 nHeight)
{
    _GeoSolver = new GeoSolver();
}

FrontStitch::~FrontStitch()
{
}







FPanInfo FrontStitch::Init(FrameInfo info, float AzRange, float PtRange)
{
    _panPara.fAglRes = 0.02;
    // 设置当前拼接起始位置
    _GeoSolver->SetOriginPoint(info);

    cv::Mat H = _GeoSolver->findHomography(info);

    // 计算中心点的极坐标
    Pole centPole = getPoleFromImgWithH(H, cv::Point2f(info.nWidth / 2, info.nHeight / 2), info.nEvHeight);

    // 可以正反验证
    //cv::Point2f imgpos = getImgPosFromPole(H, centPole, info.nEvHeight);

    _panPara.center.fAz = centPole.beta;
    _panPara.center.fPt = centPole.alpha;
    _panPara.range.fAz = AzRange;
    _panPara.range.fPt = PtRange;

    // 全景图规模
    _panPara.m_pan_width = AzRange / _panPara.fAglRes;
    _panPara.m_pan_height = PtRange / _panPara.fAglRes;

    _panImage = cv::Mat::zeros(_panPara.m_pan_height, _panPara.m_pan_width, CV_8UC3);

    return _panPara;
}

BYTE8 FrontStitch::GeoStitch(GD_VIDEO_FRAME_S img, FrameInfo info)
{
    cv::Mat H = _GeoSolver->findHomography(info);
    float dep = info.nEvHeight;

    // 获取rgb源图像
    cv::Mat rgb = getRGBMatFromGDFrame(img, img.u64VirAddr[0]);

    Pole CT_map = getPoleFromImgWithH(H, cv::Point2f(img.u32Width/2, img.u32Height/2), info.nEvHeight);

    // 计算帧的map四至
	Pole leftTop_map = getPoleFromImgWithH(H,cv::Point2f(0,0), info.nEvHeight);
    Pole rightTop_map = getPoleFromImgWithH(H, cv::Point2f(img.u32Width,0), info.nEvHeight);
    Pole rightBottom_map = getPoleFromImgWithH(H, cv::Point2f(img.u32Width,img.u32Height), info.nEvHeight);
    Pole leftBottom_map = getPoleFromImgWithH(H, cv::Point2f(0,img.u32Height), info.nEvHeight);

    // 转全景图的像方
    cv::Point2f p_leftTop_map = getFPanFromPole(leftTop_map);
    cv::Point2f p_rightTop_map = getFPanFromPole(rightTop_map);
    cv::Point2f p_rightBottom_map = getFPanFromPole(rightBottom_map);
    cv::Point2f p_leftBottom_map = getFPanFromPole(leftBottom_map);

	// 计算全景图上范围
	int right = max(max(max(p_leftTop_map.x, p_rightTop_map.x), p_rightBottom_map.x), p_leftBottom_map.x);
	int left = min(min(min(p_leftTop_map.x, p_rightTop_map.x), p_rightBottom_map.x), p_leftBottom_map.x);
	int top = min(min(min(p_leftTop_map.y, p_rightTop_map.y), p_rightBottom_map.y), p_leftBottom_map.y);
	int bottom = max(max(max(p_leftTop_map.y, p_rightTop_map.y), p_rightBottom_map.y), p_leftBottom_map.y);

    int xRange = right - left;
	int yRnage = bottom - top;

	//反映射到像素坐标
	int valid_top = std::max(0, top);
	int valid_bottom = std::min(_panImage.cols, bottom);
	int valid_left = std::max(0, left);
	int valid_right = std::min(_panImage.rows, right);

#if 1 
#pragma omp parallel for
	for (int i = valid_top; i < valid_bottom; i++)
	{
		for (int j = valid_left; j < valid_right; j++)
		{
			////转换为pixel坐标
            cv::Point2f p_img = getImgPosFromPole(H, getPoleFromFPan(Point2f(j, i)), dep);

			if (p_img.x >= 200 && p_img.y >= 200 && p_img.x < img.u32Width-200 && p_img.y < img.u32Height-200)
			{
                auto rgbVal = Interpolation_RGB24(rgb.data, rgb.cols, rgb.rows, p_img.x, p_img.y);
                _panImage.data[3 * (i * _panImage.rows + j) + 0] = rgbVal.R;
                _panImage.data[3 * (i * _panImage.rows + j) + 1] = rgbVal.G;
                _panImage.data[3 * (i * _panImage.rows + j) + 2] = rgbVal.B;
			}
		}
	}
#endif

    return 0;
}














SINT32 FrontStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
{
    return SINT32();
}

SINT32 FrontStitch::ProcessFrame()
{
    return SINT32();
}

GD_VIDEO_FRAME_S FrontStitch::ExportPanAddr()
{
    GD_VIDEO_FRAME_S pan_out;

    pan_out.enPixelFormat = GD_PIXEL_FORMAT_RGB_PACKED;
    pan_out.u32Width = _panPara.m_pan_width;
    pan_out.u32Height = _panPara.m_pan_height;
    pan_out.u64VirAddr[0] = _panImage.data;

    return pan_out;
}

Pole FrontStitch::getPoleFromImgWithH(cv::Mat H, cv::Point2f pt, float dep)
{
    // 投影到地面坐标，这一步可以利用前面优化成果
    cv::Point2f grdPt = warpPointWithH(H, pt);

    // 补深度信息并转为常用的NUE坐标系
    PointXYZ grdPtXYZ = { 0 };
    grdPtXYZ.X = grdPt.y;
    grdPtXYZ.Y = -dep;
    grdPtXYZ.Z = grdPt.x;

    // 地面点转极坐标
    Pole pole = getPoleFromXYZ(grdPtXYZ);

    return pole;

}

cv::Point2f FrontStitch::getImgPosFromPole(cv::Mat H, Pole _pole, float dep)
{
    PointXYZ virPt = 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;

    // 投影回像方
    cv::Point2f px = warpPointWithH(H.inv(), cv::Point2f(realPtGeo.X, realPtGeo.Y));

    return px;
}

Pole FrontStitch::getPoleFromFPan(cv::Point2f pt)
{
    Pole _pole = { 0 };
    _pole.beta = _panPara.center.fAz + (pt.x - _panPara.m_pan_width / 2) * _panPara.fAglRes;
    _pole.alpha = _panPara.center.fPt + (_panPara.m_pan_height / 2 - pt.y) * _panPara.fAglRes;
    return _pole;
}

cv::Point2f FrontStitch::getFPanFromPole(Pole _pole)
{
    cv::Point2f pt = { 0 };
    pt.x = DEGLIM(_pole.beta - _panPara.center.fAz) / _panPara.fAglRes + _panPara.m_pan_width / 2;
    pt.y = DEGLIM(_panPara.center.fPt - _pole.alpha) / _panPara.fAglRes + _panPara.m_pan_height / 2;
    return pt;
}