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

std::vector<SLRMatchOutput> readLRMatchDataFile(
		const char* file, uint16_t* minY, uint16_t* maxY)
{
	std::ifstream inputFile(file);
	std::string strLineTxt;
	std::vector<SLRMatchOutput> testData;
	SLRMatchOutput a_matchLine;
	int lineCounter = 0;
	bool first_valid_y = true;
	while(getline(inputFile, strLineTxt))
	{
		if (strLineTxt.empty())
			continue;

		int idx = lineCounter % 3;
		if(idx == 0)
		{
	    	int matchNum;
			int match1_seg_id, match1_flag, match1_L_idx, match1_R_idx, match1_ridL, match1_ridR;
	    	int match2_seg_id, match2_flag, match2_L_idx, match2_R_idx, match2_ridL, match2_ridR;
	    	int syncWord;
#if defined(__linux__)
	    	sscanf(strLineTxt.c_str(), "%d %d %x %x %x %d %x %x %x %x",
	    			&matchNum, &match1_seg_id, &match1_flag, &match1_L_idx, &match1_R_idx,
	    			&match2_seg_id, &match2_flag, &match2_L_idx, &match2_R_idx, &syncWord);
#else
	    	sscanf_s(strLineTxt.c_str(), "%01d %04d %01x %01x %01x %04d %04d %04d %01x %01x %01x %04d %04d %01x",
	        		&matchNum, &match1_seg_id, &match1_flag, &match1_L_idx, &match1_R_idx, &match1_ridL, &match1_ridR,
					&match2_seg_id, &match2_flag, &match2_L_idx, &match2_R_idx, &match2_ridL, &match2_ridR, &syncWord);
#endif

	    	a_matchLine.matchInfo.matchNum = (ap_uint<2>)matchNum;
	    	a_matchLine.matchInfo.match1.match_seg_id = (ap_uint<CAM4K_Y_WIDTH>)match1_seg_id;
	    	a_matchLine.matchInfo.match1.match_flag = (ap_uint<2>)match1_flag;  //1: L-R match; 2:left single; 3: right single
	    	a_matchLine.matchInfo.match1.pt_idx_L = (ap_uint<2>)match1_L_idx;
	    	a_matchLine.matchInfo.match1.pt_idx_R = (ap_uint<2>)match1_R_idx;
	    	a_matchLine.matchInfo.match1.rid_L = (ap_uint<11>)match1_ridL;
	    	a_matchLine.matchInfo.match1.rid_R = (ap_uint<11>)match1_ridR;

	    	a_matchLine.matchInfo.match2.match_seg_id = (ap_uint<CAM4K_Y_WIDTH>)match2_seg_id;
	    	a_matchLine.matchInfo.match2.match_flag = (ap_uint<2>)match2_flag;
	    	a_matchLine.matchInfo.match2.pt_idx_L = (ap_uint<2>)match2_L_idx;
	    	a_matchLine.matchInfo.match2.pt_idx_R = (ap_uint<2>)match2_R_idx;
	    	a_matchLine.matchInfo.match2.rid_L = (ap_uint<11>)match2_ridL;
	    	a_matchLine.matchInfo.match2.rid_R = (ap_uint<11>)match2_ridR;

	    	a_matchLine.syncWord = (ap_uint<2>)syncWord;
		}
		else if(idx == 1)
		{
			float x1, x2;
			int num;
	        int y1, rid1, value1, flag1, matchFlag1;
	        int y2, rid2, value2, flag2, matchFlag2;
#if defined(__linux__)
	    	sscanf(strLineTxt.c_str(), "%d %f %d %d %d %x %x %f %d %d %d %x %x",
	    			&num, &x1, &y1, &rid1, &value1, &flag1, &matchFlag1,
					&x2, &y2, &rid2, &value2, &flag2, &matchFlag2);
#else
	    	sscanf_s(strLineTxt.c_str(), "%d %f %d %d %d %x %x %f %d %d %d %x %x",
	    			&num, &x1, &y1, &rid1, &value1, &flag1, &matchFlag1,
					&x2, &y2, &rid2, &value2, &flag2, &matchFlag2);
#endif

	        a_matchLine.left_ptNum = (ap_uint<4>)num;

	        a_matchLine.left_pt1.x = x1;
	        a_matchLine.left_pt1.y = (ap_uint<CAM4K_Y_WIDTH>)y1;
	        a_matchLine.left_pt1.rid = (ap_uint<11>)rid1;
	        a_matchLine.left_pt1.value = (ap_uint<8>)value1;
	        a_matchLine.left_pt1.flag = (ap_uint<2>)flag1;
	        a_matchLine.left_pt1.matchFlag = (ap_uint<2>)matchFlag1;

	        a_matchLine.left_pt2.x = x2;
	        a_matchLine.left_pt2.y = (ap_uint<CAM4K_Y_WIDTH>)y2;
	        a_matchLine.left_pt2.rid = (ap_uint<11>)rid2;
	        a_matchLine.left_pt2.value = (ap_uint<8>)value2;
	        a_matchLine.left_pt2.flag = (ap_uint<2>)flag2;
	        a_matchLine.left_pt2.matchFlag = (ap_uint<2>)matchFlag2;
		}
		else  //idx == 2
		{
			float x1, x2;
			int num;
	        int y1, rid1, value1, flag1, matchFlag1;
	        int y2, rid2, value2, flag2, matchFlag2;
#if defined(__linux__)
	    	sscanf(strLineTxt.c_str(), "%d %f %d %d %d %x %x %f %d %d %d %x %x",
	    			&num, &x1, &y1, &rid1, &value1, &flag1, &matchFlag1,
					&x2, &y2, &rid2, &value2, &flag2, &matchFlag2);
#else
	    	sscanf_s(strLineTxt.c_str(), "%d %f %d %d %d %x %x %f %d %d %d %x %x",
	    			&num, &x1, &y1, &rid1, &value1, &flag1, &matchFlag1,
					&x2, &y2, &rid2, &value2, &flag2, &matchFlag2);
#endif

	        a_matchLine.right_ptNum = (ap_uint<4>)num;

	        a_matchLine.right_pt1.x = x1;
	        a_matchLine.right_pt1.y = (ap_uint<CAM4K_Y_WIDTH>)y1;
	        a_matchLine.right_pt1.rid = (ap_uint<11>)rid1;
	        a_matchLine.right_pt1.value = (ap_uint<8>)value1;
	        a_matchLine.right_pt1.flag = (ap_uint<2>)flag1;
	        a_matchLine.right_pt1.matchFlag = (ap_uint<2>)matchFlag1;

	        a_matchLine.right_pt2.x = x2;
	        a_matchLine.right_pt2.y = (ap_uint<CAM4K_Y_WIDTH>)y2;
	        a_matchLine.right_pt2.rid = (ap_uint<11>)rid2;
	        a_matchLine.right_pt2.value = (ap_uint<8>)value2;
	        a_matchLine.right_pt2.flag = (ap_uint<2>)flag2;
	        a_matchLine.right_pt2.matchFlag = (ap_uint<2>)matchFlag2;

	        uint16_t y = 0;
	        bool isValid = false;
	        if(a_matchLine.left_ptNum > 0)
	        {
	        	y = (uint16_t)a_matchLine.left_pt1.y;
	        	isValid = true;
	        }
	        else if(a_matchLine.right_ptNum > 0)
	        {
	        	y = (uint16_t)a_matchLine.right_pt1.y;
	        	isValid = true;
	        }
	        if(true == isValid)
	        {
	        	if(true == first_valid_y)
	        	{
	        		first_valid_y = false;
	        		*minY = y;
	        		*maxY = y;
	        	}
	        	else
	        		*maxY = y;
	        }

	        testData.push_back(a_matchLine);
		}
		lineCounter ++;
	}
	inputFile.close();
	return testData;
}

