#ifndef SUPPORT_PATCH_FACTORY_HH
#define SUPPORT_PATCH_FACTORY_HH

#include <set>
#include <queue>
#include <memory>

#include <dune/common/bitsetvector.hh>
#include <dune/common/fvector.hh>

#include <dune/fufem/referenceelementhelper.hh>
#include <dune/grid/common/mcmgmapper.hh>

#include <dune/localfunctions/lagrange/pqkfactory.hh>

//#include "../../data-structures/levelcontactnetwork.hh"
#include "hierarchicleveliterator.hh"

template <class LevelContactNetwork>
class NetworkIndexMapper {
private:
    using FiniteElementCache = Dune::PQkLocalFiniteElementCache<ctype, ctype, dim, 1>; // P1 finite element cache
    using GridType = typename LevelContactNetwork::DeformedGridType;
    using Element = typename GridType::template Codim<0>::Entity;

    static const int dim = LevelContactNetwork::dimension; //TODO
    static const FiniteElementCache cache_;

    const LevelContactNetwork& levelContactNetwork_;
    const size_t nBodies_;

    std::vector<size_t> vertexOffSet_;
    std::vector<size_t> faceOffSet_;
    std::vector<size_t> elementOffSet_;

public:
    NetworkIndexMapper(const LevelContactNetwork& levelContactNetwork) :
        levelContactNetwork_(levelContactNetwork),
        nBodies_(levelContactNetwork_.nBodies()),
        vertexOffSet_(nBodies_),
        faceOffSet_(nBodies_),
        elementOffSet_(nBodies_) {

        size_t vertexOffSet = 0;
        size_t faceOffSet = 0;
        size_t elementOffSet = 0;

        for (size_t i=0; i<nBodies_; i++) {
            const auto& gridView = levelContactNetwork_.body(i)->gridView();

            vertexOffSet_[i] = vertexOffSet + gridView.size(dim);
            vertexOffSet = vertexOffSet_[i];

            faceOffSet_[i] = faceOffSet + gridView.size(1);
            faceOffSet = faceOffSet_[i];

            elementOffSet_[i] = elementOffSet + gridView.size(0);
            elementOffSet = elementOffSet_[i];
        }
    }

    size_t vertexIndex(const size_t bodyID, const Element& elem, const size_t localVertex) const {
        const auto& gridView = levelContactNetwork_.body(bodyID)->gridView();

        auto localIdx = cache_.get(elem.type()).localCoefficients().localKey(localVertex).subEntity();
        return vertexOffSet_[bodyID] + gridView.indexSet().subIndex(elem, localIdx, dim);
    }

    template <class Intersection>
    size_t faceIndex(const size_t bodyID, const Intersection& is) const {
        const auto& gridView = levelContactNetwork_.body(bodyID)->gridView();
        return faceOffSet_[bodyID] + gridView.indexSet().subIndex(is.inside(), is.indexInInside(), 1);
    }

    size_t faceIndex(const size_t bodyID, const Element& elem, const size_t indexInInside) const {
        const auto& gridView = levelContactNetwork_.body(bodyID)->gridView();
        return faceOffSet_[bodyID] + gridView.indexSet().subIndex(elem, indexInInside, 1);
    }

    size_t elementIndex(const size_t bodyID, const Element& elem) const {
        const auto& gridView = levelContactNetwork_.body(bodyID)->gridView();
        return elementOffSet_[bodyID] + gridView.indexSet().index(elem);
    }

    static const auto& cache() const {
        return cache_;
    }
};

/**
 *  constructs BitSetVector whose entries are
 *      true,   if vertex is outside of patch or part of patch boundary
 *      false,  if vertex is in interior of patch
 */
template <class LevelContactNetwork>
class SupportPatchFactory
{
public:
    using Patch = Dune::BitSetVector<1>;

private:
    struct BodyElement {
        size_t bodyID;
        Element element;

        void set(const size_t _bodyID, const Element& _elem) {
            bodyID = _bodyID;
            element = _elem;
        }
    };

    struct RemoteIntersection {
        size_t bodyID; // used to store bodyID of outside elem
        GridGlue& glue;
        size_t intersectionID;

        bool flip = false;

