///流程
/// CG-Frontend -> pkCentering -> Convolve -> Subpix -> Calib -> Compute3D
#include <vector>
#include "algoGlobals.h"
#include "PickLazerWin.h"
#include "peakCentering.h"
#include "convolve.h"
#include "subPix.h"
#include "calib.h"
#include "compute3D.h"
#include "camAlgoSW_Export.h"

#define SV_WIN_SIZE 16
#define ALIGN_RDX  ((LAZER_WIN_SIZE-SV_WIN_SIZE)/2)
std::vector<Luma_rgnData> SvPickRgnRslt(
	const unsigned char* SrcPixel,
	int FrmH,
	int FrmW,
	PickLaserResult& PickLaserResult_inst
)
{
	int LazerWinNum = 0;
	int winW = 0x800 | SV_WIN_SIZE; 
	std::vector<Luma_rgnData> testData;
	for (unsigned short y = 0; y < FrmH; y++)
	{
		int WinNumPerLine = 0;
		for (int PickId = 0; PickId < PICK_RGN_MAX; PickId++)
		{
			if (PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_PickMsk[y][PickId] > 0)
			{
				unsigned short WinRdx = PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_WinRdx[y][PickId] + ALIGN_RDX;//PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LazerRdx[y] - ((MEANSTG_WIN_SIZE/2) - (PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LazerW[y]/2));
				unsigned short WinRltvLazerRdxStr = PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LazerRdx[y][PickId] - WinRdx;
				unsigned short WinRltvLazerRdxEnd = PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LazerRdx[y][PickId] + PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LazerW[y][PickId] - 1 - WinRdx;

				unsigned char JointWinRltvRdx = (unsigned char)(((WinRltvLazerRdxEnd & 0xf) << 4) | (WinRltvLazerRdxStr & 0xf));
				unsigned short RealRflctMsk = (PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_RflctMsk[y][PickId] & (~PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LiteRflct[y][PickId]));
				unsigned short Flag = ((RealRflctMsk << 1) | PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_OverlapMsk[y][PickId]);
				//				snprintf(TXTData, sizeof(TXTData), "%04x %04x %04x %01x", WinRdx, y, PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_Rid[y][PickId],  Flag);
				
				Luma_rgnData a_line;
				a_line.WinRdx = (uint16_t)WinRdx;
				a_line.y = (uint16_t)y;
				a_line.Rid = (uint16_t)PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_Rid[y][PickId];
				a_line.Flag = (uint8_t)Flag;
				a_line.PeakRltvRdx = (uint8_t)JointWinRltvRdx;
				for (int n = 0; n < RGN_DATA_WIN_SIZE; n++)
					a_line.data[n] = 0;
				//snprintf(TXTData, sizeof(TXTData), "%04x %04x %04x %01x %02x", WinRdx, y, PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_Rid[y][PickId], Flag, JointWinRltvRdx);
				//TXTFile << TXTData << std::endl;
				for (short WinPixCnt = 0; WinPixCnt < SV_WIN_SIZE; WinPixCnt++)//MEANSTG_WIN_SIZE; WinPixCnt++)
				{
					int PixIdx = y * FrmW + WinRdx + WinPixCnt;
					unsigned char PixVlu = 0;//SrcPixel[PixIdx];
					if (PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_RflctMsk[y][PickId] == 1 && PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LiteRflct[y][PickId] == 0)
					{
						if ((WinPixCnt >= (SV_WIN_SIZE / 2 - 1)) && (WinPixCnt <= (SV_WIN_SIZE / 2)))
						{
							PixVlu = 255;
						}
						else
						{
							PixVlu = 0;
						}
					}
					else
					{
						PixVlu = SrcPixel[PixIdx];
					}
					a_line.data[WinPixCnt] = (uint8_t)PixVlu;
					//snprintf(TXTData, sizeof(TXTData), "%02x", PixVlu);
					//TXTFile << TXTData << std::endl;
				}
				testData.push_back(a_line);
				WinNumPerLine++;
			}
		}

		if (LazerWinNum > (MAX_WIN_NUM - WinNumPerLine))
		{
			break;
		}

		LazerWinNum += WinNumPerLine;
	}
	return testData;
}

