#include <QMap>
#include <QString>
#include <QStringList>
#include <QTime>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <mm_plugin.h>

#include "model.h"
#include "common/opencvutils.h"
#include "common/qtutils.h"
#include "plugins/meta.h"
#include "plugins/regions.h"

//#ifdef MM_SDK_TRAINABLE
#include <boost/smart_ptr.hpp>
#include <exception>
#include <iostream>
#include <cstdlib>
#include <sstream>
#include <string>
#include <vector>

#include <error.hpp>
#include <global.hpp>
#include <subset.hpp>
#include <data_intervaller.hpp>
#include <data_splitter.hpp>
#include <data_splitter_5050.hpp>
#include <data_splitter_cv.hpp>
#include <data_splitter_resub.hpp>
#include <data_scaler.hpp>
#include <data_scaler_void.hpp>
#include <data_accessor_splitting_mem.hpp>
#include <criterion_wrapper.hpp>
#include <distance_euclid.hpp>
#include <classifier_knn.hpp>
#include <seq_step_straight_threaded.hpp>
#include <search_seq_dos.hpp>
#include <search_seq_sfs.hpp>
#include <search_seq_sffs.hpp>
#include <search_monte_carlo_threaded.hpp>

using namespace FST;
//#endif // MM_SDK_TRAINABLE

using namespace mm;

enum DimensionStatus {
    On,
    Off,
    Ignore
};

//#ifdef MM_SDK_TRAINABLE
template<typename DATATYPE, typename IDXTYPE, class INTERVALCONTAINER>
class FST3Data_Accessor_Splitting_MemMM : public Data_Accessor_Splitting_Mem<DATATYPE,IDXTYPE,INTERVALCONTAINER>
{
    QList<MatrixList> mll;
    QList<DimensionStatus> dsl;
    int features;
    QMap<int, int> labelCounts;

public:
    typedef Data_Accessor_Splitting_Mem<DATATYPE,IDXTYPE,INTERVALCONTAINER> DASM;
    typedef boost::shared_ptr<Data_Scaler<DATATYPE> > PScaler;
    typedef typename DASM::PSplitters PSplitters;

    FST3Data_Accessor_Splitting_MemMM(const QList<MatrixList> &_mll, const QList<DimensionStatus> &_dsl, const PSplitters _dsp, const PScaler _dsc)
        : Data_Accessor_Splitting_Mem<DATATYPE,IDXTYPE,INTERVALCONTAINER>("MM", _dsp, _dsc), mll(_mll), dsl(_dsl)
    {
        features = 0;
        foreach (DimensionStatus ds, dsl)
            if (ds != Ignore) features++;
        labelCounts = mll.first().labelCounts();
    }

    FST3Data_Accessor_Splitting_MemMM(const MatrixList &_ml, const PSplitters _dsp, const PScaler _dsc)
        : Data_Accessor_Splitting_Mem<DATATYPE,IDXTYPE,INTERVALCONTAINER>("MM", _dsp, _dsc)
    {
        mll.append(_ml);
        features = _ml.first().total() * _ml.first().channels();
        for (int i=0; i<features; i++)
            dsl.append(Off);
        labelCounts = _ml.labelCounts();
    }

    FST3Data_Accessor_Splitting_MemMM* sharing_clone() const;
    virtual std::ostream& print(std::ostream& os) const;

protected:
    FST3Data_Accessor_Splitting_MemMM(const Data_Accessor_Splitting_MemMM &damt, int x)
        : Data_Accessor_Splitting_Mem<DATATYPE,IDXTYPE,INTERVALCONTAINER>(damt, x)
    {} // weak (referencing) copy-constructor to be used in sharing_clone()

    virtual void initial_data_read();    //!< \note off-limits in shared_clone
    virtual void initial_file_prepare() {}

public:
    virtual unsigned int file_getNoOfClasses() const { return labelCounts.size(); }
    virtual unsigned int file_getNoOfFeatures() const { return features; }
    virtual IDXTYPE file_getClassSize(unsigned int cls) const { return labelCounts[cls]; }
};

template<typename DATATYPE, typename IDXTYPE, class INTERVALCONTAINER>
void FST3Data_Accessor_Splitting_MemMM<DATATYPE,IDXTYPE,INTERVALCONTAINER>::initial_data_read() //!< \note off-limits in shared_clone
{
    if (Clonable::is_sharing_clone()) throw fst_error("Data_Accessor_Splitting_MemMM()::initial_data_read() called from shared_clone instance.");
    IDXTYPE idx=0;

    // TODO: Assert that ml data type is DATATYPE
    const QList<float> labels = mll.first().labels();
    foreach (int label, labelCounts.keys()) {
        for (int i=0; i<labels.size(); i++) {
            if (labels[i] == label) {
                int dslIndex = 0;
                foreach (const MatrixList &ml, mll) {
                    const Matrix &m = ml[i];
                    const int dims = m.total() * m.channels();
                    for (int j=0; j<dims; j++)
                        if (dsl[dslIndex++] != Ignore)
                            this->data[idx++] = reinterpret_cast<float*>(m.data)[j];
                }
            }
        }
    }
}

/*template<typename DATATYPE, typename IDXTYPE, class INTERVALCONTAINER>
Data_Accessor_Splitting_MemMM<DATATYPE,IDXTYPE,INTERVALCONTAINER>* Data_Accessor_Splitting_MemMM<DATATYPE,IDXTYPE,INTERVALCONTAINER>::sharing_clone() const
{
        Data_Accessor_Splitting_MemMM<DATATYPE,IDXTYPE,INTERVALCONTAINER> *clone=new Data_Accessor_Splitting_MemMM<DATATYPE,IDXTYPE,INTERVALCONTAINER>(*this, (int)0);
        clone->set_sharing_cloned();
        return clone;
}

template<typename DATATYPE, typename IDXTYPE, class INTERVALCONTAINER>
std::ostream& Data_Accessor_Splitting_MemMM<DATATYPE,IDXTYPE,INTERVALCONTAINER>::print(std::ostream& os) const
{
        DASM::print(os);
        os << std::endl << "Data_Accessor_Splitting_MemMM()";
        return os;
}*/

//#endif // MM_SDK_TRAINABLE

class FST3DOS : public Feature
{
    friend class Maker<DOS,true>;

    int delta;

    mm::Remap remap;

    DOS(int delta = 1)
    {
        this->delta = delta;
    }

    static QString args()
    {
        return "delta = 1";
    }

    static DOS *make(const QString &args)
    {
        QStringList words = QtUtils::parse(args);
        if (words.size() > 1) qFatal("DOS::make invalid argument count.");

        int delta = 1;

        bool ok;
        switch (words.size()) {
          case 1:
            delta = words[0].toInt(&ok); if (!ok) qFatal("DOS::make expected integer delta.");
        }

        return new DOS(delta);
    }

    QSharedPointer<Feature> clone() const
    {
        return QSharedPointer<Feature>(new DOS(delta));
    }

    void train(const MatrixList &data, Matrix &metadata)
    {
        (void) metadata;
        //#ifdef MM_SDK_TRAINABLE
        try {
            typedef float RETURNTYPE; 	typedef float DATATYPE;       typedef float REALTYPE;
            typedef unsigned int IDXTYPE; typedef unsigned int DIMTYPE; typedef int BINTYPE;
            typedef Subset<BINTYPE, DIMTYPE> SUBSET;
            typedef Data_Intervaller<std::vector<Data_Interval<IDXTYPE> >,IDXTYPE> INTERVALLER;
            typedef boost::shared_ptr<Data_Splitter<INTERVALLER,IDXTYPE> > PSPLITTER;
            typedef Data_Splitter_CV<INTERVALLER,IDXTYPE> SPLITTERCV;
            typedef Data_Splitter_5050<INTERVALLER,IDXTYPE> SPLITTER5050;
            typedef Data_Splitter_Resub<INTERVALLER,IDXTYPE> SPLITTERRESUB;
            typedef Data_Accessor_Splitting_MemMM<DATATYPE,IDXTYPE,INTERVALLER> DATAACCESSOR;
            typedef Distance_Euclid<DATATYPE,DIMTYPE,SUBSET> DISTANCE;
            typedef Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE> CLASSIFIERKNN;
            typedef Criterion_Wrapper<RETURNTYPE,SUBSET,CLASSIFIERKNN,DATAACCESSOR> WRAPPERKNN;
            typedef Sequential_Step_Straight_Threaded<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,24> EVALUATOR;

            // Initialize dataset
            PSPLITTER dsp_outer(new SPLITTER5050()); // keep second half of data for independent testing of final classification performance
            PSPLITTER dsp_inner(new SPLITTERCV(3)); // in the course of search use the first half of data by 3-fold cross-validation in wrapper FS criterion evaluation
            boost::shared_ptr<Data_Scaler<DATATYPE> > dsc(new Data_Scaler_void<DATATYPE>()); // do not scale data
            boost::shared_ptr<std::vector<PSPLITTER> > splitters(new std::vector<PSPLITTER>); // set-up data access
            splitters->push_back(dsp_outer); //splitters->push_back(dsp_inner);
            boost::shared_ptr<DATAACCESSOR> da(new DATAACCESSOR(data, splitters, dsc));
            da->initialize();
            da->setSplittingDepth(0); if(!da->getFirstSplit()) throw fst_error("50/50 data split failed.");
            //da->setSplittingDepth(1); if(!da->getFirstSplit()) throw fst_error("3-fold cross-validation failure.");
            boost::shared_ptr<SUBSET> sub(new SUBSET(da->getNoOfFeatures())); // initiate the storage for subset to-be-selected
            //sub->select_all();

            // Run search
            boost::shared_ptr<CLASSIFIERKNN> cknn(new CLASSIFIERKNN); cknn->set_k(1);
            boost::shared_ptr<WRAPPERKNN> wknn(new WRAPPERKNN);
            wknn->initialize(cknn,da);
            boost::shared_ptr<EVALUATOR> eval(new EVALUATOR); // set-up the standard sequential search step object (option: hybrid, ensemble, etc.)
            //Search_DOS<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,EVALUATOR> srch(eval); // set-up Sequential Forward Floating Selection search procedure
            //srch.set_delta(delta);

            //FST::Search_SFFS<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,EVALUATOR> srch(eval);
            //srch.set_search_direction(FST::BACKWARD);

            //FST::Search_SFS<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,EVALUATOR> srch(eval);
            //srch.set_search_direction(FST::FORWARD);

            FST::Search_Monte_Carlo_Threaded<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,24> srch;
            srch.set_cardinality_randomization(0.5); // probability of inclusion of each particular feature (~implies also the expected subset size)
            srch.set_stopping_condition(0/*max trials*/,30/*seconds*/); // one or both values must have positive value

            RETURNTYPE critval_train;
            if(!srch.search(0,critval_train,sub,wknn,std::cout)) throw fst_error("Search not finished.");

            // Create map matrix
            const int dims = sub->get_d_raw();
            cv::Mat xMap(1, dims, CV_16SC1),
                    yMap(1, dims, CV_16SC1);
            int index = 0;
            for (int i=0; i<dims; i++) {
                if (sub->selected_raw(i)) {
                    xMap.at<short>(0, index) = i;
                    yMap.at<short>(0, index) = 0;
                    index++;
                }
            }

            remap = Remap(xMap, yMap, cv::INTER_NEAREST);
        }
        catch (fst_error &e) { qFatal("FST ERROR: %s, code=%d", e.what(), e.code()); }
        catch (std::exception &e) { qFatal("non-FST ERROR: %s", e.what()); }
        metadata >> remap;
        //#else // MM_SDK_TRAINABLE
        //qFatal("StreamwiseFS::train not supported.");
        //#endif // MM_SDK_TRAINABLE
    }

