/*****************************************************************************
*
* Copyright (c) 2003-2018 by The University of Queensland
* http://www.uq.edu.au
*
* Primary Business: Queensland, Australia
* Licensed under the Apache License, version 2.0
* http://www.apache.org/licenses/LICENSE-2.0
*
* Development until 2012 by Earth Systems Science Computational Center (ESSCC)
* Development 2012-2013 by School of Earth Sciences
* Development from 2014 by Centre for Geoscience Computing (GeoComp)
*
*****************************************************************************/

#include "DataFactory.h"

#include <boost/python/extract.hpp>
#include <boost/scoped_array.hpp>

#include <exception>
#include <iostream>
#include <fstream>

#ifdef ESYS_HAVE_NETCDF
 #ifdef NETCDF4
  #include <ncDim.h>
  #include <ncVar.h>
  #include <ncFile.h>

 #include "NCHelper.h"

 #else
  #include <netcdfcpp.h>
 #endif
#endif

namespace bp = boost::python;
#ifdef NETCDF4
using namespace netCDF;
#endif

namespace escript {

Data Scalar(double value, const FunctionSpace& what, bool expanded)
{
    // an empty shape is a scalar
    DataTypes::ShapeType shape;
    return Data(value, shape, what, expanded);
}

Data Scalar(DataTypes::cplx_t value, const FunctionSpace& what, bool expanded)
{
    // an empty shape is a scalar
    DataTypes::ShapeType shape;
    return Data(value, shape, what, expanded);
}

Data
ScalarFromObj(boost::python::object o,
	const FunctionSpace& what,
	bool expanded)
{
    // check for real first
    try {
        double v = bp::extract<double>(o);
        return Scalar(v, what, expanded);
    } catch(...) {
        PyErr_Clear();
    }
    // check for real first
    try {
        DataTypes::cplx_t v = bp::extract<DataTypes::cplx_t>(o);
        return Scalar(v, what, expanded);
    } catch(...) {
        PyErr_Clear();
    }
    throw DataException("Can not make a Scalar from a non-scalar value.");
}

Data Vector(double value, const FunctionSpace& what, bool expanded)
{
    DataTypes::ShapeType shape(1, what.getDomain()->getDim());
    return Data(value, shape, what, expanded);
}

Data VectorFromObj(bp::object o, const FunctionSpace& what, bool expanded)
{
    // first try to get a double and route it to the other method
    try {
        double v = bp::extract<double>(o);
        return Vector(v, what, expanded);
    } catch(...) {
        PyErr_Clear();
    }
    DataTypes::ShapeType shape(1, what.getDomain()->getDim());
    Data d(o, what, expanded);
    if (d.getDataPointShape() != shape) {
        throw DataException("VectorFromObj: Shape of vector passed to function"
               " does not match the dimension of the domain. ");
    }
    return d;
}

Data Tensor(double value, const FunctionSpace& what, bool expanded)
{
    DataTypes::ShapeType shape(2, what.getDomain()->getDim());
    return Data(value, shape, what, expanded);
}

Data TensorC(DataTypes::cplx_t value, const FunctionSpace& what, bool expanded)
{
    DataTypes::ShapeType shape(2, what.getDomain()->getDim());
    return Data(value, shape, what, expanded);
}

// We need to take some care here because this signature trumps the other one from boost's point of view
Data TensorFromObj(bp::object o, const FunctionSpace& what, bool expanded)
{
    // first try to get a double and route it to the other method
    try {
        double v = bp::extract<double>(o);
        return Tensor(v, what, expanded);
    } catch(...) {
        PyErr_Clear();
    }
    // now try to get a complex and route to scalar factory
    try {
        DataTypes::cplx_t v = bp::extract<DataTypes::cplx_t>(o);
        return TensorC(v, what, expanded);
    } catch(...) {
        PyErr_Clear();
    }
    DataTypes::ShapeType shape(2, what.getDomain()->getDim());
    Data d(o, what, expanded);
    if (d.getDataPointShape() != shape) {
        throw DataException("TensorFromObj: Shape of tensor passed to function"
               " does not match the dimension of the domain.");
    }
    return d;
}

Data Tensor3(double value, const FunctionSpace& what, bool expanded)
{
    DataTypes::ShapeType shape(3, what.getDomain()->getDim());
    return Data(value, shape, what, expanded);
}

Data Tensor3C(DataTypes::cplx_t  value, const FunctionSpace& what, bool expanded)
{
    DataTypes::ShapeType shape(3, what.getDomain()->getDim());
    return Data(value, shape, what, expanded);
}

Data Tensor3FromObj(bp::object o, const FunctionSpace& what, bool expanded)
{
    // first try to get a double and route it to the other method
    try {
        double v = bp::extract<double>(o);
        return Tensor3(v, what, expanded);
    } catch(...) {
        PyErr_Clear();
    }
    // first try to get a complex and route it to the other method
    try {
        DataTypes::cplx_t v = bp::extract<DataTypes::cplx_t>(o);
        return Tensor3C(v, what, expanded);
    } catch(...) {
        PyErr_Clear();
    }
    DataTypes::ShapeType shape(3, what.getDomain()->getDim());
    Data d(o, what, expanded);
    if (d.getDataPointShape() != shape) {
        throw DataException("Tensor3FromObj: Shape of tensor passed to "
                "function does not match the dimension of the domain.");
    }
    return d;
}

Data Tensor4(double value, const FunctionSpace& what, bool expanded)
{
    DataTypes::ShapeType shape(4, what.getDomain()->getDim());
    return Data(value, shape, what, expanded);
}

Data Tensor4C(DataTypes::cplx_t value, const FunctionSpace& what, bool expanded)
{
    DataTypes::ShapeType shape(4, what.getDomain()->getDim());
    return Data(value, shape, what, expanded);
}

Data Tensor4FromObj(bp::object o, const FunctionSpace& what, bool expanded)
{
    // first try to get a double and route it to the other method
    try {
        double v = bp::extract<double>(o);
        return Tensor4(v, what, expanded);
    } catch(...) {
        PyErr_Clear();
    }
    // first try to get a double and route it to the other method
    try {
        DataTypes::cplx_t v = bp::extract<DataTypes::cplx_t>(o);
        return Tensor4C(v, what, expanded);
    } catch(...) {
        PyErr_Clear();
    }
    DataTypes::ShapeType shape(4, what.getDomain()->getDim());
    Data d(o, what, expanded);
    if (d.getDataPointShape() != shape) {
        throw DataException("VectorFromObj: Shape of tensor passed to function"
               " does not match the dimension of the domain.");
    }
    return d;
}

#ifdef NETCDF4

// Warning: at time of writing, calls to ncVar::getVar are not range checked as was done under the previous API
// we want to see if it works first, but at some stage that should be tightened up

Data load(const std::string fileName, const AbstractDomain& domain)
{
#ifdef ESYS_HAVE_NETCDF
    JMPI mpiInfo(domain.getMPI());
    const std::string newFileName(mpiInfo->appendRankToFileName(fileName));
    NcFile dataFile;
    if (!openNcFile(dataFile, newFileName))
    {
        throw DataException("load: opening of netCDF file for input failed.");
    }
    Data out;
    int error = 0;
    std::string msg;
    try {
        int line=0;
        int rank=-1;
        int type=-1;
        int function_space_type=0;  // =0 should not actually be used but we keep compiler happy
        try
        {
            // recover function space
            NcGroupAtt fst=dataFile.getAtt("function_space_type");
            if (fst.getAttLength()!=1)
            {
                throw DataException("load: oversize attribute function_space_type");
            }
            fst.getValues(&function_space_type);
        }
        catch (exceptions::NcException* e)
        {
                throw DataException("load: cannot recover function_space_type attribute from escript netCDF file.");
        }
        line=0;
        if (!domain.isValidFunctionSpaceType(function_space_type))
        {       // jump to outer catch
            throw DataException("load: function space type code in netCDF file is invalid for given domain.");
        }
        FunctionSpace function_space=FunctionSpace(domain.getPtr(), function_space_type);
        try
        {
            line++;
            NcGroupAtt rt=dataFile.getAtt("rank");
            if (rt.getAttLength()!=1)
            {
                throw DataException("load: oversize attribute rank");
            }
            rt.getValues(&rank);
            line++;
            if (rank<0 || rank>DataTypes::maxRank)
                throw DataException("load: rank in escript netCDF file is greater than maximum rank.");
            // recover type attribute
            //   looks like we can have either "type" or "typeid"
            NcGroupAtt tatt=dataFile.getAtt("type");
            if (!tatt.isNull())
            {
                std::string type_str;
                tatt.getValues(&type_str);
                if (type_str=="constant")
                {
                    type = 0;
                }
                else if (type_str=="tagged")
                {
                    type = 1;
                }
                else // if (type_str=="expanded")
                {
                    type = 2;
                }
            }
            else
            {
                tatt=dataFile.getAtt("type_id");
                if (!tatt.isNull())
                {
                    if (tatt.getAttLength()>1)
                    {
                        throw DataException("load: oversize attribute type_id");
                    }
                    //type=dataFile.getAtt("type_id").as_int(0);
                    tatt.getValues(&type);
                }
                else
                {
                    throw DataException("load: cannot recover type attribute from escript netCDF file.");
                }
            }
        }
        catch (exceptions::NcException* e)
        {
            switch (line)
            {
            case 1: throw DataException("load: cannot recover rank attribute from escript netCDF file.");
            default:
                throw DataException("load: unspecified error.");
            }
        }
        // recover dimension
        int ndims=dataFile.getDimCount();
        int ntags =0 , num_samples =0 , num_data_points_per_sample =0, d=0, len_data_point=1;
        NcDim d_dim, tags_dim, num_samples_dim, num_data_points_per_sample_dim;
        /* recover shape */
        DataTypes::ShapeType shape;
//        long dims[DataTypes::maxRank+2];
        if (rank>0) {
            if ((d_dim=dataFile.getDim("d0")).isNull() )
                throw DataException("load: unable to recover d0 from netCDF file.");
            d=d_dim.getSize();
            shape.push_back(d);
//             dims[0]=d;
            len_data_point*=d;
        }
        if (rank>1) {
            if ((d_dim=dataFile.getDim("d1")).isNull() )
                throw DataException("load: unable to recover d1 from netCDF file.");
            d=d_dim.getSize();
            shape.push_back(d);
//             dims[1]=d;
            len_data_point*=d;
        }
        if (rank>2) {
            if ((d_dim=dataFile.getDim("d2")).isNull() )
                throw DataException("load: unable to recover d2 from netCDF file.");
            d=d_dim.getSize();
            shape.push_back(d);
//             dims[2]=d;
            len_data_point*=d;
        }
        if (rank>3) {
            if ((d_dim=dataFile.getDim("d3")).isNull() )
                throw DataException("load: unable to recover d3 from netCDF file.");
            d=d_dim.getSize();
            shape.push_back(d);
//             dims[3]=d;
            len_data_point*=d;
        }

        NcVar var, ids_var, tags_var;
        if (type == 0) {
            // constant data
            if ( ! ( (ndims == rank && rank >0) || ( ndims ==1 && rank == 0 ) ) )
                throw DataException("load: illegal number of dimensions for constant data in netCDF file.");
            if (rank == 0) {
                if ((d_dim=dataFile.getDim("l")).isNull() )
                    throw DataException("load: unable to recover d0 for scalar constant data in netCDF file.");
                int d0 = d_dim.getSize();
                if (d0 != 1)
                    throw DataException("load: d0 is expected to be one for scalar constant data in netCDF file.");
//                 dims[0]=1;
            }
            out=Data(0,shape,function_space,false);
            if ((var = dataFile.getVar("data")).isNull())
                throw DataException("load: unable to find data in netCDF file.");
            var.getVar(&(out.getDataAtOffsetRW(out.getDataOffset(0,0), static_cast<DataTypes::real_t>(0))));
        } else if (type == 1) {
            // tagged data
            if ( ! (ndims == rank + 1) )
                throw DataException("load: illegal number of dimensions for tagged data in netCDF file.");
            if ((tags_dim=dataFile.getDim("num_tags")).isNull() )
                throw DataException("load: unable to recover number of tags from netCDF file.");
            ntags=tags_dim.getSize();
//             dims[rank]=ntags;
            std::vector<int> tags(ntags);
            if (( tags_var = dataFile.getVar("tags")).isNull() )
                throw DataException("load: unable to find tags in netCDF file.");
            // oversize could be a problem here?
            tags_var.getVar(&tags[0]);

            // A) create a DataTagged dt
            // B) Read data from file
            // C) copy default value into dt
            // D) copy tagged values into dt
            // E) create a new Data based on dt

            NcVar var1;
            DataTypes::RealVectorType data1(len_data_point * ntags, 0., len_data_point * ntags);
            if ((var1 = dataFile.getVar("data")).isNull())
                throw DataException("load: unable to find data in netCDF file.");
            var1.getVar(&(data1[0]));
            DataTagged* dt=new DataTagged(function_space, shape, &tags[0], data1);
            out=Data(dt);
        } else if (type == 2) {
            // expanded data
            if ( ! (ndims == rank + 2) )
                throw DataException("load: illegal number of dimensions for expanded data in netCDF file.");
            if ((num_samples_dim = dataFile.getDim("num_samples")).isNull() )
                throw DataException("load: unable to recover number of samples from netCDF file.");
            num_samples = num_samples_dim.getSize();
            if ((num_data_points_per_sample_dim = dataFile.getDim("num_data_points_per_sample")).isNull() )
                throw DataException("load: unable to recover number of data points per sample from netCDF file.");
            num_data_points_per_sample=num_data_points_per_sample_dim.getSize();
            // check shape:
            if ( ! (num_samples == function_space.getNumSamples() && num_data_points_per_sample == function_space.getNumDataPointsPerSample()) )
                throw DataException("load: data sample layout of file does not match data layout of function space.");
            if (num_samples==0) {
                out = Data(0,shape,function_space,true);
            } else {
                // get ids
                if (( ids_var = dataFile.getVar("id")).isNull() )
                    throw DataException("load: unable to find reference ids in netCDF file.");
                const DataTypes::dim_t* ids_p=function_space.borrowSampleReferenceIDs();
                std::vector<DataTypes::dim_t> ids_of_nc(num_samples);
                // oversize could be a problem here
                ids_var.getVar(&ids_of_nc[0]);
                // check order:
                int failed=-1, local_failed=-1, i;
#pragma omp parallel private(local_failed)
                {
                    local_failed=-1;
#pragma omp for private(i) schedule(static)
                    for (i=0; i < num_samples; ++i) {
                        if (ids_of_nc[i]!=ids_p[i]) local_failed=i;
                    }
#pragma omp critical
                    if (local_failed>=0) failed = local_failed;
                }
                // get the data:
//                 dims[rank]=num_data_points_per_sample;
//                 dims[rank+1]=num_samples;
                out=Data(0,shape,function_space,true);
                if ((var = dataFile.getVar("data")).isNull())
                    throw DataException("load: unable to find data in netCDF file.");
                var.getVar(&(out.getDataAtOffsetRW(out.getDataOffset(0,0), static_cast<DataTypes::real_t>(0))));
                if (failed >= 0) {
                    try {
                        std::cout << "Information - load: start reordering data from netCDF file " << fileName << std::endl;
                        out.borrowData()->reorderByReferenceIDs(&ids_of_nc[0]);
                    } catch (std::exception&) {
                        throw DataException("load: unable to reorder data in netCDF file.");
                    }
                }
            }
        } else {
            throw DataException("load: unknown escript data type in netCDF file.");
        }
    } catch (DataException& e) {
        error=1;
        msg=e.what();
    }
    int gerror = error;
    checkResult(error, gerror, mpiInfo);
    if (gerror > 0) {
        char* gmsg;
        shipString(msg.c_str(), &gmsg, mpiInfo->comm);
        throw DataException(gmsg);
    }
    return out;
#else
    throw DataException("load: not compiled with netCDF. Please contact your"
                        " installation manager.");
#endif // ESYS_HAVE_NETCDF
}

#else

Data load(const std::string fileName, const AbstractDomain& domain)
{
#ifdef ESYS_HAVE_NETCDF
    NcAtt *type_att, *rank_att, *function_space_type_att;
    // netCDF error handler
    NcError err(NcError::silent_nonfatal);
    JMPI mpiInfo(domain.getMPI());
    const std::string newFileName(mpiInfo->appendRankToFileName(fileName));
    NcFile dataFile(newFileName.c_str(), NcFile::ReadOnly);
    Data out;
    int error = 0;
    std::string msg;
    try {
        if (!dataFile.is_valid())
            throw DataException("load: opening of netCDF file for input failed.");
       // recover function space
        if (! (function_space_type_att=dataFile.get_att("function_space_type")) )
            throw DataException("load: cannot recover function_space_type attribute from escript netCDF file.");
        int function_space_type = function_space_type_att->as_int(0);
        delete function_space_type_att;
        // test if function space id is valid and create function space instance
        if (!domain.isValidFunctionSpaceType(function_space_type))
            throw DataException("load: function space type code in netCDF file is invalid for given domain.");
        FunctionSpace function_space=FunctionSpace(domain.getPtr(), function_space_type);
        // recover rank
        if (! (rank_att=dataFile.get_att("rank")) )
            throw DataException("load: cannot recover rank attribute from escript netCDF file.");
        int rank = rank_att->as_int(0);
        delete rank_att;
        if (rank<0 || rank>DataTypes::maxRank)
            throw DataException("load: rank in escript netCDF file is greater than maximum rank.");
        // recover type attribute
        int type=-1;
        if ((type_att=dataFile.get_att("type")) ) {
            boost::scoped_array<char> type_str(type_att->as_string(0));
            if (strncmp(type_str.get(), "constant", strlen("constant")) == 0 ) {
                type = 0;
            } else if (strncmp(type_str.get(), "tagged", strlen("tagged")) == 0 ) {
                type = 1;
            } else if (strncmp(type_str.get(), "expanded", strlen("expanded")) == 0 ) {
                type = 2;
            }
        } else {
            if (! (type_att=dataFile.get_att("type_id")) )
                throw DataException("load: cannot recover type attribute from escript netCDF file.");
            type=type_att->as_int(0);
        }
        delete type_att;

        // recover dimension
        int ndims=dataFile.num_dims();
        int ntags =0 , num_samples =0 , num_data_points_per_sample =0, d=0, len_data_point=1;
        NcDim *d_dim, *tags_dim, *num_samples_dim, *num_data_points_per_sample_dim;
        /* recover shape */
        DataTypes::ShapeType shape;
        long dims[DataTypes::maxRank+2];
        if (rank>0) {
            if (! (d_dim=dataFile.get_dim("d0")) )
                throw DataException("load: unable to recover d0 from netCDF file.");
            d=d_dim->size();
            shape.push_back(d);
            dims[0]=d;
            len_data_point*=d;
        }
        if (rank>1) {
            if (! (d_dim=dataFile.get_dim("d1")) )
                throw DataException("load: unable to recover d1 from netCDF file.");
            d=d_dim->size();
            shape.push_back(d);
            dims[1]=d;
            len_data_point*=d;
        }
        if (rank>2) {
            if (! (d_dim=dataFile.get_dim("d2")) )
                throw DataException("load: unable to recover d2 from netCDF file.");
            d=d_dim->size();
            shape.push_back(d);
            dims[2]=d;
            len_data_point*=d;
        }
        if (rank>3) {
            if (! (d_dim=dataFile.get_dim("d3")) )
                throw DataException("load: unable to recover d3 from netCDF file.");
            d=d_dim->size();
            shape.push_back(d);
            dims[3]=d;
            len_data_point*=d;
        }

        NcVar *var, *ids_var, *tags_var;
        if (type == 0) {
            // constant data
            if ( ! ( (ndims == rank && rank >0) || ( ndims ==1 && rank == 0 ) ) )
                throw DataException("load: illegal number of dimensions for constant data in netCDF file.");
            if (rank == 0) {
                if (! (d_dim=dataFile.get_dim("l")) )
                    throw DataException("load: unable to recover d0 for scalar constant data in netCDF file.");
                int d0 = d_dim->size();
                if (d0 != 1)
                    throw DataException("load: d0 is expected to be one for scalar constant data in netCDF file.");
                dims[0]=1;
            }
            out=Data(0,shape,function_space,false);
            if (!(var = dataFile.get_var("data")))
                throw DataException("load: unable to find data in netCDF file.");
            if (! var->get(&(out.getDataAtOffsetRW(out.getDataOffset(0,0), static_cast<DataTypes::real_t>(0))), dims) )
                throw DataException("load: unable to recover data from netCDF file.");
        } else if (type == 1) {
            // tagged data
            if ( ! (ndims == rank + 1) )
                throw DataException("load: illegal number of dimensions for tagged data in netCDF file.");
            if (! (tags_dim=dataFile.get_dim("num_tags")) )
                throw DataException("load: unable to recover number of tags from netCDF file.");
            ntags=tags_dim->size();
            dims[rank]=ntags;
            std::vector<int> tags(ntags);
            if (! ( tags_var = dataFile.get_var("tags")) )
                throw DataException("load: unable to find tags in netCDF file.");
            if (! tags_var->get(&tags[0], ntags) )
                throw DataException("load: unable to recover tags from netCDF file.");

            // A) create a DataTagged dt
            // B) Read data from file
            // C) copy default value into dt
            // D) copy tagged values into dt
            // E) create a new Data based on dt

            NcVar* var1;
            DataTypes::RealVectorType data1(len_data_point * ntags, 0., len_data_point * ntags);
            if (!(var1 = dataFile.get_var("data")))
                throw DataException("load: unable to find data in netCDF file.");
            if (! var1->get(&(data1[0]), dims) )
                throw DataException("load: unable to recover data from netCDF file.");
            DataTagged* dt=new DataTagged(function_space, shape, &tags[0], data1);
            out=Data(dt);
        } else if (type == 2) {
            // expanded data
            if ( ! (ndims == rank + 2) )
                throw DataException("load: illegal number of dimensions for expanded data in netCDF file.");
            if ( ! (num_samples_dim = dataFile.get_dim("num_samples") ) )
                throw DataException("load: unable to recover number of samples from netCDF file.");
            num_samples = num_samples_dim->size();
            if ( ! (num_data_points_per_sample_dim = dataFile.get_dim("num_data_points_per_sample") ) )
                throw DataException("load: unable to recover number of data points per sample from netCDF file.");
            num_data_points_per_sample=num_data_points_per_sample_dim->size();
            // check shape:
            if ( ! (num_samples == function_space.getNumSamples() && num_data_points_per_sample == function_space.getNumDataPointsPerSample()) )
                throw DataException("load: data sample layout of file does not match data layout of function space.");
            if (num_samples==0) {
                out = Data(0,shape,function_space,true);
            } else {
                // get ids
                if (! ( ids_var = dataFile.get_var("id")) )
                    throw DataException("load: unable to find reference ids in netCDF file.");
                const DataTypes::dim_t* ids_p=function_space.borrowSampleReferenceIDs();
                std::vector<DataTypes::dim_t> ids_of_nc(num_samples);
                if (! ids_var->get(&ids_of_nc[0], (long) num_samples) )
                    throw DataException("load: unable to recover ids from netCDF file.");
                // check order:
                int failed=-1, local_failed=-1, i;
#pragma omp parallel private(local_failed)
                {
                    local_failed=-1;
#pragma omp for private(i) schedule(static)
                    for (i=0; i < num_samples; ++i) {
                        if (ids_of_nc[i]!=ids_p[i]) local_failed=i;
                    }
#pragma omp critical
                    if (local_failed>=0) failed = local_failed;
                }
                // get the data:
                dims[rank]=num_data_points_per_sample;
                dims[rank+1]=num_samples;
                out=Data(0,shape,function_space,true);
                if (!(var = dataFile.get_var("data")))
                    throw DataException("load: unable to find data in netCDF file.");
                if (! var->get(&(out.getDataAtOffsetRW(out.getDataOffset(0,0), static_cast<DataTypes::real_t>(0))), dims))
                    throw DataException("load: unable to recover data from netCDF file.");
                if (failed >= 0) {
                    try {
                        std::cout << "Information - load: start reordering data from netCDF file " << fileName << std::endl;
                        out.borrowData()->reorderByReferenceIDs(&ids_of_nc[0]);
                    } catch (std::exception&) {
                        throw DataException("load: unable to reorder data in netCDF file.");
                    }
                }
            }
        } else {
            throw DataException("load: unknown escript data type in netCDF file.");
        }
    } catch (DataException& e) {
        error=1;
        msg=e.what();
    }
    int gerror = error;
    checkResult(error, gerror, mpiInfo);
    if (gerror > 0) {
        char* gmsg;
        shipString(msg.c_str(), &gmsg, mpiInfo->comm);
        throw DataException(gmsg);
    }
    return out;
#else
    throw DataException("load: not compiled with netCDF. Please contact your"
                        " installation manager.");
#endif // ESYS_HAVE_NETCDF
}

#endif

bool loadConfigured()
{
#ifdef ESYS_HAVE_NETCDF
    return true;
#else
    return false;
#endif
}

Data convertToData(const bp::object& value, const FunctionSpace& what)
{
    // first we try to extract a Data object from value
    bp::extract<Data> value_data(value);
    if (value_data.check()) {
        Data extracted_data=value_data();
        if (extracted_data.isEmpty()) {
            return extracted_data;
        } else {
            return Data(extracted_data,what);
        }
    } else {
        return Data(value,what,false);
    }
}

}  // end of namespace


