/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file math.hpp
 * @author Vaclav Blazek <vaclav.blazek@citationtech.net>
 *
 * 3D mesh operations
 */

#include <unordered_set>
#include <unordered_map>

#include "meshop.hpp"
#include "parse-obj.hpp"
#include "parse-ply.hpp"
#include "triclip.hpp"

#include "utility/expect.hpp"
#include "utility/small_list.hpp"

#include <set>
#include <boost/algorithm/string.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/lexical_cast.hpp>

namespace geometry {

namespace ublas = boost::numeric::ublas;
namespace ba = boost::algorithm;

Obj asObj(const Mesh &mesh)
{
    Obj obj;
    for ( math::Point3 vertex : mesh.vertices ) {

        geometry::Obj::Vector3d overtex;
        overtex.x = vertex[0]; overtex.y = vertex[1]; overtex.z = vertex[2];

        obj.addVertex( overtex );

        //LOG( info1 ) << "[" << overtex.x << ", " << overtex.y << ", "
        //    << overtex.z << "]";
    }

    for ( math::Point2 texture : mesh.tCoords ) {

        geometry::Obj::Vector3d otexture;
        otexture.x = texture[0]; otexture.y = texture[1]; otexture.z = 0.0;

        obj.addTexture( otexture );
    }

    for ( geometry::Face face : mesh.faces ) {

        geometry::Obj::Facet facet;

        facet.v[0] = int(face.a);
        facet.v[1] = int(face.b);
        facet.v[2] = int(face.c);

        facet.t[0] = int(face.ta);
        facet.t[1] = int(face.tb);
        facet.t[2] = int(face.tc);

        obj.addFacet( facet );
    }

    return obj;
}

Mesh::pointer asMesh(const Obj &obj){
    auto newMesh(std::make_shared<geometry::Mesh>());
    for( const auto&v : obj.vertices ){
        newMesh->vertices.push_back(v);
    }

    for( const auto&t : obj.texcoords ){
        newMesh->tCoords.emplace_back( t(0), t(1) );
    }

    for( const auto&f : obj.facets ){
        newMesh->addFace(f.v[0], f.v[1], f.v[2], f.t[0], f.t[1], f.t[2]);
    }

    return newMesh;
}

std::string ObjMaterial::name(std::size_t index) const
{
    if (index < names.size()) { return names[index]; }
    return boost::lexical_cast<std::string>(index);
}

namespace {

void addMtl(std::ostream &out, const ObjMaterial &mtl, unsigned int imageId)
{
    if (mtl.libs.empty()) { return; }
    out << "usemtl " << mtl.name(imageId) << '\n';
}

} // namespace

void saveAsObj(const Mesh &mesh, std::ostream &out
               , const ObjMaterial &mtl
               , const boost::filesystem::path &filepath
               , const ObjStreamSetup &streamSetup)
{
    // keep current numeric precision for future use
    const auto oldPrecision(out.precision());

    for (const auto &lib : mtl.libs) {
        out << "mtllib " << lib << '\n';
    }

    if (!streamSetup.vertex(out)) {
        out.setf(std::ios::scientific, std::ios::floatfield);
    }

    for (const auto &vertex : mesh.vertices) {
        out << "v " << vertex(0) << ' ' << vertex(1) << ' '  << vertex(2)
            << '\n';
    }

    if (!streamSetup.tx(out)) {
        out.setf(std::ios::scientific, std::ios::floatfield);
        // reset precision to recorded one
        out.precision(oldPrecision);
    }

    for (const auto &tCoord : mesh.tCoords) {
        out << "vt " << tCoord(0) << ' ' << tCoord(1) << '\n';
    }

    const bool textured(!mesh.tCoords.empty());

    unsigned int currentImageId(static_cast<unsigned int>(-1));

    for (const auto &face : mesh.faces) {
        if (face.degenerate()) {
            continue;
        }

        if (textured && (face.imageId != currentImageId)) {
            addMtl(out, mtl, face.imageId);
            currentImageId = face.imageId;
        }

        if (textured) {
            out << "f " << face.a + 1 << '/' << face.ta + 1 << "/ "
                << face.b + 1 << '/' << face.tb + 1 << "/ "
                << face.c + 1 << '/' << face.tc + 1 << "/\n";
        } else {
            out << "f " << face.a + 1 << ' '
                << face.b + 1 << ' '
                << face.c + 1 << "\n";
        }
    }

    if (!out) {
        LOGTHROW(err3, std::runtime_error)
            << "Unable to save mesh to <" << filepath << ">.";
    }
}

void saveAsObj(const Mesh &mesh, std::ostream &out
               , const ObjMaterial &mtl
               , const boost::filesystem::path &filepath
               , bool setFormat)
{
    struct DontSetFormat : ObjStreamSetup {
        virtual bool vertex(std::ostream&) const { return true; }
        virtual bool tx(std::ostream&) const { return true; }
    };

    const ObjStreamSetup &streamSetup(setFormat
                                      ? ObjStreamSetup()
                                      : DontSetFormat());

    saveAsObj(mesh, out, mtl, filepath, streamSetup);
}

void saveAsObj(const Mesh &mesh, const boost::filesystem::path &filepath
               , const ObjMaterial &mtl, const ObjStreamSetup &streamSetup)
{
    LOG(info2) << "Saving mesh to file <" << filepath << ">.";

    std::ofstream f;
    f.exceptions(std::ios::badbit | std::ios::failbit);
    try {
        f.open(filepath.string(), std::ios_base::out | std::ios_base::trunc);
    } catch (const std::exception&) {
        LOGTHROW(err3, std::runtime_error)
            << "Unable to save mesh to <" << filepath << ">.";
    }

    saveAsObj(mesh, f, mtl, filepath, streamSetup);
}

void saveAsPly( const Mesh &mesh, const boost::filesystem::path &filepath){
    LOG(info2) << "Saving mesh to file <" << filepath << ">.";

    std::ofstream out;
    out.exceptions(std::ios::badbit | std::ios::failbit);
    out.open(filepath.string(), std::ios_base::out | std::ios_base::trunc);
    out.setf(std::ios::scientific, std::ios::floatfield);

    unsigned validFaces(0);
    for (const auto &face : mesh.faces) {
        if (!face.degenerate() && mesh.good(face))
            validFaces++;
    }

    out << "ply\n"
        << "format ascii 1.0\n"
        << "comment generated by window-mesh\n"
        << "element vertex " << mesh.vertices.size() << '\n'
        << "property float x\n"
        << "property float y\n"
        << "property float z\n"
        << "element face " << validFaces << '\n'
        << "property list uchar int vertex_indices\n"
        << "end_header\n";

    for (const auto &vertex : mesh.vertices)
    {
        out << vertex(0) << ' ' << vertex(1) << ' '  << vertex(2) << '\n';
    }

    for (const auto &face : mesh.faces)
    {
        if (face.degenerate()) {
            continue;
        }
        if (!mesh.good(face)) {
            LOG(warn2) << "Invalid vertex index in face.";
            continue;
        }
        out << "3 " << face.a << ' ' << face.b << ' ' << face.c << '\n';
    }

    out.close();

    if (!out) {
        LOGTHROW(err3, std::runtime_error)
            << "Unable to save mesh to <" << filepath << ">.";
    }
}

Mesh loadPly(const boost::filesystem::path& filepath)
{
    class SimplePlyParser : public PlyParserBase
    {
    public:
        Mesh mesh;

        void loadHeader(const std::vector<PlyElement>& elements,
                        const std::vector<std::string>& /* comments */) override
        {
            for (const auto& el : elements)
            {
                if (el.name == "vertex")
                {
                    mesh.vertices.reserve(el.count);

                    // check vertex properties
                    if (el.props.size() != 3)
                    {
                        LOGTHROW(err4, std::runtime_error)
                            << "No additional vertex properties supported in "
                               "simple parser";
                    }
                    utility::expect(el.props[0].name == "x");
                    utility::expect(el.props[1].name == "y");
                    utility::expect(el.props[2].name == "z");
                }
                else if (el.name == "face")
                {
                    mesh.faces.reserve(el.count);

                    // check face properties
                    if (el.props.size() != 1)
                    {
                        LOGTHROW(err4, std::runtime_error)
                            << "No additional face properties supported in "
                               "simple parser";
                    }
                    if (!ba::starts_with(el.props[0].type, "list"))
                    {
                        LOGTHROW(err4, std::runtime_error)
                            << "Expected face property type to be a list, but "
                               "got: "
                            << el.props[0].type;
                    }
                    // does not check the name which should generally be
                    // "vertex_index" but might vary
                }
                else
                {
                    LOGTHROW(err4, std::runtime_error)
                        << "Unexpected element in header - not supported by "
                           "simple parser: "
                        << el.name;
                }
            }
        }

        void addVertex(std::istream& f) override
        {
            double x, y, z;
            f >> x >> y >> z;
            mesh.vertices.emplace_back(x, y, z);
        }

        void addFace(std::istream& f) override
        {
            int n, a, b, c;
            f >> n >> a >> b >> c;
            if (n != 3)
            {
                LOGTHROW(err4, std::runtime_error)
                    << "Simple parser only supports loading triangular meshes, "
                       "but got face with " << n << " vertices.";
            }
            mesh.faces.emplace_back(a, b, c);
        }
    } simpleParser;

    parsePly(simpleParser, filepath);

    LOG(info1) << "Loaded mesh with " << simpleParser.mesh.vertices.size()
               << " vertices and " << simpleParser.mesh.faces.size()
               << " faces";

    return simpleParser.mesh;
}

const unsigned int imageIdLimit(1<<16);

void loadObj( ObjParserBase &parser
            , const boost::filesystem::path &filename)
{
    std::ifstream file;
    file.exceptions(std::ios::badbit | std::ios::failbit);
    file.open(filename.string());

    if (!parser.parse(file)) {
        LOGTHROW(err2, std::runtime_error)
            << "LoadObj failed to parse file " << filename << ".";
    }
}

Mesh loadObj(const boost::filesystem::path &filename, ObjMaterial *mtl)
{
    struct Obj2MeshParser : public ObjParserBase {
        Obj2MeshParser() : imageId(), namedCount() {}

        void addVertex( const Vector3d &v ) {
            mesh.vertices.push_back(v);
        }

        void addTexture( const Vector3d &t ) {
            mesh.tCoords.emplace_back(t.x, t.y);
        }

        void addFacet( const Facet &f ) {
            mesh.addFace( f.v[0], f.v[1], f.v[2]
                        , f.t[0], f.t[1], f.t[2] );
            mesh.faces.back().imageId = imageId;
        }

        void addNormal( const Vector3d& ) { }

        void materialLibrary(const std::string &lib) { seenLibs.insert(lib); }
        void useMaterial(const std::string &name) {
            auto fmtlMap(mtlMap.find(name));
            if (fmtlMap == mtlMap.end()) {
                unsigned int id(0);
                try {
                    // is id a (non negative) number?
                    auto number(boost::lexical_cast<unsigned int>(name));
                    id = number;
                } catch (const boost::bad_lexical_cast&) {
                    // not a number
                    id = imageIdLimit + namedCount++;
                }

                fmtlMap = mtlMap.insert(MtlMap::value_type(name, id)).first;
            }

            imageId = fmtlMap->second;
        }

        Mesh mesh;
        unsigned int imageId;
        std::set<std::string> seenLibs;
        typedef std::map<std::string, int> MtlMap;
        MtlMap mtlMap;
        int namedCount;
    } parser_;

    loadObj(parser_, filename);

    std::vector<std::string> materials;

    // regenerate IDs if there was any non-numeric material
    if (parser_.namedCount) {
        typedef std::map<unsigned int, unsigned int> Mapping;
        Mapping mapping;
        for (const auto &item : parser_.mtlMap) {
            mapping[item.second] = materials.size();
            materials.push_back(item.first);
        }

        for (auto &face : parser_.mesh.faces) {
            face.imageId = mapping[face.imageId];
        }
    }

    if (mtl) {
        mtl->libs.assign(parser_.seenLibs.begin(), parser_.seenLibs.end());
        mtl->names = std::move(materials);
    }

    return parser_.mesh;
}

namespace {

void clipImpl(const Mesh &omesh, Mesh &mesh
              , const std::vector<ClipPlane> &planes)
{
    ClipTriangle::list clipped;
    for (const auto &face : omesh.faces) {
        clipped.emplace_back(
              omesh.vertices[face.a]
            , omesh.vertices[face.b]
            , omesh.vertices[face.c]);
    }

    std::vector<double> tinfos;
    for (const auto &plane : planes) {
        clipped = clipTriangles(clipped, plane, tinfos);
    }

    typedef math::Points3::size_type Index;
    std::map<math::Point3, Index> pMap;
    Index next = 0;

    for (const auto &triangle : clipped)
    {
        Index indices[3];
        for (int i = 0; i < 3; i++)
        {
            auto pair = pMap.insert(std::make_pair(triangle.pos[i], next));
            if (pair.second) {
                next++;
            }
            indices[i] = pair.first->second;

            if (indices[i] >= mesh.vertices.size()) {
                mesh.vertices.push_back(triangle.pos[i]);
            }
        }
        // do not add degenerated faces
        if ((indices[0] != indices[1]) &&
            (indices[1] != indices[2]) &&
            (indices[0] != indices[2]))
        {
            mesh.addFace(indices[0], indices[1], indices[2]);
        }
    }
}

std::vector<ClipPlane> planes(const math::Extents2 &extents)
{
    std::vector<ClipPlane> planes;
    planes.emplace_back(+1.,  0., 0., extents.ll[0]);
    planes.emplace_back(-1.,  0., 0., -extents.ur[0]);
    planes.emplace_back(0.,  +1., 0., extents.ll[1]);
    planes.emplace_back(0.,  -1., 0., -extents.ur[1]);
    return planes;
}

std::vector<ClipPlane> planes(const math::Extents3 &extents)
{
    std::vector<ClipPlane> planes;
    planes.emplace_back(+1.,  0., 0., extents.ll[0]);
    planes.emplace_back(-1.,  0., 0., -extents.ur[0]);
    planes.emplace_back(0.,  +1., 0., extents.ll[1]);
    planes.emplace_back(0.,  -1., 0., -extents.ur[1]);
    planes.emplace_back(0.,  0., +1., extents.ll[2]);
    planes.emplace_back(0.,  0., -1., -extents.ur[2]);
    return planes;
}

} // namespace

Mesh clip(const Mesh &omesh, const math::Extents2 &extents)
{
    Mesh out;
    clipImpl(omesh, out, planes(extents));
    return out;
}

Mesh clip(const Mesh &omesh, const math::Extents3 &extents)
{
    Mesh out;
    clipImpl(omesh, out, planes(extents));
    return out;
}

Mesh::pointer removeNonManifoldEdges(Mesh omesh)
{
    auto ofaces = omesh.faces;
    auto pmesh(std::make_shared<geometry::Mesh>(std::move(omesh)));
    auto& mesh(*pmesh);
    mesh.faces.clear();
    mesh.faces.shrink_to_fit();

    typedef Face::index_type index_type;

    struct EdgeKey
    {
        index_type v1, v2; // vertex indices

        EdgeKey(index_type v1, index_type v2)
        {
            this->v1 = std::min(v1, v2);
            this->v2 = std::max(v1, v2);
        }

        bool operator< (const EdgeKey& other) const
        {
            return (v1 == other.v1) ? (v2 < other.v2) : (v1 < other.v1);
        }
    };

    struct Edge {
        utility::small_list<index_type, 2> facesIndices;
    };

    //count faces for each edge
    std::map<EdgeKey,Edge> edgeMap;
    for(index_type fi=0; fi<ofaces.size(); fi++){
        const auto & face(ofaces[fi]);
        EdgeKey edgeKeys[3] = { EdgeKey(face.a,face.b)
                           , EdgeKey(face.b,face.c)
                           , EdgeKey(face.c,face.a) };
        for(const auto & key : edgeKeys){
            auto it = edgeMap.find(key);
            if(it==edgeMap.end()){
                //if edge is not present insert it with current face
                Edge edge;
                edge.facesIndices.insert(fi);
                edgeMap.insert(std::make_pair(key,edge));
            }
            else{
                //if edge is present add current face to it
                it->second.facesIndices.insert(fi);
            }
        }
    }

    //collect faces incident with non-manifold edge
    std::set<index_type> facesToOmit;
    for(auto it = edgeMap.begin(); it!=edgeMap.end(); it++){
        if(it->second.facesIndices.size()>2){
            it->second.facesIndices.for_each([&](index_type fi) {
                facesToOmit.insert(fi);
            });
        }
    }

    for(index_type fi=0; fi<ofaces.size(); fi++){
        const auto & face(ofaces[fi]);
        if(facesToOmit.find(fi)==facesToOmit.end()){
            mesh.addFace( face.a, face.b, face.c
                        , face.ta, face.tb, face.tc );
        }
    }

    return pmesh;
}

Mesh::pointer removeIsolatedVertices( const Mesh& imesh ){
    auto pmesh(std::make_shared<geometry::Mesh>());
    auto & mesh(*pmesh);

    std::map<math::Points3::size_type, math::Points3::size_type> vertexMap;
    std::map<math::Points2::size_type, math::Points2::size_type> tCoordsMap;

    for( const auto& face: imesh.faces ){
        math::Points3::size_type vindices[3] { face.a , face.b, face.c };
        math::Points2::size_type tindices[3] { face.ta , face.tb, face.tc };

        for(unsigned int i=0; i<3; ++i){
            auto vit = vertexMap.find(vindices[i]);
            auto tit = tCoordsMap.find(tindices[i]);

            if(vit == vertexMap.end()){
                mesh.vertices.push_back(imesh.vertices[vindices[i]]);
                vit = vertexMap.insert(std::make_pair(vindices[i],mesh.vertices.size()-1)).first;
            }
            if(imesh.tCoords.size() > 0 && tit == tCoordsMap.end()){
                mesh.tCoords.push_back(imesh.vertices[tindices[i]]);
                tit = tCoordsMap.insert(std::make_pair(tindices[i],mesh.tCoords.size()-1)).first;
            }

            vindices[i] = vit->second;
            tindices[i] = tit->second;
        }

        if(imesh.tCoords.size() > 0){
            mesh.addFace( vindices[0], vindices[1], vindices[2]
                        , tindices[0], tindices[1], tindices[2] );
        }
        else{
            mesh.addFace( vindices[0], vindices[1], vindices[2] );
        }
    }

    return pmesh;
}

Mesh::pointer refine( const Mesh & omesh, unsigned int maxFacesCount)
{
    auto pmesh(std::make_shared<geometry::Mesh>(omesh));
    auto & mesh(*pmesh);

    struct EdgeKey
    {
        std::size_t v1, v2; // vertex indices

        EdgeKey(std::size_t v1, std::size_t v2)
        {
            this->v1 = std::min(v1, v2);
            this->v2 = std::max(v1, v2);
        }

        bool operator< (const EdgeKey& other) const
        {
            return (v1 == other.v1) ? (v2 < other.v2) : (v1 < other.v1);
        }
    };

    struct Edge {
        typedef enum {
            AB,
            BC,
            CA
        } EdgeType;

        std::size_t v1, v2;
        int f1, f2;
        EdgeType et1, et2;

        float length;

        Edge(std::size_t pv1, std::size_t pv2, float plength)
            :v1(std::min(pv1,pv2)),v2(std::max(pv1,pv2)), length(plength){
            f1 = -1;
            f2 = -1;
        }

        void addFace(std::size_t pv1, std::size_t pv2, int fid, EdgeType type)
        {
            if(pv1<pv2){
                f1=fid;
                et1 = type;
            }
            else{
                f2=fid;
                et2 = type;
            }
        }

        bool operator< (const Edge& other) const
        {
            return length < other.length;
        }
    };

    struct EdgeMap {
        std::map<EdgeKey, std::shared_ptr<Edge>> map;
        std::vector<std::shared_ptr<Edge>> heap;

        bool compareEdgePtr(const std::shared_ptr<Edge> &a, const std::shared_ptr<Edge> &b){
            return a->length<b->length;
        }

        void addFaceEdge(std::size_t pv1, std::size_t pv2, int fid
                         , Edge::EdgeType type, float length)
        {
            EdgeKey key(pv1, pv2);
            auto it(map.find(key));
            if(it!=map.end()){
                it->second->addFace(pv1, pv2, fid, type );
            }
            else{
                heap.push_back(std::make_shared<Edge>(pv1, pv2, length));
                heap.back()->addFace(pv1, pv2, fid, type );
                map.insert(std::make_pair(key,heap.back()));
                std::push_heap(heap.begin(),heap.end(), [this]( const std::shared_ptr<Edge> &a
                                                              , const std::shared_ptr<Edge> &b){
                    return this->compareEdgePtr(a,b);
                });
            }
        }

        Edge pop_top_edge(){
            auto edge = *heap[0];
            std::pop_heap(heap.begin(), heap.end(), [this]( const std::shared_ptr<Edge> &a
                                                          , const std::shared_ptr<Edge> &b){
                return this->compareEdgePtr(a,b);
            });
            heap.pop_back();
            map.erase(EdgeKey(edge.v1, edge.v2));
            return edge;
        }

        Edge top_edge(){
            return *heap[0];
        }

        void addFaceEdges(const Mesh & mesh, int fid){
            const auto& f = mesh.faces[fid];
            auto e1Length =  float(ublas::norm_2(mesh.vertices[f.a]-mesh.vertices[f.b]));
            addFaceEdge(f.a, f.b, fid, Edge::EdgeType::AB, e1Length);

            auto e2Length =  float(ublas::norm_2(mesh.vertices[f.b]-mesh.vertices[f.c]));
            addFaceEdge(f.b, f.c, fid, Edge::EdgeType::BC, e2Length);

            auto e3Length =  float(ublas::norm_2(mesh.vertices[f.c]-mesh.vertices[f.a]));
            addFaceEdge(f.c, f.a, fid, Edge::EdgeType::CA, e3Length);
        }

        std::size_t size(){
            return heap.size();
        }

    };

    EdgeMap edgeMap;

    auto splitEdge = [&mesh,&edgeMap]( int fid, Edge::EdgeType type
                       , std::size_t vid) mutable -> void{
        auto & face = mesh.faces[fid];
        switch(type){
            case Edge::EdgeType::AB:
                {
                    if(mesh.tCoords.size()>0){
                        math::Point2 tcMiddle = ( mesh.tCoords[face.ta]
                                                + mesh.tCoords[face.tb]) * 0.5;
                        mesh.tCoords.push_back(tcMiddle);
                    }
                    mesh.addFace( mesh.faces[fid].b, mesh.faces[fid].c
                                , vid
                                , mesh.faces[fid].tb, mesh.faces[fid].tc
                                , mesh.tCoords.size()-1);

                    mesh.faces[fid].b = vid;
                    mesh.faces[fid].tb = mesh.tCoords.size()-1;

                    edgeMap.addFaceEdges(mesh, fid);
                    edgeMap.addFaceEdges(mesh, int(mesh.faces.size()-1));
                }
                break;
            case Edge::EdgeType::BC:
                {
                    if(mesh.tCoords.size()>0){
                        math::Point2 tcMiddle = (mesh.tCoords[face.tb]
                                                + mesh.tCoords[face.tc]) * 0.5;
                        mesh.tCoords.push_back(tcMiddle);
                    }
                    mesh.addFace( mesh.faces[fid].c,mesh.faces[fid].a, vid
                                , mesh.faces[fid].tc,mesh.faces[fid].ta
                                , mesh.tCoords.size()-1);

                    mesh.faces[fid].c = vid;
                    mesh.faces[fid].tc = mesh.tCoords.size()-1;

                    edgeMap.addFaceEdges(mesh, fid);
                    edgeMap.addFaceEdges(mesh, int(mesh.faces.size()-1));
                }
                break;
            case Edge::EdgeType::CA:
                {
                    if(mesh.tCoords.size()>0){
                        math::Point2 tcMiddle = (mesh.tCoords[face.tc]
                                                + mesh.tCoords[face.ta]) * 0.5;
                        mesh.tCoords.push_back(tcMiddle);
                    }

                    mesh.addFace( mesh.faces[fid].a,mesh.faces[fid].b, vid
                                , mesh.faces[fid].ta,mesh.faces[fid].tb
                                , mesh.tCoords.size()-1);

                    mesh.faces[fid].a = vid;
                    mesh.faces[fid].ta = mesh.tCoords.size()-1;

                    edgeMap.addFaceEdges(mesh, fid);
                    edgeMap.addFaceEdges(mesh, int(mesh.faces.size()-1));
                }
                break;
        }
    };

    for (std::size_t i=0; i<mesh.faces.size(); ++i ) {
        //add all 3 edges
        edgeMap.addFaceEdges(mesh, int(i));
    }

    //sort edges by length
    while( mesh.faces.size() < maxFacesCount && edgeMap.size()>0 ){
        //split edge
        auto edge = edgeMap.pop_top_edge();

        //find middle
        math::Point3 middle = (mesh.vertices[edge.v1]
                            + mesh.vertices[edge.v2]) * 0.5;
        mesh.vertices.push_back(middle);

        //split first face
        if(edge.f1>=0){
            splitEdge(edge.f1, edge.et1, mesh.vertices.size()-1);
        }

        //split second face
        if(edge.f2>=0){
            splitEdge(edge.f2, edge.et2, mesh.vertices.size()-1);
        }
    }

    return pmesh;
}

MeshInfo measurePly(std::istream &is, const boost::filesystem::path &path)
{
    // read header
    std::string line;
    int nvert = -1, ntris = -1;
    do {
        if (getline(is, line).eof()) break;
        std::sscanf(line.c_str(), "element vertex %d", &nvert);
        std::sscanf(line.c_str(), "element face %d", &ntris);
    } while (line != "end_header");

    if (nvert < 0 || ntris < 0) {
        LOGTHROW(err2, std::runtime_error)
            << "Unknown PLY format in file " << path << ".";
    }

    return MeshInfo(nvert, ntris);
}

void loadPly(ObjParserBase &parser, std::istream &is
             , const boost::filesystem::path &path)
{
    const auto mi(measurePly(is, path));

    // load points
    ObjParserBase::Vector3d v;
    for (std::size_t i = 0; i < mi.vertexCount; i++) {
        is >> v.x >> v.y >> v.z;
        parser.addVertex(v);
    }

    // load triangles
    ObjParserBase::Facet f;
    int n;
    for (std::size_t i = 0; i < mi.faceCount; i++) {
        is >> n >> f.v[0] >> f.v[1] >> f.v[2];
        utility::expect(n == 3
                        , "Only triangles are supported in PLY files (&s)."
                        , path);
        parser.addFacet(f);
    }
}

MeshInfo measurePly(const boost::filesystem::path &path)
{
    std::ifstream f(path.string());
    if (!f.good()) {
        LOGTHROW(err2, std::runtime_error)
            << "Can't open PLY file " << path << ".";
    }

    f.exceptions(std::ios::badbit | std::ios::failbit);
    const auto mi(measurePly(f, path));
    f.close();

    return mi;
}

bool objHasVertexOrFace(const boost::filesystem::path &path)
{
    bool hasVorF(false);

    std::ifstream f(path.string());
    if (!f.good()) {
        LOGTHROW(err2, std::runtime_error)
            << "Can't open OBJ file " << path << ".";
    }

    f.exceptions(std::ios::badbit);

    std::string line;
    do {
        getline(f, line);
        if (f.eof()) break;
        if (f.fail()) {
            LOGTHROW(err2, std::runtime_error)
                << "Failbit when reading <" << path << ">";
        }

        if (line.empty()) continue;
        if (line[0] == 'v' || line[0] == 'f') {
            hasVorF = true;
            break;
        }
    } while (true);
    f.close();

    return hasVorF;
}

void loadPly(ObjParserBase &parser, const boost::filesystem::path &path)
{
    std::ifstream f(path.string());
    if (!f.good()) {
        LOGTHROW(err2, std::runtime_error)
            << "Can't open PLY file " << path << ".";
    }

    f.exceptions(std::ios::badbit | std::ios::failbit);

    loadPly(parser, f, path);

    f.close();
}

void append(Mesh &mesh, const Mesh &added)
{
    // remember vertex indexing shift and append vertices
    const auto vShift(mesh.vertices.size());
    mesh.vertices.insert(mesh.vertices.end(), added.vertices.begin()
                         , added.vertices.end());

    // remember texturing coord indexing shift and append texturing coordinates
    const auto tcShift(mesh.tCoords.size());
    mesh.tCoords.insert(mesh.tCoords.end(), added.tCoords.begin()
                        , added.tCoords.end());

    // append faces with shifts applied
    for (const auto &f : added.faces) {
        mesh.addFace(f.a + vShift, f.b + vShift, f.c + vShift
                     , f.ta + tcShift, f.tb + tcShift, f.tc + tcShift
                     , f.imageId);
    }
}

Mesh::list splitById(const Mesh &mesh)
{
    typedef unsigned int ImageId;

    struct MeshBuilder {
        typedef std::unordered_map<ImageId, MeshBuilder> map;

        typedef Face::index_type Index;

        typedef std::unordered_map<Index, Index> VMap;

        VMap vertices;
        Mesh mesh;

        Index vertex(const Mesh &m, Index v) {
            auto fvertices(vertices.find(v));
            if (fvertices == vertices.end()) {
                auto nv(mesh.vertices.size());
                mesh.vertices.push_back(m.vertices[v]);
                fvertices = vertices.insert(VMap::value_type(v, nv)).first;
            }
            return fvertices->second;
        }

        void add(const Mesh &m, const Face &face) {
            mesh.faces.emplace_back(vertex(m, face.a)
                                    , vertex(m, face.b)
                                    , vertex(m, face.c)
                                    , 0, 0, 0, face.imageId);
        }
    };

    MeshBuilder::map builders;
    std::unordered_set<ImageId> unique;
    for (const auto &face : mesh.faces) {
        auto fbuilders(builders.find(face.imageId));
        if (fbuilders == builders.end()) {
            fbuilders
                = builders.insert(MeshBuilder::map::value_type
                                  (face.imageId, MeshBuilder())).first;
        }
        fbuilders->second.add(mesh, face);
    }

    Mesh::list out;
    out.reserve(builders.size());
    for (auto &builder : builders) {
        out.push_back(std::move(builder.second.mesh));
    }
    return out;
}

} // namespace geometry


