Wed Jul 29 20:06:21 UTC 2009 Ben Podgursky <bpodgursky@gmail.com>

21 files changed, 708 insertions, 129 deletions

diff --git a/C2W/C2W_Demo.cpp b/C2W/C2W_Demo.cpp
index eeac07d8cc9..cd0e9923ac9 100644
--- a/C2W/C2W_Demo.cpp
+++ b/C2W/C2W_Demo.cpp
@@ -252,7 +252,7 @@ int main (int argc, char* argv[])
     //planner->add_out_adapter (&s_out);
 	  planner->add_out_adapter (&screen_out);
 
-    planner->plan (100, goal);
+    planner->plan (15, goal);
   } catch (std::string e) {
     std::cerr << "ERROR while planning:" << std::endl;
     std::cerr << e;
diff --git a/ChangeLog b/ChangeLog
index f71bdca723d..54a8bc1859b 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,3 +1,41 @@
+Wed Jul 29 20:06:21 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
+
+        * C2W/C2W_Demo.cpp:
+
+        * PlanCommands.h:
+        * PlanHeuristics.h:
+        * Planner.h:
+        * Planner.cpp:
+
+        * SANet/SANet.cpp:
+        * SANet/SANetFileIn.cpp:
+        * SANet/SANode.h:
+        * SANet/SANode.cpp:
+
+	Now adds an initial action with an effect of setting all true conditions true.  
+	So the loop in get unsat preconditions that eliminates preconditions true in the environment
+	is gone, since it could generate incorrect plans in environments with threats
+
+	SANet also sets the utility of this initial condition higher than all other actions, as it should
+	be the first try always
+
+        * SA_POP_Types.h:
+        * SA_PlanCommands.h:
+        * SA_PlanCommands.cpp:
+        * SA_PlanHeuristics.h:
+        * SA_PlanHeuristics.cpp:
+        * SA_PlanStrategy.h:
+        * SA_PlanStrategy.cpp:
+        * SA_SchedStrategy.cpp:
+        * SA_WorkingPlan.h:
+        * SA_WorkingPlan.cpp:
+        * TaskMapFileIn.cpp:
+
+	Map from last commit has been fixed.
+	
+	Implements a suspended condition map for fixing looping
+	
+
 Thu Jul 16 06:13:54 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
 
         * PlanCommands.h:
@@ -11,7 +49,7 @@ Thu Jul 16 06:13:54 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
         * SA_WorkingPlan.cpp:
         * WorkingPlan.h:
 
-	Added basic loop detection; has a map of open conditions at time of resolving open threat.
+        Added basic loop detection; has a map of open conditions at time of resolving open threat.
 
 Fri Jul 10 22:27:30 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
 
diff --git a/PlanCommands.h b/PlanCommands.h
index a9df9c3d584..57cab6458ba 100644
--- a/PlanCommands.h
+++ b/PlanCommands.h
@@ -84,6 +84,9 @@ namespace SA_POP {
      * @return  Log text for most recent execution of command.
      */
     virtual std::string get_log_text (void) = 0;
+
+
+
     int choices;
 
 
@@ -198,6 +201,8 @@ namespace SA_POP {
 	virtual TaskInstSet get_satisfied_tasks(void) = 0;
 
 	virtual Condition get_condition(void) = 0;
+
+	virtual CLSet get_causal_insertions(void) = 0;
   };
 
   /**
@@ -228,6 +233,8 @@ namespace SA_POP {
     virtual TaskInstID get_task_inst (void) = 0;
 
 	virtual void set_satisfied_insts(TaskInstSet set)=0;
+	
+	virtual void set_added_links(CLSet set) = 0;
   };
 
   /**
diff --git a/PlanHeuristics.h b/PlanHeuristics.h
index 22867c60275..8a1d0dcc1ff 100644
--- a/PlanHeuristics.h
+++ b/PlanHeuristics.h
@@ -48,6 +48,7 @@ namespace SA_POP {
      * @return  Next open condition to satisfy.
      */
     virtual Condition choose_cond (const OpenCondMap &open_conds) = 0;
+	virtual Condition choose_cond_suspension (const OpenCondMap &open_conds) = 0;
 
   protected:
     /// Pointer to Planner object.
diff --git a/Planner.cpp b/Planner.cpp
index 2e82ee21fd5..0d8c8a3ea70 100644
--- a/Planner.cpp
+++ b/Planner.cpp
@@ -52,6 +52,12 @@ cur_cmd_ (0)
 
   //****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****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****
   this->init_added = false;
+
+  this->not_backtracking.decision_pt = -1;
+  this->not_backtracking.seq_num = -1;
+  this->not_backtracking.step = -1;
+
+  this->backtrack_cmd = this->not_backtracking;
 };
 
 
@@ -120,6 +126,7 @@ bool Planner::plan (size_t sa_max_steps, SA_POP::Goal goal)
 
   // Set planning strategy goals and satisfy open conditions.
   this->plan_strat_->set_goals (goal.goal_conds);
+
   if (this->plan_strat_->satisfy_open_conds ()) {
     this->plan_ = this->working_plan_->get_plan ();
     this->notify_plan_changed ();
@@ -272,6 +279,14 @@ void Planner::add_command (PlanCommand *command)
 // On current command, undo last execution (if any) & execute next option.
 bool Planner::try_next (CommandID id)
 {
+	if(this->backtrack_cmd != this->not_backtracking){
+		if(id != this->backtrack_cmd){
+			return false;
+		}else{
+			this->backtrack_cmd = this->not_backtracking;
+		}
+	}
+
   if (this->cur_cmd_->get_id () != id) {
     char char_buf[35];
     sprintf(char_buf, "%d.%d.%d", id.step, id.decision_pt, id.seq_num); 
@@ -384,6 +399,9 @@ CondSet Planner::get_unsat_preconds (TaskID task_id)
 {
   CondSet temp = this->get_preconds (task_id);
 
+  //Note:  removed because when threats exist, it is wrong.  Uses initial condition
+  ///instead
+  /*
   // Remove satisfied preconditions.
   for (CondSet::iterator iter = temp.begin ();
     iter != temp.end (); )
@@ -400,6 +418,9 @@ CondSet Planner::get_unsat_preconds (TaskID task_id)
     if (cur_prob > this->cond_prob_thresh_)
       temp.erase (prev_iter);
   }
+  
+  */
+  
 
   return temp;
 }
diff --git a/Planner.h b/Planner.h
index 30b0e44ebcc..5f75fe82d6b 100644
--- a/Planner.h
+++ b/Planner.h
@@ -522,6 +522,7 @@ namespace SA_POP {
 
   virtual WorkingPlan*  get_working_plan(void){return this->working_plan_;};
 
+  virtual void set_backtrack_cmd_id(CommandID cmd){backtrack_cmd = cmd;};
 
   protected:
     /// Threshold for current probability of a condition to be satisfied.
@@ -560,6 +561,9 @@ namespace SA_POP {
     /// Notify all output adapters that plans have changed.
     virtual void notify_plan_changed (void);
 
+	CommandID backtrack_cmd;
+
+	CommandID not_backtracking;
   };
 
 };  /* SA_POP namespace */
diff --git a/SANet/SANet.cpp b/SANet/SANet.cpp
index 163ceda79a8..02432326a9c 100644
--- a/SANet/SANet.cpp
+++ b/SANet/SANet.cpp
@@ -426,6 +426,17 @@ void SANet::Network::update (int max_steps)
       }
     }
   }
+
+  double max_util = 0;
+  for(TaskNodeMap::iterator it = task_nodes_.begin(); it != task_nodes_.end(); it++){
+	  if(it->second->get_pos_util().utility > max_util){
+		  max_util = it->second->get_pos_util().utility;
+	  }
+	  
+  }
+
+  task_nodes_.find(20)->second->set_pos_util(max_util*2);
+
 };
 
 // Update a condition's current value (probability of being true).