/*
 * Copyright (c) 2017, <copyright holder> <email>
 * 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 the <organization> 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 <copyright holder> <email> ''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 <copyright holder> <email> 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.
 *
 */

#include "extractgeometryfeature.h"
#include <boost/graph/graph_concepts.hpp>

namespace geomeasurer {

keypoint::keypoint()
{

}
keypoint::keypoint(const int & index_, const double& score_)
{
index=index_;
score=score_;
}

bool keypoint::compare(const keypoint& kp1, const keypoint& kp2)
{
    if(kp1.score>kp2.score)
     {
       return true;
    }
    else{
      return false;
    }
}

extractGeometryFeature::extractGeometryFeature()
{
}

extractGeometryFeature::extractGeometryFeature(const PointCloud& pcd)
{
   candiate_pcd=pcd;
}

extractGeometryFeature::extractGeometryFeature(const sensor::rangeData& ranges_data)
{
  candiate_pcd=sensor::fromRangeData(ranges_data);
  ranges=ranges_data;
}

void extractGeometryFeature::setSlideWindowSizes(const int& slide_window_sizes_)
{
  slide_window_sizes=slide_window_sizes_;
}

void extractGeometryFeature::setRegionGrowRadius(const double& region_grow_radius_)
{
  region_grow_radius=region_grow_radius_;
}

void extractGeometryFeature::setClusterMinSize(const uint32_t& cluster_min_size_)
{
  cluster_min_size=cluster_min_size_;
}

void extractGeometryFeature::setMinAloneNum(const int& alone_)
{
  alone=alone_;
}

void extractGeometryFeature::setMinIncludedAngle(const double min_angle_)
{
  min_angle=min_angle_;
}

void extractGeometryFeature::setNeigh_a(const double& a_)
{
  a=a_;
}

void extractGeometryFeature::setNeigh_b(const double& b_)
{
  b=b_;
}

void extractGeometryFeature::setIsClustering(const bool& IsClustering_)
{
  IsClustering=IsClustering_;
}

void extractGeometryFeature::setMinNeighNum(const int& min_point_num_)
{
  min_point_num=min_point_num_;
}

void extractGeometryFeature::setNMSRadius(const double& NMS_radius_)
{
 NMS_radius=NMS_radius_;
}

void extractGeometryFeature::setGridSetorNum(const int gridSectorsNum_)
{
  gridSectorsNum=gridSectorsNum_;
}

void extractGeometryFeature::ChooseMethodofComputingOrientation(const bool& keypoints_based_method_)
{
  keypoints_based_method=keypoints_based_method_;
}

void extractGeometryFeature::setDesSectorNum(const int& dscpSectorsNum_)
{
  dscpSectorsNum=dscpSectorsNum_;
}

void extractGeometryFeature::setMatchDistinctRatio(const double distinct_ratio_)
{
  distinct_ratio=distinct_ratio_;
}

void extractGeometryFeature::setMatchDistThres(const double& match_dist_thres_)
{
  match_dist_thres=match_dist_thres_;
}

void extractGeometryFeature::setMatchMinScore(const double match_score_min_)
{
  match_score_min=match_score_min_;
}

std::vector< pcl::PointIndices > extractGeometryFeature::EuclideanClusterExtraction() const
{
  std::vector<pcl::PointIndices> ret;
  pcl::PointIndices cluster_indices;
  int indice=0;
  point3d bound_point;
  for(auto point:candiate_pcd.points)
  {

    if(cluster_indices.indices.empty())
    {
     bound_point=point;
     cluster_indices.indices.emplace_back(indice);
    }
    else
    {
      double dist=math::pointDistance(bound_point,point);
      DALKO_DEBUG("distance:"<<dist<<std::endl);
      bound_point=point;
          if(dist<region_grow_radius)
	  {
	    //std::cout<<"该类新增加一个点"<<std::endl;
	    cluster_indices.indices.emplace_back(indice);
	   }
	  else
	  {
	    DALKO_DEBUG("发现一个新的类,其尺寸为："<<cluster_indices.indices.size()<< std::endl);
	     ret.emplace_back(cluster_indices);
	     cluster_indices.indices.clear();
	     cluster_indices.indices.emplace_back(indice);
	  }
    }
    indice++;
  }
  ret.emplace_back(cluster_indices);
  return ret;
}

void extractGeometryFeature::setInputRanges(const sensor::rangeData& ranges_)
{
    candiate_pcd.clear();
    candiate_pcd=sensor::fromRangeData(ranges_);
   ranges=ranges_;
}

/**
 * @brief FAKLO
 */
featurePointSet extractGeometryFeature::extractFAKLO()
{
	featurePointSet ret_keypoints;
	falkolib::FALKOExtractor fe;
	fe.setMinScoreTh(0);
	fe.setMinExtractionRange(0.5);
	fe.setMaxExtractionRange(30);
	fe.enableSubbeam(false);
	fe.setNMSRadius(0.2);
	fe.setNeighB(0.07);
	fe.setBRatio(3);//2.5
	fe.setGridSectors(36);//16

	falkolib::LaserScan scan(ranges.angle_min,6*3.1419f/4, ranges.ranges.size());
	scan.fromRanges(ranges.ranges);
	std::vector<falkolib::FALKO> keypoints;
	fe.extract(scan, keypoints);
	for(falkolib::FALKO keypoint:keypoints)
	{
	  ret_keypoints.push_back(candiate_pcd.points[keypoint.index]);
	}
	return ret_keypoints;
}

void extractGeometryFeature::getNeiborhoodMember
(const int &numberofCenter2clusterboundary, const size_t& center_index, const double& radius, std::vector< point3d >& neigh, bool flag_Left)
{

     //根据邻域半径找到左右邻域内的点
     double growing_dl;
    int j=0;
     while(1)
     {
       if(flag_Left)
       {
       j++;
       }
       else
       {
	 j--;
      }
        if(std::abs(j)<=numberofCenter2clusterboundary)
       {
	 growing_dl=math::pointDistance(candiate_pcd.points[center_index],candiate_pcd.points[center_index-j]);
	 if(growing_dl<radius)
	 {
	    neigh.push_back(candiate_pcd.points[center_index-j]);
	 }
	 else{
	   break;
	}
       }
       else
       {
	 break;
      }
    }
}

std::tuple< double, double,double> extractGeometryFeature::jointEvaluate
(const point3d& center, const std::vector< point3d >& LeftNeigh, const std::vector< point3d >& rightNeigh)
{

    //size_t max_scale= std::max<size_t>(LeftNeigh.size(),rightNeigh.size());
  std::vector<double> response_Angle_ratios;
  std::vector<double> all_angle_ratios;
  double score=0;
  int count=0, Response_count=0;
  double response_area=0;

  for(auto left:LeftNeigh)
  {
    for(auto right:rightNeigh)
    {
      double area=computeAreaGivenEndpointCoordinates(center,left,right);
      double edg1,edg2, angleRatio;
    edg1=math::pointDistance(center,left);
    edg2=math::pointDistance(center,right);
    double area_min=getAreaThres(edg1,edg2);
    angleRatio=area/area_min;
    if(area>area_min)
    {
      Response_count++;
      response_area+=angleRatio;
      response_Angle_ratios.push_back(angleRatio);
    }
    score+=angleRatio;
    all_angle_ratios.push_back(angleRatio);
    count++;
    }
  }

  //数据分布均值
  double mean=score/double(count);
  //理论分布均值
  double response_mean=response_area/double(Response_count);

  double error=0, error1=0, error2=0;
  double sigma=0;

  for(auto angle_Ratio:response_Angle_ratios)
  {
    double residual=angle_Ratio-response_mean;
     error+=residual*residual;
  }
    double variance_response=error/double(Response_count-1);

    for(auto angle_Ratio:all_angle_ratios)
  {
     double residual=angle_Ratio-mean;
      error1+=residual*residual;
  }
    double variance_all=error1/double(count-1);

    for(auto angle_Ratio:all_angle_ratios)
  {
    double residual=angle_Ratio-response_mean;
     error2+=residual*residual;
  }
  double mutual_variance=error2/double(count-1);

  //计算相对熵
  double sigma_theroy=std::sqrt(variance_response);
  double sigma_data=std::sqrt(variance_all);
  double alpha_data=1/(double(std::sqrt(M_PI)*sigma_data));
  double alpha_theroy=alpha_data*(sigma_data/sigma_theroy);

  double D_P_Q=0;

  std::vector<double> P, Q;
  double sum_p=0,sum_q=0;
    for(auto angle_Ratio:all_angle_ratios)
  {
      //   std::cout<<"alpha_data:"<<alpha_data<<std::endl;
   //std::cout<<"alpha_theroy:"<<alpha_theroy<<std::endl;
    double residual=angle_Ratio-response_mean;
    double residual_data=angle_Ratio-mean;
   // std::cout<<"residual:"<<residual<<std::endl;
   //std::cout<<"residual_data:"<<residual_data<<std::endl;
    double qi=alpha_theroy*exp(-1/2.d*(residual*residual)/variance_response);
    double pi=alpha_data*exp(-1/2.d*(residual_data*residual_data)/variance_all);
   //std::cout<<"qi:"<<qi<<std::endl;
    sum_p+=pi;
    sum_q+=qi;
   //std::cout<<"pi:"<<qi<<std::endl;
    P.push_back(pi);
    Q.push_back(qi);
   //double d=(qi*log(qi/pi)+pi*log(pi/qi))/2;
   // D_P_Q+=d;
  }

  for(int i=0;i<P.size();i++)
  {
      D_P_Q+=(P[i]/sum_p*log(P[i]*sum_q/(sum_p*Q[i]))+Q[i]/sum_q*log(Q[i]*sum_p/(sum_q*P[i])))/2;
    // D_P_Q+=Q[i]/sum_q*log(Q[i]*sum_p/(sum_q*P[i]));
    // D_P_Q+= P[i]/sum_p*log(P[i]*sum_q/(sum_p*Q[i]));
  }

  // D_P_Q=sum_q;
  return std::make_tuple(response_mean,mutual_variance,D_P_Q);

}

featurePointSet extractGeometryFeature::extractCornerWithimprovedFAKLO(const ClusterIndices& cluster)
{
  featurePointSet ret;
  keypoints.clear();
    std::vector<point3d> NeighL;
    std::vector<point3d> NeighR;
  for(int i=0;i<cluster.indices.size();i++)
  {
    point3d point_i;

    point_i=candiate_pcd.points[cluster.indices[i]];
    if(i<min_point_num)
    {
      continue;
    }

    //计算左右邻域
    double Neigh_Radius=getNeiborhood(ranges.ranges[cluster.indices[i]]);

    ratio_invariance=adaptiveResponsefactor(Neigh_Radius);

    DALKO_DEBUG("Neigh_R:"<<Neigh_Radius<<std::endl);

    double numofcenter2clusterboundary;
    numofcenter2clusterboundary=i;
    getNeiborhoodMember(numofcenter2clusterboundary, cluster.indices[i],Neigh_Radius, NeighL,true);
    if(NeighL.size()>=min_point_num)
    {
      numofcenter2clusterboundary=cluster.indices.size()-i-1;
      getNeiborhoodMember(numofcenter2clusterboundary,cluster.indices[i],Neigh_Radius, NeighR,false);
    }
    else{
      NeighL.clear();
      continue;
    }
   if(NeighR.size()<min_point_num)
    {
     NeighL.clear();
     NeighR.clear();
    continue;
    }

    //根据面积决定是否被选为特征点候选点
    point3d x_L=candiate_pcd.points[cluster.indices[i-NeighL.size()]];
    point3d x_R=candiate_pcd.points[cluster.indices[i+NeighR.size()]];

   //double area=computeAreaGivenEndpointCoordinates(point_i,x_L,x_R);
   // double edg1=math::pointDistance(point_i, x_L);
   // double edg2=math::pointDistance(point_i, x_R);

    //****************************************
    //直接使用面积作为响应
   //****************************************
    /*
    area_min_size=getAreaThres(edg1,edg2);
    std::cout<<"area_min_size: "<<area_min_size<<std::endl;
   if(area>area_min_size)*/

    //*********************
     //下面为FAKLO使用的方法
    //*********************
  /*
    //底边长度
    double BaseEdge=std::abs(math::pointDistance(x_L,x_R));
    //高
    double High_length=area/BaseEdge;
     std::cout<<"底边:"<<BaseEdge<<std::endl;
     std::cout<<"高:"<<High_length<<std::endl;
     std::cout<<"Thres:"<<ri/b_ratio<<std::endl;
     //if(deterBaseEdge>=(ri/b_ratio)&&deterHigh>=(ri/b_ratio))*/

     std::pair<double,double> InvarianceAndRightAngleScore;

    InvarianceAndRightAngleScore=EvaulateInvarianceNeigh(point_i,NeighL,NeighR);
   if(InvarianceAndRightAngleScore.first>ratio_invariance)
    {
      //计算栅格分值
         double ScoreL=0, ScoreR=0,score=0,extra_reward=0;
	ScoreL=EvaulateNeighDivergence(point_i,NeighL);
	// std::cout<<ScoreL<<std::endl;
	ScoreR=EvaulateNeighDivergence(point_i,NeighR);
	double DivergenceScore=ScoreL+ScoreR;
	// double AngleInvarianceScore=EvaulateAngleQuality(point_i,NeighL,NeighR);
	//score=InvarianceAndRightAngleScore.second;
	//score=AngleInvarianceScore;
	//score=gainClosingRightAngle*RightAnglescore;
	//score=DivergenceScore;
	//score=AngleInvarianceScore+gainClosingRightAngle*RightAnglescore+DivergenceScore;
	double RightAnglescore,Variance;
	double responseScore;
      std::tie(RightAnglescore,Variance,responseScore) =jointEvaluate(point_i,NeighL,NeighR);
      //score=1000+0.8*RightAnglescoreAndVariance.first-0*RightAnglescoreAndVariance.second;
      //score=responseScore*RightAnglescore/std::sqrt(Variance);
       double expect=1/sin(min_angle/180.d*M_PI);
      // score=1000-responseScore;
      score=exp((expect-RightAnglescore)*(expect-RightAnglescore)*Variance);
      DALKO_DEBUG("score:"<<score<<std::endl);
      keypoints.emplace_back(keypoint(cluster.indices[i], score));
    }
   NeighL.clear();
   NeighR.clear();
  }

  //如果不判断，当为空的情况下，迭代器就不会生效，这是对迭代器进行取值就会出错。
    if(!keypoints.empty())
    {
     auto kp_with_maxvalue=std::max_element(keypoints.begin(),keypoints.end(),keypoint::compare);
     keypoint temkp=*kp_with_maxvalue;
    double max_score=temkp.score;

	for(auto iter=keypoints.begin();iter!=keypoints.end();++iter)
	{
	  double tem=(*iter).score;
	  (*iter).score=tem;
	}
    }

  //孤立点抑制
 alonePointSupress();
 //非极大值抑制
 return NonMaxSupress();
 return fromKeypoints();

}

//TODO
featurePointSet extractGeometryFeature::extractFLIRT()
{
// PeakFinder * pf;
// RangeDetector range_detector(pf);
// std::vector<double> phl, rho;
// for(int i=0;i<ranges.ranges.size();i++)
// {
//   phl.emplace_back(ranges.angle_min+i*ranges.angle_increment);
//   rho.emplace_back(ranges.ranges[i]);
// }
// LaserReading Laser_reading(phl,rho);
// std::vector<InterestPoint*> kps;
// range_detector.detect(Laser_reading,kps);
}

/*
discriptors extractGeometryFeature::getGCdiscriptor(const point3d &center)
{
  getNeiborhoodMember();

}*/

//主要提取特征的主函数

featurePointSet extractGeometryFeature::extractgfs(std::string T)
{

   KeypointsVclear();
  featurePointSet ret;
   if(T== "FAKLO")
     {
         ret=extractFAKLO();
	 return ret;
     }
  //首先对点云进行聚类
  //不能使用大于阈值的特征点响应方法，要使用NMS的方法，每一个点都有一个响应值，在一个区域里面挑出一个最大的
  //对于二维首先统计出稳定的线点，然后对这点使用区域生成法恢复直线，然后对所有点进行聚类
  //由于初始的种子点可能在一个类中，所以，当种子被聚类到某一类中时，这个种子就会被删除点
  //然后再在每一个类中找到角点
  DALKO_DEBUG("点云尺寸："<<candiate_pcd.size()<<std::endl);
  assert(!candiate_pcd.empty());
  std::vector<pcl::PointIndices> clustersWithindices;
  if(IsClustering)
  {
   clustersWithindices=EuclideanClusterExtraction();
  }
  else
  {
    pcl::PointIndices tempCluster;
    for(int i=0;i<ranges.ranges.size();i++)
    {
      tempCluster.indices.emplace_back(i);
    }
    clustersWithindices.emplace_back(tempCluster);
  }

  DALKO_DEBUG("聚类个数："<<clustersWithindices.size()<<std::endl);

  for(pcl::PointIndices cluster: clustersWithindices)
  {
    if( T=="IFAKLO")
   {
         if(cluster.indices.size()>cluster_min_size)
	 {
           ret+=extractCornerWithimprovedFAKLO(cluster);
	 }
   }
    else if( T=="AT")
    {
        ret+=extractCornerWithAreaTsensor(cluster);
    }
    else
    {
       std::cout<<"please set feature type you would like to chose from FAKLO, IFAKLO, AT"<<std::endl;
    }
 }
    return ret;
}

featurePointSet extractGeometryFeature::extractCornerWithAreaTsensor(const ClusterIndices &cluster)
{
    featurePointSet ret;
    keypoints.clear();
  /*
   * 首先两个端点，用它构建面积张量, 在里面滑动我们的窗口，窗口的点选为端点。求出这个窗口内，在当前窗口形态下的响应值
   * 这样应该就很好检测直线特征点
   * 相对于最小least square的好处在于不用计算法向量，法向量十分容易受到噪声干扰
   */
  std::vector<std::pair<int,double> > responses;

  bool line_flag=false;
  int step=1;
  double rep,rep1,rep2;
  Eigen::Matrix3d S;
  S.col(2)<<1,1,1;
  if(cluster.indices.size()>=cluster_min_size)
  {
    for(int n=0;n<cluster.indices.size()-slide_window_sizes-1;   n+=step)
    {
     std::cout<<"点在类中位置："<<n<<std::endl;
      if(line_flag==false)
      {
	 std::vector<point3d> Neigh;
        double growingR=math::pointDistance(candiate_pcd.points[cluster.indices[n]],candiate_pcd.points[cluster.indices[n+1]]);
	std::cout<<"growingR:"<<growingR<<std::endl;
	       int j=1;
	       ri=getNeiborhood(ranges.ranges[cluster.indices[n]]);
	   while(growingR<=ri&&(n+j)<cluster.indices.size())
	  {
	      Neigh.push_back(candiate_pcd.points[cluster.indices[n+j]]);
	      growingR=math::pointDistance(candiate_pcd.points[cluster.indices[n]],candiate_pcd.points[cluster.indices[n+j]]);
	      j++;
	   }
         slide_window_sizes=Neigh.size();
	 std::cout<<"slide_window_sizes:" <<slide_window_sizes<<std::endl;
	 Neigh.clear();
	 if(slide_window_sizes<min_point_num)
	 {
	   step=1;
	   continue;
	}
	  S(0,0)=candiate_pcd.points[cluster.indices[n]].x;
	  S(0,1)=candiate_pcd.points[cluster.indices[n]].y;
	  S(1,0)=candiate_pcd.points[cluster.indices[n+slide_window_sizes]].x;
	  S(1,1)=candiate_pcd.points[cluster.indices[n+slide_window_sizes]].y;
	  for(int j=0;j<slide_window_sizes;j++)
	  {
	    S(2,0)=candiate_pcd.points[cluster.indices[n+j]].x;
	    S(2,1)=candiate_pcd.points[cluster.indices[n+j]].y;
	      rep=std::abs(S.determinant());
              std::cout<<"case0 当前面积："<<rep<<std::endl;
	      if(rep>area_min_size)
		{
		  std::cout<<"case1 当前面积："<<rep<<std::endl;
		  line_flag=false;
		  step=1;
		 break;
		}
		else
		{
		  line_flag=true;
		  step++;
		}
	  } //for(int j=0;j<slide_window_sizes;j++)
      }
      else
      {
        S(2,0)=candiate_pcd.points[cluster.indices[n]].x;
        S(2,1)=candiate_pcd.points[cluster.indices[n]].y;

	rep=std::abs(S.determinant());

	if(rep>area_min_size)
	    {
	    std::cout<<"case2 当前面积："<<rep<<std::endl;
	    line_flag=false;
	    S(2,0)=candiate_pcd.points[cluster.indices[n+1]].x;
	    S(2,1)=candiate_pcd.points[cluster.indices[n+1]].y;
	   rep1=std::abs(S.determinant());
	    S(2,0)=candiate_pcd.points[cluster.indices[n+2]].x;
	    S(2,1)=candiate_pcd.points[cluster.indices[n+2]].y;
	     rep2=std::abs(S.determinant());
	   if(rep1>area_min_size&&rep2>area_min_size)
	      keypoints.emplace_back(keypoint(cluster.indices[n-1],rep));
	     break;
	    }
	    else
	    {
	      step=1;
	     }
      }
    }//for
  }//if
  //return fromKeypoints();
  return ret=NonMaxSupress();
}

featurePointSet extractGeometryFeature::NonMaxSupress()
{

  //仍然有待改进，也最好是先计算索引窗口范围，在这个索引窗口内直接抑制。同时，对于打分十分接近的，直接选用打分接近的结构上在中间的点，就不要在抑制了
  //先找出最大值，然后找到在该点抑制半径范围内的点，将这些点去除
  //直到容器为空
  KeyPoints nmsed_keypoints;
    featurePointSet ret;
  if(keypoints.empty())
  {
        return ret;
  }

   /*
   std::cout<<"检测到特征点个数"<<keypoints.size()<<std::endl;
   for(auto kp:keypoints)
   {
     std::cout<<"index:"<<kp.index<<" "<<"score:"<<kp.score<<std::endl;
  }
  */
   double max_score=0;
   bool first=true;
  while(!keypoints.empty())
  {
     KeyPoints tem_keypoints;
     auto kp_with_maxvalue=std::max_element(keypoints.begin(),keypoints.end(),keypoint::compare);
     keypoint temkp=*kp_with_maxvalue;
     if(first)
     {
       max_score=temkp.score;
    }
    nmsed_keypoints.emplace_back(temkp);
    keypoints.erase(kp_with_maxvalue);
    if(keypoints.empty())
    {
      break;
    }
    //注意，当中间元素被删除之后，容器后面的迭代器就会失效，所以不能直接在以下程序中使用keypoints.erase(it)。
    //为防止这种情况发生，加入一个临时容器
    for(KeyPoints::iterator it=keypoints.begin();it!=keypoints.end();++it)
    {
      double dist=math::pointDistance(candiate_pcd.points[temkp.index],candiate_pcd.points[(*it).index]);

      DALKO_DEBUG("dist:"<<dist<<std::endl);
        if(dist>NMS_radius)
      {
	tem_keypoints.emplace_back(*it);
      }
    }
   keypoints.clear();
   if(!tem_keypoints.empty())
   {
    keypoints.assign(tem_keypoints.begin(),tem_keypoints.end());
   }
   else
   {
     break;
  }
  }
  keypoints.clear();
  keypoints.assign(nmsed_keypoints.begin(),nmsed_keypoints.end());

  for(keypoint kp:keypoints)
  {
    if(kp.score>=minValPercent*max_score)
    {
      ret.push_back(candiate_pcd.points[kp.index]);
      Allkeypoints.push_back(kp);
    }
  }
  //keypoints.clear();
  DALKO_DEBUG("NMS后特征点个数:"<<ret.size()<<std::endl);
  return ret;
 }

featurePointSet extractGeometryFeature::alonePointSupress()
{
  KeyPoints tem_keypoints;
  for(auto kpi:keypoints)
  {
    int count=0;
    for(auto kpj:keypoints)
    {
      double dist=math::pointDistance(candiate_pcd.points[kpi.index],candiate_pcd.points[kpj.index]);
      if(dist<=alone_radius)
      {
	count++;
      }
      if(count>=alone)
      {
	tem_keypoints.emplace_back(kpi);
	//这个break很重要，不然太消耗时间
	break;
      }
    }
  }
  keypoints.clear();
  keypoints.assign(tem_keypoints.begin(),tem_keypoints.end());
  return fromKeypoints();
}

