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

void readSubpixCalibDataFile(
		const char* file, std::vector<InterpolateCalibSubpix>& testData)
{
	std::ifstream inputFile(file);
	std::string strLineTxt;
	int size = 0;
	InterpolateCalibSubpix a_line;
	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<CAM4K_Y_WIDTH>)y;
		a_line.rid = (ap_uint<11>)Rid;
		a_line.value = (ap_uint<8>)pkValue;
		a_line.flag = (ap_uint<2>)Flag;
		a_line.Sync = (ap_uint<2>)Sync;
		testData.push_back(a_line);
	}
	inputFile.close();
	return;
}

void genLRMatchTestSream(
		std::vector<InterpolateCalibSubpix>& inCalibData,
		hls::stream<InterpolateCalibSubpix>& inCalibStream,
		int LRflag, //0-left image; 1-right image
		const Luma_frameInfo frameInfo)
{
	InterpolateCalibSubpix inData = {0.0f, 0, 0, 0, 0, 0};
	UintFloat tmp;
	//1st:FrmNo
	inData.Sync = 0b10;
	tmp.unData = frameInfo.FrmNo;
	inData.x = tmp.fdata;
	inCalibStream.write(inData);
	//2nd: timeStamp
	inData.Sync = 0b00;
	tmp.unData = frameInfo.TimeStamp;
	inData.x = tmp.fdata;
	inCalibStream.write(inData);
	//3rd:encInfo
	tmp.unData = frameInfo.encInfo;
	inData.x = tmp.fdata;
	inCalibStream.write(inData);
	//4th: frmW, frmH
	tmp.unData = (frameInfo.H << 16)| frameInfo.W;
	inData.x = tmp.fdata;
	inCalibStream.write(inData);
	//5th: frmX, WinNum
	uint16_t total_point_num = (uint16_t)inCalibData.size();
	tmp.unData = total_point_num << 16;
	if(LRflag == 0)
		tmp.unData = tmp.unData |frameInfo.left_L;
	else
		tmp.unData = tmp.unData |frameInfo.right_L;
	inData.x = tmp.fdata;
	inCalibStream.write(inData);
	//6th: frmY
	if(LRflag == 0)
		tmp.unData = frameInfo.left_T;
	else
		tmp.unData = frameInfo.right_T;
	inData.x = tmp.fdata;
	if(total_point_num == 0)
		inData.Sync = 0b11;
	else
		inData.Sync = 0b00;
	inCalibStream.write(inData);
	//output data
	if(total_point_num > 0)
	{
		for(int i = 0; i < total_point_num; i ++)
		{
			if(i == total_point_num-1)
				int kkk = 1; //debug point

			inData = inCalibData[i];
			if(i == total_point_num-1)
				inData.Sync = 0b11;
			else
				inData.Sync = 0b00;
			inCalibStream.write(inData);
		}
	}
	return;
}

void convertSubpixCalibStreamToArray(
		hls::stream<SLRMatchOutput>& streamData,
		std::vector<SLRMatchOutput>& arrayData,
		uint32_t* FrmNo,
		uint32_t* timeStamp,
		uint32_t* encInfo,
		uint16_t* minY,
		uint16_t* maxY)
{
	SLRMatchOutput VSync = streamData.read();
	UintFloat tmp;
	tmp.fdata = VSync.left_pt1.x;
	*FrmNo = tmp.unData;
	tmp.fdata = VSync.left_pt2.x;
	*timeStamp = tmp.unData;
	tmp.fdata = VSync.right_pt1.x;
	*encInfo = tmp.unData;
	tmp.fdata = VSync.right_pt2.x;
	*minY = tmp.unData & 0xffff;
	*maxY = tmp.unData >> 16;

	uint16_t ptNum = *maxY - *minY + 1;
	//output data
	for(int i = 0; i < ptNum; i ++)
	{
		SLRMatchOutput data_in = streamData.read();
		arrayData.push_back(data_in);
		if(data_in.syncWord == 0b11)
			break;
	}
	return;
}

