/* SPDX-License-Identifier: BSD-3-Clause */
/* Copyright © 2020 Fragcolor Pte. Ltd. */

#ifndef CB_NO_BIGINT_BLOCKS

#include "math.h"
#include "shared.hpp"

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;

namespace chainblocks {
namespace BigInt {
inline Var to_var(const cpp_int &bi, std::vector<uint8_t> &buffer) {
  buffer.clear();
  buffer.emplace_back(uint8_t(bi < 0));
  export_bits(bi, std::back_inserter(buffer), 8);
  return Var(&buffer.front(), buffer.size());
}

inline cpp_int from_var(const CBVar &op) {
  cpp_int bib;
  import_bits(bib, op.payload.bytesValue + 1,
              op.payload.bytesValue + op.payload.bytesSize);
  auto negative = bool(op.payload.bytesValue[0]);
  if (negative)
    bib *= -1;
  return bib;
}

struct ToBigInt {
  std::vector<uint8_t> _buffer;

  static inline Types InputTypes{CoreInfo::IntType, CoreInfo::FloatType,
                                 CoreInfo::StringType, CoreInfo::BytesType};
  static CBTypesInfo inputTypes() { return InputTypes; }
  static CBTypesInfo outputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString outputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }

  CBVar activate(CBContext *context, const CBVar &input) {
    cpp_int bi;
    switch (input.valueType) {
    case Int: {
      bi = input.payload.intValue;
    } break;
    case Float: {
      bi = cpp_int(input.payload.floatValue);
    } break;
    case String: {
      bi = cpp_int(input.payload.stringValue);
    } break;
    case Bytes: {
      import_bits(bi, input.payload.bytesValue,
                  input.payload.bytesValue + input.payload.bytesSize);
    } break;
    default: {
      throw ActivationError("Invalid input type");
    }
    }
    return to_var(bi, _buffer);
  }
};

template <typename T>
struct BigIntBinaryOp : public ::chainblocks::Math::BinaryOperation<T> {
  std::deque<std::vector<uint8_t>> _buffers;
  size_t _offset{0};

  static inline Types BigIntInputTypes{
      {CoreInfo::BytesType, CoreInfo::BytesSeqType}};

  static CBTypesInfo inputTypes() { return BigIntInputTypes; }
  static CBOptionalString inputHelp() {
    return CBCCSTR("Any valid big integer(s) represented as bytes supported by "
                   "this operation.");
  }
  static CBTypesInfo outputTypes() { return BigIntInputTypes; }

  CBParametersInfo parameters() {
    static Parameters params{
        {"Operand",
         CBCCSTR("The bytes variable representing the operand"),
         {CoreInfo::BytesVarType, CoreInfo::BytesVarSeqType}}};
    return params;
  }

  CBVar activate(CBContext *context, const CBVar &input) {
    _offset = 0;
    return ::chainblocks::Math::BinaryOperation<T>::activate(context, input);
  }
};

#define BIGINT_MATH_OP(__NAME__, __OP__)                                       \
  struct __NAME__ : public BigIntBinaryOp<__NAME__> {                          \
    void operator()(CBVar &output, const CBVar &input, const CBVar &operand,   \
                    void *pself) {                                             \
      auto self = reinterpret_cast<__NAME__ *>(pself);                         \
      std::vector<uint8_t> *buffer = nullptr;                                  \
      if (self->_buffers.size() <= _offset) {                                  \
        buffer = &self->_buffers.emplace_back();                               \
      } else {                                                                 \
        buffer = &self->_buffers[_offset];                                     \
      }                                                                        \
      cpp_int bia = from_var(input);                                           \
      cpp_int bib = from_var(operand);                                         \
      cpp_int bres = bia __OP__ bib;                                           \
      output = to_var(bres, *buffer);                                          \
      _offset++;                                                               \
    }                                                                          \
  };

struct BigOperandBase {
  std::vector<uint8_t> _buffer;

  static CBTypesInfo inputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString inputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }
  static CBTypesInfo outputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString outputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }

  CBParametersInfo parameters() {
    static Parameters params{
        {"Operand",
         CBCCSTR("The bytes variable representing the operand"),
         {CoreInfo::BytesVarType}}};
    return params;
  }

  ParamVar _op{};

  void setParam(int index, const CBVar &value) { _op = value; }

  CBVar getParam(int index) { return _op; }

  void cleanup() { _op.cleanup(); }

  void warmup(CBContext *context) { _op.warmup(context); }

  const CBVar &getOperand() {
    CBVar &op = _op.get();
    if (op.valueType == None) {
      throw ActivationError("Operand is None, should be valid bigint bytes");
    }
    return op;
  }
};

struct RegOperandBase {
  std::vector<uint8_t> _buffer;

  static CBTypesInfo inputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString inputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }
  static CBTypesInfo outputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString outputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }

  CBParametersInfo parameters() {
    static Parameters params{{"Operand",
                              CBCCSTR("The integer operand, can be a variable"),
                              {CoreInfo::IntType, CoreInfo::IntVarType}}};
    return params;
  }

  ParamVar _op{};

  void setParam(int index, const CBVar &value) { _op = value; }

  CBVar getParam(int index) { return _op; }

  void cleanup() { _op.cleanup(); }

  void warmup(CBContext *context) { _op.warmup(context); }

  const CBVar &getOperand() {
    CBVar &op = _op.get();
    if (op.valueType == None) {
      throw ActivationError("Operand is None, should be an integer");
    }
    return op;
  }
};

BIGINT_MATH_OP(Add, +);
BIGINT_MATH_OP(Subtract, -);
BIGINT_MATH_OP(Multiply, *);
BIGINT_MATH_OP(Divide, /);
BIGINT_MATH_OP(Xor, ^);
BIGINT_MATH_OP(And, &);
BIGINT_MATH_OP(Or, |);
BIGINT_MATH_OP(Mod, %);

#define BIGINT_LOGIC_OP(__NAME__, __OP__)                                      \
  struct __NAME__ : public BigOperandBase {                                    \
    static CBTypesInfo outputTypes() { return CoreInfo::BoolType; }            \
    static CBOptionalString outputHelp() {                                     \
      return CBCCSTR(                                                          \
          "A boolean value repesenting the result of the logic operation.");   \
    }                                                                          \
                                                                               \
    CBVar activate(CBContext *context, const CBVar &input) {                   \
      cpp_int bia = from_var(input);                                           \
      auto op = getOperand();                                                  \
      cpp_int bib = from_var(op);                                              \
      bool res = bia __OP__ bib;                                               \
      return Var(res);                                                         \
    }                                                                          \
  }

BIGINT_LOGIC_OP(Is, ==);
BIGINT_LOGIC_OP(IsNot, !=);
BIGINT_LOGIC_OP(IsMore, >);
BIGINT_LOGIC_OP(IsLess, <);
BIGINT_LOGIC_OP(IsMoreEqual, >=);
BIGINT_LOGIC_OP(IsLessEqual, <=);

#define BIGINT_BINARY_OP(__NAME__, __OP__)                                     \
  struct __NAME__ : public BigIntBinaryOp<__NAME__> {                          \
    void operator()(CBVar &output, const CBVar &input, const CBVar &operand,   \
                    void *pself) {                                             \
      auto self = reinterpret_cast<__NAME__ *>(pself);                         \
      std::vector<uint8_t> *buffer = nullptr;                                  \
      if (self->_buffers.size() <= _offset) {                                  \
        buffer = &self->_buffers.emplace_back();                               \
      } else {                                                                 \
        buffer = &self->_buffers[_offset];                                     \
      }                                                                        \
      cpp_int bia = from_var(input);                                           \
      cpp_int bib = from_var(operand);                                         \
      cpp_int bres = __OP__(bia, bib);                                         \
      output = to_var(bres, *buffer);                                          \
      _offset++;                                                               \
    }                                                                          \
  };

BIGINT_BINARY_OP(Min, std::min);
BIGINT_BINARY_OP(Max, std::max);

#define BIGINT_REG_BINARY_OP(__NAME__, __OP__)                                 \
  struct __NAME__ : public RegOperandBase {                                    \
    CBVar activate(CBContext *context, const CBVar &input) {                   \
      cpp_int bia = from_var(input);                                           \
      auto op = getOperand();                                                  \
      if (op.valueType != Int)                                                 \
        throw ActivationError("Pow operand should be an Int");                 \
      cpp_int bres = __OP__(bia, op.payload.intValue);                         \
      return to_var(bres, _buffer);                                            \
    }                                                                          \
  }

BIGINT_REG_BINARY_OP(Pow, pow);

#define BIGINT_UNARY_OP(__NAME__, __OP__)                                      \
  struct __NAME__ : public RegOperandBase {                                    \
    CBParametersInfo parameters() { return {}; }                               \
    CBVar activate(CBContext *context, const CBVar &input) {                   \
      cpp_int bia = from_var(input);                                           \
      cpp_int bres = __OP__(bia);                                              \
      return to_var(bres, _buffer);                                            \
    }                                                                          \
  }

BIGINT_UNARY_OP(Sqrt, sqrt);

struct ShiftBase {
  ParamVar _shift{Var(0)};

  void setParam(int index, const CBVar &value) { _shift = value; }

  CBVar getParam(int index) { return _shift; }

  void cleanup() { _shift.cleanup(); }

  void warmup(CBContext *context) { _shift.warmup(context); }
};

struct Shift : public ShiftBase {
  std::vector<uint8_t> _buffer;

  static CBTypesInfo inputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString inputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }
  static CBTypesInfo outputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString outputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }

  CBParametersInfo parameters() {
    static Parameters params{
        {"By",
         CBCCSTR(
             "The shift is of the decimal point, i.e. of powers of ten, and is "
             "to the left if n is negative or to the right if n is positive."),
         {CoreInfo::IntType, CoreInfo::IntVarType}}};
    return params;
  }

  CBVar activate(CBContext *context, const CBVar &input) {
    cpp_int bi = from_var(input);
    cpp_dec_float_100 bf(bi);

    cpp_dec_float_100 bshift(_shift.get().payload.intValue);
    bshift = pow(cpp_dec_float_100(10), bshift);

    auto bres = cpp_int(bf * bshift);

    return to_var(bres, _buffer);
  }
};

struct ToFloat : public ShiftBase {
  static CBOptionalString help() {
    return CBCCSTR("Converts a big integer value to a floating point number.");
  }

  static CBTypesInfo inputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString inputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }

  static CBTypesInfo outputTypes() { return CoreInfo::FloatType; }
  static CBOptionalString outputHelp() {
    return CBCCSTR(
        "Floating point number representation of the big integer value.");
  }

  CBParametersInfo parameters() {
    static Parameters params{
        {"ShiftedBy",
         CBCCSTR(
             "The shift is of the decimal point, i.e. of powers of ten, and is "
             "to the left if n is negative or to the right if n is positive."),
         {CoreInfo::IntType}}};
    return params;
  }

  CBVar activate(CBContext *context, const CBVar &input) {
    cpp_int bi = from_var(input);
    cpp_dec_float_100 bf(bi);

    cpp_dec_float_100 bshift(_shift.get().payload.intValue);
    bshift = pow(cpp_dec_float_100(10), bshift);

    auto bres = bf * bshift;

    return Var(bres.convert_to<double>());
  }
};

struct ToInt {
  static CBOptionalString help() {
    return CBCCSTR("Converts a big integer value to an integer.");
  }

  static CBTypesInfo inputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString inputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }

  static CBTypesInfo outputTypes() { return CoreInfo::IntType; }
  static CBOptionalString outputHelp() {
    return CBCCSTR("Integer representation of the big integer value.");
  }

  CBVar activate(CBContext *context, const CBVar &input) {
    cpp_int bi = from_var(input);
    return Var(bi.convert_to<int64_t>());
  }
};

struct FromFloat : public ShiftBase {
  static CBOptionalString help() {
    return CBCCSTR("Converts a floating point number to a big integer.");
  }

  static CBTypesInfo inputTypes() { return CoreInfo::FloatType; }
  static CBOptionalString inputHelp() {
    return CBCCSTR("Floating point number.");
  }

