Tue Jul 7 23:10:22 UTC 2009 Ben Podgursky <bpodgursky@gmail.com>

12 files changed, 354 insertions, 80 deletions

diff --git a/ChangeLog b/ChangeLog
index 4f453325073..0af2e0dec09 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,3 +1,23 @@
+Tue Jul  7 23:10:22 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
+
+        * SA_PlanCommands.h:
+		* SA_PlanHeuristics.cpp:
+		
+		Changed the order in which tasks are chosen to satisfy open conditions.
+		Working on getting it to choose the correct tasks.
+		
+		* PlanCommands.h:
+        * PlanHeuristics.h:
+        * Planner.h:
+        * SA_POP_Types.h:
+        * SA_PlanCommands.cpp:
+        * SA_PlanHeuristics.h:
+        * SA_PlanStrategy.cpp:
+        * SA_WorkingPlan.h:
+        * SA_WorkingPlan.cpp:
+		
+		
+
 Mon Jul  6 20:27:43 UTC 2009  Daniel L.C. Mack  <daniel.l.mack@vanderbilt.edu>
 
         * ChangeLog:
@@ -5,11 +25,11 @@ Mon Jul  6 20:27:43 UTC 2009  Daniel L.C. Mack  <daniel.l.mack@vanderbilt.edu>
         * SA_PlanStrategy.cpp:
         * SA_WorkingPlan.cpp:
 
-	Cleaned up Changelog for consistency.
+        Cleaned up Changelog for consistency.
 
-	Commented out halt for task_inst
+        Commented out halt for task_inst
 
-	Erased fix for Initial Action in favor of throwing an exception.
+        Erased fix for Initial Action in favor of throwing an exception.
 
 
 Mon Jul  6 18:42:01 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