    void project(const Matrix &src, Matrix &dst) const
    {
        dst = src;
        dst >> remap;
    }

    void store(QDataStream &stream) const
    {
        stream << remap;
    }

    void load(QDataStream &stream)
    {
        stream >> remap;
    }
};

MM_REGISTER(Feature, FST3DOS, true)

class FST3StreamwiseFS : public Feature
{
    friend class Maker<StreamwiseFS,true>;

    QSharedPointer<Feature> weakLearnerTemplate;
    int time;

    mm::Dup dup;
    mm::Remap remap;

    StreamwiseFS(const QSharedPointer<Feature> &weakLearnerTemplate, int time)
        : dup(weakLearnerTemplate, 1)
    {
        this->weakLearnerTemplate = weakLearnerTemplate;
        this->time = time;
    }

    static QString args()
    {
        return "<feature> weakLearnerTemplate, int time";
    }

    static StreamwiseFS *make(const QString &args)
    {
        QStringList words = QtUtils::parse(args);
        if (words.size() != 2) qFatal("StreamwiseFS::make invalid argument count.");

        QSharedPointer<Feature> weakLearnerTemplate = Feature::make(words[0]);
        bool ok;
        int time = words[1].toInt(&ok); assert(ok);

        return new StreamwiseFS(weakLearnerTemplate, time);
    }

    QSharedPointer<Feature> clone() const
    {
        return QSharedPointer<Feature>(new StreamwiseFS(weakLearnerTemplate, time));
    }

    void train(const MatrixList &data, Matrix &metadata)
    {
        QList< QSharedPointer<Feature> > weakLearners;
        QList<MatrixList> projectedDataList;
        QList<int> weakLearnerDimsList;
        QList<DimensionStatus> dimStatusList;

        QTime timer; timer.start();
        while (timer.elapsed() / 1000 < time) {
            // Construct a new weak learner
            QSharedPointer<Feature> newWeakLearner = weakLearnerTemplate->clone();
            Matrix metadataCopy(metadata);
            newWeakLearner->train(data, metadataCopy);
            weakLearners.append(newWeakLearner);

            MatrixList projectedData = data;
            projectedData >> *newWeakLearner;
            projectedDataList.append(projectedData);
            weakLearnerDimsList.append(projectedData.first().total() * projectedData.first().channels());
            for (int i=0; i<weakLearnerDimsList.last(); i++) dimStatusList.append(Off);

            //#ifdef MM_SDK_TRAINABLE
            try
            {
                typedef float RETURNTYPE; 	typedef float DATATYPE;       typedef float REALTYPE;
                typedef unsigned int IDXTYPE; typedef unsigned int DIMTYPE; typedef int BINTYPE;
                typedef Subset<BINTYPE, DIMTYPE> SUBSET;
                typedef Data_Intervaller<std::vector<Data_Interval<IDXTYPE> >,IDXTYPE> INTERVALLER;
                typedef boost::shared_ptr<Data_Splitter<INTERVALLER,IDXTYPE> > PSPLITTER;
                typedef Data_Splitter_CV<INTERVALLER,IDXTYPE> SPLITTERCV;
                typedef Data_Splitter_5050<INTERVALLER,IDXTYPE> SPLITTER5050;
                typedef Data_Accessor_Splitting_MemMM<DATATYPE,IDXTYPE,INTERVALLER> DATAACCESSOR;
                typedef Distance_Euclid<DATATYPE,DIMTYPE,SUBSET> DISTANCE;
                typedef Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE> CLASSIFIERKNN;
                typedef Criterion_Wrapper<RETURNTYPE,SUBSET,CLASSIFIERKNN,DATAACCESSOR> WRAPPERKNN;
                typedef Sequential_Step_Straight_Threaded<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,24> EVALUATOR;

                // Initialize dataset
                PSPLITTER dsp_outer(new SPLITTER5050()); // keep second half of data for independent testing of final classification performance
                PSPLITTER dsp_inner(new SPLITTERCV(3)); // in the course of search use the first half of data by 3-fold cross-validation in wrapper FS criterion evaluation
                boost::shared_ptr<Data_Scaler<DATATYPE> > dsc(new Data_Scaler_void<DATATYPE>()); // do not scale data
                boost::shared_ptr<std::vector<PSPLITTER> > splitters(new std::vector<PSPLITTER>); // set-up data access
                splitters->push_back(dsp_outer); splitters->push_back(dsp_inner);
                boost::shared_ptr<DATAACCESSOR> da(new DATAACCESSOR(projectedDataList, dimStatusList, splitters, dsc));
                da->initialize();
                da->setSplittingDepth(0); if(!da->getFirstSplit()) throw fst_error("50/50 data split failed.");
                da->setSplittingDepth(1); if(!da->getFirstSplit()) throw fst_error("3-fold cross-validation failure.");
                boost::shared_ptr<SUBSET> sub(new SUBSET(da->getNoOfFeatures())); // initiate the storage for subset to-be-selected

                { // Initialize subset from previous iteration results
                    sub->deselect_all();
                    int index = 0;
                    for (int i=0; i<dimStatusList.size(); i++) {
                        if (dimStatusList[i] == On) sub->select(index);
                        if (dimStatusList[i] != Ignore) index++;
                    }
                }

                // Run search
                boost::shared_ptr<CLASSIFIERKNN> cknn(new CLASSIFIERKNN); cknn->set_k(3); // set-up 3-Nearest Neighbor classifier based on Euclidean distances
                boost::shared_ptr<WRAPPERKNN> wknn(new WRAPPERKNN); // wrap the 3-NN classifier to enable its usage as FS criterion (criterion value will be estimated by 3-fold cross-val.)
                wknn->initialize(cknn,da);
                boost::shared_ptr<EVALUATOR> eval(new EVALUATOR); // set-up the standard sequential search step object (option: hybrid, ensemble, etc.)
                Search_DOS<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,EVALUATOR> srch(eval); // set-up Sequential Forward Floating Selection search procedure
                srch.set_delta(1);
                RETURNTYPE critval_train;
                if(!srch.search(0,critval_train,sub,wknn,std::cout)) throw fst_error("Search not finished.");

                { // Update results
                    int dslIndex = dimStatusList.size() - 1;
                    int subIndex = da->getNoOfFeatures() - 1;
                    for (int wlIndex = weakLearnerDimsList.size()-1; wlIndex >= 0; wlIndex--) {
                        const int weakLearnerDims = weakLearnerDimsList[wlIndex];
                        int numSelectedDims = 0;
                        for (int i=0; i<weakLearnerDims; i++) {
                            if (dimStatusList[dslIndex] != Ignore)
                                dimStatusList[dslIndex] = sub->selected_raw(subIndex--) ? numSelectedDims++, On : Ignore;
                            dslIndex--;
                        }

                        if (numSelectedDims == 0) {
                            for (int j=0; j<weakLearnerDims; j++)
                                dimStatusList.removeAt(dslIndex+1);
                            weakLearnerDimsList.removeAt(wlIndex);
                            projectedDataList.removeAt(wlIndex);
                            weakLearners.removeAt(wlIndex);
                        }
                    }
                }
            }
            catch (fst_error &e) { qFatal("FST ERROR: %s, code=%d", e.what(), e.code()); }
            catch (std::exception &e) { qFatal("non-FST ERROR: %s", e.what()); }
            //#else // MM_SDK_TRAINABLE
            //qFatal("StreamwiseFS::train not supported.");
            //#endif // MM_SDK_TRAINABLE
        }

        dup = Dup(weakLearners);

        // Create map matrix
        int dims = 0;
        foreach (DimensionStatus ds, dimStatusList) if (ds == On) dims++;
        cv::Mat xMap(1, dims, CV_16SC1),
                yMap(1, dims, CV_16SC1);
        int index = 0;
        for (int i=0; i<dimStatusList.size(); i++) {
            if (dimStatusList[i] == On) {
                xMap.at<short>(0, index) = i;
                yMap.at<short>(0, index) = 0;
                index++;
            }
        }

        remap = Remap(xMap, yMap, cv::INTER_NEAREST);
    }