  static CBTypesInfo outputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString outputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }

  CBParametersInfo parameters() {
    static Parameters params{
        {"ShiftedBy",
         CBCCSTR(
             "The shift is of the decimal point, i.e. of powers of ten, and is "
             "to the left if n is negative or to the right if n is positive."),
         {CoreInfo::IntType}}};
    return params;
  }

  CBVar activate(CBContext *context, const CBVar &input) {
    cpp_dec_float_100 bi(input.payload.floatValue);

    cpp_dec_float_100 bshift(_shift.get().payload.intValue);
    bshift = pow(cpp_dec_float_100(10), bshift);

    auto bres = bi * bshift;
    cpp_int bo(bres);

    return to_var(bo, _buffer);
  }

private:
  std::vector<uint8_t> _buffer;
};

struct ToString {
  static CBOptionalString help() {
    return CBCCSTR("Converts the value to a string representation.");
  }

  static CBTypesInfo inputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString inputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }
  static CBTypesInfo outputTypes() { return CoreInfo::StringType; }
  static CBOptionalString outputHelp() {
    return CBCCSTR("String representation of the big integer value.");
  }

  CBVar activate(CBContext *context, const CBVar &input) {
    cpp_int bi = from_var(input);
    _buffer = bi.str();
    return Var(_buffer);
  }

private:
  std::string _buffer;
};

struct ToBytes {
  std::vector<uint8_t> _buffer;

  static CBTypesInfo inputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString inputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }
  static CBTypesInfo outputTypes() { return CoreInfo::BytesType; }

  CBParametersInfo parameters() {
    static Parameters params{{"Bits",
                              CBCCSTR("The desired amount of bits for the "
                                      "output or 0 for automatic packing."),
                              {CoreInfo::IntType}}};
    return params;
  }

  ParamVar _bits{Var(0)};

  void setParam(int index, const CBVar &value) { _bits = value; }

  CBVar getParam(int index) { return _bits; }

  void warmup(CBContext *context) { _bits.warmup(context); }
  void cleanup() { _bits.cleanup(); }

  CBVar activate(CBContext *context, const CBVar &input) {
    const auto bits = _bits.get().payload.intValue;
    if (bits <= 0) {
      CBVar fixedInput = input;
      fixedInput.payload.bytesValue++;
      fixedInput.payload.bytesSize--;
      return fixedInput;
    } else {
      cpp_int bi = from_var(input);
      const auto usedBits = msb(bi) + 1;
      if (usedBits > bits) {
        throw ActivationError(
            "The number of used bits is higher than the requested bits");
      }
      const auto padding = bits - usedBits;
      _buffer.clear();
      export_bits(bi, std::back_inserter(_buffer), 8);
      // this is because we are using little endianess
      _buffer.insert(_buffer.begin(), padding / 8, 0);
      return Var(_buffer);
    }
  }
};

struct ToHex {
  static CBOptionalString help() {
    return CBCCSTR("Converts the value to a hexadecimal representation.");
  }

  static inline Types toHexTypes{CoreInfo::IntType, CoreInfo::BytesType,
                                 CoreInfo::StringType};
  static CBTypesInfo inputTypes() { return toHexTypes; }

  static CBTypesInfo outputTypes() { return CoreInfo::StringType; }
  static CBOptionalString outputHelp() {
    return CBCCSTR("Hexadecimal representation of the integer value.");
  }

  CBVar activate(CBContext *context, const CBVar &input) {
    CBVar fixedInput = input;
    fixedInput.payload.bytesValue++;
    fixedInput.payload.bytesSize--;
    _stream.tryWriteHex(fixedInput);
    return Var(_stream.str());
  }

private:
  VarStringStream _stream;
};

struct Abs {
  static CBOptionalString help() {
    return CBCCSTR("Computes the absolute value of a big integer.");
  }

  static CBTypesInfo inputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString inputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }

  static CBTypesInfo outputTypes() { return CoreInfo::BytesType; }
  static CBOptionalString outputHelp() {
    return CBCCSTR("Big integer represented as bytes.");
  }

  CBVar activate(CBContext *context, const CBVar &input) {
    cpp_int bi = from_var(input);
    cpp_int abi = abs(bi);
    return to_var(abi, _buffer);
  }

private:
  std::vector<uint8_t> _buffer;
};

void registerBlocks() {
  REGISTER_CBLOCK("BigInt", ToBigInt);
  REGISTER_CBLOCK("BigInt.Add", Add);
  REGISTER_CBLOCK("BigInt.Subtract", Subtract);
  REGISTER_CBLOCK("BigInt.Multiply", Multiply);
  REGISTER_CBLOCK("BigInt.Divide", Divide);
  REGISTER_CBLOCK("BigInt.Xor", Xor);
  REGISTER_CBLOCK("BigInt.And", And);
  REGISTER_CBLOCK("BigInt.Or", Or);
  REGISTER_CBLOCK("BigInt.Mod", Mod);
  REGISTER_CBLOCK("BigInt.Shift", Shift);
  REGISTER_CBLOCK("BigInt.ToFloat", ToFloat);
  REGISTER_CBLOCK("BigInt.ToInt", ToInt);
  REGISTER_CBLOCK("BigInt.FromFloat", FromFloat);
  REGISTER_CBLOCK("BigInt.ToString", ToString);
  REGISTER_CBLOCK("BigInt.ToBytes", ToBytes);
  REGISTER_CBLOCK("BigInt.ToHex", ToHex);
  REGISTER_CBLOCK("BigInt.Is", Is);
  REGISTER_CBLOCK("BigInt.IsNot", IsNot);
  REGISTER_CBLOCK("BigInt.IsMore", IsMore);
  REGISTER_CBLOCK("BigInt.IsLess", IsLess);
  REGISTER_CBLOCK("BigInt.IsMoreEqual", IsMoreEqual);
  REGISTER_CBLOCK("BigInt.IsLessEqual", IsLessEqual);
  REGISTER_CBLOCK("BigInt.Min", Min);
  REGISTER_CBLOCK("BigInt.Max", Max);
  REGISTER_CBLOCK("BigInt.Pow", Pow);
  REGISTER_CBLOCK("BigInt.Abs", Abs);
  REGISTER_CBLOCK("BigInt.Sqrt", Sqrt);
}
} // namespace BigInt
} // namespace chainblocks

#else
namespace chainblocks {
namespace BigInt {
void registerBlocks() {}
} // namespace BigInt
} // namespace chainblocks
#endif


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <eve/function/ellint_rd.hpp>
#include <boost/math/special_functions/ellint_rd.hpp>
#include <eve/wide.hpp>

