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

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

	CalibData 	InData;
	InData = inCalibData.read();
	if(0b10 != InData.Sync)  //wait for syncFirst
		return;

	Byte8 dataBuff;
	dataBuff.dData = InData.calibK[0];
	ap_uint<12> Lines = 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;
	Lines(11,0) = d32_t5h.range(23,12);  //lines
	//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);

	Byte8 dataConver_x, dataConver_y;
	//double kx, ky;
	float kx_1, kx_2, ky_1, ky_2;

	//line_pt0
	RgnSubPixCalib calibPnt;
	float rawx,rawy;
	double x2, x3,x4,x5,x6,x7;
	double y2, y3,y4,y5,y6,y7;
	float xmul0=0, xmul1=0, xmul2=0, xmul3=0, xmul4=0, xmul5=0, xmul6=0, xmul7=0;
	float ymul0=0, ymul1=0, ymul2=0, ymul3=0, ymul4=0, ymul5=0, ymul6=0, ymul7=0;

	//line_pt1
	RgnSubPixCalib calibPnt_pt1;
	float rawx_pt1,rawy_pt1;
	double x2_pt1, x3_pt1,x4_pt1,x5_pt1,x6_pt1,x7_pt1;
	double y2_pt1, y3_pt1,y4_pt1,y5_pt1,y6_pt1,y7_pt1;
	float xmul0_pt1=0, xmul1_pt1=0, xmul2_pt1=0, xmul3_pt1=0, xmul4_pt1=0, xmul5_pt1=0, xmul6_pt1=0, xmul7_pt1=0;
	float ymul0_pt1=0, ymul1_pt1=0, ymul2_pt1=0, ymul3_pt1=0, ymul4_pt1=0, ymul5_pt1=0, ymul6_pt1=0, ymul7_pt1=0;

	bool vldFlag = 0;
	bool vldFlag_pt1 = 0;
	for(int n = 0; n < Lines; n ++)
	{
#pragma HLS LOOP_TRIPCOUNT min=4096 max=4096
		for(int j = 0; j <= 5; j ++) //6 clocks: 0-pt0, 1-pt1, 2£¬3£¬4£¬5 - K
		{
#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 = (float)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 = (float)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 = InData.Sync;
				calibPnt.rsv = 0;
				if(rawx < 0)
					vldFlag = 0;
				else
					vldFlag = 1;

				double dtmp_1 = (double)rawx;
				double dtmp_2 = (double)rawy;
				x2 = dtmp_1 * dtmp_1;
				y2 = dtmp_2 * dtmp_2;
			}
			else if( 1 == j)//read subpix_pt1
			{
				calibPnt_pt1.x = (float)(xmul0_pt1 + xmul1_pt1 + xmul2_pt1 + xmul3_pt1 + xmul4_pt1 + xmul5_pt1+ xmul6_pt1 + xmul7_pt1);
				calibPnt_pt1.y = (float)(ymul0_pt1 + ymul1_pt1 + ymul2_pt1 + ymul3_pt1 + ymul4_pt1 + ymul5_pt1+ ymul6_pt1 + ymul7_pt1);
				if( (n > 0) && (vldFlag_pt1 == 1))
					outSubpixCalib.write(calibPnt_pt1);

				dataBuff.dData = InData.calibK[0];
				rawx_pt1 = (float)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_pt1 = (float)dy_12;
				calibPnt_pt1.rid = apData32.range(22, 12);
				calibPnt_pt1.flag = apData32.range(26,23);
				dataBuff.dData = InData.calibK[1];
				apData32 = (ap_uint<32>)dataBuff.nData[0];
				calibPnt_pt1.value = apData32.range(7,0);
				calibPnt_pt1.Sync = InData.Sync;
				calibPnt_pt1.rsv = 0;
				if(rawx_pt1 < 0)
					vldFlag_pt1 = 0;
				else
					vldFlag_pt1 = 1;

				double dtmp_1 = (double)rawx_pt1;
				double dtmp_2 = (double)rawy_pt1;
				x2_pt1 = dtmp_1 * dtmp_1;
				y2_pt1 = dtmp_2 * dtmp_2;
			}
			else if(2 == j)//k0, k1*x
			{
				//kx = InData.calibK[0];
				//ky = InData.calibK[1];
				dataConver_x.dData = InData.calibK[0];
				kx_1 = (float)dataConver_x.fData[0];
				ky_1 = (float)dataConver_x.fData[1];
				dataConver_y.dData = InData.calibK[1];
				kx_2 = (float)dataConver_y.fData[0];
				ky_2 = (float)dataConver_y.fData[1];

				xmul0 = kx_1;
				ymul0 = ky_1;
				xmul1 = kx_2 * rawx;
				ymul1 = ky_2 * rawy;

				double dtmp_1 = (double)rawx;
				double dtmp_2 = (double)rawy;
				x3 = x2 * dtmp_1;
				y3 = y2 * dtmp_2;

				xmul0_pt1 = kx_1;
				ymul0_pt1 = ky_1;
				xmul1_pt1 = kx_2 * rawx_pt1;
				ymul1_pt1 = ky_2 * rawy_pt1;

				double dtmp_3 = (double)rawx_pt1;
				double dtmp_4 = (double)rawy_pt1;
				x3_pt1 = x2_pt1 * dtmp_3;
				y3_pt1 = y2_pt1 * dtmp_4;
			}
			else if(3 == j)//k2 * x^2, k3 * x^3
			{
				dataConver_x.dData = InData.calibK[0];
				kx_1 = (float)dataConver_x.fData[0];
				ky_1 = (float)dataConver_x.fData[1];
				dataConver_y.dData = InData.calibK[1];
				kx_2 = (float)dataConver_y.fData[0];
				ky_2 = (float)dataConver_y.fData[1];

				float x2_tmp = (float)x2;
				float y2_tmp = (float)y2;
				float x3_tmp = (float)x3;
				float y3_tmp = (float)y3;
				xmul2 = kx_1 * x2_tmp;
				ymul2 = ky_1 * y2_tmp;
				xmul3 = kx_2 * x3_tmp;
				ymul3 = ky_2 * y3_tmp;

				x4 = x2 * x2;
				y4 = y2 * y2;
				x5 = x3 * x2;
				y5 = y3 * x2;

				float x2_tmp_pt1 = (float)x2_pt1;
				float y2_tmp_pt1 = (float)y2_pt1;
				float x3_tmp_pt1 = (float)x3_pt1;
				float y3_tmp_pt1 = (float)y3_pt1;
				xmul2_pt1 = kx_1 * x2_tmp_pt1;
				ymul2_pt1 = ky_1 * y2_tmp_pt1;
				xmul3_pt1 = kx_2 * x3_tmp_pt1;
				ymul3_pt1 = ky_2 * y3_tmp_pt1;


				x4_pt1 = x2_pt1 * x2_pt1;
				y4_pt1 = y2_pt1 * y2_pt1;
				x5_pt1 = x3_pt1 * x2_pt1;
				y5_pt1 = y3_pt1 * x2_pt1;
			}
			else if(4 == j) //k4 * x^4, k5 * x^5
			{
				dataConver_x.dData = InData.calibK[0];
				kx_1 = (float)dataConver_x.fData[0];
				ky_1 = (float)dataConver_x.fData[1];
				dataConver_y.dData = InData.calibK[1];
				kx_2 = (float)dataConver_y.fData[0];
				ky_2 = (float)dataConver_y.fData[1];

				float x4_tmp = (float)x4;
				float y4_tmp = (float)y4;
				float x5_tmp = (float)x5;
				float y5_tmp = (float)y5;
				xmul4 = kx_1 * x4_tmp;
				ymul4 = ky_1 * y4_tmp;
				xmul5 = kx_2 * x5_tmp;
				ymul5 = ky_2 * y5_tmp;

				x6 = x4 * x2;
				y6 = y4 * x2;
				x7 = x5 * x2;
				y7 = y5 * x2;

				float x4_tmp_pt1 = (float)x4_pt1;
				float y4_tmp_pt1 = (float)y4_pt1;
				float x5_tmp_pt1 = (float)x5_pt1;
				float y5_tmp_pt1 = (float)y5_pt1;
				xmul4_pt1 = kx_1 * x4_tmp_pt1;
				ymul4_pt1 = ky_1 * y4_tmp_pt1;
				xmul5_pt1 = kx_2 * x5_tmp_pt1;
				ymul5_pt1 = ky_2 * y5_tmp_pt1;

				x6_pt1 = x4_pt1 * x2_pt1;
				y6_pt1 = y4_pt1 * x2_pt1;
				x7_pt1 = x5_pt1 * x2_pt1;
				y7_pt1 = y5_pt1 * x2_pt1;
			}
			else if(5 == j)//k6 * x^6, k7 * x^7
			{
				dataConver_x.dData = InData.calibK[0];
				kx_1 = (float)dataConver_x.fData[0];
				ky_1 = (float)dataConver_x.fData[1];
				dataConver_y.dData = InData.calibK[1];
				kx_2 = (float)dataConver_y.fData[0];
				ky_2 = (float)dataConver_y.fData[1];

				float x6_tmp = (float)x6;
				float y6_tmp = (float)y6;
				float x7_tmp = (float)x7;
				float y7_tmp = (float)y7;
				xmul6 = kx_1 * x6_tmp;
				ymul6 = ky_1 * y6_tmp;
				xmul7 = kx_2 * x7_tmp;
				ymul7 = ky_2 * y7_tmp;

				float x6_tmp_pt1 = (float)x6_pt1;
				float y6_tmp_pt1 = (float)y6_pt1;
				float x7_tmp_pt1 = (float)x7_pt1;
				float y7_tmp_pt1 = (float)y7_pt1;
				xmul6_pt1 = kx_1 * x6_tmp_pt1;
				ymul6_pt1 = ky_1 * y6_tmp_pt1;
				xmul7_pt1 = kx_2 * x7_tmp_pt1;
				ymul7_pt1 = ky_2 * y7_tmp_pt1;
			}
#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( (Lines > 0)&& (vldFlag == 1))
	{
		calibPnt.x = (float)(xmul0 + xmul1 + xmul2 + xmul3 + xmul4 + xmul5+ xmul6 + xmul7);
		calibPnt.y = (float)(ymul0 + ymul1 + ymul2 + ymul3 + ymul4 + ymul5+ ymul6 + ymul7);
		outSubpixCalib.write(calibPnt);
	}

	if( (Lines > 0)&& (vldFlag_pt1 == 1))
	{
		calibPnt_pt1.x = (float)(xmul0_pt1 + xmul1_pt1 + xmul2_pt1 + xmul3_pt1 + xmul4_pt1 + xmul5_pt1+ xmul6_pt1 + xmul7_pt1);
		calibPnt_pt1.y = (float)(ymul0_pt1 + ymul1_pt1 + ymul2_pt1 + ymul3_pt1 + ymul4_pt1 + ymul5_pt1+ ymul6_pt1 + ymul7_pt1);
		outSubpixCalib.write(calibPnt_pt1);
	}
}