 //把特征点用点云输出
featurePointSet extractGeometryFeature::fromKeypoints()
{
  featurePointSet kp_PCD;
     for(auto kp:keypoints)
     {
       kp_PCD.push_back(candiate_pcd.points[kp.index]);
    }
    return kp_PCD;
}

featurePointSet extractGeometryFeature::fromAllkeypoints()
{
    keypointPcd.clear();
     for(auto kp:Allkeypoints)
     {
      keypointPcd.push_back(candiate_pcd.points[kp.index]);
    }
    return keypointPcd;
}

float extractGeometryFeature::getNeiborhood(const double& range)
{
  ri=a*exp(b*range);
  return ri;
}

double extractGeometryFeature::getAreaThres()
{
  return 1/2.d*ri*ri*sin(min_angle/180.d*M_PI);
}

double extractGeometryFeature::getAreaThres(double edge1, double edge2)
{
  return 1/2.d*edge1*edge2*sin(min_angle/180.d*M_PI);
}

double extractGeometryFeature::extra_reward_from(const double& area,const double& area_min)
{
   return gainClosingRightAngle*area/area_min;
}

std::vector< int > extractGeometryFeature::computeNeighPlorDistribution(const point3d& center, const std::vector< point3d >& singleSideNeigh)
{
 std::vector<int> V_sector;
      double scoreSingleSide=0;
       int count=0;
      for(auto point:singleSideNeigh)
      {
	int SecNum=std::floor(gridSectorsNum/(2*M_PI)*std::atan2(point.y-center.y, point.x-center.x));
	if(SecNum<0)
	{
	  SecNum=gridSectorsNum+SecNum;
	}
	V_sector.push_back(SecNum);
      }
      return V_sector;
}

double extractGeometryFeature::EvaulateAngleQuality(const point3d& center, const std::vector< point3d >& LeftNeigh, const std::vector< point3d >& rightNeigh)
{
  std::vector<int> V_left,V_Right, V_dist;
  V_left=computeNeighPlorDistribution(center,LeftNeigh);
  V_Right=computeNeighPlorDistribution(center,rightNeigh);
  int sum_dist=0,count=0,dist=0;
  for(auto left:V_left)
  {
    for(auto right:V_Right)
    {
     count++;
     dist= std::abs((abs(left-right)+gridSectorsNum/2)%gridSectorsNum-gridSectorsNum/2);
     sum_dist+=dist;
     V_dist.emplace_back(dist);
    }
  }
  double mean_dist=sum_dist/double(count);
  double error=0,residual=0;
 for(auto dist:V_dist)
 {
   residual=dist-mean_dist;
   error+=residual*residual;
}
double variance=error/double(count-1);
return variance;
}

double extractGeometryFeature::EvaulateNeighDivergence(const point3d &center, const  std::vector< point3d > &singleSideNeigh)
{
      std::vector<int> V_sector;
      double scoreSingleSide=0;
       int count=0;
      for(auto point:singleSideNeigh)
      {
	int SecNum=std::floor(gridSectorsNum/(2*M_PI)*std::atan2(point.y-center.y, point.x-center.x));
	if(SecNum<0)
	{
	  SecNum=gridSectorsNum+SecNum;
	}
	if(V_sector.size()>0)
	{
	    for(auto Sec_i:V_sector)
	    {
	      count++;
	      scoreSingleSide+=std::abs((abs(SecNum-Sec_i)+gridSectorsNum/2)%gridSectorsNum-gridSectorsNum/2);
	    }
	  }
	V_sector.push_back(SecNum);
      }
     //scoreSingleSide=scoreSingleSide/count;
     V_sector.clear();
      return scoreSingleSide;
}

std::pair<double,double> extractGeometryFeature::EvaulateInvarianceNeigh(const point3d& center, const std::vector< point3d >& LeftNeigh, const std::vector< point3d >& rightNeigh)
{
  /*
  size_t max_scale= std::max<size_t>(LeftNeigh.size(),rightNeigh.size());
  std::vector<double> sin_Angle_ratios;
  double score=0;
  int Response_count=0;
  for(int i=0;i<max_scale;i++)
  {
    double area=computeAreaGivenEndpointCoordinates(center,LeftNeigh[i],rightNeigh[i]);
    double edg1,edg2, angleRatio;
    edg1=math::pointDistance(center,LeftNeigh[i]);
    edg2=math::pointDistance(center,rightNeigh[i]);
    double area_min=getAreaThres(edg1,edg2);
    angleRatio=area/area_min;
    score+=angleRatio;
    if(area>area_min) Response_count++;
    }
    return std::make_pair(Response_count/double(max_scale),score/double(max_scale));
    */

  double score=0;
  int count=0, Response_count=0;
  double response_area=0;
  for(auto left:LeftNeigh)
  {
    for(auto right:rightNeigh)
    {
      double area=computeAreaGivenEndpointCoordinates(center,left,right);
      double edg1,edg2, angleRatio;
    edg1=math::pointDistance(center,left);
    edg2=math::pointDistance(center,right);
    double area_min=getAreaThres(edg1,edg2);
    angleRatio=area/area_min;
    if(area>area_min)
    {
      Response_count++;
    }
    count++;
    }
  }
 return std::make_pair(Response_count/double(count),score/double(count));

}

double extractGeometryFeature::computeAreaGivenEndpointCoordinates(const point3d& A1, const point3d& A2, const point3d& A3)
{
    Eigen::Matrix3f S;
    S.col(2)<<1,1,1;
    S(0,0)=A1.x;    S(0,1)=A1.y;
    S(1,0)=A2.x;    S(1,1)=A2.y;
    S(2,0)=A3.x;    S(2,1)=A3.y;
    //面积
    double area=1/2.d*std::abs(S.determinant());
    return area;
}

double extractGeometryFeature::adaptiveResponsefactor(const double & ri)
{
   return double(0.5-5*(ri-0.2)*(ri-2));
}

Discriptors extractGeometryFeature::getGCdiscriptor()
{

    fromAllkeypoints();
    GCdiscriptors.resize(dscpSectorsNum,Allkeypoints.size());
    DALKO_DEBUG(GCdiscriptors.rows()<<std::endl);
    DALKO_DEBUG(GCdiscriptors.cols()<<std::endl);
//     std::cout<<GCdiscriptors.rows()<<std::endl;
//     std::cout<<GCdiscriptors.cols()<<std::endl;
    GCdiscriptors.setZero();
         int j=0,i=0;
    for(keypoint kp:Allkeypoints)
    {
    point3d current_point=candiate_pcd.points[kp.index];

    int Cornerorientation;
    if(!keypoints_based_method)
    { Cornerorientation=getCornerOrientationfromNeigh(kp.index);}
    else {Cornerorientation=getCornerOrientationBasedonKeypoints(kp.index);}

    //计算特征点在栅格内的分布
    for(keypoint kpj:Allkeypoints)
      {
      if(kpj.index==kp.index)
      {
	continue;
      }
      point3d kppoint=candiate_pcd.points[kpj.index];
       int kp_sector_num;
       kp_sector_num=floor(dscpSectorsNum/(2*M_PI)*atan2(kppoint.y-current_point.y,kppoint.x-current_point.x));
       if(kp_sector_num<0)
       {
	 kp_sector_num+=dscpSectorsNum;
      }
      int diff=kp_sector_num-Cornerorientation;
      if(diff<0)
      {
	kp_sector_num=dscpSectorsNum+diff;
      }
      else{
	kp_sector_num=diff;
      }
       double dist=math::pointDistance(kppoint,current_point);
       if(GCdiscriptors(kp_sector_num,i)==0)
       GCdiscriptors(kp_sector_num,i)=log(dist);
       else if (GCdiscriptors(kp_sector_num,i)>log(dist))
       {
	    GCdiscriptors(kp_sector_num,i)=log(dist);
       }
    }
     i++;
    }
    return GCdiscriptors;
}

int extractGeometryFeature::getCornerOrientationfromNeigh(const int& index)
{
    double r=getNeiborhood(ranges.ranges[index]);
      std::vector<point3d> NeighL,NeighR;
      point3d current_point=candiate_pcd.points[index];
      getNeiborhoodMember(index,index,r,NeighL,true);
      getNeiborhoodMember(ranges.ranges.size()-index-1,index,r,NeighR, false);

      point3d centriod, centriod_left, centriod_right;
      centriod.x=0;centriod.y=0;centriod.z=0;
      centriod_left.x=0;centriod_left.y=0;centriod_left.z=0;
      centriod_right.x=0;centriod_right.y=0;centriod_right.z=0;

      //计算左右边各自的栅格分布
      double left_edge=0, right_edge=0;
      for(point3d temp:NeighL)
      {
	centriod_left.x+=temp.x;
	centriod_left.y+=temp.y;
	centriod_left.z+=temp.z;
      }
      centriod_left.x/=NeighL.size();
      centriod_left.y/=NeighL.size();
      int Orientation_l=floor(dscpSectorsNum/(2*M_PI)*atan2(centriod_left.y-current_point.y,centriod_left.x-current_point.x));
      if(Orientation_l<0)
      {
	Orientation_l+=dscpSectorsNum;
      }

      for(point3d temp:NeighR)
      {
	centriod_right.x+=temp.x;
	centriod_right.y+=temp.y;
	centriod_right.z+=temp.z;
      }
      centriod_right.x/=NeighR.size();
      centriod_right.y/=NeighR.size();
      int Orientation_r=floor(dscpSectorsNum/(2*M_PI)*atan2(centriod_right.y-current_point.y,centriod_right.x-current_point.x));
      if(Orientation_r<0)
      {
	Orientation_r+=dscpSectorsNum;
      }

      int Cornerorientation=1/2*(Orientation_l+Orientation_r);
      if ((distanceofSectors(Orientation_l,Cornerorientation)+distanceofSectors(Orientation_r,Cornerorientation))>(dscpSectorsNum/2.d))
      {
         Cornerorientation=(Cornerorientation+dscpSectorsNum/2)%dscpSectorsNum;
      }
      return Cornerorientation;
}

int extractGeometryFeature::getCornerOrientationBasedonKeypoints(const int & index)
{
      int left_endpoint_index,right_endpoint_index;
      point3d current_point=candiate_pcd.points[index];
      double dist_min, dist_second;
      bool first=true;
     if(!isCreatedKdtree)
     {
      kdtree_keypoints.setInputCloud(keypointPcd.makeShared());
      isCreatedKdtree=true;
     }

       size_t K=3;
       std:: vector<int> pointIdxNKNSearch(K);//用来存放搜索到的点的index
       std::vector<float> pointNKNSquaredDistance(K);//存放搜索到的点到当前的欧式距离
       kdtree_keypoints.nearestKSearch(current_point,K,pointIdxNKNSearch,pointNKNSquaredDistance);
       left_endpoint_index=pointIdxNKNSearch[1];
       right_endpoint_index=pointIdxNKNSearch[2];
      point3d left_endpoint=keypointPcd.points[left_endpoint_index];
      point3d right_endpoint=keypointPcd.points[right_endpoint_index];
      point3d centriod;
      centriod.x=(left_endpoint.x+right_endpoint.x+current_point.x)/3;
      centriod.y=(left_endpoint.y+right_endpoint.y+current_point.y)/3;
     int Cornerorientation=floor(dscpSectorsNum/(2*M_PI)*atan2(centriod.y-current_point.y,centriod.x-current_point.x));
      if (Cornerorientation<0)
      {
         Cornerorientation+=dscpSectorsNum;
      }
      return Cornerorientation;
}

int extractGeometryFeature::distanceofSectors(const int& index1, const int& index2)
{
    int fyb=abs(int(abs(index1-index2)+dscpSectorsNum/2.f)%dscpSectorsNum-dscpSectorsNum/2);
    return fyb;
}

std::vector<std::tuple< int, int, double > > extractGeometryFeature::match(const KeyPoints& kps, const Discriptors& GCS)
{
     int i=0;
     std::vector<std::tuple<int,int,double>> pairs;
    assert(GCS.cols()>0&&GCS.rows()>0);
    for(keypoint kpi:Allkeypoints)
    {double max_score=0, second_score=0;
      int max_index;
     int  j=0;
      for(keypoint kpj:kps)
      {
	double score=0;
	for(int k=0;k<GCS.rows();k++)
	{
	  if(GCS(k,j)>0&&GCdiscriptors(k,i)>0)
	  {
	  double dist=abs(GCS(k,j)-GCdiscriptors(k,i));
	  if(abs(dist)<=match_dist_thres)
	  {
	    score+=20.d;
	  }
	  }
	}
	 if(score>=max_score)
	 {
	second_score=max_score;
	max_score=score;
	max_index=kpj.index;
	 }
	j++;
      }
      if((max_score/second_score)>=distinct_ratio&&(max_score>=match_score_min))
      {
	pairs.push_back(std::make_tuple(kpi.index,max_index,max_score));
      }
      i++;
    }
    return pairs;
}

/**
 * 特征点，还有有一个特性也很重要，就是明显区别于其他点。这样可以减少在匹配中产生模糊性，可以想象，如果没有
 * 使用面积张量来检测特征点
 */
featurePointSet extractGeometryFeature::extractfromTensorField(const ClusterIndices & cluster)
{
    featurePointSet ret;
    keypoints.clear();
  /*
   * 首先两个端点，用它构建面积张量, 在里面滑动我们的窗口，窗口的点选为端点。求出这个窗口内，在当前窗口形态下的响应值
   * 这样应该就很好检测直线特征点
   * 相对于最小least square的好处在于不用计算法向量，法向量十分容易受到噪声干扰
   */
  std::vector<std::pair<int,double> > responses;

  if(cluster.indices.size()>=cluster_min_size)
  {
    for(int n=0;n<cluster.indices.size()-slide_window_sizes;n++)
    {
      Eigen::Matrix3d S;
      S.col(2)<<1,1,1;
      S(0,0)=candiate_pcd.points[cluster.indices[n]].x;
      S(0,1)=candiate_pcd.points[cluster.indices[n]].y;
      S(1,0)=candiate_pcd.points[cluster.indices[n+slide_window_sizes]].x;
      S(1,1)=candiate_pcd.points[cluster.indices[n+slide_window_sizes]].y;

      for(int j=0;j<slide_window_sizes;j++)
      {
	S(2,0)=candiate_pcd.points[n+j].x;
        S(2,1)=candiate_pcd.points[n+j].y;
	  double rep=std::abs(S.determinant());
	  if(rep>area_min_size)
	    {
	      responses.push_back(std::make_pair(cluster.indices[n+j],rep));
	    }
	    else
	    {
	      responses.push_back(std::make_pair(cluster.indices[n+j],0.d));
	    }
      }
    }
  }
  //创建一个积分图像
  std::vector<double> integral_response;
  double rep_integ=0;
  for(auto response:responses)
  {
    rep_integ+=response.second;
    integral_response.emplace_back(rep_integ);
    std::cout<<response.second<<" ";
  }
   std::cout<<std::endl;

   for(auto response:responses)
   {
     keypoints.emplace_back(keypoint(response.first, response.second));
  }

   //局部最大值
   for(int j=0;j<responses.size();j++)
   {
     if(j>1&&j<responses.size()-2)
     {
       if((responses[j].second>responses[j-1].second&&responses[j].second>responses[j+1].second)
	 ||(responses[j].second<responses[j-1].second&&responses[j].second<responses[j+1].second))
       {
	double variance=3*responses[j].second-(integral_response[j+1]-integral_response[j-1]);
	std::cout<<variance<<std::endl;
	keypoints.emplace_back(keypoint(responses[j].first, responses[j].second));
       }
    }
  }
  //return ret=fromKeypoints();
  return ret=NonMaxSupress();
}

KeyPoints extractGeometryFeature::getKeypoints()
{
   return Allkeypoints;
}

point3d extractGeometryFeature::GetPoint3dfromIndex(const int & pointIndex)
{
  return candiate_pcd.points[pointIndex];
}

void extractGeometryFeature::KeypointsVclear()
{
  Allkeypoints.clear();
  keypointPcd.clear();
  keypoints.clear();
}

}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program 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.
//
// This program 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 this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_MOTIFS_HH
#define GRAPH_MOTIFS_HH

