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

#ifndef BOOST_MATH_INTERPOLATORS_DETAIL_CARDINAL_TRIGONOMETRIC_HPP
#define BOOST_MATH_INTERPOLATORS_DETAIL_CARDINAL_TRIGONOMETRIC_HPP
#include <cmath>
#include <stdexcept>
#include <fftw3.h>
#include <boost/math/constants/constants.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <quadmath.h>
#endif

namespace boost { namespace math { namespace interpolators { namespace detail {

template<typename Real>
class cardinal_trigonometric_detail {
public:
  cardinal_trigonometric_detail(const Real* data, size_t length, Real t0, Real h)
  {
    m_data = data;
    m_length = length;
    m_t0 = t0;
    m_h = h;
    throw std::domain_error("Not implemented.");
  }
private:
  size_t m_length;
  Real m_t0;
  Real m_h;
  Real* m_data;
};

template<>
class cardinal_trigonometric_detail<float> {
public:
  cardinal_trigonometric_detail(const float* data, size_t length, float t0, float h) : m_t0{t0}, m_h{h}
  {
    if (length == 0)
    {
      throw std::logic_error("At least one sample is required.");
    }
    if (h <= 0)
    {
      throw std::logic_error("The step size must be > 0");
    }
    // The period sadly must be stored, since the complex vector has length that cannot be used to recover the period:
    m_T = m_h*length;
    m_complex_vector_size = length/2 + 1;
    m_gamma = fftwf_alloc_complex(m_complex_vector_size);
    // The const_cast is legitimate: FFTW does not change the data as long as FFTW_ESTIMATE is provided.
    fftwf_plan plan = fftwf_plan_dft_r2c_1d(length, const_cast<float*>(data), m_gamma, FFTW_ESTIMATE);
    // FFTW says a null plan is impossible with the basic interface we are using, and I have no reason to doubt them.
    // But it just feels weird not to check this:
    if (!plan)
    {
      throw std::logic_error("A null fftw plan was created.");
    }

    fftwf_execute(plan);
    fftwf_destroy_plan(plan);

    float denom = length;
    for (size_t k = 0; k < m_complex_vector_size; ++k)
    {
      m_gamma[k][0] /= denom;
      m_gamma[k][1] /= denom;
    }

    if (length % 2 == 0)
    {
      m_gamma[m_complex_vector_size -1][0] /= 2;
      // numerically, m_gamma[m_complex_vector_size -1][1] should be zero . . .
      // I believe, but need to check, that FFTW guarantees that it is identically zero.
    }
  }

  cardinal_trigonometric_detail(const cardinal_trigonometric_detail& old)  = delete;

  cardinal_trigonometric_detail& operator=(const cardinal_trigonometric_detail&) = delete;

  cardinal_trigonometric_detail(cardinal_trigonometric_detail &&) = delete;

  float operator()(float t) const
  {
    using std::sin;
    using std::cos;
    using boost::math::constants::two_pi;
    using std::exp;
    float s = m_gamma[0][0];
    float x = two_pi<float>()*(t - m_t0)/m_T;
    fftwf_complex z;
    // boost::math::cos_pi with a redefinition of x? Not now . . .
    z[0] = cos(x);
    z[1] = sin(x);
    fftwf_complex b{0, 0};
    // u = b*z
    fftwf_complex u;
    for (size_t k = m_complex_vector_size - 1; k >= 1; --k) {
      u[0] = b[0]*z[0] - b[1]*z[1];
      u[1] = b[0]*z[1] + b[1]*z[0];
      b[0] = m_gamma[k][0] + u[0];
      b[1] = m_gamma[k][1] + u[1];
    }

    s += 2*(b[0]*z[0] - b[1]*z[1]);
    return s;
  }

  float prime(float t) const
  {
      using std::sin;
      using std::cos;
      using boost::math::constants::two_pi;
      using std::exp;
      float x = two_pi<float>()*(t - m_t0)/m_T;
      fftwf_complex z;
      z[0] = cos(x);
      z[1] = sin(x);
      fftwf_complex b{0, 0};
      // u = b*z
      fftwf_complex u;
      for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
      {
        u[0] = b[0]*z[0] - b[1]*z[1];
        u[1] = b[0]*z[1] + b[1]*z[0];
        b[0] = k*m_gamma[k][0] + u[0];
        b[1] = k*m_gamma[k][1] + u[1];
      }
      // b*z = (b[0]*z[0] - b[1]*z[1]) + i(b[1]*z[0] + b[0]*z[1])
      return -2*two_pi<float>()*(b[1]*z[0] + b[0]*z[1])/m_T;
  }

  float double_prime(float t) const
  {
      using std::sin;
      using std::cos;
      using boost::math::constants::two_pi;
      using std::exp;
      float x = two_pi<float>()*(t - m_t0)/m_T;
      fftwf_complex z;
      z[0] = cos(x);
      z[1] = sin(x);
      fftwf_complex b{0, 0};
      // u = b*z
      fftwf_complex u;
      for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
      {
        u[0] = b[0]*z[0] - b[1]*z[1];
        u[1] = b[0]*z[1] + b[1]*z[0];
        b[0] = k*k*m_gamma[k][0] + u[0];
        b[1] = k*k*m_gamma[k][1] + u[1];
      }
      // b*z = (b[0]*z[0] - b[1]*z[1]) + i(b[1]*z[0] + b[0]*z[1])
      return -2*two_pi<float>()*two_pi<float>()*(b[0]*z[0] - b[1]*z[1])/(m_T*m_T);
  }

  float period() const
  {
    return m_T;
  }

  float integrate() const
  {
    return m_T*m_gamma[0][0];
  }

  float squared_l2() const
  {
    float s = 0;
    // Always add smallest to largest for accuracy.
    for (size_t i = m_complex_vector_size - 1; i >= 1; --i)
    {
        s += (m_gamma[i][0]*m_gamma[i][0] + m_gamma[i][1]*m_gamma[i][1]);
    }
    s *= 2;
    s += m_gamma[0][0]*m_gamma[0][0];
    return s*m_T;
  }

  ~cardinal_trigonometric_detail()
  {
    if (m_gamma)
    {
      fftwf_free(m_gamma);
      m_gamma = nullptr;
    }
  }

private:
  float m_t0;
  float m_h;
  float m_T;
  fftwf_complex* m_gamma;
  size_t m_complex_vector_size;
};

template<>
class cardinal_trigonometric_detail<double> {
public:
  cardinal_trigonometric_detail(const double* data, size_t length, double t0, double h) : m_t0{t0}, m_h{h}
  {
    if (length == 0)
    {
      throw std::logic_error("At least one sample is required.");
    }
    if (h <= 0)
    {
      throw std::logic_error("The step size must be > 0");
    }
    m_T = m_h*length;
    m_complex_vector_size = length/2 + 1;
    m_gamma = fftw_alloc_complex(m_complex_vector_size);
    fftw_plan plan = fftw_plan_dft_r2c_1d(length, const_cast<double*>(data), m_gamma, FFTW_ESTIMATE);
    if (!plan)
    {
      throw std::logic_error("A null fftw plan was created.");
    }

    fftw_execute(plan);
    fftw_destroy_plan(plan);

    double denom = length;
    for (size_t k = 0; k < m_complex_vector_size; ++k)
    {
      m_gamma[k][0] /= denom;
      m_gamma[k][1] /= denom;
    }

    if (length % 2 == 0)
    {
      m_gamma[m_complex_vector_size -1][0] /= 2;
    }
  }

  cardinal_trigonometric_detail(const cardinal_trigonometric_detail& old)  = delete;

  cardinal_trigonometric_detail& operator=(const cardinal_trigonometric_detail&) = delete;

  cardinal_trigonometric_detail(cardinal_trigonometric_detail &&) = delete;

  double operator()(double t) const
  {
    using std::sin;
    using std::cos;
    using boost::math::constants::two_pi;
    using std::exp;
    double s = m_gamma[0][0];
    double x = two_pi<double>()*(t - m_t0)/m_T;
    fftw_complex z;
    z[0] = cos(x);
    z[1] = sin(x);
    fftw_complex b{0, 0};
    // u = b*z
    fftw_complex u;
    for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
    {
      u[0] = b[0]*z[0] - b[1]*z[1];
      u[1] = b[0]*z[1] + b[1]*z[0];
      b[0] = m_gamma[k][0] + u[0];
      b[1] = m_gamma[k][1] + u[1];
    }

    s += 2*(b[0]*z[0] - b[1]*z[1]);
    return s;
  }

  double prime(double t) const
  {
      using std::sin;
      using std::cos;
      using boost::math::constants::two_pi;
      using std::exp;
      double x = two_pi<double>()*(t - m_t0)/m_T;
      fftw_complex z;
      z[0] = cos(x);
      z[1] = sin(x);
      fftw_complex b{0, 0};
      // u = b*z
      fftw_complex u;
      for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
      {
        u[0] = b[0]*z[0] - b[1]*z[1];
        u[1] = b[0]*z[1] + b[1]*z[0];
        b[0] = k*m_gamma[k][0] + u[0];
        b[1] = k*m_gamma[k][1] + u[1];
      }
      // b*z = (b[0]*z[0] - b[1]*z[1]) + i(b[1]*z[0] + b[0]*z[1])
      return -2*two_pi<double>()*(b[1]*z[0] + b[0]*z[1])/m_T;
  }

  double double_prime(double t) const
  {
      using std::sin;
      using std::cos;
      using boost::math::constants::two_pi;
      using std::exp;
      double x = two_pi<double>()*(t - m_t0)/m_T;
      fftw_complex z;
      z[0] = cos(x);
      z[1] = sin(x);
      fftw_complex b{0, 0};
      // u = b*z
      fftw_complex u;
      for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
      {
        u[0] = b[0]*z[0] - b[1]*z[1];
        u[1] = b[0]*z[1] + b[1]*z[0];
        b[0] = k*k*m_gamma[k][0] + u[0];
        b[1] = k*k*m_gamma[k][1] + u[1];
      }
      // b*z = (b[0]*z[0] - b[1]*z[1]) + i(b[1]*z[0] + b[0]*z[1])
      return -2*two_pi<double>()*two_pi<double>()*(b[0]*z[0] - b[1]*z[1])/(m_T*m_T);
  }

  double period() const
  {
    return m_T;
  }

  double integrate() const
  {
    return m_T*m_gamma[0][0];
  }

  double squared_l2() const
  {
    double s = 0;
    for (size_t i = m_complex_vector_size - 1; i >= 1; --i)
    {
        s += (m_gamma[i][0]*m_gamma[i][0] + m_gamma[i][1]*m_gamma[i][1]);
    }
    s *= 2;
    s += m_gamma[0][0]*m_gamma[0][0];
    return s*m_T;
  }

  ~cardinal_trigonometric_detail()
  {
    if (m_gamma)
    {
      fftw_free(m_gamma);
      m_gamma = nullptr;
    }
  }

private:
  double m_t0;
  double m_h;
  double m_T;
  fftw_complex* m_gamma;
  size_t m_complex_vector_size;
};

template<>
class cardinal_trigonometric_detail<long double> {
public:
  cardinal_trigonometric_detail(const long double* data, size_t length, long double t0, long double h) : m_t0{t0}, m_h{h}
  {
    if (length == 0)
    {
      throw std::logic_error("At least one sample is required.");
    }
    if (h <= 0)
    {
      throw std::logic_error("The step size must be > 0");
    }
    m_T = m_h*length;
    m_complex_vector_size = length/2 + 1;
    m_gamma = fftwl_alloc_complex(m_complex_vector_size);
    fftwl_plan plan = fftwl_plan_dft_r2c_1d(length, const_cast<long double*>(data), m_gamma, FFTW_ESTIMATE);
    if (!plan)
    {
      throw std::logic_error("A null fftw plan was created.");
    }

    fftwl_execute(plan);
    fftwl_destroy_plan(plan);

    long double denom = length;
    for (size_t k = 0; k < m_complex_vector_size; ++k)
    {
      m_gamma[k][0] /= denom;
      m_gamma[k][1] /= denom;
    }

    if (length % 2 == 0) {
      m_gamma[m_complex_vector_size -1][0] /= 2;
    }
  }

