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

// PTGs:
#include "CPTG1.h"
#include "CPTG2.h"
#include "CPTG3.h"
#include "CPTG4.h"
#include "CPTG5.h"
#include "CPTG6.h"
#include "CPTG7.h"

// Holonomic methods:
#include "CHolonomicND.h"
#include "CHolonomicVFF.h"

#include <MRPT/UTILS/utils.h>

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

#define		PREVIOUS_VALUES_IN_LOG		50

/*---------------------------------------------------------------
					Constructor
  ---------------------------------------------------------------*/
CReactiveNavigationSystem::CReactiveNavigationSystem(
    TRobotMotionControl		&rmc,
    TSensors				&sensors,
    void	(*emul_printf)(const char *s),
    TEventsLaunching		&evnts,
    const UTILS::CConfigFileBase &configIni,
    const UTILS::CConfigFileBase &configRobotIni,
    bool					enableConsoleOutput,
    bool					enableLogToFile)
		: CAbstractReactiveNavigationSystem(rmc,sensors,emul_printf,evnts)
{
	// For use within the user application of classes streaming capabilities.
	MRML::registerAllClasses();

	// Initialize some members:
	logFile				= NULL;
	holonomicMethod		= NULL;
	enableConsoleOutput = enableConsoleOutput;
	CerrandoHilo		= false;
	nIteration			= 0;
	meanExecutionPeriod	= 0.1f;
	last_cmd_v			= 0;
	last_cmd_w			= 0;

	PTGs.resize(0);
	enableLogFile( enableLogToFile );

	// Reset behavior:
	navigatorBehavior = beNormalNavigation;

	// Load CONFIG file:
	loadConfigFile(configIni,configRobotIni );
}

/*---------------------------------------------------------------
						initialize
  ---------------------------------------------------------------*/
void CReactiveNavigationSystem::initialize()
{
	// Compute collision grids:
	STEP1_CollisionGridsBuilder();
}


/*---------------------------------------------------------------
						changeRobotShape
  ---------------------------------------------------------------*/
void CReactiveNavigationSystem::changeRobotShape( UTILS::CPolygon &shape )
{
	collisionGridsMustBeUpdated = true;

	if ( shape.verticesCount()<3 )
		THROW_EXCEPTION("The robot shape has less than 3 vertices!!")

	robotShape = shape;
}

/*---------------------------------------------------------------
						getLastLogRecord
	Provides a copy of the last log record with information
		about execution.
  ---------------------------------------------------------------*/
void CReactiveNavigationSystem::getLastLogRecord( CLogFileRecord &o )
{
	// Lock
	critZoneLastLog.enter();


	try
	{
		o = lastLogRecord;
	}
	catch (...)
	{
	}

	// Unlock
	critZoneLastLog.leave();
}


/*---------------------------------------------------------------
						loadConfigFile
  ---------------------------------------------------------------*/
void CReactiveNavigationSystem::loadConfigFile(const UTILS::CConfigFileBase &ini, const UTILS::CConfigFileBase &robotIni )
{
	MRPT_TRY_START;
	unsigned int	i,nSecDists;

	collisionGridsMustBeUpdated = true;

	// Load config from INI file:
	// ------------------------------------------------------------
	robotName = robotIni.read_string("ROBOT_NAME","Name", "", true );

	unsigned int PTG_COUNT = ini.read_int(robotName,"PTG_COUNT",0, true );

//	nSecDists = GetPrivateProfileInt(robotName,"SECURITY_DISTANCES",1,INI_FILE_NAME);
	nSecDists = 1;

	securityDistances.resize( nSecDists );
	TextoDebug("\nLoading %u security distances: ", (int)nSecDists );
	for (i=0;i<nSecDists;i++)
	{
		char	key[100];
		MRPT_OS::sprintf(key,100,"SECURITY_DISTANCE_%u",i);
		securityDistances[i] = ini.read_float(robotName,key,0);

		TextoDebug("%f ", securityDistances[i] );
	}
	TextoDebug("\n");


	refDistance = ini.read_float(robotName,"MAX_REFERENCE_DISTANCE",5 );
	colGridRes_x = ini.read_float(robotName,"RESOLUCION_REJILLA_X",0.02f );
	colGridRes_y = ini.read_float(robotName,"RESOLUCION_REJILLA_Y",0.02f);

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(robotMax_V_mps,float,  ini,robotName);
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(robotMax_W_degps,float,  ini,robotName);
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(ROBOTMODEL_TAU,float,  ini,robotName);
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(ROBOTMODEL_DELAY,float,  ini,robotName);


	ini.read_vector( robotName, "weights", vector_float(0), weights, true );
	ASSERT_(weights.size()==6);

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(minObstaclesHeight,float,  ini,robotName);
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(maxObstaclesHeight,float,  ini,robotName);

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(DIST_TO_TARGET_FOR_SENDING_EVENT,float,  ini,robotName);


	int HoloMethod = ini.read_int("GLOBAL_CONFIG","HOLONOMIC_METHOD",1, true);

	setHolonomicMethod( (THolonomicMethod) HoloMethod );

	holonomicMethod->initialize( ini );


	badNavAlarm_AlarmTimeout = ini.read_float("GLOBAL_CONFIG","ALARM_SEEMS_NOT_APPROACHING_TARGET_TIMEOUT", 10, true);

	// -----
	DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2 = ini.read_float("DOOR_CROSSING","DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2", 12.0f );
	VORONOI_MINIMUM_CLEARANCE		   	   = ini.read_float("DOOR_CROSSING","VORONOI_MINIMUM_CLEARANCE", 0.2f );
	DISABLE_PERIOD_AFTER_FAIL		   	   = ini.read_float("DOOR_CROSSING","DISABLE_PERIOD_AFTER_FAIL",3.0f );
	VORONOI_PATH_DIST_FROM_DOORWAY		   = ini.read_float("DOOR_CROSSING","VORONOI_PATH_DIST_FROM_DOORWAY",1.0f );
	DOORCROSSING_HEADING_ACCURACY_DEG	   = ini.read_float("DOOR_CROSSING","DOORCROSSING_HEADING_ACCURACY_DEG",10.0f );
	DOORCROSSING_ROTATION_CTE_DEG		   = ini.read_float("DOOR_CROSSING","DOORCROSSING_ROTATION_CTE_DEG", 50.0f );
	DOOR_CROSSING_DIST_TO_AUX_TARGETS	   = ini.read_float("DOOR_CROSSING","DOOR_CROSSING_DIST_TO_AUX_TARGETS", 0.15f );
	DOOR_CROOSING_BEH3_TIMEOUT			   = ini.read_float("DOOR_CROSSING","DOOR_CROOSING_BEH3_TIMEOUT", 4.0f );
	DOOR_CROSSING_MAXIMUM_DOORWAY_SIZE	   = ini.read_float("DOOR_CROSSING","DOOR_CROSSING_MAXIMUM_DOORWAY_SIZE", 1.0f );

	// Load robot shape:
	// ---------------------------------------------
	UTILS::CPolygon		shape;
	vector_float        xs,ys;

	ini.read_vector(robotName,"RobotModel_shape2D_xs",vector_float(0), xs, true );
	ini.read_vector(robotName,"RobotModel_shape2D_ys",vector_float(0), ys, true );
	ASSERT_(xs.size()==ys.size());

	// Add to polygon
	for (i=0;i<xs.size();i++)
		shape.AddVertex(xs[i],ys[i]);

	changeRobotShape( shape );

	// Load PTGs from file:
	// ---------------------------------------------
	// Free previous PTGs:
	for (i=0;i<PTGs.size();i++)	delete PTGs[i];

	PTGs.resize(PTG_COUNT);

	TextoDebug("\n");

	for ( unsigned int n=0;n<PTG_COUNT;n++ )
	{
		char    key[100];
		// Datos a leer del fichero INI para esta GPT:
		int     GPT_Type;
		int     nAlfas;
		float   v_max_mps, w_max_gps;
		float   cte_a0v,cte_a0w,K;

		// Leer de fichero:
		MRPT_OS::sprintf(key,100, "PTG%u_Type", n );
		GPT_Type = ini.read_int(robotName,key,1, true );

		MRPT_OS::sprintf(key,100, "PTG%u_nAlfas", n );
		nAlfas = ini.read_int(robotName,key,100, true );

		MRPT_OS::sprintf(key,100, "PTG%u_v_max_mps", n );
		v_max_mps = ini.read_float(robotName,key, 5, true);

		MRPT_OS::sprintf(key,100, "PTG%u_w_max_gps", n );
		w_max_gps= ini.read_float(robotName,key, 0, true);

		MRPT_OS::sprintf(key,100, "PTG%u_cte_a0v_deg", n );
		cte_a0v= DEG2RAD( ini.read_float(robotName,key, 0, false) );

		MRPT_OS::sprintf(key,100, "PTG%u_cte_a0w_deg", n );
		cte_a0w= DEG2RAD( ini.read_float(robotName,key, 0, false) );

		MRPT_OS::sprintf(key,100, "PTG%u_K", n );
		K= ini.read_float(robotName,key, 1, false);

		// Generar:
		TextoDebug("[loadConfigFile] Generating PTG#%i...",(int)n);
		switch ( GPT_Type )
		{
		case 1:
			PTGs[n] = new CPTG1(	refDistance,
			                     colGridRes_x,
			                     colGridRes_y,
			                     v_max_mps,
			                     (float)DEG2RAD( w_max_gps ),
			                     ROBOTMODEL_TAU,
			                     ROBOTMODEL_DELAY,
			                     securityDistances,
			                     K
			                   );
			break;
		case 2:
			PTGs[n] = new CPTG2(	refDistance,
			                     colGridRes_x,
			                     colGridRes_y,
			                     v_max_mps,
			                     (float)DEG2RAD( w_max_gps ),
			                     ROBOTMODEL_TAU,
			                     ROBOTMODEL_DELAY,
			                     securityDistances,
			                     cte_a0v,
			                     cte_a0w );
			break;
		case 3:
			PTGs[n] = new CPTG3(	refDistance,
			                     colGridRes_x,
			                     colGridRes_y,
			                     v_max_mps,
			                     (float)DEG2RAD( w_max_gps ),
			                     ROBOTMODEL_TAU,
			                     ROBOTMODEL_DELAY,
			                     securityDistances,
			                     K );

			break;
		case 4:
			PTGs[n] = new CPTG4(	refDistance,
			                     colGridRes_x,
			                     colGridRes_y,
			                     v_max_mps,
			                     (float)DEG2RAD( w_max_gps ),
			                     ROBOTMODEL_TAU,
			                     ROBOTMODEL_DELAY,
			                     securityDistances,
			                     K );

			break;
		case 5:
			PTGs[n] = new CPTG5(	refDistance,
			                     colGridRes_x,
			                     colGridRes_y,
			                     v_max_mps,
			                     (float)DEG2RAD( w_max_gps ),
			                     ROBOTMODEL_TAU,
			                     ROBOTMODEL_DELAY,
			                     securityDistances,
			                     K );

			break;
		case 6:
			PTGs[n] = new CPTG6(	refDistance,
			                     colGridRes_x,
			                     colGridRes_y,
			                     v_max_mps,
			                     (float)DEG2RAD( w_max_gps ),
			                     ROBOTMODEL_TAU,
			                     ROBOTMODEL_DELAY,
			                     securityDistances,
			                     cte_a0v,
			                     cte_a0w );

			break;
		case 7:
			PTGs[n] = new CPTG7(	refDistance,
			                     colGridRes_x,
			                     colGridRes_y,
			                     v_max_mps,
			                     (float)DEG2RAD( w_max_gps ),
			                     ROBOTMODEL_TAU,
			                     ROBOTMODEL_DELAY,
			                     securityDistances,
			                     cte_a0v,
			                     cte_a0w );

			break;
		}
		TextoDebug(PTGs[n]->getDescription().c_str());
		PTGs[n]->simulateTrajectories(
		    nAlfas,					// alfas,
		    75,						// max.tim,
		    refDistance,			// max.dist,
		    600,					// max.n,
		    0.010f,					// diferencial_t
		    0.015f					// min_dist
		);

		// Solo para depurar, hacer graficas, etc...
		PTGs[n]->debugDumpInFiles(n);

		TextoDebug("...OK!\n");
	}
	TextoDebug("\n");

	// Mostrar configuracion cargada de fichero:
	// --------------------------------------------------------
	TextoDebug("\tLOADED CONFIGURATION:\n");
	TextoDebug("-------------------------------------------------------------\n");
	TextoDebug("  Holonomic method \t\t= ");
	switch (  HoloMethod )
	{
	case hmVIRTUAL_FORCE_FIELDS:
		TextoDebug("VFF (Virtual Force Fields)");
		break;
	case hmSEARCH_FOR_BEST_GAP:
		TextoDebug("ND (Nearness Diagram)");
		break;
	default:
		TextoDebug("Unknown!! (Selecting default one)");
		break;
	};

	TextoDebug("\n");
	TextoDebug("  Robot name \t\t\t= ");
	TextoDebug(robotName.c_str());
	TextoDebug("\n  GPT Count\t\t\t= %u\n", (int)PTG_COUNT );
	TextoDebug("  Max. ref. distance\t\t= %f\n", refDistance );
	TextoDebug("  Cells resolution (x,y) \t= (%.04f,%.04f)\n", colGridRes_x,colGridRes_y );
	TextoDebug("  Max. speed (v,w)\t\t= (%.04f m/sec, %.04f deg/sec)\n", robotMax_V_mps, robotMax_W_degps );
	TextoDebug("  Robot Shape Points Count \t= %u\n", robotShape.verticesCount() );
	TextoDebug("  Obstacles 'z' axis range \t= [%.03f,%.03f]\n", minObstaclesHeight, maxObstaclesHeight );
	TextoDebug("\n\n");

	MRPT_TRY_END;
}



