//
//! Copyright © 2017
//! Brandon Kohn
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef GEOMETRIX_SOLID_LEAF_BSPTREE_HPP
#define GEOMETRIX_SOLID_LEAF_BSPTREE_HPP
#pragma once

#include <geometrix/primitive/axis_aligned_bounding_box.hpp>
#include <geometrix/utility/unique_ptr.hpp>
#include <geometrix/algorithm/classify_simplex_to_plane.hpp>
#include <geometrix/algorithm/intersection/segment_plane_intersection.hpp>
#include <geometrix/algorithm/distance/point_segment_distance.hpp>
#include <geometrix/algorithm/distance/segment_segment_distance.hpp>
#include <geometrix/algorithm/distance/segment_plane_distance.hpp>
#include <geometrix/algorithm/distance/segment_plane_closest_point.hpp>
#include <geometrix/primitive/hyperplane_traits.hpp>
#include <geometrix/algorithm/intersection/ray_segment_intersection.hpp>
#include <geometrix/algorithm/point_in_solid_classification.hpp>
#include <geometrix/utility/ignore_unused_warnings.hpp>

#include <boost/range/concepts.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/mpl/inherit.hpp>
#include <boost/mpl/empty_base.hpp>
#include <stack>
#include <type_traits>

namespace geometrix {

    template <typename Length>
    struct solid_bsp_ray_tracing_result
    {
        using length_type = Length;

        typedef void (solid_bsp_ray_tracing_result::*bool_type)() const;

        solid_bsp_ray_tracing_result() = default;

        solid_bsp_ray_tracing_result(bool isIntersecting, const length_type& thit = length_type(), const std::uint32_t data = static_cast<std::uint32_t>(-1) )
            : result(isIntersecting)
            , thit(thit)
            , data(data)
        {}

        bool is_intersecting() const { return result; }
        length_type intersection_distance() const { return thit; }

        operator bool_type() const
        {
            return is_intersecting() ? &solid_bsp_ray_tracing_result::bool_type_mfn : 0;
        }

        bool operator == (bool b) const { return b == is_intersecting(); }

        void set_data(std::uint32_t d) { data = d; }
        std::uint32_t get_data() const { return data; }

    private:

        void bool_type_mfn() const {}

        bool result{ false };
        length_type thit;
        std::uint32_t data{ static_cast<std::uint32_t>(-1) };
    };

    namespace bsp_detail
    {
        //! Split a segment.
        template <typename Simplex, typename Hyperplane, typename NumberComparisonPolicy, typename std::enable_if<is_segment<Simplex>::value, int>::type = 0>
        inline void split_simplex(const Simplex &smplx, const Hyperplane& plane, Simplex& frontPoly, Simplex& backPoly, const NumberComparisonPolicy& cmp)
        {
            using simplex_t = typename std::decay<Simplex>::type;
            using point_t = typename geometric_traits<simplex_t>::point_type;
            using dimensionless_t = typename geometric_traits<point_t>::dimensionless_type;

            auto a = get_vertex(smplx, 0);
            auto b = get_vertex(smplx, 1);

            //! Create (and return) two new simplices from the two vertex lists
            auto aSide = classify_point_to_plane(a, plane, cmp);
            auto bSide = classify_point_to_plane(b, plane, cmp);
            if (aSide == plane_orientation::in_front_of_plane)
            {
                if (bSide == plane_orientation::in_back_of_plane)
                {
                    //! Edge (a, b) straddles, output intersection point to both sides
                    dimensionless_t t = constants::zero<dimensionless_t>();
                    point_t xpoint;
                    auto intersects = segment_plane_intersection(b, a, plane, t, xpoint, cmp);
                    ignore_unused_warning_of(intersects);
                    GEOMETRIX_ASSERT(intersects);
                    GEOMETRIX_ASSERT(classify_point_to_plane(xpoint, plane, cmp) == plane_orientation::coplanar_with_plane);

                    frontPoly = construct<Simplex>(a, xpoint);
                    backPoly = construct<Simplex>(xpoint, b);
                } else
                {
                    GEOMETRIX_ASSERT(false);//! This case should not happen.
                    GEOMETRIX_ASSERT(bSide == plane_orientation::coplanar_with_plane);//! If both are on front.. why the split?
                    frontPoly = construct<Simplex>(a, b);
                }
            } else if (aSide == plane_orientation::in_back_of_plane)
            {
                if (bSide == plane_orientation::in_front_of_plane)
                {
                    dimensionless_t t = constants::zero<dimensionless_t>();
                    point_t xpoint;
                    auto intersects = segment_plane_intersection(b, a, plane, t, xpoint, cmp);
                    ignore_unused_warning_of(intersects);
                    GEOMETRIX_ASSERT(intersects);
                    GEOMETRIX_ASSERT(classify_point_to_plane(xpoint, plane, cmp) == plane_orientation::coplanar_with_plane);

                    backPoly = construct<Simplex>(a, xpoint);
                    frontPoly = construct<Simplex>(xpoint, b);
                } else
                {
                    GEOMETRIX_ASSERT(false);//! This case should not happen.
                    GEOMETRIX_ASSERT(bSide == plane_orientation::coplanar_with_plane);//! If both are on back.. should not split?
                    backPoly = construct<Simplex>(a, b);
                }
            } else
            {
                GEOMETRIX_ASSERT(false);//! This case should not happen.
                //! A is on the plane.
                if (bSide == plane_orientation::in_back_of_plane)
                    backPoly = construct<Simplex>(a, b);
                else
                    frontPoly = construct<Simplex>(a, b);
            }
        }

        //! Split polygon
        template <typename Simplex, typename Hyperplane, typename NumberComparisonPolicy, typename std::enable_if<!is_segment<Simplex>::value, int>::type = 0>
        inline void split_simplex(const Simplex &smplx, const Hyperplane& plane, Simplex& frontPoly, Simplex& backPoly, const NumberComparisonPolicy& cmp)
        {
            using simplex_t = typename std::decay<Simplex>::type;
            using point_t = typename geometric_traits<simplex_t>::point_type;
            using dimensionless_t = typename geometric_traits<point_t>::dimensionless_type;

            std::vector<point_t> frontVerts, backVerts;
            // Test all edges (a, b) starting with edge from last to first vertex
            auto numVerts = number_vertices(smplx);
            auto a = get_vertex(smplx, numVerts - 1);
            auto aSide = classify_point_to_plane(a, plane, cmp);
            // Loop over all edges given by vertex pair (n -1, n)
            for (auto n = 0; n < numVerts; ++n)
            {
                auto b = get_vertex(smplx, n);
                auto bSide = classify_point_to_plane(b, plane, cmp);
                if (bSide == plane_orientation::in_front_of_plane)
                {
                    if (aSide == plane_orientation::in_back_of_plane)
                    {
                        // Edge (a, b) straddles, output intersection point to both sides
                        auto t = constants::zero<dimensionless_t>();
                        point_t xpoint;
                        auto intersects = segment_plane_intersection(b, a, plane, t, xpoint, cmp);
                        GEOMETRIX_ASSERT(intersects);
                        GEOMETRIX_ASSERT(classify_point_to_plane(xpoint, plane, cmp) == plane_orientation::coplanar_with_plane);
                        frontVerts.push_back(xpoint);
                        backVerts.push_back(xpoint);
                    }
                    // In all three cases, output b to the front side
                    frontVerts.push_back(b);
                } else if (bSide == plane_orientation::in_back_of_plane)
                {
                    if (aSide == plane_orientation::in_front_of_plane)
                    {
                        // Edge (a, b) straddles plane, output intersection point
                        auto t = constants::zero<dimensionless_t>();
                        point_t xpoint;
                        auto intersects = segment_plane_intersection(b, a, plane, t, xpoint, cmp);
                        GEOMETRIX_ASSERT(intersects);
                        GEOMETRIX_ASSERT(classify_point_to_plane(xpoint, plane, cmp) == plane_orientation::coplanar_with_plane);
                        frontVerts.push_back(xpoint);
                        backVerts.push_back(xpoint);
                    } else if (aSide == plane_orientation::coplanar_with_plane)
                    {
                        // Output a when edge (a, b) goes from ‘on’ to ‘behind’ plane
                        backVerts.push_back(a);
                    }
                    // In all three cases, output b to the back side
                    backVerts.push_back(b);
                } else
                {
                    // b is on the plane. In all three cases output b to the front side
                    frontVerts.push_back(b);
                    // In one case, also output b to back side
                    if (aSide == plane_orientation::in_back_of_plane)
                        backVerts.push_back(b);
                }
                // Keep b as the starting point of the next edge
                a = b;
                aSide = bSide;
            }

            // Create (and return) two new simplices from the two vertex lists
            frontPoly = construct<Simplex>(frontVerts);
            backPoly = construct<Simplex>(backVerts);
        }

        template <typename Point, typename Vector, typename Simplex, typename Length, typename NumberComparisonPolicy, typename std::enable_if<is_segment<Simplex>::value, int>::type = 0>
        inline bool ray_simplex_intersect(const Point& p, const Vector& v, const Simplex &smplx, Length& t, const NumberComparisonPolicy& cmp)
        {
            using length_t = typename arithmetic_type_of<Point>::type;
            geometrix::point<length_t, dimension_of<Point>::value> xPoints[2];
            return ray_segment_intersection(p, v, smplx, t, xPoints, cmp);
        }

        template <typename Length>
        struct square_type
        {
            using type = decltype(std::declval<Length>() * std::declval<Length>());
        };

        template <typename Point, typename Simplex, typename NumberComparisonPolicy, typename std::enable_if<is_segment<Simplex>::value, int>::type = 0>
        inline typename square_type<typename arithmetic_type_of<Point>::type>::type point_simplex_distance_squared(const Point& p, const Simplex &smplx, const NumberComparisonPolicy&)
        {
            return  point_segment_distance_sqrd(p, smplx);
        }
        template <typename Segment, typename Simplex, typename NumberComparisonPolicy, typename std::enable_if<is_segment<Simplex>::value, int>::type = 0>
        inline typename square_type<typename arithmetic_type_of<Segment>::type>::type segment_simplex_distance_squared(const Segment& s, const Simplex &smplx, const NumberComparisonPolicy& cmp)
        {
            return  segment_segment_distance_sqrd(s, smplx, cmp);
        }
    }//! namespace bsp_detail;

    struct identity_simplex_extractor
    {
        template <typename T>
        T& operator()(T&& item) const { return item; }

        template <typename T, typename U>
        T& make_split(T&& item, U&&) const { return item; }
    };

