/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */
#ifndef CPose2DGridTemplate_H
#define CPose2DGridTemplate_H

#include <mrpt/utils/CSerializable.h>
#include <mrpt/utils/round.h> // for round()
#include <mrpt/utils/bits.h>  // for DEG2RAD()

namespace mrpt
{
namespace poses
{
	/** This is a template class for storing a 3D (2D+heading) grid containing any kind of data.
	 * \ingroup poses_pdf_grp
	 */
	template<class T>
	class CPose2DGridTemplate
	{
	protected:
		/** The limits and resolution of the grid:
		  */
		double				m_xMin, m_xMax,
							m_yMin, m_yMax,
							m_phiMin, m_phiMax,
							m_resolutionXY,m_resolutionPhi;

		/** The size of "m_data" is m_sizeX * m_sizeY * m_sizePhi
		  */
		size_t		m_sizeX,m_sizeY,m_sizePhi, m_sizeXY;

		/** The indexes of the "left" borders:
		  */
		int					m_idxLeftX, m_idxLeftY, m_idxLeftPhi;

		/** The data:
		  */
		std::vector<T>		m_data;

	public:
		/** Returns "indexes" from coordinates:
		  */
		size_t  x2idx(double x) const
		{
			int idx = mrpt::utils::round( (x-m_xMin) / m_resolutionXY );
			ASSERT_(idx>=0 && idx< static_cast<int>(m_sizeX));
			return idx;
		}

		/** Returns "indexes" from coordinates:
		  */
		size_t  y2idx(double y) const
		{
			int idx = mrpt::utils::round( (y-m_yMin) / m_resolutionXY );
			ASSERT_(idx>=0 && idx< static_cast<int>(m_sizeY));
			return idx;
		}

		/** Returns "indexes" from coordinates:
		  */
		size_t  phi2idx(double phi) const
		{
			int idx = mrpt::utils::round( (phi-m_phiMin) / m_resolutionPhi );
			ASSERT_(idx>=0 && idx< static_cast<int>(m_sizePhi) );
			return idx;
		}

		/** Returns coordinates from "indexes":
		  */
		double  idx2x(size_t x) const
		{
			ASSERT_(x<m_sizeX);
			return m_xMin + x * m_resolutionXY;
		}

		/** Returns coordinates from "indexes":
		  */
		double  idx2y(size_t y) const
		{
			ASSERT_(y<m_sizeY);
			return m_yMin + y * m_resolutionXY;
		}

		/** Returns coordinates from "indexes":
		  */
		double  idx2phi(size_t phi) const
		{
			ASSERT_(phi<m_sizePhi);
			return m_phiMin + phi * m_resolutionPhi;
		}

		/** Default constructor:
		  */
		CPose2DGridTemplate(
			double		xMin = -1.0f,
			double		xMax = 1.0f,
			double		yMin = -1.0f,
			double		yMax = 1.0f,
			double		resolutionXY = 0.5f,
			double		resolutionPhi = mrpt::utils::DEG2RAD(180),
			double		phiMin = -M_PIf,
			double		phiMax = M_PIf
			) :
				m_xMin(), m_xMax(),
				m_yMin(), m_yMax(),
				m_phiMin(), m_phiMax(),
				m_resolutionXY(),m_resolutionPhi(),
				m_sizeX(),m_sizeY(),m_sizePhi(), m_sizeXY(),
				m_idxLeftX(), m_idxLeftY(), m_idxLeftPhi(),
				m_data()
		{
			setSize(xMin,xMax,yMin,yMax,resolutionXY,resolutionPhi,phiMin,phiMax);
		}

		virtual ~CPose2DGridTemplate() { }