#include <boost/graph/isomorphism.hpp>
#include <unordered_set>
#include <boost/functional/hash.hpp>
#include <algorithm>
#include <vector>

#include "random.hh"

namespace graph_tool
{
using namespace boost;

template <class Value>
void insert_sorted(std::vector<Value>& v, const Value& val)
{
    typeof(v.begin()) iter = lower_bound(v.begin(), v.end(), val);
    if (iter != v.end() && *iter == val)
        return; // no repetitions
    v.insert(iter, val);
}

template <class Value>
bool has_val(std::vector<Value>& v, const Value& val)
{
    typeof(v.begin()) iter = lower_bound(v.begin(), v.end(), val);
    if (iter == v.end())
        return false;
    return *iter == val;
}

// gets all the subgraphs starting from vertex v and store it in subgraphs.
template <class Graph, class Sampler>
void get_subgraphs(Graph& g, typename graph_traits<Graph>::vertex_descriptor v,
                   size_t n,
                   std::vector<std::vector<typename graph_traits<Graph>::vertex_descriptor> >& subgraphs,
                   Sampler sampler)
{
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

    // extension and subgraph stack
    std::vector<std::vector<vertex_t> > ext_stack(1);
    std::vector<std::vector<vertex_t> > sub_stack(1);
    std::vector<std::vector<vertex_t> > sub_neighbours_stack(1);

    sub_stack[0].push_back(v);
    typename graph_traits<Graph>::out_edge_iterator e, e_end;
    for (tie(e, e_end) = out_edges(v, g); e != e_end; ++e)
    {
        typename graph_traits<Graph>::vertex_descriptor u = target(*e, g);
        if (u > v && !has_val(ext_stack[0], u))
        {
            insert_sorted(ext_stack[0], u);
            insert_sorted(sub_neighbours_stack[0],u);
        }
    }

    while (!sub_stack.empty())
    {
        std::vector<vertex_t>& ext = ext_stack.back();
        std::vector<vertex_t>& sub = sub_stack.back();
        std::vector<vertex_t>& sub_neighbours = sub_neighbours_stack.back();

        if (sub.size() == n)
        {
            // found a subgraph of the desired size; put it in the list and go
            // back a level
            subgraphs.push_back(sub);
            sub_stack.pop_back();
            ext_stack.pop_back();
            sub_neighbours_stack.pop_back();
            continue;
        }

        if (ext.empty())
        {
            // no where else to go
            ext_stack.pop_back();
            sub_stack.pop_back();
            sub_neighbours_stack.pop_back();
            continue;
        }
        else
        {
            // extend subgraph
            std::vector<vertex_t> new_ext, new_sub = sub,
                new_sub_neighbours = sub_neighbours;

            // remove w from ext
            vertex_t w = ext.back();
            ext.pop_back();

            // insert w in subgraph
            insert_sorted(new_sub, w);

            // update new_ext
            new_ext = ext;
            for (tie(e, e_end) = out_edges(w, g); e != e_end; ++e)
            {
                vertex_t u = target(*e,g);
                if (u > v)
                {
                    if (!has_val(sub_neighbours, u))
                        insert_sorted(new_ext, u);
                    insert_sorted(new_sub_neighbours, u);
                }
            }

            sampler(new_ext, ext_stack.size());

            ext_stack.push_back(new_ext);
            sub_stack.push_back(new_sub);
            sub_neighbours_stack.push_back(new_sub_neighbours);
        }
    }
}

// sampling selectors

struct sample_all
{
    template <class val_type>
    void operator()(std::vector<val_type>&, size_t) {}
};

struct sample_some
{
    sample_some(std::vector<double>& p, rng_t& rng): _p(&p), _rng(&rng) {}
    sample_some() {}

