/// @file sensor_behaviors.hh
/// @author Joshua Petitt <petitj01@tartarus.uwa.edu.au>
/// @author Stefan Schmitt <sschmitt@ee.uwa.edu.au>
/// @version 1.0
/// @date 2003-07
///  
/// Declares sensing behaviors.

#ifndef SENSOR_BEHAVIORS_HH
#define SENSOR_BEHAVIORS_HH

#include "behavior/behavior.hh"
#include "behavior/linktypes.hh"
#include "behavior/stdc_linktypes.hh"
#include "behavior/control_behaviors.hh"
#include "input/eyebot_sensors.hh"
#include "output/eyebot_actors.hh"


namespace EyeMind {

#if 0
/// Measures the distances in left, front, right directions.
/// @arg Input: SignalLink
/// @arg Output: DistancesLink (all)
class FeelPsds : public Behavior
{
private:
	Psd& psdl; ///< Left PSD.
	Psd& psdf; ///< Front PSD.
	Psd& psdr; ///< Right PSD.
	Psd& psdb; ///< Back PSD.
	DistancesType dist; ///< Measured distances.

protected:
	virtual void Feed(SignalLink& l);
	virtual int Execute();

public:
	/// Default constructor; registers with mind.
	FeelPsds();

};

FeelPsds& operator<<(DistanceLink& l, FeelPsds& i);
DistanceLink& operator>>(FeelPsds& n, DistanceLink& o);

#endif

/// Measure the distance in front and back psd
/// @arg Input: SignalLink
/// @arg Output: DistancesLink (all)
class FeelFrontBackPsds : public Behavior
{
private:
	Psd& psdf; ///< Front PSD.
	Psd& psdb; ///< Back PSD.
	DistancesType dist; ///< Measured distances.

protected:
	virtual void Feed(SignalLink& l);
	virtual int Execute();

public:
	/// Default constructor; registers with mind.
	FeelFrontBackPsds();
};

FeelFrontBackPsds& operator<<(DistanceLink& l, FeelFrontBackPsds& i);
DistanceLink& operator>>(FeelFrontBackPsds& n, DistanceLink& o);


/// Measure the distance in front psd
/// @arg Input: SignalLink
/// @arg Output: DistancesLink (all)
class FeelLeftRightPsds : public Behavior
{
private:
	Psd& psdl; ///< Left PSD.
	Psd& psdr; ///< Right PSD.
	DistancesType dist; ///< Measured distances.

protected:
	virtual void Feed(SignalLink& l);
	virtual int Execute();

public:
	/// Default constructor; registers with mind.
	FeelLeftRightPsds();
};

FeelLeftRightPsds& operator<<(DistanceLink& l, FeelLeftRightPsds& i);
DistanceLink& operator>>(FeelLeftRightPsds& n, DistanceLink& o);



#if 0
/// Abstract meta class; measures the agent's position.
/// Input: SignalLink
/// Output: PositionLink (all)

class Odometry : public Behavior
{
protected:
	PositionType pos; /// Calculated position.

public:
	/// Default constructor; registers with Mind.
	Odometry();

	/// Set current position.
	void Position(float x, float y, float q);

	/// Set current angle, leave x,y.
	void Angle(float q);

	/// Zero position.
	void Reset();
};


Odometry& operator<<(PositionLink& l, Odometry& i);
PositionLink& operator>>(Odometry& n, PositionLink& o);

/// Odometry implemented for a two-wheeled agent.
/// @arg Input: SignaLink
/// @arg Output: PositionLink (all)

class OdometryTwoWheeled : public Odometry
{
private:
	DCMotor& motor_left;  ///< .
	DCMotor& motor_right; ///< .
	float Q_left;       ///< Measured motor position (rad).
	float prev_Q_left;  ///< Previous measured motor position (rad).
	float Q_right;      ///< Measured motor position (rad).
	float prev_Q_right; ///< Previous measured motor position (rad).

	float radius; ///< Radius of the wheels.
	float width;  ///< Width of the robot.

protected:
	/// Odometry calculation function
	virtual int Execute();
	virtual void Feed(SignalLink& l);

public:
	/// Default constructor; connects motors to id.
	OdometryTwoWheeled();
};
#endif

/// Detect if a robot is not moving (stalled).
/// Input: SpeedLink (direction the robot is supposed to be moving - linked from VWmove)
/// Output: SpeedLink
///
/// Output signal excitation is HI_MAX if a stall is detected.
/// If a stall has been detected, and the robot is not able
/// to recover after 1 time-unit, excitation is EXT (extreme).
/// Otherwise, excitation is LOW_MIN.
///
/// Output Speed is opposite to the input speed at the time the
/// stall is detected. Link output to VWMove to recover from the stall.
class DetectStall : public Behavior
{
private:
	SpeedType new_speed; 		/// Expected speed
	SpeedType original_speed; 	/// Desired speed at the time a stall began.

	unsigned t;			/// Time-units since stall began
	unsigned time_limit;
	float K;			/// Gain to increase velocity over time

	MotorControlLeft& left_controller; ///< .
	MotorControlRight& right_controller; ///< .

protected:
	virtual int Execute();
	virtual void Feed(SignalLink& l);

public:
	DetectStall();
};

DetectStall& operator>>(SpeedLink& l, DetectStall& o);
DetectStall& operator<<(SpeedLink& l, DetectStall& i);
SpeedLink& operator>>(DetectStall& n, SpeedLink& o);
SpeedLink& operator<<(DetectStall& n, SpeedLink& i);

#if 0
/// Monitor or "feel" the input keypad.
/// @arg Input: SignalLink
/// @arg Output: SignalLink
/// @bug This should output a key value on an appropriate outlink
/// @bug Implemented in header file
/// @bug Ill-designed, so not documented.

class FeelKeys : public Behavior
{
private:
	Keypad& keys;

public:
	FeelKeys() : Behavior(), keys(Keypad::I()){}

	int Execute(){ return keys.Update(); }
};
#endif


/// Monitors the compass.
/// Input: SignalLink
/// Output: SignalLink
/// @bug Should output its value on a PolarLink
/// @bug Implemented in header file
/// @bug Ill-designed, so not documented.

class FeelCompass : public Behavior
{
private:
	Compass& compass;

public:
	FeelCompass() : Behavior(), compass(Compass::I()){}

	int Execute()
	{
		compass.Update();
		return 1;
	}
	
	int Angle(){ return compass.Angle(); }
};


/// A Behavior for listening to the RadioComm
/// Input: SignalLink
/// Output: StringLink

class ListenRadio : public Behavior
{
friend CharArrayLink& operator>>(ListenRadio& n, CharArrayLink& o);
friend ListenRadio& operator<<(CharArrayLink& l, ListenRadio& i);

private:
	RadioComm& radio;

protected:
	virtual int Execute();

public:
	ListenRadio() : Behavior(), radio(RadioComm::I()){}
};

CharArrayLink& operator>>(ListenRadio& n, CharArrayLink& o);
ListenRadio& operator<<(CharArrayLink& l, ListenRadio& i);


/// A Behavior for listening to the Infrared TV remote
/// Input: SignalLink
/// Output: IntLink - Current button press

class ListenIRtv : public Behavior
{
private:
	IRtv& ir;

protected:
	virtual int Execute();
	virtual void Feed(SignalLink& l);

public:
	ListenIRtv() : Behavior(), ir(IRtv::I()){}
};

IntLink& operator>>(ListenIRtv& n, IntLink& o);
ListenIRtv& operator<<(IntLink& l, ListenIRtv& i);


}; // namespace EyeMind

#endif // SENSOR_BEHAVIORS_HH