#include "graph.h"

#include "node_impl.h"
#include "edge_impl.h"

#include <yandex/maps/wiki/topo/exception.h>

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

namespace maps {
namespace wiki {
namespace topo {

Graph::Graph(std::unique_ptr<index::SpatialIndex> index, double tolerance)
    : index_(std::move(index))
    , tolerance_(tolerance)
{}

namespace {

template <class PtrMap>
typename PtrMap::mapped_type::element_type&
find(typename PtrMap::key_type id, PtrMap& map)
{
    typename PtrMap::iterator i = map.find(id);
    REQUIRE(i != map.end(), " object with id " << id << " not found");
    return *i->second;
}

template <class PtrMap>
const typename PtrMap::mapped_type::element_type&
find(typename PtrMap::key_type id, const PtrMap& map)
{
    typename PtrMap::const_iterator i = map.find(id);
    REQUIRE(i != map.end(), " object with id " << id << " not found");
    return *i->second;
}

template <class Map, class T>
T& insert(Map& objects, const T& object)
{
    typename Map::key_type id = object.id();
    typename Map::iterator i = objects.find(id);
    if (i == objects.end()) {
        i = objects.insert(
            std::make_pair(id, std::unique_ptr<T>(new T(object)))).first;
    }
    return *i->second;
}

} // namespace

const Node& Graph::node(NodeID nodeId) const
{
    return find(nodeId, nodes_);
}

Node& Graph::node(NodeID nodeId)
{
    return find(nodeId, nodes_);
}

bool Graph::nodeExists(NodeID nodeId) const
{
    return nodes_.find(nodeId) != nodes_.end();
}

const Edge& Graph::edge(EdgeID edgeId) const
{
    return find(edgeId, edges_);
}

Edge& Graph::edge(EdgeID edgeId)
{
    return find(edgeId, edges_);
}

bool Graph::edgeExists(EdgeID edgeId) const
{
    return edges_.find(edgeId) != edges_.end();
}

void Graph::addNode(const Node& node)
{
    if (!nodeExists(node.id())) {
        const Node& nodeRef = insert(nodes_, node);
        index_->addNode(nodeRef.id(), nodeRef.pos());
    }
}

void Graph::addEdge(const Edge& edge)
{
    if (!edgeExists(edge.id())) {
        Edge& edgeRef = insert(edges_, edge);
        find(edgeRef.startNode(), nodes_).impl_->addEdge(&edgeRef, IncidenceType::Start);
        find(edgeRef.endNode(), nodes_).impl_->addEdge(&edgeRef, IncidenceType::End);
        index_->addEdge(edgeRef.id(), edgeRef.geom());
        checkEdge(edge.id());
    }
}

void Graph::deleteNode(NodeID nodeId)
{
    const Node& node = find(nodeId, nodes_);
    REQUIRE(node.isIsolated(),
        "Error deleting node from graph: "
        "node " << node.id() << " is not isolated");
    index_->deleteNode(nodeId);
    nodes_.erase(nodeId);
}

void Graph::deleteEdge(EdgeID edgeId)
{
    Edge& edge = find(edgeId, edges_);
    find(edge.startNode(), nodes_).impl_->removeEdge(edgeId, IncidenceType::Start);
    find(edge.endNode(), nodes_).impl_->removeEdge(edgeId, IncidenceType::End);
    index_->deleteEdge(edgeId);
    edges_.erase(edgeId);
}

OptionalNodeID
Graph::commonNode(EdgeID edgeId1, EdgeID edgeId2) const
{
    OptionalNodeID res;
    const Edge& edge1 = find(edgeId1, edges_);
    const Edge& edge2 = find(edgeId2, edges_);
    if (edge1.isIncident(edge2.startNode())) {
        res.reset(edge2.startNode());
    } else if (edge1.isIncident(edge2.endNode())) {
        res.reset(edge2.endNode());
    }
    return res;
}

void Graph::setEdgeGeom(EdgeID edgeId, const geolib3::Polyline2& newGeom)
{
    Edge& edg = edge(edgeId);
    edg.impl_->geom = newGeom;
    index_->addEdge(edgeId, newGeom);
    checkEdge(edg.id());
}

void Graph::moveNode(NodeID nodeId, const geolib3::Point2& newPos)
{
    Node& node = find(nodeId, nodes_);
    EdgePtrVector edges = node.impl_->incidentEdges();
    for (auto edgePtr : edges) {
        geolib3::PointsVector points = edgePtr->geom().points();
        if (edgePtr->isStart(nodeId)) {
            points.front() = newPos;
        }
        if (edgePtr->isEnd(nodeId)) {
            points.back() = newPos;
        }
        edgePtr->impl_->geom = geolib3::Polyline2(std::move(points));
        index_->addEdge(edgePtr->id(), edgePtr->geom());
    }
    node.impl_->setPos(newPos);
    index_->addNode(nodeId, newPos);
    checkNode(nodeId);
}

EdgePtrVector Graph::incidentEdges(Node& node)
{
    return node.impl_->incidentEdges();
}

EdgePtrVector Graph::incidentEdges(NodeID nodeId)
{
    return find(nodeId, const_cast<NodePtrMap&>(nodes_)).impl_->incidentEdges();
}

//namespace {

template <class ID, class Type, class Container>
Range<ID, Type>
makeRange(const Container& cont)
{
    return Range<ID, Type>(
        Iterator<ID, Type>(std::make_unique<IteratorImpl<ID, Type> >(cont.begin())),
        Iterator<ID, Type>(std::make_unique<IteratorImpl<ID, Type> >(cont.end())));
}

//} // namespace

NodeConstIteratorRange Graph::nodes() const
{
    return makeRange<NodeID, const Node>(nodes_);
}

EdgeConstIteratorRange Graph::edges() const
{
    return makeRange<EdgeID, const Edge>(edges_);
}

const index::SpatialIndex& Graph::index() const
{
    return *index_;
}

void Graph::checkNode(NodeID nodeId) const
{
    const Node& node = find(nodeId, nodes_);
    for (const auto& edgePtr : node.incidentEdges()) {
        REQUIRE(edgePtr->isIncident(nodeId), "Internal error: "
            "edge " << edgePtr->id() << " is not incident to node " << node.id());
        const bool isStart = edgePtr->isStart(nodeId);
        const geolib3::Point2& edgePoint = isStart
            ? edgePtr->geom().points().front()
            : edgePtr->geom().points().back();
        REQUIRE(geolib3::distance(edgePoint, node.pos()) < tolerance_,
            "Edge and node geom not consistent: "
            "edge " << edgePtr->id() << ", node " << node.id() <<
            (isStart ? ", start" : ", end"));
    }
}

void Graph::checkNodes(const NodeIDSet& nodeIds) const
{
    for (auto nodeId : nodeIds) {
        checkNode(nodeId);
    }
}

void Graph::checkEdge(EdgeID edgeId) const
{
    auto pointToStr = [] (const geolib3::Point2& p) -> std::string
    {
        std::ostringstream os;
        os << "(" << p.x() << ", " << p.y() << ")";
        return os.str();
    };

    const Edge& edge = find(edgeId, edges_);
    const Node& start = find(edge.startNode(), nodes_);
    const Node& end = find(edge.endNode(), nodes_);

    REQUIRE(geolib3::distance(start.pos(), edge.geom().points().front()) <= tolerance_,
        " edge and node geom not consistent: "
        "edge " << edge.id() << ", node " << start.id() << ", start;" <<
        " edge coord: " << pointToStr(edge.geom().points().front()) <<
        "; node pos: " << pointToStr(start.pos()));

    REQUIRE(geolib3::distance(end.pos(), edge.geom().points().back()) <= tolerance_,
        " edge and node geom not consistent: "
        "edge " << edge.id() << ", node " << end.id() << ", end;" <<
        " edge coord: " << pointToStr(edge.geom().points().back()) <<
        "; node pos: " << pointToStr(end.pos()));
}

void Graph::checkEdges(const EdgeIDSet& edgeIds) const
{
    for (auto edgeId : edgeIds) {
        checkEdge(edgeId);
    }
}

} // namespace topo
} // namespace wiki
} // namespace maps
