/**
 * PathSetThreadPool.cpp
 *
 *  Created on: Feb 18, 2014
 *      Author: redheli
 */

#include "PathSetThreadPool.hpp"
#include "Path.hpp"
#include "geospatial/streetdir/A_StarShortestTravelTimePathImpl.hpp"
#include "geospatial/streetdir/A_StarShortestPathImpl.hpp"
#include "logging/Log.hpp"

#include <pthread.h>
#include <semaphore.h>
#include <cstdlib>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <iterator>
#include <algorithm>
#include <vector>

using namespace std;

namespace
{
    //sim_mob::BasicLogger & logger = sim_mob::Logger::log("pathset.log");
}

sim_mob::PathSetWorkerThread::PathSetWorkerThread() :
        path(NULL), linkLookup(NULL), fromNode(NULL), toNode(NULL), hasPath(false), timeBased(false), dbgStr(std::string()), graph(NULL)
{
}

sim_mob::PathSetWorkerThread::~PathSetWorkerThread() { }

//1.Create Blacklist
//2.clear the shortestWayPointpath
//3.Populate a vector<WayPoint> with a blacklist involved
//  or
//  Populate a vector<WayPoint> without blacklist involvement
//4.populate a singlepath instance
void sim_mob::PathSetWorkerThread::run()
{
    //Convert the blacklist into a list of blocked Vertices.
    std::set<StreetDirectory::Edge> blacklistEdges;
    std::map<const Link*, std::set<StreetDirectory::Edge> >::const_iterator lookIt;
    for (std::set<const Link*>::iterator it = excludedLinks.begin(); it != excludedLinks.end(); it++)
    {
        lookIt = linkLookup->find(*it);
        if (lookIt != linkLookup->end())
        {
            blacklistEdges.insert(lookIt->second.begin(), lookIt->second.end());
        }
    }

    //used for error checking and validation
    std::pair<StreetDirectory::Edge, bool> dbgPrevEdge;
    //output container
    vector<WayPoint> wps;

    if (blacklistEdges.empty())
    {
        if(timeBased)
        {
            std::list<StreetDirectory::Vertex> partialRes;
            //Use A* to search for a path
            //Taken from: http://www.boost.org/doc/libs/1_38_0/libs/graph/example/astar-cities.cpp
            vector<StreetDirectory::Vertex> p(boost::num_vertices(*graph)); //Output variable
            vector<double> d(boost::num_vertices(*graph)); //Output variable
            try
            {
                boost::astar_search(*graph, fromVertex, sim_mob::A_StarShortestTravelTimePathImpl::DistanceHeuristicGraph(graph, toVertex),
                                    boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(sim_mob::A_StarShortestTravelTimePathImpl::GoalVisitor(toVertex)));
            }
            catch (sim_mob::A_StarShortestTravelTimePathImpl::Goal& goal)
            {
                //Build backwards.
                for (StreetDirectory::Vertex v = toVertex;; v = p[v])
                {
                    partialRes.push_front(v);
                    if (p[v] == v)
                    {
                        break;
                    }
                }
                //Now build forwards.
                std::list<StreetDirectory::Vertex>::const_iterator prev = partialRes.end();
                for (std::list<StreetDirectory::Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++)
                {
                    //Add this edge.
                    if (prev != partialRes.end())
                    {
                        //This shouldn't fail.
                        std::pair < StreetDirectory::Edge, bool> edge = boost::edge(*prev, *it, *graph);
                        if (!edge.second)
                        {
                            Warn() << "ERROR: Boost can't find an edge that it should know about." << std::endl;
                        }
                        //Retrieve, add this edge's WayPoint.
                        WayPoint wp = boost::get(boost::edge_name, *graph, edge.first);
                        wps.push_back(wp);
                    }
                    //Save for later.
                    prev = it;
                }
            }
        }
        else
        {
            std::list<StreetDirectory::Vertex> partialRes;
            //Use A* to search for a path
            //Taken from: http://www.boost.org/doc/libs/1_38_0/libs/graph/example/astar-cities.cpp
            vector<StreetDirectory::Vertex> p(boost::num_vertices(*graph)); //Output variable
            vector<double> d(boost::num_vertices(*graph)); //Output variable
            try
            {
                boost::astar_search(*graph, fromVertex, sim_mob::A_StarShortestPathImpl::DistanceHeuristicGraph(graph, toVertex),
                                    boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(sim_mob::A_StarShortestPathImpl::GoalVisitor(toVertex)));
            }
            catch (sim_mob::A_StarShortestPathImpl::Goal& goal)
            {
                //Build backwards.
                for (StreetDirectory::Vertex v = toVertex;; v = p[v])
                {
                    partialRes.push_front(v);
                    if (p[v] == v)
                    {
                        break;
                    }
                }
                //Now build forwards.
                std::list<StreetDirectory::Vertex>::const_iterator prev = partialRes.end();
                for (std::list<StreetDirectory::Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++)
                {
                    //Add this edge.
                    if (prev != partialRes.end())
                    {
                        //This shouldn't fail.
                        std::pair < StreetDirectory::Edge, bool> edge = boost::edge(*prev, *it, *graph);
                        if (!edge.second)
                        {
                            Warn() << "ERROR: Boost can't find an edge that it should know about." << std::endl;
                        }
                        //Retrieve, add this edge's WayPoint.
                        WayPoint wp = boost::get(boost::edge_name, *graph, edge.first);
                        wps.push_back(wp);
                    }
                    //Save for later.
                    prev = it;
                }
            }
        }
    }
    else
    {
        if (timeBased)
        {
            //logger << "Blacklist NOT empty" << blacklistV.size() << std::endl;
            //Filter it.

            sim_mob::A_StarShortestTravelTimePathImpl::BlackListEdgeConstraint filter(blacklistEdges);
            boost::filtered_graph<StreetDirectory::Graph, sim_mob::A_StarShortestTravelTimePathImpl::BlackListEdgeConstraint> filtered(*graph, filter);
            ////////////////////////////////////////
            // TODO: This code is copied (since filtered_graph is not the same as adjacency_list) from searchShortestPath.
            ////////////////////////////////////////
            std::list<StreetDirectory::Vertex> partialRes;

            vector<StreetDirectory::Vertex> p(boost::num_vertices(filtered)); //Output variable
            vector<double> d(boost::num_vertices(filtered)); //Output variable

            //Use A* to search for a path
            //Taken from: http://www.boost.org/doc/libs/1_38_0/libs/graph/example/astar-cities.cpp
            //...which is available under the terms of the Boost Software License, 1.0
            try
            {
                if(!p.empty() && !d.empty())
                {
                    boost::astar_search(filtered, fromVertex, sim_mob::A_StarShortestTravelTimePathImpl::DistanceHeuristicFiltered(&filtered, toVertex),
                                        boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(sim_mob::A_StarShortestTravelTimePathImpl::GoalVisitor(toVertex)));
                }
            }
            catch (sim_mob::A_StarShortestTravelTimePathImpl::Goal& goal)
            {
                //Build backwards.
                for (StreetDirectory::Vertex v = toVertex;; v = p[v])
                {
                    partialRes.push_front(v);
                    if (p[v] == v)
                    {
                        break;
                    }
                }
                //Now build forwards.
                std::list<StreetDirectory::Vertex>::const_iterator prev = partialRes.end();
                for (std::list<StreetDirectory::Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++)
                {
                    //Add this edge.
                    if (prev != partialRes.end())
                    {
                        //This shouldn't fail.
                        std::pair < StreetDirectory::Edge, bool> edge = boost::edge(*prev, *it, filtered);
                        if (!edge.second)
                        {
                            std::cerr << "ERROR: Boost can't find an edge that it should know about." << std::endl;
                        }
                        //Retrieve, add this edge's WayPoint.
                        WayPoint wp = boost::get(boost::edge_name, filtered, edge.first);
                        wps.push_back(wp);
                    }

                    //Save for later.
                    prev = it;
                }
            } //catch
        }
        else
        {
            //logger << "Blacklist NOT empty" << blacklistV.size() << std::endl;
            //Filter it.
            sim_mob::A_StarShortestPathImpl::BlackListEdgeConstraint filter(blacklistEdges);
            boost::filtered_graph<StreetDirectory::Graph, sim_mob::A_StarShortestPathImpl::BlackListEdgeConstraint> filtered(*graph, filter);
            ////////////////////////////////////////
            // TODO: This code is copied (since filtered_graph is not the same as adjacency_list) from searchShortestPath.
            ////////////////////////////////////////
            std::list<StreetDirectory::Vertex> partialRes;

            vector<StreetDirectory::Vertex> p(boost::num_vertices(filtered)); //Output variable
            vector<double> d(boost::num_vertices(filtered)); //Output variable

            //Use A* to search for a path
            //Taken from: http://www.boost.org/doc/libs/1_38_0/libs/graph/example/astar-cities.cpp
            //...which is available under the terms of the Boost Software License, 1.0
            try
            {
                if(!p.empty() && !d.empty())
                {
                    boost::astar_search(filtered, fromVertex, sim_mob::A_StarShortestPathImpl::DistanceHeuristicFiltered(&filtered, toVertex),
                                        boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(sim_mob::A_StarShortestPathImpl::GoalVisitor(toVertex)));
                }
            }
            catch (sim_mob::A_StarShortestPathImpl::Goal& goal)
            {
                //Build backwards.
                for (StreetDirectory::Vertex v = toVertex;; v = p[v])
                {
                    partialRes.push_front(v);
                    if (p[v] == v)
                    {
                        break;
                    }
                }
                //Now build forwards.
                std::list<StreetDirectory::Vertex>::const_iterator prev = partialRes.end();
                for (std::list<StreetDirectory::Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++)
                {
                    //Add this edge.
                    if (prev != partialRes.end())
                    {
                        //This shouldn't fail.
                        std::pair < StreetDirectory::Edge, bool> edge = boost::edge(*prev, *it, filtered);
                        if (!edge.second)
                        {
                            std::cerr << "ERROR: Boost can't find an edge that it should know about." << std::endl;
                        }
                        //Retrieve, add this edge's WayPoint.
                        WayPoint wp = boost::get(boost::edge_name, filtered, edge.first);
                        wps.push_back(wp);
                    }

                    //Save for later.
                    prev = it;
                }
            }
        }
    }

    if (wps.empty())
    {
        hasPath = false;
    }
    else
    {
        // make sp id
        std::string id = sim_mob::makePathString(wps);
        if (id.empty())
        {
            hasPath = false;
        }
        else
        {
            path = new sim_mob::SinglePath();
            // fill data
            path->isNeedSave2DB = true;
            hasPath = true;
            path->pathSetId = pathSet->id;
            path->scenario = pathSet->scenario + dbgStr;
            path->init(wps);
            path->id = id;
            path->pathSize = 0;
            if (this->path->path.begin()->link->getFromNodeId() != this->pathSet->subTrip.origin.node->getNodeId())
            {
                safe_delete_item(path);
                hasPath = false;
            }
        }
    }
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock. 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 BOOST_MULTIPRECISION_MPC_HPP
#define BOOST_MULTIPRECISION_MPC_HPP

#include <boost/multiprecision/number.hpp>
#include <boost/cstdint.hpp>
#include <boost/multiprecision/detail/digits.hpp>
#include <boost/multiprecision/traits/is_variable_precision.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/logged_adaptor.hpp>
#include <boost/functional/hash_fwd.hpp>
#include <mpc.h>
#include <cmath>
#include <algorithm>
#include <complex>

#ifndef BOOST_MULTIPRECISION_MPFI_DEFAULT_PRECISION
#  define BOOST_MULTIPRECISION_MPFI_DEFAULT_PRECISION 20
#endif

namespace boost{
namespace multiprecision{
namespace backends{

template <unsigned digits10>
struct mpc_complex_backend;

} // namespace backends

template <unsigned digits10>
struct number_category<backends::mpc_complex_backend<digits10> > : public mpl::int_<number_kind_complex>{};

namespace backends{

namespace detail{

inline void mpc_copy_precision(mpc_t dest, const mpc_t src)
{
   mpfr_prec_t p_dest = mpc_get_prec(dest);
   mpfr_prec_t p_src = mpc_get_prec(src);
   if (p_dest != p_src)
      mpc_set_prec(dest, p_src);
}
inline void mpc_copy_precision(mpc_t dest, const mpc_t src1, const mpc_t src2)
{
   mpfr_prec_t p_dest = mpc_get_prec(dest);
   mpfr_prec_t p_src1 = mpc_get_prec(src1);
   mpfr_prec_t p_src2 = mpc_get_prec(src2);
   if (p_src2 > p_src1)
      p_src1 = p_src2;
   if (p_dest != p_src1)
      mpc_set_prec(dest, p_src1);
}

template <unsigned digits10>
struct mpc_complex_imp
{
#ifdef BOOST_HAS_LONG_LONG
   typedef mpl::list<long, boost::long_long_type>                     signed_types;
   typedef mpl::list<unsigned long, boost::ulong_long_type>   unsigned_types;
#else
   typedef mpl::list<long>                                signed_types;
   typedef mpl::list<unsigned long>                       unsigned_types;
#endif
   typedef mpl::list<double, long double>                 float_types;
   typedef long                                           exponent_type;

   mpc_complex_imp()
   {
      mpc_init2(m_data, multiprecision::detail::digits10_2_2(digits10 ? digits10 : get_default_precision()));
      mpc_set_ui(m_data, 0u, GMP_RNDN);
   }
   mpc_complex_imp(unsigned digits2)
   {
      mpc_init2(m_data, digits2);
      mpc_set_ui(m_data, 0u, GMP_RNDN);
   }

   mpc_complex_imp(const mpc_complex_imp& o)
   {
      mpc_init2(m_data, mpc_get_prec(o.m_data));
      if(o.m_data[0].re[0]._mpfr_d)
         mpc_set(m_data, o.m_data, GMP_RNDN);
   }
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
   mpc_complex_imp(mpc_complex_imp&& o) BOOST_NOEXCEPT
   {
      m_data[0] = o.m_data[0];
      o.m_data[0].re[0]._mpfr_d = 0;
   }
#endif
   mpc_complex_imp& operator = (const mpc_complex_imp& o)
   {
      if( (o.m_data[0].re[0]._mpfr_d) && (this != &o) )
      {
         if (m_data[0].re[0]._mpfr_d == 0)
            mpc_init2(m_data, mpc_get_prec(o.m_data));
         mpc_set(m_data, o.m_data, GMP_RNDD);
      }
      return *this;
   }
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
   mpc_complex_imp& operator = (mpc_complex_imp&& o) BOOST_NOEXCEPT
   {
      mpc_swap(m_data, o.m_data);
      return *this;
   }
#endif
#ifdef BOOST_HAS_LONG_LONG
#ifdef _MPFR_H_HAVE_INTMAX_T
   mpc_complex_imp& operator = (boost::ulong_long_type i)
   {
      if(m_data[0].re[0]._mpfr_d == 0)
         mpc_init2(m_data, multiprecision::detail::digits10_2_2(digits10 ? digits10 : get_default_precision()));
      mpc_set_uj(data(), i, GMP_RNDD);
      return *this;
   }
   mpc_complex_imp& operator = (boost::long_long_type i)
   {
      if(m_data[0].re[0]._mpfr_d == 0)
         mpc_init2(m_data, multiprecision::detail::digits10_2_2(digits10 ? digits10 : get_default_precision()));
      mpc_set_sj(data(), i, GMP_RNDD);
      return *this;
   }
#else
   mpc_complex_imp& operator = (boost::ulong_long_type i)
   {
      mpfr_float_backend<digits10> f(0uL, mpc_get_prec(m_data));
      f = i;
      mpc_set_fr(this->data(), f.data(), GMP_RNDN);
      return *this;
   }
   mpc_complex_imp& operator = (boost::long_long_type i)
   {
      mpfr_float_backend<digits10> f(0uL, mpc_get_prec(m_data));
      f = i;
      mpc_set_fr(this->data(), f.data(), GMP_RNDN);
      return *this;
   }
#endif
#endif
   mpc_complex_imp& operator = (unsigned long i)
   {
      if(m_data[0].re[0]._mpfr_d == 0)
         mpc_init2(m_data, multiprecision::detail::digits10_2_2(digits10 ? digits10 : get_default_precision()));
      mpc_set_ui(m_data, i, GMP_RNDN);
      return *this;
   }
   mpc_complex_imp& operator = (long i)
   {
      if(m_data[0].re[0]._mpfr_d == 0)
         mpc_init2(m_data, multiprecision::detail::digits10_2_2(digits10 ? digits10 : get_default_precision()));
      mpc_set_si(m_data, i, GMP_RNDN);
      return *this;
   }
   mpc_complex_imp& operator = (double d)
   {
      if(m_data[0].re[0]._mpfr_d == 0)
         mpc_init2(m_data, multiprecision::detail::digits10_2_2(digits10 ? digits10 : get_default_precision()));
      mpc_set_d(m_data, d, GMP_RNDN);
      return *this;
   }
   mpc_complex_imp& operator = (long double d)
   {
      if (m_data[0].re[0]._mpfr_d == 0)
         mpc_init2(m_data, multiprecision::detail::digits10_2_2(digits10 ? digits10 : get_default_precision()));
      mpc_set_ld(m_data, d, GMP_RNDN);
      return *this;
   }
   mpc_complex_imp& operator = (mpz_t i)
   {
      if (m_data[0].re[0]._mpfr_d == 0)
         mpc_init2(m_data, multiprecision::detail::digits10_2_2(digits10 ? digits10 : get_default_precision()));
      mpc_set_z(m_data, i, GMP_RNDN);
      return *this;
   }
   mpc_complex_imp& operator = (gmp_int i)
   {
      if (m_data[0].re[0]._mpfr_d == 0)
         mpc_init2(m_data, multiprecision::detail::digits10_2_2(digits10 ? digits10 : get_default_precision()));
      mpc_set_z(m_data, i.data(), GMP_RNDN);
      return *this;
   }

   mpc_complex_imp& operator = (const char* s)
   {
      using default_ops::eval_fpclassify;

      if(m_data[0].re[0]._mpfr_d == 0)
         mpc_init2(m_data, multiprecision::detail::digits10_2_2(digits10 ? digits10 : get_default_precision()));

      mpfr_float_backend<digits10> a(0uL, mpc_get_prec(m_data)), b(0uL, mpc_get_prec(m_data));

      if(s && (*s == '('))
      {
         std::string part;
         const char* p = ++s;
         while(*p && (*p != ',') && (*p != ')'))
            ++p;
         part.assign(s, p);
         if(part.size())
            a = part.c_str();
         else
            a = 0uL;
         s = p;
         if(*p && (*p != ')'))
         {
            ++p;
            while(*p && (*p != ')'))
               ++p;
            part.assign(s + 1, p);
         }
         else
            part.erase();
         if(part.size())
            b = part.c_str();
         else
            b = 0uL;
      }
      else
      {
         a = s;
         b = 0uL;
      }

      if(eval_fpclassify(a) == (int)FP_NAN)
      {
         mpc_set_fr(this->data(), a.data(), GMP_RNDN);
      }
      else if(eval_fpclassify(b) == (int)FP_NAN)
      {
         mpc_set_fr(this->data(), b.data(), GMP_RNDN);
      }
      else
      {
         mpc_set_fr_fr(m_data, a.data(), b.data(), GMP_RNDN);
      }
      return *this;
   }
   void swap(mpc_complex_imp& o) BOOST_NOEXCEPT
   {
      mpc_swap(m_data, o.m_data);
   }
   std::string str(std::streamsize digits, std::ios_base::fmtflags f)const
   {
      BOOST_ASSERT(m_data[0].re[0]._mpfr_d);

      mpfr_float_backend<digits10> a(0uL, mpc_get_prec(m_data)), b(0uL, mpc_get_prec(m_data));

      mpc_real(a.data(), m_data, GMP_RNDD);
      mpc_imag(b.data(), m_data, GMP_RNDD);

      if(eval_is_zero(b))
         return a.str(digits, f);

      return "(" + a.str(digits, f) + "," + b.str(digits, f) + ")";
   }
   ~mpc_complex_imp() BOOST_NOEXCEPT
   {
      if(m_data[0].re[0]._mpfr_d)
         mpc_clear(m_data);
   }
   void negate() BOOST_NOEXCEPT
   {
      BOOST_ASSERT(m_data[0].re[0]._mpfr_d);
      mpc_neg(m_data, m_data, GMP_RNDD);
   }
   int compare(const mpc_complex_imp& o)const BOOST_NOEXCEPT
   {
      BOOST_ASSERT(m_data[0].re[0]._mpfr_d && o.m_data[0].re[0]._mpfr_d);
      return mpc_cmp(m_data, o.m_data);
   }
   int compare(const mpc_complex_backend<digits10>& o)const BOOST_NOEXCEPT
   {
      BOOST_ASSERT(m_data[0].re[0]._mpfr_d && o.m_data[0].re[0]._mpfr_d);
      return mpc_cmp(m_data, o.data());
   }
   int compare(long int i)const BOOST_NOEXCEPT
   {
      BOOST_ASSERT(m_data[0].re[0]._mpfr_d);
      return mpc_cmp_si(m_data, i);
   }
   int compare(unsigned long int i)const BOOST_NOEXCEPT
   {
      BOOST_ASSERT(m_data[0].re[0]._mpfr_d);
      static const unsigned long int max_val = (std::numeric_limits<long>::max)();
      if (i > max_val)
      {
         mpc_complex_imp d(mpc_get_prec(m_data));
         d = i;
         return compare(d);
      }
      return mpc_cmp_si(m_data, (long)i);
   }
   template <class V>
   int compare(const V& v)const BOOST_NOEXCEPT
   {
      mpc_complex_imp d(mpc_get_prec(m_data));
      d = v;
      return compare(d);
   }
   mpc_t& data() BOOST_NOEXCEPT
   {
      BOOST_ASSERT(m_data[0].re[0]._mpfr_d);
      return m_data;
   }
   const mpc_t& data()const BOOST_NOEXCEPT
   {
      BOOST_ASSERT(m_data[0].re[0]._mpfr_d);
      return m_data;
   }
protected:
   mpc_t m_data;
   static unsigned& get_default_precision() BOOST_NOEXCEPT
   {
      static unsigned val = BOOST_MULTIPRECISION_MPFI_DEFAULT_PRECISION;
      return val;
   }
};

} // namespace detail

template <unsigned digits10>
struct mpc_complex_backend : public detail::mpc_complex_imp<digits10>
{
   mpc_complex_backend() : detail::mpc_complex_imp<digits10>() {}
   mpc_complex_backend(const mpc_complex_backend& o) : detail::mpc_complex_imp<digits10>(o) {}
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
   mpc_complex_backend(mpc_complex_backend&& o) : detail::mpc_complex_imp<digits10>(static_cast<detail::mpc_complex_imp<digits10>&&>(o)) {}
#endif
   template <unsigned D>
   mpc_complex_backend(const mpc_complex_backend<D>& val, typename enable_if_c<D <= digits10>::type* = 0)
       : detail::mpc_complex_imp<digits10>()
   {
      mpc_set(this->m_data, val.data(), GMP_RNDN);
   }
   template <unsigned D>
   explicit mpc_complex_backend(const mpc_complex_backend<D>& val, typename disable_if_c<D <= digits10>::type* = 0)
       : detail::mpc_complex_imp<digits10>()
   {
      mpc_set(this->m_data, val.data(), GMP_RNDN);
   }
   template <unsigned D>
   mpc_complex_backend(const mpfr_float_backend<D>& val, typename enable_if_c<D <= digits10>::type* = 0)
       : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_fr(this->m_data, val.data(), GMP_RNDN);
   }
   template <unsigned D>
   explicit mpc_complex_backend(const mpfr_float_backend<D>& val, typename disable_if_c<D <= digits10>::type* = 0)
       : detail::mpc_complex_imp<digits10>()
   {
      mpc_set(this->m_data, val.data(), GMP_RNDN);
   }
   mpc_complex_backend(const mpc_t val)
       : detail::mpc_complex_imp<digits10>()
   {
      mpc_set(this->m_data, val, GMP_RNDN);
   }
   mpc_complex_backend(const std::complex<float>& val)
       : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_d_d(this->m_data, val.real(), val.imag(), GMP_RNDN);
   }
   mpc_complex_backend(const std::complex<double>& val)
       : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_d_d(this->m_data, val.real(), val.imag(), GMP_RNDN);
   }
   mpc_complex_backend(const std::complex<long double>& val)
       : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_ld_ld(this->m_data, val.real(), val.imag(), GMP_RNDN);
   }
   mpc_complex_backend(mpz_srcptr val) : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_z(this->m_data, val, GMP_RNDN);
   }
   mpc_complex_backend& operator=(mpz_srcptr val)
   {
      mpc_set_z(this->m_data, val, GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(gmp_int const& val) : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_z(this->m_data, val.data(), GMP_RNDN);
   }
   mpc_complex_backend& operator=(gmp_int const& val)
   {
      mpc_set_z(this->m_data, val.data(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(mpf_srcptr val) : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_f(this->m_data, val, GMP_RNDN);
   }
   mpc_complex_backend& operator=(mpf_srcptr val)
   {
      mpc_set_f(this->m_data, val, GMP_RNDN);
      return *this;
   }
   template <unsigned D10>
   mpc_complex_backend(gmp_float<D10> const& val) : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_f(this->m_data, val.data(), GMP_RNDN);
   }
   template <unsigned D10>
   mpc_complex_backend& operator=(gmp_float<D10> const& val)
   {
      mpc_set_f(this->m_data, val.data(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(mpq_srcptr val) : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_q(this->m_data, val, GMP_RNDN);
   }
   mpc_complex_backend& operator=(mpq_srcptr val)
   {
      mpc_set_q(this->m_data, val, GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(gmp_rational const& val) : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_q(this->m_data, val.data(), GMP_RNDN);
   }
   mpc_complex_backend& operator=(gmp_rational const& val)
   {
      mpc_set_q(this->m_data, val.data(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(mpfr_srcptr val) : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_fr(this->m_data, val, GMP_RNDN);
   }
   mpc_complex_backend& operator=(mpfr_srcptr val)
   {
      mpc_set_fr(this->m_data, val, GMP_RNDN);
      return *this;
   }
   template <unsigned D10, mpfr_allocation_type AllocationType>
   mpc_complex_backend(mpfr_float_backend<D10, AllocationType> const& val) : detail::mpc_complex_imp<digits10>()
   {
      mpc_set_fr(this->m_data, val.data(), GMP_RNDN);
   }
   template <unsigned D10, mpfr_allocation_type AllocationType>
   mpc_complex_backend& operator=(mpfr_float_backend<D10, AllocationType> const& val)
   {
      mpc_set_fr(this->m_data, val.data(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend& operator=(const mpc_complex_backend& o)
   {
      *static_cast<detail::mpc_complex_imp<digits10>*>(this) = static_cast<detail::mpc_complex_imp<digits10> const&>(o);
      return *this;
   }
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
   mpc_complex_backend& operator=(mpc_complex_backend&& o) BOOST_NOEXCEPT
   {
      *static_cast<detail::mpc_complex_imp<digits10>*>(this) = static_cast<detail::mpc_complex_imp<digits10>&&>(o);
      return *this;
   }
#endif
   template <class V>
   mpc_complex_backend& operator=(const V& v)
   {
      *static_cast<detail::mpc_complex_imp<digits10>*>(this) = v;
      return *this;
   }
   mpc_complex_backend& operator=(const mpc_t val)
   {
      mpc_set(this->m_data, val, GMP_RNDN);
      return *this;
   }
   mpc_complex_backend& operator=(const std::complex<float>& val)
   {
      mpc_set_d_d(this->m_data, val.real(), val.imag(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend& operator=(const std::complex<double>& val)
   {
      mpc_set_d_d(this->m_data, val.real(), val.imag(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend& operator=(const std::complex<long double>& val)
   {
      mpc_set_ld_ld(this->m_data, val.real(), val.imag(), GMP_RNDN);
      return *this;
   }
   // We don't change our precision here, this is a fixed precision type:
   template <unsigned D>
   mpc_complex_backend& operator=(const mpc_complex_backend<D>& val)
   {
      mpc_set(this->m_data, val.data(), GMP_RNDN);
      return *this;
   }
};

template <>
struct mpc_complex_backend<0> : public detail::mpc_complex_imp<0>
{
   mpc_complex_backend() : detail::mpc_complex_imp<0>() {}
   mpc_complex_backend(const mpc_t val)
      : detail::mpc_complex_imp<0>(mpc_get_prec(val))
   {
      mpc_set(this->m_data, val, GMP_RNDN);
   }
   mpc_complex_backend(const mpc_complex_backend& o) : detail::mpc_complex_imp<0>(o) {}
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
   mpc_complex_backend(mpc_complex_backend&& o) BOOST_NOEXCEPT : detail::mpc_complex_imp<0>(static_cast<detail::mpc_complex_imp<0>&&>(o)) {}
#endif
   mpc_complex_backend(const mpc_complex_backend& o, unsigned digits10)
      : detail::mpc_complex_imp<0>(multiprecision::detail::digits10_2_2(digits10))
   {
      mpc_set(this->m_data, o.data(), GMP_RNDN);
   }
   template <unsigned D>
   mpc_complex_backend(const mpc_complex_backend<D>& val)
      : detail::mpc_complex_imp<0>(mpc_get_prec(val.data()))
   {
      mpc_set(this->m_data, val.data(), GMP_RNDN);
   }
   template <unsigned D>
   mpc_complex_backend(const mpfr_float_backend<D>& val)
      : detail::mpc_complex_imp<0>(mpfr_get_prec(val.data()))
   {
      mpc_set_fr(this->m_data, val.data(), GMP_RNDN);
   }
   mpc_complex_backend(mpz_srcptr val) : detail::mpc_complex_imp<0>()
   {
      mpc_set_z(this->m_data, val, GMP_RNDN);
   }
   mpc_complex_backend& operator=(mpz_srcptr val)
   {
      mpc_set_z(this->m_data, val, GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(gmp_int const& val) : detail::mpc_complex_imp<0>()
   {
      mpc_set_z(this->m_data, val.data(), GMP_RNDN);
   }
   mpc_complex_backend& operator=(gmp_int const& val)
   {
      mpc_set_z(this->m_data, val.data(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(mpf_srcptr val) : detail::mpc_complex_imp<0>((unsigned)mpf_get_prec(val))
   {
      mpc_set_f(this->m_data, val, GMP_RNDN);
   }
   mpc_complex_backend& operator=(mpf_srcptr val)
   {
      if ((mp_bitcnt_t)mpc_get_prec(data()) != mpf_get_prec(val))
      {
         mpc_complex_backend t(val);
         t.swap(*this);
      }
      else
         mpc_set_f(this->m_data, val, GMP_RNDN);
      return *this;
   }
   template <unsigned digits10>
   mpc_complex_backend(gmp_float<digits10> const& val) : detail::mpc_complex_imp<0>((unsigned)mpf_get_prec(val.data()))
   {
      mpc_set_f(this->m_data, val.data(), GMP_RNDN);
   }
   template <unsigned digits10>
   mpc_complex_backend& operator=(gmp_float<digits10> const& val)
   {
      if (mpc_get_prec(data()) != mpf_get_prec(val.data()))
      {
         mpc_complex_backend t(val);
         t.swap(*this);
      }
      else
         mpc_set_f(this->m_data, val.data(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(mpq_srcptr val) : detail::mpc_complex_imp<0>()
   {
      mpc_set_q(this->m_data, val, GMP_RNDN);
   }
   mpc_complex_backend& operator=(mpq_srcptr val)
   {
      mpc_set_q(this->m_data, val, GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(gmp_rational const& val) : detail::mpc_complex_imp<0>()
   {
      mpc_set_q(this->m_data, val.data(), GMP_RNDN);
   }
   mpc_complex_backend& operator=(gmp_rational const& val)
   {
      mpc_set_q(this->m_data, val.data(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(mpfr_srcptr val) : detail::mpc_complex_imp<0>(mpfr_get_prec(val))
   {
      mpc_set_fr(this->m_data, val, GMP_RNDN);
   }
   mpc_complex_backend& operator=(mpfr_srcptr val)
   {
      if (mpc_get_prec(data()) != mpfr_get_prec(val))
      {
         mpc_complex_backend t(val);
         t.swap(*this);
      }
      else
         mpc_set_fr(this->m_data, val, GMP_RNDN);
      return *this;
   }
   mpc_complex_backend(const std::complex<float>& val)
      : detail::mpc_complex_imp<0>()
   {
      mpc_set_d_d(this->m_data, val.real(), val.imag(), GMP_RNDN);
   }
   mpc_complex_backend(const std::complex<double>& val)
      : detail::mpc_complex_imp<0>()
   {
      mpc_set_d_d(this->m_data, val.real(), val.imag(), GMP_RNDN);
   }
   mpc_complex_backend(const std::complex<long double>& val)
      : detail::mpc_complex_imp<0>()
   {
      mpc_set_ld_ld(this->m_data, val.real(), val.imag(), GMP_RNDN);
   }
   // Construction with precision:
   template <class T, class U>
   mpc_complex_backend(const T& a, const U& b, unsigned digits10)
      : detail::mpc_complex_imp<0>(multiprecision::detail::digits10_2_2(digits10))
   {
      // We can't use assign_components here because it copies the precision of
      // a and b, not digits10....
      mpfr_float ca(a), cb(b);
      mpc_set_fr_fr(this->data(), ca.backend().data(), cb.backend().data(), GMP_RNDN);
   }
   template <unsigned N>
   mpc_complex_backend(const mpfr_float_backend<N>& a, const mpfr_float_backend<N>& b, unsigned digits10)
      : detail::mpc_complex_imp<0>(multiprecision::detail::digits10_2_2(digits10))
   {
      mpc_set_fr_fr(this->data(), a.data(), b.data(), GMP_RNDN);
   }

   mpc_complex_backend& operator=(const mpc_complex_backend& o)
   {
      if (this != &o)
      {
         detail::mpc_copy_precision(this->m_data, o.data());
         mpc_set(this->m_data, o.data(), GMP_RNDN);
      }
      return *this;
   }
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
   mpc_complex_backend& operator=(mpc_complex_backend&& o) BOOST_NOEXCEPT
   {
      *static_cast<detail::mpc_complex_imp<0>*>(this) = static_cast<detail::mpc_complex_imp<0> &&>(o);
      return *this;
   }
#endif
   template <class V>
   mpc_complex_backend& operator=(const V& v)
   {
      *static_cast<detail::mpc_complex_imp<0>*>(this) = v;
      return *this;
   }
   mpc_complex_backend& operator=(const mpc_t val)
   {
      mpc_set_prec(this->m_data, mpc_get_prec(val));
      mpc_set(this->m_data, val, GMP_RNDN);
      return *this;
   }
   template <unsigned D>
   mpc_complex_backend& operator=(const mpc_complex_backend<D>& val)
   {
      mpc_set_prec(this->m_data, mpc_get_prec(val.data()));
      mpc_set(this->m_data, val.data(), GMP_RNDN);
      return *this;
   }
   template <unsigned D>
   mpc_complex_backend& operator=(const mpfr_float_backend<D>& val)
   {
      mpc_set_prec(this->m_data, mpfr_get_prec(val.data()));
      mpc_set_fr(this->m_data, val.data(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend& operator=(const std::complex<float>& val)
   {
      mpc_set_d_d(this->m_data, val.real(), val.imag(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend& operator=(const std::complex<double>& val)
   {
      mpc_set_d_d(this->m_data, val.real(), val.imag(), GMP_RNDN);
      return *this;
   }
   mpc_complex_backend& operator=(const std::complex<long double>& val)
   {
      mpc_set_ld_ld(this->m_data, val.real(), val.imag(), GMP_RNDN);
      return *this;
   }
   static unsigned default_precision() BOOST_NOEXCEPT
   {
      return get_default_precision();
   }
   static void default_precision(unsigned v) BOOST_NOEXCEPT
   {
      get_default_precision() = v;
   }
   unsigned precision()const BOOST_NOEXCEPT
   {
      return multiprecision::detail::digits2_2_10(mpc_get_prec(this->m_data));
   }
   void precision(unsigned digits10) BOOST_NOEXCEPT
   {
      mpfr_prec_round(mpc_realref(this->m_data), multiprecision::detail::digits10_2_2((digits10)), GMP_RNDN);
      mpfr_prec_round(mpc_imagref(this->m_data), multiprecision::detail::digits10_2_2((digits10)), GMP_RNDN);
   }
};

template <unsigned digits10, class T>
inline typename enable_if<is_arithmetic<T>, bool>::type eval_eq(const mpc_complex_backend<digits10>& a, const T& b) BOOST_NOEXCEPT
{
   return a.compare(b) == 0;
}
template <unsigned digits10, class T>
inline typename enable_if<is_arithmetic<T>, bool>::type eval_lt(const mpc_complex_backend<digits10>& a, const T& b) BOOST_NOEXCEPT
{
   return a.compare(b) < 0;
}
template <unsigned digits10, class T>
inline typename enable_if<is_arithmetic<T>, bool>::type eval_gt(const mpc_complex_backend<digits10>& a, const T& b) BOOST_NOEXCEPT
{
   return a.compare(b) > 0;
}

template <unsigned D1, unsigned D2>
inline void eval_add(mpc_complex_backend<D1>& result, const mpc_complex_backend<D2>& o)
{
   mpc_add(result.data(), result.data(), o.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_add(mpc_complex_backend<D1>& result, const mpfr_float_backend<D2>& o)
{
   mpc_add_fr(result.data(), result.data(), o.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_subtract(mpc_complex_backend<D1>& result, const mpc_complex_backend<D2>& o)
{
   mpc_sub(result.data(), result.data(), o.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_subtract(mpc_complex_backend<D1>& result, const mpfr_float_backend<D2>& o)
{
   mpc_sub_fr(result.data(), result.data(), o.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_multiply(mpc_complex_backend<D1>& result, const mpc_complex_backend<D2>& o)
{
   if((void*)&result == (void*)&o)
      mpc_sqr(result.data(), o.data(), GMP_RNDN);
   else
      mpc_mul(result.data(), result.data(), o.data(), GMP_RNDN);
}
template <unsigned D1, unsigned D2>
inline void eval_multiply(mpc_complex_backend<D1>& result, const mpfr_float_backend<D2>& o)
{
   mpc_mul_fr(result.data(), result.data(), o.data(), GMP_RNDN);
}
template <unsigned D1, unsigned D2>
inline void eval_divide(mpc_complex_backend<D1>& result, const mpc_complex_backend<D2>& o)
{
   mpc_div(result.data(), result.data(), o.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_divide(mpc_complex_backend<D1>& result, const mpfr_float_backend<D2>& o)
{
   mpc_div_fr(result.data(), result.data(), o.data(), GMP_RNDD);
}
template <unsigned digits10>
inline void eval_add(mpc_complex_backend<digits10>& result, unsigned long i)
{
   mpc_add_ui(result.data(), result.data(), i, GMP_RNDN);
}
template <unsigned digits10>
inline void eval_subtract(mpc_complex_backend<digits10>& result, unsigned long i)
{
   mpc_sub_ui(result.data(), result.data(), i, GMP_RNDN);
}
template <unsigned digits10>
inline void eval_multiply(mpc_complex_backend<digits10>& result, unsigned long i)
{
   mpc_mul_ui(result.data(), result.data(), i, GMP_RNDN);
}
template <unsigned digits10>
inline void eval_divide(mpc_complex_backend<digits10>& result, unsigned long i)
{
   mpc_div_ui(result.data(), result.data(), i, GMP_RNDN);
}
template <unsigned digits10>
inline void eval_add(mpc_complex_backend<digits10>& result, long i)
{
   if(i > 0)
      mpc_add_ui(result.data(), result.data(), i, GMP_RNDN);
   else
      mpc_sub_ui(result.data(), result.data(), boost::multiprecision::detail::unsigned_abs(i), GMP_RNDN);
}
template <unsigned digits10>
inline void eval_subtract(mpc_complex_backend<digits10>& result, long i)
{
   if(i > 0)
      mpc_sub_ui(result.data(), result.data(), i, GMP_RNDN);
   else
      mpc_add_ui(result.data(), result.data(), boost::multiprecision::detail::unsigned_abs(i), GMP_RNDN);
}
template <unsigned digits10>
inline void eval_multiply(mpc_complex_backend<digits10>& result, long i)
{
   mpc_mul_ui(result.data(), result.data(), boost::multiprecision::detail::unsigned_abs(i), GMP_RNDN);
   if(i < 0)
      mpc_neg(result.data(), result.data(), GMP_RNDN);
}
template <unsigned digits10>
inline void eval_divide(mpc_complex_backend<digits10>& result, long i)
{
   mpc_div_ui(result.data(), result.data(), boost::multiprecision::detail::unsigned_abs(i), GMP_RNDN);
   if(i < 0)
      mpc_neg(result.data(), result.data(), GMP_RNDN);
}
//
// Specialised 3 arg versions of the basic operators:
//
template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_add(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, const mpc_complex_backend<D3>& y)
{
   mpc_add(a.data(), x.data(), y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_add(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, const mpfr_float_backend<D3>& y)
{
   mpc_add_fr(a.data(), x.data(), y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_add(mpc_complex_backend<D1>& a, const mpfr_float_backend<D2>& x, const mpc_complex_backend<D3>& y)
{
   mpc_add_fr(a.data(), y.data(), x.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_add(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, unsigned long y)
{
   mpc_add_ui(a.data(), x.data(), y, GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_add(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, long y)
{
   if(y < 0)
      mpc_sub_ui(a.data(), x.data(), boost::multiprecision::detail::unsigned_abs(y), GMP_RNDD);
   else
      mpc_add_ui(a.data(), x.data(), y, GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_add(mpc_complex_backend<D1>& a, unsigned long x, const mpc_complex_backend<D2>& y)
{
   mpc_add_ui(a.data(), y.data(), x, GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_add(mpc_complex_backend<D1>& a, long x, const mpc_complex_backend<D2>& y)
{
   if(x < 0)
   {
      mpc_ui_sub(a.data(), boost::multiprecision::detail::unsigned_abs(x), y.data(), GMP_RNDN);
      mpc_neg(a.data(), a.data(), GMP_RNDD);
   }
   else
      mpc_add_ui(a.data(), y.data(), x, GMP_RNDD);
}
template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_subtract(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, const mpc_complex_backend<D3>& y)
{
   mpc_sub(a.data(), x.data(), y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_subtract(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, const mpfr_float_backend<D3>& y)
{
   mpc_sub_fr(a.data(), x.data(), y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_subtract(mpc_complex_backend<D1>& a, const mpfr_float_backend<D2>& x, const mpc_complex_backend<D3>& y)
{
   mpc_fr_sub(a.data(), x.data(), y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_subtract(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, unsigned long y)
{
   mpc_sub_ui(a.data(), x.data(), y, GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_subtract(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, long y)
{
   if(y < 0)
      mpc_add_ui(a.data(), x.data(), boost::multiprecision::detail::unsigned_abs(y), GMP_RNDD);
   else
      mpc_sub_ui(a.data(), x.data(), y, GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_subtract(mpc_complex_backend<D1>& a, unsigned long x, const mpc_complex_backend<D2>& y)
{
   mpc_ui_sub(a.data(), x, y.data(), GMP_RNDN);
}
template <unsigned D1, unsigned D2>
inline void eval_subtract(mpc_complex_backend<D1>& a, long x, const mpc_complex_backend<D2>& y)
{
   if(x < 0)
   {
      mpc_add_ui(a.data(), y.data(), boost::multiprecision::detail::unsigned_abs(x), GMP_RNDD);
      mpc_neg(a.data(), a.data(), GMP_RNDD);
   }
   else
      mpc_ui_sub(a.data(), x, y.data(), GMP_RNDN);
}

template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_multiply(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, const mpc_complex_backend<D3>& y)
{
   if((void*)&x == (void*)&y)
      mpc_sqr(a.data(), x.data(), GMP_RNDD);
   else
      mpc_mul(a.data(), x.data(), y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_multiply(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, const mpfr_float_backend<D3>& y)
{
   mpc_mul_fr(a.data(), x.data(), y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_multiply(mpc_complex_backend<D1>& a, const mpfr_float_backend<D2>& x, const mpc_complex_backend<D3>& y)
{
   mpc_mul_fr(a.data(), y.data(), x.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_multiply(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, unsigned long y)
{
   mpc_mul_ui(a.data(), x.data(), y, GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_multiply(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, long y)
{
   if(y < 0)
   {
      mpc_mul_ui(a.data(), x.data(), boost::multiprecision::detail::unsigned_abs(y), GMP_RNDD);
      a.negate();
   }
   else
      mpc_mul_ui(a.data(), x.data(), y, GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_multiply(mpc_complex_backend<D1>& a, unsigned long x, const mpc_complex_backend<D2>& y)
{
   mpc_mul_ui(a.data(), y.data(), x, GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_multiply(mpc_complex_backend<D1>& a, long x, const mpc_complex_backend<D2>& y)
{
   if(x < 0)
   {
      mpc_mul_ui(a.data(), y.data(), boost::multiprecision::detail::unsigned_abs(x), GMP_RNDD);
      mpc_neg(a.data(), a.data(), GMP_RNDD);
   }
   else
      mpc_mul_ui(a.data(), y.data(), x, GMP_RNDD);
}

template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_divide(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, const mpc_complex_backend<D3>& y)
{
   mpc_div(a.data(), x.data(), y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_divide(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, const mpfr_float_backend<D3>& y)
{
   mpc_div_fr(a.data(), x.data(), y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2, unsigned D3>
inline void eval_divide(mpc_complex_backend<D1>& a, const mpfr_float_backend<D2>& x, const mpc_complex_backend<D3>& y)
{
   mpc_fr_div(a.data(), x.data(), y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_divide(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, unsigned long y)
{
   mpc_div_ui(a.data(), x.data(), y, GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_divide(mpc_complex_backend<D1>& a, const mpc_complex_backend<D2>& x, long y)
{
   if(y < 0)
   {
      mpc_div_ui(a.data(), x.data(), boost::multiprecision::detail::unsigned_abs(y), GMP_RNDD);
      a.negate();
   }
   else
      mpc_div_ui(a.data(), x.data(), y, GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_divide(mpc_complex_backend<D1>& a, unsigned long x, const mpc_complex_backend<D2>& y)
{
   mpc_ui_div(a.data(), x, y.data(), GMP_RNDD);
}
template <unsigned D1, unsigned D2>
inline void eval_divide(mpc_complex_backend<D1>& a, long x, const mpc_complex_backend<D2>& y)
{
   if(x < 0)
   {
      mpc_ui_div(a.data(), boost::multiprecision::detail::unsigned_abs(x), y.data(), GMP_RNDD);
      mpc_neg(a.data(), a.data(), GMP_RNDD);
   }
   else
      mpc_ui_div(a.data(), x, y.data(), GMP_RNDD);
}

template <unsigned digits10>
inline bool eval_is_zero(const mpc_complex_backend<digits10>& val) BOOST_NOEXCEPT
{
   return (0 != mpfr_zero_p(mpc_realref(val.data()))) && (0 != mpfr_zero_p(mpc_imagref(val.data())));
}
template <unsigned digits10>
inline int eval_get_sign(const mpc_complex_backend<digits10>&)
{
   BOOST_STATIC_ASSERT_MSG(digits10 == UINT_MAX, "Complex numbers have no sign bit."); // designed to always fail
   return 0;
}

template <unsigned digits10>
inline void eval_convert_to(unsigned long* result, const mpc_complex_backend<digits10>& val)
{
   if (0 == mpfr_zero_p(mpc_imagref(val.data())))
   {
      BOOST_THROW_EXCEPTION(std::runtime_error("Could not convert imaginary number to scalar."));
   }
   mpfr_float_backend<digits10> t;
   mpc_real(t.data(), val.data(), GMP_RNDN);
   eval_convert_to(result, t);
}
template <unsigned digits10>
inline void eval_convert_to(long* result, const mpc_complex_backend<digits10>& val)
{
   if (0 == mpfr_zero_p(mpc_imagref(val.data())))
   {
      BOOST_THROW_EXCEPTION(std::runtime_error("Could not convert imaginary number to scalar."));
   }
   mpfr_float_backend<digits10> t;
   mpc_real(t.data(), val.data(), GMP_RNDN);
   eval_convert_to(result, t);
}
#ifdef _MPFR_H_HAVE_INTMAX_T
template <unsigned digits10>
inline void eval_convert_to(boost::ulong_long_type* result, const mpc_complex_backend<digits10>& val)
{
   if (0 == mpfr_zero_p(mpc_imagref(val.data())))
   {
      BOOST_THROW_EXCEPTION(std::runtime_error("Could not convert imaginary number to scalar."));
   }
   mpfr_float_backend<digits10> t;
   mpc_real(t.data(), val.data(), GMP_RNDN);
   eval_convert_to(result, t);
}
template <unsigned digits10>
inline void eval_convert_to(boost::long_long_type* result, const mpc_complex_backend<digits10>& val)
{
   if (0 == mpfr_zero_p(mpc_imagref(val.data())))
   {
      BOOST_THROW_EXCEPTION(std::runtime_error("Could not convert imaginary number to scalar."));
   }
   mpfr_float_backend<digits10> t;
   mpc_real(t.data(), val.data(), GMP_RNDN);
   eval_convert_to(result, t);
}
#endif
template <unsigned digits10>
inline void eval_convert_to(double* result, const mpc_complex_backend<digits10>& val) BOOST_NOEXCEPT
{
   if (0 == mpfr_zero_p(mpc_imagref(val.data())))
   {
      BOOST_THROW_EXCEPTION(std::runtime_error("Could not convert imaginary number to scalar."));
   }
   mpfr_float_backend<digits10> t;
   mpc_real(t.data(), val.data(), GMP_RNDN);
   eval_convert_to(result, t);
}
template <unsigned digits10>
inline void eval_convert_to(long double* result, const mpc_complex_backend<digits10>& val) BOOST_NOEXCEPT
{
   if (0 == mpfr_zero_p(mpc_imagref(val.data())))
   {
      BOOST_THROW_EXCEPTION(std::runtime_error("Could not convert imaginary number to scalar."));
   }
   mpfr_float_backend<digits10> t;
   mpc_real(t.data(), val.data(), GMP_RNDN);
   eval_convert_to(result, t);
}

template <unsigned D1, unsigned D2, mpfr_allocation_type AllocationType>
inline void assign_components(mpc_complex_backend<D1>& result, const mpfr_float_backend<D2, AllocationType>& a, const mpfr_float_backend<D2, AllocationType>& b)
{
   //
   // This is called from class number's constructors, so if we have variable
   // precision, then copy the precision of the source variables.
   //
   if (!D1)
   {
      unsigned long prec = std::max(mpfr_get_prec(a.data()), mpfr_get_prec(b.data()));
      mpc_set_prec(result.data(), prec);
   }
   using default_ops::eval_fpclassify;
   if(eval_fpclassify(a) == (int)FP_NAN)
   {
      mpc_set_fr(result.data(), a.data(), GMP_RNDN);
   }
   else if(eval_fpclassify(b) == (int)FP_NAN)
   {
      mpc_set_fr(result.data(), b.data(), GMP_RNDN);
   }
   else
   {
      mpc_set_fr_fr(result.data(), a.data(), b.data(), GMP_RNDN);
   }
}

template <unsigned D1, unsigned D2, mpfr_allocation_type AllocationType>
inline void assign_components(mpc_complex_backend<D1>& result, unsigned long a, unsigned long b)
{
   mpc_set_ui_ui(result.data(), a, b, GMP_RNDN);
}

template <unsigned D1, unsigned D2, mpfr_allocation_type AllocationType>
inline void assign_components(mpc_complex_backend<D1>& result, long a, long b)
{
   mpc_set_si_si(result.data(), a, b, GMP_RNDN);
}

#if defined(BOOST_HAS_LONG_LONG) && defined(_MPFR_H_HAVE_INTMAX_T)
template <unsigned D1, unsigned D2, mpfr_allocation_type AllocationType>
inline void assign_components(mpc_complex_backend<D1>& result, unsigned long long a, unsigned long long b)
{
   mpc_set_uj_uj(result.data(), a, b, GMP_RNDN);
}

template <unsigned D1, unsigned D2, mpfr_allocation_type AllocationType>
inline void assign_components(mpc_complex_backend<D1>& result, long long a, long long b)
{
   mpc_set_sj_sj(result.data(), a, b, GMP_RNDN);
}
#endif

template <unsigned D1, unsigned D2, mpfr_allocation_type AllocationType>
inline void assign_components(mpc_complex_backend<D1>& result, double a, double b)
{
   if ((boost::math::isnan)(a))
   {
      mpc_set_d(result.data(), a, GMP_RNDN);
   }
   else if ((boost::math::isnan)(b))
   {
      mpc_set_d(result.data(), b, GMP_RNDN);
   }
   else
   {
      mpc_set_d_d(result.data(), a, b, GMP_RNDN);
   }
}

template <unsigned D1, unsigned D2, mpfr_allocation_type AllocationType>
inline void assign_components(mpc_complex_backend<D1>& result, long double a, long double b)
{
   if ((boost::math::isnan)(a))
   {
      mpc_set_d(result.data(), a, GMP_RNDN);
   }
   else if ((boost::math::isnan)(b))
   {
      mpc_set_d(result.data(), b, GMP_RNDN);
   }
   else
   {
      mpc_set_ld_ld(result.data(), a, b, GMP_RNDN);
   }
}

//
// Native non-member operations:
//
template <unsigned Digits10>
inline void eval_sqrt(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& val)
{
   mpc_sqrt(result.data(), val.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_pow(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& b, const mpc_complex_backend<Digits10>& e)
{
   mpc_pow(result.data(), b.data(), e.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_exp(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_exp(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_log(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_log(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_log10(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_log10(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_sin(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_sin(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_cos(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_cos(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_tan(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_tan(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_asin(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_asin(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_acos(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_acos(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_atan(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_atan(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_sinh(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_sinh(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_cosh(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_cosh(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_tanh(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_tanh(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_asinh(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_asinh(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_acosh(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_acosh(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_atanh(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_atanh(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_conj(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_conj(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_proj(mpc_complex_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpc_proj(result.data(), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_real(mpfr_float_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpfr_set_prec(result.data(), mpfr_get_prec(mpc_realref(arg.data())));
   mpfr_set(result.data(), mpc_realref(arg.data()), GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_imag(mpfr_float_backend<Digits10>& result, const mpc_complex_backend<Digits10>& arg)
{
   mpfr_set_prec(result.data(), mpfr_get_prec(mpc_imagref(arg.data())));
   mpfr_set(result.data(), mpc_imagref(arg.data()), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const mpfr_float_backend<Digits10>& arg)
{
   mpfr_set(mpc_imagref(result.data()), arg.data(), GMP_RNDN);
}

template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const mpfr_float_backend<Digits10>& arg)
{
   mpfr_set(mpc_realref(result.data()), arg.data(), GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const gmp_int& arg)
{
   mpfr_set_z(mpc_realref(result.data()), arg.data(), GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const gmp_rational& arg)
{
   mpfr_set_q(mpc_realref(result.data()), arg.data(), GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const unsigned& arg)
{
   mpfr_set_ui(mpc_realref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const unsigned long& arg)
{
   mpfr_set_ui(mpc_realref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const int& arg)
{
   mpfr_set_si(mpc_realref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const long& arg)
{
   mpfr_set_si(mpc_realref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const float& arg)
{
   mpfr_set_flt(mpc_realref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const double& arg)
{
   mpfr_set_d(mpc_realref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const long double& arg)
{
   mpfr_set_ld(mpc_realref(result.data()), arg, GMP_RNDN);
}
#if defined(BOOST_HAS_LONG_LONG) && defined(_MPFR_H_HAVE_INTMAX_T)
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const unsigned long long& arg)
{
   mpfr_set_uj(mpc_realref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_real(mpc_complex_backend<Digits10>& result, const long long& arg)
{
   mpfr_set_sj(mpc_realref(result.data()), arg, GMP_RNDN);
}
#endif

template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const gmp_int& arg)
{
   mpfr_set_z(mpc_imagref(result.data()), arg.data(), GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const gmp_rational& arg)
{
   mpfr_set_q(mpc_imagref(result.data()), arg.data(), GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const unsigned& arg)
{
   mpfr_set_ui(mpc_imagref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const unsigned long& arg)
{
   mpfr_set_ui(mpc_imagref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const int& arg)
{
   mpfr_set_si(mpc_imagref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const long& arg)
{
   mpfr_set_si(mpc_imagref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const float& arg)
{
   mpfr_set_flt(mpc_imagref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const double& arg)
{
   mpfr_set_d(mpc_imagref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const long double& arg)
{
   mpfr_set_ld(mpc_imagref(result.data()), arg, GMP_RNDN);
}
#if defined(BOOST_HAS_LONG_LONG) && defined(_MPFR_H_HAVE_INTMAX_T)
template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const unsigned long long& arg)
{
   mpfr_set_uj(mpc_imagref(result.data()), arg, GMP_RNDN);
}
template <unsigned Digits10>
inline void eval_set_imag(mpc_complex_backend<Digits10>& result, const long long& arg)
{
   mpfr_set_sj(mpc_imagref(result.data()), arg, GMP_RNDN);
}
#endif

template <unsigned Digits10>
inline std::size_t hash_value(const mpc_complex_backend<Digits10>& val)
{
   std::size_t result = 0;
   std::size_t len = val.data()[0].re[0]._mpfr_prec / mp_bits_per_limb;
   if(val.data()[0].re[0]._mpfr_prec % mp_bits_per_limb)
      ++len;
   for(std::size_t i = 0; i < len; ++i)
      boost::hash_combine(result, val.data()[0].re[0]._mpfr_d[i]);
   boost::hash_combine(result, val.data()[0].re[0]._mpfr_exp);
   boost::hash_combine(result, val.data()[0].re[0]._mpfr_sign);

   len = val.data()[0].im[0]._mpfr_prec / mp_bits_per_limb;
   if(val.data()[0].im[0]._mpfr_prec % mp_bits_per_limb)
      ++len;
   for(std::size_t i = 0; i < len; ++i)
      boost::hash_combine(result, val.data()[0].im[0]._mpfr_d[i]);
   boost::hash_combine(result, val.data()[0].im[0]._mpfr_exp);
   boost::hash_combine(result, val.data()[0].im[0]._mpfr_sign);
   return result;
}

} // namespace backends

#ifdef BOOST_NO_SFINAE_EXPR

namespace detail{

template<unsigned D1, unsigned D2>
struct is_explicitly_convertible<backends::mpc_complex_backend<D1>, backends::mpc_complex_backend<D2> > : public mpl::true_ {};

}
#endif

namespace detail
{
   template<>
   struct is_variable_precision<backends::mpc_complex_backend<0> > : public true_type {};
}

template<>
struct number_category<detail::canonical<mpc_t, backends::mpc_complex_backend<0> >::type> : public mpl::int_<number_kind_floating_point>{};

using boost::multiprecision::backends::mpc_complex_backend;

typedef number<mpc_complex_backend<50> >    mpc_complex_50;
typedef number<mpc_complex_backend<100> >   mpc_complex_100;
typedef number<mpc_complex_backend<500> >   mpc_complex_500;
typedef number<mpc_complex_backend<1000> >  mpc_complex_1000;
typedef number<mpc_complex_backend<0> >     mpc_complex;

template <unsigned Digits10, expression_template_option ExpressionTemplates>
struct component_type<number<mpc_complex_backend<Digits10>, ExpressionTemplates> >
{
   typedef number<mpfr_float_backend<Digits10>, ExpressionTemplates> type;
};

template <unsigned Digits10, expression_template_option ExpressionTemplates>
struct component_type<number<logged_adaptor<mpc_complex_backend<Digits10> >, ExpressionTemplates> >
{
   typedef number<mpfr_float_backend<Digits10>, ExpressionTemplates> type;
};

template <unsigned Digits10, expression_template_option ExpressionTemplates>
struct complex_result_from_scalar<number<mpfr_float_backend<Digits10>, ExpressionTemplates> >
{
   typedef number<mpc_complex_backend<Digits10>, ExpressionTemplates> type;
};

} // namespace multiprecision

}  // namespaces

#endif


// This file is part of the dune-gdt project:
//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2018 dune-gdt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Rene Milk      (2018)
//   Tobias Leibner (2017)

#ifndef DUNE_GDT_HYPERBOLIC_PROBLEMS_MOMENTMODELS_BASISFUNCTIONS_BASE_HH
#define DUNE_GDT_HYPERBOLIC_PROBLEMS_MOMENTMODELS_BASISFUNCTIONS_BASE_HH

#include <memory>
#include <vector>
#include <string>

#include <boost/math/special_functions/legendre.hpp>
#include <boost/math/special_functions/spherical_harmonic.hpp>

#include <dune/xt/common/math.hh>
#include <dune/xt/common/string.hh>
#include <dune/xt/common/tuple.hh>

#include <dune/xt/functions/affine.hh>

#include <dune/xt/grid/gridprovider/cube.hh>

#include <dune/xt/la/container.hh>

#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/operators/l2.hh>
#include <dune/gdt/spaces/cg.hh>
#include <dune/gdt/test/hyperbolic/problems/momentmodels/triangulation.hh>

namespace Dune {
namespace GDT {
namespace Hyperbolic {
namespace Problems {

// take a DiscreteFunction and return a DiscreteFunction corresponding to component ii
template <size_t ii, class DiscreteFunctionType>
auto get_factor_discrete_function(const DiscreteFunctionType& discrete_function) ->
    typename Dune::GDT::DiscreteFunction<
        typename XT::Common::tuple_element<ii, typename DiscreteFunctionType::SpaceType::SpaceTupleType>::type,
        typename DiscreteFunctionType::VectorType>
{
  typedef typename Dune::GDT::DiscreteFunction<
      typename XT::Common::tuple_element<ii, typename DiscreteFunctionType::SpaceType::SpaceTupleType>::type,
      typename DiscreteFunctionType::VectorType>
      FactorDiscreteFunctionType;
  static_assert(ii < DiscreteFunctionType::SpaceType::num_factors, "This factor does not exist.");
  const auto& space = discrete_function.space();
  const auto& factor_space = space.template factor<ii>();
  typename DiscreteFunctionType::VectorType factor_vector(factor_space.mapper().size());
  const auto it_end = space.grid_layer().template end<0>();
  for (auto it = space.grid_layer().template begin<0>(); it != it_end; ++it) {
    const auto& entity = *it;
    for (size_t jj = 0; jj < factor_space.mapper().numDofs(entity); ++jj)
      factor_vector.set_entry(factor_space.mapper().mapToGlobal(entity, jj),
                              discrete_function.vector().get_entry(space.mapper().mapToGlobal(ii, entity, jj)));
  }
  FactorDiscreteFunctionType factor_discrete_function(factor_space);
  factor_discrete_function.vector() = factor_vector;
  //  typedef Dune::GDT::DiscreteFunctionDataHandle<FactorDiscreteFunctionType> DataHandleType;
  //  DataHandleType handle(factor_discrete_function);
  //  factor_space.grid_layer().template communicate<DataHandleType>(
  //      handle, Dune::InteriorBorder_All_Interface, Dune::ForwardCommunication);
  return factor_discrete_function;
}

// static for loop to sum components of a DiscreteFunction
template <size_t index, size_t N>
struct static_discrete_function_loop
{
  template <class DiscreteFunctionType>
  static typename DiscreteFunctionType::VectorType sum_vectors(const DiscreteFunctionType& discrete_function)
  {
    return static_discrete_function_loop<index, N / 2>::sum_vectors(discrete_function)
           + static_discrete_function_loop<index + N / 2, N - N / 2>::sum_vectors(discrete_function);
  }

  template <class DiscreteFunctionType>
  static typename DiscreteFunctionType::VectorType
  sum_vectors_divisible_by(const DiscreteFunctionType& discrete_function, const size_t divisor)
  {
    return static_discrete_function_loop<index, N / 2>::sum_vectors_divisible_by(discrete_function, divisor)
           + static_discrete_function_loop<index + N / 2, N - N / 2>::sum_vectors_divisible_by(discrete_function,
                                                                                divisor);
  }
};

// specialization to end the loop
template <size_t index>
struct static_discrete_function_loop<index, 1>
{
  template <class DiscreteFunctionType>
  static typename DiscreteFunctionType::VectorType sum_vectors(const DiscreteFunctionType& discrete_function)
  {
    return get_factor_discrete_function<index, DiscreteFunctionType>(discrete_function).vector();
  }

  template <class DiscreteFunctionType>
  static typename DiscreteFunctionType::VectorType
  sum_vectors_divisible_by(const DiscreteFunctionType& discrete_function, const size_t divisor)
  {
    if (!(index % divisor))
      return get_factor_discrete_function<index, DiscreteFunctionType>(discrete_function).vector();
    else
      return typename DiscreteFunctionType::VectorType(
          discrete_function.space().template factor<index>().mapper().size(), 0.);
  }
};

// visualizes sum of components of discrete_function
template <class DiscreteFunctionType, size_t dimRange>
void sum_visualizer(const DiscreteFunctionType& u_n, const std::string& filename_prefix, const size_t ii)
{
  auto sum_function = get_factor_discrete_function<0, DiscreteFunctionType>(u_n);
  sum_function.vector() = static_discrete_function_loop<0, dimRange>::sum_vectors(u_n);
  sum_function.visualize(filename_prefix + "_" + Dune::XT::Common::to_string(ii));
}

// visualizes sum of components with index divisible by divisor
template <class DiscreteFunctionType, size_t dimRange>
void sum_divisible_by_visualizer(const DiscreteFunctionType& u_n,
                                 const std::string& filename_prefix,
                                 const size_t ii,
                                 const size_t divisor)
{
  auto sum_function = get_factor_discrete_function<0, DiscreteFunctionType>(u_n);
  sum_function.vector() = static_discrete_function_loop<0, dimRange>::sum_vectors_divisible_by(u_n, divisor);
  sum_function.visualize(filename_prefix + "_" + Dune::XT::Common::to_string(ii));
}

// visualizes factor * component of discrete function
template <class DiscreteFunctionType, size_t dimRange, size_t component>
void component_visualizer(const DiscreteFunctionType& u_n,
                          const std::string& filename_prefix,
                          const size_t ii,
                          const double factor = 1.)
{
  auto u_n_comp = get_factor_discrete_function<component, DiscreteFunctionType>(u_n);
  u_n_comp.vector() *= factor;
  u_n_comp.visualize(filename_prefix + "_" + Dune::XT::Common::to_string(ii));
}

// see https://en.wikipedia.org/wiki/Tridiagonal_matrix#Inversion
template <class FieldType, int rows>
Dune::DynamicMatrix<FieldType> tridiagonal_matrix_inverse(const DynamicMatrix<FieldType>& matrix)
{
  typedef Dune::DynamicMatrix<FieldType> MatrixType;
  size_t cols = rows;
#ifndef NDEBUG
  for (size_t rr = 0; rr < rows; ++rr)
    for (size_t cc = 0; cc < cols; ++cc)
      if ((cc > rr + 1 || cc + 1 < rr) && XT::Common::FloatCmp::ne(matrix[rr][cc], 0.))
        DUNE_THROW(XT::Common::Exceptions::you_are_using_this_wrong, "Matrix has to be tridiagonal!");
#endif // NDEBUG
  MatrixType ret(rows, rows, 0);
  Dune::FieldVector<FieldType, rows + 1> a(0), b(0), c(0), theta(0);
  Dune::FieldVector<FieldType, rows + 2> phi(0);
  for (size_t ii = 1; ii < rows + 1; ++ii) {
    a[ii] = matrix[ii - 1][ii - 1];
    if (ii < rows) {
      b[ii] = matrix[ii - 1][ii];
      c[ii] = matrix[ii][ii - 1];
    }
  }
  theta[0] = 1;
  theta[1] = a[1];
  for (size_t ii = 2; ii < rows + 1; ++ii)
    theta[ii] = a[ii] * theta[ii - 1] - b[ii - 1] * c[ii - 1] * theta[ii - 2];
  phi[rows + 1] = 1;
  phi[rows] = a[rows];
  for (size_t ii = rows - 1; ii > 0; --ii)
    phi[ii] = a[ii] * phi[ii + 1] - b[ii] * c[ii] * phi[ii + 2];
  for (size_t ii = 1; ii < rows + 1; ++ii) {
    for (size_t jj = 1; jj < cols + 1; ++jj) {
      if (ii == jj)
        ret[ii - 1][jj - 1] = theta[ii - 1] * phi[jj + 1] / theta[rows];
      else if (ii < jj) {
        ret[ii - 1][jj - 1] = std::pow(-1, ii + jj) * theta[ii - 1] * phi[jj + 1] / theta[rows];
        for (size_t kk = ii; kk < jj; ++kk)
          ret[ii - 1][jj - 1] *= b[kk];
      } else if (ii > jj) {
        ret[ii - 1][jj - 1] = std::pow(-1, ii + jj) * theta[jj - 1] * phi[ii + 1] / theta[rows];
        for (size_t kk = jj; kk < ii; ++kk)
          ret[ii - 1][jj - 1] *= c[kk];
      }
    } // jj
  } // ii
#ifndef NDEBUG
  for (size_t ii = 0; ii < rows; ++ii)
    for (size_t jj = 0; jj < cols; ++jj)
      if (std::isnan(ret[ii][jj]) || std::isinf(ret[ii][jj]))
        DUNE_THROW(Dune::MathError, "Inversion of triangular matrix failed!");
#endif
  return ret;
} // ... tridiagonal_matrix_inverse(...)

// After each refinement step:
// num_vertices_new = num_vertices_old + num_intersections_old
// num_intersections_new = 2*num_intersections_old + 3*num_faces_old
// num_faces_new = 4*num_faces_old
// Initially, there are 6 vertices, 12 intersections and 8 faces.
template <size_t refinements>
struct OctaederStatistics
{
  static constexpr size_t num_faces()
  {
    return 8 * (1 << 2 * refinements);
  }

  static constexpr size_t num_intersections()
  {
    return 2 * OctaederStatistics<refinements - 1>::num_intersections()
           + 3 * OctaederStatistics<refinements - 1>::num_faces();
  }

  static constexpr size_t num_vertices()
  {
    return OctaederStatistics<refinements - 1>::num_vertices()
           + OctaederStatistics<refinements - 1>::num_intersections();
  }
};

template <>
struct OctaederStatistics<0>
{
  static constexpr size_t num_faces()
  {
    return 8;
  }

  static constexpr size_t num_intersections()
  {
    return 12;
  }

  static constexpr size_t num_vertices()
  {
    return 6;
  }
};

template <class DomainFieldImp,
          size_t domainDim,
          class RangeFieldImp,
          size_t rangeDim,
          size_t rangeDimCols = 1,
          size_t fluxDim = domainDim>
class BasisfunctionsInterface
{
public:
  static const size_t dimDomain = domainDim;
  static const size_t dimRange = rangeDim;
  static const size_t dimRangeCols = rangeDimCols;
  static const size_t dimFlux = fluxDim;
  typedef DomainFieldImp DomainFieldType;
  typedef XT::Common::FieldVector<DomainFieldType, dimDomain> DomainType;
  typedef RangeFieldImp RangeFieldType;
  typedef DynamicMatrix<RangeFieldType> MatrixType;
  typedef typename XT::Functions::RangeTypeSelector<RangeFieldType, dimRange, dimRangeCols>::type RangeType;
  template <class DiscreteFunctionType>
  using VisualizerType = typename std::function<void(const DiscreteFunctionType&, const std::string&, const size_t)>;
  using StringifierType = std::function<std::string(const RangeType&)>;
  typedef typename Dune::QuadratureRule<RangeFieldType, dimDomain> QuadratureType;

  virtual ~BasisfunctionsInterface(){};

  virtual RangeType evaluate(const DomainType& v) const = 0;

  virtual RangeType integrated() const = 0;

  virtual MatrixType mass_matrix() const = 0;

  virtual MatrixType mass_matrix_inverse() const = 0;

  virtual FieldVector<MatrixType, dimFlux> mass_matrix_with_v() const = 0;

  static QuadratureRule<RangeFieldType, 2> barycentre_rule()
  {
    Dune::QuadratureRule<RangeFieldType, 2> ret;
    ret.push_back(Dune::QuadraturePoint<RangeFieldType, 2>({1. / 3., 1. / 3.}, 0.5));
    return ret;
  }

protected:
  std::vector<std::vector<size_t>> create_decomposition(const size_t num_threads, const size_t size) const
  {
    std::vector<std::vector<size_t>> decomposition(num_threads);
    for (size_t ii = 0; ii < num_threads - 1; ++ii) {
      decomposition[ii].reserve(size / num_threads * (ii + 1) - size / num_threads * ii);
      for (size_t jj = size / num_threads * ii; jj < size / num_threads * (ii + 1); ++jj)
        decomposition[ii].push_back(jj);
    }
    decomposition[num_threads - 1].reserve(size - (size / num_threads) * (num_threads - 1));
    for (size_t jj = size / num_threads * (num_threads - 1); jj < size; ++jj)
      decomposition[num_threads - 1].push_back(jj);
    return decomposition;
  }

  virtual void parallel_quadrature(const QuadratureType& quadrature,
                                   MatrixType& matrix,
                                   const size_t v_index,
                                   const bool reflecting = false) const
  {
    size_t num_threads = std::min(XT::Common::threadManager().max_threads(), quadrature.size());
    auto decomposition = create_decomposition(num_threads, quadrature.size());
    std::vector<std::thread> threads(num_threads);
    // Launch a group of threads
    std::vector<MatrixType> local_matrices(num_threads, MatrixType(matrix.N(), matrix.M(), 0.));
    for (size_t ii = 0; ii < num_threads; ++ii)
      threads[ii] = std::thread(&BasisfunctionsInterface::calculate_in_thread,
                                this,
                                std::cref(quadrature),
                                std::ref(local_matrices[ii]),
                                v_index,
                                std::cref(decomposition[ii]),
                                reflecting);
    // Join the threads with the main thread
    for (size_t ii = 0; ii < num_threads; ++ii)
      threads[ii].join();
    // add local matrices
    matrix *= 0.;
    for (size_t ii = 0; ii < num_threads; ++ii)
      matrix += local_matrices[ii];
  } // void parallel_quadrature(...)

  virtual void calculate_in_thread(const QuadratureType& quadrature,
                                   MatrixType& local_matrix,
                                   const size_t v_index,
                                   const std::vector<size_t>& indices,
                                   const bool reflecting) const
  {
    for (const auto& jj : indices) {
      const auto& quad_point = quadrature[jj];
      const auto& v = quad_point.position();
      auto v_reflected = v;
      if (reflecting)
        v_reflected[v_index] *= -1.;
      const auto basis_evaluated = evaluate(v);
      const auto basis_reflected = evaluate(v_reflected);
      const auto& weight = quad_point.weight();
      const auto factor = (reflecting || v_index == size_t(-1)) ? 1. : v[v_index];
      for (size_t nn = 0; nn < local_matrix.N(); ++nn)
        for (size_t mm = 0; mm < local_matrix.M(); ++mm)
          local_matrix[nn][mm] +=
              basis_evaluated[nn] * (reflecting ? basis_reflected[mm] : basis_evaluated[mm]) * factor * weight;
    } // ii
  } // void calculate_in_thread(...)

  RangeType integrated_initializer(const QuadratureType& quadrature) const
  {
    size_t num_threads = std::min(XT::Common::threadManager().max_threads(), quadrature.size());
    auto decomposition = create_decomposition(num_threads, quadrature.size());
    std::vector<std::thread> threads(num_threads);
    std::vector<RangeType> local_vectors(num_threads, RangeType(0.));
    for (size_t ii = 0; ii < num_threads; ++ii)
      threads[ii] = std::thread(&BasisfunctionsInterface::integrated_initializer_thread,
                                this,
                                std::cref(quadrature),
                                std::ref(local_vectors[ii]),
                                std::cref(decomposition[ii]));
    // Join the threads with the main thread
    for (size_t ii = 0; ii < num_threads; ++ii)
      threads[ii].join();
    // add local matrices
    RangeType ret(0.);
    for (size_t ii = 0; ii < num_threads; ++ii)
      ret += local_vectors[ii];
    return ret;
  }

  void integrated_initializer_thread(const QuadratureType& quadrature,
                                     RangeType& local_range,
                                     const std::vector<size_t>& indices) const
  {
    for (const auto& jj : indices) {
      const auto& quad_point = quadrature[jj];
      auto basis_evaluated = evaluate(quad_point.position());
      basis_evaluated *= quad_point.weight();
      local_range += basis_evaluated;
    } // jj
  } // void calculate_in_thread(...)
};

} // namespace Problems
} // namespace Hyperbolic
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_HYPERBOLIC_PROBLEMS_MOMENTMODELS_BASISFUNCTIONS_BASE_HH


#define BIORBD_API_EXPORTS
#include "Utils/Quaternion.h"

#include <rbdl/rbdl_math.h>
#include <Eigen/Dense>
#include "Utils/Vector3d.h"
#include "Utils/Vector.h"
#include "Utils/RotoTrans.h"
#include "Utils/Error.h"
#include "Utils/Rotation.h"

biorbd::utils::Quaternion::Quaternion (
        double kStabilizer) :
    Eigen::Vector4d (1, 0, 0, 0),
    m_Kstab(kStabilizer) {

}

biorbd::utils::Quaternion::Quaternion(const biorbd::utils::Quaternion &other) :
    Eigen::Vector4d (other),
    m_Kstab(other.m_Kstab) {

}

biorbd::utils::Quaternion::Quaternion (
        double w,
        double x,
        double y,
        double z,
        double kStabilizer) :
    Eigen::Vector4d(w, x, y, z),
    m_Kstab(kStabilizer) {

}

biorbd::utils::Quaternion::Quaternion (
        double w,
        const biorbd::utils::Vector3d &vec3,
        double kStabilizer) :
    Eigen::Vector4d(w, vec3[0], vec3[1], vec3[2]),
    m_Kstab(kStabilizer) {

}

biorbd::utils::Quaternion::Quaternion(
        const biorbd::utils::Vector &vec,
        double kStabilizer):
    Eigen::Vector4d (vec),
    m_Kstab(kStabilizer) {

}

double biorbd::utils::Quaternion::w() const
{
    return (*this)(0);
}
double biorbd::utils::Quaternion::x() const
{
    return (*this)(1);
}
double biorbd::utils::Quaternion::y() const
{
    return (*this)(2);
}
double biorbd::utils::Quaternion::z() const
{
    return (*this)(3);
}

void biorbd::utils::Quaternion::setKStab(double newKStab)
{
    m_Kstab = newKStab;
}

double biorbd::utils::Quaternion::kStab() const
{
    return m_Kstab;
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*(
        const biorbd::utils::Quaternion& q) const
{
    return biorbd::utils::Quaternion (
        (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] - (*this)[3] * q[3],
        (*this)[0] * q[1] + (*this)[1] * q[0] + (*this)[2] * q[3] - (*this)[3] * q[2],
        (*this)[0] * q[2] + (*this)[2] * q[0] + (*this)[3] * q[1] - (*this)[1] * q[3],
        (*this)[0] * q[3] + (*this)[3] * q[0] + (*this)[1] * q[2] - (*this)[2] * q[1],
            (this->m_Kstab + q.m_Kstab) / 2);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*(
        double scalar) const
{
    return biorbd::utils::Quaternion (
                this->Eigen::Vector4d::operator*(scalar), this->m_Kstab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*(
        float scalar) const
{
    return biorbd::utils::Quaternion (
                this->Eigen::Vector4d::operator*(scalar), this->m_Kstab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::operator+(
        const biorbd::utils::Quaternion& other) const
{
    return biorbd::utils::Quaternion(this->Eigen::Vector4d::operator+(other),
                                     (this->m_Kstab + other.m_Kstab) / 2);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::operator-(
        const biorbd::utils::Quaternion& other) const
{
    return biorbd::utils::Quaternion(this->Eigen::Vector4d::operator-(other),
                                     (this->m_Kstab + other.m_Kstab) / 2);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromGLRotate(
        double angle,
        double x,
        double y,
        double z,
        double kStab) {
    double st = std::sin (angle * M_PI / 360.);
    return biorbd::utils::Quaternion (
                std::cos (angle * M_PI / 360.), st * x, st * y, st * z, kStab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromAxisAngle(
        double angle,
        const biorbd::utils::Vector3d &axis,
        double kStab) {
    double d = axis.norm();
    double s2 = std::sin (angle * 0.5) / d;
    return biorbd::utils::Quaternion (
                std::cos(angle * 0.5),
                axis[0] * s2, axis[1] * s2, axis[2] * s2, kStab
            );
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromMatrix(
        const biorbd::utils::RotoTrans &rt,
        double kStab) {
    return fromMatrix(rt.rot(), kStab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromMatrix(
        const biorbd::utils::Rotation &mat,
        double kStab) {
    double w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5;
    return Quaternion (
                w,
                (mat(2,1) - mat(1,2)) / (w * 4.),
                (mat(0,2) - mat(2,0)) / (w * 4.),
                (mat(1,0) - mat(0,1)) / (w * 4.),
                kStab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromZYXAngles(
        const biorbd::utils::Vector3d &zyx_angles,
        double kStab) {
    return fromAxisAngle (zyx_angles[2], biorbd::utils::Vector3d (0., 0., 1.), kStab)
            * fromAxisAngle (zyx_angles[1], biorbd::utils::Vector3d (0., 1., 0.), kStab)
            * fromAxisAngle (zyx_angles[0], biorbd::utils::Vector3d (1., 0., 0.), kStab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromYXZAngles(
        const biorbd::utils::Vector3d &yxz_angles,
        double kStab) {
    return fromAxisAngle (yxz_angles[1], biorbd::utils::Vector3d (0., 1., 0.), kStab)
            * fromAxisAngle (yxz_angles[0], biorbd::utils::Vector3d (1., 0., 0.), kStab)
            * fromAxisAngle (yxz_angles[2], biorbd::utils::Vector3d (0., 0., 1.), kStab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromXYZAngles(
        const biorbd::utils::Vector3d &xyz_angles, double kStab) {
    return fromAxisAngle (xyz_angles[0], biorbd::utils::Vector3d (1., 0., 0.), kStab)
            * fromAxisAngle (xyz_angles[1], biorbd::utils::Vector3d (0., 1., 0.), kStab)
            * fromAxisAngle (xyz_angles[2], biorbd::utils::Vector3d (0., 0., 1.), kStab);
}

biorbd::utils::Rotation biorbd::utils::Quaternion::toMatrix(
        bool skipAsserts) const {
    if (!skipAsserts) {
        biorbd::utils::Error::check(fabs(this->squaredNorm() - 1.) < 1e-10,
                                    "The Quaternion norm is not equal to one");
    }

    double w = (*this)[0];
    double x = (*this)[1];
    double y = (*this)[2];
    double z = (*this)[3];
    biorbd::utils::Rotation out;
    out <<
        1 - 2*y*y - 2*z*z,  2*x*y - 2*w*z,      2*x*z + 2*w*y,
        2*x*y + 2*w*z,      1 - 2*x*x - 2*z*z,  2*y*z - 2*w*x,
        2*x*z - 2*w*y,      2*y*z + 2*w*x,      1 - 2*x*x - 2*y*y;
    return out;
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::slerp(
        double alpha,
        const biorbd::utils::Quaternion &quat) const {
    // check whether one of the two has 0 length
    double s = std::sqrt (squaredNorm() * quat.squaredNorm());

    // division by 0.f is unhealthy!
    assert (s != 0.);

    double angle = acos (dot(quat) / s);
    if (angle == 0. || std::isnan(angle)) {
        return *this;
    }
    assert(!std::isnan(angle));

    double d = 1. / std::sin (angle);
    double p0 = std::sin ((1. - alpha) * angle);
    double p1 = std::sin (alpha * angle);

    if (dot (quat) < 0.) {
        return Quaternion( ((*this) * p0 - quat * p1) * d, this->m_Kstab);
    }
    return Quaternion( ((*this) * p0 + quat * p1) * d,
                       (this->m_Kstab + quat.m_Kstab) / 2);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::conjugate() const {
    return biorbd::utils::Quaternion (
                (*this)[0],
                -(*this)[1],-(*this)[2],-(*this)[3],
                this->kStab()
            );
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::timeStep(
        const biorbd::utils::Vector3d &omega,
        double dt) {
    double omega_norm = omega.norm();
    return fromAxisAngle (
                dt * omega_norm, omega / omega_norm, this->m_Kstab) * (*this);
}

biorbd::utils::Vector3d biorbd::utils::Quaternion::rotate(
        const biorbd::utils::Vector3d &vec) const {
    biorbd::utils::Quaternion vec_quat (0., vec);

    biorbd::utils::Quaternion res_quat(vec_quat * (*this));
    res_quat = conjugate() * res_quat;

    return biorbd::utils::Vector3d(res_quat[1], res_quat[2], res_quat[3]);
}

#include <iostream>
biorbd::utils::Quaternion biorbd::utils::Quaternion::omegaToQDot(
        const biorbd::utils::Vector3d &omega) const {
    Eigen::MatrixXd m(4, 3);
    m(0, 0) = -(*this)[1];   m(0, 1) = -(*this)[2];   m(0, 2) = -(*this)[3];
    m(1, 0) =  (*this)[0];   m(1, 1) = -(*this)[3];   m(1, 2) =  (*this)[2];
    m(2, 0) =  (*this)[3];   m(2, 1) =  (*this)[0];   m(2, 2) = -(*this)[1];
    m(3, 0) = -(*this)[2];   m(3, 1) =  (*this)[1];   m(3, 2) =  (*this)[0];
    return biorbd::utils::Quaternion(0.5 * m * omega, this->m_Kstab);
}

biorbd::utils::Vector3d  biorbd::utils::Quaternion::eulerDotToOmega(
            const biorbd::utils::Vector3d &eulerDot,
            const biorbd::utils::Vector3d &euler,
            const biorbd::utils::String& seq) {

    biorbd::utils::Vector3d w;
    double dph, dth, dps, ph, th, ps;
    dph = eulerDot[0]; dth = eulerDot[1]; dps = eulerDot[2];
    ph = euler[0]; th = euler[1]; ps = euler[2];
    if (!seq.compare("xyz")) {          // xyz
        w[0] = dph*std::cos(th)*std::cos(ps) + dth*std::sin(ps);
        w[1] = dth*std::cos(ps) - dph*std::cos(th)*std::sin(ps);
        w[2] = dph*std::sin(th) + dps;
    } else {
        biorbd::utils::Error::raise("Angle sequence is either nor implemented or not recognized");
    }
    return w;
}

void biorbd::utils::Quaternion::derivate(
        const biorbd::utils::Vector &w)
{
    // Création du quaternion de "préproduit vectoriel"
    double& qw = (*this)(0);
    double& qx = (*this)(1);
    double& qy = (*this)(2);
    double& qz = (*this)(3);
    Eigen::Matrix4d Q;
    Q <<    qw, -qx, -qy, -qz,
            qx,  qw, -qz,  qy,
            qy,  qz,  qw, -qx,
            qz, -qy,  qx,  qw;

    // Ajout du paramètre de stabilisation
    Eigen::Vector4d w_tp (m_Kstab*w.norm()*(1-this->norm()), w(0), w(1), w(2));
    Eigen::Vector4d newQuat(0.5 * Q * w_tp);

    // Assigning is slightly faster than create a new Quaternion
    qw = newQuat[0];
    qx = newQuat[1];
    qy = newQuat[2];
    qz = newQuat[3];

}

void biorbd::utils::Quaternion::normalize()
{
    *this = *this / this->norm();
}