///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)

//  What's in this header:
//  The frontend of a plan-like FFT interface.

#ifndef BOOST_MATH_DFTAPI_HPP
  #define BOOST_MATH_DFTAPI_HPP

#include <algorithm>
#include <vector>
#include <complex>

// TODO: once this file is split as described inside it, then this include can be removed, because the user code will include the complex type of their choosing and along with the necessary traits.
#include <boost/math/fft/multiprecision_complex.hpp>

  namespace boost { namespace math { namespace fft {
  namespace detail {

  // fftw_plan-like Fourier Transform API

  /*
    RingType axioms:
    1. Abelian group addition (operator+)
      -> closure
      -> associativity
      -> neutral element (0)
      -> inverse (operator-)
      -> commutativity
    2. Monoid multiplication (operator*)
      -> closure
      -> associativity
      -> neutral element (1)
    3. addition and multiplication compatibility
      -> left distributivity, ie. a*(b+c) == a*b + a*c
      -> right distributivity, ie. (b+c)*a == b*a + c*a
  */

  /*
    Type A to type B execution API.
  */
  template<typename A_t, typename B_t, typename allocator_t>
  class asymmetric_executor
  {
    public:
    using value_type1 = A_t;
    using value_type2 = B_t;
    //using allocator_type1 = allocator_t;
    //using allocator_type2 = allocator_t;
    using allocator_type1 = typename std::allocator_traits<allocator_t>::template rebind_alloc<value_type1>;
    using allocator_type2 = typename std::allocator_traits<allocator_t>::template rebind_alloc<value_type2>;

    private:

    using buffer_type1 = std::vector<value_type1,allocator_type1> ;
    using buffer_type2 = std::vector<value_type2,allocator_type2> ;
    buffer_type1 my_mem_1;
    buffer_type2 my_mem_2;

    public:
    constexpr asymmetric_executor(const allocator_t& in_alloc = allocator_t{})
      : my_mem_1(in_alloc), my_mem_2(in_alloc)
    { }

    template<typename InputIteratorType,
             typename OutputIteratorType,
             typename EngineType >
    void execute(
      InputIteratorType in_first, InputIteratorType /* in_last */,
      OutputIteratorType out,
      EngineType engine,
      typename std::enable_if<(   (std::is_convertible<InputIteratorType,  const value_type1*>::value == true)
                               && (std::is_convertible<OutputIteratorType,       value_type2*>::value == true))>::type* = nullptr)
    {
      engine(in_first,out);
    }

    template<typename InputIteratorType,
             typename OutputIteratorType,
             typename EngineType>
    void execute(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out,
      EngineType engine,
      typename std::enable_if<(   (std::is_convertible<InputIteratorType,  const value_type1*>::value == false)
                               && (std::is_convertible<OutputIteratorType,       value_type2*>::value == true))>::type* = nullptr)
    {
      my_mem_1.resize(std::distance(in_first,in_last));
      std::copy(in_first, in_last, std::begin(my_mem_1));
      engine(my_mem_1.data(),out);
    }

    template<typename InputIteratorType,
             typename OutputIteratorType,
             typename EngineType>
    void execute(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out,
      EngineType engine,
      typename std::enable_if<(   (std::is_convertible<InputIteratorType,  const value_type1*>::value == true)
                               && (std::is_convertible<OutputIteratorType,       value_type2*>::value == false))>::type* = nullptr)
    {
      my_mem_2.resize(std::distance(in_first,in_last));
      engine(in_first,my_mem_2.data());
      std::copy(std::begin(my_mem_2), std::end(my_mem_2), out);
    }

    template<typename InputIteratorType,
             typename OutputIteratorType,
             typename EngineType>
    void execute(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out,
      EngineType engine,
      typename std::enable_if<(   (std::is_convertible<InputIteratorType,  const value_type1*>::value == false)
                               && (std::is_convertible<OutputIteratorType,       value_type2*>::value == false))>::type* = nullptr)
    {
      my_mem_1.resize(std::distance(in_first,in_last));
      my_mem_2.resize(my_mem_1.size());
      std::copy(in_first, in_last, std::begin(my_mem_1));
      engine(my_mem_1.data(),my_mem_2.data());
      std::copy(std::begin(my_mem_2),std::end(my_mem_2), out);
    }

  };

  /*
    Type T to type T execution API.
  */
  template<typename A_t, typename allocator_t>
  class symmetric_executor
  {
    public:
    using value_type = A_t;
    using allocator_type = allocator_t;

    private:
    using buffer_type = std::vector<value_type,allocator_type> ;

    protected:
    buffer_type my_mem;

    public:
    constexpr symmetric_executor(const allocator_type& in_alloc = allocator_type{})
      : my_mem(in_alloc)
    { }

    template<typename InputIteratorType,
             typename OutputIteratorType,
             typename EngineType >
    void execute(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out,
      EngineType engine,
      typename std::enable_if<(   (std::is_convertible<InputIteratorType,  const value_type*>::value == true)
                               && (std::is_convertible<OutputIteratorType,       value_type*>::value == true))>::type* = nullptr)
    {
      engine(in_first,out);
    }

    template<typename InputIteratorType,
             typename OutputIteratorType,
             typename EngineType>
    void execute(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out,
      EngineType engine,
      typename std::enable_if<(   (std::is_convertible<InputIteratorType,  const value_type*>::value == false)
                               && (std::is_convertible<OutputIteratorType,       value_type*>::value == true))>::type* = nullptr)
    {
      std::copy(in_first, in_last, out);
      engine(out,out);
    }

    template<typename InputIteratorType,
             typename OutputIteratorType,
             typename EngineType>
    void execute(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out,
      EngineType engine,
      typename std::enable_if<(   (std::is_convertible<InputIteratorType,  const value_type*>::value == true)
                               && (std::is_convertible<OutputIteratorType,       value_type*>::value == false))>::type* = nullptr)
    {
      my_mem.resize(std::distance(in_first,in_last));
      engine(in_first,my_mem.data());
      std::copy(std::begin(my_mem), std::end(my_mem), out);
    }

    template<typename InputIteratorType,
             typename OutputIteratorType,
             typename EngineType>
    void execute(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out,
      EngineType engine,
      typename std::enable_if<(   (std::is_convertible<InputIteratorType,  const value_type*>::value == false)
                               && (std::is_convertible<OutputIteratorType,       value_type*>::value == false))>::type* = nullptr)
    {
      my_mem.resize(std::distance(in_first,in_last));
      std::copy(in_first, in_last, std::begin(my_mem));
      engine(my_mem.data(),my_mem.data());
      std::copy(std::begin(my_mem),std::end(my_mem), out);
    }

  };

  template< template<class ... Args> class BackendType, class T, class allocator_t >
  class algebraic_dft :
        public BackendType<T,allocator_t> ,
        public symmetric_executor<T,allocator_t>
  {
    public:
    using value_type      = T;
    using allocator_type  = allocator_t;

    using backend         = BackendType<value_type,allocator_type>;
    using executor        = symmetric_executor<value_type,allocator_type>;

    template<class U, class A>
    using other = algebraic_dft<BackendType,U,A>;

    private:
    allocator_type alloc;
    value_type root,inverse_root;

  public:
    using backend::size;
    using backend::resize;

    // complex types ctor. n: the size of the dft
    constexpr algebraic_dft(unsigned int n, value_type w, const allocator_type& in_alloc = allocator_type{} )
      : backend(n,in_alloc),
        executor(in_alloc),
        alloc{in_alloc} ,
        root{w},
        inverse_root{ backend::inverse_root(w) }
    { }

    template<typename InputIteratorType,
             typename OutputIteratorType>
    void forward(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out)
    {
      resize(std::distance(in_first,in_last));
      executor::execute(in_first,in_last,out,
        [this](const value_type* i, value_type* o)
        {
          backend::dft(i,o,root);
        });
    }

    template<typename InputIteratorType,
             typename OutputIteratorType>
    void backward(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out)
    {
      resize(std::distance(in_first,in_last));
      executor::execute(in_first,in_last,out,
        [this](const value_type* i, value_type* o)
        {
          backend::dft(i,o,inverse_root);
        });
    }
  };

  template< template<class ... Args> class BackendType, class T, class allocator_t >
  class complex_dft :
        public BackendType<T,allocator_t> ,
        public symmetric_executor<T,allocator_t>
  {
    public:
    using complex_type    = T;
    using real_type       = typename T::value_type;
    using allocator_type  = allocator_t;

    using value_type      = complex_type;

    using backend         = BackendType<complex_type,allocator_type>;
    using executor_C2C    = symmetric_executor<complex_type,allocator_type>;

    template<class U, class A>
    using other = complex_dft<BackendType,U,A>;

    private:
    allocator_type alloc;

  public:
    using backend::size;
    using backend::resize;

    // complex types ctor. n: the size of the dft
    constexpr complex_dft(unsigned int n, const allocator_type& in_alloc = allocator_type{} )
      : backend(n,in_alloc),
        executor_C2C(in_alloc),
        alloc{in_alloc} { }

    template<typename InputIteratorType,
             typename OutputIteratorType>
    void forward(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out)
    {
      resize(std::distance(in_first,in_last));
      executor_C2C::execute(in_first,in_last,out,
        [this](const complex_type* i, complex_type* o)
        {
          backend::forward(i,o);
        });
    }

    template<typename InputIteratorType,
             typename OutputIteratorType>
    void backward(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out)
    {
      resize(std::distance(in_first,in_last));
      executor_C2C::execute(in_first,in_last,out,
        [this](const complex_type* i, complex_type* o)
        {
          backend::backward(i,o);
        });
    }
  };

  template< template<class ... Args> class BackendType, class T, class allocator_t >
  class real_dft :
        public BackendType<T,allocator_t> ,
        public symmetric_executor<T,allocator_t>
  {
    public:
    using value_type      = T;
    using real_type       = T;
    using complex_type    = boost::multiprecision::complex<T>;
    using allocator_type  = allocator_t;

    using backend         = BackendType<real_type,allocator_type>;
    using executor_halfcomplex = symmetric_executor<real_type,allocator_type>;

    template<class U, class A>
    using other = real_dft<BackendType,U,A>;

    private:
    allocator_type alloc;

    template<typename InputIteratorType>
    void encode_halfcomplex(
      InputIteratorType in_first, InputIteratorType in_last,
      real_type* out) const
    {
      unsigned int i=0,j=size();
      {
        *out = in_first->real();
        ++in_first, ++out, ++i; --j;
      }
      while(i<=j)
      {
        *out = in_first->real();
        ++in_first, ++out, ++i, --j;
      }
      while(j>0)
      {
        *out = in_first->imag();
        ++in_first, ++out, ++i, --j;
      }
    }
    template<typename OutputIteratorType>
    void decode_halfcomplex(
      const real_type *in_first, const real_type* in_last,
      OutputIteratorType out) const
    {
      using complex_type = typename std::iterator_traits<OutputIteratorType>::value_type;

      real_type const * const zeroth = in_first;

      {
        *out = *in_first;
        ++out, ++in_first; --in_last;
      }
      while(in_first < in_last)
      {
        *out = complex_type{*in_first, - *in_last};
        ++out, ++in_first; --in_last;
      }
      if(in_first == in_last)
      {
        *out = *in_first;
        ++out, ++in_first; --in_last;
      }
      while(in_last>zeroth)
      {
        *out = complex_type{*in_last,*in_first};
        ++out, ++in_first; --in_last;
      }
   }
    using executor_halfcomplex::my_mem;

  public:
    using backend::size;

    void resize(std::size_t n)
    {
        backend::resize(n);
        my_mem.resize(n);
    }

    // complex types ctor. n: the size of the dft
    constexpr real_dft(unsigned int n, const allocator_type& in_alloc = allocator_type{} )
      : backend(n,in_alloc),
        executor_halfcomplex(in_alloc),
        alloc{in_alloc} { }

    template<typename InputIteratorType,
             typename OutputIteratorType>
    void real_to_halfcomplex(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out)
    {
      resize(std::distance(in_first,in_last));
      executor_halfcomplex::execute(in_first,in_last,out,
        [this](const real_type* i, real_type* o)
        {
          backend::real_to_halfcomplex(i,o);
        });
    }
    template<typename InputIteratorType,
             typename OutputIteratorType>
    void halfcomplex_to_real(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out)
    {
      resize(std::distance(in_first,in_last));
      executor_halfcomplex::execute(in_first,in_last,out,
        [this](const real_type* i, real_type* o)
        {
          backend::halfcomplex_to_real(i,o);
        });
    }

    template<typename InputIteratorType,
             typename OutputIteratorType>
    void real_to_complex(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out)
    {
      resize(std::distance(in_first,in_last));

      executor_halfcomplex::execute(
        in_first,in_last,
        my_mem.data(),
        [this](const real_type* i, real_type* o)
        {
          backend::real_to_halfcomplex(i,o);
        });

      decode_halfcomplex(
        my_mem.data(),
        my_mem.data()+size(),
        out);
    }

    template<typename InputIteratorType,
             typename OutputIteratorType>
    void complex_to_real(
      InputIteratorType in_first, InputIteratorType in_last,
      OutputIteratorType out)
    {
      resize(std::distance(in_first,in_last));

      encode_halfcomplex(
        in_first,
        in_last,
        my_mem.data());

      executor_halfcomplex::execute(
        my_mem.data(),
        my_mem.data()+size(),
        out,
        [this](const real_type* i, real_type* o)
        {
          backend::halfcomplex_to_real(i,o);
        });
    }

  };

  } // namespace detail

  template< class dft_plan_t >
  struct transform
  {

    template<class RingType, class allocator_t = std::allocator<RingType> >
    using plan_type = typename dft_plan_t::template other< RingType,allocator_t > ;

    // std::transform-like Fourier Transform API
    // for complex types
    template<typename InputIterator,
             typename OutputIterator>
    static void forward(InputIterator  input_begin,
                     InputIterator  input_end,
                     OutputIterator output)
    {
      using input_value_type  = typename std::iterator_traits<InputIterator >::value_type;
      plan_type<input_value_type> plan(static_cast<unsigned int>(std::distance(input_begin, input_end)));
      plan.forward(input_begin, input_end, output);
    }

    // std::transform-like Fourier Transform API
    // for complex types
    template<typename InputIterator,
             typename OutputIterator>
    static void backward(InputIterator  input_begin,
                      InputIterator  input_end,
                      OutputIterator output)
    {
      using input_value_type  = typename std::iterator_traits<InputIterator >::value_type;
      plan_type<input_value_type> plan(static_cast<unsigned int>(std::distance(input_begin, input_end)));
      plan.backward(input_begin, input_end, output);
    }

    // std::transform-like Fourier Transform API
    // for Ring types
    template<typename InputIterator,
             typename OutputIterator,
             typename value_type>
    static void forward(InputIterator  input_begin,
                     InputIterator  input_end,
                     OutputIterator output,
                     value_type w)
    {
      using input_value_type  = typename std::iterator_traits<InputIterator >::value_type;
      plan_type<input_value_type> plan(static_cast<unsigned int>(std::distance(input_begin, input_end)),w);
      plan.forward(input_begin, input_end, output);
    }

    // std::transform-like Fourier Transform API
    // for Ring types
    template<typename InputIterator,
             typename OutputIterator,
             typename value_type>
    static void backward(InputIterator  input_begin,
                      InputIterator  input_end,
                      OutputIterator output,
                      value_type w)
    {
      using input_value_type  = typename std::iterator_traits<InputIterator >::value_type;
      plan_type<input_value_type> plan(static_cast<unsigned int>(std::distance(input_begin, input_end)),w);
      plan.backward(input_begin, input_end, output);
    }

    // std::transform-like Fourier Transform API
    // for real types
    template<typename InputIterator,
             typename OutputIterator>
    static void real_to_complex(
                     InputIterator  input_begin,
                     InputIterator  input_end,
                     OutputIterator output)
    {
      using input_value_type  = typename std::iterator_traits<InputIterator >::value_type;
      // using output_value_type = typename std::iterator_traits<OutputIterator>::value_type;
      plan_type<input_value_type> plan(static_cast<unsigned int>(std::distance(input_begin, input_end)));
      plan.real_to_complex(input_begin, input_end, output);
    }

    // std::transform-like Fourier Transform API
    // for real types
    template<typename InputIterator,
             typename OutputIterator>
    static void complex_to_real(InputIterator  input_begin,
                      InputIterator  input_end,
                      OutputIterator output)
    {
      using input_value_type  = typename std::iterator_traits<InputIterator >::value_type;
      // using output_value_type = typename std::iterator_traits<OutputIterator>::value_type;
      plan_type<input_value_type> plan(static_cast<unsigned int>(std::distance(input_begin, input_end)));
      plan.complex_to_real(input_begin, input_end, output);
    }

    // std::transform-like Fourier Transform API
    // for real types
    template<typename InputIterator,
             typename OutputIterator>
    static void real_to_halfcomplex(
                     InputIterator  input_begin,
                     InputIterator  input_end,
                     OutputIterator output)
    {
      using input_value_type  = typename std::iterator_traits<InputIterator >::value_type;
      plan_type<input_value_type> plan(static_cast<unsigned int>(std::distance(input_begin, input_end)));
      plan.real_to_halfcomplex(input_begin, input_end, output);
    }

    // std::transform-like Fourier Transform API
    // for real types
    template<typename InputIterator,
             typename OutputIterator>
    static void halfcomplex_to_real(InputIterator  input_begin,
                      InputIterator  input_end,
                      OutputIterator output)
    {
      using input_value_type  = typename std::iterator_traits<InputIterator >::value_type;
      plan_type<input_value_type> plan(static_cast<unsigned int>(std::distance(input_begin, input_end)));
      plan.halfcomplex_to_real(input_begin, input_end, output);
    }

    template<typename InputIterator1,
             typename InputIterator2,
             typename OutputIterator>
    static void convolution(
        InputIterator1 input1_begin,
        InputIterator1 input1_end,
        InputIterator2 input2_begin,
        OutputIterator output)
    {
      using input_value_type  = typename std::iterator_traits<InputIterator1>::value_type;
      using real_value_type  = typename input_value_type::value_type;
      // using allocator_type    = std::allocator<input_value_type>;
      const long N = std::distance(input1_begin,input1_end);
      plan_type<input_value_type> plan(static_cast<unsigned int>(N));

      std::vector<input_value_type> In1(N),In2(N),Out(N);

      std::copy(input1_begin,input1_end,In1.begin());

      InputIterator2 input2_end{input2_begin};
      std::advance(input2_end,N);
      std::copy(input2_begin,input2_end,In2.begin());

      plan.forward(In1.begin(),In1.end(),In1.begin());
      plan.forward(In2.begin(),In2.end(),In2.begin());

      // direct convolution
      std::transform(In1.begin(),In1.end(),In2.begin(),Out.begin(),std::multiplies<input_value_type>());

      plan.backward(Out.begin(),Out.end(),Out.begin());

      const real_value_type inv_N = real_value_type{1}/N;
      for(auto & x : Out)
          x *= inv_N;

      std::copy(Out.begin(),Out.end(),output);
    }

  };

  } } } // namespace boost::math::fft