    struct null_node_visitor
    {
        template <typename TreePtr, typename SplitItem, typename Node>
        void operator()(TreePtr&&, SplitItem&&, Node&&) const {}
    };

    enum class node_orientation
    {
        front
      , back
      , root
    };

    //! TODO: Ray tracing doesn't work with front side solid convention yet.
//  template <typename Simplex>
//  struct front_solid_leaf_bsp_traits
//  {
//      static const node_orientation solid_side = node_orientation::front;
//  };

    template <typename Simplex>
    struct back_solid_leaf_bsp_traits
    {
        static const node_orientation solid_side = node_orientation::back;
    };

    //! From Real Time Collision Detection:
    template <typename Simplex, typename Traits = back_solid_leaf_bsp_traits<Simplex> >
    class solid_leaf_bsp_tree
    {
        using traits_type = Traits;
        using simplex_type = Simplex;
        using plane_type = typename result_of::make_hyperplane<simplex_type>::type;
        using plane_access = hyperplane_access_traits<plane_type>;
        using dimensionless_type = typename dimensionless_type_of<typename plane_access::vector_type>::type;
        using vector_type = vector<dimensionless_type, dimension_of<typename plane_access::vector_type>::value>;
        using length_type = typename plane_access::arithmetic_type;
        using point_type = point<length_type, dimension_of<vector_type>::value>;
        using aabb_type = axis_aligned_bounding_box<point_type>;
        using index_type = std::uint32_t;
        using undefined_index = std::integral_constant<index_type, static_cast<index_type>(-1)>;
        using index_vector = std::vector<index_type>;

        static index_vector make_index_range(index_vector&& r)
        {
            boost::iota(r, 0);
            return std::move(r);
        }

    public:

        solid_leaf_bsp_tree() = default;

        template <typename Simplices, typename SimplexSelector, typename NumberComparisonPolicy, typename SimplexExtractor>
        solid_leaf_bsp_tree(const Simplices& pgons, const SimplexSelector& selector, const NumberComparisonPolicy& cmp, SimplexExtractor&& extract)
        {
            for (const auto& pgon : pgons)
            {
                auto smplx = extract(pgon);
                m_simplices.emplace_back(smplx);
                m_planes.emplace_back(make_hyperplane(smplx));
            }

            m_root = build_root(pgons, boost::dynamic_bitset<>(boost::size(pgons)), make_index_range(index_vector(boost::size(pgons))), selector, extract, cmp);
        }

        template <typename Simplices, typename SimplexSelector, typename NumberComparisonPolicy>
        solid_leaf_bsp_tree(const Simplices& pgons, const SimplexSelector& selector, const NumberComparisonPolicy& cmp)
            : solid_leaf_bsp_tree(pgons, selector, cmp, identity_simplex_extractor())
        {

        }

        solid_leaf_bsp_tree(solid_leaf_bsp_tree&& rhs)
            : m_front(std::move(rhs.m_front))
            , m_back(std::move(rhs.m_back))
            , m_node_planes(std::move(rhs.m_node_planes))
            , m_in_solid(std::move(rhs.m_in_solid))
            , m_indices(std::move(rhs.m_indices))
            , m_simplices(std::move(rhs.m_simplices))
            , m_planes(std::move(rhs.m_planes))
            , m_root(rhs.m_root)
        {}

        solid_leaf_bsp_tree& operator =(solid_leaf_bsp_tree&& rhs)
        {
            m_front = std::move(rhs.m_front);
            m_back = std::move(rhs.m_back);
            m_node_planes = std::move(rhs.m_node_planes);
            m_in_solid = std::move(rhs.m_in_solid);
            m_indices = std::move(rhs.m_indices);
            m_simplices = std::move(rhs.m_simplices);
            m_planes = std::move(rhs.m_planes);
            m_root = rhs.m_root;
            return *this;
        }

        template <typename Point, typename NumberComparisonPolicy>
        point_in_solid_classification point_in_solid_space(const Point& p, const NumberComparisonPolicy& cmp) const
        {
            return point_in_solid_space(m_root, p, cmp);
        }

        template <typename Point, typename Vector, typename NumberComparisonPolicy>
        solid_bsp_ray_tracing_result<typename arithmetic_type_of<Point>::type> ray_intersection(const Point& p, const Vector& d, const NumberComparisonPolicy& cmp) const
        {
            return ray_intersection_impl(p, d, false, cmp);
        }

        template <typename Point, typename Vector, typename NumberComparisonPolicy>
        solid_bsp_ray_tracing_result<typename arithmetic_type_of<Point>::type> ray_intersection_only_border(const Point& p, const Vector& d, const NumberComparisonPolicy& cmp) const
        {
            return ray_intersection_impl(p, d, true, cmp);
        }

        template <typename Point, typename NumberComparisonPolicy>
        typename bsp_detail::square_type<typename arithmetic_type_of<Point>::type>::type get_min_distance_sqrd_to_solid(const Point& p, std::size_t& simplexIndex, const NumberComparisonPolicy& cmp) const
        {
            return get_min_distance_sqrd_to_solid_impl(p, simplexIndex, cmp);
        }

        //! Currently supports points and segments.
        template <typename T, typename NumberComparisonPolicy>
        typename arithmetic_type_of<T>::type get_min_distance_to_solid(const T& p, std::size_t& simplexIndex, const NumberComparisonPolicy& cmp) const
        {
			static_assert( is_point<T>::value || is_segment<T>::value, "solid_leaf_bsp_tree::get_min_distance_to_solid supports points or segments." );
            using std::sqrt;
            return sqrt(get_min_distance_sqrd_to_solid_impl(p, simplexIndex, cmp));
        }

    private:

        index_type create_leaf(bool isSolid)
        {
            index_type id = static_cast<index_type>(m_front.size());
            m_front.push_back(undefined_index::value);
            m_back.push_back(undefined_index::value);
            m_node_planes.push_back(undefined_index::value);
            m_in_solid.push_back(isSolid ? point_in_solid_classification::in_solid : point_in_solid_classification::in_empty_space);
            m_indices.resize(m_indices.size() + 1);
            return id;
        }

        index_type create_leaf(index_vector&& sIndices, bool isSolid)
        {
            index_type id = static_cast<index_type>(m_front.size());
            m_front.push_back(undefined_index::value);
            m_back.push_back(undefined_index::value);
            m_node_planes.push_back(undefined_index::value);
            m_in_solid.push_back(isSolid ? point_in_solid_classification::in_solid : point_in_solid_classification::in_empty_space);
            m_indices.emplace_back(std::forward<index_vector>(sIndices));
            return id;
        }

        index_type create_node(index_type sIndex, index_vector&& sIndices, index_type f, index_type b)
        {
            index_type id = static_cast<index_type>(m_front.size());
            m_front.push_back(f);
            m_back.push_back(b);
            m_node_planes.push_back(sIndex);
            m_in_solid.push_back(point_in_solid_classification::in_empty_space);
            m_indices.emplace_back(std::forward<index_vector>(sIndices));
            return id;
        }

        template <typename Simplices, typename SimplexSelector, typename SimplexExtractor, typename NumberComparisonPolicy>
        BOOST_FORCEINLINE index_type build_root(const Simplices& simplices, boost::dynamic_bitset<>&& usedBits, index_vector&& sIndices, const SimplexSelector& selector, const SimplexExtractor& extract, const NumberComparisonPolicy& cmp)
        {
            return build_tree<node_orientation::root>(simplices, std::forward<boost::dynamic_bitset<>>(usedBits), std::forward<index_vector>(sIndices), selector, extract, cmp);
        }

        //! Constructs BSP tree from an input vector of simplices.
        template <node_orientation Side, typename Simplices, typename SimplexSelector, typename SimplexExtractor, typename NumberComparisonPolicy>
        index_type build_tree(const Simplices& simplices, boost::dynamic_bitset<>&& usedBits, index_vector&& sIndices, const SimplexSelector& selector, const SimplexExtractor& extract, const NumberComparisonPolicy& cmp)
        {
            BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<Simplices>));
            using item_t = typename boost::range_value<Simplices>::type;

            //! Return empty node if there are no simplices
            if (boost::empty(simplices))
                return create_leaf(Side == traits_type::solid_side);

            // Get number of simplices in the input vector
            //auto nSimplices = boost::size(simplices);

            //! Select best possible partitioning plane based on the input geometry
            auto selected = selector(simplices, usedBits);

            //! Test each polygon against the dividing plane, adding them
            //! to the front list, back list, or both, as appropriate
            if (selected == simplices.end())
            {
                return create_leaf(std::move(sIndices), Side == traits_type::solid_side);
            }

            auto sIndex = sIndices[std::distance(boost::begin(simplices), selected)];
            auto splitPlane = m_planes[sIndex];
            std::vector<item_t> frontList, backList;
            boost::dynamic_bitset<> frontBits, backBits;
            index_vector frontIndices, backIndices, coplanarIndices;
            std::size_t i = 0;
            auto add_to_front = [&i, &frontList, &frontBits, &usedBits, &frontIndices, &sIndices](const item_t& item) -> void {
                frontList.push_back(item);
                frontBits.push_back(usedBits[i]);
                frontIndices.push_back(sIndices[i]);
            };
            auto add_to_back = [&i, &backList, &backBits, &usedBits, &backIndices, &sIndices](const item_t& item) -> void {
                backList.push_back(item);
                backBits.push_back(usedBits[i]);
                backIndices.push_back(sIndices[i]);
            };
            for (const auto& item : simplices)
            {
                auto smplx = extract(item);

                switch (classify_simplex_to_plane(smplx, splitPlane, cmp))
                {
                case plane_orientation::coplanar_with_plane:
                {
                    //! What’s done in this case depends on what type of tree is being
                    //! built. For a node-storing tree, the polygon is stored inside
                    //! the node at this level (along with all other simplices coplanar
                    //! with the plane). Here, for a leaf-storing tree, coplanar simplices
                    //! are sent to either side of the plane. In this case, to the front
                    //! side, by falling through to the next case

                    coplanarIndices.push_back(sIndices[i]);

                    //! Attempt to push according to the normal of the smplex.
                    auto smplxPlane = m_planes[sIndices[i]];
                    auto smplxNormal = plane_access::get_normal_vector(smplxPlane);
                    auto planeNormal = plane_access::get_normal_vector(splitPlane);
                    auto dp = dot_product(smplxNormal, planeNormal);
                    //GEOMETRIX_ASSERT(cmp.equals(dp, constants::one<decltype(dp)>()) || cmp.equals(dp, -constants::one<decltype(dp)>()));
                    if (dp < constants::zero<decltype(dp)>())
                    {
                        add_to_back(item);
                        break;
                    }
                    BOOST_FALLTHROUGH;
                }
                case plane_orientation::in_front_of_plane:
                    add_to_front(item);
                    break;
                case plane_orientation::in_back_of_plane:
                    add_to_back(item);
                    break;
                case plane_orientation::straddling_plane:
                {
                    //! Split polygon to plane and send a part to each side of the plane
                    simplex_type frontPart;
                    simplex_type backPart;
                    bsp_detail::split_simplex(smplx, splitPlane, frontPart, backPart, cmp);
                    add_to_front(extract.make_split(frontPart, item));
                    add_to_back(extract.make_split(backPart, item));
                    break;
                }
                };

                ++i;
            }

