#include <stdio.h>
#include <vector>
#include <map>
#include <random>
#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <arc_utilities/simple_robot_model_interface.hpp>

#ifndef SIMPLE_LINKED_ROBOT_MODEL_HPP
#define SIMPLE_LINKED_ROBOT_MODEL_HPP

namespace simple_linked_robot_model
{
    class SimpleJointModel
    {
    public:

        enum JOINT_TYPE : uint32_t {PRISMATIC=4, REVOLUTE=1, CONTINUOUS=2, FIXED=0};

    protected:

        std::pair<double, double> limits_;
        double value_;
        JOINT_TYPE type_;

    public:

        static inline uint64_t Serialize(const SimpleJointModel& model, std::vector<uint8_t>& buffer)
        {
            return model.SerializeSelf(buffer);
        }

        static inline std::pair<SimpleJointModel, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current)
        {
            SimpleJointModel temp_model;
            const uint64_t bytes_read = temp_model.DeserializeSelf(buffer, current);
            return std::make_pair(temp_model, bytes_read);
        }

        inline SimpleJointModel(const std::pair<double, double>& limits, const double value, const JOINT_TYPE type)
        {
            type_ = type;
            if (IsContinuous())
            {
                limits_ = std::pair<double, double>(-std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
            }
            else
            {
                if (limits.first > limits.second)
                {
                    throw std::invalid_argument("limits.first > limits.second");
                }
                limits_ = limits;
            }
            SetValue(value);
        }

        inline SimpleJointModel()
        {
            limits_ = std::pair<double, double>(0.0, 0.0);
            type_ = JOINT_TYPE::FIXED;
            SetValue(0.0);
        }

        inline uint64_t SerializeSelf(std::vector<uint8_t>& buffer) const
        {
            const uint64_t start_buffer_size = buffer.size();
            arc_helpers::SerializeFixedSizePOD<double>(limits_.first, buffer);
            arc_helpers::SerializeFixedSizePOD<double>(limits_.second, buffer);
            arc_helpers::SerializeFixedSizePOD<double>(value_, buffer);
            arc_helpers::SerializeFixedSizePOD<uint32_t>((uint32_t)type_, buffer);
            // Figure out how many bytes were written
            const uint64_t end_buffer_size = buffer.size();
            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
            return bytes_written;
        }

        inline uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current)
        {
            uint64_t current_position = current;
            const std::pair<double, uint64_t> deserialized_limits_first = arc_helpers::DeserializeFixedSizePOD<double>(buffer, current_position);
            limits_.first = deserialized_limits_first.first;
            current_position += deserialized_limits_first.second;
            const std::pair<double, uint64_t> deserialized_limits_second = arc_helpers::DeserializeFixedSizePOD<double>(buffer, current_position);
            limits_.second = deserialized_limits_second.first;
            current_position += deserialized_limits_second.second;
            const std::pair<double, uint64_t> deserialized_value = arc_helpers::DeserializeFixedSizePOD<double>(buffer, current_position);
            value_ = deserialized_value.first;
            current_position += deserialized_value.second;
            const std::pair<uint32_t, uint64_t> deserialized_type = arc_helpers::DeserializeFixedSizePOD<uint32_t>(buffer, current_position);
            type_ = (JOINT_TYPE)deserialized_type.first;
            current_position += deserialized_type.second;
            // Figure out how many bytes were read
            const uint64_t bytes_read = current_position - current;
            return bytes_read;
        }

