#include "ImageTailor.h"
#include <libyuv.h>
#include "OptimizeCal.h"
#include "FusionAndTailor.cuh"


std::mutex g_CurFrameMutex;
std::mutex g_PanoMutex;

ImageTailor::ImageTailor()
{
	pPanoImageBuffer = NULL;
	pTailorThread = NULL;
	this->bTailorThreadOut = true;
	StartTailorThread();
	bTailorThreadWork = false;
}

ImageTailor::~ImageTailor()
{
	this->bTailorThreadOut = false;
	if (pTailorThread->joinable())
		pTailorThread->join();
	if (pTailorThread!=NULL)
	{
		delete pTailorThread;
		pTailorThread = NULL;
	}

	if (pPanoImageBuffer != NULL)
	{
		delete[] pPanoImageBuffer;
		cudaFree(pPanoImageBufferInGpu);
		pPanoImageBuffer = NULL;
	}

}

void ImageTailor::Init(unsigned char* pImage, int nPanoWidth, int nPanoHeight, cv::Point cvLeftTop)
{
	//get the pointer
	pPanoImageBuffer = pImage;
	//get the width
	this->nPanoWidth = nPanoWidth;
	//get the height
	this->nPanoHeight = nPanoHeight;
	//get the lefttop point
	this->LeftTop = cvLeftTop;
	//get the panoimage pitch
	this->nPanoPitch = (nPanoWidth*3 + 3) / 4 * 4;

}

void ImageTailor::Init_Gpu(unsigned char* pImage, unsigned char* pImageGpu, int nPanoWidth, int nPanoHeight, cv::Point cvLeftTop)
{
	//get the pointer
	pPanoImageBuffer = pImage;
	//get the width
	this->nPanoWidth = nPanoWidth;
	//get the height
	this->nPanoHeight = nPanoHeight;
	//get the lefttop point
	this->LeftTop = cvLeftTop;
	//get the panoimage pitch
	this->nPanoPitch = (nPanoWidth * 3 + 3) / 4 * 4;

	pPanoImageBufferInGpu = pImageGpu;
}

int ImageTailor::StitchingOnPano(cv::Point LeftTop, cv::Mat* FrameImage,std::vector<std::vector<int>>* pvvOverlap)
{
	//in the paon image ,the lefttop point is (0,0),stitching the image on the pano image


	//get the lefttop point
	cv::Point cvLeftTop = LeftTop;
	//get the width
	int nFrameWidth = FrameImage->cols;
	//get the height
	int nFrameHeight = FrameImage->rows;
	//get the pitch
	int nFramePitch = FrameImage->step[0];

	//set a pointer to pano image stitching target
	unsigned char* pPanoImageTarget = pPanoImageBuffer + cvLeftTop.y * nPanoPitch + cvLeftTop.x * 3;

	cv::Mat PanoImage = cv::Mat(nPanoHeight, nPanoWidth, CV_8UC3, pPanoImageBuffer);

	//stitching the image

	for (int i = 0; i < nFrameHeight; i++)
	{
		//get this line overlap 
		std::vector<int>& vOverlap = (*pvvOverlap)[i];
		//copy the image
		
		FastCopy_Any(FrameImage->ptr<uchar>(i) + vOverlap[0] * 3
			, pPanoImageTarget + nPanoPitch * i + vOverlap[0] * 3
			, (vOverlap[1]- vOverlap[0]) * 3);
		
	}

	return 0;
}

