Thu Oct 1 17:25:36 UTC 2009 Ben Podgursky <bpodgursky@gmail.com>

16 files changed, 2849 insertions, 2785 deletions

diff --git a/C2W/C2W_Demo.cpp b/C2W/C2W_Demo.cpp
index cd0e9923ac9..2ad8c0355e8 100644
--- a/C2W/C2W_Demo.cpp
+++ b/C2W/C2W_Demo.cpp
@@ -14,6 +14,10 @@
  */
 //=============================================================================
 
+#define _CRTDBG_MAP_ALLOC
+#include <stdlib.h>
+#include <crtdbg.h>
+
 #include <iostream>
 #include <fstream>
 #include "SA_POP_Types.h"
@@ -191,8 +195,12 @@ int main (int argc, char* argv[])
   
 
   try {
+
+
     sanet_in.build_net (sanet_filename, &builder);
     tm_in.build_task_map (tm_filename, &builder);
+
+
   } catch (std::string e) {
     std::cerr << "ERROR while building task network and task map from files:";
     std::cerr << std::endl;
@@ -253,6 +261,7 @@ int main (int argc, char* argv[])
 	  planner->add_out_adapter (&screen_out);
 
     planner->plan (15, goal);
+
   } catch (std::string e) {
     std::cerr << "ERROR while planning:" << std::endl;
     std::cerr << e;
@@ -323,7 +332,7 @@ int main (int argc, char* argv[])
         tm_in.build_task_map (tm_filename, &rebuilder);
         planner = rebuilder.get_planner ();
         planner->add_out_adapter (&graph_out);
-        */
+*/
         for(std::map<SANet::CondID, double>::iterator cIter = condMap.begin(); cIter != condMap.end(); cIter++)
         {
           planner->update_cond_val((*cIter).first, (*cIter).second);
@@ -347,6 +356,9 @@ int main (int argc, char* argv[])
   std::cin>>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
 
+
+  _CrtDumpMemoryLeaks();
+
   return 0;
 }
 
diff --git a/ChangeLog b/ChangeLog
index db455c5d257..3ee453d8d7b 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,10 +1,37 @@
+Thu Oct  1 17:25:36 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
+
+        * C2W/C2W_Demo.cpp:
+
+        * PlanHeuristics.h:
+        * Planner.cpp:
+
+        * SANet/SANetFileIn.cpp:
+
+        * SA_POP_Types.h:
+        * SA_PlanStrategy.h:
+        * SA_PlanStrategy.cpp:
+        * SA_SchedStrategy.cpp:
+        * SA_WorkingPlan.h:
+        * SA_WorkingPlan.cpp:
+
+        * utils/PDDLtoSANetTranslator/PDDLParser/ferry-example.pddl:
+        * utils/PDDLtoSANetTranslator/PDDLParser/ferry-problem-example.pddl:
+
+        * utils/PDDLtoSANetTranslator/PDDLParser/src/PDDLtoSAN.java:
+
+        * utils/PDDLtoSANetTranslator/PDDLParser/src/Translator/PDDL_Translator.java:
+
+        * utils/PDDLtoSANetTranslator/PDDLParser/src/Translator/SAN_Data/TaskNode.java:
+		
+		Various changes to the PDDL parser
+
 Wed Aug 12 19:15:30 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
 
         * utils/PDDLtoSANetTranslator:
 
         * utils/PDDLtoSANetTranslator/PDDLParser:
-		
-		Cleanup
+
+                Cleanup
 
 Wed Aug 12 19:02:39 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
 
diff --git a/PlanHeuristics.h b/PlanHeuristics.h
index 8a1d0dcc1ff..7468e1458ee 100644
--- a/PlanHeuristics.h
+++ b/PlanHeuristics.h
@@ -49,6 +49,7 @@ namespace SA_POP {
      */
     virtual Condition choose_cond (const OpenCondMap &open_conds) = 0;
 	virtual Condition choose_cond_suspension (const OpenCondMap &open_conds) = 0;
+	virtual Condition choose_cond_suspension_most_constrained (const OpenCondMap &open_conds)=0;
 
   protected:
     /// Pointer to Planner object.
diff --git a/Planner.cpp b/Planner.cpp
index 0d8c8a3ea70..68adafd2b87 100644
--- a/Planner.cpp
+++ b/Planner.cpp
@@ -265,8 +265,11 @@ void Planner::undo_command (CommandID id)
     throw msg;
   }
 
+  PlanCommand* tmp = this->cur_cmd_;
   this->cur_cmd_->undo ();
   this->cur_cmd_ = this->cur_cmd_->get_prev ();
+
+  delete tmp;
 };
 
 // Add a command to be executed later with execute_next().
@@ -318,6 +321,7 @@ void Planner::undo_through (CommandID id)
   temp->undo ();
   this->cur_cmd_ = temp->get_prev ();
 
+  delete temp;
 
 
   this->undo_through (id);
diff --git a/SANet/SANetFileIn.cpp b/SANet/SANetFileIn.cpp
index 1d6fbea107e..5025f4091ed 100644
--- a/SANet/SANetFileIn.cpp
+++ b/SANet/SANetFileIn.cpp
@@ -205,11 +205,9 @@ 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;
-	//Note: 0 here because of the initial action.  Changed from probTrue--Ben
 	builder->add_cond (nodeID, utility, probTrue, name, cond);
 
-//	if((probTrue-.5)*2 > 0)
-		builder->set_effect(20, nodeID, "", (probTrue-.5)*2);
+	builder->set_effect(20, nodeID, "", (probTrue-.5)*2);
   }
 
   // Get precondition->task links.
diff --git a/SA_POP_Types.h b/SA_POP_Types.h
index cbd9a3d3e77..10a6a16c509 100644
--- a/SA_POP_Types.h
+++ b/SA_POP_Types.h
@@ -3,17 +3,18 @@
 
 //=============================================================================
 /**
- * @file  SA_POP_Types.h
- *
- * This file contains the definitions of types used throughout SA-POP.
- *
- * @author  John S. Kinnebrew <john.s.kinnebrew@vanderbilt.edu>
- */
+* @file  SA_POP_Types.h
+*
+* This file contains the definitions of types used throughout SA-POP.
+*
+* @author  John S. Kinnebrew <john.s.kinnebrew@vanderbilt.edu>
+*/
 //=============================================================================
 
 #ifndef SA_POP_TYPES_H_
 #define SA_POP_TYPES_H_
 
+
 #include <string>
 #include <set>
 #include <list>
@@ -28,144 +29,145 @@
 #include "ace/Log_Priority.h"
 #endif  /* SA_POP_HAS_ACE */
 
+
 #define ANKET 1
 #define SA_POP_DEBUG_NORMAL 5
 #define SPARTAN 9
 #define SA_POP_DEBUG_HIGH 10
-
+#define _CRTDBG_MAP_ALLOC
 #if defined (SA_POP_HAS_ACE)
 #define SA_POP_DEBUG(x,y) \
-if (x > 8) \
-ACE_DEBUG ((LM_DEBUG, ACE_LIB_TEXT("SA-POP: %s\n"), y));
+	if (x > 8) \
+	ACE_DEBUG ((LM_DEBUG, ACE_LIB_TEXT("SA-POP: %s\n"), y));
 #define SA_POP_DEBUG_STR(x,y) \
-if (x > 8) \
-ACE_DEBUG ((LM_DEBUG, ACE_LIB_TEXT("SA-POP: %s\n"), y.c_str ()));
+	if (x > 8) \
+	ACE_DEBUG ((LM_DEBUG, ACE_LIB_TEXT("SA-POP: %s\n"), y.c_str ()));
 #else  /* SA_POP_HAS_ACE not defined */
 #include <iostream>
 #define SA_POP_DEBUG(x,y) \
-if (x > 0) \
-  std::cout << "SA-POP: " << y << std::endl;
+	if (x > 0) \
+	std::cout << "SA-POP: " << y << std::endl;
 #define SA_POP_DEBUG_STR(x,y) \