    void project(const Matrix &src, Matrix &dst) const
    {
        dst = src;
        dst >> dup >> mm::Cat >> remap;
    }

    void store(QDataStream &stream) const
    {
        stream << dup << remap;
    }

    void load(QDataStream &stream)
    {
        stream >> dup >> remap;
    }
};

MM_REGISTER(Feature, FST3StreamwiseFS, true)


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Wed 4 Apr 2018 09:58:26

#ifndef MDM_SLHA_IO_H
#define MDM_SLHA_IO_H

#include "MDM_mass_eigenstates.hpp"
#include "MDM_model_slha.hpp"
#include "MDM_info.hpp"
#include "MDM_observables.hpp"
#include "MDM_physical.hpp"
#include "problems.hpp"
#include "spectrum_generator_problems.hpp"
#include "standard_model_two_scale_model.hpp"
#include "slha_io.hpp"
#include "ckm.hpp"
#include "ew_input.hpp"
#include "lowe.h"

#include <Eigen/Core>
#include <string>
#include <tuple>
#include <utility>

#include <boost/fusion/include/for_each.hpp>
#include <boost/fusion/adapted/std_tuple.hpp>

#define Pole(p) physical.p
#define PHYSICAL(p) model.get_physical().p
#define PHYSICAL_SLHA(p) model.get_physical_slha().p
#define LOCALPHYSICAL(p) physical.p
#define MODEL model
#define MODELPARAMETER(p) model.get_##p()
#define EXTRAPARAMETER(p) model.get_##p()
#define OBSERVABLES observables
#define LowEnergyConstant(p) Electroweak_constants::p
#define SCALES(p) scales.p

namespace flexiblesusy {

struct MDM_input_parameters;
class Spectrum_generator_settings;

template <class T>
class MDM;

struct MDM_scales {
   double HighScale{0.}, SUSYScale{0.}, LowScale{0.};
   double pole_mass_scale{0.};
};

class MDM_slha_io {
public:
   MDM_slha_io();

   void clear();

   void fill(softsusy::QedQcd& qedqcd) const { slha_io.fill(qedqcd); }
   void fill(MDM_input_parameters&) const;
   void fill(MDM_mass_eigenstates&) const;
   template <class Model> void fill(MDM_slha<Model>&) const;
   void fill(Physical_input&) const;
   void fill(Spectrum_generator_settings&) const;
   double get_parameter_output_scale() const;
   const SLHA_io& get_slha_io() const { return slha_io; }
   void read_from_file(const std::string&);
   void read_from_source(const std::string&);
   void read_from_stream(std::istream&);
   void set_block(const std::string& str, SLHA_io::Position position = SLHA_io::back) { slha_io.set_block(str, position); }
   void set_blocks(const std::vector<std::string>& vec, SLHA_io::Position position = SLHA_io::back) { slha_io.set_blocks(vec, position); }
   template <class Model> void set_extra(const MDM_slha<Model>&, const MDM_scales&, const MDM_observables&);
   void set_input(const MDM_input_parameters&);
   void set_modsel(const SLHA_io::Modsel&);
   void set_physical_input(const Physical_input&);
   void set_settings(const Spectrum_generator_settings&);
   void set_sminputs(const softsusy::QedQcd&);
   template <class... Ts> void set_spectrum(const std::tuple<Ts...>&);
   template <class Model> void set_spectrum(const MDM_slha<Model>&);
   template <class T> void set_spectrum(const MDM<T>&);
   void set_spectrum(const standard_model::Standard_model& m) { slha_io.set_spectrum(m); }
   void set_spinfo(const Spectrum_generator_problems&);
   void set_spinfo(const Problems&);
   void set_spinfo(const std::vector<std::string>&, const std::vector<std::string>&);
   void set_print_imaginary_parts_of_majorana_mixings(bool);
   void write_to(const std::string&) const;
   void write_to_file(const std::string& file_name) const { slha_io.write_to_file(file_name); }
   void write_to_stream(std::ostream& ostr = std::cout) const { slha_io.write_to_stream(ostr); }

   static void fill_minpar_tuple(MDM_input_parameters&, int, double);
   static void fill_extpar_tuple(MDM_input_parameters&, int, double);
   static void fill_imminpar_tuple(MDM_input_parameters&, int, double);
   static void fill_imextpar_tuple(MDM_input_parameters&, int, double);

   template <class Model>
   static void fill_slhaea(SLHAea::Coll&, const MDM_slha<Model>&, const softsusy::QedQcd&, const MDM_scales&, const MDM_observables&);

   template <class Model>
   static SLHAea::Coll fill_slhaea(const MDM_slha<Model>&, const softsusy::QedQcd&, const MDM_scales&, const MDM_observables&);

private:
   SLHA_io slha_io; ///< SLHA io class
   bool print_imaginary_parts_of_majorana_mixings;

   void set_extpar(const MDM_input_parameters&);
   void set_imminpar(const MDM_input_parameters&);
   void set_imextpar(const MDM_input_parameters&);
   void set_minpar(const MDM_input_parameters&);
   void set_mass(const MDM_physical&, bool);
   void set_mixing_matrices(const MDM_physical&, bool);
   template <class Model> void set_model_parameters(const MDM_slha<Model>&);
   void set_ckm(const Eigen::Matrix<std::complex<double>,3,3>&, double);
   void set_pmns(const Eigen::Matrix<std::complex<double>,3,3>&, double);
   double read_scale() const;
   void fill_drbar_parameters(MDM_mass_eigenstates&) const;
   void fill_physical(MDM_physical&) const;
};

/**
 * Reads DR-bar parameters, pole masses and mixing matrices from a
 * SLHA output file.
 */
template <class Model>
void MDM_slha_io::fill(MDM_slha<Model>& model) const
{
   fill(static_cast<MDM_mass_eigenstates&>(model));
   fill_physical(model.get_physical_slha());
}

template <class Model>
void MDM_slha_io::fill_slhaea(
   SLHAea::Coll& slhaea, const MDM_slha<Model>& model,
   const softsusy::QedQcd& qedqcd, const MDM_scales& scales,
   const MDM_observables& observables)
{
   MDM_slha_io slha_io;
   const MDM_input_parameters& input = model.get_input();
   const auto& problems = model.get_problems();
   const bool error = problems.have_problem();

   slha_io.set_spinfo(problems);
   slha_io.set_sminputs(qedqcd);
   slha_io.set_input(input);
   if (!error) {
      slha_io.set_spectrum(model);
      slha_io.set_extra(model, scales, observables);
   }

   slhaea = slha_io.get_slha_io().get_data();
}

template <class Model>
SLHAea::Coll MDM_slha_io::fill_slhaea(
   const MDM_slha<Model>& model, const softsusy::QedQcd& qedqcd,
   const MDM_scales& scales, const MDM_observables& observables)
{
   SLHAea::Coll slhaea;
   MDM_slha_io::fill_slhaea(slhaea, model, qedqcd, scales, observables);

   return slhaea;
}

/**
 * Stores the model (DR-bar) parameters in the SLHA object.
 *
 * @param model model class
 */
template <class Model>
void MDM_slha_io::set_model_parameters(const MDM_slha<Model>& model)
{
   {
      std::ostringstream block;
      block << "Block gauge Q= " << FORMAT_SCALE(model.get_scale()) << '\n'
            << FORMAT_ELEMENT(1, (MODELPARAMETER(g1) * 0.7745966692414834), "g1 * 0.7745966692414834")
            << FORMAT_ELEMENT(2, (MODELPARAMETER(g2)), "g2")
            << FORMAT_ELEMENT(3, (MODELPARAMETER(g3)), "g3")
      ;
      slha_io.set_block(block);
   }
   slha_io.set_block("Yu", ToMatrix(MODELPARAMETER(Yu_slha)), "Yu", model.get_scale());
   slha_io.set_block("Yd", ToMatrix(MODELPARAMETER(Yd_slha)), "Yd", model.get_scale());
   slha_io.set_block("Ye", ToMatrix(MODELPARAMETER(Ye_slha)), "Ye", model.get_scale());
   {
      std::ostringstream block;
      block << "Block SM Q= " << FORMAT_SCALE(model.get_scale()) << '\n'
            << FORMAT_ELEMENT(1, (MODELPARAMETER(mu2)), "mu2")
            << FORMAT_ELEMENT(2, (MODELPARAMETER(LamH)), "LamH")
      ;
      slha_io.set_block(block);
   }
   {
      std::ostringstream block;
      block << "Block HMIX Q= " << FORMAT_SCALE(model.get_scale()) << '\n'
            << FORMAT_ELEMENT(3, (MODELPARAMETER(v)), "v")
      ;
      slha_io.set_block(block);
   }

}

/**
 * Writes extra SLHA blocks
 *
 * @param model model class
 * @param scales struct of boundary condition scales
 * @param observables struct of observables
 */
template <class Model>
void MDM_slha_io::set_extra(
   const MDM_slha<Model>& model, const MDM_scales& scales,
   const MDM_observables& observables)
{
   const MDM_physical physical(model.get_physical_slha());

   {
      std::ostringstream block;
      block << "Block FlexibleSUSYLowEnergy Q= " << FORMAT_SCALE(model.get_scale()) << '\n'
            << FORMAT_ELEMENT(1, (OBSERVABLES.a_muon), "Delta(g-2)_muon/2 FlexibleSUSY")
      ;
      slha_io.set_block(block);
   }
   {
      std::ostringstream block;
      block << "Block EFFHIGGSCOUPLINGS" << '\n'
            << FORMAT_RANK_THREE_TENSOR(25, 22, 22, (Abs(OBSERVABLES.eff_cp_higgs_photon_photon)), "Abs(effective H-Photon-Photon coupling)")
            << FORMAT_RANK_THREE_TENSOR(25, 21, 21, (Abs(OBSERVABLES.eff_cp_higgs_gluon_gluon)), "Abs(effective H-Gluon-Gluon coupling)")
      ;
      slha_io.set_block(block);
   }

}

/**
 * Stores the model (DR-bar) parameters, masses and mixing matrices of
 * all given models in the SLHA object.
 *
 * @todo Use generic lambda instead of Set_spectrum in C++14
 *
 * @param models model classes
 */
template <class... Ts>
void MDM_slha_io::set_spectrum(const std::tuple<Ts...>& models)
{
   Set_spectrum<MDM_slha_io> ss(this);
   boost::fusion::for_each(models, ss);
}

/**
 * Stores the model (DR-bar) parameters, masses and mixing matrices in
 * the SLHA object.
 *
 * @param model model class in BPMZ convention
 */
template <class T>
void MDM_slha_io::set_spectrum(const MDM<T>& model)
{
   set_spectrum(MDM_slha<MDM<T> >(model));
}

/**
 * Stores the model (DR-bar) parameters, masses and mixing matrices in
 * the SLHA object.
 *
 * @param model model class in SLHA convention
 */
template <class Model>
void MDM_slha_io::set_spectrum(const MDM_slha<Model>& model)
{
   const MDM_physical physical(model.get_physical_slha());
   const bool write_sm_masses = model.do_calculate_sm_pole_masses();

   set_model_parameters(model);
   set_mass(physical, write_sm_masses);
   set_mixing_matrices(physical, write_sm_masses);

   if (slha_io.get_modsel().quark_flavour_violated)
      set_ckm(model.get_ckm_matrix(), model.get_scale());

   if (slha_io.get_modsel().lepton_flavour_violated)
      set_pmns(model.get_pmns_matrix(), model.get_scale());
}

} // namespace flexiblesusy