#endif // BOOST_MATH_DFTAPI_HPP


/*
 * $Revision: 565 $ $Date: 2011-02-15 16:00:43 -0800 (Tue, 15 Feb 2011) $
 *
 * Copyright by Astos Solutions GmbH, Germany
 *
 * this file is published under the Astos Solutions Free Public License
 * For details on copyright and terms of use see
 * http://www.astos.de/Astos_Solutions_Free_Public_License.html
 */

#include "AlignedEllipsoid.h"
#include <Eigen/LU>
#include <Eigen/QR>
#include <cmath>

using namespace vesta;
using namespace Eigen;
using namespace std;

static void
planeToSpanningVectors(const Hyperplane<double, 3>& plane,
                       Vector3d& origin, Vector3d& v0, Vector3d& v1)
{
    origin = plane.normal() * -plane.offset();
    v0 = plane.normal().unitOrthogonal();
    v1 = v0.cross(plane.normal());
}

/* Only required for alternate implementation of plane intersection method

static Hyperplane<double, 3> spanningVectorsToPlane(const Vector3d& origin,
                                                    const Vector3d& v0,
                                                    const Vector3d& v1)
{
    Vector3d n = v0.normalized().cross(v1.normalized());
    return Hyperplane<double, 3>(n, n.dot(origin));
}

*/

