// plan.c
// 
// Version	Date	 Author	 Description
//	--------------------------------------------------------------------------
//  0.01	24/06/01 BWW	 Generate a plan

#include <c:\psx\include\libps.h>
#include "planner.h"
#include "funcprototypes.h"

extern	UnmetPreconditions_Struct	UnmetPreconditions;
extern	ActionEffects_Struct		ActionEffects;
extern	Route_Struct				Route;
extern	Door_Struct					Door[];
extern	Effects_Struct				Effects;
extern	SearchGoalNodes_Struct		SearchGoalNodes;
extern	Exit_Struct					Exit;
extern	Agent_Struct				Agent;
extern	Key_Struct					Key;
extern 	CausalLinks_Struct			CausalLinks;
extern	Orderings_Struct			Orderings;


// Global variables for search routines ..... Temporary measure to get over parameter passing problems
extern	int	startx, starty,destinationx, destinationy;

extern	int	BRIDGENUM;
extern	int	DOORNUM;



//	--------------------------------------------------------------------------
// 	Initialise the Plan
//	--------------------------------------------------------------------------
//	Description:	Initialise the structures that will hold the plan and set the
//					agent's goals
//
//	Parameters:		None	
//							
//	Returns:		None

void InitPlan()
	{
	
	// Reset the structures that will hold the plan	
	UnmetPreconditions.numpreconditions = 0;
	ActionEffects.numeffects = 0;
	Orderings.numorderings = 0;
	CausalLinks.numcausallinks = 0;

	// Set the Agent's ultimate goal as the first Precondition on the plan
	SetAgentGoal();	
	
	}


//	--------------------------------------------------------------------------
// 	Build a plan
//	--------------------------------------------------------------------------
//	Description:	Build a new plan
//
//	Parameters:		None
//							
//	Returns:		None

void BuildPlan()
	{
	
	int index, result = 0;
	
	// Initialise the plan
	InitPlan();

	// Find action effects to met each of the plan's goal preconditions
	for (index=0;((index<UnmetPreconditions.numpreconditions)&&(result!=-1));index++)
		{
		result = FindEffect(index);
		}
				
	// If the planning has failed to find a solution then add a WAIT action, otherwise
	// add a PLAN action of a duration in direct relation to the number of actions in the plan
	if (result==-1)
		{
		InitPlan();
		AddUnmetPrecondition(PRECON_WAIT,PARAM_OBJECTAGENT,NULL,PARAM_DURATION,DURATION_WAIT,NULL,NULL);
		AddActionEffect(EFFECT_WAIT,PARAM_OBJECTAGENT,NULL,PARAM_DURATION,DURATION_WAIT,NULL,NULL,DURATION_WAIT);	
		// Add a Causal Link to link the index of the Action Effect to the index of the Precondition it meets
		AddCausalLink((ActionEffects.numeffects-1),(UnmetPreconditions.numpreconditions-1));
		}
	else
		{
		// Add a PLAN precondition, and a PLAN Action Effect				
		AddUnmetPrecondition(PRECON_PLAN,PARAM_OBJECTAGENT,NULL,PARAM_DURATION,
							(50+(DURATION_PLANSTEP*ActionEffects.numeffects)),NULL,NULL);
		AddActionEffect(EFFECT_PLAN,PARAM_OBJECTAGENT,NULL,PARAM_DURATION,
						(50+(DURATION_PLANSTEP*ActionEffects.numeffects)),NULL,NULL,
						(50+(DURATION_PLANSTEP*ActionEffects.numeffects)));	

		// Add a Causal Link to link the index of the Action Effect to the index of the Precondition it meets
		AddCausalLink((ActionEffects.numeffects-1),(UnmetPreconditions.numpreconditions-1));

		// Add Orderings to ensure that the PLAN Precondition is met before anything else
		for (index=0;(index<UnmetPreconditions.numpreconditions-1);index++)		
			{
			AddOrdering((UnmetPreconditions.numpreconditions-1),index);
			}

		}
				
				
//	printf("Number Unmet Preconditions: %d \n",UnmetPreconditions.numpreconditions);
//	for (index=0;index<UnmetPreconditions.numpreconditions;index++)
//		{
//		printf("Index: %d Condition: %d P1: %d %d P2: %d %d P3: %d %d Met: %d\n",
//				index,
//				UnmetPreconditions.condition[index].condition,
//				UnmetPreconditions.condition[index].parameter1,
//				UnmetPreconditions.condition[index].value1,
//				UnmetPreconditions.condition[index].parameter2,
//				UnmetPreconditions.condition[index].value2,
//				UnmetPreconditions.condition[index].parameter3,
//				UnmetPreconditions.condition[index].value3,
//				UnmetPreconditions.met[index]);
//				}
//	printf("Number Action Effects: %d \n",ActionEffects.numeffects);
//	for (index=0;index<ActionEffects.numeffects;index++)
//		{
//		printf("Index: %d Condition: %d P1: %d %d P2: %d %d P3: %d %d Ex: %d Duration: %d\n",
//				index,
//				ActionEffects.effect[index].condition,
//				ActionEffects.effect[index].parameter1,
//				ActionEffects.effect[index].value1,
//				ActionEffects.effect[index].parameter2,
//				ActionEffects.effect[index].value2,
//				ActionEffects.effect[index].parameter3,
//				ActionEffects.effect[index].value3,
//				ActionEffects.executed[index],
//				ActionEffects.duration[index]);
//				}
//	printf("Number Causal Links: %d \n",CausalLinks.numcausallinks);
//	for (index=0;index<CausalLinks.numcausallinks;index++)
//		{
//		printf("Index: %d Effect Index: %d Precondition Index: %d\n",
//				index,
//				CausalLinks.actioneffect[index],
//				CausalLinks.unmetprecondition[index]);
//				}
//	printf("Number Orderings: %d \n",Orderings.numorderings);
//	for (index=0;index<Orderings.numorderings;index++)
//		{
//		printf("Index: %d Predecessor Precondition Index: %d Successor Precondition Index: %d\n",
//				index,
//				Orderings.predecessor[index],
//				Orderings.successor[index]);
//				}

//	printf("Number of routes to goal: %d  Best route: %d \n\n", SearchGoalNodes.numgoalnodes, SearchGoalNodes.lowestcostnode);
	
	return;

	}


