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

std::vector<RgnSubPixCalib> readSubpixCalibDataFile(
		const char* file)
{
	std::ifstream inputFile(file);
	std::string strLineTxt;
	int size = 0;
	std::vector<RgnSubPixCalib> testData;
	RgnSubPixCalib a_line;
	while(getline(inputFile, strLineTxt))
	{
		if (strLineTxt.empty())
			continue;

		float x,y;
		int Rid, Flag, pkValue;
#if defined(__linux__)
		sscanf(strLineTxt.c_str(), "%f %f %d %x %d", &x, &y, &Rid, &Flag, &pkValue);
#else
		sscanf_s(strLineTxt.c_str(), "%f %f %d %x %d", &x, &y, &Rid, &Flag, &pkValue);
#endif
		a_line.x = (float)x;
		a_line.y = (float)y;
		a_line.rid = (ap_uint<11>)Rid;
		a_line.flag = (ap_uint<4>)Flag;
		a_line.value = (ap_uint<8>)pkValue;
		testData.push_back(a_line);
	}
	inputFile.close();
	return testData;
}

void genCompute3DTestSream(
		std::vector<RgnSubPixCalib>& subpixCalibData,
		hls::stream<RgnSubPixCalib>& conpute3DStream,
		const uint32_t FrmNo,
		const uint32_t timeStamp,
		const uint32_t encInfo,
		const uint16_t frameROI_w,
		const uint16_t frameROI_h,
		const uint16_t frameROI_x,
		const uint16_t frameROI_y)
{
	RgnSubPixCalib VSync;
	UintFloat dataBuff;
	//1st:FrmNo
	dataBuff.unData = FrmNo;
	VSync.Sync = 0b10;
	VSync.x = dataBuff.fdata;
	VSync.y = 0;
	VSync.rid = 0;
	VSync.flag = 0;
	VSync.value = 0;
	VSync.rsv = 0;
	conpute3DStream.write(VSync);

	//2nd:FrmNo
	dataBuff.unData = timeStamp;
	VSync.Sync = 0b00;
	VSync.x = dataBuff.fdata;
	conpute3DStream.write(VSync);
	//3rd:encInfo
	dataBuff.unData = encInfo;
	VSync.Sync = 0b00;
	VSync.x = dataBuff.fdata;
	conpute3DStream.write(VSync);
	//4th:frmW(12bit), frmH(12bit)
	ap_uint<16> data16_1, data16_2;
	data16_1 = frameROI_w;
	data16_2 = frameROI_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.unData = (uint32_t)tmpData;
	VSync.x = dataBuff.fdata;
	VSync.Sync = 0b00;
	conpute3DStream.write(VSync);
	//5th: frmX, WinNum
	int winNum = (int)subpixCalibData.size();
	data16_1 = frameROI_x;
	data16_2 = (uint16_t)winNum;
	tmpData(11,0) = data16_1.range(11,0);
	tmpData(23,12) = data16_2.range(11,0);
	tmpData(31,24) = 0;
	dataBuff.unData = (uint32_t)tmpData;
	VSync.x = dataBuff.fdata;
	VSync.Sync = 0b00;
	conpute3DStream.write(VSync);
	//6th: frmY
	data16_1 = frameROI_y;
	data16_2 = 0;
	tmpData(11,0) = data16_1.range(11,0);
	tmpData(23,12) = data16_2.range(11,0);
	tmpData(31,24) = 0;
	dataBuff.unData = (uint32_t)tmpData;
	VSync.x = dataBuff.fdata;
	VSync.Sync = 0b00;
	conpute3DStream.write(VSync);
	//output data
	for(int i = 0; i < winNum; i ++)
	{
		RgnSubPixCalib a_line = subpixCalibData[i];
		if(i == 0)
			a_line.Sync = 0b01;
		else
			a_line.Sync = 0b00;
		a_line.rsv = 0;
		conpute3DStream.write(a_line);
	}
	return;
}

void convertSubpixCalibStreamToArray(
		hls::stream<Pnt3D>& streamData,
		std::vector<Pnt3D>& 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
	Pnt3D 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 ++)
	{
		Pnt3D data_in = streamData.read();
		Pnt3D a_pnt;
		a_pnt.x = data_in.x;
		a_pnt.y = data_in.y;
		a_pnt.z = data_in.z;
		a_pnt.value = (ap_uint<8>)data_in.value;
		a_pnt.Sync = 0;
		arrayData.push_back(a_pnt);
	}
	return;
}

void writeCompute3DData(
		const char* fileName,
		std::vector<Pnt3D>& outData)
{
    std::ofstream sw(fileName);
    int i_max = outData.size();
    for (int i = 0; i < i_max; i++)
    {
        char data[250];
        uint8_t value = (uint8_t)outData[i].value;
#if defined(__linux__)
        sprintf(data, "%.3f %.3f %.3f %02d",
        		outData[i].x, outData[i].y, outData[i].z, value);
#else
        sprintf_s(data, "%.3f %.3f %.3f %02d",
        		outData[i].x, outData[i].y, outData[i].z, value);
#endif
        sw << data << std::endl;
    }
    sw.close();
}

#define TST_GROUP	1

int main()
{
#if defined(__linux__)
	const char* InDataPath[TST_GROUP] = {
			"/home/zengHQ/CamTestData/testSample/Calib/testData.txt"
	};
	const char* OutDataPath[TST_GROUP] = {
			"/home/zengHQ/CamTestData/testSample/Compute3D/testData.txt"
	};
#else
	const char* InDataPath[TST_GROUP] = {
			"E:/CamTestData/testSample/Calib/testData.txt"
	};
	const char* OutDataPath[TST_GROUP] = {
			"E:/CamTestData/testSample/Compute3D/testData.txt"
	};
#endif

	for(int n = 0; n < TST_GROUP; n ++)
	{
		std::vector<RgnSubPixCalib> testData = readSubpixCalibDataFile(InDataPath[n]);
		//gen test stream
		hls::stream<RgnSubPixCalib> inStream;
		uint32_t FrmNo = 0x1;
		uint32_t timeStamp = 0x2;
		uint32_t encInfo = 0x3;
		uint16_t frameROI_w = 0x100;
		uint16_t frameROI_h = 2048;
		uint16_t frameROI_x = 0;
		uint16_t frameROI_y = 0;
		genCompute3DTestSream(testData, inStream, FrmNo, timeStamp, encInfo, frameROI_w, frameROI_h, frameROI_x, frameROI_y);

		float u0 = 620.1;
		float v0 = 1023.51;
		float F_inv = 0.0009933051235;
		float plane_a = -2.67733;
		float plane_b = 0.0501805;
		float plane_c = 631.944;
		hls::stream<Pnt3D> outStream;
		compute3D(inStream, outStream, u0, v0, F_inv, plane_a, plane_b, plane_c );

		//output
		std::vector<Pnt3D> 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);
		writeCompute3DData(OutDataPath[n], outData);
	}
	printf("done!\n");
}