#undef Pole
#undef PHYSICAL
#undef PHYSICAL_SLHA
#undef LOCALPHYSICAL
#undef MODEL
#undef MODELPARAMETER
#undef EXTRAPARAMETER
#undef OBSERVABLES
#undef LowEnergyConstant
#undef SCALES

#endif


#include <NTL/ctools.h>

#include <cstdlib>
#include <cmath>

/*
 * An IEEE double x is finite if and only if x - x == 0.
 * The function _ntl_IsFinite implements this logic;  however,
 * it does not completely trust that an optimizing compiler
 * really implements this correctly, and so it goes out of its way to
 * confuse the compiler.  For a good compiler that respects IEEE floating
 * point arithmetic, this may not be necessary, but it is better
 * to be a bit paranoid.
 *
 * Like the routine _ntl_ForceToMem below, this routine has the
 * side effect of forcing its argument into memory.
 *
 * I've checked the assembly code generated by various
 * versions of GCC, ICC, and MSVC++, and it all looks good.
 */

long _ntl_IsFinite(double *p)
{
   volatile double x = *p;
   *p = x;

   double y = x;
   double diff = y - x;
   return diff == 0.0;
}

/*
 * On machines with wide floating point registers, the routine _ntl_ForceToMem
 * is used to force a floating point double to a memory location.
 *
 */

void _ntl_ForceToMem(double *p)
{
   volatile double x = *p;
   *p = x;
}

/*
 * The routine _ntl_ldexp(x, e) is like the standard ldexp(x, e) routine,
 * except that it takes a long exponent e, rather than an int exponenet.
 * Some care is taken to ensure reasonable overflow/undeflow behavior.
 * If the value of e does not fit into an int, then the result
 * is x*infinity or x*0, as appropriate.
 * Of course, this can only happen on platforms where long is wider
 * than int (e.g., most 64-bit platforms).
 *
 * We go out of our way to hide the fact that we are multiplying/dividing
 * by zero, so as to avoid unnecessary warnings, and to prevent
 * overly-agressive optimizing compilers from screwing things up.
 */

volatile double _ntl_ldexp_zero = 0.0;

double _ntl_ldexp(double x, long e)
{
   if (e > NTL_MAX_INT)
      return x/_ntl_ldexp_zero;
   else if (e < NTL_MIN_INT)
      return x*_ntl_ldexp_zero;
   else
      return std::ldexp(x, ((int) e));
}


/*
Copyright (c) 2015 - 2016, Tianwei Shen
All rights reserved.

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.

* Neither the name of libvot nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

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.

*/

/*! \file vocab_tree.cpp
 * \brief vocabulary tree functions implementations
 */
#include <iostream>
#include <cmath>
#include <cassert>
#include <limits>
#include <thread>
#include <algorithm>
#include <numeric>

#include "vocab_tree.h"
#include "clustering.h"

#include <Eigen/Dense>

using std::cout;
using std::endl;

inline float l2sq(const DTYPE *a, const DTYPE *b)
{
	typedef Eigen::Matrix<DTYPE, 1, FDIM> MatrixType;
	typedef Eigen::Map<const MatrixType> MapTypeConst;		// a read-only map

	MapTypeConst a_map(a, FDIM);
	MapTypeConst b_map(b, FDIM);
	return (a_map.cast<float>() - b_map.cast<float>()).squaredNorm();
}

