#include <maps/wikimap/mapspro/services/autocart/libs/post_processing/include/align_along_road.h>
#include <maps/wikimap/mapspro/services/autocart/libs/geometry/include/polygon_processing.h>

#include <maps/libs/geolib/include/polygon.h>
#include <maps/libs/geolib/include/spatial_relation.h>
#include <maps/libs/geolib/include/intersection.h>
#include <maps/libs/geolib/include/vector.h>
#include <maps/libs/geolib/include/distance.h>
#include <maps/libs/geolib/include/convex_hull.h>
#include <maps/libs/geolib/include/segment.h>
#include <maps/libs/geolib/include/line.h>
#include <maps/libs/geolib/include/direction.h>
#include <maps/libs/geolib/include/bounding_box.h>
#include <maps/libs/geolib/include/static_geometry_searcher.h>
#include <maps/libs/geolib/include/conversion_geos.h>
#include <maps/libs/geolib/include/projection.h>

#include <maps/libs/common/include/exception.h>

#include <opencv2/opencv.hpp>

#include <contrib/libs/geos/include/geos/geom/Geometry.h>
#include <contrib/libs/geos/include/geos/geom/Polygon.h>
#include <contrib/libs/geos/include/geos/geom/LineString.h>

#include <vector>
#include <cmath>
#include <utility>
#include <algorithm>
#include <limits>
#include <memory>
#include <optional>

