/Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ApproximateBeliefState/BSUObjectiveFunction.cpp

//
// Copyright (c) 2005           Model-based Embedded and Robotic Systems Group
// All Rights Reserved          Massachusetts Institute of Technology
//
// This software is provided as is. Model-based Embedded and Robotic Systems
// (MERS) group does not assume any responsibility.
//
// Organization:        Model-based Embedded and Robotic Systems Group
//                      Massachusetts Institute of Technology
//

#include "BSUObjectiveFunction.h"

#ifdef _MSC_VER
#pragma warning( disable : 4786 )
#endif

#include <Mers/Estimation/Discrete/ProbabilityRules/RuleTriggering/RuleTrigger.h>
#include <Mers/Estimation/Discrete/ProbabilityRules/ObsProbRule.h>
#include <Mers/Estimation/Discrete/ProbabilityRules/Rule.h>

#include <Mers/Models/Base/BaseModelVariableArray.h>
#include <Mers/Models/Base/BaseModelAssignment.h>


using Mers::Estimation::Discrete::ApproximateBeliefState::BSUObjectiveFunction;
using Mers::Estimation::Discrete::ProbabilityRules::RuleTriggering::RuleTrigger;
using Mers::Estimation::Discrete::ProbabilityRules::ObsProbRule;
using Mers::Estimation::Discrete::ProbabilityRules::Rule;
using Mers::Models::Base::BaseModelVariableArray;
using Mers::Models::Base::BaseModelAssignment;
using std::vector;
using std::cout;
using std::endl;
using std::map;

//      CONSTRUCTOR

BSUObjectiveFunction::BSUObjectiveFunction(BaseModelVariableArray const * dVs,
                const map<unsigned int,unsigned int> pMap, vector<double> allTransProbs, 
                map<unsigned int,double> sProbs, vector<const ObsProbRule *> & theObsProbabilityRules)
: BSEObjectiveFunction(dVs, pMap, allTransProbs, sProbs) 
{
        while(theObsProbabilityRules.size() > 0)
        {
                triggerRules.push_back((ObsProbRule *)theObsProbabilityRules.back());
                theObsProbabilityRules.pop_back();
        }
        obsProbabilityRuleTrigger = new RuleTrigger(triggerRules);
}


BSUObjectiveFunction::~BSUObjectiveFunction()
{
        delete obsProbabilityRuleTrigger;
}


//      PUBLIC METHODS


//      PROTECTED METHODS

double 
BSUObjectiveFunction::calculateUtility(vector<BaseModelAssignment const *> & bmAssignments,
                                                                           int sourceStateIndex) const
{
        (void)sourceStateIndex; // Not used by BSEObjectiveFunction

        // The BSEObjectiveFunction handles the propagation equation. The complete solution is the
        // product of that and the observation probabilities.
        double fX = BSEObjectiveFunction::calculateUtility(bmAssignments);
        
        // First reset the trigger
        obsProbabilityRuleTrigger->reset();

        // Add in all the current mode variable assignments
        for(size_t i=0; i<bmAssignments.size(); i++)
                obsProbabilityRuleTrigger->loadAssignment(bmAssignments[i]->getVariable()->getUID(), 
                                bmAssignments[i]->getValue()->getUID());

        // Determine which rules are fired and multiply the total probability by them
        vector<Rule *> enabledProbabilityRules = obsProbabilityRuleTrigger->getTriggeredRules();
        while(enabledProbabilityRules.size() > 0)
        {
                fX *= ((ObsProbRule *) enabledProbabilityRules.back() )->getProbability();
                enabledProbabilityRules.pop_back();
        }

        return fX;
}

//      PRIVATE METHODS

---