/**
 * 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/computing_behaviors.hh"
#include "behavior/vision_behaviors.hh"
#include "behavior/control_behaviors.hh"
#include "behavior/soccer_behaviors.hh"

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

#include "eyebot_behavior_sem.h"


namespace Soccer03 {

using namespace EyeMind;

class SoccerId : public Id
{
private:
	// Root connector
	BehaviorRoot root;

	// Control behaviors
	BehaviorSwitch obeyradio;
	BehaviorSwitch obeytv;
	ConstantInteger constint;

	// Communication behaviors
	SpeakRadio speakradio;
	ListenRadio listenradio;
	InterpretString interpretradio;

	ListenIRtv listentv;
	InterpretIRtv interprettv;

	SayConstString sayattack;
	SayConstString saydefend;

	// Primative motion behaviors
	VWDriveTwoWheeled vwdrive;
	VWMove vwmove;
	DetectStall detectstall;

	// Obstacle avoidance
	FeelFrontBackPsds feelfbpsds;
	FeelLeftRightPsds feellrpsds;
	AvoidObstacles avoid;

	// Skilled motion behaviors
	VWMotion stop;
	VWMotion kickoff;
	Wander play;

	// Playing skills
	TrackHeading attack;
	TrackHeading defend;

	// Ball detection
	Look look;
	FindBall findball;
	TurnHead turnhead;

	// Ball skills
	TurnCommand turntoball;
	TrackHeading attackball;
	Reacquire reacquire;

	// Links
	SignalLink rootsignallink[7];

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

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

	SpeedLink vwspeedlink[7];
	DistanceLink distlink[2];
	BallLink balllink[4];
	ImageLink imagelink[1];

	SignalLink signallink[10];
protected:

public:

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

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

		player_t* pt = (player_t*)HDTFindEntry(PLAYER, 0);
		OSPanicIf(pt==NULL,"Cannot find\n player data\n");

		// Create roots
		*this >> root;
		root >> rootsignallink[0] >> listentv;
		root >> rootsignallink[1] >> obeytv;
		root >> rootsignallink[2] >> listenradio;
		root >> rootsignallink[3] >> look;
		root >> rootsignallink[4] >> feelfbpsds;
		root >> rootsignallink[5] >> feellrpsds;
		root >> rootsignallink[6] >> vwmove;

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

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


		// Obstacle Avoidance
		feelfbpsds >> distlink[0] >> avoid;
		feellrpsds >> distlink[1] >> avoid;
		avoid >> vwspeedlink[3] >> vwmove;
		vwspeedlink[3].Gain(EXCITE_HI_AVG);
		avoid.Threshold(2);


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

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

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

		obeytv.Select(1);
		tvspeedlink[0].Gain(EXCITE_MED_AVG);
		tvspeedlink[1].Gain(EXCITE_MED_AVG);
		tvspeedlink[2].Gain(EXCITE_MED_AVG);

		stop >> signallink[0] >> feelfbpsds;
		stop >> signallink[1] >> feellrpsds;
		stop >> signallink[2] >> look;
		kickoff >> signallink[3] >> feelfbpsds;
		kickoff >> signallink[4] >> feellrpsds;
		kickoff >> signallink[5] >> look;

		signallink[0].Gain(-1);
		signallink[1].Gain(-1);
		signallink[2].Gain(-1);
		signallink[3].Gain(-1);
		signallink[4].Gain(-1);
		signallink[5].Gain(-1);

		kickoff >> signallink[6] >> constint;
		constint >> tvintlink[2] >> obeytv;
		constint.Threshold(pt->kick);
		tvintlink[2].Gain(EXCITE_MED_AVG);
		tvintlink[2].Decay(1);


		// Team control
		listenradio >> rfchararraylink[0] >> interpretradio;
		interpretradio >> rfintlink[0] >> obeyradio;

		obeyradio >> rfsignallink[0] >> attack;
		obeyradio >> rfsignallink[1] >> defend;

		attack >> rfspeedlink[0] >> vwmove;
		defend >> rfspeedlink[1] >> vwmove;

		attack >> rfsignallink[2] >> play;
		defend >> rfsignallink[3] >> play;

		rfsignallink[2].Gain(-1);
		rfsignallink[3].Gain(-1);

		// packets arrive in bursts, so only need a small excitation
		rfspeedlink[0].Gain(EXCITE_LOW_AVG);
		rfspeedlink[1].Gain(EXCITE_LOW_AVG);

		sayattack >> rfchararraylink[1] >> speakradio;
		saydefend >> rfchararraylink[2] >> speakradio;


		// Ball detection and tracking
		look >> imagelink[0] >> findball;
		findball >> balllink[0] >> turnhead;

		// Ball kicking skills
		turnhead >> balllink[1] >> attackball;
		attackball >> vwspeedlink[4] >> vwmove;

		turnhead >> balllink[2] >> turntoball;
		turntoball >> vwspeedlink[5] >> vwmove;

		turntoball >> signallink[7] >> feelfbpsds;
		turntoball >> signallink[8] >> play;
		turntoball >> signallink[9] >> sayattack;

		vwspeedlink[4].Gain(EXCITE_MED_AVG);
		vwspeedlink[5].Gain(EXCITE_MED_AVG);
		signallink[7].Gain(-1);
		signallink[8].Gain(-1);


		// Reacquire
		turnhead >> balllink[3] >> reacquire;
		reacquire >> vwspeedlink[6] >> vwmove;
		vwspeedlink[6].Gain(EXCITE_MED_MIN);

		
		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);
		DEBUG_LCDCLEAR;

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

}; // soccer03