//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/fast.hpp>
#include <tuple>
#include <utility>
#include <simd_test.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;
  using iT = bd::as_integer_t<T>;
  using i_t = bs::pack<iT, N>;

  T a1[N], m[N];
  iT e[N];

  for(std::size_t i = 0; i < N; ++i)
  {
     a1[i] = (i%2) ? T(i) : T(-i);
     std::tie(m[i], e[i])   = bs::frexp(a1[i]);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t mm1, mm(&m[0], &m[0]+N);
  i_t ee1, ee(&e[0], &e[0]+N);

  std::tie(mm1, ee1) = bs::frexp(aa1);
  STF_IEEE_EQUAL(mm1, mm);
  STF_IEEE_EQUAL(ee1, ee);
}

STF_CASE_TPL("Check frexp on pack" , STF_IEEE_TYPES)
{

  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

template <typename T, std::size_t N, typename Env>
void test_fast(Env& $)
{
  using p_t = bs::pack<T, N>;
  using iT = bd::as_integer_t<T>;
  using i_t = bs::pack<iT, N>;

  T a1[N], m[N];
  iT e[N];

  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(1+i) : T(1-i);
    std::tie(m[i], e[i]) = bs::fast_(bs::frexp)(a1[i]);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t mm , mm1(&m[0], &m[0]+N);
  i_t ee , ee1(&e[0], &e[0]+N);
  std::tie(mm, ee) = bs::fast_(bs::frexp)(aa1);

  STF_IEEE_EQUAL(mm, mm1);
  STF_IEEE_EQUAL(ee, ee1);
}

STF_CASE_TPL("Check fast(frexp) on pack" , STF_IEEE_TYPES)
{

  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test_fast<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}


/*
@copyright Louis Dionne 2015
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/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/eval_if.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/not.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/logical.hpp>
namespace hana = boost::hana;

int main() {
    // eval_if
    {
        auto t = [](auto) { return hana::test::ct_eq<3>{}; };
        auto e = [](auto) { return hana::test::ct_eq<4>{}; };

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::eval_if(hana::true_c, t, e),
            hana::test::ct_eq<3>{}
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::eval_if(hana::false_c, t, e),
            hana::test::ct_eq<4>{}
        ));
    }

    // not_
    {
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::not_(hana::true_c),
            hana::false_c
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::not_(hana::false_c),
            hana::true_c
        ));
    }

    // laws
    hana::test::TestLogical<hana::integral_constant_tag<int>>{hana::make_tuple(
        hana::int_c<-2>, hana::int_c<0>, hana::int_c<1>, hana::int_c<3>
    )};

    hana::test::TestLogical<hana::integral_constant_tag<bool>>{hana::make_tuple(
        hana::false_c, hana::true_c
    )};
}


//==================================================================================================
/*!
  @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)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/erfc.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/ten.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/true.hpp>
#include <boost/simd/function/is_less.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
     a1[i] = T(i)/N;
     b[i] = bs::erfc(a1[i]);
   }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  STF_ULP_EQUAL(bs::erfc(aa1), bb, 32);

}

STF_CASE_TPL("Check erfc2 on pack" , STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

STF_CASE_TPL (" erfc",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::erfc;
  using p_t = bs::pack<T>;

  using r_t = decltype(erfc(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(erfc(bs::Inf<p_t>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(erfc(bs::Minf<p_t>()), bs::Two<r_t>(), 0);
  STF_ULP_EQUAL(erfc(bs::Nan<p_t>()), bs::Nan<r_t>(), 0);
#endif
  STF_ULP_EQUAL(erfc(bs::Mzero<p_t>()), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(erfc(p_t(0.1)), p_t(0.887537083981715107796724928256), 10);
  STF_ULP_EQUAL(erfc(p_t(0.4)), p_t(0.57160764495333154489639615468), 10);
  STF_ULP_EQUAL(erfc(bs::Half<p_t>()), p_t(0.479500122186953462317253346108), 15);
  STF_ULP_EQUAL(erfc(bs::One<p_t>()), p_t(0.157299207050285130658779364917), 10);
  STF_ULP_EQUAL(erfc(bs::Two<p_t>()), p_t(0.00467773498104726583793074363275), 10);
  STF_ULP_EQUAL(erfc(bs::Zero<p_t>()), bs::One<r_t>(), 0.5);
  STF_EQUAL(bs::is_less(erfc(p_t(6)), p_t(0.00000000000000003)), bs::True<p_t>());
  STF_EQUAL(bs::is_less(erfc((bs::Ten<p_t>()- p_t(2.088487583762545e-45))), bs::Eps<p_t>()), bs::True<p_t>());
  STF_EQUAL(bs::is_less(erfc(p_t(15))- p_t(7.212994172451206e-100), bs::Eps<p_t>()), bs::True<p_t>());

  STF_ULP_EQUAL(erfc(-p_t(0.1)),        bs::std_(erfc)(-p_t(0.1)),        3.5);
  STF_ULP_EQUAL(erfc(-p_t(0.4)),        bs::std_(erfc)(-p_t(0.4)),        3.5);
  STF_ULP_EQUAL(bs::std_(erfc)(-bs::Half<p_t>()), (erfc)(-bs::Half<p_t>()), 3.5);
  STF_ULP_EQUAL(erfc(-bs::One<p_t>()),  bs::std_(erfc)(-bs::One<p_t>()),  1.5);
  STF_ULP_EQUAL(erfc(-bs::Two<p_t>()),  bs::std_(erfc)(-bs::Two<p_t>()),  1);
  STF_ULP_EQUAL(erfc(p_t(-6)), p_t(2), 0);
}


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

#include "Domain/CoordinateMaps/SpecialMobius.hpp"

#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <cmath>
#include <pup.h>

#include "DataStructures/Tensor/Tensor.hpp"
#include "ErrorHandling/Assert.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/DereferenceWrapper.hpp"
#include "Utilities/EqualWithinRoundoff.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/MakeWithValue.hpp"
#include "Utilities/StdArrayHelpers.hpp"

namespace domain {
namespace CoordinateMaps {

SpecialMobius::SpecialMobius(const double mu) noexcept
    : mu_(mu), is_identity_(mu_ == 0.0) {
  // Note: Empirically we have found that the map is accurate
  // to 12 decimal places for mu = 0.96.
  ASSERT(abs(mu) < 0.96, "The magnitude of mu must be less than 0.96.");
}

template <typename T>
std::array<tt::remove_cvref_wrap_t<T>, 3> SpecialMobius::mobius_distortion(
    const std::array<T, 3>& coords, const double mu) const noexcept {
  using ReturnType = tt::remove_cvref_wrap_t<T>;
  const ReturnType& x = coords[0];
  const ReturnType& y = coords[1];
  const ReturnType& z = coords[2];
  const double mu_squared = square(mu);
  const ReturnType r_squared = square(x) + square(y) + square(z);
  const ReturnType lambda = 1.0 / (1.0 - 2.0 * mu * x + mu_squared * r_squared);
  return std::array<ReturnType, 3>{
      {lambda * ((1.0 + mu_squared) * x - mu * (1.0 + r_squared)),
       (1.0 - mu_squared) * lambda * y, (1.0 - mu_squared) * lambda * z}};
}

template <typename T>
tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame>
SpecialMobius::mobius_distortion_jacobian(const std::array<T, 3>& coords,
                                          const double mu) const noexcept {
  using ReturnType = tt::remove_cvref_wrap_t<T>;
  const ReturnType& x = coords[0];
  const ReturnType& y = coords[1];
  const ReturnType& z = coords[2];
  const double mu_squared = square(mu);
  const ReturnType r_squared = square(x) + square(y) + square(z);
  const ReturnType common_factor =
      (mu_squared - 1.0) / square(1.0 - 2.0 * mu * x + mu_squared * r_squared);
  auto jacobian_matrix =
      make_with_value<tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame>>(
          dereference_wrapper(coords[0]), 0.0);

  get<0, 0>(jacobian_matrix) =
      -(mu_squared * (2.0 * square(x) - r_squared) - 2.0 * mu * x + 1.0) *
      common_factor;
  get<1, 0>(jacobian_matrix) = (2.0 * mu * y * (mu * x - 1.0)) * common_factor;
  get<2, 0>(jacobian_matrix) = (2.0 * mu * z * (mu * x - 1.0)) * common_factor;

  get<0, 1>(jacobian_matrix) = -1.0 * get<1, 0>(jacobian_matrix);
  get<1, 1>(jacobian_matrix) =
      -(mu_squared * (r_squared - 2.0 * square(y)) - 2.0 * mu * x + 1.0) *
      common_factor;
  get<2, 1>(jacobian_matrix) = 2.0 * mu_squared * y * z * common_factor;

  get<0, 2>(jacobian_matrix) = -1.0 * get<2, 0>(jacobian_matrix);
  get<1, 2>(jacobian_matrix) = get<2, 1>(jacobian_matrix);
  get<2, 2>(jacobian_matrix) =
      -(mu_squared * (r_squared - 2.0 * square(z)) - 2.0 * mu * x + 1.0) *
      common_factor;
  return jacobian_matrix;
}

template <typename T>
std::array<tt::remove_cvref_wrap_t<T>, 3> SpecialMobius::operator()(
    const std::array<T, 3>& source_coords) const noexcept {
  return mobius_distortion(source_coords, mu_);
}

boost::optional<std::array<double, 3>> SpecialMobius::inverse(
    const std::array<double, 3>& target_coords) const noexcept {
  // Invert only points inside or on the unit sphere.
  const auto r_squared = magnitude(target_coords);
  if (r_squared <= 1.0 or equal_within_roundoff(r_squared,1.0)) {
    return mobius_distortion(target_coords, -mu_);
  }
  return boost::none;
}

template <typename T>
tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> SpecialMobius::jacobian(
    const std::array<T, 3>& source_coords) const noexcept {
  return mobius_distortion_jacobian(source_coords, mu_);
}

template <typename T>
tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame>
SpecialMobius::inv_jacobian(const std::array<T, 3>& source_coords) const
    noexcept {
  return mobius_distortion_jacobian((*this)(source_coords), -mu_);
}

void SpecialMobius::pup(PUP::er& p) noexcept {
  p | mu_;
  p | is_identity_;
}

bool operator==(const SpecialMobius& lhs, const SpecialMobius& rhs) noexcept {
  return lhs.mu_ == rhs.mu_ and lhs.is_identity_ == rhs.is_identity_;
}

bool operator!=(const SpecialMobius& lhs, const SpecialMobius& rhs) noexcept {
  return not(lhs == rhs);
}

// Explicit instantiations
/// \cond
#define DTYPE(data) BOOST_PP_TUPLE_ELEM(0, data)

#define INSTANTIATE(_, data)                                                   \
  template std::array<tt::remove_cvref_wrap_t<DTYPE(data)>, 3> SpecialMobius:: \
  operator()(const std::array<DTYPE(data), 3>& source_coords) const noexcept;  \
  template tnsr::Ij<tt::remove_cvref_wrap_t<DTYPE(data)>, 3, Frame::NoFrame>   \
  SpecialMobius::jacobian(const std::array<DTYPE(data), 3>& source_coords)     \
      const noexcept;                                                          \
  template tnsr::Ij<tt::remove_cvref_wrap_t<DTYPE(data)>, 3, Frame::NoFrame>   \
  SpecialMobius::inv_jacobian(const std::array<DTYPE(data), 3>& source_coords) \
      const noexcept;

GENERATE_INSTANTIATIONS(INSTANTIATE, (double, DataVector,
                                      std::reference_wrapper<const double>,
                                      std::reference_wrapper<const DataVector>))

#undef DTYPE
#undef INSTANTIATE
/// \endcond
}  // namespace CoordinateMaps
}  // namespace domain


// Boost.Geometry Index
//
// squared distance between point and nearest point of the box or point
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_COMPARABLE_DISTANCE_NEAR_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_COMPARABLE_DISTANCE_NEAR_HPP

#include <boost/geometry/index/detail/algorithms/comparable_distance_near.hpp>

#include <boost/geometry/extensions/nsphere/views/center_view.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

template <
    typename Point,
    typename Indexable,
    size_t N>
struct sum_for_indexable<Point, Indexable, nsphere_tag, comparable_distance_near_tag, N>
{
    typedef typename default_distance_result<Point, center_view<const Indexable> >::type result_type;

    inline static result_type apply(Point const& pt, Indexable const& i)
    {
        result_type center_dist = ::sqrt( comparable_distance(pt, center_view<const Indexable>(i)) );
        result_type dist = get_radius<0>(i) < center_dist ? center_dist - get_radius<0>(i) : 0;
        return dist;

        // return dist * dist to be conformant with comparable_distance?
        // CONSIDER returning negative value related to the distance or normalized distance to the center if dist < radius
    }
};

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_COMPARABLE_DISTANCE_NEAR_HPP


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_BESSEL_ITERATORS_HPP
#define BOOST_MATH_BESSEL_ITERATORS_HPP

#include <boost/math/tools/recurrence.hpp>

namespace boost {
   namespace math {
      namespace detail {

         template <class T>
         struct bessel_jy_recurrence
         {
            bessel_jy_recurrence(T v, T z) : v(v), z(z) {}
            boost::math::tuple<T, T, T> operator()(int k)
            {
               return boost::math::tuple<T, T, T>(1, -2 * (v + k) / z, 1);
            }

            T v, z;
         };
         template <class T>
         struct bessel_ik_recurrence
         {
            bessel_ik_recurrence(T v, T z) : v(v), z(z) {}
            boost::math::tuple<T, T, T> operator()(int k)
            {
               return boost::math::tuple<T, T, T>(1, -2 * (v + k) / z, -1);
            }

            T v, z;
         };
      } // namespace detail

      template <class T>
      struct bessel_j_backwards_iterator
      {
         typedef std::ptrdiff_t difference_type;
         typedef T value_type;
         typedef T* pointer;
         typedef T& reference;
         typedef std::input_iterator_tag iterator_category;

         bessel_j_backwards_iterator(const T& v, const T& x)
            : it(detail::bessel_jy_recurrence<T>(v, x), boost::math::cyl_bessel_j(v, x))
         {
            if(v < 0)
               boost::math::policies::raise_domain_error("bessel_j_backwards_iterator<%1%>", "Order must be > 0 stable backwards recurrence but got %1%", v, boost::math::policies::policy<>());
         }

         bessel_j_backwards_iterator(const T& v, const T& x, const T& J_v)
            : it(detail::bessel_jy_recurrence<T>(v, x), J_v)
         {
            if(v < 0)
               boost::math::policies::raise_domain_error("bessel_j_backwards_iterator<%1%>", "Order must be > 0 stable backwards recurrence but got %1%", v, boost::math::policies::policy<>());
         }
         bessel_j_backwards_iterator(const T& v, const T& x, const T& J_v_plus_1, const T& J_v)
            : it(detail::bessel_jy_recurrence<T>(v, x), J_v_plus_1, J_v)
         {
            if (v < -1)
               boost::math::policies::raise_domain_error("bessel_j_backwards_iterator<%1%>", "Order must be > 0 stable backwards recurrence but got %1%", v, boost::math::policies::policy<>());
         }

         bessel_j_backwards_iterator& operator++()
         {
            ++it;
            return *this;
         }

         bessel_j_backwards_iterator operator++(int)
         {
            bessel_j_backwards_iterator t(*this);
            ++(*this);
            return t;
         }

         T operator*() { return *it; }

      private:
         boost::math::tools::backward_recurrence_iterator< detail::bessel_jy_recurrence<T> > it;
      };

      template <class T>
      struct bessel_i_backwards_iterator
      {
         typedef std::ptrdiff_t difference_type;
         typedef T value_type;
         typedef T* pointer;
         typedef T& reference;
         typedef std::input_iterator_tag iterator_category;

         bessel_i_backwards_iterator(const T& v, const T& x)
            : it(detail::bessel_ik_recurrence<T>(v, x), boost::math::cyl_bessel_i(v, x))
         {
            if(v < -1)
               boost::math::policies::raise_domain_error("bessel_i_backwards_iterator<%1%>", "Order must be > 0 stable backwards recurrence but got %1%", v, boost::math::policies::policy<>());
         }
         bessel_i_backwards_iterator(const T& v, const T& x, const T& I_v)
            : it(detail::bessel_ik_recurrence<T>(v, x), I_v)
         {
            if(v < -1)
               boost::math::policies::raise_domain_error("bessel_i_backwards_iterator<%1%>", "Order must be > 0 stable backwards recurrence but got %1%", v, boost::math::policies::policy<>());
         }
         bessel_i_backwards_iterator(const T& v, const T& x, const T& I_v_plus_1, const T& I_v)
            : it(detail::bessel_ik_recurrence<T>(v, x), I_v_plus_1, I_v)
         {
            if(v < -1)
               boost::math::policies::raise_domain_error("bessel_i_backwards_iterator<%1%>", "Order must be > 0 stable backwards recurrence but got %1%", v, boost::math::policies::policy<>());
         }

         bessel_i_backwards_iterator& operator++()
         {
            ++it;
            return *this;
         }

         bessel_i_backwards_iterator operator++(int)
         {
            bessel_i_backwards_iterator t(*this);
            ++(*this);
            return t;
         }

         T operator*() { return *it; }

      private:
         boost::math::tools::backward_recurrence_iterator< detail::bessel_ik_recurrence<T> > it;
      };

      template <class T>
      struct bessel_i_forwards_iterator
      {
         typedef std::ptrdiff_t difference_type;
         typedef T value_type;
         typedef T* pointer;
         typedef T& reference;
         typedef std::input_iterator_tag iterator_category;

         bessel_i_forwards_iterator(const T& v, const T& x)
            : it(detail::bessel_ik_recurrence<T>(v, x), boost::math::cyl_bessel_i(v, x))
         {
            if(v > 1)
               boost::math::policies::raise_domain_error("bessel_i_forwards_iterator<%1%>", "Order must be < 0 stable forwards recurrence but got %1%", v, boost::math::policies::policy<>());
         }
         bessel_i_forwards_iterator(const T& v, const T& x, const T& I_v)
            : it(detail::bessel_ik_recurrence<T>(v, x), I_v)
         {
            if (v > 1)
               boost::math::policies::raise_domain_error("bessel_i_forwards_iterator<%1%>", "Order must be < 0 stable forwards recurrence but got %1%", v, boost::math::policies::policy<>());
         }
         bessel_i_forwards_iterator(const T& v, const T& x, const T& I_v_plus_1, const T& I_v)
            : it(detail::bessel_ik_recurrence<T>(v, x), I_v_plus_1, I_v)
         {
            if (v > 1)
               boost::math::policies::raise_domain_error("bessel_i_forwards_iterator<%1%>", "Order must be < 0 stable forwards recurrence but got %1%", v, boost::math::policies::policy<>());
         }

         bessel_i_forwards_iterator& operator++()
         {
            ++it;
            return *this;
         }

         bessel_i_forwards_iterator operator++(int)
         {
            bessel_i_forwards_iterator t(*this);
            ++(*this);
            return t;
         }

         T operator*() { return *it; }

      private:
         boost::math::tools::forward_recurrence_iterator< detail::bessel_ik_recurrence<T> > it;
      };

   }
} // namespaces

#endif // BOOST_MATH_BESSEL_ITERATORS_HPP


// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP
#define KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP

#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"
#include <Eigen/Core>

namespace KDL
{
    /**
     * Implementation of a inverse velocity kinematics algorithm based
     * on the generalize pseudo inverse to calculate the velocity
     * transformation from Cartesian to joint space of a general
     * KDL::Chain. It uses a svd-calculation based on householders
     * rotations.
     *
     * In case of a redundant robot this solver optimizes the following criterium:
     * g=0.5*sum(weight*(Desired_joint_positions - actual_joint_positions))^2 as described in
     *  A. Liegeois. Automatic supervisory control of the configuration and
     * behavior of multibody mechanisms. IEEE Transactions on Systems, Man, and
     * Cybernetics, 7(12):868–871, 1977
     *
     * @ingroup KinematicFamily
     */
    class ChainIkSolverVel_pinv_nso : public ChainIkSolverVel
    {
    public:
        /**
         * Constructor of the solver
         *
         * @param chain the chain to calculate the inverse velocity
         * kinematics for
         * @param opt_pos the desired positions of the chain used by to resolve the redundancy
         * @param weights the weights applied in the joint space
         * @param eps if a singular value is below this value, its
         * inverse is set to zero, default: 0.00001
         * @param maxiter maximum iterations for the svd calculation,
         * default: 150
         * @param alpha the null-space velocity gain
         *
         */
        ChainIkSolverVel_pinv_nso(const Chain& chain, const JntArray& opt_pos, const JntArray& weights, double eps=0.00001,int maxiter=150, double alpha = 0.25);
        explicit ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, double alpha = 0.25);
        ~ChainIkSolverVel_pinv_nso();

        virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
        /**
         * not (yet) implemented.
         *
         */
        virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};

        /**
         * Request the joint weights for optimization criterion
         *
         *
         * @return const reference to the joint weights
         */
        const JntArray& getWeights()const
        {
            return weights;
        }

        /**
         * Request the optimal joint positions
         *
         *
         * @return const reference to the optimal joint positions
         */
        const JntArray& getOptPos()const
        {
            return opt_pos;
        }

        /**
         * Request null space velocity gain
         *
         *
         * @return const reference to the null space velocity gain
         */
        const double& getAlpha()const
        {
            return alpha;
        }

        /**
         *Set joint weights for optimization criterion
         *
         *@param weights the joint weights
         *
         */
        virtual int setWeights(const JntArray &weights);

        /**
         *Set optimal joint positions
         *
         *@param opt_pos optimal joint positions
         *
         */
        virtual int setOptPos(const JntArray &opt_pos);

        /**
         *Set null psace velocity gain
         *
         *@param alpha NUllspace velocity cgain
         *
         */
        virtual int setAlpha(const double alpha);

        /**
         * Retrieve the latest return code from the SVD algorithm
         * @return 0 if CartToJnt() not yet called, otherwise latest SVD result code.
         */
        int getSVDResult()const {return svdResult;};

        /// @copydoc KDL::SolverI::updateInternalDataStructures
        virtual void updateInternalDataStructures();

    private:
        const Chain& chain;
        ChainJntToJacSolver jnt2jac;
        unsigned int nj;
        Jacobian jac;
        Eigen::MatrixXd U;
        Eigen::VectorXd S;
        Eigen::VectorXd Sinv;
        Eigen::MatrixXd V;
        Eigen::VectorXd tmp;
        Eigen::VectorXd tmp2;
        double eps;
        int maxiter;
        int svdResult;
        double alpha;
        JntArray weights;
        JntArray opt_pos;
    };
}
#endif


