/Users/fpy/titan-1-5-MBARI/Mers/Generators/BFIG/BFIG.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 "BFIG.h"

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

#include <cstdlib>
#include <cassert>
#include "BFIGImplicant.h"
#include "BFIGImplicantNoMPI.h"
#include "BFIGConstraint.h"
#include "BFIGVariable.h"
#include "BFIGAssignment.h"
#include "BFIGObjectiveFunction.h"

using namespace Mers::Generators::BFIG;
using Mers::Generators::BFIG::BFIG;
using Mers::Generators::BFIG::BFIGVariable;
using Mers::Generators::BFIG::BFIGConstraint;
using Mers::Generators::BFIG::BFIGImplicant;
using log4cplus::LogLevel;

using Mers::Utils::Lang::String;

using std::vector;
using std::endl;



#ifdef _DEBUG
const unsigned int bfigInitialDebugState = 10;
#else
const unsigned int bfigInitialDebugState = 10;
#endif


Logger BFIG::mLogger(Logger::getInstance("Mers.Generators.BFIG.BFIG"));


BFIG::BFIG(std::vector<BFIGVariable *> const & variableSet, double cutoffCost, 
                BFIGObjectiveFunction const * of)
: mName("Best-First Implicant Generator"),
        mVariables(variableSet), mObjectiveFunction(of), mConflicts(), 
        mImplicantQueue(false, cutoffCost, true), mCandidate(NULL),
        mNodesExpanded(0), mLargestQueue(1), mLargestQueueSinceReset(1),
        mDebug(bfigInitialDebugState), mNumSolutions(0), mNumCandidatesSinceReset(0)
{
        setDebug(bfigInitialDebugState);
}


BFIG::BFIG(vector<BFIGVariable *> const & variableSet, double cutoffCost)
: mName("Best-First Implicant Generator"),
        mVariables(variableSet), mObjectiveFunction(NULL), mConflicts(), 
        mImplicantQueue(false, cutoffCost, true), mCandidate(NULL),
        mNodesExpanded(0), mLargestQueue(1), mLargestQueueSinceReset(1),
        mDebug(bfigInitialDebugState), mNumSolutions(0), mNumCandidatesSinceReset(0)
{
        setDebug(bfigInitialDebugState);
}


BFIG::BFIG(vector<BFIGVariable *> const & variableSet)
: mName("Best-First Implicant Generator"),
        mVariables(variableSet), mObjectiveFunction(NULL), mConflicts(), 
        mImplicantQueue(false, true), mCandidate(NULL),
        mNodesExpanded(0), mLargestQueue(1), mLargestQueueSinceReset(1),
        mDebug(bfigInitialDebugState), mNumSolutions(0), mNumCandidatesSinceReset(0)
{
        setDebug(bfigInitialDebugState);
}


BFIG::~BFIG()
{
        // should be taken care of by caller, but isn't:
        vector<BFIGConstraint const *>::const_iterator cIter;
        for (cIter = mConflicts.begin(); cIter != mConflicts.end(); cIter++)
        {
                if (*cIter)
                        delete (*cIter);
        }

        mImplicantQueue.makeEmpty();

        if(mCandidate != NULL)
                delete mCandidate;

#ifdef _DEBUG
  //printStatistics(mLogger);
#endif
}


String const
BFIG::getName() const
{
        return mName;
}


Mers::Generators::BFIG::BFIGVariable *
BFIG::getVariableByName(String variableName)  //const
{
        vector<BFIGVariable *>::iterator variableIter;

        for (variableIter = mVariables.begin(); variableIter != mVariables.end();
                 variableIter++)
        {
                if (variableName == (*variableIter)->getNameString())
                        return (*variableIter);
        }

        return NULL;
}


Mers::Generators::BFIG::BFIGVariable *
BFIG::getVariableByName(unsigned int variableName)  //const
{
        vector<BFIGVariable *>::iterator variableIter;

        for (variableIter = mVariables.begin(); variableIter != mVariables.end();
                 variableIter++)
        {
                if (variableName == (*variableIter)->getName())
                        return (*variableIter);
        }

        return NULL;
}


vector<Mers::Generators::BFIG::BFIGVariable *>
BFIG::getVariables() const
{
        return mVariables;
}


void
BFIG::addConflict(BFIGConstraint const * newConflict)
{
        mConflicts.push_back(newConflict);

        LOG4CPLUS_DEBUG(mLogger, "Added conflict: " << *newConflict);

        if (mImplicantQueue.isEmpty() && mCandidate == NULL)
        {
                LOG4CPLUS_ERROR_STR(mLogger, "Adding conflict to empty BFIG tree!");

                BFIGImplicant * newImp = createNewImplicant(mConflicts);
                mImplicantQueue.insert(newImp);
        } else {
                unsigned int x;
                for(x = 0; x < mImplicantQueue.getLength(); x++){
                        mImplicantQueue[x]->addUnentailedConflict(newConflict);
                }

                // Don't forget to add it to our current candidate.
                if(mCandidate != NULL) {
                        mCandidate->addUnentailedConflict(newConflict);
                }
        }
}