TTS_CASE_TPL("Check eve::ellint_rd behavior", EVE_TYPE)
{
  using elt_t = eve::element_type_t<T>;
  TTS_ULP_EQUAL(eve::ellint_rd(T(0.2), T(0.4), T(0.1)),  T(boost::math::ellint_rd(elt_t(0.2), elt_t(0.4), elt_t(0.1))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rd(T(1.5), T(1), T(1.7)),T(boost::math::ellint_rd(elt_t(1.5), elt_t(1), elt_t(1.7))), 1.0);
  TTS_ULP_EQUAL(eve::ellint_rd(T(2), T(0), T(7)),  T(boost::math::ellint_rd(elt_t(2), elt_t(0), elt_t(7))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rd(T(0), T(5), T(3.4)),  T(boost::math::ellint_rd(elt_t(0), elt_t(5), elt_t(3.4))),   1.0);
  TTS_ULP_EQUAL(eve::ellint_rd(T(2), T(5), T(0.5)),  T(boost::math::ellint_rd(elt_t(2), elt_t(5), elt_t(0.5))),   1.0);
                                                                        }


//==================================================================================================
/*!
  @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_CONSTANT_DEGINRAD_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEGINRAD_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Deginrad Deginrad (function template)

  Generates the constant \f$\frac\pi{180}\f$.

  @headerref{<boost/simd/constant/deginrad.hpp>}

  @par Description

  1.  @code
      template<typename T> T Deginrad();
      @endcode

  2.  @code
      template<typename T> T Deginrad( boost::simd::as_<T> const& target );
      @endcode

  Generates the constant \f$\frac\pi{180}\f$ usable to convert degrees to radians.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `Pi<T>()/180`.

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/deginrad.hpp>
#include <boost/simd/constant/simd/deginrad.hpp>

#endif


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  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
//!\brief Tests for computing the value of a Bessel function using a trapezoid integration of (fixed_point).

#include <cmath>

#define BOOST_TEST_MODULE test_negatable_math_trapezoid_integral
#define BOOST_LIB_DIAGNOSTIC

#include <boost/cstdint.hpp>
#include <boost/fixed_point/fixed_point.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  template<typename NumericType, typename RealFunctionType>
  NumericType integral(const NumericType& a,
                       const NumericType& b,
                       const NumericType& tol,
                       RealFunctionType real_function)
  {
    boost::uint_fast32_t n2(1);

    NumericType step = ((b - a) / 2U);

    NumericType result = (real_function(a) + real_function(b)) * step;

    const boost::uint_fast8_t k_max = UINT8_C(16);

    for(boost::uint_fast8_t k = UINT8_C(0); k < k_max; ++k)
    {
      NumericType sum(0);

      for(boost::uint_fast32_t j(0U); j < n2; ++j)
      {
        const boost::uint_fast32_t two_j_plus_one = (j * UINT32_C(2)) + UINT32_C(1);

        sum += real_function(a + (step * two_j_plus_one));
      }

      const NumericType tmp = result;

      result = (result / 2U) + (step * sum);

      using std::fabs;
      const NumericType ratio = fabs(tmp / result);
      const NumericType delta = fabs(ratio - 1U);

      if((k > UINT8_C(1)) && (delta < tol))
      {
        break;
      }

      n2 *= 2U;

      step /= 2U;
    }

    return result;
  }

  template<typename NumericType>
  NumericType cyl_bessel_j(const boost::uint_fast8_t n, const NumericType& x)
  {
    using std::sqrt;
    const NumericType tol = sqrt(std::numeric_limits<NumericType>::epsilon());

    const NumericType jn =
      local::integral(NumericType(0),
                      boost::math::constants::pi<NumericType>(),
                      tol,
                      [&x, &n](const NumericType& t) -> NumericType
                      {
                        using std::cos;
                        using std::sin;

                        return cos(x * sin(t) - (t * n));
                      }) / boost::math::constants::pi<NumericType>();

    return jn;
  }
}

BOOST_AUTO_TEST_CASE(test_negatable_math_trapezoid_integral)
{
  typedef boost::fixed_point::negatable<15, -240> fixed_point_type;

  typedef fixed_point_type::float_type float_point_type;

  const fixed_point_type tol = ldexp(fixed_point_type(1), fixed_point_type::resolution + 6);

  // Compute y = cyl_bessel_j(2, 123 / 100).
  const fixed_point_type j2   = local::cyl_bessel_j(UINT8_C(2), fixed_point_type(123) / 100);

  // Assign the known reference value of the Bessel function.
  const float_point_type reference = float_point_type("0.166369383786814073512678524315131594371033482453328555149562207827319927054822411949870923");

  BOOST_CHECK_CLOSE_FRACTION(j2, reference, tol);
}


// geometric_examples.cpp

// Copyright Paul A. Bristow 2010.

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

// This file is written to be included from a Quickbook .qbk document.
// It can still be compiled by the C++ compiler, and run.
// Any output can also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// Examples of using the geometric distribution.

//[geometric_eg1_1
/*`
For this example, we will opt to #define two macros to control
the error and discrete handling policies.
For this simple example, we want to avoid throwing
an exception (the default policy) and just return infinity.
We want to treat the distribution as if it was continuous,
so we choose a discrete_quantile policy of real,
rather than the default policy integer_round_outwards.
*/
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error
#define BOOST_MATH_DISCRETE_QUANTILE_POLICY real
/*`
[caution It is vital to #include distributions etc *after* the above #defines]
After that we need some includes to provide easy access to the negative binomial distribution,
and we need some std library iostream, of course.
*/
#include <boost/math/distributions/geometric.hpp>
  // for geometric_distribution
  using ::boost::math::geometric_distribution; //
  using ::boost::math::geometric; // typedef provides default type is double.
  using  ::boost::math::pdf; // Probability mass function.
  using  ::boost::math::cdf; // Cumulative density function.
  using  ::boost::math::quantile;

#include <boost/math/distributions/negative_binomial.hpp>
  // for negative_binomial_distribution
  using boost::math::negative_binomial; // typedef provides default type is double.

#include <boost/math/distributions/normal.hpp>
  // for negative_binomial_distribution
  using boost::math::normal; // typedef provides default type is double.

#include <iostream>
  using std::cout; using std::endl;
  using std::noshowpoint; using std::fixed; using std::right; using std::left;
#include <iomanip>
  using std::setprecision; using std::setw;

#include <limits>
  using std::numeric_limits;
//] [geometric_eg1_1]

int main()
{
  cout <<"Geometric distribution example" << endl;
  cout << endl;

  cout.precision(4); // But only show a few for this example.
  try
  {
//[geometric_eg1_2
/*`
It is always sensible to use try and catch blocks because defaults policies are to
throw an exception if anything goes wrong.

Simple try'n'catch blocks (see below) will ensure that you get a
helpful error message instead of an abrupt (and silent) program abort.

[h6 Throwing a dice]
The Geometric distribution describes the probability (/p/) of a number of failures
to get the first success in /k/ Bernoulli trials.
(A [@http://en.wikipedia.org/wiki/Bernoulli_distribution Bernoulli trial]
is one with only two possible outcomes, success of failure,
and /p/ is the probability of success).

Suppose an 'fair' 6-face dice is thrown repeatedly:
*/
    double success_fraction = 1./6; // success_fraction (p) = 0.1666
    // (so failure_fraction is 1 - success_fraction = 5./6 = 1- 0.1666 = 0.8333)

/*`If the dice is thrown repeatedly until the *first* time a /three/ appears.
The probablility distribution of the number of times it is thrown *not* getting a /three/
 (/not-a-threes/ number of failures to get a /three/)
is a geometric distribution with the success_fraction = 1/6 = 0.1666[recur].

We therefore start by constructing a geometric distribution
with the one parameter success_fraction, the probability of success.
*/
    geometric g6(success_fraction); // type double by default.
/*`
To confirm, we can echo the success_fraction parameter of the distribution.
*/
    cout << "success fraction of a six-sided dice is " << g6.success_fraction() << endl;
/*`So the probability of getting a three at the first throw (zero failures) is
*/
    cout << pdf(g6, 0) << endl; // 0.1667
    cout << cdf(g6, 0) << endl; // 0.1667
/*`Note that the cdf and pdf are identical because the is only one throw.
If we want the probability of getting the first /three/ on the 2nd throw:
*/
    cout << pdf(g6, 1) << endl; // 0.1389

/*`If we want the probability of getting the first /three/ on the 1st or 2nd throw
(allowing one failure):
*/
    cout << "pdf(g6, 0) + pdf(g6, 1) = " << pdf(g6, 0) + pdf(g6, 1) << endl;
/*`Or more conveniently, and more generally,
we can use the Cumulative Distribution Function CDF.*/

    cout << "cdf(g6, 1) = " << cdf(g6, 1) << endl; // 0.3056

/*`If we allow many more (12) throws, the probability of getting our /three/ gets very high:*/
    cout << "cdf(g6, 12) = " << cdf(g6, 12) << endl; // 0.9065 or 90% probability.
/*`If we want to be much more confident, say 99%,
we can estimate the number of throws to be this sure
using the inverse or quantile.
*/
    cout << "quantile(g6, 0.99) = " << quantile(g6, 0.99) << endl; // 24.26
/*`Note that the value returned is not an integer:
if you want an integer result you should use either floor, round or ceil functions,
or use the policies mechanism.
See [link math_toolkit.policy.pol_tutorial.understand_dis_quant
Understanding Quantiles of Discrete Distributions]

The geometric distribution is related to the negative binomial
__spaces `negative_binomial_distribution(RealType r, RealType p);` with parameter /r/ = 1.
So we could get the same result using the negative binomial,
but using the geometric the results will be faster, and may be more accurate.
*/
    negative_binomial nb(1, success_fraction);
    cout << pdf(nb, 1) << endl; // 0.1389
    cout << cdf(nb, 1) << endl; // 0.3056
/*`We could also the complement to express the required probability
as 1 - 0.99 = 0.01 (and get the same result):
*/
    cout << "quantile(complement(g6, 1 - p))  " << quantile(complement(g6, 0.01)) << endl; // 24.26
/*`
Note too that Boost.Math geometric distribution is implemented as a continuous function.
Unlike other implementations (for example R) it *uses* the number of failures as a *real* parameter,
not as an integer. If you want this integer behaviour, you may need to enforce this by
rounding the parameter you pass, probably rounding down, to the nearest integer.
For example, R returns the success fraction probability for all values of failures
from 0 to 0.999999 thus:
[pre
__spaces R> formatC(pgeom(0.0001,0.5, FALSE), digits=17) "               0.5"
] [/pre]
So in Boost.Math the equivalent is
*/
    geometric g05(0.5);  // Probability of success = 0.5 or 50%
    // Output all potentially significant digits for the type, here double.

#ifdef BOOST_NO_CXX11_NUMERIC_LIMITS
  int max_digits10 = 2 + (boost::math::policies::digits<double, boost::math::policies::policy<> >() * 30103UL) / 100000UL;
  cout << "BOOST_NO_CXX11_NUMERIC_LIMITS is defined" << endl;
#else
  int max_digits10 = std::numeric_limits<double>::max_digits10;
#endif
  cout << "Show all potentially significant decimal digits std::numeric_limits<double>::max_digits10 = "
    << max_digits10 << endl;
  cout.precision(max_digits10); //

    cout << cdf(g05, 0.0001) << endl; // returns 0.5000346561579232, not exact 0.5.
/*`To get the R discrete behaviour, you simply need to round with,
for example, the `floor` function.
*/
    cout << cdf(g05, floor(0.0001)) << endl; // returns exactly 0.5
/*`
[pre
`> formatC(pgeom(0.9999999,0.5, FALSE), digits=17) [1] "              0.25"`
`> formatC(pgeom(1.999999,0.5, FALSE), digits=17)[1] "              0.25" k = 1`
`> formatC(pgeom(1.9999999,0.5, FALSE), digits=17)[1] "0.12500000000000003" k = 2`
] [/pre]
shows that R makes an arbitrary round-up decision at about 1e7 from the next integer above.
This may be convenient in practice, and could be replicated in C++ if desired.

[h6 Surveying customers to find one with a faulty product]
A company knows from warranty claims that 2% of their products will be faulty,
so the 'success_fraction' of finding a fault is 0.02.
It wants to interview a purchaser of faulty products to assess their 'user experience'.

To estimate how many customers they will probably need to contact
in order to find one who has suffered from the fault,
we first construct a geometric distribution with probability 0.02,
and then chose a confidence, say 80%, 95%, or 99% to finding a customer with a fault.
Finally, we probably want to round up the result to the integer above using the `ceil` function.
(We could also use a policy, but that is hardly worthwhile for this simple application.)

(This also assumes that each customer only buys one product:
if customers bought more than one item,
the probability of finding a customer with a fault obviously improves.)
*/
    cout.precision(5);
    geometric g(0.02); // On average, 2 in 100 products are faulty.
    double c = 0.95; // 95% confidence.
    cout << " quantile(g, " << c << ") = " << quantile(g, c) << endl;

    cout << "To be " << c * 100
      << "% confident of finding we customer with a fault, need to survey "
      <<  ceil(quantile(g, c)) << " customers." << endl; // 148
    c = 0.99; // Very confident.
    cout << "To be " << c * 100
      << "% confident of finding we customer with a fault, need to survey "
      <<  ceil(quantile(g, c)) << " customers." << endl; // 227
    c = 0.80; // Only reasonably confident.
    cout << "To be " << c * 100
      << "% confident of finding we customer with a fault, need to survey "
      <<  ceil(quantile(g, c)) << " customers." << endl; // 79

/*`[h6 Basket Ball Shooters]
According to Wikipedia, average pro basket ball players get
[@http://en.wikipedia.org/wiki/Free_throw free throws]
in the baskets 70 to 80 % of the time,
but some get as high as 95%, and others as low as 50%.
Suppose we want to compare the probabilities
of failing to get a score only on the first or on the fifth shot?
To start we will consider the average shooter, say 75%.
So we construct a geometric distribution
with success_fraction parameter 75/100 = 0.75.
*/
    cout.precision(2);
    geometric gav(0.75); // Shooter averages 7.5 out of 10 in the basket.
/*`What is probability of getting 1st try in the basket, that is with no failures? */
    cout << "Probability of score on 1st try = " << pdf(gav, 0) << endl; // 0.75
/*`This is, of course, the success_fraction probability 75%.
What is the probability that the shooter only scores on the fifth shot?
So there are 5-1 = 4 failures before the first success.*/
    cout << "Probability of score on 5th try = " << pdf(gav, 4) << endl; // 0.0029
/*`Now compare this with the poor and the best players success fraction.
We need to constructing new distributions with the different success fractions,
and then get the corresponding probability density functions values:
*/
    geometric gbest(0.95);
    cout << "Probability of score on 5th try = " << pdf(gbest, 4) << endl; // 5.9e-6
    geometric gmediocre(0.50);
    cout << "Probability of score on 5th try = " << pdf(gmediocre, 4) << endl; // 0.031
/*`So we can see the very much smaller chance (0.000006) of 4 failures by the best shooters,
compared to the 0.03 of the mediocre.*/

/*`[h6 Estimating failures]
Of course one man's failure is an other man's success.
So a fault can be defined as a 'success'.

If a fault occurs once after 100 flights, then one might naively say
that the risk of fault is obviously 1 in 100 = 1/100, a probability of 0.01.

This is the best estimate we can make, but while it is the truth,
it is not the whole truth,
for it hides the big uncertainty when estimating from a single event.
"One swallow doesn't make a summer."
To show the magnitude of the uncertainty, the geometric
(or the negative binomial) distribution can be used.

If we chose the popular 95% confidence in the limits, corresponding to an alpha of 0.05,
because we are calculating a two-sided interval, we must divide alpha by two.
*/
    double alpha = 0.05;
    double k = 100; // So frequency of occurence is 1/100.
    cout << "Probability is failure is " << 1/k << endl;
    double t = geometric::find_lower_bound_on_p(k, alpha/2);
    cout << "geometric::find_lower_bound_on_p(" << int(k) << ", " << alpha/2 << ") = "
      << t << endl; // 0.00025
    t = geometric::find_upper_bound_on_p(k, alpha/2);
    cout << "geometric::find_upper_bound_on_p(" << int(k) << ", " << alpha/2 << ") = "
      << t << endl; // 0.037
/*`So while we estimate the probability is 0.01, it might lie between 0.0003 and 0.04.
Even if we relax our confidence to alpha = 90%, the bounds only contract to 0.0005 and 0.03.
And if we require a high confidence, they widen to 0.00005 to 0.05.
*/
    alpha = 0.1; // 90% confidence.
    t = geometric::find_lower_bound_on_p(k, alpha/2);
    cout << "geometric::find_lower_bound_on_p(" << int(k) << ", " << alpha/2 << ") = "
      << t << endl; // 0.0005
    t = geometric::find_upper_bound_on_p(k, alpha/2);
    cout << "geometric::find_upper_bound_on_p(" << int(k) << ", " << alpha/2 << ") = "
      << t << endl; // 0.03

    alpha = 0.01; // 99% confidence.
    t = geometric::find_lower_bound_on_p(k, alpha/2);
    cout << "geometric::find_lower_bound_on_p(" << int(k) << ", " << alpha/2 << ") = "
      << t << endl; // 5e-005
    t = geometric::find_upper_bound_on_p(k, alpha/2);
    cout << "geometric::find_upper_bound_on_p(" << int(k) << ", " << alpha/2 << ") = "
        << t << endl; // 0.052
/*`In real life, there will usually be more than one event (fault or success),
when the negative binomial, which has the neccessary extra parameter, will be needed.
*/

/*`As noted above, using a catch block is always a good idea,
even if you hope not to use it!
*/
  }
  catch(const std::exception& e)
  { // Since we have set an overflow policy of ignore_error,
    // an overflow exception should never be thrown.
     std::cout << "\nMessage from thrown exception was:\n " << e.what() << std::endl;
/*`
For example, without a ignore domain error policy,
if we asked for ``pdf(g, -1)`` for example,
we would get an unhelpful abort, but with a catch:
[pre
Message from thrown exception was:
 Error in function boost::math::pdf(const exponential_distribution<double>&, double):
 Number of failures argument is -1, but must be >= 0 !
] [/pre]
*/
//] [/ geometric_eg1_2]
  }
  return 0;
}  // int main()

/*
Output is:

  Geometric distribution example

  success fraction of a six-sided dice is 0.1667
  0.1667
  0.1667
  0.1389
  pdf(g6, 0) + pdf(g6, 1) = 0.3056
  cdf(g6, 1) = 0.3056
  cdf(g6, 12) = 0.9065
  quantile(g6, 0.99) = 24.26
  0.1389
  0.3056
  quantile(complement(g6, 1 - p))  24.26
  0.5000346561579232
  0.5
   quantile(g, 0.95) = 147.28
  To be 95% confident of finding we customer with a fault, need to survey 148 customers.
  To be 99% confident of finding we customer with a fault, need to survey 227 customers.
  To be 80% confident of finding we customer with a fault, need to survey 79 customers.
  Probability of score on 1st try = 0.75
  Probability of score on 5th try = 0.0029
  Probability of score on 5th try = 5.9e-006
  Probability of score on 5th try = 0.031
  Probability is failure is 0.01
  geometric::find_lower_bound_on_p(100, 0.025) = 0.00025
  geometric::find_upper_bound_on_p(100, 0.025) = 0.037
  geometric::find_lower_bound_on_p(100, 0.05) = 0.00051
  geometric::find_upper_bound_on_p(100, 0.05) = 0.03
  geometric::find_lower_bound_on_p(100, 0.005) = 5e-005
  geometric::find_upper_bound_on_p(100, 0.005) = 0.052

*/


/*
    This file is part of control-lib.

    Copyright (c) 2020, 2021, 2022 Bernardo Fichera <bernardo.fichera@gmail.com>

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef CONTROLLIB_SPATIAL_SE3_HPP
#define CONTROLLIB_SPATIAL_SE3_HPP

#include <Eigen/Geometry>

namespace control_lib {
    namespace spatial {
        struct SE3 {
            /* Init via translation and orientation */
            SE3(const Eigen::Matrix3d& rot, const Eigen::Vector3d& trans) : _rot(rot), _trans(trans) {}

            /* Init via vector representation */
            SE3(const Eigen::Matrix<double, 6, 1>& x) : _trans(x.head(3)), _rot(Eigen::AngleAxisd(x.tail(3).norm(), x.tail(3).normalized())) {}

            /* Default constructor */
            SE3() = default;

            /* Space elements difference */
            Eigen::Matrix<double, 6, 1> operator-(SE3 const& obj) const { return obj.actionInverse(*this).log(); }

            /* Space dimension */
            constexpr static size_t dimension() { return 6; }

            /* Translation & rotation */
            Eigen::Vector3d _trans;
            Eigen::Matrix3d _rot;

            /* Tangent and contagent plane elements (optionals) */
            Eigen::Matrix<double, 6, 1> _vel, _acc, _eff;

        protected:
            SE3 action(const SE3& pose) const { return SE3(_rot * pose._rot, _trans + _rot * pose._trans); }

            SE3 actionInverse(const SE3& pose) const { return SE3(_rot.transpose() * pose._rot, _rot.transpose() * (pose._trans - _trans)); }

            Eigen::Matrix<double, 6, 1> log(const SE3& pose) const
            {
                Eigen::AngleAxisd aa(pose._rot);

                Eigen::Vector3d omega = aa.angle() * aa.axis();

                Eigen::Matrix3d omega_skew;
                omega_skew << 0, -omega(2), omega(1),
                    omega(2), 0, -omega(0),
                    -omega(1), omega(0), 0;

                double theta = omega.norm(), A = std::sin(theta) / theta, B = (1 - std::cos(theta)) / std::pow(theta, 2);

                return (Eigen::Matrix<double, 6, 1>() << (Eigen::Matrix3d::Identity() - 0.5 * omega_skew + (1 - 0.5 * A / B) / std::pow(theta, 2) * omega_skew * omega_skew) * pose._trans, omega).finished();
            }

            Eigen::Matrix<double, 6, 1> log() { return log(*this); }
        };
    } // namespace spatial

} // namespace control_lib

