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

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

		if(y == 580)
			int kkk = 1;

		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;
}

struct fittingPara
{
	float fittingPara[8];
};

void readFittingCalibParaFile(const char* file, std::vector<fittingPara>& fittingParas_x, std::vector<fittingPara>& fittingParas_y)
{
	std::ifstream inputFile(file);

	std::string strLineTxt;
	std::vector<fittingPara> fittingParas;
	while(getline(inputFile, strLineTxt))
	{
		if (strLineTxt.empty())
			continue;

		float k0_x, k1_x, k2_x, k3_x, k4_x, k5_x, k6_x, k7_x;
		float k0_y, k1_y, k2_y, k3_y, k4_y, k5_y, k6_y, k7_y;
#if defined(__linux__)
		sscanf(strLineTxt.c_str(), "%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
				&k0_x, &k1_x, &k2_x, &k3_x, &k4_x, &k5_x, &k6_x, &k7_x, &k0_y, &k1_y, &k2_y, &k3_y, &k4_y, &k5_y, &k6_y, &k7_y);
#else
		sscanf_s(strLineTxt.c_str(), "%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
				&k0_x, &k1_x, &k2_x, &k3_x, &k4_x, &k5_x, &k6_x, &k7_x, &k0_y, &k1_y, &k2_y, &k3_y, &k4_y, &k5_y, &k6_y, &k7_y);
#endif
		fittingPara a_para_x;
		a_para_x.fittingPara[0]= k0_x;
		a_para_x.fittingPara[1]= k1_x;
		a_para_x.fittingPara[2]= k2_x;
		a_para_x.fittingPara[3]= k3_x;
		a_para_x.fittingPara[4]= k4_x;
		a_para_x.fittingPara[5]= k5_x;
		a_para_x.fittingPara[6]= k6_x;
		a_para_x.fittingPara[7]= k7_x;
		fittingParas_x.push_back(a_para_x);

		fittingPara a_para_y;
		a_para_y.fittingPara[0]= k0_y;
		a_para_y.fittingPara[1]= k1_y;
		a_para_y.fittingPara[2]= k2_y;
		a_para_y.fittingPara[3]= k3_y;
		a_para_y.fittingPara[4]= k4_y;
		a_para_y.fittingPara[5]= k5_y;
		a_para_y.fittingPara[6]= k6_y;
		a_para_y.fittingPara[7]= k7_y;
		fittingParas_y.push_back(a_para_y);
	}
	inputFile.close();
	return;
}

