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


#include "mind/mind.hh"
#include "behavior/standard_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"


namespace TVControl {

using namespace EyeMind;

class SoccerId : public Id
{
private:

	// Root connector
	BehaviorRoot root;

	// Control behavior
	BehaviorSwitch obeytv;

	// Communication behaviors
	ListenIRtv listentv;
	InterpretIRtv interprettv;

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

	// Obstacle avoidance
	FeelPsds feelpsds;
	AvoidObstacles avoid;

	// Skilled motion behaviors
	VWMotion stop;
	VWMotion drive;
	VWMotion turnrightwide;
	VWMotion turnright;
	VWMotion turnleftwide;
	VWMotion turnleft;
	VWMotion backup;
	VWMotion backupleft;
	VWMotion backupright;

	// Links
	SignalLink rootsignallink;

	IntLink intlink[3];

	SpeedLink tvspeedlink[16];

	SignalLink signallink[4];
	SignalLink tvsignallink[16];

	DistanceLink distlink[1];

	PolarLink polarlink[8];
	ImageLink imagelink[1];
protected:

public:

	// Must initialise classes in constructor
	SoccerId() :
		stop(B_MOTION_STOP),
		drive(B_MOTION_DRIVE_STRAIGHT),
		turnrightwide(B_MOTION_TURN_RIGHT_WIDE),
		turnright(B_MOTION_TURN_RIGHT),
		turnleftwide(B_MOTION_TURN_LEFT_WIDE),
		turnleft(B_MOTION_TURN_LEFT),
		backup(B_MOTION_BACKUP),
		backupleft(B_MOTION_BACKUP_LEFT),
		backupright(B_MOTION_BACKUP_RIGHT)
	{
		DEBUG_PRINT("Creating Id...\n");
	}

	// Create behavior tree(s)
	void Ready()
	{
		DEBUG_PRINT("Ready Id...\n");

		// Create root(s)
		*this >> root;

		// Root behaviors
		root >> rootsignallink[1] >> listentv;
		root >> rootsignallink[3] >> obeytv;
		root >> rootsignallink[4] >> vwmove;
		root >> rootsignallink[5] >> feelpsds;

		// Communication
		listentv >> intlink[0] >> interprettv;
		interprettv >> intlink[1] >> obeytv;
		intlink[1].Gain(EXCITE_HI_MAX);

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

		// Obstacle Avoidance
		feelpsds >> distlink[0] >> avoid;
		avoid >> speedlink[1] >> vwmove;
		speedlink[1].Gain(EXCITE_HI_MAX);

		// Detect stall
		vwmove >> speedlink[2] >> detectstall;
		detectstall >> speedlink[3] >> vwmove;
		speedlink[3].Gain(EXCITE_HI_MAX);

		// User motion control
		obeytv >> tvsignallink[0] >> stop;
		obeytv >> tvsignallink[1] >> turnleftwide;
		obeytv >> tvsignallink[2] >> drive;
		obeytv >> tvsignallink[3] >> turnrightwide;
		obeytv >> tvsignallink[4] >> turnleft;
		obeytv >> tvsignallink[5] >> stop;
		obeytv >> tvsignallink[6] >> turnright;
		obeytv >> tvsignallink[7] >> backupleft;
		obeytv >> tvsignallink[8] >> backup;
		obeytv >> tvsignallink[9] >> backupright;

		turnleftwide  >> tvspeedlink[1] >> vwmove;
		drive         >> tvspeedlink[2] >> vwmove;
		turnrightwide >> tvspeedlink[3] >> vwmove;
		turnleft      >> tvspeedlink[4] >> vwmove;
		stop          >> tvspeedlink[5] >> vwmove;
		turnright     >> tvspeedlink[6] >> vwmove;
		backupleft    >> tvspeedlink[7] >> vwmove;
		backup        >> tvspeedlink[8] >> vwmove;
		backupright   >> tvspeedlink[9] >> vwmove;

		tvspeedlink[1].Gain(EXCITE_MED_MAX);
		tvspeedlink[2].Gain(EXCITE_MED_MAX);
		tvspeedlink[3].Gain(EXCITE_MED_MAX);
		tvspeedlink[4].Gain(EXCITE_MED_MAX);
		tvspeedlink[5].Gain(EXCITE_MED_MAX);
		tvspeedlink[6].Gain(EXCITE_MED_MAX);
		tvspeedlink[7].Gain(EXCITE_MED_MAX);
		tvspeedlink[8].Gain(EXCITE_MED_MAX);
		tvspeedlink[9].Gain(EXCITE_MED_MAX);

		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;

}; // tvcontrol.hh