/* +---------------------------------------------------------------------------+
   |          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/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */

#if defined(_MSC_VER)
	#pragma warning(disable:4267)
#endif

#include "CParameterizedTrajectoryGenerator.h"

#include <MRPT/UTILS/SystemUtils.h>
#include <MRPT/UTILS/GEO.h>

using namespace UTILS;
using namespace MRPTAPPS::RNAV;
using namespace std;

/*---------------------------------------------------------------
					Constructor
  ---------------------------------------------------------------*/
CParameterizedTrajectoryGenerator::CParameterizedTrajectoryGenerator(
		float	refDistance,
		float	xResolution,
		float	yResolution,
		float	V_MAX,
		float	W_MAX,
		float	system_TAU,
		float	system_DELAY,
		vector_float	securityDistances)
{
	this->refDistance	= refDistance;
	this->V_MAX			= V_MAX;
	this->W_MAX			= W_MAX;
	this->TAU			= system_TAU;
	this->DELAY			= system_DELAY;

    alfaValuesCount=0;
    nVertices = 0;
	turningRadiusReference = 0.10f;

	// Buffers:
	CPoints.clear();
	vertexPoints_x.clear();
	vertexPoints_y.clear();

	initializeCollisionsGrid( refDistance, xResolution, yResolution,securityDistances );
}

/*---------------------------------------------------------------
					initializeCollisionsGrid
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::initializeCollisionsGrid(
			float			refDistance,
			float			xResolution,
			float			yResolution,
			vector_float	securityDistances
			)
{
	std::vector<CColisionGrid*>::iterator	it_grid;
	vector_float::iterator					sec;

	// "securityDistances" sets the number of collision grid:
	collisionGrids.resize(securityDistances.size(),NULL);

	for (sec = securityDistances.begin(),it_grid = collisionGrids.begin();
		 sec!=securityDistances.end();
		 sec++,it_grid++)
	{
		*it_grid = new CColisionGrid(
                -refDistance,
                refDistance,
                -refDistance,
                refDistance,
                xResolution,
                yResolution,
				*sec );
	}
}

/*---------------------------------------------------------------
					Destructor
  ---------------------------------------------------------------*/
CParameterizedTrajectoryGenerator::~CParameterizedTrajectoryGenerator()
{
        // Free other memory buffers:
        FreeMemory();

		// Free the collision grids:
		for (std::vector<CColisionGrid*>::iterator	it=collisionGrids.begin();
			it != collisionGrids.end();
			it++)
		{
			delete *it;
		}
		collisionGrids.clear();
}