-if (x > 0) \
-  std::cout << "SA-POP: " << y << std::endl;
+	if (x > 0) \
+	std::cout << "SA-POP: " << y << std::endl;
 #endif  /* SA_POP_HAS_ACE */
 
 
 namespace SA_POP {
 
-  /// Type of a node id (tasks and conditions).
-  /// (must be > 0 and unique across all tasks *and* conditions).
-  typedef int NodeID;
+	/// Type of a node id (tasks and conditions).
+	/// (must be > 0 and unique across all tasks *and* conditions).
+	typedef int NodeID;
+
+	/// Type of a condition id.
+	/// (must be > 0 and unique across all tasks *and* conditions).
+	typedef NodeID CondID;
 
-  /// Type of a condition id.
-  /// (must be > 0 and unique across all tasks *and* conditions).
-  typedef NodeID CondID;
+	/// Type of a condition value.
+	typedef bool CondValue;
 
-  /// Type of a condition value.
-  typedef bool CondValue;
+	/// Type of a condition kind/type.
+	enum CondKind {ENVIRON, SYSTEM, DATA};
 
-  /// Type of a condition kind/type.
-  enum CondKind {ENVIRON, SYSTEM, DATA};
+	/// Type of a condition.
+	struct Condition {
+		CondID id;
+		CondValue value;
+		CondKind kind;
+		bool operator== (const Condition &s) const { return this->id == s.id && this->value == s.value; };
+		bool operator!= (const Condition &s) const { return !(*this == s); };
+		bool operator< (const Condition &s) const { return this->id < s.id; };
+	};
+
+	/// Type of a task ID.
+	/// (must be > 0 and unique across all tasks *and* conditions).
+	typedef NodeID TaskID;
 
-  /// Type of a condition.
-  struct Condition {
-    CondID id;
-    CondValue value;
-    CondKind kind;
-	bool operator== (const Condition &s) const { return this->id == s.id && this->value == s.value; };
-    bool operator!= (const Condition &s) const { return !(*this == s); };
-    bool operator< (const Condition &s) const { return this->id < s.id; };
-  };
+	/// Type of a task type ID.
+	typedef std::string TaskTypeID;
 
-  /// Type of a task ID.
-  /// (must be > 0 and unique across all tasks *and* conditions).
-  typedef NodeID TaskID;
+	/// NULL task ID (for unknown/uninitialized tasks).
+	const TaskID NULL_TASK_ID = 0;
 
-  /// Type of a task type ID.
-  typedef std::string TaskTypeID;
+	/// NULL condition ID (for unknown/uninitialized conditions).
+	const CondID NULL_COND_ID = 0;
 
-  /// NULL task ID (for unknown/uninitialized tasks).
-  const TaskID NULL_TASK_ID = 0;
+	/// Type of a port id.
+	typedef std::string PortID;
 
-  /// NULL condition ID (for unknown/uninitialized conditions).
-  const CondID NULL_COND_ID = 0;
+	/// Type of an expected utility calculation (basetype for others).
+	/// (N.B. this must be a double or float to handle [0,1] range probabilities
+	/// and multiplication factors).
+	typedef double EUCalc;
 
-  /// Type of a port id.
-  typedef std::string PortID;
+	/// Type of a condition utility.
+	typedef EUCalc Utility;
 
-  /// Type of an expected utility calculation (basetype for others).
-  /// (N.B. this must be a double or float to handle [0,1] range probabilities
-  /// and multiplication factors).
-  typedef double EUCalc;
-          
-  /// Type of a condition utility.
-  typedef EUCalc Utility;
+	/// Type of a probability.
+	typedef EUCalc Probability;
 
-  /// Type of a probability.
-  typedef EUCalc Probability;
+	/// Type of a (precondition or effect) link weight.
+	typedef EUCalc LinkWeight;
 
-  /// Type of a (precondition or effect) link weight.
-  typedef EUCalc LinkWeight;
+	/// Type of a task cost.
+	typedef EUCalc TaskCost;
 
-  /// Type of a task cost.
-  typedef EUCalc TaskCost;
+	/// Type of a multiplicative factor (e.g. attenuation factor).
+	typedef EUCalc MultFactor;
 
-  /// Type of a multiplicative factor (e.g. attenuation factor).
-  typedef EUCalc MultFactor;
+	/// Type of a precondition link.
+	typedef std::pair<CondID, TaskID> PrecondLink;
 
-  /// Type of a precondition link.
-  typedef std::pair<CondID, TaskID> PrecondLink;
+	/// Type of an effect link.
+	typedef std::pair<TaskID, CondID> EffectLink;
 
-  /// Type of an effect link.
-  typedef std::pair<TaskID, CondID> EffectLink;
+	/// Type of a pair of ports for a link.
+	typedef std::pair<PortID, PortID> LinkPorts;
 
-  /// Type of a pair of ports for a link.
-  typedef std::pair<PortID, PortID> LinkPorts;
+	/// Type of a list of tasks.
+	typedef std::list<TaskID> TaskList;
 
-  /// Type of a list of tasks.
-  typedef std::list<TaskID> TaskList;
+	/// Type of a set of goal conditions with associated utilities.
+	typedef std::map<CondID, Utility> GoalMap;
 
-  /// Type of a set of goal conditions with associated utilities.
-  typedef std::map<CondID, Utility> GoalMap;
+	/// Type of a set of tasks with associated expected utilities.
+	typedef std::map<TaskID, Utility> TaskEUMap;
 
-  /// Type of a set of tasks with associated expected utilities.
-  typedef std::map<TaskID, Utility> TaskEUMap;
+	/// Type of a set of conditions (condition & value).
+	typedef std::set<Condition> CondSet;
 
-  /// Type of a set of conditions (condition & value).
-  typedef std::set<Condition> CondSet;
+	/// Type of a set of task ids.
+	typedef std::set<TaskID> TaskSet;
 
-  /// Type of a set of task ids.
-  typedef std::set<TaskID> TaskSet;
 
+	/// Type of a task instance id.
+	/// (must be unique across all task instances).
+	typedef long TaskInstID;
 
-  /// Type of a task instance id.
-  /// (must be unique across all task instances).
-  typedef long TaskInstID;
+	/// Goal task instance ID (for task instance placeholders in open goals).
+	const TaskInstID GOAL_TASK_INST_ID = -1;
 
-  /// Goal task instance ID (for task instance placeholders in open goals).
-  const TaskInstID GOAL_TASK_INST_ID = -1;
+	/// Null task instance ID (for unknown/uninitialized task instances).
+	const TaskInstID NULL_TASK_INST_ID = -2;
 
-  /// Null task instance ID (for unknown/uninitialized task instances).
-  const TaskInstID NULL_TASK_INST_ID = -2;
+	/// Type of a task implementation id.
+	/// (must be unique across all task implementations).
+	typedef std::string TaskImplID;
 
-  /// Type of a task implementation id.
-  /// (must be unique across all task implementations).
-  typedef std::string TaskImplID;
+	/// Null task implementation ID (for unknown/uninitialized task implementations).
+	const TaskImplID NULL_TASK_IMPL_ID = "NULL_TASK_IMPL_ID";
 
-  /// Null task implementation ID (for unknown/uninitialized task implementations).
-  const TaskImplID NULL_TASK_IMPL_ID = "NULL_TASK_IMPL_ID";
+	/// Type of a task implementation parameter id/name.
+	typedef std::string ImplParamID;
 
-  /// Type of a task implementation parameter id/name.
-  typedef std::string ImplParamID;
+	/// Type of a task implementation parameter kind/type.
+	typedef std::string ImplParamKind;
 
-  /// Type of a task implementation parameter kind/type.
-  typedef std::string ImplParamKind;
+	/// Type of a task implementation parameter value.
+	typedef std::string ImplParamValue;
+	enum TaskChoiceType{REUSE_INST, NEW_INST};
+	struct TaskChoice {
 
-  /// 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;
@@ -173,626 +175,586 @@ namespace SA_POP {
 
 
 
-  ///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 {
-    ImplParamID id;
-    ImplParamKind kind;
-    ImplParamValue value;
-    bool operator== (const ImplParam &s) const { return this->id == s.id; };
-    bool operator!= (const ImplParam &s) const { return !(*this == s); };
-    bool operator< (const ImplParam &s) const { return this->id < s.id; };
-  };
-
-  /// Type of a resource id.
-  /// (must be unique across all resources).
-  typedef std::string ResourceID;
-
-  const ResourceID NULL_RESOURCE_ID = "NULL_RESOURCE_ID";
-
-  /// Type of a resource capacity or utilization value.
-  /// (must be >= 0.)
-  typedef int ResourceValue;
-
-  /// Type of a resource kind/type.
-  enum ResourceKind {UNARY, DISCRETE, RESERVOIR};
-
-  /// Type of a resource.
-  struct Resource {
-    ResourceID id;
-    ResourceValue capacity;
-    ResourceKind kind;
-    bool operator== (const Resource &s) const { return this->id == s.id; };
-    bool operator!= (const Resource &s) const { return !(*this == s); };
-    bool operator< (const Resource &s) const { return this->id < s.id; };
-  };
-
-  /// Type of a time (Seconds since Jan. 1, 1970).
-  /// (must be >= 0, or = NULL_TIME.)
-  typedef long TimeValue;
-
-  /// NULL time (for unknown or unconstrained times).
-  #define NULL_TIME -1
-
-  /// Time Window (e.g. start or end window for a task instance or plan).
-  typedef std::pair<SA_POP::TimeValue, SA_POP::TimeValue> TimeWindow;
-
-  /// Type of a command id, indicating when command was issued.
-  /// (indicates planning step, decision point within step, and
-  /// number of command within decision point and step.)
-  struct CommandID {
-    int step;
-    int decision_pt;
-    int seq_num;
-    bool operator== (const CommandID &s) const { return (this->step == s.step && this->decision_pt == s.decision_pt && this->seq_num == s.seq_num); };
-    bool operator!= (const CommandID &s) const { return !(*this == s); };
-    bool operator< (const CommandID &s) const
-    {
-      if (this->step == s.step) {
-        if (this->decision_pt == s.decision_pt)
-          return this->seq_num < s.seq_num;
-        return this->decision_pt < s.decision_pt;
-      }
-      return this->step < s.step;
-    };
-  };
-
-
-  /// Type of a causal link.
-  /// ("first" task instance achieves "cond" for "second" task instance.)
-  struct CausalLink {
-    TaskInstID first;
-    TaskInstID second;
-    Condition cond;
-    bool operator== (const CausalLink &s) const { return (this->first == s.first && this->second == s.second && this->cond == s.cond); };
-    bool operator!= (const CausalLink &s) const { return !(*this == s); };
-    bool operator< (const CausalLink &s) const
-    {
-      if (this->first == s.first) {
-        if (this->second == s.second)
-          return this->cond < s.cond;
-        return this->second < s.second;
-      }
-      return this->first < s.first;
-    };
-  };
-
-  // Type of a set of causal links.
-  typedef std::set<CausalLink> CLSet;
-
-  /// Type of a causal link threat.
-  /// ("threat" task instance threatens "clink" causal link)
-  struct CLThreat {
-    CausalLink clink;
-    TaskInstID threat;
-    bool operator== (const CLThreat &s) const { return (this->clink == s.clink && this->threat == s.threat); };
-    bool operator!= (const CLThreat &s) const { return !(*this == s); };
-    bool operator< (const CLThreat &s) const
-    {
-      if (this->clink == s.clink)
-        return this->threat < s.threat;
-      return this->clink < s.clink;
-    };
-  };
-
-  /// Type of a precedence relation.
-  enum PrecedenceRelation {UNRANKED, BEFORE, AFTER, SIMUL,
-    BEFORE_SIMUL, AFTER_SIMUL};
-
-  /// Type of a set of task implementation ids.
-  typedef std::set<TaskImplID> TaskImplSet;
-
-  /// Type of a list of task implementation ids.
-  typedef std::list<TaskImplID> TaskImplList;
-
-  /// Type of a set of task instance ids.
-  typedef std::set<TaskInstID> TaskInstSet;
-
-  /// Type of a set of task implementation parameters and associated values.
-  typedef std::set<ImplParam> ImplParamSet;
-
-  /// Type of a set of task instance ids.
-  typedef std::set<TaskInstID> TaskInstSet;
-
-  /// Type of a set of task instances mapped to task implementations.
-  typedef std::map <TaskInstID, TaskImplID> InstToImplMap;
-
-  /// Type of a set of resources and associated utilization.
-  typedef std::map<ResourceID, ResourceValue> ResourceMap;
-
-  /// Type of a set of resources (map from id to resource).
-  typedef std::map<ResourceID, Resource> Resources;
+	///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 {
+		ImplParamID id;
+		ImplParamKind kind;
+		ImplParamValue value;
+		bool operator== (const ImplParam &s) const { return this->id == s.id; };
+		bool operator!= (const ImplParam &s) const { return !(*this == s); };
+		bool operator< (const ImplParam &s) const { return this->id < s.id; };
+	};
+
+	/// Type of a resource id.
+	/// (must be unique across all resources).
+	typedef std::string ResourceID;
+
+	const ResourceID NULL_RESOURCE_ID = "NULL_RESOURCE_ID";
+
+	/// Type of a resource capacity or utilization value.
+	/// (must be >= 0.)
+	typedef int ResourceValue;
+
+	/// Type of a resource kind/type.
+	enum ResourceKind {UNARY, DISCRETE, RESERVOIR};
+
+	/// Type of a resource.
+	struct Resource {
+		ResourceID id;
+		ResourceValue capacity;
+		ResourceKind kind;
+		bool operator== (const Resource &s) const { return this->id == s.id; };
+		bool operator!= (const Resource &s) const { return !(*this == s); };
+		bool operator< (const Resource &s) const { return this->id < s.id; };
+	};
+
+	/// Type of a time (Seconds since Jan. 1, 1970).
+	/// (must be >= 0, or = NULL_TIME.)
+	typedef long TimeValue;
+
+	/// NULL time (for unknown or unconstrained times).
+#define NULL_TIME -1
+
+	/// Time Window (e.g. start or end window for a task instance or plan).
+	typedef std::pair<SA_POP::TimeValue, SA_POP::TimeValue> TimeWindow;
+
+	/// Type of a command id, indicating when command was issued.
+	/// (indicates planning step, decision point within step, and
+	/// number of command within decision point and step.)
+	struct CommandID {
+		int step;
+		int decision_pt;
+		int seq_num;
+		bool operator== (const CommandID &s) const { return (this->step == s.step && this->decision_pt == s.decision_pt && this->seq_num == s.seq_num); };
+		bool operator!= (const CommandID &s) const { return !(*this == s); };
+		bool operator< (const CommandID &s) const
+		{
+			if (this->step == s.step) {
+				if (this->decision_pt == s.decision_pt)
+					return this->seq_num < s.seq_num;
+				return this->decision_pt < s.decision_pt;
+			}
+			return this->step < s.step;
+		};
+	};
+
+
+	/// Type of a causal link.
+	/// ("first" task instance achieves "cond" for "second" task instance.)
+	struct CausalLink {
+		TaskInstID first;
+		TaskInstID second;
+		Condition cond;
+		bool operator== (const CausalLink &s) const { return (this->first == s.first && this->second == s.second && this->cond == s.cond); };
+		bool operator!= (const CausalLink &s) const { return !(*this == s); };
+		bool operator< (const CausalLink &s) const
+		{
+			if (this->first == s.first) {
+				if (this->second == s.second)
+					return this->cond < s.cond;
+				return this->second < s.second;
+			}
+			return this->first < s.first;
+		};
+	};
+
+	// Type of a set of causal links.
+	typedef std::set<CausalLink> CLSet;
+
+	/// Type of a causal link threat.
+	/// ("threat" task instance threatens "clink" causal link)
+	struct CLThreat {
+		CausalLink clink;
+		TaskInstID threat;
+		bool operator== (const CLThreat &s) const { return (this->clink == s.clink && this->threat == s.threat); };
+		bool operator!= (const CLThreat &s) const { return !(*this == s); };
+		bool operator< (const CLThreat &s) const
+		{
+			if (this->clink == s.clink)
+				return this->threat < s.threat;
+			return this->clink < s.clink;
+		};
+	};
+
+	/// Type of a precedence relation.
+	enum PrecedenceRelation {UNRANKED, BEFORE, AFTER, SIMUL,
+		BEFORE_SIMUL, AFTER_SIMUL};
+
+	/// Type of a set of task implementation ids.
+	typedef std::set<TaskImplID> TaskImplSet;
+
+	/// Type of a list of task implementation ids.
+	typedef std::list<TaskImplID> TaskImplList;
+
+	/// Type of a set of task instance ids.
+	typedef std::set<TaskInstID> TaskInstSet;
+
+	/// Type of a set of task implementation parameters and associated values.
+	typedef std::set<ImplParam> ImplParamSet;
+
+	/// Type of a set of task instance ids.
+	typedef std::set<TaskInstID> TaskInstSet;
+
+	/// Type of a set of task instances mapped to task implementations.
+	typedef std::map <TaskInstID, TaskImplID> InstToImplMap;
+
+	/// Type of a set of resources and associated utilization.
+	typedef std::map<ResourceID, ResourceValue> ResourceMap;
+
+	/// Type of a set of resources (map from id to resource).
+	typedef std::map<ResourceID, Resource> Resources;
 
 	/// Type of Criticality as a pair of the Resource it is in and the value.
 	typedef std::pair<ResourceID,double> Criticality;
 
 	/// Type of a set of conditions (condition & value).
-  typedef std::set<Condition> CondSet;
-
-  
-
-  /// Type of a set of causal link threats.
-  typedef std::set<CLThreat> CLThreatSet;
-
-  /// Type of a data connection in a plan.
-  struct PlanConnection {
-    TaskInstID first_task_inst;
-    TaskInstID second_task_inst;
-    PortID first_port;
-    PortID second_port;
-    CondID cond;
-    bool operator== (const PlanConnection &s) const { return (this->first_task_inst == s.first_task_inst && this->second_task_inst == s.second_task_inst && this->first_port == s.first_port && this->second_port == s.second_port && this->cond == s.cond); };
-    bool operator!= (const PlanConnection &s) const { return !(*this == s); };
-    bool operator< (const PlanConnection &s) const
-    {
-      if (this->first_task_inst == s.first_task_inst) {
-        if (this->second_task_inst == s.second_task_inst) {
-          if (this->first_port == s.first_port) {
-            if (this->second_port == s.second_port)
-              return this->cond < s.cond;
-            return this->second_port < s.second_port;
-          }
-          return this->first_port < s.first_port;
-        }
-        return this->second_task_inst < s.second_task_inst;
-      }
-      return this->first_task_inst < s.first_task_inst;
-    };
-  };
-
-  /// Type of a set of plan connections.
-  typedef std::set<PlanConnection> PlanConnSet;
-
-  /// Type of an instance in a plan.
-  struct PlanTaskInst {
-    // Unique ID of this task instance.
-    TaskInstID inst_id;
-
-    // Human-readable name (for debugging, not identification).
-    std::string name;
-
-    // Task ID of this instance.
-    TaskID task_id;
-
-    // Task type ID of this instance.
-    TaskTypeID type_id;
-
-    // ID of suggested implementation.
-    TaskImplID suggested_impl;
-
-    // Derived start window.
-    TimeWindow start_window;
-
-    // Derived end window.
-    TimeWindow end_window;
-
-    bool operator== (const PlanTaskInst &s) const { return (this->inst_id == s.inst_id); };
-    bool operator!= (const PlanTaskInst &s) const { return !(*this == s); };
-    bool operator< (const PlanTaskInst &s) const
-    {
-      return this->inst_id < s.inst_id;
-    };
-  };
-
-  /// Type of a set of plan instances.
-  typedef std::set<PlanTaskInst> PlanInstSet;
-
-  /// Type of a generic ordering link.
-  typedef std::pair<TaskInstID, TaskInstID> OrderLink;
-
-  /// Type of a scheduling link.
-  typedef OrderLink SchedLink;
-
-  /// Type of a set of scheduling links.
-  typedef std::set<SchedLink> SchedLinkSet;
-
-  // Type of a threat link.
-  typedef OrderLink ThreatLink;
-
-  /// Type of a set of threat links.
-  typedef std::set<ThreatLink> ThreatLinkSet;
-
-  /// Type of a goal ID.
-  typedef std::string GoalID;
-
-  /// Type of a plan ID.
-  typedef GoalID PlanID;
-
-  /// Type of a plan generated by SA-POP (output of a WorkingPlan object).
-  struct Plan {
-    // Unique ID of plan (from goal).
-    PlanID plan_id;
-
-    // Human-readable name of plan (used for debugging, not identification).
-    std::string name;
-
-    // Task instances in plan.
-    PlanInstSet task_insts;
-
-    // Data connections in plan.
-    PlanConnSet connections;
-
-    // Causal links (non-data) in plan.
-    CLSet causal_links;
-
-    // Scheduling links (added to prevent resource conflicts) in plan.
-    SchedLinkSet sched_links;
-
-    // Threat links (added to prevent causal link threats) in plan.
-    ThreatLinkSet threat_links;
-
-    // Expected start window.
-    TimeWindow start_window;
+	typedef std::set<Condition> CondSet;
+
+
+
+	/// Type of a set of causal link threats.
+	typedef std::set<CLThreat> CLThreatSet;
+
+	/// Type of a data connection in a plan.
+	struct PlanConnection {
+		TaskInstID first_task_inst;
+		TaskInstID second_task_inst;
+		PortID first_port;
+		PortID second_port;
+		CondID cond;
+		bool operator== (const PlanConnection &s) const { return (this->first_task_inst == s.first_task_inst && this->second_task_inst == s.second_task_inst && this->first_port == s.first_port && this->second_port == s.second_port && this->cond == s.cond); };
+		bool operator!= (const PlanConnection &s) const { return !(*this == s); };
+		bool operator< (const PlanConnection &s) const
+		{
+			if (this->first_task_inst == s.first_task_inst) {
+				if (this->second_task_inst == s.second_task_inst) {
+					if (this->first_port == s.first_port) {
+						if (this->second_port == s.second_port)
+							return this->cond < s.cond;
+						return this->second_port < s.second_port;
+					}
+					return this->first_port < s.first_port;
+				}
+				return this->second_task_inst < s.second_task_inst;
+			}
+			return this->first_task_inst < s.first_task_inst;
+		};
+	};
 
-    // Expected end window.
-    TimeWindow end_window;
+	/// Type of a set of plan connections.
+	typedef std::set<PlanConnection> PlanConnSet;
 
-    bool operator== (const Plan &s) const { return (this->task_insts == s.task_insts && this->connections == s.connections); };
-    bool operator!= (const Plan &s) const { return !(*this == s); };
-    bool operator< (const Plan &s) const
-    {
-      if (this->task_insts == s.task_insts)
-        return this->connections < s.connections;
-      return this->task_insts < s.task_insts;
-    };
-  };
+	/// Type of an instance in a plan.
+	struct PlanTaskInst {
+		// Unique ID of this task instance.
+		TaskInstID inst_id;
 
-  /// Type of a set of conditions with associated time constraints.
-  typedef std::map<CondID, TimeValue> TimeConstraintMap;
+		// Human-readable name (for debugging, not identification).
+		std::string name;
 
-  /// Type of a goal for SA-POP.
-  struct Goal
-  {
-    // Unique ID of goal.
-    GoalID goal_id;
+		// Task ID of this instance.
+		TaskID task_id;
 
-    // Human-readable name of goal (used for debugging, not identification).
-    std::string name;
+		// Task type ID of this instance.
+		TaskTypeID type_id;
 
-    // Goal conditions with utility values.
-    GoalMap goal_conds;
+		// ID of suggested implementation.
+		TaskImplID suggested_impl;
 
-    // Start-by and end-by constraints on conditions, in relative time
-    // (relative to start of opstring to achieve this goal).
-    TimeConstraintMap rel_times;
+		// Derived start window.
+		TimeWindow start_window;
 
-    // Start-by and end-by constraints on conditions, in clock time.
-    TimeConstraintMap abs_times;
+		// Derived end window.
+		TimeWindow end_window;
 
-    // Expected time range for start of opstring (for achieving this goal)
-    // execution.
-    TimeWindow start_window;
+		bool operator== (const PlanTaskInst &s) const { return (this->inst_id == s.inst_id); };
+		bool operator!= (const PlanTaskInst &s) const { return !(*this == s); };
+		bool operator< (const PlanTaskInst &s) const
+		{
+			return this->inst_id < s.inst_id;
+		};
+	};
 
-    bool operator== (const Goal &s) const { return (this->goal_id == s.goal_id); };
-    bool operator!= (const Goal &s) const { return !(*this == s); };
-    bool operator< (const Goal &s) const { return this->goal_id < s.goal_id; };
-  };
+	/// Type of a set of plan instances.
+	typedef std::set<PlanTaskInst> PlanInstSet;
 
-  struct TaskInstEndTimeSet{
-	TaskInstID inst;
-	double end_time;
+	/// Type of a generic ordering link.
+	typedef std::pair<TaskInstID, TaskInstID> OrderLink;
 
-	bool operator< (const TaskInstEndTimeSet & compare) const{
-		return end_time < compare.end_time;
-	};
-  };
+	/// Type of a scheduling link.
+	typedef OrderLink SchedLink;
+
+	/// Type of a set of scheduling links.
+	typedef std::set<SchedLink> SchedLinkSet;
+
+	// Type of a threat link.
+	typedef OrderLink ThreatLink;
+
+	/// Type of a set of threat links.
+	typedef std::set<ThreatLink> ThreatLinkSet;
+
+	/// Type of a goal ID.
+	typedef std::string GoalID;
+
+	/// Type of a plan ID.
+	typedef GoalID PlanID;
+
+	/// Type of a plan generated by SA-POP (output of a WorkingPlan object).
+	struct Plan {
+		// Unique ID of plan (from goal).
+		PlanID plan_id;
 
-  
-  /// Type of particular Task Implementation mapped to a Task Implementation Set.
-  /// This is a particular Precedence set. Like Before, After etc.
-  typedef std::map <TaskInstID, TaskInstSet> PrecedenceSet;
+		// Human-readable name of plan (used for debugging, not identification).
+		std::string name;
 
-  /// Type of a Precedence Relation mapped to a Precedence Set.
-  /// This gives the whole Precedence Graph
-  typedef std::map <PrecedenceRelation, PrecedenceSet> PrecedenceGraph;
-  
+		// Task instances in plan.
+		PlanInstSet task_insts;
 
-  /// Type of a set of open conditions, each associated with task instances
-  /// for which it is a precondition.
-//  typedef std::multimap<Condition, TaskInstID> OpenCondMap;
+		// Data connections in plan.
+		PlanConnSet connections;
 
-  /// Type of a set of open conditions, each associated with task instances
-  /// for which it is a precondition. With new list functionality
-  typedef SA_POP::ListMultiMap<Condition, TaskInstID> OpenCondMap;
+		// Causal links (non-data) in plan.
+		CLSet causal_links;
 
+		// Scheduling links (added to prevent resource conflicts) in plan.
+		SchedLinkSet sched_links;
 
+		// Threat links (added to prevent causal link threats) in plan.
+		ThreatLinkSet threat_links;
 
-  struct StoredCondition{
-	
-	Condition satisfied_cond;
-	TaskID satisfying_task;
-	bool satisfied_to;
+		// Expected start window.
+		TimeWindow start_window;
 
-	StoredCondition(){};
+		// Expected end window.
+		TimeWindow end_window;
 
-	StoredCondition(Condition cond, TaskID task){
-		this->satisfied_cond = cond;
-		this->satisfied_to = cond.value;
-		this->satisfying_task = task;
-	}
+		bool operator== (const Plan &s) const { return (this->task_insts == s.task_insts && this->connections == s.connections); };
+		bool operator!= (const Plan &s) const { return !(*this == s); };
+		bool operator< (const Plan &s) const
+		{
+			if (this->task_insts == s.task_insts)
+				return this->connections < s.connections;
+			return this->task_insts < s.task_insts;
+		};
+	};
+
+	/// Type of a set of conditions with associated time constraints.
+	typedef std::map<CondID, TimeValue> TimeConstraintMap;
+
+	/// Type of a goal for SA-POP.
+	struct Goal
+	{
+		// Unique ID of goal.
+		GoalID goal_id;
+
+		// Human-readable name of goal (used for debugging, not identification).
+		std::string name;
+
+		// Goal conditions with utility values.
+		GoalMap goal_conds;
+
+		// Start-by and end-by constraints on conditions, in relative time
+		// (relative to start of opstring to achieve this goal).
+		TimeConstraintMap rel_times;
+
+		// Start-by and end-by constraints on conditions, in clock time.
+		TimeConstraintMap abs_times;
+
+		// Expected time range for start of opstring (for achieving this goal)
+		// execution.
+		TimeWindow start_window;
 
-	bool operator==(const StoredCondition & s)const{
-		return (satisfied_cond == s.satisfied_cond &&
-			satisfying_task == s.satisfying_task &&
-			satisfied_to == s.satisfied_to);
+		bool operator== (const Goal &s) const { return (this->goal_id == s.goal_id); };
+		bool operator!= (const Goal &s) const { return !(*this == s); };
+		bool operator< (const Goal &s) const { return this->goal_id < s.goal_id; };
 	};
 
-	bool operator<(const StoredCondition & s) const{
-		if(this->satisfied_cond == s.satisfied_cond)
-			return this->satisfying_task < s.satisfying_task;
-		return this->satisfied_cond < s.satisfied_cond;
+	struct TaskInstEndTimeSet{
+		TaskInstID inst;
+		double end_time;
+
+		bool operator< (const TaskInstEndTimeSet & compare) const{
+			return end_time < compare.end_time;
+		};
 	};
-  };
 
-  struct StoredConditionKey{
 
-	CommandID satisfying_cmd;
-	CommandID free_pass_cmd;
+	/// Type of particular Task Implementation mapped to a Task Implementation Set.
+	/// This is a particular Precedence set. Like Before, After etc.
+	typedef std::map <TaskInstID, TaskInstSet> PrecedenceSet;
+
+	/// Type of a Precedence Relation mapped to a Precedence Set.
+	/// This gives the whole Precedence Graph
+	typedef std::map <PrecedenceRelation, PrecedenceSet> PrecedenceGraph;
+
+
+	/// Type of a set of open conditions, each associated with task instances
+	/// for which it is a precondition. With new list functionality
+	typedef SA_POP::ListMultiMap<Condition, TaskInstID> OpenCondMap;
 
-	StoredCondition satisfy_set;
-	bool free_pass_used;
 
-//	StoredConditionKey(){};
+	//Following structures form the condition-checking heuristic we use; if a task satisfies a condition for a 
+	//instance task once, it cannot satisfy another instance of that task unless some of the open conditions open
+	//when it was first used
 
-	StoredConditionKey(Condition cond, TaskID task, CommandID cmd){
-		satisfying_cmd = cmd;
-		satisfy_set.satisfied_cond = cond;
-		satisfy_set.satisfying_task = task;
+	struct StoredCondition{
 
-		free_pass_cmd.decision_pt = 0;
-		free_pass_cmd.step = 0;
-		free_pass_cmd.seq_num = 0;
+		Condition satisfied_cond;
+		TaskID satisfying_task;
+		bool satisfied_to;
 
-		free_pass_used = false;
-	}
+		StoredCondition(){};
 
-	bool operator==(const StoredConditionKey & s)const{
-		return (this->satisfy_set == s.satisfy_set);
+		StoredCondition(Condition cond, TaskID task){
+			this->satisfied_cond = cond;
+			this->satisfied_to = cond.value;
+			this->satisfying_task = task;
+		}
+
+		bool operator==(const StoredCondition & s)const{
+			return (satisfied_cond == s.satisfied_cond &&
+				satisfying_task == s.satisfying_task &&
+				satisfied_to == s.satisfied_to);
+		};
+
+		bool operator<(const StoredCondition & s) const{
+			if(this->satisfied_cond == s.satisfied_cond)
+				return this->satisfying_task < s.satisfying_task;
+			return this->satisfied_cond < s.satisfied_cond;
+		};
 	};
 
-	bool operator<(const StoredConditionKey & s) const {
-	//	if(this->satisfying_cmd == s.satisfying_cmd)
+	struct StoredConditionKey{
+
+		CommandID satisfying_cmd;
+		CommandID free_pass_cmd;
+
+		StoredCondition satisfy_set;
+		bool free_pass_used;
+
+		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{
+			return (this->satisfy_set == s.satisfy_set);
+		};
+
+		bool operator<(const StoredConditionKey & s) const {
 			return this->satisfy_set < s.satisfy_set;
-	//	return this->satisfying_cmd < s.satisfying_cmd;
+		};
+
 	};
 
-  };
 
 
+	typedef std::set<StoredCondition> StoredConditionSet;
+	typedef std::multimap<StoredConditionKey, StoredConditionSet> StoredConditionMap;
 
-  typedef std::set<StoredCondition> StoredConditionSet;
-  typedef std::multimap<StoredConditionKey, StoredConditionSet> StoredConditionMap;
+	struct StoredConditionEvaluator{
+		StoredConditionMap condition_map;
 
-  struct StoredConditionEvaluator{
-	  StoredConditionMap condition_map;
-	
-	  std::pair<bool, CommandID> should_continue(CommandID cur_cmd, Condition satisfied_cond, 
-		  TaskID satisfying_task, OpenCondMap & open_conds, InstToTaskMap & task_insts){
+		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);
+				StoredConditionKey stored_cond (satisfied_cond, satisfying_task, cur_cmd);
 
-			  std::pair<StoredConditionMap::iterator,
-				   StoredConditionMap::iterator>
-				  range 
-				  = condition_map.equal_range(stored_cond);
+				std::pair<StoredConditionMap::iterator,
+					StoredConditionMap::iterator>
+					range 
+					= 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 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)
-			  {
-				for(StoredConditionMap::iterator it = range.first; it != range.second; it++)
-				{
-					bool all_pairs_in_open_conds = true;
+				bool using_free_pass = false;
 
-					StoredConditionSet old_open_conds = it->second;
-		
-					for(StoredConditionSet::iterator it2 = old_open_conds.begin();
-						it2 != old_open_conds.end(); it2++)
+				if(range.first != range.second)
+				{
+					for(StoredConditionMap::iterator it = range.first; it != range.second; it++)
 					{
+						bool all_pairs_in_open_conds = true;
+
+						StoredConditionSet old_open_conds = it->second;
+
+						for(StoredConditionSet::iterator it2 = old_open_conds.begin();
+							it2 != old_open_conds.end(); it2++)
+						{
 
 
-						std::pair<OpenCondMap::iterator, OpenCondMap::iterator>
-							cond_map_range = open_conds.equal_range((*it2).satisfied_cond);
+							std::pair<OpenCondMap::iterator, OpenCondMap::iterator>
+								cond_map_range = open_conds.equal_range((*it2).satisfied_cond);
 
-						bool found_no_equiv_in_open = true;
+							bool found_no_equiv_in_open = true;
 
-						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;
+							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 &&
-									(*it2).satisfied_to == ((*it3).first).value){
-									found_no_equiv_in_open = false;
+								else{
+									if((*it3).second == -1)
+									{}
+									else if((*it2).satisfying_task == task_insts.find((*it3).second)->second &&
+										(*it2).satisfied_to == ((*it3).first).value){
+											found_no_equiv_in_open = false;
+									}
 								}
 							}
-						}
 
-						if(found_no_equiv_in_open){
-							all_pairs_in_open_conds = false;
-							break;
+							if(found_no_equiv_in_open){
+								all_pairs_in_open_conds = false;
+								break;
+							}
 						}
-					}
-
-					if(all_pairs_in_open_conds){
-
-			//			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;
+						if(all_pairs_in_open_conds){
 
-			//				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;
 				}
-			}else{
-				should_continue = true;
-			}
-
-			if(should_continue && !using_free_pass)
-			{
 
-				StoredConditionSet current_open;
-				for(OpenCondMap::iterator it = open_conds.begin(); it != open_conds.end(); it++)
+				if(should_continue && !using_free_pass)
 				{
-					if(it->second == -1)
+
+					StoredConditionSet current_open;
+					for(OpenCondMap::iterator it = open_conds.begin(); it != open_conds.end(); it++)
 					{
-						current_open.insert(StoredCondition(it->first, -1));
-					}else{
-						current_open.insert(StoredCondition(it->first, task_insts.find(it->second)->second));
+						if(it->second == -1)
+						{
+							current_open.insert(StoredCondition(it->first, -1));
+						}else{
+							current_open.insert(StoredCondition(it->first, task_insts.find(it->second)->second));
+						}
 					}
-				}
 
-				StoredConditionKey this_choice(satisfied_cond, satisfying_task, cur_cmd);
+					StoredConditionKey this_choice(satisfied_cond, satisfying_task, cur_cmd);
 
-				this->condition_map.insert(
-					std::pair<StoredConditionKey, StoredConditionSet>(this_choice, current_open));
-			}
+					this->condition_map.insert(
+						std::pair<StoredConditionKey, StoredConditionSet>(this_choice, current_open));
+				}
 
-			return std::pair<bool, CommandID>(should_continue, return_to);
+				return std::pair<bool, CommandID>(should_continue, return_to);
 		}
 
-	  void undo_binding(CommandID cur_cmd, Condition satisfied_cond, TaskID satisfying_task){
-		StoredConditionKey key(satisfied_cond, satisfying_task, cur_cmd);
-
-		std::pair<StoredConditionMap::iterator, StoredConditionMap::iterator> range
-			= condition_map.equal_range(key);
-
-		StoredConditionMap::iterator it;
+		void undo_binding(CommandID cur_cmd, Condition satisfied_cond, TaskID satisfying_task){
+			StoredConditionKey key(satisfied_cond, satisfying_task, cur_cmd);
 
-		for(it = range.first; it != range.second; it++){
-			if(it->first.satisfying_cmd == cur_cmd){
+			std::pair<StoredConditionMap::iterator, StoredConditionMap::iterator> range
+				= condition_map.equal_range(key);
 
-		//		if(it->first.free_pass_used == true){
-		//			StoredConditionKey key2 = it->first;
-		//			StoredConditionSet set2 = it->second;
+			StoredConditionMap::iterator it;
 
-		//			condition_map.erase(it);
-
-		//			key2.free_pass_used = false;
-
-		//			condition_map.insert(std::pair<StoredConditionKey, StoredConditionSet>(key2, set2));
-		//			break;
-		//		}else{				
+			for(it = range.first; it != range.second; it++){
+				if(it->first.satisfying_cmd == cur_cmd){	
 					condition_map.erase(it);
 					break;
-		//		}
+				}
 			}
 		}
+	};
 
+	//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::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)
+	{
+		std::ostringstream o;
+		if (!(o << x))
+			throw "to_string(int) error writing to ostringstream";
+		return o.str();
+	};
+
+	inline std::string to_string(long x)
+	{
+		std::ostringstream o;
+		if (!(o << x))
+			throw "to_string(long) error writing to ostringstream";
+		return o.str();
+	};
 
-	  }
-  };
-
-  //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)
-  {
-    std::ostringstream o;
-    if (!(o << x))
-      throw "to_string(int) error writing to ostringstream";
-    return o.str();
-  };
-
-  inline std::string to_string(long x)
-  {
-    std::ostringstream o;
-    if (!(o << x))
-      throw "to_string(long) error writing to ostringstream";
-    return o.str();
-  };
-
-  inline std::string to_string(unsigned long x)
-  {
-    std::ostringstream o;
-    if (!(o << x))
-      throw "to_string(unsigned long) error writing to ostringstream";
-    return o.str();
-  };
-
-  inline std::string to_string(double x)
-  {
-    std::ostringstream o;
-    if (!(o << x))
-      throw "to_string(double) error writing to ostringstream";
-    return o.str();
-  };
-
-  inline std::string to_string(float x)
-  {
-    std::ostringstream o;
-    if (!(o << x))
-      throw "to_string(float) error writing to ostringstream";
-    return o.str();
-  };
-
-  inline std::string to_string(CommandID x)
-  {
-    std::ostringstream o;
-    if (!(o << x.step << "." << x.decision_pt << "." << x.seq_num))
-      throw "to_string(CommandID) error writing to ostringstream";
-    return o.str();
-  };
+	inline std::string to_string(unsigned long x)
+	{
+		std::ostringstream o;
+		if (!(o << x))
+			throw "to_string(unsigned long) error writing to ostringstream";
+		return o.str();
+	};
+
+	inline std::string to_string(double x)
+	{
+		std::ostringstream o;
+		if (!(o << x))
+			throw "to_string(double) error writing to ostringstream";
+		return o.str();
+	};
+
+	inline std::string to_string(float x)
+	{
+		std::ostringstream o;
+		if (!(o << x))
+			throw "to_string(float) error writing to ostringstream";
+		return o.str();
+	};
+
+	inline std::string to_string(CommandID x)
+	{
+		std::ostringstream o;
+		if (!(o << x.step << "." << x.decision_pt << "." << x.seq_num))
+			throw "to_string(CommandID) error writing to ostringstream";
+		return o.str();
+	};
 
 };  /* SA_POP namespace */
 
diff --git a/SA_PlanStrategy.cpp b/SA_PlanStrategy.cpp
index e3023bff98a..b3b517598c4 100644
--- a/SA_PlanStrategy.cpp
+++ b/SA_PlanStrategy.cpp
@@ -3,14 +3,14 @@
 
 //=============================================================================
 /**
- * @file  SA_PlanStrategy.cpp
- *
- * This file contains the implementation of the SA_PlanStrategy concrete class,
- * which implements a PlanStrategy for use with spreading activation networks
- * and precedence graphs.
- *
- * @author  John S. Kinnebrew <john.s.kinnebrew@vanderbilt.edu>
- */
+* @file  SA_PlanStrategy.cpp
+*
+* This file contains the implementation of the SA_PlanStrategy concrete class,
+* which implements a PlanStrategy for use with spreading activation networks
+* and precedence graphs.
+*
+* @author  John S. Kinnebrew <john.s.kinnebrew@vanderbilt.edu>
+*/
 //=============================================================================
 
 #include <set>
@@ -22,179 +22,184 @@
 #include "SA_WorkingPlan.h"
 #include <fstream>
 
+
+
+
 using namespace SA_POP;
 
 // Constructor.
 SA_PlanStrategy::SA_PlanStrategy (SA_POP::Planner *planner,
-  CondStrategy *cond_choice, TaskStrategy *task_choice,
-  ImplStrategy *impl_choice)
-: PlanStrategy (planner),
-has_cmds_ (false),
-cur_task_ (SA_POP::NULL_TASK_ID),
-cur_task_inst_ (SA_POP::NULL_TASK_ID),
-cur_step_ (0),
-cur_decision_pt_ (SA_POP::SA_PlanStrategy::TASK_DECISION),
-cur_seq_num_ (1),
-cond_choice_ (cond_choice),
-task_choice_ (task_choice),
-impl_choice_ (impl_choice),
-add_conds_cmd_ (0),
-rmv_conds_cmd_ (0),
-add_threats_cmd_ (0),
-rmv_threats_cmd_ (0),
-add_task_cmd_ (0),
-assoc_impl_cmd_ (0),
-resolve_threat_cmd_ (0),
-open_conds_(SA_POP::FrontBack::BACK, SA_POP::FrontBack::FRONT)
+								  CondStrategy *cond_choice, TaskStrategy *task_choice,
+								  ImplStrategy *impl_choice)
+								  : PlanStrategy (planner),
+								  has_cmds_ (false),
+								  cur_task_ (SA_POP::NULL_TASK_ID),
+								  cur_task_inst_ (SA_POP::NULL_TASK_ID),
+								  cur_step_ (0),
+								  cur_decision_pt_ (SA_POP::SA_PlanStrategy::TASK_DECISION),
+								  cur_seq_num_ (1),
+								  cond_choice_ (cond_choice),
+								  task_choice_ (task_choice),
+								  impl_choice_ (impl_choice),
+								  add_conds_cmd_ (0),
+								  rmv_conds_cmd_ (0),
+								  add_threats_cmd_ (0),
+								  rmv_threats_cmd_ (0),
+								  add_task_cmd_ (0),
+								  assoc_impl_cmd_ (0),
+								  resolve_threat_cmd_ (0),
+								  open_conds_(SA_POP::FrontBack::BACK, SA_POP::FrontBack::FRONT)
 {
-  this->add_conds_cmd_ = new SA_AddOpenCondsCmd (this);
-  this->rmv_conds_cmd_ = new SA_RemoveOpenCondsCmd (this);
-  this->add_threats_cmd_ = new SA_AddOpenThreatsCmd (this);
-  this->rmv_threats_cmd_ = new SA_RemoveOpenThreatsCmd (this);
-  this->open_conds_.clear ();
-  this->open_threats_.clear ();
+	this->add_conds_cmd_ = new SA_AddOpenCondsCmd (this);
+	this->rmv_conds_cmd_ = new SA_RemoveOpenCondsCmd (this);
+	this->add_threats_cmd_ = new SA_AddOpenThreatsCmd (this);
+	this->rmv_threats_cmd_ = new SA_RemoveOpenThreatsCmd (this);
+	this->open_conds_.clear ();
+	this->open_threats_.clear ();
 };
 
 // Destructor.
 SA_PlanStrategy::~SA_PlanStrategy (void)
 {
-  // Delete heuristic strategies.
-  delete this->cond_choice_;
-  delete this->task_choice_;
-  delete this->impl_choice_;
+	// Delete heuristic strategies.
+	delete this->cond_choice_;
+	delete this->task_choice_;
+	delete this->impl_choice_;
 };
 
 void SA_PlanStrategy::reset()
 {
-  has_cmds_ = false;
-  cur_task_ = SA_POP::NULL_TASK_ID;
-  cur_task_inst_ = SA_POP::NULL_TASK_ID;
-  cur_step_ = 0;
-  cur_decision_pt_ = SA_POP::SA_PlanStrategy::TASK_DECISION;
-  cur_seq_num_ = 1;
-  this->add_conds_cmd_ = new SA_AddOpenCondsCmd (this);
-  this->rmv_conds_cmd_ = new SA_RemoveOpenCondsCmd (this);
-  this->add_threats_cmd_ = new SA_AddOpenThreatsCmd (this);
-  this->rmv_threats_cmd_ = new SA_RemoveOpenThreatsCmd (this);
-  this->open_conds_.clear ();
-  this->open_threats_.clear ();
+	has_cmds_ = false;
+	cur_task_ = SA_POP::NULL_TASK_ID;
+	cur_task_inst_ = SA_POP::NULL_TASK_ID;
+	cur_step_ = 0;
+	cur_decision_pt_ = SA_POP::SA_PlanStrategy::TASK_DECISION;
+	cur_seq_num_ = 1;
+	this->add_conds_cmd_ = new SA_AddOpenCondsCmd (this);
+	this->rmv_conds_cmd_ = new SA_RemoveOpenCondsCmd (this);
+	this->add_threats_cmd_ = new SA_AddOpenThreatsCmd (this);
+	this->rmv_threats_cmd_ = new SA_RemoveOpenThreatsCmd (this);
+	this->open_conds_.clear ();
+	this->open_threats_.clear ();
 };
 
 // Set command prototypes to use in planning.
 void SA_PlanStrategy::set_commands (AddTaskCmd *add_task_cmd,
-  AssocTaskImplCmd *assoc_impl_cmd,
-  ResolveCLThreatCmd *resolve_threat_cmd)
+									AssocTaskImplCmd *assoc_impl_cmd,
+									ResolveCLThreatCmd *resolve_threat_cmd)
 {
-  this->has_cmds_ = true;
-
-  if (add_task_cmd)
-    this->add_task_cmd_ = add_task_cmd;
-  else if (this->add_task_cmd_ == 0)
-    this->has_cmds_ = false;
-
-  if (assoc_impl_cmd)
-    this->assoc_impl_cmd_ = assoc_impl_cmd;
-  else if (this->assoc_impl_cmd_ == 0)
-    this->has_cmds_ = false;
-
-  if (resolve_threat_cmd)
-    this->resolve_threat_cmd_ = resolve_threat_cmd;
-  else if (this->resolve_threat_cmd_ == 0)
-    this->has_cmds_ = false;
+	this->has_cmds_ = true;
+
+	if (add_task_cmd)
+		this->add_task_cmd_ = add_task_cmd;
+	else if (this->add_task_cmd_ == 0)
+		this->has_cmds_ = false;
+
+	if (assoc_impl_cmd)
+		this->assoc_impl_cmd_ = assoc_impl_cmd;
+	else if (this->assoc_impl_cmd_ == 0)
+		this->has_cmds_ = false;
+
+	if (resolve_threat_cmd)
+		this->resolve_threat_cmd_ = resolve_threat_cmd;
+	else if (this->resolve_threat_cmd_ == 0)
+		this->has_cmds_ = false;
 };
 
 // Set goals.
 void SA_PlanStrategy::set_goals (GoalMap goals)
 {
-  for (SA_POP::GoalMap::iterator iter = goals.begin ();
-    iter != goals.end (); iter++)
-  {
-    Condition cond;
-    cond.id = iter->first;
-    if (iter->second > 0)
-      cond.value = true;
-    else
-      cond.value = false;
-    //****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
-    // Get type of condition.
-    cond.kind = SA_POP::SYSTEM;
-
-    this->open_conds_.insert (std::make_pair (cond, SA_POP::GOAL_TASK_INST_ID));
-  }
+	for (SA_POP::GoalMap::iterator iter = goals.begin ();
+		iter != goals.end (); iter++)
+	{
+		Condition cond;
+		cond.id = iter->first;
+		if (iter->second > 0)
+			cond.value = true;
+		else
+			cond.value = false;
+		//****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
+		// Get type of condition.
+		cond.kind = SA_POP::SYSTEM;
+
+		this->open_conds_.insert (std::make_pair (cond, SA_POP::GOAL_TASK_INST_ID));
+	}
 };
 
 // Get command ID to use for next command.
 CommandID SA_PlanStrategy::get_next_cmd_id (void)
 {
-  CommandID temp;
-  temp.step = this->cur_step_;
-  temp.decision_pt = this->cur_decision_pt_;
-  temp.seq_num = this->cur_seq_num_;
+	CommandID temp;
+	temp.step = this->cur_step_;
+	temp.decision_pt = this->cur_decision_pt_;
+	temp.seq_num = this->cur_seq_num_;
 
-  this->cur_seq_num_++;
-  return temp;
+	this->cur_seq_num_++;
+	return temp;
 };
 
 // Recursively satisfy all open conditions (including recursive
 // scheduling constraint satisfaction through call back).
 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();
-//))//!(this->planner_->get_working_plan()->get_all_insts().size() > 8))
-  if(true){
-	  // Increment step counter.
-	  this->cur_step_++;
-
-	  // Set decision point and reset sequence number for commands.
-	  this->cur_decision_pt_ = SA_PlanStrategy::TASK_DECISION;
-	  this->cur_seq_num_ = 1;
-
-	  // Variable for preconditions of current task.
-	  CondSet preconds;
-
-	  // Choose an open condition to satisfy.
-	  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)){
+	// If all open conditions have been satisfied, then return true for success.
+	if (this->open_conds_.empty ())
+		return this->planner_->full_sched();
+
+	//Note: change this number to limit the size of the final plan.  Set true for unlimited
+	//!(this->planner_->get_working_plan()->get_all_insts().size() > 12)
+	if(!(this->planner_->get_working_plan()->get_all_insts().size() > 8)){
+		// Increment step counter.
+		this->cur_step_++;
+
+		// Set decision point and reset sequence number for commands.
+		this->cur_decision_pt_ = SA_PlanStrategy::TASK_DECISION;
+		this->cur_seq_num_ = 1;
+
+		// Variable for preconditions of current task.
+		CondSet preconds;
+
+		// Choose an open condition to satisfy.
+		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.
+		AddTaskCmd *add_task_cmd = this->satisfy_cond (open_cond);
 
-	  // Choose task to satisfy open condition (actually an ordered list of
-	  // tasks to try), passing command to planner to be executed next.
-	  AddTaskCmd *add_task_cmd = this->satisfy_cond (open_cond);
+		TaskInstSet old_satisfied_insts = this->satisfied_insts;
 
-	  TaskInstSet old_satisfied_insts = this->satisfied_insts;
+		this->satisfied_insts = add_task_cmd->get_satisfied_tasks();
 
-	  this->satisfied_insts = add_task_cmd->get_satisfied_tasks();
-	  
-	  TaskInstID prev_cur_inst = this->cur_task_inst_;
+		TaskInstID prev_cur_inst = this->cur_task_inst_;
 
-	  // Try tasks until one yields a complete plan or all have been tried.
-	  while (this->planner_->try_next (add_task_cmd->get_id ())) {
+		// 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();
+			this->added_links = add_task_cmd->get_causal_insertions();
 
-		// Get current task and task instance.
-		this->cur_task_ = add_task_cmd->get_task ();
+			// Get current task and task instance.
+			this->cur_task_ = add_task_cmd->get_task ();
 
-		this->cur_task_inst_ = add_task_cmd->get_task_inst ();
-		// Remove open condition.
-		CommandID rmv_cond_cmd_id = this->rmv_open_cond (open_cond, add_task_cmd->get_satisfied_tasks());
+			this->cur_task_inst_ = add_task_cmd->get_task_inst ();
+			// Remove open condition.
+			CommandID rmv_cond_cmd_id = this->rmv_open_cond (open_cond, add_task_cmd->get_satisfied_tasks());
 
-		TaskID stored_task = this->cur_task_;
+			TaskID stored_task = this->cur_task_;
 
 			bool to_fail = false;
 
 			// Add preconditions of this task of we didn't reuse the task instance.
 			CommandID add_preconds_cmd_id;
 			if(!add_task_cmd->inst_exists())
-  			{
-     			preconds = this->planner_->get_unsat_preconds (this->cur_task_);
-			  add_preconds_cmd_id = 
-			  this->add_open_conds (preconds, this->cur_task_inst_);
-  			}
+			{
+				preconds = this->planner_->get_unsat_preconds (this->cur_task_);
+				add_preconds_cmd_id = 
+					this->add_open_conds (preconds, this->cur_task_inst_);
+			}
 
 			//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(), 
@@ -206,14 +211,10 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
 				if (this->satisfy_everything())
 					return true;
 
-			}else{
-				bool oops = true;
 			}
 
 			this->store_map.undo_binding(add_task_cmd->get_id(), add_task_cmd->get_condition(), stored_task);
 
-
-
 			std::ostringstream debug_text;
 			debug_text<<" the task instance being deleted is "<<add_task_cmd->get_task_inst()<<std::endl;
 			SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
@@ -223,176 +224,154 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
 			if(!add_task_cmd->inst_exists())
 				this->planner_->undo_command (add_preconds_cmd_id);
 
+			// Undo removal of open condition.
+			this->planner_->undo_command (rmv_cond_cmd_id);
 
+		}
 
-		// Undo removal of open condition.
-		this->planner_->undo_command (rmv_cond_cmd_id);
-
-	  }
+		this->cur_task_inst_ = prev_cur_inst;
 
-	  this->cur_task_inst_ = prev_cur_inst;
+		SA_POP_DEBUG (SA_POP_DEBUG_NORMAL, "Backtracking to previous step...");
+		// Undo addition of task.
 
-	  SA_POP_DEBUG (SA_POP_DEBUG_NORMAL, "Backtracking to previous step...");
-	  // Undo addition of task.
-	  
-	  this->satisfied_insts = old_satisfied_insts;
-	  this->planner_->undo_command (add_task_cmd->get_id ());
+		this->satisfied_insts = old_satisfied_insts;
+		this->planner_->undo_command (add_task_cmd->get_id ());
 
 
 
-	  // Decrement step.
-	  this->cur_step_--;
+		// Decrement step.
+		this->cur_step_--;
 
-  }else{
-//	  std::cout<<"Backing up beccause of iterative deepening"<<std::endl;
-  }
+	}
 
-  // No task could satisfy open condition, so return failure.
-  return false;
+	// No task could satisfy open condition, so return failure.
+	return false;
 };
 
 bool SA_PlanStrategy::satisfy_everything(){
 
-  //for each implementation
-    this->cur_decision_pt_ = SA_PlanStrategy::IMPL_DECISION;
-    this->cur_seq_num_ = 1;
+	//for each implementation
+	this->cur_decision_pt_ = SA_PlanStrategy::IMPL_DECISION;
+	this->cur_seq_num_ = 1;
 
-    AssocTaskImplCmd *assoc_impl_cmd;
-    TaskImplList impl_list;
+	AssocTaskImplCmd *assoc_impl_cmd;
+	TaskImplList impl_list;
 
-    // Choose a task implementation.
+	// Choose a task implementation.
 	assoc_impl_cmd =
-		  static_cast<AssocTaskImplCmd *> (this->assoc_impl_cmd_->clone ());
-		if(!this->planner_->inst_exists(this->cur_task_inst_)) impl_list = this->impl_choice_->choose_impl (this->cur_task_inst_);
-		else impl_list.push_back(this->planner_->get_impl_id(this->cur_task_inst_));
-		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);
+		static_cast<AssocTaskImplCmd *> (this->assoc_impl_cmd_->clone ());
+	if(!this->planner_->inst_exists(this->cur_task_inst_)) impl_list = this->impl_choice_->choose_impl (this->cur_task_inst_);
+	else impl_list.push_back(this->planner_->get_impl_id(this->cur_task_inst_));
+	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);
 
