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

#include "BFBSU.h"

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


#include <Mers/Estimation/Discrete/Trajectories.h>
#include <Mers/Estimation/Discrete/EstimateHeap.h>
#include <Mers/Estimation/Discrete/ApproximateBeliefState/BSUObjectiveFunction.h>
#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/BaseModelAssignmentArray.h>

#include <math.h>
#include <map>

using Mers::Estimation::Discrete::ApproximateBeliefState::BFBSU::BFBSU;
using Mers::Estimation::Discrete::ApproximateBeliefState::BFBSE::BFBSE;
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::Estimation::Discrete::Trajectories;
using Mers::Models::CCA::CCAModel;
using Mers::Models::Base::BaseModelAssignmentArray;
using std::vector;
using std::map;
using std::cout;
using std::endl;

        //-----------------------------------------------
        //      CONSTRUCTOR
        //-----------------------------------------------

BFBSU::BFBSU(const CCAModel &cca, vector<const ObsProbRule *> & theObsProbRules, Trajectories * initialTrajectories)
: BFBSE(cca, initialTrajectories)
{
        // Remove base objective function
        delete objectiveFunction;
 
        // Probability map that maps UIDs instead of just array indices
        map<unsigned int, unsigned int> varUIDtoProbMap;
        
        unsigned int nextVarPos = 0;
        for(unsigned int i=0; i < decisionVariables->getLength(); i++)
        {
                varUIDtoProbMap[(*decisionVariables)[i]->getUID()] = nextVarPos;
                nextVarPos += (*decisionVariables)[i]->getDomain()->getLength();
        }
 
        vector<double> emptyTransProbs;
        map<unsigned int,double> emptyStateProbs;

        objectiveFunction = new BSUObjectiveFunction(decisionVariables, varUIDtoProbMap, 
                        emptyTransProbs, emptyStateProbs, theObsProbRules);
}


        //-----------------------------------------------
        //      DESTRUCTOR
        //-----------------------------------------------


BFBSU::~BFBSU()
{
}


        //-----------------------------------------------
        //      PUBLIC METHODS
        //-----------------------------------------------

        //-----------------------------------------------
        //      PRIVATE METHODS
        //-----------------------------------------------

---