/// @file motion_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 behaviors for moving an agent.

#ifndef MOTION_BEHAVIORS_HH
#define MOTION_BEHAVIORS_HH

#include "behavior/behavior.hh"
#include "behavior/linktypes.hh"
#include "behavior/control_behaviors.hh"
#include "input/eyebot_sensors.hh"
#include "output/eyebot_actors.hh"
#include <cstring>

namespace EyeMind {

/// Speed arbitration behavior.
/// @arg Input: SpeedLink (range)
/// @arg Output: SpeedLink (all)
class VWMove : public Behavior
{
private:
	SpeedType speed; ///< Calculated speed.

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

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

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



/// Abstract meta class for driving the agent.
/// When deriving from this class, your derived class should instantiate (when
/// constructing) a subtree of all behaviors necessary for this task.
/// @arg Input: SpeedLink (last)
/// @arg Output: not defined
class VWDrive : public Behavior
{
friend VWDrive& operator>>(SpeedLink& l, VWDrive& o);
friend SpeedLink& operator<<(VWDrive& n, SpeedLink& i);
protected:
	SpeedLink* speed; ///< Input SpeedLink.

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


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



/// Drives a two-wheeled, differential drive agent.
/// @arg Input: SpeedLink (range)
/// @arg Output: FloatLink (2x, custom)
class VWDriveTwoWheeled : public VWDrive
{
private:
	FloatLink left;  ///< To the left MotorControl.
	FloatLink right; ///< To the right MotorControl.
	MotorControlLeft& left_controller; ///< .
	MotorControlRight& right_controller; ///< .

	float w_l; ///< Angular velocity for left wheel.
	float w_r; ///< Angular velocity for right wheel.

	float radius; ///< radius of the wheels
	float width;  ///< width of the robot
protected:
	virtual int Execute();

public:
	/// Default constructor; instantiates behavior subtree.
	VWDriveTwoWheeled();
};



/// VWMotion Behavior
/// @arg Input: SignalLink
/// @arg Output: SpeedLink (all)
class VWMotion : public Behavior
{
private:
	SpeedType speed; ///< Calculated speed.

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

public:
	/// Default constructor; initializes data members.
	VWMotion();
	VWMotion(int);
};

SpeedLink& operator>>(VWMotion& n, SpeedLink& o);



/// Creates a "random" motion pattern.
/// Periodically drives straight and turns. Alternates
/// between left and right turning.
/// @arg Input: SignalLink
/// @arg Output: SpeedLink (all)

class Wander : public Behavior
{
private:
	SpeedType speed; ///< Calculated speed.
	unsigned count;  ///< Excite counter.
	unsigned period; ///< Period for pattern change.
	float v, w;
	float sign;      ///< Pattern sign buffer.

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

public:
	/// Default constructor; initializes data members.
	Wander();

	/// @name Attribute manipulation functions
	//@{
	void Period(unsigned p){ period = p; } ///< Set pattern period.
	//@}
};


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


}; // namespace EyeMind

#endif //MOTION_BEHAVIORS_HH