#include "object_rays_intersection.h"

#include <maps/libs/geolib/include/distance.h>
#include <maps/libs/geolib/include/units.h>

namespace maps::mrc::sensors_feature_positioner {

const double MIN_METERS_TO_SIGN = 3.0;
const double MAX_METERS_TO_SIGN = 80;

const double MAX_INTERSECTION_ERROR_METERS = 2;
const geolib3::Radians MAX_INTERSECTION_ERROR_RADIANS(0.15);

// Returns {p1, p2} where segment
// (line1Begin + p1 * line1Direction,
//  line2Begin + p2 * line2Direction)
// is the shortest segment between these lines
// http://geomalgorithms.com/a07-_distance.html
std::pair<double, double> shortestSegmentBetweenRays(
    geolib3::Point3 ray1Begin, geolib3::Vector3 ray1Direction,
    geolib3::Point3 ray2Begin, geolib3::Vector3 ray2Direction)
{
    geolib3::Vector3 w = ray1Begin - ray2Begin;

    double a = geolib3::squaredLength(ray2Direction);
    double b = geolib3::innerProduct(ray1Direction, ray2Direction);
    double c = geolib3::squaredLength(ray1Direction);
    double d = geolib3::innerProduct(w, ray1Direction);
    double e = geolib3::innerProduct(w, ray2Direction);

    double D = a * c - b * b;

    REQUIRE(std::abs(D) > 0.001, "Rays are almost parallel");

    double p1 = (b * e - c * d) / D;
    double p2 = (a * e - b * d) / D;

    return {p1, p2};
}

ObjectIntersection getIntersectionPosition(const Ray& ray1,
                                           const Ray& ray2,
                                           IdGenerator& idGenerator)
{
    geolib3::Point3 camera1Pos(ray1.cameraPos.odometerMercatorPosition().x(),
                               ray1.cameraPos.odometerMercatorPosition().y(),
                               0);
    geolib3::Point3 camera2Pos(ray2.cameraPos.odometerMercatorPosition().x(),
                               ray2.cameraPos.odometerMercatorPosition().y(),
                               0);

    geolib3::Vector3 camera1Dir
        = ray1.directionToObject
        * geolib3::toMercatorUnits(1, ray1.cameraPos.mercatorPosition());

    geolib3::Vector3 camera2Dir
        = ray2.directionToObject
        * geolib3::toMercatorUnits(1, ray1.cameraPos.mercatorPosition());
    // length of each cameraDir vector is still 1 meter

    auto [distanceFromCamera1, distanceFromCamera2]
        = shortestSegmentBetweenRays(camera1Pos, camera1Dir,
                                     camera2Pos, camera2Dir);

    geolib3::Point3 objectPos1 = camera1Pos + camera1Dir * distanceFromCamera1;
    geolib3::Point3 objectPos2 = camera2Pos + camera2Dir * distanceFromCamera2;
    geolib3::Point3 objectPos = objectPos1 + (objectPos2 - objectPos1) / 2.0;

    double errorMeters = geolib3::toMeters(geolib3::length(objectPos2 - objectPos1),
                                           ray1.cameraPos.mercatorPosition());

    geolib3::Radians photo1AngleError(std::atan2(errorMeters,
                                                 distanceFromCamera1));
    geolib3::Radians photo2AngleError(std::atan2(errorMeters,
                                                 distanceFromCamera2));
    geolib3::Radians errorRadians = std::max(photo1AngleError,
                                             photo2AngleError);
    return ObjectIntersection{idGenerator.newId(),
                              objectPos,
                              errorMeters,
                              errorRadians,
                              ray1,
                              ray2,
                              distanceFromCamera1,
                              distanceFromCamera2};
}

std::optional<ObjectIntersection> createObjectFrom2Photos(const Ray& ray1,
                                                          const Ray& ray2,
                                                          IdGenerator& idGenerator)
{
    if (ray1.objectTypeId != ray2.objectTypeId) {
        return std::nullopt;
    }

    double distanceBetweenPhotos = geolib3::toMeters(
        geolib3::distance(ray1.cameraPos.odometerMercatorPosition(),
                          ray2.cameraPos.odometerMercatorPosition()),
        ray1.cameraPos.mercatorPosition());
    if (distanceBetweenPhotos > MAX_METERS_BETWEEN_CAMERAS
        || distanceBetweenPhotos < MIN_METERS_BETWEEN_CAMERAS)
    {
        return std::nullopt;
    }

    double cosBetweenCameras = geolib3::innerProduct(ray1.directionToObject,
                                                     ray2.directionToObject);
    if (cosBetweenCameras > geolib3::cos(MIN_ANGLE_BETWEEN_CAMERAS)
        || cosBetweenCameras < geolib3::cos(MAX_ANGLE_BETWEEN_CAMERAS))
    {
        return std::nullopt;
    }

    ObjectIntersection objectPosition = getIntersectionPosition(ray1, ray2,
                                                            idGenerator);

    if (objectPosition.metersFromCamera1 < MIN_METERS_TO_SIGN
        || objectPosition.metersFromCamera1 > MAX_METERS_TO_SIGN
        || objectPosition.metersFromCamera2 < MIN_METERS_TO_SIGN
        || objectPosition.metersFromCamera2 > MAX_METERS_TO_SIGN)
    {
        return std::nullopt;
    }

    if (objectPosition.errorMeters > MAX_INTERSECTION_ERROR_METERS) {
        return std::nullopt;
    }
    if (objectPosition.errorRadians > MAX_INTERSECTION_ERROR_RADIANS) {
        return std::nullopt;
    }

    return objectPosition;
}

} // namespace maps::mrc::sensors_feature_positioner
