/**
 * A simple implementation of the Ego to test functionality
 */


#include "mind/mind.hh"

#include "behavior/speech_behaviors.hh"
#include "behavior/vision_behaviors.hh"
#include "behavior/control_behaviors.hh"
#include "behavior/motion_behaviors.hh"
#include "behavior/sensor_behaviors.hh"
#include "behavior/computing_behaviors.hh"
#include "output/eyebot_actors.hh"

#include "state/motion_states.hh"
#include "state/sensor_states.hh"
#include "state/soccer_states.hh"


class SimpleId : public Id
{
friend class SimpleEgo;		//must make the specific Ego class a friend

private:

	// Communication behaviors
//	Speak speak;
//	Listen listen;
//	StringLink stringlink;

	// Ball detection
	Look look;
	FindBall findball;

	// Motion behaviors
//	Wander wander;
	FeelPsds feelpsds;
	AvoidObstacles avoid;
	OdometryTwoWheeled odometry;
	VWMove vwmove;
	VWDriveTwoWheeled vwdrive;
	TurnCommand turncommand, turncommand2;
//	TrackHeading turntogoal;
	TurnHeadTo turnhead;
	TrackCloseObjects trackclose;

	// Links
	SignalLink signallink[1];
	DistanceLink distlink[1];
	PolarLink polarlink[6];
	SpeedLink speedlink[6];
	FloatLink floatlink[4];
	WordLink wordlink[1];
	ImageLink imagelink[1];
	PositionLink positionlink[1];

public:
	SimpleId(){}

	~SimpleId()
	{
	}

	// CREATE THE BEHAVIOR TREES HERE
	virtual int Ready()
	{
		DEBUG_PRINT("Ready Id\n");

		vwdrive.Limits(-10,10);
		vwdrive.Params(3,.07,0);

		avoid.Params(110,110,0.0,0.0,3.0,0.8);

		turncommand.SetGains(0.01,2.0);
//		turncommand2.SetGains(0.0,2.0);

		// Ball detection and tracking
		*this >> look;
		look >> imagelink[0] >> findball;
		findball >> polarlink[0] >> turnhead >> polarlink[1] >> turncommand >> speedlink[0] >> vwmove;

		// Orientation
//		findball >> signallink[0] >> turntogoal;
//		turntogoal >> speedlink[1] >> vwmove;

		// Avoidance
		*this >> trackclose;
		trackclose >> polarlink[4] >> turnhead;
		turnhead >> polarlink[5] >> trackclose;
		*this >> feelpsds;
		feelpsds >> distlink[0] >> avoid >> speedlink[2] >> vwmove;

		// Random walk
		//*this >> wander;
		//wander >> speedlink[3] >> vwmove;

		// Motion
		vwmove >> speedlink[4] >> vwdrive;
		*this >> odometry;

		// Communication
		//wordlink[0].W(Say(W_HI));
		//wordlink[0].Update();

		//wordlink >> speak;
		//*this >> speak;
//		*this >> listen;



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

		Active(true);
		return 1;
	}

} myId;




class SimpleEgo : public Ego
{
private:
//	PlayingSoccerState playingsoccer;
//	SeeBallState seeball;
//	HaveBallState haveball;

//	StringLink stringlink;

	PositionState position;
//	Oriented oriented;

	SpeedLink speedlink[2];
	PositionLink positionlink;

public:

	int Ready()
	{
		DEBUG_PRINT("Ready Ego\n");

		// Connect states to behaviors
//		speedlink >> myId.vwmove;
//		myId.listen >> stringlink;

//		playingsoccer >> speedlink[0];
//		playingsoccer << stringlink;

		position.X(1.15);
		position.Y(0.0);
		position.Q(0.0);

		position >> speedlink[1];
		position << positionlink;
		
		speedlink[1] >> myId.vwmove;
		myId.odometry >> positionlink;

		// Create truth "trees"


		/// Add "normal" states (i.e. always want to be true)
		//states + playingsoccer;
		states + position;

		/// Add "task" states (i.e. list of states to accomplish a task)
		//task + position;


		DEBUG_PRINTF("%d states(s)\n",states.Count());
		DEBUG_PRINTF("%d task(s)\n",task.Count());

		DEBUG_WAIT(100);
		Active(true);
		return 1;
	}

} myEgo;



/**
 * The acting function
 */

void Act(void)
{
	if(myId.Active())
	{
		//DEBUG_PRINT("Acting\n");
		myId.ExciteBehaviors();
	}
}


/**
 * The thinking function
 */

void Think(void)
{
	if(myEgo.Active())
	{
		//DEBUG_PRINT("Thinking\n");
		myEgo.Think();
	}
}



int main()
{
	/* Ready the id and the ego */
	myId.Ready();
	myEgo.Ready();

	while(KEYRead()!=KEY4)
	{
		Think();
		Act();
	}

	return 0;
}