/* +---------------------------------------------------------------------------+
   |          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 CDynamicGrid_H
#define CDynamicGrid_H

#include <MRPT/UTILS/utils_defs.h>

namespace UTILS
{
	/** A 2D grid of dynamic size which stores any kind of data at each cell.
	 */
	template <class T>
	class CDynamicGrid
	{
	protected:
		/** The cells.
		  */
		std::vector<T>		m_map;

		float				m_x_min,m_x_max,m_y_min,m_y_max;
		float				m_resolution;
		unsigned int		m_size_x, m_size_y;

	public:
		/** Constructor
		  */
		CDynamicGrid(	float	x_min = -10.0f,
						float	x_max = 10.0f,
						float	y_min = -10.0f,
						float	y_max = 10.0f,
						float	resolution  = 0.10f) :
							m_map(),
							m_x_min(),m_x_max(),m_y_min(),m_y_max(),
							m_resolution(),
							m_size_x(), m_size_y()
		{
			setSize(x_min,x_max,y_min,y_max,resolution);
		}

		/** Destructor
		  */
		virtual ~CDynamicGrid()
		{
		}

		/** Changes the size of the grid, ERASING all previous contents.
		  * \sa resize
		  */
		void  setSize(	float	x_min,
									float	x_max,
									float	y_min,
									float	y_max,
									float	resolution )
		{
			// Adjust sizes to adapt them to full sized cells acording to the resolution:
			m_x_min = resolution*round(x_min/resolution);
			m_y_min = resolution*round(y_min/resolution);
			m_x_max = resolution*round(x_max/resolution);
			m_y_max = resolution*round(y_max/resolution);

			// Res:
			m_resolution = resolution;

			// Now the number of cells should be integers:
			m_size_x = round((m_x_max-m_x_min)/m_resolution);
			m_size_y = round((m_y_max-m_y_min)/m_resolution);

			// Cells memory:
			m_map.resize(m_size_x*m_size_y);
		}

		/** Erase the contents of all the cells.
		  */
		void  clear()
		{
			m_map.clear();
			m_map.resize(m_size_x*m_size_y);
		}

		/** Fills all the cells with the same value
		  */
		void  fill( const T& value )
		{
			for (typename std::vector<T>::iterator	it = m_map.begin();it!=m_map.end();it++)
				*it = value;
		}

		/** Changes the size of the grid, maintaining previous contents.
		  * \sa setSize
		  */
		virtual void  resize(
			float	new_x_min,
			float	new_x_max,
			float	new_y_min,
			float	new_y_max,
			const T& defaultValueNewCells,
			float	additionalMarginMeters = 2.0f )
		{
			MRPT_TRY_START;

			unsigned int				x,y;
			unsigned int				extra_x_izq,extra_y_arr,new_size_x=0,new_size_y=0;
			typename std::vector<T>				new_map;
			typename std::vector<T>::iterator	itSrc,itDst;

			// Is resize really necesary?
			if (new_x_min>=m_x_min &&
				new_y_min>=m_y_min &&
				new_x_max<=m_x_max &&
				new_y_max<=m_y_max)	return;

			if (new_x_min>m_x_min) new_x_min=m_x_min;
			if (new_x_max<m_x_max) new_x_max=m_x_max;
			if (new_y_min>m_y_min) new_y_min=m_y_min;
			if (new_y_max<m_y_max) new_y_max=m_y_max;

			// Añadir un margen mas:
			if (additionalMarginMeters>0)
			{
				if (new_x_min<m_x_min) new_x_min= floor(new_x_min-additionalMarginMeters);
				if (new_x_max>m_x_max) new_x_max= ceil(new_x_max+additionalMarginMeters);
				if (new_y_min<m_y_min) new_y_min= floor(new_y_min-additionalMarginMeters);
				if (new_y_max>m_y_max) new_y_max= ceil(new_y_max+additionalMarginMeters);
			}

			// Adjust sizes to adapt them to full sized cells acording to the resolution:
			if (fabs(new_x_min/m_resolution - round(new_x_min/m_resolution))>0.05f )
				new_x_min = m_resolution*round(new_x_min/m_resolution);
			if (fabs(new_y_min/m_resolution - round(new_y_min/m_resolution))>0.05f )
				new_y_min = m_resolution*round(new_y_min/m_resolution);
			if (fabs(new_x_max/m_resolution - round(new_x_max/m_resolution))>0.05f )
				new_x_max = m_resolution*round(new_x_max/m_resolution);
			if (fabs(new_y_max/m_resolution - round(new_y_max/m_resolution))>0.05f )
				new_y_max = m_resolution*round(new_y_max/m_resolution);

			// Change the map size: Extensions at each side:
			extra_x_izq = round((m_x_min-new_x_min) / m_resolution);
			extra_y_arr = round((m_y_min-new_y_min) / m_resolution);

			new_size_x = round((new_x_max-new_x_min) / m_resolution);
			new_size_y = round((new_y_max-new_y_min) / m_resolution);

			// Reserve new memory:
			new_map.resize(new_size_x*new_size_y,defaultValueNewCells);

			// Copy previous rows:
			for (y=0;y<m_size_y;y++)
			{
				for (x=0,itSrc=(m_map.begin()+y*m_size_x),itDst=(new_map.begin()+extra_x_izq + (y+extra_y_arr)*new_size_x);
						x<m_size_x;
					x++,itSrc++,itDst++)
				{
					*itDst = *itSrc;
				}
			}

			// Update the new map limits:
			m_x_min = new_x_min;
			m_x_max = new_x_max;
			m_y_min = new_y_min;
			m_y_max = new_y_max;

			m_size_x = new_size_x;
			m_size_y = new_size_y;

			// Keep the new map only:
			m_map = new_map;
			new_map.clear();

			MRPT_TRY_END;

		}

		/** Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map extensions.
		  */
		T*		cellByPos( float x, float y )
		{
			int cx = x2idx(x);
			int cy = y2idx(y);

			if( cx<0 || cx>=static_cast<int>(m_size_x) ) return NULL;
			if( cy<0 || cy>=static_cast<int>(m_size_y) ) return NULL;

			return &m_map[ cx + cy*m_size_x ];
		}

		/** Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map extensions.
		  */
		const T*		cellByPos( float x, float y ) const
		{
			int cx = x2idx(x);
			int cy = y2idx(y);

			if( cx<0 || cx>=static_cast<int>(m_size_x) ) return NULL;
			if( cy<0 || cy>=static_cast<int>(m_size_y) ) return NULL;

			return &m_map[ cx + cy*m_size_x ];
		}

		/** Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the map extensions.
		  */
		T*		cellByIndex( unsigned int cx, unsigned int cy )
		{
			if( cx>=m_size_x || cy>=m_size_y)
					return NULL;
			else	return &m_map[ cx + cy*m_size_x ];
		}

		/** Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the map extensions.
		  */
		const T*		cellByIndex( unsigned int cx, unsigned int cy ) const
		{
			if( cx>=m_size_x || cy>=m_size_y)
					return NULL;
			else	return &m_map[ cx + cy*m_size_x ];
		}

		/** Returns the horizontal size of grid map in cells count.
			*/
		unsigned int   getSizeX() { return m_size_x; }

		/** Returns the vertical size of grid map in cells count.
			*/
		unsigned int   getSizeY() { return m_size_y; }

		/** Returns the "x" coordinate of left side of grid map.
			*/
		float  getXMin()const  { return m_x_min; }

		/** Returns the "x" coordinate of right side of grid map.
			*/
		float  getXMax()const  { return m_x_max; }

		/** Returns the "y" coordinate of top side of grid map.
			*/
		float  getYMin()const  { return m_y_min; }

		/** Returns the "y" coordinate of bottom side of grid map.
			*/
		float  getYMax()const  { return m_y_max; }

		/** Returns the resolution of the grid map.
			*/
		float  getResolution()const  { return m_resolution; }

		void  saveToTextFile(const char *fileName)
		{
			FILE	*f=MRPT_OS::fopen(fileName,"wt");
			if(!f) return;

			for (unsigned int cy=0;cy<m_size_y;cy++)
			{
				for (unsigned int cx=0;cx<m_size_x;cx++)
					MRPT_OS::fprintf(f,"%f ",static_cast<float>(m_map[ cx + cy*m_size_x ]));

				MRPT_OS::fprintf(f,"\n");
			}


			MRPT_OS::fclose(f);
		}

		/** Transform a coordinate values into cell indexes.
		  */
		inline int   x2idx(float x) const { return static_cast<int>( (x-m_x_min)/m_resolution ); }
		inline int   y2idx(float y) const { return static_cast<int>( (y-m_y_min)/m_resolution ); }
		inline int   xy2idx(float x,float y) const { return x2idx(x) + y2idx(y)*m_size_x; }

		/** Transform a cell index into a coordinate value.
		  */
		inline float   idx2x(int cx) const { return m_x_min+(cx+0.5f)*m_resolution; }
		inline float   idx2y(int cy) const { return m_y_min+(cy+0.5f)*m_resolution; }

		/** Transform a coordinate value into a cell index, using a diferent "x_min" value
			*/
		inline int   x2idx(float x,float x_min) const { return static_cast<int>((x-m_x_min)/m_resolution ); }
		inline int   y2idx(float y, float y_min) const { return static_cast<int>((y-m_y_min)/m_resolution ); }

	};

} // End of namespace

#endif
