#include "util/coordinate_calculation.hpp"
#include "util/cheap_ruler.hpp"
#include "util/coordinate.hpp"
#include "util/trigonometry_table.hpp"
#include "util/web_mercator.hpp"

#include <boost/assert.hpp>

#include <algorithm>
#include <iterator>
#include <limits>
#include <optional>
#include <utility>

namespace osrm::util::coordinate_calculation
{

namespace
{
class CheapRulerContainer
{
  public:
    CheapRulerContainer(const int number_of_rulers)
        : cheap_ruler_cache(number_of_rulers, mapbox::cheap_ruler::CheapRuler(0)),
          step(90.0 * COORDINATE_PRECISION / number_of_rulers)
    {
        for (int n = 0; n < number_of_rulers; n++)
        {
            cheap_ruler_cache[n] =
                mapbox::cheap_ruler::CheapRuler(step * (n + 0.5) / COORDINATE_PRECISION);
        }
    };

    mapbox::cheap_ruler::CheapRuler &getRuler(const FixedLatitude lat_1, const FixedLatitude lat_2)
    {
        auto lat = (lat_1 + lat_2) / util::FixedLatitude{2};
        return getRuler(lat);
    }

    mapbox::cheap_ruler::CheapRuler &getRuler(const FixedLatitude lat)
    {
        BOOST_ASSERT(step > 2);
        // the |lat| > 0  ->  |lat|-1 > -1  ->  (|lat|-1)/step > -1/step > -1/2 >= -1  ->  bin >= 0
        std::size_t bin = (std::abs(static_cast<int>(lat)) - 1) / step;
        BOOST_ASSERT(bin < cheap_ruler_cache.size());
        return cheap_ruler_cache[bin];
    };

  private:
    std::vector<mapbox::cheap_ruler::CheapRuler> cheap_ruler_cache;
    const int step;
};
static CheapRulerContainer cheap_ruler_container(1800);
} // namespace

// Does not project the coordinates!
std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs)
{
    std::int64_t d_lon = static_cast<std::int32_t>(lhs.lon - rhs.lon);
    std::int64_t d_lat = static_cast<std::int32_t>(lhs.lat - rhs.lat);

    std::int64_t sq_lon = d_lon * d_lon;
    std::int64_t sq_lat = d_lat * d_lat;

    std::uint64_t result = static_cast<std::uint64_t>(sq_lon + sq_lat);

    return result;
}

double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coordinate_2)
{
    // Should be within 0.1% or so of Vincenty method (assuming 19 buckets are enough)
    // Should be more faster and more precise than Haversine
    const auto lon1 = static_cast<double>(util::toFloating(coordinate_1.lon));
    const auto lat1 = static_cast<double>(util::toFloating(coordinate_1.lat));
    const auto lon2 = static_cast<double>(util::toFloating(coordinate_2.lon));
    const auto lat2 = static_cast<double>(util::toFloating(coordinate_2.lat));
    return cheap_ruler_container.getRuler(coordinate_1.lat, coordinate_2.lat)
        .distance({lon1, lat1}, {lon2, lat2});
}

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location,
                             Coordinate &nearest_location,
                             double &ratio)
{
    using namespace coordinate_calculation;

    BOOST_ASSERT(query_location.IsValid());

    FloatCoordinate projected_nearest;
    std::tie(ratio, projected_nearest) =
        projectPointOnSegment(web_mercator::fromWGS84(segment_source),
                              web_mercator::fromWGS84(segment_target),
                              web_mercator::fromWGS84(query_location));
    nearest_location = web_mercator::toWGS84(projected_nearest);

    const double approximate_distance = greatCircleDistance(query_location, nearest_location);
    BOOST_ASSERT(0.0 <= approximate_distance);
    return approximate_distance;
}

double perpendicularDistance(const Coordinate source_coordinate,
                             const Coordinate target_coordinate,
                             const Coordinate query_location)
{
    double ratio;
    Coordinate nearest_location;

    return perpendicularDistance(
        source_coordinate, target_coordinate, query_location, nearest_location, ratio);
}

Coordinate centroid(const Coordinate lhs, const Coordinate rhs)
{
    Coordinate centroid;
    // The coordinates of the midpoints are given by:
    // x = (x1 + x2) /2 and y = (y1 + y2) /2.
    centroid.lon = (lhs.lon + rhs.lon) / FixedLongitude{2};
    centroid.lat = (lhs.lat + rhs.lat) / FixedLatitude{2};
    return centroid;
}

