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

void subpix(
	std::vector<RgnPixConvolve> 	&InConvolvePnt,
	std::vector<RgnSubPix> 			&OutSubpixPnt
)
{
	ushort WinNum = 0;
	RgnPixConvolve 	InData;
	int readPtr = 0;
	InData = InConvolvePnt[readPtr++];
	uchar VSync = InData.Sync & 0x02;
	if(0 == VSync)  //wait for syncFirst
		return;

	RgnSubPix outData = {0,0,0,0,0,0};
	//1st: frameNo
	outData.Sync = InData.Sync;
	outData.rsv = 0;
	uint_32 data_32;
	data_32 = InData.LazerWinRid & 0xff;
	data_32 <<= 12;
	data_32 += InData.LazerWinY & 0xfff;
	data_32 <<= 12;
	data_32 += InData.LazerWinX & 0xfff;
	UintFloat tmp;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.push_back(outData);
	//2nd: timeStamp
	InData = InConvolvePnt[readPtr++];
	outData.Sync = InData.Sync;
	data_32 = InData.LazerWinRid & 0xff;
	data_32 <<= 12;
	data_32 += InData.LazerWinY & 0xfff;
	data_32 <<= 12;
	data_32 += InData.LazerWinX & 0xfff;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.push_back(outData);
	//3rd:encInfo
	InData = InConvolvePnt[readPtr++];
	outData.Sync = InData.Sync;
	data_32 = InData.LazerWinRid & 0xff;
	data_32 <<= 12;
	data_32 += InData.LazerWinY & 0xfff;
	data_32 <<= 12;
	data_32 += InData.LazerWinX & 0xfff;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.push_back(outData);
	//4th: frmW, frmH
	InData = InConvolvePnt[readPtr++];
	outData.Sync = InData.Sync;
	data_32 = InData.LazerWinY & 0xfff;
	data_32 <<= 12;
	data_32 += InData.LazerWinX & 0xfff;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.push_back(outData);
	//5th: frmX, WinNum
	InData = InConvolvePnt[readPtr++];
	outData.Sync = InData.Sync;
	data_32 = InData.LazerWinY & 0xfff;
	data_32 <<= 12;
	data_32 += InData.LazerWinX & 0xfff;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	WinNum = InData.LazerWinY & 0xfff;
	OutSubpixPnt.push_back(outData);
	//6th: frmY
	InData = InConvolvePnt[readPtr++];
	outData.Sync = InData.Sync;
	data_32 = InData.LazerWinY & 0xfff;
	data_32 <<= 12;
	data_32 += InData.LazerWinX & 0xfff;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	OutSubpixPnt.push_back(outData);

	for(int n = 0; n < WinNum; n ++)
	{

		InData = InConvolvePnt[readPtr++];

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

		RgnSubPix a_pnt;
		if(n == 1)
			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.push_back(a_pnt);
	}
}