// Copyright (c) 2019, Torsten Sattler
// 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 copyright holder 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 <COPYRIGHT HOLDER> 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.
//
// author: Torsten Sattler, torsten.sattler.de@googlemail.com

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <iostream>
#include <limits>
#include <random>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

#include <RansacLib/hybrid_ransac.h>
#include <RansacLib/ransac.h>
#include "hybrid_line_estimator.h"

// Generates a random transformation.
void GenerateRandomTransform(Eigen::Matrix2d* R, Eigen::Vector2d* t) {
  std::random_device rand_dev;
  std::mt19937 rng(rand_dev());
  std::uniform_real_distribution<double> distr(-0.5, 0.5);

  const double kAngle = distr(rng) * M_PI;
  *R << std::cos(kAngle), -std::sin(kAngle), std::sin(kAngle), std::cos(kAngle);
  *t = Eigen::Vector2d(distr(rng) * 10.0, distr(rng) * 10.0);
}

// Assumes that inlier threshold << 0.5.
void GenerateRandomInstance(const int num_inliers, const int num_outliers,
                            double inlier_threshold, const Eigen::Matrix2d& R,
                            const Eigen::Vector2d& t,
                            Eigen::Matrix4Xd* points_with_normals) {
  const int kNumPoints = num_inliers + num_outliers;
  points_with_normals->resize(4, kNumPoints);

  std::vector<int> indices(kNumPoints);
  std::iota(indices.begin(), indices.end(), 0);

  std::random_device rand_dev;
  std::mt19937 rng(rand_dev());

  std::shuffle(indices.begin(), indices.end(), rng);

  // Generates num_inliers points along the x-axis in the interval [0, 1] with
  // a y-value in the range (-inlier_threshold, inlier_threshold) choosen
  // at random. All normals of the inliers are set to [0, 1]
  std::uniform_real_distribution<double> distr(-inlier_threshold,
                                               inlier_threshold);

  const double kXStep = 1.0 / static_cast<double>(num_inliers);
  double x = 0.0;
  for (int i = 0; i < num_inliers; ++i, x += kXStep) {
    const int kIndex = indices[i];
    points_with_normals->col(kIndex)[0] = x;
    while (true) {
      points_with_normals->col(kIndex)[1] = distr(rng);
      if (points_with_normals->col(kIndex)[1] > -inlier_threshold) {
        break;
      }
    }
    points_with_normals->col(kIndex)[2] = 0.0;
    points_with_normals->col(kIndex)[3] = 1.0;
  }

  // Randomly generates outliers in the range [0, 1] x [-0.5, 0.5].
  std::uniform_real_distribution<double> distr_x(0.0, 1.0);
  std::uniform_real_distribution<double> distr_y(-0.5, 0.5);
  for (int i = num_inliers; i < kNumPoints; ++i) {
    double x = distr_x(rng);
    double y = distr_y(rng);
    while (std::fabs(y) < 5.0 * inlier_threshold) {
      y = distr_y(rng);
    }

    const int kIndex = indices[i];
    points_with_normals->col(kIndex)[0] = x;
    points_with_normals->col(kIndex)[1] = y;

    points_with_normals->col(kIndex)[2] = 0.0;
    points_with_normals->col(kIndex)[3] = 0.0;
    while (points_with_normals->col(kIndex).tail<2>().norm() < 0.5) {
      points_with_normals->col(kIndex)[2] = distr_y(rng);
      points_with_normals->col(kIndex)[3] = distr_y(rng);
      points_with_normals->col(kIndex).tail<2>().normalize();
    }
  }

  // Rotates and translates the points.
  for (int i = 0; i < kNumPoints; ++i) {
    Eigen::Vector2d p = R * points_with_normals->col(i).head<2>() + t;
    points_with_normals->col(i).head<2>() = p;
    Eigen::Vector2d n = R * points_with_normals->col(i).tail<2>();
    points_with_normals->col(i).tail<2>() = n;
  }
}

