#include "API_VideoStitch.h"
#include "Arith_VideoStitch.h"
#include "Arith_Utils.h"
#include "Arith_CoordModule.h"
#include <opencv2/opencv.hpp>
#include <omp.h>

using namespace std;
using namespace cv;

API_VideoStitch * API_VideoStitch::Create(SINT32 nWidth, SINT32 nHeight)
{
    return new VideoStitch(nWidth,nHeight);
}


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


VideoStitch::VideoStitch(SINT32 nWidth, SINT32 nHeight)
{
	_GeoStitcher = new GeoStitcher();
	memset(&_pan, 0, sizeof(GD_VIDEO_FRAME_S));

	_BATask = new BA_Task(_GeoStitcher);
}

VideoStitch::~VideoStitch()
{
	delete _GeoStitcher;
}


void VideoStitch::Test()
{

}

PanInfo VideoStitch::Init(FrameInfo info)
{
	_panPara = _GeoStitcher->InitMap(info);

	_BATask->setPanPara(_panPara);

	_pan = cv::Mat::zeros(_panPara.m_pan_height, _panPara.m_pan_width, CV_8UC1);

	return _panPara;
}

BYTE8 VideoStitch::GeoStitch(GD_VIDEO_FRAME_S img, FrameInfo para)
{
	Proj t_Proj = _GeoStitcher->AnlayseTform(para);

	// 计算帧的map四至
	cv::Point2f leftTop_map = _GeoStitcher->back_project(cv::Point2f(0,0), t_Proj, _panPara);
	cv::Point2f rightTop_map = _GeoStitcher->back_project(cv::Point2f(img.u32Width,0), t_Proj, _panPara);
	cv::Point2f rightBottom_map = _GeoStitcher->back_project(cv::Point2f(img.u32Width,img.u32Height), t_Proj, _panPara);
	cv::Point2f leftBottom_map = _GeoStitcher->back_project(cv::Point2f(0,img.u32Height), t_Proj, _panPara);


	auto H = _GeoStitcher->findHomography(t_Proj,_panPara);
	leftTop_map = warpPointWithH(H,cv::Point2f(0,0));
	rightTop_map = warpPointWithH(H,cv::Point2f(img.u32Width,0));
	rightBottom_map = warpPointWithH(H,cv::Point2f(img.u32Width,img.u32Height));
	leftBottom_map = warpPointWithH(H,cv::Point2f(0,img.u32Height));

	// 计算全景图的范围
	int right = max(max(max(leftTop_map.x, leftBottom_map.x), rightTop_map.x), rightBottom_map.x);
	int left = min(min(min(leftTop_map.x, leftBottom_map.x), rightTop_map.x), rightBottom_map.x);
	int top = min(min(min(leftTop_map.y, leftBottom_map.y), rightTop_map.y), rightBottom_map.y);
	int bottom = max(max(max(leftTop_map.y, leftBottom_map.y), rightTop_map.y), rightBottom_map.y);


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

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

#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 = _GeoStitcher->project(Point2f(j, i), t_Proj, _panPara);

			p_img = warpPointWithH(H.inv(),Point2f(j, i));
			if (p_img.x >= 0 && p_img.y >= 0 && p_img.x < img.u32Width && p_img.y < img.u32Height)
			{
				_pan.data[i * _pan.rows + j] =
					FourPointInterpolation(img.u64VirAddr[0], img.u32Width, img.u32Height, p_img.x, p_img.y);
			}
		}
	}


    return 0;
}

SINT32 VideoStitch::BAStitch(GD_VIDEO_FRAME_S img, FrameInfo para)
{
	
	SINT32 nFrameCnt = _BATask->addFrame(img,para);
	

	return 0;
}

SINT32 VideoStitch::BAOpt()
{
	_BATask->optimizeBA();
	return 0;
}

GD_VIDEO_FRAME_S VideoStitch::ExportPanAddr()
{
	GD_VIDEO_FRAME_S pan_out;

	pan_out.enPixelFormat = GD_PIXEL_FORMAT_GRAY_Y8;
	pan_out.u32Width = _panPara.m_pan_width;
	pan_out.u32Height = _panPara.m_pan_height;
	pan_out.u64VirAddr[0] = _pan.data;

	return pan_out;
}