// Behavior.h -- a first cut at defining behaviors and arbitration between them.

#include "DMLChanRef.h"
using namespace std;
#include <vector>
#include <list>

class Arbiter;  // forward declaration

typedef unsigned long U32;

// Behavior types -- these are bitfields, and lowest number = highest priority as well.
// highest priorities:
const U32 cBehaviorSafetyCritical	= 0x00000001;	// "Stop"
const U32 cBehaviorExplore			= 0x00000002;	// "Scan"
const U32 cBehaviorDrive			= 0x00000004;	// "Drive, Avoid"
// ...
// lowest priorities:
const U32 cBehaviorStart			= 0x80000000;	// "Start"

// Various bitmasks for enabling/disabling behavior types:
const U32 cAllBehaviors				= 0xffffffff;
const U32 cAllNonCriticalBehaviors	= cAllBehaviors & ~cBehaviorSafetyCritical;
const U32 cAllMovementBehaviors		= cBehaviorExplore | cBehaviorDrive;

const int ciDefaultAcceleration = 60;
const int ciDefaultVelocity = ciDefaultAcceleration;  // ensures 1 second to decelerate to a stop

const int ciNumBeakBehaviors = 3;  // number of "Stop" and "Start" behaviors
const int ciNumAvoidBehaviors = 6;  // number of "Avoid" behaviors

// Minimum range returned by Kurtbot's beak sensors -- measured on beige carpet
const int ciCenterFloorReturn = 500;  // never saw anything lower than 51X
const int ciLeftFloorReturn = 600;  // Could be 690...  rarely drops into the 600s and then only for one reading
const int ciRightFloorReturn = 600;  // never saw anything lower than 61X

// DML Time is measured in milliseonds (ms), which is also the time unit for Win32 in calls such as Sleep().
class DMLTimer
{
public:
	DMLTimer();	// constructor
	void Reset();	// Reset the timer to "not started"
	void Start();	// Start the timer, take a snapshot of the current time
	bool HasStarted();	// Has StartTimer() been called since timer was reset?
	bool HasTimeElapsed(int msElapsed);  // Has msElapsed time (or more) gone by since we called StartTimer()?
private:
	bool m_fHasStarted;
	DML_TIME m_TimerStart;
};

class Behavior
{
public:
	Behavior(Arbiter *pMaster);  // Constructor, no input or output channels
	Behavior(Arbiter *pMaster, DML_ChannelRef	*pInput);  // Constructor, single input channel.

	// These functions must be provided by each concrete subclass of Behavior
	virtual U32 BehaviorType() = 0;  // Report my behavior type
	virtual void Reset() = 0;  // Signal from arbiter telling this behavior to reset itself
	virtual void Act() = 0;	// Signal from arbiter allowing this behavior to execute.

protected:
	DML_ChannelRef *m_pInput;  // our data input channel
	Arbiter *m_pMaster; // the arbiter who is in charge of this instance of a Behavior
};

class Arbiter
{
public:
	Arbiter();  // constructor
	void AddBehavior(Behavior *pBehavior);  // add behavior to tail of list, it will be lower priority than previously added behaviors
	void ClearBehaviorList();  // empty the list of behaviors
	void Tick();  // A thread's service loop will periodically call Tick to give us a time-slice to use to run behaviors
			 // Tick will iterate through all enabled behaviors
	void EnableBehaviors(U32 BehaviorSet);  // Enable these behavior types (bit set)
	void DisableBehaviors(U32 BehaviorSet);	// Disable these behavior types (bit set)
	bool BehaviorIsEnabled(U32 TheBehavior);  // Is this type of behavior currently enabled?
private:
	list<Behavior *> m_lpBehaviors;  // head of list = highest priority behavior.
	U32	m_EnabledBehaviors;  // bit set of all behavior types currently enabled
};