        inline bool IsContinuous() const
        {
            if (type_ == JOINT_TYPE::CONTINUOUS)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        inline bool IsRevolute() const
        {
            if ((type_ == JOINT_TYPE::REVOLUTE) || (type_ == JOINT_TYPE::CONTINUOUS))
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        inline bool IsPrismatic() const
        {
            if (type_ == JOINT_TYPE::PRISMATIC)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        inline bool IsFixed() const
        {
            if (type_ == JOINT_TYPE::FIXED)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        inline JOINT_TYPE GetType() const
        {
            return type_;
        }

        inline bool IsSameType(const SimpleJointModel& other) const
        {
            if (type_ == other.GetType())
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        inline std::string GetTypeString() const
        {
            if (IsContinuous())
            {
                return std::string("Continuous");
            }
            else if (IsRevolute())
            {
                return std::string("Revolute");
            }
            else if (IsPrismatic())
            {
                return std::string("Prismatic");
            }
            else if (IsFixed())
            {
                return std::string("Fixed");
            }
            else
            {
                throw std::runtime_error("Invalid joint model type");
            }
        }

        inline double GetValue() const
        {
            return value_;
        }

        inline std::pair<double, double> GetLimits() const
        {
            return limits_;
        }

        inline bool InLimits(const double value) const
        {
            if (IsContinuous())
            {
                 return true;
            }
            else
            {
                if ((value < limits_.first) || (value > limits_.second))
                {
                    return false;
                }
                else
                {
                    return true;
                }
            }
        }

        inline double EnforceLimits(const double value) const
        {
            if (!std::isnan(value) && !std::isinf(value))
            {
                if (IsContinuous())
                {
                    return EigenHelpers::EnforceContinuousRevoluteBounds(value);
                }
                else
                {
                    if (value < limits_.first)
                    {
                        return limits_.first;
                    }
                    else if (value > limits_.second)
                    {
                        return limits_.second;
                    }
                    else
                    {
                        return value;
                    }
                }
            }
            else
            {
                throw std::invalid_argument("value is NAN or INF");
            }
        }

        inline void SetValue(const double value)
        {
            const double real_value = EnforceLimits(value);
            value_ = real_value;
        }

        inline double SignedDistance(const double v1, const double v2) const
        {
            if (IsContinuous())
            {
                return EigenHelpers::ContinuousRevoluteSignedDistance(v1, v2);
            }
            else
            {
                return (v2 - v1);
            }
        }

        inline double SignedDistance(const double v) const
        {
            return SignedDistance(GetValue(), v);
        }

        inline double SignedDistance(const SimpleJointModel& other) const
        {
            if (IsSameType(other))
            {
                return SignedDistance(GetValue(), other.GetValue());
            }
            else
            {
              throw std::invalid_argument("Cannot compute distance between joint models of different types");
            }
        }

        inline double Distance(const double v1, const double v2) const
        {
            return std::abs(SignedDistance(v1, v2));
        }

        inline double Distance(const double v) const
        {
            return std::abs(SignedDistance(GetValue(), v));
        }

        inline double Distance(const SimpleJointModel& other) const
        {
            return std::abs(SignedDistance(other));
        }

        inline SimpleJointModel CopyWithNewValue(const double value) const
        {
            return SimpleJointModel(limits_, value, type_);
        }
    };

    inline std::ostream& operator<<(std::ostream& strm, const SimpleJointModel& joint_model)
    {
        const std::pair<double, double> limits = joint_model.GetLimits();
        strm << joint_model.GetValue() << "[" << limits.first << "," << limits.second << ")";
        return strm;
    }

    class SimpleLinkedConfigSerializer
    {
    public:

        static inline std::string TypeName()
        {
            return std::string("SimpleLinkedConfigurationSerializer");
        }

        static inline uint64_t Serialize(const std::vector<SimpleJointModel>& value, std::vector<uint8_t>& buffer)
        {
            return arc_helpers::SerializeVector<SimpleJointModel>(value, buffer, SimpleJointModel::Serialize);
        }

        static inline std::pair<std::vector<SimpleJointModel>, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current)
        {
            return arc_helpers::DeserializeVector<SimpleJointModel>(buffer, current, SimpleJointModel::Deserialize);
        }
    };

    // Typedef to make our life a bit easier
    using SimpleLinkedConfiguration = std::vector<SimpleJointModel>;
    using SimpleLinkedConfigAlloc = std::allocator<SimpleLinkedConfiguration>;

    class RobotJoint
    {
    protected:

        Eigen::Isometry3d joint_transform_;
        Eigen::Vector3d joint_axis_;
        int64_t parent_link_index_;
        int64_t child_link_index_;
        SimpleJointModel joint_model_;
        std::string joint_name_;

    public:

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        RobotJoint(const Eigen::Isometry3d& joint_transform,
                   const Eigen::Vector3d& joint_axis,
                   const int64_t parent_link_index,
                   const int64_t child_link_index,
                   const SimpleJointModel& joint_model,
                   const std::string& joint_name)
            : joint_transform_(joint_transform),
              joint_axis_(joint_axis),
              parent_link_index_(parent_link_index),
              child_link_index_(child_link_index),
              joint_model_(joint_model),
              joint_name_(joint_name) {}

        RobotJoint() : parent_link_index_(-1), child_link_index_(-1) {}

        const Eigen::Isometry3d& JointTransform() const { return joint_transform_; }

        const Eigen::Vector3d& JointAxis() const { return joint_axis_; }

        const int64_t& ParentLinkIndex() const { return parent_link_index_; }

        const int64_t& ChildLinkIndex() const { return child_link_index_; }

        const SimpleJointModel& JointModel() const { return joint_model_; }

        const std::string& JointName() const { return joint_name_; }

        Eigen::Isometry3d& JointTransform() { return joint_transform_; }

        Eigen::Vector3d& JointAxis() { return joint_axis_; }

        int64_t& ParentLinkIndex() { return parent_link_index_; }

        int64_t& ChildLinkIndex() { return child_link_index_; }

        SimpleJointModel& JointModel() { return joint_model_; }

        std::string& JointName() { return joint_name_; }
    };

    class RobotLink
    {
    protected:

        Eigen::Isometry3d link_transform_;
        std::string link_name_;

    public:

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        RobotLink() {}

        RobotLink(const std::string& link_name) : link_name_(link_name) {}

        const Eigen::Isometry3d& LinkTransform() const { return link_transform_; }

        const std::string& LinkName() const { return link_name_; }

        std::string& LinkName() { return link_name_; }

        void UpdateLinkTransform(const Eigen::Isometry3d& transform) { link_transform_ = transform; }
    };

    class SimpleLinkedRobot : public simple_robot_model_interface::SimpleRobotModelInterface<SimpleLinkedConfiguration, SimpleLinkedConfigAlloc>
    {
    protected:

        struct KinematicSkeletonElement
        {
            int64_t parent_link_index_;
            int64_t parent_joint_index_;
            std::vector<int64_t> child_link_indices_;
            std::vector<int64_t> child_joint_indices_;

            KinematicSkeletonElement() : parent_link_index_(-1), parent_joint_index_(-1) {}
        };

        SimpleLinkedConfiguration config_;
        Eigen::Isometry3d base_transform_;
        Eigen::Isometry3d inverse_base_transform_;
        std::vector<RobotLink> links_;
        std::vector<RobotJoint> joints_;
        std::vector<KinematicSkeletonElement> kinematic_skeleton_;
        std::map<int64_t, int64_t> active_joint_index_map_;
        std::vector<double> joint_distance_weights_;
        size_t num_active_joints_;

        inline void UpdateTransforms()
        {
            // Update the transform for the first link
            links_[0].UpdateLinkTransform(base_transform_);
            // Go out the kinematic chain
            for (size_t idx = 0; idx < joints_.size(); idx++)
            {
                // Get the current joint
                const RobotJoint& current_joint = joints_[idx];
                // Get the parent link
                const RobotLink& parent_link = links_[(size_t)current_joint.ParentLinkIndex()];
                // Get the child link
                RobotLink& child_link = links_[(size_t)current_joint.ChildLinkIndex()];
                // Get the parent_link transform
                const Eigen::Isometry3d parent_transform = parent_link.LinkTransform();
                // Get the parent_link->joint transform
                const Eigen::Isometry3d parent_to_joint_transform = current_joint.JointTransform();
                // Compute the base->joint_transform
                const Eigen::Isometry3d to_joint_transform = parent_transform * parent_to_joint_transform;
                // Compute the joint transform
                if (current_joint.JointModel().IsRevolute())
                {
                    const Eigen::Isometry3d joint_transform = Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond(Eigen::AngleAxisd(current_joint.JointModel().GetValue(), current_joint.JointAxis()));
                    const Eigen::Isometry3d child_transform = to_joint_transform * joint_transform;
                    child_link.UpdateLinkTransform(child_transform);
                }
                else if (current_joint.JointModel().IsPrismatic())
                {
                    const Eigen::Translation3d joint_translation = (Eigen::Translation3d)(current_joint.JointAxis() * current_joint.JointModel().GetValue());
                    const Eigen::Isometry3d joint_transform = joint_translation * Eigen::Quaterniond::Identity();
                    const Eigen::Isometry3d child_transform = to_joint_transform * joint_transform;
                    child_link.UpdateLinkTransform(child_transform);
                }
                else
                {
                    // Joint is fixed
                    child_link.UpdateLinkTransform(to_joint_transform);
                }
            }
        }

        inline void SetConfig(const SimpleLinkedConfiguration& new_config)
        {
            if (new_config.size() == num_active_joints_)
            {
                config_.clear();
                size_t config_idx = 0u;
                for (size_t idx = 0; idx < joints_.size(); idx++)
                {
                    RobotJoint& current_joint = joints_[idx];
                    // Skip fixed joints
                    if (current_joint.JointModel().IsFixed())
                    {
                        continue;
                    }
                    else
                    {
                        if (config_idx < new_config.size())
                        {
                            const SimpleJointModel& new_joint = new_config[config_idx];
                            current_joint.JointModel().SetValue(new_joint.GetValue());
                            config_.push_back(current_joint.JointModel());
                            config_idx++;
                        }
                        else
                        {
                            throw std::runtime_error("config_idx out of range");
                        }
                    }
                }
                // Update forward kinematics
                UpdateTransforms();
            }
            else
            {
                throw std::invalid_argument("new_config.size() != num_active_joints_");
            }
        }

        static inline std::pair<size_t, std::map<int64_t, int64_t>> MakeActiveJointIndexMap(const std::vector<RobotJoint>& joints)
        {
            size_t num_active_joints = 0;
            std::map<int64_t, int64_t> active_joint_index_map;
            for (size_t idx = 0; idx < joints.size(); idx++)
            {
                const RobotJoint& current_joint = joints[idx];
                // Skip fixed joints
                if (!(current_joint.JointModel().IsFixed()))
                {
                    active_joint_index_map[(int64_t)idx] = (int64_t)num_active_joints;
                    num_active_joints++;
                }
                else
                {
                    active_joint_index_map[(int64_t)idx] = -1;
                }
            }
            return std::make_pair(num_active_joints, active_joint_index_map);
        }

        static bool ExploreChildren(const std::vector<KinematicSkeletonElement>& kinematic_skeleton, const int64_t current_index, std::map<int64_t, uint8_t>& cycle_detection_map)
        {
            // Check if we've been here before
            cycle_detection_map[current_index]++;
            const uint8_t check_val = cycle_detection_map[current_index];
            if (check_val != 0x01)
            {
                std::cerr << "Invalid kinematic structure, link " << current_index << " is part of a cycle" << std::endl;
                return true;
            }
            else
            {
                const KinematicSkeletonElement& current_element = kinematic_skeleton[current_index];
                const std::vector<int64_t>& current_child_indices = current_element.child_link_indices_;
                for (size_t idx = 0; idx < current_child_indices.size(); idx++)
                {
                    // Get the current child index
                    const int64_t current_child_index = current_child_indices[idx];
                    const bool child_cycle_detected = ExploreChildren(kinematic_skeleton, current_child_index, cycle_detection_map);
                    if (child_cycle_detected)
                    {
                        return true;
                    }
                }
                return false;
            }
        }

        static bool CheckKinematicSkeleton(const std::vector<KinematicSkeletonElement>& kinematic_skeleton)
        {
            // Step through each state in the tree. Make sure that the linkage to the parent and child states are correct
            for (size_t current_index = 0; current_index < kinematic_skeleton.size(); current_index++)
            {
                // For every element, make sure all the parent<->child linkages are valid
                const KinematicSkeletonElement& current_element = kinematic_skeleton[current_index];
                // Check the linkage to the parent state
                const int64_t parent_index = current_element.parent_link_index_;
                if ((parent_index >= 0) && (parent_index < (int64_t)kinematic_skeleton.size()))
                {
                    if (parent_index != (int64_t)current_index)
                    {
                        const KinematicSkeletonElement& parent_element = kinematic_skeleton[parent_index];
                        // Make sure the corresponding parent contains the current node in the list of child indices
                        const std::vector<int64_t>& parent_child_indices = parent_element.child_link_indices_;
                        auto index_found = std::find(parent_child_indices.begin(), parent_child_indices.end(), (int64_t)current_index);
                        if (index_found == parent_child_indices.end())
                        {
                            std::cerr << "Parent element " << parent_index << " does not contain child index for current element " << current_index << std::endl;
                            return false;
                        }
                    }
                    else
                    {
                        std::cerr << "Invalid parent index " << parent_index << " for element " << current_index << " [Indices can't be the same]" << std::endl;
                        return false;
                    }
                }
                else if (parent_index < -1)
                {
                    std::cerr << "Invalid parent index " << parent_index << " for element " << current_index << std::endl;
                    return false;
                }
                // Check the linkage to the child states
                const std::vector<int64_t>& current_child_indices = current_element.child_link_indices_;
                for (size_t idx = 0; idx < current_child_indices.size(); idx++)
                {
                    // Get the current child index
                    const int64_t current_child_index = current_child_indices[idx];
                    if ((current_child_index > 0) && (current_child_index < (int64_t)kinematic_skeleton.size()))
                    {
                        if (current_child_index != (int64_t)current_index)
                        {
                            const KinematicSkeletonElement& child_element = kinematic_skeleton[current_child_index];
                            // Make sure the child node points to us as the parent index
                            const int64_t child_parent_index = child_element.parent_link_index_;
                            if (child_parent_index != (int64_t)current_index)
                            {
                                std::cerr << "Parent index " << child_parent_index << " for current child element " << current_child_index << " does not match index " << current_index << " for current element " << std::endl;
                                return false;
                            }
                        }
                        else
                        {
                            std::cerr << "Invalid child index " << current_child_index << " for element " << current_index << " [Indices can't be the same]" << std::endl;
                            return false;
                        }
                    }
                    else
                    {
                        std::cerr << "Invalid child index " << current_child_index << " for element " << current_index << std::endl;
                        return false;
                    }
                }
            }
            // Now that we know all the linkage connections are valid, we need to make sure it is connected and acyclic
            std::map<int64_t, uint8_t> cycle_detection_map;
            const bool cycle_detected = ExploreChildren(kinematic_skeleton, 0, cycle_detection_map);
            if (cycle_detected)
            {
                return false;
            }
            // Make sure we encountered every link ONCE
            for (size_t link_idx = 0; link_idx < kinematic_skeleton.size(); link_idx++)
            {
                const uint8_t check_val = arc_helpers::RetrieveOrDefault(cycle_detection_map, (int64_t)link_idx, (uint8_t)0x00);
                if (check_val != 0x01)
                {
                    std::cerr << "Invalid kinematic structure, link " << link_idx << " not reachable from the root" << std::endl;
                    return false;
                }
            }
            return true;
        }

    public:

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        static inline std::pair<std::vector<KinematicSkeletonElement>, bool> SanityCheckRobotModel(const std::vector<RobotLink>& links, const std::vector<RobotJoint>& joints)
        {
            // Make sure we have enough links and joints
            if (links.size() != (joints.size() + 1))
            {
                std::cerr << links.size() << " links are not enough for " << joints.size() << " joints" << std::endl;
                return std::make_pair(std::vector<KinematicSkeletonElement>(), false);
            }
            // Make sure the links all have unique names
            std::map<std::string, uint32_t> link_name_check_map;
            for (size_t idx = 0; idx < links.size(); idx++)
            {
                const std::string& link_name = links[idx].LinkName();
                link_name_check_map[link_name]++;
                if (link_name_check_map[link_name] > 1)
                {
                    std::cerr << "Link " << link_name << " is not unique" << std::endl;
                    return std::make_pair(std::vector<KinematicSkeletonElement>(), false);
                }
            }
            // Make sure the joints all have unique names
            std::map<std::string, uint32_t> joint_name_check_map;
            for (size_t idx = 0; idx < joints.size(); idx++)
            {
                const std::string& joint_name = joints[idx].JointName();
                joint_name_check_map[joint_name]++;
                if (joint_name_check_map[joint_name] > 1)
                {
                    std::cerr << "Joint " << joint_name << " is not unique" << std::endl;
                    return std::make_pair(std::vector<KinematicSkeletonElement>(), false);
                }
            }
            // Make the kinematic skeleton structure for checking
            std::vector<KinematicSkeletonElement> kinematic_skeleton(links.size(), KinematicSkeletonElement());
            for (size_t joint_idx = 0; joint_idx < joints.size(); joint_idx++)
            {
                // Get the current joint
                const RobotJoint& current_joint = joints[joint_idx];
                // Check the joint axes
                const double joint_axis_norm = current_joint.JointAxis().norm();
                const double error = std::abs(joint_axis_norm - 1.0);
                if (error > std::numeric_limits<double>::epsilon())
                {
                    std::cerr << "Joint axis is not a unit vector" << std::endl;
                    return std::make_pair(std::vector<KinematicSkeletonElement>(), false);
                }
                // Add the edge into the kinematic skeleton
                const int64_t parent_link_index = current_joint.ParentLinkIndex();
                const int64_t child_link_index = current_joint.ChildLinkIndex();
                // Set the child->parent relationship
                kinematic_skeleton[(size_t)child_link_index].parent_link_index_ = parent_link_index;
                kinematic_skeleton[(size_t)child_link_index].parent_joint_index_ = joint_idx;
                // Set the parent->child relationship
                kinematic_skeleton[(size_t)parent_link_index].child_link_indices_.push_back(child_link_index);
                kinematic_skeleton[(size_t)parent_link_index].child_joint_indices_.push_back(joint_idx);
            }
            // Make sure the first link is the root
            const KinematicSkeletonElement& root_element = kinematic_skeleton[0];
            if (root_element.parent_joint_index_ != -1 || root_element.parent_link_index_ != -1)
            {
                std::cerr << "Invalid kinematic structure, first link is not the root" << std::endl;
                return std::make_pair(std::vector<KinematicSkeletonElement>(), false);
            }
            const bool valid_kinematics = CheckKinematicSkeleton(kinematic_skeleton);
            return std::make_pair(kinematic_skeleton, valid_kinematics);
        }

        SimpleLinkedRobot(const Eigen::Isometry3d& base_transform,
                          const std::vector<RobotLink>& links,
                          const std::vector<RobotJoint>& joints,
                          const SimpleLinkedConfiguration& initial_position,
                          const std::vector<double>& joint_distance_weights)
            : simple_robot_model_interface::SimpleRobotModelInterface<SimpleLinkedConfiguration, SimpleLinkedConfigAlloc>()
        {
            base_transform_ = base_transform;
            inverse_base_transform_ = base_transform_.inverse();
            // We take a list of robot links and a list of robot joints, but we have to sanity-check them first
            links_ = links;
            joints_ = joints;
            const auto model_validity_check = SanityCheckRobotModel(links_, joints_);
            if (!model_validity_check.second)
            {
                throw std::invalid_argument("Attempted to construct a SimpleLinkedRobot with an invalid robot model");
            }
            kinematic_skeleton_ = model_validity_check.first;
            // Get information about the active joints
            const auto active_joint_query = MakeActiveJointIndexMap(joints_);
            num_active_joints_ = active_joint_query.first;
            active_joint_index_map_ = active_joint_query.second;
            if (joint_distance_weights.size() != num_active_joints_)
            {
                throw std::invalid_argument("joint_distance_weights.size() != num_active_joints_");
            }
            joint_distance_weights_ = EigenHelpers::Abs(joint_distance_weights);
            // Generate the self colllision map
            SetPosition(initial_position);
        }

        virtual simple_robot_model_interface::SimpleRobotModelInterface<SimpleLinkedConfiguration, SimpleLinkedConfigAlloc>* Clone() const
        {
            return new SimpleLinkedRobot(static_cast<const SimpleLinkedRobot&>(*this));
        }

        inline std::vector<std::string> GetActiveJointNames() const
        {
            std::vector<std::string> active_joint_names;
            active_joint_names.reserve(num_active_joints_);
            for (size_t idx = 0; idx < joints_.size(); idx++)
            {
                const RobotJoint& current_joint = joints_[idx];
                // Skip fixed joints
                if (!(current_joint.JointModel().IsFixed()))
                {
                    active_joint_names.push_back(current_joint.JointName());
                }
            }
            active_joint_names.shrink_to_fit();
            if (active_joint_names.size() == num_active_joints_)
            {
                return active_joint_names;
            }
            else
            {
                throw std::runtime_error("Number of active joint names goes not match number of active joints");
            }
        }

        inline void UpdateBaseTransform(const Eigen::Isometry3d& base_transform)
        {
            base_transform_ = base_transform;
            UpdateTransforms();
        }

        inline Eigen::Isometry3d GetBaseTransform() const
        {
            return base_transform_;
        }

        inline Eigen::Isometry3d GetInverseBaseTransform() const
        {
            return inverse_base_transform_;
        }

        virtual const SimpleLinkedConfiguration& GetPosition() const
        {
            return config_;
        }

        virtual const SimpleLinkedConfiguration& SetPosition(const SimpleLinkedConfiguration& position)
        {
            SetConfig(position);
            return GetPosition();
        }

        virtual std::vector<std::string> GetLinkNames() const
        {
            std::vector<std::string> link_names(links_.size());
            for (size_t idx = 0; idx < links_.size(); idx++)
            {
                const RobotLink& current_link = links_[idx];
                link_names[idx] = current_link.LinkName();
            }
            return link_names;
        }

        inline Eigen::Isometry3d GetLinkTransform(const std::string& link_name) const
        {
            for (size_t idx = 0; idx < links_.size(); idx++)
            {
                const RobotLink& current_link = links_[idx];
                if (current_link.LinkName() == link_name)
                {
                    return current_link.LinkTransform();
                }
            }
            throw std::invalid_argument("Invalid link_name");
        }

        virtual EigenHelpers::VectorIsometry3d GetLinkTransforms() const
        {
            EigenHelpers::VectorIsometry3d link_transforms(links_.size());
            for (size_t idx = 0; idx < links_.size(); idx++)
            {
                const RobotLink& current_link = links_[idx];
                link_transforms[idx] = current_link.LinkTransform();
            }
            return link_transforms;
        }

        virtual EigenHelpers::MapStringIsometry3d GetLinkTransformsMap() const
        {
            EigenHelpers::MapStringIsometry3d link_transforms_map;
            for (size_t idx = 0; idx < links_.size(); idx++)
            {
                const RobotLink& current_link = links_[idx];
                link_transforms_map[current_link.LinkName()] = current_link.LinkTransform();
            }
            return link_transforms_map;
        }

        static inline Eigen::VectorXd ComputeUnweightedPerDimensionConfigurationRawDistance(const SimpleLinkedConfiguration& config1, const SimpleLinkedConfiguration& config2)
        {
            if (config1.size() == config2.size())
            {
                Eigen::VectorXd distances = Eigen::VectorXd::Zero((ssize_t)(config1.size()));
                for (size_t idx = 0; idx < config1.size(); idx++)
                {
                    const SimpleJointModel& j1 = config1[idx];
                    const SimpleJointModel& j2 = config2[idx];
                    distances((int64_t)idx) = j1.SignedDistance(j2);
                }
                return distances;
            }
            else
            {
                throw std::invalid_argument("config1.size() != config2.size()");
            }
        }

        virtual Eigen::VectorXd ComputePerDimensionConfigurationSignedDistance(const SimpleLinkedConfiguration& config1, const SimpleLinkedConfiguration& config2) const
        {
            if ((config1.size() == num_active_joints_) && (config2.size() == num_active_joints_))
            {
                Eigen::VectorXd distances = Eigen::VectorXd::Zero((ssize_t)(config1.size()));
                for (size_t idx = 0; idx < config1.size(); idx++)
                {
                    const SimpleJointModel& j1 = config1[idx];
                    const SimpleJointModel& j2 = config2[idx];
                    const double raw_distance = j1.SignedDistance(j2);
                    const double joint_distance_weight = joint_distance_weights_[idx];
                    distances((int64_t)idx) = raw_distance * joint_distance_weight;
                }
                return distances;
            }
            else
            {
                throw std::invalid_argument("Size of config1 and/or config2 does not match num_active_joints_");
            }
        }

        virtual double ComputeConfigurationDistance(const SimpleLinkedConfiguration& config1, const SimpleLinkedConfiguration& config2) const
        {
            return ComputePerDimensionConfigurationSignedDistance(config1, config2).norm();
        }

        virtual SimpleLinkedConfiguration InterpolateBetweenConfigurations(const SimpleLinkedConfiguration& start, const SimpleLinkedConfiguration& end, const double ratio) const
        {
            if (start.size() == end.size())
            {
                // Make the interpolated config
                SimpleLinkedConfiguration interpolated;
                interpolated.reserve(start.size());
                for (size_t idx = 0; idx < start.size(); idx++)
                {
                    const SimpleJointModel& j1 = start[idx];
                    const SimpleJointModel& j2 = end[idx];
                    if (j1.IsSameType(j2))
                    {
                        if (j1.IsContinuous())
                        {
                            const double interpolated_value = EigenHelpers::InterpolateContinuousRevolute(j1.GetValue(), j2.GetValue(), ratio);
                            interpolated.push_back(j1.CopyWithNewValue(interpolated_value));
                        }
                        else
                        {
                            const double interpolated_value = EigenHelpers::Interpolate(j1.GetValue(), j2.GetValue(), ratio);
                            interpolated.push_back(j1.CopyWithNewValue(interpolated_value));
                        }
                    }
                    else
                    {
                        throw std::invalid_argument("Joint model types do not match");
                    }
                }
                return interpolated;
            }
            else
            {
                throw std::invalid_argument("start.size() != end.size()");
            }
        }

        virtual SimpleLinkedConfiguration AverageConfigurations(const std::vector<SimpleLinkedConfiguration, SimpleLinkedConfigAlloc>& configurations) const
        {
            if (configurations.size() > 0)
            {
                // Safety checks
                const SimpleLinkedConfiguration& representative_config = configurations.front();
                for (size_t idx = 0; idx < configurations.size(); idx++)
                {
                    const SimpleLinkedConfiguration& current_config = configurations[idx];
                    if (representative_config.size() == current_config.size())
                    {
                        for (size_t jdx = 0; jdx < representative_config.size(); jdx++)
                        {
                            const SimpleJointModel& jr = representative_config[jdx];
                            const SimpleJointModel& jc = current_config[jdx];
                            if (jr.IsSameType(jc) == false)
                            {
                                throw std::invalid_argument("Joint model of configuration does not match joint model in reference configuration");
                            }
                        }
                    }
                    else
                    {
                        throw std::invalid_argument("Configuration does not match size of reference configuration");
                    }
                }
                // Get number of DoF
                const size_t config_size = representative_config.size();
                // Separate the joint values
                std::vector<std::vector<double>> raw_values(config_size);
                for (size_t idx = 0; idx < configurations.size(); idx++)
                {
                    const SimpleLinkedConfiguration& q = configurations[idx];
                    for (size_t qdx = 0; qdx < config_size; qdx++)
                    {
                        const double jval = q[qdx].GetValue();
                        raw_values[qdx].push_back(jval);
                    }
                }
                // Average each joint
                SimpleLinkedConfiguration average_config;
                average_config.reserve(config_size);
                for (size_t jdx = 0; jdx < config_size; jdx++)
                {
                    const std::vector<double>& values = raw_values[jdx];
                    const SimpleJointModel& representative_joint = representative_config[jdx];
                    if (representative_joint.IsContinuous())
                    {
                        const double average_value = EigenHelpers::AverageContinuousRevolute(values);
                        average_config.push_back(representative_joint.CopyWithNewValue(average_value));
                    }
                    else
                    {
                        const double average_value = EigenHelpers::AverageStdVectorDouble(values);
                        average_config.push_back(representative_joint.CopyWithNewValue(average_value));
                    }
                }
                return average_config;
            }
            else
            {
                return SimpleLinkedConfiguration();
            }
        }

        inline Eigen::Matrix<double, 6, Eigen::Dynamic> ComputeLinkPointJacobian(const std::string& link_name, const Eigen::Vector4d& link_relative_point) const
        {
            // Get the link transform (by extension, this ensures we've been given a valid link)
            const Eigen::Isometry3d link_transform = GetLinkTransform(link_name);
            // Transform the point into world frame
            const Eigen::Vector3d world_point = (link_transform * link_relative_point).block<3, 1>(0, 0);
            // Make the jacobian storage
            Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian = Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero((ssize_t)6, (ssize_t)num_active_joints_);
            // First, check if the link name is a valid link name
            int64_t link_idx = -1;
            for (size_t idx = 0; idx < links_.size(); idx++)
            {
                const std::string& current_link_name = links_[idx].LinkName();
                if (current_link_name == link_name)
                {
                    link_idx = (int64_t)idx;
                    break;
                }
            }
            if (link_idx < 0)
            {
                std::cerr << "Link " << link_name << " does not exist" << std::endl;
                return jacobian;
            }
            // Second, check if the link name is the first (root) link
            if (link_idx == 0)
            {
                // If so, return zeros for the jacobian, because the point *CANNOT MOVE*
                return jacobian;
            }
            // Walk up the kinematic tree and fill in the blocks one at a time
            int64_t current_link_index = link_idx;
            while (current_link_index > 0)
            {
                const KinematicSkeletonElement& current_element = kinematic_skeleton_[current_link_index];
                const int64_t parent_joint_index = current_element.parent_joint_index_;
                if (parent_joint_index < 0)
                {
                    throw std::runtime_error("Encountered invalid parent joint index (<0)");
                }
                const int64_t active_joint_index = active_joint_index_map_.at(parent_joint_index);
                // Get the current joint
                const RobotJoint& parent_joint = joints_[parent_joint_index];
                // Get the child link
                const RobotLink& child_link = links_[(size_t)parent_joint.ChildLinkIndex()];
                // Get the transform of the current joint
                const Eigen::Isometry3d& joint_transform = child_link.LinkTransform();
                // Compute the jacobian for the current joint
                if (parent_joint.JointModel().IsRevolute())
                {
                    const Eigen::Vector3d joint_axis = (Eigen::Vector3d)(joint_transform.rotation() * parent_joint.JointAxis());
                    jacobian.block<3,1>(0, active_joint_index) = joint_axis.cross(world_point - joint_transform.translation());
                    jacobian.block<3,1>(3, active_joint_index) = joint_axis;
                }
                else if (parent_joint.JointModel().IsPrismatic())
                {
                    const Eigen::Vector3d joint_axis = (Eigen::Vector3d)(joint_transform * parent_joint.JointAxis());
                    jacobian.block<3,1>(0, active_joint_index) = joint_axis;
                }
                else
                {
                    // We do nothing for fixed joints
                    if (parent_joint.JointModel().IsFixed() == false)
                    {
                        throw std::runtime_error("Invalid parent joint model type");
                    }
                }
                // Update
                current_link_index = current_element.parent_link_index_;
            }
            return jacobian;
        }

        virtual Eigen::Matrix<double, 3, Eigen::Dynamic> ComputeLinkPointTranslationJacobian(const std::string& link_name, const Eigen::Vector4d& link_relative_point) const
        {
            const Eigen::Matrix<double, 6, Eigen::Dynamic> full_jacobian = ComputeLinkPointJacobian(link_name, link_relative_point);
            Eigen::Matrix<double, 3, Eigen::Dynamic> trans_only_jacobian(3, full_jacobian.cols());
            trans_only_jacobian << full_jacobian.row(0), full_jacobian.row(1), full_jacobian.row(2);
            return trans_only_jacobian;
        }
    };
}

#endif // SIMPLE_LINKED_ROBOT_MODEL_HPP


#include <NTL/GF2X.h>

NTL_CLIENT

struct wd {
   int amt;

   wd(int x) { amt = x; }
};

#define WD(x,y)  wd(x) << (y)

ostream& operator<<(ostream& s, const wd& w)
{
   s.width(w.amt);
   return s;
}

int main()
{
   long n;
   GF2X a, b, c, c1, ss, ss1, tt, tt1;
   double t;
   long iter, i;

   cout << WD(12,"n") << WD(12,"OldGCD") <<  WD(12,"GCD") << WD(12,"OldXGCD")
        << WD(12, "XGCD") << "\n";

   cout.precision(3);
   cout.setf(ios::scientific);

   for (n = 32; n <= (1L << 18); n = n << 1) {
      random(a, n);
      random(b, n);
      OldGCD(c, a, b);
      GCD(c1, a, b);
      OldXGCD(c, ss, tt, a, b);
      XGCD(c1, ss1, tt1, a, b);
      if (c1 != c || ss1 != ss || tt1 != tt) {
         cerr << "**** GF2XTest FAILED!\n";
         return 1;
      }

      cout << WD(12,n);

      iter = 0;
      do {
         iter = iter ? (2*iter) : 1;
         t = GetTime();
         for (i = 0; i < iter; i++)
            OldGCD(c, a, b);
         t = GetTime()-t;
      } while (t < 0.5);

      cout << WD(12,t/iter);

      iter = 0;
      do {
         iter = iter ? (2*iter) : 1;
         t = GetTime();
         for (i = 0; i < iter; i++)
            GCD(c, a, b);
         t = GetTime()-t;
      } while (t < 0.5);

      cout << WD(12,t/iter);

      iter = 0;
      do {
         iter = iter ? (2*iter) : 1;
         t = GetTime();
         for (i = 0; i < iter; i++)
            OldXGCD(c, ss, tt, a, b);
         t = GetTime()-t;
      } while (t < 0.5);

      cout << WD(12,t/iter);

      iter = 0;
      do {
         iter = iter ? (2*iter) : 1;
         t = GetTime();
         for (i = 0; i < iter; i++)
            XGCD(c, ss, tt, a, b);
         t = GetTime()-t;
      } while (t < 0.5);

      cout << WD(12,t/iter);

      cout << "\n";
   }

   return 0;
}


// Copyright 2015-2020 Tier IV, Inc. All rights reserved.
//
// 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.

#ifndef TRAFFIC_SIMULATOR__MATH__BOUNDING_BOX_HPP_
#define TRAFFIC_SIMULATOR__MATH__BOUNDING_BOX_HPP_

#include <boost/assert.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/optional.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <traffic_simulator/math/transform.hpp>
#include <traffic_simulator_msgs/msg/bounding_box.hpp>
#include <vector>

namespace traffic_simulator
{
namespace math
{
boost::optional<double> getPolygonDistance(
  const geometry_msgs::msg::Pose & pose0, const traffic_simulator_msgs::msg::BoundingBox & bbox0,
  const geometry_msgs::msg::Pose & pose1, const traffic_simulator_msgs::msg::BoundingBox & bbox1);
const boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double>> get2DPolygon(
  const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox);
std::vector<geometry_msgs::msg::Point> getPointsFromBbox(
  traffic_simulator_msgs::msg::BoundingBox bbox, double width_extension_right = 0.0,
  double width_extension_left = 0.0, double length_extension_front = 0.0,
  double length_extension_rear = 0.0);
}  // namespace math
}  // namespace traffic_simulator

#endif  // TRAFFIC_SIMULATOR__MATH__BOUNDING_BOX_HPP_


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/graph_traits_Surface_mesh.h>

#include <CGAL/Polygon_mesh_processing/triangulate_faces.h>

#include <boost/foreach.hpp>

#include <fstream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3                    Point;
typedef CGAL::Surface_mesh<Point>          Surface_mesh;

int main()
{
  Surface_mesh mesh;
  std::ifstream input("data/elephant.off");

  if (!input || !(input >> mesh) || mesh.is_empty())
  {
    std::cerr << "Not a valid off file." << std::endl;
    return 1;
  }

  CGAL::Polygon_mesh_processing::triangulate_faces(mesh);
  CGAL::Polygon_mesh_processing::triangulate_faces(mesh,
                                                   CGAL::Polygon_mesh_processing::parameters::all_default());
  CGAL::Polygon_mesh_processing::triangulate_faces(faces(mesh), mesh);
  CGAL::Polygon_mesh_processing::triangulate_faces(faces(mesh), mesh,
                                                   CGAL::Polygon_mesh_processing::parameters::all_default());

  BOOST_FOREACH(boost::graph_traits<Surface_mesh>::face_descriptor fit, faces(mesh))
    if (next(next(halfedge(fit, mesh), mesh), mesh)
        !=   prev(halfedge(fit, mesh), mesh)) {
      CGAL::Polygon_mesh_processing::triangulate_face(fit, mesh);
      CGAL::Polygon_mesh_processing::triangulate_face(fit, mesh,
                                                      CGAL::Polygon_mesh_processing::parameters::all_default());
    }
}


#include "MeanCurvatureSkeleton3D.h"
#undef PI
#include <CGAL/Nef_polyhedron_3.h>
#include <CGAL/boost/graph/split_graph_into_polylines.h>
#include <CGAL/boost/graph/convert_nef_polyhedron_to_polygon_mesh.h>
#include <CGAL/subdivision_method_3.h>
#include <CGAL/Polygon_mesh_processing/triangulate_faces.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <CGAL/Polygon_mesh_processing/refine.h>
#include <CGAL/Polygon_mesh_processing/fair.h>
#include <CGAL/boost/graph/helpers.h>
#include <CGAL/squared_distance_3.h>
#include <CGAL/Bbox_3.h>
#include <boost/foreach.hpp>
#include <boost/function_output_iterator.hpp>
#include "MPProblem/Environment/Environment.h"

/*------------------------- Static Initializers ------------------------------*/

MeanCurvatureSkeleton3D::Parameters MeanCurvatureSkeleton3D::m_defaultParams;

/*------------------------------- Construction -------------------------------*/

MeanCurvatureSkeleton3D::
MeanCurvatureSkeleton3D() : m_params(m_defaultParams) {}

/*------------------------- Constructors ----------------------------------*/

void
MeanCurvatureSkeleton3D::
SetDefaultParameters(XMLNode& _node) {
  m_defaultParams.space = _node.Read("space", false, 1, 0, MAX_INT, "Free vs obstacle space construction");
  m_defaultParams.useConvex = _node.Read("useConvex", false, false, "Use the convex hull of the obstacle.");
  m_defaultParams.m_iterations = _node.Read("iteration", false, 1000, 0, MAX_INT, "Number of iterations");
  m_defaultParams.m_wH = _node.Read("quality", false, 0.1, 0.0, MAX_DBL, "Quality Parameter");
  m_defaultParams.m_wM = _node.Read("medial", false, 0.2, 0.0, MAX_DBL, "Medial parameter");
  m_defaultParams.ioType = _node.Read("io", false, "write", "IO type for skeleton: read or write");
  m_defaultParams.filebase = _node.Read("filebase", false, "example", "filename for output files");
}

MeanCurvatureSkeleton3D::
~MeanCurvatureSkeleton3D() {
  Reset();
}

/*------------------------- Helper function ---------------------------*/

Point3d ToPMPLPoint3d(const CGAL::Simple_cartesian<double>::Point_3& _p) {
	return Point3d(_p[0], _p[1], _p[2]);
}

/*--------------------------- Modifiers ------------------------------*/

void
MeanCurvatureSkeleton3D::
SetEnvironment(const Environment* _env) {
  // Do nothing if we're reading in from a file.
  if(m_params.ioType == "read")
    return;

  if(this->m_debug)
    cout << "MeanCurvatureSkeleton3D::Construct()" << endl;

  if(m_params.space == 1)
    InitializeFreeSpace(_env);
  else
    InitializeObstacleSpace(_env);
}

void
MeanCurvatureSkeleton3D::
BuildSkeleton(const WorkspaceDecomposition* _tetrahedralization, bool _curve) {
  if(m_params.ioType == "read") {
    Read(m_params.filebase);
    return;
  }

  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::BuildSkeleton()" << endl;

  SurfaceMesh inputMesh;
  // Copy vertices.
  vector<Vector3d> vertices = _tetrahedralization->GetPoints();

  typedef CGAL::Simple_cartesian<double> K;
  typedef SurfaceMesh::vertex_index vertex_descriptor;
  for(auto v : vertices) {
    inputMesh.add_vertex(K::Point_3(v[0], v[1], v[2]));
  }

  for(auto tetra = _tetrahedralization->begin();
      tetra != _tetrahedralization->end(); ++tetra) {
    for(const auto& facet : tetra->property().GetFacets()) {
      // Sort point indexes so opposite-facing triangles have the same
      // representation.
      vertex_descriptor v[3] = {static_cast<vertex_descriptor>(facet[0]),
                                static_cast<vertex_descriptor>(facet[1]),
                                static_cast<vertex_descriptor>(facet[2])};
      sort(v, v + 3);
      inputMesh.add_face(v[0], v[1], v[2]);
    }
  }

  Skeletonization mcs(inputMesh);
  mcs.set_quality_speed_tradeoff(m_params.m_wH);//10
  mcs.set_min_edge_length(1);
  mcs.set_medially_centered_speed_tradeoff(m_params.m_wM);//100
  mcs.set_max_iterations(m_params.m_iterations);

  //if(m_debug)
  cout<<"Parameters: WH "<< mcs.quality_speed_tradeoff() << "\n"
    << " WM "<< mcs.medially_centered_speed_tradeoff() << "\n"
    << " max triangle angle "<< mcs.max_triangle_angle() << "\n"
    << " min edge length " << mcs.min_edge_length() << "\n"
    << " iterations "<< mcs.max_iterations() << "\n"
    << " area variation " << mcs.area_variation_factor() << "\n"
    << "is medially centered: " << mcs.is_medially_centered() << endl;

  // Contract the mesh by mean curvature flow.
  mcs.contract_until_convergence();

  // Get the output as curve skeleton / meso-skeleton surface mesh
  if(_curve) {
    m_meso = mcs.meso_skeleton();
    cout<<"Number of faces in meso skeleton: "<<num_faces(m_meso)<<endl;
    cout<<"Number of vertices in meso skeleton: "<<num_vertices(m_meso)<<endl;
    cout<<"Number of edges in meso skeleton: "<<num_edges(m_meso)<<endl;
    // Convert the contracted mesh into a curve skeleton and
    // get the correspondent surface points
    mcs.convert_to_skeleton(m_mcs);
    // Validation
    if(m_debug) {
      cout << "Number of vertices of the skeleton: " << boost::num_vertices(m_mcs) << "\n";
      cout << "Number of edges of the skeleton: " << boost::num_edges(m_mcs) << "\n";
    }
  }
  else
    m_meso = mcs.meso_skeleton();
}

void
MeanCurvatureSkeleton3D::
BuildSkeleton(bool _curve) {
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::BuildSkeleton()" << endl;

  if(m_params.ioType == "read") {
    Read(m_params.filebase);
    return;
  }

  // validate whether the input mesh is triangulated
  if(!is_triangle_mesh(m_input)) {
    if(m_debug)
      cerr <<"Input mesh needs to be triangulated" << endl;
    // Triangulate mesh if not
    if(!CGAL::Polygon_mesh_processing::triangulate_faces(m_input)){
      if(m_debug)
        cerr << "Mesh cannot be fully triangulated "<<endl;
      return;
    }
  }

  if(m_debug)
    cout<<"Input mesh has " << m_input.number_of_vertices() <<" vertices "
      << m_input.number_of_edges() <<" edges "
      << m_input.number_of_faces() <<" faces "<<endl;

  // build the skeleton only if the mesh is triangulated

  Skeletonization mcs(m_input);
  mcs.set_quality_speed_tradeoff(m_params.m_wH);//10
  //mcs.set_min_edge_length(1);
  mcs.set_medially_centered_speed_tradeoff(m_params.m_wM);//100
  mcs.set_max_iterations(m_params.m_iterations);

  //if(m_debug)
  cout<<"Parameters: WH "<< mcs.quality_speed_tradeoff() << "\n"
    << " WM "<< mcs.medially_centered_speed_tradeoff() << "\n"
    << " max triangle angle "<< mcs.max_triangle_angle() << "\n"
    << " min edge length " << mcs.min_edge_length() << "\n"
    << " iterations "<< mcs.max_iterations() << "\n"
    << " area variation " << mcs.area_variation_factor() << "\n"
    << "is medially centered: " << mcs.is_medially_centered() << endl;

  // Contract the mesh by mean curvature flow.
  mcs.contract_until_convergence();
  m_meso = mcs.meso_skeleton();

  // Get the output as curve skeleton / meso-skeleton surface mesh
  if(_curve) {
    cout<<"Number of faces in meso skeleton: "<<num_faces(m_meso)<<endl;
    cout<<"Number of vertices in meso skeleton: "<<num_vertices(m_meso)<<endl;
    cout<<"Number of edges in meso skeleton: "<<num_edges(m_meso)<<endl;
    // Convert the contracted mesh into a curve skeleton and
    // get the correspondent surface points
    mcs.convert_to_skeleton(m_mcs);
    // Validation
    if(m_debug) {
      cout << "Number of vertices of the skeleton: " << boost::num_vertices(m_mcs) << "\n";
      cout << "Number of edges of the skeleton: " << boost::num_edges(m_mcs) << "\n";
    }
  }
}
/*
pair<WorkspaceSkeleton, MeanCurvatureSkeleton3D::AnnotationType>
MeanCurvatureSkeleton3D::
AdjustSkeleton(const Environment* _env) {
  typedef WorkspaceSkeleton::GraphType Graph;
  Graph g = m_skeleton.GetGraph();
  auto obst = _env->
}
*/
/*--------------------------- Accessors ------------------------------*/

pair<WorkspaceSkeleton, MeanCurvatureSkeleton3D::AnnotationType>
MeanCurvatureSkeleton3D::
GetSkeleton() {
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::GetSkeleton()" << endl;

  if(m_params.ioType != "read") {
    typedef WorkspaceSkeleton::GraphType Graph;
    Graph g;
    // Map stores already inserted vertices in the graph
    map<MCVD, typename Graph::vertex_descriptor> vertexMap;
    // Segment the graph as set of polylines
    vector<vector<MCVD>> plines;
    ToPolyLines(plines);
    vector<pair<typename Graph::vertex_descriptor, WitnessType>> vertexWitnesses;
    vector<pair<typename Graph::edge_descriptor, vector<WitnessType>>> edgeWitnesses;
    // For each polyline - create an edge
    for(auto pline : plines){
      // Get the two end points
      MCVD epts[2];
      epts[0] = pline.front();
      epts[1] = pline.back();
      // vertex descriptors of end points
      Graph::vertex_descriptor vd[2];
      // For each end point create a vertex if not already in the graph
      for(size_t i = 0; i < 2; ++i) {
        auto findPt = vertexMap.find(epts[i]);
        // If end point not already in the graph - add vertex
        if(findPt == vertexMap.end()) {
          size_t deg = degree(epts[i], m_mcs); if(deg > 6 ) cout<<"degree"<<deg<<endl;
          vd[i] = g.add_vertex(ToPMPLPoint3d(Point(epts[i])));
          vertexMap.insert(make_pair(epts[i],vd[i]));
          // Store the witness points for the vertex
          vertexWitnesses.push_back(make_pair(vd[i], WitnessType()));
          GetWitnesses(epts[i], vertexWitnesses.back().second);
        }
        else
          vd[i] = findPt->second;
      }
      //Add edge
      vector<Point3d> intermediates;
      vector<WitnessType> intermediateWitnesses(pline.size());
      for(size_t i = 0; i < pline.size(); i++) {
        intermediates.push_back(ToPMPLPoint3d(Point(pline[i])));
        // Get the witnesses for the intermediate points in the edge
        GetWitnesses(pline[i], intermediateWitnesses[i]);
      }
      auto ed = g.add_edge(vd[0], vd[1], intermediates);
      // Store the witnesses of the edge points
      edgeWitnesses.push_back(make_pair(ed, intermediateWitnesses));
    }
    m_skeleton.SetGraph(g);
    // Set witnesses as annotation to the skeleton
    m_annotation = AnnotationType(&m_skeleton);
    for(auto vwitness : vertexWitnesses)
      m_annotation.SetVertexProperty(vwitness.first, vwitness.second);
    for(auto ewitness : edgeWitnesses)
      m_annotation.SetEdgeProperty(ewitness.first, ewitness.second);
  }

  if(m_params.ioType == "write")
    Write(m_params.filebase);

  return make_pair(m_skeleton, m_annotation);
}

pair<WorkspaceSkeleton, MeanCurvatureSkeleton3D::AnnotationType>
MeanCurvatureSkeleton3D::
GetMesoSkeleton() {
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::GetMesoSkeleton()" << endl;

  if(m_params.ioType != "read") {
    typedef WorkspaceSkeleton::GraphType Graph;
    Graph g;
    vector<pair<typename Graph::vertex_descriptor, WitnessType>> vertexWitnesses;
    vector<pair<typename Graph::edge_descriptor, vector<WitnessType>>> edgeWitnesses;
    // Map stores already inserted vertices in the graph
    typedef Skeletonization::vertex_descriptor VD;
    map<VD, typename Graph::vertex_descriptor> vertexMap;

    // Get all vertices
    BOOST_FOREACH(VD vd, vertices(m_meso)){
      auto svd = g.add_vertex(ToPMPLPoint3d(vd->point()));
      vertexMap.insert(make_pair(vd,svd));
      // Store the witness points for the vertex
      vertexWitnesses.push_back(make_pair(svd, WitnessType()));
      GetWitnesses(vd, vertexWitnesses.back().second);
    }

    // Get all edges
    typedef Skeletonization::edge_descriptor ED;
    typedef Skeletonization::face_descriptor FD;
    typedef Skeletonization::halfedge_descriptor HD;

    BOOST_FOREACH(ED ed, edges(m_meso)) {
      Graph::vertex_descriptor vd[2];
      VD src = source(ed, m_meso);
      auto findvd = vertexMap.find(src);
      if(findvd == vertexMap.end())
        continue;
      else
        vd[0] = findvd->second;
      VD tgt = target(ed, m_meso);
      findvd = vertexMap.find(tgt);
      if(findvd == vertexMap.end())
        continue;
      else
        vd[1] = findvd->second;
      //Add edge
      vector<Point3d> intermediates;
      vector<WitnessType> intermediateWitnesses(2);
      intermediates.push_back(ToPMPLPoint3d(src->point()));
      // Get the witnesses for the intermediate points in the edge
      GetWitnesses(src, intermediateWitnesses[0]);
      intermediates.push_back(ToPMPLPoint3d(tgt->point()));
      // Get the witnesses for the intermediate points in the edge
      GetWitnesses(tgt, intermediateWitnesses[1]);

      auto sed = g.add_edge(vd[0], vd[1], intermediates);
      // Store the witnesses of the edge points
      edgeWitnesses.push_back(make_pair(sed, intermediateWitnesses));
    }

    // Return the annotated skeleton
    m_skeleton.SetGraph(g);
    // Set witnesses as annotation to the skeleton
    m_annotation = AnnotationType(&m_skeleton);
    for(auto vwitness : vertexWitnesses)
      m_annotation.SetVertexProperty(vwitness.first, vwitness.second);
    for(auto ewitness : edgeWitnesses)
      m_annotation.SetEdgeProperty(ewitness.first, ewitness.second);
  }
  if(m_params.ioType == "write")
    Write(m_params.filebase);

  return make_pair(m_skeleton, m_annotation);
}

/*--------------------------- Helpers ------------------------------*/

void
MeanCurvatureSkeleton3D::
InitializeFreeSpace(const Environment* _env){
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::InitializeFreeSpace()" << endl;

  // Initialize the freespace model as an outward-facing boundary.

  //if the flag is set, build the free internal space instead.
  CGAL::Polyhedron_3<CGAL::Exact_predicates_exact_constructions_kernel> cp;
  if(m_params.useConvex) {
    std::cout << "building a convex hull boundary" << endl;
    auto convex = _env->GetObstacle(0)->GetBody(0)->GetWorldPolyhedron().
        ComputeConvexHull();
    convex.Scale(1.2);
    cp = convex.CGAL();
  }
  else
    cp = _env->GetBoundary()->CGAL();

  double len = 0, minr = numeric_limits<double>::max();
  for(size_t i = 0; i < 3; ++i) {
    len += pow( _env->GetBoundary()->GetRange(i).Length(), 2);
    minr = min(minr , _env->GetBoundary()->GetRange(i).Length());
  }
  len = sqrt(len);
  typedef CGAL::Nef_polyhedron_3<typename GMSPolyhedron::CGALKernel> NefPolyhedron;
  NefPolyhedron freespace(cp);

  // Subtract each obstacle from the freespace.
  for(size_t i = 0; i < _env->NumObstacles(); ++i) {
    if(m_debug)
      cout << "\tAdding obstacle " << i << "..." << endl;

    auto obst = _env->GetObstacle(i);
    if(!obst->IsInternal()) {
      // Make CGAL representation of this obstacle.
      auto ocp = obst->GetBody(0)->GetWorldPolyhedron().CGAL();

      if(m_debug)
        cout << "\t\tobstacle is " << (ocp.is_closed() ? "" : "not ")
             << "closed" << endl;

      // Subtract it from the freespace.
      freespace -= NefPolyhedron(ocp);
    }
  }

  // Check whether the input is 2-manifold. Mean Curvature Skeleton cannot handle
  // degenaries
  if(!freespace.is_simple())
    cerr<<"Mesh is not 2-manifold. Cannot construct Mean Curvature Skeleton for it."<<endl;

  // Convert the nef polyhedron to polyhedron with same point kernel as nef polyhedron
  if(m_debug)
    cout <<" NefPolhedron: Vertices "<<freespace.number_of_vertices()
      <<" Edges "<<freespace.number_of_edges()
      <<" Faces "<<freespace.number_of_facets()<< endl;
  GMSPolyhedron::CGALPolyhedron smesh;
  freespace.convert_to_polyhedron(smesh);

  // Change the surface mesh to the surface with simple cartesian point kernel
  // otherwise skeletonization does not work with exact kernel
  AddPolyhedron(smesh,0.01*len);
}

void
MeanCurvatureSkeleton3D::
InitializeObstacleSpace(const Environment* _env){
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::InitializeObstacleSpace()" << endl;

  // Initialize the obstaclespace model as empty.
  auto cp = _env->GetBoundary()->CGAL();
  double len = 0, minr = numeric_limits<double>::max();
  for(size_t i = 0; i < 3; ++i) {
    len += pow( _env->GetBoundary()->GetRange(i).Length(), 2);
    minr = min(minr , _env->GetBoundary()->GetRange(i).Length());
  }
  len = sqrt(len);
  cout<<"Diagonal length"<<len<<" "<<minr<<endl;
  typedef CGAL::Nef_polyhedron_3<typename GMSPolyhedron::CGALKernel> NefPolyhedron;
  NefPolyhedron obstspace(NefPolyhedron::EMPTY);

  // Add all obstacle
  for(size_t i = 0; i < _env->NumObstacles(); ++i) {
    if(m_debug)
      cout << "\tAdding obstacle " << i << "..." << endl;

    auto obst = _env->GetObstacle(i);
    if(!obst->IsInternal()) {
      // Make CGAL representation of this obstacle.
      auto ocp = obst->GetBody(0)->GetWorldPolyhedron().CGAL();
      if(m_debug)
        cout << "\t\tobstacle is " << (ocp.is_closed() ? "" : "not ")
             << "closed" << endl;

      // Find the intersection of the obstacle with the boundary
      NefPolyhedron obst(cp);
      obst *= NefPolyhedron(ocp);
      // Add it to the obstaclespace.
      obstspace += obst;//NefPolyhedron(ocp);
    }
  }

  // Check whether the input is 2-manifold. Mean Curvature Skeleton cannot handle
  // degenaries
  if(!obstspace.is_simple())
    cerr<<"Mesh is not 2-manifold. Cannot construct Mean Curvature Skeleton for it."<<endl;

  // Convert the nef polyhedron to polyhedron with same point kernel as nef polyhedron
  if(m_debug)
    cout <<" NefPolhedron: Vertices "<<obstspace.number_of_vertices()
      <<" Edges "<<obstspace.number_of_edges()
      <<" Faces "<<obstspace.number_of_facets()<< endl;
  GMSPolyhedron::CGALPolyhedron smesh;
  obstspace.convert_to_polyhedron(smesh);

  // Change the surface mesh to the surface with simple cartesian point kernel
  // otherwise skeletonization does not work with exact kernel
  AddPolyhedron(smesh,0.01*len);
}

void
MeanCurvatureSkeleton3D::
GetWitnesses(MCVD& _v, vector<Point3d>& _witness) {
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::GetWitnesses()" << endl;

  typedef boost::graph_traits<SurfaceMesh>::vertex_descriptor SMVD;
  BOOST_FOREACH(SMVD vd, m_mcs[_v].vertices){
    _witness.push_back(ToPMPLPoint3d(get(CGAL::vertex_point, m_input, vd)));
  }
}

void
MeanCurvatureSkeleton3D::
GetWitnesses(MESO::Vertex& _v, vector<Point3d>& _witness) {
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::GetWitnesses()" << endl;

 typedef boost::graph_traits<SurfaceMesh>::vertex_descriptor SMVD;
 BOOST_FOREACH(SMVD vd, _v.vertices)
    _witness.push_back(ToPMPLPoint3d(get(CGAL::vertex_point, m_input, vd)));
}

void
MeanCurvatureSkeleton3D::
GetWitnesses(Skeletonization::vertex_descriptor& _v, vector<Point3d>& _witness){
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::GetWitnesses()" << endl;

  typedef boost::graph_traits<SurfaceMesh>::vertex_descriptor SMVD;
  BOOST_FOREACH(SMVD vd, _v->vertices)
    _witness.push_back(ToPMPLPoint3d(get(CGAL::vertex_point, m_input, vd)));
}

void MeanCurvatureSkeleton3D::Refine(SurfaceMesh& _m, double _f)  {
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::Refine()" << endl;

  CGAL::Polygon_mesh_processing::isotropic_remeshing(_m.faces(), _f,
    _m,
    CGAL::Polygon_mesh_processing::parameters::number_of_iterations(1));
}

void MeanCurvatureSkeleton3D::Reset() {
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::Reset()" << endl;

  m_input.clear();
  m_mcs.clear();
}

void
MeanCurvatureSkeleton3D::
ToPolyLines(vector<vector<MCVD>>& _plines, bool _isSimplify)	{
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::ToPolyLines()" << endl;

  if(!_isSimplify) {
    // Putting all the edges
    BOOST_FOREACH(MCED e, edges(m_mcs)) {
      _plines.push_back(vector<MCVD>());
      _plines.back().push_back(source(e, m_mcs));
		  _plines.back().push_back(target(e, m_mcs));
    }
		return;
  }
	// Helper structure that breaks any boost graph edges into polylines
  // It works like a visitor structure in graph algos
  // The function signatures are provided by CGAL
  struct SplitPolyline{
    const MCSkeleton& skeleton;
    vector<vector<MCVD>> lines;

    // Functions that needs to be defined that are used in CGAL function
    // Signature provided by CGAL (Do not change the signature)
    void start_new_polyline(){
      lines.push_back(vector<MCVD>());
    }
    void add_node(MCVD v){
      lines.back().push_back(v);
    }
    void end_polyline(){ }
    // User defined functions
    // This functions including the signature can be changed by the user
    SplitPolyline(const MCSkeleton& _sk): skeleton(_sk) {
      lines.clear();
    }
  };
  SplitPolyline sp(m_mcs);
  // Split the graph into polylines
  CGAL::split_graph_into_polylines(m_mcs, sp);
  _plines.insert(_plines.end(), sp.lines.begin(), sp.lines.end());
}

/*--------------------------- I/O ------------------------------------*/

void
MeanCurvatureSkeleton3D::
Read(string _filename) {
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::Read()" << endl;

  m_skeleton.Read(_filename + "_skeleton");

  m_annotation = AnnotationType(&m_skeleton);
  m_annotation.Read(_filename + "_annotation");
}

void
MeanCurvatureSkeleton3D::
Write(string _filename) {
  if(m_debug)
    cout << "MeanCurvatureSkeleton3D::Write()" << endl;

  m_skeleton.Write(_filename + "_skeleton");
  m_annotation.Write(_filename + "_annotation");
}


// Boost.Geometry

// Copyright (c) 2016, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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 BOOST_GEOMETRY_STRATEGIES_SPHERICAL_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_INTERSECTION_HPP

#include <algorithm>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/algorithms/detail/assign_values.hpp>
#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/algorithms/detail/equals/point_point.hpp>
#include <boost/geometry/algorithms/detail/recalculate.hpp>

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/cross_product.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/formulas/spherical.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/geometries/concepts/segment_concept.hpp>

#include <boost/geometry/policies/robustness/segment_ratio.hpp>

#include <boost/geometry/strategies/side_info.hpp>
#include <boost/geometry/strategies/intersection.hpp>
#include <boost/geometry/strategies/intersection_result.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

// NOTE:
// The coordinates of crossing IP may be calculated with small precision in some cases.
// For double, near the equator noticed error ~1e-9 so far greater than
// machine epsilon which is ~1e-16. This error is ~0.04m.
// E.g. consider two cases, one near the origin and the second one rotated by 90 deg around Z or SN axis.
// After the conversion from spherical degrees to cartesian 3d the following coordinates
// are calculated:
// for sph (-1 -1,  1 1) deg cart3d ys are -0.017449748351250485 and  0.017449748351250485
// for sph (89 -1, 91 1) deg cart3d xs are  0.017449748351250571 and -0.017449748351250450
// During the conversion degrees must first be converted to radians and then radians
// are passed into trigonometric functions. The error may have several causes:
// 1. Radians cannot represent exactly the same angles as degrees.
// 2. Different longitudes are passed into sin() for x, corresponding to cos() for y,
//    and for different angle the error of the result may be different.
// 3. These non-corresponding cartesian coordinates are used in calculation,
//    e.g. multiplied several times in cross and dot products.
// If it was a problem this strategy could e.g. "normalize" longitudes before the conversion using the source units
// by rotating the globe around Z axis, so moving longitudes always the same way towards the origin,
// assuming this could help which is not clear.
// For now, intersection points near the endpoints are checked explicitly if needed (if the IP is near the endpoint)
// to generate precise result for them. Only the crossing (i) case may suffer from lower precision.

template <typename Policy, typename CalculationType = void>
struct relate_spherical_segments
{
    typedef typename Policy::return_type return_type;

    enum intersection_point_flag { ipi_inters = 0, ipi_at_a1, ipi_at_a2, ipi_at_b1, ipi_at_b2 };

    template <typename CoordinateType, typename SegmentRatio, typename Vector3d>
    struct segment_intersection_info
    {
        typedef typename select_most_precise
            <
                CoordinateType, double
            >::type promoted_type;

        promoted_type comparable_length_a() const
        {
            return robust_ra.denominator();
        }

        promoted_type comparable_length_b() const
        {
            return robust_rb.denominator();
        }

        template <typename Point, typename Segment1, typename Segment2>
        void assign_a(Point& point, Segment1 const& a, Segment2 const& b) const
        {
            assign(point, a, b);
        }
        template <typename Point, typename Segment1, typename Segment2>
        void assign_b(Point& point, Segment1 const& a, Segment2 const& b) const
        {
            assign(point, a, b);
        }

        template <typename Point, typename Segment1, typename Segment2>
        void assign(Point& point, Segment1 const& a, Segment2 const& b) const
        {
            if (ip_flag == ipi_inters)
            {
                // TODO: assign the rest of coordinates
                point = formula::cart3d_to_sph<Point>(intersection_point);
            }
            else if (ip_flag == ipi_at_a1)
            {
                detail::assign_point_from_index<0>(a, point);
            }
            else if (ip_flag == ipi_at_a2)
            {
                detail::assign_point_from_index<1>(a, point);
            }
            else if (ip_flag == ipi_at_b1)
            {
                detail::assign_point_from_index<0>(b, point);
            }
            else // ip_flag == ipi_at_b2
            {
                detail::assign_point_from_index<1>(b, point);
            }
        }

        Vector3d intersection_point;
        SegmentRatio robust_ra;
        SegmentRatio robust_rb;
        intersection_point_flag ip_flag;
    };

    // Relate segments a and b
    template <typename Segment1, typename Segment2, typename RobustPolicy>
    static inline return_type apply(Segment1 const& a, Segment2 const& b,
                                    RobustPolicy const& robust_policy)
    {
        typedef typename point_type<Segment1>::type point1_t;
        typedef typename point_type<Segment2>::type point2_t;
        point1_t a1, a2;
        point2_t b1, b2;

        // TODO: use indexed_point_view if possible?
        detail::assign_point_from_index<0>(a, a1);
        detail::assign_point_from_index<1>(a, a2);
        detail::assign_point_from_index<0>(b, b1);
        detail::assign_point_from_index<1>(b, b2);

        return apply(a, b, robust_policy, a1, a2, b1, b2);
    }

    // Relate segments a and b
    template <typename Segment1, typename Segment2, typename RobustPolicy, typename Point1, typename Point2>
    static inline return_type apply(Segment1 const& a, Segment2 const& b,
                                    RobustPolicy const&,
                                    Point1 const& a1, Point1 const& a2, Point2 const& b1, Point2 const& b2)
    {
        BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<Segment1>) );
        BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<Segment2>) );

