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

#include <MRPT/UTILS/CDynamicGrid.h>

namespace MRPTAPPS
{
  namespace RNAV
  {

	/** This is the base class for any user defined PTG.
	 *
	 * Changes history:
	 *		- 30/JUN/2004: Creation (JLBC)
     *		- 16/SEP/2004: Totally redesigned.
	 *		- 15/SEP/2005: Totally rewritten again, for integration into MRPT Applications Repository.
	 */
	class  CParameterizedTrajectoryGenerator
	{
	public:
        /** Constructor
		 */
        CParameterizedTrajectoryGenerator(
					float	refDistance,
					float	xResolution,
					float	yResolution,
					float	V_MAX,
					float	W_MAX,
					float	system_TAU,
					float	system_DELAY,
					vector_float	securityDistances);


		/** Gets a short textual description of the PTG and its parameters.
		  */
		virtual std::string getDescription() = 0;

		/** Initialized the grid information.
		 *  The "securityDistances" vector length will set the number of collision grids.
		 */
        void initializeCollisionsGrid(float refDistance, float xResolution, float yResolution, vector_float	securityDistances);

        /** Destructor
		 */
        virtual ~CParameterizedTrajectoryGenerator();

        /** The main method: solves the diferential equation to generate a family of parametrical trajectories.
		 */
        void simulateTrajectories(
				unsigned short	alfaValuesCount,
				float			max_time,
				float			max_dist,
				unsigned int	max_n,
				float			diferencial_t,
				float			min_dist,
				float			*out_max_acc_v = NULL,
				float			*out_max_acc_w = NULL);

        /** The "lambda" function, see paper for info. It takes the (a,d) pair that is closest to a given location.
		 */
        virtual void lambdaFunction( float x, float y, int &out_k, float &out_d );

        /** Converts an "alfa" value (into the discrete set) into a feasible motion command.
		 */
        void directionToMotionCommand( unsigned short k, float &out_v, float &out_w );

        size_t getAlfaValuesCount() { return alfaValuesCount; };
        size_t getPointsCountInCPath_k(unsigned short k) { return CPoints[k].size(); };

        void   getCPointWhen_d_Is ( float d, unsigned short k, float &x, float &y, float &phi, float &t, float *v = NULL, float *w = NULL );

        float  GetCPathPoint_x( unsigned short k, int n ){ return CPoints[k][n].x; }
        float  GetCPathPoint_y( unsigned short k, int n ){ return CPoints[k][n].y; }
        float  GetCPathPoint_phi(unsigned short k, int n ){ return CPoints[k][n].phi; }
        float  GetCPathPoint_t( unsigned short k, int n ){ return CPoints[k][n].t; }
        float  GetCPathPoint_d( unsigned short k, int n ){ return CPoints[k][n].dist; }
        float  GetCPathPoint_v( unsigned short k, int n ){ return CPoints[k][n].v; }
        float  GetCPathPoint_w( unsigned short k, int n ){ return CPoints[k][n].w; }

        void    allocMemForVerticesData( int nVertices );

        void    setVertex_xy( unsigned short k, int n, int m, float x, float y )
        {
			vertexPoints_x[k][ n*nVertices + m ] = x;
            vertexPoints_y[k][ n*nVertices + m ] = y;
        }

        float  getVertex_x( unsigned short k, int n, int m )
        {
                int idx = n*nVertices + m;
//                assert( idx>=0);assert(idx<nVertices * nPointsInEachPath[k] );
				return vertexPoints_x[k][idx];
        }

        float  getVertex_y( unsigned short k, int n, int m )
        {
                int idx = n*nVertices + m;
//                assert( idx>=0);assert(idx<nVertices * nPointsInEachPath[k] );
				return vertexPoints_y[k][idx];
        }

		float*  getVertixesArray_x( unsigned short k, int n )
		{
                int idx = n*nVertices;
				return &vertexPoints_x[k][idx];
		}

		float*  getVertixesArray_y( unsigned short k, int n )
		{
                int idx = n*nVertices;
				return &vertexPoints_y[k][idx];
		}

		unsigned int getVertixesCount() { return nVertices; }

        float   getMax_V() { return V_MAX; }
        float   getMax_W() { return W_MAX; }
        float   getMax_V_inTPSpace() { return maxV_inTPSpace; }

		/** Alfa value for the discrete corresponding value.
		 * \sa alfa2index
		 */
		float  index2alfa( unsigned short k )
		{
			return (float)(M_PI * (-1 + 2 * (k+0.5f) / ((float)alfaValuesCount) ));
		}

		/** Discrete index value for the corresponding alfa value.
		 * \sa index2alfa
		 */
		unsigned short  alfa2index( float alfa )
		{
			if (alfa>M_PI)  alfa-=(float)M_2PI;
			if (alfa<-M_PI) alfa+=(float)M_2PI;
			return (unsigned short)(0.5f*(alfaValuesCount*(1+alfa/M_PI) - 1));
		}

		/** Dump PTG trajectories in files in directory "./PTGs/".
		 */
        void    debugDumpInFiles(int nPT);

        /** An internal class for storing the collision grid
		  */
        class CColisionGrid
        {
          public:
                CColisionGrid( float rangoXmin, float rangoXmax, float rangoYmin, float rangoYmax, float resolucionX, float resolucionY, float securityDistance );
                ~CColisionGrid();