namespace maps {
namespace wiki {
namespace autocart {

namespace {

/**
 * @brief Строит прямоугольник, окаймляющий отрезок.
 *      Построенный прямоугольник имеет размеры length(segment) x width
 * @param seg    отрезок, для которого находится прямоугольник
 * @param width  ширина окаймляющего прямоугольника
 * @return окаймляющий прямоугольник
 */
geolib3::Polygon2
getBufferedSegment(const geolib3::Segment2& seg, double width) {
    REQUIRE(width > 0, "Buffer width should be positive");
    std::vector<geolib3::Point2> bufferPts;
    geolib3::Vector2 normal = geolib3::rotateBy90(seg.vector(),
                                                  geolib3::Orientation::Clockwise);
    normal.normalize();

    bufferPts.push_back(seg[0] + width * normal / 2.);
    bufferPts.push_back(seg[0] - width * normal / 2.);
    bufferPts.push_back(seg[1] - width * normal / 2.);
    bufferPts.push_back(seg[1] + width * normal / 2.);

    return geolib3::Polygon2(bufferPts);
}

/**
 * @brief Разбивает ломанные линии на отрезки и для наждого отрезка находит
 *     окаймляющий прямоугольник.
 * @param lines  набор ломанных линий
 * @param width  ширина окаймляющих прямоугльников
 * @return набор окаймляющих прямогульников
 */
std::vector<std::vector<geolib3::Polygon2>>
getBufferedSegments(const std::vector<geolib3::Polyline2>& lines, double width) {
    std::vector<std::vector<geolib3::Polygon2>> bufferedSegments(lines.size());
    for (size_t i = 0; i < lines.size(); i++) {
        for (size_t j = 0; j < lines[i].segmentsNumber(); j++) {
            geolib3::Segment2 segment = lines[i].segmentAt(j);
            if (segment[0] != segment[1]) {
                geolib3::Polygon2 bufferedSegment = getBufferedSegment(segment,
                                                                       width);
                bufferedSegments[i].push_back(std::move(bufferedSegment));
            }
        }
    }
    return bufferedSegments;
}

/**
 * @brief Находит начальную точку ломанной линии
 * @param line ломанная линия
 * @return начальная точка ломанной линии
 */
geolib3::Point2 start(const geolib3::Polyline2& line) {
    return line.pointAt(0);
}

/**
 * @brief Находит конечную точку ломанной линии
 * @param line ломаная линия
 * @return конечная точка ломанной линии
 */
geolib3::Point2 end(const geolib3::Polyline2& line) {
    return line.pointAt(line.pointsNumber() - 1);
}

/**
 * @brief Вычисляет модуль косинуса угла между двумя прямыми
 * @param line1  прямая линия
 * @param line2  прямая линия
 * @return модуль косинуса угла
 */
double getAbsCos(const geolib3::Line2& line1, const geolib3::Line2& line2) {
    geolib3::Vector2 vec1(line1.vector());
    vec1.normalize();
    geolib3::Vector2 vec2(line2.vector());
    vec2.normalize();

    return abs(geolib3::innerProduct(vec1, vec2));
}

/**
 * @brief Вычисляет угол поворота одной прямой относительно другой.
 *     Направление положительного угла - против часовой стрелки.
 * @param mainLine  линия, относительно которой измеряется угол
 * @param line  линия, для которой вычисляется угол поворота
 * @return угол в градусах
 */
double getAngle(const geolib3::Line2& mainLine, const geolib3::Line2& line) {
    geolib3::Vector2 vec1(mainLine.vector());
    geolib3::Vector2 vec2(line.vector());

    if (geolib3::innerProduct(vec1, vec2) < 0) {
        vec1 *= -1.;
    }

    return 180 * geolib3::signedAngle(vec1, vec2) / CV_PI;
}

enum ConnectType {
    startToStart,
    startToEnd,
    endToStart,
    endToEnd
};

/**
 * @brief Соединяет две ломанные линии, если их концы достаточно близки.
 * @param mainLine  линия, к которой пытаемся присоединить другую линию.
 * @param addLine   линия, которую пытаемся присоединить к главной линии.
 * @param epsilon   максимальное расстояние между концами линий.
 * @return результат соединения двух линии, если они достаточно близки.
 */
std::optional<geolib3::Polyline2>
connectPolylines(const geolib3::Polyline2& mainLine,
                 const geolib3::Polyline2& addLine,
                 double epsilon) {
    std::vector<geolib3::Point2> pts(mainLine.pointsNumber());
    for (size_t i = 0; i < mainLine.pointsNumber(); i++) {
        pts[i] = mainLine.pointAt(i);
    }

    std::vector<geolib3::Point2> addPts(addLine.pointsNumber());
    for (size_t i = 0; i < addLine.pointsNumber(); i++) {
        addPts[i] = addLine.pointAt(i);
    }

    ConnectType connectType = ConnectType::startToStart;
    double minDist = geolib3::distance(pts.front(), addPts.front());

    double distEndToStart = geolib3::distance(pts.front(), addPts.back());
    if (distEndToStart < minDist) {
        minDist = distEndToStart;
        connectType = ConnectType::endToStart;
    }

    double distStartToEnd = geolib3::distance(pts.back(), addPts.front());
    if (distStartToEnd < minDist) {
        minDist = distStartToEnd;
        connectType = ConnectType::startToEnd;
    }

    double distEndToEnd = geolib3::distance(pts.back(), addPts.back());
    if (distEndToEnd < minDist) {
        minDist = distEndToEnd;
        connectType = ConnectType::endToEnd;
    }

    std::optional<geolib3::Polyline2> result;

    if (minDist >= epsilon) {
        return result;
    }

    switch (connectType) {
    case ConnectType::startToStart:
        pts.insert(pts.begin(), addPts.rbegin(), addPts.rend() - 1);
        break;
    case ConnectType::startToEnd:
        pts.insert(pts.end(), addPts.begin() + 1, addPts.end());
        break;

    case ConnectType::endToStart:
        pts.insert(pts.begin(), addPts.begin(), addPts.end() - 1);
        break;

    case ConnectType::endToEnd:
        pts.insert(pts.end(), addPts.rbegin() + 1, addPts.rend());
        break;
    }

    result = geolib3::Polyline2(pts);
    return result;
}

/**
 * @brief Вычисляет угол между полигоном здания и ломанной линией.
 *     Если линия пересекает полигон, то угол между полигоном и линией равняется
 *     минимальному по модулю углу среди углов между прямой, проходящей через
 *     концы ломанной линии, и прямыми, параллельными пересеченным сторонам
 *     полигона.
 *     Если линия не пересекает полигон, то угол между полигоном и линией
 *     равняется минимальному по модулю углу между прямой, проходящей через
 *     концы ломанной линии, и прямыми, проходящими через стороны полигона,
 *     которые проходят через ближающую к ломанной линии точку полигона.
 * @param bld   полигон здания
 * @param line  ломанная линия
 * @return угол в градусах
 */
double getAngle(const geolib3::Polygon2& bld, const geolib3::Polyline2& line) {
    size_t ptsCnt = bld.pointsNumber();
    if (geolib3::spatialRelation(bld, line,
                                 geolib3::SpatialRelation::Intersects)) {
        std::vector<geolib3::Segment2> intersectSegemnts;
        for (size_t i = 0; i < ptsCnt; i++) {
            geolib3::Point2 pt0 = bld.pointAt(i);
            geolib3::Point2 pt1 = bld.pointAt((i + 1) % ptsCnt);
            if (pt0 == pt1) {
                continue;
            }

            geolib3::Segment2 segment(pt0, pt1);
            for (size_t j = 0; j < line.segmentsNumber(); j++) {
                if (geolib3::spatialRelation(line.segmentAt(j), segment,
                                             geolib3::SpatialRelation::Intersects)) {
                    intersectSegemnts.push_back(std::move(segment));
                    break;
                }
            }
        }

        double minAngle = std::numeric_limits<double>::max();
        geolib3::Line2 mainLine(start(line), end(line));
        for (const auto& segment : intersectSegemnts) {
            geolib3::Line2 edgeLine(segment);
            double angle = getAngle(mainLine, edgeLine);
            if (fabs(angle) < fabs(minAngle)) {
                minAngle = angle;
            }
        }
        return minAngle;
    } else {
        size_t ptIndex;
        double minDist = std::numeric_limits<double>::max();
        for (size_t i = 0; i < ptsCnt; i++) {
            double dist = geolib3::distance(bld.pointAt(i), line);
            if (dist < minDist) {
                minDist = dist;
                ptIndex = i;
            }
        }
        geolib3::Point2 closestPt = bld.pointAt(ptIndex);

        geolib3::Point2 nextPt;
        for (size_t i = 1; i < ptsCnt; i++) {
            if (closestPt != bld.pointAt((ptIndex + i) % ptsCnt)) {
                nextPt = bld.pointAt((ptIndex + i) % ptsCnt);
            }
        }

        geolib3::Point2 prevPt;
        for (size_t i = 1; i < ptsCnt; i++) {
            if (closestPt != bld.pointAt((ptsCnt + ptIndex - i) % ptsCnt)) {
                prevPt = bld.pointAt((ptsCnt + ptIndex - i) % ptsCnt);
            }
        }

        geolib3::Line2 mainLine(start(line), end(line));
        geolib3::Line2 prevEdgeLine(prevPt, closestPt);
        double prevAngle = getAngle(mainLine, prevEdgeLine);
        geolib3::Line2 nextEdgeLine(closestPt, nextPt);
        double nextAngle = getAngle(mainLine, nextEdgeLine);
        if (fabs(prevAngle) < fabs(nextAngle)) {
            return prevAngle;
        } else {
            return nextAngle;
        }
    }
}

/**
 * @brief Проверяет наличие у полигона ребра, для которого в наборе дорог
 *     найдется такая дорога, что угол между этим ребром и прямой, проходящей
 *     через концы ломанной линии дороги, меньше некоторого порогового значения.
 *     Проверка выполняется по всем дорогам, кроме дороги с заданным индексом.
 * @param bld                полигон здания
 * @param roads              набор ломанных линий, описывающих дороги
 * @param angleEpsDegree     максимальное отклонение угла ребра от угла дороги
 * @param excludedLineIndex  индекс дороги, которая не участвует в поиске
 * @return наличие такого ребра
 */
bool haveAlignEdge(const geolib3::Polygon2& bld,
                   const std::vector<geolib3::Polyline2>& roads,
                   double angleEpsDegree,
                   size_t excludedLineIndex) {
    for (size_t i = 0; i < bld.pointsNumber(); i++) {
        geolib3::Point2 pt0 = bld.pointAt(i);
        geolib3::Point2 pt1 = bld.pointAt((i + 1) % bld.pointsNumber());
        if (pt0 == pt1) {
            continue;
        }

        geolib3::Line2 edgeLine(pt0, pt1);
        for (size_t j = 0; j < roads.size(); j++) {
            if (j == excludedLineIndex) {
                continue;
            }
            geolib3::Line2 tmpLine(start(roads[j]), end(roads[j]));
            double angle = getAngle(tmpLine, edgeLine);
            if (fabs(angle) < angleEpsDegree) {
                return true;
            }
        }
    }
    return false;
}

/**
 * @brief Выравнивает полигон здания относительно, проекции на ближайщую дорогу.
 *     Если здание угол поворота здания относительно какой-то дороги уже
 *     достаточно мал, то выравнивание не производится. Если для выравнивания
 *     относительно ближайщей дороги надо совершить слишком большой поворот, то
 *     выравнивание не производится.
 * @param bld      полигон здания
 * @param prRoads  проекции полигона на дороги
 * @param params   параметры выравнивание
 * @return результат выравнивания.
 */
geolib3::Polygon2
alignAlongRoadsProjection(const geolib3::Polygon2& bld,
                          const std::vector<geolib3::Polyline2>& prRoads,
                          const AlignAlongRoadsParams& params) {
    size_t closestRoadIndex;
    double minDist = std::numeric_limits<double>::max();
    for (size_t i = 0; i < prRoads.size(); i++) {
        double dist = geolib3::distance(bld, prRoads[i]);
        if (dist < minDist) {
            closestRoadIndex = i;
            minDist = dist;
        }
    }

    if (haveAlignEdge(bld, prRoads,
                      params.alignEdgeAngleEpsDegree,
                      closestRoadIndex)) {
        return bld;
    }

    geolib3::Polyline2 closestRoad = prRoads[closestRoadIndex];
    double angleDegree = getAngle(bld, closestRoad);
    if (fabs(angleDegree) < params.maxAngleRotationDegree) {
        return rotatePolygon(bld, -angleDegree);
    } else {
        return bld;
    }
}

/**
 * @brief Находит проекции полигона на близкие к нему ломанные линии.
 * @param bld       полигон здания
 * @param roads     набор ломанных линий дорог
 * @param searcher  объект для поиска близких дорог
 * @param params    параметры для поиска ближайщих дорог
 * @return  набор проецкций на отрезки ломанных линий близких дорог
 */
std::vector<geolib3::Segment2>
projectBldToCloseRoads(const geolib3::Polygon2& bld,
                       const std::vector<geolib3::Polyline2>& roads,
                       const geolib3::StaticGeometrySearcher<geolib3::Polygon2, size_t>& searcher,
                       const AlignAlongRoadsParams& params) {
    std::vector<geolib3::Segment2> prSegments;
    auto searchResult = searcher.find(bld.boundingBox());
    for (auto it = searchResult.first; it != searchResult.second; it++) {
        const geolib3::Polyline2& road = roads[it->value()];
        if (geolib3::distance(bld, road) > params.maxDistToRoad) {
            continue;
        }

        for (size_t i = 0; i < road.segmentsNumber(); i++) {
            std::optional<geolib3::Segment2> prSegment;
            prSegment = project(bld, road.segmentAt(i));
            if (prSegment) {
                prSegments.emplace_back(*prSegment);
            }
        }
    }
    return prSegments;
}

}  // namespace

/**
 * @brief Находит проекцию полигона на отрезок прямой.
 * @param polygon  полигон, проецируемый на отрезок
 * @param segment  отрезок, на который проецируется полигон
 * @return результат проекции, если такая есть.
 */
std::optional<geolib3::Segment2> project(const geolib3::Polygon2& polygon,
                                         const geolib3::Segment2& segment) {
    geolib3::Line2 line(segment);
    geolib3::Vector2 vec = segment.vector();
    double sqrLength = std::pow(geolib3::length(vec), 2);
    double minPrCoeff = std::numeric_limits<double>::max();
    double maxPrCoeff = std::numeric_limits<double>::lowest();
    for (size_t i = 0; i < polygon.pointsNumber(); i++) {
        geolib3::Point2 pt = geolib3::projection(line, polygon.pointAt(i));
        geolib3::Vector2 vecToPt = geolib3::Segment2(segment[0], pt).vector();
        double prCoeff = geolib3::innerProduct(vec, vecToPt) / sqrLength;
        minPrCoeff = std::min(minPrCoeff, prCoeff);
        maxPrCoeff = std::max(maxPrCoeff, prCoeff);
    }

    if (minPrCoeff > 1 - DBL_EPSILON || maxPrCoeff < DBL_EPSILON) {
        return std::nullopt;
    } else {
        minPrCoeff = std::max(0., minPrCoeff);
        maxPrCoeff = std::min(1., maxPrCoeff);
        return geolib3::Segment2(segment[0] + minPrCoeff * vec,
                segment[0] + maxPrCoeff * vec);
    }
}

/**
 * @brief Проверяет ломанную линию на степень кривизны.
 *    Если ни одна из точек ломанной линии не отклоняется от прямой, проходящей
 *    через концы этой ломанной линии, больше, чем на epsilon * длинну отрезка
 *    между концами ломанной, то ломанная линия считается прямой.
 * @param polyline  ломанная линия
 * @param epsilon   доля от длинны отрезка
 * @return результат проверки
 */
bool isStraightLine(const geolib3::Polyline2& polyline, double epsilon) {
    if (!isSimple(polyline)) {
        return false;
    } else if (polyline.pointsNumber() == 2) {
        return true;
    } else if (start(polyline) == end(polyline)) {
        return false;
    } else {
        double length = geolib3::distance(start(polyline), end(polyline));
        geolib3::Line2 line(start(polyline), end(polyline));
        for (size_t i = 1; i < polyline.pointsNumber() - 1; i++) {
            geolib3::Point2 pt = polyline.pointAt(i);
            if (geolib3::distance(line, pt) > length * epsilon) {
                return false;
            }
        }

        return true;
    }
}

/**
 * @brief Соединяет набор отрезков в ломанные линии. Ломанные линии объединяются,
 *        если:
 *         1) Их концы достаточно близки.
 *         2) Угол между прямыми, проходящими через концы этих ломанный линий,
 *            досточно мал.
 *         3) Полученная после объединения линия обладает малой кривизной (см.
 *            isStraightLine).
 * @param segments  надор отрезков для объединения
 * @param params    параметры объединения отрезков
 * @return набор объединенных ломанных линий.
 */
std::vector<geolib3::Polyline2>
connectSegments(const std::vector<geolib3::Segment2>& segments,
                const AlignAlongRoadsParams& params) {
    std::vector<geolib3::Polyline2> lines;
    for (const auto& segment : segments) {
        if (segment.start() != segment.end()) {
            lines.emplace_back(segment);
        }
    }

    for (int i = (int)lines.size() - 1; i >= 0; i--) {
        int connectedIndex = -1;
        geolib3::Polyline2 connectedLine;
        double maxAbsCos = 0.;
        for (int j = 0; j < i; j++) {
            std::optional<geolib3::Polyline2> newLine =
                    connectPolylines(lines[i], lines[j],
                                     params.maxConnectedRoadDist);
            if (newLine) {
                if (isStraightLine(*newLine, params.maxLineBendingPercent)) {
                    double absCos = getAbsCos(geolib3::Line2(start(lines[i]),
                                                             end(lines[i])),
                                              geolib3::Line2(start(lines[j]),
                                                             end(lines[j])));
                    if (absCos > maxAbsCos) {
                        maxAbsCos = absCos;
                        connectedLine = *newLine;
                        connectedIndex = j;
                    }
                }
            }
        }
        if (maxAbsCos < cos(M_PI * params.maxLineBendingAngleDegree / 180.)) {
            continue;
        }

        if (connectedIndex != -1) {
            lines[connectedIndex] = connectedLine;
            lines.erase(lines.begin() + i);
        }
    }

    return lines;
}

/**
 * @brief Выравнивает полигоны домов вдоль блидайщих дорог.
 * @param blds   набор полигонов для выравнивания.
 * @param roads  набор дорог, относительно которых происходит выравнивание.
 * @param params параметры выранивания
 * @return набор результирующих полигинов
 */
std::vector<geolib3::Polygon2>
alignAlongRoads(const std::vector<geolib3::Polygon2>& blds,
                const std::vector<geolib3::Polyline2>& roads,
                const AlignAlongRoadsParams& params) {
    std::vector<std::vector<geolib3::Polygon2>>
            bufferedRoadSegments = getBufferedSegments(roads,
                                                       params.maxDistToRoad);

    geolib3::StaticGeometrySearcher<geolib3::Polygon2, size_t> searcher;
    for (size_t i = 0; i < bufferedRoadSegments.size(); i++) {
        for (size_t j = 0; j < bufferedRoadSegments[i].size(); j++) {
            searcher.insert(&bufferedRoadSegments[i][j], i);
        }
    }
    searcher.build();

    std::vector<geolib3::Polygon2> result;
    result.reserve(blds.size());
    for (const auto& bld : blds) {
        std::vector<geolib3::Segment2>
                prSegments = projectBldToCloseRoads(bld, roads, searcher,
                                                    params);
        if (!prSegments.empty()) {
            std::vector<geolib3::Polyline2> projectLines;
            projectLines = connectSegments(prSegments, params);

            geolib3::Polygon2
                    alignedBld = alignAlongRoadsProjection(bld, projectLines,
                                                           params);
            result.push_back(std::move(alignedBld));
        } else {
            result.push_back(bld);
        }
    }

    return result;
}

}  // namespace autocart
}  // namespace wiki
}  // namespace maps