#define USE_ALTERNATE_INTERSECTION_METHOD 0

/** Compute the of this ellipsoid with a plane. The intersection--if it exists--will
  * be either a point or an ellipse. This method treats the point case as if no
  * intersection occurred.
  *
  * \param p the plane to test for intersection
  * \param foundIntersect set to true if there was an intersection, false otherwise. OK if null.
  */
GeneralEllipse
AlignedEllipsoid::intersection(const Hyperplane<double, 3>& p, bool* foundIntersection) const
{
    // We will reduce the problem to the simpler one of finding the intersection of
    // a plane with a unit sphere:
    //    1. Computing transformation M that maps the ellipsoid to a unit sphere
    //    2. Apply this transformation to the plane
    //    3. Find the intersection of the transformed plane and the unit sphere
    //    4. Apply the inverse transformation to get the ellipse of intersection

    // Compute the transformation that maps the ellipsoid to a unit sphere
    Matrix3d M = m_semiAxes.cwise().inverse().asDiagonal();

    Vector3d plane_origin, plane_v0, plane_v1;
    planeToSpanningVectors(p, plane_origin, plane_v0, plane_v1);

    // Transform the plane
    plane_origin = M * plane_origin;
    plane_v0     = M * plane_v0;
    plane_v1     = M * plane_v1;

#if USE_ALTERNATE_INTERSECTION_METHOD
    Hyperplane<double, 3> p1 = spanningVectorsToPlane(plane_origin, plane_v0, plane_v1);
    planeToSpanningVectors(p1, plane_origin, plane_v0, plane_v1);

    double d = plane_origin.norm();
    if (d < 1.0)
    {
        if (foundIntersection)
        {
            *foundIntersection = true;
        }

        // Compute the radius of the circle of intersection
        double r = 1.0 - d * d;

        // Transform the spanning vectors and center back to the original space
        /*DiagonalMatrix<Vector3d>*/ Matrix3d invM = m_semiAxes.asDiagonal();
        plane_origin = invM * plane_origin;
        plane_v0     = invM * (plane_v0 * r);
        plane_v1     = invM * (plane_v1 * r);

        return GeneralEllipse(plane_origin, plane_v0, plane_v1);
    }
    else
    {
        if (foundIntersection)
        {
            *foundIntersection = false;
        }

        // Return an empty ellipse
        return GeneralEllipse(Vector3d::Zero(), Vector3d::Zero(), Vector3d::Zero());
    }
#else
    Hyperplane<double, 3> p1 = p;
    p1.transform(M);

    if (abs(p1.offset()) < 1.0)
    {
        if (foundIntersection)
        {
            *foundIntersection = true;
        }

        // Compute the radius of the circle of intersection
        double r = 1.0 - p1.offset() * p1.offset();

        // n is the plane normal, v0 and v1 are spanning vectors of the plane
        Vector3d n  = p1.normal();
        Vector3d v0 = n.unitOrthogonal();
        Vector3d v1 = v0.cross(n);

        // Transform the spanning vectors and center back to the original space
        DiagonalMatrix<Vector3d> D = m_semiAxes.asDiagonal();
        Vector3d center = D * (n * p1.offset());
        v0              = D * (v0 * r);
        v1              = D * (v1 * r);

        return GeneralEllipse(center, v0, v1);
    }
    else
    {
        if (foundIntersection)
        {
            *foundIntersection = false;
        }

        // Return an empty ellipse
        return GeneralEllipse(Vector3d::Zero(), Vector3d::Zero(), Vector3d::Zero());
    }
#endif
}

