#include "position.h"

#include "geom.h"
#include "match_function.h"

#include <maps/libs/geolib/include/distance.h>
#include <maps/libs/geolib/include/vector.h>
#include <maps/libs/log8/include/log8.h>

#include <cmath>

using maps::geolib3::Radians;
using maps::geolib3::sin;
using maps::geolib3::cos;

namespace maps::mrc::pos_improvment {

namespace {

// For each gpsEvent calculates median coordinates of neighboring points.
// We use median to ignore completely incorrect gps points
std::vector<geolib3::Point2> averagedCoordinates(const GpsEvents& gpsEvents) {
    constexpr int RANGE_SIZE = 50;
    std::vector<geolib3::Point2> averagedCoordinates;
    averagedCoordinates.reserve(gpsEvents.size());
    std::vector<double> xVals;
    std::vector<double> yVals;
    xVals.reserve(RANGE_SIZE);
    yVals.reserve(RANGE_SIZE);

    for (size_t i = 0; i < gpsEvents.size(); i++) {
        size_t rangeBegin = std::max(0, (int)i - RANGE_SIZE / 2);
        size_t rangeEnd = std::min(gpsEvents.size(), i + RANGE_SIZE / 2);
        xVals.clear();
        yVals.clear();
        for (size_t j = rangeBegin; j < rangeEnd; j++) {
            xVals.push_back(gpsEvents[j].mercatorPos.x());
            yVals.push_back(gpsEvents[j].mercatorPos.y());
        }
        std::nth_element(xVals.begin(),
                         xVals.begin() + xVals.size() / 2,
                         xVals.end());
        std::nth_element(yVals.begin(),
                         yVals.begin() + yVals.size() / 2,
                         yVals.end());

        averagedCoordinates.emplace_back(xVals[xVals.size() / 2],
                                         yVals[yVals.size() / 2]);
    }
    return averagedCoordinates;
}


// Creates track using only speed and azimuth. Starts from the first
// gps point.
// Returns positions on each gps time.
// @param events should contain speed, azimuth and gps events
std::vector<geolib3::Point2> calculateTrackUsingSpeedAndAzimuth(
    const TrackEventPtrs& events,
    const GpsEvents& gpsEvents,
    const CarGroundDirectionEvents& directionEvents,
    const CarGroundSpeedEvents& speedEvents)
{
    REQUIRE(events.size() > 0 && gpsEvents.size() > 0 &&
            directionEvents.size() > 0 && speedEvents.size(),
            "Empty input");

    std::vector<geolib3::Point2> track;
    track.reserve(gpsEvents.size());

    // averaged coordinates are used to convert meters to mercator
    std::vector<geolib3::Point2> avgCoordinates = averagedCoordinates(gpsEvents);

    Radians carDirection = directionEvents[0].angle;
    Time prevSpeedTime = speedEvents[0].time;
    prevSpeedTime = events[0]->time;
    MetersPerSec speed(0);
    size_t curGpsIndex = 0;
    geolib3::Point2 mercatorPos{0,0}; // will be initialized on the first gps point

    for (auto& trackEvent : events) {
        if (trackEvent->type == EventType::CarGroundDirection) {
            auto& event = *static_cast<CarGroundDirectionEvent const *>(trackEvent);
            carDirection = event.angle;
        }
        if (trackEvent->type == EventType::CarGroundSpeed) {
            auto& event = *static_cast<CarGroundSpeedEvent const *>(trackEvent);
            speed = event.speed;
        }

        Meters distance = speed * (trackEvent->time - prevSpeedTime);
        double mercatorDistance = metersToMercatorDistance(distance, avgCoordinates[curGpsIndex]);
        geolib3::Vector2 shift(mercatorDistance * cos(carDirection),
                               mercatorDistance * sin(carDirection));
        mercatorPos += shift;
        prevSpeedTime = trackEvent->time;

        if (trackEvent->type == EventType::Gps) {
            auto& event = *static_cast<GpsEvent const *>(trackEvent);
            if (curGpsIndex == 0) {
                // initial point is not important because only the difference between
                // neighboring points will be used in the algorithm
                mercatorPos = event.mercatorPos;
            }
            track.push_back(mercatorPos);
            curGpsIndex++;
            if (curGpsIndex >= gpsEvents.size()) {
                break;
            }
        }
    }
    return track;
}

// for each short range calculates average coordinates difference
// between track and gpsEvents. Adds this difference to the center
// point of the range. Returns modified track;
std::vector<geolib3::Point2> matchTrackToGpsPoints(
    const GpsEvents& gpsEvents,
    const std::vector<geolib3::Point2>& track)
{
    REQUIRE(gpsEvents.size() == track.size(),
            "track should contain the same number of points");
    const int FRAME_WIDTH = 50;

    std::vector<double> gpsX(track.size());
    std::vector<double> gpsY(track.size());
    std::vector<double> sensorX(track.size());
    std::vector<double> sensorY(track.size());
    for (size_t i = 0; i < gpsEvents.size(); i++) {
        gpsX[i] = gpsEvents[i].mercatorPos.x();
        gpsY[i] = gpsEvents[i].mercatorPos.y();
        sensorX[i] = track[i].x();
        sensorY[i] = track[i].y();
    }

    auto getWeight = [&](int64_t baseIndex, int64_t curIndex) {
        // the bigger the distance between two gps points, the less
        // the accuracy of the sensors track between two gps points
        double distanceWeight = 1 - 0.01 * std::abs(curIndex - baseIndex);
        return std::max(0.0, distanceWeight);
    };

    std::vector<double> actualX = matchFunction(
        gpsX, sensorX, FRAME_WIDTH, getWeight);
    std::vector<double> actualY = matchFunction(
        gpsY, sensorY, FRAME_WIDTH, getWeight);

    std::vector<geolib3::Point2> matchedTrack;
    matchedTrack.reserve(gpsEvents.size());
    for (size_t i = 0; i < gpsEvents.size(); i++) {
        matchedTrack.emplace_back(actualX[i], actualY[i]);
    }
    return matchedTrack;
}

// Removes points that are too close to each other
// In each small range keeps the point with the best accuracy
GpsEvents removeClosePoints(const GpsEvents& gpsEvents) {
    GpsEvents filteredEvents{gpsEvents[0]};
    filteredEvents.reserve(gpsEvents.size());

    for(size_t i = 1; i < gpsEvents.size(); i++) {
        Meters distanceFromLastPoint = distanceMeters(
            gpsEvents[i].mercatorPos, filteredEvents.back().mercatorPos);
        if (distanceFromLastPoint > 5.0_m) {
            filteredEvents.push_back(gpsEvents[i]);
            distanceFromLastPoint = 0.0_m;
        } else if (gpsEvents[i].accuracy < filteredEvents.back().accuracy) {
            filteredEvents.back() = gpsEvents[i];
        }
    }
    return filteredEvents;
}

} // anonymous namespace

PreciseTrack calculatePreciseTrack(GpsEvents gpsEvents,
                                   const CarGroundDirectionEvents& directionEvents,
                                   const CarGroundSpeedEvents& speedEvents)
{
    gpsEvents = removeClosePoints(std::move(gpsEvents));

    TrackEventPtrs events;
    events.reserve(gpsEvents.size() + directionEvents.size() + speedEvents.size());
    insert(events, gpsEvents);
    insert(events, directionEvents);
    insert(events, speedEvents);

    std::vector<geolib3::Point2> trackFromSensors
        = calculateTrackUsingSpeedAndAzimuth(events, gpsEvents,
                                             directionEvents, speedEvents);
    std::vector<geolib3::Point2> matchedTrack
        = matchTrackToGpsPoints(gpsEvents, trackFromSensors);

    // calculates points between gps points
    PreciseTrack preciseTrack;
    Radians carDirection = directionEvents[0].angle;
    Time prevSpeedTime = speedEvents[0].time;
    geolib3::Point2 mercatorPos = {0, 0}; // will be initialized on the first gps point
    geolib3::Point2 odometerMercatorPos = {0, 0}; // will be initialized on the first gps point

    size_t curGpsIndex = 0;
    MetersPerSec speed = speedEvents[0].speed;

    for (auto& trackEvent : events) {
        if (trackEvent->type == EventType::CarGroundDirection) {
            auto& event = *static_cast<CarGroundDirectionEvent const *>(trackEvent);
            carDirection = event.angle;
        }
        if (trackEvent->type == EventType::CarGroundSpeed) {
            auto& event = *static_cast<CarGroundSpeedEvent const *>(trackEvent);
            speed = event.speed;
        }

        Meters distance = speed * (trackEvent->time - prevSpeedTime);

        double mercatorDistance = metersToMercatorDistance(distance, matchedTrack[curGpsIndex]);
        geolib3::Vector2 shift(mercatorDistance * cos(carDirection),
                               mercatorDistance * sin(carDirection));
        mercatorPos += shift;
        odometerMercatorPos += shift;
        prevSpeedTime = trackEvent->time;

        if (trackEvent->type == EventType::Gps) {
            mercatorPos = matchedTrack[curGpsIndex];
            if (curGpsIndex == 0) {
                odometerMercatorPos = matchedTrack[0];
            }

            curGpsIndex++;
            if (curGpsIndex == gpsEvents.size()) {
                break;
            }
        }
        if (curGpsIndex > 0 && trackEvent->type == EventType::CarGroundDirection) {
            auto& event = *static_cast<CarGroundDirectionEvent const *>(trackEvent);
            preciseTrack.gpsEvents.emplace_back(event.time, mercatorPos, 0.0_m, speed, event.angle);
            preciseTrack.odometerTrack.push_back(odometerMercatorPos);
        }

    }

    return preciseTrack;
}

} // namespace maps::mrc::pos_improvment
