/*
 * MaxWalkSat.cpp
 *
 *  Created on: Mar 19, 2012
 *      Author: selman.joe@gmail.com
 */

#include "MaxWalkSat.h"
#include "../logic/syntax/ELSentence.h"
#include "../logic/Domain.h"
#include "../logic/Moves.h"
#include <boost/random/uniform_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/unordered_map.hpp>
#include <limits>

const unsigned int MWSSolver::defNumIterations = 1000;
const double MWSSolver::defProbOfRandomMove = 0.2;

Model MWSSolver::run(boost::mt19937& rng) {
    if (domain_ == NULL) {
        std::logic_error e("unable to run MWSSolver with Domain set to null ptr");
        throw e;
    }
    return run(rng, domain_->defaultModel());
}

namespace {
// we will calculate the resulting model and score for all
// moves, and choose the highest scoring one to move to.  This struct
// makes it a lot more convenient to do.
struct MWSState {
    Move move;
    Model model;
    double score;
    // the three below here are copies of the versions above
    std::vector<bool> localFormNeedUpdates;
    std::vector<double> localScores;
    std::vector<bool> localFormFullySat;
};
}

Model MWSSolver::run(boost::mt19937& rng, const Model& initialModel) {
    // validate domain
    if (domain_ == NULL) {
        std::logic_error e("unable to run MWSSolver with Domain set to null ptr");
        throw e;
    }
    // copy the domain's sentences into our own
    std::vector<ELSentence> formulas;
    std::copy(domain_->formulas_begin(), domain_->formulas_end(), std::back_inserter(formulas));

    if (formulas.size() == 0) {
        LOG(LOG_WARN) << "no formulas given - returning initial model";
        return initialModel;
    }

    // now note which sentences have valid moves
    std::vector<bool> formWithMoves(formulas.size(), false);
    for (std::vector<ELSentence>::size_type i = 0;
            i < formulas.size();
            i++) {
        // infinite weight domains are invalid
        if (formulas[i].hasInfWeight()) {
            std::logic_error e("unable to deal with infinitely weighted sentences.");
            throw e;
        }
        if (!canFindMovesFor(*formulas[i].sentence(), *domain_)) {
            LOG(LOG_WARN) << "currently cannot generate moves for sentence: \"" << formulas[i].sentence()->toString() << "\".  ignoring it for generating moves";
            formWithMoves[i] = false;
        } else {
            formWithMoves[i] = true;
        }
    }

    // now record which sentences contain each atom - this is used to cache
    // the results of previous satisfied sentences.
    boost::unordered_map<Atom, std::vector<std::vector<ELSentence>::size_type > > atomToSentence;
    for (std::vector<ELSentence>::size_type i = 0;
            i != formulas.size();
            i++) {
        AtomCollector collector;
        formulas[i].sentence()->visit(collector);
        for (AtomCollector::atom_set::const_iterator atomit = collector.atoms.begin();
                atomit != collector.atoms.end();
                atomit++) {
            atomToSentence[*atomit].push_back(i);
        }
    }

    // setup stores for the score as well as whether each sentence is fully satisfied
    std::vector<double> formScores(formulas.size(), 0.0);
    std::vector<bool> formFullySat(formulas.size(), false);
    // also setup a vector we will use to mark which scores need updating
    std::vector<bool> formNeedUpdates(formulas.size(), true);

    updateScores(formulas, initialModel, formNeedUpdates, formScores, formFullySat);

    double currentScore = std::accumulate(formScores.begin(), formScores.end(), 0.0);
    double bestScore = currentScore;
    Model currentModel = initialModel;
    Model bestModel = initialModel;

    unsigned int showPeriodMod = (numIterations_ < 20 ? 1 : numIterations_/20); // TODO: make this configurable
    for (unsigned int iteration=1; iteration <= numIterations_; iteration++) {
        if (iteration % showPeriodMod == 0) {
            std::cout << ".";
            std::cout.flush();
        }
        LOG(LOG_DEBUG) << "currentModel: " << currentModel;
        LOG(LOG_DEBUG) << "current score: " << currentScore;

        // first, check to see if all our formulas are fully satisfied - if
        // that's the case, there's no real improvement left

        // At the same time build a list of formulas we can find moves for
        // that are not fully satisfied
        bool noImprovementLeft = true;
        std::vector<std::size_t> formCandidates;
        for (std::vector<bool>::size_type i = 0;
                i < formulas.size();
                i++) {
            if (!formFullySat[i]) {
                noImprovementLeft = false;
                if (formWithMoves[i]) {
                    formCandidates.push_back(i);
                }
            }
        }
        if (noImprovementLeft) {
            LOG(LOG_DEBUG) << "exiting early, no more sentences to satisfy!";
            return currentModel;
        }
        // if formCandidates is empty, then we aren't fully satisfied but we
        // can't generate moves for any valid sentences right now.  log a
        // warning and generate a random move
        Move nextMove;
//        std::cout << "candidates = ";
//        for (std::vector<std::size_t>::const_iterator it = formCandidates.begin();
//                it != formCandidates.end();
//                it++) {
//            std::cout << formulas[*it].toString() << ", ";
//        }
//        std::cin.get();

        if (formCandidates.empty()) {
            LOG(LOG_WARN) << "cannot generate any moves for the current model (not all formulas are currently handled) - generating a random move";
            // pick a random atom
            boost::uniform_int<std::size_t> atomPick(0, domain_->atoms_size()-1);
            std::size_t atomInd = atomPick(rng);
            Domain::atom_const_iterator atomIt = domain_->atoms_begin();
            while (atomInd > 0) {
                atomIt++;
                atomInd--;
            }

            Atom atom = *atomIt;
            // now pick a spanning interval

            Interval maxInterval = domain_->maxInterval();
            boost::uniform_int<unsigned int> momentPick(maxInterval.start(), maxInterval.finish());
            unsigned int start = momentPick(rng);
            unsigned int finish = momentPick(rng);
            if (start > finish) std::swap(start, finish);

            SpanInterval si;
            if (domain_->isLiquid(atom.name())) {
                si = SpanInterval(start, finish);
            } else {
                si = SpanInterval(start, start, finish, finish);
            }
            // make a random flip to add/subtract
            Move::change ch(atom, si);
            boost::bernoulli_distribution<> flip(0.5);
            if (flip(rng)) {
                nextMove.toAdd.push_back(ch);
            } else {
                nextMove.toDel.push_back(ch);
            }
        } else {
            // choose a formula to improve at random
            boost::uniform_int<std::size_t> formulaPick(0, formCandidates.size()-1);
            std::size_t formChoseInd = formulaPick(rng);
            ELSentence formula = formulas[formCandidates[formChoseInd]];
            LOG(LOG_DEBUG) << "choosing formula: " << formula << " to improve.";

            // find the moves for it
            std::vector<Move> moves = findMovesFor(*domain_, currentModel, formula, rng);
            if (moves.size() == 0) {

                LOG(LOG_WARN) << "WARNING: unable to find moves for sentence " << formula.sentence()->toString()
                        << " even though its violated!  continuing (with a null iteration)...";
                continue; // TODO: this shouldn't happen, right?
            }
            if (FileLog::globalLogLevel() <= LOG_DEBUG) {
                std::ostringstream vecStream;
                for (std::vector<Move>::const_iterator it = moves.begin(); it != moves.end(); it++) {
                    if (it != moves.begin()) vecStream << ", ";
                    vecStream << "(" << it->toString() << ")";
                }
                LOG(LOG_DEBUG) << "moves to consider: " << vecStream.str();
            }

            boost::bernoulli_distribution<> randMovePick(probOfRandomMove_);
            if (randMovePick(rng)) {
                // take a random move
                boost::uniform_int<std::size_t> movesPick(0, moves.size()-1);
                Move aMove = moves[movesPick(rng)];
                LOG(LOG_DEBUG) << "taking random move: " << aMove.toString();

                currentModel = updateWithMove(aMove, currentModel, atomToSentence, formNeedUpdates);
                // update scores
                updateScores(formulas, currentModel, formNeedUpdates, formScores, formFullySat);
                currentScore = std::accumulate(formScores.begin(), formScores.end(), 0.0);
            } else {
                // instead of choosing a random move, choose the move that leads to the
                // highest scoring move.

                // we will calculate the resulting model and score for all
                // moves, and choose the highest scoring one to move to
                MWSState bestMWSState;

                bestMWSState.score = std::numeric_limits<double>::min();    // the lowest value possible
                // also have a vector of best models in case of ties
                std::vector<MWSState> ties;

                //std::vector<std::pair<Model, double> > nearbyModels;
                for (std::vector<Move>::const_iterator it = moves.begin(); it != moves.end(); it++) {
                    Move m = *it;
                    std::vector<bool> localFormNeedUpdates(formNeedUpdates);
                    Model nearbyModel = updateWithMove(m, currentModel, atomToSentence, localFormNeedUpdates);

                    std::vector<double> localScores(formScores);
                    std::vector<bool> localFormFullySat(formFullySat);
                    updateScores(formulas, nearbyModel, localFormNeedUpdates, localScores, localFormFullySat);
                    double nearbyScore = std::accumulate(localScores.begin(), localScores.end(), 0.0);

                    if (nearbyScore > bestMWSState.score) {
                        // save it
                        bestMWSState.move = m;
                        bestMWSState.model = nearbyModel;
                        bestMWSState.score = nearbyScore;
                        bestMWSState.localFormNeedUpdates = localFormNeedUpdates;
                        bestMWSState.localScores = localScores;
                        bestMWSState.localFormFullySat = localFormFullySat;

                        ties.clear();
                        ties.push_back(bestMWSState);
                    } else if (nearbyScore == bestMWSState.score) {
                        // found a tie
                        MWSState tieState;
                        tieState.move = m;
                        tieState.model = nearbyModel;
                        tieState.score = nearbyScore;
                        tieState.localFormNeedUpdates = localFormNeedUpdates;
                        tieState.localScores = localScores;
                        tieState.localFormFullySat = localFormFullySat;

                        ties.push_back(tieState);
                    }
                }
                assert(bestMWSState.score > std::numeric_limits<double>::min());
                if (ties.size() > 1) {
                    // pick a choice randomly
                    boost::uniform_int<std::size_t> tieChoice(0, ties.size()-1);
                    bestMWSState = ties[tieChoice(rng)];
                }
                LOG(LOG_DEBUG) << "taking move " << bestMWSState.move.toString();
                currentModel = bestMWSState.model;
                currentScore = bestMWSState.score;
                formNeedUpdates = bestMWSState.localFormNeedUpdates;
                formScores = bestMWSState.localScores;
                formFullySat = bestMWSState.localFormFullySat;
            }
        }
        // check to see if its the best score foudn so far
        if (currentScore > bestScore) {
            LOG(LOG_DEBUG) << "remembering this model as the best one seen so far.";
            bestScore = currentScore;
            bestModel = currentModel;
        }

    }
    LOG(LOG_INFO) << "returning the best model found with a score of " << bestScore;
    return bestModel;

//    for (int iteration=1; iteration <= numIterations; iteration++) {
//        if (iteration % showPeriodMod == 0) {
//            std::cout << ".";
//            std::cout.flush();
//        }
//        LOG(LOG_DEBUG) << "currentModel: " << currentModel;
//        LOG(LOG_DEBUG) << "current score: " << currentScore;
//
//        datalog << currentScore;
//
//        // make a list of the current unsatisfied formulas we can calc moves for
//        std::vector<int> notFullySatisfied = validForms;
//        std::vector<ELSentence> curFormulas = formulas;
//
//        for (std::vector<int>::iterator it = notFullySatisfied.begin(); it != notFullySatisfied.end(); ) {
//            int i = *it;
//
//            ELSentence wsent = curFormulas.at(i);
//            //const WSentence *wsentence = *it;
//            if (wsent.fullySatisfied(currentModel, d)) {
//                it = notFullySatisfied.erase(it);
//            } else {
//                it++;
//            }
//        }
//
//        if (notFullySatisfied.size()==0) {
//            // can't really improve on this
//            LOG(LOG_INFO) << "no more sentences to satisfy!  exiting early after "<< iteration-1 << " iterations";
//            return currentModel;
//        }
//
//        // pick one at random
//        boost::uniform_int<std::size_t> curFormUniformPick(0, notFullySatisfied.size()-1);
//        ELSentence toImprove = curFormulas.at(notFullySatisfied.at(curFormUniformPick(rng)));
//        LOG(LOG_DEBUG) << "choosing formula: " << toImprove << " to improve.";
//        // find the set of moves that improve it
//        std::vector<Move> moves = findMovesFor(d, currentModel, toImprove, rng);
//        if (moves.size() == 0) {
//            LOG(LOG_WARN) << "WARNING: unable to find moves for sentence " << toImprove.sentence()->toString()
//                    << " but couldn't find any (even though its violated)!  continuing...";
//            continue; // TODO: this shouldn't happen, right?
//        }
//        if (FileLog::globalLogLevel() <= LOG_DEBUG) {
//            std::ostringstream vecStream;
//            for (std::vector<Move>::const_iterator it = moves.begin(); it != moves.end(); it++) {
//                if (it != moves.begin()) vecStream << ", ";
//                vecStream << "(" << it->toString() << ")";
//            }
//            LOG(LOG_DEBUG) << "moves to consider: " << vecStream.str();
//        }
//        boost::bernoulli_distribution<> randMovePick(probOfRandomMove);
//        if (randMovePick(rng)) {
//            // take a random move
//            boost::uniform_int<std::size_t> movesPick(0, moves.size()-1);
//            Move aMove = moves[movesPick(rng)];
//            LOG(LOG_DEBUG) << "taking random move: " << aMove.toString();
//            currentModel = executeMove(d, aMove, currentModel);
//            score_pair scorePair = computeScoresForMove(d, currentModel, aMove, currentScore, formScores, occurs);
//            currentScore = scorePair.totalScore;
//            formScores = scorePair.formScores;
//        } else {
//            // find the models resulting from each move, and choose the highest scoring model as our next model
//            double bestLocalScore = 0.0;
//            std::vector<Model> bestLocalModels;
//            std::vector<Move> bestLocalMoves;
//            std::vector<score_pair> bestLocalScorePairs;
//
//            //bestLocalModels.push_back(currentModel);
//            for (std::vector<Move>::const_iterator it=moves.begin(); it != moves.end(); it++) {
//                Model nextModel = executeMove(d, *it, currentModel);
//                score_pair scorePair = computeScoresForMove(d, nextModel, *it, currentScore, formScores, occurs);
//                double nextScore = scorePair.totalScore;
//                if (nextScore > bestLocalScore) {
//                    bestLocalModels.clear();
//                    bestLocalMoves.clear();
//                    bestLocalScorePairs.clear();
//
//                    bestLocalScore = nextScore;
//                    bestLocalModels.push_back(nextModel);
//                    bestLocalMoves.push_back(*it);
//                    bestLocalScorePairs.push_back(scorePair);
//                } else if (nextScore == bestLocalScore) {
//                    bestLocalModels.push_back(nextModel);
//                    bestLocalMoves.push_back(*it);
//                    bestLocalScorePairs.push_back(scorePair);
//                }
//            }
//            boost::uniform_int<std::size_t> modelPick(0, bestLocalModels.size()-1);
//            int idx = modelPick(rng);  // choose one at random
//            currentModel = bestLocalModels[idx];
//            score_pair scorePair = bestLocalScorePairs[idx];
//            currentScore = scorePair.totalScore;
//            formScores = scorePair.formScores;
//            LOG(LOG_DEBUG) << "choosing best local move: " << bestLocalMoves[idx].toString();
//        }
//        // evaluate and see if our model is better than any found so far
//        if (currentScore > bestScore) {
//            LOG(LOG_DEBUG) << "remembering this model as best scoring so far";
//            bestModel = currentModel;
//            bestScore = currentScore;
//        }
//    }
//
//    return bestModel;

    std::runtime_error e("MWSSolver::run() not implemented.");
    throw e;
}