diff --git a/PlanCommands.h b/PlanCommands.h
index ab30a0f268f..5f7cf6a894f 100644
--- a/PlanCommands.h
+++ b/PlanCommands.h
@@ -166,7 +166,7 @@ namespace SA_POP {
     /**
      * @param tasks  Ordered list of tasks.
      */
-    virtual void set_tasks (const TaskList &tasks) = 0;
+    virtual void set_tasks (const TaskChoiceList &tasks) = 0;
 
     /// Set causal link info to add to the plan with task.
     /**
diff --git a/PlanHeuristics.h b/PlanHeuristics.h
index 941e60089cf..22867c60275 100644
--- a/PlanHeuristics.h
+++ b/PlanHeuristics.h
@@ -79,7 +79,15 @@ namespace SA_POP {
      *
      * @return  Sorted list of tasks that satisfy given condition.
      */
-    virtual TaskList choose_task (Condition open_cond) = 0;
+    virtual TaskChoiceList choose_task (Condition open_cond) = 0;
+
+    /// Choose the (ordering of) task(s) to satisfy an open condition.  Better
+    /**
+     * @param open_cond  Open condition to satisfy.
+     *
+     * @return  Sorted list of tasks that satisfy given condition.
+     */
+	virtual TaskChoiceList choose_task_fair (Condition open_cond) = 0;
 
   protected:
     /// Pointer to Planner object.
diff --git a/Planner.h b/Planner.h
index b1d3b79349e..30b0e44ebcc 100644
--- a/Planner.h
+++ b/Planner.h
@@ -520,6 +520,7 @@ namespace SA_POP {
    */
   virtual void print_graph (std::basic_ostream<char, std::char_traits<char> >& strm, std::map<std::string, std::string>& graphmap);
 
+  virtual WorkingPlan*  get_working_plan(void){return this->working_plan_;};
 
 
   protected:
diff --git a/SA_POP_Types.h b/SA_POP_Types.h
index 9ff2defbeca..d740055dc18 100644
--- a/SA_POP_Types.h
+++ b/SA_POP_Types.h
@@ -160,6 +160,42 @@ namespace SA_POP {
 
   /// Type of a task implementation parameter value.
   typedef std::string ImplParamValue;
+  enum TaskChoiceType{REUSE_INST, NEW_INST};
+  struct TaskChoice {
+	
+		TaskChoiceType choice;
+		TaskInstID task_inst_id;
+		TaskID task_id;
+	};
+
+  ///List of TaskChoices
+  typedef std::list<TaskChoice> TaskChoiceList;
+
+  typedef std::map <TaskInstID, TaskID> InstToTaskMap;
+
+  struct SortTaskByTime{
+	
+	  TaskID task_id;
+	  TaskInstID last_instance;
+	
+	  void note_instance(TaskInstID instance){
+		  if (last_instance > instance){
+			this->last_instance = instance;
+		  }
+	  }
+
+	 bool operator<(const SortTaskByTime & s) {
+		return this->last_instance < s.last_instance;
+	}
+	 bool operator!=(const SortTaskByTime & s) {
+		return this->last_instance != s.last_instance;
+	}
+	 bool operator==(const SortTaskByTime & s) {
+		return this->last_instance == s.last_instance;
+	}
+
+  };
+
 
   /// Type of an implementation parameter.
   struct ImplParam {
diff --git a/SA_PlanCommands.cpp b/SA_PlanCommands.cpp
index 174e9d3afb6..d46a7a8e94c 100644
--- a/SA_PlanCommands.cpp
+++ b/SA_PlanCommands.cpp
@@ -186,23 +186,18 @@ bool SA_AddTaskCmd::execute_next (void)
 
   bool isInitial = false;
 
-  if (this->tasks_.empty ())
+  if (this->tasks_.empty())
     return false;
 
-  if(this->tasks_.front() == 20){
+  /*
+  if(this->tasks_.front().task_id == 20 && this->tasks_.front){
     isInitial = true;
   }
+  */
 
   this->working_plan_->execute (this);
   this->num_tries_++;
 
-
-  
-  
-
-
-  
-
   return true;
 };
 
@@ -230,17 +225,17 @@ std::string SA_AddTaskCmd::get_log_text (void)
   log_str += "Adding Task (CommandID ";
   log_str += to_string (this->get_id ());
   log_str += "): ";
-  TaskList::iterator task_iter = this->tasks_.begin ();
+  TaskChoiceList::iterator task_iter = this->tasks_.begin ();
   if (task_iter == this->tasks_.end ())
     log_str += "[NO TASKS TO ADD]";
   else
-    log_str += to_string (*task_iter);
+    log_str += to_string (task_iter->task_id);
 
   return log_str;
 };
 
 // Set (ordered) list of tasks to add (one per execution) to the plan.
-void SA_AddTaskCmd::set_tasks (const TaskList &tasks)
+void SA_AddTaskCmd::set_tasks (const TaskChoiceList &tasks)
 {
   if (!this->tasks_.empty ())
     throw "SA_POP::SA_AddTaskCmd::set_tasks (): called while current task list is not empty.";
@@ -281,10 +276,14 @@ TaskInstSet SA_AddTaskCmd::get_satisfied_tasks(void){
 }
 
 /// Check if the instance id used by the task of this command already exists.
+
+
 bool SA_AddTaskCmd::inst_exists (void)
 {
-	 return !this->used_task_insts_.empty();
+	return (this->last_task_choice_.choice == REUSE_INST);
+
 }
+
 // Constructor.
 SA_AssocTaskImplCmd::SA_AssocTaskImplCmd (SA_WorkingPlan *working_plan)
 : working_plan_ (working_plan),
diff --git a/SA_PlanCommands.h b/SA_PlanCommands.h
index 4eb39e15a96..3bb970473b4 100644
--- a/SA_PlanCommands.h
+++ b/SA_PlanCommands.h
@@ -219,7 +219,7 @@ namespace SA_POP {
     /**
      * @param tasks  Ordered list of tasks.
      */
-    virtual void set_tasks (const TaskList &tasks);
+    virtual void set_tasks (const TaskChoiceList &tasks);
 
     /// Set causal link info to add to the plan with task.
     /**
@@ -255,7 +255,7 @@ namespace SA_POP {
     SA_WorkingPlan *working_plan_;
 
     /// Ordered list of tasks to try adding to the plan.
-    TaskList tasks_;
+    TaskChoiceList tasks_;
 
     /// Open condition satisfied by these tasks.
     Condition cond_;
@@ -272,6 +272,8 @@ namespace SA_POP {
     /// Instance of last task tried.
     TaskInstID last_task_inst_;
 
+	TaskChoice last_task_choice_;
+
     /// Number of tasks tried.
     size_t num_tries_;
     
diff --git a/SA_PlanHeuristics.cpp b/SA_PlanHeuristics.cpp
index f4eefbbfbdd..61c70128ed1 100644
--- a/SA_PlanHeuristics.cpp
+++ b/SA_PlanHeuristics.cpp
@@ -15,6 +15,9 @@
 #include "SA_POP_Types.h"
 #include "SA_PlanHeuristics.h"
 #include "Planner.h"
+#include "SA_WorkingPlan.h"
+#include <algorithm>
+#include <vector>
 
 using namespace SA_POP;
 
@@ -64,8 +67,122 @@ SA_TaskStrategy::~SA_TaskStrategy (void)
   // Nothing to do.
 };
 
+
+
+TaskChoiceList SA_TaskStrategy::choose_task_fair (Condition open_cond)
+{
+  TaskSet tasks = this->planner_->get_satisfying_tasks (open_cond);
+
+  /*
+  if(this->planner_->init_added){
+    tasks.erase(20);
+  }
+  */
+    
+  // Add tasks to map with EU (to sort).
+  std::multimap<EUCalc, TaskID> task_map;
+  task_map.clear ();
+  for (TaskSet::iterator iter = tasks.begin (); iter != tasks.end (); iter++)
+  {
+    task_map.insert (std::make_pair (
+      this->planner_->get_task_future_eu (*iter), *iter));
+  }
+
+  std::multimap<TaskID, TaskInstID> tasks_to_insts;
+
+  SA_WorkingPlan* working_plan = (SA_WorkingPlan*)this->planner_->get_working_plan();
+
+  InstToTaskMap inst_task_map = working_plan->get_task_insts();
+
+  for(InstToTaskMap::iterator it = inst_task_map.begin(); 
+	  it != inst_task_map.end(); it++){
+
+		  tasks_to_insts.insert(std::pair<TaskID, TaskInstID>(it->second, it->first));
+  }
+
+  // Add tasks to list in reverse order of map (highest EU first).
+  TaskChoiceList task_list;
+  task_list.clear ();
+
+  std::vector<SortTaskByTime> tasks_with_existing_instances;
+
+  for (std::multimap<EUCalc, TaskID>::reverse_iterator iter = task_map.rbegin ();
+    iter != task_map.rend (); iter++)
+  {
+
+	  if(tasks_to_insts.lower_bound(iter->second) == tasks_to_insts.upper_bound(iter->second)){
+	  
+		TaskChoice task_choice;
+		task_choice.choice = NEW_INST;
+		task_choice.task_id = iter->second;
+		task_choice.task_inst_id = -2;
+
+		task_list.push_back(task_choice);
+	  }else{
+
+		  SortTaskByTime to_sort;
+		  to_sort.task_id = iter->second;
+		  to_sort.last_instance = 0;
+
+		  for(std::multimap<TaskID, TaskInstID>::iterator it = tasks_to_insts.lower_bound(iter->second); it != tasks_to_insts.upper_bound(iter->second);
+			  it++){
+
+			TaskChoice task_choice;
+			task_choice.choice = REUSE_INST;
+			task_choice.task_id = it->first;
+			task_choice.task_inst_id = it->second;
+			task_list.push_back(task_choice);
+
+			to_sort.note_instance(it->second);
+		  }
+	
+		  tasks_with_existing_instances.push_back(to_sort);
+	  
+	  }
+  }
+
+  std::sort(tasks_with_existing_instances.begin(), tasks_with_existing_instances.end());
+
+  for(std::vector<SortTaskByTime>::iterator it = tasks_with_existing_instances.begin(); it != tasks_with_existing_instances.end(); it++){
+		
+
+	  if(it->task_id== 20 && this->planner_->init_added){
+			continue;
+	  }
+
+		TaskChoice task_choice;
+		task_choice.choice = NEW_INST;
+		task_choice.task_id = it->task_id;
+		task_choice.task_inst_id = -2;
+
+  }
+
+  	  /*
+
+	  for(std::multimap<TaskID, TaskInstID>::iterator it = tasks_to_insts.lower_bound(iter->second); it != tasks_to_insts.upper_bound(iter->second);
+		  it++){
+
+	    TaskChoice task_choice;
+		task_choice.choice = REUSE_INST;
+		task_choice.task_id = it->first;
+		task_choice.task_inst_id = it->second;
+
+		task_list.push_back(task_choice);
+	  }
+	  */
+
+
+
+  return task_list;
+
+
+};
+
+
+
+
 // Choose the (ordering of) task(s) to satisfy an open condition.
-TaskList SA_TaskStrategy::choose_task (Condition open_cond)
+TaskChoiceList SA_TaskStrategy::choose_task (Condition open_cond)
 {
   TaskSet tasks = this->planner_->get_satisfying_tasks (open_cond);
 
@@ -82,14 +199,49 @@ TaskList SA_TaskStrategy::choose_task (Condition open_cond)
       this->planner_->get_task_future_eu (*iter), *iter));
   }
 
+  std::multimap<TaskID, TaskInstID> tasks_to_insts;
+
+  SA_WorkingPlan* working_plan = (SA_WorkingPlan*)this->planner_->get_working_plan();
+
+  InstToTaskMap inst_task_map = working_plan->get_task_insts();
+
+  for(InstToTaskMap::iterator it = inst_task_map.begin(); 
+	  it != inst_task_map.end(); it++){
+
+		  tasks_to_insts.insert(std::pair<TaskID, TaskInstID>(it->second, it->first));
+  }
+
   // Add tasks to list in reverse order of map (highest EU first).
-  TaskList task_list;
+  TaskChoiceList task_list;
   task_list.clear ();
+
   for (std::multimap<EUCalc, TaskID>::reverse_iterator iter = task_map.rbegin ();
     iter != task_map.rend (); iter++)
   {
-    task_list.push_back (iter->second);
+
+	  for(std::multimap<TaskID, TaskInstID>::iterator it = tasks_to_insts.lower_bound(iter->second); it != tasks_to_insts.upper_bound(iter->second);
+		  it++){
+
+	    TaskChoice task_choice;
+		task_choice.choice = REUSE_INST;
+		task_choice.task_id = it->first;
+		task_choice.task_inst_id = it->second;
+
+		task_list.push_back(task_choice);
+	  }
+	  
+	TaskChoice task_choice;
+	task_choice.choice = NEW_INST;
+	task_choice.task_id = iter->second;
+	task_choice.task_inst_id = -2;
+
+	task_list.push_back(task_choice);
+	//  task_list.push_back(
+//	  task_list.choice_list.push_back (std::pair<int, int>(TASK_INST_ID, iter->second));
   }
+
+
+
   return task_list;
 
 
diff --git a/SA_PlanHeuristics.h b/SA_PlanHeuristics.h
index ab4a859d982..1f083d58942 100644
--- a/SA_PlanHeuristics.h
+++ b/SA_PlanHeuristics.h
@@ -15,6 +15,7 @@
 #ifndef SA_POP_SA_PLAN_HEURISTICS_H_
 #define SA_POP_SA_PLAN_HEURISTICS_H_
 
+
 #include "SA_POP_Types.h"
 #include "PlanHeuristics.h"
 
@@ -70,7 +71,15 @@ namespace SA_POP {
      *
      * @return  Sorted list of tasks that satisfy given condition.
      */
-    virtual TaskList choose_task (Condition open_cond);
+    virtual TaskChoiceList choose_task (Condition open_cond);
+  
+	/// Choose the (ordering of) task(s) to satisfy an open condition.  The cool way
+	 /**
+     * @param open_cond  Open condition to satisfy.
+     *
+     * @return  Sorted list of tasks that satisfy given condition.
+     */
+	virtual TaskChoiceList choose_task_fair (Condition open_cond);
   };
 
   /**
@@ -101,6 +110,8 @@ namespace SA_POP {
   };
 
 
+
+
 };  /* SA_POP namespace */
 
 
diff --git a/SA_PlanStrategy.cpp b/SA_PlanStrategy.cpp
index d026ee4535f..1531760b586 100644
--- a/SA_PlanStrategy.cpp
+++ b/SA_PlanStrategy.cpp
@@ -480,10 +480,10 @@ void SA_PlanStrategy::execute (SA_AddOpenThreatsCmd *cmd)
     iter != cmd->threats_.end (); iter++)
   {
     CLThreat threat = *iter;
-    if(this->closed_threats_.find(threat) == this->closed_threats_.end())
-    {
+    //if(this->closed_threats_.find(threat) == this->closed_threats_.end())
+   // {
       this->open_threats_.insert (threat);
-    }
+   // }
   }
 };
 
