/Users/fpy/titan-1-5-MBARI/Mers/Generators/BFIG/BFIGGenerator.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
//
//# CHECKED MJP 08-07-2002

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

#include "BFIGGenerator.h"

#include <cassert>
#include "BFIGAssignment.h"
#include "BFIGConstraint.h"
#include "BFIGImplicant.h"
#include "BFIGVariable.h"
#include "BFIGImplicantExpander.h"
#include "BFIGObjectiveFunction.h"
//#include "ConflictDB.h"

#include <Mers/Models/Base/BaseModelAssignmentArray.h>
#include <Mers/Models/Base/BaseModelDomain.h>
#include <Mers/Models/Base/BaseModelVariableArray.h>
#include <Mers/Models/Base/BaseModelSentence.h>

#include <Mers/Utils/Containers/IntegerArray.h>

#include <Mers/Utils/Exceptions/Exception.h>


using namespace Mers::Generators::BFIG;
using Mers::Utils::Containers::IntegerArray;
using Mers::Models::Base::BaseModelVariableArray;
using Mers::Models::Base::BaseModelVariable;
using Mers::Models::Base::BaseModelValue;
using Mers::Models::Base::BaseModelAssignmentArray;
using Mers::Models::Base::BaseModelAssignment;
using Mers::Models::Base::BaseModelDomain;
using Mers::Models::Base::BaseModelClauseArray;
using std::ostream;
using std::vector;
using std::cout;
using std::endl;


BFIGGenerator::BFIGGenerator(BFIGFactory &bfigFactory,
        BaseModelVariableArray const & variableSet,
        BFIGObjectiveFunction const * ocspObjectiveFunction, 
        BaseModelVariableArray const &allVariables,
        double cutoffCost)
        : mSearchBMVars(false), bmImplicant(true)
{
        (void) allVariables;
        debug = 0;

        // Start by sorting our variable set
        sortBMVariables(variableSet, mSearchBMVars);

        if(variableSet.getLength() != mSearchBMVars.getLength()) {
                // This works since sortBMVariables eliminates duplicates
                fprintf(stderr, "BFIGGenerator::BFIGGenerator: Error: Duplicate variables "
                                                "passed into BFIG\n");
                // Fatal error.
                throw 1;
        }

        compactBFIGVars.resize(mSearchBMVars.getLength());

        // Create a BFIG variable for every searchable BM Variable.
        for(unsigned int y = 0; y < mSearchBMVars.getLength(); y++)
        {
                unsigned int index = bfindBMVariable(mSearchBMVars, variableSet[y]->getUID());

                vector<double> costs;           // Should set the costs for all values in the domain to 0
                costs.resize(variableSet[y]->getDomain()->getLength()); 

                BFIGVariable * bfigVariable = new BFIGVariable(index, variableSet[y], costs, false);
                compactBFIGVars[index] = bfigVariable;
        }

        bfig = bfigFactory.create(compactBFIGVars, ocspObjectiveFunction, cutoffCost);
}


BFIGGenerator::BFIGGenerator(BFIGFactory &bfigFactory, 
        BaseModelVariableArray const & variableSet,
        BaseModelVariableArray const &allVariables,
        IntegerArray const &valueMap, vector<double> const & probabilities,
        double cutoffCost, bool useProb)
        : mSearchBMVars(false), bmImplicant(true)
{
        (void) allVariables;
        debug = 0;

        // Start by sorting our variable set
        sortBMVariables(variableSet, mSearchBMVars);

        if(variableSet.getLength() != mSearchBMVars.getLength()) {
                // This works since sortBMVariables eliminates duplicates
                fprintf(stderr, "BFIGGenerator::BFIGGenerator: Error: Duplicate variables "
                                                "passed into BFIG\n");
                // Fatal error.
                throw 1;
        }

        compactBFIGVars.resize(mSearchBMVars.getLength());

        // Create a BFIG variable for every searchable BM Variable.
        for(unsigned int y = 0; y < mSearchBMVars.getLength(); y++)
        {
                unsigned int index = bfindBMVariable(mSearchBMVars, 
                        variableSet[y]->getUID());
                vector<double> costs = extractVariableProbabilities(*variableSet[y], y, 
                        valueMap, probabilities);
                BFIGVariable * bfigVariable = variableToVariable(index, 
                        variableSet[y], costs, useProb);
                compactBFIGVars[index] = bfigVariable;
        }

        bfig = bfigFactory.create(compactBFIGVars, cutoffCost);
}