/*---------------------------------------------------------------
					FreeMemory
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::FreeMemory()
{
        if (alfaValuesCount)
        {
			// Free trajectories:
			CPoints.clear();

			// And the shape of the robot along them:
			vertexPoints_x.clear();
			vertexPoints_y.clear();

            // Signal an empty PTG:
            alfaValuesCount = 0;
        }
}

/*---------------------------------------------------------------
					allocMemFoVerticesData
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::allocMemForVerticesData( int nVertices )
{
		vertexPoints_x.resize(alfaValuesCount);
		vertexPoints_y.resize(alfaValuesCount);

		// Alloc the exact number of items, all of them set to 0:
        for (unsigned int i=0;i<alfaValuesCount;i++)
		{
			vertexPoints_x[i].resize( nVertices * getPointsCountInCPath_k(i), 0 );
			vertexPoints_y[i].resize( nVertices * getPointsCountInCPath_k(i), 0 );
		}

		// Save it:
        this->nVertices= nVertices;
}

/*---------------------------------------------------------------
					simulateTrajectories
	Solve trajectories and fill cells.
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::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,
        float			*out_max_acc_w)
{
        // Primero, liberar memoria:
        FreeMemory();

        // The number of discreet values for ALFA:
        this->alfaValuesCount = alfaValuesCount;

		// Reserve the size in the buffers:
		CPoints.resize( alfaValuesCount );

        // Calcular maxima distancia del contorno del robot (para 1 calculo auxiliar)
        float  radio_max_robot=1.0;    // Aprox.

        // Buffer auxiliar:
//        TCPoint         *points = (TCPoint*)calloc(max_n, sizeof(TCPoint));
//        int             nPoints;
		TCPointVector	points;

        float          alfa;
        float          x,y,phi,v,w,cmd_v,cmd_w,t, dist, girado;
        float          _x,_y,_phi;     // De la iteracion anteriormente guardada
        float          ult_dist, ult_dist1, ult_dist2;

		// For the grid:
		float		   x_min = 1e3f, x_max = -1e3;
		float		   y_min = 1e3f, y_max = -1e3;

		// Para averiguar las maximas ACELERACIONES lineales y angulares:
		float			max_acc_lin, max_acc_ang;
		float			acc_lin, acc_ang;

        int				k;

		maxV_inTPSpace = 0;
		max_acc_lin = max_acc_ang = 0;

	try
	{
        for (k=0;k<alfaValuesCount;k++)
        {
                // Simular / calcular trayectoria con "alfa":
                // --------------------------------------------------------
                alfa = index2alfa( k );

				points.clear();
                t = dist = girado = 0.0;
                x = y = phi = v = w =
                _x = _y = _phi = 0.0;
                flag1 = flag2 = false;

                // Ventana deslizante de ultimos comandos:
                vector_float    last_cmd_vs, last_cmd_ws;
                vector_float    last_vs, last_ws;
                int             M = 5;

				// cmd_v[i] = cmd_v[k-i]
				last_cmd_vs.clear();last_cmd_ws.clear();
                last_cmd_vs.resize(M,0);
                last_cmd_ws.resize(M,0);

				// cmd_v[i] = cmd_v[k-i]
                last_vs.clear();last_ws.clear();
				last_vs.resize(M,0);
                last_ws.resize(M,0);

                // ________________________________________
                // Parametros del sistema:
                //
                //            (1-alfa)·z(-NDELAY)
                //  H(z) = ------------------------
                //            1 -  z(-1)·alfa
                //
                //    alfa = exp( -1 / d ), d = TAU/T
                // ________________________________________
                int             N_Delay = round(DELAY / diferencial_t);
                if (TAU==0)	       TAU=0.01f;
                double          filter_alfa = exp(-1/(TAU/diferencial_t));

                // Add the first, initial point:
				points.push_back( TCPoint(	x,
											y,
											phi,
											t,
											dist,
											v,
											w
											) );
                // Simular hasta que se cumpla algo de esto:
                while ( t < max_time && dist < max_dist && points.size() < max_n && fabs(girado) < 1.95 * M_PI )
                {
						// Max. aceleraciones:
						if (t>1)
						{
							acc_lin = fabs( (last_vs[0]-last_vs[1])/diferencial_t);
							acc_ang = fabs( (last_ws[0]-last_ws[1])/diferencial_t);

							if (acc_lin>max_acc_lin)
									max_acc_lin = acc_lin;
							if (acc_ang>max_acc_ang)
									max_acc_ang =acc_ang;
						}

                        // Calcular nuevo comando de (v,w):
						PTG_Generator( alfa,t, x, y, phi, cmd_v,cmd_w );

                        if (t==0)
                                maxV_inTPSpace=max(maxV_inTPSpace,(float)( sqrt( square(cmd_v) + square(cmd_w*turningRadiusReference) ) ) );

                        // FILTRAR ----------------------------------
                        for (int i=M-1;i>=1;i--)
                        {
                                last_cmd_vs[i]=last_cmd_vs[i-1];
                                last_cmd_ws[i]=last_cmd_ws[i-1];
                                last_vs[i]=last_vs[i-1];
                                last_ws[i]=last_ws[i-1];
                        }
                        last_vs[0] = v;
                        last_ws[0] = w;
                        last_cmd_vs[0] = cmd_v;
                        last_cmd_ws[0] = cmd_w;

                        // Procesar respuesta del sistema para tener (v,w) "reales":
                        v = (float)(last_cmd_vs[ N_Delay ]*(1-filter_alfa) + filter_alfa*last_vs[1]);
                        w = (float)(last_cmd_ws[ N_Delay ]*(1-filter_alfa) + filter_alfa*last_ws[1]);

                        // -------------------------------------------

                        // Ecuacion en diferencias:
                        x += cos(phi)* v * diferencial_t;
                        y += sin(phi)* v * diferencial_t;
                        phi+= w * diferencial_t;

                        // Contadores:
                        girado += w * diferencial_t;

                        float v_inTPSpace = sqrt( square(v)+square(w*turningRadiusReference) );

                        dist += v_inTPSpace  * diferencial_t;

                        t += diferencial_t;

                        // Si nos hemos movido suficiente, guardar esta muestra:
                        ult_dist1 = sqrt( square( _x - x )+square( _y - y  ) );
                        ult_dist2 = fabs( radio_max_robot* ( _phi - phi ) );
                        ult_dist = max( ult_dist1, ult_dist2 );

                        if (ult_dist > min_dist)
                        {
							// Set the (v,w) to the last record:
							points.back().v = v;
							points.back().w = w;

							// And add the new record:
							points.push_back( TCPoint(	x,
														y,
														phi,
														t,
														dist,
														v,
														w
														) );

							// For the next iter:
                            _x = x;
                            _y = y;
                            _phi = phi;
                        }

					// for the grid:
					x_min = min(x_min,x); x_max = max(x_max,x);
					y_min = min(y_min,y); y_max = max(y_max,y);
                }

				// Add the final point:
				// -------------------------------------
				points.back().v = v;
				points.back().w = w;
				points.push_back( TCPoint(	x,
											y,
											phi,
											t,
											dist,
											v,
											w
											) );

                // Guardar datos en la estructura:
                // --------------------------------------------------------
                CPoints[k] = points;

        }       // for "k"

		// Poner las aceleraciones:
		if (out_max_acc_v) *out_max_acc_v = max_acc_lin;
        if (out_max_acc_w) *out_max_acc_w = max_acc_ang;

		// --------------------------------------------------------
		// Build the speeding-up grid for lambda function:
		// --------------------------------------------------------
		m_lambdaFunctionOptimizer.setSize(
			x_min-0.5f,x_max+0.5f,
			y_min-0.5f,y_max+0.5f,  0.25f);

		TCellForLambdaFunction	defCell;
		defCell.k_max = defCell.k_min = defCell.n_max = defCell.n_min = -1;
		m_lambdaFunctionOptimizer.fill( defCell );

		for (k=0;k<alfaValuesCount;k++)
		{
			for (int n=0;n<(int)CPoints[k].size();n++)
			{
				TCellForLambdaFunction	*cell = m_lambdaFunctionOptimizer.cellByPos(CPoints[k][n].x,CPoints[k][n].y);
				ASSERT_(cell);
				if (cell->k_max==-1)
				{
					// First time: copy
					cell->k_min =
					cell->k_max = k;
					cell->n_min =
					cell->n_max = n;
				}
				else
				{
					// Keep limits:
					cell->k_min = min( cell->k_min, k );
					cell->k_max = max( cell->k_max, k );
					cell->n_min = min( cell->n_min, n );
					cell->n_max = max( cell->n_max, n );
				}
			}
		}
	}
	catch(...)
	{
		std::cout << format("[CParameterizedTrajectoryGenerator::simulateTrajectories] Simulation aborted: unexpected exception!\n");
	}

}

/*---------------------------------------------------------------
					directionToMotionCommand
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::directionToMotionCommand( unsigned short k, float &v, float &w )
{
	PTG_Generator( index2alfa(k),0, 0, 0, 0, v, w );
}

/*---------------------------------------------------------------
					getCPointWhen_d_Is
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::getCPointWhen_d_Is (
		float		d,
		unsigned short k,
		float		&x,
		float		&y,
		float		&phi,
		float		&t,
                float *v,
                float *w)
{
        unsigned int     n=0;

        if (k>=alfaValuesCount)
		{
			x=y=phi=0;
			return;  // Por si acaso
		}

		while ( n < (CPoints[k].size()-1) && CPoints[k][n].dist<d )
                n++;

        x=CPoints[k][n].x;
        y=CPoints[k][n].y;
        phi=CPoints[k][n].phi;
        t=CPoints[k][n].t;
        if (v) *v =CPoints[k][n].v;
        if (w) *w =CPoints[k][n].w;
}

/*---------------------------------------------------------------
						debugDumpInFiles
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::debugDumpInFiles( int nPT )
{
	char str[100];

//#define alsoDumpForMATLAB

	SystemUtils::createDirectory( "./RNAV.logs" );
	SystemUtils::createDirectory( "./RNAV.logs/PTGs" );

	MRPT_OS::sprintf(str,100, "./RNAV.logs/PTGs/PTG%u.dat",nPT);
	FILE* f = MRPT_OS::fopen(str,"wb");

#ifdef alsoDumpForMATLAB
	sprintf(str, "./RNAV.logs/PTGs/PTG%u_x.txt",nPT); FILE* fx = fopen(str,"wt");
	sprintf(str, "./RNAV.logs/PTGs/PTG%u_y.txt",nPT); FILE* fy = fopen(str,"wt");
	sprintf(str, "./RNAV.logs/PTGs/PTG%u_p.txt",nPT); FILE* fp = fopen(str,"wt");
	sprintf(str, "./RNAV.logs/PTGs/PTG%u_t.txt",nPT); FILE* ft = fopen(str,"wt");
	sprintf(str, "./RNAV.logs/PTGs/PTG%u_d.txt",nPT); FILE* fd = fopen(str,"wt");
#endif

	int		nPaths = getAlfaValuesCount();
	//int    maxPoints=0;
	int    k;

#ifdef alsoDumpForMATLAB
	// Version texto:
	for (k=0;k<nPaths;k++)
		maxPoints = max( maxPoints, getPointsCountInCPath_k(k) );

	for ( k=0;k<nPaths;k++)
	{
		for (int n=0;n< maxPoints;n++)
		{
				int nn;

				nn= min( n, getPointsCountInCPath_k(k)-1 );

				fprintf(fx,"%0.02f ", GetCPathPoint_x(k,nn) );
				fprintf(fy,"%0.02f ", GetCPathPoint_y(k,nn) );
				fprintf(fp,"%0.02f ", GetCPathPoint_phi(k,nn) );
				fprintf(ft,"%0.02f ", GetCPathPoint_t(k,nn) );
				fprintf(fd,"%0.02f ", GetCPathPoint_d(k,nn) );
		}
		fprintf(fx,"\n" );fprintf(fy,"\n" );fprintf(fp,"\n" );fprintf(ft,"\n" );fprintf(fd,"\n" );
	}
	fclose(fx);fclose(fy);fclose(fp);fclose(ft);fclose(fd);
#endif

	// Version binaria:
	for ( k=0;k<nPaths;k++)
	{
		int     nPoints = getPointsCountInCPath_k(k);
		float   fl;
		fwrite( &nPoints ,sizeof(int),1 , f );
		for (int n=0;n<nPoints;n++)
		{
				fl = GetCPathPoint_x(k,n); fwrite(&fl,sizeof(float),1,f);
				fl = GetCPathPoint_y(k,n); fwrite(&fl,sizeof(float),1,f);
				fl = GetCPathPoint_phi(k,n); fwrite(&fl,sizeof(float),1,f);
				fl = GetCPathPoint_t(k,n); fwrite(&fl,sizeof(float),1,f);
				fl = GetCPathPoint_d(k,n); fwrite(&fl,sizeof(float),1,f);
		}
	}

	MRPT_OS::fclose(f);
}

/*---------------------------------------------------------------
					Constructor
  ---------------------------------------------------------------*/
