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

#include "CAbstractReactiveNavigationSystem.h"

using namespace MRPTAPPS::RNAV;


/*---------------------------------------------------------------
							Constructor
  ---------------------------------------------------------------*/
CAbstractReactiveNavigationSystem::CAbstractReactiveNavigationSystem(
			CAbstractReactiveNavigationSystem::TRobotMotionControl		&rmc,
			CAbstractReactiveNavigationSystem::TSensors				&sensors,
			void	(*emul_printf)(const char *s),
			CAbstractReactiveNavigationSystem::TEventsLaunching		&evnts)
{
	// Estado inicial:
	navigationState = lastNavigationState = IDLE;

	this->RobotMotionControl	= rmc;
	this->Sensors				= sensors;
	this->Debug.emul_printf		= emul_printf;
	this->EventsLaunching		= evnts;
}

/*---------------------------------------------------------------
							Cancel
  ---------------------------------------------------------------*/
void CAbstractReactiveNavigationSystem::cancel()
{
	Debug.printf("\n[CAbstractReactiveNavigationSystem::Cancel()]\n");
	navigationState = IDLE;
}


/*---------------------------------------------------------------
							Continue
  ---------------------------------------------------------------*/
void CAbstractReactiveNavigationSystem::resume()
{
	Debug.printf("\n[CAbstractReactiveNavigationSystem::Continue()]\n");
	if ( navigationState == SUSPENDED )
		navigationState = NAVIGATING;
}


/*---------------------------------------------------------------
							Continue
  ---------------------------------------------------------------*/
void  CAbstractReactiveNavigationSystem::suspend()
{
	Debug.printf("\n[CAbstractReactiveNavigationSystem::Suspend()]\n");
	if ( navigationState == NAVIGATING )
		navigationState  = SUSPENDED;
}

/*---------------------------------------------------------------
					NavigateStep

	  Se debe llamar continuamente, cada pocos milisegundos. Internamente
	   lleva el mismo el control del tiempo que pasa entre llamadas para
	   tener el cuenta el tiempo real.
  ---------------------------------------------------------------*/
void CAbstractReactiveNavigationSystem::navigationStep()
{
	TState	startingState = navigationState;
	switch ( navigationState )
	{
	//------------------------------------------------------
	//					PARAR ROBOT
	//------------------------------------------------------
	case IDLE:
	case SUSPENDED:
		try
		{
			// Si acabamos de llegar a este estado, parar el robot:
			if ( lastNavigationState == NAVIGATING )
			{
				Debug.printf("\n[CAbstractReactiveNavigationSystem::navigationStep()] Stoping Navigation\n");
				RobotMotionControl.stop();
				RobotMotionControl.stopWatchdog();
			}
		} catch (...) { }
		break;

	//------------------------------------------------------
	//					FINALIZACION POR ERROR
	//------------------------------------------------------
	case NAV_ERROR:
		try
		{
			// Enviar evento de final de navegacion??
			if ( lastNavigationState == NAVIGATING && navigationState == NAV_ERROR)
				if (EventsLaunching.sendNavigationEndDueToErrorEvent)
						EventsLaunching.sendNavigationEndDueToErrorEvent();

			// Si acabamos de llegar a este estado, parar el robot:
			if ( lastNavigationState == NAVIGATING )
			{
				Debug.printf("\n[CAbstractReactiveNavigationSystem::navigationStep()] Stoping Navigation due to an error!!\n");
				RobotMotionControl.stop();
				RobotMotionControl.stopWatchdog();
			}
		} catch (...) { }
		break;

	//------------------------------------------------------------------
	//						ALGORITMO DE NAVEGACION
	//------------------------------------------------------------------
	case NAVIGATING:
		try
		{
			// Si acabamos de llegar a este estado, parar el robot:
			if ( lastNavigationState != NAVIGATING )
			{
				TextoDebug("\n[CAbstractReactiveNavigationSystem::navigationStep()] Starting Navigation. Watchdog initiated...\n");
				TextoDebug(" TARGET = (%.03f,%.03f)\n", navigationParams.target.x, navigationParams.target.y );
				RobotMotionControl.startWatchdog( 1000 );	// Watchdog = 1 seg
			}

			// Start navigation??
			if ( lastNavigationState == IDLE )
			{
				if (EventsLaunching.sendNavigationStartEvent)
						EventsLaunching.sendNavigationStartEvent();
			}

			// The normal execution of the navigation: Execute one step
			performNavigationStep();

		}
		catch (std::exception &e)
		{
			printf(e.what());
			printf("[CAbstractReactiveNavigationSystem::navigationStep] Exceptions!!\n");
		}
		catch (...)
		{
			printf("[CAbstractReactiveNavigationSystem::navigationStep] Unexpected exception!!\n");
		}
		break;	// Fin de case NAVIGATING
	};
	lastNavigationState = startingState;
}
