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

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


#include <Mers/Estimation/Discrete/Estimate.h>
#include <Mers/Estimation/Discrete/Trigger.h>
#include <Mers/Estimation/Discrete/Trajectories.h>
#include <Mers/Estimation/Discrete/EstimateHeap.h>
#include <Mers/Estimation/Discrete/ApproximateBeliefState/BSEObjectiveFunction.h>
#include <Mers/Solvers/SAT/GATSAT/OpSAT/Opsat.h>
#include <Mers/Solvers/SAT/AssignmentTesterInterface.h>
#include <Mers/Generators/BFIG/BFIGGenerator.h>
#include <Mers/Models/Base/BaseModelAssignment.h>
#include <Mers/Models/Base/BaseModelAssignmentArray.h>
#include <Mers/Models/Base/BaseModelClause.h>
#include <Mers/Models/Base/BaseModelClauseArray.h>

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

using Mers::Estimation::Discrete::ApproximateBeliefState::BFBSE::BFBSE;
using Mers::Estimation::Discrete::ApproximateBeliefState::BSEObjectiveFunction;
using namespace Mers::Estimation::Discrete::ModeEstimationPerformance;
using namespace Mers::Estimation::Discrete::ModeEstimation;
using namespace Mers::Estimation::Discrete;
using Mers::Models::Base::BaseModelAssignment;
using Mers::Models::Base::BaseModelAssignmentArray;
using Mers::Models::Base::BaseModelClause;
using Mers::Models::Base::BaseModelClauseArray;
using Mers::Generators::BFIG::BFIGGenerator;
using Mers::Solvers::SAT::GATSAT::OpSAT::Opsat;
using Mers::Solvers::SAT::AssignmentTesterInterface;
using Mers::Models::CCA::CCAModel;
using std::vector;
using std::map;
using std::cout;
using std::cerr;
using std::endl;

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

BFBSE::BFBSE(const CCAModel &cca, Trajectories * initialTrajectories)
: ModeEstimatorDebug(cca, initialTrajectories)
{
        if(objectiveFunction != NULL)
                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 BSEObjectiveFunction(decisionVariables, varUIDtoProbMap, emptyTransProbs, emptyStateProbs);
}


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


BFBSE::~BFBSE()
{
        if(objectiveFunction != NULL)
        {
                delete objectiveFunction;
                objectiveFunction = NULL;
        }
}


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

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

bool
BFBSE::generateTrajectories(void)
{
        unsigned int i, j;
        unsigned int obsValueUID;
        unsigned int numEstimatesBeforeRequest = 0;
        BaseModelClauseArray * currentObservationAssignments = new BaseModelClauseArray(true);
        BaseModelClause * aObservationClause;
        vector<double> allProbabilities = compiledObjectTrigger->getNormalizedEnabledTransitionProbabilities();
        map<unsigned int, double> currentStateProbabilities;

        if(currentTrajectories->getNumberEstimates() < 1)
        {
                cerr << "<BFBSE> ERROR: There are no current estimates!" << endl;
                cleanAfterFailure();
                return false;
        }

        if(debug > 2)
        {
                cout << *compiledObjectTrigger << endl << endl;

                for(i=0; i<allProbabilities.size(); i++)
                {
                        if(i%(allProbabilities.size()/currentTrajectories->getNumberEstimates()) == 0)
                                cout << endl << "Probabilities from source state " << 
                                i/(allProbabilities.size()/currentTrajectories->getNumberEstimates()) << ": " << endl;
                        cout << allProbabilities[i] << ", ";
                }
                cout << endl << endl;
        }


        // Gather all the current state probabilities
        for(i=0; i < currentTrajectories->getNumberEstimates(); i++)
                currentStateProbabilities[i] = currentTrajectories->getEstimate(i)->getProbability();

        // Update the objective function with current probability data
        objectiveFunction->updateProbabilities(allProbabilities, currentStateProbabilities);

        // Create clause constraints for all the current observations
        for(i=0; i<observationVariables->getLength(); i++)
        {
                obsValueUID = compiledObjectTrigger->getCurrentCommandObservationValueUID((*observationVariables)[i]->getUID());
                if(obsValueUID != (unsigned int)Trigger::UNDEFINED)
                {
                        aObservationClause = new BaseModelClause(BaseModelClause::DISJUNCTIVE, true);
                        aObservationClause->pushBack(new BaseModelAssignment((*observationVariables)[i], (*observationVariables)[i]->getDomain()->getValue(obsValueUID)));
                        currentObservationAssignments->pushBack(aObservationClause);
                }
        }

        // Initialize OpSat if one hasn't already been declared
        if(opsats.size() < 1)
        {
                opsats.resize(1);
                opsats[0] = new Opsat(*decisionVariables, objectiveFunction, *myCCAmodel.getVariables(), *globalConstraints);
                setDebug(debug);
        }

        // Add all the observation constraints
        for(j=0; j<currentObservationAssignments->getLength(); j++)
        {
                if(debug > 2)
                        cout << "Adding " << (*currentObservationAssignments)[j] << " constraint to opsat." << endl;
                opsats[0]->getTester()->addTemporaryConstraint((*currentObservationAssignments)[j]);
        }

        // Initialize next trajectories object
        if(nextTrajectories != NULL)
                delete nextTrajectories;
        nextTrajectories = new Trajectories(currentTrajectories->getAccumulatedUncertainty(), debug);

        // Intialize estimate counter (timing requirements)
        numEstimatesGenerated = 0;

        do {
                numEstimatesBeforeRequest = nextTrajectories->getNumberEstimates();
                // Get next best estimates using Opsat
                if(!findNextBestStateAndPutInHeap(0, allProbabilities, NULL, false))
                {
                        cleanAfterFailure();
                        return false;
                }
        // Exits if k estimates have been generated or the search is exhausted
        } while(nextTrajectories->getNumberEstimates() < maxNumberEstimates && 
                                nextTrajectories->getNumberEstimates() != numEstimatesBeforeRequest);
        
        if(nextTrajectories->getNumberEstimates() < 1)
        {
                        cerr << "<BFBSE> ERROR: Couldn't find any consistent estimates for next trajectories!" << endl;
                        cleanAfterFailure();
                        return false;
        }

        releaseMemoryAndNormalize(currentObservationAssignments);

        return true;
}


bool
BFBSE::addEstimate(Estimate * theEst, EstimateHeap * nextTrajsEstHeap)
{
        (void)nextTrajsEstHeap;
        if(nextTrajectories == NULL)
        {
                cerr << "<ModeEstimatorBase> ERROR: Next trajectories memory not allocated!" << endl;
                return false;
        }
        if(!nextTrajectories->addEstimate(theEst))      // Add it to the next trajectories list
        {
                cerr << "<ModeEstimatorBase> ERROR: Couldn't add estimate to next trajectories!" << endl;
                return false;
        }
        return true;
}


bool
BFBSE::calculateEstimateProbability(double & estProbability, const BaseModelAssignmentArray * state, 
                                                                                                unsigned int generatorNum, const vector<double> &allProbs) const
// This function really belongs in a general BFBSE class
{
        (void) generatorNum;
        (void) allProbs;

        estProbability = objectiveFunction->getUtility(state);

        return true;
}

---