#include "face_gap_remover.h"

#include "../utils/geom_helpers.h"
#include "../utils/searchers.h"

#include <yandex/maps/wiki/threadutils/threadpool.h>

#include <maps/libs/geolib/include/polygon.h>
#include <maps/libs/geolib/include/contains.h>
#include <maps/libs/geolib/include/distance.h>

namespace maps {
namespace wiki {
namespace topology_fixer {

namespace {

const double MIN_SEARCH_BOX_SIZE = 10.0;
const size_t GRID_SIZE = 32;
const double EPS = 0.05;

std::list<OrderedIdSet>
splitNodeIdsByGrid(const TopologyData& data, size_t gridSize)
{
    std::list<OrderedIdSet> result;
    StaticEdgeSearcher searcher(data);
    const auto& bboxOpt = searcher.bbox();
    if (!bboxOpt) {
        return {};
    }
    auto bbox = *bboxOpt;
    const double boxWidth = std::max(bbox.width() / gridSize, MIN_SEARCH_BOX_SIZE);
    const double boxHeight = std::max(bbox.height() / gridSize, MIN_SEARCH_BOX_SIZE);

    for (double minX = bbox.minX(); minX < bbox.maxX(); minX += boxWidth) {
        for (double minY = bbox.minY(); minY < bbox.maxY(); minY += boxHeight) {
            geolib3::BoundingBox b{{minX, minY}, {minX + boxWidth, minY + boxHeight}};
            b = geolib3::resizeByValue(b, -EPS - geolib3::EPS);
            OrderedIdSet nodeIds;
            for (auto edgeId : searcher.idsByBBox(b)) {
                const auto& edge = data.edge(edgeId);
                nodeIds.insert(edge.fnodeId());
                nodeIds.insert(edge.tnodeId());
            }
            if (!nodeIds.empty()) {
                result.push_back(std::move(nodeIds));
            }
        }
    }
    return result;
}

OrderedIdSet
nodeFaceIds(const TopologyData& data, NodeId nodeId)
{
    const Node& node = data.node(nodeId);
    OrderedIdSet result;
    for (auto edgeId : node.edgeIds()) {
        const auto& faceEdges = data.edge(edgeId).faceIds();
        result.insert(faceEdges.begin(), faceEdges.end());
    }
    return result;
}

std::list<NodeIdToFaceIds>
computeBatches(const TopologyData& data)
{
    std::list<NodeIdToFaceIds> result;

    for (const auto& nodeIds : splitNodeIdsByGrid(data, GRID_SIZE)) {
        NodeIdToFaceIds nodeIdToFaceIds;

        for (auto nodeId : nodeIds) {
            const Node& node = data.node(nodeId);
            if (node.edgeIds().size() >= 3) {
                nodeIdToFaceIds.emplace(nodeId, nodeFaceIds(data, nodeId));
            }
        }

        if (!nodeIdToFaceIds.empty()) {
            result.push_back(std::move(nodeIdToFaceIds));
        }
    }

    return result;
}

typedef std::map<EdgeId, IdSet> EdgeToFaceIdsMap;

boost::optional<EdgeToFaceIdsMap>
parseNodeData(utils::TopologyDataProxy& data, NodeId nodeId)
{
    const Node& node = data.node(nodeId);
    if (node.edgeIds().size() < 3) {
        return boost::none;
    }
    EdgeToFaceIdsMap result;
    for (auto edgeId : node.edgeIds()) {
        const auto& edgeFaces = data.edge(edgeId).faceIds();
        result.emplace(edgeId, edgeFaces);
    }
    return result;
}

struct FaceChain {
    EdgeIdsList edgeIds;
    NodeId lastNodeId;
    bool isRemovable;
};

FaceChain
buildChain(
    utils::TopologyDataProxy& data,
    EdgeId startEdgeId,
    NodeId startNodeId,
    FaceId faceId,
    FaceId otherFaceId)
{
    EdgeIdsList edgeIds = {startEdgeId};
    bool chainRemovable = true;
    const Edge& edge = data.edge(startEdgeId);
    IdSet currentFaceIds = edge.faceIds();
    NodeId currentNodeId = edge.fnodeId() == startNodeId
        ? edge.tnodeId()
        : edge.fnodeId();
    bool nodeInCurrentFaceOnly = true;
    while (nodeInCurrentFaceOnly && currentNodeId != startNodeId) {
        const Node& node = data.node(currentNodeId);
        IdSet faceEdgeIds;
        for (auto edgeId : node.edgeIds()) {
            const auto& edgeFaces = data.edge(edgeId).faceIds();
            if (edgeFaces.count(faceId)) {
                faceEdgeIds.insert(edgeId);
            }
            nodeInCurrentFaceOnly = nodeInCurrentFaceOnly &&
                !edgeFaces.count(otherFaceId);
        }
        if (!nodeInCurrentFaceOnly) {
            break;
        }
        REQUIRE(faceEdgeIds.size() == 2,
            "Face " << faceId << " has " << faceEdgeIds.size()
            << " edges coming into node " << currentNodeId
            << ", start node id " << startNodeId << ", start edge id " << startEdgeId
            << ", edge ids [" << toString(faceEdgeIds) << "]"
            << ", current node edges [" << toString(node.edgeIds()) << "]");

        EdgeId currentEdgeId = *faceEdgeIds.begin() == edgeIds.back()
            ? *std::next(faceEdgeIds.begin())
            : *faceEdgeIds.begin();
        const Edge& currentEdge = data.edge(currentEdgeId);
        bool edgeInCurrentFacesOnly =
            idsSymDiff(currentEdge.faceIds(), currentFaceIds).empty();
        edgeIds.push_back(currentEdgeId);
        currentNodeId = currentEdge.fnodeId() == currentNodeId
            ? currentEdge.tnodeId()
            : currentEdge.fnodeId();
        chainRemovable = chainRemovable && edgeInCurrentFacesOnly &&
            node.edgeIds().size() == 2;
    }
    return {edgeIds, currentNodeId, chainRemovable};
}

geolib3::PointsVector
polylineFromEdgeChain(utils::TopologyDataProxy& data, const EdgeIdsList& edgeIds)
{
    const Edge& edge = data.edge(edgeIds.front());
    NodeId fnodeId = edge.fnodeId();
    NodeId tnodeId = edge.tnodeId();
    geolib3::PointsVector result = edge.linestring().points();
    for (auto it = std::next(edgeIds.begin()); it != edgeIds.end(); ++it) {
        const Edge& edge = data.edge(*it);
        const auto& edgePoints = edge.linestring().points();
        if (edge.fnodeId() == fnodeId) {
            result.insert(result.begin(), edgePoints.rbegin(), std::prev(edgePoints.rend()));
            fnodeId = edge.tnodeId();
        } else if (edge.tnodeId() == fnodeId) {
            result.insert(result.begin(), edgePoints.begin(), std::prev(edgePoints.end()));
            fnodeId = edge.fnodeId();
        } else if (edge.fnodeId() == tnodeId) {
            result.insert(result.end(), std::next(edgePoints.begin()), edgePoints.end());
            tnodeId = edge.tnodeId();
        } else {
            REQUIRE(edge.tnodeId() == tnodeId,
                "Edge " << *it << " has no common points"
                << " with other edges in chain [" << toString(edgeIds) << "]");
            result.insert(result.end(), std::next(edgePoints.rbegin()), edgePoints.rend());
            tnodeId = edge.fnodeId();
        }
    }
    return result;
}

bool
edgesChainsEquivalent(utils::TopologyDataProxy& data,
    const EdgeIdsList& removedEdgeIds, const EdgeIdsList& newEdgeIds,
    double maxGapWidth,
    double maxGapLengthFactor)
{
    geolib3::PointsVector removedPoints = polylineFromEdgeChain(data,removedEdgeIds);
    geolib3::PointsVector newPoints = polylineFromEdgeChain(data,newEdgeIds);
    REQUIRE((removedPoints.front() == newPoints.front() &&
            removedPoints.back() == newPoints.back()) ||
            (removedPoints.front() == newPoints.back() &&
            removedPoints.back() == newPoints.front()),
        "Edge chains [" << toString(removedEdgeIds)
            << "], [" << toString(newEdgeIds) << "] "
            << "do not share start and end points");
    double removedLength = geolib3::length(geolib3::Polyline2(removedPoints));
    double newLength = geolib3::length(geolib3::Polyline2(newPoints));
    if (std::fabs(newLength - removedLength) /
        std::max(newLength, removedLength) > maxGapLengthFactor)
    {
        return false;
    }
    geolib3::Polyline2 removedPolyline(removedPoints);
    for (const auto& point : newPoints) {
        if (geolib3::distance(removedPolyline, point) >
            (1.0 + maxGapLengthFactor) * maxGapWidth)
        {
            return false;
        }
    }
    if (removedPoints.front() == newPoints.front()) {
        removedPoints.insert(removedPoints.end(), std::next(newPoints.rbegin()), newPoints.rend());
    } else {
        removedPoints.insert(removedPoints.end(), std::next(newPoints.begin()), newPoints.end());
    }
    return std::fabs(topology_fixer::signedArea(removedPoints)) <
        maxGapWidth * (removedLength + newLength) / 2.0;
}

bool
newFaceGeometriesValid(
    utils::TopologyDataProxy& data,
    const EdgeIdsList& removedEdgeIds,
    const EdgeIdsList& newEdgeIds)
{
    REQUIRE(!removedEdgeIds.empty(), "Empty edge ids list");
    const IdSet& affectedFaceIds = data.edge(removedEdgeIds.front()).faceIds();

    for (auto it = std::next(removedEdgeIds.begin()); it != removedEdgeIds.end(); ++it) {
        const auto& faceIds = data.edge(*it).faceIds();
        if (!idsSymDiff(faceIds, affectedFaceIds).empty()) {
            return false;
        }
    }
    for (auto faceId : affectedFaceIds) {
        PlainEdgesData edgesData;
        for (auto edgeId : data.face(faceId).edgeIds()) {
            const Edge& edge = data.edge(edgeId);
            edgesData.insert({edgeId,
                {edge.fnodeId(), edge.tnodeId(), edge.linestring().points()}});
        }
        for (auto edgeId : removedEdgeIds) {
            edgesData.erase(edgeId);
        }
        for (auto edgeId : newEdgeIds) {
            const Edge& edge = data.edge(edgeId);
            edgesData.emplace(edgeId,
                PlainEdgeData{edge.fnodeId(), edge.tnodeId(), edge.linestring().points()});
        }
        try {
            geolib3::Polygon2 poly(buildPolygon(faceId, edgesData));
        } catch (const std::exception& e) {
            WARN() << "Face " << faceId << " geometry in invalid after "
                << "replacing edges [" << toString(removedEdgeIds)
                << "] by [" << toString(newEdgeIds) << "]";
            return false;
        }
    }
    return true;
}

void
mergeFaceEdges(
    utils::TopologyDataProxy& data,
    const EdgeIdsList& removedEdgeIds,
    const EdgeIdsList& newEdgeIds)
{
    REQUIRE(!removedEdgeIds.empty(), "Empty edge ids list");
    IdSet affectedFaceIds = data.edge(removedEdgeIds.front()).faceIds();

    for (auto it = std::next(removedEdgeIds.begin()); it != removedEdgeIds.end(); ++it) {
        const auto& faceIds = data.edge(*it).faceIds();
        REQUIRE(idsSymDiff(faceIds, affectedFaceIds).empty(),
            "Edges in removed chain [" << toString(removedEdgeIds) << " do not share same faces");
    }

    for (auto faceId : affectedFaceIds) {
        for (auto edgeId : removedEdgeIds) {
            //() << "Removed edge " << edgeId << " from face " << faceId;
            data.removeFaceEdgeRel(faceId, edgeId);
        }
    }
    for (auto edgeId : removedEdgeIds) {
        data.removeEdge(edgeId, DeletionMode::Cascade);
    }
    for (auto faceId : affectedFaceIds) {
        for (auto edgeId : newEdgeIds) {
            //INFO() << "Added edge " << edgeId << " to face " << faceId;
            data.addFaceEdge(faceId, edgeId);
        }
    }
}

} // namespace

void
FaceGapRemover::Batch::doWork()
{
    for (const auto& pair : nodeIdToFaceIds_) {
        auto nodeId = pair.first;
        const auto& faceIds = pair.second;

        auto locks = locker_.lockFaces(faceIds);

        if (!data_.nodeExists(nodeId)) {
            continue;
        }
        const Node& node = data_.node(nodeId);
        boost::optional<EdgeToFaceIdsMap> nodeData = parseNodeData(data_, nodeId);
        if (!nodeData) {
            continue;
        }
        for (const auto& edgeFaces1 : *nodeData) {
            const EdgeId edgeId1 = edgeFaces1.first;
            const auto& faceIds1 = edgeFaces1.second;
            for (auto faceId1 : faceIds1) {
                for (const auto& edgeFaces2 : *nodeData) {
                    const EdgeId edgeId2 = edgeFaces2.first;
                    const auto& faceIds2 = edgeFaces2.second;
                    if (!idsIntersection(faceIds1, faceIds2).empty()) {
                        continue;
                    }
                    for (auto faceId2 : faceIds2) {
                        if (edgeId1 == edgeId2
                            || faceId1 == faceId2
                            || !node.edgeIds().count(edgeId1)
                            || !node.edgeIds().count(edgeId2))
                        {
                            continue;
                        }
                        processEdges(nodeId, edgeId1, faceId1, edgeId2, faceId2);
                    }
                }
            }
        }
    }
}

void
FaceGapRemover::Batch::processEdges(
    NodeId commonNodeId,
    EdgeId edgeId1, FaceId faceId1, EdgeId edgeId2, FaceId faceId2)
{
    const Edge& edge1 = data_.edge(edgeId1);
    const Edge& edge2 = data_.edge(edgeId2);

    if (edge1.fnodeId() == edge1.tnodeId() || edge2.fnodeId() == edge2.tnodeId()) {
        return;
    }

    FaceChain faceChain1 = buildChain(
        data_, edgeId1, commonNodeId, faceId1, faceId2);
    FaceChain faceChain2 = buildChain(
        data_, edgeId2, commonNodeId, faceId2, faceId1);
    const auto& edgeIds1 = faceChain1.edgeIds;
    const auto& edgeIds2 = faceChain2.edgeIds;
    if ((!faceChain1.isRemovable && !faceChain2.isRemovable) ||
        faceChain1.lastNodeId != faceChain2.lastNodeId ||
        faceChain1.lastNodeId == commonNodeId ||
        !edgesChainsEquivalent(
            data_, edgeIds1, edgeIds2, maxGapWidth_, maxGapLengthFactor_))
    {
        return;
    }
    if (faceChain1.isRemovable &&
        newFaceGeometriesValid(data_, edgeIds1, edgeIds2))
    {
        const IdSet faceIds = data_.edge(edgeId1).faceIds();
        mergeFaceEdges(data_, edgeIds1, edgeIds2);
        INFO() << "Replaced edges [" << toString(edgeIds1) << "] by ["
               << toString(edgeIds2) << "] in faces " << toString(faceIds);
    } else if (faceChain2.isRemovable &&
        newFaceGeometriesValid(data_, edgeIds2, edgeIds1))
    {
        const IdSet faceIds = data_.edge(edgeId2).faceIds();
        mergeFaceEdges(data_, edgeIds2, edgeIds1);
        INFO() << "Replaced edges [" << toString(edgeIds2) << "] by ["
            << toString(edgeIds1) << "] in faces " << toString(faceIds);
    }
}

void
FaceGapRemover::operator()(TopologyData& data, FaceLocker& locker, ThreadPool& pool) const
{
    utils::ThreadSafeTopologyDataProxy dataProxy(data);

    IdSet processedNodeIds;

    Executor executor;
    auto nodesBatches = computeBatches(data);
    for (auto&& batch : nodesBatches) {
        for (const auto& pair : batch) {
            processedNodeIds.insert(pair.first);
        }

        executor.addTask(
            Batch(
                dataProxy,
                locker,
                std::move(batch),
                maxGapWidth_,
                maxGapLengthFactor_));
    }
    executor.executeAllInThreads(pool);

    NodeIdToFaceIds nodeIdToFaceIds;
    for (auto nodeId : data.nodeIds()) {
        const Node& node = data.node(nodeId);
        if (node.edgeIds().size() >= 3 && !processedNodeIds.count(nodeId)) {
            nodeIdToFaceIds.emplace(nodeId, nodeFaceIds(data, nodeId));
        }
    }

    utils::TrivialTopologyDataProxy trivialDataProxy(data);
    Batch(
        trivialDataProxy,
        locker,
        std::move(nodeIdToFaceIds),
        maxGapWidth_,
        maxGapLengthFactor_)
    ();
}

} // namespace topology_fixer
} // namespace wiki
} // namespace maps
