/// @file computing_behaviors.hh
/// @author Jarrod Bassan <bassan@iinet.com.au>
/// @author Stefan Schmitt <sschmitt@ee.uwa.edu.au>
/// @version 1.0
/// @date 2003-07
///  
/// Declares computing behaviors. Computing behaviors calculate from their
/// inputs and inlinks arbitrary values for their outlinks.

#ifndef COMPUTING_BEHAVIORS_HH
#define COMPUTING_BEHAVIORS_HH

#include "behavior/behavior.hh"
#include "behavior/sensor_behaviors.hh"
#include "behavior/linktypes.hh"
#include <cstring>

namespace EyeMind {

/// Abstract meta class; calculates a speed from all distances. This is a
/// meta-behavior from that can derive behaviors like obstacle avoidance or
/// wall-following.
/// @arg Input: DistanceLink (n/a)
/// @arg Output: SpeedLink (n/a)

class DistToSpeed : public Behavior
{
friend DistToSpeed& operator>>(DistanceLink& l, DistToSpeed& o);
friend DistToSpeed& operator<<(SpeedLink& l, DistToSpeed& i);
friend SpeedLink& operator>>(DistToSpeed& n, SpeedLink& o);
friend DistanceLink& operator<<(DistToSpeed& n, DistanceLink& i);
private:
protected:
	SpeedType speed; ///< Calculated spped.
	DistanceLink* distancelink; ///< Input link buffer.

	/// Feeds all output links with calculated speed.
	virtual void Feed(SignalLink &l);

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

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



/// Basic obstacle avoidance.
/// @arg Input: DistanceLink (last)
/// @arg Output: SpeedLink (all)
class AvoidObstacles : public DistToSpeed
{
protected:
	/// @name Various parameters, see Execute()
	//@{
	unsigned d_side_avoid;
	unsigned d_front_close;
	unsigned d_back_close;
	float w_avoid_high;
	float w_avoid_low;
	float v_avoid_high;
	float v_avoid_low;
	//@}
	int t;
	int time_filter;
	int recent;
	int grace;

	virtual int Execute();

public:
	/// Default constructor; chooses default values for parameters.
	AvoidObstacles();

	/// Set avoidance parameters, see Execeute().
	//void Speed_w(float w_hi, float w_lo) { w_avoid_high = w_hi; w_avoid_low = w_lo; }
	//void Speed_v(float v_hi, float v_lo) { v_avoid_high = v_hi; v_avoid_low = v_lo; }
};


#if 0
/// Converts a PositionLink to a PolarLink
/// @arg Input: PositionLink (highest)
/// @arg Output: PolarLink (all)

class PositionToPolar : public Behavior
{
private:
protected:
	PolarType coord; ///< Output value.

	virtual void Feed(SignalLink &l);

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

PositionToPolar& operator>>(PositionLink& l, PositionToPolar& o);
PositionToPolar& operator<<(PolarLink& l, PositionToPolar& i);
PolarLink& operator>>(PositionToPolar& n, PolarLink& o);
PositionLink& operator<<(PositionToPolar& n, PositionLink& i);
#endif

#if 0

/// Calculates an angular momentum from a position link.
/// The angular momentum is the difference in phi of successive positions.
/// @arg Input: PositionLink (highest)
/// @arg Output: PolarLink (all)
class AngularMomentum : public PositionToPolar
{
private:
protected:
	PolarType prev_coord; /// Previous momentum.

	virtual int Execute();
public:
	/// Default constructor; initializes data members.
	AngularMomentum();
};
#endif

#if 0
/// Converts a PolarLink to a SpeedLink.
/// @arg Input: PolarLink (highest)
/// @arg Output: SpeedLink (all)
class PolarToSpeed : public Behavior
{
private:
protected:
	SpeedType speed; ///< Output value.

	void Feed(SignalLink &l);

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


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



/// Converts a PolarLink to a SpeedLink with a proportional gain.
/// Useful for turning the agent.
/// @arg Input: PolarLink (highest)
/// @arg Output: SpeedLink (all)
class TurnCommand : public PolarToSpeed
{
private:
	float Kr; ///< v/r gain.
	float Kq; ///< w/phi gain.
	Compass& compass;
	int desired_angle;
	float dband_compass;

protected:
	virtual int Execute();

public:
	/// Default constructor; initializes data members.
	TurnCommand();
};
#endif

}; // namespace EyeMind

#endif // COMPUTING_BEHAVIORS_HH