/Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/BFTEObjectiveFunction.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 "BFTEObjectiveFunction.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 Mers::Estimation::Discrete::BFTEObjectiveFunction;
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

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


BFTEObjectiveFunction::~BFTEObjectiveFunction()
{
}


//      PUBLIC METHODS




//      PROTECTED METHODS

double 
BFTEObjectiveFunction::calculateUtility(vector<BaseModelAssignment const *> & bmAssignments,
                                                                                int sourceStateIndex) const
{
        double trajectoryProb = 1.0;
        unsigned int j, varUID;
        unsigned int numSourceStates = (unsigned int) sourceStateProbabilities.size();

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

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

        map<unsigned int, double>::const_iterator sProbItr = sourceStateProbabilities.find(sourceStateIndex);
        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[sourceStateIndex*(allTransitionProbabilitiesForAllStates.size() / 
                                        numSourceStates) + (*pMapItr).second + assignedValue[varUID]];
                }
                else
                {
                        map<pair<unsigned int, unsigned int>, double>::const_iterator opItr;
                        opItr = optimisticProbabilities.find(pair<unsigned int, unsigned int>(varUID, sourceStateIndex));
                        trajectoryProb *= (*opItr).second;
                }
        }

        return trajectoryProb;
}

//      PRIVATE METHODS


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

---