/// @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"


#define ALLPIXELS imagerows*imagecolumns*3
#define RED 0
#define GREEN 1
#define BLUE 2


/* macros for referencing a pixels in relation to the current pixel
   eg: NORTH(k) is the memory address within the image BYTE array for the
       pixel above pixel k
*/
//// Use inline functions if you really need it.
#define SOUTH(x) ((x)+(imagecolumns*3))
#define NORTH(x) ((x)-(imagecolumns*3))
#define EAST(x) ((x)-3)
#define WEST(x) ((x)+3)

#define div(x,y) (x/y)

namespace Soccer03 {
#if 0
using namespace EyeMind;

//// didn't you mean unsigned?
static const int lookup_range[19] = {164,108,85,71,62,56,51,47,44,41,39,37,35,33,32,31,30,29,28};


/**
 * Filter the image
 */

class FindBall : public Behavior
{
private:
	PolarType ball;
	int ball_found;
	colimage* buffer;
	float threshold_phi;
	float threshold_r;
	
	int threshold_green;
	int threshold_blue;
	int threshold_intensity_lo;
	int threshold_intensity_hi;

	int image_to_polar(int x, int y);

public:
 	FindBall();

	int Execute();

	void Image(colimage* buf){buffer=buf;}

	void Feed(SignalLink &l);

	void TargetCloseThresh(float r,float phi) { threshold_phi = phi; threshold_r = r; }

	// PolarType BallPosition(){ return ball; }
	// int FoundBall(){ return ball_found; }
};

inline FindBall& operator>>(ImageLink& l, FindBall& o)
{
	o.Image(l.Image());
	SignalLinkOutputToBehavior(l,o);
	return o;
}
inline ImageLink& operator<<(FindBall& n, ImageLink& i)
{
	n.Image(i.Image());
	BehaviorInputToSignalLink(n,i);
	return i;
}


inline FindBall& operator<<(PolarLink& l, FindBall& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}
inline PolarLink& operator>>(FindBall& n, PolarLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}



/// A behavior for turning the head of the robot.
/// Takes raw polar co-ordinates from the camera, corrects the co-ordinates
/// @arg Input: PolarLink (highest)
/// @arg Output: PolarLink (all)

class TurnHeadTo: public Behavior
{
private:
	HeadServo& head;
	float dband;			// deadband region
	PolarType angle;

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

public:
	TurnHeadTo(): Behavior(), head(HeadServo::I())
	{
		head.CalibrateRange(-50.0, 50.0);
		dband = 0.1;
		memset(&angle,0,sizeof(angle));
	}
};

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



/// Predict the position of the ball at the next image frame.
/// The prediction algorithm uses a PID controller to estimate
/// the next position of the ball.
/// @arg Input: PolarLink (?)
/// @arg Output: PolarLink (?)

class Predict : public Behavior
{
private:
	int ticks;
	PolarType actual;
	PolarType predicted;
	int ball_not_found_signal;
	int max_delay;
	int frames_held;  // Number of consecutive frames we have seen the ball
	float Kp, Kd, Ki; // Gains
	PolarType Ep, Ed, Ei; // Errors

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

public:
	void Params(float Kproportional, float Kintegral, float Kdifferential);

	Predict(): Behavior()
	{
		max_delay = 10;
		ticks = max_delay;
		ball_not_found_signal = 1;
		memset(&actual, 0, sizeof(PolarType));
		memset(&predicted, 0, sizeof(PolarType));
		memset(&Ep, 0, sizeof(PolarType));
		memset(&Ei, 0, sizeof(PolarType));
		memset(&Ed, 0, sizeof(PolarType));
		actual.r = 0.0; actual.phi = 0.0;
		predicted.r = 0.0; predicted.phi = 0.0;
		Ep.phi = 0.0; Ei.phi = 0.0; Ed.phi = 0.0;
		Ep.r = 0.0; Ei.r = 0.0; Ed.r = 0.0;

		Kp = 0.2; Ki = 0.1; Kd = 0.3;
		frames_held = 0;

		//Set the behavior parameters
		//MaxExcitation(1000);
	}
};

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




/**
 * A behavior for tracking a close object using the PSD sensors
 *
 */

/* class TrackClose : public Behavior */
/* { */
/*  private: */
/*   Psd* psd; */
/*   MotorControl* motor[2]; */

/*   float K[4]; */

/*  public: */
/*   TrackClose(Psd* p, MotorControl* left, MotorControl* right) */
/*     { */
/*       psd = p; */
/*       motor[0] = left; */
/*       motor[1] = right; */

/*       K[0] = (0.4370)*2; */
/*       K[1] = -3.2772; */
/*       K[2] = 0.002; */
/*       K[3] = 0; */
/*     } */

/*   int Execute() */
/*     { */
/*       int x, dx, dq; */
/*       int v; */

/*       psd->Update(); */
/*       x = psd->X(); */
/*       dx = psd->DX(); */
/*       dq = (motor[0]->Velocity() + motor[1]->Velocity())>>1; */

/*       v = (int)(K[0]*x + K[1]*dx + K[2]*dq + K[3]); */

/*       if(x>140) */
/* 	v = -80; */

/*       if(dq > 20 || dq < -20) */
/* 	v = 0; */

/*       motor[0]->Velocity(v); */
/*       motor[1]->Velocity(v); */

/*       return 1; */
/*     } */
/* }; */



/**
 * A Behavior for turning head towards the closest object to the robot.
 *
 * Input: PolarLink from TurnHeadTo Behavior
 *
 * Output: PolarLink for TurnHeadTo Behavior
 */

#define SCAN_POINTS 31

class TrackCloseObjects: public Behavior
{
friend TrackCloseObjects& operator>>(PolarLink& l, TrackCloseObjects& o);
friend PolarLink& operator<<(TrackCloseObjects& n, PolarLink& i);
private:
	PsdFront* psd;
	PolarLink* anglelink;
	PolarType angle;
	float dQ;		// angle between scan points
	int dist[SCAN_POINTS];
	int decrement;

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

public:
	TrackCloseObjects():Behavior()
	{
		//MaxExcitation(3);
		Threshold(2);
		//Exhaustion(2);

		psd = &PsdFront::I();

		anglelink=NULL;
		memset(&angle,0,sizeof(PolarType));

		dQ = 100.0*M_PI/(SCAN_POINTS*180.0);
		decrement = 1;

		for(int i=0;i<SCAN_POINTS;i++)
		{
			dist[i] = 0;
		}
	}
};

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

#endif
}; // namespace Soccer03

#endif // SOCCER_BEHAVIORS_HH