/** Compute the ellipse that is the limb of the ellipsoid when viewed from
  * some point outside the ellipsoid.
  */
GeneralEllipse
AlignedEllipsoid::limb(const Vector3d& p) const
{
    // For a point L on the limb, the surface normal N(L) is
    // perpendicular to the view direction L - P, thus:
    //    (L - P) dot N(L) = 0
    //

    Vector3d n = m_semiAxes.cwise().square().cwise().inverse().asDiagonal() * p;
    double mag = n.norm();
    Hyperplane<double, 3> limbPlane(n / mag, 1.0 / mag);

    bool found = false;

    return intersection(limbPlane, &found);
}

static Vector3d projectPoint(const Vector3d& point, const Vector3d& planeNormal)
{
    return point - planeNormal * (point.dot(planeNormal));
}

/** Calculate the projection of the ellipsoid onto plane with specified normal and
  * containing the ellipsoid center.
  */
GeneralEllipse
AlignedEllipsoid::orthogonalProjection(const Vector3d& planeNormal) const
{
    // Compute the transformation that maps the ellipsoid to a unit sphere
    Matrix3d M = m_semiAxes.cwise().inverse().asDiagonal();

    Hyperplane<double, 3> p(planeNormal, 0.0);
    Hyperplane<double, 3> p1 = p;
    p1.transform(M);
    p1.normal().normalize();

    Vector3d plane_origin, plane_v0, plane_v1;
    planeToSpanningVectors(p1, plane_origin, plane_v0, plane_v1);

    DiagonalMatrix<Vector3d> invM = m_semiAxes.asDiagonal();
    plane_v0     = invM * (plane_v0);
    plane_v1     = invM * (plane_v1);

    return GeneralEllipse(Vector3d::Zero(), plane_v0, plane_v1);
}


// Copyright (c) 2012, 2020 Tel-Aviv University (Israel).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
// You can redistribute it and/or modify it under the terms of the GNU
// General Public License as published by the Free Software Foundation,
// either version 3 of the License, or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// SPDX-License-Identifier: GPL-3.0+
//
// Author(s): Saurabh Singh <ssingh@cs.iitr.ac.in>
//            Ahmed Essam <theartful.ae@gmail.com>

#include <CGAL/Polynomial_traits_d.h>
#include <CGAL/polynomial_utils.h>

#include <boost/phoenix.hpp>
#include <boost/spirit/include/qi.hpp>

#include "AlgebraicCurveParser.h"

namespace phx = boost::phoenix;
namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;

template <typename Polynomial_d, typename Iterator, typename Skipper>
struct PolynomialParser : qi::grammar<Iterator, Polynomial_d(), Skipper>
{
  using Self = PolynomialParser<Polynomial_d, Iterator, Skipper>;
  using Traits = CGAL::Polynomial_traits_d<Polynomial_d>;
  using Coefficient = typename Traits::Innermost_coefficient_type;
  using Innermost_leading_coefficient =
    typename Traits::Innermost_leading_coefficient;
  using Total_degree = typename Traits::Total_degree;

  PolynomialParser() : PolynomialParser::base_type(start)
  {
    using qi::_val;
    using qi::eps;

    for (int i = 0; i < Traits::d; i++)
        vars[i] = CGAL::shift(Polynomial_d(1), 1, i);

    // { expr = expr } or { expr }
    start = (expr >> '=' >> expr)[_val = qi::_1 - qi::_2] | expr[_val = qi::_1];
    // addition and subtraction
    expr = term[_val = qi::_1] >>
           *('+' >> term[_val += qi::_1] | '-' >> term[_val -= qi::_1]);
    // multiplication using *, and implied multiplication as in (x+y)(x+y)
    term = factor[_val = qi::_1] >>
           *(('*' >> factor[_val *= qi::_1]) | pow_expr[_val *= qi::_1]);
    // uniary - and + operators
    factor = qi::char_('-') >> pow_expr[_val = -qi::_1] |
             -qi::char_('+') >> pow_expr[_val = qi::_1];
    // power
    pow_expr =
      factor2[_val = qi::_1] >>
      (('^' >> factor2[_val = phx::bind(&Self::raise, this, _val, qi::_1)]) |
       eps);
    // ( expr )
    factor2 = (('(' >> expr >> ')') | factor3)[_val = qi::_1];
    // coefficients and variables
    factor3 =
      coeff[_val = phx::construct<Polynomial_d>(qi::_1)] | var[_val = qi::_1];
    coeff = qi::as_string[qi::lexeme[+qi::digit]]
                         [_val = phx::construct<Coefficient>(qi::_1)];
    if (Traits::d == 1)
      var = qi::char_('x')[_val = vars[0]];
    else
      var = qi::char_('x')[_val = vars[0]] | qi::char_('y')[_val = vars[1]];
  }

  Polynomial_d raise(const Polynomial_d& poly, const Polynomial_d& power)
  {
    if (total_degree(power) != 0)
    {
      this->error = true;
      return {};
    }

    return CGAL::ipower(
      poly, std::lround(CGAL::to_double(innermost_leading_coefficient(power))));
  }

  Innermost_leading_coefficient innermost_leading_coefficient;
  Total_degree total_degree;

  Polynomial_d vars[Traits::d];

  qi::rule<Iterator, Polynomial_d(), Skipper> start;
  qi::rule<Iterator, Polynomial_d(), Skipper> expr;
  qi::rule<Iterator, Polynomial_d(), Skipper> term;
  qi::rule<Iterator, Polynomial_d(), Skipper> pow_expr;
  qi::rule<Iterator, Polynomial_d(), Skipper> factor;
  qi::rule<Iterator, Polynomial_d(), Skipper> factor2;
  qi::rule<Iterator, Polynomial_d(), Skipper> factor3;
  qi::rule<Iterator, Polynomial_d(), Skipper> var;
  qi::rule<Iterator, Coefficient(), Skipper> coeff;

  bool error = false;
};

static bool hasValidChars2D(const std::string& expression)
{
  const char valid_chars[] = {'x', 'y', '+', '-', '*', '(', ')', '^', '='};
  return std::all_of(expression.begin(), expression.end(), [&](char c) {
    return std::isspace(c) || std::isdigit(c) ||
           std::find(std::begin(valid_chars), std::end(valid_chars), c) !=
             std::end(valid_chars);
  });
}

static bool hasValidChars1D(const std::string& expression)
{
  const char valid_chars[] = {'x', '+', '-', '*', '(', ')', '^'};
  return std::all_of(expression.begin(), expression.end(), [&](char c) {
    return std::isspace(c) || std::isdigit(c) ||
           std::find(std::begin(valid_chars), std::end(valid_chars), c) !=
             std::end(valid_chars);
  });
}

static inline bool hasValidChars(const std::string& expression, int dimension)
{
  if (dimension == 1)
    return hasValidChars1D(expression);
  else
    return hasValidChars2D(expression);
}

template <typename Polynomial_d>
boost::optional<Polynomial_d>
AlgebraicCurveParser<Polynomial_d>::operator()(const std::string& expression)
{
  using Traits = CGAL::Polynomial_traits_d<Polynomial_d>;
  using iterator_type = std::string::const_iterator;

  if (!hasValidChars(expression, Traits::d)) return {};

  PolynomialParser<Polynomial_d, iterator_type, ascii::space_type> pparser;
  std::string::const_iterator iter = expression.begin();
  std::string::const_iterator end = expression.end();

  // parsing goes on here
  Polynomial_d poly;
  bool r = qi::phrase_parse(iter, end, pparser, ascii::space, poly);

  if (r && iter == end && !pparser.error)
    return poly;
  else
    return {};
}

#ifdef CGAL_USE_CORE
// don't want to include ArrangementTypes.h
// makes compilation slower
// template class
// AlgebraicCurveParser<demo_types::Alg_seg_traits::Polynomial_2>;
// AlgebraicCurveParser<demo_types::Rational_traits::Polynomial_1>;
#include <CGAL/Algebraic_kernel_d_1.h>
#include <CGAL/Arr_algebraic_segment_traits_2.h>