void genCalibTestSream(
		std::vector<RgnSubPix>& subpixData,
		std::vector<fittingPara>& fittingParaX,
		std::vector<fittingPara>& fittingParaY,
		hls::stream<CalibData>& calibStream,
		int LRflag, //0-left image; 1-right image
		const Luma_frameInfo frameInfo,
		const uint16_t validDataSize)
{
	CalibData VSync;
	Byte8 dataBuff;
	//1st:FrmNo
	dataBuff.nData[0] = frameInfo.FrmNo * 2 + LRflag;
	dataBuff.nData[1] = 0;
	VSync.Sync = 0b10;
	VSync.calibK[0] = dataBuff.dData;
	VSync.calibK[1] = 0;
	calibStream.write(VSync);

	//2nd:FrmNo
	dataBuff.nData[0] = frameInfo.TimeStamp;
	VSync.Sync = 0b00;
	VSync.calibK[0] = dataBuff.dData;
	VSync.calibK[1] = 0;
	calibStream.write(VSync);
	//3rd:encInfo
	dataBuff.nData[0] = frameInfo.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 = frameInfo.W;
	data16_2 = frameInfo.H;
	ap_uint<32> tmpData;
	tmpData(CAM4K_Y_WIDTH-1,0) = data16_1.range(CAM4K_Y_WIDTH-1,0);
	tmpData(15, CAM4K_Y_WIDTH) = 0;
	tmpData(16+CAM4K_Y_WIDTH-1, 16) = data16_2.range(CAM4K_Y_WIDTH-1,0);
	tmpData(31,16+ CAM4K_Y_WIDTH)= 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
	if(LRflag == 0)
		data16_1 = frameInfo.left_L;
	else
		data16_1 = frameInfo.right_L;
	data16_2 = (uint16_t)lines;
	tmpData(CAM4K_Y_WIDTH-1,0) = data16_1.range(CAM4K_Y_WIDTH-1,0);
	tmpData(15, CAM4K_Y_WIDTH) = 0;
	tmpData(16+CAM4K_Y_WIDTH-1,16) = data16_2.range(CAM4K_Y_WIDTH-1,0);
	tmpData(31,16+CAM4K_Y_WIDTH) = 0;
	dataBuff.nData[0] = (uint32_t)tmpData;
	VSync.calibK[0] = dataBuff.dData;
	VSync.calibK[1] = 0;
	VSync.Sync = 0b00;
	calibStream.write(VSync);
	//6th: frmY
	uint16_t frmY;
	if(LRflag == 0)
	{
		data16_1 = frameInfo.left_T;
		frmY = frameInfo.left_T;;
	}
	else
	{
		data16_1 = frameInfo.right_T;
		frmY = frameInfo.right_T;
	}
	data16_2 = validDataSize;
	tmpData(CAM4K_Y_WIDTH-1,0) = data16_1.range(CAM4K_Y_WIDTH-1,0);
	tmpData(15, CAM4K_Y_WIDTH) = 0;
	tmpData(16+CAM4K_Y_WIDTH-1,16) = data16_2.range(CAM4K_Y_WIDTH-1,0);
	tmpData(31,16+CAM4K_Y_WIDTH) = 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];
		if(a_line.y == 580)
			int kkk = 1;
		CalibData conData;
		dataBuff.fData[0] = a_line.x;
		ap_uint<32> apData32;
		apData32(CAM4K_Y_WIDTH-1, 0) = a_line.y.range(CAM4K_Y_WIDTH-1,0);
		apData32(10+CAM4K_Y_WIDTH, CAM4K_Y_WIDTH) = a_line.rid.range(10,0);
		apData32(14+CAM4K_Y_WIDTH, 11+CAM4K_Y_WIDTH) = a_line.flag(3,0);
		apData32(31,15+CAM4K_Y_WIDTH) = 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(CAM4K_Y_WIDTH-1, 0) = a_line.y.range(CAM4K_Y_WIDTH-1,0);
		apData32(10+CAM4K_Y_WIDTH, CAM4K_Y_WIDTH) = a_line.rid.range(10,0);
		apData32(14+CAM4K_Y_WIDTH, 11+CAM4K_Y_WIDTH) = a_line.flag(3,0);
		apData32(31,15+CAM4K_Y_WIDTH) = 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);

		//Note: from K7 to K0
		for(int m = 3; m >=0 ; m --)  //write k
		{
			conData.Sync = 0b00;
			Byte8 dataConv_0;
			dataConv_0.fData[0] = fittingParaX[i+frmY].fittingPara[m*2 + 1];
			dataConv_0.fData[1] = fittingParaY[i+frmY].fittingPara[m*2 + 1];
			Byte8 dataConv_1;
			dataConv_1.fData[0] = fittingParaX[i+frmY].fittingPara[m*2];
			dataConv_1.fData[1] = fittingParaY[i+frmY].fittingPara[m*2];

			conData.calibK[0] = dataConv_0.dData;
			conData.calibK[1] = dataConv_1.dData;

			calibStream.write(conData);
		}
	}
	return;
}

