#include <fstream>
#include <cstring>
#include <stdio.h>
#include "ap_utils.h"
#include <iostream>
#include "calib.h"
#include "../../globals/tbGlobals.h"

Luma_frameInfo readParaFile(const char* file)
{
	std::ifstream inputFile(file);
	Luma_frameInfo frameInfo;
	std::string strLineTxt;

	int data;
	getline(inputFile, strLineTxt);  //XL
	sscanf(strLineTxt.c_str(), "XL:%d", &data);
	frameInfo.L = (uint16_t)data;

	getline(inputFile, strLineTxt);  //XR

	getline(inputFile, strLineTxt);  //TL
	sscanf(strLineTxt.c_str(), "TL:%d", &data);
	frameInfo.T = (uint16_t)data;

	getline(inputFile, strLineTxt);  //TR

	getline(inputFile, strLineTxt);  //W
	sscanf(strLineTxt.c_str(), "W:%d", &data);
	frameInfo.W = (uint16_t)data;

	getline(inputFile, strLineTxt);  //H
	sscanf(strLineTxt.c_str(), "H:%d", &data);
	frameInfo.H = (uint16_t)data;

	getline(inputFile, strLineTxt);  //FrmNO
	sscanf(strLineTxt.c_str(), "FrmNO:%x", &data);
	frameInfo.FrmNo = (uint32_t)data;

	getline(inputFile, strLineTxt);  //TimStp
	sscanf(strLineTxt.c_str(), "TimStp:%x", &data);
	frameInfo.TimeStamp = (uint32_t)data;

	getline(inputFile, strLineTxt);  //EncInfo
	sscanf(strLineTxt.c_str(), "EncInfo:%x", &data);
	frameInfo.encInfo = (uint32_t)data;

	inputFile.close();
	return frameInfo;
}

std::vector<RgnSubPix> readSubpixDataFile(
		const char* file,
		uint16_t* validSize)
{
	std::ifstream inputFile(file);

	std::string strLineTxt;
	int size = 0;
	std::vector<RgnSubPix> testData;
	RgnSubPix a_line;
	uint16_t validNum = 0;
	while(getline(inputFile, strLineTxt))
	{
		if (strLineTxt.empty())
			continue;

		float x;
		int y, Rid, Flag, pkValue, Sync;
#if defined(__linux__)
		sscanf(strLineTxt.c_str(), "%f %d %d %x %d %x", &x, &y, &Rid, &Flag, &pkValue, &Sync);
#else
		sscanf_s(strLineTxt.c_str(), "%f %d %d %x %d %x", &x, &y, &Rid, &Flag, &pkValue, &Sync);
#endif

		a_line.x = (float)x;
		a_line.y = (ap_uint<12>)y;
		a_line.rid = (ap_uint<11>)Rid;
		a_line.flag = (ap_uint<4>)Flag;
		a_line.value = (ap_uint<8>)pkValue;
		a_line.Sync = (ap_uint<2>)Sync;
		testData.push_back(a_line);
		if(x >= 0)
			validNum ++;
	}
	*validSize = validNum;
	inputFile.close();
	return testData;
}

