#ifndef SRC_LIBSLIC3R_AABBTREELINES_HPP_
#define SRC_LIBSLIC3R_AABBTREELINES_HPP_

#include "libslic3r/AABBTreeIndirect.hpp"
#include "libslic3r/Line.hpp"

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) {
        VectorType nearest_point;
        const LineType &line = lines[primitive_index];
        squared_distance = line_alg::distance_to_squared(line, origin, &nearest_point);
        return nearest_point;
    }
};

}

// Build a balanced AABB Tree over a vector of float 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,
        //FIXME do we want to apply an epsilon?
        const float eps = 0)
        {
    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());
    const VectorType veps(eps, eps);
    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);
        n.m_bbox.min() -= veps;
        n.m_bbox.max() += veps;
        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, 
    typename Scalar = typename VectorType::Scalar>
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,
    Scalar max_sqr_dist = std::numeric_limits<Scalar>::infinity())
{
    if (tree.empty()) return Scalar(-1);
    auto distancer = detail::IndexedLinesDistancer<LineType, TreeType, VectorType>
            { lines, tree, point };
    hit_idx_out     = std::numeric_limits<size_t>::max();
    Scalar distance = AABBTreeIndirect::detail::squared_distance_to_indexed_primitives_recursive(
        distancer, size_t(0), Scalar(0), max_sqr_dist, hit_idx_out, hit_point_out);
    if (hit_idx_out == std::numeric_limits<size_t>::max()) return Scalar(-1);
    return distance;
}

}
}

#endif /* SRC_LIBSLIC3R_AABBTREELINES_HPP_ */