        // TODO: check only 2 first coordinates here?
        using geometry::detail::equals::equals_point_point;
        bool a_is_point = equals_point_point(a1, a2);
        bool b_is_point = equals_point_point(b1, b2);

        if(a_is_point && b_is_point)
        {
            return equals_point_point(a1, b2)
                ? Policy::degenerate(a, true)
                : Policy::disjoint()
                ;
        }

        typedef typename select_calculation_type
            <Segment1, Segment2, CalculationType>::type calc_t;

        calc_t const c0 = 0;
        calc_t const c1 = 1;

        typedef model::point<calc_t, 3, cs::cartesian> vec3d_t;

        using namespace formula;
        vec3d_t const a1v = sph_to_cart3d<vec3d_t>(a1);
        vec3d_t const a2v = sph_to_cart3d<vec3d_t>(a2);
        vec3d_t const b1v = sph_to_cart3d<vec3d_t>(b1);
        vec3d_t const b2v = sph_to_cart3d<vec3d_t>(b2);

        vec3d_t norm1 = cross_product(a1v, a2v);
        vec3d_t norm2 = cross_product(b1v, b2v);

        side_info sides;
        // not normalized normals, the same as in SSF
        sides.set<0>(sph_side_value(norm2, a1v), sph_side_value(norm2, a2v));
        if (sides.same<0>())
        {
            // Both points are at same side of other segment, we can leave
            return Policy::disjoint();
        }
        // not normalized normals, the same as in SSF
        sides.set<1>(sph_side_value(norm1, b1v), sph_side_value(norm1, b2v));
        if (sides.same<1>())
        {
            // Both points are at same side of other segment, we can leave
            return Policy::disjoint();
        }