        void set(const size_t _bodyID, const GridGlue& _glue, const size_t _intersectionID, bool _flip = false) {
            bodyID = _bodyID;
            glue = _glue;
            intersectionID = _intersectionID;
            flip = _flip;
        }

        auto& get() {
            return (flip) ? glue.getIntersection(intersectionID).flip() : glue.getIntersection(intersectionID);
        }
    };

    static const int dim = LevelContactNetwork::dimension; //TODO
    using ctype = typename LevelContactNetwork::ctype;

    //using Vertex = typename GridType::template Codim<dim>::Entity;

    using Element = typename LevelContactNetwork::DeformedGridType::template Codim<0>::Entity;
    using BodyElement = BodyElement<Element>;

    using NetworkIndexMapper = NetworkIndexMapper<LevelContactNetwork>;

    using GridGlue = ;

    const size_t nBodies_;

    const LevelContactNetwork& coarseContactNetwork_;
    const LevelContactNetwork& fineContactNetwork_;

    std::map<size_t, std::vector<RemoteIntersection>> coarseIntersectionMapper_; // map faceID to neighbor elements
    std::map<size_t, std::vector<RemoteIntersection>> fineIntersectionMapper_; // map faceID to neighbor elements
    std::map<size_t, std::vector<size_t>> neighborFaceDofs_; // map faceID to neighbor element dofs

    std::set<size_t> interiorContactDofs_;

    const NetworkIndexMapper coarseIndices_;
    const NetworkIndexMapper fineIndices_;

public:
    SupportPatchFactory(const LevelContactNetwork& coarseContactNetwork, const LevelContactNetwork& fineContactNetwork) :
        nBodies_(coarseContactNetwork_.nBodies()),
        coarseContactNetwork_(coarseContactNetwork),
        fineContactNetwork_(fineContactNetwork),
        coarseIndices_(coarseContactNetwork_),
        fineIndices_(fineContactNetwork_) {

        assert(nBodies_ == fineContactNetwork_.nBodies());

        setup(coarseContactNetwork_, coarseIndices_, coarseIntersectionMapper_);
        setup(fineContactNetwork_, fineIndices_, fineIntersectionMapper_);

        // setup neighborFaceDofs_
        const auto& contactCouplings = coarseContactNetwork_.nBodyAssembler().getContactCouplings();
        for (size_t i=0; i<contactCouplings.size(); i++) {
                const auto& coupling = nBodyAssembler.getCoupling(i);
                const auto& glue = *contactCouplings[i].getGlue();

                const size_t nmBody = coupling.gridIdx_[0];
                const size_t mBody = coupling.gridIdx_[1];

                for (const auto& rIs : intersections(glue)) {
                    size_t nmFaceIdx = coarseIndices_.faceIndex(nmBody, rIs.inside(), rIs.indexInInside());
                    size_t mFaceIdx = coarseIndices_.faceIndex(mBody, rIs.outside(), rIs.indexInOutside());

                    std::array<std::set<size_t>, 2> dofs;
                    remoteIntersectionDofs(coarseIndices_, rIs, nmBody, mBody, dofs);
                    neighborFaceDofs_[nmFaceIdx].insert(neighborFaceDofs_[nmFaceIdx].end(), dofs[1].begin(), dofs[1].end());
                    neighborFaceDofs_[mFaceIdx].insert(neighborFaceDofs_[mFaceIdx].end(), dofs[0].begin(), dofs[0].end());
                }
            }

        /*
            // neighborElementDofs_ contains dofs of connected intersections that are neighbors to a face given by faceID,
            // boundary dofs are contained once, interior dofs multiple times
            // task: remove boundary dofs and redundant multiples of interior dofs
            // TODO: only works in 2D
            for (auto& it : neighborElementDofs) {
                auto& dofs = it.second;

                std::sort(dofs.begin(), dofs.end());
                auto& dof = dofs.begin();
                while (dof != dofs.end()) {
                    auto next = dof;
                    if (*dof != *(++next))
                        dof = dofs.erase(dof);
                }
                dofs.erase(std::unique(dofs.begin(), dofs.end()), dofs.end());
            }
        */
    }