            //! Recursively build child subtrees and return new tree root combining them
            auto fNode = build_tree<node_orientation::front>(frontList, std::move(frontBits), std::move(frontIndices), selector, extract, cmp);
            auto bNode = build_tree<node_orientation::back>(backList, std::move(backBits), std::move(backIndices), selector, extract, cmp);

            return create_node(sIndex, /*std::move(coplanar),*/ std::move(coplanarIndices), fNode, bNode);
        }

        const plane_type& get_plane(index_type node) const
        {
            GEOMETRIX_ASSERT(m_node_planes[node] != -1);
			return m_planes[m_node_planes[node]];
        }

        vector_type get_normal_vector(index_type node) const
        {
            GEOMETRIX_ASSERT(m_node_planes[node] != -1);
            return plane_access::get_normal_vector(m_planes[m_node_planes[node]]);
        }

        length_type get_distance_to_origin(index_type node) const
        {
            GEOMETRIX_ASSERT(m_node_planes[node] != -1);
            return plane_access::get_distance_to_origin(m_planes[m_node_planes[node]]);
        }

        template <typename Point, typename NumberComparisonPolicy>
        point_in_solid_classification point_in_solid_space(index_type node, const Point& p, const NumberComparisonPolicy& cmp) const
        {
            while (!is_leaf(node))
            {
                //! Compute distance of point to dividing plane
                auto dist = scalar_projection(as_vector(p), get_normal_vector(node)) - get_distance_to_origin(node);
                auto zero = constants::zero<typename std::decay<decltype(dist)>::type>();
                if (cmp.greater_than(dist, zero))
                {
                    // Point in front of plane, so traverse front of tree
                    node = m_front[node];
                }
                else if (cmp.less_than(dist, zero))
                {
                    //! Point behind of plane, so traverse back of tree
                    node = m_back[node];
                }
                else
                {
                    //! Point on dividing plane; must traverse both sides
                    auto front = point_in_solid_space(m_front[node], p, cmp);
                    auto back = point_in_solid_space(m_back[node], p, cmp);
                    //! If results agree, return that, else point is on boundary
                    return (front == back) ? front : point_in_solid_classification::on_boundary;
                }

                GEOMETRIX_ASSERT(node != -1);
            }

            //! Now at a leaf, inside/outside status determined by solid flag

            return in_solid_classification(node);
        }

        template <typename T, typename NumberComparisonPolicy>
        typename bsp_detail::square_type<typename arithmetic_type_of<T>::type>::type get_min_distance_sqrd_to_solid_impl(const T& p, std::size_t& closestSimplex, const NumberComparisonPolicy& cmp) const
        {
            using length_t = typename arithmetic_type_of<T>::type;
            using area_t = decltype(std::declval<length_t>()*std::declval<length_t>());
            auto minDist2 = constants::infinity<area_t>();
            auto calc_orientation = [&cmp](length_t dist) -> plane_orientation
            {
                const auto zero = constants::zero<length_t>();
                if (cmp.greater_than(dist, zero))
                    return plane_orientation::in_front_of_plane;
                else if (cmp.less_than(dist, zero))
                    return plane_orientation::in_back_of_plane;
                //! Point on dividing plane; must traverse both sides
                return plane_orientation::straddling_plane;
            };

            std::stack<index_type> nodeStack;
            nodeStack.push(m_root);
            while (!nodeStack.empty())
            {
                auto node = nodeStack.top();
                nodeStack.pop();

                for(auto idx : m_indices[node])
                {
					if constexpr( is_point<T>::value )
					{
						auto d2 = bsp_detail::point_simplex_distance_squared( p, m_simplices[idx], cmp );
						if( d2 < minDist2 )
						{
							closestSimplex = idx;
							minDist2 = d2;
						}
					}
					if constexpr( is_segment<T>::value )
					{
						auto d2 = bsp_detail::segment_simplex_distance_squared( p, m_simplices[idx], cmp );
						if( d2 < minDist2 )
						{
							closestSimplex = idx;
							minDist2 = d2;
						}
					}
                }

                if(!is_leaf(node))
                {
                    //! Compute distance of point to dividing plane
					if constexpr( is_point<T>::value )
					{
						auto dist = scalar_projection(as_vector(p), get_normal_vector(node)) - get_distance_to_origin(node);
						auto dist2 = dist * dist;
						auto orientation = calc_orientation(dist);

						//! back
						if (dist2 < minDist2 || orientation != plane_orientation::in_front_of_plane )
							nodeStack.push(m_back[node]);

						//! front
						if (dist2 < minDist2 || orientation != plane_orientation::in_back_of_plane )
							nodeStack.push(m_front[node]);
					}
					if constexpr( is_segment<T>::value )
					{
						auto dist = signed_segment_plane_distance( p, get_plane( node ) );
						auto dist2 = dist * dist;
						auto orientation = calc_orientation( dist );

						//! back
						if( dist2 < minDist2 || orientation != plane_orientation::in_front_of_plane )
							nodeStack.push( m_back[node] );

						//! front
						if( dist2 < minDist2 || orientation != plane_orientation::in_back_of_plane )
							nodeStack.push( m_front[node] );
					}
                }
            }

            return minDist2;
        }

        template <typename Point, typename NumberComparisonPolicy>
        point_in_solid_classification point_in_solid_space_no_boundary(index_type node, const Point& p, const NumberComparisonPolicy& cmp) const
        {
            while (!is_leaf(node))
            {
                //! Compute distance of point to dividing plane
                auto dist = scalar_projection(as_vector(p), get_normal_vector(node)) - get_distance_to_origin(node);
                //! Traverse front of tree when point in front of plane, else back of tree
                auto zero = constants::zero<typename std::decay<decltype(dist)>::type>();
                node = cmp.less_than_or_equal(dist, zero) ? m_back[node] : m_front[node];
            }

            //! Now at a leaf, inside/outside status determined by solid flag
            return in_solid_classification(node);
        }

        template <typename Point, typename Vector, typename Indices, typename NumberComparisonPolicy>
        index_type find_closest_hit(const Point& p, const Vector& d, const Indices& sIndices, const NumberComparisonPolicy& cmp) const
        {
            auto minDist = (std::numeric_limits<length_type>::max)();
            length_type t;
            index_type minIndex = undefined_index::value;
            for (auto i : sIndices)
            {
                if (bsp_detail::ray_simplex_intersect(p, d, m_simplices[i], t, cmp) && t < minDist)
                {
                    minDist = t;
                    minIndex = i;
                }
            }

            return minIndex;
        }

        //! Intersect ray/segment R(t)=p+t*d, tmin <= t <= tmax, against bsp tree
        //! ’node’, returning distance along the ray thit of first intersection with a solid leaf, if any
        template <typename Point, typename Vector, typename NumberComparisonPolicy>
        solid_bsp_ray_tracing_result<typename arithmetic_type_of<Point>::type> ray_intersection_impl(const Point& p, const Vector& d, bool onlyBorder, const NumberComparisonPolicy& cmp) const
        {
            using length_t = typename arithmetic_type_of<Point>::type;
            auto tmax = (std::numeric_limits<length_t>::max)();
            auto tmin = constants::zero<length_t>();

            using elem_t = std::tuple<index_type, length_t, std::set<index_type>/*, std::vector<simplex_type>*/>;
            std::stack<elem_t> nodeStack;
            auto node = m_root;
            std::set<index_type> maxIndices, minIndices;
            //std::vector<simplex_type> maxS, minS;
            while (1)
            {
                if (!is_leaf(node))
                {
                    auto denom = scalar_projection(d, get_normal_vector(node));
                    //! Compute distance from dividing plane to p.
                    auto dist = get_distance_to_origin(node) - scalar_projection(as_vector(p), get_normal_vector(node));
                    auto nearIndex = dist > constants::zero<length_t>();
                    //! If denom is zero, ray runs parallel to plane. In this case,
                    //! just fall through to visit the near side (the one p lies on)
                    if (denom != constants::zero<decltype(denom)>())
                    {
                        auto t = dist / denom;
                        if (constants::zero<length_t>() <= t && t <= tmax)
                        {
                            if (t >= tmin)
                            {
                                //! Straddling, push far side onto stack,then visit near side
                                auto farNode = (1 ^ nearIndex) ? m_back[node] : m_front[node];
                                nodeStack.push(std::make_tuple(farNode, tmax, std::move(maxIndices)/*, std::move(maxS)*/));

                                maxIndices.insert(m_indices[node].begin(), m_indices[node].end());
                                //using boost::adaptors::transformed;
                                //boost::copy(node->indices | transformed([this](index_type i) { return m_simplices[i]; }), std::back_inserter(maxS));
                                tmax = t;
                            }
                            else
                                nearIndex = 1 ^ nearIndex;//! 0 <= t < tmin, visit far side
                        }
                    }
                    node = nearIndex ? m_back[node] : m_front[node];
                }
                else
                {
                    //! Now at a leaf. If it is solid, there’s a hit at time tmin, so exit
                    if (is_solid(node))
                    {
                        //! Look at geometry in sIndices and find the closest hit.
						if (!minIndices.empty() || !onlyBorder)
						{
							auto sIndex = find_closest_hit(p, d, minIndices, cmp);
							return solid_bsp_ray_tracing_result<length_t>(true, tmin, sIndex);
						}

						auto sIndex = find_closest_hit(p, d, maxIndices, cmp);
						return solid_bsp_ray_tracing_result<length_t>(true, tmax, sIndex);
                    }

                    //! Exit if no more subtrees to visit, else pop off a node and continue
                    if (nodeStack.empty())
                        break;
                    tmin = tmax;
                    minIndices = std::move(maxIndices);
                    //minS = maxS;
                    std::tie(node, tmax, maxIndices/*, maxS*/) = nodeStack.top();
                    nodeStack.pop();
                }

                GEOMETRIX_ASSERT(node != undefined_index::value);
            }

            //! No hit
            return solid_bsp_ray_tracing_result<length_t>(false);
        }

        bool is_leaf(index_type n) const { return m_front[n] == undefined_index::value && m_back[n] == undefined_index::value; }
        bool is_solid(index_type n) const { return m_in_solid[n] == point_in_solid_classification::in_solid; }
        bool is_solid_leaf(index_type n) const { return is_solid(n) && is_leaf(n); }
        point_in_solid_classification in_solid_classification(index_type n) const { return m_in_solid[n]; }

        index_vector m_front;
        index_vector m_back;
        index_vector m_node_planes;
        std::vector<point_in_solid_classification> m_in_solid;
        std::vector<index_vector> m_indices;
        std::vector<simplex_type> m_simplices;
        std::vector<plane_type>   m_planes;
        index_type                m_root{ undefined_index::value };
    };

}//namespace geometrix;

