#ifndef CUDAKINECTRANGETOWORLDKERNEL_H__
#define CUDAKINECTRANGETOWORLDKERNEL_H__

#include <cuda_runtime.h>
#include <driver_types.h>
#include <channel_descriptor.h>
#include <cuda_runtime_api.h>

#define M_PI 3.1415f
#define M_PI2 6.283f
#define VN 2.5066f


// Texture that holds the input image
//----------------------------------------------------------------------------
texture<float, 2, cudaReadModeElementType> InputImageTexture;


// Division. If division remainder is neq zero then the result is ceiled
//----------------------------------------------------------------------------
#define DivUp(a,b) ((a % b != 0) ? (a/b + 1) : (a/b))


//----------------------------------------------------------------------------
template<unsigned int BlockSizeX, unsigned int BlockSizeY>
__global__ void
CUDAKinectRangeToWorldKernel(float3* Output, unsigned int NX, unsigned int NY, float X2Z, float Y2Z)
{
	// 2D index and linear index within this thread block
	int tu = threadIdx.x;
	int tv = threadIdx.y;
	
	// Global 2D index and linear index.
	float gu = blockIdx.x*BlockSizeX+tu;
	float gv = blockIdx.y*BlockSizeY+tv;
	int gl = gv*NX + gu;

	// Check for out-of-bounds
	if ( gu >= NX || gv >= NY )
		return;

	// The range value
	float z = tex2D(InputImageTexture, gu, gv);

	// The corresponding x,y,z triple
	if ( z < 500.f )
		z = sqrtf(-1.0f);

	float fNormalizedX = gu / NX - 0.5f; // check for float
	float x = fNormalizedX * z * X2Z;

	float fNormalizedY = 0.5f - gv / NY;
	float y = fNormalizedY * z * Y2Z;

	// Write to output
	Output[gl].x = x;
	Output[gl].y = y;
	Output[gl].z = z;
}


#endif // CUDAKINECTRANGETOWORLDKERNEL_H__