    void build(const size_t bodyID, const Element& coarseElement, const size_t localVertex, Patch& patchDofs, const int patchDepth = 0) {
        BodyElement seedElement(bodyID, coarseElement);

        auto isPatchIntersection = [](auto& is, const size_t bodyID, const std::set<size_t>& patchVertices, std::set<size_t>& newPatchVertices) {
            std::set<size_t> dofs;
            intersectionDofs(coarseIndices_, is, bodyID, dofs);

            for (auto& dof : dofs) {
                if (patchVertices.count(dof)) {
                    newPatchVertices.insert(dofs.begin(), dofs.end());
                    return true;
                }
            }
            return false;
        };

        // construct coarse patch
        std::vector<BodyElement> patchElements;
        patchElements.emplace_back(seedElement);

        std::set<size_t> visitedElements;
        visitedElements.insert(coarseIndices_.elementIndex(seedElement.bodyID, seedElement.element));

        std::set<size_t> patchVertices;
        patchVertices.insert(coarseIndices_.vertexIndex(bodyID, coarseElement, localVertex));

        std::queue<BodyElement> patchSeeds;
        patchSeeds.push(seedElement);

        for (size_t depth=0; depth<=patchDepth; depth++) {
            std::vector<BodyElement> nextSeeds;
            std::set<size_t> newPatchVertices;

            while (!patchSeeds.empty()) {
                const auto& patchSeed = patchSeeds.front();
                patchSeeds.pop();

                size_t elemIdx = coarseIndices_.elementIndex(patchSeed.bodyID, patchSeed.element);

                if (visitedElements.count(elemIdx))
                    continue;

                visitedElements.insert(elemIdx);

                size_t bodyIdx = patchSeed.bodyID;
                const auto& elem = patchSeed.element;
                const auto& gridView = coarseContactNetwork_.body(bodyIdx)->gridView();

                for (auto& it : intersections(gridView, elem)) {
                    if (isPatchIntersection(it, bodyIdx, patchVertices, newPatchVertices)) {
                        if (it.neighbor()) {
                            BodyElement neighbor(bodyIdx, it.outside());

                            if (visitedElements.count(coarseIndices_.elementIndex(neighbor.bodyID, neighbor.element)))
                                continue;

                            patchElements.emplace_back(neighbor);
                            patchSeeds.push(neighbor);
                        } else {
                            size_t faceIdx = coarseIndices_.faceIndex(bodyIdx, it);

                            if (coarseIntersectionMapper_.count(faceIdx)) {
                                const auto& intersections = coarseIntersectionMapper_[faceIdx];
                                for (size_t i=0; i<intersections.size(); i++) {
                                    BodyElement neighbor;
                                    neighbor.set(intersections[i].bodyID, intersections[i].get().outside());

                                    if (visitedElements.count(coarseIndices_.elementIndex(neighbor.bodyID, neighbor.element)))
                                        continue;

                                    patchVertices.insert(neighborElementDofs_[faceIdx].begin(), neighborElementDofs_[faceIdx].end());
                                    patchElements.emplace_back(neighbor);
                                    patchSeeds.push(neighbor);
                                }
                            }
                        }
                    } else {
                        if (it.neighbor()) {
                            BodyElement neighbor(bodyIdx, it.outside());

                            if (visitedElements.count(coarseIndices_.elementIndex(neighbor.bodyID, neighbor.element)))
                                continue;

                            nextSeeds.emplace_back(neighbor);
                        } else {
                            size_t faceIdx = coarseIndices_.faceIndex(bodyIdx, it);

                            if (coarseIntersectionMapper_.count(faceIdx)) {
                                const auto& intersections = coarseIntersectionMapper_[faceIdx];
                                for (size_t i=0; i<intersections.size(); i++) {
                                    BodyElement neighbor;
                                    neighbor.set(intersections[i].bodyID, intersections[i].get().outside());

                                    if (visitedElements.count(coarseIndices_.elementIndex(neighbor.bodyID, neighbor.element)))
                                        continue;

                                    newPatchVertices.insert(neighborElementDofs_[faceIdx].begin(), neighborElementDofs_[faceIdx].end());
                                    nextSeeds.emplace_back(neighbor);
                                }
                            }
                        }
                    }
                }
            }

            for (size_t i=0; i<nextSeeds.size(); i++) {
                patchSeeds.push(nextSeeds[i]);
            }
            patchVertices.insert(newPatchVertices.begin(), newPatchVertices.end());
        }

        // construct fine patch
        using HierarchicLevelIteratorType = HierarchicLevelIterator<typename LevelContactNetwork::DeformedGridType>;
        std::set<size_t> patchBoundary;
        for (size_t i=0; i<patchElements.size(); ++i) {
            const auto& coarseElem = patchElements[i];

            const auto& grid = coarseContactNetwork_.body(coarseElem.bodyID)->gridView().grid();
            const auto fineLevel = fineContactNetwork_.body(coarseElem.bodyID)->gridView().level();

            // add fine patch elements
            HierarchicLevelIteratorType endHierIt(grid, coarseElem.element, HierarchicLevelIteratorType::PositionFlag::end, fineLevel);
            HierarchicLevelIteratorType hierIt(grid, coarseElem.element, HierarchicLevelIteratorType::PositionFlag::begin, fineLevel);
            for (; hierIt!=endHierIt; ++hierIt) {
                const Element& fineElem = *hierIt;
                addLocalDofs(coarseElem, fineElem, visitedElements, patchBoundary, patchDofs);
            }
        }
    }

private:
    void setup(const LevelContactNetwork& levelContactNetwork, const NetworkIndexMapper& indices, std::map<size_t, std::vector<RemoteIntersection>>& faceToRIntersections) {
        const auto& nBodyAssembler = levelContactNetwork.nBodyAssembler();
        const auto& contactCouplings = nBodyAssembler.getContactCouplings();

        for (size_t i=0; i<contactCouplings.size(); i++) {
            const auto& coupling = nBodyAssembler.getCoupling(i);
            const auto& glue = *contactCouplings[i].getGlue();

            const size_t nmBody = coupling.gridIdx_[0];
            const size_t mBody = coupling.gridIdx_[1];

            size_t rIsID = 0;
            for (const auto& rIs : intersections(glue)) {
                size_t nmFaceIdx = indices.faceIndex(nmBody, rIs.inside(), rIs.indexInInside());
                size_t mFaceIdx = indices.faceIndex(mBody, rIs.outside(), rIs.indexInOutside());

                RemoteIntersection nmIntersection(mBody, glue, rIsID, false);
                faceToRIntersections[nmFaceIdx].emplace_back(nmIntersection);

                RemoteIntersection mIntersection(nmBody, glue, rIsID, true);
                faceToRIntersections[mFaceIdx].emplace_back(mIntersection);

                rIsID++;
            }
        }
    }