        // NOTE: at this point the segments may still be disjoint

        bool collinear = sides.collinear();

        calc_t const len1 = math::sqrt(dot_product(norm1, norm1));
        calc_t const len2 = math::sqrt(dot_product(norm2, norm2));

        // point or opposite sides of a sphere, assume point
        if (math::equals(len1, c0))
        {
            a_is_point = true;
            if (sides.get<0, 0>() == 0 || sides.get<0, 1>() == 0)
            {
                sides.set<0>(0, 0);
            }
        }
        else
        {
            // normalize
            divide_value(norm1, len1);
        }

        if (math::equals(len2, c0))
        {
            b_is_point = true;
            if (sides.get<1, 0>() == 0 || sides.get<1, 1>() == 0)
            {
                sides.set<1>(0, 0);
            }
        }
        else
        {
            // normalize
            divide_value(norm2, len2);
        }

        // check both degenerated once more
        if (a_is_point && b_is_point)
        {
            return equals_point_point(a1, b2)
                ? Policy::degenerate(a, true)
                : Policy::disjoint()
                ;
        }

        // NOTE: at this point one of the segments may be degenerated
        //       and the segments may still be disjoint

        calc_t dot_n1n2 = dot_product(norm1, norm2);

        // NOTE: this is technically not needed since theoretically above sides
        //       are calculated, but just in case check the normals.
        //       Have in mind that SSF side strategy doesn't check this.
        // collinear if normals are equal or opposite: cos(a) in {-1, 1}
        if (!collinear && math::equals(math::abs(dot_n1n2), c1))
        {
            collinear = true;
            sides.set<0>(0, 0);
            sides.set<1>(0, 0);
        }