/*---------------------------------------------------------------
						setHolonomicMethod
  ---------------------------------------------------------------*/
void CReactiveNavigationSystem::setHolonomicMethod(
    MRPTAPPS::RNAV::THolonomicMethod	method,
    const char							*config_INIfile)
{
	// Delete current method:
	if (holonomicMethod) delete holonomicMethod;

	switch (method)
	{
	default:
	case hmSEARCH_FOR_BEST_GAP:
		holonomicMethod = new CHolonomicND();
		break;

	case hmVIRTUAL_FORCE_FIELDS:
		holonomicMethod = new CHolonomicVFF();
		break;
	};


}


/*---------------------------------------------------------------
						enableLogFile
  ---------------------------------------------------------------*/
void CReactiveNavigationSystem::enableLogFile(bool enable)
{
	try
	{
		// Disable:
		// -------------------------------
		if (!enable)
		{
			if (logFile)
			{
				TextoDebug("[CReactiveNavigationSystem::enableLogFile] Stopping logging.\n");
				// Close file:
				delete logFile;
				logFile = NULL;
			}
			else return;	// Already disabled.
		}
		else
		{	// Enable
			// -------------------------------
			if (logFile) return; // Already enabled:

			// Open file, find the first free file-name.
			char	aux[100];
			int     nFichero=0;
			bool    nombre_libre= false;

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

			while (!nombre_libre)
			{
				nFichero++;
				sprintf(aux, "./RNAV.logs/log_%03u.rnavlog", nFichero );

				nombre_libre = !SystemUtils::fileExists(aux);
			}

			// Open log file:
			logFile = new CFileStream(aux, fomWrite );

			TextoDebug("[CReactiveNavigationSystem::enableLogFile] Logging to file:");
			TextoDebug(aux);
			TextoDebug("\n");

		}
	} catch (...) {
		TextoDebug("[CReactiveNavigationSystem::enableLogFile] Exception!!\n");
	}

}

/*---------------------------------------------------------------
	  				performNavigationStep

  Este metodo se ejecuta periodicamente solo si se está en el estado
   NAVIGATING. Aqui se implementa el algoritmo de navegacion reactiva
  ---------------------------------------------------------------*/