    template <class val_type>
    void operator()(std::vector<val_type>& extend, size_t d)
    {
        typedef std::uniform_real_distribution<double> rdist_t;
        auto random = std::bind(rdist_t(), std::ref(*_rng));

        double pd = (*_p)[d+1];
        size_t nc = extend.size();
        double u = nc*pd - floor(nc*pd);
        size_t n;
        double r;
        {
            #pragma omp critical
            r = random();
        }
        if (r < u)
            n = size_t(ceil(nc*pd));
        else
            n = size_t(floor(nc*pd));

        if (n == extend.size())
            return;
        if (n == 0)
        {
            extend.clear();
            return;
        }

        typedef std::uniform_int_distribution<size_t> idist_t;
        for (size_t i = 0; i < n; ++i)
        {
            auto random_v = std::bind(idist_t(0, extend.size()-i-1),
                                      std::ref(*_rng));
            size_t j;
            {
                #pragma omp critical
                j = i + random_v();
            }
            swap(extend[i], extend[j]);
        }
        extend.resize(n);
    }

    std::vector<double>* _p;
    rng_t* _rng;
};

// build the actual induced subgraph from the vertex list
template <class Graph, class GraphSG>
void make_subgraph
    (std::vector<typename graph_traits<Graph>::vertex_descriptor>& vlist,
     Graph& g, GraphSG& sub)
{
    for (size_t i = 0; i < vlist.size(); ++i)
        add_vertex(sub);
    for (size_t i = 0; i < vlist.size(); ++i)
    {
        typename graph_traits<Graph>::vertex_descriptor ov = vlist[i], ot;
        typename graph_traits<GraphSG>::vertex_descriptor nv = vertex(i,sub);
        typename graph_traits<Graph>::out_edge_iterator e, e_end;
        for (tie(e, e_end) = out_edges(ov, g); e != e_end; ++e)
        {
            ot = target(*e, g);
            typeof(vlist.begin()) viter =
                lower_bound(vlist.begin(), vlist.end(), ot);
            size_t ot_index = viter - vlist.begin();
            if (viter != vlist.end() && vlist[ot_index] == ot &&
                (is_directed::apply<Graph>::type::value || ot < ov))
                add_edge(nv, vertex(ot_index, sub), sub);
        }
    }
}

// compare two graphs for labeled exactness (not isomorphism)
template <class Graph>
bool graph_cmp(Graph& g1, Graph& g2)
{
    if (num_vertices(g1) != num_vertices(g2) || num_edges(g1) != num_edges(g2))
        return false;

    typename graph_traits<Graph>::vertex_iterator v1, v1_end;
    typename graph_traits<Graph>::vertex_iterator v2, v2_end;
    tie(v2, v2_end) = vertices(g2);
    for (tie(v1, v1_end) = vertices(g1); v1 != v1_end; ++v1)
    {
        if (out_degree(*v1, g1) != out_degree(*v2, g2))
            return false;
        if (in_degreeS()(*v1, g1) != in_degreeS()(*v2, g2))
            return false;

        std::vector<typename graph_traits<Graph>::vertex_descriptor> out1, out2;
        typename graph_traits<Graph>::out_edge_iterator e, e_end;
        for (tie(e, e_end) = out_edges(*v1, g1); e != e_end; ++e)
            out1.push_back(target(*e, g1));
        for (tie(e, e_end) = out_edges(*v2, g2); e != e_end; ++e)
            out2.push_back(target(*e, g2));
        sort(out1.begin(), out1.end());
        sort(out2.begin(), out2.end());
        if (out1 != out2)
            return false;
    }
    return true;
}

// short hand for both types of subgraphs
typedef adj_list<size_t> d_graph_t;
typedef adj_list<size_t> u_graph_t;

// we need this wrap to use the UndirectedAdaptor only on directed graphs
struct wrap_undirected
{
    template <class Graph>
    struct apply
    {
        typedef typename mpl::if_<typename is_directed::apply<Graph>::type,
                                  UndirectedAdaptor<Graph>,
                                  Graph&>::type type;
    };
};

// get the signature of the graph: sorted degree sequence
template <class Graph>
void get_sig(Graph& g, std::vector<size_t>& sig)
{
    sig.clear();
    size_t N = num_vertices(g);
    if (N > 0)
        sig.resize(is_directed::apply<Graph>::type::value ? 2 * N : N);
    for (size_t i = 0; i < N; ++i)
    {
        typename graph_traits<Graph>::vertex_descriptor v = vertex(i, g);
        sig[i] = out_degree(v, g);
        if(is_directed::apply<Graph>::type::value)
            sig[i + N] = in_degreeS()(v, g);
    }
    sort(sig.begin(), sig.end());
}

// gets (or samples) all the subgraphs in graph g
struct get_all_motifs
{
    get_all_motifs(bool collect_vmaps, double p, bool comp_iso, bool fill_list,
                   rng_t& rng)
        : collect_vmaps(collect_vmaps), p(p),
          comp_iso(comp_iso), fill_list(fill_list), rng(rng) {}
    bool collect_vmaps;
    double p;
    bool comp_iso;
    bool fill_list;
    rng_t& rng;