int main(int argc, char** argv) {
  ransac_lib::HybridLORansacOptions options;
  options.min_num_iterations_ = 100u;
  options.max_num_iterations_ = 10000u;
  options.max_num_iterations_per_solver_ = 1000u;
  options.squared_inlier_thresholds_ = {0.01 * 0.01, 0.01 * 0.01};
  options.data_type_weights_ = {2.0, 0.5};

  std::random_device rand_dev;
  options.random_seed_ = rand_dev();

  // Generates random instances for outlier ratios 10%, 20%, 30%, ..., 90%,
  // and then applies HybridRANSAC on it.
  const int kNumDataPoints = 100;
  const int kNumDataPointsWithNormals = 100;
  std::vector<double> outlier_ratios = {0.1, 0.2, 0.3, 0.4, 0.5,
                                        0.6, 0.7, 0.8, 0.9};
  for (const double outlier_ratio : outlier_ratios) {
    std::cout << " Inlier ratio: " << 1.0 - outlier_ratio << std::endl;
    int num_outliers_points =
        static_cast<int>(static_cast<double>(kNumDataPoints) * outlier_ratio);
    int num_inliers_points = kNumDataPoints - num_outliers_points;

    Eigen::Matrix2d R;
    Eigen::Vector2d t;
    GenerateRandomTransform(&R, &t);

    Eigen::Matrix4Xd data;
    GenerateRandomInstance(num_inliers_points, num_outliers_points, 0.5 * 0.01,
                           R, t, &data);
    Eigen::Matrix2Xd points(2, data.cols());
    points.row(0) = data.row(0);
    points.row(1) = data.row(1);

    int num_outliers_points_with_normals = static_cast<int>(
        static_cast<double>(kNumDataPointsWithNormals) * outlier_ratio);
    int num_inliers_points_with_normals =
        kNumDataPointsWithNormals - num_outliers_points_with_normals;

    GenerateRandomInstance(num_inliers_points_with_normals,
                           num_outliers_points_with_normals, 0.5 * 0.01, R, t,
                           &data);
    Eigen::Matrix4Xd points_with_normals = data;
    std::cout << "   ... instance generated" << std::endl;

    std::vector<double> prior_probabilities = {0.2, 0.8};
    ransac_lib::HybridLineEstimator solver(points, points_with_normals,
                                           prior_probabilities);
    ransac_lib::HybridLocallyOptimizedMSAC<Eigen::Vector3d,
                                           std::vector<Eigen::Vector3d>,
                                           ransac_lib::HybridLineEstimator>
        lomsac;
    ransac_lib::HybridRansacStatistics ransac_stats;

    std::cout << "   ... running LOMSAC" << std::endl;
    Eigen::Vector3d best_model;
    int num_ransac_inliers =
        lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);
    std::cout << "   ... LOMSAC found " << num_ransac_inliers << " inliers in "
              << ransac_stats.num_iterations_per_solver[0]
              << " iterations of solver 0 and "
              << ransac_stats.num_iterations_per_solver[1]
              << " iterations of solver 1 with inlier ratios of "
              << ransac_stats.inlier_ratios[0] << " and "
              << ransac_stats.inlier_ratios[1] << std::endl;
  }
}