namespace vot
{
/** VocabTree Class Implementation */
VocabTree::VocabTree():database_image_num(0), num_nodes(0), dis_type(L1), root(nullptr) {}

VocabTree::VocabTree(int depth_, int branch_num_, int dim_, DistanceType dis_type_):
    branch_num(branch_num_), depth(depth_), dim(dim_), dis_type(dis_type_), num_nodes(0) {};

VocabTree::~VocabTree() {root = nullptr;}  // do nothing since root is undetermined

TreeInNode::~TreeInNode()
{
	if (children != nullptr) {
		delete [] children;
		children = nullptr;
	}
}

TreeLeafNode::~TreeLeafNode()
{
	inv_list.clear();
}

//////////////////////////////////////////////////////////////////////////////
//                                                                          //
//                     Vocabulary Tree Clear Funtions                       //
//                                                                          //
//////////////////////////////////////////////////////////////////////////////
bool VocabTree::ClearTree()
{
	if (root != nullptr) {
		root->ClearNode(branch_num);
	}

	std::cout << "[VocabTree] Successfully clearing the tree\n";
	return true;
}

bool TreeInNode::ClearNode(int bf)
{
	for (int i = 0; i < bf; i++) {
		if (children[i] != nullptr)
			children[i]->ClearNode(bf);
	}
	delete this;
	return true;
}

bool TreeLeafNode::ClearNode(int bf)
{
	delete this;
	return true;
}

bool TreeInNode::ClearScores(int bf)
{
	for (int i = 0; i < bf; i++) {
		if (children[i] != nullptr) {
			children[i]->ClearScores(bf);
		}
	}
	return true;
}

bool TreeLeafNode::ClearScores(int bf)
{
	score = 0.0;
	return true;
}

//////////////////////////////////////////////////////////////////////////////
//                                                                          //
//                     Vocabulary Tree Build Module                         //
//                                                                          //
//////////////////////////////////////////////////////////////////////////////
bool VocabTree::BuildTree(size_t num_keys, int dim_, int dep, int bf, DTYPE **p, int thread_num)
{
	if (dep < 1) {    	// the root of the tree is depth 0
		std::cout << "[FATAL_ERROR] The depth of the tree should be larger than 1!\n";
		return false;
	}

	branch_num = bf;
	depth = dep;
	dim = dim_;

	if (GlobalParam::Verbose) {
		std::cout << "[VocabTree Build] Begin Build Vocabulary Tree ...\n";
		std::cout << "[VocabTree Build] with depth " << dep << " and branch number " << bf << ".\n";
		std::cout << "[VocabTree Build] Approximately " << (float)sizeof(DTYPE) * dim * pow(bf, dep+1)/(1024 * 1024)
		          << "mb memory will be used to load the tree.\n";
	}

	double *means = new double [branch_num * dim];
	int *assign = new int [num_keys];
	if (means == nullptr || assign == nullptr) {
		std::cout << "[VocabTree Build] Error allocating memory in K-means\n";
		return false;
	}

	root = new TreeInNode();
	root->des = new DTYPE [dim];
	for (int i = 0; i < dim; i++)
		root->des[i] = 0;

	if (!root->RecursiveBuild(num_keys, dim, depth, 0, branch_num, p, means, assign, thread_num))
		return false;

	delete [] means;
	delete [] assign;

	std::cout << "[VocabTree Build] Finish building vocabulary tree!\n";

	return true;
}

void MultiRecursiveBuild(TreeNode *children, size_t num_keys, int dim, int depth, int depth_curr, int bf, DTYPE **p, double *means, int *assign, int sub_thread_num)
{
	if (children != nullptr) {
		children->RecursiveBuild(num_keys, dim, depth, depth_curr, bf, p, means, assign, sub_thread_num);
	}
}

bool TreeInNode::RecursiveBuild(size_t num_keys, int dim, int depth, int depth_curr, int bf, DTYPE **p, double *means, int *assign, int thread_num)
{
	if (GlobalParam::Verbose && depth_curr < 3)
		std::cout << "[RecursiveBuild] K-means in depth " << depth_curr << "\n";

	double error = Kmeans(num_keys, dim, bf, p, means, assign, thread_num);
	if (std::abs(error + 1) < 10e-6) {
		std::cerr << "[Error] Error in TreeInNode::RecursiveBuild\n";
		return false;
	}

	// the average distance between each cluster and the node descriptor
	double mean_distance = 0.0;
	for (int i = 0; i < bf; i++) {
		for (int j = 0; j < dim; j++) {
			mean_distance += (means[i*dim + j] - des[j]) * (means[i*dim + j] - des[j]);
		}
	}
	mean_distance /= bf;
	if (GlobalParam::Verbose && depth_curr < 3)
		std::cout << "[RecursiveBuild] Group/Center error: " << error/num_keys << "/" << mean_distance << "\n";

	// count the number of sift keys fallen into a interior node, stop split the node if there are too few keys.
	children = new TreeNode* [bf];
	size_t *counts = new size_t [bf];
	for (int i = 0; i < bf; i++)
		counts[i] = 0;
	for (int i = 0; i < num_keys; i++)
		counts[assign[i]]++;

	for (int i = 0; i < bf; i++) {
		if (counts[i] > 0) {   		// there are some keys fallen into this node
			if(depth_curr == depth || counts[i] < 2*bf)
				children[i] = new TreeLeafNode();
			else
				children[i] = new TreeInNode();
			children[i]->des = new DTYPE [dim];
			for (int j = 0; j < dim; j++) {
				if (sizeof(DTYPE) == 1)          // (char) round to the nearest integer
					children[i]->des[j] = (DTYPE)(means[i*dim + j] + 0.5);
				else                            // (float)
					children[i]->des[j] = (DTYPE)means[i*dim + j];
			}
		}
		else {
			children[i] = nullptr;
		}
	}

	// rearrange the pointer array so that after rearrangement, the sift keys in the first cluster appear consecutively in the array first,
	// then the second cluster, so on and so forth
	size_t idx = 0;
	size_t start_idx = 0;
	for (size_t i = 0; i < bf; i++) {
		for (size_t j = start_idx; j < num_keys; j++) {
			if (assign[j] == i) {
				// swap the pointer
				DTYPE *temp = p[idx];
				p[idx] = p[j];
				p[j] = temp;
				// swap the assignment
				unsigned int temp_assign = assign[idx];
				assign[idx] = assign[j];
				assign[j] = temp_assign;

				idx++;
			}
		}
		start_idx += counts[i];
	}
	assert(start_idx == num_keys);

	// recursively build the tree in the children nodes
	if (thread_num == 1 || thread_num < bf) {     // single-thread
		size_t offset = 0;
		for (int i = 0; i < bf; i++) {
			if (children[i] != nullptr) {
				children[i]->RecursiveBuild(counts[i], dim, depth, depth_curr+1, bf, p+offset, means, assign+offset, thread_num);
			}
			offset += counts[i];
		}
	}
	else {       // multi-thread
		std::vector<std::thread> threads;
		int subtree_threads = thread_num / bf;
		size_t offset = 0;
		std::vector<double *>sub_means(bf);
		for (int i = 0; i < bf; i++) {
			int sub_thread_num = 0;
			if (i == bf - 1) {
				sub_thread_num = thread_num - (bf - 1) * subtree_threads;
			}
			else {
				sub_thread_num = subtree_threads;
			}
			sub_means[i] = new double [bf * dim];
			memcpy(sub_means[i], means, sizeof(double) * bf * dim);
			threads.push_back(std::thread(MultiRecursiveBuild, children[i], counts[i], dim, depth, depth_curr+1, bf, p+offset, sub_means[i], assign+offset, sub_thread_num));

			offset += counts[i];
		}
		std::for_each(threads.begin(), threads.end(), std::mem_fn(&std::thread::join));
		for (int i = 0; i < bf; i++) {
			delete [] sub_means[i];
		}
	}
	if (depth_curr < 2) {
		for (int i = 0; i < bf; i++) {
			std::cout << "children " << i << " counts: " << counts[i] << std::endl;
		}
	}

	delete [] counts;
	return true;
}

bool TreeLeafNode::RecursiveBuild(size_t num_keys, int dim, int depth, int depth_curr, int bf, DTYPE **p, double *means, int *assign, int thread_num)
{
	return true;
}

//////////////////////////////////////////////////////////////////////////////
//                                                                          //
//                     Vocabulary Tree IO Module                            //
//                                                                          //
//////////////////////////////////////////////////////////////////////////////

bool VocabTree::WriteTree(const char *filename) const
{
	if (root == nullptr)
		return false;
	FILE *f = fopen(filename, "wb");
	if (f == nullptr) {
		std::cout << "[VocabTree] Error opening file " << filename << " for writing tree\n" << std::endl;
		return false;
	}

	// write header parameters
	fwrite(&branch_num, sizeof(int), 1, f);
	fwrite(&depth, sizeof(int), 1, f);
	fwrite(&dim, sizeof(int), 1, f);
	fwrite(&database_image_num, sizeof(size_t), 1, f);
	//fwrite(&num_nodes, sizeof(size_t), 1, f);

	// write node information recursively
	root->WriteNode(f, branch_num, dim);
	fclose(f);

	return true;
}

bool TreeInNode::WriteNode(FILE *f, int branch_num, int dim) const
{
	// write the current node's information
	char *has_children = new char [branch_num];
	char is_internal = 1;
	fwrite(&is_internal, sizeof(char), 1, f);
	fwrite(des, sizeof(DTYPE), dim, f);
	for (int i = 0; i < branch_num; i++) {
		if (children[i] == nullptr) {
			has_children[i] = 0;
		}
		else {
			has_children[i] = 1;
		}
	}
	fwrite(has_children, sizeof(char), branch_num, f);
	delete [] has_children;

	// recursively write children's information
	for (int i = 0; i < branch_num; i++) {
		if (children[i] != nullptr) {
			children[i]->WriteNode(f, branch_num, dim);
		}
	}
	return true;
}

bool TreeLeafNode::WriteNode(FILE *f, int branch_num, int dim) const
{
	char is_internal = 0;
	fwrite(&is_internal, sizeof(char), 1, f);
	fwrite(des, sizeof(DTYPE), dim, f);
	fwrite(&weight, sizeof(float), 1, f);

	int num_images = (int)inv_list.size();
	fwrite(&num_images, sizeof(int), 1, f);
	for (size_t i = 0; i < num_images; i++) {
		size_t img = inv_list[i].index;
		float count = inv_list[i].count;
		fwrite(&img, sizeof(size_t), 1, f);
		fwrite(&count, sizeof(float), 1, f);
	}

	return true;
}

// Read a vocabulary tree
bool VocabTree::ReadTree(const char *filename)
{
	if (root != nullptr)
		ClearTree();
	FILE *f = fopen(filename, "rb");
	if (f == nullptr) {
		std::cout << "[ReadTree] Error opening file " << filename << " for reading tree\n" << std::endl;
		return false;
	}

	// write header parameters
	char is_internal;
	size_t a, b, c, d, e;
	a = fread(&branch_num, sizeof(int), 1, f);
	b = fread(&depth, sizeof(int), 1, f);
	c = fread(&dim, sizeof(int), 1, f);
	e = fread(&database_image_num, sizeof(size_t), 1, f);
	//f = fread(&num_nodes, sizeof(size_t), 1, f);
	d = fread(&is_internal, sizeof(char), 1, f);
	if (a != 1 || b != 1 || c != 1 || d != 1 || e != 1) {
		std::cout << "[ReadTree] Reading Error\n";
		return false;
	}

	root = new TreeInNode();
	root->ReadNode(f, branch_num, dim);

	//char end = fgetc(f);
	//cout << (end == EOF) << endl;
	fclose(f);
	num_nodes = root->CountNodes(branch_num);
	num_leaves = IndexLeaves();
	size_t num_leaves1 = root->CountLeaves(branch_num);
	assert(num_leaves1 == num_leaves);

	return true;
}

bool TreeInNode::ReadNode(FILE *f, int branch_num, int dim)
{
	des = new DTYPE [dim];
	int a = (int)fread(des, sizeof(DTYPE), dim, f);
	if (a != dim) {
		std::cout << "[ReadNode] Reading error\n";
		return false;
	}
	char *has_children = new char [branch_num];
	int b = (int)fread(has_children, sizeof(char), branch_num, f);
	if (b != branch_num) {
		std::cout << "[ReadNode] Reading error\n";
		return false;
	}

	children = new TreeNode *[branch_num];
	for (int i = 0; i < branch_num; i++) {
		if (has_children[i] == 0) {
			children[i] = nullptr;
		}
		else {
			char is_internal;
			int c = (int)fread(&is_internal, sizeof(char), 1, f);
			if (c != 1) {
				std::cout << "[ReadNode] Reading error\n";
				return false;
			}
			if (is_internal) {
				children[i] = new TreeInNode();
			}
			else {
				children[i] = new TreeLeafNode();
			}
			children[i]->ReadNode(f, branch_num, dim);
		}
	}

	delete [] has_children;

	return true;
}

bool TreeLeafNode::ReadNode(FILE *f, int branch_num, int dim)
{
	des = new DTYPE [dim];
	int a = (int)fread(des, sizeof(DTYPE), dim, f);
	if (a != dim) {
		std::cout << "[ReadNode] Reading error\n";
		return false;
	}
	int num_images;
	int b = (int)fread(&weight, sizeof(float), 1, f);
	int c = (int)fread(&num_images, sizeof(int), 1, f);
	if (b != 1 || c != 1) {
		std::cout << "[ReadNode] Reading error\n";
		return false;
	}

	inv_list.resize(num_images);
	for (int i = 0; i < num_images; i++) {
		size_t img;
		float count;
		int d = (int)fread(&img, sizeof(size_t), 1, f);
		int e = (int)fread(&count, sizeof(float), 1, f);
		if (d != 1 || e != 1) {
			std::cout << "[ReadNode] Reading error\n";
			return false;
		}
		inv_list[i] = ImageCount(img, count);
	}
	return true;
}

//////////////////////////////////////////////////////////////////////////////
//                                                                          //
//                     Vocabulary Tree Utilities(Test)                      //
//                                                                          //
//////////////////////////////////////////////////////////////////////////////

void VocabTree::Show() const
{
	std::cout << "[VocabTree] depth/branch_num: " << depth << "/" << branch_num << '\n';
	std::cout << "[VocabTree] #nodes " << num_nodes << '\n';
	std::cout << "[VocabTree] #images " << database_image_num << '\n';
}

size_t TreeInNode::CountNodes(int branch_num) const
{
	size_t num_nodes = 0;
	for (int i = 0; i < branch_num; i++) {
		if (children[i] != nullptr)
			num_nodes += children[i]->CountNodes(branch_num);
	}
	return num_nodes + 1;
}

size_t TreeLeafNode::CountNodes(int branch_num) const {return 1;}

size_t TreeInNode::CountLeaves(int branch_num) const
{
	size_t num_leaves = 0;
	for (int i = 0; i < branch_num; i++) {
		if (children[i] != nullptr)
			num_leaves += children[i]->CountLeaves(branch_num);
	}
	return num_leaves;
}

size_t TreeLeafNode::CountLeaves(int branch_num) const {return 1;}

bool VocabTree::Compare(VocabTree & v) const
{
	if (v.dim != dim || v.depth != depth || v.branch_num != branch_num) {
		return false;
	}

	size_t node_count = root->CountNodes(branch_num);
	size_t other_node_count = v.root->CountNodes(branch_num);
	if (node_count != other_node_count) {
		return false;
	}

	size_t leave_count = root->CountLeaves(branch_num);
	size_t other_leave_count = v.root->CountLeaves(branch_num);
	if (leave_count != other_leave_count) {
		return false;
	}

	return root->Compare(v.root, branch_num, dim);
}

bool TreeInNode::Compare(TreeNode *in, int branch_num, int dim) const
{
	TreeInNode *other_node = dynamic_cast<TreeInNode*>(in);
	for (int i = 0; i < dim; i++)
	{
		if (des[i] != other_node->des[i])
			return false;
	}

	for (int i = 0; i < branch_num; i++) {
		if (children[i] == nullptr) {
			if (other_node->children[i] != nullptr)
				return false;
		}
		else {
			if (other_node->children[i] == nullptr)
				return false;
			children[i]->Compare(other_node->children[i], branch_num, dim);
		}
	}
	return true;
}

bool TreeLeafNode::Compare(TreeNode *leaf, int branch_num, int dim) const
{
	TreeLeafNode *other_leaf = dynamic_cast<TreeLeafNode*>(leaf);
	for (int i = 0; i < dim; i++) {
		if (des[i] != other_leaf->des[i])
			return false;
	}
	if (inv_list.size() != other_leaf->inv_list.size())    // shallow comparison
		return false;
	return true;
}

//////////////////////////////////////////////////////////////////////////////
//                                                                          //
//                     Vocabulary Tree Build Database                       //
//                                                                          //
//////////////////////////////////////////////////////////////////////////////
void MultiAddImage(TreeNode *root,
                   float *scores,
                   DTYPE *v,
                   size_t image_index,
                   int num_feature,
                   int branch_num,
                   int dim,
                   bool add)
{
	size_t off = 0;
	for (int i = 0; i < num_feature; i++) {
		root->DescendFeature(scores, v+off, image_index, branch_num, dim, true);
		off += dim;
	}
}

double VocabTree::AddImage2Tree(size_t image_index, SiftData &sift, int thread_num)
{
	float *q = new float [num_leaves];
	for (size_t i = 0; i < num_leaves; i++) {
		q[i] = 0.0;
	}

	int sift_num = sift.getFeatureNum();
	DTYPE *v = sift.getDesPointer();

	size_t off = 0;
	if (thread_num == 1) {    	// single-thread version
		for (int i = 0; i < sift_num; i++) {
			root->DescendFeature(q, v+off, image_index, branch_num, dim, true);
			off += dim;
		}
	}
	else {
		std::vector<std::thread> threads;
		for (int i = 0; i < thread_num; i++) {
			int thread_feature_num = sift_num / thread_num;
			if (i == thread_num - 1)
				thread_feature_num = sift_num - (thread_num - 1) * thread_feature_num;
			threads.push_back(std::thread(MultiAddImage, root, q, v+off, image_index, thread_feature_num, branch_num, dim, true));
			off += dim * thread_feature_num;
		}
		std::for_each(threads.begin(), threads.end(), std::mem_fn(&std::thread::join));
	}

	database_image_num++;
	delete [] q;
	return 0;

	// (optional) return the image vector magnitude (unnormalized)
	// double mag = root->ComputeImageVectorMagnitude(branch_num, dis_type);
	// switch(dis_type)
	// {
	//     case L1:
	//         return mag;
	//     case L2:
	//         return sqrt(mag);
	//     default:
	//         std::cout << "[ComputeImageVectorMagnitude] Wrong distance type\n";
	//         return 0;
	// }
}

size_t TreeInNode::DescendFeature(float *q, DTYPE *v, size_t image_index, int branch_num, int dim, bool add)
{
	int best_idx = 0;
	float min_distance = std::numeric_limits<float>::max();
	for (int i = 0; i < branch_num; i++) {
		if (children[i] != nullptr) {
			float curr_dist = l2sq(v, children[i]->des);
			if (curr_dist < min_distance) {
				min_distance = curr_dist;
				best_idx = i;
			}
		}
	}

	size_t ret = children[best_idx]->DescendFeature(q, v, image_index, branch_num, dim, add);
	return ret;
}

size_t TreeLeafNode::DescendFeature(float *q, DTYPE *v, size_t image_index, int branch_num, int dim, bool add)
{
	add_lock.lock();
	q[id] += weight;
	if (add) {    		// add this image to inverted list
		size_t curr_image_num = (size_t) inv_list.size();
		if (curr_image_num == 0)
			inv_list.push_back(ImageCount(image_index, (float)weight));
		else {
			if (inv_list[curr_image_num-1].index == image_index) {
				inv_list[curr_image_num-1].count += weight;
			}
			else {
				inv_list.push_back(ImageCount(image_index, weight));
			}
		}
	}
	add_lock.unlock();
	return id;
}

double TreeInNode::ComputeImageVectorMagnitude(int bf, DistanceType dt)
{
	double dist = 0.0;
	for (int i = 0; i < bf; i++)
	{
		if (children[i] != nullptr)
			dist += children[i]->ComputeImageVectorMagnitude(bf, dt);
	}
	return dist;
}

double TreeLeafNode::ComputeImageVectorMagnitude(int bf, DistanceType dt)
{
	switch (dt) {
		case L1:
			return score;
		case L2:
			return score * score;
		default:
			return 0.0;
	}
	return 0;
}

bool VocabTree::SetConstantWeight()
{
	if (root != nullptr) {
		root->SetConstantWeight(branch_num);
	}
	return true;
}

bool TreeInNode::SetConstantWeight(int bf)
{
	for (int i = 0; i < bf; i++) {
		if (children[i] != nullptr) {
			children[i]->SetConstantWeight(bf);
		}
	}
	return true;
}

bool TreeLeafNode::SetConstantWeight(int bf)
{
	weight = 1.0;
	return true;
}

bool VocabTree::ComputeTFIDFWeight(size_t image_num)
{
	if (root != nullptr)
		root->ComputeTFIDFWeight(branch_num, image_num);
	return true;
}

bool TreeInNode::ComputeTFIDFWeight(int bf, size_t n)
{
	for (int i = 0; i < bf; i++) {
		if (children[i] != nullptr)
			children[i]->ComputeTFIDFWeight(bf, n);
	}
	return true;
}

bool TreeLeafNode::ComputeTFIDFWeight(int bf, size_t n)
{
	size_t len = inv_list.size();
	if (len > 0) {
		weight = (float)log((double)n / (double)len);
	}
	else {
		weight = 0;
	}

	// pre-apply weight-adjustment to inverted list score
	for (size_t i = 0; i < len; i++) {
		inv_list[i].count *= weight;
	}

	return true;
}

bool VocabTree::NormalizeDatabase(size_t start_id, size_t image_num)
{
	std::vector<float> database_mag(image_num);
	database_mag.resize(image_num);
	for (size_t i = 0; i < image_num; i++) {
		database_mag[i] = 0;
	}

	root->ComputeDatabaseMagnitude(branch_num, dis_type, start_id, database_mag);
	// TODO(tianwei): figure out what is the proper way of normalizing the vocabulary tree
	if (dis_type == L2) { 		// take sqrt of the magnitude
		for (size_t i = 0; i < image_num; i++) {
			database_mag[i] = sqrt(database_mag[i]);
		}
	}

	for (size_t i = 0; i < image_num; i++) {
		std::cout << "[NormalizeDatabase] Normalized image #" << start_id+i <<
		             " vector magnitude " << database_mag[i] << std::endl;
	}
	return root->NormalizeDatabase(branch_num, start_id, database_mag);
}

bool TreeInNode::ComputeDatabaseMagnitude(int bf, DistanceType dis_type, size_t start_id, std::vector<float> &database_mag)
{
	for (int i = 0; i < bf; i++) {
		if (children[i] != nullptr)
			children[i]->ComputeDatabaseMagnitude(bf, dis_type, start_id, database_mag);
	}
	return true;
}

bool TreeLeafNode::ComputeDatabaseMagnitude(int bf, DistanceType dis_type, size_t start_id, std::vector<float> &database_mag)
{
	size_t len = inv_list.size();
	for (size_t i = 0; i < len; i++) {
		size_t index = inv_list[i].index - start_id;
		assert(index < database_mag.size());
		switch (dis_type) {
			case L1:
				database_mag[index] += inv_list[i].count;
				break;
			case L2:
				database_mag[index] += inv_list[i].count * inv_list[i].count;
				break;
			default:
				std::cout << "[ComputeDatabaseMagnitude] Error distance type\n";
				return false;
		}
	}
	return true;
}

bool TreeInNode::NormalizeDatabase(int bf, size_t start_id, std::vector<float> &database_mag)
{
	for (int i = 0; i < bf; i++) {
		if (children[i] != nullptr)
			children[i]->NormalizeDatabase(bf, start_id, database_mag);
	}
	return true;
}

bool TreeLeafNode::NormalizeDatabase(int bf, size_t start_id, std::vector<float> &database_mag)
{
	size_t len = inv_list.size();
	for (size_t i = 0; i < len; i++) {
		size_t index = inv_list[i].index - start_id;
		assert(index < database_mag.size());
		inv_list[i].count /= database_mag[index];
	}
	return true;
}

//////////////////////////////////////////////////////////////////////////////
//                                                                          //
//                     Vocabulary Tree Match Module                         //
//                                                                          //
//////////////////////////////////////////////////////////////////////////////
size_t leaves_count = 0;    // global leaf counter
bool VocabTree::Query(SiftData &sift, float *scores)
{
	float *q = new float [num_leaves];
	for (size_t i = 0; i < num_leaves; i++) {
		q[i] = 0.0;
	}

	int sift_num = sift.getFeatureNum();
	DTYPE *v = sift.getDesPointer();
	size_t off = 0;
	for (int i = 0; i < sift_num; i++) {
		root->DescendFeature(q, v+off, 0, branch_num, dim, false);
		off += dim;
	}

	double mag = 0.0;
	switch (dis_type) {
		case L1:
			for (size_t i = 0; i < num_leaves; i++)
				mag += q[i];
			break;
		case L2:
			for (size_t i = 0; i < num_leaves; i++)
				mag += q[i] * q[i];
			break;
		default:
			std::cout << "[Error] Unknow distance type in query database\n";
			return false;
	}
	if (dis_type == L2)
		mag = sqrt(mag);

	for (size_t i = 0; i < num_leaves; i++) {
		q[i] /= (float)mag;
	}
	root->ScoreQuery(q, branch_num, dis_type, scores);

	delete [] q;

	return true;
}

size_t VocabTree::IndexLeaves()
{
	leaves_count = 0;
	root->IndexLeaves(branch_num);
	return leaves_count;
}

bool TreeInNode::IndexLeaves(int bf)
{
	for (int i = 0; i < bf; i++) {
		if (children[i] != nullptr)
			children[i]->IndexLeaves(bf);
	}
	return true;
}

bool TreeLeafNode::IndexLeaves(int bf)
{
	id = leaves_count++;
	return true;
}

bool TreeInNode::FillQueryVector(float *q, int bf, float normalize)
{
	for (int i = 0; i < bf; i++) {
		if (children[i] != nullptr) {
			children[i]->FillQueryVector(q, bf, normalize);
		}
	}
	return true;
}

bool TreeLeafNode::FillQueryVector(float *q, int bf, float normalize)
{
	q[id] = score * normalize;
	return true;
}

bool TreeInNode::ScoreQuery(float *q, int bf, DistanceType dt, float *scores)
{
	for (int i = 0; i < bf; i++) {
		if (children[i] != nullptr) {
			children[i]->ScoreQuery(q, bf, dt, scores);
		}
	}
	return true;
}

bool TreeLeafNode::ScoreQuery(float *q, int bf, DistanceType dt, float *scores)
{
	if (q[id] == 0.0)
		return true;
	size_t len = inv_list.size();

	for (int i = 0; i < len; i++) {
		size_t idx = inv_list[i].index;
		switch (dt) {
			case L1:
				scores[idx] += q[id] > inv_list[i].count ? inv_list[i].count : q[id];
				break;
			case L2:
				scores[idx] += q[id] * inv_list[i].count;
				break;
			default:
				std::cout << "[ScoreQuery] Error distacne type\n";
		}
	}

	return  true;
}

}   // end of namespace vot