void genSSProcTestSream(
		std::vector<SLRMatchOutput>& LRMatchData,
		hls::stream<SLRMatchOutput>& LRMatchStream,
		const Luma_frameInfo frameInfo, const uint16_t minY, const uint16_t maxY)
{
	SLRMatchOutput VSync;
	UintFloat dataTmp;
	//1st:FrmNo
	dataTmp.unData = frameInfo.FrmNo;
	VSync.left_pt1.x = dataTmp.fdata;
	dataTmp.unData = frameInfo.TimeStamp;
	VSync.left_pt2.x = dataTmp.fdata;
	dataTmp.unData = frameInfo.encInfo;
	VSync.right_pt1.x = dataTmp.fdata;
	dataTmp.unData = minY | (maxY << 16);
	VSync.right_pt2.x = dataTmp.fdata;
	VSync.syncWord = 0b10;
	LRMatchStream.write(VSync);
	//output data
	for(uint16_t i = 0; i < (int)LRMatchData.size(); i ++)
	{
		SLRMatchOutput outWord = LRMatchData[i];
		if(i == ((int)LRMatchData.size() - 1))
			outWord.syncWord = 0b11;
		else
			outWord.syncWord = 0b00;
		LRMatchStream.write(outWord);
	}
	return;
}

void convertMatchedPtStreamToArray(
		hls::stream<LRMatchedPoint>& streamData,
		std::vector<LRMatchedPoint>& arrayData,
		uint32_t* FrmNo,
		uint32_t* timeStamp,
		uint32_t* encInfo,
		uint16_t* minY,
		uint16_t* maxY,
		uint16_t* total_match_size)
{
	LRMatchedPoint VSync = streamData.read();
	UintFloat dataTmp;
	//first
	dataTmp.fdata = VSync.xL;
	*FrmNo = dataTmp.unData;
	dataTmp.fdata = VSync.xR;
	*timeStamp = dataTmp.unData;
	//second
	VSync = streamData.read();
	dataTmp.fdata = VSync.xL;
	*encInfo = dataTmp.unData;
	dataTmp.fdata = VSync.xR;
	*minY = dataTmp.unData & 0xffff;
	*maxY = dataTmp.unData >> 16;
	//3rd
	VSync = streamData.read();
	dataTmp.fdata = VSync.xL;
	*total_match_size = dataTmp.unData;
	uint16_t size = dataTmp.unData;
	if(VSync.Sync == 0b11)
		return;

	//output data
	for(int i = 0; i < size; i ++)
	{
		LRMatchedPoint data_in = streamData.read();
		arrayData.push_back(data_in);
		if(data_in.Sync == 0b11)
			break;
	}
	return;
}

