#include "calib.h"
#include "stdlib.h"

void calib(
	std::vector<CalibData> 		&inCalibData,
	std::vector<RgnSubPixCalib> 	&outSubpixCalib
)
{
	CalibData 	InData;
	int readPtr = 0;
	InData = inCalibData[readPtr ++];
	uchar VSync = InData.Sync & 0x02; //ap_uint<1> VSync = InData.Sync.range(1,1);
	if(0b0 == VSync)  //wait for syncFirst
		return;

	Byte8 dataBuff;
	dataBuff.dData = InData.calibK[0];
	ushort WinNum = 0; // ap_uint<12> WinNum = 0;
	RgnSubPixCalib outData = {0,0,0,0,0,0,0};
	//1st: frameNo
	outData.Sync = InData.Sync;
	unsigned int data_32 = dataBuff.nData[0];
	UintFloat tmp;
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	outSubpixCalib.push_back(outData);
	//2nd: timeStamp
	InData = inCalibData[readPtr++];
	outData.Sync = InData.Sync;
	dataBuff.dData = InData.calibK[0];
	data_32 = dataBuff.nData[0];
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	outSubpixCalib.push_back(outData);
	//3rd:encInfo
	InData = inCalibData[readPtr++];
	outData.Sync = InData.Sync;
	dataBuff.dData = InData.calibK[0];
	data_32 = dataBuff.nData[0];
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	outSubpixCalib.push_back(outData);
	//4th: frmW, frmH
	InData = inCalibData[readPtr++];
	outData.Sync = InData.Sync;
	dataBuff.dData = InData.calibK[0];
	data_32 = dataBuff.nData[0];
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	outSubpixCalib.push_back(outData);
	//5th: frmX, WinNum
	InData = inCalibData[readPtr++];
	outData.Sync = InData.Sync;
	dataBuff.dData = InData.calibK[0];
	data_32 = dataBuff.nData[0];
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	uint_32 d32_tmp = data_32;
	WinNum = (d32_tmp >> 12) & 0x7ff;
	outSubpixCalib.push_back(outData);
	//6th: frmY
	InData = inCalibData[readPtr++];
	outData.Sync = InData.Sync;
	dataBuff.dData = InData.calibK[0];
	data_32 = dataBuff.nData[0];
	tmp.unData = (unsigned int)data_32;
	outData.x = tmp.fdata;
	outSubpixCalib.push_back(outData);

	RgnSubPixCalib calibPnt;
	double rawx,rawy;
	double x2, x3,x4,x5,x6,x7;
	double y2, y3,y4,y5,y6,y7;
	double kx, ky;
	double xmul0=0, xmul1=0, xmul2=0, xmul3=0, xmul4=0, xmul5=0, xmul6=0, xmul7=0;
	double ymul0=0, ymul1=0, ymul2=0, ymul3=0, ymul4=0, ymul5=0, ymul6=0, ymul7=0;
	for(int n = 0; n < WinNum; n ++)
	{
		for(int j = 0; j <= 8; j ++)
		{
			InData = inCalibData[readPtr++];
			if( 0 == j)//read subpix[readPtr++]
			{
				calibPnt.x = (float)(xmul0 + xmul1 + xmul2 + xmul3 + xmul4 + xmul5+ xmul6 + xmul7);
				calibPnt.y = (float)(ymul0 + ymul1 + ymul2 + ymul3 + ymul4 + ymul5+ ymul6 + ymul7);
				if(n > 0)
					outSubpixCalib.push_back(calibPnt);

				dataBuff.dData = InData.calibK[0];
				rawx = (double)dataBuff.fData[0];
				uint_32 apData32 = (uint_32)dataBuff.nData[1];
				ushort dy_12;
				dy_12= apData32 & 0xfff;
				rawy = (double)dy_12;
				apData32 >>= 12;
				calibPnt.rid = apData32 & 0x7ff;
				apData32 >>= 11;
				calibPnt.flag = apData32 & 0x0f;
				dataBuff.dData = InData.calibK[1];
				apData32 = (uint_32)dataBuff.nData[0];
				calibPnt.value = apData32 && 0xff;
				calibPnt.Sync = 0;
				calibPnt.rsv = 0;
			}
			else if(1 == j)//k0
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];
				xmul0 = kx;
				ymul0 = ky;
			}
			else if(2 == j)//k1 * x
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];
				xmul1 = kx * rawx;
				ymul1 = ky * rawy;
				x2 = rawx * rawx;
				y2 = rawy * rawy;
			}
			else if(3 == j) //k2 * x^2
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];
				xmul2 = kx * x2;
				ymul2 = ky * y2;
				x3 = x2 * rawx;
				y3 = y2 * rawy;
			}
			else if(4 == j)//k3 * x^3
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];
				xmul3 = kx * x3;
				ymul3 = ky * y3;
				x4 = x3 * rawx;
				y4 = y3 * rawy;
			}
			else if(5 == j) //k4 * x^4
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];
				xmul4 = kx * x4;
				ymul4 = ky * y4;
				x5 = x4 * rawx;
				y5 = y4 * rawy;
			}
			else if(6 == j) //k5 * x^5
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];
				xmul5 = kx * x5;
				ymul5 = ky * y5;
				x6 = x5 * rawx;
				y6 = y5 * rawy;
			}
			else if(7 == j)//k6 * x^6
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];
				xmul6 = kx * x6;
				ymul6 = ky * y6;
				x7 = x6 * rawx;
				y7 = y6 * rawy;
			}
			else if(8 == j)//k7 * x^7
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];
				xmul7 = kx * x7;
				ymul7 = ky * y7;
			}
		}
	}
	if(WinNum > 0)
		outSubpixCalib.push_back(calibPnt);
}