#endif // CONTROLLIB_SPATIAL_SE3_HPP


/*!
\brief
Demonstrates plotting various types (including user-defined like multiprecision that can be converted to double.
*/

//  convertible_to_double.cpp

// Copyright Paul A. Bristow 2018, 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)

#include <boost/svg_plot/svg_2d_plot.hpp>
// using namespace boost::svg;
// Very convenient to allow easy access to colors and other items.

#include <map>
// using std::map;
#include <cmath>
//using std::sin;
//using std::cos;
//using std::tan;

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

// Example of a Boost.Multiprecision type.
#include <boost/multiprecision/cpp_bin_float.hpp>
// using boost::multiprecision:: cpp_bin_float_quad;
// not is_convertible to double, but is_constructible to double.

// Example of a User Defined Type - a fixed point type can also be plotted OK.
//#include <boost/fixed_point/fixed_point_negatable.hpp>
//#include <boost/fixed_point/fixed_point_negatable_cmath.hpp>
// for typedef boost::fixed_point::negatable<15, -16> fixed_point_type;

// Some functions to generate some trig functions.
// template of floating-point type to allow testing of any type.
template <typename T>
T f(T x)
{
  return sin(x);
}

template <typename T>
T g(T x)
{
  return cos(x);
}

template <typename T>
T h(T x)
{
  return tan(x);
}

template <typename T = double>
void trig_plots()
{
  using namespace boost::svg; // Very convenient to allow easy access to colors and other items.

  std::map<T, T> sin_data, cos_data, tan_data, sincos_data;

  T step = boost::math::constants::pi<T>() / 8;  // Interval between function data points.

   // Generate some trigonometric data to plot.
  for (T i = static_cast<T>(0); i <= static_cast<T>(10); i += step)
  {
    sin_data[i] = f(i); // sin
    cos_data[i] = g(i); // cos
    tan_data[i] = h(i); // tan
    sincos_data[i] = sin(g(i)); // sincos
  } // for i

  svg_2d_plot my_plot; // Data structure to hold the plot.

                       // Size/scale settings.
  my_plot.size(700, 500) // SVG image size (pixel).
    .x_range(-0.5, 10.5) // Range of x and y axes,
    .y_range(-1.1, 1.1); // chosen to ensure that the maxima and minimax
                         // are not just on the edge of the plot window.

                         // Text settings.
  my_plot.title("Plot of sin, cos, tan &  sincos functions")
  // Note: for ampersand must use Unicode & because it is a reserved symbol in SVG XML.
  // Search engines will provide Unicodes by querying "UNicode ampersand"
  // at sites like https://unicode.org/,
  // http://www.fileformat.info/info/unicode/char/0026/index.htm and others.
    .title_font_size(28)
    .x_label("x Axis Units")
    .y_major_labels_side(left)
    .y_major_grid_on(true);

    // Layout options:
  my_plot.legend_on(true) // Want a legend box.
    .plot_window_on(true) // want a plot window with axis labels etc outside.
    .x_label_on(true) // Label X-axis ticks with their values.
                      //.y_label_on(false)  // false is default.
    ;

  // Plot color settings.
  // (Note use of chaining to add settings).
  my_plot
    .background_color(darkgreen)
    .legend_background_color(lightgray)
    .legend_border_color(black)
    .plot_background_color(lightgoldenrodyellow)
    .title_color(white)
    .y_major_grid_color(black);

    // X axis settings.
  my_plot.x_major_interval(2)
    .x_major_tick_length(14)
    .x_major_tick_width(1)
    .x_minor_tick_length(7)
    .x_minor_tick_width(1)
    .x_num_minor_ticks(3)

    // Y axis settings.
    .y_major_interval(25)
    .y_num_minor_ticks(5);

    // Legend settings.
  my_plot.legend_title_font_size(15)
    .legend_title("Legend");

  my_plot.plot(sin_data, "sin(x)")
    .line_on(true) // Line joining data points, using default color black.
    .shape(circlet) // and circle marker showing data points.
    .size(10) // Size (diameter pixels) of circlet data point marker.
    .fill_color(yellow) // Outline is default black and centre yellow.
                        // Default is no bezier.  Note angularity at the minima and maxima.
    ;

  my_plot.plot(cos_data, "cos(x)")
    .line_color(blue) // Defaults to showing line, but not in legend.
    .line_on(true) // Needed to show in the legend.
    .line_width(1) // thinner line.
    .shape(square) // Center of square has the data point coordinate.
    .size(5)
    .fill_color(red)  // Center of square.
    ;

  my_plot.plot(tan_data, "tan(x)")
    .line_on(false)  // No line joining points.
    .shape(cone) // bottom point of cone has the coordinate of the data point.
    .size(5).fill_color(blue); // Just show data point markers.

  my_plot.plot(sincos_data, "sincos(x)")
    .line_on(true)  // Just line joining points.
    .line_color(purple)
    .line_width(0.5)
    .bezier_on(true) // Note plot curve is smoother at the minima and maxima.
    .shape(none); // NO data point markers (and only shows a line in the legend).

  my_plot.write("./demo_convertible_to_double.svg"); // Final plot.

} // void trig_plots()

int main()
{
  // Plot test trig data using several floating-point fundamental or builtin or user-defined (and perhaps fixed-point) types.

  trig_plots<float>(); // OK
  trig_plots<>(); // default double OK
  trig_plots<double>(); // OK
 // trig_plots<long double>(); // not OK error C2440: 'static_cast': cannot convert from 'const _Ty2' to 'boost::quan::uncun'
  // and for higher-than-double precision types, the range from
  // (std::numeric_limits<long double>::max)() to min() is greater than for double.
  // so overflow or underflow on conversion to double is possible.

  //using boost::multiprecision::cpp_bin_float_quad;
  //trig_plots<cpp_bin_float_quad>();  // Not OK error C2440: 'static_cast': cannot convert from 'const _Ty2' to 'boost::quan::uncun'

  // As an example of a User-defined Type a fixed-point is also possible:
  ///typedef boost::fixed_point::negatable<15, -16> fixed_point_type;
  //trig_plots<fixed_point_type>(); // OK
  // But some fixed_point types might fail this example because
  // the range from max to min might not be great enough for the data to plot chosen.

  // Probably ill-advised, but works, with a warning from use of constant pi which will be 3. not 3.1459...
  //trig_plots<int>(); //  warning C4244: '=': conversion from 'double' to 'int', possible loss of data

  // Hopeless case - but does not provide a very helpful error message.
  // trig_plots<std::string>();

  return 0;
} // int main()


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#ifndef NDHIST_STATS_KURTOSIS_HPP_INCLUDED
#define NDHIST_STATS_KURTOSIS_HPP_INCLUDED 1

#include <boost/python.hpp>

#include <ndhist/ndhist.hpp>
#include <ndhist/stats/expectation.hpp>
#include <ndhist/stats/var.hpp>

namespace ndhist {
namespace stats {

namespace detail {

template <typename AxisValueType, typename WeightValueType>
double
calc_axis_kurtosis_impl(
    ndhist const & h
  , intptr_t const axis
)
{
    // Do the projection here, so it won't be done twice.
    ndhist const proj = (h.get_nd() == 1 ? h : h.project(bp::object(axis)));

    double const expact1 = calc_axis_expectation_impl<AxisValueType, WeightValueType>(proj, 1, axis);
    double const expact2 = calc_axis_expectation_impl<AxisValueType, WeightValueType>(proj, 2, axis);
    double const expact3 = calc_axis_expectation_impl<AxisValueType, WeightValueType>(proj, 3, axis);
    double const expact4 = calc_axis_expectation_impl<AxisValueType, WeightValueType>(proj, 4, axis);
    double const var = calc_axis_var_impl<AxisValueType, WeightValueType>(proj, axis);

    // Kurtosis[x] = (E[x^4] - 4 E[x] E[x^3] + 6 E[x]^2 E[x^2] - 3 E[x]^4) / V[x]^2
    return (expact4 - 4*expact1*expact3 + 6*expact1*expact1*expact2 - 3*expact1*expact1*expact1*expact1) / (var*var);
}

}// namespace detail

namespace py {

/**
 * @brief Calculates the kurtosis along the given axis of the given
 *     ndhist object. As in statistics, the kurtosis is defined as
 *     :math:`Kurtosis[x] = (E[x^4] - 4 E[x] E[x^3] + 6 E[x]^2 E[x^2] - 3 E[x]^4) / V[x]^2`.
 *     This function generates a projection along the given axis and then
 *     calculates the kurtosis.
 *     If None is given as axis, the kurtosis for all individual axes of the
 *     ndhist object will be calculated and returned as a tuple. But if the
 *     dimensionality of the ndhist object is 1, a scalar value is returned.
 *
 * @note This function is only defined for ndhist objects with POD axis values
 *     AND POD weight values.
 */
boost::python::object
kurtosis(
    ndhist const & h
  , boost::python::object const & axis = boost::python::object()
);

}// namespace py

}// namespace stats
}// namespace ndhist

