/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CAbstractReactiveNavigationSystem_H
#define CAbstractReactiveNavigationSystem_H

#include <MRPT/MRML/mrml.h>
using namespace MRML;

#include <stdarg.h>

namespace MRPTAPPS
{
  namespace RNAV
  {

	/** This is the base class for any reactive navigation system. Here is defined
	 *   the interface that users will use with derived classes where algorithms are really implemented.
	 *
	 * Changes history:
	 *		- 30/JUN/2004: Creation (JLBC)
     *		- 16/SEP/2004: Totally redesigned.
	 *		- 15/SEP/2005: Totally rewritten again, for integration into MRPT Applications Repository.
	 */
	class  CAbstractReactiveNavigationSystem
	{
	public:
		struct TNavigationParams;
		struct TRobotMotionControl;
		struct TSensors;
		struct TDebug;
		struct TEventsLaunching;

		/** Constructor
		  */
		CAbstractReactiveNavigationSystem(
			TRobotMotionControl		&rmc,
			TSensors				&sensors,
			void	(*emul_printf)(const char *s),
			TEventsLaunching		&evnts);

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

		/** Cancel current navegacion.
		 */
		void cancel();

		/** Continues with suspended navigation.
		 * \sa suspend
		 */
		void resume();

		/** Evaluates the practicability of a navigation for given parameters:
		 * \returns An estimation in the range [0,1], for 0 being imposible and 1 being easy.
		 */
		virtual float  evaluate( TNavigationParams *params )=0;

		/** This method must be called periodically in order to effectively run the navigation.
		 */
		void navigationStep();

		/** Navigation request. It starts a new navigation.
		 */
		virtual void  navigate( TNavigationParams *params )=0;

		/** Changes the parameters for current navigation
		 */
		virtual void  setParams( TNavigationParams *params)=0;

		/** Suspend current navegation
		 * \sa resume
		 */
		virtual void  suspend();

		/** The struct for configuring the navigation request.
		 */
		struct TNavigationParams
		{
			/** Coordinates of desired target location.
			 */
			CPoint2D		target;

			/** The allowed distance from target in order to end the navigation.
			 */
			float           targetAllowedDistance;

			/** The allowed distance from target in order to end the navigation.
			 */
			bool            targetIsRelative;
		};

		/** This struct is used as a placeholder for fuctors related to robot movement.
		 */
		struct TRobotMotionControl	// Old "InterfazCORBA"
		{
			TRobotMotionControl()
			{
				changeSpeeds = NULL;
				stop = NULL;
				startWatchdog = NULL;
				stopWatchdog = NULL;
				getCurrentPoseAndSpeeds = NULL;
			}

			/** This wrapper function is used to get the current pose and speeds of the robot.
			 *   \param curPose Current robot pose.
			 *   \param curV Current linear speed, in meters per second.
			 *	 \param curW Current angular speed, in radians per second.
			 */
			bool (*getCurrentPoseAndSpeeds)( MRML::CPose2D &curPose, float &curV, float &curW);

			/** This wrapper function is used to change the instantaneous speeds of robot.
			 *   \param v Linear speed, in meters per second.
			 *	 \param w Angular speed, in radians per second.
			 */
			bool (*changeSpeeds)( float v, float w );
			/** This wrapper function is used for stop the robot definitely, until a new speed is set.
			 */
			bool (*stop)();
			/** This wrapper function is used for starting the watchdog timer module.
			 * \param T_ms Period, in ms.
			 */
			bool (*startWatchdog)(float T_ms);
			/** This wrapper function is used for stoping the watchdog timer module.
			 */
			bool (*stopWatchdog)();
		};

		/** This struct is used as a placeholder for fuctors related to sensing.
		 */
		struct TSensors
		{
			TSensors()
			{
				sense = NULL;
			}

			/** This wrapper function must return all currently sensed obstacles,
			  *  as a map of points. It can be used direct methods from laser range scans, for example.
			  */
			bool (*sense)(
            	MRML::CSimplePointsMap 		&obstacles,
            	MRML::COccupancyGridMap2D 	*obstaclesGridMap );
		};

		/** This struct is used as a placeholder for functors related to robot movement.
		 */
        struct TDebug
        {
            TDebug()
			{
				emul_printf=NULL;
				debug_output = MRPT_OS::fopen("reactive_nav_debug_output.txt","wt");
			};
			~TDebug()
			{
				fclose(debug_output);
	            debug_output = NULL;
			}

            // Usado por el navegador:
            void printf(const char *s)
            {
            	if (emul_printf)
					emul_printf(s);
                if( debug_output )
                {
                	fprintf(debug_output,s);
                }
            }

            /** Place wrappers if debug console output is desired.
			 */
            void (*emul_printf)(const char *s);

			private:
				FILE * debug_output;
        };


		/** This struct is used as a placeholder for fuctors related to launching events
		 */
		struct TEventsLaunching
		{
			TEventsLaunching() :	sendNavigationStartEvent(NULL),
									sendNavigationEndEvent(NULL),
									sendNavigationEndDueToErrorEvent(NULL),
									sendWaySeemsBlockedEvent(NULL)
			{
			}

			void (*sendNavigationStartEvent) ();
			void (*sendNavigationEndEvent) ();
			void (*sendNavigationEndDueToErrorEvent) ();
			void (*sendWaySeemsBlockedEvent) ();
		};


		/** The different states for the navigation system.
		 */
		enum TState
		{
			IDLE=0,
			NAVIGATING,
			SUSPENDED,
			NAV_ERROR
		};

		/** Returns the current navigator state.
		 */
		TState getCurrentState() { return navigationState; }

	private:
		/** Last internal state of navigator:
		 */
		TState		lastNavigationState;

	protected:
		/** To be implemented in derived classes
		  */
		virtual void  performNavigationStep( )=0;

		/** Current internal state of navigator:
		 */
		TState		navigationState;

		/** Current navigation parameters:
		 */
		TNavigationParams	navigationParams;

		/** Wrappers
		  */
		TRobotMotionControl		RobotMotionControl;
		TSensors				Sensors;
		TDebug					Debug;
		TEventsLaunching		EventsLaunching;

		void TextoDebug( const char *formatStr, ...)
		{
			char	auxStr[2001];
			va_list		ap;
			va_start(ap, formatStr);
			MRPT_OS::vsprintf(auxStr,2000,formatStr,ap);
			va_end (ap);
			Debug.printf( auxStr );
		}


	};
  }
}


#endif