@@ -510,7 +510,7 @@ void SA_PlanStrategy::execute (SA_RemoveOpenThreatsCmd * cmd)
   {
       CLThreat threat = *cond_iter;
 		  this->open_threats_.erase (threat);
-      this->closed_threats_.insert (threat);
+     // this->closed_threats_.insert (threat);
   }
 };
 
@@ -521,7 +521,7 @@ void SA_PlanStrategy::undo (SA_RemoveOpenThreatsCmd * cmd)
     iter != cmd->threats_.end (); iter++)
   {
     CLThreat threat = *iter;
-    this->closed_threats_.erase (threat);
+    //this->closed_threats_.erase (threat);
     this->open_threats_.insert (threat);
   }
 };
@@ -535,7 +535,7 @@ AddTaskCmd *SA_PlanStrategy::satisfy_cond (Condition open_cond)
   // Get add task command.
   AddTaskCmd *add_task_cmd =
     static_cast<AddTaskCmd *> (this->add_task_cmd_->clone ());
-  TaskList task_list = this->task_choice_->choose_task (open_cond);
+  TaskChoiceList task_list = this->task_choice_->choose_task_fair (open_cond);
   add_task_cmd->set_id (this->get_next_cmd_id ());
   add_task_cmd->set_tasks (task_list);
 