  cardinal_trigonometric_detail(const cardinal_trigonometric_detail& old)  = delete;

  cardinal_trigonometric_detail& operator=(const cardinal_trigonometric_detail&) = delete;

  cardinal_trigonometric_detail(cardinal_trigonometric_detail &&) = delete;

  long double operator()(long double t) const
  {
    using std::sin;
    using std::cos;
    using boost::math::constants::two_pi;
    using std::exp;
    long double s = m_gamma[0][0];
    long double x = two_pi<long double>()*(t - m_t0)/m_T;
    fftwl_complex z;
    z[0] = cos(x);
    z[1] = sin(x);
    fftwl_complex b{0, 0};
    fftwl_complex u;
    for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
    {
      u[0] = b[0]*z[0] - b[1]*z[1];
      u[1] = b[0]*z[1] + b[1]*z[0];
      b[0] = m_gamma[k][0] + u[0];
      b[1] = m_gamma[k][1] + u[1];
    }

    s += 2*(b[0]*z[0] - b[1]*z[1]);
    return s;
  }

  long double prime(long double t) const
  {
      using std::sin;
      using std::cos;
      using boost::math::constants::two_pi;
      using std::exp;
      long double x = two_pi<long double>()*(t - m_t0)/m_T;
      fftwl_complex z;
      z[0] = cos(x);
      z[1] = sin(x);
      fftwl_complex b{0, 0};
      // u = b*z
      fftwl_complex u;
      for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
      {
        u[0] = b[0]*z[0] - b[1]*z[1];
        u[1] = b[0]*z[1] + b[1]*z[0];
        b[0] = k*m_gamma[k][0] + u[0];
        b[1] = k*m_gamma[k][1] + u[1];
      }
      // b*z = (b[0]*z[0] - b[1]*z[1]) + i(b[1]*z[0] + b[0]*z[1])
      return -2*two_pi<long double>()*(b[1]*z[0] + b[0]*z[1])/m_T;
  }

  long double double_prime(long double t) const
  {
      using std::sin;
      using std::cos;
      using boost::math::constants::two_pi;
      using std::exp;
      long double x = two_pi<long double>()*(t - m_t0)/m_T;
      fftwl_complex z;
      z[0] = cos(x);
      z[1] = sin(x);
      fftwl_complex b{0, 0};
      // u = b*z
      fftwl_complex u;
      for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
      {
        u[0] = b[0]*z[0] - b[1]*z[1];
        u[1] = b[0]*z[1] + b[1]*z[0];
        b[0] = k*k*m_gamma[k][0] + u[0];
        b[1] = k*k*m_gamma[k][1] + u[1];
      }
      // b*z = (b[0]*z[0] - b[1]*z[1]) + i(b[1]*z[0] + b[0]*z[1])
      return -2*two_pi<long double>()*two_pi<long double>()*(b[0]*z[0] - b[1]*z[1])/(m_T*m_T);
  }

  long double period() const
  {
    return m_T;
  }

  long double integrate() const
  {
    return m_T*m_gamma[0][0];
  }

  long double squared_l2() const
  {
    long double s = 0;
    for (size_t i = m_complex_vector_size - 1; i >= 1; --i)
    {
        s += (m_gamma[i][0]*m_gamma[i][0] + m_gamma[i][1]*m_gamma[i][1]);
    }
    s *= 2;
    s += m_gamma[0][0]*m_gamma[0][0];
    return s*m_T;
  }

  ~cardinal_trigonometric_detail()
  {
    if (m_gamma)
    {
      fftwl_free(m_gamma);
      m_gamma = nullptr;
    }
  }

private:
  long double m_t0;
  long double m_h;
  long double m_T;
  fftwl_complex* m_gamma;
  size_t m_complex_vector_size;
};

#ifdef BOOST_HAS_FLOAT128
template<>
class cardinal_trigonometric_detail<__float128> {
public:
  cardinal_trigonometric_detail(const __float128* data, size_t length, __float128 t0, __float128 h) : m_t0{t0}, m_h{h}
  {
    if (length == 0)
    {
      throw std::logic_error("At least one sample is required.");
    }
    if (h <= 0)
    {
      throw std::logic_error("The step size must be > 0");
    }
    m_T = m_h*length;
    m_complex_vector_size = length/2 + 1;
    m_gamma = fftwq_alloc_complex(m_complex_vector_size);
    fftwq_plan plan = fftwq_plan_dft_r2c_1d(length, reinterpret_cast<__float128*>(const_cast<__float128*>(data)), m_gamma, FFTW_ESTIMATE);
    if (!plan)
    {
      throw std::logic_error("A null fftw plan was created.");
    }

    fftwq_execute(plan);
    fftwq_destroy_plan(plan);

    __float128 denom = length;
    for (size_t k = 0; k < m_complex_vector_size; ++k)
    {
      m_gamma[k][0] /= denom;
      m_gamma[k][1] /= denom;
    }
    if (length % 2 == 0)
    {
      m_gamma[m_complex_vector_size -1][0] /= 2;
    }
  }

  cardinal_trigonometric_detail(const cardinal_trigonometric_detail& old)  = delete;

  cardinal_trigonometric_detail& operator=(const cardinal_trigonometric_detail&) = delete;

  cardinal_trigonometric_detail(cardinal_trigonometric_detail &&) = delete;

  __float128 operator()(__float128 t) const
  {
    using std::sin;
    using std::cos;
    using boost::math::constants::two_pi;
    using std::exp;
    __float128 s = m_gamma[0][0];
    __float128 x = two_pi<__float128>()*(t - m_t0)/m_T;
    fftwq_complex z;
    z[0] = cosq(x);
    z[1] = sinq(x);
    fftwq_complex b{0, 0};
    fftwq_complex u;
    for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
    {
      u[0] = b[0]*z[0] - b[1]*z[1];
      u[1] = b[0]*z[1] + b[1]*z[0];
      b[0] = m_gamma[k][0] + u[0];
      b[1] = m_gamma[k][1] + u[1];
    }

    s += 2*(b[0]*z[0] - b[1]*z[1]);
    return s;
  }

  __float128 prime(__float128 t) const
  {
      using std::sin;
      using std::cos;
      using boost::math::constants::two_pi;
      using std::exp;
      __float128 x = two_pi<__float128>()*(t - m_t0)/m_T;
      fftwq_complex z;
      z[0] = cosq(x);
      z[1] = sinq(x);
      fftwq_complex b{0, 0};
      // u = b*z
      fftwq_complex u;
      for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
      {
        u[0] = b[0]*z[0] - b[1]*z[1];
        u[1] = b[0]*z[1] + b[1]*z[0];
        b[0] = k*m_gamma[k][0] + u[0];
        b[1] = k*m_gamma[k][1] + u[1];
      }
      // b*z = (b[0]*z[0] - b[1]*z[1]) + i(b[1]*z[0] + b[0]*z[1])
      return -2*two_pi<__float128>()*(b[1]*z[0] + b[0]*z[1])/m_T;
  }

  __float128 double_prime(__float128 t) const
  {
      using std::sin;
      using std::cos;
      using boost::math::constants::two_pi;
      using std::exp;
      __float128 x = two_pi<__float128>()*(t - m_t0)/m_T;
      fftwq_complex z;
      z[0] = cosq(x);
      z[1] = sinq(x);
      fftwq_complex b{0, 0};
      // u = b*z
      fftwq_complex u;
      for (size_t k = m_complex_vector_size - 1; k >= 1; --k)
      {
        u[0] = b[0]*z[0] - b[1]*z[1];
        u[1] = b[0]*z[1] + b[1]*z[0];
        b[0] = k*k*m_gamma[k][0] + u[0];
        b[1] = k*k*m_gamma[k][1] + u[1];
      }
      // b*z = (b[0]*z[0] - b[1]*z[1]) + i(b[1]*z[0] + b[0]*z[1])
      return -2*two_pi<__float128>()*two_pi<__float128>()*(b[0]*z[0] - b[1]*z[1])/(m_T*m_T);
  }

  __float128 period() const
  {
    return m_T;
  }

  __float128 integrate() const
  {
    return m_T*m_gamma[0][0];
  }

  __float128 squared_l2() const
  {
    __float128 s = 0;
    for (size_t i = m_complex_vector_size - 1; i >= 1; --i)
    {
      s += (m_gamma[i][0]*m_gamma[i][0] + m_gamma[i][1]*m_gamma[i][1]);
    }
    s *= 2;
    s += m_gamma[0][0]*m_gamma[0][0];
    return s*m_T;
  }

  ~cardinal_trigonometric_detail()
  {
    if (m_gamma)
    {
      fftwq_free(m_gamma);
      m_gamma = nullptr;
    }
  }

private:
  __float128 m_t0;
  __float128 m_h;
  __float128 m_T;
  fftwq_complex* m_gamma;
  size_t m_complex_vector_size;
};
#endif

}}}}
#endif


#include <boost/dynamic_bitset.hpp>
#include <iostream>

int main()
{
  boost::dynamic_bitset<> db{3, 4};

  db.push_back(true);

  std::cout.setf(std::ios::boolalpha);
  std::cout << db.size() << '\n';
  std::cout << db.count() << '\n';
  std::cout << db.any() << '\n';
  std::cout << db.none() << '\n';

  std::cout << db[0].flip() << '\n';
  std::cout << ~db[3] << '\n';
  std::cout << db << '\n';
}