Model MWSSolver::updateWithMove(const Move& m,
        const Model& currentModel,
        const boost::unordered_map<Atom, std::vector<std::vector<ELSentence>::size_type > >& atomMap,
        std::vector<bool>& formsNeedUpdate) {
    // scan over all atoms in the move - if its being modified, mark the formula as needing update
    boost::unordered_set<Atom> moveAtoms;
    for (std::vector<Move::change>::const_iterator it = m.toAdd.begin();
            it != m.toAdd.end();
            it++) {
        moveAtoms.insert(it->get<0>());
    }
    for (std::vector<Move::change>::const_iterator it = m.toDel.begin();
            it != m.toDel.end();
            it++) {
        moveAtoms.insert(it->get<0>());
    }

    for (boost::unordered_set<Atom>::const_iterator it = moveAtoms.begin();
            it != moveAtoms.end();
            it++) {
        Atom a = *it;
        if (atomMap.count(a)) {
            // TODO: loop over all sentences
            std::vector<std::vector<ELSentence>::size_type > formsWithAtom = atomMap.at(a);
            for (std::vector<std::vector<ELSentence>::size_type >::const_iterator atomIt = formsWithAtom.begin();
                    atomIt != formsWithAtom.end();
                    atomIt++) {
            formsNeedUpdate[*atomIt] = true;
            }
        }
    }

    // now execute the move
    return executeMove(*domain_, m, currentModel);
}