template struct AlgebraicCurveParser<
  CGAL::Arr_algebraic_segment_traits_2<CORE::BigInt>::Polynomial_2>;

template struct AlgebraicCurveParser<
  typename CGAL::Algebraic_kernel_d_1<CORE::BigInt>::Polynomial_1>;
#endif


// This file is part of the pyMOR project (http://www.pymor.org).
// Copyright 2013-2018 pyMOR developers and contributors. All rights reserved.
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef EXAMPLE_HH
#define EXAMPLE_HH

#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_bicgstab.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/solution_transfer.h>
#include <deal.II/numerics/vector_tools.h>

#include <functional>

#include "rhs.hh"

using namespace dealii;

class ElasticityExample {

public:
  static constexpr size_t dim{2};

  ElasticityExample(int refine_steps);
  ~ElasticityExample();

  typedef double Number;
  typedef std::map<std::string, std::vector<Number>> Parameter;
  typedef Vector<Number> VectorType;

  void visualize(const VectorType& solution, std::string filename) const;
  VectorType solve(const Parameter& param);

  const SparseMatrix<Number>& lambda_mat() const;
  const SparseMatrix<Number>& mu_mat() const;
  const SparseMatrix<Number>& h1_mat() const;
  const Vector<Number>& rhs() const;

  Number h1_0_semi_norm(const Vector<Number>& v) const;
  Number energy_norm(const Vector<Number>& v) const;

  VectorType transfer_to(int refine_steps, const VectorType& v);

private:
  void setup_system();
  void assemble_h1();
  void assemble_system();
  void _solve(Parameter param, VectorType& solution);

protected:
  void refine_global(int refine_steps = 1);

  Triangulation<dim> triangulation_;
  DoFHandler<dim> dof_handler_;

  FESystem<dim> fe_;

  SparsityPattern sparsity_pattern_;
  SparseMatrix<Number> lambda_system_matrix_, mu_system_matrix_, h1_matrix_;
  Vector<Number> system_rhs_, tmp_data_;
};

#endif // EXAMPLE_HH


// root_finding_example.cpp

// Copyright Paul A. Bristow 2010, 2015

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Example of finding roots using Newton-Raphson, Halley.

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//#define BOOST_MATH_INSTRUMENT

/*
This example demonstrates how to use the various tools for root finding
taking the simple cube root function (`cbrt`) as an example.

It shows how use of derivatives can improve the speed.
(But is only a demonstration and does not try to make the ultimate improvements of 'real-life'
implementation of `boost::math::cbrt`, mainly by using a better computed initial 'guess'
at `<boost/math/special_functions/cbrt.hpp>`).

Then we show how a higher root (fifth) can be computed,
and in `root_finding_n_example.cpp` a generic method
for the ['n]th root that constructs the derivatives at compile-time,

These methods should be applicable to other functions that can be differentiated easily.

First some `#includes` that will be needed.

[tip For clarity, `using` statements are provided to list what functions are being used in this example:
you can of course partly or fully qualify the names in other ways.
(For your application, you may wish to extract some parts into header files,
but you should never use `using` statements globally in header files).]
*/

//[root_finding_include_1

#include <boost/math/tools/roots.hpp>
//using boost::math::policies::policy;
//using boost::math::tools::newton_raphson_iterate;
//using boost::math::tools::halley_iterate; //
//using boost::math::tools::eps_tolerance; // Binary functor for specified number of bits.
//using boost::math::tools::bracket_and_solve_root;
//using boost::math::tools::toms748_solve;

#include <boost/math/special_functions/next.hpp> // For float_distance.
#include <tuple> // for std::tuple and std::make_tuple.
#include <boost/math/special_functions/cbrt.hpp> // For boost::math::cbrt.

//] [/root_finding_include_1]

// using boost::math::tuple;
// using boost::math::make_tuple;
// using boost::math::tie;
// which provide convenient aliases for various implementations,
// including std::tr1, depending on what is available.

#include <iostream>
//using std::cout; using std::endl;
#include <iomanip>
//using std::setw; using std::setprecision;
#include <limits>
//using std::numeric_limits;

/*

Let's suppose we want to find the root of a number ['a], and to start, compute the cube root.

So the equation we want to solve is:

__spaces ['f](x) = x[cubed] - a

We will first solve this without using any information
about the slope or curvature of the cube root function.

We then show how adding what we can know about this function, first just the slope,
the 1st derivation /f'(x)/, will speed homing in on the solution.

Lastly we show how adding the curvature /f''(x)/ too will speed convergence even more.

*/

//[root_finding_noderiv_1

template <class T>
struct cbrt_functor_noderiv
{
  //  cube root of x using only function - no derivatives.
  cbrt_functor_noderiv(T const& to_find_root_of) : a(to_find_root_of)
  { /* Constructor just stores value a to find root of. */ }
  T operator()(T const& x)
  {
    T fx = x*x*x - a; // Difference (estimate x^3 - a).
    return fx;
  }
private:
  T a; // to be 'cube_rooted'.
};
//] [/root_finding_noderiv_1

/*
Implementing the cube root function itself is fairly trivial now:
the hardest part is finding a good approximation to begin with.
In this case we'll just divide the exponent by three.
(There are better but more complex guess algorithms used in 'real-life'.)

Cube root function is 'Really Well Behaved' in that it is monotonic
and has only one root (we leave negative values 'as an exercise for the student').
*/

//[root_finding_noderiv_2

template <class T>
T cbrt_noderiv(T x)
{
  // return cube root of x using bracket_and_solve (no derivatives).
  using namespace std;                          // Help ADL of std functions.
  using namespace boost::math::tools;           // For bracket_and_solve_root.

  int exponent;
  frexp(x, &exponent);                          // Get exponent of z (ignore mantissa).
  T guess = ldexp(1., exponent/3);              // Rough guess is to divide the exponent by three.
  T factor = 2;                                 // How big steps to take when searching.

  const boost::uintmax_t maxit = 20;            // Limit to maximum iterations.
  boost::uintmax_t it = maxit;                  // Initally our chosen max iterations, but updated with actual.
  bool is_rising = true;                        // So if result if guess^3 is too low, then try increasing guess.
  int digits = std::numeric_limits<T>::digits;  // Maximum possible binary digits accuracy for type T.
  // Some fraction of digits is used to control how accurate to try to make the result.
  int get_digits = digits - 3;                  // We have to have a non-zero interval at each step, so
                                                // maximum accuracy is digits - 1.  But we also have to
                                                // allow for inaccuracy in f(x), otherwise the last few
                                                // iterations just thrash around.
  eps_tolerance<T> tol(get_digits);             // Set the tolerance.
  std::pair<T, T> r = bracket_and_solve_root(cbrt_functor_noderiv<T>(x), guess, factor, is_rising, tol, it);
  return r.first + (r.second - r.first)/2;      // Midway between brackets is our result, if necessary we could
                                                // return the result as an interval here.
}

/*`

[note The final parameter specifying a maximum number of iterations is optional.
However, it defaults to `boost::uintmax_t maxit = (std::numeric_limits<boost::uintmax_t>::max)();`
which is `18446744073709551615` and is more than anyone would wish to wait for!

So it may be wise to chose some reasonable estimate of how many iterations may be needed,
In this case the function is so well behaved that we can chose a low value of 20.

Internally when Boost.Math uses these functions, it sets the maximum iterations to
`policies::get_max_root_iterations<Policy>();`.]

Should we have wished we can show how many iterations were used in `bracket_and_solve_root`
(this information is lost outside `cbrt_noderiv`), for example with:

  if (it >= maxit)
  {
    std::cout << "Unable to locate solution in " << maxit << " iterations:"
      " Current best guess is between " << r.first << " and " << r.second << std::endl;
  }
  else
  {
    std::cout << "Converged after " << it << " (from maximum of " << maxit << " iterations)." << std::endl;
  }

for output like

  Converged after 11 (from maximum of 20 iterations).
*/
//] [/root_finding_noderiv_2]