int ImageTailor::StitchingOnPanoYUV(cv::Point LeftTop, unsigned char* pImage, int nWidth, int nHeight, int nPitch, std::vector<std::vector<int>>* pvvOverlap)
{
	//get Y,U,V pointer in pano image
	unsigned char* pPanoImageY = pPanoImageBuffer;
	unsigned char* pPanoImageU = pPanoImageBuffer + nPanoPitch * nPanoHeight;
	unsigned char* pPanoImageV = pPanoImageBuffer + nPanoPitch * nPanoHeight * 5 / 4;
	cv::Mat panoImage(nPanoHeight * 3 / 2, nPanoWidth, CV_8U, pPanoImageBuffer);
	//get Y,U,V pointer in frame image
	unsigned char* pFrameImageY = pImage;
	unsigned char* pFrameImageU = pImage + nPitch * nHeight;
	unsigned char* pFrameImageV = pImage + nPitch * nHeight * 5 / 4;

	//Disassembly the pvvOverlap in rect to calculate the planepaste
	std::vector<cv::Rect> vPastePart;
	int nPanoImageHeight = int(pvvOverlap->size());
	cv::Rect rectPastePart;
	bool bChangeFlags = true;
	for (int i = 0; i < nPanoImageHeight; i++)
	{
		std::vector<int>& vOverlap = (*pvvOverlap)[i];

		if (rectPastePart.x != vOverlap[0]
			&& rectPastePart.y != vOverlap[1])
			bChangeFlags = true;

		if (bChangeFlags)
		{
			if (i != 0)
				vPastePart.push_back(rectPastePart);
			rectPastePart.x = vOverlap[0];
			rectPastePart.width = vOverlap[1] - vOverlap[0];
			rectPastePart.y = i;
			rectPastePart.height = 0;
			bChangeFlags = false;
		}
		rectPastePart.height++;
	}
	vPastePart.push_back(rectPastePart);

	int nPartNum = (int)vPastePart.size();
	for (int i = 0; i < nPartNum; i++)
	{
		cv::Rect& CurPart = vPastePart[i];
		//panoImage pointer
		unsigned char* pPanoImageTargetY = pPanoImageY + (CurPart.y + LeftTop.y) * nPanoPitch + (CurPart.x + LeftTop.x);
		unsigned char* pPanoImageTargetU = pPanoImageU + (CurPart.y + LeftTop.y) * nPanoPitch / 4 + (CurPart.x + LeftTop.x) / 2;
		unsigned char* pPanoImageTargetV = pPanoImageV + (CurPart.y + LeftTop.y) * nPanoPitch / 4 + (CurPart.x + LeftTop.x) / 2;

		//frameImage pointer 
		unsigned char* pFrameImageTargetY = pFrameImageY + CurPart.y * nPitch + CurPart.x;
		unsigned char* pFrameImageTargetU = pFrameImageU + CurPart.y * nPitch / 4 + CurPart.x / 2;
		unsigned char* pFrameImageTargetV = pFrameImageV + CurPart.y * nPitch / 4 + CurPart.x / 2;

		//copy the Y plane
		libyuv::CopyPlane(
			pPanoImageTargetY, nPanoPitch,
			pFrameImageTargetY, nPitch,
			CurPart.width, CurPart.height
		);

		//copy the U plane
		libyuv::CopyPlane(
			pPanoImageTargetU, nPanoPitch / 2,
			pFrameImageTargetU, nPitch / 2,
			CurPart.width / 2, CurPart.height / 2
		);

		//copy the V plane
		libyuv::CopyPlane(
			pPanoImageTargetV, nPanoPitch / 2,
			pFrameImageTargetV, nPitch / 2,
			CurPart.width / 2, CurPart.height / 2
		);
	}
	
	return 0;
}

