#include "lrMatch.h"
#include "stdlib.h"
#include <cstdint>
#include <cmath>
#include <cfloat>

void read_left(
		hls::stream<InterpolateCalibSubpix> 	&inSubpix_L,
		SRowData left_buf[ROW_COUNT],
		ap_uint<42> leftState[ROW_COUNT],
		uint16_t* roiL_L,
		uint16_t* roiL_T,
		uint16_t* roiL_W,
		uint16_t* roiL_H,
		uint16_t* left_min_y,
		uint16_t* left_max_y,
		uint32_t* roiL_FrmNo,
		uint32_t* roiL_TimeStamp,
		uint32_t* roiL_encInfo,
		ap_uint<40>* roiL_frameTag)
{
#pragma HLS INLINE off

	InterpolateCalibSubpix 	InData;
	ap_uint<2> VSync;
	while(1)  //wait for vSync signal
	{
		InData = inSubpix_L.read();
		VSync = InData.Sync;
		if(0b10 == VSync)  //wait for syncFirst
			break;
	}

	//1st: frameNo
	UintFloat tmp;
	tmp.fdata = InData.x;
	*roiL_FrmNo = tmp.unData;
	//2nd: timeStamp
	InData = inSubpix_L.read();
	tmp.fdata = InData.x;
	*roiL_TimeStamp = tmp.unData;
	//3rd:encInfo
	InData = inSubpix_L.read();
	tmp.fdata = InData.x;
	*roiL_encInfo = tmp.unData;
	//4th: frmW, frmH
	InData = inSubpix_L.read();
	tmp.fdata = InData.x;
	*roiL_W = tmp.unData & 0xffff;
	*roiL_H = tmp.unData >> 16;
	//5th: frmX, WinNum
	InData = inSubpix_L.read();
	tmp.fdata = InData.x;
	*roiL_L = tmp.unData & 0xffff;
	uint16_t winNum = tmp.unData >> 16;
	//6th: frmY
	InData = inSubpix_L.read();
	tmp.fdata = InData.x;
	*roiL_T = tmp.unData & 0xffff;

	static ap_uint<40> leftFrameGeneration = 0;
#pragma HLS RESET variable=leftFrameGeneration off

	const ap_uint<40> max_generation = 0xffffffffffULL;
	if (leftFrameGeneration == max_generation)
	{
	LEFT_GENERATION_WRAP_CLEAR:
	    for (unsigned row = 0; row < ROW_COUNT; ++row)
	    {
	#pragma HLS PIPELINE II=1
	        leftState[row] = 0;
	    }

	    // 清零完成后再发布 generation 1。
	    leftFrameGeneration = 1;
	}
	else
	{
	    leftFrameGeneration = leftFrameGeneration + 1;
	}

	const ap_uint<40> currFrameTag = leftFrameGeneration;

	uint16_t min_y = 0xffff;
	uint16_t max_y = 0;

	uint16_t previousY = 0xffff;
	for(uint16_t n = 0; n < winNum; n ++)
	{
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
#pragma HLS PIPELINE II=1

		InData = inSubpix_L.read();
		BuffCalibSubpix pkPt;
		pkPt.x = InData.x;
		pkPt.y = InData.y;
		pkPt.rid = InData.rid;
		pkPt.value = InData.value;
		pkPt.flag = InData.flag;
		pkPt.matchFlag = 0;
		uint16_t y = (uint16_t)InData.y;

		if (y < ROW_COUNT)
		{
			const bool firstPoint = (y != previousY);
			const bool secondPoint = (y == previousY) ;

		    if (firstPoint)
		        left_buf[y].pt1 = pkPt;
		    else if (secondPoint)
		        left_buf[y].pt2 = pkPt;

		    // 合法状态只有0、1、2：第一次写后为1，其余饱和为2。
		    ap_uint<42> newState = 0;
		    newState.range(41, 2) = currFrameTag;
		    newState.range(1, 0) = firstPoint ? ap_uint<2>(1) : ap_uint<2>(2);
		    leftState[y] = newState;
		    if (min_y > y) min_y = y;
		    if (max_y < y) max_y = y;

		    previousY = y;
		}

		if(0b11 == InData.Sync)
		{
			break;
		}
	}
	*left_min_y = min_y;
	*left_max_y = max_y;
	*roiL_frameTag = currFrameTag;
	return;
}