void  CReactiveNavigationSystem::performNavigationStep()
{
	static UTILS::CTicTac						totalExecutionTime, executionTime, tictac;
	static MRML::CSimplePointsMap				WS_Obstacles;
	static MRML::COccupancyGridMap2D			WS_ObstaclesGrid;
	unsigned int								indexPTG,i,n,nSecDist;                    // Aux
	static CLogFileRecord						newLogRec;
	static std::vector<THolonomicMovement>		selectedHolonomicMovements;
	static std::vector<vector_float>			TP_Obstacles;
	float										targetDist;
	MRML::CPoint2D								relTarget;		// The target point, relative to current robot pose.
	static std::vector<MRML::CPoint2D>			TP_Targets;		// Target location (x,y) in TP-Space
	MRML::CPose2D								curPose;
	float										curVL;			// en metros/seg
	float										curW;			// en rad/segundo
	static std::vector<THolonomicMovement>		holonomicMovements;
	THolonomicMovement							selectedHolonomicMovement;
	float										cmd_v=0,cmd_w=0;	// The non-holonomic command
	float 										desired_cmd_v, desired_cmd_w;
	std::vector<CHolonomicLogFileRecord*>		HLFRs;
	static std::vector<float>					times_TP_transformations, times_HoloNav;
	static std::vector<bool>					valid_TP;
	static float								meanExecutionTime = 0.1f;
	static float								meanTotalExecutionTime= 0.1f;
	int											nSelectedPTG;
	static vector_float							prevV,prevW,prevSelPTG;
	static int									nLastSelectedPTG = -1;
	static CDynamicWindow						DW;
	static int									DEBUG_STEP = 0;
//	static TNavigatorBehavior					lastStepBehavior;
//	TNavigatorBehavior							saveLastBehavior;

	// Already closing??
	if (CerrandoHilo) return;

	// Lock
	critZoneNavigating.enter();

	try
	{
		// Iterations count:
		nIteration++;

		// Start timer
		totalExecutionTime.Tic();

		/* ----------------------------------------------------------------
		 	  Request current robot pose and velocities
		   ---------------------------------------------------------------- */
		if ( !RobotMotionControl.getCurrentPoseAndSpeeds/*Se ha rellenado el puntero?*/ ||
		        RobotMotionControl.getCurrentPoseAndSpeeds(curPose, curVL, curW ) )
		{
			Error_ParadaDeEmergencia("ERROR calling RobotMotionControl.getCurrentPoseAndSpeeds, stopping robot and finishing navigation");
			// Unlock:
			critZoneNavigating.leave();
			return;
		}

		DEBUG_STEP = 1;

		/* ----------------------------------------------------------------
		 	  Have we reached the target location?
		   ---------------------------------------------------------------- */
		targetDist= sqrt( square(curPose.x - navigationParams.target.x)+
		                  square(curPose.y - navigationParams.target.y) );


		// Enviar evento de final de navegacion??
		if (!navigationEndEventSent && targetDist<DIST_TO_TARGET_FOR_SENDING_EVENT)
		{
			navigationEndEventSent = true;
			if (EventsLaunching.sendNavigationEndEvent)
				EventsLaunching.sendNavigationEndEvent();
		}

		if ( targetDist < navigationParams.targetAllowedDistance &&
		        navigatorBehavior == beNormalNavigation )
		{
			if (RobotMotionControl.stop) RobotMotionControl.stop();
			navigationState = IDLE;
			TextoDebug("Navigation target was reached!\n" );

			if (!navigationEndEventSent)
			{
				navigationEndEventSent = true;
				if (EventsLaunching.sendNavigationEndEvent)
					EventsLaunching.sendNavigationEndEvent();
			}


			// Unlock:
			critZoneNavigating.leave();
			return;
		}

		DEBUG_STEP = 2;

		// Check the "no approaching the target"-alarm:
		// -----------------------------------------------------------
		if (targetDist < badNavAlarm_minDistTarget )
		{
			badNavAlarm_minDistTarget = targetDist;
			badNavAlarm_lastMinDistTime =  UTILS::SystemUtils::getCurrentTime();
		}
		else
		{
			// Too much time have passed?
			if ( SystemUtils::timeDifference( badNavAlarm_lastMinDistTime, SystemUtils::getCurrentTime() ) > badNavAlarm_AlarmTimeout)
			{
				std::cout << "\n--------------------------------------------\nWARNING: Timeout for approaching toward the target expired!! Aborting navigation!! \n---------------------------------\n";

				navigationState = NAV_ERROR;
				// Unlock:
				critZoneNavigating.leave();
				return;
			}
		}


		// Compute target location relative to current robot pose:
		// ---------------------------------------------------------------------
		relTarget = navigationParams.target - curPose;

		// STEP1: Collision Grids Builder.
		// -----------------------------------------------------------------------------
		STEP1_CollisionGridsBuilder();

		DEBUG_STEP = 3;

		// STEP2: Sense obstacles.
		// -----------------------------------------------------------------------------
		if (! STEP2_Sense( WS_Obstacles, &WS_ObstaclesGrid ) )
		{
			TextoDebug("Warning: Error while sensing obstacles. Robot will be stopped.\n");
			if (RobotMotionControl.stop) RobotMotionControl.stop();
			navigationState = NAV_ERROR;
			// Unlock:
			critZoneNavigating.leave();
			return;
		}

		// Start timer
		executionTime.Tic();

		// For some behaviors:
		//  If set to true, "cmd_v" & "cmd_w" must be set to the desired values:
		bool		skipNormalReactiveNavigation = false;

		// -----------------------------------------------------------------------------
		// STEP2(b): Manage different "behaviours" and their finite-state-machine.
		//            This allows complex things like crossing doors more easily!
		//			  If the behaviour is other than "normalNavigation", the target
		//            point may be changed
		//  (This was added JAN-2007, JLBC)
		// -----------------------------------------------------------------------------
		/*		saveLastBehavior = navigatorBehavior;
		        switch (navigatorBehavior)
		        {
		        	// =========================================
					//            beNormalNavigation
		            // =======================================
		        	case beNormalNavigation:
		            {
		            	// Check for a potential 'doorCrossing' behavior:
		                static CObservation2DRangeScan		auxScan;
		                static CPose2D						nullPose(0,0,0);
		                static DWORD						lastCheck = 0;
		                CPoint2D							perpDirection;
		                CPoint2D							P1,P2;
		                CPoint2D							B1,B2;
		                int									j;

		                if ( (GetTickCount() - lastCheck)*0.001f > DISABLE_PERIOD_AFTER_FAIL )
						{
							// Compute the normalized perpendicular vector to the segment robot-target:
							perpDirection.x = -relTarget.y;
							perpDirection.y = relTarget.x;
							perpDirection *= 1.0f/sqrt(perpDirection.norm());

							// Compute the points P1 and P2:
							P1.x = relTarget.x + 0.5f * DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2 * perpDirection.x;
							P1.y = relTarget.y + 0.5f * DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2 * perpDirection.y;
							P2.x = relTarget.x - 0.5f * DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2 * perpDirection.x;
							P2.y = relTarget.y - 0.5f * DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2 * perpDirection.y;

							// Compute the "span angle" between P1 and P2:
							auxScan.aperture = fabs( atan2( P1.y,P1.x ) - atan2( P2.y,P2.x ) );

							if (auxScan.aperture < (float)DEG2RAD(10) )  auxScan.aperture = (float)DEG2RAD(10);

							// And the number of range values:
							unsigned int 	nRangesToSim = ceil(auxScan.aperture / DEG2RAD(2));

							// Now: Simulate the scan:
							auxScan.rightToLeft = true;
							auxScan.maxRange = 5.0f;
							nullPose.phi = atan2( relTarget.y, relTarget.x );
							WS_ObstaclesGrid.laserScanSimulator( auxScan, nullPose,0.51f, nRangesToSim );

		//					SystemUtils::vectorToTextFile( auxScan.scan, "_debug_auxScan.txt");
		//					CFileStream("_debug_obsGrid.gridmap",fomWrite) << WS_ObstaclesGrid;

							// From these data, try to determine wether we should change to "doorCrossing" behavior:
							//  Criterion: If there is a "deep" gap, sourrounded by obstacles:
							// Find the deepest range:
							unsigned int	maxRangeIdx;
							float			maxRange = 0;

							// Look for the maximum in the "centre" -> the direction of the target:
							int				lookIdx_ini = 0.35*((float)nRangesToSim);
							int				lookIdx_end = 0.65*((float)nRangesToSim);
							for (j=lookIdx_ini;j<=lookIdx_end;j++)
							{
		                		if ( auxScan.scan[j] > maxRange )
								{
		                    		maxRange = auxScan.scan[j];
									maxRangeIdx  = j;
								}
							}

							// Is the gap deep enough??? -> Compare to distance to target:
							float	distToTarget = sqrt( relTarget.norm() );
							if ( maxRange > 0.99f * distToTarget )
							{
								// Look to the "left":
								int 			left_gap = maxRangeIdx;
								float			left_gap_range = maxRange;
								while ( left_gap>0 && auxScan.scan[left_gap]>0.5*maxRange )
		                    		left_gap--;

								// Look to the "right":
								int 			right_gap = maxRangeIdx;
								float			right_gap_range = maxRange;
								while ( right_gap<(nRangesToSim-1) && auxScan.scan[right_gap]>0.5*maxRange )
		                    		right_gap++;

								// Compute XY coordinates of each border, B1 & B2:
								float			B1_angle = nullPose.phi + (left_gap - (nRangesToSim/2.0))*(auxScan.aperture / nRangesToSim);
								float			B2_angle = nullPose.phi + (right_gap - (nRangesToSim/2.0))*(auxScan.aperture / nRangesToSim);
								B1.x = auxScan.scan[left_gap]*cos( B1_angle );
								B1.y = auxScan.scan[left_gap]*sin( B1_angle );
								B2.x = auxScan.scan[right_gap]*cos( B2_angle );
								B2.y = auxScan.scan[right_gap]*sin( B2_angle );

								float	doorwaySize = sqrt( square( B1.x-B2.x ) + square( B1.y-B2.y ) );

								// Are borders closer enough to justify this behavior?
								if ( (2*VORONOI_MINIMUM_CLEARANCE) < doorwaySize && doorwaySize < DOOR_CROSSING_MAXIMUM_DOORWAY_SIZE  )
								{
									// Yes, it seems a door toward the target!!
									TextoDebug("[Behaviors] Door-like situation detected!! Starting more precise computations!!\n");

									// Stop the robot:
									//if (RobotMotionControl.stop) RobotMotionControl.stop();

									// Compute Voronoi to determine the mid-point:
		                    		int			voronoiKeyPoint = -1;
									float	vor_x_min = min( -1.0f, relTarget.x - 1.0f );
									float	vor_x_max = max( -1.0f, relTarget.x + 1.0f );
									float	vor_y_min = min( -1.0f, relTarget.y - 1.0f );
									float	vor_y_max = max( -1.0f, relTarget.y + 1.0f );

									int		cxMin = WS_ObstaclesGrid.x2idx( vor_x_min );
									int		cxMax = WS_ObstaclesGrid.x2idx( vor_x_max );
									int		cyMin = WS_ObstaclesGrid.y2idx( vor_y_min );
									int		cyMax = WS_ObstaclesGrid.y2idx( vor_y_max );

									WS_ObstaclesGrid.buildVoronoiDiagram( 0.51f,0.10f, cxMin,cxMax,cyMin,cyMax );
									WS_ObstaclesGrid.findCriticalPoints(0.20f);
									if ( WS_ObstaclesGrid.CriticalPointsList.x.size()>0 )
									{
		                        		float clrScale = 0.01f*WS_ObstaclesGrid.getResolution(); // The scale factor for the clearance

		                        		// Find THE point in the doorway:
										for (j=0;j<WS_ObstaclesGrid.CriticalPointsList.x.size();j++)
										{
		                            		// 2 conditions: Minimum clearance, and to be in the right direction!
											float 	clearance = clrScale * WS_ObstaclesGrid.CriticalPointsList.clearance[j];
											float 	angle =  atan2(
		                                		WS_ObstaclesGrid.idx2y( WS_ObstaclesGrid.CriticalPointsList.y[j]),
												WS_ObstaclesGrid.idx2x( WS_ObstaclesGrid.CriticalPointsList.x[j]) );

											if (( (B1_angle<angle && angle<B2_angle) ||
												  (B2_angle<angle && angle<B1_angle) ) &&
												  clearance >= VORONOI_MINIMUM_CLEARANCE )
											{
		                                		voronoiKeyPoint = j;
											}
										}
									}

									// Do we really have a door??
									if (  voronoiKeyPoint == -1)
									{
		                        		// No!!
										// Disable this check for a while...
										lastCheck = GetTickCount();
										TextoDebug("[Behaviors] Door-like situation test FAILED!!! Disabling this test for a while...\n");
									}
									else
									{
		                        		// Yes!!
										TextoDebug("[Behaviors] Door-like situation DETECTED!!! Changing to 'beDoorCrosing1' behavior!\n");

										// Compute desired path to pass the door:
		                                // ------------------------------------------------
		                                float doorway_x = WS_ObstaclesGrid.idx2x( WS_ObstaclesGrid.CriticalPointsList.x[voronoiKeyPoint] );
		                                float doorway_y = WS_ObstaclesGrid.idx2y( WS_ObstaclesGrid.CriticalPointsList.y[voronoiKeyPoint] );

		                                // Compute the "basis" points of the doorway:
		                                int		basis_x[2],basis_y[2], nBasis;
		                                WS_ObstaclesGrid.computeClearance(
		                                	WS_ObstaclesGrid.CriticalPointsList.x[voronoiKeyPoint],
		                                    WS_ObstaclesGrid.CriticalPointsList.y[voronoiKeyPoint],
		                                    basis_x, basis_y, &nBasis, false );

		                                if ( nBasis!=2 )
		                                {
			                        		// No!!
											// Disable this check for a while...
											lastCheck = GetTickCount();
											TextoDebug("[Behaviors] Door-like situation test FAILED!!! (No basis found!!!) Disabling this test for a while...\n");
											//navigatorBehavior = beNormalNavigation;
											//skipNormalReactiveNavigation = true;
		                                }
		                                else
		                                {
			                                // The vector "parallel" to the door:
		    	                            float	longDoor_vx = basis_x[1] - basis_x[0];
		        	                        float	longDoor_vy = basis_y[1] - basis_y[0];

			                                // Compute the perpendicular to the door:
		    	                            CPoint2D   p_v( -longDoor_vy, longDoor_vx );
		        	                        p_v *= 1.0f/sqrt( p_v.norm() );

		            	                    // Compute two points: one before and another after the door:
		                        	        CPoint2D   auxPass1( doorway_x + p_v.x * VORONOI_PATH_DIST_FROM_DOORWAY,doorway_y + p_v.y * VORONOI_PATH_DIST_FROM_DOORWAY  );
		                                    CPoint2D   auxPass2( doorway_x - p_v.x * VORONOI_PATH_DIST_FROM_DOORWAY,doorway_y - p_v.y * VORONOI_PATH_DIST_FROM_DOORWAY  );

			                                float		distPass1 = sqrt(auxPass1.norm());
			                                float		distPass2 = sqrt(auxPass2.norm());

		    	                            // PassPoint #1: Before the doorway
			                                // PassPoint #2: After the doorway
		    	                            if (distPass1>distPass2)
											{
												CPoint2D	sw(auxPass2);
												auxPass2 = auxPass1;
												auxPass1 = sw;
											}

											// One more check: The line "m_bePassPoint1"-"m_bePassPoint2" must be NOT occupied!
											CPoint2D A_pass = auxPass2 - auxPass1;
											bool	doorwayIsValid = true;
											for (int A_pass_step = 0;doorwayIsValid && A_pass_step<50;A_pass_step++)
											{
												if (0.49f>WS_ObstaclesGrid.getPos(
													auxPass1.x + A_pass_step*A_pass.x/50,
													auxPass1.y + A_pass_step*A_pass.y/50 ) )
														doorwayIsValid = false;
											}

											if (doorwayIsValid)
											{
												// Save the global coordinates of the 2 pass points:
												m_bePassPoint1 = curPose + auxPass1;
		                						m_bePassPoint2 = curPose + auxPass2;

												// Set next beh:
												navigatorBehavior = beDoorCrosing1;
												//skipNormalReactiveNavigation = false;
											}
										}
		                            }
								} // too wide/small!
							} // end if gap deep enough
						} // end if not in disabled time

		            } break;

		        	// =========================================
					            beDoorCrosing1
		                    Go to "m_bePassPoint1"
		        	// =========================================
		        	case beDoorCrosing1:
		            {
						// Check if the precomputed perpendicular segment is still valid:
						bool	doorwayIsValid = true;
						CPoint2D	auxPass1 = m_bePassPoint1 - curPose;
						CPoint2D	auxPass2 = m_bePassPoint2 - curPose;
						CPoint2D	A_pass =  auxPass2 - auxPass1;
						for (int A_pass_step = 0;doorwayIsValid && A_pass_step<50;A_pass_step++)
						{
							if (0.49f>WS_ObstaclesGrid.getPos(
								auxPass1.x + A_pass_step*A_pass.x/50,
								auxPass1.y + A_pass_step*A_pass.y/50 ) )
									doorwayIsValid = false;
						}

						if (!doorwayIsValid)
						{
		                	TextoDebug("[beDoorCrosing1] The doorway segment is occupied!! Reseting to normal navigation!\n");
							// Set next beh:
							navigatorBehavior = beNormalNavigation;
							skipNormalReactiveNavigation = false;
						}
						else
						{
		            		// Substitute final target by this one:
		            		relTarget = m_bePassPoint1 - curPose;

							// Finish??
							targetDist = sqrt( relTarget.norm() );

							if (targetDist<DOOR_CROSSING_DIST_TO_AUX_TARGETS)
							{
		                		TextoDebug("[beDoorCrosing1] Done!!\n");
								// Set next beh:
								navigatorBehavior = beDoorCrosing2;
								//skipNormalReactiveNavigation = true;
							}
							else
							{
								skipNormalReactiveNavigation = false;
							}
						}

		            } break;

		        	// =========================================
					            beDoorCrosing2
		                  Set heading towards 'm_bePassPoint2'
		        	// =========================================
		        	case beDoorCrosing2:
		            {
						// Check if the precomputed perpendicular segment is still valid:
						bool	doorwayIsValid = true;
						CPoint2D	auxPass1 = m_bePassPoint1 - curPose;
						CPoint2D	auxPass2 = m_bePassPoint2 - curPose;
						CPoint2D	A_pass =  auxPass2 - auxPass1;
						for (int A_pass_step = 0;doorwayIsValid && A_pass_step<50;A_pass_step++)
						{
							if (0.49f>WS_ObstaclesGrid.getPos(
								auxPass1.x + A_pass_step*A_pass.x/50,
								auxPass1.y + A_pass_step*A_pass.y/50 ) )
									doorwayIsValid = false;
						}

						if (!doorwayIsValid)
						{
		                	TextoDebug("[beDoorCrosing2] The doorway segment is occupied!! Reseting to normal navigation!\n");
							// Set next beh:
							navigatorBehavior = beNormalNavigation;
							skipNormalReactiveNavigation = false;
						}
						else
						{
		            		// Substitute final target by this one:
		            		relTarget = m_bePassPoint1 - curPose;

		            		// Compute the desired heading:
							float	headingError = atan2( relTarget.y , relTarget.x );

							// Set the desired velocities:
							cmd_v = curVL * 0.75f;
							cmd_w = DEG2RAD(robotMax_W_degps) * headingError / DEG2RAD( DOORCROSSING_ROTATION_CTE_DEG );

		            		skipNormalReactiveNavigation = true;

							if ( fabs(headingError) < DEG2RAD(DOORCROSSING_HEADING_ACCURACY_DEG) )
							{
								// Set next beh:
								cmd_w = 0;
								navigatorBehavior = beDoorCrosing3;
								skipNormalReactiveNavigation = true;
		                		TextoDebug("[beDoorCrosing2] Done!!\n");
							}
						}

		            } break;

		        	// =========================================
					            beDoorCrosing3
		        	// =========================================
		        	case beDoorCrosing3:
		            {
						static DWORD	lastTimeWeApproachTarget = 0;
						static float	lastClosestToTarget=1e3f;

		            	// Substitute final target by this one:
		            	relTarget = m_bePassPoint2 - curPose;

		                // Finish??
		                targetDist = sqrt( relTarget.norm() );

						// Is this the first iteration??
						if ( lastStepBehavior != beDoorCrosing3 )
						{
							lastTimeWeApproachTarget = GetTickCount();
							lastClosestToTarget = targetDist;
						}
						else
						{
							// Done?
							bool		weAreInSight = true;
							for (int A_pass_step = 0;weAreInSight && A_pass_step<50;A_pass_step++)
							{
								if (0.49f>WS_ObstaclesGrid.getPos(
									A_pass_step*relTarget.x/50,
									A_pass_step*relTarget.y/50 ) )
										weAreInSight = false;
							}

							if ((weAreInSight && targetDist<2*DOOR_CROSSING_DIST_TO_AUX_TARGETS) || targetDist<DOOR_CROSSING_DIST_TO_AUX_TARGETS)
							{
		                		TextoDebug("[beDoorCrosing3] Done!!\n");
								// Set next beh:
								navigatorBehavior = beNormalNavigation;
								//skipNormalReactiveNavigation = true;
							}
							else
							{
								// Keep min. distance:
								if ( targetDist < lastClosestToTarget )
								{
									lastTimeWeApproachTarget = GetTickCount();
									lastClosestToTarget = targetDist;
								}
								else // Check if we are not approaching the target:
								if ( (GetTickCount() - lastTimeWeApproachTarget)*0.001f > DOOR_CROOSING_BEH3_TIMEOUT )
								{
									// ERROR! -> reset normal beh.
		                			TextoDebug("[beDoorCrosing3] Error! Cannot find a good way! Resetting behavior to normal\n");
									navigatorBehavior = beNormalNavigation;
									//skipNormalReactiveNavigation = true;
								}
							}
						}
		            } break;

		        	// =========================================
					            beHeadDirection
		        	// =========================================
		        	case beHeadDirection:
		            {
		            	cmd_v = 0;
		                cmd_w = 0;

		            	skipNormalReactiveNavigation = true;
		            } break;

		            // Error:
			        default:
					{
						TextoDebug("Error: Invalid behavior value!!\n");
						if (RobotMotionControl.stop) RobotMotionControl.stop();
						navigationState = NAV_ERROR;
						// Unlock:
						critZoneNavigating.leave();
						return;
					}

		        }; // end switch
				lastStepBehavior = saveLastBehavior;
				*/


		if (! skipNormalReactiveNavigation )
		{
			// Clip obstacles by "z" axis coordinates:
			WS_Obstacles.clipOutOfRangeInZ( minObstaclesHeight, maxObstaclesHeight );

			// Clip obstacles out of the reactive method range:
			CPoint2D    dumm(0,0);
			WS_Obstacles.clipOutOfRange( dumm, refDistance+1.5f );


			//  STEP3: Build TP-Obstacles and transform target location into TP-Space
			// -----------------------------------------------------------------------------
			// Vectors for each PTG:
			if (PTGs.size() != TP_Targets.size() )   TP_Targets.resize( PTGs.size() );
			if (PTGs.size() != valid_TP.size() )	valid_TP.resize( PTGs.size() );

			// Vectors for each PTG & Security Distance:
			n = PTGs.size() * securityDistances.size();
			if (n != TP_Obstacles.size() )				TP_Obstacles.resize( n );
			if (n != times_TP_transformations.size() )	times_TP_transformations.resize( n );
			if (n != holonomicMovements.size() )		holonomicMovements.resize( n );
			if (n != HLFRs.size() )						HLFRs.resize( n );
			if (n != times_HoloNav.size() )				times_HoloNav.resize( n );
			newLogRec.infoPerPTG.resize( n );

			DEBUG_STEP = 4;

			// For each PTG:
			for (indexPTG=0,i=0;indexPTG<PTGs.size();indexPTG++)
			{
				// Target location:
				float	alfa,dist;
				int		k;

				// Firstly, check if target falls into the PTG domain!!
				valid_TP[indexPTG] = true;
				//valid_TP[i] = PTGs[i]->PTG_IsIntoDomain( relTarget.x,relTarget.y );

				if (valid_TP[indexPTG])
				{
					PTGs[indexPTG]->lambdaFunction(
					    relTarget.x,
					    relTarget.y,
					    k,
					    dist );

					alfa = PTGs[indexPTG]->index2alfa(k);
					TP_Targets[indexPTG].x = cos(alfa) * dist;
					TP_Targets[indexPTG].y = sin(alfa) * dist;
				}

				// And for each security distance:
				for (nSecDist=0;nSecDist<securityDistances.size();nSecDist++,i++)
				{
					tictac.Tic();

					// TP-Obstacles
					STEP3_SpaceTransformer(	WS_Obstacles,
					                        PTGs[indexPTG],
					                        TP_Obstacles[i],
					                        nSecDist );

					times_TP_transformations[i] = (float)(tictac.Tac());

				} // nSecDist

			} // indexPTG

			DEBUG_STEP = 5;

			//  STEP4: Holonomic navigation method
			// -----------------------------------------------------------------------------
			// For each PTG:
			for (indexPTG=0,i=0;indexPTG<PTGs.size();indexPTG++)
			{
				// And for each security distance:
				for (nSecDist=0;nSecDist<securityDistances.size();nSecDist++,i++)
				{
					tictac.Tic();

					holonomicMovements[i].PTG = PTGs[indexPTG];
					holonomicMovements[i].securityDistance = securityDistances[nSecDist];

					if (valid_TP[indexPTG])
					{
						STEP4_HolonomicMethod(	TP_Obstacles[i],
						                       TP_Targets[indexPTG],
						                       PTGs[indexPTG]->getMax_V_inTPSpace(),
						                       holonomicMovements[i],
						                       &HLFRs[i]);
					}
					else
					{
						holonomicMovements[i].direction = 0;
						holonomicMovements[i].evaluation = 0;
						holonomicMovements[i].speed = 0;
						HLFRs[i] = new CLogFileRecord_VFF();
					}
					times_HoloNav[i] = (float)tictac.Tac();

				} // nSecDist
			} // indexPTG

			DEBUG_STEP = 6;

			// STEP5: Evaluate each movement to assign them a "evaluation" value.
			// ---------------------------------------------------------------------

			// For each PTG:
			for (indexPTG=0,i=0;indexPTG<PTGs.size();indexPTG++)
			{
				// And for each security distance:
				for (nSecDist=0;nSecDist<securityDistances.size();nSecDist++,i++)
				{
					if (valid_TP[indexPTG])
						STEP5_Evaluator(	holonomicMovements[i],
						                 TP_Obstacles[i],
						                 relTarget,
						                 TP_Targets[indexPTG],
						                 nLastSelectedPTG == (int)indexPTG,
						                 newLogRec.infoPerPTG[i],
						                 nSecDist
						               );

					if (((CLogFileRecord_ND*)HLFRs[i])->situation == CHolonomicND::SITUATION_TARGET_DIRECTLY)
					{
						holonomicMovements[i].evaluation += 1.0f;
					}

				} // nSecDist
			} // indexPTG

			DEBUG_STEP = 7;

			// STEP6: Selects the best movement
			// ---------------------------------------------------------------------
			STEP6_Selector( holonomicMovements, selectedHolonomicMovement, nSelectedPTG );
			nLastSelectedPTG = nSelectedPTG;

			DEBUG_STEP = 8;

			// STEP7: Get the non-holonomic movement command.
			// ---------------------------------------------------------------------
			STEP7_NonHolonomicMovement( selectedHolonomicMovement, cmd_v, cmd_w);

			last_cmd_v = cmd_v;
			last_cmd_w = cmd_w;

			DEBUG_STEP = 9;

			// STEP8: Dynamics: Finds the best (cmd_v/w) closer to (desired_cmd_v/w);
			// ---------------------------------------------------------------------
			desired_cmd_v = cmd_v;
			desired_cmd_w = cmd_w;

			/*		DW.v_max = min( robotMax_V_mps, curVL + robotMax_V_accel_mpss * deg);
					DW.v_min = max(-robotMax_V_mps, curVL - robotMax_V_accel_mpss * meanExecutionPeriod);
					DW.w_max = min( DEG2RAD(robotMax_W_degps), curW + DEG2RAD(robotMax_W_accel_degpss * meanExecutionPeriod) );
					DW.w_min = max(-DEG2RAD(robotMax_W_degps), curW - DEG2RAD(robotMax_W_accel_degpss * meanExecutionPeriod) );
			*/
			// Possible cases:
			// --------------------------
			// 1) cmd is into the DW:
			if (1) /*( cmd_v>=DW.v_min && cmd_v<=DW.v_max &&
				 cmd_w>=DW.w_min && cmd_w<=DW.w_max ) */
			{
				// OK!! Command is feasible. Let it be.

			}
			else
			{
				// Cmd is not feasible. Find the most similar one into the DW:
				// ------------------------------------------------------------------
				// Cases:
				// 2) Desired curvature is into the reachable set:
				//     Find cuts of constant curvature line with DW:
				// 3) Desired curvature out of reachability: Only an
				//     approximation can be found:
				DW.findBestApproximation(
				    desired_cmd_v, desired_cmd_w,	// IN
				    cmd_v, cmd_w	// OUT
				);
			}

		} // end of "!skipNormalReactiveNavigation"

		// ---------------------------------------------------------------------
		//				SEND MOVEMENT COMMAND TO THE ROBOT
		// ---------------------------------------------------------------------
		if ( cmd_v == 0.0 && cmd_w == 0.0 )
		{
			if ( RobotMotionControl.stop )
				RobotMotionControl.stop();
		}
		else
		{
			if ( !RobotMotionControl.changeSpeeds /*El puntero es correcto?*/ ||
			        RobotMotionControl.changeSpeeds( cmd_v, cmd_w ) )
			{
				Error_ParadaDeEmergencia("\nERROR calling RobotMotionControl::changeSpeeds!! Stopping robot and finishing navigation\n");
				// Unlock:
				critZoneNavigating.leave();
				return;
			}
		}

		DEBUG_STEP = 10;

		// Statistics:
		// ----------------------------------------------------
		float	executionTimeValue = (float) executionTime.Tac();
		meanExecutionTime=  0.3f * meanExecutionTime +
		                    0.7f * executionTimeValue;
		meanTotalExecutionTime=  0.3f * meanTotalExecutionTime +
		                         0.7f * ((float)totalExecutionTime.Tac() );
		meanExecutionPeriod = 0.3f * meanExecutionPeriod +
		                      0.7f * min(1.0f, (float)timerForExecutionPeriod.Tac());

		timerForExecutionPeriod.Tic();


		TextoDebug("CMD:%.02lfm/s,%.02lfd/s \t",
		           (double)cmd_v,
		           (double)RAD2DEG( cmd_w ) );

		TextoDebug(" T=%.01lfms Exec:%.01lfms|%.01lfms \t",
		           1000.0*meanExecutionPeriod,
		           1000.0*meanExecutionTime,
		           1000.0*meanTotalExecutionTime );

		if (!skipNormalReactiveNavigation)
		{
			TextoDebug("E=%.01lf ", (double)selectedHolonomicMovement.evaluation );
			TextoDebug("PTG#%i ", nSelectedPTG);
		}
		else
		{
			nSelectedPTG = 0;
		}

		TextoDebug("BEHAVIOR:%i\n", (int)navigatorBehavior );

		DEBUG_STEP = 11;

		// ---------------------------------------
		// Generate log record
		// ---------------------------------------
		newLogRec.WS_Obstacles			= WS_Obstacles;
		newLogRec.robotOdometryPose		= curPose;
		newLogRec.WS_target_relative	= relTarget;
		newLogRec.v						= cmd_v;
		newLogRec.w						= cmd_w;
		newLogRec.nSelectedPTG			= nSelectedPTG;
		newLogRec.executionTime			= executionTimeValue;
		newLogRec.actual_v				= curVL;
		newLogRec.actual_w				= curW;
		newLogRec.estimatedExecutionPeriod = meanExecutionPeriod;
		newLogRec.nPTGs					= PTGs.size();
		newLogRec.securityDistances		= securityDistances;
		newLogRec.navigatorBehavior		= navigatorBehavior;

		if ( navigatorBehavior==beDoorCrosing1 ||
		        navigatorBehavior==beDoorCrosing2 ||
		        navigatorBehavior==beDoorCrosing3 )
		{
			newLogRec.doorCrossing_P1 = m_bePassPoint1 - curPose;
			newLogRec.doorCrossing_P2 = m_bePassPoint2 - curPose;
		}

		if (newLogRec.robotShape_x.size() != (n=robotShape.verticesCount()))
		{
			newLogRec.robotShape_x.resize(n);
			newLogRec.robotShape_y.resize(n);
		}
		for (i=0;i<n;i++)
		{
			newLogRec.robotShape_x[i]= robotShape.GetVertex_x(i);
			newLogRec.robotShape_y[i]= robotShape.GetVertex_y(i);
		}

		// Previous values:
		if (prevV.size() != PREVIOUS_VALUES_IN_LOG) prevV.resize(PREVIOUS_VALUES_IN_LOG);
		if (prevW.size() != PREVIOUS_VALUES_IN_LOG) prevW.resize(PREVIOUS_VALUES_IN_LOG);
		if (prevSelPTG.size() != PREVIOUS_VALUES_IN_LOG) prevSelPTG.resize(PREVIOUS_VALUES_IN_LOG);

		// Shift:
		for (i=0;i<PREVIOUS_VALUES_IN_LOG-1;i++)
		{
			prevV[i]=prevV[i+1];
			prevW[i]=prevW[i+1];
			prevSelPTG[i]=prevSelPTG[i+1];
		}
		// Last values:
		prevV[PREVIOUS_VALUES_IN_LOG-1] = cmd_v;
		prevW[PREVIOUS_VALUES_IN_LOG-1] = cmd_w;
		prevSelPTG[PREVIOUS_VALUES_IN_LOG-1] = (float)nSelectedPTG;

		newLogRec.prevV					= prevV;
		newLogRec.prevW					= prevW;
		newLogRec.prevSelPTG			= prevSelPTG;

		DEBUG_STEP = 12;

		// For each PTG:
		if (!skipNormalReactiveNavigation)
		{
			for (indexPTG=0,i=0;indexPTG<PTGs.size();indexPTG++)
			{
				// And for each security distance:
				for (nSecDist=0;nSecDist<securityDistances.size();nSecDist++,i++)
				{
					newLogRec.infoPerPTG[i].PTG_desc					= PTGs[indexPTG]->getDescription();
					newLogRec.infoPerPTG[i].TP_Obstacles				= TP_Obstacles[i];
					newLogRec.infoPerPTG[i].TP_Target					= TP_Targets[indexPTG];
					newLogRec.infoPerPTG[i].timeForTPObsTransformation	= times_TP_transformations[i];
					newLogRec.infoPerPTG[i].timeForHolonomicMethod		= times_HoloNav[i];
					newLogRec.infoPerPTG[i].HLFR = HLFRs[i];
					newLogRec.infoPerPTG[i].desiredDirection = holonomicMovements[i].direction;
					newLogRec.infoPerPTG[i].desiredSpeed = holonomicMovements[i].speed;
					newLogRec.infoPerPTG[i].evaluation = holonomicMovements[i].evaluation;
				}
			}
		}
		else
		{
			newLogRec.infoPerPTG.clear();
		}

		DEBUG_STEP = 13;

		// --------------------------------------
		//  Save to log file:
		// --------------------------------------
		if (logFile) (*logFile) << newLogRec;

		DEBUG_STEP = 14;

		// --------------------------------------
		// Set as last log record
		// --------------------------------------
		// Lock
		critZoneLastLog.enter();

		// COPY
		lastLogRecord = newLogRec;

		// Unlock
		critZoneLastLog.leave();

		DEBUG_STEP = 15;

	}
	catch (std::exception &e)
	{
		std::cout << e.what();
		std::cout << "[CReactiveNavigationSystem::performNavigationStep] Exceptions!!\n";
	}
	catch (...)
	{
		std::cout << "[CReactiveNavigationSystem::performNavigationStep] Unexpected exception!!:\n";
		std::cout << format("DEBUG_STEP = %u\n",DEBUG_STEP);
	}

	// Unlock:
	critZoneNavigating.leave();

}