//	--------------------------------------------------------------------------
// 	Attempt to find an Effect to meet a Precondition
//	--------------------------------------------------------------------------
//	Description:	Look through the array of possible Effects to find one that
//					achieves the Unmet Precondition.  Also identify any preconditions
//					of the effect.
//
//	Parameters:		int		Index to the Unmet Preconidition being addressed.
//							
//	Returns:		int		Index to the Effect or -1 if none exists

int FindEffect(int UnmetIndex)
	{
	int effect = -1;
	int index, oldindex;
	int preconditionindex;


//	printf("FindEffect Routine ..... UnmetIndex: %d\n", UnmetIndex);
	
	// Check each possible Effect for a match to the Precondition	
	for (index=0;((index<Effects.numeffects)&&(effect==-1));index++)
		{
	
//		printf("Index: %d Precon: %d %d %d %d  Effect: %d %d %d %d\n", 
//				index,
//				UnmetPreconditions.condition[UnmetIndex].condition,
//				UnmetPreconditions.condition[UnmetIndex].parameter1,
//				UnmetPreconditions.condition[UnmetIndex].parameter2,
//				UnmetPreconditions.condition[UnmetIndex].parameter3,
//				Effects.effect[index].condition,
//				Effects.effect[index].parameter1,
//				Effects.effect[index].parameter2,
//				Effects.effect[index].parameter3);
	
		if ((UnmetPreconditions.condition[UnmetIndex].condition  == Effects.effect[index].condition)&&
			(UnmetPreconditions.condition[UnmetIndex].parameter1 == Effects.effect[index].parameter1)&&
			(UnmetPreconditions.condition[UnmetIndex].parameter2 == Effects.effect[index].parameter2)&&
			(UnmetPreconditions.condition[UnmetIndex].parameter3 == Effects.effect[index].parameter3))
			{
			effect = index;
			}
		}

//	printf("Effect: %d\n", effect);
	
	
	// Found an Effect to meet the Precondition so flag it as 'met'
	// ******* Might want to use this as an 'Execution Met' flag rather than a 'Plan Met' flag *********
	//if (effect!=-1)
	//	{
	//	UnmetPreconditions.met[UnmetIndex] = true;
	//	}
	
	// For the MOVETO effect, generate a physcial route to the location and identify 
	// any additional preconditions that need to be addressed
	if (Effects.effect[effect].condition==EFFECT_MOVETO)
		{

//		printf("Effect = MOVETO\n");

		// (Use globals to pass the start/destination XY)
		startx = Agent.x;
		starty = Agent.y;
		destinationx = UnmetPreconditions.condition[UnmetIndex].value2;
		destinationy = UnmetPreconditions.condition[UnmetIndex].value3;
		GenerateSearchTree(false);

		// Check whether a valid route exists, else fail
		if (SearchGoalNodes.numgoalnodes > 0)
			{
			
			// Add the MoveTo action event
			AddActionEffect(Effects.effect[effect].condition,
							Effects.effect[effect].parameter1,UnmetPreconditions.condition[UnmetIndex].value1,
							Effects.effect[effect].parameter2,UnmetPreconditions.condition[UnmetIndex].value2,
							Effects.effect[effect].parameter3,UnmetPreconditions.condition[UnmetIndex].value3,
							SearchGoalNodes.lowestcostvalue);

			// Add a Causal Link to link the index of the Action Effect to the index of the Precondition it meets
			AddCausalLink((ActionEffects.numeffects-1),UnmetIndex);

			// Select the best route and determine which other preconditions need to be met
			SelectRoute();
			GetRoutePreconditions(UnmetIndex);
			}
		else
			{
			effect = -1;
			}
		}

	if (Effects.effect[effect].condition==EFFECT_UNLOCK)
		{

		// Add the Unlock action effect
		AddActionEffect(Effects.effect[effect].condition,
						Effects.effect[effect].parameter1,UnmetPreconditions.condition[UnmetIndex].value1,
						Effects.effect[effect].parameter2,UnmetPreconditions.condition[UnmetIndex].value2,
						Effects.effect[effect].parameter3,UnmetPreconditions.condition[UnmetIndex].value3,
						DURATION_UNLOCK);

		// Add a Causal Link to link the index of the Action Effect to the index of the Precondition it meets
		AddCausalLink((ActionEffects.numeffects-1),UnmetIndex);


		// The agent must be adjacent to the door to unlock it.  Heuristic rule: As the agent will always
		// start on the left hand side of the door, 'adjacent' will be one square to the left. 
		preconditionindex=AddUnmetPrecondition(PRECON_AT, PARAM_OBJECTAGENT, NULL, PARAM_XPOS, (Door[UnmetPreconditions.condition[UnmetIndex].value1].x-1),
							 PARAM_YPOS, Door[UnmetPreconditions.condition[UnmetIndex].value1].y);

		// Order which precondition should be met first
		AddOrdering(preconditionindex,UnmetIndex);

		// Check that the agent is carrying the key.  If not then GETKEY is a precondition
		if (Agent.gotkey==false)
			{
			oldindex = preconditionindex;
			preconditionindex = AddUnmetPrecondition(PRECON_GETKEY, PARAM_OBJECTKEY, NULL, PARAM_KEYSTATUS, Agent.gotkey, NULL, NULL);

			// Order which precondition should be met first - Unlock or GetKey
			AddOrdering(preconditionindex,UnmetIndex);

			// Also need to order the GetKey and the MoveTo
			AddOrdering(preconditionindex,oldindex);
			}

		}

	if (Effects.effect[effect].condition==EFFECT_OPEN)
		{

//		printf("Effect = OPEN\n");

		// Add the Open action effect
		AddActionEffect(Effects.effect[effect].condition,
						Effects.effect[effect].parameter1,UnmetPreconditions.condition[UnmetIndex].value1,
						Effects.effect[effect].parameter2,UnmetPreconditions.condition[UnmetIndex].value2,
						Effects.effect[effect].parameter3,UnmetPreconditions.condition[UnmetIndex].value3,
						DURATION_OPEN);

		// Add a Causal Link to link the index of the Action Effect to the index of the Precondition it meets
		AddCausalLink((ActionEffects.numeffects-1),UnmetIndex);

		// The agent must be adjacent to the door to open it.  Heuristic rule: As the agent will always
		// start on the left hand side of the door, 'adjacent' will be one square to the left. 
		preconditionindex = AddUnmetPrecondition(PRECON_AT, PARAM_OBJECTAGENT, NULL, PARAM_XPOS, (Door[UnmetPreconditions.condition[UnmetIndex].value1].x-1),
							 PARAM_YPOS, Door[UnmetPreconditions.condition[UnmetIndex].value1].y);

		// Order which precondition should be met first
		AddOrdering(preconditionindex,UnmetIndex);

		// Check whether the door needs to be unlocked.  If it is then add an 'unlock' precondition
		if (Door[UnmetPreconditions.condition[UnmetIndex].value1].lockstatus==DOORLOCKED)
			{
			
			oldindex = preconditionindex;
			preconditionindex = AddUnmetPrecondition(PRECON_DOORUNLOCKED, PARAM_OBJECTDOOR, UnmetPreconditions.condition[UnmetIndex].value1, 
								 PARAM_LOCKSTATUS, DOORUNLOCKED, NULL, NULL);

			// Order which precondition should be met first -  Unlock or Open
			AddOrdering(preconditionindex,UnmetIndex);

			// Also need to order the Unlock and the MoveTo
//			AddOrdering(preconditionindex,oldindex);
			}
		}

	if (Effects.effect[effect].condition==EFFECT_PICKUP)
		{

		// Add the Pickup action effect
		AddActionEffect(Effects.effect[effect].condition,
						Effects.effect[effect].parameter1,UnmetPreconditions.condition[UnmetIndex].value1,
						Effects.effect[effect].parameter2,UnmetPreconditions.condition[UnmetIndex].value2,
						Effects.effect[effect].parameter3,UnmetPreconditions.condition[UnmetIndex].value3,
						DURATION_PICKUP);

		// Add a Causal Link to link the index of the Action Effect to the index of the Precondition it meets
		AddCausalLink((ActionEffects.numeffects-1),UnmetIndex);

		// The agent must be at the same location as the KEY to pick it up
		preconditionindex = AddUnmetPrecondition(PRECON_AT, PARAM_OBJECTAGENT, NULL, PARAM_XPOS, Key.x, PARAM_YPOS, Key.y);
				
		// Order which precondition should be met first
		AddOrdering(preconditionindex,UnmetIndex);
		}

	return (effect);


	}

