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

	Program:		Range Imaging Toolkit (RITK)
	Module:			ritkTruncatedSignedDistanceFunctionInterfaces.cu
	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

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

#include "ritkCudaMacros.h"

#include "ritkTruncatedSignedDistanceFunctionInterfaces.h"
#include "ritkTruncatedSignedDistanceFunctionKernels.h"

#include <stdio.h>

#ifndef DIVUP
	#define DIVUP(a,b) (((a) % (b) != 0) ? ((a)/(b) + 1) : ((a)/(b)))
#endif


//----------------------------------------------------------------------------
template<unsigned int NX, unsigned int NY, unsigned int NZ>
void
ritkTSDFIntegration_Template(ritkTSDFIntegrationConfig *TSDFIntegrationConfig)
{
	//printf("Using dedicated template kernel for VolumeSize=[%d,%d,%d]. Id=%d\n", NX,NY,NZ,TSDFIntegrationConfig->ID );

	// Bind the textures
	RangeDataTexture.addressMode[0] = cudaAddressModeClamp;
	RangeDataTexture.addressMode[1] = cudaAddressModeClamp;
	RangeDataTexture.filterMode = cudaFilterModePoint;
	RangeDataTexture.normalized = false;
	ritkCudaSafeCall(cudaBindTextureToArray(RangeDataTexture, TSDFIntegrationConfig->RangeData));

	FlagDataTexture.addressMode[0] = cudaAddressModeClamp;
	FlagDataTexture.addressMode[1] = cudaAddressModeClamp;
	FlagDataTexture.filterMode = cudaFilterModePoint;
	FlagDataTexture.normalized = false;
	ritkCudaSafeCall(cudaBindTextureToArray(FlagDataTexture, TSDFIntegrationConfig->FlagData));
	

	// The block size
	const int NUM_THREADS_X = 32;
	const int NUM_THREADS_Y = 4;
	const int NUM_THREADS_Z = 2;

	// Set up origin and spacing for the TSDF
	float3 Origin = make_float3(TSDFIntegrationConfig->TSDFVolumeOrigin[0], TSDFIntegrationConfig->TSDFVolumeOrigin[1], TSDFIntegrationConfig->TSDFVolumeOrigin[2]);
	float3 Spacing = make_float3(TSDFIntegrationConfig->TSDFVolumeSpacing[0], TSDFIntegrationConfig->TSDFVolumeSpacing[1], TSDFIntegrationConfig->TSDFVolumeSpacing[2]);


	// Set up the projection configuration
	TSDFIntegrationKernelProjectionConfiguration ProjectionConfiguration;
	for ( int i = 0; i < 12; i++ )
		ProjectionConfiguration.ProjectionMatrix[i] = TSDFIntegrationConfig->ProjectionMatrix[i];
	for ( int i = 0; i < 3; i++ )
	{
		ProjectionConfiguration.OpticalCenter[i] = TSDFIntegrationConfig->OpticalCenter[i];
		ProjectionConfiguration.PlaneNormal[i] = TSDFIntegrationConfig->PlaneNormal[i];
	}
	for ( int i = 0; i < 2; i++ )
		ProjectionConfiguration.ProjectionPlaneSize[i] = TSDFIntegrationConfig->CameraSize[i];

	// Set up the TSDF volume configuration
	TSDFIntegrationKernelVolumeConfiguration VolumeConfiguration;
	VolumeConfiguration.Data = (float*)TSDFIntegrationConfig->TSDF.ptr;
	VolumeConfiguration.Pitch = TSDFIntegrationConfig->TSDF.pitch/sizeof(float);
	for ( int i = 0; i < 3; i++ )
	{
		VolumeConfiguration.Origin[i] = TSDFIntegrationConfig->TSDFVolumeOrigin[i];
		VolumeConfiguration.Spacing[i] = TSDFIntegrationConfig->TSDFVolumeSpacing[i];
		VolumeConfiguration.Size[i] = TSDFIntegrationConfig->TSDFVolumeSize[i];
	}

	// Kernel invocation
	dim3 DimBlock = dim3(NUM_THREADS_X,NUM_THREADS_Y,NUM_THREADS_Z);
	dim3 DimGrid = dim3(DIVUP(NX,NUM_THREADS_X), DIVUP(NY,NUM_THREADS_Y)*DIVUP(NZ,NUM_THREADS_Z));
	TSDFIntegrationKernel<DIVUP(NY,NUM_THREADS_Y)><<<DimGrid, DimBlock>>>
	(
		VolumeConfiguration,
		ProjectionConfiguration,
		TSDFIntegrationConfig->CutoffValue,
		TSDFIntegrationConfig->InvalidValue
	);

	// Unbind the texture
	ritkCudaSafeCall(cudaUnbindTexture(RangeDataTexture));
}