CParameterizedTrajectoryGenerator::CColisionGrid::CColisionGrid(
			float		rangoXmin,
			float		rangoXmax,
			float		rangoYmin,
			float		rangoYmax,
			float		resolucionX,
			float		resolucionY,
			float		securityDistance )
{
	// Copiar datos que describen la rejilla:
	this->rangoXmin= rangoXmin;
	this->rangoXmax= rangoXmax;
	this->rangoYmin= rangoYmin;
	this->rangoYmax= rangoYmax;

	// Calcular el numero de celdas necesarias para la resolucion y rangos dados:
	nCeldasX = ((int)ceil( (rangoXmax - rangoXmin) / resolucionX ));
	nCeldasY = ((int)ceil( (rangoXmax - rangoXmin) / resolucionX ));

	// Quedarse con la resolucion real (la pasada es la minima):
	this->resolucionX = (rangoXmax - rangoXmin) / nCeldasX;
	this->resolucionY = (rangoYmax - rangoYmin) / nCeldasY;

	// Reservar memoria dinamica:
	celdas = (TCell*) malloc( nCeldasX*nCeldasY * sizeof(TCell) );

	for (int i=0;i<nCeldasX*nCeldasY;i++)
	{
			celdas[i].distanciaCol = NULL;
			celdas[i].kColision = NULL;
			celdas[i].nRegistros = 0;
			celdas[i].nRegistrosMax = 0;
	}
}

