#include "HDF5PointCloudSource.h"

#include "Log.h"

HDF5PointCloudSource::HDF5PointCloudSource()
	: m_uiDepthWidth( 512 )
	, m_uiDepthHeight( 424 )
	, m_uiFrameCounter( 0 )
	, m_bOpenColorStream( false )
{

}

HDF5PointCloudSource::~HDF5PointCloudSource()
{

}

bool HDF5PointCloudSource::init(std::string strFilename, bool bOpenColorStream, unsigned int uiDepthWidth, unsigned int uiDepthHeight)
{
	m_uiDepthWidth = uiDepthWidth;
	m_uiDepthHeight = uiDepthHeight;
	m_bOpenColorStream = bOpenColorStream;

	try
	{
		m_h5File.openFile( strFilename.c_str(), H5F_ACC_RDONLY );

		/// FIRST: get the depth data
		m_depthDataSet = m_h5File.openDataSet( "/depth_measure_z_0" );

		/*
		 * Get dataspace of the dataset.
		 */
		m_depthDataSpace = m_depthDataSet.getSpace();

		/*
		 * Get the number of dimensions in the dataspace.
		 */
		int rank = m_depthDataSpace.getSimpleExtentNdims();
		if( rank != 3 )
		{
			Log::getDefaultLog() << Log::EL_ERROR << "Wrong number of dimensions in the depth image data (" << rank << ")." << Log::endl;
			return false;
		}

		/*
		 * Get the dimension size of each dimension in the dataspace and
		 * display them.
		 */
		hsize_t* dims_out = new hsize_t[rank];
		m_depthDataSpace.getSimpleExtentDims( dims_out, nullptr);

		if( dims_out[0] != uiDepthHeight || dims_out[1] != uiDepthWidth )
		{
			Log::getDefaultLog() << Log::EL_ERROR << "Depth image dimensions in the hdf5-file have the wrong size (" << dims_out[0] << "x" << dims_out[1] << ")." << Log::endl;
			return false;
		}

		m_pTempDepth = new float[m_uiDepthWidth*m_uiDepthHeight];

		/// SECOND: get the camera parameters
		m_paramsDataSet = m_h5File.openDataSet( "/camera_parameters_0" );

		/*
		 * Get dataspace of the dataset.
		 */
		m_paramsDataSpace = m_paramsDataSet.getSpace();

		/*
		 * Get the number of dimensions in the dataspace.
		 */
		rank = m_paramsDataSpace.getSimpleExtentNdims();
		if( rank != 2 )
		{
			Log::getDefaultLog() << Log::EL_ERROR << "Wrong number of dimensions in the camera parameter data (" << rank << ")." << Log::endl;
			return false;
		}

		/*
		 * Get the dimension size of each dimension in the dataspace and
		 * display them.
		 */
		hsize_t* params_dims_out = new hsize_t[rank];
		m_paramsDataSpace.getSimpleExtentDims( params_dims_out, nullptr);

		m_uiNrOfCamParams = params_dims_out[1];
		
		
		m_pDepthFrameToCameraSpaceTable = new Point2D[m_uiDepthWidth * m_uiDepthHeight];
		m_vLastCamParams.reserve(4);


		/// THIRD: get the color data if requested
		if( m_bOpenColorStream )
		{
			m_colorDataSet = m_h5File.openDataSet( "/color_measure_0" );

			/*
			 * Get dataspace of the dataset.
			 */
			m_colorDataSpace = m_colorDataSet.getSpace();

			/*
			 * Get the number of dimensions in the dataspace.
			 */
			int rank = m_colorDataSpace.getSimpleExtentNdims();
			if( rank != 4 )
			{
				Log::getDefaultLog() << Log::EL_ERROR << "Wrong number of dimensions in the color image data (" << rank << ")." << Log::endl;
				return false;
			}

			/*
			 * Get the dimension size of each dimension in the dataspace and
			 * display them.
			 */
			hsize_t* dims_out = new hsize_t[rank];
			m_colorDataSpace.getSimpleExtentDims( dims_out, nullptr);

			if( dims_out[0] != uiDepthHeight || dims_out[1] != uiDepthWidth )
			{
				Log::getDefaultLog() << Log::EL_ERROR << "Color image dimensions in the hdf5-file have the wrong size (" << dims_out[0] << "x" << dims_out[1] << ")." << Log::endl;
				return false;
			}

			m_pTempColor = new uint8_t[m_uiDepthWidth * m_uiDepthHeight * 4];
		}

	}
	// catch failure caused by the H5File operations
	catch( H5::FileIException error )
	{
	   error.printError();
	   return false;
	}
	// catch failure caused by the DataSet operations
	catch( H5::DataSetIException error )
	{
	   error.printError();
	   return false;
	}
	// catch failure caused by the DataSpace operations
	catch( H5::DataSpaceIException error )
	{
	   error.printError();
	   return false;
	}
	// catch failure caused by the DataSpace operations
	catch( H5::DataTypeIException error )
	{
	   error.printError();
	   return false;
	}

	return true;  // successfully terminated
}