#endif //GEOMETRIX_SOLID_LEAF_BSPTREE_HPP


/*
 * Copyright 2018, Gepetto team, LAAS-CNRS
 *
 * This file is part of sot-talos-balance.
 * sot-talos-balance is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-talos-balance is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "sot/talos_balance/euler-to-quat.hh"

#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <sot/core/stop-watch.hh>

#include <Eigen/Core>

namespace dynamicgraph
{
  namespace sot
  {
    namespace talos_balance
    {
      namespace dg = ::dynamicgraph;
      using namespace dg;
      using namespace dg::command;

//Size to be aligned                       "-------------------------------------------------------"
#define PROFILE_EULERTOQUAT_COMPUTATION      "EulerToQuat computation                                  "

#define INPUT_SIGNALS     m_eulerSIN

#define OUTPUT_SIGNALS m_quaternionSOUT

      /// Define EntityClassName here rather than in the header file
      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
      typedef EulerToQuat EntityClassName;

      /* --- DG FACTORY ---------------------------------------------------- */
      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(EulerToQuat,
                                         "EulerToQuat");

      /* ------------------------------------------------------------------- */
      /* --- CONSTRUCTION -------------------------------------------------- */
      /* ------------------------------------------------------------------- */
      EulerToQuat::EulerToQuat(const std::string& name)
                      : Entity(name)
                      , CONSTRUCT_SIGNAL_IN(euler, dynamicgraph::Vector)
                      , CONSTRUCT_SIGNAL_OUT(quaternion, dynamicgraph::Vector, INPUT_SIGNALS)
      {
        Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );

        /* Commands. */
        addCommand("init", makeCommandVoid0(*this, &EulerToQuat::init, docCommandVoid0("Initialize the entity.")));
      }

      /* ------------------------------------------------------------------- */
      /* --- SIGNALS ------------------------------------------------------- */
      /* ------------------------------------------------------------------- */

      DEFINE_SIGNAL_OUT_FUNCTION(quaternion, dynamicgraph::Vector)
      {
        const dynamicgraph::Vector & input = m_eulerSIN(iter);
        const size_t sz = input.size();
        if((size_t)(s.size())!=(sz+1))
          s.resize(sz+1);

        getProfiler().start(PROFILE_EULERTOQUAT_COMPUTATION);

        const Eigen::Vector3d & euler = input.segment<3>(3);

        const double roll  = euler[0];
        const double pitch = euler[1];
        const double yaw   = euler[2];

        Eigen::Quaterniond quat;
        quat = Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ())
             * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX());

        s.head<3>() = input.head<3>();

        s.segment<4>(3) = quat.coeffs();

        if(sz>6)
          s.tail(sz-6) = input.tail(sz-6);

        getProfiler().stop(PROFILE_EULERTOQUAT_COMPUTATION);

        return s;
      }

      /* --- COMMANDS ---------------------------------------------------------- */

      /* ------------------------------------------------------------------- */
      /* --- ENTITY -------------------------------------------------------- */
      /* ------------------------------------------------------------------- */

      void EulerToQuat::display(std::ostream& os) const
      {
        os << "EulerToQuat " << getName();
        try
        {
          getProfiler().report_all(3, os);
        }
        catch (ExceptionSignal e) {}
      }

    } // namespace talos_balance
  } // namespace sot
} // namespace dynamicgraph


#pragma once

#include "linear_algebra/matrix.hpp"

#include <boost/units/quantity.hpp>

#include <boost/units/systems/si/energy.hpp>
#include <boost/units/systems/si/force.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/velocity.hpp>

template <class T> using Vector2d = LinearAlgebra::Matrix<T, 2, 1>;

const double pi = 3.14159265359;

namespace Units
{

using Angle = boost::units::quantity<boost::units::si::plane_angle>;

using Force = boost::units::quantity<boost::units::si::force>;

using Length = boost::units::quantity<boost::units::si::length>;

using Mass = boost::units::quantity<boost::units::si::mass>;

using Velocity = boost::units::quantity<boost::units::si::velocity>;

} // namespace Units

template <class T, class U> Vector2d<T> castVectorTo(const Vector2d<U>& vector)
{
    typename Vector2d<T>::Storage_t res;
    for(const auto i : range(res.size()))
        res[i] = T::from_value(vector(i).value()); // TODO find a better way?
    return Vector2d<T>{res};
}


// Glauber model
// Copyright 2015 Jonah E. Bernhard, J. Scott Moreland
// MIT License

#include "event.h"

#include <algorithm>
#include <cmath>

#include <boost/program_options/variables_map.hpp>

#include "nucleus.h"

namespace glauber {

namespace {

constexpr double TINY = 1e-12;

}  // unnamed namespace

// Determine the grid parameters like so:
//   1. Read and set step size from the configuration.
//   2. Read grid max from the config, then set the number of steps as
//      nsteps = ceil(2*max/step).
//   3. Set the actual grid max as max = nsteps*step/2.  Hence if the step size
//      does not evenly divide the config max, the actual max will be marginally
//      larger (by at most one step size).
Event::Event(const VarMap& var_map)
    : norm_(var_map["normalization"].as<double>()),
      dxy_(var_map["grid-step"].as<double>()),
      nsteps_(std::ceil(2.*var_map["grid-max"].as<double>()/dxy_)),
      xymax_(.5*nsteps_*dxy_),
      TA_(boost::extents[nsteps_][nsteps_]),
      TB_(boost::extents[nsteps_][nsteps_]),
      TR_(boost::extents[nsteps_][nsteps_]) {
}

void Event::compute(const Nucleus& nucleusA, const Nucleus& nucleusB,
                    NucleonProfile& profile) {
  // Reset npart; compute_nuclear_thickness() increments it.
  npart_ = 0;
  compute_nuclear_thickness(nucleusA, profile, TA_);
  compute_nuclear_thickness(nucleusB, profile, TB_);
  compute_reduced_thickness();
  compute_observables();
}

namespace {

// Limit a value to a range.
// Used below to constrain grid indices.
template <typename T>
inline const T& clip(const T& value, const T& min, const T& max) {
  if (value < min)
    return min;
  if (value > max)
    return max;
  return value;
}

}  // unnamed namespace

void Event::compute_nuclear_thickness(
    const Nucleus& nucleus, NucleonProfile& profile, Grid& TX) {
  // Construct the thickness grid by looping over participants and adding each
  // to a small subgrid within its radius.  Compared to the other possibility
  // (grid cells as the outer loop and participants as the inner loop), this
  // reduces the number of required distance-squared calculations by a factor of
  // ~20 (depending on the nucleon size).  The Event unit test verifies that the
  // two methods agree.

  // Wipe grid with zeros.
  std::fill(TX.origin(), TX.origin() + TX.num_elements(), 0.);

  const double r = profile.radius();

  // Deposit each participant onto the grid.
  for (const auto& nucleon : nucleus) {
    if (!nucleon.is_participant())
      continue;

    ++npart_;

    // Work in coordinates relative to (-width/2, -width/2).
    double x = nucleon.x() + xymax_;
    double y = nucleon.y() + xymax_;

    // Determine min & max indices of nucleon subgrid.
    int ixmin = clip(static_cast<int>((x-r)/dxy_), 0, nsteps_-1);
    int iymin = clip(static_cast<int>((y-r)/dxy_), 0, nsteps_-1);
    int ixmax = clip(static_cast<int>((x+r)/dxy_), 0, nsteps_-1);
    int iymax = clip(static_cast<int>((y+r)/dxy_), 0, nsteps_-1);

    // Prepare profile for new nucleon.
    profile.fluctuate(nucleon);

    // Add profile to grid.
    for (auto iy = iymin; iy <= iymax; ++iy) {
      double dysq = std::pow(y - (static_cast<double>(iy)+.5)*dxy_, 2);
      for (auto ix = ixmin; ix <= ixmax; ++ix) {
        double dxsq = std::pow(x - (static_cast<double>(ix)+.5)*dxy_, 2);
        TX[iy][ix] += profile.thickness(dxsq + dysq);
      }
    }
  }
}

void Event::compute_reduced_thickness() {
  double sum = 0.;
  double ixcm = 0.;
  double iycm = 0.;

  for (int iy = 0; iy < nsteps_; ++iy) {
    for (int ix = 0; ix < nsteps_; ++ix) {
      auto t = norm_ * (TA_[iy][ix] + TB_[iy][ix]);
      TR_[iy][ix] = t;
      sum += t;
      // Center of mass grid indices.
      // No need to multiply by dxy since it would be canceled later.
      ixcm += t * static_cast<double>(ix);
      iycm += t * static_cast<double>(iy);
    }
  }

  multiplicity_ = dxy_ * dxy_ * sum;
  ixcm_ = ixcm / sum;
  iycm_ = iycm / sum;
}

void Event::compute_observables() {
  // Compute eccentricity.

  // Simple helper class for use in the following loop.
  struct EccentricityAccumulator {
    double re = 0.;  // real part
    double im = 0.;  // imaginary part
    double wt = 0.;  // weight
    double finish() const  // compute final eccentricity
    { return std::sqrt(re*re + im*im) / std::fmax(wt, TINY); }
  } e2, e3, e4, e5;

  for (int iy = 0; iy < nsteps_; ++iy) {
    for (int ix = 0; ix < nsteps_; ++ix) {
      const auto& t = TR_[iy][ix];
      if (t < TINY)
        continue;

      // Compute (x, y) relative to the CM and cache powers of x, y, r.
      auto x = static_cast<double>(ix) - ixcm_;
      auto x2 = x*x;
      auto x3 = x2*x;
      auto x4 = x2*x2;

      auto y = static_cast<double>(iy) - iycm_;
      auto y2 = y*y;
      auto y3 = y2*y;
      auto y4 = y2*y2;

      auto r2 = x2 + y2;
      auto r = std::sqrt(r2);
      auto r4 = r2*r2;

      auto xy = x*y;
      auto x2y2 = x2*y2;

      // The eccentricity harmonics are weighted averages of r^n*exp(i*n*phi)
      // over the entropy profile (reduced thickness).  The naive way to compute
      // exp(i*n*phi) at a given (x, y) point is essentially:
      //
      //   phi = arctan2(y, x)
      //   real = cos(n*phi)
      //   imag = sin(n*phi)
      //
      // However this implementation uses three unnecessary trig functions; a
      // much faster method is to express the cos and sin directly in terms of x
      // and y.  For example, it is trivial to show (by drawing a triangle and
      // using rudimentary trig) that
      //
      //   cos(arctan2(y, x)) = x/r = x/sqrt(x^2 + y^2)
      //   sin(arctan2(y, x)) = y/r = x/sqrt(x^2 + y^2)
      //
      // This is easily generalized to cos and sin of (n*phi) by invoking the
      // multiple angle formula, e.g. sin(2x) = 2sin(x)cos(x), and hence
      //
      //   sin(2*arctan2(y, x)) = 2*sin(arctan2(y, x))*cos(arctan2(y, x))
      //                        = 2*x*y / r^2
      //
      // Which not only eliminates the trig functions, but also naturally
      // cancels the r^2 weight.  This cancellation occurs for all n.
      //
      // The Event unit test verifies that the two methods agree.
      e2.re += t * (y2 - x2);
      e2.im += t * 2.*xy;
      e2.wt += t * r2;

      e3.re += t * (y3 - 3.*y*x2);
      e3.im += t * (3.*x*y2 - x3);
      e3.wt += t * r2*r;

      e4.re += t * (x4 + y4 - 6.*x2y2);
      e4.im += t * 4.*xy*(y2 - x2);
      e4.wt += t * r4;

      e5.re += t * y*(5.*x4 - 10.*x2y2 + y4);
      e5.im += t * x*(x4 - 10.*x2y2 + 5.*y4);
      e5.wt += t * r4*r;
    }
  }

  eccentricity_[2] = e2.finish();
  eccentricity_[3] = e3.finish();
  eccentricity_[4] = e4.finish();
  eccentricity_[5] = e5.finish();
}

}  // namespace glauber


