#pragma once

#include <cmath>

#include <maps/libs/tagged/include/tagged.h>
#include <maps/libs/geolib/include/units.h>
#include <maps/libs/geolib/include/vector.h>

#include <chrono>


namespace maps::mrc::pos_improvment {


namespace unit {

struct Meters;
struct MetersPerSec;
struct MetersPerSec2; // m/(s^2)
struct RadiansPerSec;

} // namespace unit

using Meters = tagged::Tagged<unit::Meters, double>;
using MetersPerSec = tagged::Tagged<unit::MetersPerSec, double>;
using MetersPerSec2 = tagged::Tagged<unit::MetersPerSec2, double>;
using RadiansPerSec = tagged::Tagged<unit::RadiansPerSec, double>;

using Seconds = std::chrono::duration<double>;
using Time = std::chrono::time_point<std::chrono::system_clock, Seconds>;


constexpr Seconds operator"" _sec (long double value)
{
    return Seconds(static_cast<double>(value));
}

constexpr Meters operator"" _m (long double value)
{
    return Meters(static_cast<double>(value));
}

constexpr MetersPerSec operator"" _mps (long double value)
{
    return MetersPerSec(static_cast<double>(value));
}

constexpr MetersPerSec2 operator"" _mps2 (long double value)
{
    return MetersPerSec2(static_cast<double>(value));
}

constexpr RadiansPerSec operator"" _rad_p_sec (long double value)
{
    return RadiansPerSec(static_cast<double>(value));
}


inline Meters operator*(MetersPerSec speed, Seconds time)
{
    return Meters{speed.value() * time.count()};
}

inline MetersPerSec operator*(MetersPerSec2 acceleration, Seconds time)
{
    return MetersPerSec{acceleration.value() * time.count()};
}

inline MetersPerSec operator/(Meters distance, Seconds time)
{
    return MetersPerSec{distance.value() / time.count()};
}

inline geolib3::Radians operator*(RadiansPerSec rotatingSpeed, Seconds time)
{
    return geolib3::Radians{rotatingSpeed.value() * time.count()};
}

inline MetersPerSec2 operator/(MetersPerSec speedDelta, Seconds time)
{
    return MetersPerSec2{speedDelta.value() / time.count()};
}

inline MetersPerSec operator/(MetersPerSec2 acceleration, RadiansPerSec rotationSpeed)
{
    return MetersPerSec{acceleration.value() / rotationSpeed.value()};
}

struct UnitVector3 : geolib3::Vector3
{
    explicit UnitVector3(const geolib3::Vector3& vector3)
        : geolib3::Vector3(vector3)
    {
        this->normalize();
    }
};

inline geolib3::Radians angleBetween(UnitVector3 v1, UnitVector3 v2) {
    return geolib3::Radians(acos(geolib3::innerProduct(v1, v2)));
}

// Acceleration vector in phone coordinate system in (m/s^2)
struct AccelerationVector : public geolib3::Vector3
{
    using geolib3::Vector3::Vector3;

    // @param axis should have length = 1
    inline MetersPerSec2 accAlongAxis(const UnitVector3& axis) const
    {
        return MetersPerSec2{geolib3::innerProduct(*this, axis)};
    }

    inline AccelerationVector operator- (const AccelerationVector& other) const
    {
        geolib3::Vector3 result = *static_cast<const geolib3::Vector3*>(this) -
            static_cast<const geolib3::Vector3&>(other);
        return static_cast<AccelerationVector&>(result);
    }

    inline geolib3::Vector3 operator/ (MetersPerSec2 accValue) const
    {
        return *this / accValue.value();
    }
};

// Rotation speed vector in phone coordinate system.
// The direction of the vector is the axis, the length of the vector
// is the rotating speed around axis in rad/s
struct RotationSpeedVector : public geolib3::Vector3
{
    using geolib3::Vector3::Vector3;

    inline RadiansPerSec rotSpeedAroundAxis(const UnitVector3& axis) const
    {
        return RadiansPerSec{geolib3::innerProduct(*this, axis)};
    }
};

inline MetersPerSec2 length(const AccelerationVector& vector3) {
    return MetersPerSec2{geolib3::length(vector3)};
}

inline RadiansPerSec length(const RotationSpeedVector& vector3) {
    return RadiansPerSec{geolib3::length(vector3)};
}

inline AccelerationVector operator* (geolib3::Vector3 vec,
                                     MetersPerSec2 acceleration) {
    vec *= acceleration.value();
    return static_cast<AccelerationVector&>(vec);
}

} // namespace maps::mrc::pos_improvment