/**
 *
 * Copyright (c) 2010 Matthias Walter (xammy@xammy.homelinux.net)
 *
 * Authors: Matthias Walter
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 */

#ifndef BOOST_GRAPH_BIPARTITE_HPP
#define BOOST_GRAPH_BIPARTITE_HPP

#include <utility>
#include <vector>
#include <exception>
#include <boost/graph/properties.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/bind.hpp>

namespace boost
{

namespace detail
{

    /**
     * The bipartite_visitor_error is thrown if an edge cannot be colored.
     * The witnesses are the edges incident vertices.
     */

    template < typename Vertex >
    struct BOOST_SYMBOL_VISIBLE bipartite_visitor_error : std::exception
    {
        std::pair< Vertex, Vertex > witnesses;

        bipartite_visitor_error(Vertex a, Vertex b) : witnesses(a, b) {}

        const char* what() const throw() { return "Graph is not bipartite."; }
    };

    /**
     * Functor which colors edges to be non-monochromatic.
     */

    template < typename PartitionMap > struct bipartition_colorize
    {
        typedef on_tree_edge event_filter;

        bipartition_colorize(PartitionMap partition_map)
        : partition_map_(partition_map)
        {
        }

        template < typename Edge, typename Graph >
        void operator()(Edge e, const Graph& g)
        {
            typedef typename graph_traits< Graph >::vertex_descriptor
                vertex_descriptor_t;
            typedef color_traits<
                typename property_traits< PartitionMap >::value_type >
                color_traits;

            vertex_descriptor_t source_vertex = source(e, g);
            vertex_descriptor_t target_vertex = target(e, g);
            if (get(partition_map_, source_vertex) == color_traits::white())
                put(partition_map_, target_vertex, color_traits::black());
            else
                put(partition_map_, target_vertex, color_traits::white());
        }

    private:
        PartitionMap partition_map_;
    };

