#include "routenetwork.h"
#include "drawingarea.h"
#include "geomutils.h"
#include <limits>
#include <cmath>
#include <forward_list>
#include <unordered_set>
#include <boost/graph/dijkstra_shortest_paths.hpp>

using namespace boost;

RouteNetwork::RouteNetwork()
{
}

RouteNetwork::RouteNetwork(RouteNetwork&& other)
{
    std::swap(_graph, other._graph);
}

RouteNetwork& RouteNetwork::operator=(RouteNetwork&& other)
{
    if (this != &other) {
        _graph.clear();
        std::swap(_graph, other._graph);
    }
    return *this;
}

RouteNetwork::vertex_range RouteNetwork::vertices() const
{
    return boost::vertices(_graph);
}
RouteNetwork::edge_range RouteNetwork::edges() const
{
    return boost::edges(_graph);
}

VertexId RouteNetwork::addVertex(QPointF const& position)
{
    VertexId v = add_vertex(_graph);
    PositionMap vertex_position = get(vertex_position_t(), _graph);
    put(vertex_position, v, position);
    return v;
}

EdgeId RouteNetwork::addEdge(VertexId start, VertexId end)
{
    EdgeId e; bool isNew;
    std::tie(e, isNew) = add_edge(start, end, _graph);
    LengthMap edge_length = get(edge_length_t(), _graph);
    QPointF p1 = vertexPosition(start);
    QPointF p2 = vertexPosition(end);
    put(edge_length, e, distance(p1, p2));
    return e;
}

namespace {
    class EdgeIsClose
    {
    public:
        EdgeIsClose(Graph& graph_, QPointF const& position_, qreal maxDistance_,
                    std::unordered_set<VertexId>& verticesToReconnect_)
            : graph(graph_), position(position_), maxDistanceSq(maxDistance_*maxDistance_),
              verticesToReconnect(verticesToReconnect_),
              vertexToPosition( get(vertex_position_t(), graph_) )
        {}
        bool operator()(EdgeId edge)
        {
            VertexId v1 = source(edge, graph);
            VertexId v2 = target(edge, graph);
            QPointF p   = vertexToPosition[v1];
            QPointF dir = vertexToPosition[v2] - p;
            qreal distSq = segmentToPointDistanceSquared(p, dir, position);
            if (distSq <= maxDistanceSq) {
                verticesToReconnect.emplace(v1);
                verticesToReconnect.emplace(v2);
                return true;
            } else {
                return false;
            }
        }
    private:
        Graph& graph;
        QPointF position;
        qreal maxDistanceSq;
        std::unordered_set<VertexId>& verticesToReconnect;
        PositionMap vertexToPosition;
    };
} // unnamed namespace

void RouteNetwork::fixConsistencyProblems()
{
    // When drawing problem instances in Ipe, it can be a pain to add vertices
    // at edge intersections and subdivide edges when they contain vertices.
    // This function does that for us.

    // Add vertices at edge intersections.
    edge_iterator ei, ej, eend;
    for (std::tie(ei, eend) = boost::edges(_graph); ei != eend; ++ei) {
        for (ej = std::next(ei); ej != eend; ++ej) {
            QPointF intersection; VertexId vertex;
            if (edgeIntersection(*ei, *ej, intersection) && !closestVertexWithin(intersection, 0.9, vertex)) {
                addVertex(intersection);
            }
        }
    }

    // Locate vertices that lie on the interior of edges, and delete those edges.
    std::forward_list< std::vector<VertexId> > edgeSubdivisions;
    remove_edge_if([this, &edgeSubdivisions](EdgeId e) {
                       std::vector<VertexId> verticesOnEdge;

                       QPointF edgeStart = edgeSourcePosition(e);
                       QPointF edgeDirection = edgeTargetPosition(e) - edgeStart;
                       for (auto v : vertices()) {
                           if (source(e, _graph) == v || target(e, _graph) == v)
                               continue;
                           qreal alpha = closestPointOnSegmentParameter(edgeStart, edgeDirection, vertexPosition(v));
                           if (alpha <= 0 || alpha >= 1)
                               continue;
                           qreal distSq = distanceSquared(vertexPosition(v), edgeStart + alpha*edgeDirection);
                           if (distSq < 1) {
                               verticesOnEdge.emplace_back(v);
                           }
                       }
                       if (verticesOnEdge.empty())
                           return false;

                       verticesOnEdge.emplace_back(source(e, _graph));
                       verticesOnEdge.emplace_back(target(e, _graph));

                       std::sort(verticesOnEdge.begin(), verticesOnEdge.end(),
                                 [this, e, &edgeStart, &edgeDirection](VertexId v1, VertexId v2) {
                                     qreal a1 = closestPointOnLineParameter(edgeStart, edgeDirection, vertexPosition(v1));
                                     qreal a2 = closestPointOnLineParameter(edgeStart, edgeDirection, vertexPosition(v2));
                                     return a1 < a2;
                                 });
                       edgeSubdivisions.emplace_front( std::move(verticesOnEdge) );
                       return true;
                   },
                   _graph);

    // Create new edges to stitch togther those vertices.
    for (auto& verticesOnEdge : edgeSubdivisions) {
        auto vi = verticesOnEdge.begin();
        auto vend = verticesOnEdge.end();
        for (auto vj = std::next(vi); vj != vend; vi = vj, ++vj) {
            addEdge(*vi, *vj);
        }
    }
}