        if (collinear)
        {
            if (a_is_point)
            {
                return collinear_one_degenerted<calc_t>(a, true, b1, b2, a1, a2, b1v, b2v, norm2, a1v);
            }
            else if (b_is_point)
            {
                // b2 used to be consistent with (degenerated) checks above (is it needed?)
                return collinear_one_degenerted<calc_t>(b, false, a1, a2, b1, b2, a1v, a2v, norm1, b1v);
            }
            else
            {
                calc_t dist_a1_a2, dist_a1_b1, dist_a1_b2;
                calc_t dist_b1_b2, dist_b1_a1, dist_b1_a2;
                // use shorter segment
                if (len1 <= len2)
                {
                    calculate_collinear_data(a1, a2, b1, b2, a1v, a2v, norm1, b1v, dist_a1_a2, dist_a1_b1);
                    calculate_collinear_data(a1, a2, b1, b2, a1v, a2v, norm1, b2v, dist_a1_a2, dist_a1_b2);
                    dist_b1_b2 = dist_a1_b2 - dist_a1_b1;
                    dist_b1_a1 = -dist_a1_b1;
                    dist_b1_a2 = dist_a1_a2 - dist_a1_b1;
                }
                else
                {
                    calculate_collinear_data(b1, b2, a1, a2, b1v, b2v, norm2, a1v, dist_b1_b2, dist_b1_a1);
                    calculate_collinear_data(b1, b2, a1, a2, b1v, b2v, norm2, a2v, dist_b1_b2, dist_b1_a2);
                    dist_a1_a2 = dist_b1_a2 - dist_b1_a1;
                    dist_a1_b1 = -dist_b1_a1;
                    dist_a1_b2 = dist_b1_b2 - dist_b1_a1;
                }

                segment_ratio<calc_t> ra_from(dist_b1_a1, dist_b1_b2);
                segment_ratio<calc_t> ra_to(dist_b1_a2, dist_b1_b2);
                segment_ratio<calc_t> rb_from(dist_a1_b1, dist_a1_a2);
                segment_ratio<calc_t> rb_to(dist_a1_b2, dist_a1_a2);

                // NOTE: this is probably not needed
                int const a1_wrt_b = position_value(c0, dist_a1_b1, dist_a1_b2);
                int const a2_wrt_b = position_value(dist_a1_a2, dist_a1_b1, dist_a1_b2);
                int const b1_wrt_a = position_value(c0, dist_b1_a1, dist_b1_a2);
                int const b2_wrt_a = position_value(dist_b1_b2, dist_b1_a1, dist_b1_a2);

                if (a1_wrt_b == 1)
                {
                    ra_from.assign(0, dist_b1_b2);
                    rb_from.assign(0, dist_a1_a2);
                }
                else if (a1_wrt_b == 3)
                {
                    ra_from.assign(dist_b1_b2, dist_b1_b2);
                    rb_to.assign(0, dist_a1_a2);
                }

                if (a2_wrt_b == 1)
                {
                    ra_to.assign(0, dist_b1_b2);
                    rb_from.assign(dist_a1_a2, dist_a1_a2);
                }
                else if (a2_wrt_b == 3)
                {
                    ra_to.assign(dist_b1_b2, dist_b1_b2);
                    rb_to.assign(dist_a1_a2, dist_a1_a2);
                }

                if ((a1_wrt_b < 1 && a2_wrt_b < 1) || (a1_wrt_b > 3 && a2_wrt_b > 3))
                {
                    return Policy::disjoint();
                }

                bool const opposite = dot_n1n2 < c0;

                return Policy::segments_collinear(a, b, opposite,
                    a1_wrt_b, a2_wrt_b, b1_wrt_a, b2_wrt_a,
                    ra_from, ra_to, rb_from, rb_to);
            }
        }
        else // crossing
        {
            if (a_is_point || b_is_point)
            {
                return Policy::disjoint();
            }

            vec3d_t i1;
            intersection_point_flag ip_flag;
            calc_t dist_a1_a2, dist_a1_i1, dist_b1_b2, dist_b1_i1;
            if (calculate_ip_data(a1, a2, b1, b2, a1v, a2v, b1v, b2v, norm1, norm2, sides,
                                  i1, dist_a1_a2, dist_a1_i1, dist_b1_b2, dist_b1_i1, ip_flag))
            {
                // intersects
                segment_intersection_info
                    <
                        calc_t,
                        segment_ratio<calc_t>,
                        vec3d_t
                    > sinfo;

                sinfo.robust_ra.assign(dist_a1_i1, dist_a1_a2);
                sinfo.robust_rb.assign(dist_b1_i1, dist_b1_b2);
                sinfo.intersection_point = i1;
                sinfo.ip_flag = ip_flag;

                return Policy::segments_crosses(sides, sinfo, a, b);
            }
            else
            {
                return Policy::disjoint();
            }
        }
    }