    /**
     * Creates a bipartition_colorize functor which colors edges
     * to be non-monochromatic.
     *
     * @param partition_map Color map for the bipartition
     * @return The functor.
     */

    template < typename PartitionMap >
    inline bipartition_colorize< PartitionMap > colorize_bipartition(
        PartitionMap partition_map)
    {
        return bipartition_colorize< PartitionMap >(partition_map);
    }

    /**
     * Functor which tests an edge to be monochromatic.
     */

    template < typename PartitionMap > struct bipartition_check
    {
        typedef on_back_edge event_filter;

        bipartition_check(PartitionMap partition_map)
        : partition_map_(partition_map)
        {
        }

        template < typename Edge, typename Graph >
        void operator()(Edge e, const Graph& g)
        {
            typedef typename graph_traits< Graph >::vertex_descriptor
                vertex_descriptor_t;

            vertex_descriptor_t source_vertex = source(e, g);
            vertex_descriptor_t target_vertex = target(e, g);
            if (get(partition_map_, source_vertex)
                == get(partition_map_, target_vertex))
                throw bipartite_visitor_error< vertex_descriptor_t >(
                    source_vertex, target_vertex);
        }

    private:
        PartitionMap partition_map_;
    };

    /**
     * Creates a bipartition_check functor which raises an error if a
     * monochromatic edge is found.
     *
     * @param partition_map The map for a bipartition.
     * @return The functor.
     */

