#include "geometry.h"

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

namespace maps::mrc::gen_targets {

namespace {
const double POINT_SIZE = 0.1;
}

bool angleInRange(double angle, std::pair<double, double> range)
{
    if (range.first < range.second) {
        if (angle >= range.first && angle < range.second) {
            return true;
        }
    } else {
        if (angle >= range.first || angle <  range.second) {
            return true;
        }
    }
    return false;
}

// we need to check spatial relation between a polygon and a point
// but geolib3 has optimized spatial relation function only for
// two polygons
geolib3::Polygon2 generateSmallPolygonFromPoint(geolib3::Point2 point)
{
    return geolib3::Polygon2(geolib3::PointsVector{
            fastGeoShift(point, geolib3::Vector2(-POINT_SIZE, 0.0)),
            fastGeoShift(point, geolib3::Vector2(0.0, POINT_SIZE)),
            fastGeoShift(point, geolib3::Vector2(POINT_SIZE, 0.0)),
            fastGeoShift(point, geolib3::Vector2(0.0, -POINT_SIZE))});
}

bool pointIsInsidePolygon(const geolib3::Point2 point,
                          const geolib3::PreparedPolygon2& polygon,
                          const geolib3::BoundingBox& polygonBbox)
{
    return geolib3::spatialRelation(polygonBbox,
                                    point,
                                    geolib3::SpatialRelation::Intersects)
        && geolib3::spatialRelation(polygon,
                                    generateSmallPolygonFromPoint(point),
                                    geolib3::SpatialRelation::Intersects);
}

geolib3::Point2 getPolylineMiddlePoint(const geolib3::Polyline2& polyline)
{
    double curLength = 0;
    double halfLength = geolib3::geoLength(polyline) / 2.0;
    for (const auto& segment : polyline.segments()) {
        curLength += geolib3::geoLength(segment);
        if (curLength >= halfLength) {
            return segment.pointByPosition(
                1.0 - (curLength - halfLength) / curLength);
        }
    }
    throw maps::Exception("Can't get polyline middle point");
}

// @brief returns true if at least half of the edge is inside the polygon
bool polylineIsMostlyInsidePolygon(const geolib3::Polyline2& polyline,
                                   const geolib3::PreparedPolygon2& polygon,
                                   const geolib3::BoundingBox& polygonBbox)
{
    int pointsInside = 0;

    if (pointIsInsidePolygon(polyline.points().front(), polygon, polygonBbox)) {
        pointsInside++;
    }
    if (pointIsInsidePolygon(polyline.points().back(), polygon, polygonBbox)) {
        pointsInside++;
    }

    if (pointsInside == 2) {
        return true;
    } else if (pointsInside == 0) {
        return false;
    }

    if (pointIsInsidePolygon(
            getPolylineMiddlePoint(polyline), polygon, polygonBbox)) {
        return true;
    } else {
        return false;
    }
}

bool turnIsRight(const geolib3::Direction2 direction1, const geolib3::Direction2 direction2)
{
    double angle = geolib3::signedAngle(direction1, direction2);
    if (angle >= -M_PI * 0.75 && angle < -M_PI * 0.25) {
        return true;
    } else {
        return false;
    }
}

bool turnIsLeft(const geolib3::Direction2 direction1, const geolib3::Direction2 direction2)
{
    double angle = geolib3::signedAngle(direction1, direction2);
    if (angle >= M_PI * 0.25 && angle < M_PI * 0.75) {
        return true;
    } else {
        return false;
    }
}

bool turnIsUTurn(const geolib3::Direction2 direction1, const geolib3::Direction2 direction2)
{
    double angle = geolib3::signedAngle(direction1, direction2);
    if (angle < -M_PI * 0.75 || angle >= M_PI * 0.75) {
        return true;
    } else {
        return false;
    }
}

bool turnIsStraight(const geolib3::Direction2 direction1, const geolib3::Direction2 direction2)
{
    double angle = geolib3::signedAngle(direction1, direction2);
    if (angle >= -M_PI * 0.25 && angle < M_PI * 0.25) {
        return true;
    } else {
        return false;
    }
}

} // namespace maps::mrc::gen_targets