void
BFIG::addConflicts(vector<BFIGConstraint const *> const & newConflicts)
{
        LOG4CPLUS_DEBUG_STR(mLogger, "Added conflicts:");

        vector<BFIGConstraint const *>::const_iterator cIter;
        for (cIter = newConflicts.begin(); cIter != newConflicts.end(); cIter++)
        {
                assert(*cIter);
                mConflicts.push_back(*cIter);

                LOG4CPLUS_DEBUG(mLogger, **cIter);
        }

        if (mImplicantQueue.isEmpty() && mCandidate == NULL)
        {
                LOG4CPLUS_ERROR_STR(mLogger, "Adding conflicts to empty BFIG tree!");

                BFIGImplicant * newImp = createNewImplicant(mConflicts);
                mImplicantQueue.insert(newImp);
        } else {
                unsigned int x;
                for(x = 0; x < mImplicantQueue.getLength(); x++){
                        mImplicantQueue[x]->addUnentailedConflicts(newConflicts);
                }

                // Don't forget to add it to our current candidate.
                if(mCandidate != NULL) {
                        mCandidate->addUnentailedConflicts(newConflicts);
                }
        }
}


Mers::Generators::BFIG::BFIGImplicant const *
BFIG::getBestImplicant(long maximumVisitedStateCount, long maxQueueSize)
{
        LOG4CPLUS_TRACE(mLogger, "BFIG::getBestImplicant(" 
                << maximumVisitedStateCount << ", " << maxQueueSize
                << ") Looking for next best implicant.  Conflicts(" << mConflicts.size() << ")");

        if(mCandidate != NULL) {
                insertExtensions(mCandidate);
                delete mCandidate;
                mCandidate = NULL;
        }

        if (mImplicantQueue.isEmpty())
        {
                return NULL;
        }

        searchForImplicant(maximumVisitedStateCount, maxQueueSize);

        if (mCandidate == NULL)
        {
                // exhausted search - failure to find a candidate
                return NULL;
        }

        if (!mCandidate->entailsAllConflicts())
        {
                LOG4CPLUS_DEBUG_STR(mLogger, "Solution found did not satisfy the "
                        "conflicts.");

                delete mCandidate;
                mCandidate = NULL;

                return NULL;
        }
        mNumCandidatesSinceReset++;
        return mCandidate;
}


void
BFIG::markBestFeasible()
{
        if(mCandidate == NULL)
                return;
        
        mNumSolutions++;
        insertSiblings(mCandidate);
        delete mCandidate;
        mCandidate = NULL;
}


void BFIG::restart()
{
        LOG4CPLUS_TRACE_STR(mLogger, "Restarting BFIG");
        BFIGImplicant * initImp = createNewImplicant(mConflicts);
        mImplicantQueue.makeEmpty();
        mImplicantQueue.insert(initImp);
        if(mCandidate != NULL) {
                delete mCandidate;
                mCandidate = NULL;
        }
}


void BFIG::reset()
{
        LOG4CPLUS_TRACE_STR(mLogger, "Resetting BFIG");
        vector<BFIGConstraint const *>::const_iterator cIter;
        for (cIter = mConflicts.begin(); cIter != mConflicts.end(); cIter++)
        {
                if (*cIter)
                        delete (*cIter);
        }
        mConflicts.resize(0);
        mLargestQueueSinceReset = 1;
        mNumCandidatesSinceReset = 0;

        restart();
}


unsigned int BFIG::getNumberOfConflicts()
{
        return mConflicts.size();
}



BFIGImplicant * 
BFIG::createNewImplicant(std::vector<BFIGConstraint const *> &con)
{
        if(mObjectiveFunction != NULL)
        {
                if(!mObjectiveFunction->isMPI())
                {       // Create a non-MPI BFIG Implicant
                        return new BFIGImplicantNoMPI(con, mObjectiveFunction);
                }
        }
        //std::cout << "Creating Implicant" << std::endl;
        // The objective function is MPI, use regular BFIGImplicant
        return new BFIGImplicant(con, mObjectiveFunction);
}

void
BFIG::enqueueImplicants(vector<BFIGImplicant *> const & newImplicants)
{
        vector<BFIGImplicant *>::const_iterator implicantIter;
        for (implicantIter = newImplicants.begin();
             implicantIter != newImplicants.end();
             implicantIter++)
        {
                assert(*implicantIter);
                enqueueImplicant(*implicantIter);
        }

        if (mImplicantQueue.getLength() > mLargestQueue)
                mLargestQueue = mImplicantQueue.getLength();

        if (mImplicantQueue.getLength() > mLargestQueueSinceReset)
                mLargestQueueSinceReset = mImplicantQueue.getLength();

        if (mDebug > 0)
        {
                LOG4CPLUS_DEBUG(mLogger, "---- Search State: "); 
                printQueue(mLogger);
                printStatistics(mLogger);
                LOG4CPLUS_DEBUG(mLogger, "----");
        }
}


