#ifndef SRC_LIBSLIC3R_AABBTREELINES_HPP_
#define SRC_LIBSLIC3R_AABBTREELINES_HPP_

#include "Point.hpp"
#include "Utils.hpp"
#include "libslic3r.h"
#include "libslic3r/AABBTreeIndirect.hpp"
#include "libslic3r/Line.hpp"
#include <algorithm>
#include <cmath>
#include <type_traits>
#include <vector>

namespace Slic3r { namespace AABBTreeLines {

namespace detail {

template<typename ALineType, typename ATreeType, typename AVectorType> struct IndexedLinesDistancer
{
    using LineType   = ALineType;
    using TreeType   = ATreeType;
    using VectorType = AVectorType;
    using ScalarType = typename VectorType::Scalar;

    const std::vector<LineType> &lines;
    const TreeType              &tree;

    const VectorType origin;

    inline VectorType closest_point_to_origin(size_t primitive_index, ScalarType &squared_distance) const
    {
        Vec<LineType::Dim, typename LineType::Scalar> nearest_point;
        const LineType                               &line = lines[primitive_index];
        squared_distance = line_alg::distance_to_squared(line, origin.template cast<typename LineType::Scalar>(), &nearest_point);
        return nearest_point.template cast<ScalarType>();
    }
};

// returns number of intersections of ray starting in ray_origin and following the specified coordinate line with lines in tree
// first number is hits in positive direction of ray, second number hits in negative direction. returns neagtive numbers when ray_origin is
// on some line exactly.
template<typename LineType, typename TreeType, typename VectorType, int coordinate>
inline std::tuple<int, int> coordinate_aligned_ray_hit_count(size_t                       node_idx,
                                                             const TreeType              &tree,
                                                             const std::vector<LineType> &lines,
                                                             const VectorType            &ray_origin)
{
    static constexpr int other_coordinate = (coordinate + 1) % 2;
    using Scalar                          = typename LineType::Scalar;
    using Floating                        = typename std::conditional<std::is_floating_point<Scalar>::value, Scalar, double>::type;
    const auto &node                      = tree.node(node_idx);
    assert(node.is_valid());
    if (node.is_leaf()) {
        const LineType &line = lines[node.idx];
        if (ray_origin[other_coordinate] < std::min(line.a[other_coordinate], line.b[other_coordinate]) ||
            ray_origin[other_coordinate] >= std::max(line.a[other_coordinate], line.b[other_coordinate])) { 
                // the second inequality is nonsharp for a reason 
                //  without it, we may count contour border twice when the lines meet exactly at the spot of intersection. this prevents is
            return {0, 0};
        }

        Scalar line_max = std::max(line.a[coordinate], line.b[coordinate]);
        Scalar line_min = std::min(line.a[coordinate], line.b[coordinate]);
        if (ray_origin[coordinate] > line_max) {
            return {1, 0};
        } else if (ray_origin[coordinate] < line_min) {
            return {0, 1};
        } else {
            // find intersection of ray with line
            //  that is when ( line.a + t * (line.b - line.a) )[other_coordinate] == ray_origin[other_coordinate]
            //  t = ray_origin[oc] - line.a[oc] / (line.b[oc] - line.a[oc]);
            //  then we want to get value of intersection[ coordinate]
            //  val_c = line.a[c] + t * (line.b[c] - line.a[c]);
            //  Note that ray and line may overlap, when  (line.b[oc] - line.a[oc]) is zero
            //  In that case, we return negative number
            Floating distance_oc = line.b[other_coordinate] - line.a[other_coordinate];
            Floating t     = (ray_origin[other_coordinate] - line.a[other_coordinate]) / distance_oc;
            Floating val_c = line.a[coordinate] + t * (line.b[coordinate] - line.a[coordinate]);
            if (ray_origin[coordinate] > val_c) {
                return {1, 0};
            } else if (ray_origin[coordinate] < val_c) {
                return {0, 1};
            } else { // ray origin is on boundary
                return {-1, -1};
            }
        }
    } else {
        int         intersections_above = 0;
        int         intersections_below = 0;
        size_t      left_node_idx       = node_idx * 2 + 1;
        size_t      right_node_idx      = left_node_idx + 1;
        const auto &node_left           = tree.node(left_node_idx);
        const auto &node_right          = tree.node(right_node_idx);
        assert(node_left.is_valid());
        assert(node_right.is_valid());

        if (node_left.bbox.min()[other_coordinate] <= ray_origin[other_coordinate] &&
            node_left.bbox.max()[other_coordinate] >=
                ray_origin[other_coordinate]) {
            auto [above, below] = coordinate_aligned_ray_hit_count<LineType, TreeType, VectorType, coordinate>(left_node_idx, tree, lines,
                                                                                                               ray_origin);
            if (above < 0 || below < 0) return {-1, -1};
            intersections_above += above;
            intersections_below += below;
        }

        if (node_right.bbox.min()[other_coordinate] <= ray_origin[other_coordinate] &&
            node_right.bbox.max()[other_coordinate] >= ray_origin[other_coordinate]) {
            auto [above, below] = coordinate_aligned_ray_hit_count<LineType, TreeType, VectorType, coordinate>(right_node_idx, tree, lines,
                                                                                                               ray_origin);
            if (above < 0 || below < 0) return {-1, -1};
            intersections_above += above;
            intersections_below += below;
        }
        return {intersections_above, intersections_below};
    }
}

template<typename LineType, typename TreeType, typename VectorType>
inline std::vector<std::pair<VectorType, size_t>> get_intersections_with_line(size_t                                node_idx,
                                                                              const TreeType                       &tree,
                                                                              const std::vector<LineType>          &lines,
                                                                              const LineType                       &line,
                                                                              const typename TreeType::BoundingBox &line_bb)
{
    const auto &node = tree.node(node_idx);
    assert(node.is_valid());
    if (node.is_leaf()) {
        VectorType intersection_pt;
        if (line_alg::intersection(line, lines[node.idx], &intersection_pt)) {
            return {std::pair<VectorType, size_t>(intersection_pt, node.idx)};
        } else {
            return {};
        }
    } else {
        size_t      left_node_idx  = node_idx * 2 + 1;
        size_t      right_node_idx = left_node_idx + 1;
        const auto &node_left      = tree.node(left_node_idx);
        const auto &node_right     = tree.node(right_node_idx);
        assert(node_left.is_valid());
        assert(node_right.is_valid());

        std::vector<std::pair<VectorType, size_t>> result;

        if (node_left.bbox.intersects(line_bb)) {
            std::vector<std::pair<VectorType, size_t>> intersections =
                get_intersections_with_line<LineType, TreeType, VectorType>(left_node_idx, tree, lines, line, line_bb);
            result.insert(result.end(), intersections.begin(), intersections.end());
        }

        if (node_right.bbox.intersects(line_bb)) {
            std::vector<std::pair<VectorType, size_t>> intersections =
                get_intersections_with_line<LineType, TreeType, VectorType>(right_node_idx, tree, lines, line, line_bb);
            result.insert(result.end(), intersections.begin(), intersections.end());
        }

        return result;
    }
}

} // namespace detail

// Build a balanced AABB Tree over a vector of lines, balancing the tree
// on centroids of the lines.
// Epsilon is applied to the bounding boxes of the AABB Tree to cope with numeric inaccuracies
// during tree traversal.
template<typename LineType>
inline AABBTreeIndirect::Tree<2, typename LineType::Scalar> build_aabb_tree_over_indexed_lines(const std::vector<LineType> &lines)
{
    using TreeType = AABBTreeIndirect::Tree<2, typename LineType::Scalar>;
    //    using              CoordType      = typename TreeType::CoordType;
    using VectorType  = typename TreeType::VectorType;
    using BoundingBox = typename TreeType::BoundingBox;

    struct InputType
    {
        size_t             idx() const { return m_idx; }
        const BoundingBox &bbox() const { return m_bbox; }
        const VectorType  &centroid() const { return m_centroid; }

        size_t      m_idx;
        BoundingBox m_bbox;
        VectorType  m_centroid;
    };

    std::vector<InputType> input;
    input.reserve(lines.size());
    for (size_t i = 0; i < lines.size(); ++i) {
        const LineType &line = lines[i];
        InputType       n;
        n.m_idx      = i;
        n.m_centroid = (line.a + line.b) * 0.5;
        n.m_bbox     = BoundingBox(line.a, line.a);
        n.m_bbox.extend(line.b);
        input.emplace_back(n);
    }

    TreeType out;
    out.build(std::move(input));
    return out;
}

// Finding a closest line, its closest point and squared distance to the closest point
// Returns squared distance to the closest point or -1 if the input is empty.
// or no closer point than max_sq_dist
template<typename LineType, typename TreeType, typename VectorType>
inline typename VectorType::Scalar squared_distance_to_indexed_lines(
    const std::vector<LineType>        &lines,
    const TreeType                     &tree,
    const VectorType                   &point,
    size_t                             &hit_idx_out,
    Eigen::PlainObjectBase<VectorType> &hit_point_out,
    typename VectorType::Scalar         max_sqr_dist = std::numeric_limits<typename VectorType::Scalar>::infinity())
{
    using Scalar = typename VectorType::Scalar;
    if (tree.empty()) return Scalar(-1);
    auto distancer = detail::IndexedLinesDistancer<LineType, TreeType, VectorType>{lines, tree, point};
    return AABBTreeIndirect::detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0), max_sqr_dist,
                                                                                      hit_idx_out, hit_point_out);
}

