Mon Jun 29 16:59:21 UTC 2009 Ben Podgursky <bpodgursky@gmail.com>

6 files changed, 134 insertions, 42 deletions

diff --git a/ChangeLog b/ChangeLog
index 1a6e1a8b2c7..4198d970f46 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,20 +1,45 @@
+Mon Jun 29 16:59:21 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
+
+        * SA_PlanCommands.cpp:
+	In execute next of AddTaskCmd, was a bug in checking for tasks_.front()==20 if
+		tasks is null.  Swapped with returning false if tasks is empty to return first.
+
+        * SA_PlanHeuristics.cpp:
+	Bug here where tasks were being stored in a map with the EU as the key.  Tasks with the
+		same EU were erasing previous tasks.  Changed to multimap.
+
+        * SA_PlanStrategy.cpp:
+
+	Undo the assoc_impl_cmd if it fails.
+
+        * SA_SchedStrategy.cpp:
+	Bug in satisfy sched.  If energy prop or time balance prop failed, it called undo_through
+		the adjust min times cmds it created, but if recurse plan failed, it left those not undone
+		causing problems in the level down.  Changed to if recurse_plan fails undoing all 
+		commands created here.
+
+        * SA_WorkingPlan.cpp:
+
+	Bug in undo of AddTaskCmd regarding ordering links fixed.
+	Made sure that causal links don't have goal targets in generate threats.
+
 Fri Jun 26 21:53:02 UTC 2009  Daniel L.C. Mack  <daniel.l.mack@vanderbilt.edu>
 
-	
+
         * C2W/C2W_Demo.cpp:
-	Cleaned up and contains code for testing MultiMapList
+        Cleaned up and contains code for testing MultiMapList
 
         * LogGraphOut.cpp:
-	Changes to various display colors	
-        
+        Changes to various display colors
+
         * SANet/SANet.h:
         * SANet/SANet.cpp:
         * SANet/SANode.h:
         * SANet/SANode.cpp:
-	Getters and Setters for accessing components of the SANets
-	
+        Getters and Setters for accessing components of the SANets
+
 
-	* PlanCommands.h:
+        * PlanCommands.h:
         * Planner.h:
         * Planner.cpp:
         * SAPOP_w_ACE.mpc:
@@ -29,11 +54,11 @@ Fri Jun 26 21:53:02 UTC 2009  Daniel L.C. Mack  <daniel.l.mack@vanderbilt.edu>
         * SA_WorkingPlan.cpp:
         * WorkingPlan.h:
 
-	This is in development changes for implementation of causal threat resolution. 
+        This is in development changes for implementation of causal threat resolution.
 
         * SA_POP_Utils.h:
         * SA_POP_Utils.cpp:
-	An implementation of a MultiMap with List like capabilities(i.e. ordering purposes)
+        An implementation of a MultiMap with List like capabilities(i.e. ordering purposes)
 
 Fri Jun 12 20:58:24 UTC 2009  John S. Kinnebrew  <john.s.kinnebrew@vanderbilt.edu>
 
diff --git a/SA_PlanCommands.cpp b/SA_PlanCommands.cpp
index 94ccc2c45c2..8679591772a 100644
--- a/SA_PlanCommands.cpp
+++ b/SA_PlanCommands.cpp
@@ -186,12 +186,14 @@ bool SA_AddTaskCmd::execute_next (void)
 
   bool isInitial = false;
 
+  if (this->tasks_.empty ())
+    return false;
+
   if(this->tasks_.front() == 20){
     isInitial = true;
   }
 
-  if (this->tasks_.empty ())
-    return false;
+
   this->working_plan_->execute (this);
   this->num_tries_++;
 
@@ -752,7 +754,7 @@ bool SA_ResolveCLThreatCmd::execute_next (void)
   }
   else
   {
-    this->undo();
+ //   this->undo();
     return false;
   }
 	
diff --git a/SA_PlanHeuristics.cpp b/SA_PlanHeuristics.cpp
index 21e1c7be467..f4eefbbfbdd 100644
--- a/SA_PlanHeuristics.cpp
+++ b/SA_PlanHeuristics.cpp
@@ -74,7 +74,7 @@ TaskList SA_TaskStrategy::choose_task (Condition open_cond)
   }
     
   // Add tasks to map with EU (to sort).