void read_right(
		hls::stream<InterpolateCalibSubpix> 	&inSubpix_R,
		SRowData right_buf[ROW_COUNT],
		ap_uint<42> rightState[ROW_COUNT],
		uint16_t* roiR_L,
		uint16_t* roiR_T,
		uint16_t* roiR_W,
		uint16_t* roiR_H,
		uint16_t* right_min_y,
		uint16_t* right_max_y,
		uint32_t* roiR_FrmNo,
		uint32_t* roiR_TimeStamp,
		uint32_t* roiR_encInfo,
		ap_uint<40>* roiR_frameTag)
{
#pragma HLS INLINE off
	InterpolateCalibSubpix 	InData;
	ap_uint<2> VSync;
	while(1)  //wait for vSync signal
	{
		InData = inSubpix_R.read();
		VSync = InData.Sync;
		if(0b10 == VSync)  //wait for syncFirst
			break;
	}

	//1st: frameNo
	UintFloat tmp;
	tmp.fdata = InData.x;
	*roiR_FrmNo = tmp.unData;
	//2nd: timeStamp
	InData = inSubpix_R.read();
	tmp.fdata = InData.x;
	*roiR_TimeStamp = tmp.unData;
	//3rd:encInfo
	InData = inSubpix_R.read();
	tmp.fdata = InData.x;
	*roiR_encInfo = tmp.unData;
	//4th: frmW, frmH
	InData = inSubpix_R.read();
	tmp.fdata = InData.x;
	*roiR_W = tmp.unData & 0xffff;
	*roiR_H = tmp.unData >> 16;
	//5th: frmX, WinNum
	InData = inSubpix_R.read();
	tmp.fdata = InData.x;
	*roiR_L = tmp.unData & 0xffff;
	uint16_t winNum = tmp.unData >> 16;
	//6th: frmY
	InData = inSubpix_R.read();
	tmp.fdata = InData.x;
	*roiR_T = tmp.unData & 0xffff;

	// 与 rightState 一起保留，运行时复位不清零。
	static ap_uint<40> rightFrameGeneration = 0;
	#pragma HLS RESET variable=rightFrameGeneration off

	const ap_uint<40> max_generation = 0xffffffffffULL;
	if (rightFrameGeneration == max_generation)
	{
	RIGHT_GENERATION_WRAP_CLEAR:
	    for (unsigned row = 0; row < ROW_COUNT; ++row)
	    {
	#pragma HLS PIPELINE II=1
	        rightState[row] = 0;
	    }
	    // 清零完成后再发布 generation 1。
	    rightFrameGeneration = 1;
	}
	else
	{
	    rightFrameGeneration = rightFrameGeneration + 1;
	}

	const ap_uint<40> currFrameTag = rightFrameGeneration;

	uint16_t min_y = 0xffff;
	uint16_t max_y = 0;

	uint16_t previousY = 0xffff;
	for(uint16_t n = 0; n < winNum; n ++)
	{
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
#pragma HLS PIPELINE II=1

		InData = inSubpix_R.read();
		BuffCalibSubpix pkPt;
		pkPt.x = InData.x;
		pkPt.y = InData.y;
		pkPt.rid = InData.rid;
		pkPt.value = InData.value;
		pkPt.flag = InData.flag;
		pkPt.matchFlag = 0;

		uint16_t y = (uint16_t)InData.y;
		if (y < ROW_COUNT)
		{
			const bool firstPoint = (y != previousY);
			const bool secondPoint = (y == previousY) ;

			if (firstPoint)
		    	right_buf[y].pt1 = pkPt;
		    else if (secondPoint)
		    	right_buf[y].pt2 = pkPt;

		    // 合法状态只有0、1、2：第一次写后为1，其余饱和为2。
		    ap_uint<42> newState = 0;
		    newState.range(41, 2) = currFrameTag;
		    newState.range(1, 0) =
		        firstPoint ? ap_uint<2>(1) : ap_uint<2>(2);

		    rightState[y] = newState;

		    if (min_y > y) min_y = y;
		    if (max_y < y) max_y = y;

		    previousY = y;
		}

		if(0b11 == InData.Sync)
		{
			break;
		}
	}
	*right_min_y = min_y;
	*right_max_y = max_y;
	*roiR_frameTag = currFrameTag;
	return;
}

void receive_lr(
		hls::stream<InterpolateCalibSubpix> 	&inSubpix_L,
		SRowData left_buf[ROW_COUNT],
		ap_uint<42> leftState[ROW_COUNT],
		uint16_t* roiL_L,
		uint16_t* roiL_T,
		uint16_t* roiL_W,
		uint16_t* roiL_H,
		uint16_t* left_min_y,
		uint16_t* left_max_y,
		uint32_t* roiL_FrmNo,
		uint32_t* roiL_TimeStamp,
		uint32_t* roiL_encInfo,
		ap_uint<40>* roiL_frameTag,

		hls::stream<InterpolateCalibSubpix> 	&inSubpix_R,
		SRowData right_buf[ROW_COUNT],
		ap_uint<42> rightState[ROW_COUNT],
		uint16_t* roiR_L,
		uint16_t* roiR_T,
		uint16_t* roiR_W,
		uint16_t* roiR_H,
		uint16_t* right_min_y,
		uint16_t* right_max_y,
		uint32_t* roiR_FrmNo,
		uint32_t* roiR_TimeStamp,
		uint32_t* roiR_encInfo,
		ap_uint<40>* roiR_frameTag)
{
#pragma HLS INLINE off
#pragma HLS DATAFLOW
	read_left(inSubpix_L, left_buf, leftState,
			roiL_L, roiL_T, roiL_W, roiL_H, left_min_y, left_max_y,
			roiL_FrmNo, roiL_TimeStamp, roiL_encInfo, roiL_frameTag);
	read_right(inSubpix_R, right_buf, rightState,
			roiR_L, roiR_T, roiR_W, roiR_H, right_min_y, right_max_y,
			roiR_FrmNo, roiR_TimeStamp, roiR_encInfo, roiR_frameTag);
}