    template <class Graph, class Sampler, class VMap>
    void operator()(Graph& g, size_t k, boost::any& list,
                    std::vector<size_t>& hist, std::vector<std::vector<VMap> >& vmaps,
                    Sampler sampler) const
    {
        typedef typename mpl::if_<typename is_directed::apply<Graph>::type,
                                  d_graph_t,
                                  u_graph_t>::type graph_sg_t;

        // the main subgraph lists
        std::vector<graph_sg_t>& subgraph_list =
            any_cast<std::vector<graph_sg_t>&>(list);

        // this hashes subgraphs according to their signature
        std::unordered_map<std::vector<size_t>,
                           std::vector<pair<size_t, graph_sg_t> >,
                           std::hash<std::vector<size_t>>> sub_list;
        std::vector<size_t> sig; // current signature

        for (size_t i = 0; i < subgraph_list.size(); ++i)
        {
            get_sig(subgraph_list[i], sig);
            sub_list[sig].push_back(make_pair(i,subgraph_list[i]));
        }

        // the subgraph count
        hist.resize(subgraph_list.size());

        typedef std::uniform_real_distribution<double> rdist_t;
        auto random = std::bind(rdist_t(), std::ref(rng));

        // the set of vertices V to be sampled (filled only if p < 1)
        std::vector<size_t> V;
        if (p < 1)
        {
            typename graph_traits<Graph>::vertex_iterator v, v_end;
            for (tie(v, v_end) = vertices(g); v != v_end; ++v)
                V.push_back(*v);

            size_t n;
            if (random() < p)
                n = size_t(ceil(V.size()*p));
            else
                n = size_t(floor(V.size()*p));

            typedef std::uniform_int_distribution<size_t> idist_t;
            for (size_t i = 0; i < n; ++i)
            {
                auto random_v = std::bind(idist_t(0, V.size()-i-1),
                                          std::ref(rng));
                size_t j = i + random_v();
                swap(V[i], V[j]);
            }
            V.resize(n);
        }

        int i, N = (p < 1) ? V.size() : num_vertices(g);
        #pragma omp parallel for default(shared) private(i, sig) \
            schedule(runtime) if (N > 100)
        for (i = 0; i < N; ++i)
        {
            std::vector<std::vector<typename graph_traits<Graph>::vertex_descriptor> >
                subgraphs;
            typename graph_traits<Graph>::vertex_descriptor v =
                (p < 1) ? V[i] : vertex(i, g);
            if (v == graph_traits<Graph>::null_vertex())
                continue;

            typename wrap_undirected::apply<Graph>::type ug(g);
            get_subgraphs(ug, v, k, subgraphs, sampler);

            #pragma omp critical

            for (size_t j = 0; j < subgraphs.size(); ++j)
            {
                graph_sg_t sub;
                make_subgraph(subgraphs[j], g, sub);
                get_sig(sub, sig);

                typeof(sub_list.begin()) iter = sub_list.find(sig);
                if(iter == sub_list.end())
                {
                    if (!fill_list)
                        continue; // avoid inserting an element in sub_list
                    sub_list[sig].clear();
                }

                bool found = false;
                size_t pos;
                typeof(sub_list.begin()) sl = sub_list.find(sig);
                if (sl != sub_list.end())
                {
                    for (size_t l = 0; l < sl->second.size(); ++l)
                    {
                        graph_sg_t& motif = sl->second[l].second;
                        if (comp_iso)
                        {
                            if (isomorphism(motif, sub,
                                            vertex_index1_map(get(vertex_index, motif)).
                                            vertex_index2_map(get(vertex_index, sub))))
                                found = true;
                        }
                        else
                        {
                            if (graph_cmp(motif, sub))
                                found = true;
                        }
                        if (found)
                        {
                            pos = sl->second[l].first;
                            hist[pos]++;
                            break;
                        }
                    }
                }

                if (found == false && fill_list)
                {
                    subgraph_list.push_back(sub);
                    sub_list[sig].push_back(make_pair(subgraph_list.size() - 1,
                                                      sub));
                    hist.push_back(1);
                    pos = hist.size() - 1;
                    found = true;
                }

                if (found && collect_vmaps)
                {
                    if (pos >= vmaps.size())
                        vmaps.resize(pos + 1);
                    vmaps[pos].push_back(VMap(get(boost::vertex_index,sub)));
                    for (size_t vi = 0; vi < num_vertices(sub); ++vi)
                        vmaps[pos].back()[vertex(vi, sub)] = subgraphs[j][vi];
                }
            }
        }
    }
};

} //graph-tool namespace

