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

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

	ap_uint<12> WinNum = 0;
	RgnPixConvolve 	InData;
	InData = InConvolvePnt.read();
	ap_uint<1> VSync = InData.Sync.range(1,1);
	if(0b0 == VSync)  //wait for syncFirst
		return;

	RgnSubPix outData = {0,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;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.write(outData);
	//5th: frmX, WinNum
	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;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	WinNum(11,0) = InData.LazerWinY.range(11,0);
	OutSubpixPnt.write(outData);
	//6th: frmY
	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;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.write(outData);

	for(int n = 0; n < WinNum; n ++)
	{
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
#pragma HLS PIPELINE II=1

		InData = InConvolvePnt.read();

		float fx;
		if(InData.nD2 == 0)
			fx = 0;
		else
			fx = (float)InData.nD1/(float)InData.nD2;

		RgnSubPix a_pnt;
		if(n == 0)
			a_pnt.Sync = 0b01;
		else
			a_pnt.Sync = 0b00;
		a_pnt.flag = InData.LazerWinFlag;
		a_pnt.y = InData.LazerWinY;
		a_pnt.rid = InData.LazerWinRid;
		a_pnt.value = InData.value;
		a_pnt.x = (float)InData.LazerWinX + 8.0f + fx;
		a_pnt.rsv = 0;
		OutSubpixPnt.write(a_pnt);
	}
}