/*
 * goniometer.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/shared_ptr.hpp>
#include <string>
#include <sstream>
#include <scitbx/constants.h>
#include <dxtbx/model/goniometer.h>
#include <dxtbx/model/kappa_goniometer.h>
#include <dxtbx/model/multi_axis_goniometer.h>
#include <dxtbx/model/boost_python/to_from_dict.h>

namespace dxtbx { namespace model { namespace boost_python {

  using namespace boost::python;
  using scitbx::deg_as_rad;

  static
  void rotate_around_origin(Goniometer &goniometer, vec3<double> axis, double angle, bool deg) {
    double angle_rad = deg ? deg_as_rad(angle) : angle;
    goniometer.rotate_around_origin(axis, angle_rad);
  }

  std::string goniometer_to_string(const Goniometer &goniometer) {
    std::stringstream ss;
    ss << goniometer;
    return ss.str();
  }

  struct GoniometerPickleSuite : boost::python::pickle_suite {
    static
    boost::python::tuple getinitargs(const Goniometer &obj) {
      return boost::python::make_tuple(
        obj.get_rotation_axis_datum(),
        obj.get_fixed_rotation(),
        obj.get_setting_rotation());
    }

    static
    boost::python::tuple getstate(boost::python::object obj)
    {
      const Goniometer &goniometer = boost::python::extract<const Goniometer &>(obj)();
      return boost::python::make_tuple(
          obj.attr("__dict__"),
          goniometer.get_setting_rotation_at_scan_points());
    }

    static
    void setstate(boost::python::object obj, boost::python::tuple state)
    {
      Goniometer &goniometer = boost::python::extract<Goniometer&>(obj)();
      DXTBX_ASSERT(boost::python::len(state) == 2);

      // restore the object's __dict__
      boost::python::dict d = boost::python::extract<boost::python::dict>(
          obj.attr("__dict__"))();
      d.update(state[0]);

      // restore the internal state of the C++ object
      scitbx::af::const_ref< mat3<double> > S_list = boost::python::extract<
        scitbx::af::const_ref< mat3<double> > >(state[1]);
      goniometer.set_setting_rotation_at_scan_points(S_list);
    }

    static bool getstate_manages_dict() { return true; }
  };

  static
  void Goniometer_set_S_at_scan_points_from_tuple(Goniometer &goniometer, boost::python::tuple l) {
    scitbx::af::shared< mat3<double> > S_list;
    for (std::size_t i = 0; i < boost::python::len(l); ++i) {
      mat3<double> S = boost::python::extract< mat3<double> >(l[i]);
      S_list.push_back(S);
    }
    goniometer.set_setting_rotation_at_scan_points(S_list.const_ref());
  }

  static
  void Goniometer_set_S_at_scan_points_from_list(Goniometer &goniometer, boost::python::list l) {
    scitbx::af::shared< mat3<double> > S_list;
    for (std::size_t i = 0; i < boost::python::len(l); ++i) {
      mat3<double> S = boost::python::extract< mat3<double> >(l[i]);
      S_list.push_back(S);
    }
    goniometer.set_setting_rotation_at_scan_points(S_list.const_ref());
  }

  template <>
  boost::python::dict to_dict<Goniometer>(const Goniometer &obj) {
    boost::python::dict result;
    result["rotation_axis"] = obj.get_rotation_axis_datum();
    result["fixed_rotation"] = obj.get_fixed_rotation();
    result["setting_rotation"] = obj.get_setting_rotation();
    if(obj.get_num_scan_points() > 0){
      boost::python::list l;
      scitbx::af::shared< mat3<double> > setting_rotation_at_scan_points =
          obj.get_setting_rotation_at_scan_points();
      for (scitbx::af::shared< mat3<double> >::iterator it = setting_rotation_at_scan_points.begin();
           it != setting_rotation_at_scan_points.end();
           ++it) {
        l.append(boost::python::tuple(*it));
      }
      result["setting_rotation_at_scan_points"] = l;
    }
    return result;
  }

  template <>
  Goniometer* from_dict<Goniometer>(boost::python::dict obj) {
    Goniometer* g = new Goniometer(
      boost::python::extract< vec3<double> >(obj["rotation_axis"]),
      boost::python::extract< mat3<double> >(obj.get("fixed_rotation",
        mat3<double>(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0))),
      boost::python::extract< mat3<double> >(obj.get("setting_rotation",
        mat3<double>(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0))));
    if(obj.has_key("setting_rotation_at_scan_points")){
      boost::python::list S_at_scan_points = boost::python::extract<boost::python::list>(obj["setting_rotation_at_scan_points"]);
      Goniometer_set_S_at_scan_points_from_list(*g, S_at_scan_points);
    }
    return g;
  }

  void export_goniometer()
  {
    class_ <GoniometerBase> ("GoniometerBase");

    class_ <Goniometer, boost::shared_ptr<Goniometer>, bases <GoniometerBase> > ("Goniometer")
      .def(init<const Goniometer&>())
      .def(init <vec3 <double> > ((
          arg("rotation_axis"))))
      .def(init <vec3 <double>,
                 mat3 <double> > ((
          arg("rotation_axis"),
          arg("fixed_rotation_matrix"))))
      .def(init <vec3 <double>,
                 mat3 <double>,
                 mat3 <double> > ((
          arg("rotation_axis"),
          arg("fixed_rotation_matrix"),
          arg("setting_rotation_matrix"))))
      .def("get_rotation_axis",
        &Goniometer::get_rotation_axis)
      .def("set_rotation_axis",
        &Goniometer::set_rotation_axis)
      .def("get_rotation_axis_datum",
        &Goniometer::get_rotation_axis_datum)
      .def("set_rotation_axis_datum",
        &Goniometer::set_rotation_axis_datum)
      .def("get_fixed_rotation",
        &Goniometer::get_fixed_rotation)
      .def("set_fixed_rotation",
        &Goniometer::set_fixed_rotation)
      .def("get_setting_rotation",
        &Goniometer::get_setting_rotation)
      .def("set_setting_rotation",
        &Goniometer::set_setting_rotation)
      .add_property("num_scan_points", &Goniometer::get_num_scan_points)
      .def("get_num_scan_points",
        &Goniometer::get_num_scan_points)
      .def("set_setting_rotation_at_scan_points",
        &Goniometer::set_setting_rotation_at_scan_points)
      .def("set_setting_rotation_at_scan_points",
        &Goniometer_set_S_at_scan_points_from_tuple)
      .def("set_setting_rotation_at_scan_points",
        &Goniometer_set_S_at_scan_points_from_list)
      .def("get_setting_rotation_at_scan_points",
        &Goniometer::get_setting_rotation_at_scan_points)
      .def("get_setting_rotation_at_scan_point",
        &Goniometer::get_setting_rotation_at_scan_point)
      .def("reset_scan_points",
        &Goniometer::reset_scan_points)
      .def("rotate_around_origin",
          &rotate_around_origin, (
            arg("axis"),
            arg("angle"),
            arg("deg")=true))
      .def("__eq__", &Goniometer::operator==)
      .def("__ne__", &Goniometer::operator!=)
      .def("is_similar_to", &Goniometer::is_similar_to, (
            arg("other"),
            arg("rotation_axis_tolerance")=1e-6,
            arg("fixed_rotation_tolerance")=1e-6,
            arg("setting_rotation_tolerance")=1e-6))
      .def("__str__", &goniometer_to_string)
      .def("to_dict", &to_dict<Goniometer>)
      .def("from_dict", &from_dict<Goniometer>,
        return_value_policy<manage_new_object>())
      .staticmethod("from_dict")
      .def_pickle(GoniometerPickleSuite());
  }

}}} // namespace dxtbx::model::boost_python


/*
 * Utils.cpp
 *
 *  Created on: Nov 25, 2016
 *      Author: victor
 */

#include "Utils.h"

#include <vector>
#include <sstream>
#include <boost/foreach.hpp>
#include <iostream>
#include <bitset>
#include <stdexcept>
using namespace std;

namespace algorithms {

	namespace utils {

		vector<unsigned char> get_bytes(std::uint32_t value) {
			vector<unsigned char> result;
			std::uint32_t mask = 0x000000FF; //0000 0000 0000 0000 0000 0000 1111 1111

			for (unsigned int i = 0; i<4; ++i){
				//std::bitset<32> x(mask);
				//cout<<x<<endl;
				//cout<<value<<" "<<mask<<" "<<(value & mask)<<endl;
				result.push_back((unsigned char)((value & mask) >> 8*i));
				mask = mask<<8;
			}

			return result;
		}

		vector<unsigned char> get_bytes(string value) {

			if (value.size() != 4){
				throw runtime_error("[ERROR get_bytes<string>] The string must have exactly 4 characters");
			}

			vector<unsigned char> result;
			for (unsigned int i = 0; i < 4; ++i){
				result.push_back((unsigned char) value[i]);
			}

			return result;
		}

		std::uint32_t bytes_to_int(vector<unsigned char> bytes){
			std::uint32_t val = 0;
			for (unsigned int i = 0; i<4; ++i){
				val = (val<<8) | bytes[3-i];
			}
			return val;
		}

		string pad_with_character(string const& message, char c, unsigned int chunk_byte_size, unsigned int last_chunk_byte_size){
			string tmp(message);
			// Append 0 ≤ k < 512 bits '0', such that the resulting message length in bits
			// is congruent to −64 ≡ 448 (mod 512)
			int last_chunk_byte_size_remainder = last_chunk_byte_size - (message.size() % chunk_byte_size);

			if( last_chunk_byte_size_remainder < 0){
				last_chunk_byte_size_remainder = (chunk_byte_size - message.size()) + last_chunk_byte_size;
			}
			tmp += std::string(last_chunk_byte_size_remainder, c);
			return tmp;
		}

		void print_32b_bin(std::uint32_t x, std::uint32_t y){
			std::bitset<32> bx(x);
			cout<<bx<<endl;
			std::bitset<32> by(y);
			cout<<by<<endl;
		}

	}

}

// Fully specialized template (partially specialized or not specialized must be defined
// in the header file).
template <>
std::uint32_t algorithms::utils::circular_left_shift<1>(std::uint32_t val){
	std::uint32_t mask = 0x80000000;
	if ((val&mask) == 0){
		return val<<1;
	}
	else{
		return (val<<1) +1;
	}
}

template <>
std::uint32_t algorithms::utils::circular_right_shift<1>(std::uint32_t val){
	std::uint32_t mask = 0x00000001;
	if ((val&mask) == 0){
		return val>>1;
	}
	else{
		return (val>>1) | 0x80000000;
	}
}


#include <boost/multiprecision/cpp_int.hpp>


// 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/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/functional/fix.hpp>
namespace hana = boost::hana;

BOOST_HANA_CONSTEXPR_STATELESS_LAMBDA auto factorial = hana::fix([](auto fact, auto n) -> int {
    if (n == 0) return 1;
    else        return n * fact(n - 1);
});

int main() {
    BOOST_HANA_CONSTEXPR_CHECK(factorial(5) == 120);
}


#define BOOST_TEST_MODULE SummationTest

#include "Summation.hpp"
#include <vector>

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/algorithm/string.hpp>

BOOST_AUTO_TEST_SUITE(SummationSuite)

using namespace com::github::nimelo;
using namespace std;

BOOST_AUTO_TEST_CASE(ShouldCorrectlyVectorOfIntegers)
{
    // Arrange
    vector<int> summands{1, 2, 3, 4, 10, 11};
    int expected_sum = 31;

    auto summation = Summation<int>(summands);

    // Act
    auto actual_sum = summation.get();

    // Assert
    BOOST_CHECK_EQUAL(expected_sum, actual_sum);
}