/*************************************************************************

                         STEP1_CollisionGridsBuilder

     -> C-Paths generation.
     -> Check de colision entre cada celda de la rejilla y el contorno
          del robot

*************************************************************************/
void CReactiveNavigationSystem::STEP1_CollisionGridsBuilder()
{
	int				nVerts=0,nSegmentos=0;
	nSegmentos = robotShape.verticesCount();
	nVerts = this->robotShape.verticesCount();

	try
	{
		// Solo una vez:
		if (collisionGridsMustBeUpdated)
		{
			// No volver a calcular:
			collisionGridsMustBeUpdated = false;

			TextoDebug("\n[STEP1_CollisionGridsBuilder] Starting... *** THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! **\n");

			for (unsigned int nPT=0;nPT<PTGs.size();nPT++)
			{
				UTILS::CTicTac		tictac;

				TextoDebug("Computing collision cells for PTG[%u]...", (int)nPT);
				tictac.Tic();

				// Reservar memoria para todos los puntos:
				PTGs[nPT]->allocMemForVerticesData( nVerts );

				int     nPaths = PTGs[nPT]->getAlfaValuesCount();

				for (int k=0;k<nPaths;k++)
				{
					int nPointsInPath = PTGs[nPT]->getPointsCountInCPath_k( k );

					for (int n=0;n<nPointsInPath;n++)
					{
						float x   = PTGs[nPT]->GetCPathPoint_x( k,n );
						float y   = PTGs[nPT]->GetCPathPoint_y( k,n );
						float phi = PTGs[nPT]->GetCPathPoint_phi( k,n );

						for (int m = 0;m<nVerts;m++)
						{
							float vx = x + cos(phi)*robotShape.GetVertex_x(m)-sin(phi)*robotShape.GetVertex_y(m);
							float vy = y + sin(phi)*robotShape.GetVertex_x(m)+cos(phi)*robotShape.GetVertex_y(m);
							PTGs[nPT]->setVertex_xy(k,n,m, vx, vy );
						}       // for v
					}       // for n
				}       // for k


				// Calcular choques del contorno del robot con las celdas de la rejilla:
				// ----------------------------------------------------------------------------
				CParameterizedTrajectoryGenerator  *PT = PTGs[nPT];
				int             Ki = PT->getAlfaValuesCount();
				char            auxStr[100];

				sprintf(auxStr,"ReacNavGrid_%s_PTG%03u.dat",robotName.c_str(),nPT);

				// Si se puede cargar de fichero, evitar recalcularla:
				if ( PT->collisionGrids.LoadFromFile( auxStr ) )
					TextoDebug("loaded from file OK\n");
				else
				{
					// RECOMPUTE THE COLLISION GRIDS:
					// ---------------------------------------
					//float		distToObstacle;

					for (int k=0;k<Ki;k++)
					{
						int     nPuntos = PT->getPointsCountInCPath_k(k);

						for (int n=0;n<(nPuntos-1);n++)
						{
							//float			*vertexs_x = PT->getVertixesArray_x(k,n);
							//float			*vertexs_y = PT->getVertixesArray_y(k,n);
							//unsigned int	nVertexs = PT->getVertixesCount();

							// ------------------------------------------------------------
							//					VERSION 1
							//		No "security distance" (Runs moderately fast)
							// ------------------------------------------------------------
							// Ver colisiones entre obs. y el segmento "s" en los
							//  instantes "n" y "n+1"
							for (int m=0;m<nSegmentos;m++)
							{
								float          v1n_x,  v1n_y;
								float          v2n_x,  v2n_y;
								float          v1n1_x, v1n1_y;
								float          v2n1_x, v2n1_y;

								float          minx,maxx,miny,maxy;

								v1n_x = PT->getVertex_x(k,n,m);
								v1n_y = PT->getVertex_y(k,n,m);

								v1n1_x = PT->getVertex_x(k,n+1,m);
								v1n1_y = PT->getVertex_y(k,n+1,m);

								v2n_x = PT->getVertex_x(k,n,(m+1) % nSegmentos);
								v2n_y = PT->getVertex_y(k,n,(m+1) % nSegmentos);

								v2n1_x = PT->getVertex_x(k,n+1,(m+1) % nSegmentos);
								v2n1_y = PT->getVertex_y(k,n+1,(m+1) % nSegmentos);

								minx=min( v1n_x, min( v2n_x, min( v1n1_x, v2n1_x ) ) );
								maxx=max( v1n_x, max( v2n_x, max( v1n1_x, v2n1_x ) ) );
								miny=min( v1n_y, min( v2n_y, min( v1n1_y, v2n1_y ) ) );
								maxy=max( v1n_y, max( v2n_y, max( v1n1_y, v2n1_y ) ) );

								// Calcular el rango de la rejilla que se ve afectada por el
								//  movimiento de este segmento del robot, y solo comprobar
								//  colisiones en ese trozo:
								int		ix_min, iy_min, ix_max,iy_max;
								PT->collisionGrids[0]->Coord_a_indice( minx,miny, ix_min, iy_min );
								PT->collisionGrids[0]->Coord_a_indice( maxx,maxy, ix_max, iy_max );

								for (int ix=ix_min;ix<ix_max;ix++)
								{
									for (int iy=iy_min;iy<iy_max;iy++)
									{
										int cellIdx = PT->collisionGrids[0]->getCellIndex( ix, iy );

										float cx1,cy1,cx2,cy2;
										PT->collisionGrids[0]->getCellCoords(cellIdx, cx1,cy1,cx2,cy2);

										if ( PointIsIntoPolygon( 0.5f*(cx1+cx2), 0.5f*(cy1+cy2), v1n_x, v1n_y, v2n_x, v2n_y, v2n1_x, v2n1_y, v1n1_x, v1n1_y ) )
										{
											// Colision!! Update cell info:
											//float dist = PT->GetCPathPoint_d(k, n+1);
											//PT->Rejilla->AddCeldaColision( idx, k, dist );
											PT->collisionGrids[0]->updateCellInfo(
											    cellIdx,					// The cell index
											    k,							// The path (Alfa value)
											    PT->GetCPathPoint_d(k, n+1) // The collision distance
											);

										}
									}	// for iy
								}	// for ix

							}	// for m

							/*

							// ------------------------------------------------------------
							//					VERSION 2:
							//	 Account for a "security distance" (EXTREMELY SLOW!!!)
							// ------------------------------------------------------------
							unsigned int	nSecurityDistances = securityDistances.size();
							unsigned int	nCells  = PT->collisionGrids[0]->getCellsTotal();

							// For each cell into the collision grids:
							for (unsigned int cellIdx=0;cellIdx<nCells;cellIdx++)
							{
								float cx1,cy1,cx2,cy2,cellSize;
								PT->collisionGrids[0]->getCellCoords(
									cellIdx,
									cx1,
									cy1,
									cx2,
									cy2);
								cellSize = fabs(cx2-cx1);


								// Compute the distance from the cell to the polygon:
								distToObstacle = GEO::distancePointToPolygon2D(
										cx1*0.5f+cx2*0.5f,
										cy1*0.5f+cy2*0.5f,
										nVertexs,
										vertexs_x,
										vertexs_y );

								// For each one out the security distances:
								for (unsigned int d=0;d<nSecurityDistances;d++)
								{
									if ( distToObstacle <= (securityDistances[d] + cellSize ) )
									{
										// Colision!! Update cell info:
										PT->collisionGrids[d]->updateCellInfo(
													cellIdx,					// The cell index
													k,							// The path (Alfa value)
													PT->GetCPathPoint_d(k, n+1) // The collision distance
													);
									} // end if colision
								} // d
							} // cellIdx
							*/

						} // n
						TextoDebug("%u/%u,",k,Ki);
					} // k

					TextoDebug("Done! [%.03lf seg]\n",tictac.Tac() );

					// Para que la proxima vez no haya que calcularlo:
					PT->collisionGrids.SaveToFile( auxStr );

				}	// "else" recompute all PTG

			}	// for nPT

		} // end of "if" needs update

	}
	catch (std::exception &e)
	{
		TextoDebug("[CReactiveNavigationSystem::STEP1_CollisionGridsBuilder] Exception:");
		TextoDebug((char*)(e.what()));
	}
	catch (...)
	{
		TextoDebug("[CReactiveNavigationSystem::STEP1_CollisionGridsBuilder] Unexpected exception!\n");
	}


}

