/Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ApproximateBeliefState/BSEObjectiveFunction.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 "BSEObjectiveFunction.h"

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

#include <Mers/Estimation/Discrete/Trajectories.h>
#include <Mers/Estimation/Discrete/Estimate.h>

#include <Mers/Generators/BFIG/BFIGAssignment.h>
#include <Mers/Generators/BFIG/BFIGVariable.h>
#include <Mers/Models/Base/BaseModelVariableArray.h>
#include <Mers/Models/Base/BaseModelVariable.h>
#include <Mers/Models/Base/BaseModelValue.h>
#include <Mers/Models/Base/BaseModelAssignment.h>
#include <Mers/Models/Base/BaseModelAssignmentArray.h>

using namespace Mers::Estimation::Discrete::ApproximateBeliefState;
using Mers::Estimation::Discrete::Trajectories;
using Mers::Estimation::Discrete::Estimate;
using Mers::Generators::BFIG::BFIGAssignment;
using Mers::Generators::BFIG::BFIGVariable;
using Mers::Models::Base::BaseModelVariableArray;
using Mers::Models::Base::BaseModelVariable;
using Mers::Models::Base::BaseModelValue;
using Mers::Models::Base::BaseModelAssignment;
using Mers::Models::Base::BaseModelAssignmentArray;
using std::vector;
using std::map;
using std::pair;
using std::cout;
using std::endl;

//      CONSTRUCTOR

BSEObjectiveFunction::BSEObjectiveFunction(BaseModelVariableArray const * dVs,
                const map<unsigned int,unsigned int> pMap, vector<double> allTransProbs, map<unsigned int,double> sProbs)
: MEObjectiveFunction(dVs, pMap, allTransProbs, sProbs)
{}


BSEObjectiveFunction::~BSEObjectiveFunction()
{
}


//      PUBLIC METHODS




//      PROTECTED METHODS

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

        double fX = 0.0;
        double trajectoryProb = 1.0;
        unsigned int i, j, varUID;
        unsigned int numSourceStates = (unsigned int) sourceStateProbabilities.size();

        unsigned int numNotAssigned = 0;
        unsigned int numAssigned = 0;

        map<unsigned int, bool> wasAssigned;
        map<unsigned int, unsigned int> assignedValue;
        for(i=0; i<decisionVariables->getLength(); i++)
                wasAssigned[(*decisionVariables)[i]->getUID()] = false;

        for(i=0; i<bmAssignments.size(); i++)
        {
                varUID = bmAssignments[i]->getVariable()->getUID();
                wasAssigned[varUID] = true;
                assignedValue[varUID] = bmAssignments[i]->getValue()->getUID();                 // Assumes probabilities are still listed in UID order
        }

        for(i=0; i<numSourceStates; i++)
        {
                map<unsigned int, double>::const_iterator sProbItr = sourceStateProbabilities.find(i);
                trajectoryProb = (*sProbItr).second;    // Direct access isn't a constant function so apparently this is required
                for(j=0; j<decisionVariables->getLength(); j++)
                {
                        varUID = (*decisionVariables)[j]->getUID();
                        if(wasAssigned[varUID])
                        {
                                map<unsigned int, unsigned int>::const_iterator pMapItr = probabilityMap.find(varUID);
                                trajectoryProb *= allTransitionProbabilitiesForAllStates[i*(allTransitionProbabilitiesForAllStates.size() / 
                                                numSourceStates) + (*pMapItr).second + assignedValue[varUID]];
                                numAssigned++;
                        }
                        else
                        {
                                map<pair<unsigned int, unsigned int>, double>::const_iterator opItr;
                                opItr = optimisticProbabilities.find(pair<unsigned int, unsigned int>(varUID, i));
                                trajectoryProb *= (*opItr).second;
                                numNotAssigned++;
                        }
                }
                fX += trajectoryProb;
        }

//      cout << "# assigned: " << numAssigned << ", # not assigned: " << numNotAssigned << ", prob: " << fX << endl;

        return fX;
}

//      PRIVATE METHODS


double 
BSEObjectiveFunction::calculatePropagationUtility(vector<BaseModelAssignment const *> & bmAssignments,
                                                                                                  int sourceStateIndex) const
{
        return BSEObjectiveFunction::calculateUtility(bmAssignments, sourceStateIndex);
}

---