		/** Changes the limits and size of the grid, erasing previous contents:
		  */
		void  setSize(
			double		xMin,
			double		xMax,
			double		yMin,
			double		yMax,
			double		resolutionXY,
			double		resolutionPhi,
			double		phiMin = -M_PIf,
			double		phiMax = M_PIf
			)
		{
			// Checks
			ASSERT_( xMax > xMin );
			ASSERT_( yMax > yMin );
			ASSERT_( phiMax >= phiMin );
			ASSERT_( resolutionXY>0 );
			ASSERT_( resolutionPhi>0 );

			// Copy data:
			m_xMin = xMin;			m_xMax = xMax;
			m_yMin = yMin;			m_yMax = yMax;
			m_phiMin = phiMin;		m_phiMax = phiMax;
			m_resolutionXY = resolutionXY;
			m_resolutionPhi = resolutionPhi;

			// Compute the indexes of the starting borders:
			m_idxLeftX = mrpt::utils::round( xMin/resolutionXY ) ;
			m_idxLeftY = mrpt::utils::round( yMin/resolutionXY ) ;
			m_idxLeftPhi = mrpt::utils::round( phiMin/resolutionPhi ) ;

			// Compute new required space:
			m_sizeX = mrpt::utils::round( xMax/resolutionXY ) - m_idxLeftX + 1;
			m_sizeY = mrpt::utils::round( yMax/resolutionXY ) - m_idxLeftY + 1;
			m_sizePhi = mrpt::utils::round( phiMax/resolutionPhi ) - m_idxLeftPhi + 1;
			m_sizeXY = m_sizeX * m_sizeY;

			// Resize "m_data":
			m_data.clear();
			m_data.resize( m_sizeX * m_sizeY * m_sizePhi );
		}

		/** Reads the contents of a cell
		  */
		const T*  getByPos( double x,double y, double phi ) const
		{
            return getByIndex( x2idx(x),y2idx(y),phi2idx(phi) );
		}

		/** Reads the contents of a cell
		  */
		T*  getByPos( double x,double y, double phi )
		{
            return getByIndex( x2idx(x),y2idx(y),phi2idx(phi) );
		}

		/** Reads the contents of a cell
		  */
		const T*  getByIndex( size_t x,size_t y, size_t phi )  const
		{
			ASSERT_(x<m_sizeX && y<m_sizeY && phi<m_sizePhi)
			return &m_data[ phi*m_sizeXY + y*m_sizeX + x ];
		}

		/** Reads the contents of a cell
		  */
		T*  getByIndex( size_t x,size_t y, size_t phi )
		{
			ASSERT_(x<m_sizeX && y<m_sizeY && phi<m_sizePhi)
			return &m_data[ phi*m_sizeXY + y*m_sizeX + x ];
		}

		/** Returns the whole grid as a matrix, for a given constant "phi" and where each row contains values for a fixed "y".
		  */
		template <class MATRIXLIKE>
		void getAsMatrix( const double &phi, MATRIXLIKE &outMat )
		{
			MRPT_START
			outMat.setSize( m_sizeY, m_sizeX );
			size_t phiIdx = phi2idx(phi);
			ASSERT_(phi<m_sizePhi);
			for (size_t y=0;y<m_sizeY;y++)
				for (size_t x=0;x<m_sizeX;x++)
					outMat(y,x)=m_data[ phiIdx*m_sizeXY + y*m_sizeX + x ];
			MRPT_END
		}

		/** Get info about the grid:
		  */
		double  getXMin() const { return m_xMin; }
		double  getXMax() const { return m_xMax; }
		double  getYMin() const { return m_yMin; }
		double  getYMax() const { return m_yMax; }
		double  getPhiMin() const { return m_phiMin; }
		double  getPhiMax() const { return m_phiMax; }
		double  getResolutionXY() const { return m_resolutionXY; }
		double  getResolutionPhi() const { return m_resolutionPhi; }
		size_t  getSizeX() const { return m_sizeX; }
		size_t  getSizeY() const { return m_sizeY; }
		size_t  getSizePhi() const { return m_sizePhi; }


	}; // End of class def.

	} // End of namespace
} // End of namespace

#endif