/*************************************************************************

                              STEP2_Sense

     Sensors adquisition and obstacle points fusion

*************************************************************************/
bool CReactiveNavigationSystem::STEP2_Sense(
    MRML::CSimplePointsMap				&out_obstacles,
    MRML::COccupancyGridMap2D			*out_obstaclesGridMap )
{
	try
	{
		return Sensors.sense( out_obstacles, out_obstaclesGridMap );
	}
	catch (std::exception &e)
	{
		TextoDebug("[CReactiveNavigationSystem::STEP2_Sense] Exception:");
		TextoDebug((char*)(e.what()));
		return false;
	}
	catch (...)
	{
		TextoDebug("[CReactiveNavigationSystem::STEP2_Sense] Unexpected exception!\n");
		return false;
	}

}

/* -------------------------------------------------------------
						PointIsIntoPolygon
   ------------------------------------------------------------- */
bool CReactiveNavigationSystem::PointIsIntoPolygon(
    float x,
    float y,
    float v1x,
    float v1y,
    float v2x,
    float v2y,
    float v3x,
    float v3y,
    float v4x,
    float v4y )
{
	// La idea: Si el punto esta dentro, la suma de todos los angulos,
	//   desde el punto a cada uno de los vertices, sumados, debera ser
	//   de 360deg = 2*PI:

	float	a1 = atan2( v1y - y , v1x - x );
	float	a2 = atan2( v2y - y , v2x - x );
	float	a3 = atan2( v3y - y , v3x - x );
	float	a4 = atan2( v4y - y , v4x - x );

	// Angulos entre los vertices:
	float   da1 = a2-a1;
	while (da1>M_PI) da1-=(float)M_2PI;
	while (da1<M_PI) da1+=(float)M_2PI;
	float   da2 = a3-a2;
	while (da2>M_PI) da2-=(float)M_2PI;
	while (da2<M_PI) da2+=(float)M_2PI;
	float   da3 = a4-a3;
	while (da3>M_PI) da3-=(float)M_2PI;
	while (da3<M_PI) da3+=(float)M_2PI;
	float   da4 = a1-a4;
	while (da4>M_PI) da4-=(float)M_2PI;
	while (da4<M_PI) da4+=(float)M_2PI;

	// Si todos tienen el mismo signo, es que el punto esta dentro:
	if    ( sign(da1)==sign(da2) &&
	        sign(da2)==sign(da3) &&
	        sign(da3)==sign(da4) &&
	        sign(da4)==sign(da1) )

		return true;
	else
		return false;

}