BOOST_AUTO_TEST_CASE(ShouldReturnZeroForNoSummands)
{
    // Arrange
    vector<int> summands;
    int expected_sum = 0;

    auto summation = Summation<int>(summands);

    // Act
    auto actual_sum = summation.get();

    // Assert
    BOOST_CHECK_EQUAL(expected_sum, actual_sum);
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#pragma once

#include <armnn/ArmNN.hpp>
#include <armnn/TypesUtils.hpp>

#include <Half.hpp>

#include <initializer_list>
#include <iterator>
#include <vector>

#include <boost/core/ignore_unused.hpp>
#include <boost/numeric/conversion/cast.hpp>

namespace armnnUtils
{

template<typename T, bool DoQuantize=true>
struct SelectiveQuantizer
{
    static T Quantize(float value, float scale, int32_t offset)
    {
        return armnn::Quantize<T>(value, scale, offset);
    }

    static float Dequantize(T value, float scale, int32_t offset)
    {
        return armnn::Dequantize(value, scale, offset);
    }
};

template<typename T>
struct SelectiveQuantizer<T, false>
{
    static T Quantize(float value, float scale, int32_t offset)
    {
        boost::ignore_unused(scale, offset);
        return value;
    }

    static float Dequantize(T value, float scale, int32_t offset)
    {
        boost::ignore_unused(scale, offset);
        return value;
    }
};

template<>
struct SelectiveQuantizer<armnn::Half, false>
{
    static armnn::Half Quantize(float value, float scale, int32_t offset)
    {
        boost::ignore_unused(scale, offset);
        return armnn::Half(value);
    }

    static float Dequantize(armnn::Half value, float scale, int32_t offset)
    {
        boost::ignore_unused(scale, offset);
        return value;
    }
};

template<typename T>
T SelectiveQuantize(float value, float scale, int32_t offset)
{
    return SelectiveQuantizer<T, armnn::IsQuantizedType<T>()>::Quantize(value, scale, offset);
};

template<typename T>
float SelectiveDequantize(T value, float scale, int32_t offset)
{
    return SelectiveQuantizer<T, armnn::IsQuantizedType<T>()>::Dequantize(value, scale, offset);
};

template<typename ItType>
struct IsFloatingPointIterator
{
    static constexpr bool value=std::is_floating_point<typename std::iterator_traits<ItType>::value_type>::value;
};

template <typename T, typename FloatIt,
typename std::enable_if<IsFloatingPointIterator<FloatIt>::value, int>::type=0 // Makes sure fp iterator is valid.
>
std::vector<T> QuantizedVector(FloatIt first, FloatIt last, float qScale, int32_t qOffset)
{
    std::vector<T> quantized;
    quantized.reserve(boost::numeric_cast<size_t>(std::distance(first, last)));

    for (auto it = first; it != last; ++it)
    {
        auto f = *it;
        T q = SelectiveQuantize<T>(f, qScale, qOffset);
        quantized.push_back(q);
    }

    return quantized;
}

template<typename T>
std::vector<T> QuantizedVector(const std::vector<float>& array, float qScale = 1.f, int32_t qOffset = 0)
{
    return QuantizedVector<T>(array.begin(), array.end(), qScale, qOffset);
}

template<typename T>
std::vector<T> QuantizedVector(std::initializer_list<float> array, float qScale = 1.f, int32_t qOffset = 0)
{
    return QuantizedVector<T>(array.begin(), array.end(), qScale, qOffset);
}

} // namespace armnnUtils


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

#include "estimators/affine_transform.h"

#include <Eigen/SVD>

#include "util/logging.h"

namespace colmap {

std::vector<AffineTransformEstimator::M_t> AffineTransformEstimator::Estimate(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2) {
  CHECK_EQ(points1.size(), points2.size());
  CHECK_GE(points1.size(), 3);

  // Sets up the linear system that we solve to obtain a least squared solution
  // for the affine transformation.
  Eigen::MatrixXd C(2 * points1.size(), 6);
  C.setZero();
  Eigen::VectorXd b(2 * points1.size(), 1);

  for (size_t i = 0; i < points1.size(); ++i) {
    const Eigen::Vector2d& x1 = points1[i];
    const Eigen::Vector2d& x2 = points2[i];

    C(2 * i, 0) = x1(0);
    C(2 * i, 1) = x1(1);
    C(2 * i, 2) = 1.0f;
    b(2 * i) = x2(0);

    C(2 * i + 1, 3) = x1(0);
    C(2 * i + 1, 4) = x1(1);
    C(2 * i + 1, 5) = 1.0f;
    b(2 * i + 1) = x2(1);
  }

  const Eigen::VectorXd nullspace =
      C.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

  Eigen::Map<const Eigen::Matrix<double, 3, 2>> A_t(nullspace.data());

  const std::vector<M_t> models = {A_t.transpose()};
  return models;
}

void AffineTransformEstimator::Residuals(const std::vector<X_t>& points1,
                                         const std::vector<Y_t>& points2,
                                         const M_t& A,
                                         std::vector<double>* residuals) {
  CHECK_EQ(points1.size(), points2.size());

  residuals->resize(points1.size());

  // Note that this code might not be as nice as Eigen expressions,
  // but it is significantly faster in various tests.

  const double A_00 = A(0, 0);
  const double A_01 = A(0, 1);
  const double A_02 = A(0, 2);
  const double A_10 = A(1, 0);
  const double A_11 = A(1, 1);
  const double A_12 = A(1, 2);

  for (size_t i = 0; i < points1.size(); ++i) {
    const double s_0 = points1[i](0);
    const double s_1 = points1[i](1);
    const double d_0 = points2[i](0);
    const double d_1 = points2[i](1);

    const double pd_0 = A_00 * s_0 + A_01 * s_1 + A_02;
    const double pd_1 = A_10 * s_0 + A_11 * s_1 + A_12;

    const double dd_0 = d_0 - pd_0;
    const double dd_1 = d_1 - pd_1;

    (*residuals)[i] = dd_0 * dd_0 + dd_1 * dd_1;
  }
}

}  // namespace colmap


#ifndef STAN_MATH_PRIM_PROB_INV_GAMMA_RNG_HPP
#define STAN_MATH_PRIM_PROB_INV_GAMMA_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a pseudorandom inverse gamma variate for the given shape
 * and scale parameters using the specified random number generator.
 *
 * alpha and beta can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_shape Type of shape parameter
 * @tparam T_scale Type of scale parameter
 * @tparam RNG type of random number generator
 * @param alpha (Sequence of) positive shape parameter(s)
 * @param beta (Sequence of) positive scale parameter(s)
 * @param rng random number generator
 * @return (Sequence of) inverse gamma random variate(s)
 * @throw std::domain_error if alpha or beta are nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_shape, typename T_scale, class RNG>
inline typename VectorBuilder<true, double, T_shape, T_scale>::type
inv_gamma_rng(const T_shape& alpha, const T_scale& beta, RNG& rng) {
  using boost::random::gamma_distribution;
  using boost::variate_generator;

  static const char* function = "inv_gamma_rng";

  check_positive_finite(function, "Shape parameter", alpha);
  check_positive_finite(function, "Scale parameter", beta);
  check_consistent_sizes(function, "Shape parameter", alpha, "Scale Parameter",
                         beta);

  scalar_seq_view<T_shape> alpha_vec(alpha);
  scalar_seq_view<T_scale> beta_vec(beta);
  size_t N = max_size(alpha, beta);
  VectorBuilder<true, double, T_shape, T_scale> output(N);

  for (size_t n = 0; n < N; ++n) {
    variate_generator<RNG&, gamma_distribution<> > gamma_rng(
        rng, gamma_distribution<>(alpha_vec[n],
                                  1 / static_cast<double>(beta_vec[n])));
    output[n] = 1 / gamma_rng();
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


#include <metal.hpp>
#include <boost/pfr.hpp>
#include <iostream>
#include <utility>
#include <tuple>
#include <string>

// Just some quick getting started finger exercises for metal/pfr
// Will remove later on

struct pod_type
{
    // plain pod type with lots of padding internally to test reflection on
    int i1;
    double d1;
    char c1;
    size_t sz1;
    float f1;
    short s1;
};

// metal mapping from type to python struct character code
// this is just a partial mapping for now. Will need to be extended and will have to
// handle array types also which will require some sort of transform
template <typename T>
using map_to_struct_code = metal::at_key<
metal::map<
    metal::pair<int, metal::number<'i'>>,
    metal::pair<double, metal::number<'d'>>,
    metal::pair<char, metal::number<'c'>>,
    metal::pair<size_t, metal::number<'Q'>>,
    metal::pair<float, metal::number<'f'>>,
    metal::pair<short, metal::number<'h'>>
>, T>;

template <typename ... Args>
struct extract_metal_list_impl
{
    using type = metal::list<Args...>;
};

template <template <typename ...> class T, typename ... Args>
auto extract_metal_list(T<Args...> prototype)
{
    return extract_metal_list_impl<Args...>();
}

template <typename T>
char extract_value()
{
    return char(T::value);
}

template <typename ...Args>
auto realize_tuple(metal::list<Args...> metal_list)
{
    return std::make_tuple(extract_value<Args>()...);
}

template<typename T, T...>
struct integer_sequence { };

template<std::size_t N, std::size_t... I>
struct gen_indices : gen_indices<(N - 1), (N - 1), I...> { };
template<std::size_t... I>
struct gen_indices<0, I...> : integer_sequence<std::size_t, I...> { };

std::string& to_string_impl(std::string& s, char h)
{
  using std::to_string;
  s += h;
  return s;
}

template<typename... T>
std::string& to_string_impl(std::string& s, char h, T&&... t)
{
  using std::to_string;
  s += h;
  return to_string_impl(s, std::forward<T>(t)...);
}

template<typename... T, std::size_t... I>
std::string to_string(const std::tuple<T...>& tup, integer_sequence<std::size_t, I...>)
{
  std::string result;
  int ctx[] = { (to_string_impl(result, std::get<I>(tup)...), 0), 0 };
  (void)ctx;
  return result;
}

template<typename... T>
std::string to_string(const std::tuple<T...>& tup)
{
  return to_string(tup, gen_indices<sizeof...(T)>{});
}

int main()
{
    auto s1 = boost::pfr::flat_structure_to_tuple(pod_type());
    using m1 = decltype(extract_metal_list(s1))::type;
    using m2 = metal::transform<metal::lambda<map_to_struct_code>, m1>;

    // Upto here, the elements of interest are encoded in the type signature of
    // a metal::list. This step now makes a real tuple out of that signature
    auto result = realize_tuple(m2());

    // Now the last bit is to turn that tuple into a string
    // https://stackoverflow.com/questions/23436406/converting-tuple-to-string
    std::cout << to_string(result) << std::endl;
    return 0;
}


#ifndef ___ASYNC_SLEEP_HPP__
#define ___ASYNC_SLEEP_HPP__
#include <boost/asio.hpp>
#include <chrono>
#include <functional>
#include <random>

typedef std::chrono::duration<uint64_t, std::milli> ms_duration;

class duration_generator {
private:
protected:
public:
  virtual ms_duration get() = 0;
  virtual ~duration_generator(){};
};
typedef std::shared_ptr<duration_generator> duration_generator_s;

class constant_duration_generator : public duration_generator {
private:
  ms_duration delay;

public:
  constant_duration_generator(const ms_duration &_delay) : delay(_delay) {}
  ms_duration get() override { return delay; }
};
typedef std::shared_ptr<constant_duration_generator> constant_duration_generator_s;

class random_duration_generator : public duration_generator {
private:
  ms_duration lower_limit;
  ms_duration upper_limit;
  std::random_device rd;
  std::mt19937 gen;
  std::uniform_int_distribution<> distrib;

public:
  random_duration_generator(ms_duration lower_limit, ms_duration upper_limit)
      : gen(rd()),
        distrib(lower_limit.count(), upper_limit.count()) {}

  random_duration_generator(const random_duration_generator &original)
      : gen(rd()),
        distrib(original.lower_limit.count(), original.upper_limit.count()) {}

  ms_duration get() { return ms_duration(distrib(rd)); }
};
typedef std::shared_ptr<random_duration_generator> random_duration_generator_s;

#endif


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../../example_func_x_jacobian.h"
#include "../../constraints_jacobian.h"

#define RENDER_PSI 0
#define RENDER_OBJECTIVE_FUNC 1
#define RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT 2

int render_type = RENDER_PSI;

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

double x_result = 0;
double x_trg = 1;

std::vector<double> path_result;
double contraint_a = 1.0;
double contraint_x_trg = 3;
double weight_constraint = 1;
int constraint_type = 0;

void obs_eq_constraint(double &delta, double a, double x, double x_trg, int type)
{
	if(type == 0){
		observation_equation_constraint(delta, a, x, x_trg);
	}else{
		observation_equation_sq_constraint(delta, a, x, x_trg);
	}
}
void obs_eq_constraint_jacobian(Eigen::Matrix<double, 1, 1> &j, double a, double x, double x_trg, int type)
{
	if(type == 0){
		observation_equation_constraint_jacobian(j, a, x, x_trg);
	}else{
		observation_equation_sq_constraint_jacobian(j, a, x, x_trg);
	}
}

int main(int argc, char *argv[]){
	path_result.push_back(x_result);
	if (false == initGL(&argc, argv)) {
		return 4;
	}
	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("simple_optimization_problem_func_x_with_constraints");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glLineWidth(2);

	switch(render_type){
		case RENDER_PSI:{
			glColor3f(1,0,0);
			glBegin(GL_LINE_STRIP);
			for(double x = -1000; x <=1000; x+=0.01){
				double y;
				example_func_x(y,x);
				glVertex3f(x,y,0);
			}
			glEnd();

			glPointSize(20);
			glColor3f(0,0,1);
			glBegin(GL_POINTS);
				double y;
				example_func_x(y,x_result);
				glVertex3f(x_result, y, 0);
			glEnd();
			glPointSize(1);

			glColor3f(0,1,0);
			glBegin(GL_LINE_STRIP);
			for(const auto &x:path_result){
				double y;
				example_func_x(y,x);
				glVertex3f(x,y,0);
			}
			glEnd();

			glColor3f(0,0,1);
			glBegin(GL_LINE_STRIP);
			for(double x = -1000; x <=1000; x+=0.01){
				double c;
				obs_eq_constraint(c,contraint_a,x,contraint_x_trg, constraint_type);
				glVertex3f(x,c,0);
			}
			glEnd();

			glColor3f(0,0,0);
			glBegin(GL_LINES);
				glVertex3f(-1000, x_trg, 0);
				glVertex3f(1000, x_trg, 0);
			glEnd();
		break;
		}
		case RENDER_OBJECTIVE_FUNC:{
			glColor3f(1,0,0);
			glBegin(GL_LINE_STRIP);
			for(double x = -1000; x <=1000; x+=0.01){
				double y;
				observation_equation_example_func_x(y,x,x_trg);
				glVertex3f(x,y*y,0);
			}
			glEnd();

			glPointSize(20);
			glColor3f(0,0,1);
			glBegin(GL_POINTS);
				double y;
				observation_equation_example_func_x(y,x_result,x_trg);
				glVertex3f(x_result, y*y, 0);
			glEnd();
			glPointSize(1);

			glColor3f(0,1,0);
			glBegin(GL_LINE_STRIP);
			for(const auto &x:path_result){
				double y;
				observation_equation_example_func_x(y,x,x_trg);
				glVertex3f(x,y*y,0);
			}
			glEnd();

			glColor3f(0,0,1);
			glBegin(GL_LINE_STRIP);
			for(double x = -1000; x <=1000; x+=0.01){
				double c;
				obs_eq_constraint(c,contraint_a,x,contraint_x_trg, constraint_type);
				glVertex3f(x,c*c,0);
			}
			glEnd();

			glColor3f(0,0,0);
			glBegin(GL_LINES);
				glVertex3f(-1000, 0, 0);
				glVertex3f(1000, 0, 0);
			glEnd();
			break;
		}
		case RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT:{
			glColor3f(1,0,0);
			glBegin(GL_LINE_STRIP);
			for(double x = -1000; x <=1000; x+=0.01){
				double y;
				observation_equation_example_func_x(y,x,x_trg);
				double c;
				obs_eq_constraint(c,contraint_a,x,contraint_x_trg, constraint_type);
				glVertex3f(x,y*y + c*c,0);
			}
			glEnd();

			glPointSize(20);
			glColor3f(0,0,1);
			glBegin(GL_POINTS);
				double y;
				observation_equation_example_func_x(y,x_result,x_trg);
				double c;
				obs_eq_constraint(c,contraint_a,x_result,contraint_x_trg, constraint_type);
				glVertex3f(x_result, y*y + c*c,0);
			glEnd();
			glPointSize(1);

			glColor3f(0,1,0);
			glBegin(GL_LINE_STRIP);
			for(const auto &x:path_result){
				double y;
				observation_equation_example_func_x(y,x,x_trg);
				double c;
				obs_eq_constraint(c,contraint_a,x,contraint_x_trg, constraint_type);
				glVertex3f(x,y*y + c*c,0);
			}
			glEnd();

			glColor3f(0,0,0);
			glBegin(GL_LINES);
				glVertex3f(-1000, 0, 0);
				glVertex3f(1000, 0, 0);
			glEnd();
			break;
		}
	}
	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'o':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			double delta;
			observation_equation_example_func_x(delta, x_result, x_trg);

			Eigen::Matrix<double, 1, 1> jacobian;
			observation_equation_example_func_x_jacobian(jacobian, x_result);

			int ir = 0;
			int ic = 0;
			tripletListA.emplace_back(ir, ic, -jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  1);
			tripletListB.emplace_back(ir, 0,  delta);

			int number_of_columns = 1;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				x_result += h_x[0];
				path_result.push_back(x_result);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'c':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			double delta;
			observation_equation_example_func_x(delta, x_result, x_trg);

			Eigen::Matrix<double, 1, 1> jacobian;
			observation_equation_example_func_x_jacobian(jacobian, x_result);

			int ir = 0;
			int ic = 0;
			tripletListA.emplace_back(ir, ic, -jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  1.0);
			tripletListB.emplace_back(ir, 0,  delta);

			ir = tripletListB.size();
			obs_eq_constraint(delta, contraint_a, x_result, contraint_x_trg, constraint_type);
		    obs_eq_constraint_jacobian(jacobian, contraint_a, x_result, contraint_x_trg, constraint_type);
		    tripletListA.emplace_back(ir, ic,  -jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  weight_constraint);
			tripletListB.emplace_back(ir, 0,  delta);

			int number_of_columns = 1;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				x_result += h_x[0];
				path_result.push_back(x_result);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			x_result = ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 20;
			path_result.clear();
			path_result.push_back(x_result);
			break;
		}
		case '-':{
			contraint_a -= 0.01;
			if(contraint_a < 0)contraint_a= 0.01;
			break;
		}
		case '=':{
			contraint_a += 0.01;
			break;
		}
		case 'd':{
			contraint_x_trg -= 0.1;
			break;
		}
		case 'i':{
			contraint_x_trg += 0.1;
			break;
		}
		case '1':{
			render_type = RENDER_PSI;
			break;
		}
		case '2':{
			render_type = RENDER_OBJECTIVE_FUNC;
			break;
		}
		case '3':{
			render_type = RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT;
			break;
		}
		case 'y':{
			x_trg += 0.1;
			std::cout << "x_trg: " << x_trg << std::endl;
			break;
		}
		case 't':{
			x_trg -= 0.1;
			std::cout << "x_trg: " << x_trg << std::endl;
			break;
		}
		case 'q':{
			x_result-=0.1;
			path_result.clear();
			path_result.push_back(x_result);
			break;
		}
		case 'w':{
			x_result+=0.1;
			path_result.clear();
			path_result.push_back(x_result);
			break;
		}
		case '4':{
			constraint_type = 0;
			break;
		}
		case '5':{
			constraint_type = 1;
			break;
		}
		case '6':{
			weight_constraint /=10;
			std::cout << "weight_constraint: " << weight_constraint << std::endl;
			break;
		}
		case '7':{
			weight_constraint *=10;
			std::cout << "weight_constraint: " << weight_constraint << std::endl;
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "o: optimize" << std::endl;
	std::cout << "c: optimize with constaint" << std::endl;
	std::cout << "-: contraint_a -= 0.01" << std::endl;
	std::cout << "=: contraint_a += 0.01" << std::endl;
	std::cout << "d: contraint_x_trg -= 0.1" << std::endl;
	std::cout << "i: contraint_x_trg += 0.1" << std::endl;
	std::cout << "1: RENDER_PSI" << std::endl;
	std::cout << "2: RENDER_OBJECTIVE_FUNC" << std::endl;
	std::cout << "3: RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT" << std::endl;
	std::cout << "y: x_trg += 0.1" << std::endl;
	std::cout << "t: x_trg -= 0.1" << std::endl;
	std::cout << "q: x_result-=0.1" << std::endl;
	std::cout << "w: x_result+=0.1" << std::endl;
	std::cout << "4: constraint_type linear" << std::endl;
	std::cout << "5: constraint_type squared" << std::endl;
	std::cout << "6: weight_constraint /=10" << std::endl;
	std::cout << "7: weight_constraint *=10" << std::endl;
	std::cout << "r: random initial guess" << std::endl;
}


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include <srook/numeric/fixed_point.hpp>
#include <srook/limits/numeric_limits.hpp>
#include <type_traits>

BOOST_AUTO_TEST_SUITE(numeric_fixed_point_test)

BOOST_AUTO_TEST_CASE(fixed_point_construct_test1)
{
    typedef srook::numeric::fixed_point<4, 4> fixed_point_type;
    SROOK_ATTRIBUTE_UNUSED fixed_point_type fp1 = srook::uint16_t(0xf);
    try {
        SROOK_ATTRIBUTE_UNUSED fixed_point_type fp2 = srook::uint16_t(0x1f);
    } catch (std::overflow_error) {}
}

BOOST_AUTO_TEST_CASE(fixed_point_construct_test2)
{
    typedef srook::numeric::fixed_point<4, 4> fixed_point_type;
    typedef srook::numeric_limits<float> float_limits;
    try {
        SROOK_ATTRIBUTE_UNUSED fixed_point_type fp = float_limits::quiet_NaN();
    } catch (std::overflow_error) {}
    try {
        SROOK_ATTRIBUTE_UNUSED fixed_point_type fp = float_limits::infinity();
    } catch (std::overflow_error) {}
    try {
        SROOK_ATTRIBUTE_UNUSED fixed_point_type fp = float_limits::max();
    } catch (std::overflow_error) {}
}

BOOST_AUTO_TEST_CASE(fixed_point_construct_test3)
{
    typedef srook::numeric::fixed_point<4, 4> fixed_point_type;
    SROOK_ST_ASSERT(std::is_nothrow_default_constructible<fixed_point_type>::value);
    SROOK_ST_ASSERT(std::is_nothrow_copy_constructible<fixed_point_type>::value);
    SROOK_ST_ASSERT(std::is_nothrow_move_constructible<fixed_point_type>::value);
    SROOK_ST_ASSERT(std::is_nothrow_copy_assignable<fixed_point_type>::value);
    SROOK_ST_ASSERT(std::is_nothrow_move_assignable<fixed_point_type>::value);

    SROOK_ATTRIBUTE_UNUSED fixed_point_type fp1{}, fp2 = fp1, fp3 = srook::move(fp2);
    fp1 = fp3;
    fp1 = srook::move(fp3);
}

BOOST_AUTO_TEST_CASE(fixed_point_construct_test4)
{
    typedef srook::numeric::fixed_point<4, 4> fixed_point_type;
    typedef srook::numeric_limits<SROOK_DEDUCED_TYPENAME fixed_point_type::value_type> value_limits;
    auto fp = fixed_point_type::from_value(value_limits::max());
    BOOST_TEST((fp == srook::numeric_limits<fixed_point_type>::max()));
    fp = fixed_point_type::from_value(value_limits::min());
    BOOST_TEST((fp == srook::numeric_limits<fixed_point_type>::min()));
}

BOOST_AUTO_TEST_SUITE_END()


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

#ifndef BOOST_MATH_CCMATH_SCALBN_HPP
#define BOOST_MATH_CCMATH_SCALBN_HPP

#include <cmath>
#include <cfloat>
#include <limits>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/abs.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>

namespace boost::math::ccmath {

namespace detail {

template <typename Real, typename Z>
inline constexpr Real scalbn_impl(Real arg, Z exp) noexcept
{
    while(exp > 0)
    {
        arg *= FLT_RADIX;
        --exp;
    }
    while(exp < 0)
    {
        arg /= FLT_RADIX;
        ++exp;
    }

    return arg;
}

} // Namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real scalbn(Real arg, int exp) noexcept
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(arg))
    {
        return boost::math::ccmath::abs(arg) == Real(0) ? arg :
               boost::math::ccmath::isinf(arg) ? arg :
               boost::math::ccmath::isnan(arg) ? arg :
               boost::math::ccmath::detail::scalbn_impl(arg, exp);
    }
    else
    {
        using std::scalbn;
        return scalbn(arg, exp);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr double scalbn(Z arg, int exp) noexcept
{
    return boost::math::ccmath::scalbn(static_cast<double>(arg), exp);
}

inline constexpr float scalbnf(float arg, int exp) noexcept
{
    return boost::math::ccmath::scalbn(arg, exp);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double scalbnl(long double arg, int exp) noexcept
{
    return boost::math::ccmath::scalbn(arg, exp);
}
#endif

} // Namespaces

#endif // BOOST_MATH_CCMATH_SCALBN_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_SORT_BY_KEY_HPP
#define BOOST_COMPUTE_ALGORITHM_SORT_BY_KEY_HPP

#include <iterator>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/detail/insertion_sort.hpp>
#include <boost/compute/algorithm/detail/radix_sort.hpp>
#include <boost/compute/algorithm/reverse.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>

namespace boost {
namespace compute {

namespace detail {

template<class KeyIterator, class ValueIterator>
inline void dispatch_sort_by_key(KeyIterator keys_first,
                        KeyIterator keys_last,
                        ValueIterator values_first,
                        less<typename std::iterator_traits<KeyIterator>::value_type> compare,
                        command_queue &queue)
{
    size_t count = detail::iterator_range_size(keys_first, keys_last);

    if(count < 32){
        detail::serial_insertion_sort_by_key(
            keys_first, keys_last, values_first, compare, queue
            );
    }
    else {
        detail::radix_sort_by_key(
            keys_first, keys_last, values_first, queue
            );
    }
}

template<class KeyIterator, class ValueIterator>
inline void dispatch_sort_by_key(KeyIterator keys_first,
                        KeyIterator keys_last,
                        ValueIterator values_first,
                        greater<typename std::iterator_traits<KeyIterator>::value_type> compare,
                        command_queue &queue)
{
    size_t count = detail::iterator_range_size(keys_first, keys_last);

    if(count < 32){
        detail::serial_insertion_sort_by_key(
            keys_first, keys_last, values_first, compare, queue
            );
    }
    else {
        // radix sorts in ascending order
        detail::radix_sort_by_key(
            keys_first, keys_last, values_first, queue
            );

        // Reverse keys, values for descending order
        ::boost::compute::reverse(keys_first, keys_last, queue);
        ::boost::compute::reverse(values_first, values_first + count, queue);

    }
}

template<class KeyIterator, class ValueIterator, class Compare>
inline void dispatch_sort_by_key(KeyIterator keys_first,
                        KeyIterator keys_last,
                        ValueIterator values_first,
                        Compare compare,
                        command_queue &queue)
{

    detail::serial_insertion_sort_by_key(
        keys_first,
        keys_last,
        values_first,
        compare,
        queue
        );
}

}

/// Performs a key-value sort using the keys in the range [\p keys_first,
/// \p keys_last) on the values in the range [\p values_first,
/// \p values_first \c + (\p keys_last \c - \p keys_first)) using \p compare.
///
/// If no compare function is specified, \c less is used.
///
/// \see sort()

template<class KeyIterator, class ValueIterator, class Compare>
inline void sort_by_key(KeyIterator keys_first,
                        KeyIterator keys_last,
                        ValueIterator values_first,
                        Compare compare,
                        command_queue &queue = system::default_queue())
{
    ::boost::compute::detail::dispatch_sort_by_key(
        keys_first,
        keys_last,
        values_first,
        compare,
        queue);
}

/// \overload
template<class KeyIterator, class ValueIterator>
inline void sort_by_key(KeyIterator keys_first,
                        KeyIterator keys_last,
                        ValueIterator values_first,
                        command_queue &queue = system::default_queue())
{
    typedef typename std::iterator_traits<KeyIterator>::value_type key_type;

    size_t count = detail::iterator_range_size(keys_first, keys_last);

    if(count < 32){
        detail::serial_insertion_sort_by_key(
            keys_first, keys_last, values_first, less<key_type>(), queue
        );
    }
    else {
        detail::radix_sort_by_key(
            keys_first, keys_last, values_first, queue
        );
    }
}

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_SORT_BY_KEY_HPP


#include "precompiled.h"
#include "boundary_conditions.h"
#include "boundary_condition_iteration.h"

#ifndef AUTOMATIC_PRECOMPILATION
#include <boost/throw_exception.hpp>
#include <tbb/tick_count.h>
#include <pretty_printer.h>
#endif

#include "logging.h"
#include "grid_iterators.h"
#include "logging.h"
#include "sparse_sherman_morisson.h"
#include "utility.h"
#include "global_config.h"

namespace fipster { namespace _boundary_conditions {

	using namespace Eigen;

	// #################################################
	// ##############   PHASE 3:			############
	// ##############   Node Bodies         ############
	// #################################################
	struct bc_line_handler : boundary_conditions_line<body_t::sg>{
		Bo_t& Bo;
		Bi_t& Bi;

		bc_line_handler(Bo_t& Bo, Bi_t& Bi)
			:Bo(Bo),Bi(Bi)
		{}

		void add_to_interior(	boundary_index<sg>& row,grid_index<sg+1>& col,
									double v, uint, bstate_t){
					Bi.insert(row,col)=v;
		}
		void add_to_boundary(	boundary_index<sg>& row,boundary_index<sg>& col,
								double v, uint, bstate_t){
			Bo.insert(row,col)=v;
		}

		void set_zero(	boundary_index<sg>&,uint variable,bstate_t state){
			//nothing to do for a zero row in a sparse matrix
		};
	};

	body_t::result_sptr_t body_t::operator()( const input_t& )
	{
		//logging
		if(1)thread_logger()<<"BCs    | START"<<endl;
		using tbb::tick_count;

		tick_count t0 = tick_count::now();

		auto& g= *arg.second;
		auto result = make_shared<result_t>();
		auto& Bi=result->Bi.resize(g);
		auto& Bo1 = result->Bo1.resize(g);
		auto& Bo = result->Bo.resize(g);

		bc_line_handler bcl(Bo,Bi);

		for(auto bcit=boundary_condition_iterator<sg,&bc::type>(arg);
				bcit.valid(); ++bcit)
			bcit.handle_bc_line(bcl);

		//cout<<_sparse_sherman_morisson::test_sparse_sherman_morisson(100)<<endl;

		if(0)
			invert_using_rowwise_sparse_sherman_morisson(*Bo1,*Bo,false);
		else
			invert_using_single_entry_sparse_sherman_morisson(*Bo1,*Bo,!FIPSTER_NDEBUG);

		//exit(0);
		if(global_config::get().test_bc_inversion){
			double BCinversion_norm = inversion_residuum(*Bo1,*Bo);
			if(BCinversion_norm>arg.first.tolerance)
				BOOST_THROW_EXCEPTION(runtime_error("BC residuum norm "+toS(BCinversion_norm)+" > BC config tolerance"));
		}

		//Eigen Bug: (A*B).pruned() did not only not prune the result,
		// instead id added zero entries(?!!!!!)
		//*Bi = (*Bo1 * *Bi).eval(); ///\todo TEST : is the product really the product

		if(1)thread_logger()<<"BCs    | DONE, assembled and inverted: "<<scientific <<setprecision(3)<<
			(tick_count::now()-t0).seconds()<<"s"<<endl;

		return result;
	}

	body_t::body_t(const bc_arg_t& arg)
		: arg(arg)
	{}

}}


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

/**
 *  @file   correction_speed.cpp
 *  @author Ross Hartley
 *  @brief  Test to determine average correction speed
 *  @date   September 25, 2018
 **/

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

#define DT_MIN 1e-6
#define DT_MAX 1

using namespace std;
using namespace inekf;
using namespace boost::posix_time;

typedef vector<pair<double,Eigen::Matrix<double,6,1> > > vectorPairIntVector6d;
typedef vector<pair<double,Eigen::Matrix<double,6,1> > >::const_iterator vectorPairIntVector6dIterator;

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

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

int main() {
    // Initialize filter
    InEKF filter;
    cout << "Robot's state is initialized to: \n";
    cout << filter.getState() << endl;

    ifstream infile("../data/correction_speed_test_data.txt");
    string line;
    Eigen::Matrix<double,6,1> m, m_last;
    double t, t_last;
    m_last << 0,0,0,0,0,0;
    t_last = 0;
    vectorPairIntVector6d measurements_vec;
    vectorLandmarks measured_landmarks;

    // Loop through data file and read in measurements line by line
    while (getline(infile, line)){
        vector<string> measurement;
        boost::split(measurement,line,boost::is_any_of(" "));
        // Handle measurements
        if (measurement[0].compare("IMU")==0){
            t = stod98(measurement[1]);
            m << stod98(measurement[2]),
                 stod98(measurement[3]),
                 stod98(measurement[4]),
                 stod98(measurement[5]),
                 stod98(measurement[6]),
                 stod98(measurement[7]);
            measurements_vec.push_back(pair<double,Eigen::Matrix<double,6,1> > (t, m));

        }
        else if (measurement[0].compare("LANDMARK")==0){
            t = stod98(measurement[1]);
            for (int i=2; i<measurement.size(); i+=4) {
                int id = stod98(measurement[i]);
                Eigen::Vector3d p_bl;
                p_bl << stod98(measurement[i+1]),
                        stod98(measurement[i+2]),
                        stod98(measurement[i+3]);
                Eigen::Matrix3d cov = 0.01*Eigen::Matrix3d::Identity();
                Landmark landmark(id, p_bl, cov);
                measured_landmarks.push_back(landmark);
            }
        }
    }

    // Propagate all IMU data
    cout << "Propagating " << measurements_vec.size() << " IMU measurements...\n";
    for (vectorPairIntVector6dIterator it=measurements_vec.begin(); it!=measurements_vec.end(); ++it) {
        // Propagate using IMU data
        t = it->first;
        m = it->second;
        double dt = t - t_last;
        if (dt > DT_MIN && dt < DT_MAX) {
            filter.Propagate(m_last, dt);
        }
        // Store previous timestamp
        t_last = t;
        m_last = m;
    }
    cout << filter.getState() << endl;

    // Correct using landmark data
    cout << "Correcting " << measured_landmarks.size() << " landmark measurements...\n";
    int64_t max_duration = 0;
    int64_t sum_duration = 0;
    for (vectorLandmarksIterator it=measured_landmarks.begin(); it!=measured_landmarks.end(); ++it) {
        vectorLandmarks landmarks;
        landmarks.push_back(*it);
	ptime start_time = second_clock::local_time();
        filter.CorrectLandmarks(landmarks);
        //cout << filter.getState() << endl;
	ptime end_time = second_clock::local_time();
        int64_t duration = (end_time - start_time).total_nanoseconds();
        //cout << "duration: " <<  duration << endl;
        sum_duration += duration;
        if (duration > max_duration)
            max_duration = duration;
    }
    cout << filter.getState() << endl;
    cout << "max duration: " <<  max_duration << endl;
    cout << "average duration: " <<  double(sum_duration)/(measured_landmarks.size()/3) << endl;
    return 0;
}


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

#include <boost/simd/arithmetic/functions/muls.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/split_multiplies.hpp>
#include <boost/simd/include/functions/simd/groups.hpp>
#include <boost/simd/sdk/meta/is_upgradable.hpp>
#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( muls_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>, X>))
                                      ((simd_<floating_<A0>, X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a0*a1;
    }
  };

  BOOST_DISPATCH_IMPLEMENT_IF          ( muls_, tag::cpu_
                                       , (A0)(X)
                                       , ( simd::meta::is_upgradable_on_ext<A0,X> )
                                       , ((simd_<integer_<A0>, X>))
                                         ((simd_<integer_<A0>, X>))
                                       )
  {

    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type utype;

      utype res0, res1;
      split_multiplies(a0, a1, res0, res1);
      return groups(res0, res1);
    }
  };
} } }

#endif


#include "KinectSDKConverterProcessor.h"

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/round.hpp>

#include <Windows.h>
#include <NuiApi.h>

#include <WindowsOnly/Grabber/KinectSDKGrabber.h>
#include <Exception/HPEException.h>
#include <DataObject/RawFrames.h>
#include <DataObject/CloudXYZRGBA.h>

namespace hpe
{
    typedef union
    {
        struct
        {
            unsigned char Blue;
            unsigned char Green;
            unsigned char Red;
            unsigned char Alpha;
        };
        float float_value;
        uint32_t long_value;
    } RGBValue;

    KinectSDKConverterProcessor::KinectSDKConverterProcessor() : m_cloudKey("Cloud")
    {
    }

    KinectSDKConverterProcessor::KinectSDKConverterProcessor(std::string key) : m_cloudKey(key)
    {
    }

    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr KinectSDKConverterProcessor::DepthRGBToPointCloud(cv::Mat &depth, cv::Mat &bgr)
    {
        cv::Size depthImageSize = depth.size();

        float centerX, centerY;
        float scaleFactorX, scaleFactorY;

        ComputeCameraIntrinsics(depthImageSize, centerX, centerY, scaleFactorX, scaleFactorY);

        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGBA>);
        result->width = depthImageSize.width;
        result->height = depthImageSize.height;
        result->is_dense = false;
        result->points.resize(depthImageSize.width * depthImageSize.height);

        for (int y = 0; y < depthImageSize.height; ++y)
        {
            for (int x = 0; x < depthImageSize.width; ++x)
            {
                pcl::PointXYZRGBA &pt = result->at(x, y);

                float depthValue = depth.at<float>(y, x);

                if (depthValue == -1000.f || depthValue == 0.0f || boost::math::isnan(depthValue))
                {
                    pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
                }
                else
                {
                    //depthValue /= 1000;
                    pt.x = (static_cast<float>(x) - centerX) * scaleFactorX * depthValue;
                    pt.y = (centerY - static_cast<float>(y)) * scaleFactorY * depthValue;
                    pt.z = depthValue;

                    int depthIndex = x + y * 640;

                    LONG actX = m_correspondencies[depthIndex * 2];
                    LONG actY = m_correspondencies[depthIndex * 2 + 1];

                    if (actX >= bgr.cols || actX < 0 || actY < 0 || actY >= bgr.rows)
                    {
                        pt.rgba = 0;
                    }
                    else
                    {
                        cv::Vec4b colorValue = bgr.at<cv::Vec4b>(actY, actX);
                        RGBValue color;
                        color.Red = colorValue[2];
                        color.Green = colorValue[1];
                        color.Blue = colorValue[0];
                        pt.rgba = color.long_value;
                    }
                }
            }
        }

        return result;
    }

    void KinectSDKConverterProcessor::ComputeCameraIntrinsics(cv::Size &imageSize, float &centerX, float &centerY, float &scaleFactorX, float &scaleFactorY)
    {
        int dims[] = {imageSize.width, imageSize.height};

        // The 525 factor default is only true for VGA. If not, we should scale
        scaleFactorX = scaleFactorY = 1 / 525.f * 640.f / dims[0];
        centerX = (static_cast<float>(dims[0]) - 1.f) / 2.f;
        centerY = (static_cast<float>(dims[1]) - 1.f) / 2.f;
    }

    void KinectSDKConverterProcessor::Process(IDataStorage::Ptr dataStorage)
    {
        std::shared_ptr<KinectCoordinatesMapper> mapperObject = std::dynamic_pointer_cast<KinectCoordinatesMapper>(dataStorage->Get("CoordinatesMapper"));
        if (mapperObject.get() == nullptr)
        {
            throw HPEException("KinectSDKConverterProcessor::Process - mapperObject.get() == nullptr");
        }
        m_correspondencies = mapperObject->correspondencies;

        IDataObject::Ptr object = dataStorage->Get("RawFrames");
        RawFrames::Ptr rawFrames = std::dynamic_pointer_cast<RawFrames>(object);
        if (rawFrames.get() == nullptr)
        {
            throw HPEException("KinectSDKConverterProcessor::Process - rawFrames.get() == nullptr");
        }

        CloudXYZRGBA::Ptr cloudObject(new CloudXYZRGBA);
        cloudObject->cloud = DepthRGBToPointCloud(rawFrames->depthFrame, rawFrames->colorFrame);
        cloudObject->cloud->width = cloudObject->cloud->width * cloudObject->cloud->height;
        cloudObject->cloud->height = 1;

        dataStorage->Set(m_cloudKey, cloudObject);
    }

}


#include <benchmark/benchmark.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "UKF/Types.h"
#include "UKF/Integrator.h"
#include "UKF/StateVector.h"

using SV16_FourQuaternions = UKF::StateVector<
    UKF::Field<1, UKF::Quaternion>,
    UKF::Field<2, UKF::Quaternion>,
    UKF::Field<3, UKF::Quaternion>,
    UKF::Field<4, UKF::Quaternion>
>;

using SV16_FourVectors = UKF::StateVector<
    UKF::Field<1, UKF::Vector<4>>,
    UKF::Field<2, UKF::Vector<4>>,
    UKF::Field<3, UKF::Vector<4>>,
    UKF::Field<4, UKF::Vector<4>>
>;

using SV16_OneVector = UKF::StateVector<
    UKF::Field<1, UKF::Vector<16>>
>;

/*
Tests to ensure setField() and getField() don't hurt performance compared to
segment().
*/
void StateVector_SetGetUsingSegment(benchmark::State& state) {
    SV16_FourVectors test_state;
    test_state.segment<4>(0) << UKF::Vector<4>(1, 2, 3, 4);
    while(state.KeepRunning()) {
        test_state.segment<4>(0) << UKF::Vector<4>(1, 2, 3, 4);
        benchmark::DoNotOptimize(test_state.segment<4>(0));
    }
}

void StateVector_SetGetUsingSetField(benchmark::State& state) {
    SV16_FourVectors test_state;
    test_state.segment<4>(0) << UKF::Vector<4>(1, 2, 3, 4);
    while(state.KeepRunning()) {
        test_state.set_field<1>(UKF::Vector<4>(1, 2, 3, 4));
        benchmark::DoNotOptimize(test_state.get_field<1>());
    }
}

BENCHMARK(StateVector_SetGetUsingSegment);
BENCHMARK(StateVector_SetGetUsingSetField);

template <typename T>
void StateVector_SigmaPointGeneration(benchmark::State& state) {
    T test_state;
    test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1;
    typename T::CovarianceMatrix covariance = T::CovarianceMatrix::Zero();
    covariance.diagonal() << UKF::Vector<T::covariance_size()>::Ones();
    while(state.KeepRunning()) {
        benchmark::DoNotOptimize(test_state.calculate_sigma_point_distribution(covariance));
    }
}

BENCHMARK_TEMPLATE(StateVector_SigmaPointGeneration, SV16_OneVector);
BENCHMARK_TEMPLATE(StateVector_SigmaPointGeneration, SV16_FourVectors);
BENCHMARK_TEMPLATE(StateVector_SigmaPointGeneration, SV16_FourQuaternions);

template <typename T>
void StateVector_SigmaPointMean(benchmark::State& state) {
    T test_state;
    test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1;
    typename T::CovarianceMatrix covariance = T::CovarianceMatrix::Zero();
    covariance.diagonal() << UKF::Vector<T::covariance_size()>::Ones();
    typename T::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance);
    while(state.KeepRunning()) {
        benchmark::DoNotOptimize(T::calculate_sigma_point_mean(sigma_points));
    }
}