/*
Model maxWalkSat(Domain& d,
        int numIterations,
        double probOfRandomMove,
        boost::mt19937& rng,
        const Model* initialModel,
        std::ostream* dataout) {
    row_out datalog(dataout);

    Model currentModel(d.maxInterval());
    if (initialModel==0) currentModel = d.defaultModel();
    else currentModel = *initialModel;

    // filter out sentences we can't currently generate moves for
    std::vector<int> validForms;
    //std::vector<int> validNorm;
    std::vector<ELSentence> formulas(d.formulas_begin(), d.formulas_end());
    //std::vector<ELSentence> formulas = formSet.formulas();
    for (std::vector<ELSentence>::size_type i = 0; i < formulas.size(); i++) {
        ELSentence form = formulas[i];
        if (form.hasInfWeight()) {
            throw std::invalid_argument("maxWalkSat(): can't solve a problem with infinite weights - rewrite first");
        }
        if (canFindMovesFor(*(form.sentence()), d)) {
            validForms.push_back(i);
        } else {
            // TODO: use a logging warning instead of stderr
            //std::cerr << "WARNING: currently cannot generate moves for sentence: \"" << d.formulas().at(i).sentence()->toString() << "\"." << std::endl;
            LOG(LOG_WARN) << "currently cannot generate moves for sentence: \"" <<form.sentence()->toString() << "\".";
        }
    }
    if (validForms.size() ==0) {
        // TODO: log an error
        std::cerr << "ERROR: no valid sentences to generate moves for!" << std::endl;
        return currentModel;
    }

    AtomOccurences occurs = findAtomOccurences(formulas);
    std::vector<double> formScores;
    double currentScore = 0.0;
    for (unsigned int i = 0; i < formulas.size(); i++) {
        ELSentence formula = formulas[i];
        double localScore = d.score(formula, currentModel);
        formScores.push_back(localScore);
        currentScore += localScore;
    }

    // initialize best score to the current score
    double bestScore = currentScore;
    Model bestModel = currentModel;

    unsigned int showPeriodMod = (numIterations < 20 ? 1 : numIterations/20);

    for (int iteration=1; iteration <= numIterations; iteration++) {
        if (iteration % showPeriodMod == 0) {
            std::cout << ".";
            std::cout.flush();
        }
        LOG(LOG_DEBUG) << "currentModel: " << currentModel;
        LOG(LOG_DEBUG) << "current score: " << currentScore;

        datalog << currentScore;

        // make a list of the current unsatisfied formulas we can calc moves for
        std::vector<int> notFullySatisfied = validForms;
        std::vector<ELSentence> curFormulas = formulas;

        for (std::vector<int>::iterator it = notFullySatisfied.begin(); it != notFullySatisfied.end(); ) {
            int i = *it;

            ELSentence wsent = curFormulas.at(i);
            //const WSentence *wsentence = *it;
            if (wsent.fullySatisfied(currentModel, d)) {
                it = notFullySatisfied.erase(it);
            } else {
                it++;
            }
        }

        if (notFullySatisfied.size()==0) {
            // can't really improve on this
            LOG(LOG_INFO) << "no more sentences to satisfy!  exiting early after "<< iteration-1 << " iterations";
            return currentModel;
        }

        // pick one at random
        boost::uniform_int<std::size_t> curFormUniformPick(0, notFullySatisfied.size()-1);
        ELSentence toImprove = curFormulas.at(notFullySatisfied.at(curFormUniformPick(rng)));
        LOG(LOG_DEBUG) << "choosing formula: " << toImprove << " to improve.";
        // find the set of moves that improve it
        std::vector<Move> moves = findMovesFor(d, currentModel, toImprove, rng);
        if (moves.size() == 0) {
            LOG(LOG_WARN) << "WARNING: unable to find moves for sentence " << toImprove.sentence()->toString()
                    << " but couldn't find any (even though its violated)!  continuing...";
            continue; // TODO: this shouldn't happen, right?
        }
        if (FileLog::globalLogLevel() <= LOG_DEBUG) {
            std::ostringstream vecStream;
            for (std::vector<Move>::const_iterator it = moves.begin(); it != moves.end(); it++) {
                if (it != moves.begin()) vecStream << ", ";
                vecStream << "(" << it->toString() << ")";
            }
            LOG(LOG_DEBUG) << "moves to consider: " << vecStream.str();
        }
        boost::bernoulli_distribution<> randMovePick(probOfRandomMove);
        if (randMovePick(rng)) {
            // take a random move
            boost::uniform_int<std::size_t> movesPick(0, moves.size()-1);
            Move aMove = moves[movesPick(rng)];
            LOG(LOG_DEBUG) << "taking random move: " << aMove.toString();
            currentModel = executeMove(d, aMove, currentModel);
            score_pair scorePair = computeScoresForMove(d, currentModel, aMove, currentScore, formScores, occurs);
            currentScore = scorePair.totalScore;
            formScores = scorePair.formScores;
        } else {
            // find the models resulting from each move, and choose the highest scoring model as our next model
            double bestLocalScore = 0.0;
            std::vector<Model> bestLocalModels;
            std::vector<Move> bestLocalMoves;
            std::vector<score_pair> bestLocalScorePairs;

            //bestLocalModels.push_back(currentModel);
            for (std::vector<Move>::const_iterator it=moves.begin(); it != moves.end(); it++) {
                Model nextModel = executeMove(d, *it, currentModel);
                score_pair scorePair = computeScoresForMove(d, nextModel, *it, currentScore, formScores, occurs);
                double nextScore = scorePair.totalScore;
                if (nextScore > bestLocalScore) {
                    bestLocalModels.clear();
                    bestLocalMoves.clear();
                    bestLocalScorePairs.clear();

                    bestLocalScore = nextScore;
                    bestLocalModels.push_back(nextModel);
                    bestLocalMoves.push_back(*it);
                    bestLocalScorePairs.push_back(scorePair);
                } else if (nextScore == bestLocalScore) {
                    bestLocalModels.push_back(nextModel);
                    bestLocalMoves.push_back(*it);
                    bestLocalScorePairs.push_back(scorePair);
                }
            }
            boost::uniform_int<std::size_t> modelPick(0, bestLocalModels.size()-1);
            int idx = modelPick(rng);  // choose one at random
            currentModel = bestLocalModels[idx];
            score_pair scorePair = bestLocalScorePairs[idx];
            currentScore = scorePair.totalScore;
            formScores = scorePair.formScores;
            LOG(LOG_DEBUG) << "choosing best local move: " << bestLocalMoves[idx].toString();
        }
        // evaluate and see if our model is better than any found so far
        if (currentScore > bestScore) {
            LOG(LOG_DEBUG) << "remembering this model as best scoring so far";
            bestModel = currentModel;
            bestScore = currentScore;
        }
    }

    return bestModel;
}

namespace {
    AtomOccurences findAtomOccurences(const std::vector<ELSentence>& sentences) {
        // set up a mapping from atom to formula index.  this represents formulas where the atom occurs
        AtomCollector collector;
        AtomOccurences occurs;
        for (std::vector<ELSentence>::size_type i = 0; i < sentences.size(); i++) {
            ELSentence formula = sentences.at(i);

            collector.atoms.clear();
            formula.sentence()->visit(collector);
            // add this index to all occurrences of our atom
            BOOST_FOREACH(Atom a, collector.atoms) {
                FormSet set = occurs[a];
                if (set.count(i) == 0) {
                    set.insert(i);
                    occurs[a] = set;
                }
            }
        }

        return occurs;
    }

    score_pair computeScoresForMove(const Domain& d,
            const Model& m,
            const Move& move,
            double currentScore,
            const std::vector<double>& curFormScores,
            const AtomOccurences& occurs) {
        // first, find the formulas we need to recompute
        std::set<int> formsToRescore;
        std::vector<Move::change> allchanges(move.toAdd);
        std::copy(move.toDel.begin(), move.toDel.end(), std::back_inserter(allchanges));

        BOOST_FOREACH(Move::change change, allchanges) {
            Atom a = change.get<0>();
            if (occurs.count(a) > 0) {
                AtomOccurences::const_iterator it = occurs.find(a);
                FormSet changedForms = it->second;
                std::copy(changedForms.begin(), changedForms.end(), std::inserter(formsToRescore, formsToRescore.end()));
            }
        }

        score_pair pair;
        pair.formScores = curFormScores;
        pair.totalScore = currentScore;

        // start recomputing, adjusting the total score as necessary
        std::vector<ELSentence> formulas(d.formulas_begin(), d.formulas_end());
        for (std::set<int>::const_iterator it = formsToRescore.begin(); it != formsToRescore.end(); it++) {
            int formNum = *it;
            ELSentence sentence = formulas[formNum];

            double score = d.score(sentence, m);
            if (score != curFormScores.at(formNum)) {   // TODO: require some sort of epsilon check?
                double difference = score - curFormScores.at(formNum);
                pair.formScores[formNum] = score;
                pair.totalScore += difference;
            }
        }

        return pair;
    }
}
*/