diff --git a/SA_WorkingPlan.cpp b/SA_WorkingPlan.cpp
index 47126b4d68b..4f4357a55cf 100644
--- a/SA_WorkingPlan.cpp
+++ b/SA_WorkingPlan.cpp
@@ -419,7 +419,7 @@ const Plan& SA_WorkingPlan::get_plan (void)
   //****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP
 
   // Add task instances to plan.
-  for (SA_WorkingPlan::InstToTaskMap::iterator iter =
+  for (InstToTaskMap::iterator iter =
     this->task_insts_.begin ();
     iter != this->task_insts_.end (); iter++)
   {
@@ -500,7 +500,7 @@ const Plan& SA_WorkingPlan::get_plan (void)
       plan_conn.second_task_inst = cl_iter->second.second;
 
       // Get task ids from instances.
-      SA_WorkingPlan::InstToTaskMap::iterator inst_task_iter;
+      InstToTaskMap::iterator inst_task_iter;
 
       inst_task_iter = this->task_insts_.find (cl_iter->second.first);
       if (inst_task_iter == this->task_insts_.end ())
@@ -538,7 +538,7 @@ const TaskInstSet *SA_WorkingPlan::get_prec_set (TaskInstID task_inst, Precedenc
 // Get task id of a task instance.
 TaskID SA_WorkingPlan::get_task_from_inst (TaskInstID inst_id)
 {
-  SA_WorkingPlan::InstToTaskMap::iterator iter =
+  InstToTaskMap::iterator iter =
     this->task_insts_.find (inst_id);
   if (iter == this->task_insts_.end ())
     throw "SA_POP::SA_WorkingPlan::get_task_from_inst (): requested instance id does not exist.";
@@ -703,7 +703,7 @@ void SA_WorkingPlan::generate_all_threats(void)
 
                 
              
-              
+              bool a = true;
               }
              }
         }
@@ -810,6 +810,40 @@ AdjustMaxTimesCmd *SA_WorkingPlan::get_AdjustMaxTimesCmd (void)
 
 void SA_WorkingPlan::execute (SA_AddTaskCmd *cmd)
 {
+
+  TaskChoice task_choice = cmd->tasks_.front();
+  TaskID task = task_choice.task_id;
+  TaskInstID task_inst;
+  Condition cond = cmd->cond_;
+
+  cmd->last_task_choice_ = task_choice;
+
+  if(task_choice.choice == REUSE_INST){
+
+		task_inst = task_choice.task_inst_id;
+			this->reused_insts_.insert(task_inst);
+	cmd->tasks_.pop_front();
+  }
+  else if(task_choice.choice == NEW_INST){
+
+    if(task == 20 && this->planner_->init_added)
+    {
+      throw "Reached SA_WorkingPlan::execute (SA_AddTaskCmd *cmd) for Special Initial Action after it was already existing instance tried";
+    }
+
+	if(task == 20)
+		this->planner_->init_added =  true;
+  
+    task_inst = this->get_next_inst_id ();
+    // Add task instance.
+    this->task_insts_.insert (std::make_pair (task_inst, task));
+    //   Remove this task from tasks still to try.
+    cmd->tasks_.pop_front ();
+  }
+
+
+/*
+
 	// Get task ID, condition, and instance ID.
   TaskID task = cmd->tasks_.front ();
 
@@ -827,6 +861,7 @@ void SA_WorkingPlan::execute (SA_AddTaskCmd *cmd)
         cmd->used_task_insts_.insert(iter->first);
   }
 
+
   // If there are no/no-more existing task instances to try, create new task instance.
   if(cmd->used_task_insts_.empty()) 
   {
@@ -838,60 +873,55 @@ void SA_WorkingPlan::execute (SA_AddTaskCmd *cmd)
 	  if(task == 20)
 		  this->planner_->init_added =  true;
     
-      task_inst = this->get_next_inst_id ();
-      // Add task instance.
-      this->task_insts_.insert (std::make_pair (task_inst, task));
-      //   Remove this task from tasks still to try.
-      cmd->tasks_.pop_front ();
-    
-    
     
   }
   else
   {
-	  // Reuse the task instance
-    task_inst = *cmd->used_task_insts_.begin();
-		this->reused_insts_.insert(task_inst);
+		  // Reuse the task instance
+		task_inst = *cmd->used_task_insts_.begin();
+			this->reused_insts_.insert(task_inst);
 
-    // NOTE: task instance removed from existing task instances to try in undo.
+		// NOTE: task instance removed from existing task instances to try in undo.
 	}
 
-  std::ostringstream debug_text;
-  debug_text << "SA_WorkingPlan::execute (SA_AddTaskCmd *cmd): Adding task (" << task << ") instance (" << task_inst << ") for condition (" << cond.id << ").";
-
-  SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+*/
 
-  // Create causal links for each task instance depending on the condition.
-  for (TaskInstSet::iterator inst_iter = cmd->task_insts_.begin ();
-    inst_iter != cmd->task_insts_.end (); inst_iter++)
-  {
-    CausalLink clink;
-    clink.cond = cond;
-    clink.first = task_inst;
-    clink.second = *inst_iter;
-    CondToCLinksMap::iterator links_iter;
-
-    // Check whether this causal link already exists in working plan.
-    for (links_iter = this->causal_links_.lower_bound (cond);links_iter != this->causal_links_.upper_bound (cond);links_iter++)
-      if(links_iter->second == clink)
-        break;
-    // If causal link not found in working plan, add it to causal links and ordering links.
-    if(links_iter==this->causal_links_.upper_bound(cond))
-    {
-      std::ostringstream debug_text2;
-      debug_text2 << "SA_WorkingPlan::execute (SA_AddTaskCmd *cmd): Adding causal link (" << clink.first << " -" << clink.cond.id << "-> " << clink.second << ")";
-      SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text2.str ());
+	  std::ostringstream debug_text;
+	  debug_text << "SA_WorkingPlan::execute (SA_AddTaskCmd *cmd): Adding task (" << task << ") instance (" << task_inst << ") for condition (" << cond.id << ").";
 
-      this->causal_links_.insert (std::make_pair (cond, clink));
+	  SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
 
-      if(clink.second != GOAL_TASK_INST_ID){
-		
-        this->ordering_links.insert(std::pair<TaskInstID, TaskInstID>(clink.first, clink.second));
-        this->reverse_ordering_links.insert(std::pair<TaskInstID, TaskInstID>(clink.second, clink.first));
-      }
-       cmd->added_links_.insert(clink);
-    }
-  }
+	  // Create causal links for each task instance depending on the condition.
+	  for (TaskInstSet::iterator inst_iter = cmd->task_insts_.begin ();
+		inst_iter != cmd->task_insts_.end (); inst_iter++)
+	  {
+			CausalLink clink;
+			clink.cond = cond;
+			clink.first = task_inst;
+			clink.second = *inst_iter;
+			CondToCLinksMap::iterator links_iter;
+
+			// Check whether this causal link already exists in working plan.
+			for (links_iter = this->causal_links_.lower_bound (cond);links_iter != this->causal_links_.upper_bound (cond);links_iter++)
+			if(links_iter->second == clink)
+				break;
+				// If causal link not found in working plan, add it to causal links and ordering links.
+			if(links_iter==this->causal_links_.upper_bound(cond))
+			{
+				  std::ostringstream debug_text2;
+				  debug_text2 << "SA_WorkingPlan::execute (SA_AddTaskCmd *cmd): Adding causal link (" << clink.first << " -" << clink.cond.id << "-> " << clink.second << ")";
+				  SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text2.str ());
+
+				  this->causal_links_.insert (std::make_pair (cond, clink));
+
+				  if(clink.second != GOAL_TASK_INST_ID){
+						
+					this->ordering_links.insert(std::pair<TaskInstID, TaskInstID>(clink.first, clink.second));
+					this->reverse_ordering_links.insert(std::pair<TaskInstID, TaskInstID>(clink.second, clink.first));
+					}
+			cmd->added_links_.insert(clink);
+			}
+		}
 
   // Set this task and task instance as last used by command.
 	cmd->last_task_ = task;
