#include "subPix.h"
#include "stdlib.h"

void subpix(
	hls::stream<RgnPixConvolve> 	&InConvolvePnt,
	hls::stream<RgnSubPix> 			&OutSubpixPnt
)
{
#if !(VITIS_HLS)
#pragma HLS DATA_PACK variable=InConvolvePnt
#pragma HLS DATA_PACK variable=OutSubpixPnt
#endif
#pragma HLS INTERFACE axis register both port=OutSubpixPnt
#pragma HLS INTERFACE axis register both port=InConvolvePnt

	ap_uint<12> WinNum = 0;
	ap_uint<12> frmH = 0;
	ap_uint<12> frmY = 0;
	RgnPixConvolve 	InData;
	InData = InConvolvePnt.read();
	if(0b10 != InData.Sync)  //wait for syncFirst
		return;

	RgnSubPix outData = {0,0,0,0,0,0};
	RgnSubPix nullOut = {-1.0f,0,0,0,0,0};
	//1st: frameNo
	outData.Sync = InData.Sync;
	outData.rsv = 0;
	ap_uint<32> data_32;
	data_32(11,0) = InData.LazerWinX.range(11,0);
	data_32(23,12) = InData.LazerWinY.range(11,0);
	data_32(31,24) = InData.LazerWinRid.range(7,0);
	UintFloat tmp;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.write(outData);
	//2nd: timeStamp
	InData = InConvolvePnt.read();
	outData.Sync = InData.Sync;
	data_32(11,0) = InData.LazerWinX.range(11,0);
	data_32(23,12) = InData.LazerWinY.range(11,0);
	data_32(31,24) = InData.LazerWinRid.range(7,0);
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.write(outData);
	//3rd:encInfo
	InData = InConvolvePnt.read();
	outData.Sync = InData.Sync;
	data_32(11,0) = InData.LazerWinX.range(11,0);
	data_32(23,12) = InData.LazerWinY.range(11,0);
	data_32(31,24) = InData.LazerWinRid.range(7,0);
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.write(outData);
	//4th: frmW, frmH
	InData = InConvolvePnt.read();
	outData.Sync = InData.Sync;
	data_32(11,0) = InData.LazerWinX.range(11,0);
	data_32(23,12) = InData.LazerWinY.range(11,0);
	data_32(31,24) = 0;
	frmH(11,0) = InData.LazerWinY.range(11,0);
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.write(outData);
	//5th: frmX, WinNum
	int nLines = (int)frmH;
	int nLoops = nLines; // * 2;
	//if(nLoops >= 4096)
	//	nLoops = 4095;
	ap_uint<12> outSize = (ap_uint<12>)nLoops;
	InData = InConvolvePnt.read();
	outData.Sync = InData.Sync;
	data_32(11,0) = InData.LazerWinX.range(11,0);
	data_32(23,12) = outSize.range(11,0);	//InData.LazerWinY.range(11,0);
	data_32(31,24) = 0;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	WinNum(11,0) = InData.LazerWinY.range(11,0);
	OutSubpixPnt.write(outData);
	//6th: frmY, validDataSize
	InData = InConvolvePnt.read();
	outData.Sync = InData.Sync;
	data_32(11,0) = InData.LazerWinX.range(11,0);
	data_32(23,12) = WinNum.range(11,0);  //validDataSize
	data_32(31,24) = 0;
	frmY(11,0) = InData.LazerWinX.range(11,0);
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.write(outData);

	int nStart = (int)frmY;
	ap_uint<12> lastLine = frmY + frmH - 1;
	ap_uint<1> readFlag = 1;
	InData = InConvolvePnt.read();
	RgnPixConvolve currData;
	int loopNum = nLoops * 2;
	for(int n = 0; n < loopNum; n ++)
	{
#pragma HLS LOOP_TRIPCOUNT min=4096 max=4096
#pragma HLS PIPELINE II=1

		int lineIndx = n / 2 + nStart;

		ap_uint<1> vldFlag = 0;
		if( InData.LazerWinY <= lineIndx)
		{
			if(1 == readFlag)
			{
				currData = InData;
				vldFlag = 1;
				if(currData.Sync != 0b11)
					InData = InConvolvePnt.read();
				else
					readFlag = 0;
			}
		}

		RgnSubPix a_pnt;
		if(vldFlag == 0)
		{
			a_pnt = nullOut;
		}
		else
		{
			float fx;
			if(currData.nD2 == 0)
				fx = 0;
			else
				fx = (float)currData.nD1/(float)currData.nD2;

			a_pnt.flag = currData.LazerWinFlag;
			a_pnt.y = currData.LazerWinY;
			a_pnt.rid = currData.LazerWinRid;
			a_pnt.value = currData.value;
			a_pnt.x = (float)currData.LazerWinX + 8.0f + fx + 0.5; //adjust point to pixel center
			a_pnt.rsv = 0;
			a_pnt.Sync = currData.Sync;
		}
		OutSubpixPnt.write(a_pnt);
	}
}