void genCalibTestSream(
		std::vector<RgnSubPix>& subpixData,
		hls::stream<CalibData>& calibStream,
		const Luma_frameInfo framwInfo,
		const uint16_t validDataSize)
{
	CalibData VSync;
	Byte8 dataBuff;
	//1st:FrmNo
	dataBuff.nData[0] = framwInfo.FrmNo;
	dataBuff.nData[1] = 0;
	VSync.Sync = 0b10;
	VSync.calibK[0] = dataBuff.dData;
	VSync.calibK[1] = 0;
	calibStream.write(VSync);

	//2nd:FrmNo
	dataBuff.nData[0] = framwInfo.TimeStamp;
	VSync.Sync = 0b00;
	VSync.calibK[0] = dataBuff.dData;
	VSync.calibK[1] = 0;
	calibStream.write(VSync);
	//3rd:encInfo
	dataBuff.nData[0] = framwInfo.encInfo;
	VSync.Sync = 0b00;
	VSync.calibK[0] = dataBuff.dData;
	VSync.calibK[1] = 0;
	calibStream.write(VSync);
	//4th:frmW(12bit), frmH(12bit)
	ap_uint<16> data16_1, data16_2;
	data16_1 = framwInfo.W;
	data16_2 = framwInfo.H;
	ap_uint<32> tmpData;
	tmpData(11,0) = data16_1.range(11,0);
	tmpData(23,12) = data16_2.range(11,0);
	tmpData(31,24) = 0;
	dataBuff.nData[0] = (uint32_t)tmpData;
	VSync.calibK[0] = dataBuff.dData;
	VSync.calibK[1] = 0;
	VSync.Sync = 0b00;
	calibStream.write(VSync);
	//5th: frmX, WinNum
	int lines = (int)subpixData.size() / 2;  //lines
	data16_1 = framwInfo.L;
	data16_2 = (uint16_t)lines;
	tmpData(11,0) = data16_1.range(11,0);
	tmpData(23,12) = data16_2.range(11,0);
	tmpData(31,24) = 0;
	dataBuff.nData[0] = (uint32_t)tmpData;
	VSync.calibK[0] = dataBuff.dData;
	VSync.calibK[1] = 0;
	VSync.Sync = 0b00;
	calibStream.write(VSync);
	//6th: frmY
	data16_1 = framwInfo.T;
	data16_2 = validDataSize;
	tmpData(11,0) = data16_1.range(11,0);
	tmpData(23,12) = data16_2.range(11,0);
	tmpData(31,24) = 0;
	dataBuff.nData[0] = (uint32_t)tmpData;
	VSync.calibK[0] = dataBuff.dData;
	VSync.calibK[1] = 0;
	VSync.Sync = 0b00;
	calibStream.write(VSync);
	//output data
	for(int i = 0; i < lines; i ++)
	{
		if(i == lines-1)
			int kkk = 1; //debug point
		RgnSubPix a_line = subpixData[i*2];
		CalibData conData;

		dataBuff.fData[0] = a_line.x;
		ap_uint<32> apData32;
		apData32(11, 0) = a_line.y.range(11,0);
		apData32(22, 12) = a_line.rid.range(10,0);
		apData32(26,23) = a_line.flag(3,0);
		apData32(31,27) = 0;
		dataBuff.nData[1] = (uint32_t)apData32;
		conData.calibK[0] = dataBuff.dData;
		apData32(7,0) = a_line.value.range(7,0);
		apData32(31,8) = 0;
		dataBuff.nData[0] = (uint32_t)apData32;
		dataBuff.nData[1] = 0;
		conData.calibK[1] = dataBuff.dData;
		conData.Sync = a_line.Sync;
		calibStream.write(conData);

		a_line = subpixData[i*2+1];
		dataBuff.fData[0] = a_line.x;
		apData32(11, 0) = a_line.y.range(11,0);
		apData32(22, 12) = a_line.rid.range(10,0);
		apData32(26,23) = a_line.flag(3,0);
		apData32(31,27) = 0;
		dataBuff.nData[1] = (uint32_t)apData32;
		conData.calibK[0] = dataBuff.dData;
		apData32(7,0) = a_line.value.range(7,0);
		apData32(31,8) = 0;
		dataBuff.nData[0] = (uint32_t)apData32;
		dataBuff.nData[1] = 0;
		conData.calibK[1] = dataBuff.dData;
		conData.Sync = a_line.Sync;
		calibStream.write(conData);

		for(int m = 0; m < 4; m ++)  //write k
		{
			conData.Sync = 0b00;
			if(m == 0)
			{
				Byte8 dataConv_0;
				dataConv_0.fData[0] = 0.0;
				dataConv_0.fData[1] = 0.0;
				Byte8 dataConv_1;
				dataConv_1.fData[0] = 1.0;
				dataConv_1.fData[1] = 1.0;
				conData.calibK[0] = dataConv_0.dData;
				conData.calibK[1] = dataConv_1.dData;
			}
			else
			{
				Byte8 dataConv;
				dataConv.fData[0] = 0.0;
				dataConv.fData[1] = 0.0;
				conData.calibK[0] = dataConv.dData;
				conData.calibK[1] = dataConv.dData;
			}
			calibStream.write(conData);
		}
	}
	return;
}

void convertSubpixCalibStreamToArray(
		hls::stream<RgnSubPixCalib>& streamData,
		std::vector<Luma_rgnSubpixCalib>& arrayData,
		uint32_t* FrmNo,
		uint32_t* timeStamp,
		uint32_t* encInfo,
		uint16_t* frameROI_w,
		uint16_t* frameROI_h,
		uint16_t* frameROI_x,
		uint16_t* frameROI_y)
{
	//1st:FrmNo
	RgnSubPixCalib VSync = streamData.read();
	UintFloat tmp;
	tmp.fdata = VSync.x;
	*FrmNo = (uint32_t)tmp.unData;
	//2nd:timeStamp
	VSync = streamData.read();
	tmp.fdata = VSync.x;
	*timeStamp = (uint32_t)tmp.unData;
	//3rd:encInfo
	VSync = streamData.read();
	tmp.fdata = VSync.x;
	*encInfo = (uint32_t)tmp.unData;
	//4th:frmW(12bit), frmH(12bit)
	VSync = streamData.read();
	tmp.fdata = VSync.x;
	*frameROI_w = tmp.unData & 0xfff;
	*frameROI_h = (tmp.unData >> 12) & 0xfff;
	//5th: frmX, WinNum
	VSync = streamData.read();
	tmp.fdata = VSync.x;
	*frameROI_x = tmp.unData & 0xfff;
	uint16_t winNum = (tmp.unData >> 12) & 0xfff;
	//6th: frmY
	VSync = streamData.read();
	tmp.fdata = VSync.x;
	*frameROI_y = tmp.unData & 0xfff;
	//output data
	for(int i = 0; i < winNum; i ++)
	{
		if(i == winNum-1)
			int kkk = 1; //debug point
		RgnSubPixCalib data_in = streamData.read();
		Luma_rgnSubpixCalib a_pnt;
		a_pnt.x = data_in.x;
		a_pnt.y = data_in.y;
		a_pnt.Rid = (uint16_t)data_in.rid;
		a_pnt.Flag = (uint8_t)data_in.flag;
		a_pnt.pkValue = (uint8_t)data_in.value;
		a_pnt.sync = (uint8_t)data_in.Sync;
		arrayData.push_back(a_pnt);
	}
	return;
}