-		assoc_impl_cmd->set_satisfied_insts(this->satisfied_insts);
+	assoc_impl_cmd->set_satisfied_insts(this->satisfied_insts);
 
-		assoc_impl_cmd->set_added_links(this->added_links);
+	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 ())) 
-		{
+	this->cur_task_inst_ = assoc_impl_cmd->get_task_inst ();
 
-			//Move this code to the threat resolution sequence
-			// Set decision point and reset sequence number for commands.
-			this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
-			this->cur_seq_num_ = 1;
+	while (this->planner_->try_next (assoc_impl_cmd->get_id ())) 
+	{
 
+		//Move this code to the threat resolution sequence
+		// Set decision point and reset sequence number for commands.
+		this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
+		this->cur_seq_num_ = 1;
 
-			//Deal with threats
 
-			//Actually build the list of threats
-			this->planner_->generate_all_threats();
+		//Deal with threats
 
+		//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;
-			if (are_threats)
-			  add_threats_cmd_id = 
-				this->add_open_threats (this->planner_->get_all_threats ());
 
-	//		((SA_WorkingPlan*)(this->planner_->get_working_plan()))->
-	//		print_precedence_graph("SA_PlanStrategy::get_next_threat_resolution");
-			if(this->get_next_threat_resolution()){
-				return true;
-			}
-			else{
-	      
-			  this->cur_decision_pt_ = SA_PlanStrategy::IMPL_DECISION;
-			}
-
-		     SA_POP_DEBUG(SA_POP_DEBUG_NORMAL, "Backtracking from task assoc...");
-			// Undo addition of causal link threats from this task.
-			if (are_threats)
-				this->planner_->undo_command (add_threats_cmd_id);
+		// Add causal link threats to open threats.
+		bool are_threats = !(this->planner_->get_all_threats().empty ());
+		CommandID add_threats_cmd_id;
+		if (are_threats)
+			add_threats_cmd_id = 
+			this->add_open_threats (this->planner_->get_all_threats ());
 
+		if(this->get_next_threat_resolution()){
+			return true;
+		}
+		else{
 
+			this->cur_decision_pt_ = SA_PlanStrategy::IMPL_DECISION;
 		}
 
-    //Undo the AssocImplCmd
+		SA_POP_DEBUG(SA_POP_DEBUG_NORMAL, "Backtracking from task assoc...");
+		// Undo addition of causal link threats from this task.
+		if (are_threats)
+			this->planner_->undo_command (add_threats_cmd_id);
+
+
+	}
+
+	//Undo the AssocImplCmd
 
 	planner_->undo_command(assoc_impl_cmd->get_id());
 
-    this->cur_decision_pt_ = SA_PlanStrategy::IMPL_DECISION;
+	this->cur_decision_pt_ = SA_PlanStrategy::IMPL_DECISION;
 
-    return false;
+	return false;
 }
 bool SA_PlanStrategy::satisfy_schedule(void){
 
 
-          // Set decision point and reset sequence number for commands.
-          this->cur_decision_pt_ = SA_PlanStrategy::SCHEDULE_DECISION;
-          this->cur_seq_num_ = 1;
+	// Set decision point and reset sequence number for commands.
+	this->cur_decision_pt_ = SA_PlanStrategy::SCHEDULE_DECISION;
+	this->cur_seq_num_ = 1;
 
-          // Try to schedule and recursively continue planning.
-          if (this->planner_->recurse_sched (this->cur_task_inst_))
-            return true;
+	// Try to schedule and recursively continue planning.
+	if (this->planner_->recurse_sched (this->cur_task_inst_))
+		return true;
 
-//       this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
-     return false;
+	return false;
 }
 
 bool SA_PlanStrategy::get_next_threat_resolution(){
- 
-  //TODO this just takes the first threat resolution and gives up otherwise.  fix soon
 
-        // Choose an open threat to satisfy and remove from open threats.
+	// Choose an open threat to satisfy and remove from open threats.
 
-    if(open_threats_.empty()){
+	if(open_threats_.empty()){
 
-       return satisfy_schedule();
-    }
-        this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
-        CLThreat threat = *this->open_threats_.begin ();
-        CommandID rmv_threat_cmd_id = this->rmv_open_threat (threat);
-
-        //Should have been done in the command
-        //this->open_threats_.erase(threat);
-
-
-
-
-        this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
-   //       this->cur_seq_num_ = 1;
-
-        // Create threat resolution command and add to planner.
-        ResolveCLThreatCmd *resolve_threat_cmd =
-          static_cast<ResolveCLThreatCmd *> (this->resolve_threat_cmd_->clone ());
-        resolve_threat_cmd->set_id (this->get_next_cmd_id ());
-        resolve_threat_cmd->set_threat (threat);
-        this->planner_->add_command (resolve_threat_cmd);
-       
+		return satisfy_schedule();
+	}
+	this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
+	CLThreat threat = *this->open_threats_.begin ();
+	CommandID rmv_threat_cmd_id = this->rmv_open_threat (threat);
 
+	this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
 
+	// Create threat resolution command and add to planner.
+	ResolveCLThreatCmd *resolve_threat_cmd =
+		static_cast<ResolveCLThreatCmd *> (this->resolve_threat_cmd_->clone ());
+	resolve_threat_cmd->set_id (this->get_next_cmd_id ());
+	resolve_threat_cmd->set_threat (threat);
+	this->planner_->add_command (resolve_threat_cmd);
 
-        while(this->planner_->try_next(resolve_threat_cmd->get_id())){
-         
-          
-          if (this->get_next_threat_resolution ())
-            return true;
+	while(this->planner_->try_next(resolve_threat_cmd->get_id())){
 
-        }
+		if (this->get_next_threat_resolution ())
+			return true;
 
-         this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
-         this->planner_->undo_command (resolve_threat_cmd->get_id ());
+	}
 
+	this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
+	this->planner_->undo_command (resolve_threat_cmd->get_id ());
 
+	this->open_threats_.insert(threat);
 
-        this->open_threats_.insert(threat);
+	// Undo removal of open threat.
+	this->planner_->undo_command (rmv_threat_cmd_id);
 
-   //     // Undo removal of open threat.
-        this->planner_->undo_command (rmv_threat_cmd_id);
+	return false;
 
-        return false;
-  
 }
 
 
@@ -400,47 +379,47 @@ bool SA_PlanStrategy::get_next_threat_resolution(){
 // which works on this strategy.
 AddOpenCondsCmd *SA_PlanStrategy::get_AddOpenCondsCmd (void)
 {
-  return static_cast<AddOpenCondsCmd *> (this->add_conds_cmd_->clone ());
+	return static_cast<AddOpenCondsCmd *> (this->add_conds_cmd_->clone ());
 };
 
 // Get a PlanCommand prototype for removing open conditions,
 // which works on this strategy.
 RemoveOpenCondsCmd *SA_PlanStrategy::get_RemoveOpenCondsCmd (void)
 {
-  return static_cast<RemoveOpenCondsCmd *> (this->rmv_conds_cmd_->clone ());
+	return static_cast<RemoveOpenCondsCmd *> (this->rmv_conds_cmd_->clone ());
 };
 
 // Get an command prototype for adding causal link threats, which works on
 // this strategy.
 AddOpenThreatsCmd *SA_PlanStrategy::get_AddOpenThreatsCmd (void)
 {
-  return static_cast<AddOpenThreatsCmd *> (this->add_threats_cmd_->clone ());
+	return static_cast<AddOpenThreatsCmd *> (this->add_threats_cmd_->clone ());
 };
 
 // Get a PlanCommand prototype for removing causal link threats,
 // which works on this strategy.
 RemoveOpenThreatsCmd *SA_PlanStrategy::get_RemoveOpenThreatsCmd (void)
 {
-  return static_cast<RemoveOpenThreatsCmd *> (this->rmv_threats_cmd_->clone ());
+	return static_cast<RemoveOpenThreatsCmd *> (this->rmv_threats_cmd_->clone ());
 };
 
 // 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();
+	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_));
+	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);
+		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));
 	}
-	cmd->link_suspended_on.insert(std::pair<Condition, CausalLink>(*iter, closest_on_path));
-  }
 };
 
 // Undo a command to add open conditions to planning.
@@ -450,30 +429,30 @@ void SA_PlanStrategy::undo (SA_AddOpenCondsCmd *cmd)
 
 	SA_WorkingPlan* work_plan_tmp = (SA_WorkingPlan*)this->planner_->get_working_plan();
 
-  // Remove open conditions mapped to the specified task instance.
+	// 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);
-      open_iter != this->open_conds_.upper_bound (*cond_iter);)
-    {
-      OpenCondMap::iterator prev_iter = open_iter;
-      open_iter++;
-      if (prev_iter->second == cmd->task_inst_)
-	  {
-		  debug_text<<"in planstrat erasing from open_conds_ "<<prev_iter->first.id<<" to "<<prev_iter->second<<std::endl;
-		  this->open_conds_.erase (prev_iter);
-	  }
+		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);
+			open_iter != this->open_conds_.upper_bound (*cond_iter);)
+		{
+			OpenCondMap::iterator prev_iter = open_iter;
+			open_iter++;
+			if (prev_iter->second == cmd->task_inst_)
+			{
+				debug_text<<"in planstrat erasing from open_conds_ "<<prev_iter->first.id<<" to "<<prev_iter->second<<std::endl;
+				this->open_conds_.erase (prev_iter);
+			}
+		}
 	}
-  }
 
 	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
 	debug_text.str("");
@@ -483,102 +462,76 @@ void SA_PlanStrategy::undo (SA_AddOpenCondsCmd *cmd)
 // Execute a command to remove open conditions from planning.
 void SA_PlanStrategy::execute (SA_RemoveOpenCondsCmd *cmd)
 {
-  // Remove open conditions, keeping track of removed cond->inst mapping.
-
-	/*
-	for (CondSet::iterator cond_iter = cmd->conds_.begin ();
-    cond_iter != cmd->conds_.end (); cond_iter++)
-  {
-    for (OpenCondMap::iterator open_iter =
-      this->open_conds_.lower_bound (*cond_iter);
-      open_iter != this->open_conds_.upper_bound (*cond_iter); )
-    {
-      cmd->removed_.insert (std::make_pair
-        (open_iter->first, open_iter->second));
-      OpenCondMap::iterator prev_iter = open_iter;
-      open_iter++;
-      this->open_conds_.erase (prev_iter);
-    }
-  }
-  */
+	// Remove open conditions, keeping track of removed cond->inst mapping.
 
 	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++){
+				task_it != cmd->tasks_.end(); task_it++){
 
-			  for(OpenCondMap::iterator o_it = open_conds_.lower_bound(*cond_it);
-				  o_it != open_conds_.upper_bound(*cond_it); ){
-			
-				  if(o_it->second == *task_it){
+					for(OpenCondMap::iterator o_it = open_conds_.lower_bound(*cond_it);
+						o_it != open_conds_.upper_bound(*cond_it); ){
 
-					OpenCondMap::iterator prev_iter = o_it;
-					o_it++;
+							if(o_it->second == *task_it){
 
+								OpenCondMap::iterator prev_iter = o_it;
+								o_it++;
 
+								cmd->removed_.insert (std::make_pair
+									(*cond_it, *task_it));
 
-					cmd->removed_.insert (std::make_pair
-						(*cond_it, *task_it));
-					
-					open_conds_.erase(prev_iter);
-				  }else{
-					  o_it++;
-				  }
+								open_conds_.erase(prev_iter);
+							}else{
+								o_it++;
+							}
 			  }
-		}
+			}
 	}
-
-
 };
 
 // Undo a command to remove open conditions from planning.
 void SA_PlanStrategy::undo (SA_RemoveOpenCondsCmd *cmd)
 {
 
-std::ostringstream debug_text;
-  // Insert removed open condition to task instance mapping.
-  for (OpenCondMap::iterator open_iter = cmd->removed_.begin ();
-      open_iter != cmd->removed_.end (); open_iter++)
-  {
-	 debug_text<<"in planstrat undo adding "<<open_iter->first.id<<" to "<<open_iter->second<<std::endl;
-	  this->open_conds_.push_front (std::make_pair
-      (open_iter->first, open_iter->second));
-  }
-  SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-debug_text.str("");
+	std::ostringstream debug_text;
+	// Insert removed open condition to task instance mapping.
+	for (OpenCondMap::iterator open_iter = cmd->removed_.begin ();
+		open_iter != cmd->removed_.end (); open_iter++)
+	{
+		debug_text<<"in planstrat undo adding "<<open_iter->first.id<<" to "<<open_iter->second<<std::endl;
+		this->open_conds_.push_front (std::make_pair
+			(open_iter->first, open_iter->second));
+	}
+	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+	debug_text.str("");
 };
 
 // Execute a command to add causal link threats to planning.
 void SA_PlanStrategy::execute (SA_AddOpenThreatsCmd *cmd)
 {
-    for (CLThreatSet::iterator iter = cmd->threats_.begin ();
-    iter != cmd->threats_.end (); iter++)
-  {
-    CLThreat threat = *iter;
-    //if(this->closed_threats_.find(threat) == this->closed_threats_.end())
-   // {
-      this->open_threats_.insert (threat);
-   // }
-  }
+	for (CLThreatSet::iterator iter = cmd->threats_.begin ();
+		iter != cmd->threats_.end (); iter++)
+	{
+		CLThreat threat = *iter;
+		this->open_threats_.insert (threat);
+	}
 };
 
 // Undo a command to add causal link threats to planning.
 void SA_PlanStrategy::undo (SA_AddOpenThreatsCmd * cmd)
 {
 	std::ostringstream debug_text;
-  // Remove open conditions mapped to the specified task instance.
+	// Remove open conditions mapped to the specified task instance.
 	debug_text<<"undoing open threats" <<std::endl;
 	for (CLThreatSet::iterator cond_iter = cmd->threats_.begin ();
-    cond_iter != cmd->threats_.end (); cond_iter++)
-  {
-      CLThreat threat = *cond_iter;
-		  this->open_threats_.erase (threat);
-  }
+		cond_iter != cmd->threats_.end (); cond_iter++)
+	{
+		CLThreat threat = *cond_iter;
+		this->open_threats_.erase (threat);
+	}
 
 	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
 	debug_text.str("");
@@ -588,31 +541,30 @@ void SA_PlanStrategy::undo (SA_AddOpenThreatsCmd * cmd)
 void SA_PlanStrategy::execute (SA_RemoveOpenThreatsCmd * cmd)
 {
 	std::ostringstream debug_text;
-    // Remove open conditions mapped to the specified task instance.
+	// Remove open conditions mapped to the specified task instance.
 	debug_text<<"removing open threats" <<std::endl;
 
 	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
 	debug_text.str("");
 
 	for (CLThreatSet::iterator cond_iter = cmd->threats_.begin ();
-    cond_iter != cmd->threats_.end (); cond_iter++)
-  {
-      CLThreat threat = *cond_iter;
-		  this->open_threats_.erase (threat);
-     // this->closed_threats_.insert (threat);
-  }
+		cond_iter != cmd->threats_.end (); cond_iter++)
+	{
+		CLThreat threat = *cond_iter;
+		this->open_threats_.erase (threat);
+	}
 };
 
 // Undo a command to remove causal link threats from planning.
 void SA_PlanStrategy::undo (SA_RemoveOpenThreatsCmd * cmd)
 {
-   for (CLThreatSet::iterator iter = cmd->threats_.begin ();
-    iter != cmd->threats_.end (); iter++)
-  {
-    CLThreat threat = *iter;
-    //this->closed_threats_.erase (threat);
-    this->open_threats_.insert (threat);
-  }
+	for (CLThreatSet::iterator iter = cmd->threats_.begin ();
+		iter != cmd->threats_.end (); iter++)
+	{
+		CLThreat threat = *iter;
+		//this->closed_threats_.erase (threat);
+		this->open_threats_.insert (threat);
+	}
 };
 
 
@@ -621,95 +573,95 @@ void SA_PlanStrategy::undo (SA_RemoveOpenThreatsCmd * cmd)
 // Satisfy an open condition with an appropriate task.
 AddTaskCmd *SA_PlanStrategy::satisfy_cond (Condition open_cond)
 {
-  // Get add task command.
-  AddTaskCmd *add_task_cmd =
-    static_cast<AddTaskCmd *> (this->add_task_cmd_->clone ());
-  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);
-
-  // Get task instances requiring this open condition, and set in command.
-  TaskInstSet inst_set;
-  inst_set.clear ();
-  for (OpenCondMap::iterator iter = this->open_conds_.lower_bound (open_cond);
-    iter != this->open_conds_.upper_bound (open_cond); iter++)
-  {
-	  if(iter->first.value == open_cond.value){
-		inst_set.insert (iter->second);
-		break;
-	  }
-  }
-  add_task_cmd->set_causal_info (open_cond, inst_set);
-
-  // Add command to planner to be executed next.
-  this->planner_->add_command (add_task_cmd);
-
-  // Return pointer to command.
-  return add_task_cmd;
+	// Get add task command.
+	AddTaskCmd *add_task_cmd =
+		static_cast<AddTaskCmd *> (this->add_task_cmd_->clone ());
+	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);
+
+	// Get task instances requiring this open condition, and set in command.
+	TaskInstSet inst_set;
+	inst_set.clear ();
+	for (OpenCondMap::iterator iter = this->open_conds_.lower_bound (open_cond);
+		iter != this->open_conds_.upper_bound (open_cond); iter++)
+	{
+		if(iter->first.value == open_cond.value){
+			inst_set.insert (iter->second);
+			break;
+		}
+	}
+	add_task_cmd->set_causal_info (open_cond, inst_set);
+
+	// Add command to planner to be executed next.
+	this->planner_->add_command (add_task_cmd);
+
+	// Return pointer to command.
+	return add_task_cmd;
 };
 
 // Add open conditions.
 CommandID SA_PlanStrategy::add_open_conds (const CondSet &open_conds,
-  TaskInstID task_inst)
+										   TaskInstID task_inst)
 {
-  // Get command to add open conditions for task instance.
-  AddOpenCondsCmd *add_conds_cmd =
-    static_cast<AddOpenCondsCmd *> (this->add_conds_cmd_->clone ());
-  add_conds_cmd->set_conds (open_conds);
-  add_conds_cmd->set_task_inst (task_inst);
-  CommandID cmd_id = this->get_next_cmd_id ();
-  add_conds_cmd->set_id (cmd_id);
-
-  // Execute command immediately and return command id.
-  this->planner_->execute_command (add_conds_cmd);
-  return cmd_id;
+	// Get command to add open conditions for task instance.
+	AddOpenCondsCmd *add_conds_cmd =
+		static_cast<AddOpenCondsCmd *> (this->add_conds_cmd_->clone ());
+	add_conds_cmd->set_conds (open_conds);
+	add_conds_cmd->set_task_inst (task_inst);
+	CommandID cmd_id = this->get_next_cmd_id ();
+	add_conds_cmd->set_id (cmd_id);
+
+	// Execute command immediately and return command id.
+	this->planner_->execute_command (add_conds_cmd);
+	return cmd_id;
 };
 
 // Remove open condition.
 CommandID SA_PlanStrategy::rmv_open_cond (Condition open_cond, TaskInstSet tasks)
 {
-  // Get commmand to remove open conditions.
-  RemoveOpenCondsCmd *rmv_conds_cmd =
-    static_cast<RemoveOpenCondsCmd *> (this->rmv_conds_cmd_->clone ());
-  CondSet conds;
-  conds.insert (open_cond);
-  rmv_conds_cmd->set_conds (conds, tasks);
-  CommandID cmd_id = this->get_next_cmd_id ();
-  rmv_conds_cmd->set_id (cmd_id);
-
-  // Execute command immediately and return command id.
-  this->planner_->execute_command (rmv_conds_cmd);
-  return cmd_id;
+	// Get commmand to remove open conditions.
+	RemoveOpenCondsCmd *rmv_conds_cmd =
+		static_cast<RemoveOpenCondsCmd *> (this->rmv_conds_cmd_->clone ());
+	CondSet conds;
+	conds.insert (open_cond);
+	rmv_conds_cmd->set_conds (conds, tasks);
+	CommandID cmd_id = this->get_next_cmd_id ();
+	rmv_conds_cmd->set_id (cmd_id);
+
+	// Execute command immediately and return command id.
+	this->planner_->execute_command (rmv_conds_cmd);
+	return cmd_id;
 };
 
 // Add open causal link threats.
 CommandID SA_PlanStrategy::add_open_threats (const CLThreatSet &threats)
 {
-  // Get commmand to add open threats.
-  AddOpenThreatsCmd *add_threats_cmd =
-    static_cast<AddOpenThreatsCmd *> (this->add_threats_cmd_->clone ());
-  add_threats_cmd->set_threats (threats);
-  CommandID cmd_id = this->get_next_cmd_id ();
-  add_threats_cmd->set_id (cmd_id);
-
-  // Execute command immediately and return command id.
-  this->planner_->execute_command (add_threats_cmd);
-  return cmd_id;
+	// Get commmand to add open threats.
+	AddOpenThreatsCmd *add_threats_cmd =
+		static_cast<AddOpenThreatsCmd *> (this->add_threats_cmd_->clone ());
+	add_threats_cmd->set_threats (threats);
+	CommandID cmd_id = this->get_next_cmd_id ();
+	add_threats_cmd->set_id (cmd_id);
+
+	// Execute command immediately and return command id.
+	this->planner_->execute_command (add_threats_cmd);
+	return cmd_id;
 };
 
 // Remove open causal link threat.
 CommandID SA_PlanStrategy::rmv_open_threat (CLThreat threat)
 {
-  // Get commmand to remove open threats.
-  RemoveOpenThreatsCmd *rmv_threats_cmd =
-    static_cast<RemoveOpenThreatsCmd *> (this->rmv_threats_cmd_->clone ());
-  CLThreatSet threats;
-  threats.insert (threat);
-  rmv_threats_cmd->set_threats (threats);
-  CommandID cmd_id = this->get_next_cmd_id ();
-  rmv_threats_cmd->set_id (cmd_id);
-
-  // Execute command immediately and return command id.
-  this->planner_->execute_command (rmv_threats_cmd);
-  return cmd_id;
+	// Get commmand to remove open threats.
+	RemoveOpenThreatsCmd *rmv_threats_cmd =
+		static_cast<RemoveOpenThreatsCmd *> (this->rmv_threats_cmd_->clone ());
+	CLThreatSet threats;
+	threats.insert (threat);
+	rmv_threats_cmd->set_threats (threats);
+	CommandID cmd_id = this->get_next_cmd_id ();
+	rmv_threats_cmd->set_id (cmd_id);
+
+	// Execute command immediately and return command id.
+	this->planner_->execute_command (rmv_threats_cmd);
+	return cmd_id;
 };
diff --git a/SA_PlanStrategy.h b/SA_PlanStrategy.h
index ad115be58f4..b3933ffe964 100644
--- a/SA_PlanStrategy.h
+++ b/SA_PlanStrategy.h
@@ -183,8 +183,6 @@ namespace SA_POP {
      */
     virtual void undo (SA_RemoveOpenThreatsCmd *cmd);
 
-    //TODO ben's crap
-
     virtual bool satisfy_everything();
     virtual bool satisfy_schedule(void);
     virtual bool get_next_threat_resolution();
diff --git a/SA_SchedStrategy.cpp b/SA_SchedStrategy.cpp
index 6a441e7ae88..d482b76f7a8 100644
--- a/SA_SchedStrategy.cpp
+++ b/SA_SchedStrategy.cpp
@@ -97,7 +97,7 @@ 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.
@@ -157,6 +157,7 @@ bool SA_SchedStrategy::satisfy_sched (TaskInstID task_inst)
 	  return false;
   }
 
+  */
 
   if(!this->planner_->recurse_plan ()){
 	  this->planner_->undo_through(cur_cmd_id);
@@ -831,7 +832,10 @@ bool SA_SchedStrategy::energy_prop (TaskInstID task_inst)
 		this->planner_->undo_through(cur_cmd_id);
 		return false;
     }
+  }else{
+	delete adj_min_times_cmd;
   }
+
   if(max_end!=end_win.second)
   {
     max_times_changed=true;
@@ -842,6 +846,8 @@ bool SA_SchedStrategy::energy_prop (TaskInstID task_inst)
       this->planner_->undo_through(cur_cmd_id);
       return false;
     }
+  }else{
+	delete adj_max_times_cmd;
   }
   if(min_times_changed || max_times_changed) 
   {
@@ -928,7 +934,11 @@ bool SA_SchedStrategy::energy_prop_after (TaskInstID task_inst)
 		this->planner_->undo_through(cur_cmd_id);
 		return false;
     }
+  }else{
+	delete adj_min_times_cmd;
   }
+
+
   if(min_times_changed) 
   {
     //Since the time windows have changed, some unranked task instances have gone to before or after.
@@ -1010,6 +1020,8 @@ bool SA_SchedStrategy::energy_prop_before (TaskInstID task_inst)
       this->planner_->undo_through(cur_cmd_id);
       return false;
     }