#endif // GRAPH_MOTIFS_HH


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_CollisionHandler.hpp
//! \author Alex Robinson
//! \brief  Collision handler class declaration
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_COLLISION_HANDLER_HPP
#define MONTE_CARLO_COLLISION_HANDLER_HPP

// Boost Includes
#include <boost/unordered_map.hpp>

// FRENSIE Includes
#include "MonteCarlo_NeutronMaterial.hpp"
#include "MonteCarlo_PhotonMaterial.hpp"
#include "MonteCarlo_ElectronMaterial.hpp"
#include "MonteCarlo_NeutronState.hpp"
#include "MonteCarlo_PhotonState.hpp"
#include "MonteCarlo_ElectronState.hpp"
#include "Geometry_ModuleTraits.hpp"

namespace MonteCarlo{

//! The collision handler class
class CollisionHandler
{

private:

  // Typedef for cell id neutron material map
  typedef boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
			       Teuchos::RCP<NeutronMaterial> >
  CellIdNeutronMaterialMap;

  // Typedef for cell id photon material map
  typedef boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
			       Teuchos::RCP<PhotonMaterial> >
  CellIdPhotonMaterialMap;

  // Typedef for cell id electron material map
  typedef boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
			       Teuchos::RCP<ElectronMaterial> >
  CellIdElectronMaterialMap;

public:

