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

namespace maps::mrc::sensors_feature_positioner {

std::vector<db::TrackPoints> separateBySourceId(db::TrackPoints trackPoints)
{
    if (trackPoints.empty()) {
        return {};
    }

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

    std::vector<db::TrackPoints> resultTracks;
    resultTracks.push_back({trackPoints[0]});

    for (size_t i = 1; i < trackPoints.size(); i++) {
        if (trackPoints[i].sourceId() != trackPoints[i - 1].sourceId()) {
            resultTracks.push_back({trackPoints[i]});
        } else if (trackPoints[i].timestamp() != resultTracks.back().back().timestamp()) {
            resultTracks.back().push_back(trackPoints[i]);
        }
    }

    return resultTracks;
}

namespace {

struct Interval {
    chrono::TimePoint begin;
    chrono::TimePoint end;
};
using Intervals = std::vector<Interval>;

chrono::TimePoint toTimePoint(pos_improvment::Time time) {
    return std::chrono::time_point_cast<chrono::TimePoint::duration>(time);
}

// finds time gaps where trackPoints or events are missed
Intervals findGaps(const db::TrackPoints& trackPoints,
                   const SensorEvents& sensorEvents)
{
    const pos_improvment::Seconds MAX_SENSORS_GAP(1);
    const std::chrono::seconds MAX_GPS_GAP(10);

    REQUIRE(trackPoints.size(), "no trackPoints were provided");
    const auto& gyroEvents = sensorEvents.gyroEvents;
    const auto& accEvents = sensorEvents.accEvents;
    Intervals gaps;

    for (size_t i = 1; i < gyroEvents.size(); i++) {
        if (gyroEvents[i].time - gyroEvents[i - 1].time > MAX_SENSORS_GAP) {
            gaps.push_back({toTimePoint(gyroEvents[i - 1].time),
                           toTimePoint(gyroEvents[i].time)});
        }
    }

    for (size_t i = 1; i < accEvents.size(); i++) {
        if (accEvents[i].time - accEvents[i - 1].time > MAX_SENSORS_GAP) {
            gaps.push_back({toTimePoint(accEvents[i - 1].time),
                           toTimePoint(accEvents[i].time)});
        }
    }

    for (size_t i = 1; i < trackPoints.size(); i++) {
        if (trackPoints[i].timestamp() - trackPoints[i - 1].timestamp() > MAX_GPS_GAP) {
            gaps.push_back({trackPoints[i - 1].timestamp(),
                            trackPoints[i].timestamp()});
        }
    }

    // add border gaps, so all the trackPoints will be between gaps.
    gaps.push_back({trackPoints.front().timestamp() - std::chrono::hours(1),
                    trackPoints.front().timestamp() - std::chrono::milliseconds(100)});
    gaps.push_back({trackPoints.back().timestamp() + std::chrono::milliseconds(100),
                    trackPoints.back().timestamp() + std::chrono::hours(1)});

    return gaps;
}

// Sort gaps and join intersecting gaps
Intervals sortAndJoinGaps(Intervals gaps) {
    if (gaps.empty()) {
        return {};
    }
    std::sort(gaps.begin(), gaps.end(),
              [](const Interval& lhs, const Interval& rhs) {
                  return lhs.begin < rhs.begin;
              });

    Intervals joinedGaps{gaps[0]};
    for (size_t i = 1; i < gaps.size(); i++) {
        if (gaps[i].begin <= joinedGaps.back().end) {
            joinedGaps.back().end = std::max(joinedGaps.back().end, gaps[i].end);
        } else {
            joinedGaps.push_back(gaps[i]);
        }
    }
    return joinedGaps;
}

// Returns intervals between gaps
Intervals getSolidRidesIntervals(const Intervals& gaps)
{
    Intervals ridesIntervals;
    for (size_t i = 1; i < gaps.size(); i++) {
        ridesIntervals.push_back({gaps[i - 1].end, gaps[i].begin});
    }
    return ridesIntervals;
}

} // anonymous namespace

Tracks splitIntoSeparateRides(const db::TrackPoints& trackPoints,
                              const SensorEvents& sensorEvents)
{
    if (trackPoints.empty()) {
        return {};
    }

    Intervals gaps = sortAndJoinGaps(findGaps(trackPoints, sensorEvents));
    Intervals ridesIntervals = getSolidRidesIntervals(gaps);

    const auto& gyroEvents = sensorEvents.gyroEvents;
    const auto& accEvents = sensorEvents.accEvents;

    size_t gyroEventsIndex = 0;
    size_t accEventsIndex = 0;
    size_t trackPointsIndex = 0;

    Tracks tracks;

    for (auto ride : ridesIntervals) {
        pos_improvment::GyroscopeEvents curGyroEvents;
        pos_improvment::AccelerometerEvents curAccEvents;
        db::TrackPoints curTrackPoints;

        for (; gyroEventsIndex < gyroEvents.size(); gyroEventsIndex++) {
            const auto& event = gyroEvents[gyroEventsIndex];
            if (toTimePoint(event.time) > ride.end) {
                break;
            }
            if (toTimePoint(event.time) >= ride.begin) {
                curGyroEvents.push_back(event);
            }
        }

        for (; accEventsIndex < accEvents.size(); accEventsIndex++) {
            const auto& event = accEvents[accEventsIndex];
            if (toTimePoint(event.time) > ride.end) {
                break;
            }
            if (toTimePoint(event.time) >= ride.begin) {
                curAccEvents.push_back(event);
            }
        }

        for (; trackPointsIndex < trackPoints.size(); trackPointsIndex++) {
            const auto trackPoint = trackPoints[trackPointsIndex];
            if (trackPoint.timestamp() > ride.end) {
                break;
            }
            if (trackPoint.timestamp() >= ride.begin) {
                curTrackPoints.push_back(trackPoint);
            }
        }
        tracks.push_back(
            {curTrackPoints, SensorEvents{curGyroEvents, curAccEvents}});
    }

    return tracks;
}

} // namespace maps::mrc::sensors_feature_positioner