double bearing(const Coordinate coordinate_1, const Coordinate coordinate_2)
{
    const auto lon1 = static_cast<double>(util::toFloating(coordinate_1.lon));
    const auto lat1 = static_cast<double>(util::toFloating(coordinate_1.lat));
    const auto lon2 = static_cast<double>(util::toFloating(coordinate_2.lon));
    const auto lat2 = static_cast<double>(util::toFloating(coordinate_2.lat));
    const auto &ruler = cheap_ruler_container.getRuler(coordinate_1.lat, coordinate_2.lat);
    auto result = ruler.bearing({lon1, lat1}, {lon2, lat2});
    if (result < 0.0)
    {
        result += 360.0;
    }
    BOOST_ASSERT(0 <= result && result <= 360);

    // If someone gives us two identical coordinates, then the concept of a bearing
    // makes no sense.  However, because it sometimes happens, we'll at least
    // return a consistent value of 0 so that the behaviour isn't random.
    BOOST_ASSERT(coordinate_1 != coordinate_2 || result == 0.);

    return result;
}

double computeAngle(const Coordinate first, const Coordinate second, const Coordinate third)
{
    using namespace coordinate_calculation;

    if (first == second || second == third)
        return 180;

    BOOST_ASSERT(first.IsValid());
    BOOST_ASSERT(second.IsValid());
    BOOST_ASSERT(third.IsValid());

    const double v1x = static_cast<double>(toFloating(first.lon - second.lon));
    const double v1y =
        web_mercator::latToY(toFloating(first.lat)) - web_mercator::latToY(toFloating(second.lat));
    const double v2x = static_cast<double>(toFloating(third.lon - second.lon));
    const double v2y =
        web_mercator::latToY(toFloating(third.lat)) - web_mercator::latToY(toFloating(second.lat));

    double angle = (atan2_lookup(v2y, v2x) - atan2_lookup(v1y, v1x)) * 180. * std::numbers::inv_pi;

    while (angle < 0.)
    {
        angle += 360.;
    }

    BOOST_ASSERT(angle >= 0);
    return angle;
}