#endif // NDHIST_STATS_KURTOSIS_HPP_INCLUDED


#include <iostream>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

typedef boost::numeric::ublas::matrix<float> Matrix44;
//typedef boost::numeric::ublas

int main()
{
	Matrix44 matrix = boost::numeric::ublas::identity_matrix<float> (4);

	std::cout << matrix << std::endl;

	return 0;
}


#include <metaSMT/support/default_visitation_unrolling_limit.hpp>
#include <metaSMT/frontend/QF_BV.hpp>
#include <metaSMT/DirectSolver_Context.hpp>
#include <metaSMT/GraphSolver_Context.hpp>
#include <metaSMT/backend/Boolector.hpp>
#include <metaSMT/backend/MiniSAT.hpp>
#include <metaSMT/backend/PicoSAT.hpp>
#include <metaSMT/backend/CUDD_Context.hpp>
#include <metaSMT/backend/SAT_Aiger.hpp>

#include <metaSMT/BitBlast.hpp>
#include <metaSMT/support/run_algorithm.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/format.hpp>
#include <boost/foreach.hpp>

using namespace metaSMT;
using namespace metaSMT::logic;
using namespace metaSMT::logic::QF_BV;
using namespace metaSMT::solver;
using namespace std;

#define foreach BOOST_FOREACH

template<typename Solver>
struct sudoku
{
  typedef bool result_type;

  sudoku ( string problem )
  {
    field_size = 16;
    bit_width = int(ceil ( log ( field_size ) / log ( 2 ) ));
    block_width = bit_width;

    init_field ();
    parse ( problem );
    field_constraints();
  }

  void parse ( string problem )
  {
    unsigned x = 0, y = 0;

    foreach ( char c, problem )
    {
      if ( c != '_')
      {
        assertion ( ctx,
            equal ( field[x][y], bvuint ( c - 'a', bit_width ) ) );
      }

      ++x;
      if ( x == field_size )
      {
        y++;
        x = 0;
      }
    }
  }

  bool operator() ()
  {
    std::cout << "Solving" << std::endl;
    bool sat = solve(ctx);
    if(sat) print_solution();
    return sat;
  }

  void init_field ()
  {
    field.resize ( field_size );

    foreach ( vector<bitvector>& row, field )
    {
      row.clear ();
      for ( unsigned i = 0; i < field_size; i++ )
      {
        row.push_back ( new_bitvector ( bit_width ) );
      }
    }
  }

  void field_constraints()
  {
    for ( unsigned i = 0; i < field_size; ++i )
    {
      row ( i );
      column ( i );
    }

    for ( unsigned i = 0; i < block_width; i++ )
    {
      for ( unsigned j = 0; j < block_width; j++ )
      {
        block ( i*block_width, j*block_width );
      }
    }

  }

  void row ( unsigned row )
  {
    for ( unsigned i = 0; i < field_size; i++ )
    {
      for ( unsigned j = i + 1; j < field_size; ++j )
      {
        assertion ( ctx,
            nequal ( field[row][i], field[row][j] ) );
      }
    }
  }

  void column ( unsigned col )
  {
    for ( unsigned i = 0; i < field_size; i++ )
    {
      for ( unsigned j = i + 1; j < field_size; ++j )
      {
        assertion ( ctx,
            nequal ( field[i][col], field[j][col] ) );
      }
    }
  }

  // gets the i-th element of block at pos (col, row)
  bitvector& index (unsigned row, unsigned col, unsigned i)
  {
    return field[row + i/block_width][col + i%block_width];
  }

  void block ( unsigned col, unsigned row )
  {
    for ( unsigned i = 0; i < field_size; i++ )
    {
      for ( unsigned j = i + 1; j < field_size; ++j )
      {
        assertion ( ctx,
            nequal ( index(row, col, i), index(row, col, j) ) );
      }
    }

  }

  void print_solution()
  {
    for (unsigned i = 0; i < field_size; ++i) {
      if( i % block_width == 0) printf("\n");
      for (unsigned k = 0; k < field_size; ++k) {
        if( k % block_width == 0 ) printf(" ");
        unsigned val = read_value(ctx, field[i][k]);
        printf("%c", 'a'+val);
      }
      printf("\n");
    }
    printf("\n");
  }

  Solver ctx;
  unsigned field_size;
  unsigned bit_width;
  unsigned block_width;
  vector < vector < bitvector > > field;

};

int
main(int argc, const char *argv[])
{
  typedef mpl::vector <
      DirectSolver_Context < Boolector >
    , DirectSolver_Context < BitBlast < SAT_Aiger < MiniSAT > > >
    , DirectSolver_Context < BitBlast < SAT_Aiger < PicoSAT > > >
    , DirectSolver_Context < BitBlast < CUDD_Context > >

    , GraphSolver_Context < Boolector >
    , GraphSolver_Context < BitBlast < SAT_Aiger < MiniSAT > > >
    , GraphSolver_Context < BitBlast < SAT_Aiger < PicoSAT > > >
    , GraphSolver_Context < BitBlast < CUDD_Context > >

      > SolverVec;

  if( argc < 2) {
    cout << "usage: "<< argv[0] << "  solver sudoku\nsolver:\n\t0 - Boolector (SMT)\n\t1 - MiniSAT (SAT)\n\t2 - PicoSAT (SAT)\n\t3 - CUDD (BDD)" << endl;
    exit(1);
  }

  unsigned solver = atoi ( argv[1] );

  bool val = run_algorithm<SolverVec, sudoku> ( solver, argv[2] );

  std::cout << "Sudoku valid? " << (val ? "yes" : "no") << std::endl;

  return val? 0 : 1;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 optimize toolbox - nedlermead"

#include <iostream>
#include <nt2/include/functions/neldermead.hpp>

#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/rowvect.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/oneo_10.hpp>
#include <nt2/table.hpp>
struct ffp
{
  template < class Tab > inline
  typename Tab::value_type operator()(const Tab & x ) const
  {
    typedef typename Tab::value_type value_type;
    return nt2::globalsum((nt2::sqr((nt2::rowvect(x)-nt2::_(value_type(1), value_type(numel(x)))))));
    //   NT2_DISPLAY(r)
  }
};

template<class Tab > typename Tab::value_type f1(const Tab & x )
{
    typedef typename Tab::value_type value_type;
    return nt2::globalsum((nt2::sqr((nt2::rowvect(x)-nt2::_(value_type(1), value_type(numel(x)))))));
}

// NT2_TEST_CASE_TPL( nedlermead_function_ptr, NT2_REAL_TYPES )
// {
//   using nt2::nedlermead;
//   using nt2::optimization::output;
//   typedef nt2::table<T> tab_t;
//   typedef typename nt2::meta::as_logical<T>::type lT;
//   typedef nt2::table<T> ltab_t;
//   tab_t x0 = nt2::zeros(nt2::of_size(1, 3), nt2::meta::as_<T>());
//   ltab_t h = nt2::is_nez(nt2::ones (nt2::of_size(1, 3), nt2::meta::as_<T>())*nt2::Half<T>());
//   tab_t r = nt2::ones (nt2::of_size(1, 3), nt2::meta::as_<T>());
//   output<tab_t,T> res = nedlermead(&f1<tab_t, tab_t>, x0, h);

//   std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
//             << " after " << res.iterations_count <<  " iterations\n";

//   NT2_TEST(res.successful);
//   NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::abs(res.minimum()-r)), nt2::Sqrteps<T>());
// }

NT2_TEST_CASE_TPL( nedlermead_functor, NT2_REAL_TYPES )
{
  using nt2::neldermead;
  using nt2::options;
  using nt2::optimization::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x0 = nt2::zeros(nt2::of_size(1, 2), nt2::meta::as_<T>());
  tab_t h = nt2::ones (nt2::of_size(1, 2), nt2::meta::as_<T>())*nt2::Oneo_10<T>();
  tab_t r = nt2::_(T(1), T(2));
  NT2_DISPLAY(x0);
  NT2_DISPLAY(h);
  output<tab_t,T> res = neldermead(ffp(), x0, h,
                                  options [ nt2::iterations_ = 100,
                                            nt2::tolerance::absolute_ = T(0.001)
                                    ]);

  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";

  NT2_TEST(res.successful);
  NT2_DISPLAY(res.minimum());
  NT2_DISPLAY(r);
  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::abs(res.minimum()-r)), T(0.001));

}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

 This simple example shows how to call dlib's optimal linear assignment problem solver.
 It is an implementation of the famous Hungarian algorithm and is quite fast, operating in
 O(N^3) time.

*/

#include <dlib/optimization/max_cost_assignment.h>
#include <iostream>

using namespace std;
using namespace dlib;

int main ()
{
    // Let's imagine you need to assign N people to N jobs.  Additionally, each person will make
    // your company a certain amount of money at each job, but each person has different skills
    // so they are better at some jobs and worse at others.  You would like to find the best way
    // to assign people to these jobs.  In particular, you would like to maximize the amount of
    // money the group makes as a whole.  This is an example of an assignment problem and is
    // what is solved by the max_cost_assignment() routine.
    //
    // So in this example, let's imagine we have 3 people and 3 jobs.  We represent the amount of
    // money each person will produce at each job with a cost matrix.  Each row corresponds to a
    // person and each column corresponds to a job.  So for example, below we are saying that
    // person 0 will make $1 at job 0, $2 at job 1, and $6 at job 2.
    matrix<int> cost(3,3);
    cost = 1, 2, 6,
           5, 3, 6,
           4, 5, 0;

    // To find out the best assignment of people to jobs we just need to call this function.
    std::vector<long> assignment = max_cost_assignment(cost);

    // This prints optimal assignments:  [2, 0, 1] which indicates that we should assign
    // the person from the first row of the cost matrix to job 2, the middle row person to
    // job 0, and the bottom row person to job 1.
    for (unsigned int i = 0; i < assignment.size(); i++)
        cout << assignment[i] << std::endl;

    // This prints optimal cost:  16.0
    // which is correct since our optimal assignment is 6+5+5.
    cout << "optimal cost: " << assignment_cost(cost, assignment) << endl;
}


#include <iostream>
#include <Eigen/Dense>

//Cross Product Equivalent
using Eigen::Matrix3f;
using Eigen::Vector3f;
Matrix3f crossProductEquivalent(Vector3f v)
{
  std::cout << v <<std::endl;
  std::cout << v(2) <<std::endl;
  Matrix3f c;
  c << 0, -v(2), v(1),
       v(2), 0, -v(0),
       -v(1), v(0), 0;
  std::cout << c << std::endl;
  return c;
}

int main()
{
  Vector3f v;
  v << 1,
       2,
       3;
  crossProductEquivalent(v);
}


#include "sfm/filter_view_pairs_from_orientation.h"

#include <glog/logging.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <unordered_map>
#include <unordered_set>

#include "math/rotation.h"
#include "math/util.h"
#include "rotation_estimation/rotation_estimator.h"
#include "sfm/twoview_info.h"
#include "util/hash.h"
#include "util/map_util.h"
#include "util/types.h"

