#include "utils.h"

#include <yandex/maps/proto/common2/response.pb.h>
#include <yandex/maps/proto/driving/route.pb.h>
#include <yandex/maps/geolib3/proto.h>

#include <maps/libs/common/include/exception.h>
#include <maps/libs/http/include/http.h>
#include <maps/libs/log8/include/log8.h>
#include <maps/libs/xml/include/xml.h>
#include <maps/libs/geolib/include/closest_point.h>
#include <maps/libs/geolib/include/conversion.h>
#include <maps/libs/geolib/include/contains.h>
#include <maps/libs/geolib/include/distance.h>

#include <boost/optional.hpp>
#include <boost/format.hpp>

#include <algorithm>
#include <iterator>
#include <iostream>
#include <utility>
#include <sstream>
#include <fstream>

namespace maps {
namespace mrc {
namespace tasks_analyzer {

namespace ymp = yandex::maps::proto;

namespace {

std::string formatPoint(const geolib3::Point2& point)
{
    return boost::str(
        boost::format("%.6f,%.6f") % point.x() % point.y());
}

std::string pointsToRll(boost::iterator_range<geolib3::PointsVector::const_iterator> range)
{
    const char SEPARATOR = '~';
    std::string text;

    for (auto pointIt : range)
    {
        if (!text.empty()) {
            text.push_back(SEPARATOR);
        }
        text += formatPoint(pointIt);
    }
    return text;
}

std::string makeViaPoints(int from, int to)
{
    const char SEPARATOR = '~';
    std::string text;
    for (int i = from; i < to; ++i)
    {
        if (!text.empty()) {
            text.push_back(SEPARATOR);
        }
        text += std::to_string(i);
    }
    return text;
}

} // namespace


void formatInEasyview(std::ostream& out, const geolib3::Point2& point, const std::string& label) {
    out << point.x() << " " << point.y();
    if (!label.empty()) {
        out << " " << label;
    }
    out << std::endl;
}

void formatInEasyview(std::ostream& out, const geolib3::Polyline2& polyline, const std::string& label)
{
    bool placeSpace = false;
    for(size_t idx = 0; idx < polyline.pointsNumber(); ++idx) {
        if (placeSpace) {
            out << " ";
        }
        placeSpace = true;
        const auto& point = polyline.pointAt(idx);
        out << point.x() << " " << point.y();
    }

    if (!label.empty()) {
        out << " " << label;
    }
    out << std::endl;
}


geolib3::PolylinesVector makeSortedPolylines(const db::ugc::Targets& targets)
{
    using IndexedPolyline = std::pair<size_t, geolib3::Polyline2>;
    std::vector<IndexedPolyline> indexedPolylines;
    indexedPolylines.reserve(targets.size() * 2);

    for (const auto& target : targets) {
        if (target.direction() == mrc::db::ugc::Direction::Forward ||
            target.direction() == mrc::db::ugc::Direction::Bidirectional)
        {

            REQUIRE(target.forwardPos(), "target position is not defined");
            indexedPolylines.emplace_back(*target.forwardPos(), target.geodeticGeom());
        }
        if (target.direction() == mrc::db::ugc::Direction::Backward ||
            target.direction() == mrc::db::ugc::Direction::Bidirectional)
        {
            REQUIRE(target.backwardPos(), "target position is not defined");
            auto geom = target.geodeticGeom();
            geom.reverse();
            indexedPolylines.emplace_back(*target.backwardPos(), geom);
        }
    }

    std::sort(indexedPolylines.begin(), indexedPolylines.end(),
             [](const IndexedPolyline& a, const IndexedPolyline& b)
             {
                 return a.first < b.first;
             }
    );

    geolib3::PolylinesVector result;
    result.reserve(indexedPolylines.size());

    for (auto indexedPolyline : indexedPolylines) {
        result.push_back(std::move(indexedPolyline.second));
    }
    return result;
}


geolib3::PointsVector
makeRouteViaPointsForTaskTargets(const geolib3::PolylinesVector& polylines)
{
    geolib3::PointsVector result;
    result.reserve(polylines.size() * 2);

    for (const auto& polyline : polylines) {
        if (geolib3::geoLength(polyline) < 10.) {
            continue;
        }

        auto shiftedGeom = tryShiftGeom(polyline, 3.);
        // if (result.empty() ||
        //     geolib3::compare(result.back(), polyline.pointAt(0)) != 0)
        // {
        //     // result.push_back(polyline.pointAt(0));



        //     result.push_back(shiftedGeom.segmentAt(0).pointByPosition(0.3));
        //     // result.push_back(shiftedGeom.segmentAt(shiftedGeom.segmentsNumber() - 1).pointByPosition(0.7));
        // }

        result.push_back(
            shiftedGeom.segmentAt(shiftedGeom.segmentsNumber() / 2)
                .pointByPosition(0.5)
        );
    }

    return result;
}


std::string makeRouteRequest(
    http::Client& client,
    const std::string& endpoint,
    boost::iterator_range<geolib3::PointsVector::const_iterator> range)
{

    http::Request request(client, http::GET, endpoint + "/v2/route");
    auto rll = pointsToRll(range);
    // INFO() << "rll=" << rll;
    request.addParam("rll", rll);

    auto pointsNumber = std::distance(range.begin(), range.end());

    if (pointsNumber > 2) {
        auto via = makeViaPoints(1, pointsNumber - 1);
        // INFO() << "via=" << via;
        request.addParam("via", via);
    }

    request.addParam("mode", "nojams"); // do not consider jams
    request.addParam("experimental_snap_to_special_struct_roads", "1");

    INFO() << "Perform request " << request.url();

    auto response = request.perform();

    REQUIRE(response.status() == 200,
        "Unexpected response status " << response.status());

    return response.readBody();
}

geolib3::PolylinesVector parseRouteResponse(const std::string& routeResponse)
{
    ymp::common2::response::Response protoResponse;
    REQUIRE(protoResponse.ParseFromString(TString(routeResponse)), "Failed to parse response");
    ASSERT(protoResponse.has_reply() && protoResponse.reply().geo_object_size() == 1);
    auto routeGeoObject = protoResponse.reply().geo_object(0);

    geolib3::PolylinesVector result;

    for (const auto& geoObject : routeGeoObject.geo_object()) {
        for (const auto& geometry: geoObject.geometry()) {
            result.push_back(maps::geolib3::proto::decode(geometry.polyline()));
            INFO() << "length " << geolib3::geoLength(maps::geolib3::proto::decode(geometry.polyline()));
        }
    }
    return result;
}

double calcRouteRatio(const db::ugc::Targets& taskTargets,
                     const geolib3::PolylinesVector& routes)
{
    double targetsLength = 0.;
    for (const auto& target : taskTargets)
    {
        auto targetLength = geolib3::geoLength(target.geodeticGeom());
        targetsLength +=
            target.direction() == db::ugc::Direction::Bidirectional
                ? 2 * targetLength
                : targetLength;
    }

    INFO() << "Total targets length: " << targetsLength << " meters";

    double routeLength = 0;
    for (const auto& route : routes)
    {
        routeLength += geolib3::geoLength(route);
    }
    INFO() << "Total route length: " << routeLength << " meters";
    double ratio = routeLength / targetsLength;
    INFO() << "Ratio is " << ratio;
    return ratio;
}

geolib3::Polyline2 tryShiftGeom(const geolib3::Polyline2& polyline,
                               double shiftMeters)
{

    if (geolib3::sign(shiftMeters) == 0) {
        return polyline;
    }
    try {
        auto mercPolyline = geolib3::convertGeodeticToMercator(polyline);
        auto shiftedLine = geolib3::equidistant(mercPolyline, shiftMeters, geolib3::Clockwise);
        return geolib3::convertMercatorToGeodetic(shiftedLine);
    } catch (const maps::Exception& ex) {
        ERROR() << "Failed to shift geom " << ex;
        return polyline;
    }
}

void savePolylinesToFile(const geolib3::PolylinesVector& polylines,
                         std::ostream& out,
                         double shiftMeters,
                         size_t from,
                         size_t points)
{
    size_t idx = 0;
    for(const auto& polyline : polylines)
    {
        auto shiftedPolyline = tryShiftGeom(polyline, shiftMeters);
        for (const auto& point : shiftedPolyline.points())
        {
            idx++;
            if (from && idx <= from) {
                continue;
            }

            out << point.x() << " " << point.y() << " ";
            if (points && idx - from > points) {
                out << std::endl;
                return;
            }
        }
        out << std::endl;
    }
    out << std::endl;
}


// true if route fragment is on plan
std::vector<std::pair<geolib3::Polyline2, bool>>
compareRoutes(const geolib3::PolylinesVector& plannedRoute,
                const geolib3::PolylinesVector& route)
{

    // constexpr int STEPS_LIMIT = 100;
    constexpr double ROUTE_DISTANCE_EPS = 10.;
    std::vector<std::pair<geolib3::Polyline2, bool>> comparisonResult;

    std::vector<geolib3::Point2> currentPart;
    bool onPlan = true;

    auto plannedRoutePartIt = plannedRoute.begin();
    size_t plannedRoutePointIdx = 0;
    geolib3::Point2 plannedPoint = plannedRoutePartIt->pointAt(plannedRoutePointIdx);

    auto routePartIt = route.begin();
    int goodRoutes = 0;
    int badRoutes = 0;

    double goodLength = 0.;
    double badLength = 0.;

    while(routePartIt != route.end()) {
        for(size_t segmentIdx = 0; segmentIdx < routePartIt->segmentsNumber(); ++segmentIdx)
        {
            auto segment = routePartIt->segmentAt(segmentIdx);
            bool containsAny = false;

            // INFO() << "geoDistance = " << geolib3::geoDistance(plannedPoint, segment);

            while(geolib3::geoDistance(plannedPoint, segment) < ROUTE_DISTANCE_EPS
                    && plannedRoutePartIt != plannedRoute.end())
            {
                containsAny = true;

                ++plannedRoutePointIdx;

                if (plannedRoutePointIdx >= plannedRoutePartIt->pointsNumber()) {
                    ++plannedRoutePartIt;
                    plannedRoutePointIdx = 0;
                }
                if (plannedRoutePartIt != plannedRoute.end()) {
                    plannedPoint = plannedRoutePartIt->pointAt(plannedRoutePointIdx);
                }
            }

            // INFO() << "segmentIdx = " << segmentIdx << " containsAny = " << containsAny;

            if ((segmentIdx + 1 == routePartIt->segmentsNumber() ||
                    containsAny != onPlan) && !currentPart.empty())
            {
                currentPart.push_back(segment.start());
                geolib3::Polyline2 polyline(std::move(currentPart));

                if (onPlan) {
                    ++goodRoutes;
                    goodLength += geolib3::geoLength(polyline);
                } else {
                    ++badRoutes;
                    badLength += geolib3::geoLength(polyline);
                }
                comparisonResult.emplace_back(std::move(polyline), onPlan);

                onPlan = containsAny;
                currentPart.clear();
            }

            currentPart.push_back(segment.start());
        }
        ++routePartIt;
    }


    INFO() << "good: " << goodRoutes << " routes " << goodLength << " m";
    INFO() << "bad: " << badRoutes << " routes " << badLength << " m";

    return comparisonResult;
}


void saveRoutesComparison(
    const std::vector<std::pair<geolib3::Polyline2, bool>>& routesComparison,
    std::ostream& out)
{
    out << "!pointstyle=green:blue:8\n";
    out << "!linestyle=green:4\n";
    for(const auto& routeComparison :  routesComparison) {
        if (routeComparison.second) {
            auto geom = routeComparison.first;
            geom = tryShiftGeom(geom, 10);
            // out << geom.pointAt(0).x() << " " << geom.pointAt(0).y() << std::endl;
            for (const auto& point : geom.points())
            {
                out << point.x() << " " << point.y() << " ";
            }
            out << std::endl;
        }

    }

    out << "!pointstyle=red:yellow:8\n";
    out << "!linestyle=red:4\n";
    for(const auto& routeComparison :  routesComparison) {
        if (!routeComparison.second) {
            auto geom = routeComparison.first;
            // geom = tryShiftGeom(geom, 10);
            // out << geom.pointAt(0).x() << " " << geom.pointAt(0).y() << std::endl;
            for (const auto& point : geom.points())
            {
                out << point.x() << " " << point.y() << " ";
            }
            out << std::endl;
        }
    }

}

void checkViaPointsPassingOrder(
        geolib3::PointsVector& viaPoints,
        geolib3::PolylinesVector& routes)
{
    constexpr int UNKNOWN_IDX = -1;
    std::vector<int> pointPos(viaPoints.size(), UNKNOWN_IDX);

    size_t seqNum = 0;
    for(const auto& route : routes) {
        for(const auto& segment : route.segments()) {
            for(size_t i = 0; i < viaPoints.size(); ++i) {
                if (pointPos[i] == UNKNOWN_IDX &&
                        geolib3::contains(segment, viaPoints[i]))
                {
                    pointPos[i] = seqNum;
                    if(i != seqNum) {
                        WARN() << "wrong pos of " << i << " via point: " << seqNum;
                    }
                    ++seqNum;
                }
            }
        }
    }
}


void saveViaPoints(geolib3::PointsVector& viaPoints, std::ostream& out, size_t limit)
{
    out << "!pointstyle=green:blue:4\n";
    for(size_t i = 0; i < viaPoints.size() && (!limit || i < limit); ++i)
    {
        out << viaPoints[i].x() << " " << viaPoints[i].y() << " " << i << std::endl;
    }
}


const geolib3::Point2 amortizedClosestPoint(const geolib3::Polyline2& polyline,
                                   const geolib3::Point2& point,
                                   const double eps)
{
    REQUIRE(polyline.pointsNumber() > 0, "Cannot find closestPoint of an empty polyline");

    if (polyline.pointsNumber() == 1) {
        return polyline.pointAt(0);
    }
    // here the polyline has at least 1 segment
    geolib3::Point2 result;
    double minDistance = std::numeric_limits<double>::max();
    for (size_t i = 0; i < polyline.segmentsNumber(); ++i) {
        const geolib3::Point2 curPoint = geolib3::closestPoint(polyline.segmentAt(i), point);
        const double curDistance = geolib3::geoDistance(curPoint, point);
        if (minDistance > curDistance) {
            minDistance = curDistance;
            result = curPoint;

            if (minDistance < eps) {
                return result;
            }
        }
    }
    return result;
}

CheckResult
checkViaPointsPassingOrder(geolib3::Polyline2 plannedRoute,
                           geolib3::Polyline2 actualRoute,
                           const geolib3::PointsVector& viaPoints)
{

    CheckResult result;

    constexpr double DISTANCE_RATIO_THRESHOLD = 2.;
    constexpr double DISTANCE_THRESHOLD_METERS = 10.;
    constexpr double ROUTE_SNAP_THRESHOLD_METERS = 3.;

    auto prevPoint = viaPoints[0];
    for(const auto& viaPoint : viaPoints) {
        auto plannedPoint = amortizedClosestPoint(plannedRoute, viaPoint, ROUTE_SNAP_THRESHOLD_METERS);
        auto plannedDistance = geolib3::geoDistance(viaPoint, plannedRoute);
        if (plannedDistance > ROUTE_SNAP_THRESHOLD_METERS)
        {
            WARN() << "planned route does not contain "
                << viaPoint.x() << " " << viaPoint.y()
                << " distance is " << plannedDistance;
            result.missingPoints.push_back(viaPoint);
            continue;
        }

        auto routePoint = amortizedClosestPoint(actualRoute, viaPoint, ROUTE_SNAP_THRESHOLD_METERS);
        auto routeDistance = geolib3::geoDistance(viaPoint, routePoint);
        if (routeDistance > ROUTE_SNAP_THRESHOLD_METERS)
        {
            WARN() << "actual route does not contain "
                << viaPoint.x() << " " << viaPoint.y()
                << " distance is " << routeDistance;
            result.missingPoints.push_back(viaPoint);
            continue;
        }

        auto plannedPart = geolib3::partitionFromStart(plannedRoute, plannedPoint);
        auto actualPart = geolib3::partitionFromStart(actualRoute, routePoint);

        auto plannedLength = geolib3::geoLength(plannedPart);
        auto actualLength = geolib3::geoLength(actualPart);

        if (actualLength / plannedLength > DISTANCE_RATIO_THRESHOLD &&
                actualLength - plannedLength > DISTANCE_THRESHOLD_METERS)
        {
            INFO() << "From " << formatPoint(prevPoint)
                << " to " << formatPoint(viaPoint)
                << " distance increase for " << actualLength - plannedLength
                << " actualLength = " << actualLength << " plannedLength = " << plannedLength;

            result.passingInfos.push_back({prevPoint, viaPoint,
                                           plannedPart, actualPart});
        }
        plannedRoute = geolib3::partitionToEnd(plannedRoute, plannedPoint);
        actualRoute = geolib3::partitionToEnd(actualRoute, routePoint);
        prevPoint = viaPoint;
    }

    return result;
}


void
saveViaPointsCheck(const CheckResult& viaPointsCheck,
                   const std::string& evFilePrefix)
{
    INFO() << "saveViaPointsCheck " << evFilePrefix;
    size_t idx = 0;
    for(const auto& passingInfo : viaPointsCheck.passingInfos) {
        std::ofstream iout(evFilePrefix + "." + std::to_string(idx));
        iout.precision(10);

        iout << "!pointstyle=green:blue:4\n";
        formatInEasyview(iout, passingInfo.from);
        iout << "!pointstyle=yellow:blue:4\n";
        formatInEasyview(iout, passingInfo.to);
        iout << "!linestyle=red:4\n";
        formatInEasyview(iout, passingInfo.actualRoute);
        iout << "!linestyle=black:2\n";
        formatInEasyview(iout, passingInfo.plannedRoute);

        idx++;
    }
}

}}} // namespace maps::mrc::tasks_analyzer

