#include <maps/wikimap/mapspro/services/mrc/libs/db/include/visibility.h>
#include <maps/wikimap/mapspro/services/mrc/libs/db/include/common.h>

#include <maps/libs/common/include/math.h>
#include <maps/libs/geolib/include/common.h>
#include <maps/libs/geolib/include/distance.h>
#include <maps/libs/geolib/include/linear_ring.h>
#include <maps/libs/geolib/include/point.h>
#include <maps/libs/geolib/include/polygon.h>
#include <maps/libs/geolib/include/spatial_relation.h>
#include <maps/libs/geolib/include/vector.h>

/// use boost::geometry for difference algorithm
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/difference.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <algorithm>
#include <cmath>

namespace maps::mrc::db {
namespace {

constexpr double VISIBILITY_METERS = 30.;
constexpr double SIDE_SHIFT_METERS = 10.;

using BPoint = boost::geometry::model::d2::point_xy<double>;
using BLinestring = boost::geometry::model::linestring<BPoint>;
using BPolygon = boost::geometry::model::polygon<BPoint>;
using BRing = BPolygon::ring_type;

BPoint toBoost(const geolib3::Point2& point) {
    return BPoint(point.x(), point.y());
}

geolib3::Point2 fromBoost(const BPoint& point) {
    return geolib3::Point2(point.x(), point.y());
}

BLinestring toBoost(const geolib3::Segment2& segment) {
    auto list = {toBoost(segment.start()), toBoost(segment.end())};
    return {list.begin(), list.end()};
}

geolib3::Segment2 fromBoost(const BLinestring& line) {
    return geolib3::Segment2(fromBoost(line.front()), fromBoost(line.back()));
}

BRing toBoost(const geolib3::LinearRing2& ring) {
    BRing result;
    result.reserve(ring.pointsNumber() + 1);
    for (size_t i = 0; i < ring.pointsNumber(); ++i) {
        result.push_back(toBoost(ring.pointAt(i)));
    }
    result.push_back(toBoost(ring.pointAt(0)));
    return result;
}

BPolygon toBoost(const geolib3::Polygon2& polygon) {
    BPolygon result;
    result.outer() = toBoost(polygon.exteriorRing());
    result.inners().reserve(polygon.interiorRingsNumber());
    for (size_t i = 0; i < polygon.interiorRingsNumber(); ++i) {
        result.inners().push_back(toBoost(polygon.interiorRingAt(i)));
    }
    return result;
}


template <typename SegmentRange>
bool isVisible(
    const Ray& ray,
    const geolib3::Direction2& moveDirection,
    const SegmentRange& track)
{
    static constexpr geolib3::Radians MAX_DIRECTION_TO_SEGMENT_ANGLE = geolib3::PI * 0.4;
    const auto view = fieldOfView(ray);

    for (const auto& segment : track) {
        auto angle = geolib3::angleBetween(moveDirection,
                                           geolib3::Direction2(segment));

        if (angle < MAX_DIRECTION_TO_SEGMENT_ANGLE
            && geolib3::spatialRelation(view, segment, geolib3::Intersects)) {
            return true;
        }
    }
    return false;
}

Segments getUncovered(
    const geolib3::Polygon2& view,
    const geolib3::Direction2& moveDirection,
    const geolib3::Segment2& segment)
{
    auto angle = geolib3::angleBetween(moveDirection,
                                       geolib3::Direction2(segment));
    if (angle >= geolib3::PI / 4) {
        return {segment};
    }

    if (!geolib3::spatialRelation(view, segment, geolib3::Intersects)) {
        return {segment};
    }

    std::vector<BLinestring> cuttings;
    boost::geometry::difference(toBoost(segment), toBoost(view), cuttings);

    Segments result;
    for (const auto& cutting : cuttings) {
        if (boost::geometry::length(cutting) > geolib3::EPS) {
            result.push_back(fromBoost(cutting));
        }
    }
    return result;
}

template <typename SegmentRange>
Segments getUncovered(
    const Ray& ray,
    const geolib3::Direction2& moveDirection,
    const SegmentRange& track)
{
    db::Segments result;
    auto fov = fieldOfView(ray);
    for (const auto& segment : track) {
        auto cuttings = getUncovered(fov, moveDirection, segment);
        result.insert(result.end(), cuttings.begin(), cuttings.end());
    }
    return result;
}

} // namespace

Ray getRay(geolib3::Point2 pos,
           geolib3::Direction2 moveDirection,
           CameraDeviation cameraDeviation)
{
    Ray ray{pos, moveDirection};
    switch (cameraDeviation) {
        case CameraDeviation::Front:
            break;
        case CameraDeviation::Right:
            ray.direction = moveDirection - geolib3::Direction2(geolib3::PI / 2);
            ray.pos = geolib3::fastGeoShift(
                    ray.pos, SIDE_SHIFT_METERS * (-ray.direction).vector());
            break;
        case CameraDeviation::Left:
            ray.direction = moveDirection + geolib3::Direction2(geolib3::PI / 2);
            ray.pos = geolib3::fastGeoShift(
                    ray.pos, SIDE_SHIFT_METERS * (-ray.direction).vector());
            break;
        case CameraDeviation::Back:
            ray.direction = -moveDirection;
            break;
    }
    return ray;
}

Ray getRay(const Feature& feature)
{
    return getRay(feature.geodeticPos(),
                  direction(feature),
                  feature.cameraDeviation());
}

geolib3::Polygon2 fieldOfView(const Ray& ray)
{
    auto a = ray.pos;
    auto unit = ray.direction.vector();
    auto meters = VISIBILITY_METERS * unit;
    auto h = geolib3::fastGeoShift(a, meters);
    auto b = geolib3::fastGeoShift(
        h, geolib3::rotateBy90(meters, geolib3::Counterclockwise));
    auto c = geolib3::fastGeoShift(
        h, geolib3::rotateBy90(meters, geolib3::Clockwise));
    return geolib3::Polygon2{geolib3::PointsVector{a, b, c}};
}


geolib3::Direction2 direction(const Feature& feature)
{
    return geolib3::Direction2(feature.heading());
}

bool isVisible(const Feature& feature, const geolib3::Polyline2& track)
{
    const Ray ray = getRay(feature);
    return isVisible(ray, direction(feature), track.segments());
}

bool isVisible(const Feature& feature, const Segments& track)
{
    const Ray ray = getRay(feature);
    return isVisible(ray, direction(feature), track);
}

bool isVisible(
    const geolib3::Point2& pos,
    const geolib3::Direction2& moveDirection,
    const CameraDeviation cameraDeviation,
    const geolib3::Polyline2& track)
{
    const Ray ray = getRay(pos, moveDirection, cameraDeviation);
    return isVisible(ray, moveDirection, track.segments());
}

bool isVisible(
    const geolib3::Point2& pos,
    const geolib3::Direction2& moveDirection,
    const CameraDeviation cameraDeviation,
    const Segments& segments)
{
    const Ray ray = getRay(pos, moveDirection, cameraDeviation);
    return isVisible(ray, moveDirection, segments);
}

geolib3::BoundingBox expandBbox(const geolib3::BoundingBox& bbox, double meters)
{
    return {geolib3::fastGeoShift(bbox.lowerCorner(), {-meters, -meters}),
            geolib3::fastGeoShift(bbox.upperCorner(), {+meters, +meters})};
}

geolib3::BoundingBox addVisibilityMargins(const geolib3::BoundingBox& bbox)
{
    static const auto MAX_VISIBILITY_METERS = VISIBILITY_METERS * sqrt(2);
    return expandBbox(bbox, MAX_VISIBILITY_METERS);
}

double length(const Segments& segments)
{
    return maps::common::sumKahan(segments,
        [](const geolib3::Segment2& segment) {
            return geolib3::length(segment);
        });
}

double geoLength(const Segments& segments)
{
    return maps::common::sumKahan(segments,
        [](const geolib3::Segment2& segment) {
            return geolib3::geoLength(segment);
        });
}

Segments getUncovered(
    const geolib3::Point2& pos,
    const geolib3::Direction2& moveDirection,
    const CameraDeviation cameraDeviation,
    const Segments& segments)
{
    const Ray ray = getRay(pos, moveDirection, cameraDeviation);
    return getUncovered(ray, moveDirection, segments);
}

Segments getUncovered(const Feature& feature, const db::Segments& segments)
{
    const Ray ray = getRay(feature);
    return getUncovered(ray, direction(feature), segments);
}

} // namespace maps::mrc::db