    template < typename PartitionMap >
    inline bipartition_check< PartitionMap > check_bipartition(
        PartitionMap partition_map)
    {
        return bipartition_check< PartitionMap >(partition_map);
    }

    /**
     * Find the beginning of a common suffix of two sequences
     *
     * @param sequence1 Pair of bidirectional iterators defining the first
     * sequence.
     * @param sequence2 Pair of bidirectional iterators defining the second
     * sequence.
     * @return Pair of iterators pointing to the beginning of the common suffix.
     */

    template < typename BiDirectionalIterator1,
        typename BiDirectionalIterator2 >
    inline std::pair< BiDirectionalIterator1, BiDirectionalIterator2 >
    reverse_mismatch(
        std::pair< BiDirectionalIterator1, BiDirectionalIterator1 > sequence1,
        std::pair< BiDirectionalIterator2, BiDirectionalIterator2 > sequence2)
    {
        if (sequence1.first == sequence1.second
            || sequence2.first == sequence2.second)
            return std::make_pair(sequence1.first, sequence2.first);

        BiDirectionalIterator1 iter1 = sequence1.second;
        BiDirectionalIterator2 iter2 = sequence2.second;

        while (true)
        {
            --iter1;
            --iter2;
            if (*iter1 != *iter2)
            {
                ++iter1;
                ++iter2;
                break;
            }
            if (iter1 == sequence1.first)
                break;
            if (iter2 == sequence2.first)
                break;
        }

        return std::make_pair(iter1, iter2);
    }

}

/**
 * Checks a given graph for bipartiteness and fills the given color map with
 * white and black according to the bipartition. If the graph is not
 * bipartite, the contents of the color map are undefined. Runs in linear
 * time in the size of the graph, if access to the property maps is in
 * constant time.
 *
 * @param graph The given graph.
 * @param index_map An index map associating vertices with an index.
 * @param partition_map A color map to fill with the bipartition.
 * @return true if and only if the given graph is bipartite.
 */

template < typename Graph, typename IndexMap, typename PartitionMap >
bool is_bipartite(
    const Graph& graph, const IndexMap index_map, PartitionMap partition_map)
{
    /// General types and variables
    typedef
        typename property_traits< PartitionMap >::value_type partition_color_t;
    typedef
        typename graph_traits< Graph >::vertex_descriptor vertex_descriptor_t;

    /// Declare dfs visitor
    //    detail::empty_recorder recorder;
    //    typedef detail::bipartite_visitor <PartitionMap,
    //    detail::empty_recorder> dfs_visitor_t; dfs_visitor_t dfs_visitor
    //    (partition_map, recorder);

    /// Call dfs
    try
    {
        depth_first_search(graph,
            vertex_index_map(index_map).visitor(make_dfs_visitor(
                std::make_pair(detail::colorize_bipartition(partition_map),
                    std::make_pair(detail::check_bipartition(partition_map),
                        put_property(partition_map,
                            color_traits< partition_color_t >::white(),
                            on_start_vertex()))))));
    }
    catch (const detail::bipartite_visitor_error< vertex_descriptor_t >&)
    {
        return false;
    }

    return true;
}

/**
 * Checks a given graph for bipartiteness.
 *
 * @param graph The given graph.
 * @param index_map An index map associating vertices with an index.
 * @return true if and only if the given graph is bipartite.
 */

template < typename Graph, typename IndexMap >
bool is_bipartite(const Graph& graph, const IndexMap index_map)
{
    typedef one_bit_color_map< IndexMap > partition_map_t;
    partition_map_t partition_map(num_vertices(graph), index_map);

    return is_bipartite(graph, index_map, partition_map);
}

/**
 * Checks a given graph for bipartiteness. The graph must
 * have an internal vertex_index property. Runs in linear time in the
 * size of the graph, if access to the property maps is in constant time.
 *
 * @param graph The given graph.
 * @return true if and only if the given graph is bipartite.
 */

template < typename Graph > bool is_bipartite(const Graph& graph)
{
    return is_bipartite(graph, get(vertex_index, graph));
}

/**
 * Checks a given graph for bipartiteness and fills a given color map with
 * white and black according to the bipartition. If the graph is not
 * bipartite, a sequence of vertices, producing an odd-cycle, is written to
 * the output iterator. The final iterator value is returned. Runs in linear
 * time in the size of the graph, if access to the property maps is in
 * constant time.
 *
 * @param graph The given graph.
 * @param index_map An index map associating vertices with an index.
 * @param partition_map A color map to fill with the bipartition.
 * @param result An iterator to write the odd-cycle vertices to.
 * @return The final iterator value after writing.
 */

template < typename Graph, typename IndexMap, typename PartitionMap,
    typename OutputIterator >
OutputIterator find_odd_cycle(const Graph& graph, const IndexMap index_map,
    PartitionMap partition_map, OutputIterator result)
{
    /// General types and variables
    typedef
        typename property_traits< PartitionMap >::value_type partition_color_t;
    typedef
        typename graph_traits< Graph >::vertex_descriptor vertex_descriptor_t;
    typedef typename graph_traits< Graph >::vertex_iterator vertex_iterator_t;
    vertex_iterator_t vertex_iter, vertex_end;

    /// Declare predecessor map
    typedef std::vector< vertex_descriptor_t > predecessors_t;
    typedef iterator_property_map< typename predecessors_t::iterator, IndexMap,
        vertex_descriptor_t, vertex_descriptor_t& >
        predecessor_map_t;

    predecessors_t predecessors(
        num_vertices(graph), graph_traits< Graph >::null_vertex());
    predecessor_map_t predecessor_map(predecessors.begin(), index_map);

    /// Initialize predecessor map
    for (boost::tie(vertex_iter, vertex_end) = vertices(graph);
         vertex_iter != vertex_end; ++vertex_iter)
    {
        put(predecessor_map, *vertex_iter, *vertex_iter);
    }

    /// Call dfs
    try
    {
        depth_first_search(graph,
            vertex_index_map(index_map).visitor(make_dfs_visitor(
                std::make_pair(detail::colorize_bipartition(partition_map),
                    std::make_pair(detail::check_bipartition(partition_map),
                        std::make_pair(
                            put_property(partition_map,
                                color_traits< partition_color_t >::white(),
                                on_start_vertex()),
                            record_predecessors(
                                predecessor_map, on_tree_edge())))))));
    }
    catch (const detail::bipartite_visitor_error< vertex_descriptor_t >& error)
    {
        typedef std::vector< vertex_descriptor_t > path_t;

        path_t path1, path2;
        vertex_descriptor_t next, current;

        /// First path
        next = error.witnesses.first;
        do
        {
            current = next;
            path1.push_back(current);
            next = predecessor_map[current];
        } while (current != next);

        /// Second path
        next = error.witnesses.second;
        do
        {
            current = next;
            path2.push_back(current);
            next = predecessor_map[current];
        } while (current != next);

        /// Find beginning of common suffix
        std::pair< typename path_t::iterator, typename path_t::iterator >
            mismatch = detail::reverse_mismatch(
                std::make_pair(path1.begin(), path1.end()),
                std::make_pair(path2.begin(), path2.end()));

        /// Copy the odd-length cycle
        result = std::copy(path1.begin(), mismatch.first + 1, result);
        return std::reverse_copy(path2.begin(), mismatch.second, result);
    }

    return result;
}

/**
 * Checks a given graph for bipartiteness. If the graph is not bipartite, a
 * sequence of vertices, producing an odd-cycle, is written to the output
 * iterator. The final iterator value is returned. Runs in linear time in the
 * size of the graph, if access to the property maps is in constant time.
 *
 * @param graph The given graph.
 * @param index_map An index map associating vertices with an index.
 * @param result An iterator to write the odd-cycle vertices to.
 * @return The final iterator value after writing.
 */

template < typename Graph, typename IndexMap, typename OutputIterator >
OutputIterator find_odd_cycle(
    const Graph& graph, const IndexMap index_map, OutputIterator result)
{
    typedef one_bit_color_map< IndexMap > partition_map_t;
    partition_map_t partition_map(num_vertices(graph), index_map);

    return find_odd_cycle(graph, index_map, partition_map, result);
}

/**
 * Checks a given graph for bipartiteness. If the graph is not bipartite, a
 * sequence of vertices, producing an odd-cycle, is written to the output
 * iterator. The final iterator value is returned. The graph must have an
 * internal vertex_index property. Runs in linear time in the size of the
 * graph, if access to the property maps is in constant time.
 *
 * @param graph The given graph.
 * @param result An iterator to write the odd-cycle vertices to.
 * @return The final iterator value after writing.
 */

template < typename Graph, typename OutputIterator >
OutputIterator find_odd_cycle(const Graph& graph, OutputIterator result)
{
    return find_odd_cycle(graph, get(vertex_index, graph), result);
}
}

#endif /// BOOST_GRAPH_BIPARTITE_HPP