/// @file control_behaviors.hh
/// @author Stefan Schmitt <sschmitt@ee.uwa.edu.au>
/// @version 1.0
/// @date 2003-07
///  
/// Declares behavior for controlling the actors.

#ifndef CONTROL_BEHAVIORS_HH
#define CONTROL_BEHAVIORS_HH

#include "behavior/behavior.hh"
#include "behavior/stdc_linktypes.hh"
#include "output/eyebot_actors.hh"
#include "input/eyebot_sensors.hh"
#include "mind/mind.hh"
#include "eyebot_behavior_types.h"

namespace EyeMind {

/// Motor velocity controller (PID).
/// @arg Input: SpeedLink (last)
/// @arg Output: FloatLink (all)

class MotorControl : public Behavior
{
friend MotorControl& operator>>(FloatLink& l, MotorControl& o);
friend FloatLink& operator<<(MotorControl& n, FloatLink& i);
private:
	DCMotor& motor; ///< Motor to be controlled.

	// the symbols for the control values are taken from a German DIN
	// standard, one might want to change this.
	float y_min; ///< Min controller output.
	float y_max; ///< Max controller output.

	float w_min; ///< Max desired value.
	float w_max; ///< Min desired value.

	float x_d[3]; ///< Controller input and previous values.
	float y[2];   ///< Controller output and previous values.

	float K_P; ///< Proportional gain.
	float T_N; ///< Reset time.
	float T_V; ///< Rate time.

	unsigned time_stalled; ///< count number of time-units that velocity has been 0.0
	unsigned time_limit; ///< maximum number of time-units before we declare that we are stalled.
	float dband;

protected:
	FloatLink* velocity; ///< Desired velocity link.

	/// Call this in your derived class and make it \c public.
	MotorControl(DCMotor& m);

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

public:

	/// @name Attribute manipulation functions
	//@{
	/// Set the controller parameters.
	/// @param p Proportional gain
	/// @param re Reset time
	/// @param ra Rate time
	void Params(float p, float re, float ra);

	/// Set the limits of the desired value.
	void Limits(float lower, float upper);

	/// Returns the actual motor velocity.
	float Velocity(){ return motor.Velocity(); }
	//@}
	
	/// Returns TRUE if the motor is in a stall.
	bool Stalled()
	{
		if (time_stalled > time_limit)
		{
			time_stalled=0;
			return true;
		}
		else
			return false;
	}
};


MotorControl& operator>>(FloatLink& l, MotorControl& o);
FloatLink& operator<<(MotorControl& n, FloatLink& i);
MotorControl& operator<<(FloatLink& l, MotorControl& i);
FloatLink& operator>>(MotorControl& n, FloatLink& o);



/// MotorControl for left driving motor.
class MotorControlLeft : public MotorControl
{
private:
	NO_COPYING(MotorControlLeft);
public:
	SINGLETON(MotorControlLeft);
	MotorControlLeft() : MotorControl(DCMotorLeft::I()){}
};



/// MotorControl for right driving motor.
class MotorControlRight : public MotorControl
{
private:
	NO_COPYING(MotorControlRight);
public:
	SINGLETON(MotorControlRight);
	MotorControlRight() : MotorControl(DCMotorRight::I()){}
};


}; // namespace EyeMind

#endif //CONTROL_BEHAVIORS_HH