/*---------------------------------------------------------------
  ---------------------------------------------------------------*/
CParameterizedTrajectoryGenerator::CColisionGrid::~CColisionGrid()
{
        // Liberar memoria:
        for (int i=0;i<nCeldasX*nCeldasY;i++)
        {
                if (celdas[i].distanciaCol)        free(celdas[i].distanciaCol);
                if (celdas[i].kColision)                free(celdas[i].kColision);
        }

        free( celdas );
}

/*---------------------------------------------------------------
					getTPObstacle
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::CColisionGrid::getTPObstacle(
		float		obsX,
		float		obsY,
		size_t		&nRegistros ,
		uint16_t**		distancias,
		uint16_t**		ks)
{
        int     idx = Coord_a_indice(obsX, obsY );

        if (idx==-1)
        {       // Fuera:
                *distancias = NULL;
                *ks = NULL;
                nRegistros = 0;
        }
        else
        {       // Direccionar a los datos de la celda correspondiente:
                *distancias = celdas[idx].distanciaCol;
                *ks = celdas[idx].kColision;
                nRegistros = celdas[idx].nRegistros;
        }
}

/*---------------------------------------------------------------
  ---------------------------------------------------------------*/
int CParameterizedTrajectoryGenerator::CColisionGrid::Coord_a_indice( float x, float y)
{
        int     idx_x = (int) floor( (x-rangoXmin) / resolucionX );
        int     idx_y = (int) floor( (y-rangoYmin) / resolucionY );

        // Fuera de la rejilla?
        if (idx_x<0 || idx_x>=nCeldasX || idx_y<0 || idx_y>=nCeldasY )
                return -1;
        else    return idx_y * nCeldasX + idx_x;
}