                // Save/Load from files.
                // ----------------------------------------------------------
                bool    saveToFile( FILE* fil );	// true = OK
                bool    loadFromFile( FILE* fil );	// true = OK

                // Retrieving basic data
                // ----------------------------------------------------------
                float   getXmin() { return rangoXmin; };
                float   getXmax() { return rangoXmax; };
                float   getYmin() { return rangoYmin; };
                float   getYmax() { return rangoYmax; };
				int     getCellsX()   { return nCeldasX;  }
				int     getCellsY()   { return nCeldasY;  }
                int     getCellsTotal() { return nCeldasX*nCeldasY; };
                int     getCellIndex( int idx_x, int idx_y ) {   return idx_y * nCeldasX + idx_x;  }
				float	getSecurityDistance()	{ return securityDistance; }

                // ----------------------------------------------------------------------
                //		Access to collision info methods
                // ----------------------------------------------------------------------
                // Para un obstaculo (x,y), devuelve un array con todos los
                //   pares (a,d) en las que el robot choca con el. Si
                //   no choca, NULL.
                void    getTPObstacle( float obsX, float obsY,size_t &nRegistros, uint16_t ** distancias, uint16_t **ks );

                void    getCellCoords( int cellIndex, float &x1, float &y1, float &x2, float &y2 );

                /** Updates the info into a cell: It updates the cell only if the distance d for the path k is lower than the previous value:
				  *	\param cellInfo The index of the cell
				  * \param k The path index (alfa discreet value)
				  * \param d The distance (in TP-Space, range 0..1) to collision.
				  *
				 */
                void    updateCellInfo( int cellIndex, unsigned short k, float dist );

                /** Coordinates to index calculation. Return -1 if out of grid.
				  */
                int     Coord_a_indice( float x, float y);

                /** Coordinates to indexes calculation.
				  */
                void    Coord_a_indice( float x, float y, int &idx_x, int &idx_y);

                /** The cell structure:
				 */
                struct TCell
                {
                        int     nRegistros;             // Los que hay
                        int     nRegistrosMax;          // Los que caben sin volver a "realloc"

                        // Para cada registro: El "k" sector choca en la distancia dada:
                        uint16_t    *distanciaCol;   // en unidades de milimetros
                        uint16_t    *kColision;
                };

          private:
                int     nCeldasX, nCeldasY;                             // Numero de celdas en cada eje
                float   rangoXmin, rangoXmax, rangoYmin, rangoYmax;     // Distancia abarcada en cada eje
                float   resolucionX, resolucionY;                       // Tamaño de celdas en cada eje
				float	securityDistance;								// In real "meters". Set to "0" for compatibility with previous works.

                TCell  *celdas;                                        // El array con todas las celdas

        };

		/** The collision grids for different security distance values:
		  */
		class CListColisionGrids : public std::vector<CColisionGrid*>
		{
		public:
                // Save/Load from files.
                // ----------------------------------------------------------
                bool    SaveToFile( char* filename );	// true = OK
                bool    LoadFromFile( char* filename ); // true = OK
		};


		CListColisionGrids	collisionGrids;

		float	refDistance;

		/** The main method to be implemented in derived classes.
		 */
        virtual void PTG_Generator( float alfa, float t, float x, float y, float phi, float &v, float &w) = 0;

		/** To be implemented in derived classes:
		  */
		virtual bool PTG_IsIntoDomain( float x, float y ) = 0;

protected:
		// Given a-priori:
        float			V_MAX, W_MAX;
		float			TAU, DELAY;

		float			turningRadiusReference;

		/** Specifies the min/max values for "k" and "n", respectively.
		  * \sa m_lambdaFunctionOptimizer
		  */
		struct TCellForLambdaFunction
		{
			TCellForLambdaFunction()
			{
				k_min=k_max=n_min=n_max=-1;
			}

			int		k_min,k_max,n_min,n_max;
		};

		/** This grid will contain indexes data for speeding-up the default, brute-force lambda function.
		  */
		CDynamicGrid<TCellForLambdaFunction>	m_lambdaFunctionOptimizer;

		// Computed from simulations while generating trajectories:
		float	maxV_inTPSpace;

        bool    flag1, flag2;

		/** The number of discrete values for ALFA between -PI and +PI.
		  */
        unsigned int     alfaValuesCount;

		/** The trajectories in the C-Space:
		  */
        struct TCPoint
        {
			TCPoint(	float	x,
						float	y,
						float	phi,
						float	t,
						float	dist,
						float	v,
						float	w)
			{
				this->x = x;
				this->y = y;
				this->phi = phi;
				this->t = t;
				this->dist = dist;
				this->v = v;
				this->w = w;
			};

			float  x, y, phi,t, dist,v,w;
        };
		typedef std::vector<TCPoint> TCPointVector;
		std::vector<TCPointVector>	CPoints;

		/** The shape of the robot along the trajectories:
		  */
		std::vector<vector_float> vertexPoints_x,vertexPoints_y;
        int     nVertices;

		/** Free all the memory buffers:
		  */
        void    FreeMemory();

	}; // end of class
  }
}


#endif

