#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/karma.hpp>

#include <iostream>
#include <string>
#include <vector>

namespace client{
  template <typename Iterator>
    bool parse_numbers(Iterator first, Iterator last, std::vector<double>& v){
      using boost::spirit::qi::double_;
      using boost::spirit::qi::phrase_parse;
      using boost::spirit::ascii::space;

      bool r = phrase_parse(first, last, double_ % ',', space, v);
      if(first != last)
        return false;
      return r;
    }

  template <typename OutputIterator>
    bool generate_numbers(OutputIterator& sink, std::vector<double> const& v){
      using boost::spirit::karma::double_;
      using boost::spirit::karma::generate_delimited;
      using boost::spirit::ascii::space;

      bool r = generate_delimited(
          sink,
          double_ % ',',
          space,
          v
      );
      return r;
    }
}

int main(){
  std::string str;
  while(getline(std::cin, str)){
    if(str.empty() || str[0] == 'q' || str[0] == 'Q')
      break;

    std::vector<double> v;
    if(client::parse_numbers(str.begin(), str.end(), v)){
      std::string generated;
      std::back_insert_iterator<std::string> sink(generated);
      if(!client::generate_numbers(sink, v)){
        std::cout << "Generating failed!\n";
      }
      else{
        std::cout << "Generated: " << generated << "\n";
      }
    }
    else{
      std::cout << "Parsing failed\n";
    }
  }

  return 0;
}


/*
   Copyright (c) Marshall Clow 2017.

   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)
*/

/// \file  transform_reduce.hpp
/// \brief Combine the (transformed) elements of a sequence (or two) into a single value.
/// \author Marshall Clow

#ifndef BOOST_ALGORITHM_TRANSFORM_REDUCE_HPP
#define BOOST_ALGORITHM_TRANSFORM_REDUCE_HPP

#include <functional>     // for std::plus
#include <iterator>       // for std::iterator_traits

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/value_type.hpp>

namespace boost { namespace algorithm {

template<class InputIterator1, class InputIterator2, class T,
         class BinaryOperation1, class BinaryOperation2>
T transform_reduce(InputIterator1 first1, InputIterator1 last1,
                   InputIterator2 first2, T init,
                 BinaryOperation1 bOp1, BinaryOperation2 bOp2)
{
    for (; first1 != last1; ++first1, (void) ++first2)
        init = bOp1(init, bOp2(*first1, *first2));
    return init;
}

template<class InputIterator, class T,
         class BinaryOperation, class UnaryOperation>
T transform_reduce(InputIterator first, InputIterator last,
                   T init, BinaryOperation bOp, UnaryOperation uOp)
{
    for (; first != last; ++first)
        init = bOp(init, uOp(*first));
    return init;
}

template<class InputIterator1, class InputIterator2, class T>
T transform_reduce(InputIterator1 first1, InputIterator1 last1,
                   InputIterator2 first2, T init)
{
    return boost::algorithm::transform_reduce(first1, last1, first2, init,
                            std::plus<T>(), std::multiplies<T>());
}

}} // namespace boost and algorithm

#endif // BOOST_ALGORITHM_TRANSFORM_REDUCE_HPP


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013 MetaScale SAS
//         Copyright 2012        Domagoj Saric, Little Endian Ltd.
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/sdk/bench/config.hpp>
#include <nt2/sdk/bench/details/median.hpp>
#include <boost/dispatch/attributes.hpp>
#include <algorithm>

namespace nt2{ namespace details
{
  template <typename T> inline
  BOOST_DISPATCH_NOTHROW BOOST_DISPATCH_NOINLINE
  T median_impl( boost::iterator_range<T *> const data )
  {
    typedef typename boost::iterator_range<T *>::iterator iterator;

    std::size_t const data_size( data.size()                  );
    iterator    const target   ( data.begin() + data_size / 2 );
    std::nth_element( data.begin(), target, data.end() );

    T const result1( *target );
    if ( data_size % 2 )
      return result1;
    else
    {
      T const result2( *( target - 1 ) );
      return ( result1 + result2 ) / 2;
    }
  }

  template  NT2_TEST_BENCHMARK_DECL nt2::uint32_t
  median_impl<nt2::uint32_t>(boost::iterator_range<nt2::uint32_t*>);

  template  NT2_TEST_BENCHMARK_DECL nt2::uint64_t
  median_impl<nt2::uint64_t>(boost::iterator_range<nt2::uint64_t*>);

  template  NT2_TEST_BENCHMARK_DECL double
  median_impl<double       >(boost::iterator_range<double*> );
} }


#ifndef nomad__src__autodiff__first_order_hpp
#define nomad__src__autodiff__first_order_hpp

#include <iomanip>
#include <string>
#include <type_traits>

#include <Eigen/Core>

#include <src/var/var.hpp>
#include <src/autodiff/exceptions.hpp>

namespace nomad {

  template<class T_var>
  void first_order_forward_adj(const T_var& v) {
    for (nomad_idx_t i = 1; i <= v.node(); ++i)
      var_nodes_[i].first_order_forward_adj();
  }

  template<class T_var>
  void first_order_reverse_adj(const T_var& v) {
    var_nodes_[v.node()].first_grad() = 1.0;
    for (nomad_idx_t i = v.node(); i > 0; --i)
      var_nodes_[i].first_order_reverse_adj();
  }

  template <typename F>
  typename std::enable_if<is_var<typename F::var_type>::value && F::var_type::order() >= 1, void >::type
  gradient(const F& functional,
           const Eigen::VectorXd& x,
           double& f,
           Eigen::VectorXd& g) {

    reset();

    try {

      auto f_var = functional(x);

      f = f_var.first_val();
      first_order_reverse_adj(f_var);

      for (eigen_idx_t i = 0; i < x.size(); ++i)
      g(i) = var_nodes_[i + 1].first_grad();

      reset();

    } catch (nomad_error& e) {
      reset();
      throw e;
    }

  }

  template <typename F>
  void gradient(const F& functional,
                const Eigen::VectorXd& x,
                Eigen::VectorXd& g) {
    double f;
    gradient(functional, x, f, g);
  }

  template <typename F>
  typename std::enable_if<is_var<typename F::var_type>::value && F::var_type::order() >= 0, void >::type
  finite_diff_gradient(const F& functional,
                       const Eigen::VectorXd& x,
                       Eigen::VectorXd& g,
                       const double epsilon = 1e-6) {

    Eigen::VectorXd x_dynam(x);

    for (eigen_idx_t i = 0; i < x.size(); ++i) {

      double delta_f = 0;

      x_dynam(i) += epsilon;
      auto v1 = functional(x_dynam);
      delta_f += v1.first_val();
      reset();

      x_dynam(i) -= 2.0 * epsilon;
      auto v2 = functional(x_dynam);
      delta_f -= v2.first_val();
      reset();

      x_dynam(i) += epsilon;

      delta_f /= 2.0 * epsilon;

      g(i) = delta_f;

    }

  }

  template <typename F>
  void test_gradient(const F& functional,
                     const Eigen::VectorXd& x,
                     const double epsilon = 1e-6) {

    Eigen::VectorXd auto_grad(x.size());
    try {
      gradient(functional, x, auto_grad);
    } catch (std::runtime_error& e) {
      std::cout << "Cannot compute Gradient Test" << std::endl;
      throw e;
    }

    Eigen::VectorXd diff_grad(x.size());
    try {
      finite_diff_gradient(functional, x, diff_grad, epsilon);
    } catch (std::runtime_error& e) {
      std::cout << "Cannot compute Gradient Test" << std::endl;
      throw e;
    }

    std::cout.precision(6);
    int width = 12;
    int n_column = 4;

    std::cout << "Gradient Test:" << std::endl;
    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;
    std::cout << "    "
              << std::setw(width) << std::left << "Component"
              << std::setw(width) << std::left << "Automatic"
              << std::setw(width) << std::left << "Finite"
              << std::setw(width) << std::left << "Delta / "
              << std::endl;
    std::cout << "    "
              << std::setw(width) << std::left << "(i)"
              << std::setw(width) << std::left << "Derivative"
              << std::setw(width) << std::left << "Difference"
              << std::setw(width) << std::left << "Stepsize^{2}"
              << std::endl;
    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;

    Eigen::VectorXd x_dynam(x);

    for (eigen_idx_t i = 0; i < x.size(); ++i) {
      std::cout << "    "
                << std::setw(width) << std::left << i
                << std::setw(width) << std::left << auto_grad(i)
                << std::setw(width) << std::left << diff_grad(i)
                << std::setw(width) << std::left << (auto_grad(i) - diff_grad(i)) / (epsilon * epsilon)
                << std::endl;

    }

    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;
    std::cout << std::endl;

  }

  template <typename F>
  typename std::enable_if<is_var<typename F::var_type>::value && F::var_type::order() >= 1, void >::type
  gradient_dot_vector(const F& functional,
                      const Eigen::VectorXd& x,
                      const Eigen::VectorXd& v,
                      double& f,
                      double& grad_dot_v) {

    reset();

    try {

      auto f_var = functional(x);

      f = f_var.first_val();

      for (eigen_idx_t i = 0; i < x.size(); ++i)
      var_nodes_[i + 1].first_grad() = v(i);

      first_order_forward_adj(f_var);

      grad_dot_v = f_var.first_grad();

      reset();

    } catch (nomad_error& e) {
      reset();
      throw e;
    }

  }

  template <typename F>
  void gradient_dot_vector(const F& functional,
                           const Eigen::VectorXd& x,
                           const Eigen::VectorXd& v,
                           double& grad_dot_v) {
    double f;
    gradient_dot_vector(functional, x, v, f, grad_dot_v);
  }

  template <typename F>
  void test_gradient_dot_vector(const F& functional,
                                const Eigen::VectorXd& x,
                                const Eigen::VectorXd& v) {

    Eigen::VectorXd g_auto(x.size());
    try {
      gradient(functional, x, g_auto);
    } catch (nomad_error& e) {
      std::cout << "Cannot compute Gradient Dot Vector Test" << std::endl;
      std::cout << e.what() << std::endl;
    }

    double g_dot_v = g_auto.dot(v);

    double g_dot_v_auto;
    try {
      gradient_dot_vector(functional, x, v, g_dot_v_auto);
    } catch (nomad_error& e) {
      std::cout << "Cannot compute Gradient Dot Vector Test" << std::endl;
      std::cout << e.what() << std::endl;
    }

    std::cout.precision(6);
    int width = 12;
    int n_column = 2;

    std::cout << "Gradient Dot Vector Test:" << std::endl;
    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;
    std::cout << "    "
              << std::setw(width) << std::left << "Automatic"
              << std::setw(width) << std::left << "Exact"
              << std::endl;
    std::cout << "    "
              << std::setw(width) << std::left << "Derivative"
              << std::setw(width) << std::left << ""
              << std::endl;
    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;

    std::cout << "    "
              << std::setw(width) << std::left << g_dot_v_auto
              << std::setw(width) << std::left << g_dot_v
              << std::endl;

    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;
    std::cout << std::endl;

  }

}