// -- StopBehavior --
// Description:
//		StopBehavior looks for a hand swipe near the IR sensor that it is instantiated with.
// Processing:
//		When a "Stop" signal from user is recognized, StopBehavior will send a SmoothStop command,
//		wait for the robot to stop moving, and then turn the motors off.
class StopBehavior : public Behavior
{
public:
	StopBehavior(Arbiter *pMaster, DML_ChannelRef	*pInput, int InstanceNumber) : Behavior(pMaster, pInput)
		{ m_InstanceNumber = InstanceNumber; };
	U32 BehaviorType();
	void Reset();
	void Act();
private:
	DMLTimer m_Timer;
	bool m_fHoldoff;
	int m_InstanceNumber;
};

// -- StartBehavior --
// Description:
//		StartBehavior looks for a hand swipe near the IR sensor that it is instantiated with.
// Processing:
//		When a "Start" signal is received, "Explore" behaviors will be enabled, as well as safety-critical behaviors.
class StartBehavior : public Behavior
{
public:
	StartBehavior(Arbiter *pMaster, DML_ChannelRef	*pInput, int InstanceNumber) : Behavior(pMaster, pInput)
		{ m_InstanceNumber = InstanceNumber; };
	U32 BehaviorType();
	void Reset();
	void Act();
private:
	DMLTimer m_Timer;
	bool m_fHoldoff;
	int m_InstanceNumber;
};

// -- ScanBehavior --
// Description:
//		ScanBehavior turns the robot in place while reading all of the IR range data,
//		and attempts to find a "safe travel vector" -- a direction and distance that the
//		robot can move without hitting anything.
// Processing:
//		ScanBehavior will turn the robot in the direction that is "least obstructed" based on a comparison
//		of left and right sensors.  It will initially turn 30 degrees, which allows it to sweep a
//		90 degree arc with the 3 front sensors.  If it cannot determine a safe travel vector,
//		it will keep turning in the same direction.
class ScanBehavior : public Behavior
{
public:
	ScanBehavior(Arbiter *pMaster) : Behavior(pMaster){};
	U32 BehaviorType();
	void Reset();
	void Act();
private:
	bool GetSafeTravelVector(int *pTurnAngle, int *pMoveDistance);
	int m_TurnAngle;  // The turn we are executing while gathering sensor data
	SensorArc m_SensorArc;  // Gather sensor data and inflict math on it
	bool m_fScanStarted;
};

// -- DriveBehavior --
// Description:
//		DriveBehavior will run after ScanBehavior has found a safe travel vector.
// Processing:
//		Turn the robot towards the "safe travel vector".
//		Drive the length of the "safe travel vector" (less sensor safety margin)
class DriveBehavior : public Behavior
{
public:
	DriveBehavior(Arbiter *pMaster) : Behavior(pMaster){};
	U32 BehaviorType();
	void Reset();
	void Act();
private:
	bool m_fTurnStarted;
	bool m_fDriveStarted;
};

// -- AvoidBehavior --
// Description:
//		AvoidBehavior looks for an obstacle on the IR sensor that it is instantiated with.
// Processing:
//		AvoidBehavior will command a stop if an obstacle is detected closer than the configured trigger distance.
//		It will then enable exploration behaviors so that the robot can turn to a new heading.
class AvoidBehavior : public Behavior
{
public:
	AvoidBehavior(Arbiter *pMaster, DML_ChannelRef	*pInput, int TriggerDistance) : Behavior(pMaster, pInput)
		{ m_TriggerDistance = TriggerDistance; };
	U32 BehaviorType();
	void Reset();
	void Act();
private:
	int m_TriggerDistance;
};

/// Temp...
class TestDriveBehavior : public Behavior
{
public:
	TestDriveBehavior(Arbiter *pMaster) : Behavior(pMaster){};
	U32 BehaviorType();
	void Reset();
	void Act();
private:
	bool m_fDrive1Started;
	bool m_fDrive2Started;
};