BENCHMARK_TEMPLATE(StateVector_SigmaPointMean, SV16_OneVector);
BENCHMARK_TEMPLATE(StateVector_SigmaPointMean, SV16_FourVectors);
BENCHMARK_TEMPLATE(StateVector_SigmaPointMean, SV16_FourQuaternions);

template <typename T>
void StateVector_SigmaPointDeltas(benchmark::State& state) {
    T test_state;
    test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1;
    typename T::CovarianceMatrix covariance = T::CovarianceMatrix::Zero();
    covariance.diagonal() << UKF::Vector<T::covariance_size()>::Ones();
    typename T::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance);
    T test_mean = T::calculate_sigma_point_mean(sigma_points);
    while(state.KeepRunning()) {
        benchmark::DoNotOptimize(test_mean.calculate_sigma_point_deltas(sigma_points));
    }
}

BENCHMARK_TEMPLATE(StateVector_SigmaPointDeltas, SV16_OneVector);
BENCHMARK_TEMPLATE(StateVector_SigmaPointDeltas, SV16_FourVectors);
BENCHMARK_TEMPLATE(StateVector_SigmaPointDeltas, SV16_FourQuaternions);

template <typename T>
void StateVector_SigmaPointCovariance(benchmark::State& state) {
    T test_state;
    test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1;
    typename T::CovarianceMatrix covariance = T::CovarianceMatrix::Zero();
    covariance.diagonal() << UKF::Vector<T::covariance_size()>::Ones();
    typename T::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance);
    T test_mean = T::calculate_sigma_point_mean(sigma_points);
    typename T::SigmaPointDeltas sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points);
    while(state.KeepRunning()) {
        benchmark::DoNotOptimize(T::calculate_sigma_point_covariance(sigma_point_deltas));
    }
}