private:
    template <typename CalcT, typename Segment, typename Point1, typename Point2, typename Vec3d>
    static inline return_type collinear_one_degenerted(Segment const& segment, bool degenerated_a,
                                                       Point1 const& a1, Point1 const& a2,
                                                       Point2 const& b1, Point2 const& b2,
                                                       Vec3d const& v1, Vec3d const& v2, Vec3d const& norm,
                                                       Vec3d const& vother)
    {
        CalcT dist_1_2, dist_1_o;
        return ! calculate_collinear_data(a1, a2, b1, b2, v1, v2, norm, vother, dist_1_2, dist_1_o)
                ? Policy::disjoint()
                : Policy::one_degenerate(segment, segment_ratio<CalcT>(dist_1_o, dist_1_2), degenerated_a);
    }

    template <typename Point1, typename Point2, typename Vec3d, typename CalcT>
    static inline bool calculate_collinear_data(Point1 const& a1, Point1 const& a2,
                                                Point2 const& b1, Point2 const& b2,
                                                Vec3d const& a1v, // in
                                                Vec3d const& a2v, // in
                                                Vec3d const& norm1, // in
                                                Vec3d const& b1v_or_b2v, // in
                                                CalcT& dist_a1_a2, CalcT& dist_a1_i1) // out
    {
        // calculate dist_a1_a2 and dist_a1_i1
        calculate_dists(a1v, a2v, norm1, b1v_or_b2v, dist_a1_a2, dist_a1_i1);

        // if i1 is close to a1 and b1 or b2 is equal to a1
        if (is_endpoint_equal(dist_a1_i1, a1, b1, b2))
        {
            dist_a1_i1 = 0;
            return true;
        }
        // or i1 is close to a2 and b1 or b2 is equal to a2
        else if (is_endpoint_equal(dist_a1_a2 - dist_a1_i1, a2, b1, b2))
        {
            dist_a1_i1 = dist_a1_a2;
            return true;
        }

        // or i1 is on b
        return segment_ratio<CalcT>(dist_a1_i1, dist_a1_a2).on_segment();
    }

    template <typename Point1, typename Point2, typename Vec3d, typename CalcT>
    static inline bool calculate_ip_data(Point1 const& a1, Point1 const& a2, // in
                                         Point2 const& b1, Point2 const& b2, // in
                                         Vec3d const& a1v, Vec3d const& a2v, // in
                                         Vec3d const& b1v, Vec3d const& b2v, // in
                                         Vec3d const& norm1, Vec3d const& norm2, // in
                                         side_info const& sides, // in
                                         Vec3d & i1, // in-out
                                         CalcT& dist_a1_a2, CalcT& dist_a1_i1, // out
                                         CalcT& dist_b1_b2, CalcT& dist_b1_i1, // out
                                         intersection_point_flag& ip_flag)     // out
    {
        // great circles intersections
        i1 = cross_product(norm1, norm2);
        // NOTE: the length should be greater than 0 at this point
        //       if the normals were not normalized and their dot product
        //       not checked before this function is called the length
        //       should be checked here (math::equals(len, c0))
        CalcT const len = math::sqrt(dot_product(i1, i1));
        divide_value(i1, len); // normalize i1

        calculate_dists(a1v, a2v, norm1, i1, dist_a1_a2, dist_a1_i1);

        // choose the opposite side of the globe if the distance is shorter
        {
            CalcT const d = abs_distance(dist_a1_a2, dist_a1_i1);
            if (d > CalcT(0))
            {
                CalcT const dist_a1_i2 = dist_of_i2(dist_a1_i1);
                CalcT const d2 = abs_distance(dist_a1_a2, dist_a1_i2);
                if (d2 < d)
                {
                    dist_a1_i1 = dist_a1_i2;
                    multiply_value(i1, CalcT(-1)); // the opposite intersection
                }
            }
        }

        bool is_on_a = false, is_near_a1 = false, is_near_a2 = false;
        if (! is_potentially_crossing(dist_a1_a2, dist_a1_i1, is_on_a, is_near_a1, is_near_a2))
        {
            return false;
        }

        calculate_dists(b1v, b2v, norm2, i1, dist_b1_b2, dist_b1_i1);

        bool is_on_b = false, is_near_b1 = false, is_near_b2 = false;
        if (! is_potentially_crossing(dist_b1_b2, dist_b1_i1, is_on_b, is_near_b1, is_near_b2))
        {
            return false;
        }

        // reassign the IP if some endpoints overlap
        using geometry::detail::equals::equals_point_point;
        if (is_near_a1)
        {
            if (is_near_b1 && equals_point_point(a1, b1))
            {
                dist_a1_i1 = 0;
                dist_b1_i1 = 0;
                //i1 = a1v;
                ip_flag = ipi_at_a1;
                return true;
            }

            if (is_near_b2 && equals_point_point(a1, b2))
            {
                dist_a1_i1 = 0;
                dist_b1_i1 = dist_b1_b2;
                //i1 = a1v;
                ip_flag = ipi_at_a1;
                return true;
            }
        }

        if (is_near_a2)
        {
            if (is_near_b1 && equals_point_point(a2, b1))
            {
                dist_a1_i1 = dist_a1_a2;
                dist_b1_i1 = 0;
                //i1 = a2v;
                ip_flag = ipi_at_a2;
                return true;
            }

            if (is_near_b2 && equals_point_point(a2, b2))
            {
                dist_a1_i1 = dist_a1_a2;
                dist_b1_i1 = dist_b1_b2;
                //i1 = a2v;
                ip_flag = ipi_at_a2;
                return true;
            }
        }

        // at this point we know that the endpoints doesn't overlap
        // reassign IP and distance if the IP is on a segment and one of
        //   the endpoints of the other segment lies on the former segment
        if (is_on_a)
        {
            if (is_near_b1 && sides.template get<1, 0>() == 0) // b1 wrt a
            {
                dist_b1_i1 = 0;
                //i1 = b1v;
                ip_flag = ipi_at_b1;
                return true;
            }

            if (is_near_b2 && sides.template get<1, 1>() == 0) // b2 wrt a
            {
                dist_b1_i1 = dist_b1_b2;
                //i1 = b2v;
                ip_flag = ipi_at_b2;
                return true;
            }
        }

        if (is_on_b)
        {
            if (is_near_a1 && sides.template get<0, 0>() == 0) // a1 wrt b
            {
                dist_a1_i1 = 0;
                //i1 = a1v;
                ip_flag = ipi_at_a1;
                return true;
            }

            if (is_near_a2 && sides.template get<0, 1>() == 0) // a2 wrt b
            {
                dist_a1_i1 = dist_a1_a2;
                //i1 = a2v;
                ip_flag = ipi_at_a2;
                return true;
            }
        }

        ip_flag = ipi_inters;

        return is_on_a && is_on_b;
    }

    template <typename Vec3d, typename CalcT>
    static inline void calculate_dists(Vec3d const& a1v, // in
                                       Vec3d const& a2v, // in
                                       Vec3d const& norm1, // in
                                       Vec3d const& i1, // in
                                       CalcT& dist_a1_a2, CalcT& dist_a1_i1) // out
    {
        CalcT const c0 = 0;
        CalcT const c1 = 1;
        CalcT const c2 = 2;
        CalcT const c4 = 4;

        CalcT cos_a1_a2 = dot_product(a1v, a2v);
        dist_a1_a2 = -cos_a1_a2 + c1; // [1, -1] -> [0, 2] representing [0, pi]

        CalcT cos_a1_i1 = dot_product(a1v, i1);
        dist_a1_i1 = -cos_a1_i1 + c1; // [0, 2] representing [0, pi]
        if (dot_product(norm1, cross_product(a1v, i1)) < c0) // left or right of a1 on a
        {
            dist_a1_i1 = -dist_a1_i1; // [0, 2] -> [0, -2] representing [0, -pi]
        }
        if (dist_a1_i1 <= -c2) // <= -pi
        {
            dist_a1_i1 += c4; // += 2pi
        }
    }

    // the dist of the ip on the other side of the sphere
    template <typename CalcT>
    static inline CalcT dist_of_i2(CalcT const& dist_a1_i1)
    {
        CalcT const c2 = 2;
        CalcT const c4 = 4;

        CalcT dist_a1_i2 = dist_a1_i1 - c2; // dist_a1_i2 = dist_a1_i1 - pi;
        if (dist_a1_i2 <= -c2)          // <= -pi
        {
            dist_a1_i2 += c4;           // += 2pi;
        }
        return dist_a1_i2;
    }

    template <typename CalcT>
    static inline CalcT abs_distance(CalcT const& dist_a1_a2, CalcT const& dist_a1_i1)
    {
        if (dist_a1_i1 < CalcT(0))
            return -dist_a1_i1;
        else if (dist_a1_i1 > dist_a1_a2)
            return dist_a1_i1 - dist_a1_a2;
        else
            return CalcT(0);
    }

    template <typename CalcT>
    static inline bool is_potentially_crossing(CalcT const& dist_a1_a2, CalcT const& dist_a1_i1, // in
                                               bool& is_on_a, bool& is_near_a1, bool& is_near_a2) // out
    {
        is_on_a = segment_ratio<CalcT>(dist_a1_i1, dist_a1_a2).on_segment();
        is_near_a1 = is_near(dist_a1_i1);
        is_near_a2 = is_near(dist_a1_a2 - dist_a1_i1);
        return is_on_a || is_near_a1 || is_near_a2;
    }

    template <typename CalcT, typename P1, typename P2>
    static inline bool is_endpoint_equal(CalcT const& dist,
                                         P1 const& ai, P2 const& b1, P2 const& b2)
    {
        using geometry::detail::equals::equals_point_point;
        return is_near(dist) && (equals_point_point(ai, b1) || equals_point_point(ai, b2));
    }

    template <typename CalcT>
    static inline bool is_near(CalcT const& dist)
    {
        CalcT const small_number = CalcT(boost::is_same<CalcT, float>::value ? 0.0001 : 0.00000001);
        return math::abs(dist) <= small_number;
    }

    template <typename ProjCoord1, typename ProjCoord2>
    static inline int position_value(ProjCoord1 const& ca1,
                                     ProjCoord2 const& cb1,
                                     ProjCoord2 const& cb2)
    {
        // S1x  0   1    2     3   4
        // S2       |---------->
        return math::equals(ca1, cb1) ? 1
             : math::equals(ca1, cb2) ? 3
             : cb1 < cb2 ?
                ( ca1 < cb1 ? 0
                : ca1 > cb2 ? 4
                : 2 )
              : ( ca1 > cb1 ? 0
                : ca1 < cb2 ? 4
                : 2 );
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

/*template <typename Policy, typename CalculationType>
struct default_strategy<spherical_polar_tag, Policy, CalculationType>
{
    typedef relate_spherical_segments<Policy, CalculationType> type;
};*/

template <typename Policy, typename CalculationType>
struct default_strategy<spherical_equatorial_tag, Policy, CalculationType>
{
    typedef relate_spherical_segments<Policy, CalculationType> type;
};

template <typename Policy, typename CalculationType>
struct default_strategy<geographic_tag, Policy, CalculationType>
{
    typedef relate_spherical_segments<Policy, CalculationType> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::intersection

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_INTERSECTION_HPP