/*---------------------------------------------------------------
                    CColisionGrid::Coord_a_indice
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::CColisionGrid::Coord_a_indice( float x, float y, int &idx_x, int &idx_y)
{
        idx_x = (int) floor( (x-rangoXmin) / resolucionX );
        idx_y = (int) floor( (y-rangoYmin) / resolucionY );

        if (idx_x<0) idx_x = 0;
        if (idx_y<0) idx_y = 0;
        if (idx_x>=nCeldasX) idx_x = nCeldasX-1;
        if (idx_y>=nCeldasY) idx_y = nCeldasY-1;
}

/*---------------------------------------------------------------
                CColisionGrid::getCellCoords
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::CColisionGrid::getCellCoords( int nCelda, float &x1, float &y1, float &x2, float &y2 )
{
        int     idx_x, idx_y;

//        assert( nCelda>=0 && nCelda<nCeldasX*nCeldasY );

        idx_x = nCelda % nCeldasX;
        idx_y = nCelda / nCeldasX;

        // Cuadrado de la celda:
        x1 = rangoXmin + idx_x * resolucionX;
        y1 = rangoYmin + idx_y * resolucionY;
        x2 = rangoXmin + (1+idx_x) * resolucionX;
        y2 = rangoYmin + (1+idx_y) * resolucionY;
}

/*---------------------------------------------------------------
	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:
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::CColisionGrid::updateCellInfo(
			int				cellIndex,
			unsigned short	k,
			float			dist )
{
        if( cellIndex<0 || cellIndex>=nCeldasX*nCeldasY ) return;

        // If that "k" is already with smaller "dist", do NOT add!
        for (int i=0;i<celdas[cellIndex].nRegistros;i++)
                if ( celdas[cellIndex].kColision[i] == k )
                 if ( celdas[cellIndex].distanciaCol[i]*0.001 > dist )
                 {      // Sustituir por nueva distancia menor
                        celdas[cellIndex].distanciaCol[i] = round(dist*1000);
                        return;
                 }
                 else
                 {
                        // Ya hay otra menor:
                        return;
                 }

        // El "k" es nuevo en esta celda:
        if ( celdas[cellIndex].nRegistros > celdas[cellIndex].nRegistrosMax - 1   )
        {
                int new_size = celdas[cellIndex].nRegistrosMax + 5;
                celdas[cellIndex].nRegistrosMax = new_size;

                // Reallocs:
                celdas[cellIndex].distanciaCol =
                        (uint16_t*) realloc( celdas[cellIndex].distanciaCol,
                                          new_size*sizeof(uint16_t) );

                celdas[cellIndex].kColision =
                        (uint16_t*) realloc( celdas[cellIndex].kColision,
                                          new_size*sizeof(uint16_t) );
        }

        // Add:
        celdas[cellIndex].kColision[ celdas[cellIndex].nRegistros ] = k;
        celdas[cellIndex].distanciaCol[ celdas[cellIndex].nRegistros ] = round( dist*1000 );
        celdas[cellIndex].nRegistros++;

}


/*---------------------------------------------------------------
					Save to file
  ---------------------------------------------------------------*/