void MWSSolver::updateScores(const std::vector<ELSentence>& formulas,
        const Model& model,
        std::vector<bool>& whichToUpdate,
        std::vector<double>& scores,
        std::vector<bool>& fullySatisfied) {
//    std::cout << "in MWSSolver::updateScores()" << std::endl;
//
//    std::cout << "fullySatisfied: ";
//    std::copy(fullySatisfied.begin(), fullySatisfied.end(), std::ostream_iterator<bool>(std::cout, ", "));
//    std::cout << std::endl;

    // first, calculate all the SISets for our sentences
    for (std::size_t i = 0;
            i < whichToUpdate.size();
            i++) {

        if (!whichToUpdate[i]) {
//            std::cout << "  asked not to update this formula" << std::endl;
            continue;    // skip elements that don't need updating
        }
        ELSentence formula = formulas[i];

        // find the quantification for the current sentence
        SISet quantification(domain_->maxSpanInterval(), false, domain_->maxInterval());
        if (formula.isQuantified()) {
            quantification = formula.quantification();
        }

        SISet formSat = formula.dSatisfied(model, *domain_);
        // next, overwrite the score for the model
        scores[i] = ((double)formSat.size()) * formula.weight();
        // finally, mark if its completely satisfied
        SISet leftover = quantification;
        leftover.subtract(formSat);
        if (leftover.empty()) {
            fullySatisfied[i] = true;
        } else {
            fullySatisfied[i] = false;
        }
        // done updating!  make a note
        whichToUpdate[i] = false;
    }
//    std::cout << "leaving MWSSolver::updateScores()";
//    std::cout << "fullySatisfied: ";
//    std::copy(fullySatisfied.begin(), fullySatisfied.end(), std::ostream_iterator<bool>(std::cout, ", "));
//    std::cout << std::endl;

}


/*
Copyright or © or Copr. CNRS

This software is a computer program whose purpose is to estimate
phylogenies and evolutionary parameters from a dataset according to
the maximum likelihood principle.

This software is governed by the CeCILL  license under French law and
abiding by the rules of distribution of free software.  You can  use,
modify and/ or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and  rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty  and the software's author,  the holder of the
economic rights,  and the successive licensors  have only  limited
liability.

In this respect, the user's attention is drawn to the risks associated
with loading,  using,  modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean  that it is complicated to manipulate,  and  that  also
therefore means  that it is reserved for developers  and  experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and,  more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.
*/

/**

@file
@author Wandrille Duchemin
@created 06-Nov-2015
@modified 16-Jun-2016

**/

#include <stdlib.h>
#include <time.h>
#include <boost/foreach.hpp>

#include "MyCladesAndTripartitions.h"

//Private functions

/*
Takes:
 - CladeForest * CF : pointer to a cladeForest instance
 - int idU : current idU
 - double probaFixed = 1.0 : probability not to choose the split (hypothetically) described in the CladeForest

Returns:
	(pair <int,int>):  a split of that clade chosen according to the CladeForest or randomly drawn (according to its split ratio)
*//*
pair <int,int> getNextOrRandomClade(CladeForest * CF , int idU , double probaFixed)
{

	pair <int,int> chosenSplit;

	double r = ((double) rand()/ RAND_MAX); //random results
	double ratio;//to store the various ratios

	int nbSplit = getSplitCount(idU);
	int counter;

	bool foundRandom = false;
	bool preferFixed = true;

	double rFixed = ((double) rand()/ RAND_MAX); //random results
	if( rFixed > probaFixed ) // we will chose at random
	{
		preferFixed = false;
	}

	for(counter=0;counter<nbSplit;counter++)//for each split
	{
		if(preferFixed) // we chose the clade from the cladeForest if possible
		{
			pair <int,int> tempresult= getCladeSplit(idU,counter); // check if the clade exists in the CladeForest

			if( ( CF->hasClade(tempresult.first) ) || ( CF->hasClade(tempresult.second) ) ) // found
			{
				chosenSplit = tempresult;
				break;
			}
		}

		if(!foundRandom)
		{
			ratio = getSplitRatio(idU,counter);
			if( r <= ratio)
			{
				chosenSplit = getCladeSplit(idU,counter);//the chosen clade is found and returned
				foundRandom = true;
			}
			r = r - ratio;//not the chosen clade, we update r
		}

		if((foundRandom) && (!preferFixed))
			break; // we found the random choice and won't take the fixed choice -> no need to continue
	}

	return chosenSplit;
}
*/

/**
Takes:
	- int idU : id of a clade

Returns:
	pair<int,int> a split of that clade drawn randomly (according to its split ratio)
**/
pair<int,int> MyCladesAndTripartitions::getRandomClade(int idU)
{

	double r = ((double) rand()/ RAND_MAX); //random results
	double ratio;//to store the various ratios

	int nbSplit = getSplitCount(idU);
	int counter;
	for(counter=0;counter<nbSplit;counter++)//for each split
	{
		ratio = getSplitRatio(idU,counter);
		if( r <= ratio)
			return getCladeSplit(idU,counter);//the chosen clade is found and returned
		r = r - ratio;//not the chosen clade, we update r
	}
	return getCladeSplit(idU,nbSplit - 1);//return the last split

}

/**
Takes:
	- int idU : id of a clade

Returns:
	pair<int,int> a split of that clade that has the maximum split ratio
**/
pair<int,int> MyCladesAndTripartitions::getMaxClade(int idU)
{

	//First, we find the list of all split of clade idU with the maximum ratio
	double maxRatio = 0;
	vector<int> maxIndex;//vector to keep the indexes of the split with the max ratio

	double ratio;

	int nbSplit = getSplitCount(idU);

	//cout << idU << ":" << nbSplit << endl;
	int counter;
	for(counter=0;counter<nbSplit;counter++)//for each split
	{
		ratio = getSplitRatio(idU,counter);
		if(ratio > maxRatio)
		{
			maxRatio = ratio;
			maxIndex.clear();
			maxIndex.push_back(counter);
		}
		else if(ratio == maxRatio)
		{
			maxIndex.push_back(counter);
		}
	}

	if(maxIndex.size() == 1)//There is only one split with the maximum ratio: return it
	{
		return getCladeSplit(idU,maxIndex[0]);
	}

	//There is more than one split: return a random one
	int r = rand() % maxIndex.size();
	return getCladeSplit(idU,maxIndex[r]);

}

/**
Takes:
	- int &pOrd : current depth-first post-order number
	- int idU : current clade in the recursion

Returns:
	MyGeneNode *
**/
MyGeneNode *MyCladesAndTripartitions::getRandomTreeAux(int &pOrd,int idU )
{
    MyGeneNode *node = new MyGeneNode();
    pair<int,int> cladeSplit = getRandomClade( idU );
    if( cladeSplit.first != -1 ) {
        node->addSon( getRandomTreeAux( pOrd, cladeSplit.first ) );
        node->addSon( getRandomTreeAux( pOrd, cladeSplit.second ) );
        node->setBranchProperty(bpp::TreeTools::BOOTSTRAP, bpp::Number<double>(pOrd));
    } else
        node->setName( mClades.getLeafName( idU ) );

    if( mBranchLengths[idU] != -1 )
        node->setDistanceToFather( (mBranchLengths[idU] + 1)/getCladeOccurrences(idU) );

    pOrd++;

    return node;
}

/**
Takes:
	- int &pOrd : current depth-first post-order number
	- int idU : current clade in the recursion

Returns:
	MyGeneNode *
**/
MyGeneNode * MyCladesAndTripartitions::getMaxTreeAux(int &pOrd,int idU )
{
    MyGeneNode *node = new MyGeneNode();
    pair<int,int> cladeSplit = getMaxClade( idU);
    //cout << idU << "->" << cladeSplit.first << "," << cladeSplit.second << endl;
    if( cladeSplit.first != -1 ) {
        node->addSon( getMaxTreeAux( pOrd, cladeSplit.first ) );
        node->addSon( getMaxTreeAux( pOrd, cladeSplit.second ) );
        node->setBranchProperty(bpp::TreeTools::BOOTSTRAP, bpp::Number<double>(pOrd));
    } else
        node->setName( mClades.getLeafName( idU ) );

    if( mBranchLengths[idU] != -1 )
        node->setDistanceToFather( (mBranchLengths[idU] + 1)/getCladeOccurrences(idU) );

    pOrd++;

    return node;

}