#pragma once

#include <boost/math/constants/constants.hpp>

namespace boltzmann {
namespace numbers {
constexpr const double PI = boost::math::constants::pi<double>();
}
}


#define BOOST_TEST_MODULE VectorArithmetics
#include <boost/test/unit_test.hpp>
#include <vexcl/mba.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/element_index.hpp>
#include <vexcl/temporary.hpp>
#include <vexcl/function.hpp>
#include "context_setup.hpp"

template <typename T>
inline std::array<T, 2> make_array(T x,  T y) {
    std::array<T, 2> p = {{x, y}};
    return p;
}

BOOST_AUTO_TEST_CASE(mba)
{
    std::vector< std::array<double,2> > p;

    p.push_back(make_array<double>(0.0, 0.0));
    p.push_back(make_array<double>(0.0, 1.0));
    p.push_back(make_array<double>(1.0, 0.0));
    p.push_back(make_array<double>(1.0, 1.0));
    p.push_back(make_array<double>(0.4, 0.4));
    p.push_back(make_array<double>(0.6, 0.6));

    std::vector<double> v;

    v.push_back( 0.2);
    v.push_back( 0.0);
    v.push_back( 0.0);
    v.push_back(-0.2);
    v.push_back(-1.0);
    v.push_back( 1.0);

    vex::mba<2> cloud(ctx,
            make_array<double>(-0.01, -0.01),
            make_array<double>( 1.01,  1.01),
            p, v, make_array<size_t>(2, 2)
            );

    const size_t n = 11;
    vex::vector<double> x(ctx, n);
    vex::vector<double> z(ctx, n);

    x = 1.0 * vex::element_index() / (n - 1.0);
    z = sin(cloud(x, 1.0 * vex::element_index() / (n - 1.0)));

    BOOST_CHECK_CLOSE(static_cast<double>(z[ 0]), sin( 0.2), 1e-6);
    BOOST_CHECK_CLOSE(static_cast<double>(z[ 4]), sin(-1.0), 1e-6);
    BOOST_CHECK_CLOSE(static_cast<double>(z[ 6]), sin( 1.0), 1e-6);
    BOOST_CHECK_CLOSE(static_cast<double>(z[10]), sin(-0.2), 1e-6);

    auto t = vex::make_temp<1>(1.0 * vex::element_index() / (n - 1.0));

    z = cloud(t, t);

    BOOST_CHECK_CLOSE(static_cast<double>(z[ 0]),  0.2, 1e-6);
    BOOST_CHECK_CLOSE(static_cast<double>(z[ 4]), -1.0, 1e-6);
    BOOST_CHECK_CLOSE(static_cast<double>(z[ 6]),  1.0, 1e-6);
    BOOST_CHECK_CLOSE(static_cast<double>(z[10]), -0.2, 1e-6);

}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright 2010 Scott McMurray.
// 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_HASH_DETAIL_UNBOUNDED_SHIFT_HPP
#define BOOST_HASH_DETAIL_UNBOUNDED_SHIFT_HPP

