/// @file soccer_behaviors.hh
/// @author Jarrod Bassan <bassan@iinet.com.au>
/// @author Joshua Petitt <petitj01@tartarus.uwa.edu.au>

#ifndef SOCCER_BEHAVIORS_HH
#define SOCCER_BEHAVIORS_HH

#include "behavior/behavior.hh"
#include "behavior/stdc_linktypes.hh"
#include "behavior/linktypes.hh"
#include "output/soccer_actors.hh"
#include "input/eyebot_sensors.hh"
#include "eyebot_behavior_types.h"

namespace Soccer03 {

using namespace EyeMind;

/// A behavior for turning the head of the robot.
/// Takes raw polar co-ordinates from the camera, corrects the co-ordinates
/// by adding the old head-position, and turns the head servo so it is
/// facing the new ball position.
/// @arg Input: PolarLink (highest)
/// @arg Output: polarLink (all)

class TurnHead : public Behavior
{
private:
	HeadServo& head;
	float Kp;				// proportional gain
	float dband;			// deadband region
	BallType corrected;
	float threshold_phi;
	float threshold_r;
	int grace_period;
	int ticks;

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

public:
	void Gain(float Kproportional) {Kp = Kproportional;}
	void Deadband(float Deadband) {dband = Deadband;}
	TurnHead();
};

TurnHead& operator>>(BallLink& l, TurnHead& o);
TurnHead& operator<<(BallLink& l, TurnHead& i);
BallLink& operator>>(TurnHead& n, BallLink& o);
BallLink& operator<<(TurnHead& n, BallLink& i);


/// A behaviour to implement a short-term memory which will take over
/// over control immediately after the robot has lost control of the ball.
/// The purpose is to try and re-aquire a lock on the target while the
/// target is still in the vicinity of the robot.
/// @arg Input: ?
/// @arg Output: ?

class Reacquire : public Behavior
{
private:
	int ticks;
	int start_period;
	float averaged_phi;
	int threshold_signal;
	int exhaust_period;
	float reacquire_v;
	float reacquire_w;
	SpeedType command;
	HeadServo& head;


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

public:
	Reacquire();
};


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


/// A behavior to track a compass heading
/// @bug Design mistake. This should be a combination of a FeelCompass and
/// a computing behavior that feeds VWMove.

class TrackHeading : public Behavior
{
private:
	Compass& compass;
	Psd& psdl;
	Psd& psdf;
	Psd& psdr;

	SpeedType speed;
	int desired_angle;

	float dband_compass;
	float Kw;
	float dband_psd;
	float left_bias;
	float right_bias;
	float v_fast;
	float v_slow;
	float v_turn;
	float w_slow;
	float max_turn_rate;
	int compass_offset;
	int internal_threshold;

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

public:
	TrackHeading();
	TrackHeading(int device_semantics);

	void DesiredHeadingRad(float phi) { desired_angle = (int)(phi * M_PI / 180); }
	void DesiredHeadingDeg(int phi) { desired_angle = phi; }
};

SpeedLink& operator>>(TrackHeading& n, SpeedLink& o);
TrackHeading& operator<<(SpeedLink& l, TrackHeading& i);
TrackHeading& operator>>(BallLink& l, TrackHeading& o);
BallLink& operator<<(TrackHeading& n, BallLink& 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 Behavior
{
private:
	float Kr; ///< v/r gain.
	float Kq; ///< w/phi gain.
	float dist_threshold;
	float dband_phi;
	float speed_close;
	float Kq_close;
	float Kq_close_hi;
	float w_slow;
	float close_threshold;

protected:
	SpeedType speed; ///< Output value.

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

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

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


}; // namespace Soccer03

#endif // SOCCER_BEHAVIORS_HH