/**
RECURSIVE

Takes:
	- MyGeneNode * node: a tree to compute the likelihood from
	- double default_proba: proba to give to a split that is absent from the MyCladesAndTripartitions counts

Returns:
	pair<double,vector<string>>:
		double: likelihood of the node (and underlying subtree) according to the CCPs distribution.
		vector<string>: list of all the leaves in the subtree rooted at node
**/
pair<double,vector<string> > MyCladesAndTripartitions::getTreeLikelihoodAux(MyGeneNode *node, double default_proba)
{

	std::pair<double, vector<string> > toreturn;

	toreturn.first = 1;//set base proba to 1.

	if (node->isLeaf())
	{
		toreturn.second.push_back(node->getName());
		return toreturn;
	}

	int nbson,i, cladeId;

	nbson = node->getNumberOfSons();

	if(nbson ==1)//only one son -> this is a intermediary node of some sort
		return getTreeLikelihoodAux(node->getSon(0),default_proba);// return the value for its only son
	else if(nbson > 2)//more than 2 sons: error
		throw bpp::Exception("MyCladesAndTripartitions::getTreeLikelihoodAux: does not work when the tree is polytomic!");

	//We are now in the caser where there is more than one son

	std::pair<double, vector<string> > tempresult;//to put the result from each son

	std::pair<int,int> split;//split

	//first son
	tempresult = getTreeLikelihoodAux(node->getSon(0),default_proba);//getting the result for this son

	toreturn.first *= tempresult.first;//updating proba

	BOOST_FOREACH( string leafName, tempresult.second)
		toreturn.second.push_back(leafName);//updating leaf list

	split.first = mClades.getCladeIntFromLeafList(tempresult.second);

	tempresult.second.clear();//clearing

	//second son
	tempresult = getTreeLikelihoodAux(node->getSon(1),default_proba);//getting the result for this son

	toreturn.first *= tempresult.first;//updating proba
	BOOST_FOREACH(string leafName, tempresult.second)
		toreturn.second.push_back(leafName);//updating leaf list

	split.second = mClades.getCladeIntFromLeafList(tempresult.second);

	tempresult.second.clear();//clearing

	//now getting current cladeId
	cladeId = mClades.getCladeIntFromLeafList(toreturn.second);

	if(cladeId == -1 )
		toreturn.first *=default_proba;
	else if(split.first != -1 || split.second != -1)//if the son clades are known

		for(i=0 ; i < getSplitCount(cladeId) ; i++)
		{
			pair <int,int> testsplit = getCladeSplit(cladeId,i);
			if(split.first == testsplit.first || split.second == testsplit.first)//found the correct split
			{

				if(cladeId != 0 || mRooted)// special condition for the root clade: should not use split ratio if the distribution is unrooted
					toreturn.first *= getSplitRatio(cladeId,i);
				else if(cladeId == 0)
					toreturn.first *= ((double) getCladeOccurrences(split.first))/ ((double) mGeneTreeCount); //  if we are the "root" clade but the distribution is unrooted, we should multiply by the number of time we saw the child clade (clade probability)
				break;
			}
		}
		if(i == getSplitCount(cladeId))//the split was not found
			toreturn.first *=default_proba;

	return toreturn;
}

double MyCladesAndTripartitions::getMaxLikelihoodAux(int idU)
{

	if( mClades.isLeaf(idU) ) // stopping condition
		return 1;

	double maxRatio = 0;
	double ratio;

	int nbSplit = getSplitCount(idU);

	//cout << idU << ":" << nbSplit << endl;
	int counter;
	for(counter=0;counter<nbSplit;counter++)//for each split
	{
		pair<int,int> cladeSplit = getCladeSplit(idU,counter);

		ratio = 1;
		if(idU != 0 || mRooted)// special condition for the root clade: should not use split ratio if the distribution is unrooted
			ratio *= getSplitRatio(idU,counter);
		else if(idU == 0)
			ratio *= ((double) getCladeOccurrences(cladeSplit.first))/ ((double) mGeneTreeCount); //  if we are the "root" clade but the distribution is unrooted, we should multiply by the number of time we saw the child clade (clade probability)

		ratio *= getMaxLikelihoodAux(cladeSplit.first) * getMaxLikelihoodAux(cladeSplit.second);

		if(ratio > maxRatio)
		{
			maxRatio = ratio;
		}
	}

	return maxRatio;

}

// Public functions

/**
 * Constructor - Compute all clades and tripartitions for the given
 * gene trees.
 */
MyCladesAndTripartitions::MyCladesAndTripartitions( char charSep, vector<MyGeneTree*> &geneTrees, bool verbose, bool &overflow, string &errStr, bool polytomy ) : CladesAndTripartitions( charSep, geneTrees,verbose,overflow, errStr,polytomy,NULL)
{srand (time(NULL));//init the random seed
}

/**
 * Constructor - Ale reader
 *
 */
MyCladesAndTripartitions::MyCladesAndTripartitions( char charSep, string aleFileName, string &errStr, bool verbose ) : CladesAndTripartitions( charSep, aleFileName, errStr, verbose )
{srand (time(NULL));//init the random seed
}

/**
Returns a tree where bipartitions have been drawn at random according to their split ratios
**/
MyGeneTree * MyCladesAndTripartitions::getRandomTree()
{
 	if( mClades.getLeafCount() < 3 )
 		return getMaxTree();

    int pOrd = 0;
    MyGeneNode *node = getRandomTreeAux( pOrd, mClades.getRootClade() );
    return new MyGeneTree( *node );
}

/**
Returns a tree where bipartition have the maximum split ratios for the encountered clades
**/
MyGeneTree * MyCladesAndTripartitions::getMaxTree()
{
	int pOrd = 0;
	MyGeneNode *node;

	//special procedures when there is only 1 or 2 leaves in the tree.
	if(mClades.getLeafCount() == 1)
	{
		//cout << " clade count " << mClades.getCladeCount() << endl;
		//printMe();
	    node = new MyGeneNode();
	    node->setName( mClades.getLeafName( 1 ) );
	}
	else if(mClades.getLeafCount() == 2)
	{
	    node = new MyGeneNode();

		vector<int> Scl = mClades.getSortedClades();
		for(unsigned i = 0; i < Scl.size(); i++)
		{
			if(mClades.isLeaf(Scl[i]))
			{
				MyGeneNode *sonNode = new MyGeneNode();
				sonNode->setName( mClades.getLeafName( Scl[i] ) );
				node->addSon( sonNode);
				node->setBranchProperty(bpp::TreeTools::BOOTSTRAP, bpp::Number<double>(pOrd));
			}
		}
    }
    else
    {
		node = getMaxTreeAux( pOrd, mClades.getRootClade() );
    }

    return new MyGeneTree( *node );
}

/**
RECURSIVE

Takes:
	- MyGeneNode * node: a tree to compute the likelihood from
	- double default_proba: proba to give to a split that is absent from the MyCladesAndTripartitions counts

Returns:
	double: likelihood of the node (and underlying subtree) according to the CCPs distribution.

NB: requires the tree to be binary
**/
double MyCladesAndTripartitions::getTreeLikelihood(MyGeneNode *node, double default_proba)
{
	pair<double,vector<string> > result;
	result = getTreeLikelihoodAux(node,default_proba);//recursive function
	return result.first;
}

/*
Takes:
 - node (MyGeneNode *): node of a tree

Returns:
 (bool): true if the leafs of the subtree rooted at node all corresponds to existing clades in the MyCladesAndTripartitions instance
 */
bool MyCladesAndTripartitions::isCompatible(MyGeneNode * node)
{
	if(node->isLeaf())
	{
		vector <string> leafNames;
		leafNames.push_back(node->getName());

		try
		{
			mClades.getCladeIntFromLeafList(leafNames); // will throw an error if the leaf clade does not exists
		}
		catch(exception & e) // not very elegant.
		{
			return false;
		}
	}

	bool result= true;

	//recursion
	int nbson = node->getNumberOfSons();

	for(int i = 0; i < nbson; i++)
	{
		if(!isCompatible(node->getSon(i)))
		{
			result = false;
			break;
		}
	}
	return result;
}

double MyCladesAndTripartitions::getMaxLikelihood()
{
	return getMaxLikelihoodAux(mClades.getRootClade());
}