    template <class Intersection>
    bool containsInsideSubentity(const Element& elem, const Intersection& intersection, int subEntity, int codim) {
	    return ReferenceElementHelper<double, dim>::subEntityContainsSubEntity(elem.type(), intersection.indexInInside(), 1, subEntity, codim);
	}
      
    void addLocalDofs(const BodyElement& coarseElem, const Element& fineElem, const std::set<size_t>& coarsePatch, std::set<size_t>& patchBoundary, Patch& patchDofs) {
	    // insert local dofs
        const auto& lfe = cache_.get(fineElem.type());
        const auto& gridView = fineContactNetwork_.body(coarseElem.bodyID)->gridView();
        const auto& indexSet = gridView.indexSet();
	    
        const size_t bodyID = coarseElem.bodyID;

        const auto coarseLevel = coarseElem.element.level();

	    // search for parts of the global and inner boundary 
        for (const auto& is : intersections(gridView, fineElem)) {
            if (is.neighbor()) {
                std::set<size_t> isDofs;
                intersectionDofs(fineIndices_, is, bodyID, isDofs);

                auto outsideFather = coarseFather(is.outside(), coarseLevel);
		    
                if (!coarsePatch.count(coarseIndices_.elementIndex(coarseElem.bodyID, outsideFather))) {
                    addToPatchBoundary(isDofs, patchBoundary, patchDofs);
                } else {
                    addToPatch(isDofs, patchBoundary, patchDofs);
                }
            } else {
                size_t faceIdx = fineIndices_.faceIndex(bodyID, is);
                if (fineIntersectionMapper_.count(faceIdx)) {
                    const auto& intersections = coarseIntersectionMapper_[faceIdx];

                    for (size_t i=0; i<intersections.size(); i++) {
                        const auto& intersection = intersections[i];
                        const auto& rIs = intersection.get();
                        const size_t outsideID = intersection.bodyID;

                        std::set<size_t> rIsDofs;
                        remoteIntersectionDofs(fineIndices_, rIs, bodyID, outsideID, rIsDofs);

                        if (rIs.neighbor()) {
                            auto outsideFather = coarseFather(rIs.outside(), fineContactNetwork_.body(outsideID)->gridView().level());

                            if (!coarsePatch.count(coarseIndices_.elementIndex(outsideID, outsideFather))) {
                                addToPatchBoundary(rIsDofs, patchBoundary, patchDofs);
                            } else {
                                addToPatch(rIsDofs, patchBoundary, patchDofs);
                            }
                        } else {
                            addToPatchBoundary(rIsDofs, patchBoundary, patchDofs);
                        }
                    }
                } else {
                    std::set<size_t> isDofs;
                    intersectionDofs(fineIndices_, is, bodyID, isDofs);

                    addToPatchBoundary(isDofs, patchBoundary, patchDofs);
                }
            }
        }
	}