//	--------------------------------------------------------------------------
// 	Get Preconditions from the Route
//	--------------------------------------------------------------------------
//	Description:	Search backward along the Agent's route to the Exit to 
//					identify any preconditons that need to be addressed before
//					it can be traversed. 
//
//	Parameters:		None	
//							
//	Returns:		None


void GetRoutePreconditions(int UnmetIndex)
	{

	int index, index2, preconditionindex;
	
//	preconditionindex=UnmetIndex;
	
	// For each step along the route
	for (index=(Route.numsteps-1);index>=0;index--)
		{

		// Check each of the doors to see whether it is CLOSED and therefore has a precondition of being
		// OPEN before it can be traversed
		for (index2=0;index2<DOORNUM;index2++)
			{
			if ((Door[index2].x == Route.x[index])&&(Door[index2].y == Route.y[index])
				&&(Door[index2].doorstatus == DOORCLOSED))
				{
				preconditionindex = AddUnmetPrecondition(PRECON_DOOROPEN, PARAM_OBJECTDOOR, index2, PARAM_OPENSTATUS, DOOROPEN, NULL, NULL);
				
				// Order which precondition should be met first
				AddOrdering(preconditionindex,UnmetIndex);
				}
			}
		}


//	printf("Number Unmet Preconditions: %d \n",UnmetPreconditions.numpreconditions);
//	for (index=0;index<MAXPRECON;index++)
//		{
//		printf("Index: %d Condition: %d P1: %d %d P2: %d %d P3: %d %d \n",
//				index,
//				UnmetPreconditions.condition[index].condition,
//				UnmetPreconditions.condition[index].parameter1,
//				UnmetPreconditions.condition[index].value1,
//				UnmetPreconditions.condition[index].parameter2,
//				UnmetPreconditions.condition[index].value2,
//				UnmetPreconditions.condition[index].parameter3,
//				UnmetPreconditions.condition[index].value3);
//		}


	}
	