float _computePtPlaneDist(
		float xL, float xR, float y,float uL_offset,
		float plane_a_f, float plane_b_f, float plane_c_f, //d = a*UL + b*V + c
		float plane_norm
)
{
	float dist = FLT_MAX;
	float disparity = xL + uL_offset - xR;
	if(disparity <= 10)
	{
		return dist;
	}
	dist = xL * plane_a_f + y * plane_b_f - disparity + plane_c_f;
	if(dist < 0)
		dist = -dist;
	dist = dist * plane_norm;
	return dist;
}

void lrMatch(
	hls::stream<InterpolateCalibSubpix> 	&inSubpix_L,
	hls::stream<InterpolateCalibSubpix> 	&inSubpix_R,
	hls::stream<SLRMatchOutput> 			&outMatchPt,
	float uL_offset,
	uint16_t useLaserPlane,  //0- disable; 1- enable
	float plane_a_f,  //d = a*UL + b*V + c
	float plane_b_f,
	float plane_c_f,
	float plane_norm,
	float maxPtPlaneDist  //point to plane distance threshod=ld
)
{
#if !(VITIS_HLS)
#pragma HLS DATA_PACK variable=inSubpix_L
#pragma HLS DATA_PACK variable=inSubpix_R
#pragma HLS DATA_PACK variable=outMatchPt
#else
#pragma HLS aggregate variable=inSubpix_L compact=bit
#pragma HLS aggregate variable=inSubpix_R compact=bit
#pragma HLS aggregate variable=outMatchPt compact=bit
#endif
#pragma HLS INTERFACE axis register both port=inSubpix_L
#pragma HLS INTERFACE axis register both port=inSubpix_R
#pragma HLS INTERFACE axis register both port=outMatchPt

#pragma HLS INTERFACE mode=ap_none port=uL_offset register
#pragma HLS INTERFACE mode=ap_none port=useLaserPlane register
#pragma HLS INTERFACE mode=ap_none port=plane_a_f register
#pragma HLS INTERFACE mode=ap_none port=plane_b_f register
#pragma HLS INTERFACE mode=ap_none port=plane_c_f register
#pragma HLS INTERFACE mode=ap_none port=plane_norm register
#pragma HLS INTERFACE mode=ap_none port=maxPtPlaneDist register

	// 高40位：帧标记；低2位：本行点数
	// [41:2]：40位内部帧标记；[1:0]：本行点数。
	// 上电初始化为0，普通帧不清零。
	static ap_uint<42> leftState[ROW_COUNT] = {};
	static ap_uint<42> rightState[ROW_COUNT] = {};
#pragma HLS BIND_STORAGE variable=leftState type=ram_2p impl=bram
#pragma HLS BIND_STORAGE variable=rightState type=ram_2p impl=bram
#pragma HLS RESET variable=leftState off
#pragma HLS RESET variable=rightState off

	static SRowData left_buf[ROW_COUNT];
	static SRowData right_buf[ROW_COUNT];
#pragma HLS BIND_STORAGE variable=left_buf type=ram_2p impl=bram
#pragma HLS BIND_STORAGE variable=right_buf type=ram_2p impl=bram
#pragma HLS RESET variable=left_buf off
#pragma HLS RESET variable=right_buf off

	static SLineMatchInfo matchInfo_buf[ROW_COUNT];
#pragma HLS BIND_STORAGE variable=matchInfo_buf type=ram_2p impl=bram
	static SMatchSegInfo matchSegInfo_buf[ROW_COUNT];
#pragma HLS BIND_STORAGE variable=matchSegInfo_buf type=ram_2p impl=bram

	//row data initialization
	SRowData nullRowData;
	nullRowData.ptNum = 0;
	nullRowData.pt1.x = 0.0f;
	nullRowData.pt1.y = 0;;  //support up to 4k camera
	nullRowData.pt1.rid = 0;
	nullRowData.pt1.value = 0;
	nullRowData.pt1.flag = 0;	//bit0:OverLap | bit1:Rflct
	nullRowData.pt1.matchFlag = 0;

	nullRowData.pt2.x = 0.0f;
	nullRowData.pt2.y = 0;;  //support up to 4k camera
	nullRowData.pt2.rid = 0;
	nullRowData.pt2.value = 0;
	nullRowData.pt2.flag = 0;	//bit0:OverLap | bit1:Rflct
	nullRowData.pt2.matchFlag = 0;
#if 0
	for(uint16_t i= 0; i < ROW_COUNT; i ++)
	{
#pragma HLS PIPELINE II=1
		left_buf[i] = nullRowData;
		right_buf[i] = nullRowData;
		matchSegInfo_buf[i].seg_id = 0;
		matchSegInfo_buf[i].seg_ptSize = 0;
	}
#endif

	//setp 1: read steam into buff, target: II = 1
	uint16_t roiL_L, roiL_T, roiL_W, roiL_H, minY_left, maxY_left;
	uint32_t roiL_FrmNo, roiL_TimeStamp, roiL_encInfo;
	ap_uint<40> roiL_frameTag;
	uint16_t roiR_L, roiR_T, roiR_W, roiR_H, minY_right, maxY_right;
	uint32_t roiR_FrmNo, roiR_TimeStamp, roiR_encInfo;
	ap_uint<40> roiR_frameTag;
	receive_lr(
			inSubpix_L, left_buf, leftState,
			&roiL_L, &roiL_T, &roiL_W, &roiL_H, &minY_left, &maxY_left,
			&roiL_FrmNo, &roiL_TimeStamp, &roiL_encInfo, &roiL_frameTag,
			inSubpix_R, right_buf, rightState,
			&roiR_L, &roiR_T, &roiR_W, &roiR_H, &minY_right, &maxY_right,
			&roiR_FrmNo, &roiR_TimeStamp, &roiR_encInfo, &roiR_frameTag);

	bool left_empty  = (minY_left  == 0xffff);
	bool right_empty = (minY_right == 0xffff);

	uint16_t minY, maxY;
	if( left_empty || right_empty)
	{
		minY = 0;
		maxY = 0;
	}
	else
	{
		minY = minY_left > minY_right ? minY_right : minY_left;
		maxY = maxY_left < maxY_right ? maxY_right : maxY_left;
	}

	//step 2: LR match, target II = 1
	uint16_t idxL = 0;
	uint16_t idxR = 0;
	uint16_t linePtNum_L = 0;
	uint16_t linePtNum_R = 0;
	InterpolateCalibSubpix L_pt1, L_pt2;
	InterpolateCalibSubpix R_pt1, R_pt2;
	for(uint16_t i = minY; i <= maxY; i ++)
	{
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
#pragma HLS PIPELINE II=1

		SRowData line_L = left_buf[i];
		SRowData line_R = right_buf[i];
		float y = (float)line_L.pt1.y;

		const ap_uint<42> stateL = leftState[i];
		const ap_uint<40> tagL = stateL.range(41, 2);
		const ap_uint<2> numL = stateL.range(1, 0);
		const ap_uint<42> stateR = rightState[i];
		const ap_uint<40> tagR = stateR.range(41, 2);
		const ap_uint<2> numR = stateR.range(1, 0);
		const bool validL = (tagL == roiL_frameTag);
		const bool validR = (tagR == roiR_frameTag);

		SLineMatchInfo a_lineMatch;
		a_lineMatch.matchNum = 0;
		a_lineMatch.match1.match_seg_id = 0;
		a_lineMatch.match1.match_flag = 0;
		a_lineMatch.match1.pt_idx_L = 0;
		a_lineMatch.match1.pt_idx_R = 0;
		a_lineMatch.match1.rid_L = 0;
		a_lineMatch.match1.rid_R = 0;

		a_lineMatch.match2.match_seg_id = 0;
		a_lineMatch.match2.match_flag = 0;
		a_lineMatch.match2.pt_idx_L = 0;
		a_lineMatch.match2.pt_idx_R = 0;
		a_lineMatch.match2.rid_L = 0;
		a_lineMatch.match2.rid_R = 0;

		//compute distance to laser plane
		float dist_11 = _computePtPlaneDist(line_L.pt1.x, line_R.pt1.x, y, uL_offset, plane_a_f, plane_b_f, plane_c_f, plane_norm);
		float dist_12 = _computePtPlaneDist(line_L.pt1.x, line_R.pt2.x, y, uL_offset, plane_a_f, plane_b_f, plane_c_f, plane_norm);
		float dist_21 = _computePtPlaneDist(line_L.pt2.x, line_R.pt1.x, y, uL_offset, plane_a_f, plane_b_f, plane_c_f, plane_norm);
		float dist_22 = _computePtPlaneDist(line_L.pt2.x, line_R.pt2.x, y, uL_offset, plane_a_f, plane_b_f, plane_c_f, plane_norm);
		if( validL && validR && (numL == 1)&& (numR == 1))
		{
			if( (useLaserPlane == 0) || ((useLaserPlane == 1) && (dist_11 < maxPtPlaneDist)))
			{
				a_lineMatch.matchNum = 1;

				a_lineMatch.match1.match_flag = 1;
				a_lineMatch.match1.pt_idx_L = 1;
				a_lineMatch.match1.pt_idx_R = 1;
				a_lineMatch.match1.rid_L = line_L.pt1.rid;
				a_lineMatch.match1.rid_R = line_R.pt1.rid;

				a_lineMatch.match2.match_flag = 0;
				a_lineMatch.match2.pt_idx_L = 0;
				a_lineMatch.match2.pt_idx_R = 0;
				a_lineMatch.match2.rid_L = 0;
				a_lineMatch.match2.rid_R = 0;

				left_buf[i].pt1.matchFlag = 1;
				right_buf[i].pt1.matchFlag = 1;
			}
		}
		else if( validL && validR && (numL == 1)&& (numR == 2))
		{
			if(useLaserPlane == 1)
			{
				if( (dist_11 < maxPtPlaneDist) && (dist_11 < dist_12))
				{
					a_lineMatch.matchNum = 1;

					a_lineMatch.match1.match_flag = 1;
					a_lineMatch.match1.pt_idx_L = 1;
					a_lineMatch.match1.pt_idx_R = 1;
					a_lineMatch.match1.rid_L = line_L.pt1.rid;
					a_lineMatch.match1.rid_R = line_R.pt1.rid;

					a_lineMatch.match2.match_flag = 0;
					a_lineMatch.match2.pt_idx_L = 0;
					a_lineMatch.match2.pt_idx_R = 0;
					a_lineMatch.match2.rid_L = 0;
					a_lineMatch.match2.rid_R = 0;

					left_buf[i].pt1.matchFlag = 1;
					right_buf[i].pt1.matchFlag = 1;
				}
				else if( (dist_12 < maxPtPlaneDist) && (dist_12 < dist_11))
				{
					a_lineMatch.matchNum = 1;
					a_lineMatch.match1.match_flag = 1;
					a_lineMatch.match1.pt_idx_L = 1;
					a_lineMatch.match1.pt_idx_R = 2;
					a_lineMatch.match1.rid_L = line_L.pt1.rid;
					a_lineMatch.match1.rid_R = line_R.pt2.rid;

					a_lineMatch.match2.match_flag = 0;
					a_lineMatch.match2.pt_idx_L = 0;
					a_lineMatch.match2.pt_idx_R = 0;
					a_lineMatch.match2.rid_L = 0;
					a_lineMatch.match2.rid_R = 0;

					left_buf[i].pt1.matchFlag = 1;
					right_buf[i].pt2.matchFlag = 1;
				}
			}
		}
		else if( validL && validR && (numL == 2)&& (numR == 1))
		{
			if(useLaserPlane == 1)
			{
				if( (dist_11 < maxPtPlaneDist) && (dist_11 < dist_21))
				{
					a_lineMatch.matchNum = 1;

					a_lineMatch.match1.match_flag = 1;
					a_lineMatch.match1.pt_idx_L = 1;
					a_lineMatch.match1.pt_idx_R = 1;
					a_lineMatch.match1.rid_L = line_L.pt1.rid;
					a_lineMatch.match1.rid_R = line_R.pt1.rid;

					a_lineMatch.match2.match_flag = 0;
					a_lineMatch.match2.pt_idx_L = 0;
					a_lineMatch.match2.pt_idx_R = 0;
					a_lineMatch.match2.rid_L = 0;
					a_lineMatch.match2.rid_R = 0;

					left_buf[i].pt1.matchFlag = 1;
					right_buf[i].pt1.matchFlag = 1;
				}
				else if( (dist_21 < maxPtPlaneDist) && (dist_21 < dist_11))
				{
					a_lineMatch.matchNum = 1;

					a_lineMatch.match1.match_flag = 1;
					a_lineMatch.match1.pt_idx_L = 2;
					a_lineMatch.match1.pt_idx_R = 1;
					a_lineMatch.match1.rid_L = line_L.pt2.rid;
					a_lineMatch.match1.rid_R = line_R.pt1.rid;

					a_lineMatch.match2.match_flag = 0;
					a_lineMatch.match2.pt_idx_L = 0;
					a_lineMatch.match2.pt_idx_R = 0;
					a_lineMatch.match2.rid_L = 0;
					a_lineMatch.match2.rid_R = 0;

					left_buf[i].pt2.matchFlag = 1;
					right_buf[i].pt1.matchFlag = 1;
				}
			}
		}
		else if( validL && validR && (numL == 2)&& (numR == 2))
		{
			if(useLaserPlane == 1)
			{
				if( (dist_11 < maxPtPlaneDist) && (dist_11 < dist_12))
				{
					if( dist_22 < maxPtPlaneDist)
					{
						a_lineMatch.matchNum = 2;

						a_lineMatch.match1.match_flag = 1;
						a_lineMatch.match1.pt_idx_L = 1;
						a_lineMatch.match1.pt_idx_R = 1;
						a_lineMatch.match1.rid_L = line_L.pt1.rid;
						a_lineMatch.match1.rid_R = line_R.pt1.rid;

						a_lineMatch.match2.match_flag = 1;
						a_lineMatch.match2.pt_idx_L = 2;
						a_lineMatch.match2.pt_idx_R = 2;
						a_lineMatch.match2.rid_L = line_L.pt2.rid;
						a_lineMatch.match2.rid_R = line_R.pt2.rid;

						left_buf[i].pt1.matchFlag = 1;
						right_buf[i].pt1.matchFlag = 1;
						left_buf[i].pt2.matchFlag = 1;
						right_buf[i].pt2.matchFlag = 1;
					}
					else
					{
						a_lineMatch.matchNum = 1;

						a_lineMatch.match1.match_flag = 1;
						a_lineMatch.match1.pt_idx_L = 1;
						a_lineMatch.match1.pt_idx_R = 1;
						a_lineMatch.match1.rid_L = line_L.pt1.rid;
						a_lineMatch.match1.rid_R = line_R.pt1.rid;

						a_lineMatch.match2.match_flag = 0;
						a_lineMatch.match2.pt_idx_L = 0;
						a_lineMatch.match2.pt_idx_R = 0;
						a_lineMatch.match2.rid_L = 0;
						a_lineMatch.match2.rid_R = 0;

						left_buf[i].pt1.matchFlag = 1;
						right_buf[i].pt1.matchFlag = 1;
					}
				}
				else if( (dist_12 < maxPtPlaneDist) && (dist_12 < dist_11))
				{
					if(dist_21 < maxPtPlaneDist)
					{
						a_lineMatch.matchNum = 2;

						a_lineMatch.match1.match_flag = 1;
						a_lineMatch.match1.pt_idx_L = 1;
						a_lineMatch.match1.pt_idx_R = 2;
						a_lineMatch.match1.rid_L = line_L.pt1.rid;
						a_lineMatch.match1.rid_R = line_R.pt2.rid;

						a_lineMatch.match2.match_flag = 1;
						a_lineMatch.match2.pt_idx_L = 2;
						a_lineMatch.match2.pt_idx_R = 1;
						a_lineMatch.match2.rid_L = line_L.pt2.rid;
						a_lineMatch.match2.rid_R = line_R.pt1.rid;

						left_buf[i].pt1.matchFlag = 1;
						right_buf[i].pt2.matchFlag = 1;
						left_buf[i].pt2.matchFlag = 1;
						right_buf[i].pt1.matchFlag = 1;
					}
					else
					{
						a_lineMatch.matchNum = 1;
						a_lineMatch.match1.match_flag = 1;
						a_lineMatch.match1.pt_idx_L = 1;
						a_lineMatch.match1.pt_idx_R = 2;
						a_lineMatch.match1.rid_L = line_L.pt1.rid;
						a_lineMatch.match1.rid_R = line_R.pt2.rid;

						a_lineMatch.match2.match_flag = 0;
						a_lineMatch.match2.pt_idx_L = 0;
						a_lineMatch.match2.pt_idx_R = 0;
						a_lineMatch.match2.rid_L = 0;
						a_lineMatch.match2.rid_R = 0;

						left_buf[i].pt1.matchFlag = 1;
						right_buf[i].pt2.matchFlag = 1;
					}
				}
				else if( (dist_21 < maxPtPlaneDist) && (dist_21 < dist_22))
				{
					a_lineMatch.matchNum = 1;

					a_lineMatch.match1.match_flag = 1;
					a_lineMatch.match1.pt_idx_L = 2;
					a_lineMatch.match1.pt_idx_R = 1;
					a_lineMatch.match1.rid_L = line_L.pt2.rid;
					a_lineMatch.match1.rid_R = line_R.pt1.rid;

					a_lineMatch.match2.match_flag = 0;
					a_lineMatch.match2.pt_idx_L = 0;
					a_lineMatch.match2.pt_idx_R = 0;
					a_lineMatch.match2.rid_L = 0;
					a_lineMatch.match2.rid_R = 0;

					left_buf[i].pt2.matchFlag = 1;
					right_buf[i].pt1.matchFlag = 1;
				}
				else if( (dist_22 < maxPtPlaneDist) && (dist_22 < dist_21))
				{
					a_lineMatch.matchNum = 1;

					a_lineMatch.match1.match_flag = 1;
					a_lineMatch.match1.pt_idx_L = 2;
					a_lineMatch.match1.pt_idx_R = 2;
					a_lineMatch.match1.rid_L = line_L.pt2.rid;
					a_lineMatch.match1.rid_R = line_R.pt2.rid;

					a_lineMatch.match2.match_flag = 0;
					a_lineMatch.match2.pt_idx_L = 0;
					a_lineMatch.match2.pt_idx_R = 0;
					a_lineMatch.match2.rid_L = 0;
					a_lineMatch.match2.rid_R = 0;

					left_buf[i].pt2.matchFlag = 1;
					right_buf[i].pt2.matchFlag = 1;
				}
			}
		}
		else
		{
			a_lineMatch.matchNum = 0;

			a_lineMatch.match1.match_flag = 0;
			a_lineMatch.match1.pt_idx_L = 0;
			a_lineMatch.match1.pt_idx_R = 0;
			a_lineMatch.match1.rid_L = 0;
			a_lineMatch.match1.rid_R = 0;

			a_lineMatch.match2.match_flag = 0;
			a_lineMatch.match2.pt_idx_L = 0;
			a_lineMatch.match2.pt_idx_R = 0;
			a_lineMatch.match2.rid_L = 0;
			a_lineMatch.match2.rid_R = 0;
		}

		matchInfo_buf[i] = a_lineMatch;
	}

	// Step 3: 分配匹配段编号
	// 槽位1分配奇数编号，槽位2分配偶数编号。
	ap_uint<CAM4K_Y_WIDTH> match_seg_id_1 = 0;
	ap_uint<CAM4K_Y_WIDTH> match_seg_id_2 = 0;
	// 普通局部变量，每帧从 1 开始；0 保留为无效 ID
	ap_uint<CAM4K_Y_WIDTH> next_seg_id = 1;

	ap_uint<11> m1_ridL = 0;
	ap_uint<11> m1_ridR = 0;
	ap_uint<11> m2_ridL = 0;
	ap_uint<11> m2_ridR = 0;

	MATCH_SEGMENT:
	for (uint16_t i = minY; i <= maxY; ++i)
	{
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
#pragma HLS PIPELINE II=3

	    SLineMatchInfo a_lineMatch = matchInfo_buf[i];
	    const bool current1Valid = (a_lineMatch.matchNum > 0) && (a_lineMatch.match1.rid_L != 0) && (a_lineMatch.match1.rid_R != 0);
	    const bool current2Valid = (a_lineMatch.matchNum == 2) && (a_lineMatch.match2.rid_L != 0) && (a_lineMatch.match2.rid_R != 0);

	    // 历史槽位是否被本行使用
	    bool previousSlot1Used = false;
	    bool previousSlot2Used = false;
	    // 当前两个匹配是否已经分配编号
	    bool currentMatch1Assigned = false;
	    bool currentMatch2Assigned = false;

	    a_lineMatch.match1.match_seg_id = 0;
	    a_lineMatch.match2.match_seg_id = 0;
	    // --------------------------------------------------
	    // 1. 当前 match1 优先匹配历史槽位1，其次槽位2
	    // --------------------------------------------------
	    if (current1Valid)
	    {
	        if ((m1_ridL != 0) &&
	        	(m1_ridL == a_lineMatch.match1.rid_L) &&
				(m1_ridR == a_lineMatch.match1.rid_R))
	        {
	            a_lineMatch.match1.match_seg_id = match_seg_id_1;
	            previousSlot1Used = true;
	            currentMatch1Assigned = true;
	        }
	        else if ((m2_ridL != 0) &&
	        		(m2_ridL == a_lineMatch.match1.rid_L) &&
					(m2_ridR == a_lineMatch.match1.rid_R))
	        {
	            a_lineMatch.match1.match_seg_id = match_seg_id_2;
	            previousSlot2Used = true;
	            currentMatch1Assigned = true;
	        }
	    }

	    // --------------------------------------------------
	    // 2. 当前 match2 匹配尚未被使用的历史槽位
	    //    一个历史槽位最多匹配本行的一个匹配项
	    // --------------------------------------------------
	    if (current2Valid)
	    {
	        if (!previousSlot1Used &&
	            (m1_ridL != 0) &&
	            (m1_ridL == a_lineMatch.match2.rid_L) &&
	            (m1_ridR == a_lineMatch.match2.rid_R))
	        {
	            a_lineMatch.match2.match_seg_id = match_seg_id_1;
	            previousSlot1Used = true;
	            currentMatch2Assigned = true;
	        }
	        else if (!previousSlot2Used &&
	                 (m2_ridL != 0) &&
	                 (m2_ridL == a_lineMatch.match2.rid_L) &&
	                 (m2_ridR == a_lineMatch.match2.rid_R))
	        {
	            a_lineMatch.match2.match_seg_id = match_seg_id_2;
	            previousSlot2Used = true;
	            currentMatch2Assigned = true;
	        }
	    }

	    // --------------------------------------------------
	    // 3. 为尚未匹配的 match1 分配新段
	    //    只覆盖本行没有使用的历史槽位
	    // --------------------------------------------------
	    if (current1Valid && !currentMatch1Assigned)
	    {
	        if (!previousSlot1Used)
	        {
	            m1_ridL = a_lineMatch.match1.rid_L;
	            m1_ridR = a_lineMatch.match1.rid_R;

	            match_seg_id_1 = next_seg_id;
	            next_seg_id++;
	            a_lineMatch.match1.match_seg_id = match_seg_id_1;
	            previousSlot1Used = true;
	            currentMatch1Assigned = true;
	        }
	        else if (!previousSlot2Used)
	        {
	            m2_ridL = a_lineMatch.match1.rid_L;
	            m2_ridR = a_lineMatch.match1.rid_R;

	            match_seg_id_2 = next_seg_id;
	            next_seg_id++;
	            a_lineMatch.match1.match_seg_id = match_seg_id_2;
	            previousSlot2Used = true;
	            currentMatch1Assigned = true;
	        }
	    }

	    // --------------------------------------------------
	    // 4. 为尚未匹配的 match2 分配新段
	    // --------------------------------------------------
	    if (current2Valid && !currentMatch2Assigned)
	    {
	        if (!previousSlot1Used)
	        {
	            m1_ridL = a_lineMatch.match2.rid_L;
	            m1_ridR = a_lineMatch.match2.rid_R;

	            match_seg_id_1 = next_seg_id;
	            next_seg_id++;
	            a_lineMatch.match2.match_seg_id = match_seg_id_1;
	            //previousSlot1Used = true;
	            //currentMatch2Assigned = true;
	        }
	        else if (!previousSlot2Used)
	        {
	            m2_ridL = a_lineMatch.match2.rid_L;
	            m2_ridR = a_lineMatch.match2.rid_R;

	            match_seg_id_2 = next_seg_id;
	            next_seg_id++;
	            a_lineMatch.match2.match_seg_id = match_seg_id_2;
	            //previousSlot2Used = true;
	            //currentMatch2Assigned = true;
	        }
	    }

	    // 每行统一写回一次
	    matchInfo_buf[i] = a_lineMatch;
	}

	//output
	SLRMatchOutput outWord =
	{
		0, {0.0f, 0, 0, 0, 0, 0	}, {0.0f, 0, 0, 0, 0, 0	},
		0, {0.0f, 0, 0, 0, 0, 0	}, {0.0f, 0, 0, 0, 0, 0	},
		{0, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}},
		0
	};

	//1st: frameNo
	UintFloat outTmp;
	outTmp.unData = roiL_FrmNo;
	outWord.left_pt1.x = outTmp.fdata;
	outTmp.unData = roiL_TimeStamp;
	outWord.left_pt2.x = outTmp.fdata;
	outTmp.unData = roiL_encInfo;
	outWord.right_pt1.x = outTmp.fdata;
	outTmp.unData = minY | (maxY << 16);
	outWord.right_pt2.x = outTmp.fdata;
	outWord.syncWord = 0b10;
	outMatchPt.write(outWord);
	for(uint16_t i = minY; i <= maxY; i ++)
	{
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
#pragma HLS PIPELINE II=1
		const ap_uint<42> stateL = leftState[i];
		const ap_uint<40> tagL = stateL.range(41, 2);
		const ap_uint<2> numL = stateL.range(1, 0);
		const ap_uint<42> stateR = rightState[i];
		const ap_uint<40> tagR = stateR.range(41, 2);
		const ap_uint<2> numR = stateR.range(1, 0);
		const bool validL = (tagL == roiL_frameTag);
		const bool validR = (tagR == roiR_frameTag);

		outWord.left_ptNum = 0;
		outWord.left_pt1 = nullRowData.pt1;
		outWord.left_pt2 = nullRowData.pt2;
		if (validL && numL > 0)
		{
		    outWord.left_ptNum = numL;
		    outWord.left_pt1 = left_buf[i].pt1;

		    if (numL == 2)
		        outWord.left_pt2 = left_buf[i].pt2;
		}

		outWord.right_ptNum = 0;
		outWord.right_pt1 = nullRowData.pt1;
		outWord.right_pt2 = nullRowData.pt2;
		if (validR && numR > 0)
		{
		    outWord.right_ptNum = numR;
		    outWord.right_pt1 = right_buf[i].pt1;

		    if (numR == 2)
		        outWord.right_pt2 = right_buf[i].pt2;
		}

		outWord.matchInfo = matchInfo_buf[i];
		if(i == maxY)
			outWord.syncWord = 0b11;
		else
			outWord.syncWord = 0b00;
		outMatchPt.write(outWord);
	}
}