+  }else{
+	delete adj_max_times_cmd;
   }
   if(max_times_changed) 
   {
diff --git a/SA_WorkingPlan.cpp b/SA_WorkingPlan.cpp
index f79a1a44ff6..07e3cdcd7bc 100644
--- a/SA_WorkingPlan.cpp
+++ b/SA_WorkingPlan.cpp
@@ -3,13 +3,13 @@
 
 //=============================================================================
 /**
- * @file  SA_WorkingPlan.cpp
- *
- * This file contains the implementation of the SA_WorkingPlan concrete class,
- * which implements a WorkingPlan that uses precedence graphs for scheduling.
- *
- * @author  John S. Kinnebrew <john.s.kinnebrew@vanderbilt.edu>
- */
+* @file  SA_WorkingPlan.cpp
+*
+* This file contains the implementation of the SA_WorkingPlan concrete class,
+* which implements a WorkingPlan that uses precedence graphs for scheduling.
+*
+* @author  John S. Kinnebrew <john.s.kinnebrew@vanderbilt.edu>
+*/
 //=============================================================================
 
 #include "SA_POP_Types.h"
@@ -36,345 +36,268 @@ adj_min_times_cmd_ (0),
 adj_max_times_cmd_ (0),
 add_threats_cmd_ (0)
 {
-  this->task_insts_.clear ();
-  this->task_impls_.clear ();
-  this->causal_links_.clear ();
-  this->reused_insts_.clear();
-  this->sched_links_.clear();
-
-  // Clear goal.
-  this->goal_.goal_id = "NULL";
-  this->goal_.name = "NULL";
-  this->goal_.abs_times.clear ();
-  this->goal_.rel_times.clear ();
-  this->goal_.goal_conds.clear ();
-  this->goal_.start_window = std::make_pair (0, 0);
-
-  // Clear plan.
-  this->plan_.causal_links.clear ();
-  this->plan_.connections.clear ();
-  this->plan_.sched_links.clear ();
-  this->plan_.task_insts.clear ();
-  this->plan_.threat_links.clear ();
-
-  this->add_task_cmd_ = new SA_AddTaskCmd (this);
-  this->assoc_impl_cmd_ = new SA_AssocTaskImplCmd (this);
-  this->resolve_threat_cmd_ = new SA_ResolveCLThreatCmd (this);
-  this->resolve_sched_cmd_ = new SA_ResolveSchedOrderCmd (this);
-  this->adj_min_times_cmd_ = new SA_AdjustMinTimesCmd (this);
-  this->adj_max_times_cmd_ = new SA_AdjustMaxTimesCmd (this);
-
-  PrecedenceSet temp;
-  this->precedence_graph_.insert(std::make_pair(BEFORE,temp));
-  this->precedence_graph_.insert(std::make_pair(AFTER,temp));
-  this->precedence_graph_.insert(std::make_pair(SIMUL,temp));
-  this->precedence_graph_.insert(std::make_pair(UNRANKED,temp));
-
-for(int i = 0; i < 2; i++){
-	this->init_start.insert(std::make_pair(i,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-	this->init_end.insert(std::make_pair(i,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-}
+	this->task_insts_.clear ();
+	this->task_impls_.clear ();
+	this->causal_links_.clear ();
+	this->reused_insts_.clear();
+	this->sched_links_.clear();
+
+	// Clear goal.
+	this->goal_.goal_id = "NULL";
+	this->goal_.name = "NULL";
+	this->goal_.abs_times.clear ();
+	this->goal_.rel_times.clear ();
+	this->goal_.goal_conds.clear ();
+	this->goal_.start_window = std::make_pair (0, 0);
+
+	// Clear plan.
+	this->plan_.causal_links.clear ();
+	this->plan_.connections.clear ();
+	this->plan_.sched_links.clear ();
+	this->plan_.task_insts.clear ();
+	this->plan_.threat_links.clear ();
+
+	this->add_task_cmd_ = new SA_AddTaskCmd (this);
+	this->assoc_impl_cmd_ = new SA_AssocTaskImplCmd (this);
+	this->resolve_threat_cmd_ = new SA_ResolveCLThreatCmd (this);
+	this->resolve_sched_cmd_ = new SA_ResolveSchedOrderCmd (this);
+	this->adj_min_times_cmd_ = new SA_AdjustMinTimesCmd (this);
+	this->adj_max_times_cmd_ = new SA_AdjustMaxTimesCmd (this);
+
+	PrecedenceSet temp;
+	this->precedence_graph_.insert(std::make_pair(BEFORE,temp));
+	this->precedence_graph_.insert(std::make_pair(AFTER,temp));
+	this->precedence_graph_.insert(std::make_pair(SIMUL,temp));
+	this->precedence_graph_.insert(std::make_pair(UNRANKED,temp));
+
+	for(int i = 0; i < 2; i++){
+		this->init_start.insert(std::make_pair(i,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
+		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;
+	this->no_condition.id = -4;
+	this->no_link.cond.id = -4;
+	this->no_link.first = -4;
+	this->no_link.second = -4;
 
 };
 
 // Destructor.
 SA_WorkingPlan::~SA_WorkingPlan (void)
 {
-  // Nothing to do.
+	// Nothing to do.
 };
 
 //Reset Plan.
 
 void SA_WorkingPlan::reset_plan ()
 {
-	
+
 	next_inst_id_ =1;
 	this->task_insts_.clear ();
 	this->task_impls_.clear ();
-  this->causal_links_.clear ();
-  this->reused_insts_.clear();
-  this->sched_links_.clear();
-
-  // Clear goal.
-  this->goal_.goal_id = "NULL";
-  this->goal_.name = "NULL";
-  this->goal_.abs_times.clear ();
-  this->goal_.rel_times.clear ();
-  this->goal_.goal_conds.clear ();
-  this->goal_.start_window = std::make_pair (0, 0);
-
-  // Clear plan.
-  this->plan_.causal_links.clear ();
-  this->plan_.connections.clear ();
-  this->plan_.sched_links.clear ();
-  this->plan_.task_insts.clear ();
-  this->plan_.threat_links.clear ();
-
-  this->precedence_graph_.clear();
-
-  this->init_start.clear();
-  this->init_end.clear();
-
-  PrecedenceSet temp;
-  this->precedence_graph_.insert(std::make_pair(BEFORE,temp));
-  this->precedence_graph_.insert(std::make_pair(AFTER,temp));
-  this->precedence_graph_.insert(std::make_pair(SIMUL,temp));
-  this->precedence_graph_.insert(std::make_pair(UNRANKED,temp));
-
-  /*
-
-this->init_start.insert(std::make_pair(1,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(1,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(2,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(2,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(3,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(3,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(4,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(4,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(5,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(5,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(6,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(6,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(7,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(7,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(8,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(8,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(9,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(9,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(10,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(10,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(11,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(11,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(12,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(12,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(13,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(13,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(14,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(14,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(15,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(15,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(16,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(16,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(17,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(17,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(17,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(17,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(18,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(18,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(19,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(19,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(20,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(20,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(21,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(21,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(22,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(22,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(23,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(23,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(24,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(24,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(25,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(25,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(26,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(26,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(27,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(27,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(28,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(28,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(29,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(29,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(30,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(30,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(31,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(31,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_start.insert(std::make_pair(32,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-this->init_end.insert(std::make_pair(32,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-
-*/
+	this->causal_links_.clear ();
+	this->reused_insts_.clear();
+	this->sched_links_.clear();
+
+	// Clear goal.
+	this->goal_.goal_id = "NULL";
+	this->goal_.name = "NULL";
+	this->goal_.abs_times.clear ();
+	this->goal_.rel_times.clear ();
+	this->goal_.goal_conds.clear ();
+	this->goal_.start_window = std::make_pair (0, 0);
+
+	// Clear plan.
+	this->plan_.causal_links.clear ();
+	this->plan_.connections.clear ();
+	this->plan_.sched_links.clear ();
+	this->plan_.task_insts.clear ();
+	this->plan_.threat_links.clear ();
+
+	this->precedence_graph_.clear();
+
+	this->init_start.clear();
+	this->init_end.clear();
+
+	PrecedenceSet temp;
+	this->precedence_graph_.insert(std::make_pair(BEFORE,temp));
+	this->precedence_graph_.insert(std::make_pair(AFTER,temp));
+	this->precedence_graph_.insert(std::make_pair(SIMUL,temp));
+	this->precedence_graph_.insert(std::make_pair(UNRANKED,temp));
 }
 
 // Set goal.
 void SA_WorkingPlan::set_goal (const SA_POP::Goal &goal)
 {
-  this->goal_ = goal; 
+	this->goal_ = goal; 
 };
 
 // Set command prototypes to use.
 void SA_WorkingPlan::set_commands (AddOpenThreatsCmd *add_threats_cmd)
 {
-  if (add_threats_cmd == 0)
-    return;
+	if (add_threats_cmd == 0)
+		return;
 
-  this->add_threats_cmd_ = add_threats_cmd;
-  this->has_cmds_ = true;
+	this->add_threats_cmd_ = add_threats_cmd;
+	this->has_cmds_ = true;
 };
 
 // Helper function to create next instance id.
 TaskInstID SA_WorkingPlan::get_next_inst_id (void)
 {
-  TaskInstID inst_id = this->next_inst_id_;
-  this->next_inst_id_++;
-  return inst_id;
+	TaskInstID inst_id = this->next_inst_id_;
+	this->next_inst_id_++;
+	return inst_id;
 };
 
 // Get current plan.
 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
-  // Should track whether plan has changed rather than regenerating every time.
-
-  // Clear plan.
-  this->plan_.causal_links.clear ();
-  this->plan_.connections.clear ();
-  this->plan_.sched_links.clear ();
-  this->plan_.task_insts.clear ();
-  this->plan_.threat_links.clear ();
-
-  //****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
-  // (Need absolute windows from goal)
-  // Set time windows.
-  this->plan_.start_window = std::make_pair (0, 0);
-  this->plan_.end_window = std::make_pair (0, 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****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP
-  // (Need goal structure to set plan ID & name)
-  // Set plan ID and name from goal.
-  this->plan_.plan_id = "1";
-  this->plan_.name = this->plan_.plan_id;
-  //****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 (InstToTaskMap::iterator iter =
-    this->task_insts_.begin ();
-    iter != this->task_insts_.end (); iter++)
-  {
-    // Creat task instance and set instance and task IDs from task instance map.
-    PlanTaskInst inst;
-    inst.inst_id = iter->first;
-    inst.task_id = iter->second;
-
-    //****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
-    // (Need to add separate Type to task nodes instead of using name.)
-    // Set instance type.
-    inst.type_id = this->planner_->get_task_name (inst.task_id);
-    // Give instance a name.
-    inst.name = "TaskInst";
-    inst.name += to_string (inst.inst_id);
-    inst.name += "_";
-    inst.name += inst.type_id;
-    //****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
-
-    // Set start and end windows for this instance.
-    SA_WorkingPlan::InstToWinMap::iterator win_iter;
-    win_iter = this->start_window_map_.find (inst.inst_id);
-    if (win_iter != this->start_window_map_.end ())
-      inst.start_window = win_iter->second;
-    else
-      throw "SA_POP::SA_WorkingPlan::get_plan (): Unable to find start window for task instance.";
-    win_iter = this->end_window_map_.find (inst.inst_id);
-    if (win_iter != this->end_window_map_.end ())
-      inst.end_window = win_iter->second;
-    else
-      throw "SA_POP::SA_WorkingPlan::get_plan (): Unable to find end window for task instance.";
-
-    // Set suggested implementation for this instance.
-    InstToImplMap::iterator impl_iter;
-    impl_iter = this->task_impls_.find (inst.inst_id);
-    if (impl_iter != this->task_impls_.end ())
-      inst.suggested_impl = impl_iter->second;
-    else
-      throw "SA_POP::SA_WorkingPlan::get_plan (): Unable to find implementation from task instance.";
-
-    // Add instance to plan.
-    this->plan_.task_insts.insert (inst);
-  }
-
-  // Add scheduling links.
-  for (SA_WorkingPlan::SchedulingLinks::iterator sched_iter =
-    this->sched_links_.begin ();
-    sched_iter != this->sched_links_.end (); sched_iter++)
-  {
-    this->plan_.sched_links.insert (std::make_pair (
-      sched_iter->first, sched_iter->second));
-  }
-
-  //****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
-  // (Need to include threat links.)
-  // Add threat links.
-  //****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 plan data connections and causal links.
-  for (SA_WorkingPlan::CondToCLinksMap::iterator cl_iter = 
-    this->causal_links_.begin ();
-    cl_iter != this->causal_links_.end (); cl_iter++)
-  {
-    // If this is the placeholder goal task instance, don't add connection.
-    if (cl_iter->second.second == SA_POP::GOAL_TASK_INST_ID)
-      continue;
-
-    // If this condition is a data condition, add data connection to plan.
-    if (cl_iter->first.kind == SA_POP::DATA) {
-      PlanConnection plan_conn;
-
-      // Set condition.
-      plan_conn.cond = cl_iter->first.id;
-
-      // Set task instances.
-      plan_conn.first_task_inst = cl_iter->second.first;
-      plan_conn.second_task_inst = cl_iter->second.second;
-
-      // Get task ids from instances.
-      InstToTaskMap::iterator inst_task_iter;
-
-      inst_task_iter = this->task_insts_.find (cl_iter->second.first);
-      if (inst_task_iter == this->task_insts_.end ())
-        throw "SA_POP::SA_WorkingPlan::get_plan (): couldn't find task id for task instance to get port for plan connection.";
-      TaskID first_task = inst_task_iter->second;
-
-      inst_task_iter = this->task_insts_.find (cl_iter->second.second);
-      if (inst_task_iter == this->task_insts_.end ())
-        throw "SA_POP::SA_WorkingPlan::get_plan (): couldn't find task id for task instance to get port for plan connection.";
-      TaskID second_task = inst_task_iter->second;
-
-      // Get ports.
-      LinkPorts ports = this->planner_->get_clink_ports
-        (first_task, cl_iter->first.id, second_task);
-      plan_conn.first_port = ports.first;
-      plan_conn.second_port = ports.second;
-
-      this->plan_.connections.insert (plan_conn);
-    } else {  // This is a non-data condition so add causal link to plan.
-      this->plan_.causal_links.insert (cl_iter->second);
-    }
-  }
-
-  return this->plan_;
+	//****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
+	// Should track whether plan has changed rather than regenerating every time.
+
+	// Clear plan.
+	this->plan_.causal_links.clear ();
+	this->plan_.connections.clear ();
+	this->plan_.sched_links.clear ();
+	this->plan_.task_insts.clear ();
+	this->plan_.threat_links.clear ();
+
+	//****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
+	// (Need absolute windows from goal)
+	// Set time windows.
+	this->plan_.start_window = std::make_pair (0, 0);
+	this->plan_.end_window = std::make_pair (0, 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****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP
+	// (Need goal structure to set plan ID & name)
+	// Set plan ID and name from goal.
+	this->plan_.plan_id = "1";
+	this->plan_.name = this->plan_.plan_id;
+	//****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 (InstToTaskMap::iterator iter =
+		this->task_insts_.begin ();
+		iter != this->task_insts_.end (); iter++)
+	{
+		// Creat task instance and set instance and task IDs from task instance map.
+		PlanTaskInst inst;
+		inst.inst_id = iter->first;
+		inst.task_id = iter->second;
+
+		//****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
+		// (Need to add separate Type to task nodes instead of using name.)
+		// Set instance type.
+		inst.type_id = this->planner_->get_task_name (inst.task_id);
+		// Give instance a name.
+		inst.name = "TaskInst";
+		inst.name += to_string (inst.inst_id);
+		inst.name += "_";
+		inst.name += inst.type_id;
+		//****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
+
+		// Set start and end windows for this instance.
+		SA_WorkingPlan::InstToWinMap::iterator win_iter;
+		win_iter = this->start_window_map_.find (inst.inst_id);
+		if (win_iter != this->start_window_map_.end ())
+			inst.start_window = win_iter->second;
+		else
+			throw "SA_POP::SA_WorkingPlan::get_plan (): Unable to find start window for task instance.";
+		win_iter = this->end_window_map_.find (inst.inst_id);
+		if (win_iter != this->end_window_map_.end ())
+			inst.end_window = win_iter->second;
+		else
+			throw "SA_POP::SA_WorkingPlan::get_plan (): Unable to find end window for task instance.";
+
+		// Set suggested implementation for this instance.
+		InstToImplMap::iterator impl_iter;
+		impl_iter = this->task_impls_.find (inst.inst_id);
+		if (impl_iter != this->task_impls_.end ())
+			inst.suggested_impl = impl_iter->second;
+		else
+			throw "SA_POP::SA_WorkingPlan::get_plan (): Unable to find implementation from task instance.";
+
+		// Add instance to plan.
+		this->plan_.task_insts.insert (inst);
+	}
+
+	// Add scheduling links.
+	for (SA_WorkingPlan::SchedulingLinks::iterator sched_iter =
+		this->sched_links_.begin ();
+		sched_iter != this->sched_links_.end (); sched_iter++)
+	{
+		this->plan_.sched_links.insert (std::make_pair (
+			sched_iter->first, sched_iter->second));
+	}
+
+	// Add plan data connections and causal links.
+	for (SA_WorkingPlan::CondToCLinksMap::iterator cl_iter = 
+		this->causal_links_.begin ();
+		cl_iter != this->causal_links_.end (); cl_iter++)
+	{
+		// If this is the placeholder goal task instance, don't add connection.
+		if (cl_iter->second.second == SA_POP::GOAL_TASK_INST_ID)
+			continue;
+
+		// If this condition is a data condition, add data connection to plan.
+		if (cl_iter->first.kind == SA_POP::DATA) {
+			PlanConnection plan_conn;
+
+			// Set condition.
+			plan_conn.cond = cl_iter->first.id;
+
+			// Set task instances.
+			plan_conn.first_task_inst = cl_iter->second.first;
+			plan_conn.second_task_inst = cl_iter->second.second;
+
+			// Get task ids from instances.
+			InstToTaskMap::iterator inst_task_iter;
+
+			inst_task_iter = this->task_insts_.find (cl_iter->second.first);
+			if (inst_task_iter == this->task_insts_.end ())
+				throw "SA_POP::SA_WorkingPlan::get_plan (): couldn't find task id for task instance to get port for plan connection.";
+			TaskID first_task = inst_task_iter->second;
+
+			inst_task_iter = this->task_insts_.find (cl_iter->second.second);
+			if (inst_task_iter == this->task_insts_.end ())
+				throw "SA_POP::SA_WorkingPlan::get_plan (): couldn't find task id for task instance to get port for plan connection.";
+			TaskID second_task = inst_task_iter->second;
+
+			// Get ports.
+			LinkPorts ports = this->planner_->get_clink_ports
+				(first_task, cl_iter->first.id, second_task);
+			plan_conn.first_port = ports.first;
+			plan_conn.second_port = ports.second;
+
+			this->plan_.connections.insert (plan_conn);
+		} else {  // This is a non-data condition so add causal link to plan.
+			this->plan_.causal_links.insert (cl_iter->second);
+		}
+	}
+
+	return this->plan_;
 };
 
 // Get set of task instances for a precedence relation to a task instance.
 const TaskInstSet *SA_WorkingPlan::get_prec_set (TaskInstID task_inst, PrecedenceRelation prec_rel)
 {
-  PrecedenceGraph::iterator piter = this->precedence_graph_.find(prec_rel);
-  PrecedenceSet::iterator titer = piter->second.find(task_inst);
-  return &(titer->second);
+	PrecedenceGraph::iterator piter = this->precedence_graph_.find(prec_rel);
+	PrecedenceSet::iterator titer = piter->second.find(task_inst);
+	return &(titer->second);
 };
 
 // Get task id of a task instance.
 TaskID SA_WorkingPlan::get_task_from_inst (TaskInstID inst_id)
 {
-  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.";
+	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.";
 
-  return iter->second;
+	return iter->second;
 };
 
 /// Get task implementation id of a task instance.
@@ -385,201 +308,185 @@ TaskImplID SA_WorkingPlan::get_task_impl_from_inst(TaskInstID inst_id)
 }
 
 bool SA_WorkingPlan::inst_has_impl(TaskInstID inst_id){
-	
+
 	return !(task_impls_.find(inst_id) == task_impls_.end());
 }
 
+//Go through and find all conflicting tasks
 void SA_WorkingPlan::generate_all_threats(void)
 {
-  threat_set.clear();
-
-  std::ostringstream debug_text;
-  debug_text << "SA_WorkingPlan::generate_all_threats:  All Causal Links: " << std::endl;
-  
-  
-  
-  for(CondToCLinksMap::iterator nit = causal_links_.begin(); nit != causal_links_.end(); nit++){
-
-            CausalLink causal_threatened = (*nit).second;
-            TaskID threatened_task = this->task_insts_.find(causal_threatened.first)->second;
-            if(causal_threatened.second != -1)
-              debug_text << "  Task ("<<threatened_task<<") " << "Inst ("<<causal_threatened.first<<") -(" << causal_threatened.cond.id << ")-> Task ("<<this->task_insts_.find(causal_threatened.second)->second<< ") Inst ("<< causal_threatened.second << ")" << std::endl;           
-
-  }
-  SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-  debug_text.str("");
-
-
-  debug_text << "SA_WorkingPlan::generate_all_threats:  All Tasks Instances: " << std::endl;
-    
-  for(InstToTaskMap::iterator iterator = this->task_insts_.begin(); iterator != this->task_insts_.end(); iterator++){
-    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;
-  SA_POP_DEBUG_STR (SPARTAN, debug_text.str ());
-  debug_text.str("");
-
-  debug_text << "SA_WorkingPlan::generate_all_threats:  All Causal Threats: " << std::endl;
-
-  //Iterate through task instances
-  for(InstToTaskMap::iterator iterator = this->task_insts_.begin(); iterator != this->task_insts_.end(); iterator++){
-    TaskInstID threat_possibility = iterator->first;
-    TaskID threat_possibility_taskid = iterator->second;
-    CondSet set = this->planner_->get_effects(threat_possibility_taskid);
-  
-    //Iterate through the effects of each task instance
-    for(CondSet::iterator arr = set.begin(); arr != set.end(); arr++){ 
-
-        Condition condition = (*arr);
-
-        std::pair<
-          CondToCLinksMap::iterator,
-          CondToCLinksMap::iterator
-            > ret = this->causal_links_.equal_range(condition);
-      
-        //Iterates through causal links
-        for(CondToCLinksMap::iterator nit = ret.first; nit != ret.second; nit++){
-
-            CausalLink causal_threatened = (*nit).second;
-            TaskID threatened_task = this->task_insts_.find(causal_threatened.first)->second;
-
-            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)){
-
-              if(causal_threatened.first != threat_possibility && causal_threatened.second != threat_possibility)
-              {
-				bool a = false;
-				bool b = false; 
-				PrecedenceSet::iterator after_thr_it = (&this->precedence_graph_.find(AFTER)->second)->find(threat_possibility);
-				PrecedenceSet::iterator before_thr_it= (&this->precedence_graph_.find(BEFORE)->second)->find(threat_possibility);
-
-	//			if(
-	//			   after_thr_it != (&this->precedence_graph_.find(AFTER)->second)->end() &&
-	//			   before_thr_it != (&this->precedence_graph_.find(BEFORE)->second)->end()){
-
-					TaskInstSet after_threat = after_thr_it->second;
-					TaskInstSet before_threat = before_thr_it->second;
-
-					a = after_threat.find(causal_threatened.first) != after_threat.end();
-					b = before_threat.find(causal_threatened.second) != before_threat.end();
-					
-		//			if(a && b)
-		//					bool wat = true;
-
-					
-				//		continue;
-
-		//			if( (a && !b)||(!a && b)){
-		//				continue;
-		//			}
-		//		}
-
-				if(!(a || b))
-				{
-					CLThreat new_threat;
-					new_threat.clink = causal_threatened;
-					new_threat.threat = threat_possibility;
+	threat_set.clear();
 
-	               
+	std::ostringstream debug_text;
+	debug_text << "SA_WorkingPlan::generate_all_threats:  All Causal Links: " << std::endl;
 
-					std::pair<CLThreatSet::iterator, bool> asdf = threat_set.insert(new_threat);
 
-	              
 
-					TaskID threatened_task1 = this->task_insts_.find(causal_threatened.first)->second;
-					if(causal_threatened.second != SA_POP::GOAL_TASK_INST_ID)
-					{
-					 TaskID threatened_task2 = this->task_insts_.find(causal_threatened.second)->second;
-					 debug_text <<"  Causal link from Task ("<< threatened_task1 <<") Inst ("<< causal_threatened.first <<") to Task ("<< threatened_task2 <<") Inst ("<< causal_threatened.second <<") using condition "<<causal_threatened.cond.id<<" threatened by Task ("<< threat_possibility_taskid <<") Inst ("<<threat_possibility<<")" << std::endl;
+	for(CondToCLinksMap::iterator nit = causal_links_.begin(); nit != causal_links_.end(); nit++){
 
-					}
-					else
+		CausalLink causal_threatened = (*nit).second;
+		TaskID threatened_task = this->task_insts_.find(causal_threatened.first)->second;
+		if(causal_threatened.second != -1)
+			debug_text << "  Task ("<<threatened_task<<") " << "Inst ("<<causal_threatened.first<<") -(" << causal_threatened.cond.id << ")-> Task ("<<this->task_insts_.find(causal_threatened.second)->second<< ") Inst ("<< causal_threatened.second << ")" << std::endl;           
+
+	}
+	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+	debug_text.str("");
+
+
+	debug_text << "SA_WorkingPlan::generate_all_threats:  All Tasks Instances: " << std::endl;
+
+	for(InstToTaskMap::iterator iterator = this->task_insts_.begin(); iterator != this->task_insts_.end(); iterator++){
+		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;
+	SA_POP_DEBUG_STR (SPARTAN, debug_text.str ());
+	debug_text.str("");
+
+	debug_text << "SA_WorkingPlan::generate_all_threats:  All Causal Threats: " << std::endl;
+
+	//Iterate through task instances
+	for(InstToTaskMap::iterator iterator = this->task_insts_.begin(); iterator != this->task_insts_.end(); iterator++){
+		TaskInstID threat_possibility = iterator->first;
+		TaskID threat_possibility_taskid = iterator->second;
+		CondSet set = this->planner_->get_effects(threat_possibility_taskid);
+
+		//Iterate through the effects of each task instance
+		for(CondSet::iterator arr = set.begin(); arr != set.end(); arr++){ 
+
+			Condition condition = (*arr);
+
+			std::pair<
+				CondToCLinksMap::iterator,
+				CondToCLinksMap::iterator
+			> ret = this->causal_links_.equal_range(condition);
+
+			//Iterates through causal links
+			for(CondToCLinksMap::iterator nit = ret.first; nit != ret.second; nit++){
+
+				CausalLink causal_threatened = (*nit).second;
+				TaskID threatened_task = this->task_insts_.find(causal_threatened.first)->second;
+
+				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)){
+
+					if(causal_threatened.first != threat_possibility && causal_threatened.second != threat_possibility)
 					{
-					  debug_text <<"  Causal link from Task ("<< threatened_task1 <<") Inst ("<< causal_threatened.first <<") to Task ( GOAL_TASK )Inst (GOAL_TASK_INST) using condition "<<causal_threatened.cond.id<<" threatened by Task ("<< threat_possibility_taskid <<") Inst ("<<threat_possibility<<")" << std::endl;
-					}
+						bool a = false;
+						bool b = false; 
+						PrecedenceSet::iterator after_thr_it = (&this->precedence_graph_.find(AFTER)->second)->find(threat_possibility);
+						PrecedenceSet::iterator before_thr_it= (&this->precedence_graph_.find(BEFORE)->second)->find(threat_possibility);
+
+						TaskInstSet after_threat = after_thr_it->second;
+						TaskInstSet before_threat = before_thr_it->second;
+
+						a = after_threat.find(causal_threatened.first) != after_threat.end();
+						b = before_threat.find(causal_threatened.second) != before_threat.end();
 
+						if(!(a || b))
+						{
+							CLThreat new_threat;
+							new_threat.clink = causal_threatened;
+							new_threat.threat = threat_possibility;
+
+
+
+							std::pair<CLThreatSet::iterator, bool> asdf = threat_set.insert(new_threat);
+
+
+
+							TaskID threatened_task1 = this->task_insts_.find(causal_threatened.first)->second;
+							if(causal_threatened.second != SA_POP::GOAL_TASK_INST_ID)
+							{
+								TaskID threatened_task2 = this->task_insts_.find(causal_threatened.second)->second;
+								debug_text <<"  Causal link from Task ("<< threatened_task1 <<") Inst ("<< causal_threatened.first <<") to Task ("<< threatened_task2 <<") Inst ("<< causal_threatened.second <<") using condition "<<causal_threatened.cond.id<<" threatened by Task ("<< threat_possibility_taskid <<") Inst ("<<threat_possibility<<")" << std::endl;
+
+							}
+							else
+							{
+								debug_text <<"  Causal link from Task ("<< threatened_task1 <<") Inst ("<< causal_threatened.first <<") to Task ( GOAL_TASK )Inst (GOAL_TASK_INST) using condition "<<causal_threatened.cond.id<<" threatened by Task ("<< threat_possibility_taskid <<") Inst ("<<threat_possibility<<")" << std::endl;
+							}
+
+						}
+					}
 				}
-             
-       //       bool a = true;
-              }
-             }
-        }
-      }
-   }
-
-  
-  SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-  debug_text.str("");
-
-  print_precedence_graph("SA_WorkingPlan::generate_all_threats");
-  
+			}
+		}
+	}
+
+
+	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+	debug_text.str("");
+
+	print_precedence_graph("SA_WorkingPlan::generate_all_threats");
+
 }
 
 void SA_WorkingPlan::print_precedence_graph(std::string source){
 
-  std::ostringstream debug_text;
-
-    PrecedenceSet* new_befores = &this->precedence_graph_.find(BEFORE)->second;
-    PrecedenceSet* new_afters = &this->precedence_graph_.find(AFTER)->second;
-    PrecedenceSet* new_simuls = &this->precedence_graph_.find(SIMUL)->second;
-    PrecedenceSet* new_unrankeds = &this->precedence_graph_.find(UNRANKED)->second;
-
-    debug_text << source<<": Precendence Sets: BEFORE: " << std::endl;
-    for(PrecedenceSet::iterator it = new_befores->begin(); it != new_befores->end(); it++){
-      debug_text<<"  TaskInst ID "<<it->first<<" : ";
-      for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
-        debug_text<<" "<<*jt;
-      }
-      debug_text<<std::endl;
-    }
-    SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+	std::ostringstream debug_text;
+
+	PrecedenceSet* new_befores = &this->precedence_graph_.find(BEFORE)->second;
+	PrecedenceSet* new_afters = &this->precedence_graph_.find(AFTER)->second;
+	PrecedenceSet* new_simuls = &this->precedence_graph_.find(SIMUL)->second;
+	PrecedenceSet* new_unrankeds = &this->precedence_graph_.find(UNRANKED)->second;
+
+	debug_text << source<<": Precendence Sets: BEFORE: " << std::endl;
+	for(PrecedenceSet::iterator it = new_befores->begin(); it != new_befores->end(); it++){
+		debug_text<<"  TaskInst ID "<<it->first<<" : ";
+		for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
+			debug_text<<" "<<*jt;
+		}
+		debug_text<<std::endl;
+	}
+	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
 	debug_text.str("");
 
-    debug_text << source<<": Precendence Sets: AFTER: " << std::endl;
-    for(PrecedenceSet::iterator it = new_afters->begin(); it != new_afters->end(); it++){
-      debug_text<<"  TaskInst ID "<<it->first<<" : ";
-      for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
-        debug_text<<" "<<*jt;
-      }
-            debug_text<<std::endl;
-    }
-    
-    SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+	debug_text << source<<": Precendence Sets: AFTER: " << std::endl;
+	for(PrecedenceSet::iterator it = new_afters->begin(); it != new_afters->end(); it++){
+		debug_text<<"  TaskInst ID "<<it->first<<" : ";
+		for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
+			debug_text<<" "<<*jt;
+		}
+		debug_text<<std::endl;
+	}
+
+	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
 	debug_text.str("");
 
 
-    debug_text << source<<": Precendence Sets: SIMUL: " << std::endl;
-    for(PrecedenceSet::iterator it = new_simuls->begin(); it != new_simuls->end(); it++){
-      debug_text<<"  TaskInst ID "<<it->first<<" : ";
-      for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
-        debug_text<<" "<<*jt;
-      }
-            debug_text<<std::endl;
-    }
-
-    SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-    debug_text.str("");
-
-    debug_text << source<<": Precendence Sets: UNRANKED: " << std::endl;
-    for(PrecedenceSet::iterator it = new_unrankeds->begin(); it != new_unrankeds->end(); it++){
-      debug_text<<"  TaskInst ID "<<it->first<<" : ";
-      for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
-        debug_text<<" "<<*jt;
-      }
-            debug_text<<std::endl;
-    }
-    
-    SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-    debug_text.str("");
+	debug_text << source<<": Precendence Sets: SIMUL: " << std::endl;
+	for(PrecedenceSet::iterator it = new_simuls->begin(); it != new_simuls->end(); it++){
+		debug_text<<"  TaskInst ID "<<it->first<<" : ";
+		for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
+			debug_text<<" "<<*jt;
+		}
+		debug_text<<std::endl;
+	}
+
+	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+	debug_text.str("");
+
+	debug_text << source<<": Precendence Sets: UNRANKED: " << std::endl;
+	for(PrecedenceSet::iterator it = new_unrankeds->begin(); it != new_unrankeds->end(); it++){
+		debug_text<<"  TaskInst ID "<<it->first<<" : ";
+		for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
+			debug_text<<" "<<*jt;
+		}
+		debug_text<<std::endl;
+	}
+
+	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+	debug_text.str("");
 
 }
 
@@ -587,68 +494,68 @@ void SA_WorkingPlan::print_precedence_graph(std::string source){
 // Get all current causal link threats.
 CLThreatSet SA_WorkingPlan::get_all_threats (void)
 { 
-  return threat_set;
+	return threat_set;
 };
 
 //Get all the causal and data links TO the task instance
 CLSet SA_WorkingPlan::get_before(TaskInstID task_inst)
 {
-  CLSet ret;
-  for(CondToCLinksMap::iterator iter = this->causal_links_.begin();iter!=this->causal_links_.end();iter++)
-    if(iter->second.second == task_inst) ret.insert(iter->second);
-  return ret;
+	CLSet ret;
+	for(CondToCLinksMap::iterator iter = this->causal_links_.begin();iter!=this->causal_links_.end();iter++)
+		if(iter->second.second == task_inst) ret.insert(iter->second);
+	return ret;
 }
 //Get all the causal and data links FROM the task instance
 CLSet SA_WorkingPlan::get_after(TaskInstID task_inst)
 {
-  CLSet ret;
-  for(CondToCLinksMap::iterator iter = this->causal_links_.begin();iter!=this->causal_links_.end();iter++)
-	  if(iter->second.second != GOAL_TASK_INST_ID && iter->second.second != NULL_TASK_INST_ID && iter->second.first == task_inst) ret.insert(iter->second);
-  return ret;
+	CLSet ret;
+	for(CondToCLinksMap::iterator iter = this->causal_links_.begin();iter!=this->causal_links_.end();iter++)
+		if(iter->second.second != GOAL_TASK_INST_ID && iter->second.second != NULL_TASK_INST_ID && iter->second.first == task_inst) ret.insert(iter->second);
+	return ret;
 }
 
 /// Get the Causal and Scheduling orderings to this task instance
 TaskInstSet SA_WorkingPlan::before_orderings (TaskInstID inst_id)
 {
-  CLSet before_links = this->get_before(inst_id);
-  TaskInstSet ret;
-  for(CLSet::iterator iter=before_links.begin();iter!=before_links.end();iter++)
-  {
-    if(iter->cond.kind!=DATA) ret.insert(iter->first);
-  }
-  for(SchedulingLinks::iterator iter=sched_links_.begin();iter!=sched_links_.end();iter++)
-    if(iter->second==inst_id) ret.insert(iter->first);
-  return ret;
+	CLSet before_links = this->get_before(inst_id);
+	TaskInstSet ret;
+	for(CLSet::iterator iter=before_links.begin();iter!=before_links.end();iter++)
+	{
+		if(iter->cond.kind!=DATA) ret.insert(iter->first);
+	}
+	for(SchedulingLinks::iterator iter=sched_links_.begin();iter!=sched_links_.end();iter++)
+		if(iter->second==inst_id) ret.insert(iter->first);
+	return ret;
 }
 /// Get the Causal and Scheduling orderings from this task instance
 TaskInstSet SA_WorkingPlan::after_orderings (TaskInstID inst_id)
 {
-  CLSet after_links = this->get_after(inst_id);
-  TaskInstSet ret;
-  for(CLSet::iterator iter=after_links.begin();iter!=after_links.end();iter++)
-    if(iter->cond.kind!=DATA) ret.insert(iter->second);
-  for(SchedulingLinks::iterator iter=sched_links_.begin();iter!=sched_links_.end();iter++)
-    if(iter->first==inst_id) ret.insert(iter->second);
-  return ret;
+	CLSet after_links = this->get_after(inst_id);
+	TaskInstSet ret;
+	for(CLSet::iterator iter=after_links.begin();iter!=after_links.end();iter++)
+		if(iter->cond.kind!=DATA) ret.insert(iter->second);
+	for(SchedulingLinks::iterator iter=sched_links_.begin();iter!=sched_links_.end();iter++)
+		if(iter->first==inst_id) ret.insert(iter->second);
+	return ret;
 }
 /// Get a PlanCommand prototype for adding a task to the plan.
 AddTaskCmd *SA_WorkingPlan::get_AddTaskCmd (void)
 {
-  return static_cast<AddTaskCmd *> (this->add_task_cmd_->clone ());
+	return static_cast<AddTaskCmd *> (this->add_task_cmd_->clone ());
 };
 
 // Get a PlanCommand prototype for associating an implementation with a
 // task instance in the plan.
 AssocTaskImplCmd *SA_WorkingPlan::get_AssocTaskImplCmd (void)
 {
-  return static_cast<AssocTaskImplCmd *> (this->assoc_impl_cmd_->clone ());
+	return static_cast<AssocTaskImplCmd *> (this->assoc_impl_cmd_->clone ());
 };
 
 // Get a PlanCommand prototype for resolving a causal link threat in the
 // plan (with promotion or demotion).
 ResolveCLThreatCmd *SA_WorkingPlan::get_ResolveCLThreatCmd (void)
 {
-  return static_cast<ResolveCLThreatCmd *> (this->resolve_threat_cmd_->clone ());
+	return static_cast<ResolveCLThreatCmd *> (this->resolve_threat_cmd_->clone ());
 };
 
 // Get a PlanCommand prototype for resolving a scheduling conflict (i.e.
@@ -656,119 +563,109 @@ ResolveCLThreatCmd *SA_WorkingPlan::get_ResolveCLThreatCmd (void)
 // between two task instances in the plan.
 ResolveSchedOrderCmd *SA_WorkingPlan::get_ResolveSchedOrderCmd (void)
 {
-  return static_cast<ResolveSchedOrderCmd *> (this->resolve_sched_cmd_->clone ());
+	return static_cast<ResolveSchedOrderCmd *> (this->resolve_sched_cmd_->clone ());
 };
 
 // Get a PlanCommand prototype for adjusting the minimum times of a
 // task instance in the plan.
 AdjustMinTimesCmd *SA_WorkingPlan::get_AdjustMinTimesCmd (void)
 {
-  return static_cast<AdjustMinTimesCmd *> (this->adj_min_times_cmd_->clone ());
+	return static_cast<AdjustMinTimesCmd *> (this->adj_min_times_cmd_->clone ());
 };
 
 // Get a PlanCommand prototype for adjusting the maximum times of a
 // task instance in the plan.
 AdjustMaxTimesCmd *SA_WorkingPlan::get_AdjustMaxTimesCmd (void)
 {
-  return static_cast<AdjustMaxTimesCmd *> (this->adj_max_times_cmd_->clone ());
+	return static_cast<AdjustMaxTimesCmd *> (this->adj_max_times_cmd_->clone ());
 };
 
 void SA_WorkingPlan::execute (SA_AddTaskCmd *cmd)
 {
 
-//	if(cmd->id_.step == 13 && cmd->id_.decision_pt == 1 && cmd->id_.seq_num == 1 && cmd->task_choice == 20)
+	TaskChoice task_choice = cmd->tasks_.front();
+	TaskID task = task_choice.task_id;
+	TaskInstID task_inst;
+	Condition cond = cmd->cond_;
 
-  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;
 
-  cmd->last_task_choice_ = task_choice;
 
 
+	if(task_choice.choice == REUSE_INST){
 
-  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){
 
-	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)
-    {
-      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;
+		}
 
-	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 ();
 	}
-  
-    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 ();
-  }
 
 
 