diff --git a/SANet/SANetFileIn.cpp b/SANet/SANetFileIn.cpp
index 38edf710018..467b3bc0f86 100644
--- a/SANet/SANetFileIn.cpp
+++ b/SANet/SANetFileIn.cpp
@@ -184,6 +184,8 @@ void SANetFileIn::build_net (std::string filename, SA_POP::Builder *builder)
     
     builder->add_task (nodeID, priorProb, name);
   }
+  
+	builder->add_task(20, .5, "initact");
 
   // Get condition nodes.
   for (SANet::XML::Network::condNode_iterator iter = xml_net.begin_condNode ();
@@ -203,7 +205,11 @@ void SANetFileIn::build_net (std::string filename, SA_POP::Builder *builder)
 	if(cond_kind==cond_kind.ENVIRON) cond=::SA_POP::ENVIRON;
 	else if(cond_kind==cond_kind.SYSTEM) cond=::SA_POP::SYSTEM;
   else cond=::SA_POP::DATA;
-	builder->add_cond (nodeID, utility, probTrue, name, cond);
+	//Note: 0 here because of the initial action.  Changed from probTrue--Ben
+	builder->add_cond (nodeID, utility, 0, name, cond);
+
+	if((probTrue-.5)*2 > 0)
+		builder->set_effect(20, nodeID, "", (probTrue-.5)*2);
   }
 
   // Get precondition->task links.
diff --git a/SANet/SANode.cpp b/SANet/SANode.cpp
index a9280952a09..3bc88337ad3 100644
--- a/SANet/SANode.cpp
+++ b/SANet/SANode.cpp
@@ -120,6 +120,14 @@ TaskNode::~TaskNode (void)
   // Nothing to do.
 };
 
