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

std::vector<LRMatchedPoint> readMatchedPointsFile(
		const char* file,
		uint16_t* minY,
		uint16_t* maxY)
{
	*minY = 0xffff;
	*maxY = 0;
	std::ifstream inputFile(file);
	std::string strLineTxt;
	int size = 0;
	std::vector<LRMatchedPoint> testData;
	LRMatchedPoint a_line;
	while(getline(inputFile, strLineTxt))
	{
		if (strLineTxt.empty())
			continue;

		float xL, xR;
		int y, valueL, valueR, match_seg_id, matchFlag, Sync;
#if defined(__linux__)
		sscanf(strLineTxt.c_str(), "%f %f %d %d %d %d %x %x",
				&xL, &xR, &y, &valueL, &valueR, &match_seg_id, &matchFlag, &Sync);
#else
		sscanf_s(strLineTxt.c_str(), "%f %f %d %d %d %d %x %x",
				&xL, &xR, &y, &valueL, &valueR, &match_seg_id, &matchFlag, &Sync);
#endif
		a_line.xL = xL;
		a_line.xR = xR;
		a_line.y = (ap_uint<CAM4K_Y_WIDTH>)y;
		a_line.valueL = (ap_uint<8>)valueL;
		a_line.valueR = (ap_uint<8>)valueR;
		a_line.match_seg_id = (ap_uint<CAM4K_Y_WIDTH>)match_seg_id;
		a_line.matchFlag = (ap_uint<2>)matchFlag;
		a_line.Sync = (ap_uint<2>)Sync;

		testData.push_back(a_line);

		if(*minY > y)
			*minY = y;
		if(*maxY < y)
			*maxY = y;
	}
	inputFile.close();
	return testData;
}

void genCompute3DTestSream(
		std::vector<LRMatchedPoint>& matchedPointsData,
		hls::stream<LRMatchedPoint>& matchedPointsStream,
		const Luma_frameInfo frameInfo,
		const uint16_t minY,
		const uint16_t maxY)
{
	LRMatchedPoint outWord;
	UintFloat dataBuff;
	//first
	dataBuff.unData = frameInfo.FrmNo;
	outWord.xL = dataBuff.fdata;
	dataBuff.unData = frameInfo.TimeStamp;
	outWord.xR = dataBuff.fdata;
	outWord.Sync = 0b10;
	matchedPointsStream.write(outWord);
	//second
	dataBuff.unData = frameInfo.encInfo;
	outWord.xL = dataBuff.fdata;
	dataBuff.unData = minY | (maxY << 16);
	outWord.xR = dataBuff.fdata;
	outWord.Sync = 0b00;
	matchedPointsStream.write(outWord);
	//3rd
	dataBuff.unData = (int)matchedPointsData.size();
	outWord.xL = dataBuff.fdata;
	outWord.xR = 0.0f;
	uint16_t total_match_size = (uint16_t)matchedPointsData.size();
	if(total_match_size == 0)
		outWord.Sync = 0b11;
	else
		outWord.Sync = 0b00;
	matchedPointsStream.write(outWord);

	//output data
	for(int i = 0; i < (int)matchedPointsData.size(); i ++)
	{
		LRMatchedPoint a_matchedPt = matchedPointsData[i];
		if(i == (int)matchedPointsData.size()-1 )
			a_matchedPt.Sync = 0b11;
		else
			a_matchedPt.Sync = 0b00;

		matchedPointsStream.write(a_matchedPt);
	}
	return;
}

void convertCompute3DStreamToArray(
		hls::stream<Pnt3D>& streamData,
		std::vector<Pnt3D>& arrayData,
		uint32_t* FrmNo,
		uint32_t* timeStamp,
		uint32_t* encInfo,
		uint16_t* data_size)
{
	Pnt3D syncData = streamData.read();
	UintFloat dataTmp;
	dataTmp.fdata = syncData.x;
	*FrmNo = dataTmp.unData;
	dataTmp.fdata = syncData.y;
	*timeStamp = dataTmp.unData;
	dataTmp.fdata = syncData.z;
	*encInfo = dataTmp.unData;

	syncData = streamData.read();
	dataTmp.fdata = syncData.x;
	*data_size = dataTmp.unData;
	uint16_t size = dataTmp.unData;

	//output data
	for(int i = 0; i < size; i ++)
	{
		Pnt3D data_in = streamData.read();
		arrayData.push_back(data_in);
	}
	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 valueL = (uint8_t)outData[i].valueL;
        uint8_t valueR = (uint8_t)outData[i].valueR;
        uint8_t matchFlag = (uint8_t)outData[i].matchFlag;
        uint8_t sync = (uint8_t)outData[i].Sync;
#if defined(__linux__)
        sprintf(data, "%.3f %.3f %.3f %03d %03d %01x %01x",
        		outData[i].x, outData[i].y, outData[i].z, valueL, valueR, matchFlag, sync);
#else
        sprintf_s(data, "%.3f %.3f %.3f %03d %03d %01x %01x",
        		outData[i].x, outData[i].y, outData[i].z, valueL, valueR, matchFlag, sync);
#endif
        sw << data << std::endl;
    }
    sw.close();
}

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

	for(int n = 0; n < tb_group_count; n ++)
	{
		int fileStart = tb_groups[n].start_id;
		int fileEnd = tb_groups[n].end_id;
		for(int fi = fileStart; fi <= fileEnd; fi++)
		{
			TbPaths test_path;
			int rcode = tb_get_paths(currModule, n, fi, 0, &test_path);

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

			uint16_t minY =0, maxY = 0;
			std::vector<LRMatchedPoint> testData = readMatchedPointsFile(test_path.input, &minY, &maxY);
			//gen test stream
			hls::stream<LRMatchedPoint> inStream;
			genCompute3DTestSream(testData, inStream, frameInfo, minY, maxY);

			float uL_offset = 710;
			float u0 = -frameInfo.u0;
			float v0 = -frameInfo.v0;
			ap_uint<8> noise_filter_len = 3;
			hls::stream<Pnt3D> outPt3D;
			compute3D(
				inStream,
				outPt3D,
				uL_offset,
				u0,
				v0,
				frameInfo.F,
				frameInfo.baseLen,
				0.0f,  //d = a*UL + b*V + c
				0.0f,
				0.0f,
				0.0f,  //d = r * (a*UR + b*V + c)
				noise_filter_len
			);

			//output
			std::vector<Pnt3D> outData;
			uint32_t outFrmNo, outTimeStamp, outEncInfo;
			uint16_t dataSize;
			convertCompute3DStreamToArray(
					outPt3D,
					outData,
					&outFrmNo,
					&outTimeStamp,
					&outEncInfo,
					&dataSize);
			writeCompute3DData(test_path.output, outData);

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