/*------------------------------------------------------------------\
|                                                                   |
| A variation on the wall follower which uses a simple              |
| BDI architecture that switches between the tasks used by the      |
| behaviour-based wall follower.                                    |
|                                                                   |
| Simon Parsons                                                     |
| City University of New York                                       |
| June 2003                                                         |
|                                                                   |
\------------------------------------------------------------------*/

/*
   Beliefs and intentions are propositional variables, while plans 
   are tasks that implement suitable behaviours to match the 
   intentions.

   This is way more complex than the code needs to be to solve this
   problem, but hopefully illustrates how a simple BDI interpreter
   would work.
  						                          */

//------------------------------------------------------------------------
//
// Initialisation
//

// Name sensors

#define BUMP_LEFT 	SENSOR_1
#define FLOOR_LIGHT	SENSOR_2
#define BUMP_RIGHT	SENSOR_3

// Name variables

#define LEFT_MOTOR	OUT_A
#define RIGHT_MOTOR	OUT_C

// Define constants

#define THRESHOLD   30

#define DRIVE_LEFT  1
#define DRIVE_RIGHT 6
#define TURN_POWER  2

#define BACK_TIME_FIXED 10
#define BACK_TIME_RAND  20
#define TURN_TIME_FIXED 20
#define TURN_TIME_RAND  10

//------------------------------------------------------------------------
//
// Mental attitudes
//

// Define beliefs

int LIGHT_PATCH = 0;
int OBJECT_LEFT = 0;
int OBJECT_RIGHT = 0;

// Define desires

int COULD_STOP = 0;
int COULD_TURN_LEFT = 0;
int COULD_TURN_RIGHT = 0;
int COULD_DRIVE_ON = 0;

// Define intentions

int STOP = 0;
int TURN_LEFT = 0;
int TURN_RIGHT = 0;
int DRIVE_ON = 0;

//---------------------------------------------------------------------
//
// Main task
//

// Start everything up and then hand over control to BDI controller.

task main()
{

	// Initialise sensors

	SetSensor(BUMP_LEFT, SENSOR_TOUCH);
	SetSensor(BUMP_RIGHT, SENSOR_TOUCH);
	SetSensor(FLOOR_LIGHT, SENSOR_LIGHT);

	// Set random seed

	//SetRandomSeed(20);

      // Start motors

	SetPower(LEFT_MOTOR, DRIVE_LEFT);
	SetPower(RIGHT_MOTOR, DRIVE_RIGHT);
	OnFwd(LEFT_MOTOR+RIGHT_MOTOR);

	// Start BDI engine

	start BDI;
}

//--------------------------------------------------------------------
//
// Run the BDI controller
//

// Since the "plans" are quick to execute, there is no need to worry about
// over-commitment.

task BDI()
{
	while(true)
	{
		ReviseBeliefs();
		Options();
		Filter();
		SelectPlan();
	}
}

// Belief revision function; modify the belief propositions based upon
// sensor readings.

void ReviseBeliefs()
{

	// Read all the sensors and alter beliefs as appropriate

	if(BUMP_LEFT)
	{
		OBJECT_LEFT = 1;		
	}
	else
	{
		OBJECT_LEFT = 0;
	}

	if(BUMP_RIGHT)
	{
		OBJECT_RIGHT = 1;		
	}
	else
	{
		OBJECT_RIGHT = 0;
	}

	if(FLOOR_LIGHT < THRESHOLD)
	{
		LIGHT_PATCH = 1;		
	}
	else
	{
		LIGHT_PATCH = 0;
	}
}

// Generate options. Basically flag as desires all the things that could or
// should be carried out given the beliefs (we don't really need to
// look at current intentions here).

// Note that driving on is always an option.

void Options()
{
	if(OBJECT_LEFT)
	{
		COULD_TURN_LEFT = 1;		
	}
	else
	{
		COULD_TURN_LEFT = 0;
	}

	if(OBJECT_RIGHT)
	{
		COULD_TURN_RIGHT = 1;		
	}
	else
	{
		COULD_TURN_RIGHT = 0;
	}

	if(LIGHT_PATCH)
	{
		COULD_STOP = 1;		
	}
	else
	{
		COULD_STOP = 0;
	}

	COULD_DRIVE_ON = 1;
}

// A function to turn desires into intentions. This catches the interaction
// between intentions; the agent is not allowed to adopt conflicting 
// intentions.

void Filter()
{
	if(COULD_STOP)
	{
		STOP = 1;
	}
	else
	{
		STOP = 0;
	}	

	if(COULD_TURN_LEFT && !STOP)
	{
		TURN_LEFT = 1;		
	}
	else
	{
		TURN_LEFT = 0;
	}

	if(COULD_TURN_RIGHT && !COULD_TURN_LEFT && !STOP)
	{
		TURN_RIGHT = 1;
	}
	else
	{
		TURN_RIGHT = 0;
	}

	if(COULD_DRIVE_ON && !COULD_TURN_RIGHT && !COULD_TURN_LEFT && !STOP)
	{
		DRIVE_ON = 1;
	}
	else
	{
		DRIVE_ON = 0;
	}
}

// Finally, pick the behaviour that corresponds to the intention in hand.

void SelectPlan()
{
	if (TURN_LEFT)
	{
		AvoidLeft();
	}
	
	if (TURN_RIGHT)
	{
		AvoidRight();
	}


	if (DRIVE_ON)
	{
		DriveOn();
	}

	if (STOP)
	{
		HaltMotors();
	}
}
//--------------------------------------------------------------------
//
// Behaviours to achieve the intentions.
//

// Backup and turn to the right.

void AvoidLeft()
{
		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Motors off and
		  OnRev(LEFT_MOTOR+RIGHT_MOTOR);      // then back up.
		  Wait(BACK_TIME_FIXED
		       +Random(BACK_TIME_RAND));

		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Motors off again,
		  SetPower(LEFT_MOTOR, TURN_POWER);   // then reset power,
		  SetPower(RIGHT_MOTOR, TURN_POWER);  // and turn.
		  OnFwd(LEFT_MOTOR);              
		  OnRev(RIGHT_MOTOR);
		  Wait(TURN_TIME_FIXED
		       +Random(TURN_TIME_RAND));
		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Finally, shut 
  							    // turn motors off.
}

// Backup and turn to the left.

void AvoidRight()
{
		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Motors off and
		  OnRev(LEFT_MOTOR+RIGHT_MOTOR);      // then back up.
		  Wait(BACK_TIME_FIXED
		       +Random(BACK_TIME_RAND));

		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Motors off again,
		  SetPower(LEFT_MOTOR, TURN_POWER);   // then reset power,
		  SetPower(RIGHT_MOTOR, TURN_POWER);  // and turn.
		  OnFwd(RIGHT_MOTOR);              
		  OnRev(LEFT_MOTOR);
		  Wait(TURN_TIME_FIXED
		       +Random(TURN_TIME_RAND));
		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Finally, shut 
							    // turn motors off.
}

// Drive ahead, but the motor powers set above make the robot run to the
// left.

void DriveOn()
{
		SetPower(LEFT_MOTOR, DRIVE_LEFT);     // Set power, then 		  
       	SetPower(RIGHT_MOTOR, DRIVE_RIGHT);   // forward.
		OnFwd(LEFT_MOTOR+RIGHT_MOTOR);        // head off.
}

// Switch off the engines

void HaltMotors()
{
		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Shut off motors
}