// Returns all lines within the given radius limit
template<typename LineType, typename TreeType, typename VectorType>
inline std::vector<size_t> all_lines_in_radius(const std::vector<LineType> &lines,
                                               const TreeType              &tree,
                                               const VectorType            &point,
                                               typename VectorType::Scalar  max_distance_squared)
{
    auto distancer = detail::IndexedLinesDistancer<LineType, TreeType, VectorType>{lines, tree, point};

    if (tree.empty()) { return {}; }

    std::vector<size_t> found_lines{};
    AABBTreeIndirect::detail::indexed_primitives_within_distance_squared_recurisve(distancer, size_t(0), max_distance_squared, found_lines);
    return found_lines;
}

// return 1 if true, -1 if false, 0 for point on contour (or if cannot be determined)
template<typename LineType, typename TreeType, typename VectorType>
inline int point_outside_closed_contours(const std::vector<LineType> &lines, const TreeType &tree, const VectorType &point)
{
    if (tree.empty()) { return 1; }

    auto [hits_above, hits_below] = detail::coordinate_aligned_ray_hit_count<LineType, TreeType, VectorType, 0>(0, tree, lines, point);
    if (hits_above < 0 || hits_below < 0) {
        return 0;
    } else if (hits_above % 2 == 1 && hits_below % 2 == 1) {
        return -1;
    } else if (hits_above % 2 == 0 && hits_below % 2 == 0) {
        return 1;
    } else { // this should not happen with closed contours. lets check it in Y direction
        auto [hits_above, hits_below] = detail::coordinate_aligned_ray_hit_count<LineType, TreeType, VectorType, 1>(0, tree, lines, point);
        if (hits_above < 0 || hits_below < 0) {
            return 0;
        } else if (hits_above % 2 == 1 && hits_below % 2 == 1) {
            return -1;
        } else if (hits_above % 2 == 0 && hits_below % 2 == 0) {
            return 1;
        } else { // both results were unclear
            return 0;
        }
    }
}