  //! Add a material to the collision handler
  static void addMaterial(
	      const Teuchos::RCP<NeutronMaterial>& material,
	      const Teuchos::Array<Geometry::ModuleTraits::InternalCellHandle>&
	      cells_containing_material );

  //! Add a material to the collision handler
  static void addMaterial(
	      const Teuchos::RCP<PhotonMaterial>& material,
	      const Teuchos::Array<Geometry::ModuleTraits::InternalCellHandle>&
	      cells_containing_material );

  //! Add a material to the collision handler
  static void addMaterial(
	      const Teuchos::RCP<NeutronMaterial>& neutron_material,
	      const Teuchos::RCP<PhotonMaterial>& photon_material,
	      const Teuchos::Array<Geometry::ModuleTraits::InternalCellHandle>&
	      cells_containing_material );

  //! Add a material to the collision handler
  static void addMaterial(
	      const Teuchos::RCP<ElectronMaterial>& material,
	      const Teuchos::Array<Geometry::ModuleTraits::InternalCellHandle>&
	      cells_containing_material );

  //! Check if a cell is void
  static bool isCellVoid(const Geometry::ModuleTraits::InternalCellHandle cell,
			 const ParticleType particle_type );

  //! Get the neutron material contained in a cell
  static const Teuchos::RCP<NeutronMaterial>&
  getCellNeutronMaterial(
		       const Geometry::ModuleTraits::InternalCellHandle cell );

  //! Get the photon material contained in a cell
  static const Teuchos::RCP<PhotonMaterial>&
  getCellPhotonMaterial(
		       const Geometry::ModuleTraits::InternalCellHandle cell );

  //! Get the electron material contained in a cell
  static const Teuchos::RCP<ElectronMaterial>&
  getCellElectronMaterial(
		       const Geometry::ModuleTraits::InternalCellHandle cell );

  //! Get the total macroscopic cross section of a material
  static double getMacroscopicTotalCrossSection( const NeutronState& particle);

  //! Get the total macroscopic cross section of a material
  static double getMacroscopicTotalCrossSection( const PhotonState& particle );

  //! Get the total macroscopic cross section of a material
  static double getMacroscopicTotalCrossSection( const ElectronState& particle );

  //! Get the macroscopic cross section for a specific reaction
  static double getMacroscopicReactionCrossSection(
					  const NeutronState& particle,
					  const NuclearReactionType reaction );

  //! Get the macroscopic cross section for a specific reaction
  static double getMacroscopicReactionCrossSection(
				      const PhotonState& particle,
				      const PhotoatomicReactionType reaction );

  //! Get the macroscopic cross section for a specific reaction
  static double getMacroscopicReactionCrossSection(
				     const PhotonState& particle,
				     const PhotonuclearReactionType reaction );

  //! Get the macroscopic cross section for a specific reaction
  static double getMacroscopicReactionCrossSection(
				      const ElectronState& particle,
				      const ElectroatomicReactionType reaction );

  //! Collide with the material in a cell
  static void collideWithCellMaterial( PhotonState& particle,
				       ParticleBank& bank,
				       const bool analogue );

  //! Collide with the material in a cell
  static void collideWithCellMaterial( NeutronState& particle,
				       ParticleBank& bank,
				       const bool analogue );

  //! Collide with the material in a cell
  static void collideWithCellMaterial( ElectronState& particle,
				       ParticleBank& bank,
				       const bool analogue );

private:

  // The cell id neutron material map
  static CellIdNeutronMaterialMap master_neutron_map;

  static CellIdPhotonMaterialMap master_photon_map;

  static CellIdElectronMaterialMap master_electron_map;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_COLLISION_HANDLER_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_CollisionHandler.hpp
//---------------------------------------------------------------------------//