-	  std::ostringstream debug_text;
-	  debug_text << "SA_WorkingPlan::execute (SA_AddTaskCmd *cmd): Adding task (" << task << ") instance (" << task_inst << ") for condition (" << cond.id << ").";
+	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 ());
+	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 ();
+	// 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++)
+	{
+		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
-			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));
-				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 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::cout<<"after unsuspend"<<std::endl;
-				 
-		//		this->suspended_listener_map.insert (std::make_pair (clink, true));
+			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));
 
-				cmd->added_links_.insert(clink);
-			}
+			cmd->added_links_.insert(clink);
 		}
+	}
 
-  // Set this task and task instance as last used by command.
+	// Set this task and task instance as last used by command.
 	cmd->last_task_ = task;
 	cmd->last_task_inst_ = task_inst;
 };
 
+//Check to see if a condition is suspended
 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());
 }
 
-
+//Helps with checking if a condition needs to be suspended
 CausalLink SA_WorkingPlan::clink_on_path(Condition condition, TaskInstID required_by){
 	std::set<TaskInstID> expanded;
 	expanded.clear();
@@ -777,44 +674,46 @@ CausalLink SA_WorkingPlan::clink_on_path(Condition condition, TaskInstID require
 }
 
 CausalLink SA_WorkingPlan::clink_on_path_aux(Condition condition, TaskInstID required_by, 
-										std::set<TaskInstID> & expanded){
+											 std::set<TaskInstID> & expanded){
 
-	expanded.insert(required_by);
+												 expanded.insert(required_by);
 
-	if(required_by == -1){
-		return this->no_link;
-	}
+												 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++){
+												 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(expanded.find(it->second.second) != expanded.end()){
+															 continue;
+														 }
 
-			if(it->second.cond == condition){
-				return it->second;
-			}
+														 if(it->second.cond == condition){
+															 return it->second;
+														 }
 
-			CausalLink to_return = clink_on_path_aux(condition, it->second.second, expanded);
+														 CausalLink to_return = clink_on_path_aux(condition, it->second.second, expanded);
 
-			if(to_return != no_link){
-				return to_return;
-			}
-	}
+														 if(to_return != no_link){
+															 return to_return;
+														 }
+												 }
 
-	return no_link;
+												 return no_link;
 }
 
+//When a task is added, unsuspend tasks on that path
 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);
+
+			unsuspend_listeners_aux(it->second, exception);
 	}
 }
 
+//Auxillery for unsuspending
 void SA_WorkingPlan::unsuspend_listeners_aux(CausalLink link, TaskInstID exception){
 
 	for(SuspendedConditionListenerMap::iterator it = this->suspended_listener_map.lower_bound(link);
@@ -828,27 +727,29 @@ void SA_WorkingPlan::unsuspend_listeners_aux(CausalLink link, TaskInstID excepti
 			}
 	}
 
-	
+
 
 	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);
+			unsuspend_listeners_aux(it->second, exception);
 	}
 }
 
+//Suspend a condition
 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);
 }
 
+//Note that a condition should be removed from the suspend map
 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);
@@ -865,7 +766,7 @@ void SA_WorkingPlan::resume_condition(Condition cond, TaskInstID required_by, Ca
 
 void SA_WorkingPlan::undo (SA_AddTaskCmd *cmd)
 {
-	
+
 
 	if(cmd->last_task_choice_.choice == NEW_INST){
 		if(cmd->last_task_ == 20){
@@ -873,79 +774,78 @@ void SA_WorkingPlan::undo (SA_AddTaskCmd *cmd)
 		}
 
 		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_));
-	 }
-
-
-  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;
-    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_.erase (prev_iter);
-		  
-   //       cmd->added_links_.erase(iter);
-          break;
-        }
-      }
-    }
-  }
-
-  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());
-
-  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;
-        }
-      }
-    }
-  }
+	}else{
+		this->reused_insts_.erase(this->reused_insts_.find(cmd->last_task_inst_));
+	}
+
+
+	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;
+		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_.erase (prev_iter);
+
+					break;
+				}
+			}
+		}
+	}
+
+	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);
+
+					break;
+				}
+			}
+		}
+	}
+
+	TaskInstID to_find = *(cmd->get_satisfied_tasks().begin());
+
+	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;
+				}
+			}
+		}
+	}
 };
 
 bool SA_WorkingPlan::is_null_link(CausalLink link){
@@ -966,141 +866,77 @@ bool SA_WorkingPlan::execute (SA_AssocTaskImplCmd *cmd)
 {
 
 
-  // Associate task instance with first implementation in list from command.
-  if(this->reused_insts_.find(cmd->task_inst_)==this->reused_insts_.end())
-  {
-    this->task_impls_.insert (std::make_pair(cmd->task_inst_, cmd->impls_.front ()));
-    this->durations.insert(std::make_pair(cmd->task_inst_, this->planner_->get_impl(cmd->impls_.front())->get_duration()));
-  
-
-  }
-  // Update last implementation to this one and remove it from command.
-  cmd->last_impl_ = cmd->impls_.front ();
-  cmd->impls_.pop_front ();
-	// Update the precedence graph
+	// Associate task instance with first implementation in list from command.
+	if(this->reused_insts_.find(cmd->task_inst_)==this->reused_insts_.end())
+	{
+		this->task_impls_.insert (std::make_pair(cmd->task_inst_, cmd->impls_.front ()));
+		this->durations.insert(std::make_pair(cmd->task_inst_, this->planner_->get_impl(cmd->impls_.front())->get_duration()));
 
-  //Ben changed this.  undo by returning that stuff
 
-  PrecedenceSet* befores = &this->precedence_graph_.find(BEFORE)->second;
+	}
+	// Update last implementation to this one and remove it from command.
+	cmd->last_impl_ = cmd->impls_.front ();
+	cmd->impls_.pop_front ();
+	// Update the precedence graph
 
-  PrecedenceSet::iterator find_inst = befores->find(cmd->task_inst_);
-  if(find_inst != befores->end()){
-	  TaskInstSet before_task = find_inst->second;
+	PrecedenceSet* befores = &this->precedence_graph_.find(BEFORE)->second;
 
-	  for(TaskInstSet::iterator it = cmd->satisfied_insts.begin(); it != cmd->satisfied_insts.end(); it++){
-		  if(before_task.find(*it) != before_task.end()){
-			  return false;
-		  }
-	  }
-  }
+	PrecedenceSet::iterator find_inst = befores->find(cmd->task_inst_);
+	if(find_inst != befores->end()){
+		TaskInstSet before_task = find_inst->second;
 
-  cmd->got_to_scheduling = true;
+		for(TaskInstSet::iterator it = cmd->satisfied_insts.begin(); it != cmd->satisfied_insts.end(); it++){
+			if(before_task.find(*it) != before_task.end()){
+				return false;
+			}
+		}
+	}
 
+	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->saved_listener_map = suspended_listener_map;
+	cmd->saved_suspended_set = suspended_conditions;
 
-  bool toReturn	= this->init_prec_insert(cmd->task_inst_,cmd);
+	for(CLSet::iterator it = cmd->added_links.begin(); it != cmd->added_links.end(); it++){
+		unsuspend_listeners(*it, it->first);
+	}
 
-//  cmd->befores_after_ex = *befores;
-//  cmd->afters_after_ex = *afters;
-//  cmd->simuls_after_ex = *simuls;
-//  cmd->unrankeds_after_ex = *unrankeds;
+	bool toReturn	= this->init_prec_insert(cmd->task_inst_,cmd);
 
-  return toReturn;
+	return toReturn;
 
 };
 
-/*
-bool SA_WorkingPlan::is_cycle_in_ordering(){
-
-  std::ostringstream debug_text;
-  
-
-
-  std::map<TaskInstID, bool> un_visited_map;
-  std::map<TaskInstID, bool> visited_map;
-  std::stack<TaskInstID> s;
-
-  for(InstToTaskMap::iterator it = this->task_insts_.begin(); it != this->task_insts_.end(); it++){
-    un_visited_map.insert(std::pair<TaskInstID, bool>(it->first, true));
-
-  }
-
-  int total_num = un_visited_map.size();
-  while(!(s.size()== total_num)){
-
-    std::map<TaskInstID, bool>::iterator some_it = un_visited_map.begin();
-    TaskInstID next = some_it->first;
-    dfs_aux(next, s, visited_map, un_visited_map);
-  }
-
-  visited_map.clear();
-
-
-  while(!s.empty()){
-    TaskInstID next = s.top();
-    s.pop();
-
-    if(!dfs_aux2(next, visited_map)){  
-
-      
-      debug_text << "SA_WorkingPlan::is_cycle_in_ordering:  FOUND A LOOP IN THREAT SCHEDULE" << std::endl;
-      SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-      return true;
-    }
-  }
-
-  return false;
-}
-*/
-
 // Undo a command to associate an implementation with a
 // task instance in the plan.
 void SA_WorkingPlan::undo (SA_AssocTaskImplCmd *cmd)
 {
 	// Undo the time window adjustments and the precedence graph updations.
+	PrecedenceSet* befores = &this->precedence_graph_.find(BEFORE)->second;
+	PrecedenceSet* afters = &this->precedence_graph_.find(AFTER)->second;
+	PrecedenceSet* simuls = &this->precedence_graph_.find(SIMUL)->second;
+	PrecedenceSet* unrankeds = &this->precedence_graph_.find(UNRANKED)->second;
+
+
+	if(cmd->got_to_scheduling){
+		suspended_listener_map = cmd->saved_listener_map;
+		suspended_conditions = cmd->saved_suspended_set;
 
-  PrecedenceSet* befores = &this->precedence_graph_.find(BEFORE)->second;
-  PrecedenceSet* afters = &this->precedence_graph_.find(AFTER)->second;
-  PrecedenceSet* simuls = &this->precedence_graph_.find(SIMUL)->second;
-  PrecedenceSet* unrankeds = &this->precedence_graph_.find(UNRANKED)->second;
-	
-	
-  if(cmd->got_to_scheduling){
-	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);
-	  this->prec_erase(cmd->task_inst_,cmd);
-    cmd->causal_insertions.clear();
-    cmd->simul_insertions.clear();
-
-
-  }
-
-  if(this->reused_insts_.find(cmd->task_inst_)==this->reused_insts_.end())
-  {
-    this->durations.erase(this->durations.find(cmd->task_inst_));
-    this->task_impls_.erase (cmd->task_inst_);
-  }
-
-
-	/*
-  if(cmd->befores != *befores ||
-	  cmd->afters != *afters ||
-	  cmd->simuls != *simuls ||
-	  cmd->unrankeds != *unrankeds)
-	 {
-	bool hi = true;
-  }
-  */
+		this->undo(&cmd->max_adj_cmd);
+		this->undo(&cmd->min_adj_cmd);
+		this->prec_erase(cmd->task_inst_,cmd);
+		cmd->causal_insertions.clear();
+		cmd->simul_insertions.clear();
 
+
+	}
+
+	if(this->reused_insts_.find(cmd->task_inst_)==this->reused_insts_.end())
+	{
+		this->durations.erase(this->durations.find(cmd->task_inst_));
+		this->task_impls_.erase (cmd->task_inst_);
+	}
 };
 
 PrecedenceGraph SA_WorkingPlan::get_precedence_graph(){
@@ -1116,300 +952,178 @@ bool SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd)
 	std::ostringstream debug_text;
 
 
-  TaskInstID first_task_inst = cmd->first;
-  TaskInstID second_task_inst = cmd->second;
-  Condition condition = cmd->condition;
-
-
-//  this->ordering_links.insert(std::pair<TaskInstID, TaskInstID>(first_task_inst, second_task_inst));
-//  this->reverse_ordering_links.insert(std::pair<TaskInstID, TaskInstID>(second_task_inst, first_task_inst));
-
-
-
-
-//  if(cmd->first == 11 && cmd->second == 12){
-//	bool hi = false;
- // }
-
-
-
-  if(task_insts_.find(cmd->second)->second == 20){
-
-    debug_text << "SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd):  Cannot schedule before initial task"<<std::endl;
-    SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-    debug_text.str("");
+	TaskInstID first_task_inst = cmd->first;
+	TaskInstID second_task_inst = cmd->second;
+	Condition condition = cmd->condition;
 
-	return false;
-  }
 
-  if(cmd->first == -1){
+	if(task_insts_.find(cmd->second)->second == 20){
 
-    debug_text << "SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd):  Cannot push threat to after goal"<<std::endl;
-    SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-	debug_text.str("");
+		debug_text << "SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd):  Cannot schedule before initial task"<<std::endl;
+		SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+		debug_text.str("");
 
-	if(cmd->second == 12){
-		bool a = true;
+		return false;
 	}
 
-	return false;
-  }
-
-  PrecedenceSet* befores = &this->precedence_graph_.find(BEFORE)->second;
-  PrecedenceSet* afters = &this->precedence_graph_.find(AFTER)->second;
-  PrecedenceSet* simuls = &this->precedence_graph_.find(SIMUL)->second;
-  PrecedenceSet* unrankeds = &this->precedence_graph_.find(UNRANKED)->second;
-
+	if(cmd->first == -1){
 
-  TaskInstSet *before_A = &befores->find(cmd->first)->second;
-  TaskInstSet *after_A = &afters->find(cmd->first)->second;
-  TaskInstSet *simul_A = &simuls->find(cmd->first)->second;
-  TaskInstSet *unranked_A = &unrankeds->find(cmd->first)->second;
-	
-  TaskInstSet *before_B = &befores->find(cmd->second)->second;
-  TaskInstSet *after_B = &afters->find(cmd->second)->second;
-  TaskInstSet *simul_B = &simuls->find(cmd->second)->second;
-  TaskInstSet *unranked_B = &unrankeds->find(cmd->second)->second;
+		debug_text << "SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd):  Cannot push threat to after goal"<<std::endl;
+		SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+		debug_text.str("");
 
+		if(cmd->second == 12){
+			bool a = true;
+		}
 
+		return false;
+	}
 
+	PrecedenceSet* befores = &this->precedence_graph_.find(BEFORE)->second;
+	PrecedenceSet* afters = &this->precedence_graph_.find(AFTER)->second;
+	PrecedenceSet* simuls = &this->precedence_graph_.find(SIMUL)->second;
+	PrecedenceSet* unrankeds = &this->precedence_graph_.find(UNRANKED)->second;
 
 
-  if(before_B->find(cmd->first) != before_B->end()){
-	//Don't need to resolve
+	TaskInstSet *before_A = &befores->find(cmd->first)->second;
+	TaskInstSet *after_A = &afters->find(cmd->first)->second;
+	TaskInstSet *simul_A = &simuls->find(cmd->first)->second;
+	TaskInstSet *unranked_A = &unrankeds->find(cmd->first)->second;
 
-	  debug_text << "SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd):  Threat has already been resolved: "<<cmd->first<<" is before "<<cmd->second<<std::endl;
-    SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-	debug_text.str("");
+	TaskInstSet *before_B = &befores->find(cmd->second)->second;
+	TaskInstSet *after_B = &afters->find(cmd->second)->second;
+	TaskInstSet *simul_B = &simuls->find(cmd->second)->second;
+	TaskInstSet *unranked_B = &unrankeds->find(cmd->second)->second;
 
 
-//	if(this->is_cycle_in_ordering()){
-//		bool this_means_ben_failed = true;
-//	}
+	if(before_B->find(cmd->first) != before_B->end()){
+		//Don't need to resolve
 
-	  return true;
-  }
-
-  if(after_B->find(cmd->first) != after_B->end()){
-	//impossible?
-    debug_text << "SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd):  Cannot schedule task inst"<<cmd->first<<" before task inst"<<cmd->second<<std::endl;
-    SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-	debug_text.str("");
-//	if(!this->is_cycle_in_ordering()){
-//		bool this_means_ben_failed = true;
-//	}
+		debug_text << "SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd):  Threat has already been resolved: "<<cmd->first<<" is before "<<cmd->second<<std::endl;
+		SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+		debug_text.str("");
 
-//	  loop_detected = true;
+		return true;
+	}
 
-//	cmd->got_to_change_precedences = false;
-	return false;
+	if(after_B->find(cmd->first) != after_B->end()){
+		//impossible?
+		debug_text << "SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd):  Cannot schedule task inst"<<cmd->first<<" before task inst"<<cmd->second<<std::endl;
+		SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+		debug_text.str("");
 
-  }else{
-//	if(this->is_cycle_in_ordering()){
-//		bool this_means_ben_failed = true;
-//	}
+		return false;
 
-  }
+	}
 
-  cmd->got_to_change_precedences = true;
+	cmd->got_to_change_precedences = true;
 
-  cmd->befores = *befores;
-  cmd->afters = *afters;
-  cmd->simuls = *simuls;
-  cmd->unrankeds = *unrankeds;
+	cmd->befores = *befores;
+	cmd->afters = *afters;
+	cmd->simuls = *simuls;
+	cmd->unrankeds = *unrankeds;
 
 
-  TaskInstSet tmp;
+	TaskInstSet tmp;
 
-  before_B->insert(cmd->first);
-  unranked_B->erase(cmd->first);
+	before_B->insert(cmd->first);
+	unranked_B->erase(cmd->first);
 
- // bool loop_detected = false;
+	for(TaskInstSet::iterator it = before_A->begin(); it != before_A->end(); it++){
 
-  for(TaskInstSet::iterator it = before_A->begin(); it != before_A->end(); it++){
-  
-	before_B->insert(*it);
-    unranked_B->erase(*it);
+		before_B->insert(*it);
+		unranked_B->erase(*it);
 
-    afters->find(*it)->second.insert(cmd->second);
-    unrankeds->find(*it)->second.erase(cmd->second);
+		afters->find(*it)->second.insert(cmd->second);
+		unrankeds->find(*it)->second.erase(cmd->second);
 
 
-	for(TaskInstSet::iterator it2 = after_B->begin(); it2 != after_B->end(); it2++){
-		befores->find(*it2)->second.insert(*it);
-		unrankeds->find(*it2)->second.erase(*it);
+		for(TaskInstSet::iterator it2 = after_B->begin(); it2 != after_B->end(); it2++){
+			befores->find(*it2)->second.insert(*it);
+			unrankeds->find(*it2)->second.erase(*it);
 
-		afters->find(*it)->second.insert(*it2);
-		unrankeds->find(*it)->second.erase(*it2);
+			afters->find(*it)->second.insert(*it2);
+			unrankeds->find(*it)->second.erase(*it2);
+		}
 	}
-  }
-
-  after_A->insert(cmd->second);
-  unranked_A->erase(cmd->second);
 
-  for(TaskInstSet::iterator it = after_B->begin(); it != after_B->end(); it++){
+	after_A->insert(cmd->second);
+	unranked_A->erase(cmd->second);
 
-	after_A->insert(*it);
-    unranked_A->erase(*it);
+	for(TaskInstSet::iterator it = after_B->begin(); it != after_B->end(); it++){
 
-    befores->find(*it)->second.insert(cmd->first);
-    unrankeds->find(*it)->second.erase(cmd->first);
+		after_A->insert(*it);
+		unranked_A->erase(*it);
 
-	/*
-	for(TaskInstSet::iterator it2 = before_A->begin(); it2 != before_A->end(); it2++){
-		afters->find(*it2)->second.insert(*it);
-		unrankeds->find(*it2)->second.erase(*it);
+		befores->find(*it)->second.insert(cmd->first);
+		unrankeds->find(*it)->second.erase(cmd->first);
 
-		befores->find(*it)->second.insert(*it2);
-		unrankeds->find(*it)->second.erase(*it2);
 	}
-	*/	
-  }
-
 
-  debug_text << "SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd): Now scheduling task "<<cmd->first<<" before "<<cmd->second<<std::endl;
-  SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-  debug_text.str("");
-
-//  print_precedence_graph("SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd)");
 
+	debug_text << "SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd): Now scheduling task "<<cmd->first<<" before "<<cmd->second<<std::endl;
+	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+	debug_text.str("");
 
-  return true;
+	return true;
 };
 
-/*
-void SA_WorkingPlan::dfs_aux(TaskInstID current, std::stack<TaskInstID>& s, std::map<TaskInstID, bool>& visited, std::map<TaskInstID, bool>& unvisited){
-
-
-
-  unvisited.erase(current);
-  visited.insert(std::pair<TaskInstID, bool>(current, true));
-
-    std::pair<
-        SchedulingLinks::iterator,
-        SchedulingLinks::iterator
-            > ret = this->ordering_links.equal_range(current);
+// Undo a command to resolve a causal link threat in the
+// plan (with promotion or demotion).
+void SA_WorkingPlan::undo (SA_ResolveCLThreatCmd * cmd)
+{
 
-    for(SchedulingLinks::iterator it = ret.first; it != ret.second; it++){
-      TaskInstID hah = it->second;
-      if(visited.find(hah)==visited.end()){
-        dfs_aux(it->second, s, visited, unvisited);
-      }
-    }
+	std::ostringstream debug_text;
 
-   s.push(current);
+	debug_text << "SA_WorkingPlan::undo (SA_ResolveCLThreatCmd * cmd): Undoing scheduling task "<<cmd->first<<" before "<<cmd->second<<std::endl;
+	if(!cmd->got_to_change_precedences){
+		debug_text <<"		no changes to the precedence graph to undo"<<std::endl;
 
-}
+		SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+		debug_text.str("");
+	}
 
-bool SA_WorkingPlan::dfs_aux2(TaskInstID current, std::map<TaskInstID, bool>& visited){
+	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
+	debug_text.str("");
 
-  visited.insert(std::pair<TaskInstID, bool>(current, true));
+	if(cmd->got_to_change_precedences){
 
-    std::pair<
-        SchedulingLinks::iterator,
-        SchedulingLinks::iterator
-            > ret = this->reverse_ordering_links.equal_range(current);
 
-    for(SchedulingLinks::iterator it = ret.first; it != ret.second; it++){
-      TaskInstID hah = it->second;
-      if(visited.find(hah)==visited.end()){
-        return false;
-      }
-    }
+		PrecedenceSet* new_befores = &this->precedence_graph_.find(BEFORE)->second;
+		PrecedenceSet* new_afters = &this->precedence_graph_.find(AFTER)->second;
+		PrecedenceSet* new_simuls = &this->precedence_graph_.find(SIMUL)->second;
+		PrecedenceSet* new_unrankeds = &this->precedence_graph_.find(UNRANKED)->second;
 
-    return true;
-}
-*/
+		for(PrecedenceSet::iterator it = cmd->befores.begin(); it != cmd->befores.end(); it++){
+			new_befores->find(it->first)->second.clear();
+			for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
+				new_befores->find(it->first)->second.insert(*jt);
+			}
+		}
 
-// Undo a command to resolve a causal link threat in the
-// plan (with promotion or demotion).
-void SA_WorkingPlan::undo (SA_ResolveCLThreatCmd * cmd)
-{
+		for(PrecedenceSet::iterator it = cmd->afters.begin(); it != cmd->afters.end(); it++){
+			new_afters->find(it->first)->second.clear();
+			for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
+				new_afters->find(it->first)->second.insert(*jt);
+			}
+		}
 
-  std::ostringstream debug_text;
+		for(PrecedenceSet::iterator it = cmd->simuls.begin(); it != cmd->simuls.end(); it++){
+			new_simuls->find(it->first)->second.clear();
+			for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
+				new_simuls->find(it->first)->second.insert(*jt);
+			}
+		}
 
-  debug_text << "SA_WorkingPlan::undo (SA_ResolveCLThreatCmd * cmd): Undoing scheduling task "<<cmd->first<<" before "<<cmd->second<<std::endl;
-  if(!cmd->got_to_change_precedences){
-	  debug_text <<"		no changes to the precedence graph to undo"<<std::endl;
+		for(PrecedenceSet::iterator it = cmd->unrankeds.begin(); it != cmd->unrankeds.end(); it++){
+			new_unrankeds->find(it->first)->second.clear();
+			for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
+				new_unrankeds->find(it->first)->second.insert(*jt);
+			}
+		}
 
-	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-	debug_text.str("");
-  }
-  // if(cmd->first == 11 && cmd->second == 12){
-//	bool hi = false;
-//  }
-
-  SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
-  debug_text.str("");
-
-  if(cmd->got_to_change_precedences){
-
-
-    PrecedenceSet* new_befores = &this->precedence_graph_.find(BEFORE)->second;
-    PrecedenceSet* new_afters = &this->precedence_graph_.find(AFTER)->second;
-    PrecedenceSet* new_simuls = &this->precedence_graph_.find(SIMUL)->second;
-    PrecedenceSet* new_unrankeds = &this->precedence_graph_.find(UNRANKED)->second;
-
-    for(PrecedenceSet::iterator it = cmd->befores.begin(); it != cmd->befores.end(); it++){
-      new_befores->find(it->first)->second.clear();
-      for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
-        new_befores->find(it->first)->second.insert(*jt);
-      }
-    }
-
-    for(PrecedenceSet::iterator it = cmd->afters.begin(); it != cmd->afters.end(); it++){
-      new_afters->find(it->first)->second.clear();
-      for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
-        new_afters->find(it->first)->second.insert(*jt);
-      }
-    }
-
-    for(PrecedenceSet::iterator it = cmd->simuls.begin(); it != cmd->simuls.end(); it++){
-      new_simuls->find(it->first)->second.clear();
-      for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
-        new_simuls->find(it->first)->second.insert(*jt);
-      }
-    }
-
-    for(PrecedenceSet::iterator it = cmd->unrankeds.begin(); it != cmd->unrankeds.end(); it++){
-      new_unrankeds->find(it->first)->second.clear();
-      for(TaskInstSet::iterator jt = it->second.begin(); jt != it->second.end(); jt++){
-        new_unrankeds->find(it->first)->second.insert(*jt);
-      }
-    }
-
-  }
-
-  /*
-  std::pair<
-      std::multimap<TaskInstID, TaskInstID >::iterator,
-      std::multimap<TaskInstID, TaskInstID >::iterator
-      > ret = this->ordering_links.equal_range(cmd->first);
-
-  std::multimap<TaskInstID, TaskInstID >::iterator it;
-
-  for(it = ret.first; it != ret.second; it++){
-    if(it->second == cmd->second)
-      break;
-  }
-
-//  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);
-
-  for(it = ret.first; it != ret.second; it++){
-    if(it->second == cmd->first)
-        break;
-  }
-
-  this->reverse_ordering_links.erase(it);
-  */
-  };
+	}
+};
 
 // Execute a command to resolve a scheduling conflict (i.e.
 // non-causal-link ordering constraint with promotion or demotion)
@@ -1431,7 +1145,7 @@ bool SA_WorkingPlan::execute (SA_ResolveSchedOrderCmd *cmd)
 	first_end = this->get_end_window(first_task_inst);
 	second_start = this->get_start_window(second_task_inst);
 	second_end = this->get_end_window(second_task_inst);
-	
+
 	if(second_start.second!= NULL_TIME && (first_end.second==NULL_TIME || second_start.second<first_end.second))
 	{
 		// There is a need to adjust the max times
@@ -1450,17 +1164,17 @@ bool SA_WorkingPlan::execute (SA_ResolveSchedOrderCmd *cmd)
 			return false;
 	}
 	else cmd->adj_min_times_cmd_=NULL;
-	
+
 	PrecedenceSet *before = &this->precedence_graph_.find(BEFORE)->second;
 	PrecedenceSet *after = &this->precedence_graph_.find(AFTER)->second;
 	PrecedenceSet *simul = &this->precedence_graph_.find(SIMUL)->second;
 	PrecedenceSet *unranked = &this->precedence_graph_.find(UNRANKED)->second;
-	
+
 	before->find(second_task_inst)->second.insert(first_task_inst);
 	after->find(first_task_inst)->second.insert(second_task_inst);
 	unranked->find(first_task_inst)->second.erase(unranked->find(first_task_inst)->second.find(second_task_inst));
 	unranked->find(second_task_inst)->second.erase(unranked->find(second_task_inst)->second.find(first_task_inst));
-	
+
 	TaskInstSet after_second = this->after_orderings(second_task_inst);
 	for(TaskInstSet::iterator iter=simul->find(second_task_inst)->second.begin();iter!=simul->find(second_task_inst)->second.end();iter++)
 		after_second.insert(*iter);