namespace DAGSfM {

namespace {

bool AngularDifferenceIsAcceptable(
    const Eigen::Vector3d& orientation1, const Eigen::Vector3d& orientation2,
    const Eigen::Vector3d& relative_orientation,
    const double sq_max_relative_rotation_difference_radians) {
  const Eigen::Vector3d composed_relative_rotation =
      MultiplyRotations(orientation2, -orientation1);
  const Eigen::Vector3d loop_rotation =
      MultiplyRotations(-relative_orientation, composed_relative_rotation);
  const double sq_rotation_angular_difference_radians =
      loop_rotation.squaredNorm();
  return sq_rotation_angular_difference_radians <=
         sq_max_relative_rotation_difference_radians;
}

}  // namespace

void FilterViewPairsFromOrientation(
    const std::unordered_map<image_t, Eigen::Vector3d>& orientations,
    const double max_relative_rotation_difference_degrees,
    std::unordered_map<ImagePair, TwoViewInfo>& view_pairs,
    Database& database) {
  CHECK_GE(max_relative_rotation_difference_degrees, 0.0);

  // Precompute the squared threshold in radians.
  const double max_relative_rotation_difference_radians =
      DegToRad(max_relative_rotation_difference_degrees);
  const double sq_max_relative_rotation_difference_radians =
      max_relative_rotation_difference_radians *
      max_relative_rotation_difference_radians;

  std::unordered_set<ImagePair> view_pairs_to_remove;

  for (auto& view_pair : view_pairs) {
    const Eigen::Vector3d* orientation1 =
        FindOrNull(orientations, view_pair.first.first);
    const Eigen::Vector3d* orientation2 =
        FindOrNull(orientations, view_pair.first.second);

    // If the view pair contains a view that does not have an orientation then
    // remove it.
    if (orientation1 == nullptr || orientation2 == nullptr) {
      LOG(WARNING)
          << "View pair (" << view_pair.first.first << ", "
          << view_pair.first.second
          << ") contains a view that does not exist! Removing the view pair.";
      view_pairs_to_remove.insert(view_pair.first);
      continue;
    }

    // Remove the view pair if the relative rotation estimate is not within the
    // tolerance.
    if (!AngularDifferenceIsAcceptable(
            *orientation1, *orientation2, view_pair.second.rotation_2,
            sq_max_relative_rotation_difference_radians)) {
      view_pairs_to_remove.insert(view_pair.first);
    } else {
      // Update relative rotations.
      view_pair.second.rotation_2 = geometry::RelativeRotationFromTwoRotations(
          *orientation1, *orientation2);
    }
  }

  // Remove all the "bad" relative poses.
  for (const ImagePair view_id_pair : view_pairs_to_remove) {
    view_pairs.erase(view_id_pair);
    database.DeleteMatches(view_id_pair.first, view_id_pair.second);
    database.DeleteInlierMatches(view_id_pair.first, view_id_pair.second);
  }
  VLOG(1) << "Removed " << view_pairs_to_remove.size()
          << " view pairs by rotation filtering.";
}

}  // namespace DAGSfM


#include <iostream>
#include <Eigen/Dense>
#include "../simple_lib/include/simple_activation.h"
#include <map>
#include <string>

using namespace Eigen;

// prototype declaration
std::map<std::string, MatrixXd> init_network(void);
MatrixXd forward(std::map<std::string, MatrixXd> network, MatrixXd x);

// main function
int main(){
    using std::cout;
    using std::endl;
    using std::map;
    using std::string;

    MatrixXd X  = MatrixXd::Zero(1, 2);
    MatrixXd W1 = MatrixXd::Zero(2, 3);
    MatrixXd B1 = MatrixXd::Zero(1, 3);
    MatrixXd A1 = MatrixXd::Zero(1, 3);
    MatrixXd Z1 = MatrixXd::Zero(1, 3);
    MatrixXd W2 = MatrixXd::Zero(3, 2);
    MatrixXd B2 = MatrixXd::Zero(1, 2);
    MatrixXd A2 = MatrixXd::Zero(1, 2);
    MatrixXd Z2 = MatrixXd::Zero(1, 2);
    MatrixXd W3 = MatrixXd::Zero(2, 2);
    MatrixXd B3 = MatrixXd::Zero(1, 2);
    MatrixXd A3 = MatrixXd::Zero(1, 2);
    MatrixXd Y  = MatrixXd::Zero(1, 2);

    X << 1.0, 0.5;
    W1 << 0.1, 0.3, 0.5,  0.2, 0.4, 0.6;
    B1 << 0.1, 0.2, 0.3;

    W2 << 0.1, 0.4,  0.2, 0.5,  0.3, 0.6;
    B2 << 0.1, 0.2;

    W3 << 0.1, 0.3,  0.2, 0.4;
    B3 << 0.1, 0.2;

    A1 = X * W1 + B1;

    cout << "A1 = " << A1 << endl;

    Z1 = A1.unaryExpr([](double p){return MyDL::sigmoid<double>(p);});

    cout << "Z1 = " << Z1 << endl;

    A2 = Z1 * W2 + B2;
    Z2 = A2.unaryExpr([] (double p){return MyDL::sigmoid<double>(p);});

    cout << "A2 = " << A2 << endl;
    cout << "Z2 = " << Z2 << endl;

    A3 = Z2 * W3 + B3;
    Y = A3.unaryExpr([] (double p){return MyDL::identity_function<double>(p);});

    cout << "A3 = " << A3 << endl;
    cout << "Y = " << Y << endl;

    // call network function -> last activation is Softmax function
    map <string, MatrixXd> network;
    network = init_network();
    Y = forward(network, X);

    cout << "Function ver: Y = " << Y << endl;

    return 0;
}

// Implementation
std::map<std::string, MatrixXd> init_network(void)
{

    using std::map;
    using std::string;

    map<string, MatrixXd> network;

    MatrixXd W1 = MatrixXd::Zero(2, 3);
    MatrixXd b1 = MatrixXd::Zero(1, 3);
    MatrixXd W2 = MatrixXd::Zero(3, 2);
    MatrixXd b2 = MatrixXd::Zero(1, 2);
    MatrixXd W3 = MatrixXd::Zero(2, 2);
    MatrixXd b3 = MatrixXd::Zero(1, 2);

    W1 << 0.1, 0.3, 0.5, 0.2, 0.4, 0.6;
    b1 << 0.1, 0.2, 0.3;
    W2 << 0.1, 0.4, 0.2, 0.5, 0.3, 0.6;
    b2 << 0.1, 0.2;
    W3 << 0.1, 0.3, 0.2, 0.4;
    b3 << 0.1, 0.2;

    network["W1"] = W1;
    network["b1"] = b1;
    network["W2"] = W2;
    network["b2"] = b2;
    network["W3"] = W3;
    network["b3"] = b3;

    return network;
}

MatrixXd forward(std::map<std::string, MatrixXd> network, MatrixXd x)
{
    using std::map;
    using std::string;

    MatrixXd W1 = MatrixXd::Zero(2, 3);
    MatrixXd b1 = MatrixXd::Zero(1, 3);
    MatrixXd W2 = MatrixXd::Zero(3, 2);
    MatrixXd b2 = MatrixXd::Zero(1, 2);
    MatrixXd W3 = MatrixXd::Zero(2, 2);
    MatrixXd b3 = MatrixXd::Zero(1, 2);

    MatrixXd a1 = MatrixXd::Zero(1, 3);
    MatrixXd z1 = MatrixXd::Zero(1, 3);
    MatrixXd a2 = MatrixXd::Zero(1, 2);
    MatrixXd z2 = MatrixXd::Zero(1, 2);
    MatrixXd a3 = MatrixXd::Zero(1, 2);
    MatrixXd y = MatrixXd::Zero(1, 2);

    W1 = network["W1"];
    b1 = network["b1"];
    W2 = network["W2"];
    b2 = network["b2"];
    W3 = network["W3"];
    b3 = network["b3"];

    a1 = x * W1 + b1;
    z1 = a1.unaryExpr([] (double p){return MyDL::sigmoid<double>(p);});
    a2 = z1 * W2 + b2;
    z2 = a2.unaryExpr([] (double p){return MyDL::sigmoid<double>(p);});
    a3 = z2 * W3 + b3;
    y  = MyDL::softmax(a3);

    return y;
}


#pragma once

#include <Eigen/Dense>

/// class DigitalFilter
class DigitalFilter {
public:
  /// \{ \name Constructor and Destructor
  DigitalFilter();

  virtual ~DigitalFilter();
  /// \}

  /// Input to this filter
  virtual void Input(double input_value) = 0;

  /// Output of this filter
  virtual double Output() = 0;

  /// Reset data history in this filter
  virtual void Clear() = 0;
};

/// class ButterWorthFilter
class ButterWorthFilter : public DigitalFilter {
public:
  ButterWorthFilter(int num_sample, double dt, double cutoff_frequency);
  virtual ~ButterWorthFilter();
  virtual void Input(double input_value);
  virtual double Output();
  virtual void Clear();

private:
  double *mpBuffer;
  int mCurIdx;
  int mNumSample;
  double mDt;
  double mCutoffFreq;
  double mValue;
};

/// class LowPassFilter
class LowPassFilter : public DigitalFilter {
public:
  LowPassFilter(double w_c, double t_s);
  virtual ~LowPassFilter();
  virtual void Input(double input_value);
  virtual double Output();
  virtual void Clear();

private:
  double Lpf_in_prev[2];
  double Lpf_out_prev[2];
  double Lpf_in1, Lpf_in2, Lpf_in3, Lpf_out1, Lpf_out2;
  double lpf_out;
};

/// class SimpleMovingAverage
class SimpleMovingAverage : public DigitalFilter {
public:
  SimpleMovingAverage(int num_data);
  virtual ~SimpleMovingAverage();
  virtual void Input(double input_value);
  virtual double Output();
  virtual void Clear();

private:
  Eigen::VectorXd buffer_;
  int num_data_;
  int idx_;
  double sum_;
};

/// class DerivativeLowPassFilter
class DerivativeLowPassFilter : public DigitalFilter {
public:
  DerivativeLowPassFilter(double w_c, double t_s);
  virtual ~DerivativeLowPassFilter();
  virtual void Input(double input_value);
  virtual double Output();
  virtual void Clear();

private:
  double Lpf_in_prev[2];
  double Lpf_out_prev[2];
  double Lpf_in1, Lpf_in2, Lpf_in3, Lpf_out1, Lpf_out2;
  double lpf_out;
};


// Copyright John Maddock 2006.
// 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)
#include <boost/math/tools/test_data.hpp>
#include <boost/test/included/prg_exec_monitor.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/jacobi_zeta.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>
#include "mp_t.hpp"

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;

mp_t heuman_lambda(mp_t phi, mp_t k)
{
   mp_t kp = sqrt(1 - k *k);
   if((k * k < tools::epsilon<float>()) && (fabs(phi) >= constants::half_pi<mp_t>()))
      throw std::domain_error("");
   return ellint_1(kp, phi) / ellint_1(kp) + ellint_1(k) * jacobi_zeta(kp, phi) / constants::half_pi<mp_t>();
}

int cpp_main(int argc, char*argv [])
{
   using namespace boost::math::tools;

   parameter_info<mp_t> arg1, arg2;
   test_data<mp_t> data;

   bool cont;
   std::string line;

   if(argc < 1)
      return 1;

   do{
      if(0 == get_user_parameter_info(arg1, "phi"))
         return 1;
      if(0 == get_user_parameter_info(arg2, "k"))
         return 1;

      mp_t(*fp)(mp_t, mp_t) = &heuman_lambda;
      data.insert(fp, arg1, arg2);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=heuman_lambda_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "heuman_lambda_data.ipp";
   std::ofstream ofs(line.c_str());
   line.erase(line.find('.'));
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, line.c_str());

   return 0;
}


// Copyright Louis Dionne 2013-2017
// 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/functional/fix.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/eval_if.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/mult.hpp>
namespace hana = boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto fact = hana::fix([](auto fact, auto n) {
    return hana::eval_if(hana::equal(n, hana::ullong_c<0>),
        hana::always(hana::ullong_c<1>),
        [=](auto _) { return hana::mult(n, fact(_(n) - hana::ullong_c<1>)); }
    );
});

constexpr unsigned long long reference(unsigned long long n)
{ return n == 0 ? 1 : n * reference(n - 1); }

template <int n>
void test() {
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        fact(hana::ullong_c<n>),
        hana::ullong_c<reference(n)>
    ));
    test<n - 1>();
}

template <> void test<-1>() { }

int main() {
    test<15>();
}