int ImageTailor::StitchingOnPanoYUV(cv::Point LeftTop, cv::Mat* FrameImage, std::vector<std::vector<int>>* pvvOverlap)
{
	//get Y,U,V pointer in pano image
	unsigned char* pPanoImageY = pPanoImageBuffer;
	unsigned char* pPanoImageU = pPanoImageBuffer + ALIGN_4(nPanoWidth) * nPanoHeight;
	unsigned char* pPanoImageV = pPanoImageBuffer + ALIGN_4(nPanoWidth) * nPanoHeight * 5 / 4;
	cv::Mat panoImage(nPanoHeight * 3 / 2, nPanoWidth, CV_8U, pPanoImageBuffer);
	//get Y,U,V pointer in frame image
	unsigned char* &pFrameImageY = FrameImage->data;
	unsigned char* pFrameImageU = pFrameImageY + FrameImage->step * FrameImage->rows;
	unsigned char* pFrameImageV = pFrameImageY + FrameImage->step * FrameImage->rows * 5 / 4;

	//Disassembly the pvvOverlap in rect to calculate the planepaste
	std::vector<cv::Rect> vPastePart;
	int nPanoImageHeight = int(pvvOverlap->size());
	cv::Rect rectPastePart;
	bool bChangeFlags = true;
	for (int i = 0; i < nPanoImageHeight; i++)
	{
		std::vector<int>& vOverlap = (*pvvOverlap)[i];

		if (rectPastePart.x != vOverlap[0]
			&& rectPastePart.y != vOverlap[1])
			bChangeFlags = true;

		if (bChangeFlags)
		{
			if (i != 0)
				vPastePart.push_back(rectPastePart);
			rectPastePart.x = vOverlap[0];
			rectPastePart.width = vOverlap[1] - vOverlap[0];
			rectPastePart.y = i;
			rectPastePart.height = 0;
			bChangeFlags = false;
		}
		rectPastePart.height++;
	}
	vPastePart.push_back(rectPastePart);



	int nPartNum = (int)vPastePart.size();
	for (int i = 0; i < nPartNum; i++)
	{
		cv::Rect& CurPart = vPastePart[i];
		//panoImage pointer
		unsigned char* pPanoImageTargetY = pPanoImageY + (CurPart.y + LeftTop.y) * ALIGN_4(nPanoWidth) + (CurPart.x + LeftTop.x);
		unsigned char* pPanoImageTargetU = pPanoImageU + (CurPart.y + LeftTop.y) * ALIGN_4(nPanoWidth) / 4 + (CurPart.x + LeftTop.x) / 2;
		unsigned char* pPanoImageTargetV = pPanoImageV + (CurPart.y + LeftTop.y) * ALIGN_4(nPanoWidth) / 4 + (CurPart.x + LeftTop.x) / 2;

		//frameImage pointer 
		unsigned char* pFrameImageTargetY = pFrameImageY + CurPart.y * FrameImage->step + CurPart.x;
		unsigned char* pFrameImageTargetU = pFrameImageU + CurPart.y * FrameImage->step / 4 + CurPart.x / 2;
		unsigned char* pFrameImageTargetV = pFrameImageV + CurPart.y * FrameImage->step / 4 + CurPart.x / 2;

		//copy the Y plane
		libyuv::CopyPlane(
			pFrameImageTargetY, FrameImage->step,
			pPanoImageTargetY, ALIGN_4(nPanoWidth),
			CurPart.width, CurPart.height
		);

		//copy the U plane
		libyuv::CopyPlane(
			pFrameImageTargetU, FrameImage->step / 2,
			pPanoImageTargetU, ALIGN_4(nPanoWidth) / 2,
			CurPart.width / 2, CurPart.height / 2
		);

		//copy the V plane
		libyuv::CopyPlane(
			pFrameImageTargetV, FrameImage->step / 2,
			pPanoImageTargetV, ALIGN_4(nPanoWidth) / 2,
			CurPart.width / 2, CurPart.height / 2
		);
	}

	return 0;
}

void ImageTailor::TailorWordInThread(ImageTailor* pClass)
{
	while (pClass->bTailorThreadOut)
	{
		
		g_CurFrameMutex.lock();
		g_PanoMutex.lock();


		if (pClass->qWorkQueue.size()<=0 || pClass->qWorkQueue.size() >10000)
		{
			g_CurFrameMutex.unlock();
			g_PanoMutex.unlock();
			continue;
			//pClass->ClearWorkOrder();
			
		}
		
		pClass->bTailorThreadWork = true;

	
		//WAIT_TAILOR& WorkOrder = pClass->qWorkQueue.front();

		
		
		if (!pClass->bUseYUV)
			pClass->StitchingOnPano(pClass->qWorkQueue.front().ptPosition, pClass->qWorkQueue.front().pROI, pClass->qWorkQueue.front().pvvOverlap);
		else
			pClass->StitchingOnPanoYUVByGPU(
				pClass->qWorkQueue.front().ptPosition,
				pClass->qWorkQueue.front().pImage,
				pClass->qWorkQueue.front().nWidth,
				pClass->qWorkQueue.front().nHeight,
				ALIGN_4(pClass->qWorkQueue.front().nPitch),
				pClass->qWorkQueue.front().pvvOverlap
			);
		g_CurFrameMutex.unlock();
		g_PanoMutex.unlock();

		pClass->qWorkQueue.pop();

		if (pClass->qWorkQueue.size() > 25)
			pClass->qWorkQueue.pop();
		if( pClass->qWorkQueue.size() > 50)
			pClass->qWorkQueue.pop();

		pClass->bTailorThreadWork = false;
	}
}