BFIGGenerator::BFIGGenerator(BFIGFactory &bfigFactory, 
        BaseModelVariableArray const & variableSet,
        BaseModelVariableArray const &allVariables,
        IntegerArray const &valueMap, vector<double> const & probabilities,
        bool useProb)
        : mSearchBMVars(false), bmImplicant(true)
{
        (void) allVariables;
        debug = 0;

        // Start by sorting our variable set
        sortBMVariables(variableSet, mSearchBMVars);

        if(variableSet.getLength() != mSearchBMVars.getLength()) {
                // This works since sortBMVariables eliminates duplicates
                fprintf(stderr, "BFIGGenerator::BFIGGenerator: Error: Duplicate variables "
                                                "passed into BFIG\n");
                // Fatal error.
                throw 1;
        }

        compactBFIGVars.resize(mSearchBMVars.getLength());

        // Create a BFIG variable for every searchable BM Variable.
        for(unsigned int y = 0; y < mSearchBMVars.getLength(); y++)
        {
                unsigned int index = bfindBMVariable(mSearchBMVars, 
                        variableSet[y]->getUID());
                vector<double> costs = extractVariableProbabilities(*variableSet[y], y, 
                        valueMap, probabilities);
                BFIGVariable * bfigVariable = variableToVariable(index, 
                        variableSet[y], costs, useProb);
                compactBFIGVars[index] = bfigVariable;
        }

        bfig = bfigFactory.create(compactBFIGVars);
}


BFIGGenerator::~BFIGGenerator()
{
/*#
        //if (debug)
        //      cout << "deleting bfig... " << flush;
*/
        if (bfig)
                delete bfig;

        for(unsigned int x = 0; x < compactBFIGVars.size(); x++){
                if(compactBFIGVars[x] != NULL)
                        delete compactBFIGVars[x];
        }
/*#
        //if (debug)
        //      cout << "done." << endl;
*/
}


void
BFIGGenerator::addConflict(BaseModelAssignmentArray const * newConflict)
{
        vector<BFIGAssignment const *> inconsistentAssignments;
        unsigned int length = newConflict->getLength();
        for(unsigned int x = 0; x < length; x++){
                BaseModelAssignment const &bmAsg = *((*newConflict)[x]);
                BaseModelVariable const &bmVar = *(bmAsg.getVariable());
                BaseModelValue const &bmVal = *(bmAsg.getValue());

                BFIGVariable * bfigVar;
                if(FAILED(getBFIGVariable(bmVar.getUID(), bfigVar))) {
                        throw 1;
                }

                BFIGAssignment const * bfigAsg = 
                        bfigVar->getAssignmentByValue(bmVal.getUID());
                inconsistentAssignments.push_back(bfigAsg);
        }

        BFIGConstraint * conflict = new BFIGConstraint(inconsistentAssignments);
        bfig->addConflict(conflict);
}


void
BFIGGenerator::addConflict(BaseModelAssignmentArray const & newConflict)
{
        addConflict(&newConflict);
}


void
BFIGGenerator::addConflicts(BaseModelClauseArray const & newConflicts)
{
        unsigned int length = newConflicts.getLength();
        for(unsigned int x = 0; x < length; x++){
                addConflict(*(newConflicts[x]));
        }
}


void
BFIGGenerator::addConflict(BFIGConstraint const * newConflict)
{
        //conflictDB->addConflict(newConflict);  // not currently used.
        bfig->addConflict(newConflict);
}


void
BFIGGenerator::addConflicts(vector<BFIGConstraint const *> const & newConflicts)
{
        //conflictDB->addConflict(newConflict);  // not currently used.
        bfig->addConflicts(newConflicts);
}


void
BFIGGenerator::addConflict(BFIGConstraint const & newConflict)
{
        BFIGConstraint * conflictp = new BFIGConstraint(newConflict);
        //unfreeConstraints.push_back(conflictp);  // must free this one ourselves...
        addConflict(conflictp);
}