BENCHMARK_TEMPLATE(StateVector_SigmaPointCovariance, SV16_OneVector);
BENCHMARK_TEMPLATE(StateVector_SigmaPointCovariance, SV16_FourVectors);
BENCHMARK_TEMPLATE(StateVector_SigmaPointCovariance, SV16_FourQuaternions);

template <typename T>
void StateVector_FullAPrioriCalculation(benchmark::State& state) {
    T test_state;
    test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1;
    typename T::CovarianceMatrix covariance = T::CovarianceMatrix::Zero();
    covariance.diagonal() << UKF::Vector<T::covariance_size()>::Ones();
    typename T::SigmaPointDistribution sigma_points;
    typename T::SigmaPointDeltas sigma_point_deltas;
    T test_mean;
    typename T::CovarianceMatrix apriori_covariance;
    while(state.KeepRunning()) {
        sigma_points = test_state.calculate_sigma_point_distribution(covariance);
        test_mean = T::calculate_sigma_point_mean(sigma_points);
        sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points);
        apriori_covariance = T::calculate_sigma_point_covariance(sigma_point_deltas);
    }
}

BENCHMARK_TEMPLATE(StateVector_FullAPrioriCalculation, SV16_OneVector);
BENCHMARK_TEMPLATE(StateVector_FullAPrioriCalculation, SV16_FourVectors);
BENCHMARK_TEMPLATE(StateVector_FullAPrioriCalculation, SV16_FourQuaternions);