/// <summary>
/// Stitchings the on pano.
/// </summary>
/// <param name="LeftTop">The left top.</param>
/// <param name="FrameImage">The frame image.</param>
/// <param name="nWidth">Width of the n.</param>
/// <param name="nHeight">Height of the n.</param>
/// <param name="nPitch">The n pitch.</param>
/// <param name="pvvOverlap">The PVV overlap.</param>
/// <returns></returns>
int ImageTailor::StitchingOnPanoYUVByGPU(cv::Point LeftTop, unsigned char* FrameImage, int nWidth, int nHeight, int nPitch, std::vector<std::vector<int>>* pvvOverlap)
{	
	
	//get Y,U,V pointer in pano image
	unsigned char* pPanoImageY = pPanoImageBufferInGpu + LeftTop.y * ALIGN_4(nPanoWidth) + LeftTop.x;
	unsigned char* pPanoImageU = pPanoImageBufferInGpu + ALIGN_4(nPanoWidth) * nPanoHeight + LeftTop.y/2 * (ALIGN_4(nPanoWidth)/2) + LeftTop.x/2;
	unsigned char* pPanoImageV = pPanoImageBufferInGpu + ALIGN_4(nPanoWidth) * nPanoHeight * 5 / 4 + LeftTop.y / 2 * (ALIGN_4(nPanoWidth) / 2) + LeftTop.x / 2;
	//cv::Mat panoImage(nPanoHeight * 3 / 2, nPanoWidth, CV_8U, pPanoImageBuffer);

	unsigned char*& pFrameImageY = FrameImage;
	unsigned char* pFrameImageU = pFrameImageY + nPitch * nHeight;
	unsigned char* pFrameImageV = pFrameImageY + nPitch * nHeight * 5 / 4;

	//Disassembly the pvvOverlap in rect to calculate the planepaste
	std::vector<cv::Rect> vPastePart;
	int nPanoImageHeight = int(pvvOverlap->size());
	cv::Rect rectPastePart;
	bool bChangeFlags = true;
	for (int i = 0; i < nPanoImageHeight; i++)
	{
		std::vector<int>& vOverlap = (*pvvOverlap)[i];

		if (rectPastePart.x != vOverlap[0]
			&& rectPastePart.y != vOverlap[1])
			bChangeFlags = true;

		if (bChangeFlags)
		{
			if (i != 0)
				vPastePart.push_back(rectPastePart);
			rectPastePart.x = vOverlap[0];
			rectPastePart.width = vOverlap[1] - vOverlap[0];
			rectPastePart.y = i;
			rectPastePart.height = 0;
			bChangeFlags = false;
		}
		rectPastePart.height++;
	}
	vPastePart.push_back(rectPastePart);


	int nBlockCount =int(vPastePart.size());
	for (int i = 0; i < nBlockCount; i++)
	{
		cv::Rect CurPart = vPastePart[i];

		int nPasteWidth = CurPart.width;
		int nPasteHeight = CurPart.height;
		int nPasteLeft = CurPart.x;
		int nPasteTop = CurPart.y;

		unsigned char* pPanoImageTargetY = pPanoImageY;//; +(CurPart.y) * ALIGN_4(nPanoWidth) + (CurPart.x);
		unsigned char* pPanoImageTargetU = pPanoImageU;// +(CurPart.y) * ALIGN_4(nPanoWidth) / 4 + (CurPart.x) / 2;
		unsigned char* pPanoImageTargetV = pPanoImageV;// +(CurPart.y) * ALIGN_4(nPanoWidth) / 4 + (CurPart.x) / 2;

		//frameImage pointer 
		unsigned char* pFrameImageTargetY = pFrameImageY + CurPart.y * nPitch + CurPart.x;
		unsigned char* pFrameImageTargetU = pFrameImageU + CurPart.y * nPitch / 4 + CurPart.x / 2;
		unsigned char* pFrameImageTargetV = pFrameImageV + CurPart.y * nPitch / 4 + CurPart.x / 2;

		YUVTailorAndBlender::CopyPlane(
			pFrameImageTargetY, nWidth, nHeight, nPitch,
			pPanoImageTargetY, nPanoWidth, nPanoHeight, ALIGN_4(nPanoWidth),
			nPasteWidth, nPasteHeight, nPasteLeft, nPasteTop,
			1, NULL
		);

		YUVTailorAndBlender::CopyPlane(
			pFrameImageTargetU, nWidth / 2, nHeight / 2, nPitch / 2,
			pPanoImageTargetU, nPanoWidth / 2, nPanoHeight / 2, ALIGN_4(nPanoWidth) / 2,
			nPasteWidth / 2, nPasteHeight / 2, nPasteLeft / 2, nPasteTop / 2,
			1, NULL
		);

		YUVTailorAndBlender::CopyPlane(
			pFrameImageTargetV, nWidth / 2, nHeight / 2, nPitch / 2,
			pPanoImageTargetV, nPanoWidth / 2, nPanoHeight / 2, ALIGN_4(nPanoWidth) / 2,
			nPasteWidth / 2, nPasteHeight / 2, nPasteLeft / 2, nPasteTop / 2,
			1, NULL
		);

		cudaError Error = cudaDeviceSynchronize();

		if (Error == 700)
			int x = 10;

	}

	return 0;
}