#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <vector>
#include <chrono>

#include <getopt.h>

#include "Alpert_Transform.hpp"
#include "Utils.hpp"

#include <boost/numeric/bindings/blas/blas.h>

using namespace std;

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

  if(argc == 1 ){
    cout << argv[0] << " usage is:" << endl;
    cout << argv[0] << " --reduction 'reduction_value' [mesh input_file] [data input_file]" << endl;
    cout << "If the data input file is provided it should have the same number of points as the mesh, if not a default data field will be assigned" << endl;
    cout << "Please refer to README.txt in this folder for more info" << endl;
    exit (0);
  }

    chrono::duration <double> duration;
    chrono::system_clock::time_point t1, t2;

    int d, N;
    double R=10.0e0;
    int c;

    {
          static struct option long_options[] =
        {
          /* These options set a flag. */
          /* These options don’t set a flag.
             We distinguish them by their indices. */
          {"reduction",  required_argument, 0, 'r'},
          {0, 0, 0}
        };
      /* getopt_long stores the option index here. */
      int option_index = 0;

      while (c = getopt_long (argc, argv, "r",
			      long_options, &option_index) != -1){

	if(option_index == 0 && optarg){
	  R  = atof(optarg);
	  cout << "Reduction set to " << R << endl;
	}
      }

    }

    if(optind >= argc ) {
      cout << "Expected an input file name " << endl;
      exit(0);
    }

    cout << "Reading mesh input file" << argv[optind] << endl;
    ifstream f(argv[optind]);
    f >> N >> d;

    cout << "Number of Points " << N << endl;
    cout << "Number of dimensions " << d << endl;
    cout << "R " << R << endl;

    int M=ceil(double(N)/R);
    int NR=N-M;
   // meshpoints
    vector<vector<double> >meshPoints(N, vector<double>(d));

    // Read mesh into array
    for (int i = 0; i < N; i++)
        for (int j = 0; j < d; j++)
            f >> meshPoints[i][j];

    f.close();

    // Setting Wavelet orders
    vector<int> ki(d);
    for (int j = 0; j < d; j++)
      ki[j] = 5;

    boost::numeric::ublas::vector<double> x(N);
    boost::numeric::ublas::vector<double> w(N);
    boost::numeric::ublas::vector<double> xw(N);
    boost::numeric::ublas::vector<double> ys(N);

    // Setting data, this step can be replaced by some commands to read an external data file
    double pi=4.0e0*atan(1.0e0);
    if (argc>4) {
      cout << "Reading data input file" << argv[optind+1] << endl;
      f.open(argv[optind+1]);
      for (int i = 0; i < N; i++)
	f >> x(i);
    }
    else
      for (int i = 0; i < N; i++) x(i)=(4.0e0*sin(8.0e0*pi*meshPoints[i][0]))*(4.0e0*sin(7.0e0*pi*meshPoints[i][1]) )*3.0e0*sin(6.0e0*pi*meshPoints[i][0]);
//     for (int i = 0; i < N; i++) x[i]=(4.0*sin(2.0*pi*p[i][0]) - 4.0*sin(2.0*pi*p[i][1]))*3.0*sin(2.0*pi*p[i][0]);

    int J=-1;
    std::list<std::list<boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> > > Uj;
    std::list<std::vector<int> > part;
    std::vector<int> G;
    //void Compute_Uj(std::vector<std::vector<double> >meshPoints, std::vector<int> ki, int &J, std::list<std::list<boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> > > &Uj, std::list<std::vector<int> > &part, std::vector<int> &G)

    // Forward wavelet transform
    t1 = chrono::system_clock::now();
    Compute_Uj(meshPoints,ki,J,Uj, part, G);
    Perform_Alpert_transform(x, ki, J,1, w, Uj, part, G);

    // Sorting, wavelet compression and inverse wavelet transform
    vector<double> yv(N);
    vector<size_t> iyv(N);
    for (int i = 0; i < N; i++) {yv[i]=fabs(w(i));iyv[i]=i;}
    sortandindices(yv, iyv);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total wavelet compression time " << duration.count() << endl;

//     for (int i = 0; i < N; i++)
//       std::cout << w(i) << std::endl;

    t1 = chrono::system_clock::now();
    for (int i = NR; i < N; i++) ys(iyv[i])=w(iyv[i]);

    // xw is the reconstruction by wavelets
    Perform_Alpert_transform(ys,  ki, J,-1, xw, Uj, part, G);

    // Computing NRMSE
    double ew=0.0e0;
    for (int i = 0; i < N; i++)
      ew+=pow(x(i)-xw(i),2.0e0);

    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total wavelet decompression time " << duration.count() << endl;

    // Computing min and max values in the data x
    double mx=x(0);
    double mn=x(0);
    for (int i = 0; i < N; i++) {
      if (x(i)>mx)
	mx=x(i);
      if (x(i)<mn)
	mn=x(i);
    }

    ew=sqrt(ew/double(N))/(mx-mn);

    cout << "Alpert Wavelets Compression NRMSE = " << ew << endl;
}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 * All rights reserved.
 *
 * See LICENSE file in the root of the mrob library.
 *
 *
 * create_points.hpp
 *
 *  Created on: Feb 6, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#ifndef CREATE_POINTS_HPP_
#define CREATE_POINTS_HPP_

#include "mrob/SE3.hpp"
#include "mrob/plane.hpp"
#include "mrob/plane_registration.hpp"
#include <random>
#include <memory>
#include <utility>
#include <Eigen/StdVector> // for fixed size SE3 objects

using namespace Eigen;

namespace mrob{

/**
 * Class samples a random configuration on SE3 using a
 * uniform distribution U(-R_range, R_range)
 * or any implemented distribution
 */
class SampleUniformSE3{
  public:
    SampleUniformSE3(double R_range, double t_range);
    SampleUniformSE3(double R_min, double R_max, double t_min, double t_max);
    ~SampleUniformSE3();
    SE3 samplePose();
    Mat31 samplePosition();
    SO3 sampleOrientation();
  protected:
    std::default_random_engine generator_;
    std::uniform_real_distribution<double> rotationUniform_;
    std::uniform_real_distribution<double> tUniform_;
};

/**
 * Class samples a point on a surface over the plane XY
 * according to a fixed noise on height
 */
class SamplePlanarSurface{
  public:
    SamplePlanarSurface(double zStd, double bias = 0.0);
    ~SamplePlanarSurface();
    /**
     * samples a point with noise and biases as specified in class
     */
    Mat31 samplePoint();
    void sampleBias();

  protected:
    std::default_random_engine generator_;
    std::uniform_real_distribution<double> x_, y_;
    std::normal_distribution<double> z_, bias_;
    double xBias_, yBias_;
};

/**
 * Class generating a sequence of pointClouds given some specifications.
 */
class CreatePoints{
public:
    /**
     * Creates a class
     */
    CreatePoints(uint_t N = 10, uint_t numberPlanes = 4, uint_t numberPoses = 2, double noisePerPoint = 0.01, double noiseBias = 0.1);
    ~CreatePoints();

    /**
     * Fill the PlaneRegistration class with planes calculated here (reset)
     * and a new initial trajectory set to I's
     */
    void create_plane_registration(PlaneRegistration& planeReg);

    std::vector<Mat31>& get_point_cloud(uint_t t);
    std::vector<uint_t>& get_point_plane_ids(uint_t t);

    uint_t get_number_planes() const {return numberPlanes_;};
    uint_t get_number_poses() const {return numberPoses_;};

    std::vector<SE3>& get_ground_truth_trajectory() {return goundTruthTrajectory_;};

    std::vector<SE3>& get_plane_poses() {return planePoses_;};
    std::vector<std::pair<uint_t, std::shared_ptr<Plane> >>& get_all_planes() {return planes_;}

    void print() const;

protected:
    // generation parameters
    uint_t numberPoints_; // Number of points
    uint_t numberPlanes_; // Number of planes in the virtual environment
    double noisePerPoint_, noiseBias_;
    double rotationRange_;
    double transRange_;
    double lamdaOutlier_;
    SampleUniformSE3 samplePoses_,samplePlanes_;
    SamplePlanarSurface samplePoints_;

    // Point cloud data generated
    std::vector< std::vector<Mat31> > X_;
    // IDs for facilitating the task of DA and normal computation
    std::vector< std::vector<uint_t>> pointId_;

    // Trajectory parameters
    double xRange_, yRange_; // dimension of the workspace
    SE3 initialPose_, finalPose_;
    std::vector<SE3> goundTruthTrajectory_;// ground truth trajectory
    uint_t numberPoses_;

    // Generation of planes
    std::vector<SE3> planePoses_;
    std::vector<std::pair<uint_t, std::shared_ptr<Plane> >> planes_;

};

}
#endif /* CREATE_POINTS_HPP_ */


#include <iostream>
#include <Eigen/Core>

using namespace std;

int main()
{
    Eigen::Matrix<float,7,9>mat0;
    mat0=Eigen::MatrixXf::Random(7,9);
    cout<<"mat0:"<<endl<<mat0<<endl;

    Eigen::Matrix<float,3,3>mat1=mat0.block(0,0,3,3);
    cout<<"mat1:"<<endl<<mat1<<endl;

    mat1=Eigen::MatrixXf::Identity(3,3);
    cout<<"mat1:"<<endl<<mat1<<endl;
    return 0;
}


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

// Standard includes
#include <fstream>

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

// Local VOTCA includes
#include "votca/xtp/quadrature_factory.h"

// VOTCA includes
#include <votca/tools/eigenio_matrixmarket.h>

using namespace votca::xtp;
using namespace votca;

// defines a Gaussian as integration test
// result should by ~sqrt(pi)
class FunctionEvaluation {
 public:
  FunctionEvaluation(){};

  double operator()(Index j, double point, bool symmetry) const {
    double factor = 0.0;
    // this is only here to staisfy the unused variable warning
    if (j < 100) {
      if (symmetry) {
        factor = 2.0;
      } else {
        factor = 1.0;
      }
    }

    return factor * exp(-std::pow(point, 2));
  }
};

BOOST_AUTO_TEST_SUITE(gaussian_quadratures_test)

BOOST_AUTO_TEST_CASE(gauss_legendre) {

  QuadratureFactory::RegisterAll();
  std::unique_ptr<GaussianQuadratureBase> gq_ =
      Quadratures().Create("legendre");

  std::vector<int> orders{8, 10, 12, 14, 16, 18, 20, 40, 100};
  FunctionEvaluation f = FunctionEvaluation();

  Eigen::VectorXd integrals(9);
  for (Index i = 0; i < 9; i++) {
    gq_->configure(orders[i]);
    integrals(i) = gq_->Integrate(f);
  }

  Eigen::VectorXd integrals_ref =
      votca::tools::EigenIO_MatrixMarket::ReadVector(
          std::string(XTP_TEST_DATA_FOLDER) +
          "/gaussian_quadratures/gauss_legendre.mm");

  bool check_integral = integrals.isApprox(integrals_ref, 1e-10);
  if (!check_integral) {
    std::cout << "Gauss-Legendre" << std::endl;
    std::cout << integrals << std::endl;
    std::cout << "Gauss-Legendre ref" << std::endl;
    std::cout << integrals_ref << std::endl;
  }
  BOOST_CHECK_EQUAL(check_integral, true);
}

BOOST_AUTO_TEST_CASE(modified_gauss_legendre) {

  QuadratureFactory::RegisterAll();
  std::unique_ptr<GaussianQuadratureBase> gq_ =
      std::unique_ptr<GaussianQuadratureBase>(
          Quadratures().Create("modified_legendre"));

  std::vector<int> orders{8, 10, 12, 14, 16, 18, 20, 40, 100};
  FunctionEvaluation f = FunctionEvaluation();

  Eigen::VectorXd integrals(9);
  for (Index i = 0; i < 9; i++) {
    gq_->configure(orders[i]);
    integrals(i) = gq_->Integrate(f);
  }

  Eigen::VectorXd integrals_ref =
      votca::tools::EigenIO_MatrixMarket::ReadVector(
          std::string(XTP_TEST_DATA_FOLDER) +
          "/gaussian_quadratures/modified_gauss_legendre.mm");

  bool check_integral = integrals.isApprox(integrals_ref, 1e-10);
  if (!check_integral) {
    std::cout << "modified Gauss-Legendre" << std::endl;
    std::cout << integrals << std::endl;
    std::cout << "modified Gauss-Legendre ref" << std::endl;
    std::cout << integrals_ref << std::endl;
  }
  BOOST_CHECK_EQUAL(check_integral, true);
}