std::shared_ptr<DepthImage> HDF5PointCloudSource::process()
{
	std::shared_ptr<DepthImage> pRet( new DepthImage );

	try
	{
		pRet->uiNrOfDepthPoints = m_uiDepthWidth * m_uiDepthHeight;
		pRet->uiHeight = m_uiDepthHeight;
		pRet->uiWidth = m_uiDepthWidth;
		pRet->pImage.reset( new std::vector<unsigned short>( pRet->uiNrOfDepthPoints ) );

		/// FIRST: get the current frame in depth data
		/*
		 * Define hyperslab in the dataset; implicitly giving strike and
		 * block NULL.
		 */
		hsize_t offset[ 3 ] = { 0, 0, m_uiFrameCounter };
		hsize_t dims1[ 3 ] = { m_uiDepthHeight, m_uiDepthWidth, 1 };
		m_depthDataSpace.selectHyperslab( H5S_SELECT_SET, dims1, offset );

		/*
		 * Define the memory dataspace.
		 */
		hsize_t dimsm[ 3 ] = { m_uiDepthHeight, m_uiDepthWidth, 1 };
		H5::DataSpace memspace( 3, dimsm );

		/*
		 * Read data from hyperslab in the file into the hyperslab in
		 * memory and display the data.
		 */
		m_depthDataSet.read( m_pTempDepth, H5::PredType::NATIVE_FLOAT, memspace, m_depthDataSpace );

		unsigned short* pData = pRet->pImage->data();
		for( unsigned int i=0; i<m_uiDepthWidth*m_uiDepthHeight; ++i )
			pData[i] = (unsigned short)m_pTempDepth[i];


		/// SECOND: get the camera parameters
		/*
		 * Define hyperslab in the dataset; implicitly giving strike and
		 * block NULL.
		 */
		hsize_t params_offset[ 2 ] = { m_uiFrameCounter, 0 };
		hsize_t params_dims1[ 2 ] = { 1, m_uiNrOfCamParams };
		m_paramsDataSpace.selectHyperslab( H5S_SELECT_SET, params_dims1, params_offset );

		/*
		 * Define the memory dataspace.
		 */
		hsize_t params_dimsm[ 2 ] = { 1, m_uiNrOfCamParams };
		H5::DataSpace params_memspace( 2, params_dimsm );

		/*
		 * Read data from hyperslab in the file into the hyperslab in
		 * memory and display the data.
		 */
		m_paramsDataSet.read( pRet->pCamParams->data(), H5::PredType::NATIVE_FLOAT, params_memspace, m_paramsDataSpace );


		/// THIRD: get the current color frame if requested
		if( m_bOpenColorStream )
		{
			/*
			 * Define hyperslab in the dataset; implicitly giving strike and
			 * block NULL.
			 */
			hsize_t offset[ 4 ] = { 0, 0, 0, m_uiFrameCounter };
			hsize_t dims1[ 4 ] = { m_uiDepthHeight, m_uiDepthWidth, 4, 1 };
			m_colorDataSpace.selectHyperslab( H5S_SELECT_SET, dims1, offset );

			/*
			 * Define the memory dataspace.
			 */
			hsize_t dimsm[ 4 ] = { m_uiDepthHeight, m_uiDepthWidth, 4, 1 };
			H5::DataSpace memspace( 4, dimsm );

			/*
			 * Read data from hyperslab in the file into the hyperslab in
			 * memory and display the data.
			 */
			pRet->pColors.reset( new uint8_t[m_uiDepthWidth * m_uiDepthHeight * 4] );
			uint8_t* pData = pRet->pColors.get();
			m_colorDataSet.read( m_pTempColor, H5::PredType::NATIVE_UCHAR, memspace, m_colorDataSpace );
			for( unsigned int i=0; i<m_uiDepthHeight; ++i )
				for( unsigned int j=0; j<m_uiDepthWidth; ++j )
				{
					pData[ i * m_uiDepthWidth * 4 + j * 4 + 0 ] = m_pTempColor[ i * m_uiDepthWidth * 4 + j * 4 + 2 ];
					pData[ i * m_uiDepthWidth * 4 + j * 4 + 1 ] = m_pTempColor[ i * m_uiDepthWidth * 4 + j * 4 + 1 ];
					pData[ i * m_uiDepthWidth * 4 + j * 4 + 2 ] = m_pTempColor[ i * m_uiDepthWidth * 4 + j * 4 + 0 ];
					pData[ i * m_uiDepthWidth * 4 + j * 4 + 3 ] = m_pTempColor[ i * m_uiDepthWidth * 4 + j * 4 + 3 ];

					/*for( unsigned int k=0; k<4; ++k )
						pData[ i * m_uiDepthWidth * 4 + j * 4 + k ] = m_pTempColor[ i + j * m_uiDepthHeight + k * m_uiDepthHeight * m_uiDepthWidth ];
					*/
				}
		}

		++m_uiFrameCounter;
	}
	// catch failure caused by the H5File operations
	catch( H5::FileIException error )
	{
	   error.printError();
	   return nullptr;
	}
	// catch failure caused by the DataSet operations
	catch( H5::DataSetIException error )
	{
	   error.printError();
	   return nullptr;
	}
	// catch failure caused by the DataSpace operations
	catch( H5::DataSpaceIException error )
	{
	   error.printError();
	   return nullptr;
	}
	// catch failure caused by the DataSpace operations
	catch( H5::DataTypeIException error )
	{
	   error.printError();
	   return nullptr;
	}

	return pRet;
}


void HDF5PointCloudSource::createTransformArray( std::vector<float>* pCamParams )
{
	if( pCamParams == nullptr || pCamParams->size() < 4 ||
		( pCamParams->at(0) == m_vLastCamParams[0] && pCamParams->at(1) == m_vLastCamParams[1] &&
		  pCamParams->at(2) == m_vLastCamParams[2] && pCamParams->at(3) == m_vLastCamParams[3] ) )
		return;

	const float cx = pCamParams->at(2), cy = pCamParams->at(3);
	const float fx =  1.f / pCamParams->at(0), fy = 1.f / pCamParams->at(1);

	unsigned int i = 0;
	for( float r = -m_uiDepthHeight/2; r < m_uiDepthHeight/2; ++r )
	{
		for( float c = -m_uiDepthWidth/2; c < m_uiDepthWidth/2; ++c )
		{
			m_pDepthFrameToCameraSpaceTable[i].X = (c + 0.5 - cx) * fx;
			m_pDepthFrameToCameraSpaceTable[i].Y = (r + 0.5 - cy) * fy;
			++i;
		}
	}

	for( unsigned int i = 0; i < 4; ++i )
		m_vLastCamParams[i] = pCamParams->at(i);
}