template<bool sorted, typename VectorType, typename LineType, typename TreeType>
inline std::vector<std::pair<VectorType, size_t>> get_intersections_with_line(const std::vector<LineType> &lines,
                                                                              const TreeType              &tree,
                                                                              const LineType              &line)
{
    if (tree.empty()) {
        return {};
    }
    auto line_bb = typename TreeType::BoundingBox(line.a, line.a);
    line_bb.extend(line.b);

    auto intersections = detail::get_intersections_with_line<LineType, TreeType, VectorType>(0, tree, lines, line, line_bb);
    if (sorted) {
        using Floating =
            typename std::conditional<std::is_floating_point<typename LineType::Scalar>::value, typename LineType::Scalar, double>::type;

        std::vector<std::pair<Floating, std::pair<VectorType, size_t>>> points_with_sq_distance{};
        for (const auto &p : intersections) {
            points_with_sq_distance.emplace_back((p.first - line.a).template cast<Floating>().squaredNorm(), p);
        }
        std::sort(points_with_sq_distance.begin(), points_with_sq_distance.end(),
                  [](const std::pair<Floating, std::pair<VectorType, size_t>> &left,
                     std::pair<Floating, std::pair<VectorType, size_t>>       &right) { return left.first < right.first; });
        for (size_t i = 0; i < points_with_sq_distance.size(); i++) {
            intersections[i] = points_with_sq_distance[i].second;
        }
    }

    return intersections;
}