void writeMatchedPoints(
		const char* fileName,
		std::vector<LRMatchedPoint>& outData)
{
    std::ofstream sw(fileName);
    int i_max = (int)outData.size();
    for (int i = 0; i < i_max; i++)
    {
        char data[250];

        float xL = outData[i].xL;
        float xR = outData[i].xR;
        uint16_t y = (uint16_t)outData[i].y;  //may exceed 2048
        uint16_t valueL = (uint16_t)outData[i].valueL;
        uint16_t valueR = (uint16_t)outData[i].valueR;
        uint16_t match_seg_id = (uint16_t)outData[i].match_seg_id;
        uint16_t matchFlag = (uint16_t)outData[i].matchFlag;
        uint16_t Sync = (uint16_t)outData[i].Sync;			//FrmSync:b10 | LineSync:b01

#if defined(__linux__)
        sprintf(data, "%.3f %.3f %04d %03d %03d %04d %01x %01x",
        		xL, xR, y, valueL, valueR, match_seg_id, matchFlag, Sync);
#else
        sprintf_s(data, "%.3f %.3f %04d %03d %03d %04d %01x %01x",
        		xL, xR, y, valueL, valueR, match_seg_id, matchFlag, Sync);
#endif
        sw << data << std::endl;
    }
    sw.close();
}

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

	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<SLRMatchOutput> testData = readLRMatchDataFile(test_path.input, &minY, &maxY);

			//gen test stream
			hls::stream<SLRMatchOutput> LRMatchStream;
			genSSProcTestSream(testData, LRMatchStream, frameInfo, minY, maxY);

			ap_uint<1> enable_SS_proc = 0;
			hls::stream<LRMatchedPoint> outMatchedPtStream;
			SSProcAndNF(LRMatchStream, outMatchedPtStream, enable_SS_proc);

			//output
			std::vector<LRMatchedPoint> outData;
			uint32_t outFrmNo, outTimeStamp, outEncInfo;
			uint16_t outMinY=0, outMaxY=0, total_match_size=0;
			convertMatchedPtStreamToArray(
					outMatchedPtStream,
					outData,
					&outFrmNo,
					&outTimeStamp,
					&outEncInfo,
					&outMinY,
					&outMaxY,
					&total_match_size);

			writeMatchedPoints(test_path.output, outData);

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