void RouteNetwork::rerouteEdgesWithin(VertexId vertex2, qreal maxDistance)
{
    std::unordered_set<VertexId> verticesToReconnect;
    EdgeIsClose edgeIsClose(_graph, vertexPosition(vertex2), maxDistance, verticesToReconnect);

    // remove all edges passing close to vertex2
    remove_edge_if(edgeIsClose, _graph);

    // reconnect those edges onto vertex2
    for (auto&& vertex1 : verticesToReconnect)
    {
        addEdge(vertex1, vertex2);
    }
}

bool RouteNetwork::edgeExists(VertexId start, VertexId end) const
{
    EdgeId e; bool exists;
    std::tie(e, exists) = edge(start, end, _graph);
    return exists;
}

bool RouteNetwork::isVertexIsolated(VertexId v) const
{
    return out_degree(v, _graph) == 0 && in_degree(v, _graph) == 0;
}

QPointF RouteNetwork::vertexPosition(VertexId v) const
{
    ConstPositionMap vertexToPosition = get(vertex_position_t(), _graph);
    return vertexToPosition[v];
}

QPointF RouteNetwork::edgeSourcePosition(EdgeId e) const
{
    return vertexPosition(source(e, _graph));
}
QPointF RouteNetwork::edgeTargetPosition(EdgeId e) const
{
    return vertexPosition(target(e, _graph));
}

qreal RouteNetwork::edgeLength(EdgeId e) const
{
    ConstLengthMap edgeToLength = get(edge_length_t(), _graph);
    return edgeToLength[e];
}

qreal RouteNetwork::edgeLength(VertexId start, VertexId end) const
{
    EdgeId e; bool exists;
    std::tie(e, exists) = edge(start, end, _graph);
    if (exists) {
        return edgeLength(e);
    } else {
        return std::numeric_limits<qreal>::infinity();
    }
}

void RouteNetwork::draw(DrawingArea* drawing, DrawOptions const& options) const
{
    if (options[DrawEdges]) {
        for (EdgeId e : edges()) {
            drawing->drawExistingEdge( edgeSourcePosition(e),
                                       edgeTargetPosition(e) );
        }
    }
    if (options[DrawVertices]) {
        for (VertexId v : vertices()) {
            drawing->drawExistingVertex( vertexPosition(v) );
        }
    }
}

bool RouteNetwork::closestVertexWithin(QPointF const& position, qreal maxDistance, VertexId& result) const
{
    qreal maxDistSq = maxDistance*maxDistance;
    qreal bestDistSq = std::numeric_limits<qreal>::infinity();
    for (VertexId v : vertices()) {
        qreal distSq = distanceSquared(vertexPosition(v), position);
        if (distSq < bestDistSq && distSq <= maxDistSq) {
            bestDistSq = distSq;
            result = v;
        }
    }
    return bestDistSq < std::numeric_limits<qreal>::infinity();
}

bool RouteNetwork::edgeIntersection(EdgeId e1, EdgeId e2, QPointF& result) const
{
    QPointF p1   = vertexPosition(source(e1, _graph));
    QPointF dir1 = vertexPosition(target(e1, _graph)) - p1;
    QPointF p2   = vertexPosition(source(e2, _graph));
    QPointF dir2 = vertexPosition(target(e2, _graph)) - p2;
    qreal alpha1 = lineIntersectionParameter(p1,dir1, p2,dir2);
    if (0 <= alpha1 && alpha1 <= 1) {
        qreal alpha2 = lineIntersectionParameter(p2,dir2, p1,dir1);
        if (0 <= alpha2 && alpha2 <= 1) {
            result = p1 + alpha1*dir1;
            return true;
        }
    }
    return false;
}

bool RouteNetwork::closestIntersectionWithin(QPointF const& position, qreal maxDistance, QPointF& result) const
{
    qreal maxDistSq = maxDistance*maxDistance;
    edge_iterator ei1, ei2, ei_end;
    qreal bestDistSq = std::numeric_limits<qreal>::infinity();
    for (std::tie(ei1, ei_end) = boost::edges(_graph); ei1 != ei_end; ++ei1) {
        for (ei2 = std::next(ei1); ei2 != ei_end; ++ei2) {
            QPointF intersection;
            if (edgeIntersection(*ei1, *ei2, intersection)) {
                qreal distSq = distanceSquared(intersection, position);
                if (distSq < bestDistSq && distSq <= maxDistSq) {
                    bestDistSq = distSq;
                    result = intersection;
                }
            }
        }
    }
    return bestDistSq < std::numeric_limits<qreal>::infinity();
}

void RouteNetwork::singleSourceShortestPaths(
        VertexId startVertex, PredecessorMap& predecessors, DistanceMap& distances) const
{
    predecessors.clear();
    distances.clear();
    ConstLengthMap edgeLength = get(edge_length_t(), _graph);

    dijkstra_shortest_paths(_graph, startVertex, predecessor_map(make_assoc_property_map(predecessors))
                                               .distance_map(make_assoc_property_map(distances))
                                               .weight_map(edgeLength));
}