+Utility_Info TaskNode::get_pos_util(void){
+	return this->pos_util_;
+}
+
+void TaskNode::set_pos_util(double util){
+	this->pos_util_.utility = util;
+}
+
 bool TaskNode::update (void)
 {
   // Reset change flags.
@@ -409,6 +417,11 @@ bool TaskNode::update (void)
   // Update step (at end of step for task node).
   step_++;
 
+//  if(this->ID_ == 20){
+//	  this->pos_util_.utility = 100;
+//  }
+
+
   // Return boolean changed value based on change flags.
   return (prob_changed_ || util_changed_);
 
diff --git a/SANet/SANode.h b/SANet/SANode.h
index 8dba108d618..f478d6887b0 100644
--- a/SANet/SANode.h
+++ b/SANet/SANode.h
@@ -156,6 +156,7 @@ namespace SANet {
     virtual void print_xml (std::basic_ostream<char, std::char_traits<char> >& strm) = 0;
 
 
+
   protected:
     /// Unique ID of node (for identification within network).
     NodeID ID_;
@@ -335,6 +336,11 @@ namespace SANet {
      */
     virtual void update_effect (CondID ID, CondNode *node, LinkWeight weight);
 
+	void set_pos_util(double util);
+
+	Utility_Info get_pos_util();
+
+
   protected:
     /// Unconditional prior probability of success.
     Probability prior_prob_;
diff --git a/SA_POP_Types.h b/SA_POP_Types.h
index 79f713b41fb..d25830d85d6 100644
--- a/SA_POP_Types.h
+++ b/SA_POP_Types.h
@@ -265,6 +265,7 @@ namespace SA_POP {
     };
   };
 
+
   /// Type of a causal link.
   /// ("first" task instance achieves "cond" for "second" task instance.)
   struct CausalLink {
@@ -495,6 +496,15 @@ namespace SA_POP {
     bool operator< (const Goal &s) const { return this->goal_id < s.goal_id; };
   };
 
+  struct TaskInstEndTimeSet{
+	TaskInstID inst;
+	double end_time;
+
+	bool operator< (const TaskInstEndTimeSet & compare) const{
+		return end_time < compare.end_time;
+	};
+  };
+
   
   /// Type of particular Task Implementation mapped to a Task Implementation Set.
   /// This is a particular Precedence set. Like Before, After etc.
@@ -542,12 +552,23 @@ namespace SA_POP {
   struct StoredConditionKey{
 
 	CommandID satisfying_cmd;
+	CommandID free_pass_cmd;
+
 	StoredCondition satisfy_set;
+	bool free_pass_used;
+
+//	StoredConditionKey(){};
 
 	StoredConditionKey(Condition cond, TaskID task, CommandID cmd){
 		satisfying_cmd = cmd;
 		satisfy_set.satisfied_cond = cond;
 		satisfy_set.satisfying_task = task;
+
+		free_pass_cmd.decision_pt = 0;
+		free_pass_cmd.step = 0;
+		free_pass_cmd.seq_num = 0;
+
+		free_pass_used = false;
 	}
 
 	bool operator==(const StoredConditionKey & s)const{
@@ -570,11 +591,9 @@ namespace SA_POP {
   struct StoredConditionEvaluator{
 	  StoredConditionMap condition_map;
 	
-	  bool should_continue(CommandID cur_cmd, Condition satisfied_cond, 
+	  std::pair<bool, CommandID> should_continue(CommandID cur_cmd, Condition satisfied_cond, 
 		  TaskID satisfying_task, OpenCondMap & open_conds, InstToTaskMap & task_insts){
 
-
-
 			  StoredConditionKey stored_cond (satisfied_cond, satisfying_task, cur_cmd);
 
 			  std::pair<StoredConditionMap::iterator,
@@ -583,6 +602,13 @@ namespace SA_POP {
 				  = condition_map.equal_range(stored_cond);
 
 			  bool should_continue = true;
+			  CommandID return_to;
+			  return_to.decision_pt = 0;
+			  return_to.seq_num = 0;
+			  return_to.step = 0;
+
+
+			  bool using_free_pass = false;
 	
 			  if(range.first != range.second)
 			  {
@@ -592,22 +618,30 @@ namespace SA_POP {
 
 					StoredConditionSet old_open_conds = it->second;
 		
-					for(StoredConditionSet::iterator it = old_open_conds.begin();
-						it != old_open_conds.end(); it++)
+					for(StoredConditionSet::iterator it2 = old_open_conds.begin();
+						it2 != old_open_conds.end(); it2++)
 					{
-						if((*it).satisfying_task == -1) 
+						if((*it2).satisfying_task == -1) 
 							bool breakkk = 0;
 
 
 						std::pair<OpenCondMap::iterator, OpenCondMap::iterator>
-							cond_map_range = open_conds.equal_range((*it).satisfied_cond);
+							cond_map_range = open_conds.equal_range((*it2).satisfied_cond);
 
 						bool found_no_equiv_in_open = true;
 
-						for(OpenCondMap::iterator it2 = cond_map_range.first; it2 != cond_map_range.second; it2++){
-
-							if((*it).satisfying_task == task_insts.find((*it2).second)->second){
-								found_no_equiv_in_open = false;
+						for(OpenCondMap::iterator it3 = cond_map_range.first; it3 != cond_map_range.second; it3++){
+							if((*it2).satisfying_task == -1){
+								if((*it3).second == -1){
+									found_no_equiv_in_open = false;
+								}
+							}
+							else{
+								if((*it3).second == -1)
+								{}
+								else if((*it2).satisfying_task == task_insts.find((*it3).second)->second){
+									found_no_equiv_in_open = false;
+								}
 							}
 						}
 
@@ -618,15 +652,35 @@ namespace SA_POP {
 					}
 
 					if(all_pairs_in_open_conds){
-						should_continue = false;
-						break;
+
+			//			if(!(it->first.free_pass_used)){
+							
+			//				StoredConditionKey key2 = it->first;
+			//				StoredConditionSet set2 = it->second;
+
+			//				condition_map.erase(it);
+							
+			//				key2.free_pass_used = true;
+			//				key2.free_pass_cmd = cur_cmd;
+
+			//				condition_map.insert(std::pair<StoredConditionKey, StoredConditionSet>(key2, set2));
+			
+			//				using_free_pass = true;
+
+			//				break;
+			//			}else{
+							should_continue = false;
+				//			return_to = it->first.free_pass_cmd;
+							return_to = it->first.satisfying_cmd;
+							break;
+			//			}
 					}
 				}
 			}else{
 				should_continue = true;
 			}
 
-			if(should_continue)
+			if(should_continue && !using_free_pass)
 			{
 
 				StoredConditionSet current_open;
@@ -646,7 +700,7 @@ namespace SA_POP {
 					std::pair<StoredConditionKey, StoredConditionSet>(this_choice, current_open));
 			}
 
-			return should_continue;
+			return std::pair<bool, CommandID>(should_continue, return_to);
 		}
 
 	  void undo_binding(CommandID cur_cmd, Condition satisfied_cond, TaskID satisfying_task){
@@ -659,15 +713,36 @@ namespace SA_POP {
 
 		for(it = range.first; it != range.second; it++){
 			if(it->first.satisfying_cmd == cur_cmd){
-				break;
+
+		//		if(it->first.free_pass_used == true){
+		//			StoredConditionKey key2 = it->first;
+		//			StoredConditionSet set2 = it->second;
+
+		//			condition_map.erase(it);
+
+		//			key2.free_pass_used = false;
+
+		//			condition_map.insert(std::pair<StoredConditionKey, StoredConditionSet>(key2, set2));
+		//			break;
+		//		}else{				
+					condition_map.erase(it);
+					break;
+		//		}
 			}
 		}
 
-		condition_map.erase(it);
+
 
 	  }
   };
 
+  //Map from a causal link to the suspended conditions that are waiting for something to come
+  //between them and the orig. condition so they can be unsuspended
+ // typedef std::map<CLink, std::list<std::pair<Condition, TaskInstID>>> SuspendedConditionListenerMap;
+  typedef std::multimap<CausalLink, std::pair<Condition, TaskInstID>> SuspendedConditionListenerMap;
+
+  //List of all condition/task insts that are suspended
+  typedef std::set<std::pair<Condition, TaskInstID>> SuspendedConditionSet;
 
   inline std::string to_string(int x)
   {
diff --git a/SA_PlanCommands.cpp b/SA_PlanCommands.cpp
index 41b3bbcb267..5330e4ba280 100644
--- a/SA_PlanCommands.cpp
+++ b/SA_PlanCommands.cpp
@@ -385,6 +385,10 @@ void SA_AssocTaskImplCmd::set_satisfied_insts(TaskInstSet set){
 	this->satisfied_insts = set;
 }
 
+void SA_AssocTaskImplCmd::set_added_links(CLSet set){
+	this->added_links = set;
+}
+
 
 // Constructor.
 SA_AddOpenCondsCmd::SA_AddOpenCondsCmd (SA_PlanStrategy *plan_strat)
diff --git a/SA_PlanCommands.h b/SA_PlanCommands.h
index b81e049ce77..8df1edc62d3 100644
--- a/SA_PlanCommands.h
+++ b/SA_PlanCommands.h
@@ -252,6 +252,8 @@ namespace SA_POP {
 
 	virtual Condition get_condition(void){return cond_;};
 
+	virtual CLSet get_causal_insertions(void){return added_links_;};
+
   protected:
     /// WorkingPlan object that this command works on.
     SA_WorkingPlan *working_plan_;
@@ -341,6 +343,8 @@ namespace SA_POP {
     virtual TaskInstID get_task_inst (void);
 
 	virtual void set_satisfied_insts(TaskInstSet set);
+	
+	virtual void set_added_links(CLSet links);
 
   protected:
     /// WorkingPlan object that this command works on.
@@ -371,6 +375,12 @@ namespace SA_POP {
 	std::set< std::pair<TaskInstID,TaskInstID> > simul_insertions;
 
 	TaskInstSet satisfied_insts;
+
+	CLSet added_links;
+
+	SuspendedConditionListenerMap saved_listener_map;
+	SuspendedConditionSet saved_suspended_set;
+
 /*
     std::map <TaskInstID, TaskInstSet> befores;
     std::map <TaskInstID, TaskInstSet> afters;
@@ -453,6 +463,10 @@ namespace SA_POP {
 
     /// Flag for whether this has been executed.
     bool has_executed_;
+
+	std::map<Condition, CausalLink> link_suspended_on;
+
+
   };
 
   /**
diff --git a/SA_PlanHeuristics.cpp b/SA_PlanHeuristics.cpp
index 79a213e00c6..7fd50172445 100644
--- a/SA_PlanHeuristics.cpp
+++ b/SA_PlanHeuristics.cpp
@@ -46,12 +46,43 @@ Condition SA_CondStrategy::choose_cond (const OpenCondMap &open_conds)
   {
     if (iter->first.kind == SA_POP::DATA)
       return iter->first;
-  }
+  }  
 
   // If no data conditions, just return first condition.
   return open_conds.front().first;
 };
 
+// Choose the next open condition to satisfy.
+Condition SA_CondStrategy::choose_cond_suspension (const OpenCondMap &open_conds)
+{
+  if (open_conds.empty ())
+    throw "SA_POP::SA_CondStrategy::choose_cond (): Empty condition list.";
+
+  // Return first data condition.
+  for (OpenCondMap::const_iterator iter = open_conds.begin ();
+    iter != open_conds.end (); iter++)
+  {
+    if (iter->first.kind == SA_POP::DATA)
+      return iter->first;
+  }
+
+  SA_WorkingPlan* working_plan = (SA_WorkingPlan*)this->planner_->get_working_plan();
+
+  for(OpenCondMap::const_iterator it = open_conds.begin(); it != open_conds.end(); it++){
+
+	  if(!working_plan->condition_in_suspended(it->first, it->second)){
+			return it->first;
+	  }else{
+		bool grah = true;
+	  }
+  }
+
+  return working_plan->get_no_condition();
+
+  // If no data conditions, just return first condition.
+//  return open_conds.front().first;
+};
+
 
 
 // Constructor.
diff --git a/SA_PlanHeuristics.h b/SA_PlanHeuristics.h
index 1f083d58942..216d7391c76 100644
--- a/SA_PlanHeuristics.h
+++ b/SA_PlanHeuristics.h
@@ -45,6 +45,8 @@ namespace SA_POP {
      * @return  Next open condition to satisfy.
      */
     virtual Condition choose_cond (const OpenCondMap &open_conds);
+
+	virtual Condition choose_cond_suspension (const OpenCondMap &open_conds);
   };
 
   /**
diff --git a/SA_PlanStrategy.cpp b/SA_PlanStrategy.cpp
index bd6f9d6f269..e6cbb685b12 100644
--- a/SA_PlanStrategy.cpp
+++ b/SA_PlanStrategy.cpp
@@ -142,8 +142,8 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
   // If all open conditions have been satisfied, then return true for success.
   if (this->open_conds_.empty ())
 	  return this->planner_->full_sched();
-//)
-  if(!(this->planner_->get_working_plan()->get_all_insts().size() > 8)){
+//))!(this->planner_->get_working_plan()->get_all_insts().size() > 8)
+  if(true){
 	  // Increment step counter.
 	  this->cur_step_++;
 
@@ -155,9 +155,11 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
 	  CondSet preconds;
 
 	  // Choose an open condition to satisfy.
-	  Condition open_cond = this->cond_choice_->choose_cond (this->open_conds_);
-
-
+	  Condition open_cond = this->cond_choice_->choose_cond_suspension (this->open_conds_);
+	  SA_WorkingPlan* working_plan_tmp = (SA_WorkingPlan*)this->planner_->get_working_plan();
+	  if(working_plan_tmp->is_null_condition(open_cond)){
+			return false;
+	  }
 
 	  // Choose task to satisfy open condition (actually an ordered list of
 	  // tasks to try), passing command to planner to be executed next.
@@ -172,6 +174,8 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
 	  // Try tasks until one yields a complete plan or all have been tried.
 	  while (this->planner_->try_next (add_task_cmd->get_id ())) {
 
+       this->added_links = add_task_cmd->get_causal_insertions();
+
 		// Get current task and task instance.
 		this->cur_task_ = add_task_cmd->get_task ();
 
@@ -192,22 +196,22 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
 			  this->add_open_conds (preconds, this->cur_task_inst_);
   			}
 
-			if(this->store_map.should_continue(add_task_cmd->get_id(), add_task_cmd->get_condition(), 
-				stored_task, this->open_conds_, this->planner_->get_working_plan()->get_task_insts()))
+			//Do not execute if the condition has been noted before and not helped
+			std::pair<bool, CommandID> return_data = this->store_map.should_continue(add_task_cmd->get_id(), add_task_cmd->get_condition(), 
+				stored_task, this->open_conds_, this->planner_->get_working_plan()->get_task_insts());
+
+			if(return_data.first)
 			{
 				// Try to satisfy threats and continue recursive planning.
 				if (this->satisfy_everything())
 					return true;
-				this->store_map.undo_binding(add_task_cmd->get_id(), add_task_cmd->get_condition(), stored_task);
 
 			}else{
-				to_fail = true;
-				bool k = 0;
+				bool oops = true;
 			}
-			//	else{
-			//		this->planner_->undo_command (rmv_cond_cmd_id);
-			//		break;
-			//	}
+
+			this->store_map.undo_binding(add_task_cmd->get_id(), add_task_cmd->get_condition(), stored_task);
+
 
 
 			std::ostringstream debug_text;
@@ -224,9 +228,6 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
 		// Undo removal of open condition.
 		this->planner_->undo_command (rmv_cond_cmd_id);
 
-		if(to_fail){
-			break;
-		}
 	  }
 
 	  this->cur_task_inst_ = prev_cur_inst;
@@ -270,6 +271,9 @@ bool SA_PlanStrategy::satisfy_everything(){
 
 		assoc_impl_cmd->set_satisfied_insts(this->satisfied_insts);
 
+		assoc_impl_cmd->set_added_links(this->added_links);
+
+
 		this->cur_task_inst_ = assoc_impl_cmd->get_task_inst ();
 		
 		while (this->planner_->try_next (assoc_impl_cmd->get_id ())) 
@@ -286,6 +290,8 @@ bool SA_PlanStrategy::satisfy_everything(){
 			//Actually build the list of threats
 			this->planner_->generate_all_threats();
 
+
+
 			// Add causal link threats to open threats.
 			bool are_threats = !(this->planner_->get_all_threats().empty ());
 			CommandID add_threats_cmd_id;
@@ -421,10 +427,19 @@ RemoveOpenThreatsCmd *SA_PlanStrategy::get_RemoveOpenThreatsCmd (void)
 // Execute a command to add open conditions to planning.
 void SA_PlanStrategy::execute (SA_AddOpenCondsCmd *cmd)
 {
+  SA_WorkingPlan* working_plan_tmp = (SA_WorkingPlan*)this->planner_->get_working_plan();
+
   for (CondSet::iterator iter = cmd->conds_.begin ();
     iter != cmd->conds_.end (); iter++)
   {
     this->open_conds_.insert (std::make_pair (*iter, cmd->task_inst_));
+
+	CausalLink closest_on_path = working_plan_tmp->clink_on_path(*iter, cmd->task_inst_);
+	if(!working_plan_tmp->is_null_link(closest_on_path)){
+		working_plan_tmp->suspend_condition(*iter, cmd->task_inst_, closest_on_path);
+
+	}
+	cmd->link_suspended_on.insert(std::pair<Condition, CausalLink>(*iter, closest_on_path));
   }
 };
 
@@ -433,11 +448,18 @@ void SA_PlanStrategy::undo (SA_AddOpenCondsCmd *cmd)
 {
 	std::ostringstream debug_text;
 
+	SA_WorkingPlan* work_plan_tmp = (SA_WorkingPlan*)this->planner_->get_working_plan();
+
   // Remove open conditions mapped to the specified task instance.
 	debug_text<<"removing open conds mapped to "<<cmd->task_inst_<<std::endl;
 	for (CondSet::iterator cond_iter = cmd->conds_.begin ();
     cond_iter != cmd->conds_.end (); cond_iter++)
   {
+	CausalLink lnk = (cmd->link_suspended_on.find(*cond_iter))->second;
+	  if(!work_plan_tmp->is_null_link(lnk)){
+		  work_plan_tmp->resume_condition(*cond_iter, cmd->task_inst_, lnk);
+	  }
+
 	  debug_text<<"checking for "<<cond_iter->id<<std::endl;
 	  for (OpenCondMap::iterator open_iter =
       this->open_conds_.lower_bound (*cond_iter);
@@ -480,8 +502,13 @@ void SA_PlanStrategy::execute (SA_RemoveOpenCondsCmd *cmd)
   }
   */
 
+	SA_WorkingPlan* working_plan_tmp = (SA_WorkingPlan*)this->planner_->get_working_plan();
+
 	for(CondSet::iterator cond_it = cmd->conds_.begin(); cond_it != cmd->conds_.end();
 		cond_it++){
+
+
+
 			for(TaskInstSet::iterator task_it = cmd->tasks_.begin(); 
 			task_it != cmd->tasks_.end(); task_it++){
 
@@ -492,6 +519,9 @@ void SA_PlanStrategy::execute (SA_RemoveOpenCondsCmd *cmd)
 
 					OpenCondMap::iterator prev_iter = o_it;
 					o_it++;
+
+
+
 					cmd->removed_.insert (std::make_pair
 						(*cond_it, *task_it));
 					
@@ -499,7 +529,6 @@ void SA_PlanStrategy::execute (SA_RemoveOpenCondsCmd *cmd)
 				  }else{
 					  o_it++;
 				  }
-				  
 			  }
 		}
 	}
diff --git a/SA_PlanStrategy.h b/SA_PlanStrategy.h
index 3b99e8a5a61..ad115be58f4 100644
--- a/SA_PlanStrategy.h
+++ b/SA_PlanStrategy.h
@@ -225,6 +225,7 @@ namespace SA_POP {
 	///Holds info about which tasks an add task cmd has satisfied
 	TaskInstSet satisfied_insts;
 
+	CLSet added_links;
 
     // ************************************************************************
     // Decision point numbers.
diff --git a/SA_SchedStrategy.cpp b/SA_SchedStrategy.cpp
index ca2ad899608..6a441e7ae88 100644
--- a/SA_SchedStrategy.cpp
+++ b/SA_SchedStrategy.cpp
@@ -14,10 +14,13 @@
 
 #include "SA_POP_Types.h"
 #include "SA_SchedStrategy.h"
+#include "SA_WorkingPlan.h"
 #include "Planner.h"
 #include <list>
 #include <set>
 #include <fstream>
+#include <algorithm>
+#include <vector>
 using namespace SA_POP;
 
 // Constructor.
@@ -83,6 +86,9 @@ CommandID SA_SchedStrategy::get_next_cmd_id (void)
 // satisfaction of open conditions by recursive call back to planning).
 bool SA_SchedStrategy::satisfy_sched (TaskInstID task_inst)
 {
+
+
+
   //****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
   //return this->planner_->recurse_plan ();
   // Adjust schedule.
@@ -91,10 +97,13 @@ bool SA_SchedStrategy::satisfy_sched (TaskInstID task_inst)
 	CommandID cur_cmd_id = this->planner_->cur_command_id();
 	this->cur_seq_num_=1;
 
+
+
 	// Do the energy propogation for this task instance
 	// This function automatically does this for the task instances before and after it.
   
 
+	
   if(!this->energy_prop(task_inst))
   {
 
@@ -147,7 +156,7 @@ bool SA_SchedStrategy::satisfy_sched (TaskInstID task_inst)
 	  this->planner_->undo_through(cur_cmd_id);
 	  return false;
   }
-	
+
 
   if(!this->planner_->recurse_plan ()){
 	  this->planner_->undo_through(cur_cmd_id);
@@ -170,6 +179,124 @@ bool SA_SchedStrategy::satisfy_full_sched ()
 	  this->planner_->undo_through(cur_cmd_id);
 	  return false;
   }
+	
+  SA_WorkingPlan* working_plan_tmp = (SA_WorkingPlan*)this->planner_->get_working_plan();
+
+ // working_plan_tmp->get_precedence_graph()
+  PrecedenceSet befores = (working_plan_tmp->get_precedence_graph().find(BEFORE)->second);
+  PrecedenceSet afters = (working_plan_tmp->get_precedence_graph().find(AFTER)->second);
+  PrecedenceSet simuls = (working_plan_tmp->get_precedence_graph().find(SIMUL)->second);
+  PrecedenceSet unrankeds = (working_plan_tmp->get_precedence_graph().find(UNRANKED)->second);
+
+  //double current_execution_time = 0
+  //vector currently executing actions
+
+
+  std::vector<TaskInstEndTimeSet> executing_tasks;
+
+  double current_execution_time = 0;
+
+  int total_tasks = befores.size();
+  int completed_tasks = 0;
+
+  while(completed_tasks < total_tasks){
+
+	  for(PrecedenceSet::iterator it = befores.begin(); it != befores.end();){
+		
+		  PrecedenceSet::iterator prev_it = it++;
+		  if((befores.find(prev_it->first))->second.empty()){
+			
+			  TaskInstEndTimeSet new_execute;
+			  new_execute.inst = prev_it->first;
+			  new_execute.end_time = current_execution_time + this->planner_->get_impl(working_plan_tmp->get_impl_id(prev_it->first))
+				  ->get_duration();
+			  executing_tasks.push_back(new_execute);
+			  befores.erase(prev_it);
+		  }
+	  }
+
+	  std::sort(executing_tasks.begin(), executing_tasks.end());
+
+	  TaskInstEndTimeSet next_done = *executing_tasks.begin();
+	  current_execution_time = next_done.end_time;
+
+	  std::list<TaskInstEndTimeSet> to_remove;
+
+	  for(std::vector<TaskInstEndTimeSet>::iterator it3 = executing_tasks.begin(); 
+		  it3 != executing_tasks.end();){
+
+			if(it3->end_time != current_execution_time){	 
+			  break;
+			 }
+		  std::vector<TaskInstEndTimeSet>::iterator prev_it = it3++;
+
+		  std::cout<<"Task "<<prev_it->inst<<" finishes at time "<<prev_it->end_time<<std::endl;
+		  completed_tasks++;
+
+		  for(PrecedenceSet::iterator it2 = befores.begin(); it2 != befores.end(); it2++){
+			  it2->second.erase(prev_it->inst);
+		  }
+		  befores.erase(prev_it->inst);
+		  to_remove.push_front(*prev_it);
+	//	  executing_tasks.erase(prev_it);
+	  }
+
+	  for( std::list<TaskInstEndTimeSet>::iterator it3 = to_remove.begin(); it3 != to_remove.end(); it3++){
+		  std::vector<TaskInstEndTimeSet>::iterator it4;
+		  for(it4 = executing_tasks.begin(); it4 != executing_tasks.end(); it4++){
+			  if(it4->inst == it3->inst){
+				break;
+			  }
+		  }
+		  executing_tasks.erase(it4);
+	  }
+  }
+
+  //while #actions finished != total actions
+  //find all actions with no more befores
+  //add to currently executing actions sorted by current_time + their duration
+  //take action(s if there are multiple that end at the same time) off currently executing actions
+  //
+  //remove them from the befores of all other actions
+  //remove them from befores
+  //current execution time = when they finish
+
+  /*
+  std::list<TaskInstID> execute_this_time;
+
+  int time = 0;
+  while(!befores.empty()){
+
+	  execute_this_time.clear();
+
+	  for(PrecedenceSet::iterator it = befores.begin(); it != befores.end();){
+		
+		  PrecedenceSet::iterator prev_it = it++;
+
+		  if((befores.find(prev_it->first))->second.empty()){
+			
+			  execute_this_time.push_front(prev_it->first);
+
+			  befores.erase(prev_it);
+		  }
+	  }
+
+	
+	  for(std::list<TaskInstID>::iterator it = execute_this_time.begin(); it != execute_this_time.end(); it++){
+	
+		  std::cout<<"Task "<<(*it)<<" can execute at time "<<time<<std::endl;
+
+		  for(PrecedenceSet::iterator it2 = befores.begin(); it2 != befores.end(); it2++){
+				it2->second.erase(*it);
+		  }
+	  }
+	  time++;
+
+
+
+  }
+  */
+
   return true;
 };
 /// Calculate the min and max levels for the consumer of a task instance
@@ -733,23 +860,24 @@ bool SA_SchedStrategy::energy_prop (TaskInstID task_inst)
   const TaskInstSet *aft = this->planner_->get_prec_insts(task_inst,AFTER);
   const TaskInstSet *bef = this->planner_->get_prec_insts(task_inst,BEFORE);
 
-  for(TaskInstSet::const_iterator iter = aft->begin();iter!=aft->end();iter++)
-    {
-      if(!this->energy_prop_after(*iter))
-      {
-		this->planner_->undo_through(cur_cmd_id);
-		return false;
-      }
-    }
+	  for(TaskInstSet::const_iterator iter = aft->begin();iter!=aft->end();iter++)
+		{
+		  if(!this->energy_prop_after(*iter))
+		  {
+			this->planner_->undo_through(cur_cmd_id);
+			return false;
+		  }
+		}
 
-    for(TaskInstSet::const_iterator iter = bef->begin();iter!=bef->end();iter++)
-    {
-      if(!this->energy_prop_before(*iter))
-      {
-      this->planner_->undo_through(cur_cmd_id);
-      return false;
-      }
-    }
+		for(TaskInstSet::const_iterator iter = bef->begin();iter!=bef->end();iter++)
+		{
+		  if(!this->energy_prop_before(*iter))
+		  {
+		  this->planner_->undo_through(cur_cmd_id);
+		  return false;
+		  }
+		}
+	
   return true;
 }
 /// Do the uni directional energy precedence propogation in the after direction
@@ -810,19 +938,20 @@ bool SA_SchedStrategy::energy_prop_after (TaskInstID task_inst)
 		this->planner_->undo_through(cur_cmd_id);
 		return false;
     }
-  }
-	//TaskInstSet aft = this->planner_->after_orderings(task_inst);
-	//TaskInstSet bef = this->planner_->before_orderings(task_inst);
-  const TaskInstSet *aft = this->planner_->get_prec_insts(task_inst,AFTER);
+  
+		//TaskInstSet aft = this->planner_->after_orderings(task_inst);
+		//TaskInstSet bef = this->planner_->before_orderings(task_inst);
+	  const TaskInstSet *aft = this->planner_->get_prec_insts(task_inst,AFTER);
 
-  for(TaskInstSet::const_iterator iter = aft->begin();iter!=aft->end();iter++)
-    {
-      if(!this->energy_prop_after(*iter))
-      {
-		this->planner_->undo_through(cur_cmd_id);
-		return false;
-      }
-    }
+	  for(TaskInstSet::const_iterator iter = aft->begin();iter!=aft->end();iter++)
+		{
+		  if(!this->energy_prop_after(*iter))
+		  {
+			this->planner_->undo_through(cur_cmd_id);
+			return false;
+		  }
+		}
+	}
   return true;
 }
 /// Do the uni directional energy precedence propogation in the after direction
@@ -891,9 +1020,9 @@ bool SA_SchedStrategy::energy_prop_before (TaskInstID task_inst)
 		this->planner_->undo_through(cur_cmd_id);
 		return false;
     }
-  }
+
 	//TaskInstSet bef = this->planner_->before_orderings(task_inst);
-  const TaskInstSet *bef = this->planner_->get_prec_insts(task_inst,BEFORE);
+	const TaskInstSet *bef = this->planner_->get_prec_insts(task_inst,BEFORE);
 
     for(TaskInstSet::const_iterator iter = bef->begin();iter!=bef->end();iter++)
     {
@@ -903,5 +1032,7 @@ bool SA_SchedStrategy::energy_prop_before (TaskInstID task_inst)
       return false;
       }
     }
+	
+	}
   return true;
 }
diff --git a/SA_WorkingPlan.cpp b/SA_WorkingPlan.cpp
index 42ef0ba8544..f79a1a44ff6 100644
--- a/SA_WorkingPlan.cpp
+++ b/SA_WorkingPlan.cpp
@@ -75,6 +75,11 @@ for(int i = 0; i < 2; i++){
 	this->init_end.insert(std::make_pair(i,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
 }
 
+this->no_condition.id = -4;
+this->no_link.cond.id = -4;
+this->no_link.first = -4;
+this->no_link.second = -4;
+
 };
 
 // Destructor.
@@ -411,6 +416,10 @@ void SA_WorkingPlan::generate_all_threats(void)
     TaskInstID threat_possibility = iterator->first;
     TaskID threat_possibility_taskid = iterator->second;
     
+	if(iterator->first >= 858){
+		bool here = true;
+	}
+
 	debug_text <<"  Task (" <<threat_possibility_taskid << ")"<< ": Inst (" <<iterator->first << ")" << std::endl;
   }
   debug_text<<std::endl;
@@ -443,7 +452,6 @@ void SA_WorkingPlan::generate_all_threats(void)
 
             SANet::LinkWeight threat_effect = this->planner_->get_link(threat_possibility_taskid, condition.id);
             SANet::LinkWeight causal_effect = this->planner_->get_link(threatened_task, causal_threatened.cond.id);
-
            
             if((threat_effect > 0 && causal_effect < 0 )|| (threat_effect < 0 && causal_effect > 0)){
 
@@ -724,23 +732,23 @@ void SA_WorkingPlan::execute (SA_AddTaskCmd *cmd)
 			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 causal link not found in working plan, add it to causal 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_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));
+				this->causal_links_.insert (std::make_pair (cond, clink));
+				this->causal_links_by_first.insert (std::make_pair (clink.first, clink));
+				this->causal_links_by_second.insert (std::make_pair (clink.second, 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);
+				std::cout<<"after unsuspend"<<std::endl;
+				 
+		//		this->suspended_listener_map.insert (std::make_pair (clink, true));
+
+				cmd->added_links_.insert(clink);
 			}
 		}
 
@@ -749,6 +757,111 @@ void SA_WorkingPlan::execute (SA_AddTaskCmd *cmd)
 	cmd->last_task_inst_ = task_inst;
 };
 
+bool SA_WorkingPlan::condition_in_suspended(Condition condition, TaskInstID required_by){
+	
+	if(!suspended_conditions.empty()){
+		bool k = true;
+	}
+
+	std::pair<Condition, TaskInstID> condition_pair(condition, required_by);
+
+	return (this->suspended_conditions.find(condition_pair) != this->suspended_conditions.end());
+}
+
+
+CausalLink SA_WorkingPlan::clink_on_path(Condition condition, TaskInstID required_by){
+	std::set<TaskInstID> expanded;
+	expanded.clear();
+
+	return clink_on_path_aux(condition, required_by, expanded);
+}
+
+CausalLink SA_WorkingPlan::clink_on_path_aux(Condition condition, TaskInstID required_by, 
+										std::set<TaskInstID> & expanded){
+
+	expanded.insert(required_by);
+
+	if(required_by == -1){
+		return this->no_link;
+	}
+
+	for(TaskToCLinksMap::iterator it = causal_links_by_first.lower_bound(required_by);
+		it != causal_links_by_first.upper_bound(required_by); it++){
+
+			if(expanded.find(it->second.second) != expanded.end()){
+				continue;
+			}
+
+			if(it->second.cond == condition){
+				return it->second;
+			}
+
+			CausalLink to_return = clink_on_path_aux(condition, it->second.second, expanded);
+
+			if(to_return != no_link){
+				return to_return;
+			}
+	}
+
+	return no_link;
+}
+
+void SA_WorkingPlan::unsuspend_listeners(CausalLink link, TaskInstID exception){
+	
+	for(TaskToCLinksMap::iterator it = this->causal_links_by_first.lower_bound(link.second); 
+		it != this->causal_links_by_first.upper_bound(link.second); it++){
+			
+		unsuspend_listeners_aux(it->second, exception);
+	}
+}
+
+void SA_WorkingPlan::unsuspend_listeners_aux(CausalLink link, TaskInstID exception){
+
+	for(SuspendedConditionListenerMap::iterator it = this->suspended_listener_map.lower_bound(link);
+		it != this->suspended_listener_map.upper_bound(link);){
+
+			SuspendedConditionListenerMap::iterator prev_it = it++;
+
+			if(exception != prev_it->second.second){
+				this->suspended_conditions.erase(prev_it->second);
+				suspended_listener_map.erase(prev_it);
+			}
+	}
+
+	
+
+	for(TaskToCLinksMap::iterator it = this->causal_links_by_first.lower_bound(link.second); 
+		it != this->causal_links_by_first.upper_bound(link.second); it++){
+		unsuspend_listeners_aux(it->second, exception);
+	}
+}
+
+void SA_WorkingPlan::suspend_condition(Condition cond, TaskInstID required_by, CausalLink suspended_by){
+
+	std::pair<Condition, TaskInstID> require_set (cond, required_by);
+	std::pair<CausalLink, std::pair<Condition, TaskInstID>> to_insert (suspended_by, require_set);
+	
+	this->suspended_listener_map.insert(to_insert);
+	this->suspended_conditions.insert(require_set);
+}
+
+void SA_WorkingPlan::resume_condition(Condition cond, TaskInstID required_by, CausalLink suspended_by){
+	std::pair<Condition, TaskInstID> require_set (cond, required_by);
+	std::pair<CausalLink, std::pair<Condition, TaskInstID>> to_insert (suspended_by, require_set);
+	
+	std::multimap<CausalLink, std::pair<Condition, TaskInstID>> ::iterator it;
+
+	for(it = suspended_listener_map.lower_bound(suspended_by);
+		it != suspended_listener_map.upper_bound(suspended_by); it++){
+			if(it->second == require_set){
+				break;
+			}
+	}
+
+	suspended_listener_map.erase(it);
+
+	this->suspended_conditions.erase(require_set);
+}
 
 void SA_WorkingPlan::undo (SA_AddTaskCmd *cmd)
 {
@@ -765,8 +878,7 @@ void SA_WorkingPlan::undo (SA_AddTaskCmd *cmd)
 	 }
 
 
-  for (SA_WorkingPlan::CondToCLinksMap::iterator cl_iter =
-    this->causal_links_.lower_bound (cmd->cond_);
+  for (SA_WorkingPlan::CondToCLinksMap::iterator cl_iter = this->causal_links_.lower_bound (cmd->cond_);
     cl_iter != this->causal_links_.upper_bound (cmd->cond_);)
   {
     SA_WorkingPlan::CondToCLinksMap::iterator prev_iter = cl_iter;
@@ -781,31 +893,54 @@ void SA_WorkingPlan::undo (SA_AddTaskCmd *cmd)
           CausalLink clink = prev_iter->second;
           
           this->causal_links_.erase (prev_iter);
-          cmd->added_links_.erase(iter);
-
-		 /*
-		  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;
-				}
-			  }              
+		  
+   //       cmd->added_links_.erase(iter);
+          break;
+        }
+      }
+    }
+  }
 
-			  this->ordering_links.erase(it);
+  for (SA_WorkingPlan::TaskToCLinksMap::iterator cl_iter = this->causal_links_by_first.lower_bound (cmd->last_task_choice_.task_inst_id);
+    cl_iter != this->causal_links_by_first.upper_bound (cmd->last_task_choice_.task_inst_id);)
+  {
+    SA_WorkingPlan::TaskToCLinksMap::iterator prev_iter = cl_iter;
+    cl_iter++;
+    if (cmd->task_insts_.find (prev_iter->second.second) != cmd->task_insts_.end ())
+    {
+      for(CLSet::iterator iter = cmd->added_links_.begin();iter!=cmd->added_links_.end();iter++)
+      {
+        if(prev_iter->second == *iter)  
+        {
+          
+          CausalLink clink = prev_iter->second;
+          
+          this->causal_links_by_first.erase (prev_iter);
+    //      cmd->added_links_.erase(iter);
+          break;
+        }
+      }
+    }
+  }
+  
+  TaskInstID to_find = *(cmd->get_satisfied_tasks().begin());
 
-			  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);
-       
-		  }
-		  */
+  for (SA_WorkingPlan::TaskToCLinksMap::iterator cl_iter = this->causal_links_by_second.lower_bound (to_find);
+    cl_iter != this->causal_links_by_second.upper_bound (to_find);)
+  {
+    SA_WorkingPlan::TaskToCLinksMap::iterator prev_iter = cl_iter;
+    cl_iter++;
+    if (cmd->task_insts_.find (prev_iter->second.second) != cmd->task_insts_.end ())
+    {
+      for(CLSet::iterator iter = cmd->added_links_.begin();iter!=cmd->added_links_.end();iter++)
+      {
+        if(prev_iter->second == *iter)  
+        {
+          
+          CausalLink clink = prev_iter->second;
+          
+          this->causal_links_by_second.erase (prev_iter);
+          cmd->added_links_.erase(iter);
           break;
         }
       }
@@ -813,6 +948,18 @@ void SA_WorkingPlan::undo (SA_AddTaskCmd *cmd)
   }
 };
 
+bool SA_WorkingPlan::is_null_link(CausalLink link){
+	return link == no_link;
+}
+
+bool SA_WorkingPlan::is_null_condition(Condition cond){
+	return cond == no_condition;
+}
+
+Condition SA_WorkingPlan::get_no_condition(){
+	return no_condition;
+}
+
 // Execute a command to associate an implementation with a
 // task instance in the plan.
 bool SA_WorkingPlan::execute (SA_AssocTaskImplCmd *cmd)
@@ -847,17 +994,16 @@ bool SA_WorkingPlan::execute (SA_AssocTaskImplCmd *cmd)
 	  }
   }
 
-  /*
-  if(is_cycle_in_ordering()){
-   // cmd->got_to_scheduling = false;
-    bool oops = true;
-  }
-*/
+  cmd->got_to_scheduling = true;
 
 
+  cmd->saved_listener_map = suspended_listener_map;
+  cmd->saved_suspended_set = suspended_conditions;
+
+  for(CLSet::iterator it = cmd->added_links.begin(); it != cmd->added_links.end(); it++){
+	  unsuspend_listeners(*it, it->first);
+  }
 
-  cmd->got_to_scheduling = true;
-  
   bool toReturn	= this->init_prec_insert(cmd->task_inst_,cmd);
 
 //  cmd->befores_after_ex = *befores;
@@ -926,17 +1072,8 @@ void SA_WorkingPlan::undo (SA_AssocTaskImplCmd *cmd)
 	
 	
   if(cmd->got_to_scheduling){
-
-	  /*
-	  if(cmd->afters_after_ex != *afters ||
-		  cmd->befores_after_ex != *befores ||
-		  cmd->simuls_after_ex != *simuls ||
-		  cmd->unrankeds_after_ex != *unrankeds)
-		 {
-		bool hi = true;
-	  } 
-	  */
-
+	suspended_listener_map = cmd->saved_listener_map;
+	suspended_conditions = cmd->saved_suspended_set;
 
     this->undo(&cmd->max_adj_cmd);
     this->undo(&cmd->min_adj_cmd);
@@ -966,6 +1103,10 @@ void SA_WorkingPlan::undo (SA_AssocTaskImplCmd *cmd)
 
 };
 
+PrecedenceGraph SA_WorkingPlan::get_precedence_graph(){
+	return this->precedence_graph_;
+}
+
 // Execute a command to resolve a causal link threat in the
 // plan (with promotion or demotion).
 bool SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd)
diff --git a/SA_WorkingPlan.h b/SA_WorkingPlan.h
index 299a6c278e7..f0a4d559b3c 100644
--- a/SA_WorkingPlan.h
+++ b/SA_WorkingPlan.h
@@ -338,6 +338,28 @@ namespace SA_POP {
 
 	void print_precedence_graph(std::string source);
 
+	bool condition_in_suspended(Condition condition, TaskInstID required_by_);
+
+	CausalLink clink_on_path(Condition condition, TaskInstID required_by);
+
+	CausalLink clink_on_path_aux(Condition condition, TaskInstID required_by, 
+										std::set<TaskInstID> & expanded);
+
+	void unsuspend_listeners(CausalLink link, TaskInstID exception);
+	void unsuspend_listeners_aux(CausalLink link, TaskInstID exception);
+
+	void suspend_condition(Condition cond, TaskInstID required_by, CausalLink suspended_by);
+
+	void resume_condition(Condition cond, TaskInstID required_by, CausalLink suspended_by);
+
+	bool is_null_condition(Condition cond);
+
+	bool is_null_link(CausalLink link);
+
+	Condition get_no_condition();
+
+	PrecedenceGraph get_precedence_graph();
+
   protected:
     // ************************************************************************
     // State information.
@@ -362,9 +384,16 @@ namespace SA_POP {
     // Type of a map from conditions to causal links.
     typedef std::multimap <Condition, CausalLink> CondToCLinksMap;
 
+	typedef std::multimap <TaskInstID, CausalLink> TaskToCLinksMap;
+
     /// Causal links in plan (mapping from conditions to all causal links
     /// containing that condition).
     CondToCLinksMap causal_links_;
+	TaskToCLinksMap causal_links_by_first;
+	TaskToCLinksMap causal_links_by_second;
+
+	SuspendedConditionListenerMap suspended_listener_map;
+	SuspendedConditionSet suspended_conditions;
 
     /// Current plan (generated upon request).
     Plan plan_;
@@ -373,7 +402,8 @@ namespace SA_POP {
     virtual TaskInstID get_next_inst_id (void);
 
 
-
+	Condition no_condition;
+	CausalLink no_link;
 
 	/// PrecedenceGraph
 	PrecedenceGraph precedence_graph_;
diff --git a/TaskMapFileIn.cpp b/TaskMapFileIn.cpp
index a91a44de6e1..37c89381d69 100644
--- a/TaskMapFileIn.cpp
+++ b/TaskMapFileIn.cpp
@@ -99,6 +99,16 @@ void TaskMapFileIn::build_task_map (std::string filename, Builder *builder)
     builder->add_task_impl (impl);
   }
 
+  ImplParamSet init_params;
+  ImplParam init_param;
+  init_param.id = "test_param1";
+  init_param.kind = "test_kind1";
+  init_param.value = "test_param_value1";
+  init_params.insert (init_param);
+
+  TaskImpl* initimpl = new TaskImpl ("initact_impl", init_params);
+  builder->add_task_impl (initimpl);
+
   // Get resources.
   for (SA_POP::XML::TaskMap::resource_iterator res_iter =
     xml_tm.begin_resource ();
@@ -130,6 +140,8 @@ void TaskMapFileIn::build_task_map (std::string filename, Builder *builder)
     resource.capacity = capacity;
 
     builder->add_resource (resource);
+
+	builder->assoc_impl_with_resource ("initact_impl", resourceID, 1);
   }
 
   // Get task to implementation associations.
@@ -152,6 +164,8 @@ void TaskMapFileIn::build_task_map (std::string filename, Builder *builder)
     builder->assoc_task_with_impl (taskID, implID, duration);
   }
 
+  builder->assoc_task_with_impl (20, "initact_impl", 1);
+
   // Get task implementation to resource associations.
   for (SA_POP::XML::TaskMap::implToResource_iterator iassocr_iter =
     xml_tm.begin_implToResource ();