//	--------------------------------------------------------------------------
// 	Add an Unmet Precondition
//	--------------------------------------------------------------------------
//	Description:	Add an unmet precondition that needs to be satisfied to the plan
//
//	Parameters:		int		Condition
//					int		Parameter 1
//					int 	Value 1	
//					int		Parameter 2
//					int 	Value 2	
//					int		Parameter 3
//					int 	Value 3	
//							
//	Returns:		None

int AddUnmetPrecondition(int condition, int param1, int value1, int param2, int value2, int param3, int value3)
	{


	int index,PreconditionIndex=-1;

	// Check whether the exact same precondition has already been added
	for (index=0;((index<UnmetPreconditions.numpreconditions)&&(PreconditionIndex==-1));index++)
		{
		if ((UnmetPreconditions.condition[index].condition == condition) &&
			(UnmetPreconditions.condition[index].parameter1 == param1) &&
			(UnmetPreconditions.condition[index].value1 == value1) &&
			(UnmetPreconditions.condition[index].parameter2 == param2) &&
			(UnmetPreconditions.condition[index].value2 == value2) &&
			(UnmetPreconditions.condition[index].parameter3 == param3) &&
			(UnmetPreconditions.condition[index].value3 == value3))
			{
			PreconditionIndex=index;
			}
		}

	// If not, then add it
	if (PreconditionIndex==-1)
		{
		UnmetPreconditions.condition[UnmetPreconditions.numpreconditions].condition   	= condition;
		UnmetPreconditions.condition[UnmetPreconditions.numpreconditions].parameter1  	= param1;
		UnmetPreconditions.condition[UnmetPreconditions.numpreconditions].value1		= value1;
		UnmetPreconditions.condition[UnmetPreconditions.numpreconditions].parameter2	= param2;
		UnmetPreconditions.condition[UnmetPreconditions.numpreconditions].value2		= value2;
		UnmetPreconditions.condition[UnmetPreconditions.numpreconditions].parameter3	= param3;
		UnmetPreconditions.condition[UnmetPreconditions.numpreconditions].value3		= value3;
		UnmetPreconditions.met[UnmetPreconditions.numpreconditions]						= false;
		UnmetPreconditions.numpreconditions++;			
		PreconditionIndex=(UnmetPreconditions.numpreconditions-1);
		}
	
	return (PreconditionIndex);
	
	}