// Cube root with 1st derivative (slope)

/*
We now solve the same problem, but using more information about the function,
to show how this can speed up finding the best estimate of the root.

For the root function, the 1st differential (the slope of the tangent to a curve at any point) is known.

If you need some reminders then
[@http://en.wikipedia.org/wiki/Derivative#Derivatives_of_elementary_functions Derivatives of elementary functions]
may help.

Using the rule that the derivative of ['x[super n]] for positive n (actually all nonzero n) is ['n x[super n-1]],
allows us to get the 1st differential as ['3x[super 2]].

To see how this extra information is used to find a root, view
[@http://en.wikipedia.org/wiki/Newton%27s_method Newton-Raphson iterations]
and the [@http://en.wikipedia.org/wiki/Newton%27s_method#mediaviewer/File:NewtonIteration_Ani.gif animation].

We need to define a different functor `cbrt_functor_deriv` that returns
both the evaluation of the function to solve, along with its first derivative:

To \'return\' two values, we use a `std::pair` of floating-point values
(though we could equally have used a std::tuple):
*/

//[root_finding_1_deriv_1

template <class T>
struct cbrt_functor_deriv
{ // Functor also returning 1st derivative.
  cbrt_functor_deriv(T const& to_find_root_of) : a(to_find_root_of)
  { // Constructor stores value a to find root of,
    // for example: calling cbrt_functor_deriv<T>(a) to use to get cube root of a.
  }
  std::pair<T, T> operator()(T const& x)
  {
    // Return both f(x) and f'(x).
    T fx = x*x*x - a;                // Difference (estimate x^3 - value).
    T dx =  3 * x*x;                 // 1st derivative = 3x^2.
    return std::make_pair(fx, dx);   // 'return' both fx and dx.
  }
private:
  T a;                               // Store value to be 'cube_rooted'.
};

/*`Our cube root function is now:*/

template <class T>
T cbrt_deriv(T x)
{
  // return cube root of x using 1st derivative and Newton_Raphson.
  using namespace boost::math::tools;
  int exponent;
  frexp(x, &exponent);                                // Get exponent of z (ignore mantissa).
  T guess = ldexp(1., exponent/3);                    // Rough guess is to divide the exponent by three.
  T min = ldexp(0.5, exponent/3);                     // Minimum possible value is half our guess.
  T max = ldexp(2., exponent/3);                      // Maximum possible value is twice our guess.
  const int digits = std::numeric_limits<T>::digits;  // Maximum possible binary digits accuracy for type T.
  int get_digits = static_cast<int>(digits * 0.6);    // Accuracy doubles with each step, so stop when we have
                                                      // just over half the digits correct.
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = newton_raphson_iterate(cbrt_functor_deriv<T>(x), guess, min, max, get_digits, it);
  return result;
}

//] [/root_finding_1_deriv_1]

/*
[h3:cbrt_2_derivatives Cube root with 1st & 2nd derivative (slope & curvature)]

Finally we define yet another functor `cbrt_functor_2deriv` that returns
both the evaluation of the function to solve,
along with its first *and second* derivatives:

__spaces[''f](x) = 6x

To \'return\' three values, we use a `tuple` of three floating-point values:
*/

//[root_finding_2deriv_1

template <class T>
struct cbrt_functor_2deriv
{
  // Functor returning both 1st and 2nd derivatives.
  cbrt_functor_2deriv(T const& to_find_root_of) : a(to_find_root_of)
  { // Constructor stores value a to find root of, for example:
    // calling cbrt_functor_2deriv<T>(x) to get cube root of x,
  }
  std::tuple<T, T, T> operator()(T const& x)
  {
    // Return both f(x) and f'(x) and f''(x).
    T fx = x*x*x - a;                     // Difference (estimate x^3 - value).
    T dx = 3 * x*x;                       // 1st derivative = 3x^2.
    T d2x = 6 * x;                        // 2nd derivative = 6x.
    return std::make_tuple(fx, dx, d2x);  // 'return' fx, dx and d2x.
  }
private:
  T a; // to be 'cube_rooted'.
};

/*`Our cube root function is now:*/

template <class T>
T cbrt_2deriv(T x)
{
  // return cube root of x using 1st and 2nd derivatives and Halley.
  //using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools;
  int exponent;
  frexp(x, &exponent);                                // Get exponent of z (ignore mantissa).
  T guess = ldexp(1., exponent/3);                    // Rough guess is to divide the exponent by three.
  T min = ldexp(0.5, exponent/3);                     // Minimum possible value is half our guess.
  T max = ldexp(2., exponent/3);                      // Maximum possible value is twice our guess.
  const int digits = std::numeric_limits<T>::digits;  // Maximum possible binary digits accuracy for type T.
  // digits used to control how accurate to try to make the result.
  int get_digits = static_cast<int>(digits * 0.4);    // Accuracy triples with each step, so stop when just
                                                      // over one third of the digits are correct.
  boost::uintmax_t maxit = 20;
  T result = halley_iterate(cbrt_functor_2deriv<T>(x), guess, min, max, get_digits, maxit);
  return result;
}

//] [/root_finding_2deriv_1]

//[root_finding_2deriv_lambda

template <class T>
T cbrt_2deriv_lambda(T x)
{
   // return cube root of x using 1st and 2nd derivatives and Halley.
   //using namespace std;  // Help ADL of std functions.
   using namespace boost::math::tools;
   int exponent;
   frexp(x, &exponent);                                // Get exponent of z (ignore mantissa).
   T guess = ldexp(1., exponent / 3);                    // Rough guess is to divide the exponent by three.
   T min = ldexp(0.5, exponent / 3);                     // Minimum possible value is half our guess.
   T max = ldexp(2., exponent / 3);                      // Maximum possible value is twice our guess.
   const int digits = std::numeric_limits<T>::digits;  // Maximum possible binary digits accuracy for type T.
   // digits used to control how accurate to try to make the result.
   int get_digits = static_cast<int>(digits * 0.4);    // Accuracy triples with each step, so stop when just
   // over one third of the digits are correct.
   boost::uintmax_t maxit = 20;
   T result = halley_iterate(
      // lambda function:
      [x](const T& g){ return std::make_tuple(g * g * g - x, 3 * g * g, 6 * g); },
      guess, min, max, get_digits, maxit);
   return result;
}

//] [/root_finding_2deriv_lambda]
/*

[h3 Fifth-root function]
Let's now suppose we want to find the [*fifth root] of a number ['a].

The equation we want to solve is :

__spaces['f](x) = x[super 5] - a

If your differentiation is a little rusty
(or you are faced with an equation whose complexity is daunting),
then you can get help, for example from the invaluable
[@http://www.wolframalpha.com/ WolframAlpha site.]

For example, entering the commmand: `differentiate x ^ 5`

or the Wolfram Language command: ` D[x ^ 5, x]`

gives the output: `d/dx(x ^ 5) = 5 x ^ 4`

and to get the second differential, enter: `second differentiate x ^ 5`

or the Wolfram Language command: `D[x ^ 5, { x, 2 }]`

to get the output: `d ^ 2 / dx ^ 2(x ^ 5) = 20 x ^ 3`

To get a reference value, we can enter: [^fifth root 3126]

or: `N[3126 ^ (1 / 5), 50]`

to get a result with a precision of 50 decimal digits:

5.0003199590478625588206333405631053401128722314376

(We could also get a reference value using Boost.Multiprecision - see below).

The 1st and 2nd derivatives of x[super 5] are:

__spaces['f]\'(x) = 5x[super 4]

__spaces['f]\'\'(x) = 20x[super 3]

*/

//[root_finding_fifth_1
//] [/root_finding_fifth_1]

//[root_finding_fifth_functor_2deriv

/*`Using these expressions for the derivatives, the functor is:
*/

template <class T>
struct fifth_functor_2deriv
{
  // Functor returning both 1st and 2nd derivatives.
  fifth_functor_2deriv(T const& to_find_root_of) : a(to_find_root_of)
  { /* Constructor stores value a to find root of, for example: */ }