void genTestSream(
	std::vector<Luma_rgnData>& srcData,
	std::vector<RgnPix>& inStream,
	const FramePara inFramePara)
{
	RgnPix VSync = { 0,0,0,0,0,0,0 };
	uint_32 data_32;
	//1st:FrmNo
	data_32 = inFramePara.FrmNo;
	VSync.Sync = 0b10;
	VSync.LazerWinX = data_32 & 0xfff; //range(11, 0);
	data_32 = data_32 >> 12;
	VSync.LazerWinY = data_32 & 0xfff;
	data_32 = data_32 >> 12;
	VSync.LazerWinRid = data_32 & 0xff;
	inStream.push_back(VSync);
	//2nd:FrmNo
	data_32 = inFramePara.timeStamp;
	VSync.Sync = 0b00;
	VSync.LazerWinX = data_32 & 0xfff;
	data_32 = data_32 >> 12;
	VSync.LazerWinY = data_32 & 0xfff;
	data_32 = data_32 >> 12;
	VSync.LazerWinRid = data_32 & 0xff;
	inStream.push_back(VSync);
	//3rd:encInfo
	data_32 = inFramePara.encInfo;
	VSync.Sync = 0b00;
	VSync.LazerWinX = data_32 & 0xfff;
	data_32 = data_32 >> 12;
	VSync.LazerWinY = data_32 & 0xfff;
	data_32 = data_32 >> 12;
	VSync.LazerWinRid = data_32 & 0xff;
	inStream.push_back(VSync);
	//4th:frmW(12bit), frmH(12bit)
	ushort data16_1, data16_2;
	data16_1 = inFramePara.frameROI_w;
	data16_2 = inFramePara.frameROI_h;
	VSync.Sync = 0b00;
	VSync.LazerWinX = data16_1;
	VSync.LazerWinY = data16_2;
	inStream.push_back(VSync);
	//5th: frmX, WinNum
	int winNum = (int)srcData.size();
	data16_1 = inFramePara.frameROI_x;
	data16_2 = (uint16_t)winNum;
	VSync.Sync = 0b00;
	VSync.LazerWinX = data16_1;
	VSync.LazerWinY = data16_2;
	inStream.push_back(VSync);
	//6th: frmY
	data16_1 = inFramePara.frameROI_y;
	VSync.Sync = 0b00;
	VSync.LazerWinX = data16_1;
	VSync.LazerWinY = 0;
	inStream.push_back(VSync);
	//output data
	for (int i = 0; i < winNum; i++)
	{
		Luma_rgnData* a_line = &srcData[i];
		RgnPix Hsync;
		Hsync.LazerWinX = a_line->WinRdx;
		Hsync.LazerWinY = a_line->y;
		Hsync.LazerWinRid = a_line->Rid;
		Hsync.LazerWinFlag = a_line->Flag;
		Hsync.LazerWinRsv = 0;
		Hsync.RltvRdx = a_line->PeakRltvRdx;
		Hsync.Sync = 0b01;
		inStream.push_back(Hsync);
		for (int j = 0; j < RGN_DATA_WIN_SIZE / 2; j++)
		{
			RgnPix a_data = { 0,0,0,0,0,0,0 };
			uchar data8_1, data8_2;
			data8_1 = a_line->data[j * 2];
			data8_2 = a_line->data[j * 2 + 1];
			a_data.LazerWinX = data8_1;
			a_data.LazerWinY = data8_2;
			a_data.Sync = 0b00;
			inStream.push_back(a_data);
		}
	}
	return;
}

std::vector<CalibData> _combineSubpixCalibData(
	std::vector<RgnSubPix>& subpixPnt,
	LineCalibK lineCalibKx[MAX_HEIGHT],
	LineCalibK lineCalibKy[MAX_HEIGHT])
{
	std::vector<CalibData> outStream;
	//1st: timeStamp
	int readPtr = 0;
	CalibData 	OutData;
	RgnSubPix subpix = subpixPnt[readPtr++];
	OutData.Sync = subpix.Sync;
	Byte8 dataBuff;
	UintFloat tmpData;
	tmpData.fdata = subpix.x;
	dataBuff.nData[0] = tmpData.unData;
	OutData.calibK[0] = dataBuff.dData;
	OutData.calibK[1] = 0;
	outStream.push_back(OutData);
	//2nd: timeStamp
	subpix = subpixPnt[readPtr++];
	tmpData.fdata = subpix.x;
	dataBuff.nData[0] = tmpData.unData;
	OutData.Sync = subpix.Sync;
	OutData.calibK[0] = dataBuff.dData;
	outStream.push_back(OutData);
	//3rd:encInfo
	subpix = subpixPnt[readPtr++];
	OutData.Sync = subpix.Sync;
	tmpData.fdata = subpix.x;
	dataBuff.nData[0] = tmpData.unData;
	OutData.calibK[0] = dataBuff.dData;
	outStream.push_back(OutData);
	//4th: frmW, frmH
	subpix = subpixPnt[readPtr++];
	OutData.Sync = subpix.Sync;
	tmpData.fdata = subpix.x;
	dataBuff.nData[0] = tmpData.unData;
	OutData.calibK[0] = dataBuff.dData;
	outStream.push_back(OutData);
	//5th: frmX, WinNum
	subpix = subpixPnt[readPtr++];
	OutData.Sync = subpix.Sync;
	tmpData.fdata = subpix.x;
	dataBuff.nData[0] = tmpData.unData;
	int WinNum = (tmpData.unData >> 12) & 0x7ff;
	OutData.calibK[0] = dataBuff.dData;
	outStream.push_back(OutData);
	//6th: frmY
	subpix = subpixPnt[readPtr++];
	OutData.Sync = subpix.Sync;
	tmpData.fdata = subpix.x;
	dataBuff.nData[0] = tmpData.unData;
	OutData.calibK[0] = dataBuff.dData;
	outStream.push_back(OutData);

	for (int n = 0; n < WinNum; n++)
	{
		subpix = subpixPnt[readPtr++];
		uint_32 y = subpix.y;
		for (int j = 0; j <= 8; j++)
		{
			if (0 == j)//read subpix
			{
				OutData.Sync = 0x01;
				dataBuff.dData = 0;
				dataBuff.fData[0] = subpix.x;
				int tmpData = subpix.flag & 0x0f;
				tmpData <<= 11;
				tmpData += subpix.rid & 0x7ff;
				tmpData <<= 12;
				tmpData += subpix.y & 0xfff;
				dataBuff.nData[1] = (uint_32)tmpData;
				OutData.calibK[0] = dataBuff.dData;
				dataBuff.dData = 0;
				dataBuff.nData[0] = (uint_32)subpix.value;
				OutData.calibK[1] = dataBuff.dData;
				outStream.push_back(OutData);
			}
			else
			{
				OutData.Sync = 0x0;
				OutData.calibK[0] = lineCalibKx[y].calibK[j - 1];
				OutData.calibK[1] = lineCalibKy[y].calibK[j - 1];
				outStream.push_back(OutData);
			}
		}
	}
	return outStream;
}