void writeLRMatchData(
		const char* fileName,
		std::vector<SLRMatchOutput>& outData)
{
    std::ofstream sw(fileName);

    int i_max = outData.size();
    for (int i = 0; i < i_max; i++)
    {
        char data[250];

        SLRMatchOutput a_matchPt = outData[i];

    	int matchNum = a_matchPt.matchInfo.matchNum;
    	int match1_seg_id = a_matchPt.matchInfo.match1.match_seg_id;
    	int match1_flag = a_matchPt.matchInfo.match1.match_flag;  //1: L-R match; 2:left single; 3: right single
    	int match1_L_idx = a_matchPt.matchInfo.match1.pt_idx_L;
    	int match1_R_idx = a_matchPt.matchInfo.match1.pt_idx_R;
    	int match1_ridL = a_matchPt.matchInfo.match1.rid_L;
    	int match1_ridR = a_matchPt.matchInfo.match1.rid_R;

    	int match2_seg_id = a_matchPt.matchInfo.match2.match_seg_id;
    	int match2_flag = a_matchPt.matchInfo.match2.match_flag;
    	int match2_L_idx = a_matchPt.matchInfo.match2.pt_idx_L;
    	int match2_R_idx = a_matchPt.matchInfo.match2.pt_idx_R;
    	int match2_ridL = a_matchPt.matchInfo.match2.rid_L;
    	int match2_ridR = a_matchPt.matchInfo.match2.rid_R;
    	int syncWord = a_matchPt.syncWord;
#if defined(__linux__)
        sprintf(data, "%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);
#else
        sprintf_s(data, "%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
        sw << data << std::endl;

        int num = (int)a_matchPt.left_ptNum;

        int y1 = a_matchPt.left_pt1.y;
        int rid1 = a_matchPt.left_pt1.rid;
        int value1 = a_matchPt.left_pt1.value;
        int flag1 = a_matchPt.left_pt1.flag;
        int matchFlag1 = a_matchPt.left_pt1.matchFlag;

        int y2 = a_matchPt.left_pt2.y;
        int rid2 = a_matchPt.left_pt2.rid;
        int value2 = a_matchPt.left_pt2.value;
        int flag2 = a_matchPt.left_pt2.flag;
        int matchFlag2 = a_matchPt.left_pt2.matchFlag;

#if defined(__linux__)
        sprintf(data, "%d %.3f %04d %04d %03d %01x %01x %.3f %04d %04d %03d %01x %01x",
        		num, a_matchPt.left_pt1.x, y1, rid1, value1, flag1, matchFlag1, a_matchPt.left_pt2.x, y2, rid2, value2, flag2, matchFlag2);
#else
        sprintf_s(data, "%d %.3f %04d %04d %03d %01x %01x %.3f %04d %04d %03d %01x %01x",
        		num, a_matchPt.left_pt1.x, y1, rid1, value1, flag1, matchFlag1, a_matchPt.left_pt2.x, y2, rid2, value2, flag2, matchFlag2);
#endif
        sw << data << std::endl;

        num = (int)a_matchPt.right_ptNum;

        y1 = a_matchPt.right_pt1.y;
        rid1 = a_matchPt.right_pt1.rid;
        value1 = a_matchPt.right_pt1.value;
        flag1 = a_matchPt.right_pt1.flag;
        matchFlag1 = a_matchPt.right_pt1.matchFlag;

        y2 = a_matchPt.right_pt2.y;
        rid2 = a_matchPt.right_pt2.rid;
        value2 = a_matchPt.right_pt2.value;
        flag2 = a_matchPt.right_pt2.flag;
        matchFlag2 = a_matchPt.right_pt2.matchFlag;

#if defined(__linux__)
        sprintf(data, "%d %.3f %04d %04d %03d %01x %01x %.3f %04d %04d %03d %01x %01x",
        		num, a_matchPt.right_pt1.x, y1, rid1, value1, flag1, matchFlag1, a_matchPt.right_pt2.x, y2, rid2, value2, flag2, matchFlag2);
#else
        sprintf_s(data, "%d %.3f %04d %04d %03d %01x %01x %.3f %04d %04d %03d %01x %01x",
        		num, a_matchPt.right_pt1.x, y1, rid1, value1, flag1, matchFlag1, a_matchPt.right_pt2.x, y2, rid2, value2, flag2, matchFlag2);
#endif
        sw << data << std::endl;
    }
    sw.close();
}

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

	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_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);

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

			std::vector<InterpolateCalibSubpix> testData_L;
			readSubpixCalibDataFile(test_path_L.input, testData_L);
			std::vector<InterpolateCalibSubpix> testData_R;
			readSubpixCalibDataFile(test_path_R.input, testData_R);

			hls::stream<InterpolateCalibSubpix> inStream_L;
			genLRMatchTestSream(testData_L,	inStream_L,	0, frameInfo);
			hls::stream<InterpolateCalibSubpix> inStream_R;
			genLRMatchTestSream(testData_R,	inStream_R,	1, frameInfo);

			hls::stream<SLRMatchOutput> matchPoints;
			float uL_offset = 710;
			uint16_t useLaserPlane = 0;  //0- disable; 1- enable
			float plane_a_f = 0.0f, plane_b_f=0.0f, plane_c_f= 0.0f, plane_norm=0.0f;   //d = a*UL + b*V + c
			float maxPtPlaneDist = 3.0;  //point to plane distance threshod=ld
			lrMatch(inStream_L, inStream_R, matchPoints,
					uL_offset,
					useLaserPlane, plane_a_f, plane_b_f, plane_c_f, plane_norm, //d = a*UL + b*V + c
					maxPtPlaneDist  //point to plane distance threshod=ld
			);

			//output
			std::vector<SLRMatchOutput> outData;
			uint32_t outFrmNo, outTimeStamp, outEncInfo;
			uint16_t minY, maxY;
			convertSubpixCalibStreamToArray(
					matchPoints,
					outData,
					&outFrmNo,
					&outTimeStamp,
					&outEncInfo,
					&minY,
					&maxY);
			writeLRMatchData(test_path_L.output, outData);

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