/*************************************************************************

                              STEP3_SpaceTransformer

     Transformador del espacio de obstaculos segun un PT determinado

*************************************************************************/
void CReactiveNavigationSystem::STEP3_SpaceTransformer(
    MRML::CPointsMap					&in_obstacles,
    CParameterizedTrajectoryGenerator	*in_PTG,
    vector_float						&out_TPObstacles,
    unsigned int						securityDistanceIndex
)
{
	int	debugPoint = 0;
	size_t      nRegistros,i=0;
	uint16_t    *distances;
	uint16_t    *ks;

	try
	{
		size_t Ki = in_PTG->getAlfaValuesCount();

		// Ver si hay espacio ya reservado en los TP-Obstacles:
		if ( out_TPObstacles.size() != Ki )
			out_TPObstacles.resize( Ki );

		// Coger "k"s y "distances" a las que choca cada punto de obstaculo
		//  de la "Rejilla" del PT dado.
		// --------------------------------------------------------------------
		size_t     nObs = in_obstacles.getPointsCount();
		debugPoint = 1;

		for (size_t k=0;k<Ki;k++)
		{
			// Iniciar a max. distancia o hasta que abs(phi)=pi
			out_TPObstacles[k] = in_PTG->refDistance;

			// Si acaba girado 180deg, acabar ahi:
			float phi = in_PTG->GetCPathPoint_phi(k,in_PTG->getPointsCountInCPath_k(k)-1);

			if (fabs(phi) >= M_PI* 0.95 )
				out_TPObstacles[k]= in_PTG->GetCPathPoint_d(k,in_PTG->getPointsCountInCPath_k(k)-1);
		}

		//float	minDistObs = 1e9f;
		for (size_t obs=0;obs<nObs;obs++)
		{
			debugPoint = 2;

			float ox,oy;
			in_obstacles.getPoint(obs, ox,oy);


			in_PTG->collisionGrids[securityDistanceIndex]->getTPObstacle(ox, oy, nRegistros, &distances, &ks);

			debugPoint = 3;

			for (i=0;i<nRegistros;i++)
				if (  distances[i]*0.001f < out_TPObstacles[ ks[i] ] )
					out_TPObstacles[ks[i]] = distances[i]*0.001f;

		}


		debugPoint = 4;

		// Distances in TP-Space are normalized to [0,1]:
		for (i=0;i<Ki;i++)
			out_TPObstacles[i] /= in_PTG->refDistance;

		debugPoint = 5;

	}
	catch (std::exception &e)
	{
		TextoDebug("[CReactiveNavigationSystem::STEP3_SpaceTransformer] Exception:");
		TextoDebug((char*)(e.what()));
	}
	catch (...)
	{
		std::cout << format("\n[CReactiveNavigationSystem::STEP3_SpaceTransformer] Unexpected exception DEBUGPOINT=%u!:\n",debugPoint);
		std::cout << format("securityDistanceIndex  = %u\n",securityDistanceIndex);
		std::cout << format("*in_PTG = %p\n", (void*)in_PTG );
		if (in_PTG)
			std::cout << format("PTG = %s\n",in_PTG->getDescription().c_str());
		std::cout << format("nRegistros = %u\n",(unsigned int)nRegistros);
		std::cout << format("i = %u\n",(unsigned int)i);
		std::cout << format("*distances = %p\n",(void*)distances);
		std::cout << format("*ks = %p\n",(void*)ks);
		std::cout << format("\n");
	}


}

