/**@file simpleid.cc
 *
 * A sample application for testing behaviors.
 * Change the SimpleId class to include your test behaviors.
 *
 */

#include "mind/mind.hh"


#include "behavior/speech_behaviors.hh"

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



/**
 * A simple Id class for testing behaviors
 */

class SimpleId : public Id
{
private:
// Image
	Look look;
	ImageLink imagelink;
	ShowImage showimage;
	FindBall findball;

// Control
	TurnCommand turncommand;
	TurnHead turnhead;

// Communication
	Speak speak;


// Avoidance
	FeelPsds feelpsds;
	AvoidObstacles avoid;
	OdometryTwoWheeled odometry;
	VWMove vwmove;
	VWDriveTwoWheeled vwdrive;

// Links
	DistanceLink distlink;
	SpeedLink speedlink1, speedlink2, speedlink3, mstraight;
	FloatLink floatlink, floatlink2;
	PolarLink polarlink1, polarlink2;
	WordLink wordlink;

public:
	SimpleId(){}
	~SimpleId(){}

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

		wordlink.W(Say(W_HI));
		wordlink.Update();

		wordlink >> speak;
		*this >> speak;

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

//		Stationary Behaviour
 		SpeedType s = {0.0,0.0};
 		mstraight.Speed(s);
 		mstraight.Signal(5);
 		mstraight >> vwmove;

//		Ball detection
		*this >> look; 	// Attatch as a root behaviour
		look >> imagelink >> findball;

//		Ball chasing behaviours
		findball >> polarlink1 >> turnhead >> polarlink2 >> turncommand >> speedlink1 >> vwmove;

//		Motion Behaviours
		vwmove >> speedlink2 >> vwdrive;

//		Avoidance Behaviours
		*this >> feelpsds;
		feelpsds >> distlink;
		distlink >> avoid >> speedlink3;
		speedlink3 >> vwmove;

		DEBUG_PRINTF("%d root(s)\n",behaviors.Count());
		DEBUG_WAIT(100);

		Active(true);
		return 1;
	}
} myId;



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


/**
 * The main program
 */

int main()
{
	DEBUG_PRINT("Simple Id\n");

	/* Ready the id, this should create the behavior trees */
	myId.Ready();


	/* Run */
	while(KEYRead()!=KEY4)
	{
		Act();
		//OSWait(100);
	}

	return 0;
}