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

void compute3D(
	std::vector<RgnSubPixCalib> 	&inSubCalib,
	std::vector<Pnt3D> 			&outPt3D,
	double u0,
	double v0,
	double F_inv,
	double plane_a,
	double plane_b,
	double plane_c
)
{
	RgnSubPixCalib 	InData;
	int readPtr = 0;
	InData = inSubCalib[readPtr++];
	uchar VSync = InData.Sync &0x02;
	if(0 == VSync)  //wait for syncFirst
		return;

	ushort WinNum = 0;
	Pnt3D outData = {0,0,0,0,0};
	//1st: frameNo
	outData.Sync = InData.Sync;
	outData.x = InData.x;
	outPt3D.push_back(outData);
	//2nd: timeStamp
	InData = inSubCalib[readPtr++];
	outData.Sync = InData.Sync;
	outData.x = InData.x;
	outPt3D.push_back(outData);
	//3rd:encInfo
	InData = inSubCalib[readPtr++];
	outData.Sync = InData.Sync;
	outData.x = InData.x;
	outPt3D.push_back(outData);
	//4th: frmW, frmH
	InData = inSubCalib[readPtr++];
	//outData.Sync = InData.Sync;
	//outData.x = InData.x;
	//outPt3D.write(outData);
	//5th: frmX, WinNum
	InData = inSubCalib[readPtr++];
	outData.Sync = InData.Sync;
	UintFloat tmp;
	tmp.fdata = InData.x;
	uint_32 d32 = (uint_32)tmp.unData;
	WinNum = (d32 >> 12) & 0x7fff; // .range(23, 12);
	tmp.unData = (unsigned int)WinNum;
	outData.x = tmp.fdata;
	outPt3D.push_back(outData);
	//6th: frmY
	InData = inSubCalib[readPtr++];
	//outData.Sync = InData.Sync;
	//outData.x = InData.x;
	//outPt3D.write(outData);

	for(int n = 0; n < WinNum; n ++)
	{
		InData = inSubCalib[readPtr++];

		double u = (double)InData.x;
		double v = (double)InData.y;
		double tmp_x = (u - u0) * F_inv;
		double tmp_y = (v - v0) * F_inv;
		double z_inv = plane_a * tmp_x + plane_b * tmp_y - 1;
		double z = 1 / z_inv;
		z = -z * plane_c;

		double x = tmp_x * z ;
		double y = tmp_y * z ;
		outData.x = (float)x;
		outData.y = (float)y;
		outData.z = (float)z;
		outData.value = InData.value;
		outData.Sync = 0;
		outPt3D.push_back(outData);
	}
}