#include "coverage.h"
#include "inspect_quality.h"
#include "tools.h"

#include <maps/libs/common/include/math.h>
#include <maps/libs/geolib/include/common.h>
#include <maps/libs/geolib/include/distance.h>

/// use boost::geometry for rtree
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <algorithm>
#include <iterator>

namespace maps {
namespace mrc {
namespace img_qa {
namespace {

using BPoint = boost::geometry::model::d2::point_xy<double>;
using BBox = boost::geometry::model::box<BPoint>;

struct IndexableFeature {
    using result_type = BPoint;

    BPoint operator()(const db::Feature& feature) const
    {
        return {feature.geodeticPos().x(), feature.geodeticPos().y()};
    }
};

using FeatureRTree
    = boost::geometry::index::rtree<db::Feature,
                                    boost::geometry::index::quadratic<16>,
                                    IndexableFeature>;

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

Segments getUncovered(const geolib3::Segment2& segment,
                      const db::Features& features)
{
    Segments result{segment};
    for (const auto& feature : features) {
        result = db::getUncovered(feature, result);
    }
    return result;
}

Segments getCovered(const geolib3::Segment2& segment, Segments uncoveredParts)
{
    constexpr double MIN_COVERED_PART_LENGTH_METERS = 2.0;

    std::sort(
        uncoveredParts.begin(), uncoveredParts.end(),
        [&](const geolib3::Segment2& lhs, const geolib3::Segment2& rhs) {
            return geolib3::geoDistance(segment.start(), lhs.start())
                   < geolib3::geoDistance(segment.start(), rhs.start());
        });

    Segments coveredParts;
    geolib3::Point2 lastPoint = segment.start();

    for (const auto& uncoveredPart : uncoveredParts) {
        if (geolib3::geoDistance(lastPoint, uncoveredPart.start())
            > MIN_COVERED_PART_LENGTH_METERS) {
            coveredParts.emplace_back(lastPoint, uncoveredPart.start());
        }
        lastPoint = uncoveredPart.end();
    }
    if (geolib3::geoDistance(lastPoint, segment.end())
        > MIN_COVERED_PART_LENGTH_METERS) {
        coveredParts.emplace_back(lastPoint, segment.end());
    }
    return coveredParts;
}

auto makeTuple(const db::Feature& feature)
{
    return std::make_tuple(feature.sourceId(), feature.timestamp());
}

} // anonymous namespace

SegmentsCoverage getCoverage(const Segments& segments,
                             const db::Features& features)
{
    SegmentsCoverage result;

    double segmentsLength = db::geoLength(segments);
    if (segmentsLength < geolib3::EPS) { // to avoid division by zero
        result.coveredParts = segments;
        result.coverageFraction = 1.;
        return result;
    }

    FeatureRTree rTree{features.begin(), features.end()};
    for (const auto& segment : segments) {
        auto bbox = db::addVisibilityMargins(segment.boundingBox());
        db::Features candidates;
        rTree.query(
            boost::geometry::index::intersects(BBox{
                toBoost(bbox.lowerCorner()), toBoost(bbox.upperCorner())}),
            std::back_inserter(candidates));
        Segments uncoveredParts = getUncovered(segment, candidates);
        result.uncoveredParts += uncoveredParts;
        result.coveredParts += getCovered(segment, std::move(uncoveredParts));
    }

    result.coverageFraction
        = 1. - (db::geoLength(result.uncoveredParts) / segmentsLength);
    return result;
}

CoverageStatistics inspectCoverage(Context& ctx,
                                   db::TId assignmentId,
                                   db::CameraDeviation cameraDeviation)
{
    CoverageStatistics result{.assignmentId = assignmentId,
                              .cameraDeviation = cameraDeviation};

    auto trackPoints = ctx.loadAssignmentTrackPoints(assignmentId);
    result.processedPoints = trackPoints.size();
    sortUniqueByTime(trackPoints);
    const auto unmatchedPath = makePath(trackPoints);
    result.track = toPolylines(getSegments(unmatchedPath));

    auto photos = ctx.loadProcessedPhotos(assignmentId, cameraDeviation);
    result.processedPhotos = photos.size();
    if (!photos.empty()) {
        auto[firstShotTime, lastShotTime]
            = std::minmax_element(photos.begin(), photos.end(),
                [](const auto& lhs, const auto& rhs) {
                    return lhs.timestamp()
                        < rhs.timestamp();
                });
        result.firstShotTime = firstShotTime->timestamp();
        result.lastShotTime = lastShotTime->timestamp();
        std::sort(photos.begin(), photos.end(), [](const auto& lhs,
                                                    const auto& rhs) {
            return makeTuple(lhs) < makeTuple(rhs);
        });
    }

    db::Features rays, goodRays;
    slice(photos, [&](db::Features::iterator begin,
                      db::Features::iterator end) {
        const auto photoToStatusMap
            = loadPhotoToTolokaStatusMap(ctx, {begin, end});

        db::Features features{begin, end};

        for (const auto& feature : features) {
            if (feature.hasPos() && feature.hasHeading()) {
                rays.push_back(feature);
                if (isPhotoGood(feature, photoToStatusMap)) {
                    goodRays.push_back(feature);
                }
            }
        }
    });

    const auto assignment = ctx.loadAssignment(assignmentId);
    const auto task = ctx.loadTask(assignment.taskId(), db::ugc::LoadNames::No,
                                   db::ugc::LoadTargets::Yes);
    auto taskSegments = getSegments(task);
    result.allPhotosCoverage = getCoverage(taskSegments, rays);
    result.goodPhotosCoverage = getCoverage(taskSegments, goodRays);
    for (const auto& edge : unmatchedPath) {
        result.trackDistanceInMeters
            += geolib3::geoDistance(edge.segment.start(), edge.segment.end());
        result.trackDuration
            += std::chrono::duration_cast<std::chrono::seconds>(
                edge.endTime - edge.startTime);
    }
    result.goodPhotos = goodRays.size();
    return result;
}

} // img_qa
} // mrc
} // maps