-  std::map<EUCalc, TaskID> task_map;
+  std::multimap<EUCalc, TaskID> task_map;
   task_map.clear ();
   for (TaskSet::iterator iter = tasks.begin (); iter != tasks.end (); iter++)
   {
@@ -85,12 +85,14 @@ TaskList SA_TaskStrategy::choose_task (Condition open_cond)
   // Add tasks to list in reverse order of map (highest EU first).
   TaskList task_list;
   task_list.clear ();
-  for (std::map<EUCalc, TaskID>::reverse_iterator iter = task_map.rbegin ();
+  for (std::multimap<EUCalc, TaskID>::reverse_iterator iter = task_map.rbegin ();
     iter != task_map.rend (); iter++)
   {
     task_list.push_back (iter->second);
   }
   return task_list;
+
+
 };
 
 
diff --git a/SA_PlanStrategy.cpp b/SA_PlanStrategy.cpp
index 38410b6997a..00dcebbc2eb 100644
--- a/SA_PlanStrategy.cpp
+++ b/SA_PlanStrategy.cpp
@@ -199,6 +199,7 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
       return true;
 
 
+
     SA_POP_DEBUG(SA_POP_DEBUG_NORMAL, "Backtracking from task addition...");
     // Undo addition of causal link threats from this task.
     if (are_threats)
@@ -241,7 +242,10 @@ bool SA_PlanStrategy::satisfy_everything(){
     assoc_impl_cmd->set_id (this->get_next_cmd_id ());
     assoc_impl_cmd->set_assoc (this->cur_task_inst_, impl_list);
     this->planner_->add_command (assoc_impl_cmd);
-		 this->cur_task_inst_ = assoc_impl_cmd->get_task_inst ();
+
+	TaskInstID prev_task_inst = this->cur_task_inst_;
+
+	this->cur_task_inst_ = assoc_impl_cmd->get_task_inst ();
 
     while (this->planner_->try_next (assoc_impl_cmd->get_id ())) 
     {
@@ -252,6 +256,7 @@ bool SA_PlanStrategy::satisfy_everything(){
         //this->planner_->undo_command(assoc_impl_cmd->get_id());
       
           this->cur_decision_pt_ = SA_PlanStrategy::IMPL_DECISION;
+	//	  this->cur_task_inst_ = assoc_impl_cmd->get_task_inst ();
       }
   //    assoc_impl_cmd =
   //      static_cast<AssocTaskImplCmd *> (this->assoc_impl_cmd_->clone ());
@@ -267,6 +272,9 @@ bool SA_PlanStrategy::satisfy_everything(){
 
     //Undo the AssocImplCmd
 
+	planner_->undo_command(assoc_impl_cmd->get_id());
+//	this->cur_task_inst_ = prev_task_inst;
+
     return false;
 }
 bool SA_PlanStrategy::satisfy_schedule(void){
@@ -280,7 +288,7 @@ bool SA_PlanStrategy::satisfy_schedule(void){
           if (this->planner_->recurse_sched (this->cur_task_inst_))
             return true;
 
-
+       this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
      return false;
 }
 
@@ -291,7 +299,7 @@ bool SA_PlanStrategy::get_next_threat_resolution(){
         // Choose an open threat to satisfy and remove from open threats.
 
     if(open_threats_.empty()){
-       this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
+
        return satisfy_schedule();
     }
         this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
diff --git a/SA_SchedStrategy.cpp b/SA_SchedStrategy.cpp
index fd3f0c456bc..697a783b8bb 100644
--- a/SA_SchedStrategy.cpp
+++ b/SA_SchedStrategy.cpp
@@ -146,7 +146,13 @@ bool SA_SchedStrategy::satisfy_sched (TaskInstID task_inst)
 	  this->planner_->undo_through(cur_cmd_id);
 	  return false;
   }
-	return this->planner_->recurse_plan ();
+	
+  if(!this->planner_->recurse_plan ()){
+	this->planner_->undo_through(cur_cmd_id);
+	return false;
+  }else{
+	return true;
+  }
 };
 
 // Satisfy fully instantiated plan.
diff --git a/SA_WorkingPlan.cpp b/SA_WorkingPlan.cpp
index 505b2d48648..bb1c6a56bf3 100644
--- a/SA_WorkingPlan.cpp
+++ b/SA_WorkingPlan.cpp
@@ -670,7 +670,8 @@ void SA_WorkingPlan::generate_all_threats(void)
            
             if((threat_effect > 0 && causal_effect < 0 )|| (threat_effect < 0 && causal_effect > 0)){
 
-              if(causal_threatened.first != threat_possibility && causal_threatened.second != threat_possibility)
+              if(causal_threatened.first != threat_possibility && causal_threatened.second != threat_possibility
+				  && causal_threatened.second != -1)
               {
                 TaskID threatened_task1 = this->task_insts_.find(causal_threatened.first)->second;
                 TaskID threatened_task2 = this->task_insts_.find(causal_threatened.second)->second;
@@ -814,7 +815,8 @@ void SA_WorkingPlan::execute (SA_AddTaskCmd *cmd)
       throw "Reached SA_WorkingPlan::execute (SA_AddTaskCmd *cmd) for Special Initial Action after it was already existing instance tried";
     }
 
-    this->planner_->init_added =  true;
+	if(task == 20)
+		this->planner_->init_added =  true;
   
     task_inst = this->get_next_inst_id ();
     // Add task instance.
@@ -863,6 +865,7 @@ void SA_WorkingPlan::execute (SA_AddTaskCmd *cmd)
       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));
       }
@@ -875,6 +878,45 @@ void SA_WorkingPlan::execute (SA_AddTaskCmd *cmd)
 	cmd->last_task_inst_ = task_inst;
 };
 