/*************************************************************************

                             STEP4_HolonomicMethod

*************************************************************************/
void CReactiveNavigationSystem::STEP4_HolonomicMethod(
    vector_float						&in_Obstacles,
    MRML::CPoint2D						&in_Target,
    float								in_maxRobotSpeed,
    THolonomicMovement					&out_selectedMovement,
    CHolonomicLogFileRecord				**in_HLFR )
{
	try
	{
		holonomicMethod->navigate(	in_Target,
		                           in_Obstacles,
		                           in_maxRobotSpeed,
		                           out_selectedMovement.direction,
		                           out_selectedMovement.speed,
		                           in_HLFR );
	}
	catch (std::exception &e)
	{
		TextoDebug("[CReactiveNavigationSystem::STEP4_HolonomicMethod] Exception:");
		TextoDebug((char*)(e.what()));
	}
	catch (...)
	{
		TextoDebug("[CReactiveNavigationSystem::STEP4_HolonomicMethod] Unexpected exception!\n");
	}

}



/*************************************************************************

						 	STEP5_Evaluator

*************************************************************************/
void CReactiveNavigationSystem::STEP5_Evaluator(
    THolonomicMovement					&in_holonomicMovement,
    vector_float						&in_TPObstacles,
    MRML::CPoint2D						&WS_Target,
    MRML::CPoint2D						&TP_Target,
    bool								wasSelectedInLast,
    CLogFileRecord::TInfoPerPTG			&log,
    unsigned int						securityDistanceIndex)
{
	int		DEBUG_POINT = 0;

	try
	{
		float	a;
		float	factor1, factor2,factor3,factor4,factor5,factor6;

		if (TP_Target.x!=0 || TP_Target.y!=0)
			a = atan2( TP_Target.y, TP_Target.x);
		else	a = 0;

		DEBUG_POINT = 1;

		int		TargetSector = in_holonomicMovement.PTG->alfa2index( a );
		float	TargetDist = sqrt(TP_Target.norm());
		int		kDirection = in_holonomicMovement.PTG->alfa2index( in_holonomicMovement.direction );
		float	refDist	   = in_holonomicMovement.PTG->refDistance;

		DEBUG_POINT = 2;

		// Las coordenadas en C-Space representativas de la trajectoria seleccionada:
		float	x,y,phi,t,d;
		d = min( in_TPObstacles[ kDirection ], 0.90f*TargetDist);
		in_holonomicMovement.PTG->getCPointWhen_d_Is( d, kDirection,x,y,phi,t );

		DEBUG_POINT = 3;

		// Factor 1: Distancia hasta donde llego por esta GPT:
		// -----------------------------------------------------
		factor1 = in_TPObstacles[kDirection];

		//	if (kDirection == TargetSector )	// Si es TARGET_DIRECTLY:
		//			factor1 = 1 - TargetDist;				// Llego todo lo lejos q quiero ir!! :-)
		//	else	factor1 = in_TPObstacles[kDirection];

		DEBUG_POINT = 4;

		// Factor 2: Distancia en sectores:
		// -------------------------------------------
		float   dif = fabs(((float)( TargetSector - kDirection )));
		float	nSectors = (float)in_TPObstacles.size();
		if ( dif > (0.5f*nSectors)) dif = nSectors - dif;
		factor2= exp(-square( dif / (in_TPObstacles.size()/3.0f))) ;

		DEBUG_POINT = 5;

		// Factor 3: Angulo que hará el robot con target en (x,y):
		// -----------------------------------------------------
		float   t_ang = atan2( WS_Target.y - y, WS_Target.x - x );
		t_ang -= phi;

		while (t_ang> M_PI)  t_ang-=(float)M_2PI;
		while (t_ang<-M_PI)  t_ang+=(float)M_2PI;

		factor3 = exp(-square( t_ang / (float)(0.5f*M_PI)) );

		DEBUG_POINT = 5;

		// Factor4:		DECREMENTO de la distancia euclidea entre (x,y) y target:
		//  Se valora negativamente el alejarse del target
		// -----------------------------------------------------
		float dist_eucl_final = sqrt( square( x-WS_Target.x ) + square( y-WS_Target.y ) );
		float dist_eucl_now   = sqrt( square( WS_Target.x ) + square( WS_Target.y ) );

		//    float dist_eucl_final = sqrt( square( d*cos(in_holonomicMovement.direction)-TP_Target.x ) + square( d*sin(in_holonomicMovement.direction) -TP_Target.y ) );
		//	float dist_eucl_now   = sqrt( square( TP_Target.x ) + square( TP_Target.y ) );

		factor4 = min(2.0f*refDist,max(0.0f,((dist_eucl_now - dist_eucl_final)+refDist)))/(2*refDist);

		// ---------
		//	float decrementDistanc = dist_eucl_now - dist_eucl_final;
		//	if (dist_eucl_now>0)
		//			factor4 = min(1.0,min(refDist*2,max(0,decrementDistanc + refDist)) / dist_eucl_now);
		//	else	factor4 = 0;
		// ---------
		//	factor4 = min(2*refDist2,max(0,decrementDistanc + refDist2)) / (2*refDist2);
		//  factor4=  (refDist2 - min( refDist2, dist_eucl ) ) / refDist2;

		DEBUG_POINT = 6;

		// Factor5: Histeresis:
		// -----------------------------------------------------
		float	want_v,want_w;
		in_holonomicMovement.PTG->directionToMotionCommand( kDirection, want_v,want_w);

		float	likely_v = exp( -fabs(want_v-last_cmd_v)/0.10f );
		float	likely_w = exp( -fabs(want_w-last_cmd_w)/0.40f );

		factor5 = min( likely_v,likely_w );
		//factor5 = wasSelectedInLast ? 1:0;

		DEBUG_POINT = 7;

		// Factor6: Security distance !!
		// -----------------------------------------------------
		factor6 = ((float)securityDistanceIndex) / (securityDistances.size()-0.99f);

		DEBUG_POINT = 8;

		// --------------------
		//  SAVE LOG
		// --------------------
		log.evalFactors.resize(6);
		log.evalFactors[0] = factor1;
		log.evalFactors[1] = factor2;
		log.evalFactors[2] = factor3;
		log.evalFactors[3] = factor4;
		log.evalFactors[4] = factor5;
		log.evalFactors[5] = factor6;

		DEBUG_POINT = 9;

		if (in_holonomicMovement.speed==0)
		{
			// If no movement has been found -> the worst evaluation:
			in_holonomicMovement.evaluation = 0;
		}
		else
		{
			// Sum: Dos casos:
			if (dif<2	&&										// Heading the target
			        in_TPObstacles[kDirection]*0.95f>TargetDist &&	// and free space towards the target
			        in_holonomicMovement.securityDistance>0			// and some sec. distance employed!
			   )
			{
				// Caso de camino directo al target:
//				in_holonomicMovement.evaluation = 1.0f + (1 - TargetDist) + factor5 * weight5 + factor6*weight6;
				in_holonomicMovement.evaluation = 1.0f + (1 - t/15.0f) + factor5 * weights[4] + factor6*weights[5];
			}
			else
			{
				// Caso general:
				in_holonomicMovement.evaluation = (
				                                      factor1 * weights[0] +
				                                      factor2 * weights[1] +
				                                      factor3 * weights[2] +
				                                      factor4 * weights[3] +
				                                      factor5 * weights[4] +
				                                      factor6 * weights[5]
				                                  ) / ( MATH::sum(weights));
			}
		}

		DEBUG_POINT = 10;
	}
	catch (std::exception &e)
	{
		THROW_STACKED_EXCEPTION(e);
	}
	catch (...)
	{
		std::cout << "[CReactiveNavigationSystem::STEP5_Evaluator] Unexpected exception!:\n";
		std::cout << format("DEBUG_POINT = %u\n",DEBUG_POINT );
	}

}

