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

	Program:		Range Imaging Toolkit (RITK)
	Module:			ritkTruncatedSignedDistanceFunctionRayCasterKernels.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 RITKTRUNCATEDSIGNEDDISTANCEFUNCTIONRAYCASTERKERNELS_H__
#define RITKTRUNCATEDSIGNEDDISTANCEFUNCTIONRAYCASTERKERNELS_H__

#include "ritkTruncatedSignedDistanceFunctionRayCasterInterfaces.h"
#include "ritkCudaRayCasterCommon.h"

/// Our volume texture
/// ==================
texture<float,3,cudaReadModeElementType> VolumeTexture;

/**	@name	The parameters for ray casting */
//@{
__constant__ ritkCudaRayCasterVolumeParameters c_VP;
__constant__ ritkCudaRayCasterProjectionImageParameters<float> c_IP;
__constant__ ritkCudaRayCasterProjectionParameters c_PP;
//@}



/// The actual ray casting kernel
/// =============================
__global__ void
TSDFRayCastKernel(const ritkCudaRayCasterVolumeParameters VP, const ritkCudaRayCasterProjectionImageParameters<float> RIP, const ritkCudaRayCasterProjectionImageParameters<float3> WIP, const ritkCudaRayCasterProjectionParameters PP, float3 WorldToTex, float StepLength)
{
	// Get the output image indices for this thread
	int IX = blockIdx.x*blockDim.x + threadIdx.x;
	int IY = blockIdx.y*blockDim.y + threadIdx.y;

	// Check for out-of-bounds
	if ( IX >= RIP.Size.x || IY >= RIP.Size.y )
		return;

	// The linear output index for this threaed
	int r_pos = IY*RIP.Pitch + IX;
	int w_pos = IY*WIP.Pitch + IX;

	// Compute physical (aka continous) image indices from the pixel indices for this thread
	float PX = (RIP.Origin.x/RIP.Spacing.x) + IX;
	float PY = (RIP.Origin.y/RIP.Spacing.y) + IY;

	// Set up the ray for this position in the image plan
	float3 RayDirection;
	ComputeRayDirection(PX,PY,PP.InverseProjectionMatrix,&RayDirection);

	// Compute intersection with the volume (results are stored in the ray)
	float2 Alpha;
	IntersectVolume(&PP.OpticalCenter, &RayDirection, &VP, &Alpha); 

	// Sanity check
	if ( Alpha.y <= Alpha.x || fabsf(Alpha.y-Alpha.x) < 1e-3f )
	{
		RIP.DataPtr[r_pos] = 0.f;
		WIP.DataPtr[w_pos] = make_float3(0.f,0.f,0.f);
		return;
	}

	// Initialize step length
	float3 Delta = StepLength*RayDirection;

	// Texture position (SamplePosition-VolumeOrigin)*WorldToTex and incremental StepLength*WorldToTex
	float3 TexCoord = (PP.OpticalCenter + Alpha.x * RayDirection - VP.Origin) * WorldToTex;
	float3 TexCoordDelta = Delta*WorldToTex;

	// Evaluate the ray
	float PreviousDist = 0.f;
	float Depth = Alpha.x;
	while ( Depth < Alpha.y )
	{
		// Update position
		TexCoord += TexCoordDelta;

		// Get value
		float CurrentDist = tex3D(VolumeTexture,TexCoord.x,TexCoord.y,TexCoord.z);

		// Check for zero crossing
		if ( CurrentDist < 0.f )
		{
			float scale = (PreviousDist - 0.f)/(PreviousDist - CurrentDist);
			float Length = scale*StepLength;
			Depth += Length;
			//RIP.DataPtr[r_pos] = Depth;

			/*Depth -= StepLength*PreviousDist/(CurrentDist-PreviousDist);
			RIP.DataPtr[r_pos] = Depth;*/


			float3 WorldCoord = PP.OpticalCenter + Depth*RayDirection;
			//WIP.DataPtr[w_pos] = make_float3(0.f);
			RIP.DataPtr[r_pos] = WorldCoord.z;
			return;
		}

		// Update the previous dist and the number of steps performed
		PreviousDist = CurrentDist;
		Depth += StepLength;
	}

	// Set values in the output image
	RIP.DataPtr[r_pos] = 42.f;
	//WIP.DataPtr[w_pos] = make_float3(0.f,0.f,0.f);
}


