1 files changed, 68 insertions, 6 deletions

diff --git a/SA_POP/SA_PlanHeuristics.cpp b/SA_POP/SA_PlanHeuristics.cpp
index 88bd07bee3a..dfc0fc568de 100644
--- a/SA_POP/SA_PlanHeuristics.cpp
+++ b/SA_POP/SA_PlanHeuristics.cpp
@@ -98,7 +98,7 @@ Condition SA_CondStrategy::choose_cond_suspension_most_constrained (const OpenCo
       return iter->first;
   }
 
-	std::map<int, std::pair<Condition, TaskInstID> > by_num_satisfying;
+	std::multimap<int, std::pair<Condition, TaskInstID> > by_num_satisfying;
 
 	for(OpenCondMap::const_iterator iter = open_conds.begin();
 		iter != open_conds.end(); iter++){
@@ -111,7 +111,7 @@ Condition SA_CondStrategy::choose_cond_suspension_most_constrained (const OpenCo
 
   SA_WorkingPlan* working_plan = (SA_WorkingPlan*)this->planner_->get_working_plan();
 
-  for(std::map<int, std::pair<Condition, TaskInstID> >::iterator it = by_num_satisfying.begin(); it != by_num_satisfying.end(); it++){
+  for(std::multimap<int, std::pair<Condition, TaskInstID> >::iterator it = by_num_satisfying.begin(); it != by_num_satisfying.end(); it++){
 	  
 
 	if(!working_plan->condition_in_suspended(it->second.first, it->second.second)){
@@ -259,6 +259,72 @@ TaskChoiceList SA_TaskStrategy::choose_task_fair (Condition open_cond)
 
 };
 
+TaskChoiceList SA_TaskStrategy::choose_task_once(Condition open_cond){
+  TaskSet tasks = this->planner_->get_satisfying_tasks (open_cond);
+
+  // 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 ();
+
+  //If init can handle it, put it on here first
+  if(this->planner_->get_cond_val(open_cond.id) == open_cond.value){
+		TaskChoice init_choice;
+		init_choice.choice = REUSE_INST;
+		init_choice.task_id = INIT_TASK_ID;
+		init_choice.task_inst_id = INIT_TASK_INST_ID;
+
+		task_list.push_back(init_choice);
+  }
+
+
+  for (std::multimap<EUCalc, TaskID>::reverse_iterator iter = task_map.rbegin ();
+    iter != task_map.rend (); iter++)
+  {
+
+	  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);
+	  }
+	  
+  if(tasks_to_insts.find(iter->second) == tasks_to_insts.end())
+  {
+	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);
+  }
+}
+
+  return task_list;
+}
 
 
 
@@ -266,10 +332,6 @@ TaskChoiceList SA_TaskStrategy::choose_task_fair (Condition open_cond)
 TaskChoiceList SA_TaskStrategy::choose_task (Condition open_cond)
 {
   TaskSet tasks = this->planner_->get_satisfying_tasks (open_cond);
-
-//  if(this->planner_->init_added){
-//    tasks.erase(INIT_TASK_ID);
-//  }
     
   // Add tasks to map with EU (to sort).
   std::multimap<EUCalc, TaskID> task_map;