/**
 * Implentation of the mind for the soccer robots
 */


#include "mind/mind.hh"
#include "behavior/standard_behaviors.hh"
#include "behavior/constant_behaviors.hh"
#include "behavior/sensor_behaviors.hh"
#include "behavior/speech_behaviors.hh"
#include "behavior/motion_behaviors.hh"
#include "behavior/goalie_vision_behaviors.hh"
#include "behavior/control_behaviors.hh"
#include "behavior/goalie_behaviors.hh"

#include "behavior/stdc_linktypes.hh"
#include "behavior/linktypes.hh"


namespace Soccer03 {

using namespace EyeMind;

class SoccerId : public Id
{
private:

	// Root connector
	BehaviorRoot root;

	// Control behavior
	BehaviorSwitch obeytv;
	ConstantInteger constint;

	// Communication behaviors
	SpeakRadio speakradio;

	ListenIRtv listentv;
	InterpretIRtv interprettv;

	SayConstString sayattack;
	SayConstString saydefend;

	// Primative motion behaviors
	VWDriveTwoWheeled vwdrive;
	VWMove vwmove;

	DetectStall detectstall;

	// Skilled motion behaviors
	VWMotion stop;
	VWMotion kickoff;

	// Ball detection
	Look look;
	FindBall findball;
	MoveHead2DOF turnhead;
	CheckBallFar checkballfar;

	// search
	HeadSearch2DOF search;

	// Centring behaviours
	CentreGoalie centre;

	// Kicking behaviors
	KickBall kickball;
	KickBall timed_kick;

	// Links
	SignalLink rootsignallink[7];

	SignalLink rfsignallink[2];
	SpeedLink rfspeedlink[2];
	IntLink rfintlink[1];
	CharArrayLink rfchararraylink[3];

	SignalLink tvsignallink[3];
	SpeedLink tvspeedlink[3];
	IntLink tvintlink[3];

	SpeedLink vwspeedlink[7];

	IntLink intlink[1];
	DistanceLink distlink[1];

	BallLink balllink[4];
//	FloatLink floatlink[4];
	ImageLink imagelink[1];
	SignalLink signallink[6];

protected:

public:

	// Must initialise classes in constructor
	SoccerId() :
		kickoff(B_MOTION_KICKOFF),
		stop(B_MOTION_STOP),
		sayattack("attack\0"),
		saydefend("defend\0")
	{
		DEBUG_PRINT("Creating Id...\n");
	}

	// Create behavior trees for socccer robots
	void Ready()
	{
		DEBUG_PRINT("Ready Id...\n");

		// Create roots
		*this >> root;
		root >> rootsignallink[0] >> listentv;
		root >> rootsignallink[1] >> obeytv;
		root >> rootsignallink[2] >> look;
		root >> rootsignallink[3] >> centre;
		root >> rootsignallink[4] >> search;
		root >> rootsignallink[5] >> vwmove;
		root >> rootsignallink[6] >> timed_kick;

		// Motion
		vwmove >> vwspeedlink[0] >> vwdrive;

		// Detect stall
		vwmove >> vwspeedlink[1] >> detectstall;
		detectstall >> vwspeedlink[2] >> vwmove;
		vwspeedlink[2].Gain(EXCITE_MED_AVG);

		// User motion control
		listentv >> tvintlink[0] >> interprettv;
		interprettv >> tvintlink[1] >> obeytv;

		obeytv >> tvsignallink[0] >> centre;	// play
		obeytv >> tvsignallink[1] >> stop;	// stop
		obeytv >> tvsignallink[2] >> kickoff;	// OK

		centre    >> tvspeedlink[0] >> vwmove;
		stop    >> tvspeedlink[1] >> vwmove;
		kickoff >> tvspeedlink[2] >> vwmove;

		obeytv.Select(1);
		tvintlink[1].Gain(EXCITE_MED_MAX);
		tvintlink[1].Bias(EXCITE_IDLE);
		tvspeedlink[0].Gain(EXCITE_MED_MAX);
		tvspeedlink[1].Gain(EXCITE_MED_MAX);
		tvspeedlink[2].Gain(EXCITE_MED_MAX);

		kickoff >> signallink[2] >> constint;
		constint >> tvintlink[2] >> obeytv;
		constint.Threshold(EXCITE_LOW_MAX);
		tvintlink[2].Gain(EXCITE_MED_MAX);

		// Team control
		sayattack >> rfchararraylink[1] >> speakradio;
		saydefend >> rfchararraylink[2] >> speakradio;

		// Ball detection and tracking
		look >> imagelink[0] >> findball;
		findball >> balllink[0] >> turnhead;
		turnhead >> vwspeedlink[3] >> vwmove;
		vwspeedlink[3].Gain(EXCITE_MED_MAX);
		//turnhead >> signallink[0] >> vwmove;
		//signallink[0].Gain(-1);

		findball >> balllink[1] >> checkballfar;
		checkballfar >> signallink[1] >> saydefend;

		// Orientation
		centre >>  vwspeedlink[4] >> vwmove;
		vwspeedlink[4].Gain(EXCITE_LOW_AVG);

		// Kicking
		findball >> balllink[2] >> kickball;
		kickball >> signallink[2] >> sayattack;

		// Search
		turnhead >> signallink[3] >> search;
		signallink[3].Gain(-2);

		DEBUG_PRINTF("%d root(s)\n",behaviors.Count());
		DEBUG_PRINTF("%d inputs(s)\n",inputs.Count());
		DEBUG_PRINTF("%d outputs(s)\n",outputs.Count());

		DEBUG_WAIT(100);

		/* Activate and return */
		Active(1);
	}
} myId;

}; // soccer03