bool CParameterizedTrajectoryGenerator::CListColisionGrids::SaveToFile( char* filename )
{
	unsigned int	n;

	FILE	*f=MRPT_OS::fopen(filename,"wb");
	if (!f) return false;

	n = size();
	fwrite(&n,sizeof(n),1,f);

	for (iterator it=begin();it!=end();it++)
		(*it)->saveToFile(f);

	MRPT_OS::fclose(f);
	return true;
}

/*---------------------------------------------------------------
					Load from file
  ---------------------------------------------------------------*/
bool CParameterizedTrajectoryGenerator::CListColisionGrids::LoadFromFile( char* filename )
{
	unsigned int	n;

	FILE	*f=MRPT_OS::fopen(filename,"rb");
	if (!f) return false;

	fread(&n,sizeof(n),1,f);
	if (n!=size())
	{
		std::cout << format("[CListColisionGrids::LoadFromFile] WARNING: n!=size() --> return false;\n");
		fclose(f);
		return false;
	}

	for (iterator it=begin();it!=end();it++)
		(*it)->loadFromFile(f);

	MRPT_OS::fclose(f);
	return true;
}

/*---------------------------------------------------------------
					Save to file
  ---------------------------------------------------------------*/
bool CParameterizedTrajectoryGenerator::CColisionGrid::saveToFile( FILE *f )
{
		if (!f) return false;

        // Datos descriptivos de la rejilla:
        fwrite( &rangoXmin, sizeof(float),1,f );
        fwrite( &rangoXmax, sizeof(float),1,f );
        fwrite( &rangoYmin, sizeof(float),1,f );
        fwrite( &rangoYmax, sizeof(float),1,f );
        fwrite( &nCeldasX, sizeof(int),1,f );
        fwrite( &nCeldasY, sizeof(int),1,f );

        // Ahora todos los datos de las celdas:
        for (int i=0;i<nCeldasY*nCeldasX;i++)
        {
                fwrite( &celdas[i].nRegistros,sizeof(int),1,f);

                fwrite( celdas[i].distanciaCol, sizeof(unsigned short),celdas[i].nRegistros,f);
                fwrite( celdas[i].kColision, sizeof(unsigned short),celdas[i].nRegistros,f);
        }

		return true;
}

/*---------------------------------------------------------------
  ---------------------------------------------------------------*/
bool CParameterizedTrajectoryGenerator::CColisionGrid::loadFromFile( FILE *f )
{
        if (!f) return false;

        float ff;
        int ii;

        // Datos descriptivos de la rejilla:
        fread( &ff, sizeof(float),1,f );  if (ff!=rangoXmin) {fclose(f);return false;}
        fread( &ff, sizeof(float),1,f );  if (ff!=rangoXmax) {fclose(f);return false;}
        fread( &ff, sizeof(float),1,f );  if (ff!=rangoYmin) {fclose(f);return false;}
        fread( &ff, sizeof(float),1,f );  if (ff!=rangoYmax) {fclose(f);return false;}
        fread( &ii, sizeof(int),1,f );  if (ii!=nCeldasX) {fclose(f);return false;}
        fread( &ii, sizeof(int),1,f );  if (ii!=nCeldasY) {fclose(f);return false;}


        // Ahora todos los datos de las celdas:
        for (int i=0;i<nCeldasY*nCeldasX;i++)
        {
                int     n;
                fread( &n,sizeof(int),1,f);

                celdas[i].nRegistros =
                celdas[i].nRegistrosMax = n;

                celdas[i].distanciaCol =
                        (uint16_t*) realloc( celdas[i].distanciaCol,
                                          n * sizeof(uint16_t) );

                celdas[i].kColision =
                        (uint16_t*) realloc( celdas[i].kColision,
                                          n * sizeof(uint16_t) );


                fread( celdas[i].distanciaCol, sizeof(unsigned short),celdas[i].nRegistros,f);
                fread( celdas[i].kColision, sizeof(unsigned short),celdas[i].nRegistros,f);
        }

		return true;
}