#endif


#include <iostream>
#include <vector>
#include <fstream>
#include <boost/multiprecision/gmp.hpp>

#include "../../external_libs/json.hpp"
#include "../components/order.hpp"
#include "../components/token.hpp"
#include "../api/aux.hpp"
#include "./optimize.hpp"

/************************************** Global variables that are defined as static in various classes *************************************/

int Token::num_tokens{ 0 };                     // static integer variable of class Token, that counts number of tokens in our instance
std::map<std::string, int> Token::idx_tokens;   // static map of class Token, that maps the address of the token to a unique index

/*******************************************************************************************************************************************/

int main()
{

/********************************************************** VARIABLE DECLARATIONS **********************************************************/

    std::vector<Token> tokens;      // the vector containing all the tokens appearing in any order/AMM
    std::vector<Order> orders;      // the vector containing all the "regular" orders
    std::vector<CP_AMM> amms;       // the vector containing all the AMMs

/*******************************************************************************************************************************************/

    nlohmann::json json_file;
    std::ifstream input_file("./sample_instances/sample.json");
    input_file >> json_file;

    // Parsing the input
    parse_json_file(tokens, orders, amms, Token::num_tokens, Token::idx_tokens, json_file);

    // Just printing out the input
    std::cout.precision(std::numeric_limits<boost::multiprecision::mpf_float>::digits10);

    for (auto &i: tokens)
        print_token(i);

    for (auto &i: orders)
        print_order(i, tokens);

    for (auto &i: amms)
        print_cp_amm(i, tokens);

    // solve batch auction
    solve_auction(tokens, orders, amms);

    // return solution

    return 0;
}


//! \file examples/Minkowski_sum_2/approx_offset.cpp
// Computing the approximated offset of a polygon.

#include <fstream>
#include <boost/timer.hpp>

#include <CGAL/approximated_offset_2.h>

#include "bops_circular.h"

typedef CGAL::Polygon_2<Kernel>                         Linear_polygon;

int main(int argc, char* argv[])
{
  // Open the input file and read a polygon.
  const char* filename = (argc > 1) ? argv[1] : "spiked.dat";
  std::ifstream in_file(filename);
  if (! in_file.is_open()) {
    std::cerr << "Failed to open the input file." << std::endl;
    return -1;
  }
  Linear_polygon  P;
  in_file >> P;
  in_file.close();
  std::cout << "Read an input polygon with " << P.size() << " vertices."
            << std::endl;

  // Approximate the offset polygon with radius 5 and error bound 0.00001.
  boost::timer timer;
  Polygon_with_holes_2 offset = CGAL::approximated_offset_2(P, 5, 0.00001);
  double secs = timer.elapsed();

  std::cout << "The offset polygon has " << offset.outer_boundary().size()
            << " vertices, " << offset.number_of_holes() << " holes."
            << std::endl;
  std::cout << "Offset computation took " << secs << " seconds." << std::endl;
  return 0;
}


//
// Copyright (c) 2018 CNRS
//

#ifndef __pinocchio_lie_group_variant_hpp__
#define __pinocchio_lie_group_variant_hpp__

#include "pinocchio/multibody/liegroup/vector-space.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product.hpp"
#include "pinocchio/multibody/liegroup/special-orthogonal.hpp"
#include "pinocchio/multibody/liegroup/special-euclidean.hpp"

#include <boost/variant.hpp>

namespace pinocchio
{
  typedef boost::variant< SpecialOrthogonalOperationTpl<2,double,0>
                         ,SpecialOrthogonalOperationTpl<3,double,0>
                         ,SpecialEuclideanOperationTpl<2,double,0>
                         ,SpecialEuclideanOperationTpl<3,double,0>
                         ,VectorSpaceOperationTpl<1,double>
                         ,VectorSpaceOperationTpl<2,double>
                         ,VectorSpaceOperationTpl<3,double>
                         ,VectorSpaceOperationTpl<Eigen::Dynamic,double>
                        > LieGroupVariant;

}

#endif // ifndef __pinocchio_lie_group_variant_hpp__


//  (C) Copyright John Maddock 2021.
//  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)

#define BOOST_MATH_NO_ATOMIC_INT

#include <boost/math/special_functions/bernoulli.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include "compile_test/test_compile_result.hpp"

void compile_and_link_test()
{
   check_result<boost::multiprecision::cpp_bin_float_50>(boost::math::bernoulli_b2n<boost::multiprecision::cpp_bin_float_50>(4));
}


// Copyright (c) 2008-2017 Emil Dotchevski and Reverge Studios, Inc.

// 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_QVM_E5F500D4618DBE7B51573B33821F
#define BOOST_QVM_E5F500D4618DBE7B51573B33821F

// This file was generated by a program. Do not edit manually.

#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/inline.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/vec_traits.hpp>

namespace boost {
namespace qvm {
template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<A>::rows == 3 &&
                                  mat_traits<A>::cols == 3 &&
                                  vec_traits<B>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    operator*(A const &a, B const &b) {
  typedef typename mat_traits<A>::scalar_type Ta;
  typedef typename vec_traits<B>::scalar_type Tb;
  Ta const a00 = mat_traits<A>::template read_element<0, 0>(a);
  Ta const a01 = mat_traits<A>::template read_element<0, 1>(a);
  Ta const a02 = mat_traits<A>::template read_element<0, 2>(a);
  Ta const a10 = mat_traits<A>::template read_element<1, 0>(a);
  Ta const a11 = mat_traits<A>::template read_element<1, 1>(a);
  Ta const a12 = mat_traits<A>::template read_element<1, 2>(a);
  Ta const a20 = mat_traits<A>::template read_element<2, 0>(a);
  Ta const a21 = mat_traits<A>::template read_element<2, 1>(a);
  Ta const a22 = mat_traits<A>::template read_element<2, 2>(a);
  Tb const b0 = vec_traits<B>::template read_element<0>(b);
  Tb const b1 = vec_traits<B>::template read_element<1>(b);
  Tb const b2 = vec_traits<B>::template read_element<2>(b);
  typedef typename deduce_vec2<A, B, 3>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 3);
  R r;
  vec_traits<R>::template write_element<0>(r) = a00 * b0 + a01 * b1 + a02 * b2;
  vec_traits<R>::template write_element<1>(r) = a10 * b0 + a11 * b1 + a12 * b2;
  vec_traits<R>::template write_element<2>(r) = a20 * b0 + a21 * b1 + a22 * b2;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}

namespace qvm_detail {
template <int R, int C> struct mul_mv_defined;

template <> struct mul_mv_defined<3, 3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<B>::rows == 3 &&
                                  mat_traits<B>::cols == 3 &&
                                  vec_traits<A>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    operator*(A const &a, B const &b) {
  typedef typename vec_traits<A>::scalar_type Ta;
  typedef typename mat_traits<B>::scalar_type Tb;
  Ta const a0 = vec_traits<A>::template read_element<0>(a);
  Ta const a1 = vec_traits<A>::template read_element<1>(a);
  Ta const a2 = vec_traits<A>::template read_element<2>(a);
  Tb const b00 = mat_traits<B>::template read_element<0, 0>(b);
  Tb const b01 = mat_traits<B>::template read_element<0, 1>(b);
  Tb const b02 = mat_traits<B>::template read_element<0, 2>(b);
  Tb const b10 = mat_traits<B>::template read_element<1, 0>(b);
  Tb const b11 = mat_traits<B>::template read_element<1, 1>(b);
  Tb const b12 = mat_traits<B>::template read_element<1, 2>(b);
  Tb const b20 = mat_traits<B>::template read_element<2, 0>(b);
  Tb const b21 = mat_traits<B>::template read_element<2, 1>(b);
  Tb const b22 = mat_traits<B>::template read_element<2, 2>(b);
  typedef typename deduce_vec2<A, B, 3>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 3);
  R r;
  vec_traits<R>::template write_element<0>(r) = a0 * b00 + a1 * b10 + a2 * b20;
  vec_traits<R>::template write_element<1>(r) = a0 * b01 + a1 * b11 + a2 * b21;
  vec_traits<R>::template write_element<2>(r) = a0 * b02 + a1 * b12 + a2 * b22;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}

namespace qvm_detail {
template <int R, int C> struct mul_vm_defined;

template <> struct mul_vm_defined<3, 3> { static bool const value = true; };
} // namespace qvm_detail

} // namespace qvm
} // namespace boost

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_BESSEL_FUNCTIONS_SCALAR_J0_HPP_INCLUDED
#define NT2_BESSEL_FUNCTIONS_SCALAR_J0_HPP_INCLUDED

#include <nt2/bessel/functions/j0.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/cos.hpp>
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/include/constants/digits.hpp>
#include <boost/simd/sdk/math.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( j0_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;
    NT2_FUNCTOR_CALL(1)
    {
      return nt2::j0(result_type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( j0_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      if (is_inf(a0)) return Zero<A0>();
#if defined(BOOST_SIMD_HAS__J0)
      return ::_j0(a0);
#elif defined(BOOST_SIMD_HAS_J0)
      return ::j0(a0);
#else
#error j0 not supported
#endif
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( j0_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type stype;
      if (is_inf(a0)) return Zero<A0>();
      A0 x = nt2::abs(a0);
      // if (x < 1.0e-3f) return oneminus(Quarter<A0>()*sqr(x));
      if (x <= Two<A0>())
      {
        A0 z = sqr(x);
        return (z-single_constant<float,0x40b90fdc> ())*
               horner< NT2_HORNER_COEFF_T(stype, 5,
                      (0xb382511c,
                       0x36d660a0,
                       0xb9d01fb1,
                       0x3c5a6271,
                       0xbe3110a6
                      ) ) > (z);
      }
      A0 q = rec(x);
      const A0 p3 = nt2::sqrt(q) *
        horner< NT2_HORNER_COEFF_T(stype, 8,
               (0xbd8c100e,
                0x3e3ef887,
                0xbe5ba616,
                0x3df54214,
                0xbb69539e,
                0xbd4b8bc1,
                0xb6612dc2,
                0x3f4c422a
                ) ) > (q);
            const A0 xn =  q*
              horner< NT2_HORNER_COEFF_T(stype, 8,
               (0x4201aee0,
                0xc2113945,
                0x418c7f6a,
                0xc09f3306,
                0x3f8040aa,
                0xbe46a57f,
                0x3d84ed6e,
                0xbdffff97
                ) ) > (sqr(q))-Pio_4<A0>();
      return p3*nt2::cos(xn+x);
    }
  };
} }