void camAlgoSW(
	int inFrmH, 
	int inFrmW, 
	unsigned char* inData,
	ProcObj& frontendPara,
	FramePara& inFramePara,
	CamPara& calibPara,
	LineCalibK* lineCalibKx,
	LineCalibK* lineCalibKy,
#if _ALGO_MODULE_DEBUG
	std::vector<Luma_rgnData>& frontendData_debug,
	std::vector<RgnPix>& centeringPnt_debug,
	std::vector<RgnSubPix>& subpixPnt_debug,
	std::vector<RgnSubPixCalib>& calibSubpixPnt_debug,
#endif
	std::vector<Pnt3D>& resul3DPnts)
{

	PickLaserResult pFrontendResult;
	PickLazerWin(
		1,
		inFrmW,
		inFrmH,
		inData,

		frontendPara.MinStgTh,
		frontendPara.LazerWMin,
		frontendPara.LazerWMax,
		frontendPara.RflctPixTh,
		frontendPara.RflctOutEna,
		frontendPara.OverlapPixTh,
		frontendPara.EnhanStep,
		frontendPara.PickEna,
		frontendPara.RgnFltrTh,
		frontendPara.RmvWkEndEna,
		frontendPara.RmvWkEndTh,
		frontendPara.RmvWkEndMultCoe,
		frontendPara.RmvWkEndMinLen,
		frontendPara.PickBFEna,
		frontendPara.PickBkGrnd,
		frontendPara.PickRLenTh,

		frontendPara.EnergyPickEna,
		frontendPara.PickEnergyType,
		frontendPara.RgnEnergyPLen,
		frontendPara.RgnMeanETh,
		&pFrontendResult
	);
	//将前端数据转成输入数据格式
	std::vector<Luma_rgnData> frontendData = SvPickRgnRslt(
		inData,
		inFrmH,
		inFrmW,
		pFrontendResult
	);
#if _ALGO_MODULE_DEBUG
	frontendData_debug.insert(frontendData_debug.end(), frontendData.begin(), frontendData.end());
#endif

	std::vector<RgnPix> InRgnPnt;
	genTestSream(
		frontendData,
		InRgnPnt,
		inFramePara);
	//pkCentering
	std::vector<RgnPix> OutCenteringPnt;
	peakCentering(
		InRgnPnt,
		OutCenteringPnt
	);
#if _ALGO_MODULE_DEBUG
	centeringPnt_debug.insert(centeringPnt_debug.end(), OutCenteringPnt.begin(), OutCenteringPnt.end());
#endif

	//Convolve
	std::vector<RgnPixConvolve> OutConvolvePnt;
	convolve(
		OutCenteringPnt,
		OutConvolvePnt);

	// Subpix
	std::vector<RgnSubPix> OutSubpixPnt;
	subpix(
		OutConvolvePnt,
		OutSubpixPnt);
#if _ALGO_MODULE_DEBUG
	subpixPnt_debug.insert(subpixPnt_debug.end(), OutSubpixPnt.begin(), OutSubpixPnt.end());
#endif

	//将subpix转成RgnSubPixCalib格式。在硬件实现时这一步骤由硬件完成
	//数据格式：每行由行同步和后续的8拍构成。行同步传送x和y数据，后8拍依次传送KX0~KX7, 和KY0 ~ KY7
	std::vector<CalibData> inCalibData = _combineSubpixCalibData(
		OutSubpixPnt,
		lineCalibKx,
		lineCalibKy);
	//Calib
	std::vector<RgnSubPixCalib> outSubpixCalib;
	calib(
		inCalibData,
		outSubpixCalib
	);
#if _ALGO_MODULE_DEBUG
	calibSubpixPnt_debug.insert(calibSubpixPnt_debug.end(), outSubpixCalib.begin(), outSubpixCalib.end());
#endif

	// Compute3D
	compute3D(
		outSubpixCalib,
		resul3DPnts,
		calibPara.u0,
		calibPara.v0,
		calibPara.F_inv,
		calibPara.plane_a,
		calibPara.plane_b,
		calibPara.plane_c
	);
	return;
}