/Users/fpy/titan-1-5-MBARI/Mers/Generators/BFIG/unit-tests/BFIGHelper.cpp

//
// Copyright (c) 2004           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 "BFIGHelper.h"

#include <Mers/Generators/BFIG/BFIGConstraint.h>


using Mers::Generators::BFIG::BFIGVariable;
using Mers::Generators::BFIG::BFIGConstraint;
using Mers::Generators::BFIG::BFIGAssignment;
using Mers::Models::Base::BaseModelVariable;
using Mers::Models::Base::BaseModelDomain;
using std::vector;


tut::BFIGHelper::BFIGHelper() : 
        mBFIGVariables(false), mAllVariables(true), mDomains(true)
{

}


tut::BFIGHelper::~BFIGHelper()
{
        for(unsigned int d = 0; d < mBFIGVarList.size(); d++) {
                if(mBFIGVarList[d] != NULL) {
                        delete mBFIGVarList[d];
                }
        }
}


void tut::BFIGHelper::initBFIGData(unsigned int numDomains, 
        const struct BFIGDomainEntry * domains,
        unsigned int numVariables, const struct BFIGVariableEntry * variables)
        throw()
{
        for(unsigned int d = 0; d < numDomains; d++) {
                BaseModelDomain * domain = loadDomain(&(domains[d].bmData), d);
                mDomains.pushBack(domain);
        }

        // Load our Variables & Probabilities/Costs

        // Note that our first probability/cost is stored in index 1.
        mProbabilities.push_back(0);
        for(unsigned int v = 0; v < numVariables; v++) {
                BaseModelVariable * variable = 
                        loadVariable(mDomains, &(variables[v].bmData), v);

                mAllVariables.pushBack(variable);

                bool probsOrCostsPresent = variables[v].probsOrCosts != NULL;
                if(variables[v].registered) {

                        mBFIGVariables.pushBack(variable);
                        mValueMap.pushBack(mProbabilities.size());

                        ensure("At least one cost metric provided", probsOrCostsPresent);

                        unsigned int domainSize = 
                                mDomains[variables[v].bmData.domainIndex]->getLength();
                        double const * values = variables[v].probsOrCosts;

                        for(unsigned int l = 0; l < domainSize; l++) {
                                mProbabilities.push_back(values[l]);
                        }
                } else {
                        ensure("No cost metric provided for non-BFIG variables", 
                                                 !probsOrCostsPresent);
                }
        }
}


void tut::BFIGHelper::initBFIGVariables(bool isProbabilities) throw()
{
        ensure("Sufficient number of probabilities.", 
                                 mProbabilities.size() > mBFIGVariables.getLength());
        ensure("At least one variable.", mBFIGVariables.getLength() > 0);
        ensure("At value map matches variables.",
                                 mBFIGVariables.getLength() == mValueMap.getLength());

        unsigned int numVars = mBFIGVariables.getLength();
        mBFIGVarList.resize(numVars);

        for(unsigned int v = 0; v < numVars; v++) {
                BaseModelVariable const * var = mBFIGVariables[v];
                BaseModelDomain const * domain = var->getDomain();
                const unsigned int domainSize = domain->getLength();
                const unsigned int baseIndex = mValueMap[v];
                vector<double> costs;

                ensure("Value Map is in range.", 
                                         domainSize + baseIndex <= mProbabilities.size());

                costs.resize(domainSize);

                for(unsigned int d = 0; d < domainSize; d++) {
                        costs[d] = mProbabilities[d + baseIndex];
                }

                BFIGVariable * bfigVar = 
                        new BFIGVariable(v, var, costs, isProbabilities);

                mBFIGVarList.push_back(bfigVar);
        }
}


void tut::BFIGHelper::loadAssignments(vector<BFIGAssignment *> &assignments,
        const unsigned int assignmentsData[])
{
        const unsigned int noVal = (unsigned int) -1;
        const unsigned int n = mBFIGVarList.size();

        ensure("At least one variable exists.", n > 0);

        for(unsigned int v = 0; v < n; v++)
        {
                unsigned int valIndex = assignmentsData[v];
                if(valIndex != noVal) {
                        BFIGVariable * var = mBFIGVarList[v];
                        ensure("BFIGHelper::loadAssignments: Value Index is within range",
                                                 valIndex < var->getDomainSize());
                        
                        BFIGAssignment * asg = var->getAssignmentByValue(valIndex);
                        
                        assignments.push_back(asg);
                }
        }
}


void tut::BFIGHelper::loadAssignments(
        vector<BFIGAssignment const *> &assignments,
        const unsigned int assignmentsData[])
{
        vector<BFIGAssignment *> regAssignments;
        loadAssignments(regAssignments, assignmentsData);

        convertVector<BFIGAssignment>(assignments, regAssignments);
}


void tut::BFIGHelper::loadConstraints(vector<BFIGConstraint *> &constraints,
        const unsigned int constraintValues[], const bool constraintParity[])
{
        const unsigned int noVal = (unsigned int) -1;
        const unsigned int n = mBFIGVarList.size();

        ensure("At least one variable exists.", n > 0);

        for(unsigned int v = 0; v < n; v++)
        {
                unsigned int valIndex = constraintValues[v];
                if(valIndex != noVal) {
                        BFIGVariable * var = mBFIGVarList[v];
                        ensure("BFIGHelper::loadConstraints: Value Index is within range",
                                                 valIndex < var->getDomainSize());
                        
                        BFIGAssignment * asg = var->getAssignmentByValue(valIndex);
                        
                        bool parity = constraintParity[v];
                        BFIGConstraint * cons = new BFIGConstraint(asg, parity);

                        constraints.push_back(cons);
                }
        }
}


void tut::BFIGHelper::loadConstraints(
        vector<BFIGConstraint const *> &constraints,
        const unsigned int constraintValues[], const bool constraintParity[])
{
        vector<BFIGConstraint *> regConstraints;
        loadConstraints(regConstraints, constraintValues, constraintParity);

        convertVector<BFIGConstraint>(constraints, regConstraints);
}

---