////////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (c) 2021, Andrew Kennings
// 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 copyright holder 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.

////////////////////////////////////////////////////////////////////////////////
// Description:
// An implementation of maximum independent set matching to reduce either
// wirelength of displacement.
//
// The idea is to color the nodes (for wirelength).  Then, one can solve
// a matching problem to optimize for wirelength.  Nodes with the same
// color do not share nets so they can be repositioned without worsening
// wirelength.  Note that for displacement optimization, colors are not
// needed.
//
// There are likely many improvements which can be made to this code
// regarding the selection of nodes, etc.

#include "detailed_mis.h"
#include <lemon/cost_scaling.h>
#include <lemon/cycle_canceling.h>
#include <lemon/list_graph.h>
#include <lemon/preflow.h>
#include <lemon/network_simplex.h>
#include <lemon/smart_graph.h>
#include <boost/format.hpp>
#include <boost/tokenizer.hpp>
#include <deque>
#include <set>
#include <vector>
#include "architecture.h"
#include "color.h"
#include "detailed_manager.h"
#include "detailed_segment.h"
#include "network.h"
#include "router.h"
#include "utl/Logger.h"

using utl::DPO;

namespace dpo {

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
class DetailedMis::Bucket {
 public:
  virtual ~Bucket() {}

  void clear() { m_nodes.clear(); }