@@ -939,6 +969,18 @@ void SA_WorkingPlan::execute (SA_AddTaskCmd *cmd)
 
 void SA_WorkingPlan::undo (SA_AddTaskCmd *cmd)
 {
+
+	  if(cmd->last_task_ == 20)
+  {
+	 planner_->init_added = false;
+  }
+	  if(cmd->last_task_choice_.choice == NEW_INST){
+		this->task_insts_.erase (this->task_insts_.find(cmd->last_task_inst_));  
+	  }else{
+		  this->reused_insts_.erase(this->reused_insts_.find(cmd->last_task_inst_));
+	  }
+
+/*
   if(cmd->last_task_ == 20)
   {
 	 planner_->init_added = false;
@@ -954,7 +996,7 @@ void SA_WorkingPlan::undo (SA_AddTaskCmd *cmd)
 	  cmd->used_task_insts_.erase(cmd->used_task_insts_.begin());
     this->reused_insts_.erase(this->reused_insts_.find(cmd->last_task_inst_));
   }
-
+*/
 
 
 
diff --git a/SA_WorkingPlan.h b/SA_WorkingPlan.h
index 0d522d66dd5..a41d1c42e11 100644
--- a/SA_WorkingPlan.h
+++ b/SA_WorkingPlan.h
@@ -331,6 +331,9 @@ namespace SA_POP {
 
     virtual void generate_all_threats(void);
 
+
+	virtual InstToTaskMap get_task_insts(void){return task_insts_;};
+
   protected:
     // ************************************************************************
     // State information.
@@ -345,7 +348,7 @@ namespace SA_POP {
     /// Task instance ID to use for next instance created.
     TaskInstID next_inst_id_;
 
-    typedef std::map <TaskInstID, TaskID> InstToTaskMap;
+
     /// Task instances in plan (mapping to task id).
     InstToTaskMap task_insts_;