#include "include/geometry.h"

#include <maps/libs/common/include/exception.h>
#include <maps/libs/geolib/include/polyline.h>
#include <maps/libs/geolib/include/intersection.h>

namespace maps::mrc::common::geometry {

namespace {

constexpr int sign(double x)
{
    return geolib3::sign(x, geolib3::EPS);
}

constexpr bool close(double a, double b)
{
    return sign(a - b) == 0;
}

} // namespace

SubSegment::SubSegment(double begin, double end)
    : begin_(begin)
    , end_(end)
{
    validate(begin, end);
}

SubSegment& SubSegment::setBegin(double begin)
{
    validate(begin, end_);
    begin_ = begin;
    return *this;
}

SubSegment& SubSegment::setEnd(double end)
{
    validate(begin_, end);
    end_ = end;
    return *this;
}

bool SubSegment::isDegenerate() const {
    return close(end_, begin_);
}

void SubSegment::validate(double begin, double end) const
{
    ASSERT(!(begin < MIN_REL_POS));
    ASSERT(!(end > MAX_REL_POS));
    ASSERT(sign(end - begin) >= 0);
}

bool operator==(const SubSegment& one, const SubSegment& other)
{
    return (close(one.begin(), other.begin())) &&
        (close(one.end(), other.end()));
}

std::ostream& operator<<(std::ostream& out, const SubSegment& segment)
{
    out << "(" << segment.begin() << ", " << segment.end() << ")";
    return out;
}

std::ostream& operator<<(std::ostream& out, const PolylinePosition& pos)
{
    out << "(" << pos.segmentIdx() << ", " << pos.segmentRelPosition() << ")";
    return out;
}

PolylinePosition::PolylinePosition(size_t segmentIdx, double segmentRelPosition)
: segmentIdx_(segmentIdx)
, segmentRelPosition_(segmentRelPosition)
{
    ASSERT(!(segmentRelPosition_ < MIN_REL_POS || segmentRelPosition_ > MAX_REL_POS));
}

bool operator<(const PolylinePosition& one, const PolylinePosition& other)
{
    if (one == other) {
        return false;
    }

    if (one.segmentIdx() == other.segmentIdx()) {
        return sign(one.segmentRelPosition() - other.segmentRelPosition()) == -1;
    }
    return one.segmentIdx() < other.segmentIdx();
}

bool operator>(const PolylinePosition& one, const PolylinePosition& other)
{
    return other < one;
}

bool operator==(const PolylinePosition& one, const PolylinePosition& other)
{
    if (one.segmentIdx() == other.segmentIdx()) {
        return close(one.segmentRelPosition(), other.segmentRelPosition());
    } else if (one.segmentIdx() == other.segmentIdx() + 1) {
        return close(other.segmentRelPosition(), MAX_REL_POS) &&
            close(one.segmentRelPosition(), MIN_REL_POS);
    } else if (other.segmentIdx() == one.segmentIdx() + 1) {
        return close(one.segmentRelPosition(), MAX_REL_POS) &&
            close(other.segmentRelPosition(), MIN_REL_POS);
    }

    return false;
}

bool operator!=(const PolylinePosition& one, const PolylinePosition& other)
{
    return !(one == other);
}

bool operator<=(const PolylinePosition& one, const PolylinePosition& other)
{
    return (one < other) || (one == other);
}

bool operator>=(const PolylinePosition& one, const PolylinePosition& other)
{
    return (one > other) || (one == other);
}

std::ostream& operator<<(std::ostream& out, const SubPolyline& subpolyline)
{
    out << "[" << subpolyline.begin() << ", " << subpolyline.end() << ")";
    return out;
}

SubPolyline::SubPolyline(PolylinePosition begin, PolylinePosition end)
: begin_(std::move(begin))
, end_(std::move(end))
{
    validate(begin_, end_);
}

SubPolyline& SubPolyline::setBegin(PolylinePosition begin)
{
    validate(begin, end_);
    begin_ = begin;
    return *this;
}

SubPolyline& SubPolyline::setEnd(PolylinePosition end)
{
    validate(begin_, end);
    end_ = end;
    return *this;
}

void SubPolyline::validate(PolylinePosition begin, PolylinePosition end) {
    REQUIRE(!(begin > end), "Begin " << begin << " can't be greater than end " << end);
}


bool precedes(const SubPolyline& one, const SubPolyline& other)
{
    return one.end() <= other.begin();
}

bool intersects(const SubPolyline& one, const SubPolyline& other)
{
    return (one.end() > other.begin()) && (one.begin() < other.end());
}

bool contains(const SubPolyline& one, const SubPolyline& other)
{
    return (one.begin() <= other.begin()) && (one.end() >= other.end());
}

SubPolyline intersection(const SubPolyline& one, const SubPolyline& other)
{
    ASSERT(intersects(one, other));
    return {std::max(one.begin(), other.begin()),
            std::min(one.end(), other.end())};
}

SubPolylines merge(SubPolylines subpolylines)
{
    if (subpolylines.empty()) {
        return {};
    }
    std::sort(subpolylines.begin(), subpolylines.end());
    SubPolylines result;

    SubPolyline current = subpolylines[0];
    for (size_t i = 1; i < subpolylines.size(); ++i) {
        auto other = subpolylines[i];
        if (other.begin() > current.end()) {
            result.push_back(current);
            current = other;
        } else {
            current.setEnd(std::max(current.end(), other.end()));
        }
    }
    result.push_back(current);
    return result;
}

} // maps::mrc::common::geometry