//----------------------------------------------------------------------------
void
ritkTSDFIntegration(ritkTSDFIntegrationConfig *TSDFIntegrationConfig)
{
	/*printf("TSDFIntegration\n");
	printf("	ProjectionParameters = [%f,%f,%f,%f,%d,%d]\n", TSDFIntegrationConfig->ProjectionParameters[0], TSDFIntegrationConfig->ProjectionParameters[1], TSDFIntegrationConfig->ProjectionParameters[2], TSDFIntegrationConfig->ProjectionParameters[3], (int)TSDFIntegrationConfig->ProjectionParameters[4], (int)TSDFIntegrationConfig->ProjectionParameters[5]);
	printf("	TSDFVolumeOrigin = [%f,%f,%f]\n", TSDFIntegrationConfig->TSDFVolumeOrigin[0], TSDFIntegrationConfig->TSDFVolumeOrigin[1], TSDFIntegrationConfig->TSDFVolumeOrigin[2]);
	printf("	TSDFVolumeSpacing = [%f,%f,%f]\n", TSDFIntegrationConfig->TSDFVolumeSpacing[0], TSDFIntegrationConfig->TSDFVolumeSpacing[1], TSDFIntegrationConfig->TSDFVolumeSpacing[2]);
	printf("	TSDFVolumeSize = [%d,%d,%d]\n", TSDFIntegrationConfig->TSDFVolumeSize[0], TSDFIntegrationConfig->TSDFVolumeSize[1], TSDFIntegrationConfig->TSDFVolumeSize[2]);
	printf("	TSDFVolumePitch= [%d]\n", TSDFIntegrationConfig->TSDF.pitch);*/

	// For convenience
	int nx = TSDFIntegrationConfig->TSDFVolumeSize[0];
	int ny = TSDFIntegrationConfig->TSDFVolumeSize[1];
	int nz = TSDFIntegrationConfig->TSDFVolumeSize[2];
 
	// Test version
#if 0
	{
		int threadsInX = 16;
		int threadsInY = 4;
		int threadsInZ = 4;

		int blocksInX = (nx+threadsInX-1)/threadsInX;
		int blocksInY = (ny+threadsInY-1)/threadsInY;
		int blocksInZ = (nz+threadsInZ-1)/threadsInZ;

		int blocksInX2 = DIVUP(nx,threadsInX);
		int blocksInY2 = DIVUP(ny,threadsInY);
		int blocksInZ2 = DIVUP(nz,threadsInZ);

		//printf("BlockSize=[%d,%d,%d] vs [%d,%d,%d]\n", blocksInX, blocksInY, blocksInZ, blocksInX2, blocksInY2, blocksInZ2);

		dim3 DimGrid = dim3(blocksInX, blocksInY*blocksInZ);
		dim3 DimBlock = dim3(threadsInX, threadsInY, threadsInZ);

		TestKernel<<<DimGrid, DimBlock>>>((float*)TSDFIntegrationConfig->TSDF.ptr, TSDFIntegrationConfig->TSDF.pitch/sizeof(float), nx, ny, nz, blocksInY, 1.0f/(float)blocksInY);
	}
#endif

	// Resolve the volume size
	if ( nx == 256 && ny == 256 && nz == 256 )
	{
		ritkTSDFIntegration_Template<256,256,256>(TSDFIntegrationConfig);
	}
	else if ( nx == 384 && ny == 384 && nz == 384 )
	{
		ritkTSDFIntegration_Template<384,384,384>(TSDFIntegrationConfig);
	}
	else if ( nx == 512 && ny == 512 && nz == 512 )
	{
		ritkTSDFIntegration_Template<512,512,512>(TSDFIntegrationConfig);
	}

	//ritkCudaSafeCall(cudaDeviceSynchronize());
}