//	--------------------------------------------------------------------------
// 	Add an Action Effect
//	--------------------------------------------------------------------------
//	Description:	Add an Action Effect to the plan
//
//	Parameters:		int		Condition
//					int		Parameter 1
//					int 	Value 1	
//					int		Parameter 2
//					int 	Value 2	
//					int		Parameter 3
//					int 	Value 3	
//							
//	Returns:		None

void AddActionEffect(int condition, int param1, int value1, int param2, int value2, int param3, int value3, int duration)
	{

	ActionEffects.effect[ActionEffects.numeffects].condition   	= condition;
	ActionEffects.effect[ActionEffects.numeffects].parameter1  	= param1;
	ActionEffects.effect[ActionEffects.numeffects].value1		= value1;
	ActionEffects.effect[ActionEffects.numeffects].parameter2	= param2;
	ActionEffects.effect[ActionEffects.numeffects].value2		= value2;
	ActionEffects.effect[ActionEffects.numeffects].parameter3	= param3;
	ActionEffects.effect[ActionEffects.numeffects].value3		= value3;
	ActionEffects.duration[ActionEffects.numeffects]			= duration;
	ActionEffects.executed[ActionEffects.numeffects]			= false;
	ActionEffects.numeffects++;			
	
	}


//	--------------------------------------------------------------------------
// 	Add an Ordering 
//	--------------------------------------------------------------------------
//	Description:	Add an Ordering to which the Preconditions are addressed.
//
//	Parameters:		int		The Predecessor precondition index
//					int		The Successor precondition index
//							
//	Returns:		None

void AddOrdering(int predecessor, int successor)
	{

	Orderings.predecessor[Orderings.numorderings] 	= predecessor;
	Orderings.successor[Orderings.numorderings] 	= successor;
	Orderings.executed[Orderings.numorderings] 		= false;
	Orderings.numorderings++;
	
	}


//	--------------------------------------------------------------------------
// 	Add a Causal Link  
//	--------------------------------------------------------------------------
//	Description:	Add a Causal Link to the plan to show which Precondition the
//					Action Effect meets.
//
//	Parameters:		int		Action Effect index
//					int		Precondition index
//							
//	Returns:		None

void AddCausalLink(int actioneffect, int precondition)
	{

	CausalLinks.actioneffect[CausalLinks.numcausallinks] 		= actioneffect;
	CausalLinks.unmetprecondition[CausalLinks.numcausallinks]	= precondition;
	CausalLinks.executed[CausalLinks.numcausallinks]			= false;
	CausalLinks.numcausallinks++;
		
	}


//	--------------------------------------------------------------------------
// 	Set the Agent Goal
//	--------------------------------------------------------------------------
//	Description:	Set the goal situation that the agent is endeavouring to achieve
//
//	Parameters:		None	
//							
//	Returns:		None

void SetAgentGoal()
	{
	
	// Agent Goal is to reach the XPos, YPos of the Exit
	UnmetPreconditions.condition[0].condition  	= PRECON_AT;
	UnmetPreconditions.condition[0].parameter1 	= PARAM_OBJECTAGENT;
	UnmetPreconditions.condition[0].value1		= NULL;
	UnmetPreconditions.condition[0].parameter2	= PARAM_XPOS;
	UnmetPreconditions.condition[0].value2		= Exit.x;
	UnmetPreconditions.condition[0].parameter3	= PARAM_YPOS;
	UnmetPreconditions.condition[0].value3		= Exit.y;
	UnmetPreconditions.met[0]					= false;
	UnmetPreconditions.numpreconditions++;			
	
	}