#include "segments_overlapper.h"
#include "../geom_helpers.h"

#include <maps/libs/geolib/include/distance.h>
#include <maps/libs/geolib/include/line.h>
#include <maps/libs/geolib/include/direction.h>
#include <maps/libs/geolib/include/projection.h>

#include <maps/libs/log8/include/log8.h>

#include <initializer_list>
#include <algorithm>
#include <deque>

#include <iomanip>

namespace maps {
namespace wiki {
namespace topology_fixer {
namespace utils {

SegmentsProcessingResult
SegmentsPairOverlapper::Impl::processSegmentsOverlap(
    SegmentId segmentId1, SegmentId segmentId2)
{
    if (segmentId1 == segmentId2) {
        return {{segmentId1}, {segmentId2}, false};
    }
    gl::Segment2 segmentGeom1 = graph_.segmentGeom(segmentId1);
    gl::Segment2 segmentGeom2 = graph_.segmentGeom(segmentId2);

    if (gl::length(segmentGeom1) < tolerance_ || gl::length(segmentGeom2) < tolerance_)
    {
        return {{segmentId1}, {segmentId2}, false};
    }

    bool firstAndSecondOverlap = segmentsOverlap(
        segmentGeom1, segmentGeom2, tolerance_, maxAngleBetweenSegments_);
    bool secondAndFirstOverlap = segmentsOverlap(
        segmentGeom2, segmentGeom1, tolerance_, maxAngleBetweenSegments_);

    if (!firstAndSecondOverlap && !secondAndFirstOverlap) {
        return {{segmentId1}, {segmentId2}, false};
    }

    const Segment& segment1 = graph_.segment(segmentId1);
    const Segment& segment2 = graph_.segment(segmentId2);
    auto pointIsMergeable = [&] (PointId pointId) -> bool
    {
        const Point& point = graph_.point(pointId);
        return !point.nodeId;
    };
    SnappedPointsVector snapped1 =
        {{segment1.startPointId, 0.0}, {segment1.endPointId, 1.0}};
    SnappedPointsVector snapped2 =
        {{segment2.startPointId, 0.0}, {segment2.endPointId, 1.0}};

    std::map<PointId, PointId> mergedPoints;
    if (firstAndSecondOverlap) {
        snap(snapped2, snapped1, segmentGeom1, pointIsMergeable, mergedPoints);
    }
    if (secondAndFirstOverlap) {
        snap(snapped1, snapped2, segmentGeom2, pointIsMergeable, mergedPoints);
    }
    if (mergedPointsValid(mergedPoints) && snappedPointsValid(snapped1, mergedPoints) &&
        snappedPointsValid(snapped2, mergedPoints))
    {
        mergePoints(mergedPoints);
        auto newSegments1 =
            processSnappedPointsVector(segmentId1, snapped1, mergedPoints);
        auto newSegments2 =
            processSnappedPointsVector(segmentId2, snapped2, mergedPoints);
        ASSERT(!newSegments1.empty() && !newSegments2.empty());
        bool changed = !mergedPoints.empty() ||
            newSegments1.size() > 1 || newSegments1.front() != segmentId1 ||
            newSegments2.size() > 1 || newSegments2.front() != segmentId2;
        return {std::move(newSegments1), std::move(newSegments2), changed};
    }
    WARN() << "Segments " << segmentId1 << ", " << segmentId2 << " were not processed";
    return {{segmentId1}, {segmentId2}, false};
}

SegmentsPairOverlapper::SnappedPointIterators
SegmentsPairOverlapper::Impl::findSnapIterators(
    const SnappedPoint& snappedPoint,
    SnappedPointsVector& otherPoints,
    double epsFactor)
{
    auto insertIt = std::upper_bound(
        otherPoints.begin(), otherPoints.end(),
        snappedPoint,
        [] (const SnappedPoint& lhs, const SnappedPoint& rhs) -> bool
        {
            return lhs.projFactor < rhs.projFactor;
        });
    const double projFactor = snappedPoint.projFactor;
    typedef SnappedPointsVector::iterator Iterator;
    std::list<Iterator> mergeIterators;
    if (insertIt != otherPoints.end() &&
        std::fabs(insertIt->projFactor - projFactor) < epsFactor)
    {
        mergeIterators.push_back(insertIt);
    }
    if (insertIt != otherPoints.begin()) {
        auto prevIt = std::prev(insertIt);
        if (std::fabs(projFactor - prevIt->projFactor) < epsFactor) {
            mergeIterators.push_back(prevIt);
        }
    }
    auto mergeIt = otherPoints.end();
    if (!mergeIterators.empty()) {
        auto it = std::min_element(
            mergeIterators.begin(), mergeIterators.end(),
            [&] (const Iterator& lhs, const Iterator& rhs) -> bool
            {
                return std::fabs(projFactor - lhs->projFactor) <
                    std::fabs(projFactor - rhs->projFactor);
            });
        ASSERT(it != mergeIterators.end());
        mergeIt = *it;
    }
    return {insertIt, mergeIt};
}

void
SegmentsPairOverlapper::Impl::snap(
    SnappedPointsVector& thisPoints, SnappedPointsVector& otherPoints,
    const gl::Segment2& otherSegment,
    SegmentsPairOverlapper::MergeablePointsFilter isPointMergeable,
    std::map<PointId, PointId>& mergedPoints)
{
    for (auto& point : thisPoints) {
        auto snapResult = snapPoint(
            graph_.point(point.id), otherPoints, otherSegment, isPointMergeable);
        if (!snapResult) {
            continue;
        }
        const auto& snappedPoint = *snapResult;
        if (snappedPoint.id != point.id) {
            auto it1 = mergedPoints.find(snappedPoint.id);
            auto it2 = mergedPoints.find(point.id);
            if ((it1 != mergedPoints.end() && it1->second == point.id) ||
                (it2 != mergedPoints.end() && it2->second == snappedPoint.id))
            {
                continue;
            }
            mergedPoints.insert({point.id, snappedPoint.id});
        }
    }
}

boost::optional<SegmentsPairOverlapper::SnappedPoint>
SegmentsPairOverlapper::Impl::snapPoint(
    const Point& point,
    SnappedPointsVector& otherPoints,
    const gl::Segment2& otherSegment,
    MergeablePointsFilter pointIsMergeable)
{
    double projFactor = gl::projectionFactor(otherSegment, point.pos);
    const double epsFactor = tolerance_ / gl::length(otherSegment);
    if (projFactor > 1 + epsFactor || projFactor < -epsFactor) {
        return boost::none;
    }
    SnappedPoint snappedPoint = {point.id, projFactor};
    SnappedPointIterators iterators = findSnapIterators(snappedPoint, otherPoints, epsFactor);
    if (iterators.mergeIterator != otherPoints.end()) {
        if (!pointIsMergeable(point.id)) {
            return boost::none;
        }
        if (iterators.mergeIterator->id != point.id) {
            return *iterators.mergeIterator;
        } else {
            return boost::none;
        }
    }
    otherPoints.insert(iterators.insertIterator, snappedPoint);
    return snappedPoint;
}

SegmentIdsList
SegmentsPairOverlapper::Impl::processSnappedPointsVector(
    SegmentId segmentId,
    const SnappedPointsVector& snappedPoints,
    const std::map<PointId, PointId>& mergedPoints)
{
    SegmentIdsList result;
    auto mapPointId = [&] (PointId id) -> PointId
    {
        auto it = mergedPoints.find(id);
        return it == mergedPoints.end() ? id : it->second;
    };
    if (snappedPoints.size() > 2) {
        graph_.removeSegment(segmentId);
        for (size_t i = 0; i < snappedPoints.size() - 1; ++i) {
            auto startPointId = mapPointId(snappedPoints[i].id);
            auto endPointId = mapPointId(snappedPoints[i + 1].id);
            const Segment& segment = graph_.addSegment(startPointId, endPointId);
            result.push_back(segment.id);
        }
    } else {
        result.push_back(segmentId);
    }
    return result;
}

bool
SegmentsPairOverlapper::Impl::mergedPointsValid(
    const std::map<PointId, PointId>& mergedPoints) const
{
    for (const auto& pointsPair : mergedPoints) {
        if (graph_.point(pointsPair.first).nodeId ||
            graph_.pointsShareSegment(pointsPair.first, pointsPair.second))
        {
            return false;
        }
    }
    return true;
}

void
SegmentsPairOverlapper::Impl::mergePoints(
    const std::map<PointId, PointId>& mergedPoints)
{
    for (const auto& pointsPair : mergedPoints) {
        graph_.mergePoints(pointsPair.first, pointsPair.second);
    }
}

bool
SegmentsPairOverlapper::Impl::snappedPointsValid(
    const SnappedPointsVector& snappedPoints,
    const std::map<PointId, PointId>& mergedPoints) const
{
    REQUIRE(snappedPoints.size() >= 2, "Too few snapped points in vector");
    auto mapPointId = [&] (PointId id) -> PointId
    {
        auto it = mergedPoints.find(id);
        return it == mergedPoints.end() ? id : it->second;
    };
    for (size_t i = 1; i < snappedPoints.size(); ++i) {
        const Point& prev = graph_.point(mapPointId(snappedPoints[i - 1].id));
        const Point& cur = graph_.point(mapPointId(snappedPoints[i].id));
        if (prev.id == cur.id || gl::distance(prev.pos, cur.pos) < tolerance_) {

            return false;
        }
    }
    return true;
}

SegmentsProcessingResult
SegmentsPairOverlapper::operator () (
    SegmentsGraph& graph, SegmentId segmentId1, SegmentId segmentId2) const
{
    Impl processor(graph, tolerance_, maxAngleBetweenSegments_);
    return processor.processSegmentsOverlap(segmentId1, segmentId2);
}

// SegmentsOverlapper

namespace {

struct SegmentIdsPair {
    SegmentId segmentId1;
    SegmentId segmentId2;
};

typedef std::map<PointId, std::list<SegmentIdsPair>> PointToSegmentsPairMap;

PointToSegmentsPairMap
buildPointToOverlappingSegmentsMapping(
    const SegmentsGraph& graph,
    const SegmentIdsList& segmentIds,
    const SegmentIdsList& otherSegmentIds,
    double tolerance,
    double maxAngleBetweenSegments)
{
    PointToSegmentsPairMap result;
    for (auto id : segmentIds) {
        const Segment& seg = graph.segment(id);
        const gl::Segment2& segGeom = graph.segmentGeom(id);
        for (auto oid : otherSegmentIds) {
            if (id == oid) {
                continue;
            }
            const Segment& oseg = graph.segment(oid);
            const gl::Segment2& osegGeom = graph.segmentGeom(oid);
            if (segmentsOverlap(segGeom, osegGeom, tolerance, maxAngleBetweenSegments) ||
                segmentsOverlap(osegGeom, segGeom, tolerance, maxAngleBetweenSegments))
            {
                SegmentIdsPair segmentIdsPair = {id, oid};
                result[seg.startPointId].push_back(segmentIdsPair);
                result[seg.endPointId].push_back(segmentIdsPair);
                result[oseg.startPointId].push_back(segmentIdsPair);
                result[oseg.endPointId].push_back(segmentIdsPair);
                break;
            }
        }
    }
    return result;
}

boost::optional<PointId>
findMergePoint(
    const SegmentsGraph& graph, const Point& point, const Segment& seg,
    double tolerance)
{
    if (seg.startPointId == point.id || seg.endPointId == point.id) {
        return boost::none;
    }
    const gl::Segment2& segGeom = graph.segmentGeom(seg.id);
    const double projFactor = gl::projectionFactor(segGeom, point.pos);
    const double epsFactor = tolerance / gl::length(segGeom);
    if (std::fabs(projFactor) < epsFactor) {
        return seg.startPointId;
    } else if (std::fabs(1 - projFactor) < epsFactor) {
        return seg.endPointId;
    }
    return boost::none;
}

typedef std::map<SegmentId, SegmentIdsList::iterator> SegmentIdToListItMap;

SegmentIdToListItMap
buildSegmentListIteratorsMap(SegmentIdsList& ids)
{
    SegmentIdToListItMap result;
    for (auto it = ids.begin(); it != ids.end(); ++it) {
        REQUIRE(result.insert({*it, it}).second,
            "Segment " << *it << " found twice in list");
    }
    return result;
}

} // namespace

void
SegmentsOverlapper::Impl::doBeforeProcessing()
{
    PointToSegmentsPairMap pointToSegmentsMapping =
        buildPointToOverlappingSegmentsMapping(
            graph_, segmentIds_, otherSegmentIds_,
            tolerance_, maxAngleBetweenSegments_);

    SegmentIdToListItMap idsMap1 = buildSegmentListIteratorsMap(segmentIds_);
    SegmentIdToListItMap idsMap2 = buildSegmentListIteratorsMap(otherSegmentIds_);
    SegmentIdsSet removedSegmentIds;

    for (const auto& mapEl : pointToSegmentsMapping) {
        const PointId pointId = mapEl.first;
        const Point& point = graph_.point(pointId);
        if (point.nodeId) {
            continue;
        }
        for (const SegmentIdsPair& segments : mapEl.second) {
            //make_optional prevents "maybe-uninitialized" warning
            auto mergePointId = boost::make_optional(false, PointId());
            if (!removedSegmentIds.count(segments.segmentId1)) {
                const Segment& seg1 = graph_.segment(segments.segmentId1);
                mergePointId = findMergePoint(graph_, point, seg1, tolerance_);
            }
            if (!mergePointId && !removedSegmentIds.count(segments.segmentId2)) {
                const Segment& seg2 = graph_.segment(segments.segmentId2);
                mergePointId = findMergePoint(graph_, point, seg2, tolerance_);
            }
            if (mergePointId) {
                for (auto segId : graph_.sharedSegmentIds(point.id, *mergePointId)) {
                    auto thisIt = idsMap1.find(segId);
                    if (thisIt != idsMap1.end()) {
                        segmentIds_.erase(thisIt->second);
                    }
                    auto otherIt = idsMap2.find(segId);
                    if (otherIt != idsMap2.end()) {
                        otherSegmentIds_.erase(otherIt->second);
                    }
                    graph_.removeSegment(segId);
                    removedSegmentIds.insert(segId);
                }
                graph_.mergePoints(point.id, *mergePointId);
                break;
            }
        }
    }
    removeSnags();
    removeDuplicatedSegments();
}

void
SegmentsOverlapper::Impl::doAfterProcessing()
{
    removeSnags();
    removeDuplicatedSegments();
}

SegmentsProcessingResult
processOverlaps(SegmentsGraph& graph,
    const SegmentIdsList& segmentIds1, const SegmentIdsList& segmentIds2,
    double tolerance, double maxAngleBetweenSegments)
{
    SegmentsOverlapper overlapper(tolerance, maxAngleBetweenSegments);
    return overlapper(graph, segmentIds1, segmentIds2);
}

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