#include <iostream>
#include <Eigen/Dense>
//#include <mpi.h>
#include <fstream>
#include <ntsid.h>
#include <omp.h>
#include <vector>

using std::cin;
using std::cout;
using std::endl;
using namespace Eigen;
using namespace std;

// install eigen
// mpic++ -I /Users/mustafaatik/Downloads/eigen/ main.cpp -o main && mpirun  --allow-run-as-root  -np 2 ./main
// http://stackoverflow.com/questions/15939757/is-it-possible-to-run-openmpi-on-a-local-computer-and-a-remote-cluster
// http://mpitutorial.com/tutorials/running-an-mpi-cluster-within-a-lan/
// https://www.open-mpi.org/papers/sc-2009/jjhursey-iu-booth.pdf
// http://stackoverflow.com/questions/2546298/vector-usage-in-mpic
// http://stackoverflow.com/questions/38592854/how-to-send-the-data-in-an-eigenmatrixxd-with-mpi

void mat2csv(MatrixXd mat);
void factorize();
void als(std::vector<std::vector<int>> ratings, int maxUserId, int maxMovieId, int rank, int threadNumber);

std::vector< std::vector<int> > load_csv (const std::string & path) {
    std::ifstream indata;
    indata.open(path);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    std::vector< std::vector<int> > ratings;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        std::vector<int> data;
        while (std::getline(lineStream, cell, ',')) {
            data.push_back(std::stod(cell));
        }
        // user - movie -rating
        ratings.push_back(data);
    }
    return ratings;
//    return Map<const Matrix<typename M::Scalar, M::RowsAtCompileTime, M::ColsAtCompileTime, RowMajor>>(values.data(), rows, values.size()/rows);
}

int main(int argc, char ** argv)
{
    factorize();
    return 0;
}

void factorize() {
    std::vector< std::vector<int> > ratings = load_csv("../data.txt");

    int maxUserId = 7;
    int maxUMovieId= 9;
    int cols = maxUMovieId, rows = maxUserId;

    for (int i = 4; i < 5; ++i) {
        cout << "Thread number: " << i << endl;
        als(ratings, maxUserId, maxUMovieId, 4, 1);
    }
}

void als(std::vector<std::vector<int>> ratings, int maxUserId, int maxMovieId, int rank, int threadNumber) {

    MatrixXd U = MatrixXd::Random(maxUserId, rank);
    MatrixXd M = MatrixXd::Random(rank, maxMovieId);

    MatrixXd A = MatrixXd::Zero(maxUserId, maxMovieId);
    MatrixXd W = MatrixXd::Zero(maxUserId, maxMovieId);
    for (int i = 0; i < ratings.size(); ++i) {
        int ui = ratings.at(i).at(0) - 1; // userId index
        int mi = ratings.at(i).at(1) - 1; // movieId index
        int ri = ratings.at(i).at(2); // rating
        A(ui, mi) = ri;
        W(ui, mi) = 1;
    }

    cout << A << endl << endl;
    double Eta = 0.03;
    double eta = Eta;
    int maxIteration = 2000;
    cout << U * M << endl << endl;

    int ui, mi, ri;
    double error;
    Eigen::VectorXd tempU;
    std::vector<double> errorHistory;

    double start = omp_get_wtime();
    for (int iteration = 0; iteration < maxIteration; ++iteration) {

        #pragma omp parallel for num_threads(threadNumber) schedule(static) private(ui, mi, ri, error, tempU)
        for (int i = 0; i < ratings.size(); ++i) {
            ui = ratings.at(i).at(0) - 1; // userId index
            mi = ratings.at(i).at(1) - 1; // movieId index
            ri = ratings.at(i).at(2); // rating
            error = ri - U.row(ui) * M.col(mi);

            tempU = U.row(ui) + eta * error * M.col(mi).transpose();
            M.col(mi) = M.col(mi) + eta * error * U.row(ui).transpose();
            U.row(ui) = tempU;
        }

        if (iteration % ((int) maxIteration / 15) == 0) {
            eta *= 0.8;
            errorHistory.push_back(W.cwiseProduct(A - U * M).array().square().sum());
        }
    }
    double elapsed = omp_get_wtime() - start;
    cout << "elapsed: " << elapsed << endl;
    cout << "error:" << endl;
    for (int j = 0; j < errorHistory.size(); ++j) {
        cout << "error: " << errorHistory.at(j) << endl;
    }
    cout << U * M << endl << endl;
//    mat2csv(A);
    mat2csv(U*M);
}

void mat2csv(MatrixXd mat) {
    std::stringstream buf;
    for (int i = 0; i < mat.rows(); ++i) {

        for (int j = 0; j < mat.cols(); ++j) {
            buf << mat(i, j) << ", ";
        }
        buf << "\n";
    }
    cout << endl << buf.str() << endl;
}


#include <gtest/gtest.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include "UKF/Types.h"
#include "UKF/Integrator.h"
#include "UKF/StateVector.h"
#include "UKF/MeasurementVector.h"
#include "UKF/Core.h"
#include "comparisons.h"

/*
Set up state vector class. The order of these is changed to prevent
linker collisions with the ones in TestCore.cpp.
*/
enum MyStateFields {
    Attitude,
    AngularVelocity,
    Position,
    Velocity
};

using MyStateVector = UKF::StateVector<
    UKF::Field<Position, UKF::Vector<3>>,
    UKF::Field<Velocity, UKF::Vector<3>>,
    UKF::Field<Attitude, UKF::Quaternion>,
    UKF::Field<AngularVelocity, UKF::Vector<3>>
>;

namespace UKF {
namespace Parameters {

template <> constexpr real_t AlphaSquared<MyStateVector> = 1e-6;
}

/*
State vector process model. One version takes body frame kinematic
acceleration and angular acceleration as inputs, the other doesn't (assumes
zero accelerations).
*/
template <> template <>
MyStateVector MyStateVector::derivative<UKF::Vector<3>, UKF::Vector<3>>(
        const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) const {
    MyStateVector temp;

    /* Position derivative. */
    temp.set_field<Position>(get_field<Velocity>());

    /* Velocity derivative. */
    temp.set_field<Velocity>(get_field<Attitude>().conjugate() * acceleration);

    /* Attitude derivative. */
    UKF::Quaternion temp_q;
    temp_q.vec() = get_field<AngularVelocity>();
    temp_q.w() = 0;
    temp.set_field<Attitude>(temp_q);

    /* Angular velocity derivative. */
    temp.set_field<AngularVelocity>(angular_acceleration);

    return temp;
}

template <> template <>
MyStateVector MyStateVector::derivative<>() const {
    return derivative(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(0, 0, 0));
}

}

/*
Set up measurement vector class. The order of these is changed to prevent
linker collisions with the ones in TestCore.cpp.
*/
enum MyMeasurementFields {
    Accelerometer,
    Magnetometer,
    Gyroscope,
    GPS_Position,
    GPS_Velocity
};

using MyMeasurementVector = UKF::DynamicMeasurementVector<
    UKF::Field<GPS_Position, UKF::Vector<3>>,
    UKF::Field<GPS_Velocity, UKF::Vector<3>>,
    UKF::Field<Accelerometer, UKF::Vector<3>>,
    UKF::Field<Magnetometer, UKF::FieldVector>,
    UKF::Field<Gyroscope, UKF::Vector<3>>
>;

using MyCore = UKF::SquareRootCore<
    MyStateVector,
    MyMeasurementVector,
    UKF::IntegratorRK4
>;