void
BFIG::enqueueImplicant(BFIGImplicant * imp)
{
        assert(imp);
        mImplicantQueue.insert(imp);

        if (mImplicantQueue.getLength() > mLargestQueue)
                mLargestQueue = mImplicantQueue.getLength();

        if (mImplicantQueue.getLength() > mLargestQueueSinceReset)
                mLargestQueueSinceReset = mImplicantQueue.getLength();
}


unsigned int
BFIG::getLargestQueue() const
{
        return mLargestQueue;
}


unsigned int
BFIG::getLargestQueueSinceReset() const
{
        return mLargestQueueSinceReset;
}


unsigned int
BFIG::getNodesExpanded() const
{
        return mNodesExpanded;
}


unsigned int
BFIG::getNumberCandidatesSinceReset() const
{
        return mNumCandidatesSinceReset;
}


void
BFIG::setDebug(unsigned int debugState)
{
        mDebug = debugState;
        LOG4CPLUS_DEBUG(mLogger, "Debug Level set to " << mDebug);
}


void
BFIG::printStatistics(bool verbose) const
{
        printStatistics(mLogger, verbose);
}


void
BFIG::printStatistics(Logger logger, bool verbose, 
        LogLevel logLevel) const
{
#if !defined(LOG4CPLUS_DISABLE_FATAL)
        if(mLogger.isEnabledFor(logLevel)) {
                log4cplus::tostringstream log4cplus_buf;

                if(verbose) {
                        size_t maxQueue = getLargestQueue() * 
                                (sizeof(BFIGImplicant) + sizeof(BFIGImplicant*));

                        log4cplus_buf << "Nodes expanded: " << getNodesExpanded() << endl;
                        log4cplus_buf << "Max queue size: " << getLargestQueue() << " (" << 
                                maxQueue << " bytes)" << endl;
                        log4cplus_buf << "Conflicts generated: " << mConflicts.size() << endl;
                        log4cplus_buf << "Feasible implicants generated: " << mNumSolutions;
                } else {
                        log4cplus_buf << "Expanded Nodes(" << getNodesExpanded() <<
                                "); Max Queue(" << getLargestQueue() << "); Conflicts(" <<
                                mConflicts.size() << "); Candidates(" << mNumSolutions << ");";
                }

                logger.forcedLog(logLevel, log4cplus_buf.str(), __FILE__, __LINE__);
        }
#else
        (void)verbose;
        (void)logger;
        (void)logLevel;
#endif
}


void
BFIG::printQueue(bool verbose) const
{
        printQueue(mLogger, verbose);
}


void
BFIG::printQueue(Logger logger, bool verbose, LogLevel logLevel) const
{
#if !defined(LOG4CPLUS_DISABLE_FATAL)
        if(mLogger.isEnabledFor(logLevel)) {
                unsigned int printLimit = 10;
                log4cplus::tostringstream log4cplus_buf;

                log4cplus_buf << "Implicant Queue:" << endl;
        
                for (unsigned int x = 0; 
                                 x < mImplicantQueue.getLength() && (verbose || x < printLimit); x++)
                {
                        if(x != 0)
                                log4cplus_buf << endl;
                        log4cplus_buf << "    " << (*(mImplicantQueue[x]));
                }
                if (!verbose && mImplicantQueue.getLength() > printLimit)
                {
                        log4cplus_buf << endl << "    " << "..." << '(' 
                                << mImplicantQueue.getLength() << ')';
                }

                logger.forcedLog(logLevel, log4cplus_buf.str(), __FILE__, __LINE__);
        }
#else
        (void)logger;
        (void)verbose;
        (void)logLevel;
#endif
}


void BFIG::insertSiblings(BFIGImplicant * sibling)
{
        BFIGImplicant * newSibling;

        newSibling = sibling->generateSibling();


        if (newSibling != NULL) {
                LOG4CPLUS_DEBUG(mLogger, *sibling << " has the sibling:" << endl << 
                        "    " << *newSibling);

                enqueueImplicant(newSibling);
        } else {
                if (mDebug > 0) {
                        LOG4CPLUS_DEBUG(mLogger, *sibling << " has no siblings.");
                }
        }
}


void BFIG::insertChildren(BFIGImplicant * parent)
{
        vector<BFIGImplicant *> children;

        // give variables so we can extend even without conflicts.
        children = parent->generateChildren(&mVariables);


        if (!children.empty()) {
                if(mDebug > 0) {
                        LOG4CPLUS_DEBUG(mLogger, *parent << " has the children:");
                        unsigned int x;

                        for(x = 0; x < children.size(); x++) {
                                LOG4CPLUS_DEBUG(mLogger, *children[x]);
                        }
                }

                enqueueImplicants(children);
        } else {
                if (mDebug > 0) {
                        LOG4CPLUS_DEBUG(mLogger, *parent << " has no children.");
                }
        }
}


void BFIG::insertExtensions(BFIGImplicant * node)
{
        insertSiblings(node);
        insertChildren(node);
}

---