  std::tuple<T, T, T> operator()(T const& x)
  {
    // Return both f(x) and f'(x) and f''(x).
    T fx = boost::math::pow<5>(x) - a;    // Difference (estimate x^3 - value).
    T dx = 5 * boost::math::pow<4>(x);    // 1st derivative = 5x^4.
    T d2x = 20 * boost::math::pow<3>(x);  // 2nd derivative = 20 x^3
    return std::make_tuple(fx, dx, d2x);  // 'return' fx, dx and d2x.
  }
private:
  T a;                                    // to be 'fifth_rooted'.
}; // struct fifth_functor_2deriv

//] [/root_finding_fifth_functor_2deriv]

//[root_finding_fifth_2deriv

/*`Our fifth-root function is now:
*/

template <class T>
T fifth_2deriv(T x)
{
  // return fifth root of x using 1st and 2nd derivatives and Halley.
  using namespace std;                  // Help ADL of std functions.
  using namespace boost::math::tools;   // for halley_iterate.

  int exponent;
  frexp(x, &exponent);                  // Get exponent of z (ignore mantissa).
  T guess = ldexp(1., exponent / 5);    // Rough guess is to divide the exponent by five.
  T min = ldexp(0.5, exponent / 5);     // Minimum possible value is half our guess.
  T max = ldexp(2., exponent / 5);      // Maximum possible value is twice our guess.
  // Stop when slightly more than one of the digits are correct:
  const int digits = static_cast<int>(std::numeric_limits<T>::digits * 0.4);
  const boost::uintmax_t maxit = 50;
  boost::uintmax_t it = maxit;
  T result = halley_iterate(fifth_functor_2deriv<T>(x), guess, min, max, digits, it);
  return result;
}

//] [/root_finding_fifth_2deriv]

