#include "gps.h"

#include "geom.h"
#include "azimuth.h"

#include <maps/libs/common/include/exception.h>
#include <maps/libs/geolib/include/direction.h>
#include <maps/libs/log8/include/log8.h>

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

namespace maps::mrc::pos_improvment {

const MetersPerSec MAX_BACKWARD_SPEED(5);
const MetersPerSec LOW_SPEED(1);
const int MAX_ERRORS = 10;
const MetersPerSec MAX_POSSIBLE_SPEED(40);

namespace {

GpsEvents::iterator findFirstEventWithDirection(GpsEvents& events) {
    return std::find_if(
        events.begin(), events.end(),
        [](const GpsEvent& event) {
            return event.hasValidDirection();
        });
}

} // anonymous namespace

// To smooth azimuth we need to reverse gps azimuth when the car is
// moving backward.
// To find the places where the car is moving backward we use 2 rules:
//   1) if the car is instantly rotating 90+ degrees, the car is changing its
//     forward/backward mode
//   2) if the car speed > MAX_BACKWARD_SPEED, the car is moving forward
// The first rule is not reliable because the azimuth can be random at
// small speed. To fix it we rely on the second rule: if the speed is big,
// but we think that the car is moving backward, we should reverse
// previous points until the last car stop.
// Also we use gyroscope to better maintain direction
GpsEvents smoothAzimuth(GpsEvents gpsEvents,
                        const GyroscopeEvents& gyroEvents,
                        UnitVector3 carUpVector)
{
    REQUIRE(gyroEvents.size(), "gyroEvents is empty");
    GravityEvents gravityEvents {
        GravityEvent(gyroEvents.front().time - 1.0_sec, UnitVector3(-carUpVector)),
        GravityEvent(gyroEvents.back().time + 1.0_sec, UnitVector3(-carUpVector))};
    TrackEventPtrs events;
    events.reserve(gpsEvents.size() + gravityEvents.size() + gyroEvents.size());
    insert(events, gpsEvents);
    insert(events, gravityEvents);
    insert(events, gyroEvents);

    std::vector<Radians> gyroAzimuthForEachGps = findGyroAzimuthForEachGps(
        events, gpsEvents, gravityEvents, gyroEvents);


    const auto firstEventWithDirection = findFirstEventWithDirection(gpsEvents);
    REQUIRE(firstEventWithDirection != gpsEvents.end(),
            "GpsEvents doesn't have points with valid direction");
    Radians curDirection = *firstEventWithDirection->direction;

    // Error is a situation when the car speed is very high, but we assume
    // that the car moves backward. If errors == 0, we are sure that
    // curDirection is the proper car direction. If errors == MAX_ERRORS, we
    // sure that curDirection is opposite to the real carDirection.
    // At the beginning we assume that the car moves forward, but
    // actually we don't known, so we use MAX_ERRORS / 2 initial value.
    int errors = MAX_ERRORS / 2;

    // if there are a lot of errors, we should reverse previous points
    // until lastPossibleReversePoint
    auto lastPossibleReversePoint = firstEventWithDirection;

    size_t pointsWithAzimuth = 0;
    size_t reverseMovingPoints = 0;
    size_t gpsEventsIndex = firstEventWithDirection - gpsEvents.begin();

    for (auto event = firstEventWithDirection + 1;
         event != gpsEvents.end();
         ++event)
    {
        gpsEventsIndex++;
        // correcting curDirection using gyroscope
        curDirection += gyroAzimuthForEachGps[gpsEventsIndex]
            - gyroAzimuthForEachGps[gpsEventsIndex-1];

        if (errors == 0
            && event->speed
            && abs(*event->speed) <= abs(*lastPossibleReversePoint->speed)
            && lastPossibleReversePoint != firstEventWithDirection)
        {
            lastPossibleReversePoint = event;
        }
        if (!event->hasValidDirection()) {
            continue;
        }
        if (abs(*event->speed) > LOW_SPEED) {
            pointsWithAzimuth++;
        }
        const geolib3::Radians dirDiff = geolib3::orientedAngleBetween(
            geolib3::Direction2(curDirection),
            geolib3::Direction2(*event->direction));

        // event->direction = (event->direction + N * 2PI) with some N
        *event->direction = curDirection + dirDiff;

        if (abs(dirDiff) > PI / 2) {
            if (*event->speed > MAX_BACKWARD_SPEED) {
                errors++;
            }

            if (errors > MAX_ERRORS) {
                // our assumption is wrong, we should reverse some
                // previous points
                geolib3::Radians shift = dirDiff > 0.0_rad ? PI : -PI;
                if (abs(*lastPossibleReversePoint->speed) > LOW_SPEED
                    && lastPossibleReversePoint != firstEventWithDirection)
                {
                    WARN() << "Strange car reverse moving points";
                }
                for (auto oldEvent = lastPossibleReversePoint;
                     oldEvent != event;
                     ++oldEvent)
                {
                    if (oldEvent->hasValidDirection()) {
                        *oldEvent->speed = -*oldEvent->speed;
                        *oldEvent->direction += shift;
                        if (*oldEvent->speed < -LOW_SPEED) {
                            reverseMovingPoints++;
                        } else if (*oldEvent->speed > LOW_SPEED) {
                            reverseMovingPoints--;
                        }
                    }
                }
                curDirection += shift;
                lastPossibleReversePoint = event;
                errors = MAX_ERRORS / 2;
            } else {
                // reverse the current point
                *event->speed = -*event->speed;
                if (*event->speed < -LOW_SPEED) {
                    reverseMovingPoints++;
                }
                if (dirDiff > PI / 2) {
                    *event->direction -= PI;
                } else {
                    *event->direction += PI;
                }
            }
        } else {
            if (*event->speed > MAX_BACKWARD_SPEED) {
                if (errors > 0) {
                    errors--;
                }
                if (errors == 0) {
                    // we are sure, that the previous points are correct
                    lastPossibleReversePoint = event;
                }
            }
        }

        // Сorrecting curDirection using current point direction.
        // The higher the speed, the less we use gyroscope value
        double weight = std::min(0.1, std::abs(event->speed->value()) / 30);
        curDirection = curDirection * (1.0 - weight)
            + *event->direction * weight;
    }

    REQUIRE(reverseMovingPoints < pointsWithAzimuth * 0.10,
            "Looks like we can't properly recognize car reverse moving "
            << reverseMovingPoints << " " << pointsWithAzimuth);
    return gpsEvents;
}

namespace {

// calculates speed for curEvent using speed between curEvent and prevEvent
// and speed between nextEvent and curEvent
MetersPerSec interpolateSpeed(MetersPerSec prevSpeed,
                              MetersPerSec nextSpeed,
                              const GpsEvent& curEvent,
                              const GpsEvent& prevEvent,
                              const GpsEvent& nextEvent) {
    // if there is a time gap between some gps points, we should use
    // the closest point
    double nextSpeedWeight = (curEvent.time - prevEvent.time).count();
    double prevSpeedWeight = (nextEvent.time - curEvent.time).count();
    return (nextSpeed * nextSpeedWeight + prevSpeed * prevSpeedWeight)
        / (nextSpeedWeight + prevSpeedWeight);
}

} // anonymous namespace

GpsEvents use2PointsGpsSpeed(GpsEvents gpsEvents)
{
    for (size_t i = 1; i < gpsEvents.size() - 1; i++) {
        MetersPerSec gpsNextSpeed
            = distanceMeters(gpsEvents[i].mercatorPos, gpsEvents[i + 1].mercatorPos)
            / (gpsEvents[i + 1].time - gpsEvents[i].time);
        MetersPerSec gpsPrevSpeed
            = distanceMeters(gpsEvents[i].mercatorPos, gpsEvents[i - 1].mercatorPos)
            / (gpsEvents[i].time - gpsEvents[i - 1].time);

        MetersPerSec newSpeed = interpolateSpeed(
            gpsPrevSpeed, gpsNextSpeed,
            gpsEvents[i], gpsEvents[i - 1], gpsEvents[i + 1]);
        if (newSpeed >= 0.0_mps && newSpeed < MAX_POSSIBLE_SPEED) {
            if (*gpsEvents[i].speed > 0.0_mps) {
                gpsEvents[i].speed = newSpeed;
            } else {
                gpsEvents[i].speed = -newSpeed;
            }
        }
    }
    return gpsEvents;
}

GpsEvents use2PointsGpsSpeed(GpsEvents gpsEvents,
                             const CarGroundDirectionEvents& directionEvents)
{
    REQUIRE(directionEvents.size(), "no direction events provided");
    const geolib3::Radians ARC_MIN_ANGLE(0.04);
    TrackEventPtrs events;
    events.reserve(gpsEvents.size() + directionEvents.size());
    insert(events, gpsEvents);
    insert(events, directionEvents);

    std::vector<Radians> directionForEachGps;
    Radians carDirection = directionEvents[0].angle;
    directionForEachGps.reserve(gpsEvents.size());

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

    // convert a chord length of a circle to the corresponding arc length
    auto chordToArc = [](Meters chordLength, geolib3::Radians arcAngle) {
        return chordLength * arcAngle.value() / 2 /  sin(arcAngle / 2);
    };

    auto findSpeedBetween2Points
        = [&] (size_t gpsIndex1, size_t gpsIndex2, geolib3::Radians direction) {
        Meters distance = distanceMeters(gpsEvents[gpsIndex2].mercatorPos,
                                         gpsEvents[gpsIndex1].mercatorPos);
        // calc distance along car direction
        distance = distance *
            geolib3::cos(direction -
                         geolib3::Direction2(
                             geolib3::Segment2(gpsEvents[gpsIndex1].mercatorPos,
                                               gpsEvents[gpsIndex2].mercatorPos)).radians());

        geolib3::Radians azimuthChange
            = abs(directionForEachGps[gpsIndex2]
                  - directionForEachGps[gpsIndex1]);

        if (azimuthChange > ARC_MIN_ANGLE) {
            // assume that the car moves along an arc of some circle
            // convert from projection to chord
            auto chord = distance / cos(azimuthChange / 2);
            distance = chordToArc(chord, azimuthChange);
        }
        return distance / (gpsEvents[gpsIndex2].time - gpsEvents[gpsIndex1].time);
    };

    for (size_t i = 1; i < gpsEvents.size() - 1; i++) {
        MetersPerSec prevSpeed = findSpeedBetween2Points(
            i - 1, i, directionForEachGps[i]);
        MetersPerSec nextSpeed = findSpeedBetween2Points(
        i, i + 1, directionForEachGps[i]);

        auto newSpeed = interpolateSpeed(
            prevSpeed, nextSpeed,
            gpsEvents[i], gpsEvents[i - 1], gpsEvents[i + 1]);

        if (newSpeed > -MAX_POSSIBLE_SPEED
            && newSpeed < MAX_POSSIBLE_SPEED)
        {
            gpsEvents[i].speed = newSpeed;
        }
    }

    return gpsEvents;
}


GpsEvents use2PointsGpsAzimuth(GpsEvents gpsEvents)
{
    const Meters MIN_DISTANCE_TO_CALC_AZIMUTH(0.1);
    const MetersPerSec MIN_SPEED_TO_CALC_AZIMUTH(0.1);

    for (size_t i = 1; i < gpsEvents.size() - 1; i++) {
        if (distanceMeters(gpsEvents[i-1].mercatorPos,
                           gpsEvents[i].mercatorPos) < MIN_DISTANCE_TO_CALC_AZIMUTH
            || distanceMeters(gpsEvents[i].mercatorPos,
                              gpsEvents[i+1].mercatorPos) < MIN_DISTANCE_TO_CALC_AZIMUTH
            || !gpsEvents[i].speed
            || gpsEvents[i].speed < MIN_SPEED_TO_CALC_AZIMUTH)
        {
            gpsEvents[i].direction = std::nullopt;
            continue;
        }
        auto direction1 = geolib3::Direction2(
                geolib3::Segment2(gpsEvents[i - 1].mercatorPos,
                                  gpsEvents[i].mercatorPos));
        auto direction2 = geolib3::Direction2(
                geolib3::Segment2(gpsEvents[i].mercatorPos,
                                  gpsEvents[i + 1].mercatorPos));
        auto diff = geolib3::orientedAngleBetween(direction1, direction2);
        gpsEvents[i].direction = direction1.radians() + diff / 2;
    }
    return gpsEvents;
}

} // namespace maps::mrc::pos_improvment