  std::deque<Node*> m_nodes;
  double m_xmin = 0.0;
  double m_xmax = 0.0;
  double m_ymin = 0.0;
  double m_ymax = 0.0;
  int m_i = 0;
  int m_j = 0;
  int m_travId = 0;
};

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
DetailedMis::DetailedMis(Architecture* arch, Network* network,
                         RoutingParams* rt)
    : m_mgrPtr(0),
      m_arch(arch),
      m_network(network),
      m_rt(rt),
      m_dimW(0),
      m_dimH(0),
      m_stepX(0.0),
      m_stepY(0.0),
      m_skipEdgesLargerThanThis(100),
      m_maxProblemSize(25),
      m_traversal(0),
      m_useSameSize(true),
      m_useSameColor(true),
      m_maxTimesUsed(2),
      m_obj(DetailedMis::Hpwl) {}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
DetailedMis::~DetailedMis() { clearGrid(); }

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void DetailedMis::run(DetailedMgr* mgrPtr, std::string command) {
  // A temporary interface to allow for a string which we will decode to create
  // the arguments.
  std::string scriptString = command;
  boost::char_separator<char> separators(" \r\t\n;");
  boost::tokenizer<boost::char_separator<char> > tokens(scriptString,
                                                        separators);
  std::vector<std::string> args;
  for (boost::tokenizer<boost::char_separator<char> >::iterator it =
           tokens.begin();
       it != tokens.end(); it++) {
    args.push_back(*it);
  }
  run(mgrPtr, args);
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void DetailedMis::run(DetailedMgr* mgrPtr, std::vector<std::string>& args) {
  // Given the arguments, figure out which routine to run to do the reordering.

  m_mgrPtr = mgrPtr;

  // Defaults.
  m_obj = DetailedMis::Hpwl;

  int passes = 1;
  double tol = 0.01;
  for (size_t i = 1; i < args.size(); i++) {
    if (args[i] == "-p" && i + 1 < args.size()) {
      passes = std::atoi(args[++i].c_str());
    } else if (args[i] == "-t" && i + 1 < args.size()) {
      tol = std::atof(args[++i].c_str());
    } else if (args[i] == "-d") {
      m_obj = DetailedMis::Disp;
    }
  }
  tol = std::max(tol, 0.01);
  passes = std::max(passes, 1);

  double last_hpwl, curr_hpwl, init_hpwl, hpwl_x, hpwl_y;
  double last_disp, curr_disp, init_disp, tot_disp, max_disp, avg_disp;
  double curr_obj, curr_imp;
  std::string obj =
      (m_obj == DetailedMis::Hpwl) ? "wirelength" : "displacement";
  m_mgrPtr->getLogger()->info(DPO, 300, "Set matching objective is {:s}.",
                              obj.c_str());

  // If using displacement objective, then it isn't required to use colors.
  if (m_obj == DetailedMis::Disp) {
    m_useSameColor = false;
  }

  m_mgrPtr->resortSegments();
  curr_hpwl = Utility::hpwl(m_network, hpwl_x, hpwl_y);
  init_hpwl = curr_hpwl;

  curr_disp = Utility::disp_l1(m_network, tot_disp, max_disp, avg_disp);
  init_disp = curr_disp;

  // Do some things that only need to be done once regardless
  // of the number of passes.
  collectMovableCells(); // Movable cells.
  colorCells(); // Color the cells.
  buildGrid(); // Grid for searching for neigbours.

  for (int p = 1; p <= passes; p++) {
    curr_obj = (m_obj == DetailedMis::Hpwl) ? curr_hpwl : curr_disp;

    m_mgrPtr->getLogger()->info(
        DPO, 301, "Pass {:3d} of matching; objective is {:.6e}.", p, curr_obj);

    // Run the algo here...
    place();

    last_hpwl = curr_hpwl;
    curr_hpwl = Utility::hpwl(m_network, hpwl_x, hpwl_y);
    if (m_obj == DetailedMis::Hpwl &&
        std::fabs(curr_hpwl - last_hpwl) / last_hpwl <= tol) {
      break;
    }
    last_disp = curr_disp;
    curr_disp = Utility::disp_l1(m_network, tot_disp, max_disp, avg_disp);
    if (m_obj == DetailedMis::Disp &&
        std::fabs(curr_disp - last_disp) / last_disp <= tol) {
      break;
    }
  }
  m_mgrPtr->resortSegments();

  double hpwl_imp = (((init_hpwl - curr_hpwl) / init_hpwl) * 100.);
  double disp_imp = (((init_disp - curr_disp) / init_disp) * 100.);
  curr_imp = (m_obj == DetailedMis::Hpwl) ? hpwl_imp : disp_imp;
  curr_obj = (m_obj == DetailedMis::Hpwl) ? curr_hpwl : curr_disp;
  m_mgrPtr->getLogger()->info(
      DPO, 302,
      "End of matching; objective is {:.6e}, improvement is {:.2f} percent.",
      curr_obj, curr_imp);
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void DetailedMis::place() {
  // Populate the grid.  Used for searching.
  populateGrid();

  m_timesUsed.resize(m_network->getNumNodes() );
  std::fill(m_timesUsed.begin(), m_timesUsed.end(), 0);

  // Select candidates and solve matching problem.  Note that we need to do
  // something to make this more efficient, otherwise we will solve way too
  // many problems for larger circuits.  I think one effective idea is to
  // keep track of how many problems a candidate cell has been involved in;
  // if it has been involved is >= a certain number of problems, it has "had
  // some chance" to be moved, so skip it.
  Utility::random_shuffle(m_candidates.begin(), m_candidates.end(),
                          m_mgrPtr->m_rng);
  for (size_t i = 0; i < m_candidates.size(); i++) {
    // Pick a candidate as a seed.
    Node* ndi = m_candidates[i];

    // Skip seed if it has been used already.
    if (m_timesUsed[ndi->getId()] >= m_maxTimesUsed) {
      continue;
    }

    // Get other cells within the vicinity.
    if (!gatherNeighbours(ndi)) {
      continue;
    }

    // Solve the flow.
    solveMatch();

    // Increment times each node has been used.
    for (size_t j = 0; j < m_neighbours.size(); j++) {
      Node* ndj = m_neighbours[j];
      ++m_timesUsed[ndj->getId()];
    }

    // Update grid?  Or, do we need to even bother?
    ;
  }
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void DetailedMis::collectMovableCells() {
  m_candidates.erase(m_candidates.begin(), m_candidates.end());
  m_candidates.insert(m_candidates.end(), m_mgrPtr->m_singleHeightCells.begin(),
                      m_mgrPtr->m_singleHeightCells.end());
  for (size_t i = 2; i < m_mgrPtr->m_multiHeightCells.size(); i++) {
    m_candidates.insert(m_candidates.end(),
                        m_mgrPtr->m_multiHeightCells[i].begin(),
                        m_mgrPtr->m_multiHeightCells[i].end());
  }
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void DetailedMis::colorCells() {

  m_colors.resize(m_network->getNumNodes() );
  std::fill(m_colors.begin(), m_colors.end(), -1);

  m_movable.resize(m_network->getNumNodes() );
  std::fill(m_movable.begin(), m_movable.end(), false);
  for (size_t i = 0; i < m_candidates.size(); i++) {
    Node* ndi = m_candidates[i];
    m_movable[ndi->getId()] = true;
  }

  Graph gr(m_network->getNumNodes() );
  for (int e = 0; e < m_network->getNumEdges(); e++) {
    Edge* edi = m_network->getEdge(e);

    int numPins = edi->getPins().size();
    if (numPins <= 1 || numPins > m_skipEdgesLargerThanThis) {
      continue;
    }

    for (int pi = 0; pi < edi->getPins().size(); pi++) {
      Pin* pini = edi->getPins()[pi];
      Node* ndi = pini->getNode();
      if (!m_movable[ndi->getId()]) {
        continue;
      }

      for (int pj = pi + 1; pj < edi->getPins().size(); pj++) {
        Pin* pinj = edi->getPins()[pj];
        Node* ndj = pinj->getNode();
        if (!m_movable[ndj->getId()]) {
          continue;
        }
        if (ndj == ndi) {
          continue;
        }

        gr.addEdge(ndi->getId(), ndj->getId());
      }
    }
  }

  // The actual coloring.
  gr.removeDuplicates();
  gr.greedyColoring();

  std::vector<int> hist;
  for (size_t i = 0; i < m_network->getNumNodes() ; i++) {
    Node* ndi = m_network->getNode(i);

    int color = gr.getColor(i);
    if (color < 0 || color >= gr.getNColors()) {
      m_mgrPtr->internalError( "Unable to color cells during matching" );
    }
    if (m_movable[ndi->getId()]) {
      m_colors[ndi->getId()] = color;

      if (color >= hist.size()) {
        hist.resize(color + 1, 0);
      }
      ++hist[color];
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void DetailedMis::buildGrid() {
  // Builds a coarse grid over the placement region for locating cells.
  m_traversal = 0;

  double xmin = m_arch->getMinX();
  double xmax = m_arch->getMaxX();
  double ymin = m_arch->getMinY();
  double ymax = m_arch->getMaxY();

  // Design each grid bin to hold a few hundred cells.  Do this based on the
  // average width and height of the cells.
  double avgH = 0.;
  double avgW = 0.;
  double avgA = 0.;
  for (size_t i = 0; i < m_candidates.size(); i++) {
    Node* ndi = m_candidates[i];
    avgH += ndi->getHeight();
    avgW += ndi->getWidth();
    avgA += ndi->getHeight() * ndi->getWidth();
  }
  avgH /= (double)m_candidates.size();
  avgW /= (double)m_candidates.size();
  avgA /= (double)m_candidates.size();

  m_stepX = avgW * std::sqrt(m_maxProblemSize);
  m_stepY = avgH * std::sqrt(m_maxProblemSize);

  m_dimW = (int)std::ceil((xmax - xmin) / m_stepX);
  m_dimH = (int)std::ceil((ymax - ymin) / m_stepY);

  clearGrid();
  m_grid.resize(m_dimW);
  for (size_t i = 0; i < m_grid.size(); i++) {
    m_grid[i].resize(m_dimH);
    for (size_t j = 0; j < m_grid[i].size(); j++) {
      m_grid[i][j] = new Bucket;
      m_grid[i][j]->m_xmin = xmin + (i)*m_stepX;
      m_grid[i][j]->m_xmax = xmin + (i + 1) * m_stepX;
      m_grid[i][j]->m_ymin = ymin + (j)*m_stepY;
      m_grid[i][j]->m_ymax = ymin + (j + 1) * m_stepY;
      m_grid[i][j]->m_i = i;
      m_grid[i][j]->m_j = j;
      m_grid[i][j]->m_travId = m_traversal;
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void DetailedMis::populateGrid() {
  // Inserts movable cells into the grid.

  for (size_t i = 0; i < m_grid.size(); i++) {
    for (size_t j = 0; j < m_grid[i].size(); j++) {
      m_grid[i][j]->clear();
    }
  }

  double xmin = m_arch->getMinX();
  double ymin = m_arch->getMinY();

  // Insert cells into the constructed grid.
  m_cellToBinMap.clear();
  for (size_t n = 0; n < m_candidates.size(); ++n) {
    Node* ndi = m_candidates[n];

    int i = std::max(
        std::min((int)((ndi->getX() - xmin) / m_stepX), m_dimW - 1), 0);
    int j = std::max(
        std::min((int)((ndi->getY() - ymin) / m_stepY), m_dimH - 1), 0);

    m_grid[i][j]->m_nodes.push_back(ndi);
    m_cellToBinMap[ndi] = m_grid[i][j];
  }
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void DetailedMis::clearGrid()
// Clear out any old grid.  The dimensions of the grid are also stored in the
// class...
{
  for (size_t i = 0; i < m_grid.size(); i++) {
    for (size_t j = 0; j < m_grid[i].size(); j++) {
      delete m_grid[i][j];
    }
    m_grid[i].clear();
  }
  m_grid.clear();
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
bool DetailedMis::gatherNeighbours(Node* ndi) {
  double singleRowHeight = m_arch->getRow(0)->getHeight();

  m_neighbours.clear();
  m_neighbours.push_back(ndi);

  std::map<Node*, Bucket*>::iterator it;

  // Scan the grid structure gathering up cells which are compatible with the
  // current cell.

  if (m_cellToBinMap.end() == (it = m_cellToBinMap.find(ndi))) {
    return false;
  }

  int spanned_i = (int)(ndi->getHeight() / singleRowHeight + 0.5);

  std::deque<Bucket*> Q;
  Q.push_back(it->second);
  ++m_traversal;
  while (Q.size() > 0) {
    Bucket* currPtr = Q.front();
    Q.pop_front();

    if (currPtr->m_travId == m_traversal) {
      continue;
    }
    currPtr->m_travId = m_traversal;

    // Scan all the cells in this bucket.  If they are compatible with the
    // original cell, then add them to the neighbour list.
    for (size_t j = 0; j < currPtr->m_nodes.size(); j++) {
      Node* ndj = currPtr->m_nodes[j];

      int spanned_j = (int)(ndj->getHeight() / singleRowHeight + 0.5);

      // Check to make sure the cell is not the original, that they have
      // the same region, that they have the same size (if applicable),
      // and that they have the same color (if applicable).
      bool compat = true;
      if (compat) {
        // Same node!
        if (ndj == ndi) {
          compat = false;
        }
      }
      if (compat) {
        // Must be the same color to avoid sharing nets.
        if (m_useSameColor &&
            m_colors[ndi->getId()] != m_colors[ndj->getId()]) {
          compat = false;
        }
      }
      if (compat) {
        // Must be the same size.
        if (m_useSameSize && (ndi->getWidth() != ndj->getWidth() ||
                              ndi->getHeight() != ndj->getHeight())) {
          compat = false;
        }
      }
      if (compat) {
        // Must span the same number of rows and also be voltage compatible.
        if (spanned_i != spanned_j ||
            ndi->getBottomPower() != ndj->getBottomPower() ||
            ndi->getTopPower() != ndj->getTopPower()) {
          compat = false;
        }
      }
      if (compat) {
        // Must be in the same region.
        if (ndj->getRegionId() != ndi->getRegionId()) {
          compat = false;
        }
      }

      // If compatible, include this current cell.
      if (compat) {
        m_neighbours.push_back(ndj);
      }
    }

    if (m_neighbours.size() >= m_maxProblemSize) {
      break;
    }

    // Add more bins to the queue if we have not yet collected enough cells.
    if (currPtr->m_i - 1 >= 0)
      Q.push_back(m_grid[currPtr->m_i - 1][currPtr->m_j]);
    if (currPtr->m_i + 1 <= m_dimW - 1)
      Q.push_back(m_grid[currPtr->m_i + 1][currPtr->m_j]);
    if (currPtr->m_j - 1 >= 0)
      Q.push_back(m_grid[currPtr->m_i][currPtr->m_j - 1]);
    if (currPtr->m_j + 1 <= m_dimH - 1)
      Q.push_back(m_grid[currPtr->m_i][currPtr->m_j + 1]);
  }
  return true;
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void DetailedMis::solveMatch() {
  if (m_neighbours.size() <= 1) {
    return;
  }
  std::vector<Node*>& nodes = m_neighbours;
  double singleRowHeight = m_arch->getRow(0)->getHeight();

  int nNodes = (int)nodes.size();
  int nSpots = (int)nodes.size();
  std::vector<std::pair<double, double> >
      pos;  // Original location of each node.
  pos.resize(nNodes);
  std::vector<std::vector<DetailedSeg*> >
      seg;  // Original segments for each node.
  seg.resize(nNodes);
  for (size_t i = 0; i < nodes.size(); i++) {
    Node* ndi = nodes[i];
    pos[i] = std::make_pair(ndi->getX(), ndi->getY());     // COPY!
    seg[i] = m_mgrPtr->m_reverseCellToSegs[ndi->getId()];  // COPY!
  }

  lemon::ListDigraph g;
  std::vector<lemon::ListDigraph::Node> nodeForCell;
  std::vector<lemon::ListDigraph::Node> nodeForSpot;
  nodeForCell.resize(nNodes);
  nodeForSpot.resize(nNodes);
  for (size_t i = 0; i < nodes.size(); i++) {
    nodeForCell[i] = g.addNode();
    nodeForSpot[i] = g.addNode();
  }
  lemon::ListDigraph::Node supplyNode = g.addNode();
  lemon::ListDigraph::Node demandNode = g.addNode();

  // Hook up the graph.
  lemon::ListDigraph::ArcMap<int> l_i(g);  // Lower bound on flow.
  lemon::ListDigraph::ArcMap<int> u_i(g);  // Upper bound on flow.
  lemon::ListDigraph::ArcMap<int> c_i(g);  // Cost of flow.

  std::map<lemon::ListDigraph::Arc, std::pair<int, int> > reverseMap;

  int origCost = 0, icost;
  for (int i = 0; i < nNodes; i++) {
    // Supply to node.
    lemon::ListDigraph::Arc arc_sv = g.addArc(supplyNode, nodeForCell[i]);
    l_i[arc_sv] = 0;
    u_i[arc_sv] = 1;
    c_i[arc_sv] = 0;

    // Spot to demand.
    lemon::ListDigraph::Arc arc_vt = g.addArc(nodeForSpot[i], demandNode);
    l_i[arc_vt] = 0;
    u_i[arc_vt] = 1;
    c_i[arc_vt] = 0;

    // Nodes to spots.
    Node* ndi = nodes[i];
    for (int j = 0; j < nSpots; j++) {
      icost = 0;
      if (m_obj == DetailedMis::Hpwl) {
        icost = (int)getHpwl(ndi, pos[j].first, pos[j].second);
      } else {
        icost = (int)getDisp(ndi, pos[j].first, pos[j].second);
      }
      origCost += (i != j) ? 0 : icost;

      bool addEdge = true;
      // Reasons to skip the edge?
      ;

      // Add the edge if its okay.
      if (addEdge) {
        // Node to spot.
        lemon::ListDigraph::Arc arc_vu =
            g.addArc(nodeForCell[i], nodeForSpot[j]);
        l_i[arc_vu] = 0;
        u_i[arc_vu] = 1;
        c_i[arc_vu] = icost;

        reverseMap[arc_vu] = std::make_pair(i, j);
      }
    }
  }
  // Try max flow.
  lemon::Preflow<lemon::ListDigraph> preflow(g, u_i, supplyNode, demandNode);
  preflow.run();
  int maxFlow = preflow.flowValue();
  if (maxFlow != nNodes) {
    return;
  }
  // Find mincost flow.
  //lemon::CycleCanceling<lemon::ListDigraph> mincost(g);
  lemon::NetworkSimplex<lemon::ListDigraph> mincost(g);
  mincost.lowerMap(l_i);
  mincost.upperMap(u_i);
  mincost.costMap(c_i);
  mincost.stSupply(supplyNode, demandNode, maxFlow);
  //lemon::CycleCanceling<lemon::ListDigraph>::ProblemType ret = mincost.run();
  lemon::NetworkSimplex<lemon::ListDigraph>::ProblemType ret = mincost.run();
  if (ret != lemon::NetworkSimplex<lemon::ListDigraph>::OPTIMAL) {
    return;
  }

  // Get the solution and assign nodes to new spots.  We also need to update the
  // assignment of cells to segments!  I _believe_ it should be fine to go cell
  // by cell and remove, reposition and update segment assignments one-by-one.
  //
  // This is somewhat tricky.  We need to use the target spot to figure out the
  // segments into which the cell needs to be replaced.

  lemon::ListDigraph::ArcMap<int> flow(g);
  mincost.flowMap(flow);
  int supplyFlow = 0;
  int demandFlow = 0;
  int finalCost = 0;
  int nMoved = 0;

  for (lemon::ListDigraph::ArcMap<int>::ItemIt it(flow); it != lemon::INVALID;
       ++it) {
    if (g.target(it) == demandNode) {
      demandFlow += mincost.flow(it);
    }
    if (g.source(it) == supplyNode) {
      supplyFlow += mincost.flow(it);
    }

    if (g.target(it) != demandNode && g.source(it) != supplyNode &&
        mincost.flow(it) != 0) {
      std::map<lemon::ListDigraph::Arc, std::pair<int, int> >::iterator it1 =
          reverseMap.find(it);
      if (reverseMap.end() == it1) {
        m_mgrPtr->internalError( "Unable to interpret flow during matching" );
      }

      int i = it1->second.first;
      int j = it1->second.second;

      // If cell "i" is assigned to location "i", it means that it has not
      // moved. We don't need to remove and reinsert it...

      Node* ndi = nodes[i];
      Node* ndj =
          nodes[j];  // We effectively "moved" into this cells's location.
      int spanned_i = (int)(ndi->getHeight() / singleRowHeight + 0.5);
      int spanned_j = (int)(ndj->getHeight() / singleRowHeight + 0.5);

      if (ndi != ndj) {
        ++nMoved;
        if (spanned_i != spanned_j ||
            ndi->getWidth() != ndj->getWidth() ||
            ndi->getHeight() != ndj->getHeight()) {
          m_mgrPtr->internalError( "Unable to interpret flow during matching" );
        }

        // Remove cell "i" from its old segments.
        std::vector<DetailedSeg*>& old_segs = seg[i];
        if (spanned_i != old_segs.size()) {
          // This means an error someplace else...
          m_mgrPtr->internalError( "Unable to interpret flow during matching" );
        }
        for (size_t s = 0; s < old_segs.size(); s++) {
          DetailedSeg* segPtr = old_segs[s];
          int segId = segPtr->getSegId();
          m_mgrPtr->removeCellFromSegment(ndi, segId);
        }

        // Update the postion of cell "i".
        ndi->setX(pos[j].first);
        ndi->setY(pos[j].second);

        // Determine new segments and add cell "i" to its new segments.
        std::vector<DetailedSeg*>& new_segs = seg[j];
        if (spanned_i != new_segs.size()) {
          // Not setup for non-same size stuff right now.
          m_mgrPtr->internalError( "Unable to interpret flow during matching" );
        }
        for (size_t s = 0; s < new_segs.size(); s++) {
          DetailedSeg* segPtr = new_segs[s];
          int segId = segPtr->getSegId();
          m_mgrPtr->addCellToSegment(ndi, segId);
        }
      }

      icost = 0;
      if (m_obj == DetailedMis::Hpwl) {
        icost = (int)getHpwl(ndi, pos[j].first, pos[j].second);
      } else {
        icost = (int)getDisp(ndi, pos[j].first, pos[j].second);
      }
      finalCost += icost;
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
double DetailedMis::getDisp(Node* ndi, double xi, double yi) {
  // Compute displacement of cell ndi if placed at (xi,y1) from its orig pos.
  double dx = std::fabs(xi - ndi->getOrigX());
  double dy = std::fabs(yi - ndi->getOrigY());
  return dx + dy;
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
double DetailedMis::getHpwl(Node* ndi, double xi, double yi) {
  // Compute the HPWL of nets connected to ndi assuming ndi is at the
  // specified (xi,yi).

  double hpwl = 0.;
  double x, y, l, r, b, t;
  for (int pi = 0; pi < ndi->getPins().size(); pi++) {
    Pin* pini = ndi->getPins()[pi];

    Edge* edi = pini->getEdge();

    int npins = edi->getPins().size();
    if (npins <= 1 || npins > m_skipEdgesLargerThanThis) {
      continue;
    }

    l = std::numeric_limits<double>::max();
    r = std::numeric_limits<double>::lowest();
    b = std::numeric_limits<double>::max();
    t = std::numeric_limits<double>::lowest();

    for (int pj = 0; pj < edi->getPins().size(); pj++) {
      Pin* pinj = edi->getPins()[pj];

      Node* ndj = pinj->getNode();

      x = (ndj == ndi) ? (xi + pinj->getOffsetX())
                       : (ndj->getX() + pinj->getOffsetX());
      y = (ndj == ndi) ? (yi + pinj->getOffsetY())
                       : (ndj->getY() + pinj->getOffsetY());

      l = std::min(l, x);
      r = std::max(r, x);
      b = std::min(b, y);
      t = std::max(t, y);
    }
    if (r >= l && t >= b) {
      hpwl += ((r - l) + (t - b));
    }
  }

  return hpwl;
}

}  // namespace dpo


//////////////////////////////////////////////////////////////////////
// This file is distributed under the University of Illinois/NCSA Open Source
// License.  See LICENSE file in top directory for details.
//
// Copyright (c) 2016 Jeongnim Kim and QMCPACK developers.
//
// File developed by:
// Miguel A. Morales, moralessilva2@llnl.gov
//    Lawrence Livermore National Laboratory
//
// File created by:
// Miguel A. Morales, moralessilva2@llnl.gov
//    Lawrence Livermore National Laboratory
////////////////////////////////////////////////////////////////////////////////

#ifndef QMCPLUSPLUS_AFQMC_EXCITATIONS_HPP
#define QMCPLUSPLUS_AFQMC_EXCITATIONS_HPP

#include <boost/iterator/iterator_facade.hpp>
#include <map>
#include "AFQMC/config.h"
#include "AFQMC/Matrix/array_of_sequences.hpp"

namespace qmcplusplus
{
namespace afqmc
{
// CHEAT!!!
template<class TP, class integer>
size_t get_index(TP const& tp_, integer loc)
{
  if (loc == 0)
    return size_t(std::get<0>(tp_));
  else if (loc == 1)
    return size_t(std::get<1>(tp_));
  else
    throw std::runtime_error(" Error in qmcplusplus::afqmc::get_index<TP,integer>(). \n");
}

template<class Vector>
void push_excitation(Vector const& abij, Vector& v)
{
  if (abij.size() == 0)
    return;
  assert(v.size() % abij.size() == 0);
  size_t n = abij.size();
  for (typename Vector::iterator it = v.begin(); it < v.end(); it += n)
    if (std::equal(abij.begin(), abij.end(), it))
      return;
  v.insert(v.end(), abij.begin(), abij.end());
}

template<class Vector>
size_t find_excitation(Vector const& abij, Vector& v)
{
  if (abij.size() == 0)
    return 0; // this assumes that the reference is 0
  assert(v.size() % abij.size() == 0);
  size_t n   = abij.size();
  size_t loc = 0;
  for (typename Vector::iterator it = v.begin(); it < v.end(); it += n, loc++)
    if (std::equal(abij.begin(), abij.end(), it))
      return loc;
  APP_ABORT("Error: Sequence not found in find_excitation.\n");
  return 0;
}

template<class excitations>
std::map<int, int> find_active_space(bool single_list, excitations const& abij, int NMO, int NAEA, int NAEB)
{
  std::map<int, int> mo2active;
  for (int i = 0; i < 2 * NMO; i++)
    mo2active[i] = -1;
  std::vector<size_t> count(2 * NMO);
  // reference first
  auto refc = abij.reference_configuration();
  for (int i = 0; i < NAEA + NAEB; i++, ++refc)
    ++count[*refc];
  auto nex = abij.maximum_excitation_number();
  for (int n = 1; n < nex[0]; ++n)
  {
    auto it    = abij.alpha_begin(n);
    auto itend = abij.alpha_end(n);
    for (; it < itend; ++it)
    {
      auto exct = *it + n; // skip locations
      for (int ak = 0; ak < n; ++ak, ++exct)
        ++count[*exct];
    }
  }
  for (int n = 1; n < nex[1]; ++n)
  {
    auto it    = abij.beta_begin(n);
    auto itend = abij.beta_end(n);
    for (; it < itend; ++it)
    {
      auto exct = *it + n; // skip locations
      for (int ak = 0; ak < n; ++ak, ++exct)
        ++count[*exct];
    }
  }
  if (not single_list)
  {
    int ik = 0;
    for (int i = 0; i < NMO; ++i)
      if (count[i] > 0 || count[i + NMO] > 0)
      {
        if (count[i] > 0)
          mo2active[i] = ik;
        if (count[i + NMO] > 0)
          mo2active[i + NMO] = ik;
        ++ik;
      }
  }
  else
  {
    int ik = 0;
    for (int i = 0; i < NMO; ++i)
      if (count[i] > 0)
        mo2active[i] = ik++;
    ik = 0;
    for (int i = NMO; i < 2 * NMO; ++i)
      if (count[i] > 0)
        mo2active[i] = ik++;
  }
  return mo2active;
}

/*
 * - exct stores, for each electron excitaion, the location of the orbital in the reference
 *    being excited and the index of the excited orbital.
 */
template<class Vector, class T>
int get_excitation_number(bool getIndx, Vector& refc, Vector& confg, Vector& exct, T& ci, Vector& Iwork)
{
  int NE = refc.size();
  exct.clear();
  int cnt = 0;
  if (getIndx)
    std::copy(refc.begin(), refc.end(), Iwork.begin());
  auto it = refc.data();
  assert(Iwork.size() >= refc.size());
  for (int i = 0; i < NE; i++, it++)
    if (!std::binary_search(confg.begin(), confg.end(), *it))
    {
      if (getIndx)
      {
        // store the location, NOT the index!!!
        //exct.emplace_back(*it);
        exct.emplace_back(i);
      }
      cnt++;
    }
  if (!getIndx)
    return cnt;
  it       = confg.data();
  int cnt2 = 0;
  for (int i = 0; i < NE; i++, it++)
    if (!std::binary_search(refc.begin(), refc.end(), *it))
    {
      exct.emplace_back(*it);
      Iwork[exct[cnt2]] = *it;
      cnt2++;
    }
  assert(cnt == cnt2);
  // sort Iwork and count number of exchanges to determine permutation sign
  // sooo slow but sooo simple too
  for (int i = 0; i < NE; i++)
    for (int j = i + 1; j < NE; j++)
    {
      if (Iwork[j] < Iwork[i])
      {
        ci *= T(-1.0);
        std::swap(Iwork[i], Iwork[j]);
      }
    }
  return cnt;
}

template<typename intT, class csr>
inline std::vector<size_t> get_nnz(csr const& PsiT_MO, intT* refc, size_t N, size_t shift)
{
  std::vector<size_t> res(N);
  for (size_t i = 0; i < N; i++)
    res[i] = PsiT_MO.num_non_zero_elements(*(refc + i) - shift);
  return res;
}

// try putting this in shared memory later on
template<class I       = int,
         class VType   = std::complex<double>,
         class Alloc   = shared_allocator<I>,
         class is_root = ma::sparse::is_root>
struct ph_excitations
{
public:
  using integer_type       = I;
  using configuration_type = std::tuple<int, int, VType>;

private:
  using IAllocator = Alloc;
  using CAllocator = typename Alloc::template rebind<configuration_type>::other;
  using confg_aos  = ma::sparse::array_of_sequences<configuration_type, int, CAllocator, is_root>;
  using index_aos  = ma::sparse::array_of_sequences<integer_type, int, IAllocator, is_root>;

  IAllocator i_allocator_;
  CAllocator c_allocator_;

  template<typename Integer>
  class Iterator
      : public boost::
            iterator_facade<Iterator<Integer>, Integer*, std::random_access_iterator_tag, Integer*, std::ptrdiff_t>
  {
  public:
    using difference_type = std::ptrdiff_t;
    using reference       = Integer*;
    using const_reference = Integer const*;
    using value_tupe      = Integer*;

    Iterator(Integer* index, size_t d_) : p_index(index), D(d_) {}

    // What we implement is determined by the boost::forward_traversal_tag
  private:
    friend class boost::iterator_core_access;

    void increment() { p_index += 2 * D; }

    bool equal(Iterator const& other) const { return this->p_index == other.p_index; }

    reference dereference() const { return reference(p_index); }

    void decrement() { p_index -= 2 * D; }

    void advance(int n) { p_index += 2 * D * n; }

    difference_type distance_to(Iterator const& z) const { return ((z.p_index - p_index) / 2 / D); }

  private:
    Integer* p_index;
    size_t D;
  };

  template<typename Integer>
  class Iterator_const : public boost::iterator_facade<Iterator_const<Integer>,
                                                       Integer const*,
                                                       std::random_access_iterator_tag,
                                                       Integer const*,
                                                       std::ptrdiff_t>
  {
  public:
    using difference_type = std::ptrdiff_t;
    using reference       = Integer const*;
    using const_reference = Integer const*;
    using value_tupe      = Integer*;

    Iterator_const(Integer* index, size_t d_) : p_index(index), D(d_) {}
    Iterator_const(Integer const* index, size_t d_) : p_index(index), D(d_) {}

    // What we implement is determined by the boost::forward_traversal_tag
  private:
    friend class boost::iterator_core_access;

    void increment() { p_index += 2 * D; }

    bool equal(Iterator_const const& other) const { return this->p_index == other.p_index; }

    reference dereference() const { return reference(p_index); }

    void decrement() { p_index -= 2 * D; }

    void advance(int n) { p_index += 2 * D * n; }

    difference_type distance_to(Iterator_const const& z) const { return ((z.p_index - p_index) / 2 / D); }

  private:
    Integer* p_index;
    size_t D;
  };

public:
  using Excitation_Iterator       = Iterator<integer_type>;
  using Excitation_const_Iterator = Iterator_const<integer_type>;

  ph_excitations() = delete;

  // Note: terms_per_excitation[0] has special meaning, the number of electrons in the calculation.
  // coefficients[0] will store the reference configuration itself.
  ph_excitations(size_t number_of_configurations,
                 int na_,
                 int nb_,
                 std::vector<size_t>& unique_alpha_counts,
                 std::vector<size_t>& unique_beta_counts,
                 Alloc alloc_ = Alloc{})
      : i_allocator_(alloc_),
        c_allocator_(alloc_),
        NAEA(na_),
        NAEB(nb_),
        configurations(1, number_of_configurations, c_allocator_),
        reference(1, NAEA + NAEB, i_allocator_),
        unique_alpha(unique_alpha_counts.size(), unique_alpha_counts, i_allocator_),
        unique_beta(unique_beta_counts.size(), unique_beta_counts, i_allocator_)
  {
    size_t emax = std::max(unique_alpha_counts.size(), unique_beta_counts.size());
    sum_of_exct.resize(emax + 1);
    sum_of_exct[0] = {0, 0};
    sum_of_exct[1] = {1, 1};
    for (size_t n = 1; n < unique_alpha.size(); ++n)
      sum_of_exct[n + 1][0] = sum_of_exct[n][0] + unique_alpha_counts[n] / size_t(2) / n;
    for (size_t n = unique_alpha.size() + 1; n <= emax; n++)
      sum_of_exct[n][0] = sum_of_exct[n - 1][0];
    for (size_t n = 1; n < unique_beta.size(); ++n)
      sum_of_exct[n + 1][1] = sum_of_exct[n][1] + unique_beta_counts[n] / size_t(2) / n;
    for (size_t n = unique_beta.size() + 1; n <= emax; n++)
      sum_of_exct[n][1] = sum_of_exct[n - 1][1];
  }

  ph_excitations(ph_excitations const& other) = delete;
  //ph_excitations(ph_excitations && other) = default;
  ph_excitations(ph_excitations&& other)
      : i_allocator_(other.i_allocator_),
        c_allocator_(other.c_allocator_),
        NAEA(other.NAEA),
        NAEB(other.NAEB),
        configurations(std::move(other.configurations)),
        reference(std::move(other.reference)),
        unique_alpha(std::move(other.unique_alpha)),
        unique_beta(std::move(other.unique_beta)),
        sum_of_exct(std::move(other.sum_of_exct))
  {}

  ph_excitations& operator=(ph_excitations const& other) = delete;
  ph_excitations& operator=(ph_excitations&& other) = default;

  std::array<size_t, 2> maximum_excitation_number() const { return {unique_alpha.size(), unique_beta.size()}; }
  size_t number_of_unique_alpha_excitations(int n) const
  {
    if (n == 0)
      return 1;
    return unique_alpha.num_elements(n) / 2 / n;
  }
  size_t number_of_unique_beta_excitations(int n) const
  {
    if (n == 0)
      return 1;
    return unique_beta.num_elements(n) / 2 / n;
  }
  std::array<size_t, 2> number_of_unique_excitations(int n) const
  {
    if (n == 0)
      return {1, 1};
    std::array<size_t, 2> res{0, 0};
    if (n < unique_alpha.size())
      res[0] = unique_alpha.num_elements(n) / 2 / n;
    if (n < unique_beta.size())
      res[1] = unique_beta.num_elements(n) / 2 / n;
    return res;
  }
  std::array<size_t, 2> number_of_unique_excitations() const { return sum_of_exct.back(); }

  size_t number_of_configurations() const { return configurations.num_elements(0); }

  // returns the number of unique excitations with particle number less than n
  std::array<size_t, 2> number_of_unique_smaller_than(int n) const { return sum_of_exct[n]; }

  template<class intIt>
  void add_alpha(size_t n, intIt indx)
  {
    assert(n < unique_alpha.size());
    assert(n > 0);
    for (int i = 0; i < 2 * n; i++, ++indx)
      unique_alpha.emplace_back(n, static_cast<integer_type>(*indx));
  }

  template<class intIt>
  void add_beta(size_t n, intIt indx)
  {
    assert(n < unique_beta.size());
    assert(n > 0);
    for (int i = 0; i < 2 * n; i++, ++indx)
      unique_beta.emplace_back(n, static_cast<integer_type>(*indx));
  }

  template<class Vector>
  void add_reference(Vector& refa, Vector& refb)
  {
    for (auto k : refa)
      reference.emplace_back(0, static_cast<integer_type>(k));
    for (auto k : refb)
      reference.emplace_back(0, static_cast<integer_type>(k));
  }

  // index=0 is reserved for the reference!!!
  template<typename integer, typename value>
  void add_configuration(integer alpha_indx, integer beta_index, value ci)
  {
    configurations.emplace_back(0, configuration_type{alpha_indx, beta_index, ci});
  }

  typename Excitation_Iterator::const_reference reference_configuration(int spin = 0) const
  {
    return to_address(reference.values(0)) + (spin == 0 ? 0 : NAEA);
  }

  typename Excitation_Iterator::reference reference_configuration(int spin = 0)
  {
    return to_address(reference.values(0)) + (spin == 0 ? 0 : NAEA);
  }

  configuration_type const* configurations_begin() const { return to_address(configurations.values(0)); }

  configuration_type const* configurations_end() const
  {
    return to_address(configurations.values()) + (*configurations.pointers_end(0));
  }

  configuration_type const* configuration(int i) const { return to_address(configurations.values()) + i; }

  Excitation_Iterator alpha_begin(int n)
  {
    assert(n > 0);
    if (n < unique_alpha.size())
    {
      return Excitation_Iterator(to_address(unique_alpha.values(n)), n);
    }
    else
      return alpha_end(n);
  }

  Excitation_Iterator alpha_end(int n)
  {
    assert(n > 0);
    if (n < unique_alpha.size())
      return Excitation_Iterator(to_address(unique_alpha.values()) + (*unique_alpha.pointers_end(n)), n);
    else
      return Excitation_Iterator(to_address(unique_alpha.values()) +
                                     (*unique_alpha.pointers_end(unique_alpha.size() - 1)),
                                 1);
  }

  Excitation_const_Iterator alpha_begin(int n) const
  {
    assert(n > 0);
    if (n < unique_alpha.size())
    {
      return Excitation_const_Iterator(to_address(unique_alpha.values(n)), n);
    }
    else
      return alpha_end(n);
  }

  Excitation_const_Iterator alpha_end(int n) const
  {
    assert(n > 0);
    if (n < unique_alpha.size())
      return Excitation_const_Iterator(to_address(unique_alpha.values()) + (*unique_alpha.pointers_end(n)), n);
    else
      return Excitation_const_Iterator(to_address(unique_alpha.values()) +
                                           (*unique_alpha.pointers_end(unique_alpha.size() - 1)),
                                       1);
  }

  Excitation_Iterator beta_begin(int n)
  {
    assert(n > 0);
    if (n < unique_beta.size())
      return Excitation_Iterator(to_address(unique_beta.values(n)), n);
    else
      return beta_end(n);
  }

  Excitation_Iterator beta_end(int n)
  {
    assert(n > 0);
    if (n < unique_beta.size())
      return Excitation_Iterator(to_address(unique_beta.values()) + (*unique_beta.pointers_end(n)), n);
    else
      return Excitation_Iterator(to_address(unique_beta.values()) + (*unique_beta.pointers_end(unique_beta.size() - 1)),
                                 1);
  }

  Excitation_const_Iterator beta_begin(int n) const
  {
    assert(n > 0);
    if (n < unique_beta.size())
      return Excitation_const_Iterator(to_address(unique_beta.values(n)), n);
    else
      return beta_end(n);
  }

  Excitation_const_Iterator beta_end(int n) const
  {
    assert(n > 0);
    if (n < unique_beta.size())
      return Excitation_const_Iterator(to_address(unique_beta.values()) + (*unique_beta.pointers_end(n)), n);
    else
      return Excitation_const_Iterator(to_address(unique_beta.values()) +
                                           (*unique_beta.pointers_end(unique_beta.size() - 1)),
                                       1);
  }

  // for generic access
  std::array<Excitation_Iterator, 2> unique_begin(int n)
  {
    return std::array<Excitation_Iterator, 2>{alpha_begin(n), beta_begin(n)};
  }
  std::array<Excitation_Iterator, 2> unique_end(int n)
  {
    return std::array<Excitation_Iterator, 2>{alpha_end(n), beta_end(n)};
  }
  std::array<Excitation_const_Iterator, 2> unique_begin(int n) const
  {
    return std::array<Excitation_const_Iterator, 2>{alpha_begin(n), beta_begin(n)};
  }
  std::array<Excitation_const_Iterator, 2> unique_end(int n) const
  {
    return std::array<Excitation_const_Iterator, 2>{alpha_end(n), beta_end(n)};
  }

  template<class Vector>
  void get_alpha_configuration(size_t index, Vector& confg) const
  {
    assert(confg.size() >= NAEA);
    std::copy_n(to_address(reference.values(0)), NAEA, confg.data());
    if (index == 0)
      return;
    // could use lower bound
    for (int i = 1; i < unique_alpha.size(); i++)
    {
      if (index >= sum_of_exct[i][0] && index < sum_of_exct[i + 1][0])
      {
        size_t dn = index - sum_of_exct[i][0];
        auto exct = unique_alpha.values(i) + 2 * i * dn;
        for (int n = 0; n < i; n++)
          confg[exct[n]] = exct[n + i];
        return;
      }
    }
    APP_ABORT(" Error in ph_excitations::get_alpha_configuration() \n");
  }

  template<class Vector>
  void get_beta_configuration(size_t index, Vector& confg) const
  {
    assert(confg.size() >= NAEB);
    std::copy_n(to_address(reference.values(0)) + NAEA, NAEB, confg.data());
    if (index == 0)
      return;
    // could use lower bound
    for (int i = 1; i < unique_beta.size(); i++)
    {
      if (index >= sum_of_exct[i][1] && index < sum_of_exct[i + 1][1])
      {
        size_t dn = index - sum_of_exct[i][1];
        auto exct = unique_beta.values(i) + 2 * i * dn;
        for (int n = 0; n < i; n++)
          confg[exct[n]] = exct[n + i];
        return;
      }
    }
    APP_ABORT(" Error in ph_excitations::get_beta_configuration() \n");
  }

  template<class Vector>
  void get_configuration(int spin, size_t index, Vector& confg) const
  {
    if (spin == 0)
      get_alpha_configuration(index, confg);
    else
      get_beta_configuration(index, confg);
  }

private:
  // using array_of_seq until I switch to Boost.Multi to be able to use shared_allocator
  int NAEA, NAEB;
  confg_aos configurations;
  index_aos reference;
  index_aos unique_alpha;
  index_aos unique_beta;
  std::vector<std::array<size_t, 2>> sum_of_exct;
};

} // namespace afqmc

} // namespace qmcplusplus

#endif