#include "compute3D.h"
#include "stdlib.h"

void compute3D(
	hls::stream<RgnSubPixCalib> 	&inSubCalib,
	hls::stream<Pnt3D> 			&outPt3D,
	double u0,
	double v0,
	double F_inv,
	double plane_a,
	double plane_b,
	double plane_c
)
{
#pragma HLS DATA_PACK variable=inSubCalib
#pragma HLS DATA_PACK variable=outPt3D
#pragma HLS INTERFACE axis register both port=inSubCalib
#pragma HLS INTERFACE axis register both port=outPt3D


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

	ap_uint<12> WinNum = 0;
	Pnt3D outData = {0,0,0,0,0};
	//1st: frameNo
	outData.Sync = InData.Sync;
	outData.x = InData.x;
	outPt3D.write(outData);
	//2nd: timeStamp
	InData = inSubCalib.read();
	outData.Sync = InData.Sync;
	outData.x = InData.x;
	outPt3D.write(outData);
	//3rd:encInfo
	InData = inSubCalib.read();
	outData.Sync = InData.Sync;
	outData.x = InData.x;
	outPt3D.write(outData);
	//4th: frmW, frmH
	InData = inSubCalib.read();
	//outData.Sync = InData.Sync;
	//outData.x = InData.x;
	//outPt3D.write(outData);
	//5th: frmX, WinNum
	InData = inSubCalib.read();
	outData.Sync = InData.Sync;
	UintFloat tmp;
	tmp.fdata = InData.x;
	ap_uint<32> d32 = (ap_uint<32>)tmp.unData;
	WinNum(11,0) = d32.range(23,12);
	tmp.unData = (unsigned int)WinNum;
	outData.x = tmp.fdata;
	outPt3D.write(outData);
	//6th: frmY
	InData = inSubCalib.read();
	//outData.Sync = InData.Sync;
	//outData.x = InData.x;
	//outPt3D.write(outData);

	for(int n = 0; n < WinNum; n ++)
	{
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
#pragma HLS PIPELINE II=1

		InData = inSubCalib.read();

		double u = (double)InData.x;
		double v = (double)InData.y;
		double z_inv = u * plane_a + v * plane_b + plane_c;
		double z = 1/ z_inv;
		double x = (u - u0) * z * F_inv;
		double y = (v - v0) * z * F_inv;
		outData.x = (float)x;
		outData.y = (float)y;
		outData.z = (float)z;
		outData.value = InData.value;
		outData.Sync = 0;
		outPt3D.write(outData);
	}
}