namespace UKF {
/*
Define measurement model to be used in tests. NOTE: These are just for
testing, don't expect them to make any physical sense whatsoever.
*/
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, GPS_Position>(const MyStateVector& state) {
    return state.get_field<Position>();
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, GPS_Velocity>(const MyStateVector& state) {
    return state.get_field<Velocity>();
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Accelerometer>(const MyStateVector& state) {
    return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8);
}

template <> template <>
UKF::FieldVector MyMeasurementVector::expected_measurement
<MyStateVector, Magnetometer>(const MyStateVector& state) {
    return state.get_field<Attitude>() * UKF::FieldVector(1, 0, 0);
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Gyroscope>(const MyStateVector& state) {
    return state.get_field<AngularVelocity>();
}

/*
These versions of the predicted measurement functions take kinematic
acceleration and angular acceleration as inputs. Note that in reality, the
inputs would probably be a control vector and the accelerations would be
calculated using the state vector and a dynamics model.
*/
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, GPS_Position>(const MyStateVector& state,
        const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) {
    return state.get_field<Position>();
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, GPS_Velocity>(const MyStateVector& state,
        const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) {
    return state.get_field<Velocity>();
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Accelerometer, UKF::Vector<3>>(const MyStateVector& state,
        const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) {
    return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8) + acceleration;
}

template <> template <>
UKF::FieldVector MyMeasurementVector::expected_measurement
<MyStateVector, Magnetometer, UKF::Vector<3>>(const MyStateVector& state,
        const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) {
    return state.get_field<Attitude>() * UKF::FieldVector(1, 0, 0);
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Gyroscope, UKF::Vector<3>>(const MyStateVector& state,
        const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) {
    return state.get_field<AngularVelocity>();
}

}

/*
Initialise covariances as square roots to be comparable with TestCore.cpp.
*/
MyCore create_initialised_sr_test_filter() {
    MyCore test_filter;
    test_filter.state.set_field<Position>(UKF::Vector<3>(0, 0, 0));
    test_filter.state.set_field<Velocity>(UKF::Vector<3>(0, 0, 0));
    test_filter.state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_filter.state.set_field<AngularVelocity>(UKF::Vector<3>(0, 0, 0));
    test_filter.root_covariance = MyStateVector::CovarianceMatrix::Zero();
    test_filter.root_covariance.diagonal() <<
        10000, 10000, 10000, 100, 100, 100, 1, 1, 5, 10, 10, 10;
    test_filter.root_covariance = test_filter.root_covariance.llt().matrixU();
    test_filter.measurement_root_covariance <<
        10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05;
    test_filter.measurement_root_covariance = test_filter.measurement_root_covariance.array().sqrt();

    real_t a, b;
    real_t dt = 0.01;
    a = std::sqrt(0.1*dt*dt);
    b = std::sqrt(0.1*dt);
    test_filter.process_noise_root_covariance << a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                                 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                                 0, 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                                 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 0,
                                                 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0,
                                                 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0,
                                                 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0,
                                                 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0,
                                                 0, 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0,
                                                 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 0,
                                                 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0,
                                                 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b;
    test_filter.process_noise_root_covariance = test_filter.process_noise_root_covariance.llt().matrixU();

    return test_filter;
}

TEST(SquareRootCoreTest, Initialisation) {
    MyCore test_filter = create_initialised_sr_test_filter();
}

/*
All these tests check that the estimated state matches the 'actual' state to
within 2-sigma.
*/
TEST(SquareRootCoreTest, APrioriStep) {
    MyCore test_filter = create_initialised_sr_test_filter();

    test_filter.a_priori_step(0.01);

    EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits<real_t>::epsilon());
    EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field<Position>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(0).norm()*2);
    EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field<Velocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(3).norm()*2);
    EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field<Attitude>()))),
        test_filter.root_covariance.diagonal().segment<3>(6).norm()*2);
    EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field<AngularVelocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(9).norm()*2);
}

TEST(SquareRootCoreTest, APrioriStepWithInputs) {
    MyCore test_filter = create_initialised_sr_test_filter();

    test_filter.a_priori_step(0.01, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0));

    EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits<real_t>::epsilon());
    EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field<Position>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(0).norm()*2);
    EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field<Velocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(3).norm()*2);
    EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field<Attitude>()))),
        test_filter.root_covariance.diagonal().segment<3>(6).norm()*2);
    EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field<AngularVelocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(9).norm()*2);
}

TEST(SquareRootCoreTest, InnovationStep) {
    MyCore test_filter = create_initialised_sr_test_filter();
    MyMeasurementVector m;

    m.set_field<GPS_Position>(UKF::Vector<3>(100, 10, -50));
    m.set_field<GPS_Velocity>(UKF::Vector<3>(20, 0, 0));
    m.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -9.8));
    m.set_field<Magnetometer>(UKF::FieldVector(0, -1, 0));
    m.set_field<Gyroscope>(UKF::Vector<3>(0.5, 0, 0));

    test_filter.a_priori_step(0.01);
    test_filter.innovation_step(m);

    /*
    With the field vector, we expect the determinant to be approximately zero,
    so allow for it to be slightly negative due to numerical precision.
    */
    EXPECT_GE(test_filter.innovation_root_covariance.determinant(), -std::numeric_limits<real_t>::epsilon());

    test_filter.a_posteriori_step();

    EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits<real_t>::epsilon());
    EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field<Position>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(0).norm()*2);
    EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field<Velocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(3).norm()*2);
    EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field<Attitude>()))),
        test_filter.root_covariance.diagonal().segment<3>(6).norm()*2);
    EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field<AngularVelocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(9).norm()*2);
}

TEST(SquareRootCoreTest, InnovationStepPartialMeasurement) {
    MyCore test_filter = create_initialised_sr_test_filter();
    MyMeasurementVector m;

    m.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -9.8));
    m.set_field<Magnetometer>(UKF::FieldVector(0, -1, 0));
    m.set_field<Gyroscope>(UKF::Vector<3>(0.5, 0, 0));

    test_filter.a_priori_step(0.01);
    test_filter.innovation_step(m);

    EXPECT_GE(test_filter.innovation_root_covariance.determinant(), -std::numeric_limits<real_t>::epsilon());

    test_filter.a_posteriori_step();

    EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits<real_t>::epsilon());
    EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field<Position>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(0).norm()*2);
    EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field<Velocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(3).norm()*2);
    EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field<Attitude>()))),
        test_filter.root_covariance.diagonal().segment<3>(6).norm()*2);
    EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field<AngularVelocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(9).norm()*2);
}

TEST(SquareRootCoreTest, InnovationStepWithInputs) {
    MyCore test_filter = create_initialised_sr_test_filter();
    MyMeasurementVector m;

    m.set_field<GPS_Position>(UKF::Vector<3>(100, 10, -50));
    m.set_field<GPS_Velocity>(UKF::Vector<3>(20, 0, 0));
    m.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -15));
    m.set_field<Magnetometer>(UKF::FieldVector(0, -1, 0));
    m.set_field<Gyroscope>(UKF::Vector<3>(0.5, 0, 0));

    test_filter.a_priori_step(0.01, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0));
    test_filter.innovation_step(m, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0));

    EXPECT_GE(test_filter.innovation_root_covariance.determinant(), -std::numeric_limits<real_t>::epsilon());

    test_filter.a_posteriori_step();

    EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits<real_t>::epsilon());
    EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field<Position>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(0).norm()*2);
    EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field<Velocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(3).norm()*2);
    EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field<Attitude>()))),
        test_filter.root_covariance.diagonal().segment<3>(6).norm()*2);
    EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field<AngularVelocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(9).norm()*2);
}

TEST(SquareRootCoreTest, InnovationStepPartialMeasurementWithInputs) {
    MyCore test_filter = create_initialised_sr_test_filter();
    MyMeasurementVector m;

    m.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -15));
    m.set_field<Magnetometer>(UKF::FieldVector(0, -1, 0));
    m.set_field<Gyroscope>(UKF::Vector<3>(0.5, 0, 0));

    test_filter.a_priori_step(0.01, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0));
    test_filter.innovation_step(m, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0));

    EXPECT_GE(test_filter.innovation_root_covariance.determinant(), -std::numeric_limits<real_t>::epsilon());

    test_filter.a_posteriori_step();

    EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits<real_t>::epsilon());
    EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field<Position>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(0).norm()*2);
    EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field<Velocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(3).norm()*2);
    EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field<Attitude>()))),
        test_filter.root_covariance.diagonal().segment<3>(6).norm()*2);
    EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field<AngularVelocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(9).norm()*2);
}

TEST(SquareRootCoreTest, APosterioriStep) {
    MyCore test_filter = create_initialised_sr_test_filter();
    MyMeasurementVector m;

    m.set_field<GPS_Position>(UKF::Vector<3>(100, 10, -50));
    m.set_field<GPS_Velocity>(UKF::Vector<3>(20, 0, 0));
    m.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -9.8));
    m.set_field<Magnetometer>(UKF::FieldVector(0, -1, 0));
    m.set_field<Gyroscope>(UKF::Vector<3>(0.5, 0, 0));

    test_filter.a_priori_step(0.01);
    test_filter.innovation_step(m);
    test_filter.a_posteriori_step();

    EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits<real_t>::epsilon());
    EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field<Position>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(0).norm()*2);
    EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field<Velocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(3).norm()*2);
    EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field<Attitude>()))),
        test_filter.root_covariance.diagonal().segment<3>(6).norm()*2);
    EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field<AngularVelocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(9).norm()*2);
}

TEST(SquareRootCoreTest, FullStep) {
    MyCore test_filter = create_initialised_sr_test_filter();
    MyMeasurementVector m;

    m.set_field<GPS_Position>(UKF::Vector<3>(100, 10, -50));
    m.set_field<GPS_Velocity>(UKF::Vector<3>(20, 0, 0));
    m.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -14.8));
    m.set_field<Magnetometer>(UKF::FieldVector(0, -1, 0));
    m.set_field<Gyroscope>(UKF::Vector<3>(0.5, 0, 0));

    test_filter.step(0.01, m, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0));

    EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits<real_t>::epsilon());
    EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field<Position>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(0).norm()*2);
    EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field<Velocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(3).norm()*2);
    EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field<Attitude>()))),
        test_filter.root_covariance.diagonal().segment<3>(6).norm()*2);
    EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field<AngularVelocity>()).norm(),
        test_filter.root_covariance.diagonal().segment<3>(9).norm()*2);
}


// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "lane_departure_checker/lane_departure_checker.hpp"

#include "lane_departure_checker/util/create_vehicle_footprint.hpp"

#include <autoware_utils/geometry/geometry.hpp>
#include <autoware_utils/math/normalization.hpp>
#include <autoware_utils/math/unit_conversion.hpp>
#include <autoware_utils/system/stop_watch.hpp>

#include <boost/geometry.hpp>

#include <lanelet2_core/geometry/Polygon.h>
#include <tf2/utils.h>

#include <algorithm>
#include <vector>

using autoware_utils::LinearRing2d;
using autoware_utils::MultiPoint2d;
using autoware_utils::Point2d;

