#include <maps/wikimap/mapspro/services/mrc/libs/sensors_feature_positioner/include/sensors_feature_positioner_pool.h>

#include <maps/libs/log8/include/log8.h>
#include <maps/wikimap/mapspro/services/mrc/libs/sensors_feature_positioner/include/sensors_loader.h>
#include <maps/wikimap/mapspro/services/mrc/libs/sensors_feature_positioner/include/separate_rides.h>
#include <maps/libs/geolib/include/distance.h>

#include <algorithm>
#include <string>

namespace maps::mrc::sensors_feature_positioner {

// position_improvment algorithm can't calculate precise phone orientation on
// a short track
const double MIN_TRACK_LENGTH_METERS = 3000;

namespace {

double trackLengthMeters(const db::TrackPoints& trackPoints) {
    const double MAX_METERS_BETWEEN_POINTS = 50;
    double trackLength = 0;
    for (size_t i = 1; i < trackPoints.size(); i++) {
        double d = maps::geolib3::fastGeoDistance(trackPoints[i - 1].geodeticPos(),
                                                  trackPoints[i].geodeticPos());
        if (d < MAX_METERS_BETWEEN_POINTS) {
            trackLength += d;
        }
    }
    return trackLength;
}

}

void SensorsFeaturePositionerPool::addPositioners(
    const std::string& sourceId,
    const Tracks& tracks,
    const adapters::Matcher& graphMatcher)
{
    double allTracksLengthMeters = 0;
    double addedTracksLengthMeters = 0;
    for (const auto& track : tracks) {
        if (track.trackPoints.empty()) {
            INFO() << "feature positioner for sourceId = " << sourceId
                   << " was not created because there are no track points";
            continue;
        }
        double trackLenMeters = trackLengthMeters(track.trackPoints);
        allTracksLengthMeters += trackLenMeters;

        std::string warningPrefix
            = "feature positioner for sourceId = " + sourceId
            + " with length = " + std::to_string(trackLenMeters / 1000.0) + " km"
            + " start time = "
            + std::to_string(track.trackPoints.front().timestamp().time_since_epoch().count())
            + " end time = "
            + std::to_string(track.trackPoints.back().timestamp().time_since_epoch().count())
            + " was not created";
        if (track.sensorEvents.gyroEvents.empty()
            || track.sensorEvents.accEvents.empty())
        {
            INFO() << warningPrefix << " because there is no sensors data";
            continue;
        }
        if (trackLenMeters < MIN_TRACK_LENGTH_METERS)
        {
            INFO() << warningPrefix << " because the track is too short";
            continue;
        }

        try {
            featurePositionersBySourceId_[sourceId].emplace_back(
                graphMatcher,
                track.trackPoints,
                track.sensorEvents);
            addedTracksLengthMeters += trackLenMeters;
            INFO() << "Added positioner of length "
                   << trackLenMeters / 1000.0 << " km";
        }
        catch (const maps::Exception& e) {
            WARN() << warningPrefix << ": " << e.what();
        }
    }
    INFO() << "Tried to add positioners with total length "
           << allTracksLengthMeters / 1000.0 << " km";
    INFO() << "Added positioners with total length "
           << addedTracksLengthMeters / 1000.0 << " km";
}

SensorsFeaturePositionerPool::SensorsFeaturePositionerPool(
    mds::Mds& mdsClient,
    pgpool3::Pool& pool,
    const adapters::Matcher& graphMatcher,
    db::TId assignmentId,
    db::TrackPoints allTrackPoints)
{
    try {
        if (allTrackPoints.empty()) {
            return;
        }

        std::sort(allTrackPoints.begin(), allTrackPoints.end(),
                  [](const db::TrackPoint& lhs, const db::TrackPoint& rhs) {
                      return lhs.timestamp() < rhs.timestamp();
                  });

        std::vector<db::TrackPoints> pointsForEachSourceId = separateBySourceId(
            allTrackPoints);

        for (const auto& trackPoints : pointsForEachSourceId) {
            REQUIRE(trackPoints.size(), "no points for such sourceId");
            auto sourceId = trackPoints.front().sourceId();

            SensorEvents sensorEvents = loadAssignmentSensors(
                mdsClient,
                pool,
                trackPoints.front().timestamp(),
                trackPoints.back().timestamp(),
                sourceId,
                assignmentId);
            if (sensorEvents.gyroEvents.empty() || sensorEvents.accEvents.empty()) {
                INFO() << "feature positioner for sourceId = " << sourceId
                       << " with " << trackPoints.size() << " trackPoints"
                       << " was not created because there are no sensors";
                continue;
            }

            auto tracks = splitIntoSeparateRides(trackPoints, sensorEvents);
            addPositioners(sourceId, tracks, graphMatcher);
            INFO() << "Created SensorsFeaturePositioners for track of lenght "
                   << trackLengthMeters(trackPoints) / 1000.0 << " km";
        }
    }
    catch (const maps::Exception& e) {
        ERROR() << e;
    }
    catch (const std::runtime_error& e) {
        ERROR() << e.what();
    }
    catch (...) {
        ERROR() << "unkwown error";
    }
}

SensorsFeaturePositionerPool::SensorsFeaturePositionerPool(
    mds::Mds& mdsClient,
    pgpool3::Pool& pool,
    const adapters::Matcher& graphMatcher,
    const std::string& sourceId,
    chrono::TimePoint minTime,
    chrono::TimePoint maxTime)
{
    try {
        auto txn = pool.slaveTransaction();
        db::TrackPointGateway tpGtw(*txn);
        db::TrackPoints trackPoints = tpGtw.load(
            db::table::TrackPoint::sourceId.equals(sourceId)
            && db::table::TrackPoint::timestamp >= minTime
            && db::table::TrackPoint::timestamp <= maxTime);

        if (trackPoints.empty()) {
            return;
        }

        std::sort(trackPoints.begin(), trackPoints.end(),
                  [](const db::TrackPoint& lhs, const db::TrackPoint& rhs) {
                      return lhs.timestamp() < rhs.timestamp();
                  });

        SensorEvents sensorEvents = loadRideSensors(
            mdsClient,
            pool,
            trackPoints.front().timestamp(),
            trackPoints.back().timestamp(),
            sourceId);
        if (sensorEvents.gyroEvents.empty() || sensorEvents.accEvents.empty()) {
            INFO() << "feature positioner for sourceId = " << sourceId
                   << " with " << trackPoints.size() << " trackPoints"
                   << " was not created because there are no sensors";
            return;
        }

        auto tracks = splitIntoSeparateRides(trackPoints, sensorEvents);
        addPositioners(sourceId, tracks, graphMatcher);
    }
    catch (const maps::Exception& e) {
        ERROR() << e;
    }
    catch (const std::runtime_error& e) {
        ERROR() << e.what();
    }
    catch (...) {
        ERROR() << "unkwown error";
    }
}

std::optional<pos_improvment::ImprovedGpsEvent>
SensorsFeaturePositionerPool::getPositionByTime(const std::string& sourceId,
                                                chrono::TimePoint time) const
{
    auto it = featurePositionersBySourceId_.find(sourceId);
    if (it == featurePositionersBySourceId_.end()) {
        return std::nullopt;
    }
    const auto& featurePositioners = it->second;

    auto positionerIt = std::lower_bound(
        featurePositioners.begin(), featurePositioners.end(), time,
        [](const SensorsFeaturePositioner& lhs, chrono::TimePoint rhs) {
            return lhs.trackEndTime() < rhs;
        });
    if (positionerIt == featurePositioners.end()
        || positionerIt->trackStartTime() > time) {
        return std::nullopt;
    }

    return positionerIt->getPositionByTime(time);
}

std::optional<db::Feature>
SensorsFeaturePositionerPool::calculatePosition(db::Feature feature) const
{
    if (feature.graph() == db::GraphType::Pedestrian) {
        return std::nullopt;
    }
    auto pos = getPositionByTime(feature.sourceId(), feature.timestamp());
    if (!pos) {
        return std::nullopt;
    }

    feature.setMercatorPos(pos->mercatorPosition());
    feature.setOdometerMercatorPos(pos->odometerMercatorPosition());
    feature.setHeading(pos->carHeading());
    feature.setCameraRodrigues(pos->cameraRodrigues());
    return feature;
}

void SensorsFeaturePositionerPool::applyPositionIfPossible(
    db::Features& features) const
{
    applyPositionIfPossible(features.begin(), features.end());
}

void SensorsFeaturePositionerPool::applyPositionIfPossible(db::Features::iterator first, db::Features::iterator last) const
{
try {
    size_t modifiedFeaturesCounter = 0;
    for (auto it = first; it != last; it++) {
        auto modifiedFeature = calculatePosition(*it);
        if (modifiedFeature) {
            *it = *modifiedFeature;
            modifiedFeaturesCounter++;
        }
    }
    INFO() << "sensors positioner handled " << modifiedFeaturesCounter
           << " of " << std::distance(first, last);
}
catch (const maps::Exception& e) {
    ERROR() << e;
}
catch (const std::runtime_error& e) {
    ERROR() << e.what();
}
catch (...) {
    ERROR() << "unkwown error";
}
}

} // namespace maps::mrc::sensors_feature_positioner