    auto&& coarseFather(const Element& fineElem, const size_t coarseLevel) const {
        Element coarseElem = fineElem;
        while (coarseElem.level() > coarseLevel) {
            coarseElem = coarseElem.father();
        }
        return coarseElem;
    }

    void addToPatchBoundary(const std::set<size_t>& dofs, std::set<size_t>& patchBoundary, Patch& patchDofs) {
        for (auto& dof : dofs) {
            patchBoundary.insert(dof);
            patchDofs[dof][0] = true;
        }
    }

    template <class Intersection>
    void addToPatch(const std::set<size_t>& dofs, const std::set<size_t>& patchBoundary, Patch& patchDofs) {
        for (auto& dof : dofs) {
            if (!patchBoundary.count(dof)) {
                patchDofs[dof][0] = false;
            }
        }
    }

    template <class RIntersection>
    void remoteIntersectionDofs(const NetworkIndexMapper& indices, const RIntersection& is, const size_t insideID, const size_t outsideID, std::array<std::set<size_t>, 2>& dofs) {
        dofs.clear();

        const auto& inside = is.inside();
        const auto& insideGeo = inside.geometry();
        const auto& insideRefElement = Dune::ReferenceElements<ctype, dim>::general(inside.type());

        for (int i=0; i<insideRefElement.size(dim); i++) {
            const auto& localCorner = isGeo.local(insideGeo.corner(i));

            if (isRefElement.checkInside(localCorner)) {
                dofs[0].insert(indices.vertexIndex(insideID, inside, i));
            }
        }

        const auto& isGeo = is.geometry();
        const auto& isRefElement = Dune::ReferenceElements<ctype, dim-1>::general(is.type());

        const auto& outside = is.outside();
        const auto& outsideGeo = outside.geometry();
        const auto& outsideRefElement = Dune::ReferenceElements<ctype, dim>::general(outside.type());

        for (int i=0; i<outsideRefElement.size(dim); i++) {
            const auto& localCorner = isGeo.local(outsideGeo.corner(i));

            if (isRefElement.checkInside(localCorner)) {
                dofs[1].insert(indices.vertexIndex(outsideID, outside, i));
            }
        }
    }

    template <class Intersection>
    void intersectionDofs(const NetworkIndexMapper& indices, const Intersection& is, const size_t bodyID, std::set<size_t>& dofs) {
        dofs.clear();

        const Element& elem = is.inside();
        size_t intersectionIndex = is.indexInInside();

        const int dimElement = Element::dimension;
        const auto& refElement = Dune::ReferenceElements<double, dimElement>::general(elem.type());

        for (int i=0; i<refElement.size(intersectionIndex, 1, dimElement); i++) {
            size_t idxInElement = refElement.subEntity(intersectionIndex, 1, i, dimElement);
            dofs.insert(indices.vertexIndex(bodyID, elem, idxInElement));
        }
    }
};
#endif