#pragma once

#include "types.h"

#include <maps/libs/geolib/include/conversion.h>
#include <maps/libs/geolib/include/direction.h>
#include <maps/libs/geolib/include/point.h>
#include <maps/libs/geolib/include/units_literals.h>

#include <algorithm>
#include <map>
#include <vector>
#include <optional>

using namespace maps::geolib3::literals;

namespace maps::mrc::pos_improvment {

// all the 3d coordinates are in the Android phone coordinate system
// https://developer.android.com/images/axis_device.png
// https://developer.android.com/guide/topics/sensors/sensors_overview

enum class EventType {
    Gps,
    Accelerometer,
    Gyroscope,

    Gravity, // gravity vector in phone coordinate system
    CarHorizontalFrontVector, // horizontal front vector of the car in
                           // the phone coordinate system
    CarGroundDirection, // car azimuth CCW from east
    CarGroundSpeed, // car ground speed (m/s)
    TurningSpeed, // car speed based on centrifugal force
    GpsSpeed, // car ground speed (m/s) from gps data
};

struct TrackEvent {
    TrackEvent() = delete;
protected:
    TrackEvent(EventType type, Time time)
        : type(type)
        , time(time) {}

public:
    EventType type;
    Time time;
};
using TrackEventPtrs = std::vector<TrackEvent const *>;

struct GpsEvent : public TrackEvent {
    GpsEvent(Time time,
             geolib3::Point2 mercatorPos,
             Meters accuracy,
             std::optional<MetersPerSec> speed,
             std::optional<geolib3::Radians> direction)
        : TrackEvent(EventType::Gps, time)
        , mercatorPos(mercatorPos)
        , accuracy(accuracy)
        , speed(speed)
        , direction(direction){}

    bool hasValidDirection() const {
        // gps direction can't exist when the car doesn't move
        constexpr MetersPerSec MIN_SPEED_THRESHOLD(0.1);
        return direction && direction != 0_rad
            && speed && abs(*speed) > MIN_SPEED_THRESHOLD;
    }

    bool operator == (const GpsEvent& other) const {
        return type == other.type
            && time == other.time
            && mercatorPos == other.mercatorPos
            && accuracy == other.accuracy
            && speed == other.speed
            && direction == other.direction;
    }

    geolib3::Point2 mercatorPos;
    Meters accuracy;
    std::optional<MetersPerSec> speed;
    std::optional<geolib3::Radians> direction; // azimuth CCW from east
};
using GpsEvents = std::vector<GpsEvent>;

using GpsSegment = std::pair<GpsEvent, GpsEvent>;
using GpsSegments = std::vector<GpsSegment>;

struct Vector3Event: public TrackEvent {
    Vector3Event() = delete;
protected:
    Vector3Event(EventType eventType, Time time, const geolib3::Vector3& values)
        : TrackEvent(eventType, time)
        , values_(values)
    {}
    geolib3::Vector3 values_;
};
using Vector3Events = std::vector<Vector3Event>;

template<EventType eventType, typename Vector3Type = geolib3::Vector3>
struct TVector3Event: public Vector3Event {
    TVector3Event(Time time, const Vector3Type& values)
        : Vector3Event(eventType, time, values)
    {}

    bool operator == (const TVector3Event& other) const {
        return type == other.type
            && time == other.time
            && values_ == other.values_;
    }

    Vector3Type& values() { return static_cast<Vector3Type&>(values_); }
    const Vector3Type& values() const { return static_cast<const Vector3Type&>(values_); }
};

template<EventType eventType, typename Vector3Type>
using TVector3Events = std::vector<TVector3Event<eventType, Vector3Type>>;

using AccelerometerEvent = TVector3Event<EventType::Accelerometer, AccelerationVector>;
using AccelerometerEvents = std::vector<AccelerometerEvent>;

using GyroscopeEvent = TVector3Event<EventType::Gyroscope, RotationSpeedVector>;
using GyroscopeEvents = std::vector<GyroscopeEvent>;

// gravity direction, the length is always 1
using GravityEvent = TVector3Event<EventType::Gravity, UnitVector3>;
using GravityEvents = std::vector<GravityEvent>;

// horizontal front vector of the car in the Android coordinate system.
// the length is always 1.
using CarHorizontalFrontVecEvent = TVector3Event<EventType::CarHorizontalFrontVector, UnitVector3>;
using CarHorizontalFrontVecEvents = std::vector<CarHorizontalFrontVecEvent>;

struct GpsSpeedEvent: public Vector3Event {
    GpsSpeedEvent(const GpsEvent& gpsEvent)
        : Vector3Event(EventType::GpsSpeed,
                       gpsEvent.time,
                       geolib3::Vector3{gpsEvent.speed->value(), 0, 0})
    {}

    MetersPerSec gpsSpeed() const { return MetersPerSec(values_.x()); }
};
using GpsSpeedEvents = std::vector<GpsSpeedEvent>;

struct CarGroundDirectionEvent : public TrackEvent {
    CarGroundDirectionEvent(EventType type, Time time, geolib3::Radians angle)
        : TrackEvent(type, time)
        , angle(angle) {};

    geolib3::Radians angle;
};
using CarGroundDirectionEvents = std::vector<CarGroundDirectionEvent>;

struct CarGroundSpeedEvent : public TrackEvent {
    CarGroundSpeedEvent(EventType type, Time time, MetersPerSec speed)
        : TrackEvent(type, time)
        , speed(speed) {};

    MetersPerSec speed;
};
using CarGroundSpeedEvents = std::vector<CarGroundSpeedEvent>;

using TurnSpeedEvent = CarGroundSpeedEvent;
using TurnSpeedEvents = std::vector<TurnSpeedEvent>;

// Merge two vectors of events to the first vector.
// The resulting vector contains only pointers.
// If both vectors are sorted by time, the resulting vector is also
// sorted by time
template<typename Event>
TrackEventPtrs& insert(TrackEventPtrs& expanded,
                       const std::vector<Event>& toAdd)
{
    for (const auto& event : toAdd) {
        expanded.push_back(&event);
    }
    std::inplace_merge(expanded.begin(),
                       expanded.end() - toAdd.size(),
                       expanded.end(),
                       [](const TrackEvent* lhs, const TrackEvent* rhs) {
                           return lhs->time < rhs->time;
                       });

    return expanded;
}

// Merge two vectors of events to the first vector.
// If both vectors are sorted by time, the resulting vector is also
// sorted by time
template<typename Vector3EventType>
Vector3Events& insert(Vector3Events& expanded,
                      const std::vector<Vector3EventType>& toAdd)
{
    for (const auto& event : toAdd) {
        expanded.push_back(static_cast<const Vector3Event&>(event));
    }
    std::inplace_merge(
        expanded.begin(),
        expanded.end() - toAdd.size(),
        expanded.end(),
        [](const Vector3Event& lhs, const Vector3Event& rhs) {
            return lhs.time < rhs.time;
        });

    return expanded;
}

} // namespace maps::mrc::pos_improvment
