#include <maps/wikimap/mapspro/services/mrc/eye/lib/generate_hypothesis/include/directed_point.h>

#include <maps/wikimap/mapspro/services/mrc/eye/lib/location/include/rotation.h>

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

namespace maps::mrc::eye {

using namespace geolib3::literals;

DirectedPoint::DirectedPoint(geolib3::Heading direction, const geolib3::Point2& mercator)
    : heading_(direction)
    , mercator_(mercator)
{}

DirectedPoint::DirectedPoint(const db::eye::ObjectLocation& location)
{
    const auto& [heading, orientation, pitch] = decomposeRotation(location.rotation());
    heading_ = geolib3::reverse(heading);
    mercator_ = location.mercatorPos();
}

geolib3::Heading DirectedPoint::heading() const { return heading_; }

geolib3::Point2 DirectedPoint::mercator() const { return mercator_; }

geolib3::Point2 DirectedPoint::geo() const { return geolib3::mercator2GeoPoint(mercator_); }

DirectedPoint DirectedPoint::moveForward(double meters) const
{
    const double distance = geolib3::toMercatorUnits(meters, mercator_);
    const geolib3::Vector2 move = geolib3::Direction2(heading_).vector() * distance;

    return {heading_, mercator_ + move};
}

DirectedPoint DirectedPoint::rotateClockwise(geolib3::Degrees angle) const
{
    return {geolib3::normalize(heading_ + angle.value()), mercator_};
}

DirectedPoint DirectedPoint::reverse() const { return rotateClockwise(180_deg); }

} // namespace maps::mrc::eye