namespace
{
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using TrajectoryPoints = std::vector<TrajectoryPoint>;

double calcBrakingDistance(
  const double abs_velocity, const double max_deceleration, const double delay_time)
{
  return (abs_velocity * abs_velocity) / (2.0 * max_deceleration) + delay_time * abs_velocity;
}

bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2d & point)
{
  for (const auto & ll : candidate_lanelets) {
    if (boost::geometry::within(point, ll.polygon2d().basicPolygon())) {
      return true;
    }
  }

  return false;
}

size_t findNearestIndex(const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose)
{
  std::vector<double> distances;
  distances.reserve(trajectory.points.size());
  std::transform(
    trajectory.points.cbegin(), trajectory.points.cend(), std::back_inserter(distances),
    [&](const TrajectoryPoint & p) {
      const auto p1 = autoware_utils::fromMsg(p.pose.position).to_2d();
      const auto p2 = autoware_utils::fromMsg(pose.position).to_2d();
      return boost::geometry::distance(p1, p2);
    });

  const auto min_itr = std::min_element(distances.cbegin(), distances.cend());
  const auto min_idx = static_cast<size_t>(std::distance(distances.cbegin(), min_itr));

  return min_idx;
}

LinearRing2d createHullFromFootprints(const std::vector<LinearRing2d> & footprints)
{
  MultiPoint2d combined;
  for (const auto & footprint : footprints) {
    for (const auto & p : footprint) {
      combined.push_back(p);
    }
  }

  LinearRing2d hull;
  boost::geometry::convex_hull(combined, hull);

  return hull;
}

lanelet::ConstLanelets getCandidateLanelets(
  const lanelet::ConstLanelets & route_lanelets,
  const std::vector<LinearRing2d> & vehicle_footprints)
{
  lanelet::ConstLanelets candidate_lanelets;

  // Find lanes within the convex hull of footprints
  const auto footprint_hull = createHullFromFootprints(vehicle_footprints);
  for (const auto & route_lanelet : route_lanelets) {
    const auto poly = route_lanelet.polygon2d().basicPolygon();
    if (!boost::geometry::disjoint(poly, footprint_hull)) {
      candidate_lanelets.push_back(route_lanelet);
    }
  }

  return candidate_lanelets;
}
}  // namespace

namespace lane_departure_checker
{
Output LaneDepartureChecker::update(const Input & input)
{
  Output output{};

  autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;

  output.trajectory_deviation =
    calcTrajectoryDeviation(*input.reference_trajectory, input.current_pose->pose);
  output.processing_time_map["calcTrajectoryDeviation"] = stop_watch.toc(true);

  {
    constexpr double min_velocity = 0.01;
    const auto & raw_abs_velocity = std::abs(input.current_odom->twist.twist.linear.x);
    const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity;

    const auto braking_distance =
      calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time);

    output.resampled_trajectory = cutTrajectory(
      resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance);
    output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true);
  }
  output.vehicle_footprints =
    createVehicleFootprints(input.current_odom->pose, output.resampled_trajectory, param_);
  output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true);

  output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints);
  output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true);

  output.candidate_lanelets = getCandidateLanelets(input.route_lanelets, output.vehicle_footprints);
  output.processing_time_map["getCandidateLanelets"] = stop_watch.toc(true);

  output.will_leave_lane = willLeaveLane(output.candidate_lanelets, output.vehicle_footprints);
  output.processing_time_map["willLeaveLane"] = stop_watch.toc(true);

  output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front());
  output.processing_time_map["isOutOfLane"] = stop_watch.toc(true);

  return output;
}

PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation(
  const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose)
{
  const auto nearest_idx = findNearestIndex(trajectory, pose);
  return autoware_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose);
}

TrajectoryPoints LaneDepartureChecker::resampleTrajectory(
  const Trajectory & trajectory, const double interval)
{
  TrajectoryPoints resampled;

  resampled.push_back(trajectory.points.front());
  for (size_t i = 1; i < trajectory.points.size() - 1; ++i) {
    const auto & point = trajectory.points.at(i);

    const auto p1 = autoware_utils::fromMsg(resampled.back().pose.position);
    const auto p2 = autoware_utils::fromMsg(point.pose.position);

    if (boost::geometry::distance(p1.to_2d(), p2.to_2d()) > interval) {
      resampled.push_back(point);
    }
  }
  resampled.push_back(trajectory.points.back());

  return resampled;
}

TrajectoryPoints LaneDepartureChecker::cutTrajectory(
  const TrajectoryPoints & trajectory, const double length)
{
  TrajectoryPoints cut;

  double total_length = 0.0;
  cut.push_back(trajectory.front());
  for (size_t i = 1; i < trajectory.size(); ++i) {
    const auto & point = trajectory.at(i);

    const auto p1 = autoware_utils::fromMsg(cut.back().pose.position);
    const auto p2 = autoware_utils::fromMsg(point.pose.position);
    const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d());

    const auto remain_distance = length - total_length;

    // Over length
    if (remain_distance <= 0) {
      break;
    }

    // Require interpolation
    if (remain_distance <= points_distance) {
      const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized();

      TrajectoryPoint p;
      p.pose.position.x = p_interpolated.x();
      p.pose.position.y = p_interpolated.y();
      p.pose.position.z = p_interpolated.z();
      p.pose.orientation = point.pose.orientation;

      cut.push_back(p);
      break;
    }

    cut.push_back(point);
    total_length += points_distance;
  }

  return cut;
}

std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
  const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory,
  const Param & param)
{
  // Calculate longitudinal and lateral margin based on covariance
  const auto margin = calcFootprintMargin(covariance, param.footprint_margin_scale);

  // Create vehicle footprint in base_link coordinate
  const auto local_vehicle_footprint = createVehicleFootprint(*vehicle_info_ptr_, margin);

  // Create vehicle footprint on each TrajectoryPoint
  std::vector<LinearRing2d> vehicle_footprints;
  for (const auto & p : trajectory) {
    vehicle_footprints.push_back(
      transformVector(local_vehicle_footprint, autoware_utils::pose2transform(p.pose)));
  }

  return vehicle_footprints;
}

std::vector<LinearRing2d> LaneDepartureChecker::createVehiclePassingAreas(
  const std::vector<LinearRing2d> & vehicle_footprints)
{
  // Create hull from two adjacent vehicle footprints
  std::vector<LinearRing2d> areas;
  for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) {
    const auto & footprint1 = vehicle_footprints.at(i);
    const auto & footprint2 = vehicle_footprints.at(i + 1);
    areas.push_back(createHullFromFootprints({footprint1, footprint2}));
  }

  return areas;
}

bool LaneDepartureChecker::willLeaveLane(
  const lanelet::ConstLanelets & candidate_lanelets,
  const std::vector<LinearRing2d> & vehicle_footprints)
{
  for (const auto & vehicle_footprint : vehicle_footprints) {
    if (isOutOfLane(candidate_lanelets, vehicle_footprint)) {
      return true;
    }
  }

  return false;
}

bool LaneDepartureChecker::isOutOfLane(
  const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint)
{
  for (const auto & point : vehicle_footprint) {
    if (!isInAnyLane(candidate_lanelets, point)) {
      return true;
    }
  }

  return false;
}
}  // namespace lane_departure_checker


#pragma once

#include "yaml/include/myYaml/yaml.h"
#include <Eigen/Dense>
#include <algorithm>
#include <chrono>
#include <iostream>
#include <math.h>

// =============================================================================
// YAML Readers
// =============================================================================
template <typename YamlType>
YamlType readParameter(const YAML::Node &node, const std::string &name) {
  try {
    return node[name.c_str()].as<YamlType>();
  } catch (...) {
    throw std::runtime_error(name);
  }
};

template <typename YamlType>
void readParameter(const YAML::Node &node, const std::string &name,
                   YamlType &parameter) {
  try {
    parameter = readParameter<YamlType>(node, name);
  } catch (...) {
    throw std::runtime_error(name);
  }
};

// =============================================================================
// Rotations
// =============================================================================
// Convention here for euler angle is to rotate in Z-Y-X order and represent
// euler angles in (x,y,z) order
Eigen::Matrix3d euler_xyz_to_rot(const Eigen::Vector3d &euler_xyz);
Eigen::Quaternion<double> euler_xyz_to_quat(const Eigen::Vector3d &euler_xyz);
Eigen::Vector3d quat_to_euler_xyz(const Eigen::Quaternion<double> &quat);
Eigen::MatrixXd adjoint(const Eigen::MatrixXd &R, const Eigen::Vector3d &p);
Eigen::MatrixXd adjoint(const Eigen::Isometry3d &iso);
Eigen::Matrix3d skew_symmetric(const Eigen::Vector3d &omg);

// =============================================================================
// Pretty Print
// =============================================================================
void pretty_print(Eigen::Isometry3d const &iso, std::ostream &os,
                  std::string const &title, std::string const &prefix = "",
                  bool nonl = false);
void pretty_print(Eigen::VectorXd const &vv, std::ostream &os,
                  std::string const &title, std::string const &prefix = "",
                  bool nonl = false);
void pretty_print(Eigen::MatrixXd const &mm, std::ostream &os,
                  std::string const &title, std::string const &prefix = "",
                  bool vecmode = false, bool nonl = false);
void pretty_print(Eigen::Quaternion<double> const &qq, std::ostream &os,
                  std::string const &title, std::string const &prefix = "",
                  bool nonl = false);
void pretty_print(Eigen::Vector3d const &vv, std::ostream &os,
                  std::string const &title, std::string const &prefix = "",
                  bool nonl = false);
void pretty_print(const std::vector<double> &_vec, const char *title);
void pretty_print(const std::vector<int> &_vec, const char *title);
std::string pretty_string(Eigen::VectorXd const &vv);
std::string pretty_string(Eigen::MatrixXd const &mm, std::string const &prefix);
std::string pretty_string(double vv);

// =============================================================================
// Stat
// =============================================================================
Eigen::VectorXd Normalize(const Eigen::VectorXd &v, const Eigen::VectorXd &mean,
                          const Eigen::VectorXd &standard_dev);

Eigen::VectorXd Denormalize(const Eigen::VectorXd &v,
                            const Eigen::VectorXd &mean,
                            const Eigen::VectorXd &standard_dev);

// =============================================================================
// Hermite Curves
// =============================================================================
class HermiteCurve {
public:
  HermiteCurve();
  HermiteCurve(const double &start_pos, const double &start_vel,
               const double &end_pos, const double &end_vel);
  ~HermiteCurve();
  double evaluate(const double &s_in);
  double evaluateFirstDerivative(const double &s_in);
  double evaluateSecondDerivative(const double &s_in);

private:
  double p1;
  double v1;
  double p2;
  double v2;