#endif


#include <SBGATMassProperties.hpp>
#include <SBGATMassPropertiesUQ.hpp>

#include <vtkCleanPolyData.h>
#include <vtkOBJReader.h>

#include <json.hpp>
#include <boost/progress.hpp>
#include <chrono>

int main(){

	// Load the input file
	std::ifstream i("input_file.json");
	nlohmann::json input_data;
	i >> input_data;

	// Path to considered shape
	std::string PATH_SHAPE = input_data["PATH_SHAPE"];

	// Correlation distance (always in m)
	double CORRELATION_DISTANCE =  input_data["CORRELATION_DISTANCE"];

	// Standard deviation in normal error (always in m)
	double ERROR_STANDARD_DEV  = input_data["ERROR_STANDARD_DEV"];

	// Bulk density (always in kg/m^3)
	double DENSITY  = input_data["DENSITY"];

	// True is $PATH_SHAPE points to an .obj shape whose coordinates are expressed in
	// meters, False otherwise
	bool UNIT_IN_METERS  = input_data["UNIT_IN_METERS"];

	// Number of Monte-Carlo samples
	int N_MONTE_CARLO = input_data["N_MONTE_CARLO"];

	// Path to directory in which the output results must be saved
	std::string OUTPUT_DIR = input_data["OUTPUT_DIR"];

	std::cout << "- Path to shape: " << PATH_SHAPE << std::endl;
	std::cout << "- Standard deviation on point coordinates (m) : " << ERROR_STANDARD_DEV << std::endl;
	std::cout << "- Correlation distance (m) : " << CORRELATION_DISTANCE << std::endl;
	std::cout << "- Density (kg/m^3) : " << DENSITY << std::endl;
	std::cout << "- Monte-Carlo draws : " << N_MONTE_CARLO << std::endl;

	// Reading shape model
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName(PATH_SHAPE.c_str());
	reader -> Update();

	// An instance of SBGATMassProperties is created to evaluate the mass properties
	// of the considered shape
	vtkSmartPointer<SBGATMassProperties> mass_filter = vtkSmartPointer<SBGATMassProperties>::New();
	mass_filter -> SetInputConnection(reader -> GetOutputPort());
	mass_filter -> SetDensity(DENSITY);

	// Sets the scaling factor of the input shape
	if(UNIT_IN_METERS){
		mass_filter -> SetScaleMeters();
	} else{
		mass_filter -> SetScaleKiloMeters();
	}

	// Computes the volume, center-of-mass,...
	mass_filter -> Update();

	// An instance of SBGATMassPropertiesUQ is created to perform
	// uncertainty quantification in the inertias of the considered shape
	SBGATMassPropertiesUQ mass_uq;

	// The SBGATMassPropertiesUQ is provided with the reference inertias
	mass_uq.SetModel(mass_filter);

	// Populate the shape vertices covariance
	mass_uq.ComputeVerticesCovarianceGlobal(ERROR_STANDARD_DEV,CORRELATION_DISTANCE);

	// Regularizing the covariance
	int regularized_eigen_values = mass_uq.RegularizeCovariance();

	std::cout << regularized_eigen_values << " eigenvalues were regularized\n";

	arma::mat C_CC = mass_uq.GetCovarianceSquareRoot();
	arma::mat P_CC = mass_uq.GetVerticesCovariance();
	std::cout << "Maximum absolute error in covariance square root: " << arma::abs(P_CC - C_CC * C_CC.t()).max() << std::endl;

	std::cout << "Saving shape covariance ...\n";
	P_CC.save(OUTPUT_DIR + "full_covariance.txt",arma::raw_ascii);

	// Saving baseline slices
	mass_uq.TakeAndSaveSlice(0,OUTPUT_DIR + "baseline_slice_x.txt",1e-6);
	mass_uq.TakeAndSaveSlice(1,OUTPUT_DIR + "baseline_slice_y.txt",1e-6);
	mass_uq.TakeAndSaveSlice(2,OUTPUT_DIR + "baseline_slice_z.txt",1e-6);

	// The analytical uncertainties are computed
	auto start = std::chrono::system_clock::now();
	mass_uq.PrecomputeMassPropertiesPartials();
	const arma::mat & partialIPartialC = mass_uq.GetPartialIPartialC();
	const arma::mat & partialComPartialC = mass_uq.GetPartialComPartialC();
	const arma::rowvec & partialVolumePartialC = mass_uq.GetPartialVolumePartialC();
	double sigma_vol = std::sqrt(arma::dot(partialVolumePartialC.t(),P_CC * partialVolumePartialC.t()));
	arma::mat cov_com = partialComPartialC * P_CC * partialComPartialC.t();
	arma::mat cov_I = partialIPartialC * P_CC * partialIPartialC.t();

	std::cout << "Standard deviation on volume (m^3): " << sigma_vol << " \n";
	cov_com.print("Covariance in center-of-mass (m^4): ");
	cov_I.print("Covariance in unit-density inertia tensor (m^10): ");

	auto end = std::chrono::system_clock::now();
	std::chrono::duration<double> elapsed_seconds = end - start;
	std::cout << "Time elapsed computing analytical uncertainties: " << elapsed_seconds.count() << " s" << std::endl;

	// The containers storing the MC results are created
	arma::vec all_volumes;
	arma::mat all_com;
	arma::mat all_inertia;
	arma::mat deviations;

	start = std::chrono::system_clock::now();

	// A monte-carlo simulation is run
	SBGATMassPropertiesUQ::RunMCUQVolumeCOMInertia(PATH_SHAPE,
		DENSITY,
		UNIT_IN_METERS,
		C_CC,
		N_MONTE_CARLO,
		OUTPUT_DIR,
		std::min(60,N_MONTE_CARLO),
		deviations,
		all_volumes,
		all_com,
		all_inertia);

	double sigma_vol_MC = arma::stddev(all_volumes);
	arma::mat cov_com_MC = arma::cov(all_com.t(),all_com.t());
	arma::mat cov_I_MC = arma::cov(all_inertia.t(),all_inertia.t());

	std::cout << "Standard deviation on volume (m^3): " << sigma_vol_MC << " \n";
	cov_com_MC.print("Covariance in center-of-mass (m^4): ");
	cov_I_MC.print("Covariance in unit-density inertia tensor (m^10): ");
	end = std::chrono::system_clock::now();
	elapsed_seconds = end - start;
	std::cout << "Time elapsed computing Monte Carlo distribution: " << elapsed_seconds.count() << " s" << std::endl;

	std::cout << "Error volume (\%): " << (sigma_vol - sigma_vol_MC)/mass_filter -> GetVolume() << std::endl;
	std::cout << "Error center-of-mass (\%): " << (arma::sqrt(arma::abs(cov_com - cov_com_MC))).max()/arma::norm(mass_filter -> GetCenterOfMass()) << std::endl;
	std::cout << "Error Inertia (\%): " << (arma::sqrt(arma::abs(cov_I - cov_I_MC))).max()/arma::trace(mass_filter -> GetUnitDensityInertiaTensor()) << std::endl;

	std::cout << "##############################################################################\n";

	return 0;
}


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the MPI-IS nor the names
 *     of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written
 *     permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Andreas Orthey */

#include <ompl/base/spaces/special/MobiusStateSpace.h>
#include <ompl/tools/config/MagicConstants.h>
#include <cstring>
#include <cmath>
#include <boost/math/constants/constants.hpp>

using namespace boost::math::double_constants;  // pi
using namespace ompl::base;

MobiusStateSpace::MobiusStateSpace(double intervalMax, double radius) : radius_(radius)
{
    setName("Mobius" + getName());
    type_ = STATE_SPACE_MOBIUS;

    StateSpacePtr SO2(std::make_shared<SO2StateSpace>());
    StateSpacePtr R1(std::make_shared<RealVectorStateSpace>(1));
    R1->as<RealVectorStateSpace>()->setBounds(-intervalMax, +intervalMax);

    addSubspace(SO2, 1.0);
    addSubspace(R1, 1.0);
    lock();
}

double MobiusStateSpace::distance(const State *state1, const State *state2) const
{
    double theta1 = state1->as<MobiusStateSpace::StateType>()->getU();
    double theta2 = state2->as<MobiusStateSpace::StateType>()->getU();

    double diff = theta2 - theta1;

    if (std::abs(diff) <= pi)
    {
        return CompoundStateSpace::distance(state1, state2);
    }
    else
    {
        // requires interpolation over the gluing strip
        const auto *cstate1 = static_cast<const CompoundState *>(state1);
        const auto *cstate2 = static_cast<const CompoundState *>(state2);

        // distance on S1 as usual
        double dist = 0.0;
        dist += weights_[0] * components_[0]->distance(cstate1->components[0], cstate2->components[0]);

        double r1 = state1->as<MobiusStateSpace::StateType>()->getV();
        double r2 = state2->as<MobiusStateSpace::StateType>()->getV();

        r2 = -r2;

        dist += std::sqrt((r2 - r1) * (r2 - r1));
        return dist;
    }
}

void MobiusStateSpace::interpolate(const State *from, const State *to, double t, State *state) const
{
    double theta1 = from->as<MobiusStateSpace::StateType>()->getU();
    double theta2 = to->as<MobiusStateSpace::StateType>()->getU();

    double diff = theta2 - theta1;

    if (std::abs(diff) <= pi)
    {
        // interpolate as it would be a cylinder
        CompoundStateSpace::interpolate(from, to, t, state);
    }
    else
    {
        // requires interpolation over the gluing strip
        const auto *cfrom = static_cast<const CompoundState *>(from);
        const auto *cto = static_cast<const CompoundState *>(to);
        auto *cstate = static_cast<CompoundState *>(state);

        // interpolate S1 as usual
        components_[0]->interpolate(cfrom->components[0], cto->components[0], t, cstate->components[0]);

        double r1 = from->as<MobiusStateSpace::StateType>()->getV();
        double r2 = to->as<MobiusStateSpace::StateType>()->getV();

        // Need to mirror point for interpolation
        r2 = -r2;

        double r = r1 + (r2 - r1) * t;

        // check again if we need to invert (only if we already crossed gluing
        // line)
        double thetaNew = state->as<MobiusStateSpace::StateType>()->getU();
        double diff2 = theta2 - thetaNew;

        if (std::abs(diff2) <= pi)
        {
            r = -r;
        }

        state->as<MobiusStateSpace::StateType>()->setV(r);
    }
}

State *MobiusStateSpace::allocState() const
{
    auto *state = new StateType();
    allocStateComponents(state);
    return state;
}