int MyCladesAndTripartitions::RestrictToClade(vector <int> cladesToForce)
{
	//1. from clade to force to clade to delete
	// the clades to delete are the clades incompatible with the ones we want to force
	vector< vector<int> > noncompatibleLists = mClades.PUBLICgetNoncompatible();
	map <int,bool> cladeToDeleteMAP; // we first put the clades to delete in a map to avoid doubles
	//cout << "restricting to clades" ;
	for(unsigned i = 0 ; i < cladesToForce.size(); i++)
	{
		int idU = cladesToForce[i];
		//cout << " " << idU ;
		for(unsigned j = 0 ; j < noncompatibleLists[idU].size(); j++)
		{
			cladeToDeleteMAP[ noncompatibleLists[idU][j] ] = true;
		}
	}

	for(unsigned i = 0 ; i < cladesToForce.size(); i++)// remove the clades to restrict from the clade to delete list to if they appear in the map
	{
		int idU = cladesToForce[i];
		cladeToDeleteMAP[ idU ] = false;
	}

	//cout << endl;
	vector <int> cladeToDelete;
	for (map<int,bool>::iterator it=cladeToDeleteMAP.begin(); it!=cladeToDeleteMAP.end(); ++it)
	{
		if(it->second) // that way we don't remove a clade to restrict
	  		cladeToDelete.push_back( it->first );
  	}
	//cout << "MyCladesAndTripartitions::RestrictToClade. Deleting "<< cladeToDelete.size() << " clades"<<endl;
	//for(unsigned i = 0 ; i < cladeToDelete.size(); i++)
	//	cout << cladeToDelete[i] << " ";
	//cout << endl;
  	//2. delete the clades to delete
	return deleteClades(cladeToDelete);
}

int MyCladesAndTripartitions::RestrictToClade(vector <vector <string> > cladesToForce)
{
	vector <int> cladesToForceIDU;

	for( unsigned i = 0 ; i < cladesToForce.size() ; i++)
	{
		int idU = mClades.getCladeIntFromLeafList( cladesToForce[i] );
		if(idU >= 0 )
			cladesToForceIDU.push_back( idU );

		//cout << idU <<" <- ";
		//for(unsigned j = 0 ; j < cladesToForce[i].size() ; j++)
		//	cout << cladesToForce[i][j] << " ";
		//cout << endl;
	}
	return RestrictToClade(cladesToForceIDU);
}

/*
works if source is amalgamted trees
*/
MyCladesAndTripartitions::MyCladesAndTripartitions(MyCladesAndTripartitions * source)
{

    mRooted = false;   ///< amalgamated tree is not rooted

    mGeneTreeCount = source->getmGeneTreeCount();

    mNewickGeneTree = source->getmNewickGeneTree(); ///< save newick string for ALE output

	mClades = source->mClades;

	vector <int> sortedClades = mClades.getSortedClades();

	for(unsigned i = 0 ; i < sortedClades.size(); i++)
	{
		int cladeNum = i;// as this is how things are organized in the cladesandtripartition instance, we keep this structure

		int splitCount = source->getSplitCount( cladeNum );

		mCladeSplits.push_back( vector< pair<int,int> > () );        ///< All splits for each clade (by clade id)
		mSplitsRatio.push_back( vector <double> ()  );        ///< Occurence of each split as a vector, divided by clade occurrences.
		for(unsigned splitNum = 0 ; splitNum < splitCount; splitNum++)
		{
			mCladeSplits[cladeNum].push_back( source->getCladeSplit( cladeNum, splitNum ) ) ;
			mSplitsRatio[cladeNum].push_back( source->getSplitRatio( cladeNum, splitNum ) ) ;
		}

	    mCladeOccurrences.push_back( source->getCladeOccurrences( cladeNum ) );///< Occurence of each clade, indexed by clade number.

		mBranchLengths.push_back( source->getmBranchLengths( cladeNum ) ); ///< branch length for each clade

	}

}


/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef DART_OPTIMIZATION_SOLVER_HPP_
#define DART_OPTIMIZATION_SOLVER_HPP_

#include <iostream>
#include <memory>

#include <Eigen/Dense>

namespace dart {
namespace optimization {

class Problem;

/// Abstract class that provides a common interface for different Solvers. The
/// different Solver implementations each use a different nonlinear problem
/// solving library, which could lead to differences in performance for various
/// problem types. This base class allows the different Solver implementations
/// to be swapped out with each other quickly and easily to help with testing,
/// benchmarking, and experimentation.
class Solver
{
public:
  /// The Solver::Properties class contains Solver parameters that are common
  /// to all Solver types. Most (but not necessarily all) Solvers will make use
  /// of these parameters, and these parameters can be directly copied or
  /// transferred between all Solver types.
  struct Properties
  {
    /// Nonlinear optimization Problem to be solved
    std::shared_ptr<Problem> mProblem;

    /// The relative tolerance on the optimization parameters. For example, the
    /// distance between the last parameters and the current parameters to be
    /// considered converged.
    double mTolerance;

    /// The maximum number of iterations that the solver should use. Use 0 for
    /// infinite iterations.
    std::size_t mNumMaxIterations;

    /// How many iterations between printing the Solver's progress to the
    /// terminal. Use 0 for no printing.
    std::size_t mIterationsPerPrint;

    /// Stream for printing the Solver's progress. Default is std::cout.
    std::ostream* mOutStream;

    /// Set to true if the final result should be printed to the terminal.
    bool mPrintFinalResult;

    /// Publish the results of the optimization to a file. Leave this string
    /// empty to avoid publishing anything.
    std::string mResultFile;

    Properties(
        std::shared_ptr<Problem> _problem = nullptr,
        double _tolerance = 1e-9,
        std::size_t _numMaxIterations = 500,
        std::size_t _iterationsPerPrint = 0,
        std::ostream* _ostream = &std::cout,
        bool _printFinalResult = false,
        const std::string& _resultFile = "");
  };

  /// Default Constructor
  explicit Solver(const Properties& _properties = Properties());

  /// Alternative Constructor
  explicit Solver(std::shared_ptr<Problem> _problem);

  /// \brief Destructor
  virtual ~Solver() = default;

  /// Solve optimization problem
  virtual bool solve() = 0;

  /// Get the type (implementation) of this Solver
  virtual std::string getType() const = 0;

  /// Create an identical clone of this Solver
  virtual std::shared_ptr<Solver> clone() const = 0;

  /// Set the generic Properties of this Solver
  void setProperties(const Properties& _properties);

  /// Get the generic Properties of this Solver
  const Properties& getSolverProperties() const;

  /// Copy the generic Properties of another Solver
  void copy(const Solver& _otherSolver);

  /// Copy the generic Properties of another Solver
  Solver& operator=(const Solver& _otherSolver);

  /// Set the nonlinear optimization problem
  virtual void setProblem(std::shared_ptr<Problem> _newProblem);

  /// Get nonlinear optimization problem
  std::shared_ptr<Problem> getProblem() const;

  /// Set the maximum step size allowed for the Problem to be considered
  /// converged
  virtual void setTolerance(double _newTolerance);

  /// Get the maximum step size allowed for the Problem to be considered
  /// converged
  double getTolerance() const;

  /// Set the maximum number of iterations that the Solver should use
  virtual void setNumMaxIterations(std::size_t _newMax);

  /// Get the maximum number of iterations that the Solver should use
  std::size_t getNumMaxIterations() const;

  /// Set the number of iterations that should pass between printing progress to
  /// the terminal. Use 0 for no printing.
  virtual void setIterationsPerPrint(std::size_t _newRatio);

  /// Get the number of iterations that should pass between printing progress to
  /// the terminal. A value of 0 means there will be no printing.
  std::size_t getIterationsPerPrint() const;

  /// Set the output stream that prints the Solver's progress.
  virtual void setOutStream(std::ostream* _os);

  /// Get the output stream that prints the Solver's progress.
  std::ostream* getOutStream() const;

  /// Set to true if the final result should be printed to the terminal
  virtual void setPrintFinalResult(bool _print);

  /// Returns true if the final result should be printed to the terminal
  bool getPrintFinalResult() const;

  /// Set the name of the file that results should be printed to. Use an empty
  /// string to indicate that results should not be printed to a file.
  virtual void setResultFileName(const std::string& _resultFile);

  /// Get the name of the file that results should be printed to. An empty
  /// string indicates that results should not be printed to a file.
  const std::string& getResultFileName() const;

protected:
  Properties mProperties;
};

} // namespace optimization
} // namespace dart

#endif // DART_OPTIMIZATION_SOLVER_HPP_


#ifndef ATM_SUBSTRINGS_FROM_LONGEST_HPP
#define ATM_SUBSTRINGS_FROM_LONGEST_HPP

#include <cmath>
#include <map>
#include <vector>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/range/algorithm/find.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/iterator.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/size.hpp>
#include <boost/range/sub_range.hpp>
#include <boost/range/value_type.hpp>
#include <boost/utility.hpp>
#include "sast/sast.hpp"