BaseModelAssignmentArray const *
BFIGGenerator::generateBestImplicant(long maximumVisitedStateCount,
                                           long maxSize, bool expand)
{
        (void) expand;
        bmImplicant.makeEmpty();

        // non-copied memory:
        BFIGImplicant const * bestImplicant_p =
                bfig->getBestImplicant(maximumVisitedStateCount, maxSize);

        if (!bestImplicant_p)  // no best generated!
        {
                if (debug)
                        cout << "(BFIG found no best implicant)" << endl;
                return NULL;
        }

        vector<BFIGAssignment const *> bfigAsgs = bestImplicant_p->getAssignments();

        unsigned int bfigAsgsLength = bfigAsgs.size();
        for(unsigned int x = 0; x < bfigAsgsLength; x++){
                BFIGAssignment const &bfigAsg = *(bfigAsgs[x]);
                BaseModelVariable * bmVar = mSearchBMVars[bfigAsg.getNameInt()];
                BaseModelAssignment * bmAsg = 
                        new BaseModelAssignment(bmVar, bmVar->getValue(bfigAsg.getValueInt()),
                        BaseModelAssignment::Positive);
                bmImplicant.pushBack(bmAsg);
        }

        return &bmImplicant;
}


void
BFIGGenerator::markBestFeasible()
{
        bfig->markBestFeasible();
}


void
BFIGGenerator::printStatistics(bool verbose) const
{
        bfig->printStatistics(verbose);
}


void BFIGGenerator::restart()
{
        bfig->restart();
}


void BFIGGenerator::reset()
{
        bfig->reset();
}


void
BFIGGenerator::setDebug(unsigned int state)
{
        AssignmentGeneratorInterface::setDebug(state);
        bfig->setDebug(state);
}


/*
void BFIGGenerator::normalizeCosts()
{
        // Normalize every variable
        unsigned int varUID;
        for (varUID = 0; varUID != compactBFIGVars.size(); varUID++)
        {
                BFIGVariable * variable = compactBFIGVars[varUID];
                variable->normalizeCosts();
        }
}
*/


int BFIGGenerator::updateProbability(unsigned int variableUID,
        unsigned int valueUID, double prob)
{
        BFIGVariable * variable;
        if(FAILED(getBFIGVariable(variableUID, variable)))
                return 1;

        double cost = variable->probToCost(prob);
        return variable->setCost(valueUID, cost);
}


int BFIGGenerator::updateProbabilities(unsigned int variableUID, double probs[])
{
        BFIGVariable * variable;
        if(FAILED(getBFIGVariable(variableUID, variable)))
                return 1;
        
        unsigned int numVals = variable->getDomainTotalSize();
        double * costs = new double[numVals];
        for(unsigned int x = 0; x < numVals; x++) {
                costs[x] = variable->probToCost(probs[x]);
        }

        return variable->setCosts(costs);
}


int
BFIGGenerator::updateAllProbabilities(BaseModelVariableArray const &decisionVars, 
                IntegerArray const &valueMap, vector<double> const &allProbabilities)
{
        unsigned int success = 0;
        unsigned int i, j;
        vector<double> variableProbabilities;
        for(i = 0; i < decisionVars.getLength(); i++)
        {
                BaseModelVariable const * dVar = decisionVars[i];
                variableProbabilities = extractVariableProbabilities(*dVar, i, 
                        valueMap, allProbabilities);

                BaseModelDomain const &dDom = *(dVar->getDomain());
                unsigned int dDomLen = dDom.getLength();
                for(j = 0; j < dDomLen; j++)
                        success += updateProbability(dVar->getUID(), dDom[j]->getUID(),
                                variableProbabilities[j]);
        }
        return success;
}


int BFIGGenerator::updateCost(unsigned int variableUID,
        unsigned int valueUID, double cost)
{
        BFIGVariable * variable;
        if(FAILED(getBFIGVariable(variableUID, variable)))
                return 1;

        return variable->setCost(valueUID, cost);
}


int BFIGGenerator::updateCosts(unsigned int variableUID, double costs[])
{
        BFIGVariable * variable;
        if(FAILED(getBFIGVariable(variableUID, variable)))
                return 1;

        return variable->setCosts(costs);
}


unsigned int BFIGGenerator::getFullAssignmentSize()
{
        return compactBFIGVars.size();
}


Mers::Generators::BFIG::BFIGVariable *
BFIGGenerator::variableToVariable(unsigned int bfigVarIndex, 
        BaseModelVariable const * varIn, vector<double> &costs, bool useProb) const
{

        return new BFIGVariable(bfigVarIndex, varIn, costs, useProb);

}


