/*--------------------------------------------------------------------
 * project ....: Darpa Urban Challenge 2007
 * authors ....: Team AnnieWAY
 * organization: Transregional Collaborative Research Center 28 /
 *               Cognitive Automobiles
 * creation ...: xx/xx/2007
 * revisions ..: $Id:$
---------------------------------------------------------------------*/
#ifndef AW_STBASE_HPP
#define AW_STBASE_HPP

#include <string>
#include <iostream>
#include <boost/shared_ptr.hpp>

#include "aw_ChsmPlanner.hpp"

namespace vlr {

enum TransitionEnum {
	TRANSIT_DEFAULT,
	TRANSIT_TO_GOAL,
	TRANSIT_TO_KTURN,
	TRANSIT_TO_INTERSECTION,
	TRANSIT_TO_ZONE,
	TRANSIT_TO_LANECHANGE,
  TRANSIT_TO_STOPLINE,
  TRANSIT_TO_TRAFFIC_LIGHT,
  TRANSIT_TO_CROSSWALK
};

//! Base for all states.
/*! Using the curiously recurring template pattern to get the ChsmPlanner context in this base class.
 */
template < class DerivedState >
class StBase
{
public:
	// on enter
	StBase(const std::string& n) : recovery_time(RECOVERY_TIMEOUT), name_(n) {
		// entry
		ChsmPlanner& planner = static_cast< DerivedState* >(this)->template context<ChsmPlanner>();
		planner.addMessage("Entering " + name());
		std::cout << "[CHSM] Entering " << name() << std::endl;
		clearRecoveryIndicator();
		savedTransitionCounter = planner.transitionCounter;
	};
	// on exit
	virtual ~StBase(){
		ChsmPlanner& planner = static_cast< DerivedState* >(this)->template context<ChsmPlanner>();
		planner.addMessage("Exiting " + name());
		std::cout << "[CHSM] Exiting " << name() << std::endl;
		//assert(savedTransitionCounter != planner.transitionCounter && "you have to call detectedErrornousTransitions() in EVERY react() method");
	};
	// returns the name of this state
	std::string name() const { return name_; };

	bool checkRecovery() {
	  // measure progress
	  ChsmPlanner& planner = static_cast< DerivedState* >(this)->template context<ChsmPlanner>();
	  if (!planner.params().enable_recovery) {
	  	clearRecoveryIndicator();
	  	return false;
	  }
	  if (planner.distance(recovery_last_pose_) > RECOVERY_PROGRESS_INDICATOR) {
	  	clearRecoveryIndicator();
	  	return false;
	  }
	  // check expiration
	  Timestamp now;
	  now.now();
	  //std::cout << "recovery_timeout for " << name() << " is " << recovery_timeout << std::endl;
	  return now > recovery_timeout;
	}
	virtual void setRecoveryTime(double time) {
		recovery_time = time;
		clearRecoveryIndicator();
	}
	//! resets the recovery indicator so that recovery timer starts over again
	/*! use with care because it can easyly countermeasure recover mode if called too often */
	void clearRecoveryIndicator() {
		recovery_timeout.now();
		recovery_timeout += recovery_time;
		recovery_last_pose_ = static_cast< DerivedState* >(this)->template context< ChsmPlanner>().currentPose(); // TODO: causes mutex lock
	}
protected:
	typedef StBase kogmo_base;
	bool isExpired(const Timestamp& t) {
		Timestamp now;
		now.now();
		return now > t;
	}

	bool detectedErrornousTransitions() {

		ChsmPlanner& planner = static_cast< DerivedState* >(this)->template context< ChsmPlanner>();

		//std::cout << "HELLLLLLLLO OO OOOOOOOOO: " << planner.transitionCounter << std::endl;
		planner.transitionCounter = planner.transitionCounter+1;
		if (planner.transitionCounter > 16) { // doesn't work, don"t know why
			planner.addMessage("errournous transitions detected");
			return true;
		} else {
			return false;
		}

	}
  Timestamp recovery_timeout;
  Pose recovery_last_pose_;
  double recovery_time;
private:
	std::string name_;
	uint savedTransitionCounter;
};

} // namespace vlr

#endif // AW_STBASE_HPP