/// @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 to centre a Goalie on the field
 */
class CentreGoalie : public Behavior
{
private:
	Compass& compass;
	Psd& psdl;
	Psd& psdb;
	Psd& psdr;

	SpeedType speed;
	float dband_v;
	float dband_w;
	float Kw;
	float Kv;
	float left_bias;
	float right_bias;
	float max_speed;
	float max_rate_of_turn;
	float desired_angle;
	float second_angle;
	bool reset_distance;
	int min_rear_distance;
	int max_rear_distance;
	int reset_rear_distance;

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

public:
	CentreGoalie();
};

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

/**
 * A behavior to track a compass heading
 */
class KickBall : public Behavior
{
private:
	KickerServo& boot;
	float fire_range;
	int last_kick_time;

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

public:
	KickBall();
};

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

/**
 * A behavior to return an excitation if the ball is visible but not close.
 */
class CheckBallFar : public Behavior
{
private:

protected:
	//void Feed(SignalLink &l);
	int Execute();
public:
	CheckBallFar(): Behavior() { }
};

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

/**
 * A behavior for moviing the head of the robot (with 2DOF).
 * Takes raw polar co-ordinates from the camera, corrects the co-ordinates
 * by adding the old head-position, and manipulates the head servos so the
 * head is facing the new ball position
 * Takes the input with the largest signal.
 * Input Links: PolarLink
 * Output Links: polarLink
 * @author Jarrod Bassan
 * @date 11-Jul-2003
 */

class MoveHead2DOF: public Behavior
{
private:
	HeadServo& headturn;
	TiltServo& headtilt;
	Compass& compass;
	float Kp_turn, Kp_tilt;				// proportional gains
	float Kp_drive;
	float dband_turn, dband_tilt;			// deadband regions
	BallType raw;
	SpeedType speed;
	float max_speed;
	float range_modifier;
	float desired_angle;
	int compass_offset;
	int compass_dband;
	float Kw;
	float max_rate_of_turn;

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

public:
	MoveHead2DOF();
};

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


/**
 * A behavior for moving a 2DOF head in a search pattern.
 * Uses a simple state machine
 */
class HeadSearch2DOF : public Behavior
{
private:
	HeadServo& headturn;
	TiltServo& headtilt;
	int speed_x, speed_y;
	int orig_speed_x, orig_speed_y;
	int x_pos, y_pos;
	int x_max, x_min;
	int y_max, y_min;
	int look_far;
	int look_close;

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

public:
	HeadSearch2DOF();
};

}; // namespace Soccer03

#endif // SOCCER_BEHAVIORS_HH