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

void calib(
	hls::stream<CalibData> 		&inCalibData,
	hls::stream<RgnSubPixCalib> 	&outSubpixCalib
)
{
#pragma HLS DATA_PACK variable=inCalibData
#pragma HLS DATA_PACK variable=outSubpixCalib
#pragma HLS INTERFACE axis register both port=inCalibData
#pragma HLS INTERFACE axis register both port=outSubpixCalib

	CalibData 	InData;
	InData = inCalibData.read();
	ap_uint<1> VSync = InData.Sync.range(1,1);
	if(0b0 == VSync)  //wait for syncFirst
		return;

	Byte8 dataBuff;
	dataBuff.dData = InData.calibK[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.write(outData);
	//2nd: timeStamp
	InData = inCalibData.read();
	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.write(outData);
	//3rd:encInfo
	InData = inCalibData.read();
	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.write(outData);
	//4th: frmW, frmH
	InData = inCalibData.read();
	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.write(outData);

	//5th: frmX, WinNum
	InData = inCalibData.read();
	outData.Sync = InData.Sync;
	dataBuff.dData = InData.calibK[0];
	data_32 = dataBuff.nData[0];
	ap_uint<32> d32_t5h = data_32;
	WinNum(11,0) = d32_t5h.range(23,12);
	//outSubpixCalib.write(outData);

	//6th: frmY
	ap_uint<12> validDataSize = 0;
	RgnSubPixCalib outData_1 = {0,0,0,0,0,0,0};
	InData = inCalibData.read();
	outData_1.Sync = InData.Sync;
	dataBuff.dData = InData.calibK[0];
	data_32 = dataBuff.nData[0];
	ap_uint<32> d32_tmp = data_32;
	validDataSize(11,0) = d32_tmp.range(23,12);
	d32_tmp(31,12) = 0;
	tmp.unData = (unsigned int)data_32;
	outData_1.x = tmp.fdata;

	//output validDataSize
	d32_t5h(23,12) = validDataSize.range(11,0);
	tmp.unData = (unsigned int)d32_t5h;
	outData.x = tmp.fdata;
	outSubpixCalib.write(outData);
	outSubpixCalib.write(outData_1);

	RgnSubPixCalib calibPnt;
	double rawx,rawy;
	double x2, x3,x4,x5,x6,x7;
	double y2, y3,y4,y5,y6,y7;
	//double kx, ky;
	double kx_1, kx_2, ky_1, ky_2;
	Byte8 dataConver_x, dataConver_y;
	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;
	bool vldFlag = 0;
	for(int n = 0; n < WinNum; n ++)
	{
#pragma HLS LOOP_TRIPCOUNT min=4096 max=4096
		for(int j = 0; j <= 4; j ++)
		{
#pragma HLS PIPELINE II=1
			InData = inCalibData.read();
			if( 0 == j)//read subpix
			{
				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) && (vldFlag == 1))
					outSubpixCalib.write(calibPnt);

				dataBuff.dData = InData.calibK[0];
				rawx = (double)dataBuff.fData[0];
				ap_uint<32> apData32 = (ap_uint<32>)dataBuff.nData[1];
				ap_uint<12> dy_12;
				dy_12(11,0)= apData32.range(11, 0);
				rawy = (double)dy_12;
				calibPnt.rid = apData32.range(22, 12);
				calibPnt.flag = apData32.range(26,23);
				dataBuff.dData = InData.calibK[1];
				apData32 = (ap_uint<32>)dataBuff.nData[0];
				calibPnt.value = apData32.range(7,0);
				calibPnt.Sync = 0;
				calibPnt.rsv = 0;
				if(rawx < 0)
					vldFlag = 0;
				else
					vldFlag = 1;

				x2 = rawx * rawx;
				y2 = rawy * rawy;
			}
			else if(1 == j)//k0, k1*x
			{
				//kx = InData.calibK[0];
				//ky = InData.calibK[1];
				dataConver_x.dData = InData.calibK[0];
				kx_1 = (double)dataConver_x.fData[0];
				kx_2 = (double)dataConver_x.fData[1];
				dataConver_y.dData = InData.calibK[0];
				ky_1 = (double)dataConver_y.fData[0];
				ky_2 = (double)dataConver_y.fData[1];
				xmul0 = kx_1;
				ymul0 = ky_1;
				xmul1 = kx_2 * rawx;
				ymul1 = ky_2 * rawy;
				x3 = x2 * rawx;
				y3 = y2 * rawy;
			}
			else if(2 == j)//k2 * x^2, k3 * x^3
			{
				dataConver_x.dData = InData.calibK[0];
				kx_1 = (double)dataConver_x.fData[0];
				kx_2 = (double)dataConver_x.fData[1];
				dataConver_y.dData = InData.calibK[0];
				ky_1 = (double)dataConver_y.fData[0];
				ky_2 = (double)dataConver_y.fData[1];

				xmul2 = kx_1 * x2;
				ymul2 = ky_1 * y2;
				xmul3 = kx_2 * x3;
				ymul3 = ky_2 * y3;

				x4 = x3 * rawx;
				y4 = y3 * rawy;
				x5 = x3 * x2;
				y5 = y3 * x2;
			}
			else if(3 == j) //k4 * x^4, k5 * x^5
			{
				dataConver_x.dData = InData.calibK[0];
				kx_1 = (double)dataConver_x.fData[0];
				kx_2 = (double)dataConver_x.fData[1];
				dataConver_y.dData = InData.calibK[0];
				ky_1 = (double)dataConver_y.fData[0];
				ky_2 = (double)dataConver_y.fData[1];

				xmul4 = kx_1 * x4;
				ymul4 = ky_1 * y4;
				xmul5 = kx_2 * x5;
				ymul5 = ky_2 * y5;

				x6 = x5 * rawx;
				y6 = y5 * rawy;
				x7 = x5 * x2;
				y7 = y5 * x2;
			}
			else if(4 == j)//k6 * x^6, k7 * x^7
			{
				dataConver_x.dData = InData.calibK[0];
				kx_1 = (double)dataConver_x.fData[0];
				kx_2 = (double)dataConver_x.fData[1];
				dataConver_y.dData = InData.calibK[0];
				ky_1 = (double)dataConver_y.fData[0];
				ky_2 = (double)dataConver_y.fData[1];

				xmul6 = kx_1 * x6;
				ymul6 = ky_1 * y6;
				xmul7 = kx_2 * x7;
				ymul7 = ky_2 * y7;
			}
#if 0
			else if(5 == j) //
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];

				x5 = x4 * rawx;
				y5 = y4 * rawy;
			}
			else if(6 == j) //
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];


			}
			else if(7 == j)//
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];


			}
			else if(8 == j)//
			{
				kx = InData.calibK[0];
				ky = InData.calibK[1];

			}
#endif
		}
	}
	if( (WinNum > 0)&& (vldFlag == 1))
		outSubpixCalib.write(calibPnt);
}