/*---------------------------------------------------------------
                lambdaFunction
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::lambdaFunction( float x, float y, int &k_out, float &d_out )
{
	// Esta en la zona donde las trayectorias son curvas:
	//   comparar con simulaciones
	int     selected_k = -1;
	float	selected_d= 0;
	float   selected_dist;
	int		n,k;
	float   dist_a_punto;

	// -------------------------------------------------------------------
	// Optimization: (24-JAN-2007 @ Jose Luis Blanco):
	//  Use a "grid" to determine the range of [k,d] values to check!!
	//  If the point (x,y) is not found in the grid, then directly skip
	//  to the next step.
	// -------------------------------------------------------------------
	int		k_min = 0;
	int		k_max = alfaValuesCount-1;
	int		n_min = 0;
	int		n_max = 0, n_max_this;

	// Cell indexes:
	int		cx0 = m_lambdaFunctionOptimizer.x2idx(x);
	int		cy0 = m_lambdaFunctionOptimizer.y2idx(y);

	// (cx,cy)
	bool	firstCell = true;
	for (int cx=cx0-1;cx<=cx0+1;cx++)
	{
		for (int cy=cy0-1;cy<=cy0+1;cy++)
		{
			TCellForLambdaFunction	*cell = m_lambdaFunctionOptimizer.cellByIndex(cx,cy);
			if (cell)
			{
				if (cell->k_max!=-1)
				{
					if (firstCell)
					{
						k_min = cell->k_min;	k_max = cell->k_max;
						n_min = cell->n_min;	n_max = cell->n_max;
						firstCell = false;
					}
					else
					{
						k_min = min(cell->k_min,k_min);	k_max = max(cell->k_max,k_max);
						n_min = min(cell->n_min,n_min);	n_max = max(cell->n_max,n_max);
					}
				}
			}
		}
	}

	// Try to find a closest point to the paths:
	// ----------------------------------------------
	selected_dist = square( 0.20f );
	//for ( k=0;k<alfaValuesCount;k++)
	for (k=k_min;k<=k_max;k++)
	{
		n_max_this = min( (int)CPoints[k].size()-1, n_min);

		for (n = n_min;n<=n_max_this; n++)
		{
			if ((dist_a_punto= square( CPoints[k][n].x - x ) + square( CPoints[k][n].y - y ))<selected_dist)
			{
				selected_dist = dist_a_punto;
				selected_k = k;
				selected_d = CPoints[k][n].dist;
			}
		}
	}

	if (selected_k!=-1)
	{
		k_out = selected_k;
		d_out = selected_d / refDistance;
		return;
	}

	// If not found, compute an extrapolation!
	// ----------------------------------------------

	// ------------------------------------------------------------------------------------
	// Given a point (x,y), compute the "k_closest" whose extrapolation
	//  is closest to the point, and the associated "d_closest" distance,
	//  which can be normalized by "1/refDistance" to get TP-Space distances.
	// ------------------------------------------------------------------------------------
	selected_dist = 1e6;
	for ( k=0;k<(int)alfaValuesCount;k++)
	{
		n = CPoints[k].size()-1;
		if ((dist_a_punto= square( CPoints[k][n].x - x ) + square( CPoints[k][n].y - y ))<selected_dist)
		{
			selected_dist = dist_a_punto;
			selected_k = k;
			selected_d = CPoints[k][n].dist + sqrt(dist_a_punto);
		}
	}

	k_out = selected_k;
	d_out = selected_d / refDistance;
}