#if 0
/// The actual ray casting kernel
/// =============================
template<int NUMSAMPLES>
__global__ void
TSDFRayCastKernel(const ritkCudaRayCasterVolumeParameters VP, const ritkCudaRayCasterProjectionImageParameters<float> RIP, const ritkCudaRayCasterProjectionImageParameters<float3> WIP, const ritkCudaRayCasterProjectionParameters PP, float3 WorldToTex, float CutoffValue, float InvalidValue)
{
	// Get the output image indices for this thread
	int IX = blockIdx.x*blockDim.x + threadIdx.x;
	int IY = blockIdx.y*blockDim.y + threadIdx.y;

	// Check for out-of-bounds
	if ( IX >= RIP.Size.x || IY >= RIP.Size.y )
		return;

	// The linear output index for this threaed
	int r_pos = IY*RIP.Pitch + IX;
	int w_pos = IY*WIP.Pitch + IX;

	// Compute physical (aka continous) image indices from the pixel indices for this thread
	float PX = (RIP.Origin.x/RIP.Spacing.x) + IX;
	float PY = (RIP.Origin.y/RIP.Spacing.y) + IY;

	// Set up the ray for this position in the image plan
	float3 RayDirection;
	ComputeRayDirection(PX,PY,PP.InverseProjectionMatrix,&RayDirection);

	// Compute intersection with the volume (results are stored in the ray)
	float2 Alpha;
	IntersectVolume(&PP.OpticalCenter, &RayDirection, &VP, &Alpha); 

	// Sanity check
	if ( Alpha.y <= Alpha.x || fabsf(Alpha.y-Alpha.x) < 1e-3f )
	{
		RIP.DataPtr[r_pos] = 0.f;
		WIP.DataPtr[w_pos] = make_float3(0.f,0.f,0.f);
		return;
	}

	// Initialize step length
	float3 Delta = RayDirection * (Alpha.y-Alpha.x)/(NUMSAMPLES-1);
	float StepLength = sqrtf(Delta.x*Delta.x + Delta.y*Delta.y + Delta.z*Delta.z);

	// Texture position (SamplePosition-VolumeOrigin)*WorldToTex and incremental StepLength*WorldToTex
	float3 TexCoord = (PP.OpticalCenter + Alpha.x * RayDirection - VP.Origin) * WorldToTex;
	float3 TexCoordDelta = Delta*WorldToTex;

	// Evaluate the ray
	int Cnt = 0;
	float PreviousDist = 0.f;
	float Depth = 0.f;
	for ( int i = 0; i < NUMSAMPLES; i++ )
	{
		// Update position
		TexCoord += TexCoordDelta;

		// Get value
		float CurrentDist = tex3D(VolumeTexture,TexCoord.x,TexCoord.y,TexCoord.z);

		// Check for zero crossing
		if ( CurrentDist < 0 )
		{
			float scale = (PreviousDist - 0.f)/(PreviousDist - CurrentDist);
			float Length = scale*StepLength;
			Depth = Alpha.x + Cnt*StepLength + Length;
			break;
		}

		// Update the previous dist and the number of steps performed
		PreviousDist = CurrentDist;
		Cnt++;
	}

	// Set values in the output image
	RIP.DataPtr[r_pos] = Depth;
	float3 WorldCoord = PP.OpticalCenter + Depth*RayDirection;
	WIP.DataPtr[w_pos] = WorldCoord;
}
#endif





#endif // RITKTRUNCATEDSIGNEDDISTANCEFUNCTIONRAYCASTERKERNELS_H__