7 files changed, 49 insertions, 18 deletions

diff --git a/SA_POP/C2W/C2W_Demo.cpp b/SA_POP/C2W/C2W_Demo.cpp
index 43c56781fc3..8f7c9651833 100644
--- a/SA_POP/C2W/C2W_Demo.cpp
+++ b/SA_POP/C2W/C2W_Demo.cpp
@@ -263,8 +263,7 @@ int main (int argc, char* argv[])
 	  //planner->add_out_adapter (&screen_out);
 
     planner->plan (15, goal);
-	
-
+	planner->calculate_plan_utility(115);
 
   //} 
   /*
diff --git a/SA_POP/Planner.cpp b/SA_POP/Planner.cpp
index 3c9379dfb2e..a6d6710d937 100644
--- a/SA_POP/Planner.cpp
+++ b/SA_POP/Planner.cpp
@@ -512,22 +512,34 @@ void Planner::notify_plan_changed (void)
   }
 };
 
-void Planner::calculate_plan_utility(size_t sa_max_steps)
+double Planner::calculate_plan_utility(size_t sa_max_steps)
 {
+	sanet_->reset_step();
 	sanet_->set_nodes_state(false);
 
 	for(CLSet::iterator it = this->plan_.causal_links.begin(); it != 
 		this->plan_.causal_links.end(); it++){
-
-			this->sanet_->set_task_state((*it).first, true);
-			this->sanet_->set_task_state((*it).second, true);
+		if((*it).first != INIT_TASK_INST_ID){
+			this->sanet_->set_task_state(this->working_plan_->get_task_from_inst((*it).first), true);
+			this->sanet_->set_task_state(this->working_plan_->get_task_from_inst((*it).second), true);
 			this->sanet_->set_cond_state((*it).cond.id, true);
+		}
 	}
 
 	sanet_->update(sa_max_steps);
 
+	GoalMap goals = this->get_goals();
+
+	double conj_utils = 0;
+	for(GoalMap::iterator it = goals.begin(); it != goals.end(); it++){
+		conj_utils+=(it->second * this->get_cond_val(it->first));
+	}
+
+	std::cout<<"Plan utility: "<<conj_utils<<std::endl;
+
 	//TODO the rest
 
+	return conj_utils;
 }
 
 /// Get the Task instances in a particular set of the specified task instance
diff --git a/SA_POP/Planner.h b/SA_POP/Planner.h
index 9b46f06cdcd..7a2661bd6c4 100644
--- a/SA_POP/Planner.h
+++ b/SA_POP/Planner.h
@@ -519,7 +519,7 @@ namespace SA_POP {
 
   virtual void set_backtrack_cmd_id(CommandID cmd){backtrack_cmd = cmd;};
 
-  virtual void Planner::calculate_plan_utility(size_t sa_max_steps);
+  virtual double calculate_plan_utility(size_t sa_max_steps);
 
   protected:
     /// Threshold for current probability of a condition to be satisfied.
diff --git a/SA_POP/SANet/SANet.cpp b/SA_POP/SANet/SANet.cpp
index c29dbe5ce8d..2f9fcef0ff4 100644
--- a/SA_POP/SANet/SANet.cpp
+++ b/SA_POP/SANet/SANet.cpp
@@ -74,6 +74,18 @@ void SANet::Network::add_task (TaskID ID, std::string name, MultFactor atten_fac
   }
 };
 
+void SANet::Network::reset_step(){
+	step_ = 0;
+
+	for(CondNodeMap::iterator it = this->cond_nodes_.begin(); it != cond_nodes_.end(); it++){
+		it->second->reset_step();
+	}
+
+	for(TaskNodeMap::iterator it = this->task_nodes_.begin(); it != task_nodes_.end(); it++){
+		it->second->reset_step();
+	}
+}
+
 Probability SANet::Network::get_prior(TaskID ID)
 {
   TaskNodeMap::iterator task_iter = task_nodes_.find (ID);
diff --git a/SA_POP/SANet/SANet.h b/SA_POP/SANet/SANet.h
index a0b726119a6..9373ad5be80 100644
--- a/SA_POP/SANet/SANet.h
+++ b/SA_POP/SANet/SANet.h
@@ -390,6 +390,8 @@ namespace SANet {
 	 */
 	virtual TimeValue get_duration (TaskID task_id);
 
+	void reset_step();
+
   protected:
     /// Map from ID to node pointer for all task nodes in network.
     TaskNodeMap task_nodes_;
diff --git a/SA_POP/SANet/SANode.cpp b/SA_POP/SANet/SANode.cpp
index 8dae4e4eb0f..55a26486d5c 100644
--- a/SA_POP/SANet/SANode.cpp
+++ b/SA_POP/SANet/SANode.cpp
@@ -101,6 +101,10 @@ NodeID Node::get_ID (void)
   return ID_;
 };
 
+void Node::reset_step(){
+	step_= 0;
+}
+
 // Get pre-links (nodes with links to this node).
 const LinkMap& Node::get_pre (void)
 {
@@ -144,6 +148,7 @@ bool TaskNode::update (void)
   if(!active)
   {
     //the node should return as not changing the network
+	step_++;
     return false;
   }
 
@@ -1195,17 +1200,18 @@ bool CondNode::update (void)
     }
 
     // Get current probability info.
-    try {
+//    try {
       cur_prob = cur_node->get_prob (step_);
-    } catch (Invalid_Step e) {
-      std::cerr << "Error in condition node update:  Invalid step value.";
-      std::cerr << std::endl;
-      throw Update_Error ();
-    } catch (...) {
-      std::cerr << "Unexpected exception thrown in condition node update.";
-      std::cerr << std::endl;
-      throw Update_Error ();
-    }
+//    } catch (Invalid_Step e) {
+//      std::cerr << "Error in condition node update:  Invalid step value.";
+//      std::cerr << std::endl;
+//      throw Update_Error ();
+//    } 
+	//  catch (...) {
+ //     std::cerr << "Unexpected exception thrown in condition node update.";
+ //     std::cerr << std::endl;
+ //     throw Update_Error ();
+ //   }
 
     // Update probability based on current multiplier.
     cur_prob.probability = cur_mult * cur_prob.probability;
diff --git a/SA_POP/SANet/SANode.h b/SA_POP/SANet/SANode.h
index f10462ffbc1..96366a7a75d 100644
--- a/SA_POP/SANet/SANode.h
+++ b/SA_POP/SANet/SANode.h
@@ -162,7 +162,7 @@ namespace SANet {
      */
     virtual void print_xml (std::basic_ostream<char, std::char_traits<char> >& strm) = 0;
 
-
+	void reset_step();
 
   protected:
     /// Unique ID of node (for identification within network).