void BFIGGenerator::sortBMVariables(
        BaseModelVariableArray const &unsortedArray,
        BaseModelVariableArray &sortedArray)
{
        // make sure we're aware of the case where the two arrays are the same.
        bool sameArray = (&unsortedArray) == (&sortedArray);

        // Since this is likely sorted most of the time, first check if we
        // need to do anything.
        bool sorted = true;
        for(unsigned int i = 1; i < unsortedArray.getLength() && sorted; i++) {
                sorted = unsortedArray[i - 1]->getUID() < unsortedArray[i]->getUID();
        }

        // If we're already sorted, just copy the data.
        if(sorted) {
                if(!sameArray) {
                        sortedArray.makeEmpty();
                        sortedArray.resize(unsortedArray.getLength());
                        for(unsigned int j = 0; j < unsortedArray.getLength(); j++) {
                                sortedArray[j] = (BaseModelVariable *) unsortedArray[j];
                        }
                }
                return;
        }

        // We presently don't handle the case where the two arrays are the same
        // and we need to sort.
        if(sameArray) {
                fprintf(stderr, "BFIGGenerator::sortBMVariables: Error: Invalide use:" 
                                                " Same source and destination\n");
                // Throw instead of return because this is unrecoverable (Code bug).
                // Probably should throw a real exception.  
                throw 1;
        }

        sortedArray.makeEmpty();

        // We need to sort the array.
        for(unsigned int j = 0; j < unsortedArray.getLength(); j++) {
                BaseModelVariable * var = (BaseModelVariable *) unsortedArray[j];
                unsigned int i = bfindBMVariable(sortedArray, var->getUID());
                if(i >= sortedArray.getLength() || sortedArray[i]->getUID() != var->getUID()) {
                        sortedArray.pushBack(NULL);
                        for(unsigned int k = sortedArray.getLength() - 1; k > i; k--) {
                                sortedArray[k] = sortedArray[k - 1];
                        }
                        sortedArray[i] = var;
                }
        }
}


unsigned int BFIGGenerator::bfindBMVariable(BaseModelVariableArray &sortedVars,
        unsigned int uid)
{
        unsigned int l, r;
        l = 0;
        r = sortedVars.getLength();
        while(l < r) {
                unsigned int m = (l + r) >> 1;
                unsigned int varUID = sortedVars[m]->getUID();
                if(varUID < uid) {
                        l = m + 1;
                } else if(varUID > uid) {
                        r = m;
                } else {
                        return m;
                }
        }
        return l;
}


unsigned int BFIGGenerator::getBFIGVariable(unsigned int bmVarUID, 
        BFIGVariable * &bfigVar)
{
        unsigned int index = bfindBMVariable(mSearchBMVars, bmVarUID);
        if(index >= mSearchBMVars.getLength() ||
                 mSearchBMVars[index]->getUID() != bmVarUID) {
                fprintf(stderr, "BFIGGenerator::getBFIGVariable: Warning, variable %d "
                        "was not found\n", bmVarUID);
                return 1;
        }
        bfigVar = compactBFIGVars[index];
        if(bfigVar == NULL)
        {
                fprintf(stderr, "BFIGGenerator::getBFIGVariable: Warning, variable %d "
                        "(BFIG Variable %d) was NULL\n", bmVarUID, index);
                return 1;
        }
        return 0;
}


vector<double>
BFIGGenerator::extractVariableProbabilities(BaseModelVariable const &myVariable, 
                unsigned int index, IntegerArray const &valueMap, vector<double> const &allProbabilities) const
{
        vector<double> myProbabilities;
        unsigned int numValues = myVariable.getDomain()->getLength();

        unsigned int probsIndex = valueMap[index];                      // Shouldn't have to use index... should be myVariable.getUID()
        unsigned int probsOffs;

        for(probsOffs = 0; probsOffs < numValues && 
                probsIndex + probsOffs < allProbabilities.size(); probsOffs++) {
                myProbabilities.push_back(allProbabilities[probsIndex + probsOffs]);
        }

        if(probsOffs < numValues) {
                fprintf(stderr, "Error, the variable %d %s does not have an "
                        "associated probability for %d values\n", myVariable.getUID(),
                        myVariable.getName(), numValues - probsOffs);
        }

        for(; probsOffs < numValues; probsOffs++) {
                myProbabilities.push_back(0);
        }

        return myProbabilities;
}

---