#include <iostream>
#include <iomanip>
#include <array>
#include <vector>
#include <deque>
#include <forward_list>
#include <list>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <string>
#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>

#ifndef PRETTY_PRINT_HPP
#define PRETTY_PRINT_HPP

// Handy functions for printing vectors and pairs
namespace PrettyPrint
{
    // Base template function for printing types
    template <typename T>
    inline std::string PrettyPrint(const T& toprint, const bool add_delimiters=false, const std::string& separator=", ")
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        std::ostringstream strm;
        strm << toprint;
        return strm.str();
    }

    /////////////////////////////////////////////////////////////////////////////////////////////////////////
    /////                                       PROTOTYPES ONLY                                         /////
    ///// Specializations for specific types - if you want a specialization for a new type, add it here /////
    /////////////////////////////////////////////////////////////////////////////////////////////////////////

    template<>
    inline std::string PrettyPrint(const bool& bool_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::Vector2d& vector_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::Vector3d& vector_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::Vector4d& vector_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::VectorXd& vector_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::MatrixXd& matrix_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::Quaterniond& quaternion_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::Isometry3d& transform_to_print, const bool add_delimiters, const std::string& separator);

    template <typename A, typename B>
    inline std::string PrettyPrint(const std::pair<A, B>& pairtoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, size_t N>
    inline std::string PrettyPrint(const std::array<T, N>& arraytoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::vector<T, Allocator>& vectoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::list<T, Allocator>& listtoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::forward_list<T, Allocator>& listtoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::deque<T, Allocator>& dequetoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename A, typename B, typename Compare=std::less<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
    inline std::string PrettyPrint(const std::map<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename A, typename B, typename Compare=std::less<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
    inline std::string PrettyPrint(const std::multimap<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Compare=std::less<T>, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::set<T, Compare, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Compare=std::less<T>, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::multiset<T, Compare, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename A, typename B, typename Hash=std::hash<A>, typename Predicate=std::equal_to<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
    inline std::string PrettyPrint(const std::unordered_map<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename A, typename B, typename Hash=std::hash<A>, typename Predicate=std::equal_to<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
    inline std::string PrettyPrint(const std::unordered_multimap<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Hash=std::hash<T>, typename Predicate=std::equal_to<T>, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::unordered_set<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Hash=std::hash<T>, typename Predicate=std::equal_to<T>, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::unordered_multiset<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");

    /////////////////////////////////////////////////////////////////////////////////////////////////////////
    /////                                   IMPLEMENTATIONS ONLY                                        /////
    ///// Specializations for specific types - if you want a specialization for a new type, add it here /////
    /////////////////////////////////////////////////////////////////////////////////////////////////////////

    template<>
    inline std::string PrettyPrint(const bool& bool_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        if (bool_to_print)
        {
            return "true";
        }
        else
        {
            return "false";
        }
    }

    template<>
    inline std::string PrettyPrint(const Eigen::Vector2d& vector_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        std::ostringstream strm;
        strm << std::setprecision(12) << "Vector2d: <x: " << vector_to_print(0) << " y: " << vector_to_print(1) << ">";
        return strm.str();
    }

    template<>
    inline std::string PrettyPrint(const Eigen::Vector3d& vector_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        std::ostringstream strm;
        strm << std::setprecision(12) << "Vector3d: <x: " << vector_to_print.x() << " y: " << vector_to_print.y() << " z: " << vector_to_print.z() << ">";
        return strm.str();
    }

    template<>
    inline std::string PrettyPrint(const Eigen::Vector4d& vector_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        std::ostringstream strm;
        strm << std::setprecision(12) << "Vector4d: <x: " << vector_to_print(0) << " y: " << vector_to_print(1) << " z: " << vector_to_print(2) << " w: " << vector_to_print(3) << ">";
        return strm.str();
    }

    template<>
    inline std::string PrettyPrint(const Eigen::VectorXd& vector_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        if (vector_to_print.size() > 1)
        {
            std::ostringstream strm;
            strm << "VectorXd: <" << std::to_string(vector_to_print(0));
            for (std::ptrdiff_t idx = 1; idx < vector_to_print.size(); idx++)
            {
                strm << ", " << std::to_string(vector_to_print(idx));
            }
            strm << ">";
            return strm.str();
        }
        else if (vector_to_print.size() == 1)
        {
            return "VectorXd: <" + std::to_string(vector_to_print(0)) + ">";
        }
        else
        {
            return "VectorXd: <>";
        }
    }

    template<>
    inline std::string PrettyPrint(const Eigen::MatrixXd& matrix_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        if (matrix_to_print.rows() > 0 && matrix_to_print.cols() > 0)
        {
            std::ostringstream strm;
            strm << "MatrixXd:\n[";
            strm << std::to_string(matrix_to_print(0, 0));
            for (int64_t col = 1; col < matrix_to_print.cols(); col++)
            {
                strm << ", " << std::to_string(matrix_to_print(0, col));
            }
            for (int64_t row = 1; row < matrix_to_print.rows(); row++)
            {
                strm << "\n" << std::to_string(matrix_to_print(row, 0));
                for (int64_t col = 1; col < matrix_to_print.cols(); col++)
                {
                    strm << ", " << std::to_string(matrix_to_print(row, col));
                }
            }
            strm << "]";
            return strm.str();
        }
        else
        {
            return "MatrixXd:\n[]";
        }
    }

    template<>
    inline std::string PrettyPrint(const Eigen::Quaterniond& quaternion_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        return "Quaterniond <x: " + std::to_string(quaternion_to_print.x()) + " y: " + std::to_string(quaternion_to_print.y()) + " z: " + std::to_string(quaternion_to_print.z()) + " w: " + std::to_string(quaternion_to_print.w()) + ">";
    }

    template<>
    inline std::string PrettyPrint(const Eigen::Isometry3d& transform_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        Eigen::Vector3d vector_to_print = transform_to_print.translation();
        Eigen::Quaterniond quaternion_to_print(transform_to_print.rotation());
        return "Isometry3d <x: " + std::to_string(vector_to_print.x()) + " y: " + std::to_string(vector_to_print.y()) + " z: " + std::to_string(vector_to_print.z()) + ">, <x: " + std::to_string(quaternion_to_print.x()) + " y: " + std::to_string(quaternion_to_print.y()) + " z: " + std::to_string(quaternion_to_print.z()) + " w: " + std::to_string(quaternion_to_print.w()) + ">";
    }

    template <typename A, typename B>
    inline std::string PrettyPrint(const std::pair<A, B>& pairtoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (add_delimiters)
        {
            strm << "<" << PrettyPrint(pairtoprint.first, add_delimiters, separator) << ": " << PrettyPrint(pairtoprint.second, add_delimiters, separator) << ">";
        }
        else
        {
            strm << PrettyPrint(pairtoprint.first, add_delimiters, separator) << ": " << PrettyPrint(pairtoprint.second, add_delimiters, separator);
        }
        return strm.str();
    }

    template <typename T, size_t N>
    inline std::string PrettyPrint(const std::array<T, N>& arraytoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (arraytoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "[" << PrettyPrint(arraytoprint[0], add_delimiters, separator);
                for (size_t idx = 1; idx < arraytoprint.size(); idx++)
                {
                    strm << separator << PrettyPrint(arraytoprint[idx], add_delimiters, separator);
                }
                strm << "]";
            }
            else
            {
                strm << PrettyPrint(arraytoprint[0], add_delimiters, separator);
                for (size_t idx = 1; idx < arraytoprint.size(); idx++)
                {
                    strm << separator << PrettyPrint(arraytoprint[idx], add_delimiters, separator);
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Allocator>
    inline std::string PrettyPrint(const std::vector<T, Allocator>& vectoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (vectoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "[" << PrettyPrint(vectoprint[0], add_delimiters, separator);
                for (size_t idx = 1; idx < vectoprint.size(); idx++)
                {
                    strm << separator << PrettyPrint(vectoprint[idx], add_delimiters, separator);
                }
                strm << "]";
            }
            else
            {
                strm << PrettyPrint(vectoprint[0], add_delimiters, separator);
                for (size_t idx = 1; idx < vectoprint.size(); idx++)
                {
                    strm << separator << PrettyPrint(vectoprint[idx], add_delimiters, separator);
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Allocator>
    inline std::string PrettyPrint(const std::list<T, Allocator>& listtoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (listtoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "[";
                typename std::list<T, Allocator>::const_iterator itr;
                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
                {
                    if (itr != listtoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << "]";
            }
            else
            {
                typename std::list<T, Allocator>::const_iterator itr;
                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
                {
                    if (itr != listtoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Allocator>
    inline std::string PrettyPrint(const std::forward_list<T, Allocator>& listtoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (listtoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "[";
                typename std::forward_list<T, Allocator>::const_iterator itr;
                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
                {
                    if (itr != listtoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << "]";
            }
            else
            {
                typename std::forward_list<T, Allocator>::const_iterator itr;
                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
                {
                    if (itr != listtoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Allocator>
    inline std::string PrettyPrint(const std::deque<T, Allocator>& dequetoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (dequetoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "[";
                typename std::deque<T, Allocator>::const_iterator itr;
                for (itr = dequetoprint.begin(); itr != dequetoprint.end(); ++itr)
                {
                    if (itr != dequetoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << "]";
            }
            else
            {
                typename std::deque<T, Allocator>::const_iterator itr;
                for (itr = dequetoprint.begin(); itr != dequetoprint.end(); ++itr)
                {
                    if (itr != dequetoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename A, typename B, typename Compare, typename Allocator>
    inline std::string PrettyPrint(const std::map<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (maptoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "{";
                typename std::map<A, B, Compare, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
                strm << "}";
            }
            else
            {
                typename std::map<A, B, Compare, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename A, typename B, typename Compare, typename Allocator>
    inline std::string PrettyPrint(const std::multimap<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (maptoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "{";
                typename std::multimap<A, B, Compare, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
                strm << "}";
            }
            else
            {
                typename std::multimap<A, B, Compare, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Compare, typename Allocator>
    inline std::string PrettyPrint(const std::set<T, Compare, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (settoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "(";
                typename std::set<T, Compare, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << ")";
            }
            else
            {
                typename std::set<T, Compare, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Compare, typename Allocator>
    inline std::string PrettyPrint(const std::multiset<T, Compare, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (settoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "(";
                typename std::multiset<T, Compare, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << ")";
            }
            else
            {
                typename std::multiset<T, Compare, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename A, typename B, typename Hash, typename Predicate, typename Allocator>
    inline std::string PrettyPrint(const std::unordered_map<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (maptoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "{";
                typename std::unordered_map<A, B, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
                strm << "}";
            }
            else
            {
                typename std::unordered_map<A, B, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename A, typename B, typename Hash, typename Predicate, typename Allocator>
    inline std::string PrettyPrint(const std::unordered_multimap<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (maptoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "{";
                typename std::unordered_multimap<A, B, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
                strm << "}";
            }
            else
            {
                typename std::unordered_multimap<A, B, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Hash, typename Predicate, typename Allocator>
    inline std::string PrettyPrint(const std::unordered_set<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (settoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "(";
                typename std::unordered_set<T, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << ")";
            }
            else
            {
                typename std::unordered_set<T, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Hash, typename Predicate, typename Allocator>
    inline std::string PrettyPrint(const std::unordered_multiset<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (settoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "(";
                typename std::unordered_multiset<T, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << ")";
            }
            else
            {
                typename std::unordered_multiset<T, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }
}

#endif // PRETTY_PRINT_HPP


/*! \file Peridigm_EnergyReleaseDamageModel.cpp */

//@HEADER
// ************************************************************************
//
//                             Peridigm
//                 Copyright (2011) Sandia Corporation
//
// Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
// the U.S. Government retains certain rights in this software.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the Corporation nor the names of the
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY SANDIA CORPORATION "AS IS" AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL SANDIA CORPORATION OR THE
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Questions?
// David J. Littlewood   djlittl@sandia.gov
// John A. Mitchell      jamitch@sandia.gov
// Michael L. Parks      mlparks@sandia.gov
// Stewart A. Silling    sasilli@sandia.gov
//
// ************************************************************************
//////////////////////////////////////////////////////////////////////////////////////
// Routine developed for Peridigm by
// DLR Composite Structures and Adaptive Systems
//                                __/|__
//                                /_/_/_/
//            www.dlr.de/fa/en      |/ DLR
//////////////////////////////////////////////////////////////////////////////////////
// Questions?
// Christian Willberg  christian.willberg@dlr.de
// Martin Raedel       martin.raedel@dlr.de
//////////////////////////////////////////////////////////////////////////////////////
//@HEADER

#include "Peridigm_EnergyReleaseDamageModel.hpp"
#include "Peridigm_Field.hpp"
#include "material_utilities.h"
#include <thread>
#include <Teuchos_Assert.hpp>
#include <Epetra_SerialComm.h>
#include <Sacado.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

using namespace std;

PeridigmNS::EnergyReleaseDamageModel::EnergyReleaseDamageModel(const Teuchos::ParameterList& params)
: DamageModel(params),
m_applyThermalStrains(false),
m_modelCoordinatesFieldId(-1),
m_coordinatesFieldId(-1),
m_damageFieldId(-1),
m_bondDamageFieldId(-1),
m_deltaTemperatureFieldId(-1),
m_dilatationFieldId(-1),
m_weightedVolumeFieldId(-1),
m_horizonFieldId(-1),
m_damageModelFieldId(-1),
m_OMEGA(PeridigmNS::InfluenceFunction::self().getInfluenceFunction()) {

    if (params.isParameter("Critical Energy")) {
        m_criticalEnergyTension = params.get<double>("Critical Energy Tension");
        m_criticalEnergyCompression = params.get<double>("Critical Energy Compression");
        m_criticalEnergyShear = params.get<double>("Critical Energy Shear");

    } else {
        if (params.isParameter("Critical Energy Tension"))
            m_criticalEnergyTension = params.get<double>("Critical Energy Tension");
        else
            m_criticalEnergyTension = -1.0;
        if (params.isParameter("Critical Energy Compression"))
            m_criticalEnergyCompression = params.get<double>("Critical Energy Compression");
        else
            m_criticalEnergyCompression = -1.0;
        if (params.isParameter("Critical Energy Shear"))
            m_criticalEnergyShear = params.get<double>("Critical Energy Shear");
        else
            m_criticalEnergyShear = -1.0;
        m_type = 1;
        if (params.isType<string>("Energy Criterion"))
            m_type = 1;
        if (params.isType<string>("Power Law"))
            m_type = 2;
        if (params.isType<string>("Separated"))
            m_type = 3;
    }

    m_pi = 3.14159;

    if (params.isParameter("Thermal Expansion Coefficient")) {
        m_alpha = params.get<double>("Thermal Expansion Coefficient");
        m_applyThermalStrains = true;
    }

    PeridigmNS::FieldManager& fieldManager = PeridigmNS::FieldManager::self();
    m_modelCoordinatesFieldId = fieldManager.getFieldId("Model_Coordinates");
    m_coordinatesFieldId = fieldManager.getFieldId("Coordinates");
    m_volumeFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Volume");
    m_weightedVolumeFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Weighted_Volume");
    m_dilatationFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::TWO_STEP, "Dilatation");
    m_damageFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::ELEMENT, PeridigmNS::PeridigmField::SCALAR, PeridigmNS::PeridigmField::TWO_STEP, "Damage");
    m_bondDamageFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::BOND, PeridigmNS::PeridigmField::SCALAR, PeridigmNS::PeridigmField::TWO_STEP, "Bond_Damage");
    m_horizonFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::ELEMENT, PeridigmNS::PeridigmField::SCALAR, PeridigmNS::PeridigmField::CONSTANT, "Horizon");
    m_damageModelFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::NODE, PeridigmNS::PeridigmField::VECTOR, PeridigmNS::PeridigmField::TWO_STEP, "Damage_Model_Data");
    m_fieldIds.push_back(m_volumeFieldId);
    m_fieldIds.push_back(m_modelCoordinatesFieldId);
    m_fieldIds.push_back(m_coordinatesFieldId);
    m_fieldIds.push_back(m_weightedVolumeFieldId);
    m_fieldIds.push_back(m_dilatationFieldId);
    m_fieldIds.push_back(m_volumeFieldId);
    m_fieldIds.push_back(m_damageFieldId);
    m_fieldIds.push_back(m_damageModelFieldId);
    m_fieldIds.push_back(m_bondDamageFieldId);
    m_fieldIds.push_back(m_horizonFieldId);

}

PeridigmNS::EnergyReleaseDamageModel::~EnergyReleaseDamageModel() {
}

void
PeridigmNS::EnergyReleaseDamageModel::initialize(const double dt,
        const int numOwnedPoints,
        const int* ownedIDs,
        const int* neighborhoodList,
        PeridigmNS::DataManager& dataManager) const {
    double *damage, *bondDamage;

    dataManager.getData(m_damageFieldId, PeridigmField::STEP_NP1)->ExtractView(&damage);
    dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);

    // Initialize damage to zero
    int neighborhoodListIndex(0);
    int bondIndex(0);
    int nodeId, numNeighbors;
    int iID, iNID;

    for (iID = 0; iID < numOwnedPoints; ++iID) {
        nodeId = ownedIDs[iID];
        damage[nodeId] = 0.0;
        numNeighbors = neighborhoodList[neighborhoodListIndex++];
        neighborhoodListIndex += numNeighbors;
        for (iNID = 0; iNID < numNeighbors; ++iNID) {
            bondDamage[bondIndex] = 0.0;
            bondIndex += 1;
        }
    }

}

void
PeridigmNS::EnergyReleaseDamageModel::computeDamage(const double dt,
        const int numOwnedPoints,
        const int* ownedIDs,
        const int* neighborhoodList,
        PeridigmNS::DataManager& dataManager) const {

    double *x, *y, *damage, *bondDamageNP1, *horizon;
    double *cellVolume, *weightedVolume, *damageModel;
    double criticalEnergyTension(-1.0), criticalEnergyCompression(-1.0), criticalEnergyShear(-1.0);
    // for temperature dependencies easy to extent
    double *deltaTemperature = NULL;
    double m_alpha = 0;

    dataManager.getData(m_damageFieldId, PeridigmField::STEP_NP1)->ExtractView(&damage);
    dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
    dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
    dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
    dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
    //////////////////////////////////////////////////////////////
    // transfer of data is done in ComputeDilation --> PeridigmMaterial.cpp
    //////////////////////////////////////////////////////////////
    dataManager.getData(m_damageModelFieldId, PeridigmField::STEP_NP1)->ExtractView(&damageModel);
    dataManager.getData(m_horizonFieldId, PeridigmField::STEP_NONE)->ExtractView(&horizon);
    ////////////////////////////////////
    ////////////////////////////////////
    // transfer of data is done in ComputeDilation --> PeridigmMaterial.cpp
    dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamageNP1);

    ////////////////////////////////////////////////////
    double trialDamage(0.0);
    int neighborhoodListIndex(0), bondIndex(0);
    int nodeId, numNeighbors, neighborID, iID, iNID;
    double totalDamage;
    double alphaP1, alphaP2;
    double nodeInitialX[3], nodeCurrentX[3], relativeExtension(0.0);
    double bondEnergyIsotropic(0.0), bondEnergyDeviatoric(0.0);
    double omegaP1, omegaP2;
    double critDev, critIso, critComp;
    double BulkMod1, BulkMod2;
    double degradationFactor = 1.0; // Optional parameter if bond should be degradated and not fully destroyed instantaneously
    double avgHorizon, quadhorizon;
    //---------------------------
    // INITIALIZE PROCESS STEP t
    //---------------------------

    if (m_criticalEnergyTension > 0.0)
        criticalEnergyTension = m_criticalEnergyTension;
    if (m_criticalEnergyCompression > 0.0)
        criticalEnergyCompression = m_criticalEnergyCompression;
    if (m_criticalEnergyShear > 0.0)
        criticalEnergyShear = m_criticalEnergyShear;

    // Update the bond damage
    // Break bonds if the bond energy potential is greater than the critical bond energy potential
    //---------------------------
    // DAMAGE ANALYSIS
    //---------------------------
    bondIndex = 0;

    for (iID = 0; iID < numOwnedPoints; ++iID) {
        numNeighbors = neighborhoodList[neighborhoodListIndex++];

        nodeId = ownedIDs[iID];
        nodeInitialX[0] = x[nodeId*3];
        nodeInitialX[1] = x[nodeId*3+1];
        nodeInitialX[2] = x[nodeId*3+2];
        nodeCurrentX[0] = y[nodeId*3];
        nodeCurrentX[1] = y[nodeId*3+1];
        nodeCurrentX[2] = y[nodeId*3+2];

        double dilatationP1 = damageModel[3*nodeId];

        alphaP1 = 15.0 * damageModel[3*nodeId+2]; // weightedVolume is already included in --> Perdigm_Material.cpp;

        for (iNID = 0; iNID < numNeighbors; ++iNID) {

            trialDamage = 0.0;
            neighborID = neighborhoodList[neighborhoodListIndex++];

            double zeta =
            distance(nodeInitialX[0], nodeInitialX[1], nodeInitialX[2],
                 x[neighborID*3], x[neighborID*3+1], x[neighborID*3+2]);

            double dY =
            distance(nodeCurrentX[0], nodeCurrentX[1], nodeCurrentX[2],
                 y[neighborID*3], y[neighborID*3+1], y[neighborID*3+2]);

            relativeExtension = (dY - zeta)/zeta;
             // the direction switches between both bonds. This results in a switch of the forces
             // as well. Therefore, all forces and bond deformations are normalized.
            double eP = dY - zeta;
            //double normZeta = sqrt(zeta*zeta);

            alphaP2 = 15.0 * damageModel[3*neighborID+2]; // weightedVolume is already included in --> Perdigm_Material.cpp;

            BulkMod1 = damageModel[3*nodeId+1];     // weightedVolume is already included in the variable --> Perdigm_Material.cpp;
            BulkMod2 = damageModel[3*neighborID+1]; // weightedVolume is already included in the variable --> Perdigm_Material.cpp;

            double dilatationP2 = damageModel[3*neighborID];

            omegaP1 = MATERIAL_EVALUATION::scalarInfluenceFunction(zeta, horizon[nodeId]);
            omegaP2 = MATERIAL_EVALUATION::scalarInfluenceFunction(zeta, horizon[neighborID]);

            double eiP1 = dilatationP1 * zeta / 3.0;
            double eiP2 = dilatationP2 * zeta / 3.0;
            double tiP1 = 3*BulkMod1*omegaP1*dilatationP1*zeta;
            double tiP2 = 3*BulkMod2*omegaP2*dilatationP2*zeta;

            double edP1 = eP - sqrt(eiP1*eiP1);
            double edP2 = eP - sqrt(eiP2*eiP2);
            double tdP1 = omegaP1*alphaP1*edP1;
            double tdP2 = omegaP2*alphaP2*edP2;
            // absolute values of energy, because it is positive and coordinate errors are avoided.
            bondEnergyIsotropic  = (1.0 - bondDamageNP1[bondIndex])*(sqrt(tiP1*eiP1*tiP1*eiP1) + sqrt(tiP2*eiP2*tiP2*eiP2));
            bondEnergyDeviatoric = (1.0 - bondDamageNP1[bondIndex])*(sqrt(tdP1*edP1*tdP2*edP2) + sqrt(tdP2*edP2*tdP2*edP2));
            // the average horizon is taken, if multiple horizons are used
            avgHorizon = 0.5*(horizon[nodeId]+horizon[neighborID]);

            // geometrical part of the energy value
            quadhorizon =  16.0 /( m_pi * avgHorizon * avgHorizon * avgHorizon * avgHorizon );
            // the factor 16 can be split in three parts:
            // 4 comes from the integration from Foster et al. (2009) Journal for Multiscale Computational Engineering;
            // 2 comes from the force split motivated by the bond based formulation Bobaru et al. (2017) "Handbook of Peridynamik Modeling", page 48 ;
            // 2 comes from the energy formulation itself
            critIso = 0.0;
            if (relativeExtension>0&&criticalEnergyTension != -1.0){
               critIso = (bondEnergyIsotropic/(criticalEnergyTension*quadhorizon));
            }
            critDev = 0.0;
            if (criticalEnergyShear != -1.0){
               critDev = (bondEnergyDeviatoric/(criticalEnergyShear*quadhorizon));
            }
            critComp = 0.0;
            if (relativeExtension < 0.0 && criticalEnergyCompression != -1.0){
                critComp = (bondEnergyIsotropic/(criticalEnergyCompression*quadhorizon));
                critIso = 0.0;
            }
            if (m_type == 1){ // Energy Criterion by Foster et al.(2009) Journal for Multiscale Computational Engineering;
                double bondEnergy = sqrt((tdP1+tiP1)*(tdP1+tiP1)*eP*eP) + sqrt((tdP2 + tiP2)*(tdP2 + tiP2)*eP*eP);
                critIso = bondEnergy/(criticalEnergyTension*quadhorizon);
                if (m_criticalEnergyTension > 0.0 && critIso > 1.0 ) {
                    trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
            }

            if (m_type == 2){// Power Law

                if (m_criticalEnergyTension > 0.0 &&critIso*critIso + critDev*critDev + critComp*critComp > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
            }
            if (m_type == 3){ // Separated
                if (m_criticalEnergyTension > 0.0  &&critIso > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
                if (m_criticalEnergyTension > 0.0 && critDev > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
                if (m_criticalEnergyCompression > 0.0  && critComp > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
            }

            if (trialDamage > bondDamageNP1[bondIndex]) {
                if (trialDamage>1)trialDamage = 1;
                bondDamageNP1[bondIndex] = trialDamage;

            }

            bondIndex += 1;

            }

        }

    //  Update the element damage (percent of bonds broken)

    neighborhoodListIndex = 0;
    bondIndex = 0;
    for (iID = 0; iID < numOwnedPoints; ++iID) {
        nodeId = ownedIDs[iID];
        numNeighbors = neighborhoodList[neighborhoodListIndex++];
        //neighborhoodListIndex += numNeighbors;
        damageModel[3*nodeId] = 0.0;
        damageModel[3*nodeId+1] = 0.0;
        damageModel[3*nodeId+2] = 0.0;
        totalDamage = 0.0;
        for (iNID = 0; iNID < numNeighbors; ++iNID) {

            neighborID = neighborhoodList[neighborhoodListIndex++];
            // must be zero to avoid synchronization errors
            damageModel[3*neighborID] = 0.0;
            damageModel[3*neighborID+1] = 0.0;
            damageModel[3*neighborID+2] = 0.0;

            totalDamage += bondDamageNP1[bondIndex];

            bondIndex += 1;
        }
        if (numNeighbors > 0)
            totalDamage /= numNeighbors;
        else
            totalDamage = 0.0;
        damage[nodeId] = totalDamage;
    }
}


/*

Copyright (c) 2013, Project OSRM, Dennis Luxen, others
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include <osrm/Coordinate.h>
#include "../Util/MercatorUtil.h"
#include "../Util/SimpleLogger.h"
#include "../Util/StringUtil.h"
#include "../Util/TrigonometryTables.h"

#include <boost/assert.hpp>

#ifndef NDEBUG
#include <bitset>
#endif
#include <iostream>
#include <limits>

FixedPointCoordinate::FixedPointCoordinate()
    : lat(std::numeric_limits<int>::min()), lon(std::numeric_limits<int>::min())
{
}

FixedPointCoordinate::FixedPointCoordinate(int lat, int lon) : lat(lat), lon(lon)
{
#ifndef NDEBUG
    if (0 != (std::abs(lat) >> 30))
    {
        std::bitset<32> y(lat);
        SimpleLogger().Write(logDEBUG) << "broken lat: " << lat << ", bits: " << y;
    }
    if (0 != (std::abs(lon) >> 30))
    {
        std::bitset<32> x(lon);
        SimpleLogger().Write(logDEBUG) << "broken lon: " << lon << ", bits: " << x;
    }
#endif
}

void FixedPointCoordinate::Reset()
{
    lat = std::numeric_limits<int>::min();
    lon = std::numeric_limits<int>::min();
}
bool FixedPointCoordinate::isSet() const
{
    return (std::numeric_limits<int>::min() != lat) && (std::numeric_limits<int>::min() != lon);
}
bool FixedPointCoordinate::isValid() const
{
    if (lat > 90 * COORDINATE_PRECISION || lat < -90 * COORDINATE_PRECISION ||
        lon > 180 * COORDINATE_PRECISION || lon < -180 * COORDINATE_PRECISION)
    {
        return false;
    }
    return true;
}
bool FixedPointCoordinate::operator==(const FixedPointCoordinate &other) const
{
    return lat == other.lat && lon == other.lon;
}

double FixedPointCoordinate::ApproximateDistance(const int lat1,
                                                 const int lon1,
                                                 const int lat2,
                                                 const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
    double RAD = 0.017453292519943295769236907684886;
    double lt1 = lat1 / COORDINATE_PRECISION;
    double ln1 = lon1 / COORDINATE_PRECISION;
    double lt2 = lat2 / COORDINATE_PRECISION;
    double ln2 = lon2 / COORDINATE_PRECISION;
    double dlat1 = lt1 * (RAD);

    double dlong1 = ln1 * (RAD);
    double dlat2 = lt2 * (RAD);
    double dlong2 = ln2 * (RAD);

    double dLong = dlong1 - dlong2;
    double dLat = dlat1 - dlat2;

    double aHarv = pow(sin(dLat / 2.0), 2.0) + cos(dlat1) * cos(dlat2) * pow(sin(dLong / 2.), 2);
    double cHarv = 2. * atan2(sqrt(aHarv), sqrt(1.0 - aHarv));
    // earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
    // The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
    const double earth = 6372797.560856;
    return earth * cHarv;
}

double FixedPointCoordinate::ApproximateDistance(const FixedPointCoordinate &c1,
                                                 const FixedPointCoordinate &c2)
{
    return ApproximateDistance(c1.lat, c1.lon, c2.lat, c2.lon);
}

float FixedPointCoordinate::ApproximateEuclideanDistance(const FixedPointCoordinate &c1,
                                                 const FixedPointCoordinate &c2)
{
    return ApproximateEuclideanDistance(c1.lat, c1.lon, c2.lat, c2.lon);
}

float FixedPointCoordinate::ApproximateEuclideanDistance(const int lat1,
                                                          const int lon1,
                                                          const int lat2,
                                                          const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());

    const float RAD = 0.017453292519943295769236907684886;
    const float float_lat1 = (lat1 / COORDINATE_PRECISION) * RAD;
    const float float_lon1 = (lon1 / COORDINATE_PRECISION) * RAD;
    const float float_lat2 = (lat2 / COORDINATE_PRECISION) * RAD;
    const float float_lon2 = (lon2 / COORDINATE_PRECISION) * RAD;

    const float x = (float_lon2 - float_lon1) * cos((float_lat1 + float_lat2) / 2.);
    const float y = (float_lat2 - float_lat1);
    const float earth_radius = 6372797.560856;
    return sqrt(x * x + y * y) * earth_radius;
}

float FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &point,
                                                          const FixedPointCoordinate &segA,
                                                          const FixedPointCoordinate &segB)
{
    const float x = lat2y(point.lat / COORDINATE_PRECISION);
    const float y = point.lon / COORDINATE_PRECISION;
    const float a = lat2y(segA.lat / COORDINATE_PRECISION);
    const float b = segA.lon / COORDINATE_PRECISION;
    const float c = lat2y(segB.lat / COORDINATE_PRECISION);
    const float d = segB.lon / COORDINATE_PRECISION;
    float p, q, nY;
    if (std::abs(a - c) > std::numeric_limits<float>::epsilon())
    {
        const float m = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x + (m * y)) + (m * m * a - m * b)) / (1. + m * m);
        q = b + m * (p - a);
    }
    else
    {
        p = c;
        q = y;
    }
    nY = (d * p - c * q) / (a * d - b * c);

    // discretize the result to coordinate precision. it's a hack!
    if (std::abs(nY) < (1. / COORDINATE_PRECISION))
    {
        nY = 0.;
    }

    float r = (p - nY * a) / c;
    if (std::isnan(r))
    {
        r = ((segB.lat == point.lat) && (segB.lon == point.lon)) ? 1. : 0.;
    }
    else if (std::abs(r) <= std::numeric_limits<float>::epsilon())
    {
        r = 0.;
    }
    else if (std::abs(r - 1.) <= std::numeric_limits<float>::epsilon())
    {
        r = 1.;
    }
    FixedPointCoordinate nearest_location;
    BOOST_ASSERT(!std::isnan(r));
    if (r <= 0.)
    { // point is "left" of edge
        nearest_location.lat = segA.lat;
        nearest_location.lon = segA.lon;
    }
    else if (r >= 1.)
    { // point is "right" of edge
        nearest_location.lat = segB.lat;
        nearest_location.lon = segB.lon;
    }
    else
    { // point lies in between
        nearest_location.lat = y2lat(p) * COORDINATE_PRECISION;
        nearest_location.lon = q * COORDINATE_PRECISION;
    }
    BOOST_ASSERT(nearest_location.isValid());
    const float approximated_distance =
        FixedPointCoordinate::ApproximateEuclideanDistance(point, nearest_location);
    BOOST_ASSERT(0. <= approximated_distance);
    return approximated_distance;
}

float FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &coord_a,
                                                   const FixedPointCoordinate &coord_b,
                                                   const FixedPointCoordinate &query_location,
                                                   FixedPointCoordinate &nearest_location,
                                                   float &r)
{
    BOOST_ASSERT(query_location.isValid());

    const float x = lat2y(query_location.lat / COORDINATE_PRECISION);
    const float y = query_location.lon / COORDINATE_PRECISION;
    const float a = lat2y(coord_a.lat / COORDINATE_PRECISION);
    const float b = coord_a.lon / COORDINATE_PRECISION;
    const float c = lat2y(coord_b.lat / COORDINATE_PRECISION);
    const float d = coord_b.lon / COORDINATE_PRECISION;
    float p, q /*,mX*/, nY;
    if (std::abs(a - c) > std::numeric_limits<float>::epsilon())
    {
        const float m = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x + (m * y)) + (m * m * a - m * b)) / (1. + m * m);
        q = b + m * (p - a);
    }
    else
    {
        p = c;
        q = y;
    }
    nY = (d * p - c * q) / (a * d - b * c);

    // discretize the result to coordinate precision. it's a hack!
    if (std::abs(nY) < (1. / COORDINATE_PRECISION))
    {
        nY = 0.;
    }

    r = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need
    // not calculate the explicit values of m an n as we
    // are just interested in the ratio
    if (std::isnan(r))
    {
        r = ((coord_b.lat == query_location.lat) && (coord_b.lon == query_location.lon)) ? 1. : 0.;
    }
    else if (std::abs(r) <= std::numeric_limits<float>::epsilon())
    {
        r = 0.;
    }
    else if (std::abs(r - 1.) <= std::numeric_limits<float>::epsilon())
    {
        r = 1.;
    }
    BOOST_ASSERT(!std::isnan(r));
    if (r <= 0.)
    {
        nearest_location = coord_a;
    }
    else if (r >= 1.)
    {
        nearest_location = coord_b;
    }
    else
    {
        // point lies in between
        nearest_location.lat = y2lat(p) * COORDINATE_PRECISION;
        nearest_location.lon = q * COORDINATE_PRECISION;
    }
    BOOST_ASSERT(nearest_location.isValid());

    // TODO: Replace with euclidean approximation when k-NN search is done
    // const float approximated_distance = FixedPointCoordinate::ApproximateEuclideanDistance(
    const float approximated_distance =
        FixedPointCoordinate::ApproximateEuclideanDistance(query_location, nearest_location);
    BOOST_ASSERT(0. <= approximated_distance);
    return approximated_distance;
}

void FixedPointCoordinate::convertInternalLatLonToString(const int value, std::string &output)
{
    char buffer[12];
    buffer[11] = 0; // zero termination
    output = printInt<11, 6>(buffer, value);
}

void FixedPointCoordinate::convertInternalCoordinateToString(const FixedPointCoordinate &coord,
                                                             std::string &output)
{
    std::string tmp;
    tmp.reserve(23);
    convertInternalLatLonToString(coord.lon, tmp);
    output = tmp;
    output += ",";
    convertInternalLatLonToString(coord.lat, tmp);
    output += tmp;
}

void
FixedPointCoordinate::convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord,
                                                                std::string &output)
{
    std::string tmp;
    tmp.reserve(23);
    convertInternalLatLonToString(coord.lat, tmp);
    output = tmp;
    output += ",";
    convertInternalLatLonToString(coord.lon, tmp);
    output += tmp;
}

void FixedPointCoordinate::Output(std::ostream &out) const
{
    out << "(" << lat / COORDINATE_PRECISION << "," << lon / COORDINATE_PRECISION << ")";
}

float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &A, const FixedPointCoordinate &B)
{
    const float delta_long = DegreeToRadian(B.lon / COORDINATE_PRECISION - A.lon / COORDINATE_PRECISION);
    const float lat1 = DegreeToRadian(A.lat / COORDINATE_PRECISION);
    const float lat2 = DegreeToRadian(B.lat / COORDINATE_PRECISION);
    const float y = sin(delta_long) * cos(lat2);
    const float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(delta_long);
    float result = RadianToDegree(std::atan2(y, x));
    while (result < 0.f)
    {
        result += 360.f;
    }

    while (result >= 360.f)
    {
        result -= 360.f;
    }
    return result;
}

float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &other) const
{
    const float delta_long = DegreeToRadian(lon / COORDINATE_PRECISION - other.lon / COORDINATE_PRECISION);
    const float lat1 = DegreeToRadian(other.lat / COORDINATE_PRECISION);
    const float lat2 = DegreeToRadian(lat / COORDINATE_PRECISION);
    const float y = std::sin(delta_long) * std::cos(lat2);
    const float x = std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(delta_long);
    float result = RadianToDegree(std::atan2(y, x));

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

    while (result >= 360.f)
    {
        result -= 360.f;
    }
    return result;
}

float FixedPointCoordinate::DegreeToRadian(const float degree)
{
    return degree * (M_PI / 180.f);
}

float FixedPointCoordinate::RadianToDegree(const float radian)
{
    return radian * (180.f / M_PI);
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/range/combine.hpp>
#include <boost/tuple/tuple.hpp>
#include <cstddef>

#include "DataStructures/Index.hpp"
#include "DataStructures/SliceIterator.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"

/// @{
/*!
 * \ingroup DataStructuresGroup
 * \brief Slices volume `Tensor`s into a `Variables`
 *
 * The slice has a constant logical coordinate in direction `sliced_dim`,
 * slicing the volume at `fixed_index` in that dimension.  For
 * example, to get the lower boundary of `sliced_dim`, pass `0` for
 * `fixed_index`; to get the upper boundary, pass
 * `extents[sliced_dim] - 1`.
 */
template <typename... TagsToSlice, size_t VolumeDim>
void data_on_slice(
    const gsl::not_null<Variables<tmpl::list<TagsToSlice...>>*> interface_vars,
    const Index<VolumeDim>& element_extents, const size_t sliced_dim,
    const size_t fixed_index, const typename TagsToSlice::type&... tensors) {
  const size_t interface_grid_points =
      element_extents.slice_away(sliced_dim).product();
  if (interface_vars->number_of_grid_points() != interface_grid_points) {
    *interface_vars =
        Variables<tmpl::list<TagsToSlice...>>(interface_grid_points);
  }
  for (SliceIterator si(element_extents, sliced_dim, fixed_index); si; ++si) {
    const auto lambda = [&si](auto& interface_tensor,
                              const auto& volume_tensor) {
      for (decltype(auto) interface_and_volume_tensor_components :
           boost::combine(interface_tensor, volume_tensor)) {
        boost::get<0>(
            interface_and_volume_tensor_components)[si.slice_offset()] =
            boost::get<1>(
                interface_and_volume_tensor_components)[si.volume_offset()];
      }
      return '0';
    };
    expand_pack(lambda(get<TagsToSlice>(*interface_vars), tensors)...);
  }
}

template <typename... TagsToSlice, size_t VolumeDim>
Variables<tmpl::list<TagsToSlice...>> data_on_slice(
    const Index<VolumeDim>& element_extents, const size_t sliced_dim,
    const size_t fixed_index, const typename TagsToSlice::type&... tensors) {
  Variables<tmpl::list<TagsToSlice...>> interface_vars(
      element_extents.slice_away(sliced_dim).product());
  data_on_slice<TagsToSlice...>(make_not_null(&interface_vars), element_extents,
                                sliced_dim, fixed_index, tensors...);
  return interface_vars;
}
/// @}


//---------------------------------------------------------------------------//
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_INJECT_HASH_HPP
#define CRYPTO3_INJECT_HASH_HPP

#include <boost/crypto3/detail/stream_endian.hpp>
#include <boost/crypto3/detail/basic_functions.hpp>
#include <boost/crypto3/detail/unbounded_shift.hpp>
#include <boost/crypto3/detail/endian_shift.hpp>

namespace boost {
    namespace crypto3 {
        namespace detail {

            template<typename Endianness, std::size_t WordBits, std::size_t BlockWords, std::size_t BlockBits>
            struct word_injector;

            template<int UnitBits, std::size_t WordBits, std::size_t BlockWords, std::size_t BlockBits>
            struct word_injector<stream_endian::big_unit_big_bit<UnitBits>, WordBits, BlockWords, BlockBits>
                : public basic_functions<WordBits> {

                constexpr static const std::size_t word_bits = basic_functions<WordBits>::word_bits;
                typedef typename basic_functions<WordBits>::word_type word_type;

                typedef std::array<word_type, BlockWords> block_type;

                static void inject(word_type w, std::size_t word_seen, block_type &b, std::size_t &block_seen) {
                    // Insert word_seen-bit part of word into the block b according to endianness

                    // Check whether we fall out of the block
                    if (block_seen + word_seen <= BlockBits) {
                        std::size_t last_word_ind = block_seen / word_bits;
                        std::size_t last_word_seen = block_seen % word_bits;

                        // Remove garbage
                        w &= high_bits<word_bits>(~word_type(), word_seen);
                        b[last_word_ind] &= high_bits<word_bits>(~word_type(), last_word_seen);

                        // Add significant word bits to block word
                        b[last_word_ind] |= unbounded_shr(w, last_word_seen);

                        // If we fall out of the block word, push the remainder of element to the next block word
                        if (last_word_seen + word_seen > word_bits)
                            b[last_word_ind + 1] = unbounded_shl(w, word_bits - last_word_seen);

                        block_seen += word_seen;
                    }
                }
            };

            template<int UnitBits, std::size_t WordBits, std::size_t BlockWords, std::size_t BlockBits>
            struct word_injector<stream_endian::little_unit_big_bit<UnitBits>, WordBits, BlockWords, BlockBits>
                : public basic_functions<WordBits> {

                constexpr static const std::size_t word_bits = basic_functions<WordBits>::word_bits;
                typedef typename basic_functions<WordBits>::word_type word_type;

                typedef std::array<word_type, BlockWords> block_type;

                static void inject(word_type w, std::size_t word_seen, block_type &b, std::size_t &block_seen) {
                    // Insert word_seen-bit part of word into the block b according to endianness

                    // Check whether we fall out of the block
                    if (block_seen + word_seen <= BlockBits) {
                        std::size_t last_word_ind = block_seen / word_bits;
                        std::size_t last_word_seen = block_seen % word_bits;

                        // Remove garbage
                        std::size_t w_rem = word_seen % UnitBits;
                        std::size_t w_unit_bits = word_seen - w_rem;
                        word_type mask =
                            low_bits<word_bits>(~word_type(), w_unit_bits) |
                            unbounded_shl(low_bits<word_bits>(~word_type(), w_rem), w_unit_bits + UnitBits - w_rem);
                        w &= mask;

                        std::size_t b_rem = last_word_seen % UnitBits;
                        std::size_t b_unit_bits = last_word_seen - b_rem;
                        mask = low_bits<word_bits>(~word_type(), b_unit_bits) |
                               unbounded_shl(low_bits<word_bits>(~word_type(), b_rem), b_unit_bits + UnitBits - b_rem);
                        b[last_word_ind] &= mask;

                        // Split and combine parts of unit values
                        std::size_t sz[2] = {UnitBits - b_rem, b_rem};
                        word_type masks[2];
                        masks[0] = unbounded_shl(low_bits<word_bits>(~word_type(), UnitBits - b_rem), b_rem);
                        masks[1] = low_bits<word_bits>(~word_type(), b_rem);
                        std::size_t bw_space = word_bits - last_word_seen;
                        std::size_t w_space = word_seen;
                        word_type w_split = 0;
                        std::size_t sz_ind = 0;

                        while (bw_space && w_space) {
                            w_split |= (!sz_ind ? unbounded_shr(w & masks[0], b_rem) :
                                                  unbounded_shl(w & masks[1], UnitBits + sz[0]));
                            bw_space -= sz[sz_ind];
                            w_space -= (w_space >= sz[sz_ind]) ? sz[sz_ind] : w_space;
                            masks[sz_ind] = unbounded_shl(masks[sz_ind], UnitBits);
                            sz_ind = 1 - sz_ind;
                        }

                        // Add significant word bits to block word
                        b[last_word_ind] |= unbounded_shl(w_split, b_unit_bits);

                        // If we fall out of the block word, push the remainder of element to the next block word
                        if (last_word_seen + word_seen > word_bits) {
                            w = unbounded_shr(w, word_bits - b_unit_bits - UnitBits);
                            w_split = 0;
                            masks[0] =
                                unbounded_shl(low_bits<word_bits>(~word_type(), UnitBits - b_rem), b_rem + UnitBits);
                            masks[1] = low_bits<word_bits>(~word_type(), b_rem);

                            while (w_space) {
                                w_split |= (!sz_ind ? unbounded_shr(w & masks[0], b_rem) :
                                                      unbounded_shl(w & masks[1], UnitBits + sz[0]));
                                w_space -= (w_space >= sz[sz_ind]) ? sz[sz_ind] : w_space;
                                masks[sz_ind] = unbounded_shl(masks[sz_ind], UnitBits);
                                sz_ind = 1 - sz_ind;
                            }

                            b[last_word_ind + 1] = unbounded_shr(w_split, UnitBits);
                        }

                        block_seen += word_seen;
                    }
                }
            };

            template<int UnitBits, std::size_t WordBits, std::size_t BlockWords, std::size_t BlockBits>
            struct word_injector<stream_endian::big_unit_little_bit<UnitBits>, WordBits, BlockWords, BlockBits>
                : public basic_functions<WordBits> {

                constexpr static const std::size_t word_bits = basic_functions<WordBits>::word_bits;
                typedef typename basic_functions<WordBits>::word_type word_type;

                typedef std::array<word_type, BlockWords> block_type;

                static void inject(word_type w, std::size_t word_seen, block_type &b, std::size_t &block_seen) {
                    // Insert word_seen-bit part of word into the block b according to endianness

                    // Check whether we fall out of the block
                    if (block_seen + word_seen <= BlockBits) {
                        std::size_t last_word_ind = block_seen / word_bits;
                        std::size_t last_word_seen = block_seen % word_bits;

                        // Remove garbage
                        std::size_t w_rem = word_seen % UnitBits;
                        std::size_t w_unit_bits = word_seen - w_rem;
                        word_type mask =
                            high_bits<word_bits>(~word_type(), w_unit_bits) |
                            unbounded_shr(high_bits<word_bits>(~word_type(), w_rem), w_unit_bits + UnitBits - w_rem);
                        w &= mask;
                        std::size_t b_rem = last_word_seen % UnitBits;
                        std::size_t b_unit_bits = last_word_seen - b_rem;
                        mask = high_bits<word_bits>(~word_type(), b_unit_bits) |
                               unbounded_shr(high_bits<word_bits>(~word_type(), b_rem), b_unit_bits + UnitBits - b_rem);
                        b[last_word_ind] &= mask;

                        // Split and combine parts of unit values
                        std::size_t sz[2] = {UnitBits - b_rem, b_rem};
                        word_type masks[2] = {
                            unbounded_shr(high_bits<word_bits>(~word_type(), UnitBits - b_rem), b_rem),
                            high_bits<word_bits>(~word_type(), b_rem)};
                        std::size_t bw_space = word_bits - last_word_seen;
                        std::size_t w_space = word_seen;
                        word_type w_split = 0;
                        std::size_t sz_ind = 0;

                        while (bw_space && w_space) {
                            w_split |= (!sz_ind ? unbounded_shl(w & masks[0], b_rem) :
                                                  unbounded_shr(w & masks[1], UnitBits + sz[0]));
                            bw_space -= sz[sz_ind];
                            w_space -= (w_space >= sz[sz_ind]) ? sz[sz_ind] : w_space;
                            masks[sz_ind] = unbounded_shr(masks[sz_ind], UnitBits);
                            sz_ind = 1 - sz_ind;
                        }

                        // Add significant word bits to block word
                        b[last_word_ind] |= unbounded_shr(w_split, b_unit_bits);

                        // If we fall out of the block word, push the remainder of element to the next block word
                        if (last_word_seen + word_seen > word_bits) {
                            w = unbounded_shl(w, word_bits - b_unit_bits - UnitBits);
                            w_split = 0;
                            masks[0] =
                                unbounded_shr(high_bits<word_bits>(~word_type(), UnitBits - b_rem), b_rem + UnitBits);
                            masks[1] = high_bits<word_bits>(~word_type(), b_rem);

                            while (w_space) {
                                w_split |= (!sz_ind ? unbounded_shl(w & masks[0], b_rem) :
                                                      unbounded_shr(w & masks[1], UnitBits + sz[0]));
                                w_space -= (w_space >= sz[sz_ind]) ? sz[sz_ind] : w_space;
                                masks[sz_ind] = unbounded_shr(masks[sz_ind], UnitBits);
                                sz_ind = 1 - sz_ind;
                            }

                            b[last_word_ind + 1] = unbounded_shl(w_split, UnitBits);
                        }
                        block_seen += word_seen;
                    }
                }
            };

            template<int UnitBits, std::size_t WordBits, std::size_t BlockWords, std::size_t BlockBits>
            struct word_injector<stream_endian::little_unit_little_bit<UnitBits>, WordBits, BlockWords, BlockBits>
                : public basic_functions<WordBits> {

                constexpr static const std::size_t word_bits = basic_functions<WordBits>::word_bits;
                typedef typename basic_functions<WordBits>::word_type word_type;

                typedef std::array<word_type, BlockWords> block_type;

                static void inject(word_type w, std::size_t word_seen, block_type &b, std::size_t &block_seen) {
                    // Insert word_seen-bit part of word into the block b according to endianness

                    // Check whether we fall out of the block
                    if (block_seen + word_seen <= BlockBits) {
                        std::size_t last_word_ind = block_seen / word_bits;
                        std::size_t last_word_seen = block_seen % word_bits;

                        // Remove garbage
                        w &= low_bits<word_bits>(~word_type(), word_seen);
                        b[last_word_ind] &= low_bits<word_bits>(~word_type(), last_word_seen);

                        // Add significant word bits to block word
                        b[last_word_ind] |= unbounded_shl(w, last_word_seen);

                        // If we fall out of the block word, push the remainder of element to the next block word
                        if (last_word_seen + word_seen > word_bits)
                            b[last_word_ind + 1] = unbounded_shr(w, word_bits - last_word_seen);

                        block_seen += word_seen;
                    }
                }
            };

            template<typename Endianness, std::size_t WordBits, std::size_t BlockWords, std::size_t BlockBits>
            struct injector : word_injector<Endianness, WordBits, BlockWords, BlockBits> {

                constexpr static const std::size_t word_bits = basic_functions<WordBits>::word_bits;
                typedef typename basic_functions<WordBits>::word_type word_type;

                typedef std::array<word_type, BlockWords> block_type;

                static void inject(const block_type &b_src, std::size_t b_src_seen, block_type &b_dst,
                                   std::size_t &b_dst_seen, std::size_t block_shift = 0) {
                    // Insert word_seen-bit part of word into the block b according to endianness

                    // Check whether we fall out of the block
                    if (b_src_seen + b_dst_seen <= BlockBits) {

                        std::size_t first_word_ind = block_shift / word_bits;
                        std::size_t word_shift = block_shift % word_bits;

                        std::size_t first_word_seen =
                            (word_bits - word_shift) > b_src_seen ? b_src_seen : (word_bits - word_shift);

                        inject(b_src[first_word_ind], first_word_seen, b_dst, b_dst_seen, word_shift);

                        b_src_seen -= first_word_seen;

                        for (std::size_t i = 0; i < (b_src_seen / word_bits); i++) {
                            inject(b_src[first_word_ind + 1 + i], word_bits, b_dst, b_dst_seen);
                        }

                        if (b_src_seen % word_bits) {
                            inject(b_src[first_word_ind + 1 + b_src_seen / word_bits], b_src_seen % word_bits, b_dst,
                                   b_dst_seen);
                        }
                    }
                }

                static void inject(word_type w, std::size_t word_seen, block_type &b, std::size_t &block_seen,
                                   std::size_t word_shift = 0) {

                    word_type word_shifted = w;

                    if (word_shift > 0) {
                        endian_shift<Endianness, word_bits>::to_msb(word_shifted, word_shift);
                    }

                    word_injector<Endianness, WordBits, BlockWords, BlockBits>::inject(word_shifted, word_seen, b,
                                                                                block_seen);
                }
            };
        }    // namespace detail
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_INJECT_HASH_HPP