  double s_;

  // by default clamps within 0 and 1.
  double clamp(const double &s_in, double lo = 0.0, double hi = 1.0);
};

class HermiteCurveVec {
public:
  HermiteCurveVec();
  HermiteCurveVec(const Eigen::VectorXd &start_pos,
                  const Eigen::VectorXd &start_vel,
                  const Eigen::VectorXd &end_pos,
                  const Eigen::VectorXd &end_vel);

  void initialize(const Eigen::VectorXd &start_pos,
                  const Eigen::VectorXd &start_vel,
                  const Eigen::VectorXd &end_pos,
                  const Eigen::VectorXd &end_vel);

  ~HermiteCurveVec();
  Eigen::VectorXd evaluate(const double &s_in);
  Eigen::VectorXd evaluateFirstDerivative(const double &s_in);
  Eigen::VectorXd evaluateSecondDerivative(const double &s_in);

private:
  Eigen::VectorXd p1;
  Eigen::VectorXd v1;
  Eigen::VectorXd p2;
  Eigen::VectorXd v2;

  std::vector<HermiteCurve> curves;
  Eigen::VectorXd output;
};

class HermiteQuaternionCurve {
public:
  HermiteQuaternionCurve();
  HermiteQuaternionCurve(const Eigen::Quaterniond &quat_start,
                         const Eigen::Vector3d &angular_velocity_start,
                         const Eigen::Quaterniond &quat_end,
                         const Eigen::Vector3d &angular_velocity_end);
  ~HermiteQuaternionCurve();

  void initialize(const Eigen::Quaterniond &quat_start,
                  const Eigen::Vector3d &angular_velocity_start,
                  const Eigen::Quaterniond &quat_end,
                  const Eigen::Vector3d &angular_velocity_end);

  // All values are expressed in "world frame"
  void evaluate(const double &s_in, Eigen::Quaterniond &quat_out);
  void getAngularVelocity(const double &s_in, Eigen::Vector3d &ang_vel_out);
  void getAngularAcceleration(const double &s_in, Eigen::Vector3d &ang_acc_out);

private:
  Eigen::Quaterniond qa;   // Starting quaternion
  Eigen::Vector3d omega_a; // Starting Angular Velocity
  Eigen::Quaterniond qb;   // Ending quaternion
  Eigen::Vector3d omega_b; // Ending Angular velocity

  Eigen::AngleAxisd omega_a_aa; // axis angle representation of omega_a
  Eigen::AngleAxisd omega_b_aa; // axis angle representation of omega_b

  void initialize_data_structures();

  void computeBasis(const double &s_in); // computes the basis functions
  void computeOmegas();

  Eigen::Quaterniond q0; // quat0
  Eigen::Quaterniond q1; // quat1
  Eigen::Quaterniond q2; // quat1
  Eigen::Quaterniond q3; // quat1

  double b1; // basis 1
  double b2; // basis 2
  double b3; // basis 3

  double bdot1; // 1st derivative of basis 1
  double bdot2; // 1st derivative of basis 2
  double bdot3; // 1st derivative of basis 3

  double bddot1; // 2nd derivative of basis 1
  double bddot2; // 2nd derivative of basis 2
  double bddot3; // 2nd derivative of basis 3

  Eigen::Vector3d omega_1;
  Eigen::Vector3d omega_2;
  Eigen::Vector3d omega_3;

  Eigen::AngleAxisd omega_1aa;
  Eigen::AngleAxisd omega_2aa;
  Eigen::AngleAxisd omega_3aa;

  // Allocate memory for quaternion operations
  Eigen::Quaterniond qtmp1;
  Eigen::Quaterniond qtmp2;
  Eigen::Quaterniond qtmp3;

  // progression variable
  double s_;
  // by default clamps within 0 and 1.
  double clamp(const double &s_in, double lo = 0.0, double hi = 1.0);

  void printQuat(const Eigen::Quaterniond &quat);
};

// =============================================================================
// MISC
// =============================================================================
class Clock {
public:
  Clock() {}
  ~Clock() {}

  void start() { ini_time_ = std::chrono::high_resolution_clock::now(); }
  // return in milliseconds
  double stop() {
    end_time_ = std::chrono::high_resolution_clock::now();
    duration_ = std::chrono::duration_cast<std::chrono::microseconds>(
        end_time_ - ini_time_);
    return double(duration_.count()) * 1e-3;
  }

private:
  std::chrono::microseconds duration_;
  std::chrono::high_resolution_clock::time_point ini_time_, end_time_;
};


#include <boost/test/unit_test.hpp>

#include <iostream>
#include <stdexcept>

#include "Permutation.hpp"

using namespace mist;

template<typename T>
static void
print(Permutation<T> const& perm)
{
  std::cout << perm.to_string() << std::endl;
}

BOOST_AUTO_TEST_CASE(Permuation_constructor)
{
  Permutation<int> perm;
  Permutation<double> p2;
  Permutation<int> p3 = { 0, 1, 2, 3 };
}

BOOST_AUTO_TEST_CASE(Permuation_lexigraphic_2)
{
  Permutation<int> perm = { 0, 1 };
  Permutation<int> e1 = { 1, 0 };
  Permutation<int> e2 = { 0, 1 };
  BOOST_TEST(perm.next());
  BOOST_TEST(perm == e1);
  BOOST_TEST(!perm.next()); // reached end / beginning
  BOOST_TEST(perm == e2);
}

BOOST_AUTO_TEST_CASE(Permuation_lexigraphic_2_static)
{
  std::vector<int> e = { 1, 0 };
  std::vector<int> e0 = { 0, 1 };
  std::vector<int> e1 = { 1, 0 };
  BOOST_TEST(e == e1);
  BOOST_TEST(!permutation::next(e));
  BOOST_TEST(e == e0);
  BOOST_TEST(permutation::next(e));
  BOOST_TEST(e == e1);
}

BOOST_AUTO_TEST_CASE(Permuation_lexigraphic_3_full)
{
  // starting with ordered permutation go all the way around
  Permutation<int> perm = { 0, 1, 2 };
  Permutation<int> e0 = { 0, 1, 2 };
  Permutation<int> e1 = { 0, 2, 1 };
  Permutation<int> e2 = { 1, 0, 2 };
  Permutation<int> e3 = { 1, 2, 0 };
  Permutation<int> e4 = { 2, 0, 1 };
  Permutation<int> e5 = { 2, 1, 0 };
  BOOST_TEST(perm.next());
  BOOST_TEST(perm == e1);
  BOOST_TEST(perm.next());
  BOOST_TEST(perm == e2);
  BOOST_TEST(perm.next());
  BOOST_TEST(perm == e3);
  BOOST_TEST(perm.next());
  BOOST_TEST(perm == e4);
  BOOST_TEST(perm.next());
  BOOST_TEST(perm == e5);
  BOOST_TEST(!perm.next());
  BOOST_TEST(perm == e0);
}

BOOST_AUTO_TEST_CASE(Permuation_lexigraphic_3_full_static)
{
  // starting with ordered permutation go all the way around
  Permutation<int> e = { 0, 1, 2 };
  Permutation<int> e0 = { 0, 1, 2 };
  Permutation<int> e1 = { 0, 2, 1 };
  Permutation<int> e2 = { 1, 0, 2 };
  Permutation<int> e3 = { 1, 2, 0 };
  Permutation<int> e4 = { 2, 0, 1 };
  Permutation<int> e5 = { 2, 1, 0 };
  BOOST_TEST(e == e0);
  BOOST_TEST(permutation::next(e));
  BOOST_TEST(e == e1);
  BOOST_TEST(permutation::next(e));
  BOOST_TEST(e == e2);
  BOOST_TEST(permutation::next(e));
  BOOST_TEST(e == e3);
  BOOST_TEST(permutation::next(e));
  BOOST_TEST(e == e4);
  BOOST_TEST(permutation::next(e));
  BOOST_TEST(e == e5);
  BOOST_TEST(!permutation::next(e));
  BOOST_TEST(e == e0);
}

BOOST_AUTO_TEST_CASE(Permuation_lexigraphic_3_random_static)
{
  // starting with ordered permutation go all the way around
  Permutation<int> e = { 0, 1, 2 };
  Permutation<int> permutations[6];
  permutations[0] = { 0, 1, 2 };
  permutations[1] = { 0, 2, 1 };
  permutations[2] = { 1, 0, 2 };
  permutations[3] = { 1, 2, 0 };
  permutations[4] = { 2, 0, 1 };
  permutations[5] = { 2, 1, 0 };

  BOOST_TEST(e == permutations[0]);

  // get random permutation
  std::random_device rd;
  std::mt19937 gen(rd());
  permutation::random(e, gen);

  // check if it's valid
  bool valid_permutation = false;
  for (auto p : permutations) {
    if (p == e) {
      valid_permutation = true;
      break;
    }
  }
  BOOST_ASSERT(valid_permutation);
}

BOOST_AUTO_TEST_CASE(Permuation_lexigraphic_3_partial)
{
  // starting with unordered permutation, go around util ordered
  Permutation<int> perm = { 0, -1, 2 };
  Permutation<int> e1 = { 0, 2, -1 };
  Permutation<int> e2 = { 2, -1, 0 };
  Permutation<int> e3 = { 2, 0, -1 };
  Permutation<int> e4 = { -1, 0, 2 };
  BOOST_TEST(perm.next());
  BOOST_TEST(perm == e1);
  BOOST_TEST(perm.next());
  BOOST_TEST(perm == e2);
  BOOST_TEST(perm.next());
  BOOST_TEST(perm == e3);
  BOOST_TEST(!perm.next());
  BOOST_TEST(perm == e4);
}

BOOST_AUTO_TEST_CASE(Permuation_apply_char)
{
  std::vector<char> test1 = { 't', 'a', 's', 'k' };
  Permutation<int> perm = { 1, 2, 0, 3 };
  auto test1_p = perm.apply(test1);
  BOOST_TEST(test1_p == (std::vector<char>{ 'a', 's', 't', 'k' }));
}

BOOST_AUTO_TEST_CASE(Permuation_apply_bool)
{
  std::vector<bool> test1 = { 0, 1, 1, 1, 0, 0 };
  std::vector<bool> exp1 = { 0, 1, 1, 0, 1, 0 };
  Permutation<int> perm = { 5, 3, 1, 0, 2, 4 };
  auto test1_p = perm.apply(test1);
  BOOST_TEST(test1_p == exp1);
}