BOOST_AUTO_TEST_CASE(gauss_laguerre) {

  QuadratureFactory::RegisterAll();
  std::unique_ptr<GaussianQuadratureBase> gq_ =
      std::unique_ptr<GaussianQuadratureBase>(Quadratures().Create("laguerre"));
  std::vector<int> orders{8, 10, 12, 14, 16, 18, 20, 40, 100};
  FunctionEvaluation f = FunctionEvaluation();

  Eigen::VectorXd integrals(9);
  for (Index i = 0; i < 9; i++) {
    gq_->configure(orders[i]);
    integrals(i) = gq_->Integrate(f);
  }

  Eigen::VectorXd integrals_ref =
      votca::tools::EigenIO_MatrixMarket::ReadVector(
          std::string(XTP_TEST_DATA_FOLDER) +
          "/gaussian_quadratures/gauss_laguerre.mm");

  bool check_integral = integrals.isApprox(integrals_ref, 1e-10);
  if (!check_integral) {
    std::cout << "Gauss-Laguerre" << std::endl;
    std::cout << integrals << std::endl;
    std::cout << "Gauss-Laguerre ref" << std::endl;
    std::cout << integrals_ref << std::endl;
  }
  BOOST_CHECK_EQUAL(check_integral, true);
}

BOOST_AUTO_TEST_CASE(gauss_hermite) {

  QuadratureFactory::RegisterAll();
  std::unique_ptr<GaussianQuadratureBase> gq_ =
      std::unique_ptr<GaussianQuadratureBase>(Quadratures().Create("hermite"));
  std::vector<int> orders{8, 10, 12, 14, 16, 18, 20, 40, 100};
  FunctionEvaluation f = FunctionEvaluation();

  Eigen::VectorXd integrals(9);
  for (Index i = 0; i < 9; i++) {
    gq_->configure(orders[i]);
    integrals(i) = gq_->Integrate(f);
  }

  Eigen::VectorXd integrals_ref =
      votca::tools::EigenIO_MatrixMarket::ReadVector(
          std::string(XTP_TEST_DATA_FOLDER) +
          "/gaussian_quadratures/gauss_hermite.mm");

  bool check_integral = integrals.isApprox(integrals_ref, 1e-10);
  if (!check_integral) {
    std::cout << "Gauss-Hermite" << std::endl;
    std::cout << integrals << std::endl;
    std::cout << "Gauss-Hermite ref" << std::endl;
    std::cout << integrals_ref << std::endl;
  }
  BOOST_CHECK_EQUAL(check_integral, true);
}

BOOST_AUTO_TEST_SUITE_END()


#include <Rcpp.h>
#include <Eigen/Eigenvalues>

using namespace Rcpp;
using namespace Eigen;

// This is a simple example of exporting a C++ function to R. You can
// source this function into an R session using the Rcpp::sourceCpp
// function (or via the Source button on the editor toolbar). Learn
// more about Rcpp at:
//
//   http://www.rcpp.org/
//   http://adv-r.had.co.nz/Rcpp.html
//   http://gallery.rcpp.org/
//

// [[Rcpp::export]]
Eigen::MatrixXd rcppeigen_sqrt(const Eigen::Map<Eigen::MatrixXd> & A){
  SelfAdjointEigenSolver<MatrixXd> es(A);
  MatrixXd sqrtA = es.operatorSqrt();
  return sqrtA;
}

// [[Rcpp::export]]
Eigen::MatrixXd rcppeigen_invsqrt(const Eigen::Map<Eigen::MatrixXd> & A){
  SelfAdjointEigenSolver<MatrixXd> es(A);
  MatrixXd invsqrtA = es.operatorInverseSqrt();
  return invsqrtA;
}

// [[Rcpp::export]]
NumericVector timesTwo(NumericVector x) {
  return x * 2;
}

// You can include R code blocks in C++ files processed with sourceCpp
// (useful for testing and development). The R code will be automatically
// run after the compilation.
//

/*** R
timesTwo(42)
rcppeigen_sqrt(diag(c(2, 2)))
*/


#include "../geometry.hpp"
#include "../static_graph.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>

#include <algorithm>

namespace boost {
    namespace test_tools {
        template<>
        struct print_log_value<coordinate>
        {
            void operator()(std::ostream& os,const coordinate& coord)
            {
                ::operator<<(os, coord);
            }
        };
    }
}

namespace geometry {
inline std::ostream& operator<<(std::ostream& lhs, const geometry::point_position& rhs)
{
    switch (rhs)
    {
    case geometry::point_position::LEFT_OF_LINE:
        lhs << "LEFT_OF_LINE";
        break;
    case geometry::point_position::RIGHT_OF_LINE:
        lhs << "RIGHT_OF_LINE";
        break;
    case geometry::point_position::ON_LINE:
        lhs << "ON_LINE";
        break;
    }
    return lhs;
}
}

BOOST_AUTO_TEST_SUITE(geometry_tests)

BOOST_AUTO_TEST_CASE(cross_product_test)
{
    BOOST_CHECK_EQUAL(geometry::cross(coordinate {0, 0}, coordinate {1, 1}), 0);
    BOOST_CHECK_EQUAL(geometry::cross(coordinate {1, 0}, coordinate {1, 0}), 0);
    BOOST_CHECK_EQUAL(geometry::cross(coordinate {-1, 0}, coordinate {1, 0}), 0);
    BOOST_CHECK_EQUAL(geometry::cross(coordinate {-1, 0}, coordinate {-1, 0}), 0);
}

BOOST_AUTO_TEST_CASE(segments_intersect)
{
    BOOST_CHECK(geometry::segments_intersect(coordinate {0, 0}, coordinate {1, 1}, coordinate {0, 0.5}, coordinate {1, 0.5}));
    BOOST_CHECK(!geometry::segments_intersect(coordinate {0, 0}, coordinate {-1, -1}, coordinate {0, 0.5}, coordinate {1, 0.5}));
}

BOOST_AUTO_TEST_CASE(segment_intersection)
{
    // 0    1     2     3
    // 4    5     6     7
    // 8    9     10    11
    std::vector<coordinate> coords {
        coordinate {0, 1}, coordinate {1, 1}, coordinate {2, 1}, coordinate {3, 1},
        coordinate {0, 0}, coordinate {1, 0}, coordinate {2, 0}, coordinate {3, 0},
        coordinate {0, -1}, coordinate {1, -1}, coordinate {2, -1}, coordinate {3, -1},
    };

    {
        auto params = geometry::segment_intersection(coords[4], coords[7], coords[0], coords[9]);
        BOOST_CHECK_EQUAL(params.first_param, 1/6.0);
        BOOST_CHECK_EQUAL(params.second_param, 0.5);
    }

    {
        auto params = geometry::segment_intersection(coords[4], coords[7], coords[9], coords[0]);
        BOOST_CHECK_EQUAL(params.first_param, 1/6.0);
        BOOST_CHECK_EQUAL(params.second_param, 0.5);
    }

    {
        auto params = geometry::segment_intersection(coords[4], coords[5], coords[2], coords[11]);
        BOOST_CHECK_EQUAL(params.first_param, 2.5);
        BOOST_CHECK_EQUAL(params.second_param, 0.5);
    }

    {
        auto params = geometry::segment_intersection(coords[2], coords[9], coords[3], coords[10]);
        BOOST_CHECK(params.colinear);
    }

    {
        auto params = geometry::segment_intersection(coords[1], coords[10], coords[2], coords[9]);
        BOOST_CHECK_EQUAL(params.first_param, 0.5);
        BOOST_CHECK_EQUAL(params.second_param, 0.5);
    }
}

BOOST_AUTO_TEST_CASE(intersection_regression)
{
    auto params = geometry::segment_intersection(coordinate {0, 0}, coordinate {2, 2}, coordinate {1, 1}, coordinate {2, 1});
    BOOST_CHECK_EQUAL(params.first_param, 0.5);
    BOOST_CHECK_EQUAL(params.second_param, 0);
}

BOOST_AUTO_TEST_CASE(normal_test)
{
    BOOST_CHECK_EQUAL(geometry::line_normal(coordinate {0, 0}, coordinate {1, 1}), (coordinate{-1, 1}));
    BOOST_CHECK_EQUAL(geometry::line_normal(coordinate {0, 0}, coordinate {-1, -1}), (coordinate{1, -1}));
    BOOST_CHECK_EQUAL(geometry::line_normal(coordinate {-1, -1}, coordinate {-2, -2}), (coordinate{1, -1}));
    BOOST_CHECK_EQUAL(geometry::line_normal(coordinate {1, 1}, coordinate {3, 0.5}), (coordinate{0.5, 2}));
}

BOOST_AUTO_TEST_CASE(position_to_line_test)
{
    BOOST_CHECK_EQUAL(geometry::position_to_line(coordinate {1, 1}, coordinate {3, 0.5}, coordinate {2, 0}), geometry::point_position::RIGHT_OF_LINE);
    BOOST_CHECK_EQUAL(geometry::position_to_line(coordinate {0, 0}, coordinate {1, 1}, coordinate {0.5, 1}), geometry::point_position::LEFT_OF_LINE);
    BOOST_CHECK_EQUAL(geometry::position_to_line(coordinate {0, 0}, coordinate {1, 1}, coordinate {0.5, -1}), geometry::point_position::RIGHT_OF_LINE);
    BOOST_CHECK_EQUAL(geometry::position_to_line(coordinate {0, 0}, coordinate {1, 1}, coordinate {2, 2}), geometry::point_position::ON_LINE);
    BOOST_CHECK_EQUAL(geometry::position_to_line(coordinate {0, 0}, coordinate {-1, -1}, coordinate {-0.5, -1}), geometry::point_position::LEFT_OF_LINE);
    BOOST_CHECK_EQUAL(geometry::position_to_line(coordinate {0, 0}, coordinate {-1, -1}, coordinate {-0.5, 1}), geometry::point_position::RIGHT_OF_LINE);
    BOOST_CHECK_EQUAL(geometry::position_to_line(coordinate {0, 0}, coordinate {-1, -1}, coordinate {-2, -2}), geometry::point_position::ON_LINE);
}

BOOST_AUTO_TEST_CASE(normalized_angle)
{
    const float epsilon = 0.001f;
    BOOST_CHECK(std::abs(geometry::normalize_angle(-M_PI) - M_PI) < epsilon);
    BOOST_CHECK(std::abs(geometry::normalize_angle(M_PI) - M_PI) < epsilon);
}

BOOST_AUTO_TEST_CASE(angle_diff_test)
{
    const float epsilon = 0.001f;
    BOOST_CHECK(geometry::angle_diff(-M_PI, M_PI) < epsilon);
    BOOST_CHECK(geometry::angle_diff(M_PI, -M_PI) < epsilon);
    BOOST_CHECK(geometry::angle_diff(-M_PI_2, 3*M_PI_2) < epsilon);
    BOOST_CHECK(geometry::angle_diff(3*M_PI_2, -M_PI_2) < epsilon);
    BOOST_CHECK(std::abs(geometry::angle_diff(M_PI/4, -M_PI/4) - M_PI_2) < epsilon);
    BOOST_CHECK(std::abs(geometry::angle_diff(-M_PI/4, M_PI/4) - M_PI_2) < epsilon);
}

BOOST_AUTO_TEST_CASE(slope_compare_test)
{
    // 0
    //    1
    // x    2
    //    3
    // 4
    std::vector<coordinate> coordinates {
        coordinate {0, 1},
        coordinate {0.5, 0.5},
        coordinate {1.0, 0.0},
        coordinate {0.5, -0.5},
        coordinate {0, -1},
    };

    coordinate origin {0, 0};

    for (auto i = 0u; i < coordinates.size(); ++i)
    {
        for (auto j = i+1; j < coordinates.size(); ++j)
        {
            BOOST_CHECK(geometry::slope_compare(origin, coordinates[i], coordinates[j]));
        }
    }

}

BOOST_AUTO_TEST_SUITE_END()