template <typename T>
void StateVector_UpdateDelta(benchmark::State& state) {
    T test_state;
    test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1;
    typename T::StateVectorDelta test_delta = T::StateVectorDelta::Ones();
    while(state.KeepRunning()) {
        test_state.apply_delta(test_delta);
    }
}

BENCHMARK_TEMPLATE(StateVector_UpdateDelta, SV16_OneVector);
BENCHMARK_TEMPLATE(StateVector_UpdateDelta, SV16_FourVectors);
BENCHMARK_TEMPLATE(StateVector_UpdateDelta, SV16_FourQuaternions);


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/
#include "itkFiberCurvatureFilter.h"

#include <vtkDoubleArray.h>
#include <vtkPointData.h>
#include <boost/progress.hpp>

namespace itk{

FiberCurvatureFilter::FiberCurvatureFilter()
    : m_AngularDeviation(30)
    , m_Distance(10.0)
    , m_RemoveFibers(false)
    , m_UseMedian(false)
{

}

FiberCurvatureFilter::~FiberCurvatureFilter()
{

}

void FiberCurvatureFilter::GenerateData()
{
    vtkSmartPointer<vtkPolyData> inputPoly = m_InputFiberBundle->GetFiberPolyData();
    vtkSmartPointer<vtkPoints> vtkNewPoints = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> vtkNewCells = vtkSmartPointer<vtkCellArray>::New();

    MITK_INFO << "Applying curvature threshold";
    boost::progress_display disp(inputPoly->GetNumberOfCells());
#pragma omp parallel for
    for (int i=0; i<inputPoly->GetNumberOfCells(); i++)
    {
        std::vector< vnl_vector_fixed< double, 3 > > vertices;

#pragma omp critical
        {
            ++disp;
            vtkCell* cell = inputPoly->GetCell(i);
            int numPoints = cell->GetNumberOfPoints();
            vtkPoints* points = cell->GetPoints();

            for (int j=0; j<numPoints; j++)
            {
                double p[3];
                points->GetPoint(j, p);
                vnl_vector_fixed< double, 3 > p_vec;
                p_vec[0]=p[0]; p_vec[1]=p[1]; p_vec[2]=p[2];
                vertices.push_back(p_vec);
            }
        }

        // calculate curvatures
        int numPoints = vertices.size();
        vtkSmartPointer<vtkPolyLine> container = vtkSmartPointer<vtkPolyLine>::New();
        for (int j=0; j<numPoints; j++)
        {
            double dist = 0;
            int c = j;
            std::vector< vnl_vector_fixed< double, 3 > > vectors;
            vnl_vector_fixed< double, 3 > meanV; meanV.fill(0.0);
            while(dist<m_Distance/2 && c>1)
            {
              vnl_vector_fixed< double, 3 > p1 = vertices.at(c-1);
              vnl_vector_fixed< double, 3 > p2 = vertices.at(c);

                vnl_vector_fixed< double, 3 > v = p2-p1;
                dist += v.magnitude();
                v.normalize();
                vectors.push_back(v);
                if (m_UseMedian && c==j)
                    meanV += v;
                else if (!m_UseMedian)
                  meanV += v;
                c--;
            }
            c = j;
            dist = 0;
            while(dist<m_Distance/2 && c<numPoints-1)
            {
              vnl_vector_fixed< double, 3 > p1 = vertices.at(c);
              vnl_vector_fixed< double, 3 > p2 = vertices.at(c+1);

                vnl_vector_fixed< double, 3 > v = p2-p1;
                dist += v.magnitude();
                v.normalize();
                vectors.push_back(v);
                if (m_UseMedian && c==j)
                    meanV += v;
                else if (!m_UseMedian)
                  meanV += v;
                c++;
            }
            meanV.normalize();

            double dev = 0;
            for (auto vec : vectors)
            {
                double angle = dot_product(meanV, vec);
                if (angle>1.0)
                    angle = 1.0;
                if (angle<-1.0)
                    angle = -1.0;
                dev += acos(angle)*180/itk::Math::pi;
            }
            if (vectors.size()>0)
                dev /= vectors.size();

            if (dev<m_AngularDeviation)
            {
#pragma omp critical
              {
                vtkIdType id = vtkNewPoints->InsertNextPoint(vertices.at(j).data_block());
                container->GetPointIds()->InsertNextId(id);
              }
            }
            else
            {
                if (m_RemoveFibers)
                {
                    container = vtkSmartPointer<vtkPolyLine>::New();
                    break;
                }

                if (container->GetNumberOfPoints()>0)
                {
#pragma omp critical
                    vtkNewCells->InsertNextCell(container);
                }
                container = vtkSmartPointer<vtkPolyLine>::New();
            }
        }

#pragma omp critical
        {
          if (container->GetNumberOfPoints()>0)
              vtkNewCells->InsertNextCell(container);
        }
    }

    vtkSmartPointer<vtkPolyData> outputPoly = vtkSmartPointer<vtkPolyData>::New();
    outputPoly->SetPoints(vtkNewPoints);
    outputPoly->SetLines(vtkNewCells);
    m_OutputFiberBundle = FiberBundle::New(outputPoly);
}

}