@@ -1499,16 +1213,16 @@ bool SA_WorkingPlan::execute (SA_ResolveSchedOrderCmd *cmd)
 // between two task instances in the plan.
 void SA_WorkingPlan::undo (SA_ResolveSchedOrderCmd *cmd)
 {
-  	for(std::list<SA_ResolveSchedOrderCmd*>::reverse_iterator iter=cmd->cmds_.rbegin();iter!=cmd->cmds_.rend();iter++)
+	for(std::list<SA_ResolveSchedOrderCmd*>::reverse_iterator iter=cmd->cmds_.rbegin();iter!=cmd->cmds_.rend();iter++)
 		this->undo(*iter);
-	
+
 	TaskInstID first_task_inst = cmd->first;
 	TaskInstID second_task_inst = cmd->second;
 
 	PrecedenceSet *before = &this->precedence_graph_.find(BEFORE)->second;
 	PrecedenceSet *after = &this->precedence_graph_.find(AFTER)->second;
 	PrecedenceSet *unranked = &this->precedence_graph_.find(UNRANKED)->second;
-	
+
 	before->find(second_task_inst)->second.erase(before->find(second_task_inst)->second.find(first_task_inst));
 	after->find(first_task_inst)->second.erase(after->find(first_task_inst)->second.find(second_task_inst));
 	unranked->find(first_task_inst)->second.insert(second_task_inst);
@@ -1529,144 +1243,144 @@ bool SA_WorkingPlan::execute (SA_AdjustMinTimesCmd *cmd)
 	debug_text.str("");
 
 	TimeWindow *start_win = &this->start_window_map_.find(cmd->task_inst_)->second;
-  TimeWindow *end_win = &this->end_window_map_.find(cmd->task_inst_)->second;
-  // If this adjustment violated and time constraints, return false.
-  if(start_win->second!=NULL_TIME &&(cmd->new_end_min==NULL_TIME || cmd->new_end_min>end_win->second || cmd->new_start_min>start_win->second)) 
-	  return false;
-  // Set the new time window
-  start_win->first = cmd->new_start_min;
-  end_win->first = cmd->new_end_min;
-	
-  TimeWindow ts = this->get_start_window(cmd->task_inst_);
-  TimeWindow es = this->get_end_window(cmd->task_inst_);
-  CLSet after_links = this->get_after(cmd->task_inst_);
-  if(after_links.empty()) debug_text<<"after links is empty"<<std::endl;
-  TaskInstSet causal,sched,data;
-  for(CLSet::iterator iter=after_links.begin();iter!=after_links.end();iter++)
-  {    if(iter->cond.kind!=DATA) causal.insert(iter->second);
-       else data.insert(iter->second);
-  }
-  for(SchedulingLinks::iterator iter=sched_links_.begin();iter!=sched_links_.end();iter++)
-    if(iter->first==cmd->task_inst_) sched.insert(iter->second);
-  
-
-  // Propagate this time window change to all the task instances ordered after this task instance
-  for(TaskInstSet::iterator iter=causal.begin();iter!=causal.end();iter++)
-  {
-    TimeWindow temp_start = this->get_start_window(*iter);
-    TimeWindow temp_end = this->get_end_window(*iter);
-    if(end_win->first>temp_start.first)
-    {
-		debug_text<<"adjusting time for causal "<<*iter<<std::endl;
-		SA_AdjustMinTimesCmd* temp = static_cast<SA_AdjustMinTimesCmd *> (this->get_AdjustMinTimesCmd());
-		TimeValue dur = this->durations.find(*iter)->second;
-		if(dur!=0) temp->set_times(*iter,end_win->first,end_win->first+this->durations.find(*iter)->second);
-		else temp->set_times(*iter,end_win->first,temp_end.first);
-		cmd->min_adjust_cmds.push_back(temp);
-    }
-  }
-
-  if(sched.empty()) debug_text<<"sched is empty"<<std::endl;
-
-  SA_POP_DEBUG_STR (ANKET, debug_text.str ());
-  debug_text.str("");
-
-  for(TaskInstSet::iterator iter=sched.begin();iter!=sched.end();iter++)
-  {
-    TimeWindow temp_start = this->get_start_window(*iter);
-    TimeWindow temp_end = this->get_end_window(*iter);
-    if(end_win->first>temp_start.first)
-    {
-      SA_AdjustMinTimesCmd* temp = static_cast<SA_AdjustMinTimesCmd *> (this->get_AdjustMinTimesCmd());
-		  TimeValue dur = this->durations.find(*iter)->second;
-		  if(dur!=0) 
-        temp->set_times(*iter,end_win->first,end_win->first+this->durations.find(*iter)->second);
-      else 
-        temp->set_times(*iter,end_win->first,temp_end.first);
-      cmd->min_adjust_cmds.push_back(temp);
-  
-    }
-  }
+	TimeWindow *end_win = &this->end_window_map_.find(cmd->task_inst_)->second;
+	// If this adjustment violated and time constraints, return false.
+	if(start_win->second!=NULL_TIME &&(cmd->new_end_min==NULL_TIME || cmd->new_end_min>end_win->second || cmd->new_start_min>start_win->second)) 
+		return false;
+	// Set the new time window
+	start_win->first = cmd->new_start_min;
+	end_win->first = cmd->new_end_min;
+
+	TimeWindow ts = this->get_start_window(cmd->task_inst_);
+	TimeWindow es = this->get_end_window(cmd->task_inst_);
+	CLSet after_links = this->get_after(cmd->task_inst_);
+	if(after_links.empty()) debug_text<<"after links is empty"<<std::endl;
+	TaskInstSet causal,sched,data;
+	for(CLSet::iterator iter=after_links.begin();iter!=after_links.end();iter++)
+	{    if(iter->cond.kind!=DATA) causal.insert(iter->second);
+	else data.insert(iter->second);
+	}
+	for(SchedulingLinks::iterator iter=sched_links_.begin();iter!=sched_links_.end();iter++)
+		if(iter->first==cmd->task_inst_) sched.insert(iter->second);
+
+
+	// Propagate this time window change to all the task instances ordered after this task instance
+	for(TaskInstSet::iterator iter=causal.begin();iter!=causal.end();iter++)
+	{
+		TimeWindow temp_start = this->get_start_window(*iter);
+		TimeWindow temp_end = this->get_end_window(*iter);
+		if(end_win->first>temp_start.first)
+		{
+			debug_text<<"adjusting time for causal "<<*iter<<std::endl;
+			SA_AdjustMinTimesCmd* temp = static_cast<SA_AdjustMinTimesCmd *> (this->get_AdjustMinTimesCmd());
+			TimeValue dur = this->durations.find(*iter)->second;
+			if(dur!=0) temp->set_times(*iter,end_win->first,end_win->first+this->durations.find(*iter)->second);
+			else temp->set_times(*iter,end_win->first,temp_end.first);
+			cmd->min_adjust_cmds.push_back(temp);
+		}
+	}
+
+	if(sched.empty()) debug_text<<"sched is empty"<<std::endl;
+
+	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
+	debug_text.str("");
+
+	for(TaskInstSet::iterator iter=sched.begin();iter!=sched.end();iter++)
+	{
+		TimeWindow temp_start = this->get_start_window(*iter);
+		TimeWindow temp_end = this->get_end_window(*iter);
+		if(end_win->first>temp_start.first)
+		{
+			SA_AdjustMinTimesCmd* temp = static_cast<SA_AdjustMinTimesCmd *> (this->get_AdjustMinTimesCmd());
+			TimeValue dur = this->durations.find(*iter)->second;
+			if(dur!=0) 
+				temp->set_times(*iter,end_win->first,end_win->first+this->durations.find(*iter)->second);
+			else 
+				temp->set_times(*iter,end_win->first,temp_end.first);
+			cmd->min_adjust_cmds.push_back(temp);
+
+		}
+	}
 
 
 	// Do the same change for the task instances simultaneous to this one
-  for(TaskInstSet::iterator iter=data.begin();iter!=data.end();iter++)
-  {
-    TimeWindow temp_start = this->get_start_window(*iter);
-    if(start_win->first>temp_start.first)
-    {
-		debug_text<<"adjusting time for data "<<*iter<<std::endl;
-      SA_AdjustMinTimesCmd* temp = static_cast<SA_AdjustMinTimesCmd *> (this->get_AdjustMinTimesCmd());
-      temp->set_times(*iter,start_win->first,end_win->first);
-      cmd->min_adjust_cmds.push_back(temp);
-    }
-  }
+	for(TaskInstSet::iterator iter=data.begin();iter!=data.end();iter++)
+	{
+		TimeWindow temp_start = this->get_start_window(*iter);
+		if(start_win->first>temp_start.first)
+		{
+			debug_text<<"adjusting time for data "<<*iter<<std::endl;
+			SA_AdjustMinTimesCmd* temp = static_cast<SA_AdjustMinTimesCmd *> (this->get_AdjustMinTimesCmd());
+			temp->set_times(*iter,start_win->first,end_win->first);
+			cmd->min_adjust_cmds.push_back(temp);
+		}
+	}
+
+	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
+	debug_text.str("");
+
+	for(SA_AdjustMinTimesCmd::MinTimesAdjustList::iterator iter=cmd->min_adjust_cmds.begin();iter!=cmd->min_adjust_cmds.end();iter++)
+	{
+		if(!this->execute(*iter)) return false;
+	}
+
+	debug_text<<"the task inst is "<<cmd->task_inst_<<std::endl;
+	// Check to see whether any task instance should be removed from the unranked set due to this time window change
+	TaskInstSet *unranked = &this->precedence_graph_.find(UNRANKED)->second.find(cmd->task_inst_)->second;
+	for(TaskInstSet::iterator iter=unranked->begin();iter!=unranked->end();iter++)
+	{
+		TimeWindow temp_end = this->get_end_window(*iter);
+		if(temp_end.second!=NULL_TIME && temp_end.second<start_win->first)
+		{
+			this->precedence_graph_.find(BEFORE)->second.find(cmd->task_inst_)->second.insert(*iter);
+			this->precedence_graph_.find(AFTER)->second.find(*iter)->second.insert(cmd->task_inst_);
+			this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second.erase(this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second.find(*iter));
+			cmd->ranked.insert(*iter);
+		}
+	}
+	for(TaskInstSet::iterator iter=cmd->ranked.begin();iter!=cmd->ranked.end();iter++)
+	{
+		unranked->erase(unranked->find(*iter));
+	}
+	debug_text<<"min times of "<<cmd->task_inst_<<" : "<<cmd->new_start_min<<" "<<cmd->new_end_min<<std::endl;
 
 	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
 	debug_text.str("");
 
-  for(SA_AdjustMinTimesCmd::MinTimesAdjustList::iterator iter=cmd->min_adjust_cmds.begin();iter!=cmd->min_adjust_cmds.end();iter++)
-  {
-    if(!this->execute(*iter)) return false;
-  }
-
-  debug_text<<"the task inst is "<<cmd->task_inst_<<std::endl;
-  // Check to see whether any task instance should be removed from the unranked set due to this time window change
-  TaskInstSet *unranked = &this->precedence_graph_.find(UNRANKED)->second.find(cmd->task_inst_)->second;
-  for(TaskInstSet::iterator iter=unranked->begin();iter!=unranked->end();iter++)
-  {
-	  TimeWindow temp_end = this->get_end_window(*iter);
-	  if(temp_end.second!=NULL_TIME && temp_end.second<start_win->first)
-	  {
-		  this->precedence_graph_.find(BEFORE)->second.find(cmd->task_inst_)->second.insert(*iter);
-		  this->precedence_graph_.find(AFTER)->second.find(*iter)->second.insert(cmd->task_inst_);
-		  this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second.erase(this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second.find(*iter));
-		  cmd->ranked.insert(*iter);
-	  }
-  }
-  for(TaskInstSet::iterator iter=cmd->ranked.begin();iter!=cmd->ranked.end();iter++)
-  {
-	  unranked->erase(unranked->find(*iter));
-  }
-  debug_text<<"min times of "<<cmd->task_inst_<<" : "<<cmd->new_start_min<<" "<<cmd->new_end_min<<std::endl;
-
-  SA_POP_DEBUG_STR (ANKET, debug_text.str ());
-  debug_text.str("");
-
-  return true;
+	return true;
 };
 
 // Undo a command to adjust the minimum times of a
 // task instance in the plan.
 void SA_WorkingPlan::undo (SA_AdjustMinTimesCmd *cmd)
 {
-  TimeWindow *start_win = &this->get_start_window(cmd->task_inst_);
-  TimeWindow *end_win = &this->get_end_window(cmd->task_inst_);
-	
-  if(start_win->first==cmd->old_start_min && end_win->first==cmd->old_end_min) return;
-  // Undo all the removal of the tasks from the unranked set
-  TaskInstSet *unranked1 = &this->precedence_graph_.find(UNRANKED)->second.find(cmd->task_inst_)->second;
-  TaskInstSet *before = &this->precedence_graph_.find(BEFORE)->second.find(cmd->task_inst_)->second;
-  for(TaskInstSet::iterator iter=cmd->ranked.begin();iter!=cmd->ranked.end();iter++)
-  {
-	  TaskInstSet *unranked2 = &this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second;
-	  TaskInstSet *after = &this->precedence_graph_.find(AFTER)->second.find(*iter)->second;
-	  unranked1->insert(*iter);
-	  unranked2->insert(cmd->task_inst_);
-	  before->erase(before->find(*iter));
-	  after->erase(after->find(cmd->task_inst_));
-  }
-  
-  // Undo all the time window changes caused by this command
-  for(SA_AdjustMinTimesCmd::MinTimesAdjustList::reverse_iterator iter=cmd->min_adjust_cmds.rbegin();iter!=cmd->min_adjust_cmds.rend();iter++)
-  {
-    this->undo(*iter);
-  }
+	TimeWindow *start_win = &this->get_start_window(cmd->task_inst_);
+	TimeWindow *end_win = &this->get_end_window(cmd->task_inst_);
+
+	if(start_win->first==cmd->old_start_min && end_win->first==cmd->old_end_min) return;
+	// Undo all the removal of the tasks from the unranked set
+	TaskInstSet *unranked1 = &this->precedence_graph_.find(UNRANKED)->second.find(cmd->task_inst_)->second;
+	TaskInstSet *before = &this->precedence_graph_.find(BEFORE)->second.find(cmd->task_inst_)->second;
+	for(TaskInstSet::iterator iter=cmd->ranked.begin();iter!=cmd->ranked.end();iter++)
+	{
+		TaskInstSet *unranked2 = &this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second;
+		TaskInstSet *after = &this->precedence_graph_.find(AFTER)->second.find(*iter)->second;
+		unranked1->insert(*iter);
+		unranked2->insert(cmd->task_inst_);
+		before->erase(before->find(*iter));
+		after->erase(after->find(cmd->task_inst_));
+	}
+
+	// Undo all the time window changes caused by this command
+	for(SA_AdjustMinTimesCmd::MinTimesAdjustList::reverse_iterator iter=cmd->min_adjust_cmds.rbegin();iter!=cmd->min_adjust_cmds.rend();iter++)
+	{
+		this->undo(*iter);
+	}
 	// Reset the time window to the old one  
-  start_win->first=cmd->old_start_min;
-  end_win->first=cmd->old_end_min;
-  
-  return;
+	start_win->first=cmd->old_start_min;
+	end_win->first=cmd->old_end_min;
+
+	return;
 };
 
 // Execute a command to adjust the maximum times of a
@@ -1677,93 +1391,93 @@ bool SA_WorkingPlan::execute (SA_AdjustMaxTimesCmd *cmd)
 
 	debug_text<<"adjusting max time for "<<cmd->task_inst_<<std::endl;
 	TimeWindow *start_win = &this->start_window_map_.find(cmd->task_inst_)->second;
-  TimeWindow *end_win = &this->end_window_map_.find(cmd->task_inst_)->second;
- debug_text<<"for "<<cmd->task_inst_<<" "<<cmd->new_end_max<<" "<<end_win->first<<" "<<cmd->new_start_max<<" "<<start_win->first<<std::endl;
-  // If this adjustment violated and time constraints, return false.
-  if((end_win->first==NULL_TIME && cmd->new_end_max!= NULL_TIME) || cmd->new_end_max<end_win->first || cmd->new_start_max<start_win->first) return false;
-  // Set the new time window
-  start_win->second = cmd->new_start_max;
-  end_win->second = cmd->new_end_max;
-  CLSet before_links = this->get_before(cmd->task_inst_);
-  TaskInstSet causal,sched,data;
-  for(CLSet::iterator iter=before_links.begin();iter!=before_links.end();iter++)
-  {    if(iter->cond.kind!=DATA) causal.insert(iter->first);
-       else data.insert(iter->first);
-  }
-  for(SchedulingLinks::iterator iter=sched_links_.begin();iter!=sched_links_.end();iter++)
-    if(iter->second==cmd->task_inst_) sched.insert(iter->first);
-  
-  // Propagate this time window change to all the task instances ordered before this task instance
-  for(TaskInstSet::iterator iter=causal.begin();iter!=causal.end();iter++)
-  {
-    TimeWindow temp_end = this->get_end_window(*iter);
-	if(temp_end.second==NULL_TIME || (temp_end.second!=NULL_TIME && start_win->second<temp_end.second))
-    {
-      SA_AdjustMaxTimesCmd* temp = static_cast<SA_AdjustMaxTimesCmd *> (this->get_AdjustMaxTimesCmd());
-      temp->set_times(*iter,start_win->second-this->durations.find(*iter)->second,start_win->second);
-      cmd->max_adjust_cmds.push_back(temp);
-    }
-  }
-
-  if(sched.empty()) debug_text<<"msched is empty ("<<cmd->task_inst_<<")"<<std::endl;
-  else
-  debug_text<<"mnot sched is empty ("<<cmd->task_inst_<<")"<<std::endl;
-  
-  for(TaskInstSet::iterator iter=sched.begin();iter!=sched.end();iter++)
-  {
-    TimeWindow temp_end = this->get_end_window(*iter);
-    if(temp_end.second==NULL_TIME || (temp_end.second!=NULL_TIME && start_win->second<temp_end.second))
-    {
-      SA_AdjustMaxTimesCmd* temp = static_cast<SA_AdjustMaxTimesCmd *> (this->get_AdjustMaxTimesCmd());
-      temp->set_times(*iter,start_win->second-this->durations.find(*iter)->second,start_win->second);
-      cmd->max_adjust_cmds.push_back(temp);
-    }
-  }
+	TimeWindow *end_win = &this->end_window_map_.find(cmd->task_inst_)->second;
+	debug_text<<"for "<<cmd->task_inst_<<" "<<cmd->new_end_max<<" "<<end_win->first<<" "<<cmd->new_start_max<<" "<<start_win->first<<std::endl;
+	// If this adjustment violated and time constraints, return false.
+	if((end_win->first==NULL_TIME && cmd->new_end_max!= NULL_TIME) || cmd->new_end_max<end_win->first || cmd->new_start_max<start_win->first) return false;
+	// Set the new time window
+	start_win->second = cmd->new_start_max;
+	end_win->second = cmd->new_end_max;
+	CLSet before_links = this->get_before(cmd->task_inst_);
+	TaskInstSet causal,sched,data;
+	for(CLSet::iterator iter=before_links.begin();iter!=before_links.end();iter++)
+	{    if(iter->cond.kind!=DATA) causal.insert(iter->first);
+	else data.insert(iter->first);
+	}
+	for(SchedulingLinks::iterator iter=sched_links_.begin();iter!=sched_links_.end();iter++)
+		if(iter->second==cmd->task_inst_) sched.insert(iter->first);
+
+	// Propagate this time window change to all the task instances ordered before this task instance
+	for(TaskInstSet::iterator iter=causal.begin();iter!=causal.end();iter++)
+	{
+		TimeWindow temp_end = this->get_end_window(*iter);
+		if(temp_end.second==NULL_TIME || (temp_end.second!=NULL_TIME && start_win->second<temp_end.second))
+		{
+			SA_AdjustMaxTimesCmd* temp = static_cast<SA_AdjustMaxTimesCmd *> (this->get_AdjustMaxTimesCmd());
+			temp->set_times(*iter,start_win->second-this->durations.find(*iter)->second,start_win->second);
+			cmd->max_adjust_cmds.push_back(temp);
+		}
+	}
+
+	if(sched.empty()) debug_text<<"msched is empty ("<<cmd->task_inst_<<")"<<std::endl;
+	else
+		debug_text<<"mnot sched is empty ("<<cmd->task_inst_<<")"<<std::endl;
+
+	for(TaskInstSet::iterator iter=sched.begin();iter!=sched.end();iter++)
+	{
+		TimeWindow temp_end = this->get_end_window(*iter);
+		if(temp_end.second==NULL_TIME || (temp_end.second!=NULL_TIME && start_win->second<temp_end.second))
+		{
+			SA_AdjustMaxTimesCmd* temp = static_cast<SA_AdjustMaxTimesCmd *> (this->get_AdjustMaxTimesCmd());
+			temp->set_times(*iter,start_win->second-this->durations.find(*iter)->second,start_win->second);
+			cmd->max_adjust_cmds.push_back(temp);
+		}
+	}
 
 
 
 	// Do the same change for the task instances simultaneous to this one
-  for(TaskInstSet::iterator iter=data.begin();iter!=data.end();iter++)
-  {
-    TimeWindow temp_end = this->get_end_window(*iter);
-    if(temp_end.second==NULL_TIME || (temp_end.second!=NULL_TIME && end_win->second<temp_end.second))
-    {
-      SA_AdjustMaxTimesCmd* temp = static_cast<SA_AdjustMaxTimesCmd *> (this->get_AdjustMaxTimesCmd());
-      temp->set_times(*iter,start_win->second,end_win->second);
-      cmd->max_adjust_cmds.push_back(temp);
-    }
-  }
+	for(TaskInstSet::iterator iter=data.begin();iter!=data.end();iter++)
+	{
+		TimeWindow temp_end = this->get_end_window(*iter);
+		if(temp_end.second==NULL_TIME || (temp_end.second!=NULL_TIME && end_win->second<temp_end.second))
+		{
+			SA_AdjustMaxTimesCmd* temp = static_cast<SA_AdjustMaxTimesCmd *> (this->get_AdjustMaxTimesCmd());
+			temp->set_times(*iter,start_win->second,end_win->second);
+			cmd->max_adjust_cmds.push_back(temp);
+		}
+	}
 
 	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
 	debug_text.str("");
 
 
-  for(SA_AdjustMaxTimesCmd::MaxTimesAdjustList::iterator iter=cmd->max_adjust_cmds.begin();iter!=cmd->max_adjust_cmds.end();iter++)
-  {
-    if(!this->execute(*iter)) return false;
-  }
-  // Check to see whether any task instance should be removed from the unranked set due to this time window change
-  TaskInstSet *unranked = &this->precedence_graph_.find(UNRANKED)->second.find(cmd->task_inst_)->second;
-  for(TaskInstSet::iterator iter=unranked->begin();iter!=unranked->end();iter++)
-  {
-	  TimeWindow temp_start = this->get_start_window(*iter);
-	  if(temp_start.first>end_win->second)
-	  {
-		  this->precedence_graph_.find(AFTER)->second.find(cmd->task_inst_)->second.insert(*iter);
-		  this->precedence_graph_.find(BEFORE)->second.find(*iter)->second.insert(cmd->task_inst_);
-		  this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second.erase(this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second.find(*iter));
-		  cmd->ranked.insert(*iter);
-	  }
-  }
-  for(TaskInstSet::iterator iter=cmd->ranked.begin();iter!=cmd->ranked.end();iter++)
-  {
-	  unranked->erase(unranked->find(*iter));
-  }
-  debug_text<<"max times for "<<cmd->task_inst_<<" : "<<cmd->new_start_max<<" "<<cmd->new_end_max<<std::endl;
-  SA_POP_DEBUG_STR (ANKET, debug_text.str ());
-  debug_text.str("");
-
-  return true;
+	for(SA_AdjustMaxTimesCmd::MaxTimesAdjustList::iterator iter=cmd->max_adjust_cmds.begin();iter!=cmd->max_adjust_cmds.end();iter++)
+	{
+		if(!this->execute(*iter)) return false;
+	}
+	// Check to see whether any task instance should be removed from the unranked set due to this time window change
+	TaskInstSet *unranked = &this->precedence_graph_.find(UNRANKED)->second.find(cmd->task_inst_)->second;
+	for(TaskInstSet::iterator iter=unranked->begin();iter!=unranked->end();iter++)
+	{
+		TimeWindow temp_start = this->get_start_window(*iter);
+		if(temp_start.first>end_win->second)
+		{
+			this->precedence_graph_.find(AFTER)->second.find(cmd->task_inst_)->second.insert(*iter);
+			this->precedence_graph_.find(BEFORE)->second.find(*iter)->second.insert(cmd->task_inst_);
+			this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second.erase(this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second.find(*iter));
+			cmd->ranked.insert(*iter);
+		}
+	}
+	for(TaskInstSet::iterator iter=cmd->ranked.begin();iter!=cmd->ranked.end();iter++)
+	{
+		unranked->erase(unranked->find(*iter));
+	}
+	debug_text<<"max times for "<<cmd->task_inst_<<" : "<<cmd->new_start_max<<" "<<cmd->new_end_max<<std::endl;
+	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
+	debug_text.str("");
+
+	return true;
 };
 
 // Undo a command to adjust the maximum times of a
@@ -1771,37 +1485,37 @@ bool SA_WorkingPlan::execute (SA_AdjustMaxTimesCmd *cmd)
 void SA_WorkingPlan::undo (SA_AdjustMaxTimesCmd *cmd)
 {
 	std::ostringstream debug_text;
-  debug_text<<"task inst is "<<cmd->task_inst_<<std::endl;
+	debug_text<<"task inst is "<<cmd->task_inst_<<std::endl;
 	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
 	debug_text.str("");
 
-  TimeWindow *start_win = &this->get_start_window(cmd->task_inst_);
-  TimeWindow *end_win = &this->get_end_window(cmd->task_inst_);
-  if(start_win->second==cmd->old_start_max && end_win->second==cmd->old_end_max) return;
-  // Undo all the removal of the tasks from the unranked set
-  TaskInstSet *unranked1 = &this->precedence_graph_.find(UNRANKED)->second.find(cmd->task_inst_)->second;
-  TaskInstSet *after = &this->precedence_graph_.find(AFTER)->second.find(cmd->task_inst_)->second;
-  for(TaskInstSet::iterator iter=cmd->ranked.begin();iter!=cmd->ranked.end();iter++)
-  {
-	  TaskInstSet *unranked2 = &this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second;
-	  TaskInstSet *before = &this->precedence_graph_.find(BEFORE)->second.find(*iter)->second;
-	  unranked1->insert(*iter);
-	  unranked2->insert(cmd->task_inst_);
-	  after->erase(after->find(*iter));
-	  before->erase(before->find(cmd->task_inst_));
-  }
-  
-  // Undo all the time window changes caused by this command
-  for(SA_AdjustMaxTimesCmd::MaxTimesAdjustList::reverse_iterator iter=cmd->max_adjust_cmds.rbegin();iter!=cmd->max_adjust_cmds.rend();iter++)
-  {
-    this->undo(*iter);
-  }
-  
+	TimeWindow *start_win = &this->get_start_window(cmd->task_inst_);
+	TimeWindow *end_win = &this->get_end_window(cmd->task_inst_);
+	if(start_win->second==cmd->old_start_max && end_win->second==cmd->old_end_max) return;
+	// Undo all the removal of the tasks from the unranked set
+	TaskInstSet *unranked1 = &this->precedence_graph_.find(UNRANKED)->second.find(cmd->task_inst_)->second;
+	TaskInstSet *after = &this->precedence_graph_.find(AFTER)->second.find(cmd->task_inst_)->second;
+	for(TaskInstSet::iterator iter=cmd->ranked.begin();iter!=cmd->ranked.end();iter++)
+	{
+		TaskInstSet *unranked2 = &this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second;
+		TaskInstSet *before = &this->precedence_graph_.find(BEFORE)->second.find(*iter)->second;
+		unranked1->insert(*iter);
+		unranked2->insert(cmd->task_inst_);
+		after->erase(after->find(*iter));
+		before->erase(before->find(cmd->task_inst_));
+	}
+
+	// Undo all the time window changes caused by this command
+	for(SA_AdjustMaxTimesCmd::MaxTimesAdjustList::reverse_iterator iter=cmd->max_adjust_cmds.rbegin();iter!=cmd->max_adjust_cmds.rend();iter++)
+	{
+		this->undo(*iter);
+	}
+
 	// Reset the time window to the old one  
-  start_win->second=cmd->old_start_max;
-  end_win->second=cmd->old_end_max;
-  
-  return;
+	start_win->second=cmd->old_start_max;
+	end_win->second=cmd->old_end_max;
+
+	return;
 };
 // Get the start window of the task instance. If one doesn't exist, then assign it one.
 TimeWindow SA_WorkingPlan::get_start_window(TaskInstID task_inst)
@@ -1849,20 +1563,20 @@ TimeWindow SA_WorkingPlan::get_end_window(TaskInstID task_inst)
 		this->start_window_map_.insert(std::make_pair(task_inst,temp_start));
 		this->end_window_map_.insert(std::make_pair(task_inst,temp_end));
 	}