std::optional<Coordinate>
circleCenter(const Coordinate C1, const Coordinate C2, const Coordinate C3)
{
    // free after http://paulbourke.net/geometry/circlesphere/
    // require three distinct points
    if (C1 == C2 || C2 == C3 || C1 == C3)
    {
        return std::nullopt;
    }

    // define line through c1, c2 and c2,c3
    const double C2C1_lat = static_cast<double>(toFloating(C2.lat - C1.lat)); // yDelta_a
    const double C2C1_lon = static_cast<double>(toFloating(C2.lon - C1.lon)); // xDelta_a
    const double C3C2_lat = static_cast<double>(toFloating(C3.lat - C2.lat)); // yDelta_b
    const double C3C2_lon = static_cast<double>(toFloating(C3.lon - C2.lon)); // xDelta_b

    // check for collinear points in X-Direction / Y-Direction
    if ((std::abs(C2C1_lon) < std::numeric_limits<double>::epsilon() &&
         std::abs(C3C2_lon) < std::numeric_limits<double>::epsilon()) ||
        (std::abs(C2C1_lat) < std::numeric_limits<double>::epsilon() &&
         std::abs(C3C2_lat) < std::numeric_limits<double>::epsilon()))
    {
        return std::nullopt;
    }
    else if (std::abs(C2C1_lon) < std::numeric_limits<double>::epsilon())
    {
        // vertical line C2C1
        // due to c1.lon == c2.lon && c1.lon != c3.lon we can rearrange this way
        BOOST_ASSERT(std::abs(static_cast<double>(toFloating(C3.lon - C1.lon))) >=
                         std::numeric_limits<double>::epsilon() &&
                     std::abs(static_cast<double>(toFloating(C2.lon - C3.lon))) >=
                         std::numeric_limits<double>::epsilon());
        return circleCenter(C1, C3, C2);
    }
    else if (std::abs(C3C2_lon) < std::numeric_limits<double>::epsilon())
    {
        // vertical line C3C2
        // due to c2.lon == c3.lon && c1.lon != c3.lon we can rearrange this way
        // after rearrangement both deltas will be zero
        BOOST_ASSERT(std::abs(static_cast<double>(toFloating(C1.lon - C2.lon))) >=
                         std::numeric_limits<double>::epsilon() &&
                     std::abs(static_cast<double>(toFloating(C3.lon - C1.lon))) >=
                         std::numeric_limits<double>::epsilon());
        return circleCenter(C2, C1, C3);
    }
    else
    {
        const double C2C1_slope = C2C1_lat / C2C1_lon;
        const double C3C2_slope = C3C2_lat / C3C2_lon;

        if (std::abs(C2C1_slope) < std::numeric_limits<double>::epsilon())
        {
            // Three non-collinear points with C2,C1 on same latitude.
            // Due to the x-values correct, we can swap C3 and C1 to obtain the correct slope value
            return circleCenter(C3, C2, C1);
        }
        // valid slope values for both lines, calculate the center as intersection of the lines

        // can this ever happen?
        if (std::abs(C2C1_slope - C3C2_slope) < std::numeric_limits<double>::epsilon())
            return std::nullopt;

        const double C1_y = static_cast<double>(toFloating(C1.lat));
        const double C1_x = static_cast<double>(toFloating(C1.lon));
        const double C2_y = static_cast<double>(toFloating(C2.lat));
        const double C2_x = static_cast<double>(toFloating(C2.lon));
        const double C3_y = static_cast<double>(toFloating(C3.lat));
        const double C3_x = static_cast<double>(toFloating(C3.lon));

        const double lon = (C2C1_slope * C3C2_slope * (C1_y - C3_y) + C3C2_slope * (C1_x + C2_x) -
                            C2C1_slope * (C2_x + C3_x)) /
                           (2 * (C3C2_slope - C2C1_slope));
        const double lat = (0.5 * (C1_x + C2_x) - lon) / C2C1_slope + 0.5 * (C1_y + C2_y);
        if (lon < -180.0 || lon > 180.0 || lat < -90.0 || lat > 90.0)
            return std::nullopt;
        else
            return Coordinate(FloatLongitude{lon}, FloatLatitude{lat});
    }
}

double circleRadius(const Coordinate C1, const Coordinate C2, const Coordinate C3)
{
    // a circle by three points requires thee distinct points
    auto center = circleCenter(C1, C2, C3);
    if (center)
        return greatCircleDistance(C1, *center);
    else
        return std::numeric_limits<double>::infinity();
}

Coordinate interpolateLinear(double factor, const Coordinate from, const Coordinate to)
{
    BOOST_ASSERT(0 <= factor && factor <= 1.0);

    const auto from_lon = static_cast<std::int32_t>(from.lon);
    const auto from_lat = static_cast<std::int32_t>(from.lat);
    const auto to_lon = static_cast<std::int32_t>(to.lon);
    const auto to_lat = static_cast<std::int32_t>(to.lat);

    FixedLongitude interpolated_lon{
        static_cast<std::int32_t>(from_lon + factor * (to_lon - from_lon))};
    FixedLatitude interpolated_lat{
        static_cast<std::int32_t>(from_lat + factor * (to_lat - from_lat))};

    return {interpolated_lon, interpolated_lat};
}

// compute the signed area of a triangle
double signedArea(const Coordinate first_coordinate,
                  const Coordinate second_coordinate,
                  const Coordinate third_coordinate)
{
    const auto lat_1 = static_cast<double>(toFloating(first_coordinate.lat));
    const auto lon_1 = static_cast<double>(toFloating(first_coordinate.lon));
    const auto lat_2 = static_cast<double>(toFloating(second_coordinate.lat));
    const auto lon_2 = static_cast<double>(toFloating(second_coordinate.lon));
    const auto lat_3 = static_cast<double>(toFloating(third_coordinate.lat));
    const auto lon_3 = static_cast<double>(toFloating(third_coordinate.lon));
    return 0.5 * (-lon_2 * lat_1 + lon_3 * lat_1 + lon_1 * lat_2 - lon_3 * lat_2 - lon_1 * lat_3 +
                  lon_2 * lat_3);
}

// check if a set of three coordinates is given in CCW order
bool isCCW(const Coordinate first_coordinate,
           const Coordinate second_coordinate,
           const Coordinate third_coordinate)
{
    return signedArea(first_coordinate, second_coordinate, third_coordinate) > 0;
}