int main()
{
  std::cout << "Root finding  Examples." << std::endl;
  std::cout.precision(std::numeric_limits<double>::max_digits10);
  // Show all possibly significant decimal digits for double.
  // std::cout.precision(std::numeric_limits<double>::digits10);
  // Show all guaranteed significant decimal digits for double.

//[root_finding_main_1
  try
  {
    double threecubed = 27.;   // Value that has an *exactly representable* integer cube root.
    double threecubedp1 = 28.; // Value whose cube root is *not* exactly representable.

    std::cout << "cbrt(28) " << boost::math::cbrt(28.) << std::endl; // boost::math:: version of cbrt.
    std::cout << "std::cbrt(28) " << std::cbrt(28.) << std::endl;    // std:: version of cbrt.
    std::cout <<" cast double " << static_cast<double>(3.0365889718756625194208095785056696355814539772481111) << std::endl;

    // Cube root using bracketing:
    double r = cbrt_noderiv(threecubed);
    std::cout << "cbrt_noderiv(" << threecubed << ") = " << r << std::endl;
    r = cbrt_noderiv(threecubedp1);
    std::cout << "cbrt_noderiv(" << threecubedp1 << ") = " << r << std::endl;
//] [/root_finding_main_1]
    //[root_finding_main_2

    // Cube root using 1st differential Newton-Raphson:
    r = cbrt_deriv(threecubed);
    std::cout << "cbrt_deriv(" << threecubed << ") = " << r << std::endl;
    r = cbrt_deriv(threecubedp1);
    std::cout << "cbrt_deriv(" << threecubedp1 << ") = " << r << std::endl;

    // Cube root using Halley with 1st and 2nd differentials.
    r = cbrt_2deriv(threecubed);
    std::cout << "cbrt_2deriv(" << threecubed << ") = " << r << std::endl;
    r = cbrt_2deriv(threecubedp1);
    std::cout << "cbrt_2deriv(" << threecubedp1 << ") = " << r << std::endl;

    // Cube root using lambda's:
    r = cbrt_2deriv_lambda(threecubed);
    std::cout << "cbrt_2deriv(" << threecubed << ") = " << r << std::endl;
    r = cbrt_2deriv_lambda(threecubedp1);
    std::cout << "cbrt_2deriv(" << threecubedp1 << ") = " << r << std::endl;

    // Fifth root.

    double fivepowfive = 3125; // Example of a value that has an exact integer fifth root.
    // Exact value of fifth root is exactly 5.
    std::cout << "Fifth root  of " << fivepowfive << " is " << 5 << std::endl;

    double fivepowfivep1 = fivepowfive + 1; // Example of a value whose fifth root is *not* exactly representable.
    // Value of fifth root is 5.0003199590478625588206333405631053401128722314376 (50 decimal digits precision)
    // and to std::numeric_limits<double>::max_digits10 double precision (usually 17) is

    double root5v2 = static_cast<double>(5.0003199590478625588206333405631053401128722314376);
        std::cout << "Fifth root  of " << fivepowfivep1 << " is " << root5v2 << std::endl;

    // Using Halley with 1st and 2nd differentials.
    r = fifth_2deriv(fivepowfive);
    std::cout << "fifth_2deriv(" << fivepowfive << ") = " << r << std::endl;
    r = fifth_2deriv(fivepowfivep1);
    std::cout << "fifth_2deriv(" << fivepowfivep1 << ") = " << r << std::endl;
//] [/root_finding_main_?]
  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}  // int main()

//[root_finding_example_output
/*`
Normal output is:

[pre
  root_finding_example.cpp
  Generating code
  Finished generating code
  root_finding_example.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\root_finding_example.exe
  Cube Root finding (cbrt) Example.
  Iterations 10
  cbrt_1(27) = 3
  Iterations 10
  Unable to locate solution in chosen iterations: Current best guess is between 3.0365889718756613 and 3.0365889718756627
  cbrt_1(28) = 3.0365889718756618
  cbrt_1(27) = 3
  cbrt_2(28) = 3.0365889718756627
  Iterations 4
  cbrt_3(27) = 3
  Iterations 5
  cbrt_3(28) = 3.0365889718756627

] [/pre]

to get some (much!) diagnostic output we can add

#define BOOST_MATH_INSTRUMENT

[pre

]
*/
//] [/root_finding_example_output]

/*

cbrt(28) 3.0365889718756622
std::cbrt(28) 3.0365889718756627

*/


//
//=======================================================================
// Copyright (c) 2004 Kristopher Beevers
//
// 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)
//=======================================================================
//

#include <boost/graph/astar_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <boost/random.hpp>
#include <boost/graph/graphviz.hpp>
#include <ctime>
#include <vector>
#include <list>
#include <iostream>
#include <fstream>
#include <math.h> // for sqrt

using namespace boost;
using namespace std;

// auxiliary types
struct location
{
    float y, x; // lat, long
};
typedef float cost;

template < class Name, class LocMap > class city_writer
{
public:
    city_writer(Name n, LocMap l, float _minx, float _maxx, float _miny,
        float _maxy, unsigned int _ptx, unsigned int _pty)
    : name(n)
    , loc(l)
    , minx(_minx)
    , maxx(_maxx)
    , miny(_miny)
    , maxy(_maxy)
    , ptx(_ptx)
    , pty(_pty)
    {
    }
    template < class Vertex >
    void operator()(ostream& out, const Vertex& v) const
    {
        float px = 1 - (loc[v].x - minx) / (maxx - minx);
        float py = (loc[v].y - miny) / (maxy - miny);
        out << "[label=\"" << name[v] << "\", pos=\""
            << static_cast< unsigned int >(ptx * px) << ","
            << static_cast< unsigned int >(pty * py) << "\", fontsize=\"11\"]";
    }

private:
    Name name;
    LocMap loc;
    float minx, maxx, miny, maxy;
    unsigned int ptx, pty;
};

template < class WeightMap > class time_writer
{
public:
    time_writer(WeightMap w) : wm(w) {}
    template < class Edge > void operator()(ostream& out, const Edge& e) const
    {
        out << "[label=\"" << wm[e] << "\", fontsize=\"11\"]";
    }

private:
    WeightMap wm;
};

// euclidean distance heuristic
template < class Graph, class CostType, class LocMap >
class distance_heuristic : public astar_heuristic< Graph, CostType >
{
public:
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    distance_heuristic(LocMap l, Vertex goal) : m_location(l), m_goal(goal) {}
    CostType operator()(Vertex u)
    {
        CostType dx = m_location[m_goal].x - m_location[u].x;
        CostType dy = m_location[m_goal].y - m_location[u].y;
        return ::sqrt(dx * dx + dy * dy);
    }

private:
    LocMap m_location;
    Vertex m_goal;
};

struct found_goal
{
}; // exception for termination

// visitor that terminates when we find the goal
template < class Vertex >
class astar_goal_visitor : public boost::default_astar_visitor
{
public:
    astar_goal_visitor(Vertex goal) : m_goal(goal) {}
    template < class Graph > void examine_vertex(Vertex u, Graph& g)
    {
        if (u == m_goal)
            throw found_goal();
    }

private:
    Vertex m_goal;
};

int main(int argc, char** argv)
{

    // specify some types
    typedef adjacency_list< listS, vecS, undirectedS, no_property,
        property< edge_weight_t, cost > >
        mygraph_t;
    typedef property_map< mygraph_t, edge_weight_t >::type WeightMap;
    typedef mygraph_t::vertex_descriptor vertex;
    typedef mygraph_t::edge_descriptor edge_descriptor;
    typedef std::pair< int, int > edge;

    // specify data
    enum nodes
    {
        Troy,
        LakePlacid,
        Plattsburgh,
        Massena,
        Watertown,
        Utica,
        Syracuse,
        Rochester,
        Buffalo,
        Ithaca,
        Binghamton,
        Woodstock,
        NewYork,
        N
    };
    const char* name[] = { "Troy", "Lake Placid", "Plattsburgh", "Massena",
        "Watertown", "Utica", "Syracuse", "Rochester", "Buffalo", "Ithaca",
        "Binghamton", "Woodstock", "New York" };
    location locations[] = { // lat/long
        { 42.73, 73.68 }, { 44.28, 73.99 }, { 44.70, 73.46 }, { 44.93, 74.89 },
        { 43.97, 75.91 }, { 43.10, 75.23 }, { 43.04, 76.14 }, { 43.17, 77.61 },
        { 42.89, 78.86 }, { 42.44, 76.50 }, { 42.10, 75.91 }, { 42.04, 74.11 },
        { 40.67, 73.94 }
    };
    edge edge_array[]
        = { edge(Troy, Utica), edge(Troy, LakePlacid), edge(Troy, Plattsburgh),
              edge(LakePlacid, Plattsburgh), edge(Plattsburgh, Massena),
              edge(LakePlacid, Massena), edge(Massena, Watertown),
              edge(Watertown, Utica), edge(Watertown, Syracuse),
              edge(Utica, Syracuse), edge(Syracuse, Rochester),
              edge(Rochester, Buffalo), edge(Syracuse, Ithaca),
              edge(Ithaca, Binghamton), edge(Ithaca, Rochester),
              edge(Binghamton, Troy), edge(Binghamton, Woodstock),
              edge(Binghamton, NewYork), edge(Syracuse, Binghamton),
              edge(Woodstock, Troy), edge(Woodstock, NewYork) };
    unsigned int num_edges = sizeof(edge_array) / sizeof(edge);
    cost weights[] = { // estimated travel time (mins)
        96, 134, 143, 65, 115, 133, 117, 116, 74, 56, 84, 73, 69, 70, 116, 147,
        173, 183, 74, 71, 124
    };

    // create graph
    mygraph_t g(N);
    WeightMap weightmap = get(edge_weight, g);
    for (std::size_t j = 0; j < num_edges; ++j)
    {
        edge_descriptor e;
        bool inserted;
        boost::tie(e, inserted)
            = add_edge(edge_array[j].first, edge_array[j].second, g);
        weightmap[e] = weights[j];
    }

    // pick random start/goal
    boost::mt19937 gen(std::time(0));
    vertex start = random_vertex(g, gen);
    vertex goal = random_vertex(g, gen);

    cout << "Start vertex: " << name[start] << endl;
    cout << "Goal vertex: " << name[goal] << endl;

    ofstream dotfile;
    dotfile.open("test-astar-cities.dot");
    write_graphviz(dotfile, g,
        city_writer< const char**, location* >(
            name, locations, 73.46, 78.86, 40.67, 44.93, 480, 400),
        time_writer< WeightMap >(weightmap));

    vector< mygraph_t::vertex_descriptor > p(num_vertices(g));
    vector< cost > d(num_vertices(g));
    try
    {
        // call astar named parameter interface
        astar_search_tree(g, start,
            distance_heuristic< mygraph_t, cost, location* >(locations, goal),
            predecessor_map(
                make_iterator_property_map(p.begin(), get(vertex_index, g)))
                .distance_map(
                    make_iterator_property_map(d.begin(), get(vertex_index, g)))
                .visitor(astar_goal_visitor< vertex >(goal)));
    }
    catch (found_goal fg)
    { // found a path to the goal
        list< vertex > shortest_path;
        for (vertex v = goal;; v = p[v])
        {
            shortest_path.push_front(v);
            if (p[v] == v)
                break;
        }
        cout << "Shortest path from " << name[start] << " to " << name[goal]
             << ": ";
        list< vertex >::iterator spi = shortest_path.begin();
        cout << name[start];
        for (++spi; spi != shortest_path.end(); ++spi)
            cout << " -> " << name[*spi];
        cout << endl << "Total travel time: " << d[goal] << endl;
        return 0;
    }

    cout << "Didn't find a path from " << name[start] << "to" << name[goal]
         << "!" << endl;
    return 0;
}


#include <utils/LQR.hpp>

#include <Eigen/Eigenvalues>
#include <iostream>

#include <utils/utilities.hpp>

using namespace std;

namespace sejong{
    void LQR(const Matrix & A,
             const Matrix & B,
             const Matrix & Q,
             const Matrix & R,
             Matrix & S, bool descrete){
        int n = A.rows();

        Matrix Z(2*n, 2*n);

        if(!descrete){
            // sejong::pretty_print(A, std::cout, "A","");
            // sejong::pretty_print(B, std::cout, "B","");
            // sejong::pretty_print(Q, std::cout, "Q","");
            // sejong::pretty_print(R, std::cout, "R","");

            Z.block(0, 0, n, n) = A;

            Z.block(0, n, n, n) = -B * R.inverse() * B.transpose();

            Z.block(n, 0, n, n) = -Q;

            Z.block(n, n, n, n) = -A.transpose();

        } else {

            Matrix Ainv = A.inverse();
            Z.block(0, 0, n, n) = A + B * R.inverse() * B.transpose() * Ainv.transpose() * Q;
            Z.block(0, n, n, n) = -B * R.inverse() * B.transpose() * Ainv.transpose();
            Z.block(n, 0, n, n) = -Ainv.transpose() * Q;
            Z.block(n, n, n, n) = Ainv.transpose();

        }

        Eigen::EigenSolver<Matrix> es(Z);
        Eigen::MatrixXcd V(2*n, n);
        // cout << "The eigenvalues of Z are:" << endl << es.eigenvalues() << endl;

        int col_num(0);
        for (int i(0); i<2*n; ++i){
            if(!descrete){
                if( (es.eigenvalues()[i]).real() < 0.0){
                    V.col(col_num) = es.eigenvectors().col(i);
                    ++col_num;
                }
            } else {
                if( ((es.eigenvalues()[i]).real())*( (es.eigenvalues()[i]).real()) < 1.0){
                    V.col(col_num) = es.eigenvectors().col(i);
                    ++col_num;
                }
            }
        }
        // cout << "The eigenvalues of Z are:" << endl << es.eigenvalues() << endl;
        // cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl;
        // cout << "Finally, S = " << endl << S << endl;

        Eigen::MatrixXcd Stmp = V.block(n, 0, n, n) * (V.block(0, 0, n, n)).inverse();
        S = Stmp.real();
    }
}