void writeSubpixCalibData(
		const char* fileName,
		std::vector<Luma_rgnSubpixCalib>& outData)
{
    std::ofstream sw(fileName);
    int i_max = outData.size();
    for (int i = 0; i < i_max; i++)
    {
        char data[250];
#if defined(__linux__)
        sprintf(data, "%.2f %.2f %04d %01x %02d %01x",
        		outData[i].x, outData[i].y, outData[i].Rid, outData[i].Flag, outData[i].pkValue, outData[i].sync);
#else
        sprintf_s(data, "%.2f %.2f %04d %01x %02d %01x",
        		outData[i].x, outData[i].y, outData[i].Rid, outData[i].Flag, outData[i].pkValue, outData[i].sync);
#endif
        sw << data << std::endl;
    }
    sw.close();
}

#define TST_GROUP	3

int main()
{
#if defined(__linux__)
	const char* SrcDataPath[TST_GROUP] = {
			"/home/zengHQ/CamTestData/testSample/PickRgn_C/PickRgn_Rslt/Txt/",
			"/home/zengHQ/CamTestData/Grp_0/PickRgn_C/PickRgn_Rslt/Txt/",
			"/home/zengHQ/CamTestData/Grp_1/PickRgn_C/PickRgn_Rslt/Txt/"
	};
	const char* InDataPath[TST_GROUP] = {
			"/home/zengHQ/CamTestData/testSample/Subpix/",
			"/home/zengHQ/CamTestData/Grp_0/Subpix/",
			"/home/zengHQ/CamTestData/Grp_1/Subpix/",
	};
	const char* OutDataPath[TST_GROUP] = {
			"/home/zengHQ/CamTestData/testSample/Calib/",
			"/home/zengHQ/CamTestData/Grp_0/Calib/",
			"/home/zengHQ/CamTestData/Grp_1/Calib/",
	};
#else
	const char* SrcDataPath[TST_GROUP] = {
			"E:/CamTestData/testSample/PickRgn_C/PickRgn_Rslt/Txt/",
			"E:/CamTestData/Grp_0/PickRgn_C/PickRgn_Rslt/Txt/",
			"E:/CamTestData/Grp_1/PickRgn_C/PickRgn_Rslt/Txt/"
	};
	const char* InDataPath[TST_GROUP] = {
			"E:/CamTestData/testSample/Subpix/",
			"E:/CamTestData/Grp_0/Subpix/",
			"E:/CamTestData/Grp_1/Subpix/",
	};
	const char* OutDataPath[TST_GROUP] = {
			"E:/CamTestData/testSample/Calib/",
			"E:/CamTestData/Grp_0/Calib/",
			"E:/CamTestData/Grp_1/Calib/",
	};
#endif
	int testFileSize[TST_GROUP] = {1, 4, 4};

	for(int n = 1; n < TST_GROUP; n ++)
	{
		int winSize = 0;
		int fileSize = testFileSize[n];
		for(int fi = 0; fi < fileSize; fi++)
		{
			char fileName[200];
			char paraFileName[200];
			char outFileName[200];
#if defined(__linux__)
			sprintf(fileName, "%s%d_L_subpix.txt", InDataPath[n], fi);
			sprintf(paraFileName, "%s%d_para_verilog.txt", SrcDataPath[n], fi);
			sprintf(outFileName, "%s%d_L_calib.txt", OutDataPath[n], fi);
#else
			sprintf_s(fileName, "%s%d_L_subpix.txt", InDataPath[n], fi);
			sprintf_s(paraFileName, "%s%d_para_verilog.txt", SrcDataPath[n], fi);
			sprintf_s(outFileName, "%s%d_L_calib.txt", OutDataPath[n], fi);
#endif
			Luma_frameInfo framwInfo = readParaFile(paraFileName);

			uint16_t vldSize = 0;
			std::vector<RgnSubPix> testData = readSubpixDataFile(fileName, &vldSize);
			//gen test stream
			hls::stream<CalibData> inStream;
			genCalibTestSream(testData, inStream, framwInfo, vldSize);

			hls::stream<RgnSubPixCalib> outStream;
			calib(inStream,outStream);

			std::vector<Luma_rgnSubpixCalib> outData;
			uint32_t outFrmNo, outTimeStamp, outEncInfo;
			uint16_t outROI_w,outROI_h, outROI_x,outROI_y;
			convertSubpixCalibStreamToArray(
					outStream,
					outData,
					&outFrmNo,
					&outTimeStamp,
					&outEncInfo,
					&outROI_w,
					&outROI_h,
					&outROI_x,
					&outROI_y);

			writeSubpixCalibData(outFileName, outData);
		}
	}
	printf("done!\n");
}