-  return this->end_window_map_.find(task_inst)->second;
+	return this->end_window_map_.find(task_inst)->second;
 };
 
 //Get the duration of a task instance
 TimeValue SA_WorkingPlan::get_duration(TaskInstID task_inst)
 {
-  return this->durations.find(task_inst)->second;
+	return this->durations.find(task_inst)->second;
 };
 
 /// Adds the sched order to the sched_links_ map by putting the first task instance before the second
 void SA_WorkingPlan::add_sched_link(TaskInstID first_task_inst, TaskInstID second_task_inst)
 {
 	std::ostringstream debug_text;
-    debug_text<<"Adding sched link insert"<<std::endl;
+	debug_text<<"Adding sched link insert"<<std::endl;
 	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
 	debug_text.str("");
 
@@ -1887,365 +1601,359 @@ bool SA_WorkingPlan::init_prec_insert(TaskInstID task_inst, SA_AssocTaskImplCmd
 	if(cmd->get_id().step == 12 && cmd->get_id().decision_pt== 2 && cmd->get_id().seq_num == 1 && task_inst == 3)
 		bool yep = true;
 
-  TaskInstSet temp;
-  this->precedence_graph_.find(BEFORE)->second.insert(std::make_pair(task_inst,temp));
-  this->precedence_graph_.find(AFTER)->second.insert(std::make_pair(task_inst,temp));
-  this->precedence_graph_.find(UNRANKED)->second.insert(std::make_pair(task_inst,temp));
-  this->precedence_graph_.find(SIMUL)->second.insert(std::make_pair(task_inst,temp));
+	TaskInstSet temp;
+	this->precedence_graph_.find(BEFORE)->second.insert(std::make_pair(task_inst,temp));
+	this->precedence_graph_.find(AFTER)->second.insert(std::make_pair(task_inst,temp));
+	this->precedence_graph_.find(UNRANKED)->second.insert(std::make_pair(task_inst,temp));
+	this->precedence_graph_.find(SIMUL)->second.insert(std::make_pair(task_inst,temp));
 
 
-  if(init_start.size() <= task_inst){
-	  for(int i = init_start.size(); i < 100+task_inst; i++){
-		this->init_start.insert(std::make_pair(i,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
-		this->init_end.insert(std::make_pair(i,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
+	if(init_start.size() <= task_inst){
+		for(int i = init_start.size(); i < 100+task_inst; i++){
+			this->init_start.insert(std::make_pair(i,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
+			this->init_end.insert(std::make_pair(i,(TimeWindow)std::make_pair(NULL_TIME,NULL_TIME)));
+		}
 	}
-  }
 
-  TimeWindow win_start = this->init_start.find(task_inst)->second;
-  TimeWindow win_end = this->init_end.find(task_inst)->second;
-  TimeWindow start_win = this->get_start_window(task_inst);
-  TimeWindow end_win = this->get_end_window(task_inst);
+	TimeWindow win_start = this->init_start.find(task_inst)->second;
+	TimeWindow win_end = this->init_end.find(task_inst)->second;
+	TimeWindow start_win = this->get_start_window(task_inst);
+	TimeWindow end_win = this->get_end_window(task_inst);
 
 
 	std::ostringstream debug_text;
-  debug_text<<"for "<<task_inst<<win_start.first<<" "<<win_start.second<<" "<<win_end.first<<" "<<win_end.second<<std::endl;
-  debug_text<<"for "<<task_inst<<start_win.first<<" "<<start_win.second<<" "<<end_win.first<<" "<<end_win.second<<std::endl;
+	debug_text<<"for "<<task_inst<<win_start.first<<" "<<win_start.second<<" "<<win_end.first<<" "<<win_end.second<<std::endl;
+	debug_text<<"for "<<task_inst<<start_win.first<<" "<<start_win.second<<" "<<end_win.first<<" "<<end_win.second<<std::endl;
 	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
 	debug_text.str("");
 
-  if(win_start.first!=NULL_TIME)
-  {
-	  // An initial time constraint is set on the minimum times
-    if(win_start.second!=NULL_TIME)
-    {
-		  // An initial time constraint is set on the maximum times
-  		cmd->min_adj_cmd.set_times(task_inst,win_start.first,win_end.first);
-	  	cmd->max_adj_cmd.set_times(task_inst,win_start.second,win_end.second);
-		  if(!this->execute(&cmd->min_adj_cmd))
-		  {
-		    this->undo(&cmd->min_adj_cmd);
-			  return false;
-		  }
-		  if(!this->execute(&cmd->max_adj_cmd))
-		  {
-			  this->undo(&cmd->max_adj_cmd);
-			  this->undo(&cmd->min_adj_cmd);
-			  return false;
-		  }
-    }
-    else
-    {
-		// No initial time constraint is set on the maximum times
-  	  cmd->min_adj_cmd.set_times(task_inst,win_start.first,win_end.first);
-		  if(!this->execute(&cmd->min_adj_cmd))
-		  {
-			  this->undo(&cmd->min_adj_cmd);
-			  return false;
-		  }
-    }
-  }
-  else
-  {
-	  // No initial time constraint is set on the minimum times
-    if(win_start.second!=NULL_TIME)
-    {
- 		 // An initial time constraint is set on the maximum times
- 		cmd->max_adj_cmd.set_times(task_inst,win_start.second,win_end.second);
-	  	if(!this->execute(&cmd->max_adj_cmd))
-		  {
-			  this->undo(&cmd->min_adj_cmd);
-			  return false;
-		  }
-    }
-    else
-    {
-		// No initial time constraint.
-  		cmd->min_adj_cmd.set_times(task_inst,start_win.first,end_win.first);
-	  	cmd->max_adj_cmd.set_times(task_inst,start_win.second,end_win.second);
-
-	  std::ostringstream debug_text;
-      debug_text<<"for "<<task_inst<<" "<<start_win.first<<" "<<start_win.second<<" "<<end_win.first<<" "<<end_win.second<<std::endl;
-	  SA_POP_DEBUG_STR (ANKET, debug_text.str ());
-	  debug_text.str("");
-	  
-	  
-	  if(!this->execute(&cmd->min_adj_cmd))
-		  {
-		    this->undo(&cmd->min_adj_cmd);
-			  return false;
-		  }
-		  //if(!this->execute(&cmd->max_adj_cmd))
-		  //{
-			 // this->undo(&cmd->max_adj_cmd);
-			 // this->undo(&cmd->min_adj_cmd);
-			 // return false;
-		  //}
-    }
-  }
-  start_win = this->get_start_window(task_inst);
-  end_win = this->get_end_window(task_inst);
-  TaskInstSet *before,*after,*simul,*unranked;
-  before = &this->precedence_graph_.find(BEFORE)->second.find(task_inst)->second;
-  after = &this->precedence_graph_.find(AFTER)->second.find(task_inst)->second;
-  unranked = &this->precedence_graph_.find(UNRANKED)->second.find(task_inst)->second;
-  simul = &this->precedence_graph_.find(SIMUL)->second.find(task_inst)->second;
-  // If this task instance is not reused, insert all the task instances in it unranked set.
-  if(unranked->empty() && (this->reused_insts_.find(task_inst)== this->reused_insts_.end()))
-  {
-	std::ostringstream debug_text;
+	if(win_start.first!=NULL_TIME)
+	{
+		// An initial time constraint is set on the minimum times
+		if(win_start.second!=NULL_TIME)
+		{
+			// An initial time constraint is set on the maximum times
+			cmd->min_adj_cmd.set_times(task_inst,win_start.first,win_end.first);
+			cmd->max_adj_cmd.set_times(task_inst,win_start.second,win_end.second);
+			if(!this->execute(&cmd->min_adj_cmd))
+			{
+				this->undo(&cmd->min_adj_cmd);
+				return false;
+			}
+			if(!this->execute(&cmd->max_adj_cmd))
+			{
+				this->undo(&cmd->max_adj_cmd);
+				this->undo(&cmd->min_adj_cmd);
+				return false;
+			}
+		}
+		else
+		{
+			// No initial time constraint is set on the maximum times
+			cmd->min_adj_cmd.set_times(task_inst,win_start.first,win_end.first);
+			if(!this->execute(&cmd->min_adj_cmd))
+			{
+				this->undo(&cmd->min_adj_cmd);
+				return false;
+			}
+		}
+	}
+	else
+	{
+		// No initial time constraint is set on the minimum times
+		if(win_start.second!=NULL_TIME)
+		{
+			// An initial time constraint is set on the maximum times
+			cmd->max_adj_cmd.set_times(task_inst,win_start.second,win_end.second);
+			if(!this->execute(&cmd->max_adj_cmd))
+			{
+				this->undo(&cmd->min_adj_cmd);
+				return false;
+			}
+		}
+		else
+		{
+			// No initial time constraint.
+			cmd->min_adj_cmd.set_times(task_inst,start_win.first,end_win.first);
+			cmd->max_adj_cmd.set_times(task_inst,start_win.second,end_win.second);
 
-    for(PrecedenceSet::iterator iter=this->precedence_graph_.find(BEFORE)->second.begin();iter!=this->precedence_graph_.find(BEFORE)->second.end();iter++)
-    {
-	    if(iter->first!=task_inst)
-	    {
-		    debug_text<<"inserting "<<iter->first<<" in unranked of "<<task_inst<<std::endl;
-		    unranked->insert(iter->first);
-	    }
-    }
+			std::ostringstream debug_text;
+			debug_text<<"for "<<task_inst<<" "<<start_win.first<<" "<<start_win.second<<" "<<end_win.first<<" "<<end_win.second<<std::endl;
+			SA_POP_DEBUG_STR (ANKET, debug_text.str ());
+			debug_text.str("");
 
-	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
-	debug_text.str("");
 
-    if(unranked->empty()) return true;
-  }
-  CLSet after_links = this->get_after(task_inst);
-    
-  TaskInstSet causal,data;
-  for(CLSet::iterator iter=after_links.begin();iter!=after_links.end();iter++)
-  { 
-    if(iter->second==GOAL_TASK_INST_ID || iter->second==NULL_TASK_INST_ID) continue;
-    if(iter->cond.kind!=DATA) causal.insert(iter->second);
-    else data.insert(iter->second);
-  }
-
-  for(TaskInstSet::iterator iter=causal.begin();iter!=causal.end();iter++)
-  {
-	  TaskInstSet *iter_after = &this->precedence_graph_.find(AFTER)->second.find(*iter)->second;
-	  TaskInstSet *iter_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter)->second;
-	  TaskInstSet *iter_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second;
-	  TaskInstSet *iter_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter)->second;
-    if(after->find(*iter)!=after->end()) continue;
+			if(!this->execute(&cmd->min_adj_cmd))
+			{
+				this->undo(&cmd->min_adj_cmd);
+				return false;
+			}
+		}
+	}
+	start_win = this->get_start_window(task_inst);
+	end_win = this->get_end_window(task_inst);
+	TaskInstSet *before,*after,*simul,*unranked;
+	before = &this->precedence_graph_.find(BEFORE)->second.find(task_inst)->second;
+	after = &this->precedence_graph_.find(AFTER)->second.find(task_inst)->second;
+	unranked = &this->precedence_graph_.find(UNRANKED)->second.find(task_inst)->second;
+	simul = &this->precedence_graph_.find(SIMUL)->second.find(task_inst)->second;
+	// If this task instance is not reused, insert all the task instances in it unranked set.
+	if(unranked->empty() && (this->reused_insts_.find(task_inst)== this->reused_insts_.end()))
+	{
+		std::ostringstream debug_text;
 
+		for(PrecedenceSet::iterator iter=this->precedence_graph_.find(BEFORE)->second.begin();iter!=this->precedence_graph_.find(BEFORE)->second.end();iter++)
+		{
+			if(iter->first!=task_inst)
+			{
+				debug_text<<"inserting "<<iter->first<<" in unranked of "<<task_inst<<std::endl;
+				unranked->insert(iter->first);
+			}
+		}
 
-	std::ostringstream debug_text;
-    debug_text<<"inserting "<<*iter<<" in after set of "<<task_inst<<std::endl;
-	  TaskInstSet temp = *before;
-    temp.insert(task_inst);
-    for(TaskInstSet::iterator iter2=simul->begin();iter2!=simul->end();iter2++)
-      temp.insert(*iter2);
-    for(TaskInstSet::iterator iter2=temp.begin();iter2!=temp.end();iter2++)
-    {
-      TaskInstSet *iter2_before,*iter2_after,*iter2_simul,*iter2_unranked;
-      iter2_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter2)->second;
-      iter2_after = &this->precedence_graph_.find(AFTER)->second.find(*iter2)->second;
-      iter2_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter2)->second;
-      iter2_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter2)->second;
-      for(TaskInstSet::iterator iter1=iter_after->begin();iter1!=iter_after->end();iter1++)
-	    {
-		    debug_text<<*iter1<<" is after "<<*iter<<" so it is after "<<*iter2<<std::endl;
-	        TaskInstSet *iter1_before,*iter1_unranked;
-		    iter1_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter1)->second;
-			iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
-		    if(iter1_before->find(*iter2)!=iter1_before->end()) continue;
-			else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter1));
-			iter1_before->insert(*iter2);
-		    iter2_after->insert(*iter1);
-		    if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
-		    if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
-	    }
-	    for(TaskInstSet::iterator iter1=iter_simul->begin();iter1!=iter_simul->end();iter1++)
-	    {
-		    debug_text<<*iter1<<" is siml to "<<*iter<<" so it is after "<<*iter2<<std::endl;
-	        TaskInstSet *iter1_before,*iter1_unranked;
-		    iter1_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter1)->second;
-			iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
-		    if(iter1_before->find(*iter2)!=iter1_before->end()) continue;
-			else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter1));
-		    iter1_before->insert(*iter2);
-		    iter2_after->insert(*iter1);
-		    if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
-		    if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
-	    }
-	    if(iter_before->find(*iter2)!=iter_before->end()) continue;
-		else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter));
-	    iter_before->insert(*iter2);
-	    iter2_after->insert(*iter);
-		if(iter_unranked->find(*iter2)!=iter_unranked->end()) iter_unranked->erase(*iter2);
-	    if(iter2_unranked->find(*iter)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter));
-    }
+		SA_POP_DEBUG_STR (ANKET, debug_text.str ());
+		debug_text.str("");
 
-	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
-	debug_text.str("");
+		if(unranked->empty()) return true;
+	}
+	CLSet after_links = this->get_after(task_inst);
+
+	TaskInstSet causal,data;
+	for(CLSet::iterator iter=after_links.begin();iter!=after_links.end();iter++)
+	{ 
+		if(iter->second==GOAL_TASK_INST_ID || iter->second==NULL_TASK_INST_ID) continue;
+		if(iter->cond.kind!=DATA) causal.insert(iter->second);
+		else data.insert(iter->second);
+	}
 
-  }
-  for(TaskInstSet::iterator iter=data.begin();iter!=data.end();iter++)
-  {
-	  std::ostringstream debug_text;
-
-	  TaskInstSet *iter_after = &this->precedence_graph_.find(AFTER)->second.find(*iter)->second;
-	  TaskInstSet *iter_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter)->second;
-	  TaskInstSet *iter_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second;
-	  TaskInstSet *iter_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter)->second;
-    if(simul->find(*iter)!=simul->end()) continue;
-	  debug_text<<"inserting "<<*iter<<" in simul set of "<<task_inst<<std::endl;
-    TaskInstSet temp = *before;
-    for(TaskInstSet::iterator iter2=temp.begin();iter2!=temp.end();iter2++)
-    {
-		debug_text<<"iter2 is "<<*iter2<<std::endl;
-		TaskInstSet *iter2_before,*iter2_after,*iter2_simul,*iter2_unranked;
-      iter2_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter2)->second;
-      iter2_after = &this->precedence_graph_.find(AFTER)->second.find(*iter2)->second;
-      iter2_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter2)->second;
-      iter2_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter2)->second;
-	    for(TaskInstSet::iterator iter1=iter_after->begin();iter1!=iter_after->end();iter1++)
-	    {
-		    debug_text<<*iter1<<" is after "<<*iter<<" so it is after "<<*iter2<<std::endl;
-	        TaskInstSet *iter1_before,*iter1_unranked;
-		    iter1_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter1)->second;
-			iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
-		    if(iter1_before->find(*iter2)!=iter1_before->end()) continue;
-			else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter1));
-		    iter1_before->insert(*iter2);
-		    iter2_after->insert(*iter1);
-		    if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
-		    if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
-	    }
-	    for(TaskInstSet::iterator iter1=iter_simul->begin();iter1!=iter_simul->end();iter1++)
-	    {
-		    debug_text<<*iter1<<" is simul to "<<*iter<<" so it is after "<<*iter2<<std::endl;
-	        TaskInstSet *iter1_before,*iter1_unranked;
-		    iter1_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter1)->second;
-			iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
-		    if(iter1_before->find(*iter2)!=iter1_before->end()) continue;
-			else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter1));
-		    iter1_before->insert(*iter2);
-		    iter2_after->insert(*iter1);
-		    if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
-		    if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
-	    }    
-	    if(iter_before->find(*iter2)!=iter_before->end()) continue;
-		else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter));
-	    iter_before->insert(*iter2);
-	    iter2_after->insert(*iter);
-		if(iter_unranked->find(*iter2)!=iter_unranked->end()) iter_unranked->erase(*iter2);
-	    if(iter2_unranked->find(*iter)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter));
+	for(TaskInstSet::iterator iter=causal.begin();iter!=causal.end();iter++)
+	{
+		TaskInstSet *iter_after = &this->precedence_graph_.find(AFTER)->second.find(*iter)->second;
+		TaskInstSet *iter_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter)->second;
+		TaskInstSet *iter_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second;
+		TaskInstSet *iter_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter)->second;
+		if(after->find(*iter)!=after->end()) continue;
+
+
+		std::ostringstream debug_text;
+		debug_text<<"inserting "<<*iter<<" in after set of "<<task_inst<<std::endl;
+		TaskInstSet temp = *before;
+		temp.insert(task_inst);
+		for(TaskInstSet::iterator iter2=simul->begin();iter2!=simul->end();iter2++)
+			temp.insert(*iter2);
+		for(TaskInstSet::iterator iter2=temp.begin();iter2!=temp.end();iter2++)
+		{
+			TaskInstSet *iter2_before,*iter2_after,*iter2_simul,*iter2_unranked;
+			iter2_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter2)->second;
+			iter2_after = &this->precedence_graph_.find(AFTER)->second.find(*iter2)->second;
+			iter2_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter2)->second;
+			iter2_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter2)->second;
+			for(TaskInstSet::iterator iter1=iter_after->begin();iter1!=iter_after->end();iter1++)
+			{
+				debug_text<<*iter1<<" is after "<<*iter<<" so it is after "<<*iter2<<std::endl;
+				TaskInstSet *iter1_before,*iter1_unranked;
+				iter1_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter1)->second;
+				iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
+				if(iter1_before->find(*iter2)!=iter1_before->end()) continue;
+				else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter1));
+				iter1_before->insert(*iter2);
+				iter2_after->insert(*iter1);
+				if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
+				if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
+			}
+			for(TaskInstSet::iterator iter1=iter_simul->begin();iter1!=iter_simul->end();iter1++)
+			{
+				debug_text<<*iter1<<" is siml to "<<*iter<<" so it is after "<<*iter2<<std::endl;
+				TaskInstSet *iter1_before,*iter1_unranked;
+				iter1_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter1)->second;
+				iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
+				if(iter1_before->find(*iter2)!=iter1_before->end()) continue;
+				else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter1));
+				iter1_before->insert(*iter2);
+				iter2_after->insert(*iter1);
+				if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
+				if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
+			}
+			if(iter_before->find(*iter2)!=iter_before->end()) continue;
+			else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter));
+			iter_before->insert(*iter2);
+			iter2_after->insert(*iter);
+			if(iter_unranked->find(*iter2)!=iter_unranked->end()) iter_unranked->erase(*iter2);
+			if(iter2_unranked->find(*iter)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter));
+		}
 
 		SA_POP_DEBUG_STR (ANKET, debug_text.str ());
 		debug_text.str("");
+
 	}
+	for(TaskInstSet::iterator iter=data.begin();iter!=data.end();iter++)
+	{
+		std::ostringstream debug_text;
+
+		TaskInstSet *iter_after = &this->precedence_graph_.find(AFTER)->second.find(*iter)->second;
+		TaskInstSet *iter_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter)->second;
+		TaskInstSet *iter_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second;
+		TaskInstSet *iter_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter)->second;
+		if(simul->find(*iter)!=simul->end()) continue;
+		debug_text<<"inserting "<<*iter<<" in simul set of "<<task_inst<<std::endl;
+		TaskInstSet temp = *before;
+		for(TaskInstSet::iterator iter2=temp.begin();iter2!=temp.end();iter2++)
+		{
+			debug_text<<"iter2 is "<<*iter2<<std::endl;
+			TaskInstSet *iter2_before,*iter2_after,*iter2_simul,*iter2_unranked;
+			iter2_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter2)->second;
+			iter2_after = &this->precedence_graph_.find(AFTER)->second.find(*iter2)->second;
+			iter2_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter2)->second;
+			iter2_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter2)->second;
+			for(TaskInstSet::iterator iter1=iter_after->begin();iter1!=iter_after->end();iter1++)
+			{
+				debug_text<<*iter1<<" is after "<<*iter<<" so it is after "<<*iter2<<std::endl;
+				TaskInstSet *iter1_before,*iter1_unranked;
+				iter1_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter1)->second;
+				iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
+				if(iter1_before->find(*iter2)!=iter1_before->end()) continue;
+				else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter1));
+				iter1_before->insert(*iter2);
+				iter2_after->insert(*iter1);
+				if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
+				if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
+			}
+			for(TaskInstSet::iterator iter1=iter_simul->begin();iter1!=iter_simul->end();iter1++)
+			{
+				debug_text<<*iter1<<" is simul to "<<*iter<<" so it is after "<<*iter2<<std::endl;
+				TaskInstSet *iter1_before,*iter1_unranked;
+				iter1_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter1)->second;
+				iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
+				if(iter1_before->find(*iter2)!=iter1_before->end()) continue;
+				else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter1));
+				iter1_before->insert(*iter2);
+				iter2_after->insert(*iter1);
+				if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
+				if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
+			}    
+			if(iter_before->find(*iter2)!=iter_before->end()) continue;
+			else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter));
+			iter_before->insert(*iter2);
+			iter2_after->insert(*iter);
+			if(iter_unranked->find(*iter2)!=iter_unranked->end()) iter_unranked->erase(*iter2);
+			if(iter2_unranked->find(*iter)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter));
+
+			SA_POP_DEBUG_STR (ANKET, debug_text.str ());
+			debug_text.str("");
+		}
 
 
-    temp = *simul;
-    temp.insert(task_inst);
-    for(TaskInstSet::iterator iter2=temp.begin();iter2!=temp.end();iter2++)
-    {
-	  std::ostringstream debug_text;
-
-      TaskInstSet *iter2_before,*iter2_after,*iter2_simul,*iter2_unranked;
-      iter2_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter2)->second;
-      iter2_after = &this->precedence_graph_.find(AFTER)->second.find(*iter2)->second;
-      iter2_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter2)->second;
-      iter2_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter2)->second;
-	    for(TaskInstSet::iterator iter1=iter_after->begin();iter1!=iter_after->end();iter1++)
-	    {
-		    debug_text<<*iter1<<" is after "<<*iter<<" so it is after "<<*iter2<<std::endl;
-	        TaskInstSet *iter1_before,*iter1_unranked;
-		    iter1_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter1)->second;
-			iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
-		    if(iter1_before->find(*iter2)!=iter1_before->end()) continue;
-			else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter1));
-		    iter1_before->insert(*iter2);
-		    iter2_after->insert(*iter1);
-		    if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
-		    if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
-	    }
-	    for(TaskInstSet::iterator iter1=iter_simul->begin();iter1!=iter_simul->end();iter1++)
-	    {
-		    debug_text<<*iter1<<" is simul "<<*iter<<" so it is simul "<<*iter2<<std::endl;
-	        TaskInstSet *iter1_simul,*iter1_unranked;
-		    iter1_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter1)->second;
-			iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
-		    if(iter1_simul->find(*iter2)!=iter1_simul->end()) continue;
-			else 
+		temp = *simul;
+		temp.insert(task_inst);
+		for(TaskInstSet::iterator iter2=temp.begin();iter2!=temp.end();iter2++)
+		{
+			std::ostringstream debug_text;
+
+			TaskInstSet *iter2_before,*iter2_after,*iter2_simul,*iter2_unranked;
+			iter2_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter2)->second;
+			iter2_after = &this->precedence_graph_.find(AFTER)->second.find(*iter2)->second;
+			iter2_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter2)->second;
+			iter2_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter2)->second;
+			for(TaskInstSet::iterator iter1=iter_after->begin();iter1!=iter_after->end();iter1++)
 			{
-				cmd->simul_insertions.insert(std::make_pair(*iter2,*iter1));
+				debug_text<<*iter1<<" is after "<<*iter<<" so it is after "<<*iter2<<std::endl;
+				TaskInstSet *iter1_before,*iter1_unranked;
+				iter1_before = &this->precedence_graph_.find(BEFORE)->second.find(*iter1)->second;
+				iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
+				if(iter1_before->find(*iter2)!=iter1_before->end()) continue;
+				else cmd->causal_insertions.insert(std::make_pair(*iter2,*iter1));
+				iter1_before->insert(*iter2);
+				iter2_after->insert(*iter1);
+				if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
+				if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
+			}
+			for(TaskInstSet::iterator iter1=iter_simul->begin();iter1!=iter_simul->end();iter1++)
+			{
+				debug_text<<*iter1<<" is simul "<<*iter<<" so it is simul "<<*iter2<<std::endl;
+				TaskInstSet *iter1_simul,*iter1_unranked;
+				iter1_simul = &this->precedence_graph_.find(SIMUL)->second.find(*iter1)->second;
+				iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
+				if(iter1_simul->find(*iter2)!=iter1_simul->end()) continue;
+				else 
+				{
+					cmd->simul_insertions.insert(std::make_pair(*iter2,*iter1));
+				}
+				iter1_simul->insert(*iter2);
+				iter2_simul->insert(*iter1);
+				if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
+				if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
+			}
+			for(TaskInstSet::iterator iter1=iter_before->begin();iter1!=iter_before->end();iter1++)
+			{
+				debug_text<<*iter1<<" is before "<<*iter<<" so it is before "<<*iter2<<std::endl;
+				TaskInstSet *iter1_after,*iter1_unranked;
+				iter1_after = &this->precedence_graph_.find(AFTER)->second.find(*iter1)->second;
+				iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
+				if(iter1_after->find(*iter2)!=iter1_after->end()) continue;
+				else cmd->causal_insertions.insert(std::make_pair(*iter1,*iter2));
+				iter1_after->insert(*iter2);
+				iter2_before->insert(*iter1);
+				if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
+				if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
 			}
-			iter1_simul->insert(*iter2);
-		    iter2_simul->insert(*iter1);
-		    if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
-		    if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
-	    }
-	    for(TaskInstSet::iterator iter1=iter_before->begin();iter1!=iter_before->end();iter1++)
-	    {
-		    debug_text<<*iter1<<" is before "<<*iter<<" so it is before "<<*iter2<<std::endl;
-	        TaskInstSet *iter1_after,*iter1_unranked;
-		    iter1_after = &this->precedence_graph_.find(AFTER)->second.find(*iter1)->second;
-			iter1_unranked = &this->precedence_graph_.find(UNRANKED)->second.find(*iter1)->second;
-		    if(iter1_after->find(*iter2)!=iter1_after->end()) continue;
-			else cmd->causal_insertions.insert(std::make_pair(*iter1,*iter2));
-		    iter1_after->insert(*iter2);
-		    iter2_before->insert(*iter1);
-		    if(iter1_unranked->find(*iter2)!=iter1_unranked->end()) iter1_unranked->erase(*iter2);
-		    if(iter2_unranked->find(*iter1)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter1));
-	    }
 
-		SA_POP_DEBUG_STR (ANKET, debug_text.str ());
-		debug_text.str("");
+			SA_POP_DEBUG_STR (ANKET, debug_text.str ());
+			debug_text.str("");
+
+			if(iter_simul->find(*iter2)!=iter_simul->end()) continue;
+			else cmd->simul_insertions.insert(std::make_pair(*iter2,*iter));
+			iter2_simul->insert(*iter);
+			iter_simul->insert(*iter2);
+			if(iter_unranked->find(*iter2)!=iter_unranked->end()) iter_unranked->erase(*iter2);
+			if(iter2_unranked->find(*iter)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter));
+		}
+	}
 
-	    if(iter_simul->find(*iter2)!=iter_simul->end()) continue;
-		else cmd->simul_insertions.insert(std::make_pair(*iter2,*iter));
-	    iter2_simul->insert(*iter);
-	    iter_simul->insert(*iter2);
-		if(iter_unranked->find(*iter2)!=iter_unranked->end()) iter_unranked->erase(*iter2);
-	    if(iter2_unranked->find(*iter)!=iter2_unranked->end()) iter2_unranked->erase(iter2_unranked->find(*iter));
-    }
-  }
-
-
-  debug_text<<"for "<<task_inst<<" start window: "<<start_win.first<<"-"<<start_win.second<<" and end window "<<end_win.first<<"-"<<end_win.second<<std::endl;
-
-  
-  for(TaskInstSet::iterator iter = unranked->begin();iter!=unranked->end();iter++)
-  {
-	  TimeWindow temp_start = this->get_start_window(*iter);
-	  TimeWindow temp_end = this->get_end_window(*iter);
-	  debug_text<<"checking "<<*iter<<" in all with start window: "<<temp_start.first<<"-"<<temp_start.second<<" and end window "<<temp_end.first<<"-"<<temp_end.second<<std::endl;
-	  if(end_win.second!=NULL_TIME && temp_start.first>end_win.second)
-	  {
-		  after->insert(*iter);
-		  this->precedence_graph_.find(BEFORE)->second.find(*iter)->second.insert(task_inst);
-	  }
-	  else if(temp_end.second!=NULL_TIME && temp_end.second<start_win.first)
-	  {
-		  before->insert(*iter);
-		  this->precedence_graph_.find(AFTER)->second.find(*iter)->second.insert(task_inst);
-	  }
-	  else
-	  {
-		  this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second.insert(task_inst);
-	  }
-
-
-  }
-
-   SA_POP_DEBUG_STR (ANKET, debug_text.str ());
-  debug_text.str("");
+
+	debug_text<<"for "<<task_inst<<" start window: "<<start_win.first<<"-"<<start_win.second<<" and end window "<<end_win.first<<"-"<<end_win.second<<std::endl;
+
+
+	for(TaskInstSet::iterator iter = unranked->begin();iter!=unranked->end();iter++)
+	{
+		TimeWindow temp_start = this->get_start_window(*iter);
+		TimeWindow temp_end = this->get_end_window(*iter);
+		debug_text<<"checking "<<*iter<<" in all with start window: "<<temp_start.first<<"-"<<temp_start.second<<" and end window "<<temp_end.first<<"-"<<temp_end.second<<std::endl;
+		if(end_win.second!=NULL_TIME && temp_start.first>end_win.second)
+		{
+			after->insert(*iter);
+			this->precedence_graph_.find(BEFORE)->second.find(*iter)->second.insert(task_inst);
+		}
+		else if(temp_end.second!=NULL_TIME && temp_end.second<start_win.first)
+		{
+			before->insert(*iter);
+			this->precedence_graph_.find(AFTER)->second.find(*iter)->second.insert(task_inst);
+		}
+		else
+		{
+			this->precedence_graph_.find(UNRANKED)->second.find(*iter)->second.insert(task_inst);
+		}
+
+
+	}
+
+	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
+	debug_text.str("");
 
 	this->print_prec();
 
-  for(InstToWinMap::iterator iter=this->start_window_map_.begin();iter!=this->start_window_map_.end();iter++)
-  {
-    debug_text<<"The start window for "<<iter->first<<" is "<<iter->second.first<<" - "<<iter->second.second<<std::endl;
-  }
-  for(InstToWinMap::iterator iter=this->end_window_map_.begin();iter!=this->end_window_map_.end();iter++)
-  {
-    debug_text<<"The end window for "<<iter->first<<" is "<<iter->second.first<<" - "<<iter->second.second<<std::endl;
-  }
+	for(InstToWinMap::iterator iter=this->start_window_map_.begin();iter!=this->start_window_map_.end();iter++)
+	{
+		debug_text<<"The start window for "<<iter->first<<" is "<<iter->second.first<<" - "<<iter->second.second<<std::endl;
+	}
+	for(InstToWinMap::iterator iter=this->end_window_map_.begin();iter!=this->end_window_map_.end();iter++)
+	{
+		debug_text<<"The end window for "<<iter->first<<" is "<<iter->second.first<<" - "<<iter->second.second<<std::endl;
+	}
 
 	SA_POP_DEBUG_STR (SA_POP_DEBUG_NORMAL, debug_text.str ());
 	debug_text.str("");
 
-  return true;
+	return true;
 };
 
 // Print the precedence graph
@@ -2275,12 +1983,12 @@ void SA_WorkingPlan::print_prec()
 // Undo all the updation to the precedence graph due to the chosen task instance
 void SA_WorkingPlan::prec_erase(TaskInstID task_inst, SA_AssocTaskImplCmd *cmd)
 {
-PrecedenceSet *before = &this->precedence_graph_.find(BEFORE)->second;
-PrecedenceSet *simul = &this->precedence_graph_.find(SIMUL)->second;
-PrecedenceSet *after = &this->precedence_graph_.find(AFTER)->second;
-PrecedenceSet *unranked = &this->precedence_graph_.find(UNRANKED)->second;
+	PrecedenceSet *before = &this->precedence_graph_.find(BEFORE)->second;
+	PrecedenceSet *simul = &this->precedence_graph_.find(SIMUL)->second;
+	PrecedenceSet *after = &this->precedence_graph_.find(AFTER)->second;
+	PrecedenceSet *unranked = &this->precedence_graph_.find(UNRANKED)->second;
 
-std::ostringstream debug_text;
+	std::ostringstream debug_text;
 
 	for(std::set< std::pair<TaskInstID,TaskInstID> >::iterator iter = cmd->causal_insertions.begin();iter!=cmd->causal_insertions.end();iter++)
 	{
@@ -2300,32 +2008,23 @@ std::ostringstream debug_text;
 		unranked->find(iter->first)->second.insert(iter->second);
 		unranked->find(iter->second)->second.insert(iter->first);
 	}
-	//if(before->find(task_inst)->second.empty() && after->find(task_inst)->second.empty() && simul->find(task_inst)->second.empty())
-	//{
-	//	CondToCLinksMap::iterator iter;
-	//	for(iter = this->causal_links_.begin();iter!=this->causal_links_.end();iter++)
-	//		if(iter->second.second == GOAL_TASK_INST_ID && iter->second.first == task_inst) break;
-	//	if(iter==this->causal_links_.end() || cmd->impls_.size()!=0)
-	//	{
+
 	debug_text<<this->reused_insts_.size()<<std::endl;
 	if(this->reused_insts_.find(task_inst)==this->reused_insts_.end())
 	{
-			before->erase(before->find(task_inst));
-			after->erase(after->find(task_inst));
-			simul->erase(simul->find(task_inst));
-			unranked->erase(unranked->find(task_inst));
-			this->start_window_map_.erase(this->start_window_map_.find(task_inst));
-			this->end_window_map_.erase(this->end_window_map_.find(task_inst));
-			for(PrecedenceSet::iterator iter1 = unranked->begin();iter1!=unranked->end();iter1++)
-				iter1->second.erase(task_inst);
-		}
+		before->erase(before->find(task_inst));
+		after->erase(after->find(task_inst));
+		simul->erase(simul->find(task_inst));
+		unranked->erase(unranked->find(task_inst));
+		this->start_window_map_.erase(this->start_window_map_.find(task_inst));
+		this->end_window_map_.erase(this->end_window_map_.find(task_inst));
+		for(PrecedenceSet::iterator iter1 = unranked->begin();iter1!=unranked->end();iter1++)
+			iter1->second.erase(task_inst);
+	}
 
 	SA_POP_DEBUG_STR (ANKET, debug_text.str ());
 	debug_text.str("");
 
-//	}
-	//std::cout<<"after erasing the precedence graph is "<<std::endl;
-	//this->print_prec();
 };
 /// Get all the task instances
 TaskInstSet SA_WorkingPlan::get_all_insts()
@@ -2335,18 +2034,18 @@ TaskInstSet SA_WorkingPlan::get_all_insts()
 	{
 		all.insert(iter->first);
 	}	  
-  return all;
+	return all;
 }
 /// Check if the instance id already exists and is being reused.
 bool SA_WorkingPlan::inst_exists (TaskInstID task_inst)
 {
-  if(this->task_impls_.find(task_inst)!=this->task_impls_.end()) return true;
-  else return false;
+	if(this->task_impls_.find(task_inst)!=this->task_impls_.end()) return true;
+	else return false;
 }
 
 /// Get task implementation for a task instance.
 TaskImplID SA_WorkingPlan::get_impl_id (TaskInstID task_inst)
 {
-  return this->task_impls_.find(task_inst)->second;
+	return this->task_impls_.find(task_inst)->second;
 }
 
