#include "util.h"

#include <maps/libs/geolib/include/closest_point.h>
#include <maps/libs/geolib/include/segment.h>


namespace maps {
namespace wiki {
namespace topo {
namespace geom {


using namespace geolib3;


bool isClosed(const Polyline2& polyline, double tolerance) {
    const PointsVector& points = polyline.points();
    return (points.size() >= 3) && near(points.front(), points.back(), tolerance);
}

SnapPolyline supplementPolylineWithPoints(const SnapPolyline& polyline,
    SnapPointVector points, double tolerance)
{
    auto metric = [&polyline](const SnapPoint& point) -> double
    {
        const Point2 projection = closestPoint(polyline.geom(), point.geom());
        return distanceAlongFromStart(polyline.geom(), projection);
    };

    const std::vector<double> distances = order(points, metric);

    SnapPointVector result;
    size_t iMajor = 0;
    size_t iMinor = 0;
    double distance = 0.0;
    while ((iMajor < polyline.size()) && (iMinor < points.size())) {
        if (near(points[iMinor].geom(), polyline[iMajor].geom(), tolerance)) {
            ++iMinor;
        } else if (distances[iMinor] < distance) {
            result.push_back(points[iMinor]);
            ++iMinor;
        } else {
            result.push_back(polyline[iMajor]);
            distance += length(polyline.geom().segmentAt(iMajor));
            ++iMajor;
        }
    }

    result.insert(
        result.end(),
        polyline.points().begin() + iMajor, polyline.points().end()
    );
    result.insert(
        result.end(),
        points.begin() + iMinor, points.end()
    );

    return SnapPolyline{std::move(result)};
}

Point2 center(const Segment2& segment)
{
    return segment.midpoint();
}

Point2 center(const TwoPoints& pair)
{
    return Point2 {
        (pair.first.x() + pair.second.x()) / 2,
        (pair.first.y() + pair.second.y()) / 2
    };
}

Point2 center(const Polyline2& polyline)
{
    return polyline.segmentAt(
        polyline.segmentsNumber() / 2
    ).midpoint();
}

Polyline2 reverse(const Polyline2& polyline)
{
    return {
        polyline.points().rbegin(),
        polyline.points().rend()
    };
}

} // namespace maps::wiki::topo::geom
} // namespace maps::wiki::topo
} // namespace maps::wiki
} // namespace maps