void convertSubpixCalibStreamToArray(
		hls::stream<InterpolateCalibSubpix>& streamData,
		std::vector<Luma_rgnSubpix>& 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
	InterpolateCalibSubpix 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 & 0xffff;
	*frameROI_h = tmp.unData >> 16;
	//5th: frmX, WinNum
	VSync = streamData.read();
	tmp.fdata = VSync.x;
	*frameROI_x = tmp.unData & 0xffff;
	uint16_t winNum = tmp.unData >> 16;
	//6th: frmY
	VSync = streamData.read();
	tmp.fdata = VSync.x;
	*frameROI_y = tmp.unData & 0xffff;
	//output data
	for(int i = 0; i < winNum; i ++)
	{
		if(i == winNum-1)
			int kkk = 1; //debug point
		InterpolateCalibSubpix data_in = streamData.read();
		Luma_rgnSubpix a_pnt;
		a_pnt.x = data_in.x;
		a_pnt.y = (uint16_t)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_rgnSubpix>& 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 %04d %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 %04d %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();
}

int main()
{
	setvbuf(stdout, NULL, _IONBF, 0);   // _IONBF = ÎÞ»º³å£¬Á¢¼´Êä³ö
	TbModule currModule = TB_4_Calib;

	for(int n = 0; n < tb_group_count; n ++)
	{
		int winSize = 0;
		int fileStart = tb_groups[n].start_id;
		int fileEnd = tb_groups[n].end_id;
		for(int fi = fileStart; fi <= fileEnd; fi++)
		{
			TbPaths test_path_L;
			int rcode_L = tb_get_paths(currModule, n, fi, 0, &test_path_L);
			TbPaths test_path_R;
			int rcode_R = tb_get_paths(currModule, n, fi, 1, &test_path_R);
			TbFittingMapPaths test_fittingMap_path;
			int rcode = tb_get_fittingMap_paths(currModule, n, &test_fittingMap_path);

			Luma_frameInfo frameInfo = readParaFile(test_path_L.paras, fi);

			uint16_t vldSize_L = 0;
			std::vector<RgnSubPix> testData_L = readSubpixDataFile(test_path_L.input, &vldSize_L);
			uint16_t vldSize_R = 0;
			std::vector<RgnSubPix> testData_R = readSubpixDataFile(test_path_R.input, &vldSize_R);

			std::vector<fittingPara> fittingPara_x_L;
			std::vector<fittingPara> fittingPara_y_L;
			readFittingCalibParaFile(test_fittingMap_path.calib_fittingMap_L, fittingPara_x_L, fittingPara_y_L);
			std::vector<fittingPara> fittingPara_x_R;
			std::vector<fittingPara> fittingPara_y_R;
			readFittingCalibParaFile(test_fittingMap_path.calib_fittingMap_R, fittingPara_x_R, fittingPara_y_R);

			//gen test stream
			hls::stream<CalibData> inStream_L;
			genCalibTestSream(testData_L, fittingPara_x_L, fittingPara_y_L,  inStream_L, 0, frameInfo, vldSize_L);
			hls::stream<CalibData> inStream_R;
			genCalibTestSream(testData_R, fittingPara_x_R, fittingPara_y_R,  inStream_R, 1, frameInfo, vldSize_R);

			hls::stream<InterpolateCalibSubpix> outStream_L;
			calib(inStream_L, outStream_L);
			hls::stream<InterpolateCalibSubpix> outStream_R;
			calib(inStream_R, outStream_R);

			//output
			//left image
			std::vector<Luma_rgnSubpix> outData_L;
			uint32_t outFrmNo_L, outTimeStamp_L, outEncInfo_L;
			uint16_t outROI_w_L, outROI_h_L, outROI_x_L, outROI_y_L;
			convertSubpixCalibStreamToArray(
					outStream_L,
					outData_L,
					&outFrmNo_L,
					&outTimeStamp_L,
					&outEncInfo_L,
					&outROI_w_L,
					&outROI_h_L,
					&outROI_x_L,
					&outROI_y_L);
			writeSubpixCalibData(test_path_L.output, outData_L);
			//right image
			std::vector<Luma_rgnSubpix> outData_R;
			uint32_t outFrmNo_R, outTimeStamp_R, outEncInfo_R;
			uint16_t outROI_w_R,outROI_h_R, outROI_x_R,outROI_y_R;
			convertSubpixCalibStreamToArray(
					outStream_R,
					outData_R,
					&outFrmNo_R,
					&outTimeStamp_R,
					&outEncInfo_R,
					&outROI_w_R,
					&outROI_h_R,
					&outROI_x_R,
					&outROI_y_R);
			writeSubpixCalibData(test_path_R.output, outData_R);

			printf("%s ... done\n", test_path_L.input);
		}
	}
	printf("done!\n");
}