Eigen::Vector3f MobiusStateSpace::toVector(const State *state) const
{
    Eigen::Vector3f vec;

    const auto *s = state->as<MobiusStateSpace::StateType>();
    float u = s->getU();
    float v = s->getV();

    double R = radius_ + v * std::cos(0.5 * u);
    vec[0] = R * std::cos(u);
    vec[1] = R * std::sin(u);
    vec[2] = v * std::sin(0.5 * u);
    return vec;
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file CauchyLossFunc.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_CAUCHY_LOSS_FUNCTION_HPP
#define STEAM_CAUCHY_LOSS_FUNCTION_HPP

#include <Eigen/Core>
#include <boost/shared_ptr.hpp>

#include <steam/problem/lossfunc/LossFunctionBase.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Cauchy loss function class
//////////////////////////////////////////////////////////////////////////////////////////////
class CauchyLossFunc : public LossFunctionBase
{
 public:

  /// Convenience typedefs
  typedef boost::shared_ptr<CauchyLossFunc> Ptr;
  typedef boost::shared_ptr<const CauchyLossFunc> ConstPtr;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor -- k is the `threshold' based on number of std devs (1-3 is typical)
  //////////////////////////////////////////////////////////////////////////////////////////////
  CauchyLossFunc(double k) : k_(k) {}

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Cost function (basic evaluation of the loss function)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual double cost(double whitened_error_norm) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Weight for iteratively reweighted least-squares (influence function div. by error)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual double weight(double whitened_error_norm) const;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Cauchy constant
  //////////////////////////////////////////////////////////////////////////////////////////////
  double k_;
};

} // steam

#endif // STEAM_CAUCHY_LOSS_FUNCTION_HPP


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <boost/geometry.hpp>

namespace pyinterp::detail::geometry {

/// Coordinate type
template <typename Point>
using coordinate_t = boost::geometry::coordinate_type<Point>;

namespace point {

/// Class allowing dynamic access to the axis of a point.
/// A recursive call is made as long as the requested axis is not reached.
///
/// @tparam Point Point handled
/// @tparam Axis The current axis handled by the class.
/// @tparam Count The number of dimension handled by the point
template <typename Point, size_t Axis, size_t Count>
struct GetDimension {
  /// Get accessor
  ///
  /// @param point Point handled.
  /// @param axis The axis of the point to be read.
  static auto get(const Point &point, size_t axis) ->
      typename coordinate_t<Point>::type {
    if (Axis == axis) {
      return boost::geometry::get<Axis>(point);
    }
    return GetDimension<Point, Axis + 1, Count>::get(point, axis);
  }

  /// Set accessor
  ///
  /// @param point Point handled.
  /// @param value Value to store in the point
  /// @param axis The axis of the point to be read.
  static void set(Point &point, const typename coordinate_t<Point>::type &value,
                  size_t axis) {
    if (Axis == axis) {
      return boost::geometry::set<Axis>(point, value);
    }
    return GetDimension<Point, Axis + 1, Count>::set(point, value, axis);
  }
};

/// Class allowing dynamic access to the axis of a point.
/// This class is used when recursive calls are completed.
///
/// @tparam Point Point handled
/// @tparam Count The number of dimension handled by the point
template <typename Point, size_t Count>
struct GetDimension<Point, Count, Count> {
  /// Get accessor
  static auto get(const Point & /*point*/, size_t /*axis*/) ->
      typename coordinate_t<Point>::type {
    return std::numeric_limits<typename coordinate_t<Point>::type>::max();
  }

  /// Set accessor
  static void set(Point & /*point*/,
                  const typename coordinate_t<Point>::type & /*value*/,
                  size_t /*axis*/) {}
};

/// Get a point value for a given axis
template <typename Point>
auto get(const Point &point, size_t axis) ->
    typename coordinate_t<Point>::type {
  return GetDimension<
      Point, 0, boost::geometry::dimension<Point>::type::value>::get(point,
                                                                     axis);
}

/// Set a point value for a given axis
template <typename Point>
void set(Point &point, const typename coordinate_t<Point>::type &value,
         size_t axis) {
  return GetDimension<
      Point, 0, boost::geometry::dimension<Point>::type::value>::set(point,
                                                                     value,
                                                                     axis);
}

}  // namespace point

/// Points of a 2-dimensional Cartesian space.
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
using Point2D =
    boost::geometry::model::point<T, 2, boost::geometry::cs::cartesian>;

/// Points of a 3-dimensional Cartesian space.
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
using Point3D =
    boost::geometry::model::point<T, 3, boost::geometry::cs::cartesian>;

/// Points of a N-dimensional Cartesian space.
///
/// @tparam T Storage class of the coordinates of the point.
/// @tparam N Number of dimensions of the Cartesian space.
template <typename T, size_t N>
using PointND =
    boost::geometry::model::point<T, N, boost::geometry::cs::cartesian>;

/// Points of a 2-dimensional spherical equatorial space.
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
using EquatorialPoint2D = boost::geometry::model::point<
    T, 2, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>>;

/// Points of a 2-dimensional spherical equatorial space and time
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
class TemporalEquatorial2D : public EquatorialPoint2D<T> {
 public:
  /// Default constructor
  ///
  /// @param x X-coordinate of the point
  /// @param y Y-coordinate of the point
  /// @param timestamp Date associated with the point.
  TemporalEquatorial2D(const T &x, const T &y, const int64_t timestamp)
      : EquatorialPoint2D<T>(x, y), timestamp_(timestamp) {}

  /// Construction of a point associated with an indefinite time reference
  /// (NaT)
  ///
  /// @param x X-coordinate of the point
  /// @param y Y-coordinate of the point
  TemporalEquatorial2D(const T &x, const T &y)
      : EquatorialPoint2D<T>(x, y),
        timestamp_(std::numeric_limits<int64_t>::min()) {}

  /// Gets the time stamp associated with the point
  [[nodiscard]] constexpr auto timestamp() const -> int64_t {
    return timestamp_;
  }

 private:
  int64_t timestamp_;
};

/// Points of a 3-dimensional spherical equatorial space.
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
using EquatorialPoint3D = boost::geometry::model::point<
    T, 3, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>>;

/// Points of a 4-dimensional spherical equatorial space.
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
using EquatorialPoint4D = boost::geometry::model::point<
    T, 4, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>>;

/// Points of a 2-dimensional spheroid space.
///
/// @tparam T Storage class of the coordinates of the point.
template <typename T>
using GeographicPoint2D = boost::geometry::model::point<
    T, 2, boost::geometry::cs::geographic<boost::geometry::degree>>;

}  // namespace pyinterp::detail::geometry

namespace boost::geometry::traits {

/// Coordinate tag from TemporalEquatorial2D.
template <typename T>
struct tag<pyinterp::detail::geometry::TemporalEquatorial2D<T>> {
  /// Typedef for type
  using type = point_tag;
};

/// Coordinate type from TemporalEquatorial2D
template <typename T>
struct coordinate_type<pyinterp::detail::geometry::TemporalEquatorial2D<T>> {
  /// Typedef for type
  using type = T;
};

/// Coordinate system from TemporalEquatorial2D
template <typename T>
struct coordinate_system<pyinterp::detail::geometry::TemporalEquatorial2D<T>> {
  /// Typedef for type
  using type = cs::spherical_equatorial<degree>;
};

template <typename T>
struct dimension<pyinterp::detail::geometry::TemporalEquatorial2D<T>>
    : boost::mpl::int_<2> {};

/// access struct defining with TemporalEquatorial2D
template <typename T, size_t I>
struct access<pyinterp::detail::geometry::TemporalEquatorial2D<T>, I> {
  /// Pointer accessor
  static auto get(pyinterp::detail::geometry::TemporalEquatorial2D<T> const &p)
      -> T {
    return p.template get<I>();
  }

  /// Pointer setter
  static void set(pyinterp::detail::geometry::TemporalEquatorial2D<T> &p,
                  T const &v) {  // NOLINT
    p.template set<I>(v);
  }
};

}  // namespace boost::geometry::traits


#ifndef TRIAL_ONLINE_QUANTILE_PSQUARE_HPP
#define TRIAL_ONLINE_QUANTILE_PSQUARE_HPP

///////////////////////////////////////////////////////////////////////////////
//
// Copyright (C) 2016 Bjorn Reese <breese@users.sourceforge.net>
//
// 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)
//
///////////////////////////////////////////////////////////////////////////////

// Jain and Chlamtac, "The [Piecewise-parabolic Prediction]-Square Algorithm
//   for Dynamic Calculation of Percentiles and Histograms without Storing
//   Observations", Communications of the ACM, 28(10), pp. 1076-1086, 1985.
//
// Raatikainen, "Sequential Procedure for Simultaneous Estimation of Several
//   Percentiles", Transactions of the Society for Computer Simulations, 7(1),
//   pp. 21-44, 1990.

#include <cstddef> // std::size_t
#include <vector>
#include <array>
#include <ratio>
#include <boost/mp11/list.hpp>
#include <boost/mp11/algorithm.hpp>
#include <trial/online/detail/type_traits.hpp>

namespace trial
{
namespace online
{
namespace quantile
{

using minimum_ratio = std::ratio<0, 1>;
using maximum_ratio = std::ratio<1, 1>;

template <typename T, typename... Quantiles>
class psquare
{
    static_assert(std::is_floating_point<T>::value, "T must be a floating-point type");
    static_assert((sizeof...(Quantiles) > 0), "There must be at least one quantile");

    using QuantileList = boost::mp11::mp_sort<boost::mp11::mp_list<minimum_ratio, Quantiles..., maximum_ratio>, std::ratio_less>;
    static_assert(boost::mp11::mp_all_of<QuantileList, detail::is_ratio>::value, "Quantiles must be ratios");

public:
    using value_type = T;
    using size_type = std::size_t;

    psquare() noexcept;
    psquare(const psquare&) noexcept = default;
    psquare(psquare&&) noexcept = default;
    psquare& operator= (const psquare&) noexcept = default;
    psquare& operator= (psquare&&) noexcept = default;

    void clear() noexcept;
    bool empty() const noexcept;
    size_type size() const noexcept;

    void push(value_type) noexcept;

    // Get value by type.
    // Select middle quantile parameter by default.
    template < typename Q = boost::mp11::mp_at_c<QuantileList, 1 + sizeof...(Quantiles) / 2> >
    value_type value() const noexcept;

    // Get value by index.
    template <std::size_t Index = 1 + sizeof...(Quantiles) / 2>
    value_type get() const noexcept;

    struct parameter_type
    {
        parameter_type(size_type position,
                       value_type height) noexcept;

        bool operator== (const parameter_type& other) const noexcept
        {
            return ((position == other.position) &&
                    (height == other.height));
        }

        size_type position;
        value_type height;
    };
    std::vector<parameter_type> parameters() const;
    void parameters(const std::vector<parameter_type>&) noexcept;

private:
    void initialize() noexcept;
    value_type linear(size_type, int) const noexcept;
    value_type parabolic(size_type, int) const noexcept;

private:
    static constexpr size_type quantile_length = sizeof...(Quantiles);
    static constexpr size_type parameter_length = 2 * quantile_length + 3;
    static constexpr value_type quantiles[quantile_length] = { (Quantiles::num / value_type(Quantiles::den))... };

    size_type count {0};
    std::array<size_type, parameter_length> positions;
    std::array<value_type, parameter_length> heights;
    std::array<value_type, parameter_length> desired_positions;
    std::array<value_type, parameter_length> constant_deltas;
};

// Convenience types
using median_ratio = std::ratio<1, 2>;
using lower_quartile_ratio = std::ratio<1, 4>;
using upper_quartile_ratio = std::ratio<3, 4>;

template <typename T> using psquare_median = psquare<T, median_ratio>;
template <typename T> using psquare_quartile = psquare<T, lower_quartile_ratio, median_ratio, upper_quartile_ratio>;

} // namespace quantile
} // namespace online
} // namespace trial

#include <trial/online/quantile/detail/psquare.ipp>

#endif // TRIAL_ONLINE_QUANTILE_PSQUARE_HPP


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/ilogb.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/four.hpp>

STF_CASE_TPL (" ilogb real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ilogb;
  using r_t = decltype(ilogb(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T>));

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(ilogb(bs::Minf<T>()), bs::Zero<r_t>());
  STF_EQUAL(ilogb(bs::Nan<T>()), bs::Zero<r_t>());
#endif
  STF_EQUAL(ilogb(bs::Mone<T>()), bs::Zero<r_t>());
  STF_EQUAL(ilogb(bs::One<T>()), bs::Zero<r_t>());
  STF_EQUAL(ilogb(bs::Two<T>()), bs::One<r_t>());
  STF_EQUAL(ilogb(bs::Four<T>()), bs::Two<r_t>());
  STF_EQUAL(ilogb(bs::Zero<T>()), bs::Zero<r_t>());
}

STF_CASE_TPL (" ilogb unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ilogb;
  using r_t = decltype(ilogb(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T>));

  // specific values tests
  STF_EQUAL(ilogb(bs::One<T>()), bs::Zero<r_t>());
  STF_EQUAL(ilogb(bs::Two<T>()), bs::One<r_t>());
  STF_EQUAL(ilogb(bs::Zero<T>()), bs::Zero<r_t>());
  STF_EQUAL(ilogb(bs::Four<T>()), bs::Two<r_t>());
}

STF_CASE_TPL (" ilogb signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ilogb;
  using r_t = decltype(ilogb(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T>));

  // specific values tests
  STF_EQUAL(ilogb(bs::Mone<T>()), bs::Zero<r_t>());
  STF_EQUAL(ilogb(bs::One<T>()), bs::Zero<r_t>());
  STF_EQUAL(ilogb(bs::Two<T>()), bs::One<r_t>());
  STF_EQUAL(ilogb(bs::Zero<T>()), bs::Zero<r_t>());
  STF_EQUAL(ilogb(bs::Four<T>()), bs::Two<r_t>());
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      102511    D. Dirkx          First version of file.
 *      110120    D. Dirkx          Finalized for code check.
 *      110208    K. Kumar          Updated file header; corrected Doxygen comments; minor changes.
 *      110209    D. Dirkx          Minor changes.
 *      110209    K. Kumar          Minor changes.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120323    D. Dirkx          Removed set functions; moved functionality to constructor.
 *
 *    References
 *      E.H. Hirschel and C. Weiland, Selected Aerothermodynamic Design Problems of Hypersonic
 *          Flight Vehicles (chapter 5), Springer/AIAA, 2009.
 *      D. Dirkx, Continuous Shape Optimization of Entry Vehicles, MSc thesis, Delft University
 *          of Technology, 2011 (Unpublished).
 *
 *    Notes
 *
 */

#include <cmath>
#include <iostream>

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Mathematics/GeometricShapes/capsule.h"
#include "Tudat/Mathematics/GeometricShapes/conicalFrustum.h"
#include "Tudat/Mathematics/GeometricShapes/sphereSegment.h"
#include "Tudat/Mathematics/GeometricShapes/torus.h"

namespace tudat
{
namespace geometric_shapes
{

//! Default constructor.
Capsule::Capsule( const double noseRadius,
                  const double middleRadius,
                  const double rearLength,
                  const double rearAngle,
                  const double sideRadius )
{
    using std::sin;
    using std::cos;
    using mathematical_constants::PI;

    // Call set functions for number of single and composite surface geometries
    // with predetermined values.
    setNumberOfCompositeSurfaceGeometries( 0 );
    setNumberOfSingleSurfaceGeometries( 4 );

    // Set member shape variables
    noseRadius_ = noseRadius;
    middleRadius_ = middleRadius;
    rearLength_ = rearLength;
    rearAngle_ = rearAngle;
    sideRadius_ = sideRadius;

    // Determine and set extent of spherical nose part.
    double noseSphereAngle_ = asin( ( middleRadius_ - sideRadius_ )
                                    / ( noseRadius_ - sideRadius_ ) );

    // Create nose sphere.
    boost::shared_ptr< SphereSegment > noseSphere_ = boost::make_shared< SphereSegment >(
                noseRadius_, 0, 2 * PI, 0, noseSphereAngle_ );

    // Declare translation vector.
    Eigen::VectorXd translationVector_ = Eigen::VectorXd( 3 );
    translationVector_( 2 ) = 0.0;
    translationVector_( 1 ) = 0.0;
    translationVector_( 0 ) = - noseRadius_ * cos( noseSphereAngle_ );

    // Set nose translation vector.
    noseSphere_->setOffset( translationVector_ );

    // Set noseSphere_ in singleSurfaceList_.
    setSingleSurfaceGeometry( noseSphere_, 0 );

    // Create rear cone, fully revolved.
    boost::shared_ptr< ConicalFrustum > cone_ = boost::make_shared< ConicalFrustum >(
                rearAngle_, middleRadius_ - sideRadius_ * ( 1.0 - cos( rearAngle_ ) ),
                rearLength_ );

    // Set translation vector of cone.
    translationVector_( 0 ) = -sideRadius_ * ( sin( PI / 2.0 - noseSphereAngle_ )
                                               + sin ( -rearAngle_ ) );
    cone_->setOffset( translationVector_ );

    // Set cone in singleSurfaceList_.
    setSingleSurfaceGeometry( cone_, 1 );

    // Calculate end radius of cone.
    double endRadius_ = cone_->getStartRadius( ) + rearLength_ * tan( rearAngle_ );

    // Calculate rear sphere radius.
    double rearNoseRadius_ = endRadius_ / cos( -rearAngle_ );

    // Create rear sphere ( "end cap" ), fully revolved.
    boost::shared_ptr< SphereSegment > rearSphere_ = boost::make_shared< SphereSegment >(
                rearNoseRadius_, 0.0, 2.0 * PI, PI / 2.0 - rearAngle_, PI );

    // Set translation vector of rear sphere.
    translationVector_( 0 ) =  ( rearNoseRadius_ * sin( -rearAngle_ ) ) - rearLength_
            - ( sideRadius_ * ( sin( PI / 2.0 - noseSphereAngle_ ) + sin ( -rearAngle_ ) ) );
    rearSphere_->setOffset( translationVector_ );
    setSingleSurfaceGeometry( rearSphere_, 2 );

    // Create torus section of capsule.
    double torusMajorRadius_ = ( noseRadius_ - sideRadius_ ) * sin( noseSphereAngle_ );
    boost::shared_ptr< Torus > torus_ = boost::make_shared< Torus >(
       torusMajorRadius_, sideRadius_, 0.0, 2.0 * PI, PI / 2.0 - noseSphereAngle_, rearAngle_ );

    // Set translation vector of rear sphere.
    translationVector_( 0 ) = -cos( noseSphereAngle_ ) * sideRadius_;
    torus_->setOffset( translationVector_ );
    setSingleSurfaceGeometry( torus_, 3 );

    // Set rotation matrix fo each part to be compatible with flow direction in
    // aerodynamic analysis.
    Eigen::MatrixXd rotationMatrix = Eigen::MatrixXd( 3, 3 );
    double angle_ = PI / 2.0;
    rotationMatrix( 0, 0 ) = cos( angle_ );
    rotationMatrix( 0, 1 ) = 0.0;
    rotationMatrix( 0, 2 ) = sin( angle_ );
    rotationMatrix( 1, 0 ) = 0.0;
    rotationMatrix( 1, 1 ) = 1.0;
    rotationMatrix( 1, 2 ) = 0.0;
    rotationMatrix( 2, 0 ) = -sin( angle_ );
    rotationMatrix( 2, 1 ) = 0.0;
    rotationMatrix( 2, 2 ) = cos( angle_ );

    // Set rotation matrix for single surface geometries.
    for ( unsigned i = 0; i < numberOfSingleSurfaceGeometries_ ; i++ )
    {
        singleSurfaceGeometryList_[ i ]->setRotationMatrix( rotationMatrix );
    }
}

//! Overload ostream to print class information.
std::ostream &operator<<( std::ostream &stream, Capsule& capsule )
{
    using std::endl;

    stream << "This is a capsule." << endl;
    stream << "The defining parameters are: "<< endl
           << "Nose radius: " << capsule.getNoseRadius( ) << endl
           << "Mid radius: " << capsule.getMiddleRadius( ) << endl
           << "Rear length: " << capsule.getRearLength( ) << endl
           << "Rear angle: " << capsule.getRearAngle( ) << endl
           << "Side radius: " << capsule.getSideRadius( )<< endl;

    return stream;
}

} // namespace geometric_shapes
} // namespace tudat


/*
 [auto_generated]
 boost/numeric/odeint/stepper/explicit_generic_rk.hpp

 [begin_description]
 Implementation of the generic Runge-Kutta steppers. This is the base class for many Runge-Kutta steppers.
 [end_description]

 Copyright 2011-2013 Mario Mulansky
 Copyright 2011-2013 Karsten Ahnert
 Copyright 2012 Christoph Koke

 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_NUMERIC_ODEINT_STEPPER_EXPLICIT_GENERIC_RK_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_GENERIC_RK_HPP_INCLUDED

#include <boost/array.hpp>

#include <boost/numeric/odeint/stepper/base/explicit_stepper_base.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_algorithm.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

//forward declarations

#ifndef DOXYGEN_SKIP
template<
size_t StageCount,
size_t Order,
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
class explicit_generic_rk;

struct stage_vector;

template< class T , class Constant >
struct array_wrapper
{
    typedef const typename boost::array< T , Constant::value > type;
};

template< class T , size_t i >
struct stage
{
    T c;
    boost::array< T , i > a;
};

template< class T , class Constant >
struct stage_wrapper
{
    typedef stage< T , Constant::value > type;
};
#endif

template<
size_t StageCount,
size_t Order,
class State ,
class Value ,
class Deriv ,
class Time ,
class Algebra ,
class Operations ,
class Resizer
>
#ifndef DOXYGEN_SKIP
class explicit_generic_rk : public explicit_stepper_base<
explicit_generic_rk< StageCount , Order , State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
Order , State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
class explicit_generic_rk : public explicit_stepper_base
#endif
{

public:

    #ifndef DOXYGEN_SKIP
    typedef explicit_stepper_base<
            explicit_generic_rk< StageCount , Order , State , Value , Deriv ,Time , Algebra , Operations , Resizer > ,
            Order , State , Value , Deriv , Time , Algebra ,
            Operations , Resizer > stepper_base_type;
    #else
    typedef explicit_stepper_base< ... > stepper_base_type;
    #endif

    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;

    #ifndef DOXYGEN_SKIP
    typedef explicit_generic_rk< StageCount , Order , State , Value , Deriv ,Time , Algebra , Operations , Resizer > stepper_type;
    #endif

    typedef detail::generic_rk_algorithm< StageCount , Value , Algebra , Operations > rk_algorithm_type;

    typedef typename rk_algorithm_type::coef_a_type coef_a_type;
    typedef typename rk_algorithm_type::coef_b_type coef_b_type;
    typedef typename rk_algorithm_type::coef_c_type coef_c_type;

    #ifndef DOXYGEN_SKIP
    static const size_t stage_count = StageCount;
    #endif

public:

    explicit_generic_rk( const coef_a_type &a , const coef_b_type &b , const coef_c_type &c ,
            const algebra_type &algebra = algebra_type() )
    : stepper_base_type( algebra ) , m_rk_algorithm( a , b , c )
    { }

    template< class System , class StateIn , class DerivIn , class StateOut >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt ,
            time_type t , StateOut &out , time_type dt )
    {
        m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );

        // actual calculation done in generic_rk.hpp
        m_rk_algorithm.do_step( stepper_base_type::m_algebra , system , in , dxdt , t , out , dt , m_x_tmp.m_v , m_F );
    }

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_impl( x );
        stepper_base_type::adjust_size( x );
    }

private:

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized( false );
        resized |= adjust_size_by_resizeability( m_x_tmp , x , typename is_resizeable<state_type>::type() );
        for( size_t i = 0 ; i < StageCount-1 ; ++i )
        {
            resized |= adjust_size_by_resizeability( m_F[i] , x , typename is_resizeable<deriv_type>::type() );
        }
        return resized;
    }

    rk_algorithm_type m_rk_algorithm;

    resizer_type m_resizer;

    wrapped_state_type m_x_tmp;
    wrapped_deriv_type m_F[StageCount-1];

};

/*********** DOXYGEN *************/

/**
 * \class explicit_generic_rk
 * \brief A generic implementation of explicit Runge-Kutta algorithms. This class is as a base class
 * for all explicit Runge-Kutta steppers.
 *
 * This class implements the explicit Runge-Kutta algorithms without error estimation in a generic way.
 * The Butcher tableau is passed to the stepper which constructs the stepper scheme with the help of a
 * template-metaprogramming algorithm. ToDo : Add example!
 *
 * This class derives explicit_stepper_base which provides the stepper interface.
 *
 * \tparam StageCount The number of stages of the Runge-Kutta algorithm.
 * \tparam Order The order of the stepper.
 * \tparam State The type representing the state of the ODE.
 * \tparam Value The floating point type which is used in the computations.
 * \tparam Time The type representing the independent variable - the time - of the ODE.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn explicit_generic_rk::explicit_generic_rk( const coef_a_type &a , const coef_b_type &b , const coef_c_type &c , const algebra_type &algebra )
     * \brief Constructs the explicit_generic_rk class. See examples section for details on the coefficients.
     * \param a Triangular matrix of parameters b in the Butcher tableau.
     * \param b Last row of the butcher tableau.
     * \param c Parameters to calculate the time points in the Butcher tableau.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

    /**
     * \fn explicit_generic_rk::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
     * \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
     * The result is updated out of place, hence the input is in `in` and the output in `out`.
     * Access to this step functionality is provided by explicit_stepper_base and
     * `do_step_impl` should not be called directly.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt The derivative of x at t.
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dt The step size.
     */

    /**
     * \fn explicit_generic_rk::adjust_size( const StateIn &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

}
}
}
#endif // BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_GENERIC_RK_HPP_INCLUDED


/*
 * Copyright Nick Thompson, John Maddock 2020
 * 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)
 */

#ifndef BOOST_MATH_SPECIAL_DAUBECHIES_SCALING_HPP
#define BOOST_MATH_SPECIAL_DAUBECHIES_SCALING_HPP
#include <vector>
#include <array>
#include <cmath>
#include <thread>
#include <future>
#include <iostream>
#include <boost/math/special_functions/detail/daubechies_scaling_integer_grid.hpp>
#include <boost/math/filters/daubechies.hpp>
#include <boost/math/interpolators/detail/cubic_hermite_detail.hpp>
#include <boost/math/interpolators/detail/quintic_hermite_detail.hpp>
#include <boost/math/interpolators/detail/septic_hermite_detail.hpp>

namespace boost::math {

template<class Real, int p, int order>
std::vector<Real> daubechies_scaling_dyadic_grid(int64_t j_max)
{
    using std::isnan;
    using std::sqrt;
    auto c = boost::math::filters::daubechies_scaling_filter<Real, p>();
    Real scale = sqrt(static_cast<Real>(2))*(1 << order);
    for (auto & x : c)
    {
        x *= scale;
    }

    auto phik = detail::daubechies_scaling_integer_grid<Real, p, order>();

    // Maximum sensible j for 32 bit floats is j_max = 22:
    if (std::is_same_v<Real, float>)
    {
        if (j_max > 23)
        {
            throw std::logic_error("Requested dyadic grid more dense than number of representables on the interval.");
        }
    }
    std::vector<Real> v(2*p + (2*p-1)*((1<<j_max) -1), std::numeric_limits<Real>::quiet_NaN());
    v[0] = 0;
    v[v.size()-1] = 0;
    for (int64_t i = 0; i < static_cast<int64_t>(phik.size()); ++i) {
        v[i*(1uLL<<j_max)] = phik[i];
    }

    for (int64_t j = 1; j <= j_max; ++j)
    {
        int64_t k_max = v.size()/(int64_t(1) << (j_max-j));
        for (int64_t k = 1; k < k_max;  k += 2)
        {
            // Where this value will go:
            int64_t delivery_idx = k*(1uLL << (j_max-j));
            // This is a nice check, but we've tested this exhaustively, and it's an expensive check:
            //if (delivery_idx >= static_cast<int64_t>(v.size())) {
            //    std::cerr << "Delivery index out of range!\n";
            //    continue;
            //}
            Real term = 0;
            for (int64_t l = 0; l < static_cast<int64_t>(c.size()); ++l)
            {
                int64_t idx = k*(int64_t(1) << (j_max - j + 1)) - l*(int64_t(1) << j_max);
                if (idx < 0)
                {
                    break;
                }
                if (idx < static_cast<int64_t>(v.size()))
                {
                    term += c[l]*v[idx];
                }
            }
            // Again, another nice check:
            //if (!isnan(v[delivery_idx])) {
            //    std::cerr << "Delivery index already populated!, = " << v[delivery_idx] << "\n";
            //    std::cerr << "would overwrite with " << term << "\n";
            //}
            v[delivery_idx] = term;
        }
    }
    return v;
}

namespace detail {

template<class RandomAccessContainer>
class matched_holder {
public:
    using Real = typename RandomAccessContainer::value_type;

    matched_holder(RandomAccessContainer && y, RandomAccessContainer && dydx, int grid_refinements, Real x0) : x0_{x0}, y_{std::move(y)}, dy_{std::move(dydx)}
    {
        inv_h_ = (1 << grid_refinements);
        Real h = 1/inv_h_;
        for (auto & dy : dy_)
        {
            dy *= h;
        }
    }

    inline Real operator()(Real x) const
    {
        using std::floor;
        using std::sqrt;
        // This is the exact Holder exponent, but it's pessimistic almost everywhere!
        // It's only exactly right at dyadic rationals.
        //Real const alpha = 2 - log(1+sqrt(Real(3)))/log(Real(2));
        // We're gonna use alpha = 1/2, rather than 0.5500...
        Real s = (x-x0_)*inv_h_;
        Real ii = floor(s);
        auto i = static_cast<decltype(y_.size())>(ii);
        Real t = s - ii;
        Real dphi = dy_[i+1];
        Real diff = y_[i+1] - y_[i];
        return y_[i] + (2*dphi - diff)*t + 2*sqrt(t)*(diff-dphi);
    }

    int64_t bytes() const
    {
        return 2*y_.size()*sizeof(Real) + sizeof(this);
    }

private:
    Real x0_;
    Real inv_h_;
    RandomAccessContainer y_;
    RandomAccessContainer dy_;
};

template<class RandomAccessContainer>
class matched_holder_aos {
public:
    using Point = typename RandomAccessContainer::value_type;
    using Real = typename Point::value_type;

    matched_holder_aos(RandomAccessContainer && data, int grid_refinements, Real x0) : x0_{x0}, data_{std::move(data)}
    {
        inv_h_ = Real(1uLL << grid_refinements);
        Real h = 1/inv_h_;
        for (auto & datum : data_)
        {
            datum[1] *= h;
        }
    }

    inline Real operator()(Real x) const
    {
        using std::floor;
        using std::sqrt;
        Real s = (x-x0_)*inv_h_;
        Real ii = floor(s);
        auto i = static_cast<decltype(data_.size())>(ii);
        Real t = s - ii;
        Real y0 = data_[i][0];
        Real y1 = data_[i+1][0];
        Real dphi = data_[i+1][1];
        Real diff = y1 - y0;
        return y0 + (2*dphi - diff)*t + 2*sqrt(t)*(diff-dphi);
    }

    int64_t bytes() const
    {
        return data_.size()*data_[0].size()*sizeof(Real) + sizeof(this);
    }

private:
    Real x0_;
    Real inv_h_;
    RandomAccessContainer data_;
};

template<class RandomAccessContainer>
class linear_interpolation {
public:
    using Real = typename RandomAccessContainer::value_type;

    linear_interpolation(RandomAccessContainer && y, RandomAccessContainer && dydx, int grid_refinements) : y_{std::move(y)}, dydx_{std::move(dydx)}
    {
        s_ = (1 << grid_refinements);
    }

    inline Real operator()(Real x) const
    {
        using std::floor;
        Real y = x*s_;
        Real k = floor(y);

        int64_t kk = static_cast<int64_t>(k);
        Real t = y - k;
        return (1-t)*y_[kk] + t*y_[kk+1];
    }

    inline Real prime(Real x) const
    {
        using std::floor;
        Real y = x*s_;
        Real k = floor(y);

        int64_t kk = static_cast<int64_t>(k);
        Real t = y - k;
        return (1-t)*dydx_[kk] + t*dydx_[kk+1];
    }

    int64_t bytes() const
    {
        return (1 + y_.size() + dydx_.size())*sizeof(Real) + sizeof(y_) + sizeof(dydx_);
    }

private:
    Real s_;
    RandomAccessContainer y_;
    RandomAccessContainer dydx_;
};

template<class RandomAccessContainer>
class linear_interpolation_aos {
public:
    using Point = typename RandomAccessContainer::value_type;
    using Real = typename Point::value_type;

    linear_interpolation_aos(RandomAccessContainer && data, int grid_refinements, Real x0) : x0_{x0}, data_{std::move(data)}
    {
        s_ = Real(1uLL << grid_refinements);
    }

    inline Real operator()(Real x) const
    {
        using std::floor;
        Real y = (x-x0_)*s_;
        Real k = floor(y);

        int64_t kk = static_cast<int64_t>(k);
        Real t = y - k;
        return (t != 0) ? (1-t)*data_[kk][0] + t*data_[kk+1][0] : data_[kk][0];
    }

    inline Real prime(Real x) const
    {
        using std::floor;
        Real y = (x-x0_)*s_;
        Real k = floor(y);

        int64_t kk = static_cast<int64_t>(k);
        Real t = y - k;
        return t != 0 ? (1-t)*data_[kk][1] + t*data_[kk+1][1] : data_[kk][1];
    }

    int64_t bytes() const
    {
        return sizeof(this) + data_.size()*data_[0].size()*sizeof(Real);
    }

private:
    Real x0_;
    Real s_;
    RandomAccessContainer data_;
};

template <class T>
struct daubechies_eval_type
{
   typedef T type;

   static const std::vector<T>& vector_cast(const std::vector<T>& v) { return v; }

};
template <>
struct daubechies_eval_type<float>
{
   typedef double type;

   inline static std::vector<float> vector_cast(const std::vector<double>& v)
   {
      std::vector<float> result(v.size());
      for (unsigned i = 0; i < v.size(); ++i)
         result[i] = static_cast<float>(v[i]);
      return result;
   }
};
template <>
struct daubechies_eval_type<double>
{
   typedef long double type;

   inline static std::vector<double> vector_cast(const std::vector<long double>& v)
   {
      std::vector<double> result(v.size());
      for (unsigned i = 0; i < v.size(); ++i)
         result[i] = static_cast<double>(v[i]);
      return result;
   }
};

struct null_interpolator
{
   template <class T>
   T operator()(const T&)
   {
      return 1;
   }
};

} // namespace detail

template<class Real, int p>
class daubechies_scaling {
   //
   // Some type manipulation so we know the type of the interpolator, and the vector type it requires:
   //
   typedef std::vector<std::array<Real, p < 6 ? 2 : p < 10 ? 3 : 4>> vector_type;
   //
   // List our interpolators:
   //
   typedef std::tuple<
      detail::null_interpolator, detail::matched_holder_aos<vector_type>, detail::linear_interpolation_aos<vector_type>,
      interpolators::detail::cardinal_cubic_hermite_detail_aos<vector_type>, interpolators::detail::cardinal_quintic_hermite_detail_aos<vector_type>,
      interpolators::detail::cardinal_septic_hermite_detail_aos<vector_type> > interpolator_list;
   //
   // Select the one we need:
   //
   typedef std::tuple_element_t<
      p == 1 ? 0 :
      p == 2 ? 1 :
      p == 3 ? 2 :
      p <= 5 ? 3 :
      p <= 9 ? 4 : 5, interpolator_list> interpolator_type;

public:
   daubechies_scaling(int grid_refinements = -1)
   {
      static_assert(p < 20, "Daubechies scaling functions are only implemented for p < 20.");
      static_assert(p > 0, "Daubechies scaling functions must have at least 1 vanishing moment.");
      if constexpr (p == 1)
      {
         return;
      }
      else {
         if (grid_refinements < 0)
         {
            if (std::is_same_v<Real, float>)
            {
               if (grid_refinements == -2)
               {
                  // Control absolute error:
                  //                          p= 2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19
                  std::array<int, 20> r{ -1, -1, 18, 19, 16, 11,  8,  7,  7,  7,  5,  5,  4,  4,  4,  4,  3,  3,  3,  3 };
                  grid_refinements = r[p];
               }
               else
               {
                  // Control relative error:
                  //                          p= 2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19
                  std::array<int, 20> r{ -1, -1, 21, 21, 21, 17, 16, 15, 14, 13, 12, 11, 11, 11, 11, 11, 11, 11, 11, 11 };
                  grid_refinements = r[p];
               }
            }
            else if (std::is_same_v<Real, double>)
            {
               //                          p= 2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19
               std::array<int, 20> r{ -1, -1, 21, 21, 21, 21, 21, 21, 21, 21, 20, 20, 19, 19, 18, 18, 18, 18, 18, 18 };
               grid_refinements = r[p];
            }
            else
            {
               grid_refinements = 21;
            }
         }

         // Compute the refined grid:
         // In fact for float precision I know the grid must be computed in double precision and then cast back down, or else parts of the support are systematically inaccurate.
         std::future<std::vector<Real>> t0 = std::async(std::launch::async, [&grid_refinements]() {
            // Computing in higher precision and downcasting is essential for 1ULP evaluation in float precision:
            auto v = daubechies_scaling_dyadic_grid<typename detail::daubechies_eval_type<Real>::type, p, 0>(grid_refinements);
            return detail::daubechies_eval_type<Real>::vector_cast(v);
            });
         // Compute the derivative of the refined grid:
         std::future<std::vector<Real>> t1 = std::async(std::launch::async, [&grid_refinements]() {
            auto v = daubechies_scaling_dyadic_grid<typename detail::daubechies_eval_type<Real>::type, p, 1>(grid_refinements);
            return detail::daubechies_eval_type<Real>::vector_cast(v);
            });

         // if necessary, compute the second and third derivative:
         std::vector<Real> d2ydx2;
         std::vector<Real> d3ydx3;
         if constexpr (p >= 6) {
            std::future<std::vector<Real>> t3 = std::async(std::launch::async, [&grid_refinements]() {
               auto v = daubechies_scaling_dyadic_grid<typename detail::daubechies_eval_type<Real>::type, p, 2>(grid_refinements);
               return detail::daubechies_eval_type<Real>::vector_cast(v);
               });

            if constexpr (p >= 10) {
               std::future<std::vector<Real>> t4 = std::async(std::launch::async, [&grid_refinements]() {
                  auto v = daubechies_scaling_dyadic_grid<typename detail::daubechies_eval_type<Real>::type, p, 3>(grid_refinements);
                  return detail::daubechies_eval_type<Real>::vector_cast(v);
                  });
               d3ydx3 = t4.get();
            }
            d2ydx2 = t3.get();
         }

         auto y = t0.get();
         auto dydx = t1.get();

         if constexpr (p >= 2)
         {
            vector_type data(y.size());
            for (size_t i = 0; i < y.size(); ++i)
            {
               data[i][0] = y[i];
               data[i][1] = dydx[i];
               if constexpr (p >= 6)
                  data[i][2] = d2ydx2[i];
               if constexpr (p >= 10)
                  data[i][3] = d3ydx3[i];
            }
            if constexpr (p <= 3)
               m_interpolator = std::make_shared<interpolator_type>(std::move(data), grid_refinements, Real(0));
            else
               m_interpolator = std::make_shared<interpolator_type>(std::move(data), Real(0), Real(1) / (1 << grid_refinements));
         }
         else
            m_interpolator = std::make_shared<detail::null_interpolator>();
      }
   }

    inline Real operator()(Real x) const
    {
        if (x <= 0 || x >= 2*p-1)
        {
            return 0;
        }
        return (*m_interpolator)(x);
    }

    inline Real prime(Real x) const
    {
        static_assert(p > 2, "The 3-vanishing moment Daubechies scaling function is the first which is continuously differentiable.");
        if (x <= 0 || x >= 2*p-1)
        {
            return 0;
        }
        return m_interpolator->prime(x);
    }

    inline Real double_prime(Real x) const
    {
        static_assert(p >= 6, "Second derivatives require at least 6 vanishing moments.");
        if (x <= 0 || x >= 2*p - 1)
        {
            return Real(0);
        }
        return m_interpolator->double_prime(x);
    }

    std::pair<Real, Real> support() const
    {
        return {Real(0), Real(2*p-1)};
    }

    int64_t bytes() const
    {
       return m_interpolator->bytes() + sizeof(m_interpolator);
    }

private:
   std::shared_ptr<interpolator_type> m_interpolator;
};

}
#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SQRT_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/sqrt.hpp>
#include <boost/simd/include/constants/real.hpp>
#include <boost/simd/include/functions/scalar/is_ltz.hpp>
#include <math.h>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::sqrt_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::sqrt(result_type(a0));
    }
  };
} } }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::sqrt_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return (is_ltz(a0)) ?  Nan<A0>() : ::sqrt(a0);
    }
  };
} } }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::sqrt_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      // libc has a very poor treatment of exceptions regarding performance
      // this test is at almost no cost but improve drastically performances
      // in case negative arguments are common.
      return (is_ltz(a0)) ? Nan<A0>() : ::sqrtf(a0);
    }
  };
} } }

#endif


/* ----------------------------------------------------------------------------
 * Copyright 2018, Ross Hartley <m.ross.hartley@gmail.com>
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   landmarks.cpp
 *  @author Ross Hartley
 *  @brief  Example of invariant filtering for landmark-aided inertial navigation
 *  @date   September 25, 2018
 **/

#include <iostream>
#include <fstream>
#include <string>
#include <cstdlib>
#include <Eigen/Dense>
#include <boost/algorithm/string.hpp>
#include "inekf/inekf.hpp"

#define DT_MIN 1e-6
#define DT_MAX 1

double stod98(const std::string &s) {
    return atof(s.c_str());
}

int stoi98(const std::string &s) {
    return atoi(s.c_str());
}

using namespace std;
using namespace inekf;

int main() {
    //  ---- Initialize invariant extended Kalman filter ----- //
    RobotState initial_state;

    // Initialize state mean
    Eigen::Matrix3d R0;
    Eigen::Vector3d v0, p0, bg0, ba0;
    R0 << 1, 0, 0, // initial orientation
          0, -1, 0, // IMU frame is rotated 90deg about the x-axis
          0, 0, -1;
    v0 << 0,0,0; // initial velocity
    p0 << 0,0,0; // initial position
    bg0 << 0,0,0; // initial gyroscope bias
    ba0 << 0,0,0; // initial accelerometer bias
    initial_state.setRotation(R0);
    initial_state.setVelocity(v0);
    initial_state.setPosition(p0);
    initial_state.setGyroscopeBias(bg0);
    initial_state.setAccelerometerBias(ba0);

    // Initialize state covariance
    NoiseParams noise_params;
    noise_params.setGyroscopeNoise(0.01);
    noise_params.setAccelerometerNoise(0.1);
    noise_params.setGyroscopeBiasNoise(0.00001);
    noise_params.setAccelerometerBiasNoise(0.0001);

    // Initialize filter
    InEKF filter(initial_state, noise_params);
    cout << "Noise parameters are initialized to: \n";
    cout << filter.getNoiseParams() << endl;
    cout << "Robot's state is initialized to: \n";
    cout << filter.getState() << endl;

    // --- Optionally initialize prior landmarks --- //
    mapIntVector3d prior_landmarks;
    Eigen::Vector3d p_wl;
    int id;

    // // Landmark 1
    // id = 1;
    // p_wl << 0,-1,0;
    // prior_landmarks.insert(pair<int,Eigen::Vector3d> (id, p_wl));

    // Landmark 2
    // id = 2;
    // p_wl << 1,1,-0.5;
    // prior_landmarks.insert(pair<int,Eigen::Vector3d> (id, p_wl));

    // // Landmark 3
    // id = 3;
    // p_wl << 2,-1,0.5;
    // prior_landmarks.insert(pair<int,Eigen::Vector3d> (id, p_wl));

    // Store landmarks for localization
    filter.setPriorLandmarks(prior_landmarks);

    // Open data file
    ifstream infile("../data/imu_landmark_measurements.txt");
    string line;
    Eigen::Matrix<double,6,1> imu_measurement = Eigen::Matrix<double,6,1>::Zero();
    Eigen::Matrix<double,6,1> imu_measurement_prev = Eigen::Matrix<double,6,1>::Zero();
    double t = 0;
    double t_prev = 0;

    // Loop through data file and read in measurements line by line
    while (getline(infile, line)){
        vector<string> measurement;
        boost::split(measurement,line,boost::is_any_of(" "));
        // Handle measurements
        if (measurement[0].compare("IMU")==0){
            cout << "Received IMU Data, propagating state\n";
            assert((measurement.size()-2) == 6);
            t = stod98(measurement[1]);
            imu_measurement << stoi98(measurement[2]),
                               stoi98(measurement[3]),
                               stoi98(measurement[4]),
                               stoi98(measurement[5]),
                               stoi98(measurement[6]),
                               stoi98(measurement[7]);

            // Propagate using IMU data
            double dt = t - t_prev;
            if (dt > DT_MIN && dt < DT_MAX) {
                filter.Propagate(imu_measurement_prev, dt);
            }
        }
        else if (measurement[0].compare("LANDMARK")==0){
            cout << "Received LANDMARK observation, correcting state\n";
            assert((measurement.size()-2)%4 == 0);
            t = stod98(measurement[1]);
            vectorLandmarks measured_landmarks;
            for (int i=2; i<measurement.size(); i+=4) {
                int id = stoi98(measurement[i]);
                Eigen::Vector3d p_bl;
                p_bl << stoi98(measurement[i+1]),
                        stoi98(measurement[i+2]),
                        stoi98(measurement[i+3]);
                Eigen::Matrix3d cov = 0.01*Eigen::Matrix3d::Identity();
                Landmark landmark(id, p_bl, cov);
                measured_landmarks.push_back(landmark);
            }

            // Correct state using landmark measurements
            filter.CorrectLandmarks(measured_landmarks);
        }

        // Store previous timestamp
        t_prev = t;
        imu_measurement_prev = imu_measurement;
    }

    // Print final state
    cout << filter.getState() << endl;

    return 0;
}


#ifndef GIFS_QM_MODEL_HPP
#define GIFS_QM_MODEL_HPP

#include "properties.hpp"
#include "qm_interface.hpp"
#include "configreader.hpp"
#include <armadillo>
#include <memory>

class HamiltonianDynamics{
public:
  HamiltonianDynamics(void) {}
  virtual ~HamiltonianDynamics() {};

  virtual void update(double x) =0;
  void scan(double a, double b, int N);

  const arma::vec & energies(void)  const {return energy;}
  const arma::vec & gradients(void) const {return gradient;}
  const arma::mat & nacs(void)      const {return nac;}
  const arma::mat & overlaps(void)  const {return overlap;}

protected:
  // updates overlap and, phase matches the new eigen vectors to the old ones.
  void update_overlap(arma::mat &evec);
  void save_state(double x);

  arma::vec energy;
  arma::vec gradient;
  arma::mat nac;
  arma::mat overlap;

  bool record = true;
  std::string state_file = "./out.dat";
};

/*
  Implements system A, eq. 1, from Subotnik and Shenvi (2011)
*/
class AvoidedCrossing: public HamiltonianDynamics{
public:
  AvoidedCrossing(void);
  void update(double x) override;

private:
  const double A = 0.03;
  const double B = 1.6;
  const double C = 0.005;
};

/*
  Implements system B, eq. 3, from Subotnik and Shenvi (2011)
*/
class ReflectiveAvoidedCrossing: public HamiltonianDynamics{
public:
  ReflectiveAvoidedCrossing(void);
  void update(double x) override;

private:
  const double A = 0.03;
  const double B = 1.6;
  double C = 0.005;
};

class QM_Model: public QMInterface{
public:
  QM_Model(FileHandle& fh,
           const arma::uvec& in_qmids,
	   arma::mat& in_qm_crd,
	   arma::mat& in_mm_crd,
	   arma::vec& in_mm_chg,
	   const int charge,
	   const int mult,
	   const int excited_states,
           const int min_state);

  ~QM_Model(){ delete model; }

  void get_properties(PropMap &props) override;
  void update(void) override;

private:
  ConfigBlockReader model_reader();
  HamiltonianDynamics * model;
};

#endif


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE aomatrix3d_test

// Third party includes
#include <boost/test/unit_test.hpp>

#include <votca/tools/eigenio_matrixmarket.h>
// Local VOTCA includes
#include "votca/xtp/aomatrix.h"
#include "votca/xtp/orbitals.h"
#include <libint2/initialize.h>
using namespace votca::xtp;
using namespace votca;
using namespace std;

BOOST_AUTO_TEST_SUITE(aomatrix3d_test)

BOOST_AUTO_TEST_CASE(aomatrices_dipole_test) {
  libint2::initialize();
  Orbitals orbitals;
  orbitals.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                                  "/aomatrix3d/molecule.xyz");
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix3d/3-21G.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, orbitals.QMAtoms());

  AODipole dip;
  dip.Fill(aobasis);

  std::array<Eigen::MatrixXd, 3> dip_ref;
  for (unsigned i = 0; i < dip_ref.size(); i++) {
    dip_ref[i] = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
        std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix3d/dip_ref" +
        std::to_string(i) + ".mm");
  }

  for (unsigned i = 0; i < dip_ref.size(); i++) {
    bool check_dip = dip.Matrix()[i].isApprox(dip_ref[i], 0.0001);
    BOOST_CHECK_EQUAL(check_dip, true);
    if (!check_dip) {
      cout << "ref" << i << endl;
      cout << dip_ref[i] << endl;
      cout << "result" << i << endl;
      cout << dip.Matrix()[i] << endl;
    }
  }

  // Now the same computation with a non zero center
  Eigen::Vector3d center;
  center << 1.71, 2.34, 3.54;
  dip.setCenter(center);
  dip.Fill(aobasis);

  for (unsigned i = 0; i < dip_ref.size(); i++) {
    dip_ref[i] = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
        std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix3d/dip_ref" +
        std::to_string(i) + "_nonzero.mm");
  }

  for (unsigned i = 0; i < dip_ref.size(); i++) {
    bool check_nonzero_center = dip.Matrix()[i].isApprox(dip_ref[i], 0.0001);
    BOOST_CHECK_EQUAL(check_nonzero_center, true);
    if (!check_nonzero_center) {
      cout << "ref" << i << endl;
      cout << dip_ref[i] << endl;
      cout << "result" << i << endl;
      cout << dip.Matrix()[i] << endl;
    }
  }

  libint2::finalize();
}

BOOST_AUTO_TEST_CASE(large_l_test) {
  libint2::initialize();
  QMMolecule mol("C", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix3d/C2.xyz");

  BasisSet basisset;
  basisset.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix3d/G.xml");
  AOBasis dftbasis;
  dftbasis.Fill(basisset, mol);

  AODipole dip;
  dip.Fill(dftbasis);

  std::array<Eigen::MatrixXd, 3> dip_ref;
  for (unsigned i = 0; i < dip_ref.size(); i++) {
    dip_ref[i] = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
        std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix3d/dip_ref_large_" +
        std::to_string(i) + ".mm");
  }

  for (unsigned i = 0; i < dip_ref.size(); i++) {

    bool check_dip = dip.Matrix()[i].isApprox(dip_ref[i], 0.0001);
    BOOST_CHECK_EQUAL(check_dip, true);
    if (!check_dip) {
      cout << "ref" << i << endl;
      cout << dip_ref[i] << endl;
      cout << "result" << i << endl;
      cout << dip.Matrix()[i] << endl;
    }
  }
  libint2::finalize();
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SIND_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIND_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the sine of the angle in degree:
    \f$\sin(\pi x/180)\f$.

    @par Header <boost/simd/function/sind.hpp>

    @par Note

     As most other trigonometric function sind can be called
     with a second optional parameter  which is a tag on speed and accuracy
     (see @ref cos for further details)

   see sincosd, sin, sinpi

    @par Example:

      @snippet sind.cpp sind

    @par Possible output:

      @snippet sind.txt sind

  **/
  IEEEValue sind(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sind.hpp>
#include <boost/simd/function/simd/sind.hpp>

#endif


//  (C) Copyright John Maddock 2021.
//  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)

#define BOOST_MATH_NO_ATOMIC_INT

#include <boost/math/special_functions/bernoulli.hpp>
#include "compile_test/test_compile_result.hpp"

void compile_and_link_test()
{
   check_result<double>(boost::math::bernoulli_b2n<double>(4));
}