template<typename LineType> class LinesDistancer
{
public:
    using Scalar   = typename LineType::Scalar;
    using Floating = typename std::conditional<std::is_floating_point<Scalar>::value, Scalar, double>::type;
private:
    std::vector<LineType> lines;
    AABBTreeIndirect::Tree<2, Scalar> tree;

public:
    explicit LinesDistancer(const std::vector<LineType> &lines) : lines(lines)
    {
        tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(this->lines);
    }

    explicit LinesDistancer(std::vector<LineType> &&lines) : lines(lines)
    {
        tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(this->lines);
    }

    LinesDistancer() = default;

    // 1 true, -1 false, 0 cannot determine
    int outside(const Vec<2, Scalar> &point) const { return point_outside_closed_contours(lines, tree, point); }

    // negative sign means inside
    template<bool SIGNED_DISTANCE>
    std::tuple<Floating, size_t, Vec<2, Floating>> distance_from_lines_extra(const Vec<2, Scalar> &point) const
    {
        size_t           nearest_line_index_out = size_t(-1);
        Vec<2, Floating> nearest_point_out      = Vec<2, Floating>::Zero();
        Vec<2, Floating> p                      = point.template cast<Floating>();
        auto distance = AABBTreeLines::squared_distance_to_indexed_lines(lines, tree, p, nearest_line_index_out, nearest_point_out);

        if (distance < 0) {
            return {std::numeric_limits<Floating>::infinity(), nearest_line_index_out, nearest_point_out};
        }
        distance = sqrt(distance);

        if (SIGNED_DISTANCE) {
            distance *= outside(point);
        }

        return {distance, nearest_line_index_out, nearest_point_out};
    }

    template<bool SIGNED_DISTANCE> Floating distance_from_lines(const Vec<2, typename LineType::Scalar> &point) const
    {
        auto [dist, idx, np] = distance_from_lines_extra<SIGNED_DISTANCE>(point);
        return dist;
    }

    std::vector<size_t> all_lines_in_radius(const Vec<2, typename LineType::Scalar> &point, Floating radius)
    {
        return all_lines_in_radius(this->lines, this->tree, point, radius * radius);
    }

    template<bool sorted> std::vector<std::pair<Vec<2, Scalar>, size_t>> intersections_with_line(const LineType &line) const
    {
        return get_intersections_with_line<sorted, Vec<2, Scalar>>(lines, tree, line);
    }

    const LineType &get_line(size_t line_idx) const { return lines[line_idx]; }

    const std::vector<LineType> &get_lines() const { return lines; }
};

}} // namespace Slic3r::AABBTreeLines

#endif /* SRC_LIBSLIC3R_AABBTREELINES_HPP_ */