diff --git a/SA_WorkingPlan.h b/SA_WorkingPlan.h
index f0a4d559b3c..a5b0a6c20bd 100644
--- a/SA_WorkingPlan.h
+++ b/SA_WorkingPlan.h
@@ -365,7 +365,6 @@ namespace SA_POP {
     // State information.
     // ************************************************************************
 
-    //Ben's.  Kill if it breaks this
     CLThreatSet threat_set;
 
     /// Flag for whether command prototypes have been set.
@@ -435,8 +434,6 @@ namespace SA_POP {
 	// The set of reused task instances
 	std::multiset<TaskInstID> reused_insts_;
 
-  //I can't use sched_links_ because something craps itself when it gets a link
-  //and the tasks don't have windows.  I hate it too.
   SchedulingLinks ordering_links;
   //useful for doing the loop detection algorithm
   SchedulingLinks reverse_ordering_links;
diff --git a/utils/PDDLtoSANetTranslator/PDDLParser/ferry-example.pddl b/utils/PDDLtoSANetTranslator/PDDLParser/ferry-example.pddl
index 6ec6d060530..f104753e7aa 100644
--- a/utils/PDDLtoSANetTranslator/PDDLParser/ferry-example.pddl
+++ b/utils/PDDLtoSANetTranslator/PDDLParser/ferry-example.pddl
@@ -2,29 +2,35 @@
   (:requirements :strips :equality :typing)
 
   (:types auto place ferry)
-  (:constants the-ferry - ferry)
-  (:predicates (at-ferry ?l - place)
+  (:predicates (at-ferry ?f - ferry ?l - place)
 	       (at ?x - auto
 		   ?y - place)
-	       (empty-ferry)
+	       (empty-ferry ?f - ferry)
 	       (on ?x - auto
-		   ?f - ferry))
+		   ?f - ferry)
+		   (can-sail ?f - ferry
+		   ?x - place
+		   ?y - place
+		   )
+		   (can-debark-at ?x - place)
+		   (can-board-at ?x - place)
+	)
 
   (:action board
-	     :parameters (?x - auto ?y - place)
-	     :precondition (and (at ?x ?y)(at-ferry ?y)(empty-ferry))
+	     :parameters (?f - ferry ?x - auto ?y - place)
+	     :precondition (and (at ?x ?y)(at-ferry ?f ?y)(empty-ferry ?f)(can-board-at ?y))
 	     :effect 
-	     (and (on ?x the-ferry)
+	     (and (on ?x ?f)
 		   (not (at ?x ?y))
-		   (not (empty-ferry))))
+		   (not (empty-ferry ?f))))
   (:action sail
-	     :parameters (?x ?y - place)
-	     :precondition (and (at-ferry ?x) (not (= ?x ?y)))
-	     :effect (and (at-ferry ?y)
-			   (not (at-ferry ?x))))
+	     :parameters (?f - ferry ?x ?y - place)
+	     :precondition (and (at-ferry ?f ?x) (not (= ?x ?y)) (can-sail ?f ?x ?y))
+	     :effect (and (at-ferry ?f ?y)
+			   (not (at-ferry ?f ?x))))
   (:action debark
-	     :parameters (?x - auto ?y - place)
-	     :precondition (and (on ?x the-ferry)(at-ferry ?y))
-	     :effect (and (not (on ?x the-ferry))
+	     :parameters (?f - ferry ?x - auto ?y - place)
+	     :precondition (and (on ?x ?f)(at-ferry ?f ?y) (can-debark-at ?y))
+	     :effect (and (not (on ?x ?f))
 			   (at ?x ?y)
-			   (empty-ferry))))
\ No newline at end of file
+			   (empty-ferry ?f))))
\ No newline at end of file
diff --git a/utils/PDDLtoSANetTranslator/PDDLParser/ferry-problem-example.pddl b/utils/PDDLtoSANetTranslator/PDDLParser/ferry-problem-example.pddl
index 75bda9230b2..a4470e77c0a 100644
--- a/utils/PDDLtoSANetTranslator/PDDLParser/ferry-problem-example.pddl
+++ b/utils/PDDLtoSANetTranslator/PDDLParser/ferry-problem-example.pddl
@@ -1,11 +1,31 @@
 (define (problem ferry1) 
 	(:domain ferry)
 	(:requirements :strips :equality :typing)
-	(:objects a b  - place 
-		c1 c2 - auto)
-    (:init (at c1 a)
+	(:objects a b c - place 
+		c1 c2 - auto
+		f1 f2 - ferry)
+    (:init 
+	   (at c1 a)
 	   (at c2 a)
-	   (at-ferry a)
-	   (empty-ferry))  
-  (:goal (and (at c1 b) (at c2 b)))
+	   
+	   (at-ferry f1 a)
+	   (empty-ferry f1)  
+	   (can-sail f1 b c)
+	   (can-sail f1 c b)
+	   (can-sail f1 a b)
+	   (can-sail f1 b a)
+	   
+	   (at-ferry f2 b)
+	   (empty-ferry f2)
+	   (can-sail f2 b c)
+	   (can-sail f2 c b)
+	   
+	   (can-debark-at b)
+	   (can-debark-at c)
+	   
+	   (can-board-at a)
+	   (can-board-at b)
+	   
+	   )
+  (:goal (and (at c1 c) (at c2 c)))
 )
\ No newline at end of file
diff --git a/utils/PDDLtoSANetTranslator/PDDLParser/src/PDDLtoSAN.java b/utils/PDDLtoSANetTranslator/PDDLParser/src/PDDLtoSAN.java
index 2d1184192f8..7ed80fbefe8 100644
--- a/utils/PDDLtoSANetTranslator/PDDLParser/src/PDDLtoSAN.java
+++ b/utils/PDDLtoSANetTranslator/PDDLParser/src/PDDLtoSAN.java
@@ -20,22 +20,23 @@ public class PDDLtoSAN {
 	 */
 	public static void main(String[] args) {
 		
-		if(args.length != 5){
-			System.out.println("Wrong number of arguments: "+args.length);
-			System.exit(0);
-		}
-
-		String domain_path = args[0];
-		String problem_path = args[1];
-		boolean include_init = new Boolean(args[2]);
-		int cond_combine = new Integer(args[3]);
-		String output_name = args[4];
 		
-//		String domain_path = "ferry.pddl";
-//		String problem_path = "pb1ferry.pddl";
-//		boolean include_init = false;
-//		int cond_combine = 3;
-//		String output_name = "ferry";
+//		if(args.length != 5){
+//			System.out.println("Wrong number of arguments: "+args.length);
+//			System.exit(0);
+//		}
+//
+//		String domain_path = args[0];
+//		String problem_path = args[1];
+//		boolean include_init = new Boolean(args[2]);
+//		int cond_combine = new Integer(args[3]);
+//		String output_name = args[4];
+		
+		String domain_path = "hanoi.pddl";
+		String problem_path = "hanoi-pb1.pddl";
+		boolean include_init = false;
+		int cond_combine = 4;
+		String output_name = "ferry-orig-pruned";
 		
 		Domain domain = null;
 		Problem problem = null;
diff --git a/utils/PDDLtoSANetTranslator/PDDLParser/src/Translator/PDDL_Translator.java b/utils/PDDLtoSANetTranslator/PDDLParser/src/Translator/PDDL_Translator.java
index b31a49dd80b..05192efcf31 100644
--- a/utils/PDDLtoSANetTranslator/PDDLParser/src/Translator/PDDL_Translator.java
+++ b/utils/PDDLtoSANetTranslator/PDDLParser/src/Translator/PDDL_Translator.java
@@ -85,6 +85,8 @@ public class PDDL_Translator {
 	 */
 	public PDDL_Translator(PDDLObject pddl_object, int condition_combine_levels, boolean use_initial_action){
 		
+		long start_time = new Date().getTime();
+		
 		init_act_visible = use_initial_action;
 		
 		List<Action> action_list;
@@ -122,6 +124,8 @@ public class PDDL_Translator {
         //Go through and look at the equality/inequality constraints remaining.  
         action_data = parse_equality_constraints(action_list);
         
+       
+        
         //Make sure that there aren't untranslatable probabilities encoded
         if(!are_probabilistics_translatable(action_list)){
         	System.out.println("Probabilistics this complex cannot currently be translated to a SANet");
@@ -130,11 +134,22 @@ public class PDDL_Translator {
 
         //Propositionalize all actions
         enumerate_actions(action_data);
+        
+
                
         //Add the initial action in as an anchor--otherwise it would seem that none
         //of the conditions can come true
         setup_initial_action(init_act);
         
+        System.out.println();
+        System.out.println("Initial Network Statistics: ");
+        System.out.println();
+        calculateNetworkStatistics();
+        System.out.println();
+        
+        System.out.println("Before optimizing: "+ this.task_nodes.size()+" tasks "+ this.condition_nodes.size()+" conditions");
+        System.out.println("Elapsed: "+ (new Date().getTime()-start_time));
+        
         boolean keep_looping;
         do {
         	
@@ -145,17 +160,31 @@ public class PDDL_Translator {
 	        //Basically cut down through the tasks and conditions and only take the relevant
 	        //ones 
 	        keep_looping = eliminate_irreleevant_tasks_and_conditions()||keep_looping;
-	        
 	        //Get rid of tasks that take plan backwards and where getting there
 	        //helps nothing else
-	        keep_looping = eliminate_dominated_tasks()||keep_looping;
+	 //       keep_looping = eliminate_dominated_tasks()||keep_looping;
         } while(keep_looping);
         
         //Go through and look to see if any conditions can be replaced by a set of other
         //conditions
         //TODO maybe try something like a kmap here?
+        
+        System.out.println();
+        System.out.println("Network Statistics Before Condition Combining: ");
+        System.out.println();
+        calculateNetworkStatistics();
+        System.out.println();
+        
         combine_conditions(condition_combine_levels);
         
+        System.out.println();
+        System.out.println("Network Statistics After Condition Combining: ");
+        System.out.println();
+        calculateNetworkStatistics();
+        System.out.println();
+        
+        System.out.println("End elapsed: "+ (new Date().getTime()-start_time));
+        
         //Give preconditions links the proper probabilities
         set_probabilities();
         
@@ -163,6 +192,143 @@ public class PDDL_Translator {
         record_task_impls();
 	}
 	
+	private void calculateNetworkStatistics(){
+		
+		System.out.println("Number of conditions: "+this.condition_nodes.size());
+		System.out.println("Number of tasks: "+ this.task_nodes.size());
+		
+		
+		int total_precond_links = 0;
+		int num_sat_links = 0;
+		
+		int num_threat_links = 0;
+		
+		int total_effect_links = 0;
+		
+		int[] effects_per_cond = new int[condition_nodes.size()];
+		int[] preconds_per_cond = new int[condition_nodes.size()];
+		int[] effects_per_task = new int[task_nodes.size()-1];
+		int[] preconds_per_task = new int[task_nodes.size()-1];
+		
+		List<Integer> ways_to_sat_each_precond = new LinkedList<Integer>();
+		List<Integer> threats_to_each_precond = new LinkedList<Integer>();
+		
+		int count = 0;
+		for(CondNode cnd: this.condition_nodes.values()){
+
+			int num_pos_effect = 0;
+			int num_neg_effect = 0;
+			
+			for(EffectLink el: cnd.getAffectedBy()){
+				
+				if(el.getTaskID().equals("20")){
+					continue;
+				}
+				
+				if(el.getWeight() == 1){
+					num_pos_effect++;
+				}else if(el.getWeight() == -1){
+					num_neg_effect++;
+				}
+				
+				total_effect_links++;
+			}
+			
+			for(PrecondLink pl: cnd.getPreconditionTo()){
+				if(pl.getRequiredState()){
+					num_sat_links+=num_pos_effect;
+					ways_to_sat_each_precond.add(num_pos_effect);
+					
+					num_threat_links+=num_neg_effect;
+					threats_to_each_precond.add(num_neg_effect);
+				}else{
+					num_sat_links+=num_neg_effect;
+					ways_to_sat_each_precond.add(num_neg_effect);
+					
+					num_threat_links+=num_neg_effect;
+					threats_to_each_precond.add(num_pos_effect);
+				}
+				
+				total_precond_links++;
+			}
+			
+			preconds_per_cond[count] = cnd.getPreconditionTo().size();
+			
+			effects_per_cond[count] = cnd.getAffectedBy().size();
+			
+			count++;
+		}
+		
+		count = 0;
+		for(TaskNode tnd: this.task_nodes.values()){
+			
+			if(tnd.getName().equals("initact")){
+				continue;
+			}
+			
+			effects_per_task[count] = tnd.get_negative_effect_links().size()+tnd.get_positive_effect_links().size();
+			preconds_per_task[count] = tnd.get_positive_precondition_links().size()+tnd.get_negative_precondition_links().size();
+			count++;
+		}
+		
+		double avg_effect_each_cond = total_effect_links/(double)condition_nodes.size();
+		double avg_precond_each_cond = total_precond_links/(double)condition_nodes.size();
+		double avg_effect_each_act = total_effect_links/(double)task_nodes.size();
+		double avg_precond_each_act = total_effect_links/(double)task_nodes.size();
+		double avg_ways_to_sat_precond = num_sat_links/(double)total_precond_links;
+		double avg_threats_per_precond = num_threat_links/(double)total_precond_links;
+		
+		double stdev_effects_per_cond = get_std_dev(avg_effect_each_cond, effects_per_cond);
+		double stdev_precond_per_cond = get_std_dev(avg_precond_each_cond, preconds_per_cond);
+		double stdev_effects_per_task = get_std_dev(avg_effect_each_act, effects_per_task);
+		double stdev_precond_per_task = get_std_dev(avg_precond_each_act, preconds_per_task);
+		double stdev_ways_to_sat_precond = get_std_dev(avg_ways_to_sat_precond, ways_to_sat_each_precond);
+		double stdev_threats_pre_precond = get_std_dev(avg_threats_per_precond, threats_to_each_precond);
+		
+		System.out.println("Avg effects per cond: "+ avg_effect_each_cond);
+		System.out.println("  std dev: "+stdev_effects_per_cond);
+		System.out.println("Avg precond per cond:" + avg_precond_each_cond);
+		System.out.println("  std dev: "+stdev_precond_per_cond);
+		System.out.println("Avg effects per act: " + avg_effect_each_act);
+		System.out.println("  std dev: "+stdev_effects_per_task);
+		System.out.println("Avg precond per act: " + avg_precond_each_act);
+		System.out.println("  std dev: "+stdev_precond_per_task);
+		System.out.println("Avg ways to satisfy precond: "+ avg_ways_to_sat_precond);
+		System.out.println("  std dev: "+stdev_ways_to_sat_precond);
+		System.out.println("Avg threats to precond: "+avg_threats_per_precond);
+		System.out.println("  std dev: "+stdev_threats_pre_precond);
+		
+		//*Num conds						STD
+		//*Num actions						STD
+		//*Average effects to each cond		STD
+		//*Average precond each cond		STD
+		//*average effects each task		STD
+		//*average precond each task		STD
+		
+		//Average #ways to satisfy each precond
+		//diameter!
+		
+	}
+	
+	private double get_std_dev(double mean, int[] data){
+
+		double sum_squares = 0;
+		for(int i = 0; i < data.length; i++){
+			sum_squares+=Math.pow(mean-data[i], 2);
+		}
+		double avg = sum_squares/data.length;
+		return Math.sqrt(avg);
+	}
+	
+	private double get_std_dev(double mean, List<Integer> data){
+		double sum_squares = 0;
+		for(Integer theint: data){
+			sum_squares+=Math.pow(mean-theint, 2);
+		}
+		double avg = sum_squares/data.size();
+		return Math.sqrt(avg);
+	}
+	
 	MultiMap opp_effect_same_task;
 	MultiMap common_task;
 	
@@ -257,7 +423,6 @@ public class PDDL_Translator {
 		}
 		
         long intv2 = new Date().getTime();
-        System.out.println("Combining conditions: "+(intv2-intv1)+" ms");
 	}
 	
 	private boolean combine_conditions_aux(List<CondNode> fixed_cnds, int still_to_fix){
@@ -273,7 +438,6 @@ public class PDDL_Translator {
 					for(CondNode nd: fixed_cnds)
 					{
 						if(cln.contains(nd)){
-				//			System.out.println("Skip");
 							skip = true;
 							break;
 						}
@@ -437,6 +601,74 @@ public class PDDL_Translator {
 		boolean keep_looking = true;
 		boolean removed_something = false;
 		
+		
+		//Find all "purely consumed" conditions
+		
+		
+		Set<CondNode> pos_only_consumed = new HashSet<CondNode>();
+		Set<CondNode> neg_only_consumed = new HashSet<CondNode>();
+		
+		for(CondNode cnd: this.condition_nodes.values()){
+			//Check the positives
+			
+			//If every positive precondition link has a corresponding negative 
+			//effect from the same task
+			
+			boolean unmatched_pos_precond = false;
+			boolean unmatched_neg_precond = false;
+			
+			for(PrecondLink prl: cnd.getPreconditionTo()){
+				
+				if(prl.getRequiredState()){
+					//If Positive
+					
+					boolean found_neg_match = false;
+					
+					for(EffectLink el: prl.action.get_negative_effect_links()){
+						if(el.effect == cnd){
+							found_neg_match = true;
+							break;
+						}
+					}
+					
+					if(!found_neg_match){
+						unmatched_pos_precond = true;
+					}
+					
+				}else{
+					//If negative
+					
+					boolean found_pos_match = false;
+					
+					for(EffectLink el: prl.action.get_positive_effect_links()){
+						if(el.effect == cnd){
+							found_pos_match = true;
+							break;
+						}	
+					}
+					
+					if(!found_pos_match){
+						unmatched_neg_precond = true;
+					}
+				}
+			}
+			
+			if(!unmatched_neg_precond){
+				System.out.println("No unmatched neg "+cnd);
+				neg_only_consumed.add(cnd);
+			}
+			
+			if(!unmatched_pos_precond){
+				System.out.println("No unmatched pos "+cnd);
+				pos_only_consumed.add(cnd);
+			}
+			
+			//check the negatives
+			
+			//If every negative precondition link has a corresponding positive 
+			//effect from the same task
+		}
+		
 		while(keep_looking){
 		
 			for(TaskNode t: this.task_nodes.values()){
@@ -505,13 +737,17 @@ public class PDDL_Translator {
 					cond_list.add(c);
 				}
 				
+		//		stored_sat_precond = null;
 				if(expand_satisfaction_combination(cond_list, cond_to_satisfiers, satisfiers_precond,
-						satisfiers_effects, new LinkedList<TaskNode>(), useful_effects)){
+						satisfiers_effects, new LinkedHashMap<CondNode, TaskNode>(), useful_effects)){
 					
 					Map<CondNode, Boolean> all_sat_precond = new LinkedHashMap<CondNode, Boolean>();
-					for(TaskNode tn: stored_satisfiers_precond.keySet()){
-						all_sat_precond.putAll(stored_satisfiers_precond.get(tn));
-					}
+//					for(CondNode cnd: stored_sat_precond.keySet()){
+//						all_sat_precond.putAll(stored_sat_precond.get(cnd));
+//					}
+			//		for(CondNode cnd: all_sat_precond.keySet()){
+		
+			//		}
 					
 					//Note! bah
 					boolean all_necessary_preconditions_extraneous = true;
@@ -527,20 +763,49 @@ public class PDDL_Translator {
 					}
 	//				
 					if(all_necessary_preconditions_extraneous){
-						System.out.println("  Deleting"+t.getName());
-		//				System.out.println(t.getName()+" definitely extraneous");
+		
+				//		to_remove.add(t);
+					}
+					
+					
+					Map<CondNode, Boolean> pruned_preconditions = 
+						new LinkedHashMap<CondNode, Boolean>();
+					
+					for(CondNode cnd: preconditions.keySet()){
+						if(!(all_sat_precond.containsKey(cnd)&&all_sat_precond.get(cnd)==preconditions.get(cnd))){
+							pruned_preconditions.put(cnd, preconditions.get(cnd));
+	
+						}
+					}
+					
+					boolean all_preconds_consumed = true;
+					
+					for(CondNode cnd: pruned_preconditions.keySet()){
+						if(pruned_preconditions.get(cnd)){
+							if(!pos_only_consumed.contains(cnd)){
+								all_preconds_consumed = false;
+							}
+						}else{
+							if(!neg_only_consumed.contains(cnd)){
+								all_preconds_consumed = false;
+							}
+						}
+					}
+					
+					if(all_preconds_consumed){
+		
 						to_remove.add(t);
 					}
 					
 	//				for(EffectLink l: t.get_positive_effect_links()){
 	//					if(!this.is_effect_useful(l)){
-	//						System.out.println("Kill: "+t.getName());
+	//		
 	//					}
 	//				}
 	//				
 	//				for(EffectLink l: t.get_negative_effect_links()){
 	//					if(!this.is_effect_useful(l)){
-	//						System.out.println("Kill: "+t.getName());
+	//					
 	//					}
 	//				}
 					
@@ -550,7 +815,7 @@ public class PDDL_Translator {
 	//				for(CondNode cn: useful_effects.keySet()){
 	//					for(PrecondLink l: cn.getPreconditionTo()){
 	//						if(l.required_state == useful_effects.get(cn)){
-	//							System.out.println("  actually, "+ l);
+	//				
 	//							num_clinks++;
 	//						}
 	//					}
@@ -611,14 +876,14 @@ public class PDDL_Translator {
 
 	}
 
-	Map<TaskNode, Map<CondNode, Boolean>> stored_satisfiers_precond;
+//	Map<CondNode, Map<CondNode, Boolean>> stored_sat_precond;
 	
 	private boolean expand_satisfaction_combination(
 			List<CondNode> not_satisfied,
 			Map<CondNode, Set<TaskNode>> cond_to_satisfiers, 
 			Map<TaskNode, Map<CondNode, Boolean>> satisfiers_precond,
 			Map<TaskNode, Map<CondNode, Boolean>> satisfiers_effects,
-			List<TaskNode> added,
+			Map<CondNode, TaskNode> added,
 			Map<CondNode, Boolean> act_effects
 			){
 		
@@ -632,9 +897,8 @@ public class PDDL_Translator {
 				List<CondNode> new_to_sat = new LinkedList<CondNode>(not_satisfied);
 				new_to_sat.remove(sat_cond);
 				
-				List<TaskNode> fixed_tasks = new LinkedList<TaskNode>(added);
-				fixed_tasks.add(tn);
-				
+				Map<CondNode, TaskNode> fixed_tasks = new LinkedHashMap<CondNode, TaskNode>(added);
+				fixed_tasks.put(sat_cond, tn);
 				
 				boolean toret = expand_satisfaction_combination(
 					new_to_sat,
@@ -653,66 +917,174 @@ public class PDDL_Translator {
 			return true;
 			
 		}else{
+
 			
-			Map<CondNode, Boolean> all_sat_preconds = new LinkedHashMap<CondNode, Boolean>();
+			//0.  Make this map new so we can modify it
 			
-			for(TaskNode t: added){
-				
-				Map<CondNode, Boolean> to_add = satisfiers_precond.get(t);
-				for(CondNode cnode: to_add.keySet()){
-					
-					if(all_sat_preconds.containsKey(cnode)){
-						if(all_sat_preconds.get(cnode).booleanValue() !=
-							to_add.get(cnode).booleanValue()){
-							all_sat_preconds.remove(cnode);
+			Map<TaskNode, Map<CondNode, Boolean>> satisfiers_precond_local = new LinkedHashMap<TaskNode, Map<CondNode, Boolean>>();
+			
+			for(TaskNode tnd: satisfiers_precond.keySet()){
+				satisfiers_precond_local.put(tnd, new LinkedHashMap<CondNode, Boolean>(satisfiers_precond.get(tnd)));
+			}
+			
+			//1.  Build the map of effects
+			Map<CondNode, Map<CondNode, Boolean>> all_sat_effects = new LinkedHashMap<CondNode, Map<CondNode, Boolean>>();
+			for(CondNode cnd: added.keySet()){
+				Map<CondNode, Boolean> to_add = satisfiers_effects.get(added.get(cnd));
+				all_sat_effects.put(cnd, to_add);
+			}
+			
+			
+			//2.  Get rid of preconditions which must be different different places
+			
+			//a.  detect them
+			
+			List<CondNode> to_purge_from_known_preconditions = new LinkedList<CondNode>();
+			
+			for(CondNode root_cond1: added.keySet()){
+				for(CondNode precond1: satisfiers_precond_local.get(added.get(root_cond1)).keySet()){
+					for(CondNode root_cond2: added.keySet()){
+						for(CondNode precond2: satisfiers_precond_local.get(added.get(root_cond2)).keySet()){
+							if((precond1 == precond2) &&  
+									(satisfiers_precond_local.get(added.get(root_cond1)).get(precond1) !=
+										satisfiers_precond_local.get(added.get(root_cond2)).get(precond2))){
+								to_purge_from_known_preconditions.add(precond1);
+							}
 						}
-					}else{
-						all_sat_preconds.put(cnode, to_add.get(cnode));
 					}
 				}
 			}
 			
-			Map<CondNode, Boolean> all_sat_effects = new LinkedHashMap<CondNode, Boolean>();
+			//b.  erase them
 			
-			for(TaskNode t: added){
-				Map<CondNode, Boolean> to_add = satisfiers_precond.get(t);
-				all_sat_effects.putAll(to_add);
+			for(CondNode cnd: to_purge_from_known_preconditions){
+				for(TaskNode tnd: satisfiers_precond_local.keySet()){
+					satisfiers_precond_local.remove(cnd);
+				}
 			}
 			
+			//3.  Build the map of precondition to conditions needed for it, but for each check
+			//to see that it is nowhere in the map of effects except under the same key
+			
+			Map<CondNode, Map<CondNode, Boolean>> all_sat_precond = new LinkedHashMap<CondNode, Map<CondNode, Boolean>>();
 			
-			List<CondNode> effects_remove = new LinkedList<CondNode>();
-			for(CondNode c: all_sat_effects.keySet()){
-				Boolean determined_value= all_sat_preconds.get(c);
-				if(determined_value != null){
-					if(determined_value.booleanValue() != all_sat_effects.get(c).booleanValue()){
-						effects_remove.add(c);
+			for(CondNode root_cond: added.keySet()){
+				all_sat_precond.put(root_cond, new LinkedHashMap<CondNode, Boolean>());
+				
+				//Find the preconditions of the task that satisfys that cond
+				for(CondNode precond: satisfiers_precond_local.get(added.get(root_cond)).keySet()){
+					
+					boolean opp_found_in_effects = false;
+					//Check to see if it is in the effects map
+					
+					out:
+					for(CondNode effect_root: all_sat_effects.keySet()){
+						
+						if(added.get(effect_root) == added.get(root_cond)){
+							continue;
+						}
+						
+						for(CondNode effect_eff: all_sat_effects.get(effect_root).keySet()){
+							if((precond == effect_eff) &&
+									(satisfiers_precond_local.get(added.get(root_cond)).get(precond)!=
+										all_sat_effects.get(effect_root).get(effect_eff))){
+								opp_found_in_effects = true;
+								break out;
+							}
+						}
 					}
+					
+					if(!opp_found_in_effects){
+						all_sat_precond.get(root_cond).put(precond, 
+								satisfiers_precond_local.get(added.get(root_cond)).get(precond));
+					}else{
+		
+					}
+					
 				}
 			}
 			
-			for(CondNode cnd: effects_remove){
-				all_sat_preconds.remove(cnd);
-			}
 			
+			//TODO set it null after every time through
+			
+//			if(stored_sat_precond == null){
+//				stored_sat_precond = new LinkedHashMap<CondNode, Map<CondNode, Boolean>>();
+//
+//				for(CondNode cnd: added.keySet()){
+//					stored_sat_precond.put(cnd, new LinkedHashMap<CondNode, Boolean>(satisfiers_precond.get(added.get(cnd))));
+//				}
+//			}
+//			else{
+//				//Want to have stored_sat_precond.get(each precond) retain only the values
+//				//in satisfiers_precond for that precondition
+//				
+//
+//				
+//				//For every precondition
+//				for(CondNode cond: added.keySet()){
+//					List<CondNode> to_remove = new LinkedList<CondNode>();
+//					Map<CondNode, Boolean> tmp = stored_sat_precond.get(cond);
+//					
+//					//For every value in stored_sat_precond of that precond
+//					for(CondNode cnd: tmp.keySet()){
+//						
+//						//If satisfiers_precond doesn't have it, remove it
+//						if(!satisfiers_precond.get(added.get(cond)).containsKey(cnd) ||
+//						!(satisfiers_precond.get(added.get(cond)).get(cnd) == tmp.get(cnd))){
+//							to_remove.add(cnd);
+//						}
+//					}
+//					for(CondNode cnd: to_remove){
+//						tmp.remove(cnd);
+//					}	
+//				}
+//			}
+
+				
+//				for(TaskNode t: satisfiers_precond.keySet()){
+
+//					Map<CondNode, Boolean> tmp = stored_sat_precond.get(added.get(t));
+//					List<CondNode> to_remove = new LinkedList<CondNode>();
+//					for(CondNode cnd: tmp.keySet()){
+//						if(!satisfiers_precond.get(t).containsKey(cnd) ||
+//								!(satisfiers_precond.get(t).get(cnd) == tmp.get(cnd))){
+//							to_remove.add(cnd);
+//						}
+//					}
+//					
+//					for(CondNode cnd: to_remove){
+//						tmp.remove(cnd);
+//					}
+//					
+//				}
+				
+				
+//			}
+			
+
+
 			
 			//If all preconditions in the sat_preconditions, continue
 			//If any are not in it, return false
-			
+
 			
 			for(CondNode cnd: act_effects.keySet()){
-				if(all_sat_preconds.containsKey(cnd)){
-					if(all_sat_preconds.get(cnd).booleanValue() != 
-						act_effects.get(cnd).booleanValue()){
+				for(CondNode cnd2: all_sat_precond.keySet()){
+					if(all_sat_precond.get(cnd2).containsKey(cnd)){
+						if(all_sat_precond.get(cnd2).get(cnd).booleanValue() != 
+							act_effects.get(cnd).booleanValue()){
+	
+							return false;
+						}
+							
+					}else{
+	
 						return false;
 					}
-						
-				}else{
-					return false;
 				}
 			}
 			
-			stored_satisfiers_precond = satisfiers_precond;
-			
+
 			return true;
 		}
 	}
@@ -765,12 +1137,15 @@ public class PDDL_Translator {
         
         ExpansionDataSet d = new ExpansionDataSet();
         
+
         for(String s: goal_states.keySet()){
         	expand_relevant_condition(this.goal_states.get(s), this.condition_nodes.get(s), d);
         }
         
-        boolean return_value = (condition_nodes.size() != d.pos_condition_nodes.size()+
-        		d.neg_condition_nodes.size());
+        Set<String> all_conds = new HashSet<String>(d.pos_condition_nodes.keySet());
+        all_conds.addAll(d.neg_condition_nodes.keySet());
+        
+        boolean return_value = (condition_nodes.size() != all_conds.size());
         
         condition_nodes = new LinkedHashMap<String, CondNode>();
         condition_nodes.putAll(d.pos_condition_nodes);
diff --git a/utils/PDDLtoSANetTranslator/PDDLParser/src/Translator/SAN_Data/TaskNode.java b/utils/PDDLtoSANetTranslator/PDDLParser/src/Translator/SAN_Data/TaskNode.java
index 1ce17caf131..3c241a54370 100644
--- a/utils/PDDLtoSANetTranslator/PDDLParser/src/Translator/SAN_Data/TaskNode.java
+++ b/utils/PDDLtoSANetTranslator/PDDLParser/src/Translator/SAN_Data/TaskNode.java
@@ -139,23 +139,23 @@ public class TaskNode{
 	
 	public String toString(){
 		String s =  ("Action node: ["+ name+"] ID: "+nodeID);
-		
-		for(PrecondLink l: positive_preconditions){
-			s+=("\n   "+l.toString());
-		}
-		
-		for(PrecondLink l: negative_preconditions){
-			s+=("\n   "+l.toString());
-		}
-		
-		for(EffectLink l: positive_effects){
-			s+=("\n   "+l.toString());
-		}
-		
-		for(EffectLink l: negative_effects){
-			s+=("\n   "+l.toString());
-		}
-		
+//		
+//		for(PrecondLink l: positive_preconditions){
+//			s+=("\n   "+l.toString());
+//		}
+//		
+//		for(PrecondLink l: negative_preconditions){
+//			s+=("\n   "+l.toString());
+//		}
+//		
+//		for(EffectLink l: positive_effects){
+//			s+=("\n   "+l.toString());
+//		}
+//		
+//		for(EffectLink l: negative_effects){
+//			s+=("\n   "+l.toString());
+//		}
+//		
 		return s;
 	}
 }
\ No newline at end of file