int ImageTailor::Stitching(int nPostion, cv::Mat& ROI, std::vector<std::vector<int>>* pvvOverlap)
{
	return 0;
}

void ImageTailor::SetOverlap(std::vector<std::vector<int>>* pvvOverlap)
{
	vvOverlap = *pvvOverlap;
}

bool ImageTailor::StartTailorThread()
{
	pTailorThread = new std::thread(TailorWordInThread, this);
	return false;
}


bool ImageTailor::EndTailorThread()
{
	if (pTailorThread->joinable())
	{
		this->bTailorThreadOut = false;
		pTailorThread->join();
		return true;
	}
	else
	{
		return false;
	}
}

void ImageTailor::AddWorkOrderInqueue(cv::Point LeftTop, cv::Mat* FrameImage, std::vector<std::vector<int>>* pvvOverlap)
{
	WAIT_TAILOR WorkOrder;
	WorkOrder.ptPosition = LeftTop;
	WorkOrder.pROI = FrameImage;
	WorkOrder.pvvOverlap = pvvOverlap;
	qWorkQueue.push(WorkOrder);
}

void ImageTailor::ClearWorkOrder()
{
	std::queue<WAIT_TAILOR>().swap(qWorkQueue);
	/*while (!qWorkQueue.empty())
	{
		qWorkQueue.pop();
	}*/
}

void ImageTailor::AddWorkOrderInQueueGpu(cv::Point LeftTop, unsigned char* pData, int nWidth, int nHeight, int nPitch, std::vector<std::vector<int>>* pvvOverlap)
{
	WAIT_TAILOR WorkOrder;
	WorkOrder.ptPosition = LeftTop;
	WorkOrder.pImage = pData;
	WorkOrder.nWidth = nWidth;
	WorkOrder.nHeight = nHeight;
	WorkOrder.nPitch = nPitch;
	WorkOrder.pvvOverlap = pvvOverlap;
	qWorkQueue.push(WorkOrder);
}