namespace atm {

template <class RandomAccessRange, class Index>
struct substrings_from_longest {
    using range_type = RandomAccessRange;
    using char_type = typename boost::range_value<RandomAccessRange>::type;
    using index_type = Index;

protected:
    using sast_type = sast::sast<RandomAccessRange, index_type>;

public:
    struct substr {
        using iterator       = typename boost::range_iterator<RandomAccessRange>::type;
        using const_iterator = typename boost::range_const_iterator<RandomAccessRange>::type;

        index_type pos()           const { return parent_->pos(i_, j_); }
        boost::sub_range<const std::vector<index_type>> allpos() const { return parent_->allpos(i_, j_); }
        index_type length()        const { return j_ - i_; }
        index_type frequency()     const { return parent_->frequency(i_, j_); }
        double     spurity()       const { return parent_->strict_purity(i_, j_); }
        double     lpurity()       const { return parent_->loose_purity(i_, j_); }
        double     luniversality() const { return parent_->left_universality(i_, j_); }
        double     runiversality() const { return parent_->right_universality(i_, j_); }

        iterator begin() { return boost::begin(parent_->input_) + i_; }
        iterator end()   { return boost::begin(parent_->input_) + j_; }
        const_iterator begin() const { return boost::const_begin(parent_->input_) + i_; }
        const_iterator end()   const { return boost::const_begin(parent_->input_) + j_; }

        substr(const substrings_from_longest* parent, int i, int j)
            : parent_(parent), i_(i), j_(j)
        {}

    private:
        const substrings_from_longest* parent_;
        int i_;
        int j_;
    };

private:
    template <class> struct substring_iterator;

public:
    using iterator       = substring_iterator<substr>;
    using const_iterator = substring_iterator<const substr>;

    substrings_from_longest(const sast_type& sast)
        : sast_(sast),
          input_(sast_.input()),
          finder_(sast::make_positional_finder(sast_))
    {}

    iterator begin() { return iterator(this, 0, boost::size(input_)); }
    iterator end()   { return iterator(this, 0, 0); }
    const_iterator begin() const { return const_iterator(this, 0, boost::size(input_)); }
    const_iterator end()   const { return const_iterator(this, 0, 0); }

private:
    template <class Value>
    struct substring_iterator
        : public boost::iterator_facade<
            substring_iterator<Value>,
            Value,
            boost::random_access_traversal_tag,
            Value,
            int>
    {
        substring_iterator()
            : parent_(0), i_(-1), j_(-1)
        {}

        substring_iterator(const substrings_from_longest* parent, int i, int j)
            : parent_(parent), i_(i), j_(j)
        {}

        template <class OtherValue>
        substring_iterator(const substring_iterator<OtherValue>& other)
            : parent_(other.parent_), i_(other.i_), j_(other.j_)
        {}

    private:
        friend class boost::iterator_core_access;
        template <class> friend struct substring_iterator;

        void increment() {
            if (static_cast<std::size_t>(j_) == boost::size(parent_->input_)) {
                const auto width = (j_ - i_) - 1;
                i_ = 0;
                j_ = 0 + width;
            }
            else {
                ++i_;
                ++j_;
            }
        }

        void decrement() {
            if (i_ == 0) {
                const auto width = (j_ - i_) + 1;
                i_ = boost::size(parent_->input_) - width;
                j_ = boost::size(parent_->input_);
            }
            else {
                --i_;
                --j_;
            }
        }

        void advance(int n) {
            if (n >= 0) {
                auto width = j_ - i_;
                while (static_cast<std::size_t>(n) > boost::size(parent_->input_) - j_) {
                    n -= (boost::size(parent_->input_) - j_) + 1;
                    --width;
                    i_ = 0;
                    j_ = 0 + width;
                }
                i_ += n;
                j_ += n;
            }
            else {
                n = -n;
                auto width = j_ - i_;
                while (n > i_ - 0) {
                    n -= (i_ - 0) + 1;
                    ++width;
                    i_ = boost::size(parent_->input_) - width;
                    j_ = boost::size(parent_->input_);
                }
                i_ -= n;
                j_ -= n;
            }
        }

        int distance_to(const substring_iterator<Value>& other) const {
            int d = 0;
            auto width = j_ - i_;
            auto owidth = other.j_ - other.i_;
            if (owidth < width || (owidth == width && other.i_ > this->i_)) {
                int j = this->j_;
                while (owidth < width) {
                    d += (boost::size(parent_->input_) - j) + 1;
                    --width;
                    j = 0 + width;
                }
                d += other.j_ - j;
            }
            else {
                int j = other.j_;
                while (owidth > width) {
                    d += (boost::size(parent_->input_) - j) + 1;
                    --owidth;
                    j = 0 + owidth;
                }
                d += this->j_ - j;
                d = -d;
            }
            return d;
        }

        template <class OtherValue>
        bool equal(const substring_iterator<OtherValue>& other) const {
            return this->parent_ == other.parent_
                && this->i_ == other.i_
                && this->j_ == other.j_;
        }

        Value dereference() const {
            return substr(parent_, i_, j_);
        }

        const substrings_from_longest* parent_;
        int i_;
        int j_;
    };

protected:
    index_type pos(const int i, const int j) const {
        const auto len_substr = j - i;
        // substrに対応する内部ノードを見つける。
        typename sast_type::const_iterator n = finder_.find(i, j);
        if (n->length() >= len_substr) {
            // substrは2回以上出現しており、対応する内部ノードが存在する。
            return n->pos();
        }
        else {
            // substrは1回しか出現しておらず、対応する内部ノードが存在しない。
            return i;
        }
    }

    boost::sub_range<const std::vector<index_type>> allpos(const int i, const int j) const {
        const auto len_substr = j - i;
        // substrに対応する内部ノードを見つける。
        typename sast_type::const_iterator n = finder_.find(i, j);
        if (n->length() >= len_substr) {
            // substrは2回以上出現しており、対応する内部ノードが存在する。
            return n->allpos();
        }
        else {
            // substrは1回しか出現しておらず、対応する内部ノードが存在しない。
            auto found = boost::find(n->allpos(), i);
            return boost::make_iterator_range(found, boost::next(found));
        }
    }

    index_type frequency(const int i, const int j) const {
        const auto len_substr = j - i;
        // substrに対応する内部ノードを見つける。
        typename sast_type::const_iterator n = finder_.find(i, j);
        if (n->length() >= len_substr) {
            // substrは2回以上出現しており、対応する内部ノードが存在する。
            return n->frequency();
        }
        else {
            // substrは1回しか出現しておらず、対応する内部ノードが存在しない。
            return 1;
        }
    }

    double strict_purity(const int i, const int j) const {
        const auto len_substr = j - i;
        // substrに対応する内部ノードを見つける。
        typename sast_type::const_iterator n = finder_.find(i, j);
        if (n->length() >= len_substr) {
            // substrは2回以上出現しており、対応する内部ノードが存在する。
            const auto kk = n->length() - len_substr;  // ノードkではkk文字削ったことに相当する。

            return strict_purity_(n, kk);
        }
        else {
            // substrは1回しか出現しておらず、対応する内部ノードが存在しない。
            // substrと同じ出現回数のsub-substrを数える。
            uint64_t count = 0;
            {
                // substrの末尾を0文字以上削って得られるsub-substrについて考える。
                count += len_substr - n->length();
            }
            for (int l = 1; l < len_substr; ++l) {
                // substrの先頭をl文字削ったsub-substrを考える。
                const auto len_subsubstr = len_substr - l;

                // sub-substrに対応するノードを見つける。
                // d[node_to_parent_node[m]] < len_subsubstr <= d[m] を満たす m を
                // 見つける。
                typename sast_type::const_iterator m = finder_.find(i + l, j);
                if (m->length() < len_subsubstr) {
                    count += len_subsubstr - m->length();
                }
            }

            // strict purity of substr
            const uint64_t num_subsubstrs = static_cast<uint64_t>(1 + len_substr) * len_substr / 2;
            const double spurity = static_cast<double>(count) / num_subsubstrs;

            return spurity;
        }
    }