/*************************************************************************

							STEP6_Selector

*************************************************************************/
void CReactiveNavigationSystem::STEP6_Selector(
    std::vector<THolonomicMovement>		&in_holonomicMovements,
    THolonomicMovement					&out_selected,
    int									&out_nSelectedPTG)
{
	// Si no encontramos nada mejor, es que no hay movimiento posible:
	out_selected.direction= 0;
	out_selected.speed = 0;
	out_selected.PTG = NULL;
	out_selected.evaluation= 0;		// Anotacion pa mi: Don't modify this 0 and the ">" comparison
	out_nSelectedPTG = 0;

	// Coger la trayectoria con mejor evaluacion, mientras no produzca
	//  colision:
	for (unsigned int i=0;i<in_holonomicMovements.size();i++)
	{
		float ev = in_holonomicMovements[i].evaluation;
		if ( ev > out_selected.evaluation )
		{
			out_selected = in_holonomicMovements[i];
			out_selected.evaluation = ev;
			out_nSelectedPTG = i;
		}
	}
}

/*************************************************************************

						STEP7_NonHolonomicMovement

*************************************************************************/
void CReactiveNavigationSystem::STEP7_NonHolonomicMovement(
    THolonomicMovement					&in_movement,
    float								&out_v,
    float								&out_w)
{
	try
	{
		if (in_movement.speed==0)
		{
			// The robot will stop:
			out_v = out_w = 0;
		}
		else
		{
			// Take the normalized movement command:
			in_movement.PTG->directionToMotionCommand(
			    in_movement.PTG->alfa2index( in_movement.direction ),
			    out_v,
			    out_w );

			// Scale holonomic speeds to real-world one:
			float	reduction = min(1.0f, in_movement.speed / in_movement.PTG->getMax_V_inTPSpace());

			// To scale:
			out_v*=reduction;
			out_w*=reduction;

			// Assure maximum speeds:
			if (fabs(out_v)>robotMax_V_mps)
			{
				// Scale:
				float F = fabs(robotMax_V_mps / out_v);
				out_v *= F;
				out_w *= F;
			}

			if (fabs(out_w)>DEG2RAD(robotMax_W_degps))
			{
				// Scale:
				float F = fabs((float)DEG2RAD(robotMax_W_degps) / out_w);
				out_v *= F;
				out_w *= F;
			}
		}
	}
	catch (std::exception &e)
	{
		TextoDebug("[CReactiveNavigationSystem::STEP7_NonHolonomicMovement] Exception:");
		TextoDebug((char*)(e.what()));
	}
	catch (...)
	{
		TextoDebug("[CReactiveNavigationSystem::STEP7_NonHolonomicMovement] Unexpected exception!\n");
	}
}


/*************************************************************************
	 		Destructor
*************************************************************************/
CReactiveNavigationSystem::~CReactiveNavigationSystem()
{
	CerrandoHilo = true;

	// Esperar a que termine la ejecucion actual, por si esta en otro hilo:
	critZoneNavigating.enter();
	critZoneNavigating.leave();

	// Por si acaso...
	if (RobotMotionControl.stop)  RobotMotionControl.stop();
	else    TextoDebug("\n[~CReactiveNavigationSystem] Error: Can not stop robot since functor is NULL!\n");

	if (logFile)
	{
		delete logFile;
		logFile = NULL;
	}


	// Free PTGs:
	for (size_t i=0;i<PTGs.size();i++)	delete PTGs[i];
	PTGs.clear();

	// Free holonomic method:
	if (holonomicMethod)
	{
		delete holonomicMethod;
		holonomicMethod = NULL;
	}


}




/*************************************************************************
			 Evaluar navegacion:
*************************************************************************/
float  CReactiveNavigationSystem::evaluate( TNavigationParams *params )
{
	return 0.5f;
}

/*************************************************************************
			 Iniciar navegacion:
*************************************************************************/
void  CReactiveNavigationSystem::navigate(CReactiveNavigationSystem::TNavigationParams *params )
{
	navigationEndEventSent = false;

	// Solo se puede llamar si estamos en IDLE:
//	if ( estadoNavegador != IDLE ) return;		// Para permitir cambiar target de navegacion

	// Copiar datos:
	navigationParams = *params;

	// Reset behavior:
	navigatorBehavior = beNormalNavigation;

	// Si se piden coordenadas relativas, transformar a absolutas:
	if ( navigationParams.targetIsRelative )
	{
		std::cout << format("TARGET COORDS. ARE RELATIVE!! -> Translating them...\n");
		// Obtener posicion actual:
		MRML::CPose2D		currentPose;
		float				velLineal_actual,velAngular_actual;

		if ( !RobotMotionControl.getCurrentPoseAndSpeeds /*Se ha rellenado el puntero?*/ ||
		        RobotMotionControl.getCurrentPoseAndSpeeds(currentPose, velLineal_actual,velAngular_actual) )
		{
			Error_ParadaDeEmergencia("\nERROR al llamar a InterfazCORBA::LeerPosicionVelocidades!! Parando robot y terminando navegacion\n");
			return;
		}

		MRML::CPoint2D	absTarget;
		absTarget = currentPose + navigationParams.target;
		navigationParams.target = absTarget;
		navigationParams.targetIsRelative=false;       // Ya no son relativas
	}

	// new state:
	navigationState = NAVIGATING;

	// Reset the bad navigation alarm:
	badNavAlarm_minDistTarget = 1e10f;
	badNavAlarm_lastMinDistTime = SystemUtils::getCurrentTime();
}

/*************************************************************************
        Cambiar params. de la navegacion actual
*************************************************************************/
void  CReactiveNavigationSystem::setParams( CReactiveNavigationSystem::TNavigationParams  *params )
{

}

/*************************************************************************
                Para la silla y muestra un mensaje de error.
*************************************************************************/
void CReactiveNavigationSystem::Error_ParadaDeEmergencia( const char *msg )
{
	// Mostrar mensaje y parar navegacion si estamos moviendonos:
	TextoDebug( msg );
	TextoDebug( "\n");

	if ( RobotMotionControl.stop() )
		RobotMotionControl.stop();
	else    TextoDebug("[CReactiveNavigationSystem::Error_ParadaDeEmergencia] Can not stop robot due to NULL functor!!\n\n");

	navigationState = NAV_ERROR;
	return;
}


/* -------------------------------------------------------------
   ------------------------------------------------------------- */
void  CReactiveNavigationSystem::CDynamicWindow::findMinMaxCurvatures(float &minCurv, float &maxCurv)
{
	if (fabs(v_min)<0.005f) v_min=0.005f*sign(v_min);
	if (fabs(v_max)<0.005f) v_max=0.005f*sign(v_max);

	// Compute the curvature for the 4 corners:
	c1 = w_max / v_min;
	c2 = w_min / v_min;
	c3 = w_max / v_max;
	c4 = w_min / v_max;

	minCurv = min( min(c1,c2),min(c3,c4) );
	maxCurv = max( max(c1,c2),max(c3,c4) );
}


/* -------------------------------------------------------------
   ------------------------------------------------------------- */
void  CReactiveNavigationSystem::CDynamicWindow::findBestApproximation(float desV,float desW, float &outV,float &outW)
{
	// Try to find a "cut", if not, find just the closest corner.
	if (findClosestCut(desV,desW,outV,outW))
		return;

	float closestX,closestY;

	float	d[4];
	d[0] = GEO::minimumDistanceFromPointToSegment(v_min,w_max, 0,0,desV,desW,closestX,closestY);
	d[1] = GEO::minimumDistanceFromPointToSegment(v_min,w_min, 0,0,desV,desW,closestX,closestY);
	d[2] = GEO::minimumDistanceFromPointToSegment(v_max,w_max, 0,0,desV,desW,closestX,closestY);
	d[3] = GEO::minimumDistanceFromPointToSegment(v_max,w_min, 0,0,desV,desW,closestX,closestY);

	float	d_min=1e6;
	int		idx_min=-1,i;
	for (i=0;i<4;i++)
		if (d[i]<d_min)
		{
			d_min = d[i];
			idx_min = i;
		}

	switch (idx_min)
	{
	case 0:
		outV = v_min;
		outW = w_max;
		break;
	case 1:
		outV = v_min;
		outW = w_min;
		break;
	case 2:
		outV = v_max;
		outW = w_max;
		break;
	case 3:
		outV = v_max;
		outW = w_min;
		break;
	};
}


/* -------------------------------------------------------------
   ------------------------------------------------------------- */
bool  CReactiveNavigationSystem::CDynamicWindow::findClosestCut( float cmd_v, float cmd_w,	// IN
        float &out_v,float &out_w)	// OUT
{
	if (fabs(cmd_v)<0.005f) cmd_v = 0.005f * sign(cmd_v);
	//float desiredCurv	= cmd_w / cmd_v;

	// Find the 1..4 cuts:
	vector_float	vs,ws;
	vs.reserve(4);
	ws.reserve(4);
	float			v,w;

	if ( GEO::SegmentsIntersection( v_min,w_min,v_min,w_max, 0,0,cmd_v,cmd_w , v,w) )
	{
		vs.push_back(v);
		ws.push_back(w);
	}
	if ( GEO::SegmentsIntersection( v_min,w_max,v_max,w_max, 0,0,cmd_v,cmd_w , v,w) )
	{
		vs.push_back(v);
		ws.push_back(w);
	}
	if ( GEO::SegmentsIntersection( v_max,w_max,v_max,w_min, 0,0,cmd_v,cmd_w , v,w) )
	{
		vs.push_back(v);
		ws.push_back(w);
	}
	if (GEO::SegmentsIntersection( v_min,w_min,v_max,w_min, 0,0,cmd_v,cmd_w , v,w) )
	{
		vs.push_back(v);
		ws.push_back(w);
	}

	// Any cut??
	if (!vs.size()) return false;

	// Find closest cut:
	float	d_min=1e6;
	int		idx_min=-1;
	for (unsigned int i=0;i<vs.size();i++)
	{
		float d = GEO::distanceBetweenPoints( cmd_v,cmd_w, vs[i],ws[i] );
		if (d<d_min)
		{
			d_min = d;
			idx_min = i;
		}
	}

	ASSERT_(idx_min>=0 && idx_min<(int)vs.size())

	// Returns the closes cut point:
	out_v = vs[idx_min];
	out_w = ws[idx_min];

	return true;
}




