#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
#else
#pragma HLS aggregate variable=InConvolvePnt compact=bit
#pragma HLS aggregate variable=OutSubpixPnt compact=bit
#endif
#pragma HLS INTERFACE axis register both port=OutSubpixPnt
#pragma HLS INTERFACE axis register both port=InConvolvePnt

	ap_uint<CAM4K_Y_WIDTH> WinNum = 0;
	ap_uint<CAM4K_Y_WIDTH> frmH = 0;
	ap_uint<CAM4K_Y_WIDTH> frmY = 0;
	RgnPixConvolve 	InData;
	InData = InConvolvePnt.read();
	if(0b10 != InData.Sync)  //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(CAM4K_Y_WIDTH-1,0) = InData.LazerWinX.range(CAM4K_Y_WIDTH-1,0);
	data_32(CAM4K_Y_WIDTH*2-1, CAM4K_Y_WIDTH) = InData.LazerWinY.range(CAM4K_Y_WIDTH-1,0);
	data_32(31,CAM4K_Y_WIDTH*2) = InData.LazerWinRid.range(CAM4K_Y_RSV_BITS*2-1,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(CAM4K_Y_WIDTH-1,0) = InData.LazerWinX.range(CAM4K_Y_WIDTH-1,0);
	data_32(CAM4K_Y_WIDTH*2-1, CAM4K_Y_WIDTH) = InData.LazerWinY.range(CAM4K_Y_WIDTH-1,0);
	data_32(31,CAM4K_Y_WIDTH*2) = InData.LazerWinRid.range(CAM4K_Y_RSV_BITS*2-1,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(CAM4K_Y_WIDTH-1, 0) = InData.LazerWinX.range(CAM4K_Y_WIDTH-1,0);
	data_32(CAM4K_Y_WIDTH*2-1, CAM4K_Y_WIDTH) = InData.LazerWinY.range(CAM4K_Y_WIDTH-1,0);
	data_32(31,CAM4K_Y_WIDTH*2) = InData.LazerWinRid.range(CAM4K_Y_RSV_BITS*2-1,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(CAM4K_Y_WIDTH-1,0) = InData.LazerWinX.range(CAM4K_Y_WIDTH-1,0);
	data_32(15, CAM4K_Y_WIDTH) = 0;
	data_32(16+CAM4K_Y_WIDTH-1,16) = InData.LazerWinY.range(CAM4K_Y_WIDTH-1,0);
	data_32(31, 16+CAM4K_Y_WIDTH) = 0;
	frmH(CAM4K_Y_WIDTH-1,0) = InData.LazerWinY.range(CAM4K_Y_WIDTH-1,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<CAM4K_Y_WIDTH> outSize = (ap_uint<CAM4K_Y_WIDTH>)nLoops;
	InData = InConvolvePnt.read();
	outData.Sync = InData.Sync;
	data_32(CAM4K_Y_WIDTH-1,0) = InData.LazerWinX.range(CAM4K_Y_WIDTH-1,0);
	data_32(15, CAM4K_Y_WIDTH) = 0;
	data_32(16+CAM4K_Y_WIDTH-1, 16) = outSize.range(CAM4K_Y_WIDTH-1,0);	//InData.LazerWinY.range(11,0);
	data_32(31,16+CAM4K_Y_WIDTH) = 0;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	WinNum(CAM4K_Y_WIDTH-1,0) = InData.LazerWinY.range(CAM4K_Y_WIDTH-1,0);
	float xOffset = (float)InData.LazerWinX.range(CAM4K_Y_WIDTH-1,0);
	OutSubpixPnt.write(outData);
	//6th: frmY, validDataSize
	InData = InConvolvePnt.read();
	outData.Sync = InData.Sync;
	data_32(CAM4K_Y_WIDTH-1,0) = InData.LazerWinX.range(CAM4K_Y_WIDTH-1,0);
	data_32(15, CAM4K_Y_WIDTH) = 0;
	data_32(16+CAM4K_Y_WIDTH-1,16) = WinNum.range(CAM4K_Y_WIDTH-1,0);  //validDataSize
	data_32(31,16+CAM4K_Y_WIDTH) = 0;

	frmY(CAM4K_Y_WIDTH-1,0) =
	    InData.LazerWinX.range(CAM4K_Y_WIDTH-1,0);
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;

	// 输出是否结束由输出槽位数量决定。
	// frmH=0 的合法空帧应同时满足 WinNum=0。
	outData.Sync = (frmH == 0) ? 0b11 : 0b00;
	OutSubpixPnt.write(outData);

	if (frmH == 0)
	    return;

    const unsigned inputCount = (unsigned)WinNum;
    const unsigned slotCount = (unsigned)frmH * 2u;
    RgnPixConvolve pending = {};
    bool havePoint = false;
    unsigned readCount = 0;
    if (inputCount > 0)
    {
        pending = InConvolvePnt.read();
        readCount = 1;
        havePoint = true;
    }

SUBPIX_OUTPUT:
    for (unsigned n = 0; n < slotCount; ++n)
    {
#pragma HLS PIPELINE II=1
#pragma HLS LOOP_TRIPCOUNT min=0 max=6144
        // Snapshot: arithmetic uses only the old pending point.
        const RgnPixConvolve point = pending;
        const bool take = havePoint &&
            ((unsigned)point.LazerWinY <= (n >> 1));
        const bool fetchNext = take && (point.Sync != 0b11) &&
                               (readCount < inputCount);

        // Short integer-only recurrence. Never uses a floating-point result.
        if (fetchNext)
            pending = InConvolvePnt.read();
        readCount = readCount + (fetchNext ? 1u : 0u);
        havePoint = take ? fetchNext : havePoint;

        // Unconditional arithmetic: no conditional divide/merge block.
        // Invalid slots and zero denominators use 0/1.
        const bool useRatio = take && (point.nD2 != 0);
        const float numerator = useRatio ? (float)point.nD1 : 0.0f;
        const float denominator = useRatio ? (float)point.nD2 : 1.0f;
        const float fx = numerator / denominator;
        const float pixelX = take ? (float)point.LazerWinX : 0.0f;
        const float resultX = pixelX + 8.0f + fx + 0.5f + xOffset;

        RgnSubPix result = {};
        result.x = take ? resultX : -1.0f;
        if (take)
        {
            result.y = point.LazerWinY + frmY;
            result.rid = point.LazerWinRid;
            result.flag = point.LazerWinFlag;
            result.value = point.value;
        }
        result.rsv = 0;
        result.Sync = (n == slotCount - 1u) ? 0b11 : 0b00;
        OutSubpixPnt.write(result);
    }
}