// find the closest distance between a coordinate and a segment
double findClosestDistance(const Coordinate coordinate,
                           const Coordinate segment_begin,
                           const Coordinate segment_end)
{
    return greatCircleDistance(
        coordinate, projectPointOnSegment(segment_begin, segment_end, coordinate).second);
}

// find the closes distance between two sets of coordinates
double findClosestDistance(const std::vector<Coordinate> &lhs, const std::vector<Coordinate> &rhs)
{
    double current_min = std::numeric_limits<double>::max();

    const auto compute_minimum_distance_in_rhs = [&current_min, &rhs](const Coordinate coordinate)
    {
        current_min =
            std::min(current_min, findClosestDistance(coordinate, rhs.begin(), rhs.end()));
        return false;
    };
    // NOLINTNEXTLINE(bugprone-unused-return-value)
    [[maybe_unused]] auto _ =
        std::find_if(std::begin(lhs), std::end(lhs), compute_minimum_distance_in_rhs);
    return current_min;
}

std::vector<double> getDeviations(const std::vector<Coordinate> &from,
                                  const std::vector<Coordinate> &to)
{
    auto find_deviation = [&to](const Coordinate coordinate)
    { return findClosestDistance(coordinate, to.begin(), to.end()); };

    std::vector<double> deviations_from;
    deviations_from.reserve(from.size());
    std::transform(
        std::begin(from), std::end(from), std::back_inserter(deviations_from), find_deviation);

    return deviations_from;
}

Coordinate rotateCCWAroundZero(Coordinate coordinate, double angle_in_radians)
{
    /*
     * a rotation  around 0,0 in vector space is defined as
     *
     * | cos a   -sin a | . | lon |
     * | sin a    cos a |   | lat |
     *
     * resulting in cos a lon - sin a lon for the new longitude and sin a lon + cos a lat for the
     * new latitude
     */

    const auto cos_alpha = cos(angle_in_radians);
    const auto sin_alpha = sin(angle_in_radians);

    const auto lon = static_cast<double>(toFloating(coordinate.lon));
    const auto lat = static_cast<double>(toFloating(coordinate.lat));

    return {util::FloatLongitude{cos_alpha * lon - sin_alpha * lat},
            util::FloatLatitude{sin_alpha * lon + cos_alpha * lat}};
}

Coordinate difference(const Coordinate lhs, const Coordinate rhs)
{
    const auto lon_diff_int = static_cast<int>(lhs.lon) - static_cast<int>(rhs.lon);
    const auto lat_diff_int = static_cast<int>(lhs.lat) - static_cast<int>(rhs.lat);
    return {util::FixedLongitude{lon_diff_int}, util::FixedLatitude{lat_diff_int}};
}

double computeArea(const std::vector<Coordinate> &polygon)
{
    using util::coordinate_calculation::greatCircleDistance;

    if (polygon.empty())
        return 0.;

    BOOST_ASSERT(polygon.front() == polygon.back());

    // Take the reference point with the smallest latitude.
    // ⚠ ref_latitude is the standard parallel for the equirectangular projection
    // that is not an area-preserving projection
    const auto ref_point =
        std::min_element(polygon.begin(),
                         polygon.end(),
                         [](const auto &lhs, const auto &rhs) { return lhs.lat < rhs.lat; });
    const auto ref_latitude = ref_point->lat;

    // Compute area of under a curve and a line that is parallel the equator with ref_latitude
    // For closed curves it corresponds to the shoelace algorithm for polygon areas
    double area = 0.;
    auto first = polygon.begin();
    auto previous_base = util::Coordinate{first->lon, ref_latitude};
    auto previous_y = greatCircleDistance(previous_base, *first);
    for (++first; first != polygon.end(); ++first)
    {
        BOOST_ASSERT(first->lat >= ref_latitude);

        const auto current_base = util::Coordinate{first->lon, ref_latitude};
        const auto current_y = greatCircleDistance(current_base, *first);
        const auto chunk_area =
            greatCircleDistance(previous_base, current_base) * (previous_y + current_y);

        area += (current_base.lon >= previous_base.lon) ? chunk_area : -chunk_area;

        previous_base = current_base;
        previous_y = current_y;
    }

    return area / 2.;
}

} // namespace osrm::util::coordinate_calculation