#include <boost/assert.hpp>

namespace boost {
namespace hashes {
namespace detail {

template <int N, typename T>
struct unbounded_shifter {
    static T shl(T x) { return unbounded_shifter<N-1, T>::shl(T(x << 1)); }
    static T shr(T x) { return unbounded_shifter<N-1, T>::shr(T(x >> 1)); }
};
template <typename T>
struct unbounded_shifter<0, T> {
    static T shl(T x) { return x; }
    static T shr(T x) { return x; }
};

template <int N, typename T>
T unbounded_shl(T x) {
    return unbounded_shifter<N, T>::shl(x);
}
template <int N, typename T>
T unbounded_shr(T x) {
    return unbounded_shifter<N, T>::shr(x);
}

template <int N, typename T>
T low_bits(T x) {
    T highmask = unbounded_shl<N, T>(~T());
    return T(x & ~highmask);
}

} // namespace detail
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_DETAIL_UNBOUNDED_SHIFT_HPP


//==================================================================================================
/*!
  @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_FRAC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FRAC_HPP_INCLUDED

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

  /*!
    @ingroup group-ieee
    This function object returns the fractional part of the input i.e. `x-trunc(x)`.

    @see trunc, modf

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

    @par Example:

      @snippet frac.cpp frac

    @par Possible output:

      @snippet frac.txt frac

  **/
  Value frac(Value const& x);
} }
#endif

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

#endif