// Copyright 2015-2020 Tier IV, Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     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.

#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/qi.hpp>
#include <functional>
#include <openscenario_interpreter/reader/evaluate.hpp>
#include <openscenario_interpreter/scope.hpp>
#include <openscenario_interpreter/syntax/parameter_type.hpp>
#include <openscenario_interpreter/utility/overload.hpp>

#if __cplusplus >= 201606
#include <variant>
#else
#include <boost/variant.hpp>
#endif

namespace openscenario_interpreter
{
inline namespace reader
{
struct Value
{
  using ExpressionType = Variant<int, unsigned int, unsigned short, double, bool>;
  ExpressionType data;

  struct Numeric;

  Value() : data(0) {}

  Value(const Value &) = default;

  Value(Value &&) = default;

  explicit Value(int v) : data(v) {}

  explicit Value(unsigned int v) : data(v) {}

  explicit Value(unsigned short v) : data(v) {}

  explicit Value(double v) : data(v) {}

  explicit Value(bool v) : data(v) {}

  ~Value() = default;

  auto operator=(const Value &) -> Value & = default;

  auto operator=(Value &&) -> Value & = default;

  auto type_name() const
  {
    return boost::core::demangle(visit([](auto v) { return typeid(v).name(); }, data));
  }

  template <typename T>
  bool same_as() const
  {
    return visit([](auto v) { return std::is_same<T, decltype(v)>::value; }, data);
  }

  template <typename T>
  Value cast() const;

  template <typename Signature>
  struct Visitor;

  template <typename R, typename... Args>
  struct Visitor<R(Args...)>
  {
    template <typename F>
    Value apply(F && f, const std::array<Value, sizeof...(Args)> & args)
    {
      return applyImpl(std::forward<F>(f), args, std::make_index_sequence<sizeof...(Args)>())
        .template cast<R>();
    }

  private:
    template <typename F, std::size_t... I>
    Value applyImpl(
      F && f, const std::array<Value, sizeof...(Args)> & args, std::index_sequence<I...>)
    {
      return visit(
        [&f](auto &&... v) { return Value{f(std::forward<decltype(v)>(v)...)}; },
        get_arg<I>(args)...);
    }

    template <std::size_t I>
    static decltype(auto) get_arg(const std::array<Value, sizeof...(Args)> & args)
    {
      using Ret = typename std::tuple_element<I, std::tuple<Args...>>::type;
      auto & arg = std::get<I>(args);
      return arg.template cast<Ret>().data;
    }
  };

#define DEFINE_UNARY(FNAME, FUNC, ARG, RET)                                    \
  Value FNAME() const                                                          \
  {                                                                            \
    return Visitor<RET(ARG)>{}.apply([](auto v) { return FUNC(v); }, {*this}); \
  }

#define DEFINE_BINARY(OP, FUNC, TYPE)                         \
  friend Value OP(const Value & lhs, const Value & rhs)       \
  {                                                           \
    return Visitor<TYPE(TYPE, TYPE)>{}.apply(                 \
      [](auto l, auto r) { return FUNC(l, r); }, {lhs, rhs}); \
  }

  // clang-format off
  DEFINE_UNARY(round, [](double v) { return static_cast<int>(std::round(v)); }, double, int)
  DEFINE_UNARY(floor, [](double v) { return static_cast<int>(std::floor(v)); }, double, int)
  DEFINE_UNARY(ceil,  [](double v) { return static_cast<int>(std::ceil(v)); },  double, int)
  DEFINE_UNARY(sqrt,  [](double v) { return static_cast<int>(std::sqrt(v)); },  double, int)
  // clang-format on
  DEFINE_UNARY(operator-, -, Numeric, Numeric)
  DEFINE_UNARY(operator!, !, bool, bool)

  DEFINE_BINARY(operator*, std::multiplies<void>{}, Numeric)
  DEFINE_BINARY(operator+, std::plus<void>{}, Numeric)
  DEFINE_BINARY(operator-, std::minus<void>{}, Numeric)
  DEFINE_BINARY(operator%, std::fmod, Numeric)
  DEFINE_BINARY(operator/, std::divides<double>{}, double)
  DEFINE_BINARY(operator&&, std::logical_and<bool>{}, bool)
  DEFINE_BINARY(operator||, std::logical_or<bool>{}, bool)
  DEFINE_BINARY(pow, std::pow, double)

  friend std::ostream & operator<<(std::ostream & os, const Value & v)
  {
    visit([&](const auto & v) mutable { os << v; }, v.data);
    return os;
  }

  friend bool operator==(const Value & lhs, const Value & rhs)
  {
    return visit(std::equal_to<void>{}, lhs.data, rhs.data);
  }
};

template <typename T>
auto Value::cast() const -> Value
{
  if (same_as<double>() and std::is_integral<T>::value)
    throw std::runtime_error{"double cannot convert to integer implicitly."};
  return Value{visit([](auto v) { return static_cast<T>(v); }, data)};
}

template <>
auto Value::cast<Value::Numeric>() const -> Value
{
  return same_as<bool>() ? throw std::runtime_error("boolean cannot convert to numeric.") : *this;
}

template <>
Value Value::cast<bool>() const
{
  return same_as<bool>() ? *this : throw std::runtime_error("numeric cannot convert to boolean.");
}

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

template <typename Iter>
struct Grammar : qi::grammar<Iter, Value(), ascii::space_type>
{
  Grammar(const openscenario_interpreter::Scope & scope) : Grammar::base_type(lv0)
  {
    using qi::_1;
    using qi::_2;
    using qi::_val;
    const static qi::real_parser<double, qi::strict_real_policies<double>> dbl;

    // clang-format off
    lv0 = lv1[_val = _1] >> *(qi::lit("or") >> lv1[_val = _val or _1]);
    lv1 = lv2[_val = _1] >> *(qi::lit("and") >> lv2[_val = _val and _1]);
    lv2 = lv3[_val = _1] >> *(('+' >> lv3[_val = _val + _1]) | ('-' >> lv3[_val = _val - _1]));
    lv3 = lv4[_val = _1] >> *(('*' >> lv4[_val = _val * _1]) | ('/' >> lv4[_val = _val / _1]) | ('%' >> lv4[_val = _val % _1]));
    lv4 = (qi::lit('-') >> lv5)[_val = -_1]
          | (qi::lit("not") >> lv5)[_val = !_1]
          | lv5[_val = _1];
    lv5 = (qi::lit("pow") >> '(' >> lv0 >> ',' >> lv0 >> ')')[_val = ph::bind([](auto &&x, auto &&y) { return pow(x, y); }, _1, _2)]
          | (qi::lit("round") >> '(' >> lv0 >> ')')[_val = ph::bind(&Value::round, _1)]
          | (qi::lit("floor") >> '(' >> lv0 >> ')')[_val = ph::bind(&Value::floor, _1)]
          | (qi::lit("ceil") >> '(' >> lv0 >> ')')[_val = ph::bind(&Value::ceil, _1)]
          | (qi::lit("sqrt") >> '(' >> lv0 >> ')')[_val = ph::bind(&Value::sqrt, _1)]
          | lv6[_val = _1];
    lv6 = ('(' >> lv0 >> ')')[_val = _1]
          | qi::lit("true")[_val = ph::construct<Value>(true)]
          | qi::lit("false")[_val = ph::construct<Value>(false)]
          | dbl[_val = ph::construct<Value>(_1)]
          | qi::int_[_val = ph::construct<Value>(_1)]
          | qi::lexeme[qi::lit('$') >> *qi::char_("A-Za-z0-9_")][_val = ph::bind(
              [scope](auto&& chars) {
                return toValue(std::string(chars.begin(), chars.end()), scope);
              }, _1)];
    // clang-format on
  }

  static auto toValue(const std::string & key, const Scope & scope) -> Value
  {
    auto found = scope.ref(key);
    if (not found) {
      THROW_SYNTAX_ERROR(std::quoted(key), "is not declared in this scope");
    }

    if (found.is<Integer>()) {
      return Value{static_cast<int>(found.as<Integer>())};
    } else if (found.is<UnsignedInt>()) {
      return Value{static_cast<unsigned int>(found.as<UnsignedInt>())};
    } else if (found.is<UnsignedShort>()) {
      return Value{static_cast<unsigned short>(found.as<UnsignedShort>())};
    } else if (found.is<Double>()) {
      return Value{found.as<Double>()};
    } else if (found.is<Boolean>()) {
      return Value{found.as<Boolean>()};
    } else {
      THROW_SYNTAX_ERROR(std::quoted(key), "is neither numeric nor boolean");
    }
  }

  qi::rule<Iter, Value(), ascii::space_type> lv0, lv1, lv2, lv3, lv4, lv5, lv6;
};

std::string evaluate(const std::string & expression, const Scope & scope)
{
  Grammar<decltype(expression.begin())> parser{scope};
  Value output;
  auto first = expression.begin();
  auto last = expression.end();

  qi::phrase_parse(first, last, parser, ascii::space, output);

  if (first != last) {
    THROW_SYNTAX_ERROR("Failed to parse ", std::quoted(expression));
  }

  return visit(
    overload(
      [](bool v) -> std::string { return v ? "true" : "false"; },
      [](auto v) -> std::string { return std::to_string(v); }),
    output.data);
}

}  // namespace reader
}  // namespace openscenario_interpreter