    double strict_purity_(typename sast_type::const_iterator n, const int ii) const {
        // ここではノードi（iはpost-orderでの番号）に対応する部分文字列の末尾をii文字削ったsubstrを扱う。
        // iiが満たさなければならない条件: 0 <= ii < d[i] - d[node_to_parent_node[i]]
        const auto freq_substr = n->frequency();
        const auto len_substr  = n->length() - ii;

        // substrと同じ出現回数のsub-substrを数える。
        uint64_t count = 0;
        {
            // substrの末尾を0文字以上削って得られるsub-substrについて考える。
            // ノードiに対応する部分文字列をsubstr[i]とすると、substr[i]の末尾
            // を削って得られる部分文字列の内で、substr[i]と同じ頻度をもつもの
            // はsuffix tree上ではノードiにまとめられている
            // （分岐が無い <=> 頻度が同じ）。

            // ノードiの親ノードjを見つける。
            const auto m = n.parent();

            // substrの末尾を0文字以上削って得られるsub-substrの内で、出現
            // 回数がsubstrと同じものの数はd[i] - d[j]である。
            count += n->length() - m->length() - ii;
        }
        for (int j = 1; j < len_substr; ++j) {
            // substrの先頭をj文字削ったsub-substrを考える。
            const auto len_subsubstr = len_substr - j;

            // sub-substrに対応するノードを見つける。
            // {内部,葉}ノードに対応する部分文字列から先頭の1文字を削って得られ
            // る文字列には、必ず対応する{内部,葉}ノードが存在する。
            // ii ＞ 0 の場合、d[k] == len_subsubstr を満たす「ノード」が必ずし
            // も存在するとは限らない。よって、
            // d[node_to_parent_node[k]] < len_subsubstr <= d[k] を満たす k を
            // 見つける。
            auto m = n;
            for (int l = 0; l < j; ++l) {
                m = m.suffix();
            }
            while (m.parent()->length() >= len_subsubstr) {
                m = m.parent();
            }
            const auto kk = m->length() - len_subsubstr;  // ノードiでii文字削ると、ノードkではkk文字削ったことに相当する。

            // sub-substrの出現回数をチェックする。
            const auto freq_subsubstr = m->frequency();
            if (freq_subsubstr == freq_substr) {
                // ノードkの親ノードmを見つける。
                const auto mp = m.parent();

                // sub-substrの末尾を0文字以上削って得られる
                // sub-sub-substrの内で、出現回数がsub-substrと同じもの
                // の数はd[k] - d[m]である。
                count += m->length() - mp->length() - kk;
            }
        }

        // strict purity of substr
        const uint64_t num_subsubstrs = static_cast<uint64_t>(1 + len_substr) * len_substr / 2;
        const double spurity = static_cast<double>(count) / num_subsubstrs;

        return spurity;
    }

    double loose_purity(const int i, const int j) const {
        const auto len_substr = j - i;
        // substrに対応する内部ノードを見つける。
        typename sast_type::const_iterator n = finder_.find(i, j);
        if (n->length() >= len_substr) {
            // substrは2回以上出現しており、対応する内部ノードが存在する。
            const auto kk = n->length() - len_substr;  // ノードkではkk文字削ったことに相当する。

            return loose_purity_(n, kk);
        }
        else {
            // substrは1回しか出現しておらず、対応する内部ノードが存在しない。
            // substrと同じ出現回数のsub-substrを数える。
            double support = 0;
            {
                // substrの末尾を0文字以上削って得られるsub-substrについて考える。
                support += len_substr - n->length();
                for (auto m = n, p = n.parent(); m->length() > 0; m = p, p = p.parent()) {
                    const auto num_subsubstrs_of_same_frequency = m->length() - p->length();
                    const auto freq_subsubstr = m->frequency();
                    const double sup = 1.0 / freq_subsubstr;
                    support += num_subsubstrs_of_same_frequency * sup;
                }
            }
            for (int l = 1; l < len_substr; ++l) {
                // substrの先頭をl文字削ったsub-substrを考える。
                const auto len_subsubstr = len_substr - l;

                // sub-substrに対応するノードを見つける。
                // d[node_to_parent_node[m]] < len_subsubstr <= d[m] を満たす m を
                // 見つける。
                typename sast_type::const_iterator m = finder_.find(i + l, j);
                if (m->length() >= len_subsubstr) {
                    const auto mm = m->length() - len_subsubstr;  // ノードmではmm文字削ったことに相当する。

                    // sub-substrの末尾を0文字以上削って得られるsub-substrについて考える。
                    for (auto o = m, p = m.parent(); o->length() > 0; o = p, p = p.parent()) {
                        const auto num_subsubstrs_of_same_frequency = o->length() - p->length() - (o == m ? mm : 0);
                        const auto freq_subsubstr = o->frequency();
                        const double sup = 1.0 / freq_subsubstr;
                        support += num_subsubstrs_of_same_frequency * sup;
                    }
                }
                else {
                    support += len_subsubstr - m->length();
                    for (auto o = m, p = m.parent(); o->length() > 0; o = p, p = p.parent()) {
                        const auto num_subsubstrs_of_same_frequency = o->length() - p->length();
                        const auto freq_subsubstr = o->frequency();
                        const double sup = 1.0 / freq_subsubstr;
                        support += num_subsubstrs_of_same_frequency * sup;
                    }
                }
            }

            // loose purity of substr
            const uint64_t num_subsubstrs = static_cast<uint64_t>(1 + len_substr) * len_substr / 2;
            const double lpurity = support / num_subsubstrs;

            return lpurity;
        }
    }

    double loose_purity_(typename sast_type::const_iterator n, const int ii) const {
        // ここではノードi（iはpost-orderでの番号）に対応する部分文字列の末尾をii文字削ったsubstrを扱う。
        // iiが満たさなければならない条件: 0 <= ii < d[i] - d[node_to_parent_node[i]]
        const auto freq_substr = n->frequency();
        const auto len_substr  = n->length() - ii;

        double support = 0;
        {
            // substrの末尾を0文字以上削って得られるsub-substrについて考える。
            for (auto m = n, p = n.parent(); m->length() > 0; m = p, p = p.parent()) {
                const auto num_subsubstrs_of_same_frequency = m->length() - p->length() - (m == n ? ii : 0);
                const auto freq_subsubstr = m->frequency();
                const double sup = static_cast<double>(freq_substr) / freq_subsubstr;
                support += num_subsubstrs_of_same_frequency * sup;
            }
        }
        for (int j = 1; j < len_substr; ++j) {
            // substrの先頭をj文字削ったsub-substrを考える。
            const auto len_subsubstr = len_substr - j;

            // sub-substrに対応するノードを見つける。
            // ii ＞ 0 の場合、d[k] == len_subsubstr を満たす「ノード」が必ずし
            // も存在するとは限らない。よって、
            // d[node_to_parent_node[k]] < len_subsubstr <= d[k] を満たす k を
            // 見つける。
            auto m = n;
            for (int l = 0; l < j; ++l) {
                m = m.suffix();
            }
            while (m.parent()->length() >= len_subsubstr) {
                m = m.parent();
            }
            const auto kk = m->length() - len_subsubstr;  // ノードiでii文字削ると、ノードkではkk文字削ったことに相当する。

            // sub-substrの末尾を0文字以上削って得られるsub-substrについて考える。
            for (auto o = m, p = m.parent(); o->length() > 0; o = p, p = p.parent()) {
                const auto num_subsubstrs_of_same_frequency = o->length() - p->length() - (o == m ? kk : 0);
                const auto freq_subsubstr = o->frequency();
                const double sup = static_cast<double>(freq_substr) / freq_subsubstr;
                support += num_subsubstrs_of_same_frequency * sup;
            }
        }

        // loose purity of substr
        const uint64_t num_subsubstrs = static_cast<uint64_t>(1 + len_substr) * len_substr / 2;
        const double lpurity = support / num_subsubstrs;

        return lpurity;
    }

    std::map<char_type, int> left_extensions(const int i, const int j) const {
        std::map<char_type, int> char_dist;
        for (const auto pos : allpos(i, j)) {
            const auto& c = boost::const_begin(input_)[pos - 1];
            char_dist[c] += 1;
        }

        return char_dist;
    }

    std::map<char_type, int> right_extensions(const int i, const int j) const {
        const auto len_substr = j - i;

        std::map<char_type, int> char_dist;
        for (const auto pos : allpos(i, j)) {
            const auto& c = boost::const_begin(input_)[pos + len_substr];
            char_dist[c] += 1;
        }

        return char_dist;
    }

    double left_universality(const int i, const int j) const {
        const auto freq_substr = frequency(i, j);

        std::map<char_type, int> char_dist = left_extensions(i, j);

        double e = 0;
        for (const auto& kv : char_dist) {
            const double p = static_cast<double>(kv.second) / freq_substr;
            e += -p * std::log(p);
        }
        const double u = 1 - std::exp(-e);

        return u;
    }

    double right_universality(const int i, const int j) const {
        const auto freq_substr = frequency(i, j);

        std::map<char_type, int> char_dist = right_extensions(i, j);

        double e = 0;
        for (const auto& kv : char_dist) {
            const double p = static_cast<double>(kv.second) / freq_substr;
            e += -p * std::log(p);
        }
        const double u = 1 - std::exp(-e);

        return u;
    }

    const sast_type& sast_;
    const RandomAccessRange& input_;
    sast::positional_finder<RandomAccessRange, index_type> finder_;
};

}  // namespace atm

#endif  /* ATM_SUBSTRINGS_FROM_LONGEST_HPP */