+//void note_ordering_link(TaskInstID first, TaskInstID second){
+
+//	std::pair<CountedSchedulingLinks::iterator, CountedSchedulingLinks::iterator>
+//		find_pair = ordering_links.equal_range(first);
+	
+//	CountedSchedulingLinks::iterator find_it;
+
+//	for(find_it = find_pair.first; find_it != find_pair.second; find_it++){
+//		if(find_it->second.first == second){
+///			break;
+//		}
+//	}
+
+//	if(find_it == find_pair.second){
+//		ordering_links.insert(std::pair<TaskInstID, std::pair<TaskInstID, int> >(first, std::pair<TaskInstID, int>(second, 1)));
+//		reverse_ordering_links.insert(std::pair<TaskInstID, std::pair<TaskInstID, int> >(second, std::pair<TaskInstID, int>(first, 1)));
+//	}else{
+		
+	//	int prev_count = find_it->second.second;
+
+	//	ordering_links.insert(std::pair<TaskInstID, std::pair<TaskInstID, int> >(first, std::pair<TaskInstID, int>(second, prev_count+1)));
+	//	reverse_ordering_links.insert(std::pair<TaskInstID, std::pair<TaskInstID, int> >(second, std::pair<TaskInstID, int>(first, prev_count+1)));
+
+//	}
+
+//}
+
+//void decrement_ordering_link(TaskInstID first, TaskInstID second){
+
+//	std::pair<CountedSchedulingLinks::iterator, CountedSchedulingLinks::iterator>
+//		find_pair = ordering_links.equal_range(first);
+
+//	CountedSchedulingLinks::iterator find_it;
+
+//	for(find_it = find_pair.
+
+//}
+
+
 void SA_WorkingPlan::undo (SA_AddTaskCmd *cmd)
 {
 
@@ -915,25 +957,28 @@ void SA_WorkingPlan::undo (SA_AddTaskCmd *cmd)
           this->causal_links_.erase (prev_iter);
           cmd->added_links_.erase(iter);
 
-          
-          std::pair<SchedulingLinks::iterator, SchedulingLinks::iterator> ret =
-          ordering_links.equal_range(clink.first);
-          SchedulingLinks::iterator it;
-          for(it = ret.first; it != ret.second; it++){
-            if(it->second == clink.second){
-              break;
-            }
-          }              
-          this->ordering_links.erase(it);
-
-          ret = reverse_ordering_links.equal_range(clink.second);
-          for(it = ret.first; it != ret.second; it++){
-            if(it->second == clink.first){
-              break;
-            }
-          }
-          this->reverse_ordering_links.erase(it);
+ 
+		  if(clink.second != GOAL_TASK_INST_ID){
+			  std::pair<SchedulingLinks::iterator, SchedulingLinks::iterator> ret =
+			  ordering_links.equal_range(clink.first);
+			  SchedulingLinks::iterator it;
+			  for(it = ret.first; it != ret.second; it++){
+				if(it->second == clink.second){
+				  break;
+				}
+			  }              
+
+			  this->ordering_links.erase(it);
+
+			  ret = reverse_ordering_links.equal_range(clink.second);
+			  for(it = ret.first; it != ret.second; it++){
+				if(it->second == clink.first){
+				  break;
+				}
+			  }
+			  this->reverse_ordering_links.erase(it);
        
+		  }
           break;
         }
       }
@@ -1531,19 +1576,23 @@ void SA_WorkingPlan::undo (SA_ResolveCLThreatCmd * cmd)
 //  this->causal_links_.erase(firstit);
 
   std::pair<
-      std::multimap<TaskInstID, TaskInstID>::iterator,
-      std::multimap<TaskInstID, TaskInstID>::iterator
+      std::multimap<TaskInstID, TaskInstID >::iterator,
+      std::multimap<TaskInstID, TaskInstID >::iterator
       > ret = this->ordering_links.equal_range(cmd->first);
 
-  std::multimap<TaskInstID, TaskInstID>::iterator it;
+  std::multimap<TaskInstID, TaskInstID >::iterator it;
 
   for(it = ret.first; it != ret.second; it++){
     if(it->second == cmd->second)
       break;
-  //    this->ordering_links.erase(it);
   }
 
+//  int num_refs = it->second.second;s
   this->ordering_links.erase(it);
+ // if(it->second.second > 0)
+//	  this->ordering_links.insert(std::pair<TaskInstID, TaskInstID >
+//	  (it->first, it->second);
+  
 
   ret = this->reverse_ordering_links.equal_range(cmd->second);