/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT 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 the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CPose2DGridTemplate_H
#define CPose2DGridTemplate_H

#include <MRPT/UTILS/CSerializable.h>

namespace MRML
{
	/** This is a template class for storing a 3D (2D+heading) grid containing any kind of data.
	 * \sa
	 */
	template<class T>
	class CPose2DGridTemplate
	{
	protected:
		/** The limits and resolution of the grid:
		  */
		float				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(float x) const
		{
			int idx = round( (x-m_xMin) / m_resolutionXY );
			ASSERT_(idx>=0 && idx< static_cast<int>(m_sizeX));
			return idx;
		}

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

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

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

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

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

		/** Default constructor:
		  */
		CPose2DGridTemplate(
			float		xMin = -1.0f,
			float		xMax = 1.0f,
			float		yMin = -1.0f,
			float		yMax = 1.0f,
			float		resolutionXY = 0.5f,
			float		resolutionPhi = DEG2RAD(180),
			float		phiMin = -M_PIf,
			float		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(
			float		xMin,
			float		xMax,
			float		yMin,
			float		yMax,
			float		resolutionXY,
			float		resolutionPhi,
			float		phiMin = -M_PIf,
			float		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 = round( xMin/resolutionXY ) ;
			m_idxLeftY = round( yMin/resolutionXY ) ;
			m_idxLeftPhi = round( phiMin/resolutionPhi ) ;

			// Compute new required space:
			m_sizeX = round( xMax/resolutionXY ) - m_idxLeftX + 1;
			m_sizeY = round( yMax/resolutionXY ) - m_idxLeftY + 1;
			m_sizePhi = 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( float x,float y, float phi ) const
		{
            return getByIndex( x2idx(x),y2idx(y),phi2idx(phi) );
		}

		/** Reads the contents of a cell
		  */
		T*  getByPos( float x,float y, float 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>=0 && x<m_sizeX);
			ASSERT_(y>=0 && y<m_sizeY);
			ASSERT_(phi>=0 && 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>=0 && x<m_sizeX);
			ASSERT_(y>=0 && y<m_sizeY);
			ASSERT_(phi>=0 && 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".
		  */
		void getAsMatrix( const float &phi, CMatrixTemplate<T> &outMat )
		{
			MRPT_TRY_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_TRY_END
		}

		/** Get info about the grid:
		  */
		float  getXMin() const { return m_xMin; }
		float  getXMax() const { return m_xMax; }
		float  getYMin() const { return m_yMin; }
		float  getYMax() const { return m_yMax; }
		float  getPhiMin() const { return m_phiMin; }
		float  getPhiMax() const { return m_phiMax; }
		float  getResolutionXY() const { return m_resolutionXY; }
		float  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

#endif
