/*======================================================================================

	Program:		Range Imaging Toolkit (RITK)
	Module:			ritkTruncatedSignedDistanceFunctionKernels.h
	Version:		$Revision: 2965 $
	Date:			$Date: 2012-04-10 18:13:53 +0200 (Di, 10 Apr 2012) $

	Copyright (c) 2010-2011
	Pattern Recognition Lab, 
    University Erlangen-Nuremberg, Germany
    All rights reserved.

	This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 
	See RITKCopyright.txt for more information.

	http://www5.informatik.uni-erlangen.de/research/software/ritk

========================================================================================*/

#ifndef RITKTRUNCATEDSIGNEDDISTANCEFUNCTIONKERNELS_H__
#define RITKTRUNCATEDSIGNEDDISTANCEFUNCTIONKERNELS_H__


// Degree to radian conversion
//----------------------------------------------------------------------------
#define DEG2RAD(x) (3.14159265f*x)/180.0f


// L2-like distance
//----------------------------------------------------------------------------
#define dist2(x,y) ((x)-(y))*((x)-(y))


// Texture for the range and flag data
//----------------------------------------------------------------------------
texture<float, 2, cudaReadModeElementType> RangeDataTexture;
texture<unsigned char, 2, cudaReadModeElementType> FlagDataTexture;


// Struct holding the projection configuration
//----------------------------------------------------------------------------
struct TSDFIntegrationKernelProjectionConfiguration
{
	// The projection matrix including camera intrinsics and extrinsics (e.g. world transform)
	float ProjectionMatrix[12]; 
	// Must have been computed from ProjectionMatrix
	float OpticalCenter[3]; 
	// Must have been computed from ProjectionMatrix (i.e. the ray direction of the center pixel)
	float PlaneNormal[3]; 
	// Detector plane in pixel
	unsigned int ProjectionPlaneSize[2]; 
};


// Struct holding the TSDF configuration and data
//----------------------------------------------------------------------------
struct TSDFIntegrationKernelVolumeConfiguration
{
	/// The size of the TSDF volume [in pixel]
	unsigned int Size[3];
	/// The origin of the TSDF volume [in mm]
	float Origin[3];
	/// The spacing of the TSDF volume [in mm/px]
	float Spacing[3];
	/// The data ptr of the TSDF volume
	float *Data;
	/// The data ptich of the TSDF volume
	size_t Pitch;
};


//----------------------------------------------------------------------------
template<unsigned int BLOCKS_IN_Y>
__global__ 
void TSDFIntegrationKernel(TSDFIntegrationKernelVolumeConfiguration V, TSDFIntegrationKernelProjectionConfiguration P, float CutoffValue, float InvalidValue)
{
	// Compute the 3D index for this thread (see http://stackoverflow.com/questions/7318002/3d-image-indices, or http://forums.nvidia.com/index.php?showtopic=87178&st=0&p=493821&#entry493821)
	unsigned int blockIdxz = __float2uint_rd(blockIdx.y * (1.f/BLOCKS_IN_Y));
	unsigned int blockIdxy = blockIdx.y - __umul24(blockIdxz, BLOCKS_IN_Y);
	unsigned int xIndex = __umul24(blockIdx.x, blockDim.x) + threadIdx.x;
	unsigned int yIndex = __umul24(blockIdxy, blockDim.y) + threadIdx.y;
	unsigned int zIndex = __umul24(blockIdxz, blockDim.z) + threadIdx.z;

	// Only valid threads (note that yIndex < ny differs from the proposed solution)
	if ( (xIndex >= V.Size[0]) || (yIndex >= V.Size[1]) || (zIndex >= V.Size[2]) )
		return;

	// Global position
	unsigned int gidx = xIndex + yIndex*V.Pitch + zIndex*V.Size[1]*V.Pitch;

	// Compute the 3D world coordinate for this index
	float x = V.Origin[0] + xIndex*V.Spacing[0];
	float y = V.Origin[1] + yIndex*V.Spacing[1];
	float z = V.Origin[2] + zIndex*V.Spacing[2];

	// Discard pixels behind the camera and below the minimal range
	/*float x2 = x - P.OpticalCenter[0];
	float y2 = y - P.OpticalCenter[1];
	float z2 = z - P.OpticalCenter[2];*/
	/*float test = (x-P.OpticalCenter[0])*P.PlaneNormal[0] + (y - P.OpticalCenter[1])*P.PlaneNormal[1] + (z - P.OpticalCenter[2])*P.PlaneNormal[2];
	if ( test < 500.f )
	{
		return;
	}*/

	// Compute projection in the image plane
	float hx = P.ProjectionMatrix[0]*x + P.ProjectionMatrix[1]*y + P.ProjectionMatrix[2]*z + P.ProjectionMatrix[3];
	float hy = P.ProjectionMatrix[4]*x + P.ProjectionMatrix[5]*y + P.ProjectionMatrix[6]*z + P.ProjectionMatrix[7];
	float hz = P.ProjectionMatrix[8]*x + P.ProjectionMatrix[9]*y + P.ProjectionMatrix[10]*z + P.ProjectionMatrix[11];
	float px = hx/hz;
	float py = hy/hz;

	// Discard pixels that do not hit the plane
	if ( px < 0.f || py < 0.f || px >= P.ProjectionPlaneSize[0] || py >= P.ProjectionPlaneSize[1] || hz > 0.f )
		return;

	// Get the range/depth and flag value in the image plane
	float wz = tex2D(RangeDataTexture, px, py);
	unsigned char flag = tex2D(FlagDataTexture, px, py);

	// Compute the distance
	float dist = wz-z;

	// Return for unseen positions
	if ( dist < -CutoffValue || flag == 0 )
		return;

	// Compute the TSDF value
	float TSDFValue = fminf(1,dist/CutoffValue);
	
	// Merge
	float OldTSDFValue = V.Data[gidx];
	if ( fabsf(OldTSDFValue) < 1e-6f  )
	{
		V.Data[gidx] = TSDFValue;
	}
	else
	{
		V.Data[gidx] = 0.7f*OldTSDFValue + 0.3f*TSDFValue;
		//V.Data[gidx] = test;
	}
}

#endif // RITKTRUNCATEDSIGNEDDISTANCEFUNCTIONKERNELS_H__