/**
 * \file TanFilter.cpp
 */

#include "TanFilter.h"

#include <cassert>
#include <cmath>

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

namespace ATK
{
  template<typename DataType_>
  TanFilter<DataType_>::TanFilter(int nb_channels)
  :Parent(nb_channels, nb_channels), coeff(1)
  {

  }

  template<typename DataType_>
  TanFilter<DataType_>::~TanFilter()
  {

  }

  template<typename DataType_>
  void TanFilter<DataType_>::setup()
  {
    coeff = boost::math::constants::pi<DataType_>() / input_sampling_rate;
  }

  template<typename DataType_>
  void TanFilter<DataType_>::process_impl(int64_t size) const
  {
    for(int channel = 0; channel < nb_input_ports; ++channel)
    {
      const DataType* ATK_RESTRICT input = converted_inputs[channel];
      DataType* ATK_RESTRICT output = outputs[channel];
      for(int64_t i = 0; i < size; ++i)
      {
        *(output++) = static_cast<DataType>(tan(*(input++) * coeff));
      }
    }
  }

  template class TanFilter<float>;
  template class TanFilter<double>;
}


#include <arm_neon.h>
#include <array>
#include <random>
#include <algorithm>
#include <iostream>
#include <cassert>
#include <cmath>
#include <functional>
#include <boost/align/aligned_allocator.hpp>
#include <utils.hpp>

using namespace std;
using dtype = int32_t;

const size_t MIN_TEST_VEC_SIZE = 32;
const size_t MAX_TEST_VEC_SIZE = 65536;
const dtype  MAX_ELEMENT_VALUE = 1000;

void bitonic_sort(int32_t *data_in, int32_t *data_out, int32_t n)
{
    static vector<int32_t> merged_0_1(MAX_TEST_VEC_SIZE);
    static vector<int32_t> merged_2_3(MAX_TEST_VEC_SIZE);

    constexpr int vector_width_in_bytes =  16; //quadword=128bit
    constexpr int vector_width =  vector_width_in_bytes / sizeof(data_in[0]);
    int n_div_vec_width= n / vector_width;
    int log2n = static_cast<int>(log2(n_div_vec_width));
    assert(exp2(log2n) == n_div_vec_width);

    for (auto i = 0; i < log2n ; ++i)
    {
        for (auto j = i ; j >= 0; --j)
        {
            int arrow_len = 1<<j;
            for (auto k = 0; k < n_div_vec_width/2; k++)
            {
                bool is_up =  k & (1<<i);

                uint32_t  mask = (1 << j) - 1;
                size_t upper_idx = ((k & ~mask) << 1) | (k & mask);
                size_t lower_idx = upper_idx + arrow_len;
                int32x4_t v_upper = vld1q_s32(&data_in[upper_idx * vector_width]);
                int32x4_t v_lower = vld1q_s32(&data_in[(upper_idx + arrow_len)*vector_width]);
                int32x4_t v_min = vminq_s32(v_upper, v_lower);
                int32x4_t v_max = vmaxq_s32(v_upper, v_lower);
                auto max_idx = (upper_idx + static_cast<int>(is_up) * arrow_len) * vector_width;
                auto min_idx = (upper_idx + static_cast<int>(!is_up) * arrow_len) * vector_width;
                vst1q_s32(&data_in[max_idx], v_max);
                vst1q_s32(&data_in[min_idx], v_min);
            }
        }
    }

    int32_t *to_merge[4] = {data_in, data_in+1, data_in+2, data_in+3};

    merge_two_cols(to_merge[0], to_merge[1], 4, merged_0_1.data(), n >> 2);
    merge_two_cols(to_merge[2], to_merge[3], 4, merged_2_3.data(), n >> 2);

    to_merge[0] = merged_0_1.data();
    to_merge[1] = merged_2_3.data();
    merge_two_cols(to_merge[0], to_merge[1], 1, data_out, n>>1 );
}

int main()
{
    for (auto vec_size = MIN_TEST_VEC_SIZE; vec_size <= MAX_TEST_VEC_SIZE; vec_size *= 2)
    {
        using aligned_vector = vector<dtype,boost::alignment::aligned_allocator<dtype, 128>>;
        aligned_vector vec(vec_size);
        randomize(vec, -MAX_ELEMENT_VALUE, MAX_ELEMENT_VALUE);
        aligned_vector vec_cpy{vec};
        aligned_vector bitonic_out(vec_size);
        int time_ref = measure_ms([&vec](){ std::sort(vec.begin(), vec.end(), greater<dtype>());});
        int time_bitonic = measure_ms([&vec_cpy, &bitonic_out ,vec_size](){ bitonic_sort(vec_cpy.data(), bitonic_out.data(), vec_size);});
        cout << "acceleration for " << vec_size << " elements = " << (float)time_ref/time_bitonic << endl;
        assert(is_sorted(vec, greater_equal<dtype>()));
        assert(is_sorted(vec_cpy, greater_equal<dtype>(), 4));
        //for (auto &el : bitonic_result) cout << el << " ";
        assert(is_sorted(bitonic_out, greater_equal<dtype>()));
    }
    return 0;
}


#include "incidencematrices.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <array>
#include <memory>

namespace IncidenceMatrices {

/** @brief Create the mesh consisting of a triangle and quadrilateral
 *         from the exercise sheet.
 * @return Shared pointer to the hybrid2d mesh.
 */
std::shared_ptr<lf::mesh::Mesh> createDemoMesh() {
  // builder for a hybrid mesh in a world of dimension 2
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Add points
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 0});    // (0)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 0});    // (1)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 1});    // (2)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 1});    // (3)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0.5, 1});  // (4)

  // Add the triangle
  // First set the coordinates of its nodes:
  Eigen::MatrixXd nodesOfTria(2, 3);
  nodesOfTria << 1, 1, 0.5, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kTria(),  // we want a triangle
      std::array<lf::mesh::Mesh::size_type, 3>{
          {1, 2, 4}},  // indices of the nodes
      std::make_unique<lf::geometry::TriaO1>(nodesOfTria));  // node coords

  // Add the quadrilateral
  Eigen::MatrixXd nodesOfQuad(2, 4);
  nodesOfQuad << 0, 1, 0.5, 0, 0, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kQuad(),
      std::array<lf::mesh::Mesh::size_type, 4>{{0, 1, 4, 3}},
      std::make_unique<lf::geometry::QuadO1>(nodesOfQuad));

  std::shared_ptr<lf::mesh::Mesh> demoMesh_p = mesh_factory_ptr->Build();

  return demoMesh_p;
}

/** @brief Compute the edge-vertex incidence matrix G for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The edge-vertex incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<int> computeEdgeVertexIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store edge-vertex incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> G;

#if SOLUTION
  // Mesh::NumEntities(unsigned codim) returns the number of elements
  // with given codimension. Codim(Edge) = 1, Codim(Node) = 2.
  const lf::mesh::Mesh::size_type numEdges = mesh.NumEntities(1),
                                  numNodes = mesh.NumEntities(2);
  // Following the demo for the reserve()-initialising the sparse matrix given
  // in the exercise sheet. From (2.1a) we know that G has exactly 2 entries
  // per row.
  G = Eigen::SparseMatrix<int, Eigen::RowMajor>(numEdges, numNodes);
  G.reserve(Eigen::VectorXi::Constant(numEdges, 2));

  // To compute G efficiently we iterate over all edges and check the index
  // of the nodes at its end. This is the efficient way to do the assembly,
  // introduced as "distribute scheme" in class. We cannot iterative over
  // vertices, because LehrFEM++ does not allow to visit the edges
  // adjacent to a vertex
  for (const lf::mesh::Entity *edge : mesh.Entities(1)) {
    // Get index of this edge
    lf::mesh::Mesh::size_type edgeIdx = mesh.Index(*edge);
    // Get the nodes and their indices.
    // Note, that seen from the edges the nodes have codim 1, not 2,
    // hence we call SubEntities(1). This is a relative codimension!
    auto nodes = edge->SubEntities(1);
    lf::mesh::Mesh::size_type firstNodeIdx = mesh.Index(*nodes[0]);
    lf::mesh::Mesh::size_type lastNodeIdx = mesh.Index(*nodes[1]);
    // Add the matrix entries according to the definition
    G.coeffRef(edgeIdx, firstNodeIdx) += 1;
    G.coeffRef(edgeIdx, lastNodeIdx) -= 1;
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  return G;
}
/* SAM_LISTING_END_1 */

/** @brief Compute the cell-edge incidence matrix D for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The cell-edge incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<int> computeCellEdgeIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store cell-edge incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> D;

#if SOLUTION
  // Mesh::NumEntities(unsigned codim) returns the number of elements
  // with given codimension. Codim(Edge) = 0, Codim(Node) = 1.
  const lf::mesh::Mesh::size_type numCells = mesh.NumEntities(0),
                                  numEdges = mesh.NumEntities(1);
  // Following the demo for the reserve()-initialising the sparse matrix given
  // in the exercise sheet. From (2.1a) we know that D has at most 4 entries
  // per row.
  D = Eigen::SparseMatrix<int, Eigen::RowMajor>(numCells, numEdges);
  D.reserve(Eigen::VectorXi::Constant(numCells, 4));

  // To compute D efficiently we iterate over all cells and check the
  // orientations (+1 or -1, same as in the definition of the matrix D)
  // of its edges. For this we may use RelativeOrientations().
  for (const lf::mesh::Entity *cell : mesh.Entities(0)) {
    // Get cell index
    lf::mesh::Mesh::size_type cellIdx = mesh.Index(*cell);
    // Get edges and their orientations (these already the entries for D!)
    auto edges = cell->SubEntities(1);
    auto edgeOrientations = cell->RelativeOrientations();

    // Iterate over both and add to D
    auto edgeIt = edges.begin();
    auto orntIt = edgeOrientations.begin();
    for (; edgeIt != edges.end() && orntIt != edgeOrientations.end();
         ++edgeIt, ++orntIt) {
      // Get the edge index and add its orientation to D
      lf::mesh::Mesh::size_type edgeIdx = mesh.Index(**edgeIt);
      D.coeffRef(cellIdx, edgeIdx) += lf::mesh::to_sign(*orntIt);
    }
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  return D;
}
/* SAM_LISTING_END_2 */

/** @brief For a given mesh test if the product of cell-edge and edge-vertex
 *        incidence matrix is zero: D*G == 0?
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *             such as lf::mesh::hybrid2d::Mesh)
 * @return true, if the product is zero and false otherwise
 */
/* SAM_LISTING_BEGIN_3 */
bool testZeroIncidenceMatrixProduct(const lf::mesh::Mesh &mesh) {
  bool isZero = false;

#if SOLUTION
  Eigen::SparseMatrix<int> G = computeEdgeVertexIncidenceMatrix(mesh),
                           D = computeCellEdgeIncidenceMatrix(mesh);

  Eigen::SparseMatrix<int> O = D * G;
  // Possibility 1:
  // Not prone to roundoff errors, since O is an integer matrix!
  isZero = O.norm() == 0;
  // Possibility 2: But this doesn't use the fact that O is sparse.
  // isZero = Eigen::MatrixXi(O).isZero(0);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return isZero;
}
/* SAM_LISTING_END_3 */

}  // namespace IncidenceMatrices


///////////////////////////////////////////////////////////////////////////////////////////////////
/// \file formic/utils/numeric.cpp
///
/// \brief   implementation file for miscellaneous functions related to numbers
///
///////////////////////////////////////////////////////////////////////////////////////////////////

#include "numeric.h"
#include "formic/utils/mpi_interface.h"

#include <boost/scoped_array.hpp>
#include <boost/format.hpp>

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   get an offset array used in compounding pairs of distinct indices
///
///   For two indices i,j with i < j, we have:    compound(i,j) = i + ioff[j];
///
/// \param[in]       n        desired length of the array
/// \param[in,out]   ioff     on exit, the offset array
///
///////////////////////////////////////////////////////////////////////////////////////////////////
void formic::get_pair_ioff(int n, std::vector<int> & ioff) {

  if (ioff.size() != n)
    ioff.resize(n);

  if ( n <= 0 )
    return;

  ioff.at(0) = 0;
  for (int i = 1; i < n; i++)
    ioff.at(i) = ioff.at(i-1) + i - 1;

}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   computes the binomial coefficient
///
/// \param[in]     n        number of things
/// \param[in]     m        how many things to take at a time
///
/// \return the number of ways n things can be taken m at a time
///
///////////////////////////////////////////////////////////////////////////////////////////////////
int formic::binom_coeff(int n, int m) {
  if (n < 0 || m < 0 || m > n) return 0;
  double retval = 1.0;
  while (m > 0) retval = ( retval * (n--) ) / (m--);
  return int(retval+0.5);
}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   Returns the number of solutions to the equation
///          x(1) + x(2) + ... + x(n) = r
///          when the variables x(i) are constrained to be
///          integers in the range (0, 1, 2, ..., k)
///
/// \param[in]     n        number of variables
/// \param[in]     r        sum of variables
/// \param[in]     k        range of each variable
/// \param[out]    work     integer workspace, either null or size >= k+1
///
///////////////////////////////////////////////////////////////////////////////////////////////////
int formic::n_integer_solutions(const int n, const int r, const int k, int * work) {

  assert( n >= 0 );
  assert( r >= 0 );
  assert( k >= 0 );

  // if requested, dynamically allocate the work space
  boost::scoped_array<int> dynamic_work;
  if (work == 0) {
    dynamic_work.reset( new int[k+1] );
    work = dynamic_work.get();
  }

  // initialize an array to hold the number of variables having each allowed value
  int * const n_with_value = work;
  work += (k+1);
  for (int i = 0; i <= k; i++)
    n_with_value[i] = 0;

  // initialize the return value
  int retval = 0;

  // Loop over all possible distributions of variables among the values.
  // Note that we do not directly track of how many variables are equal to zero,
  // as this is known by how many variables take on other values.
  while (true) {

    // compute the number of nonzero variables
    int n_nonzero = 0;
    for (int i = 1; i <= k; i++)
      n_nonzero += n_with_value[i];

    // compute the sum of the variables
    int sum = 0;
    for (int i = 1; i <= k; i++)
      sum += i * n_with_value[i];

    // if this distribution solves the equation, count how many ways it can occur
    if (sum == r && n_nonzero <= n) {

      // determine how many variables are nonzero
      int t = 0;
      for (int i = 1; i <= k; i++)
        t += n_with_value[i];

      // count how many ways the variables can satisfy this distribution
      int occurrences = formic::binom_coeff(n, t);
      for (int i = 1; i < k; i++) {
        occurrences *= formic::binom_coeff(t, n_with_value[i]);
        t -= n_with_value[i]; // t is now equal to the number of variables greater than i
      }

      // record how many ways the variables satisfy this distribution
      retval += occurrences;

    }

    // increment to the next distribution of variables
    int p;
    for (p = k; p > 0; p--)
      if (++n_with_value[p] > n)
        n_with_value[p] = 0;
      else
        break;

    // stop iterating if all distributions have been processed
    if (p == 0) break;

  }

  // return the result
  return retval;

}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   formats a real number into a string
///
/// \param[in]     f        the formatting string used by boost::format
/// \param[in]     value    the number to be formatted
///
/// \return the string containing the formatted number
///
///////////////////////////////////////////////////////////////////////////////////////////////////
std::string formic::format_number(const std::string & f, const double value) {
  return (boost::format(f) % value).str();
}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   formats a complex number into a string
///
/// \param[in]     f        the formatting string used by boost::format
/// \param[in]     value    the number to be formatted
///
/// \return the string containing the formatted number
///
///////////////////////////////////////////////////////////////////////////////////////////////////
std::string formic::format_number(const std::string & f, const std::complex<double> value) {
  std::string retval;
  retval.append("( ");
  retval.append( (boost::format(f) % value.real()).str() );
  retval.append(", ");
  retval.append( (boost::format(f) % value.imag()).str() );
  retval.append(" )");
  return retval;
}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   computes the unbiased estimate of a ratio of means:  <f>_p / <g>_p  in which the
///          numerator and denominator values are sampled from the same probability distribution p
///
/// \param[in]     n        the number of samples
/// \param[in]     p        the probability weight for each sample
/// \param[in]     f        the numerator samples
/// \param[in]     g        the denominator samples
/// \param[out]    r        on exit, the estimate of the ratio <f>_p / <g>_p
/// \param[out]    v        on exit, the estimate of the variance in the ratio
///
///////////////////////////////////////////////////////////////////////////////////////////////////
void formic::unbiased_ratio_of_means(const int n, const double * const p, const double * const f, const double * const g, double & r, double & v) {

  // compute the normalization, the numerator and denominator means, the means of the squares, and the mean of the products
  double nm = 0.0; // normalization constant
  double mf = 0.0; // mean of numerator
  double mg = 0.0; // mean of denominator
  double sf = 0.0; // mean of the square of the numerator terms
  double sg = 0.0; // mean of the square of the denominator terms
  double mp = 0.0; // mean of the product of numerator times denominator
  for (int i = 0; i < n; i++) {
    nm += p[i];
    double x = p[i] * f[i];
    mf += x;
    sf += x * f[i];
    mp += x * g[i];
    x = p[i] * g[i];
    mg += x;
    sg += x * g[i];
  }
  mf /= nm;
  mg /= nm;
  sf /= nm;
  sg /= nm;
  mp /= nm;

  // compute the numerator and denominator variances and the covariance
  const double vf = ( sf - mf * mf ) * double(n) / double(n-1);
  const double vg = ( sg - mg * mg ) * double(n) / double(n-1);
  const double cv = ( mp - mf * mg ) * double(n) / double(n-1);

  // compute the unbiased estimate of the ratio of means
  r = ( mf / mg ) / ( 1.0 + ( vg / mg / mg - cv / mf / mg ) / double(n) );

  // compute the unbiased estimate of the variance of the ratio of means
  v = ( mf * mf / mg / mg / double(n) ) * ( vf / mf / mf + vg / mg / mg - 2.0 * cv / mf / mg );

}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   computes the unbiased estimate of a ratio of means:  <f>_p / <g>_p  in which the
///          numerator and denominator values are sampled from the same probability distribution p
///          and samples are combined across all processors
///
/// \param[in]     n        the number of samples on this process
/// \param[in]     p        the probability weight for each sample
/// \param[in]     f        the numerator samples
/// \param[in]     g        the denominator samples
/// \param[out]    r        on exit, the estimate of the ratio <f>_p / <g>_p
/// \param[out]    v        on exit, the estimate of the variance in the ratio
///
///////////////////////////////////////////////////////////////////////////////////////////////////
void formic::mpi_unbiased_ratio_of_means(const int n, const double * const p, const double * const f, const double * const g, double & r, double & v) {

  // compute the normalization, the numerator and denominator means, the means of the squares, and the mean of the products
  double y[7];
  y[0] = 0.0; // normalization constant
  y[1] = 0.0; // mean of numerator
  y[2] = 0.0; // mean of denominator
  y[3] = 0.0; // mean of the square of the numerator terms
  y[4] = 0.0; // mean of the square of the denominator terms
  y[5] = 0.0; // mean of the product of numerator times denominator
  y[6] = double(n); // number of samples
  for (int i = 0; i < n; i++) {
    y[0] += p[i];
    double x = p[i] * f[i];
    y[1] += x;
    y[3] += x * f[i];
    y[5] += x * g[i];
    x = p[i] * g[i];
    y[2] += x;
    y[4] += x * g[i];
  }
  double z[7];
  formic::mpi::allreduce(&y[0], &z[0], 7, MPI_SUM);
  const double mf = z[1] / z[0]; // mean of numerator
  const double mg = z[2] / z[0]; // mean of denominator
  const double sf = z[3] / z[0]; // mean of the square of the numerator terms
  const double sg = z[4] / z[0]; // mean of the square of the denominator terms
  const double mp = z[5] / z[0]; // mean of the product of numerator times denominator
  const double ns = z[6];        // number of samples

  // compute the numerator and denominator variances and the covariance
  const double vf = ( sf - mf * mf ) * ns / ( ns - 1.0 );
  const double vg = ( sg - mg * mg ) * ns / ( ns - 1.0 );
  const double cv = ( mp - mf * mg ) * ns / ( ns - 1.0 );

  // compute the unbiased estimate of the ratio of means
  r = ( mf / mg ) / ( 1.0 + ( vg / mg / mg - cv / mf / mg ) / ns );

  // compute the unbiased estimate of the variance of the ratio of means
  v = ( mf * mf / mg / mg ) * ( vf / mf / mf + vg / mg / mg - 2.0 * cv / mf / mg );

}


#ifndef nomad__src__autodiff__second_order_hpp
#define nomad__src__autodiff__second_order_hpp

#include <iomanip>
#include <string>
#include <Eigen/Core>

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

namespace nomad {

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

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

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

    reset();

    eigen_idx_t d = x.size();

    try {

      auto f_var = functional(x);

      f = f_var.first_val();

      // First-order
      first_order_reverse_adj(f_var);

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

      // Second-order
      for (eigen_idx_t i = 0; i < d; ++i) {

        for (eigen_idx_t j = 0; j < d; ++j)
        var_nodes_[j + 1].second_val() = static_cast<double>(i == j);

        second_order_forward_val(f_var);
        second_order_reverse_adj(f_var);

        for (eigen_idx_t j = 0; j < d; ++j)
        H(i, j) = var_nodes_[j + 1].second_grad();

      }

      reset();

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

  }

  template <typename F>
  void hessian(const F& functional,
               const Eigen::VectorXd& x,
               Eigen::MatrixXd& H) {
    double f;
    Eigen::VectorXd g(x.size());
    hessian(functional, x, f, g, H);
  }

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

    eigen_idx_t d = x.size();

    Eigen::VectorXd x_dynam(x);
    Eigen::VectorXd g_auto(d);

    for (eigen_idx_t i = 0; i < d; ++i) {

      Eigen::VectorXd g_diff = Eigen::VectorXd::Zero(d);

      x_dynam(i) += epsilon;
      gradient(functional, x_dynam, g_auto);
      g_diff += g_auto;

      x_dynam(i) -= 2.0 * epsilon;
      gradient(functional, x_dynam, g_auto);
      g_diff -= g_auto;

      x_dynam(i) += epsilon;
      g_diff /= 2.0 * epsilon;

      H.col(i) = g_diff;

    }

  }

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

    eigen_idx_t d = x.size();

    Eigen::MatrixXd auto_H(x.size(), x.size());
    try {
      hessian(functional, x, auto_H);
    } catch (nomad_error& e) {
      std::cout << "Cannot compute Hessian Test" << std::endl;
      throw e;
    }

    Eigen::MatrixXd diff_H(x.size(), x.size());
    try {
      finite_diff_hessian(functional, x, diff_H, epsilon);
    } catch (nomad_error& e) {
      std::cout << "Cannot compute Hessian Test" << std::endl;
      throw e;
    }

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

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

    for (eigen_idx_t i = 0; i < d; ++i) {
      for (eigen_idx_t j = 0; j < d; ++j) {
        std::cout << "    "
                  << std::setw(width) << std::left << i
                  << std::setw(width) << std::left << j
                  << std::setw(width) << std::left << auto_H(i, j)
                  << std::setw(width) << std::left << diff_H(i, j)
                  << std::setw(width) << std::left
                  << (auto_H(i, j) - diff_H(i, j)) / (epsilon * epsilon)
                  << std::endl;
      }
    }

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

  }

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

    reset();

    eigen_idx_t d = x.size();

    try {

      auto f_var = functional(x);

      f = f_var.first_val();

      // First-order
      first_order_reverse_adj(f_var);

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

      // Second-order
      for (eigen_idx_t i = 0; i < d; ++i)
      var_nodes_[i + 1].second_val() = v(i);

      second_order_forward_val(f_var);
      second_order_reverse_adj(f_var);

      for (eigen_idx_t i = 0; i < d; ++i)
      hessian_dot_v(i) = var_nodes_[i + 1].second_grad();

      reset();

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

  }

  template <typename F>
  void hessian_dot_vector(const F& functional,
                          const Eigen::VectorXd& x,
                          const Eigen::VectorXd& v,
                          Eigen::VectorXd& hessian_dot_v) {
    double f;
    Eigen::VectorXd g(x.size());
    hessian_dot_vector(functional, x, v, f, g, hessian_dot_v);
  }

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

    Eigen::MatrixXd H_auto(x.size(), x.size());
    try {
      hessian(functional, x, H_auto);
    } catch (nomad_error& e) {
      std::cout << "Cannot compute Hessian Doc Vector Test" << std::endl;
      throw e;
    }

    Eigen::VectorXd H_dot_v_auto(x.size());
    try {
      hessian_dot_vector(functional, x, v, H_dot_v_auto);
    } catch (nomad_error& e) {
      std::cout << "Cannot compute Hessian Doc Vector Test" << std::endl;
      throw e;
    }

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

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

    Eigen::VectorXd H_dot_v = H_auto * v;

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

      std::cout << "    "
                << std::setw(width) << std::left << i
                << std::setw(width) << std::left << H_dot_v_auto(i)
                << std::setw(width) << std::left << H_dot_v(i)
                << std::endl;

    }

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

  }

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

    reset();

    eigen_idx_t d = x.size();

    try {

      auto f_var = functional(x);

      f = f_var.first_val();

      // First-order
      first_order_reverse_adj(f_var);

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

      // Second-order
      trace_m_times_h = 0;

      for (eigen_idx_t i = 0; i < d; ++i) {

        for (eigen_idx_t j = 0; j < d; ++j)
        var_nodes_[j + 1].second_val() = M(j, i);

        second_order_forward_val(f_var);
        second_order_reverse_adj(f_var);

        trace_m_times_h += var_nodes_[i + 1].second_grad();

      }

      reset();

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

  }

  template <typename F>
  void trace_matrix_times_hessian(const F& functional,
                                  const Eigen::VectorXd& x,
                                  const Eigen::MatrixXd& M,
                                  double& trace_m_times_h) {
    double f;
    Eigen::VectorXd g(x.size());
    trace_matrix_times_hessian(functional, x, M, f, g, trace_m_times_h);
  }

  template <typename F>
  void test_trace_matrix_times_hessian(const F& functional,
                                       const Eigen::VectorXd& x,
                                       const Eigen::MatrixXd& M) {

    Eigen::MatrixXd H_auto(x.size(), x.size());
    try {
      hessian(functional, x, H_auto);
    } catch (nomad_error& e) {
      std::cout << "Cannot compute Trace Matrix Times Hessian Test" << std::endl;
      throw e;
    }

    double trace_m_times_h = (M * H_auto).trace();

    double trace_m_times_h_auto;
    try {
      trace_matrix_times_hessian(functional, x, M, trace_m_times_h_auto);
    } catch (nomad_error& e) {
      std::cout << "Cannot compute Trace Matrix Times Hessian Test" << std::endl;
      throw e;
    }

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

    std::cout << "Trace Matrix Times Hessian Test:" << std::endl;
    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;
    std::cout << "    "
              << std::setw(width) << std::left << "Automatic"
              << std::setw(width) << std::left << "Exact"
              << std::endl;
    std::cout << "    "
              << std::setw(width) << std::left << "Derivative"
              << std::setw(width) << std::left << ""
              << std::endl;
    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;

    std::cout << "    "
              << std::setw(width) << std::left << trace_m_times_h_auto
              << std::setw(width) << std::left << trace_m_times_h
              << std::endl;

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

  }

}

#endif


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

//! \file
//!\brief Tests for the transcendental logarithm function of (fixed_point) for a tiny digit range.

#define BOOST_TEST_MODULE test_negatable_func_log_tiny
#define BOOST_LIB_DIAGNOSTIC

#include <cmath>

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  template<typename FixedPointType>
  const FixedPointType& tolerance_maker(const int fuzzy_bits)
  {
    static const FixedPointType the_tolerance = ldexp(FixedPointType(1), FixedPointType::resolution + fuzzy_bits);

    return the_tolerance;
  }

  template<typename FixedPointType,
           typename FloatPointType = typename FixedPointType::float_type>
  void test_log(const int fuzzy_bits)
  {
    // Use at least 10 resolution bits.
    // Use at least  5 range bits.

    BOOST_STATIC_ASSERT(-FixedPointType::resolution >= 10);
    BOOST_STATIC_ASSERT( FixedPointType::range      >=  4);

    const FixedPointType a1(+2L    );                                      const FloatPointType b1(+2L    );
    const FixedPointType a2(+3L    );                                      const FloatPointType b2(+3L    );
    const FixedPointType a3(+4.375L);                                      const FloatPointType b3(+4.375L);
    const FixedPointType a4(+1.125L);                                      const FloatPointType b4(+1.125L);
    const FixedPointType a5(+0.125L);                                      const FloatPointType b5(+0.125L);
    const FixedPointType a6(+0.875L);                                      const FloatPointType b6(+0.875L);
    const FixedPointType a7(FixedPointType( 1) /  3);                      const FloatPointType b7(FloatPointType( 1) /  3);
    const FixedPointType a8(FixedPointType(13) / 10);                      const FloatPointType b8(FloatPointType(13) / 10);
    const FixedPointType a9(boost::math::constants::pi<FixedPointType>()); const FloatPointType b9(boost::math::constants::pi<FloatPointType>());

    using std::log;

    BOOST_CHECK_CLOSE_FRACTION(log(a1), FixedPointType(log(b1)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(log(a2), FixedPointType(log(b2)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(log(a3), FixedPointType(log(b3)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(log(a4), FixedPointType(log(b4)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(log(a5), FixedPointType(log(b5)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(log(a6), FixedPointType(log(b6)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(log(a7), FixedPointType(log(b7)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(log(a8), FixedPointType(log(b8)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(log(a9), FixedPointType(log(b9)), tolerance_maker<FixedPointType>(fuzzy_bits));
  }
}

BOOST_AUTO_TEST_CASE(test_negatable_func_log_tiny)
{
  typedef boost::fixed_point::negatable<4, -11> fixed_point_type;

  local::test_log<fixed_point_type>(3);
}


//
// Created by keszocze on 10.10.18.
//

#include <boost/multiprecision/cpp_int.hpp>
#include <catch2/catch.hpp>
#include <cudd/cplusplus/cuddObj.hh>
#include <cudd_helpers.hpp>
#include <simple.hpp>
#include <from_papers.hpp>

#include <iostream>
#include <approximation_operators.hpp>

/**
 * @brief Hardcoded example from the 2016 ASP-DAC paper by Soeken et al.
 */
TEST_CASE("Example 3 from ASP-DAC 2016 paper") {
    Cudd mgr(2);

    std::vector<BDD> fun = abo::example_bdds::example1a(mgr);
    std::vector<BDD> expected_rounded_down = abo::example_bdds::example1b(mgr);
    std::vector<BDD> expected_rounded_up = abo::example_bdds::example1c(mgr);
    std::vector<BDD> expected_rounded = abo::example_bdds::example1d(mgr);

    std::vector<BDD> self_rounded_up, self_rounded_down, self_rounded;

    for (const BDD &b : fun) {
        self_rounded_down.push_back(abo::operators::round_down(mgr, b, 2, 4));
        self_rounded_up.push_back(abo::operators::round_up(mgr, b, 2, 4));
        self_rounded.push_back(abo::operators::round_bdd(mgr, b, 2));
    }

    CHECK(expected_rounded_down == self_rounded_down);

    CHECK(expected_rounded_up == self_rounded_up);

    CHECK(expected_rounded == self_rounded);
}


#include <armadillo>

void filter_active(mat &m, ivec active) {
  mat _m(sum(active), m.n_cols);
  int ind = 0;
  for (int i = 0; i < (int)active.n_elem; i++) {
    if (active(i)) {
      _m.row(ind) = m.row(ind);
      ind++;
    }
  }
  m = _m;
}

void tag_observe_pos(vec obs, mat pos, vec &mu, mat &sigma) {
  int k = obs.n_elem;
  if (k < 2) {
    return vec(pos.n_cols, fill::zeros); // this will be the only strange position
  }

  // compute the pose from the linear regression solver
  mat obs_k = repmat(obs(span(k-1, k-1)), k-1, 1);
  mat obs = obs(span(0, k-2));
  mat pos_k = repmat(pos(span(k-1, k-1), span::all), k-1, 1);
  mat pos = pos_(span(0, k-2), span::all);

  mat A = 2 * pos - repmat(pos_k, pos.n_rows, 1);
  mat b = obs % obs - obs_k % obs_k - sum(pos % pos, 1) + sum(pos_k % pos_k, 1);

  mat x = pinv(A.t() * A) * A.t() * b;
  mu = x.col(0);

  mat diff = repmat(z.t(), pos.n_rows, 1) + pos;
  diff = sqrt(sum(diff % diff, 1));
  diff -= obs;
  sigma = diff.t() * diff / (double)diff.n_rows;
}

void tag_observe_theta(vec t, mat pos, vec z, vec &mu, mat &sigma) {
  pos -= z;
  vec theta = atan2(pos(1), pos(0));
  vec dt = theta - t;
  mu = mean(dt);
  vec diff = ones<vec>(dt.n_elem) * mu - dt;
  sigma = dot(diff, diff) / (double)diff.n_elem;
}

void tag_observe(mat obs, mat pos, ivec active, vec &mu, mat &sigma) {
  // filter both the observation and the position
  filter_active(obs, active);
  filter_active(pos, active);
  // get the obs and pos and put it into their respective functions
  vec posmu;
  mat possigma;
  vec thetamu;
  mat thetasigma;
  tag_observe_pos(obs.col(0), pos, posmu, possigma);
  tag_observe_theta(obs.col(1), pos, thetamu, thetasigma);
  // assign the mus and sigmas
  mu = vec({ posmu(0), posmu(1), thetamu });
  sigma = mat(3, 3, fill::zeros);
  sigma(span(0,1), span(0,1)) = possigma;
  sigma(2, 2) = thetasigma;
}

void tag_generate(mat


/**
 * @file mean_shift.hpp
 * @author Shangtong Zhang
 *
 * Mean Shift clustering
 */

#ifndef MLPACK_METHODS_MEAN_SHIFT_MEAN_SHIFT_HPP
#define MLPACK_METHODS_MEAN_SHIFT_MEAN_SHIFT_HPP

#include <mlpack/core.hpp>
#include <mlpack/core/kernels/gaussian_kernel.hpp>
#include <mlpack/core/kernels/kernel_traits.hpp>
#include <mlpack/core/metrics/lmetric.hpp>
#include <boost/utility.hpp>

namespace mlpack {
namespace meanshift /** Mean shift clustering. */ {

/**
 * This class implements mean shift clustering.  For each point in dataset,
 * apply mean shift algorithm until maximum iterations or convergence.  Then
 * remove duplicate centroids.
 *
 * A simple example of how to run mean shift clustering is shown below.
 *
 * @code
 * extern arma::mat data; // Dataset we want to run mean shift on.
 * arma::Col<size_t> assignments; // Cluster assignments.
 * arma::mat centroids; // Cluster centroids.
 *
 * MeanShift<> meanShift();
 * meanShift.Cluster(dataset, assignments, centroids);
 * @endcode
 *
 * @tparam UseKernel Use kernel or mean to calculate new centroid.
 *         If false, KernelType will be ignored.
 * @tparam KernelType The kernel to use.
 * @tparam MatType The type of matrix the data is stored in.
 */
template<bool UseKernel = false,
         typename KernelType = kernel::GaussianKernel,
         typename MatType = arma::mat>
class MeanShift
{
 public:
  /**
   * Create a mean shift object and set the parameters which mean shift will be
   * run with.
   *
   * @param radius If distance of two centroids is less than it, one will be
   *      removed. If this value isn't positive, an estimation will be given
   *      when clustering.
   * @param maxIterations Maximum number of iterations allowed before giving up
   *      iterations will terminate.
   * @param kernel Optional KernelType object.
   */
  MeanShift(const double radius = 0,
            const size_t maxIterations = 1000,
            const KernelType kernel = KernelType());

  /**
   * Give an estimation of radius based on given dataset.
   *
   * @param data Dataset for estimation.
   * @param ratio Percentage of dataset to use for nearest neighbor search.
   */
  double EstimateRadius(const MatType& data, const double ratio = 0.2);

  /**
   * Perform mean shift clustering on the data, returning a list of cluster
   * assignments and centroids.
   *
   * @tparam MatType Type of matrix.
   * @param data Dataset to cluster.
   * @param assignments Vector to store cluster assignments in.
   * @param centroids Matrix in which centroids are stored.
   */
  void Cluster(const MatType& data,
               arma::Col<size_t>& assignments,
               arma::mat& centroids,
               bool useSeeds = true);

  //! Get the maximum number of iterations.
  size_t MaxIterations() const { return maxIterations; }
  //! Set the maximum number of iterations.
  size_t& MaxIterations() { return maxIterations; }

  //! Get the radius.
  double Radius() const { return radius; }
  //! Set the radius.
  void Radius(double radius);

  //! Get the kernel.
  const KernelType& Kernel() const { return kernel; }
  //! Modify the kernel.
  KernelType& Kernel() { return kernel; }

 private:
  /**
   * To speed up, we can generate some seeds from data set and use
   * them as initial centroids rather than all the points in the data set.  The
   * basic idea here is that we will place our points into hypercube bins of
   * side length binSize, and any bins that contain fewer than minFreq points
   * will be removed as possible seeds.  Usually, 1 is a sufficient parameter
   * for minFreq, and the bin size can be set equal to the estimated radius.
   *
   * @param data The reference data set.
   * @param binSize Width of hypercube bins.
   * @param minFreq Minimum number of points in bin.
   * @param seed Matrix to store generated seeds in.
   */
  void GenSeeds(const MatType& data,
                const double binSize,
                const int minFreq,
                MatType& seeds);

  /**
   * Use kernel to calculate new centroid given dataset and valid neighbors.
   *
   * @param data The whole dataset
   * @param neighbors Valid neighbors
   * @param distances Distances to neighbors
   # @param centroid Store calculated centroid
   */
  template<bool ApplyKernel = UseKernel>
  typename std::enable_if<ApplyKernel, bool>::type
  CalculateCentroid(const MatType& data,
                    const std::vector<size_t>& neighbors,
                    const std::vector<double>& distances,
                    arma::colvec& centroid);

  /**
   * Use mean to calculate new centroid given dataset and valid neighbors.
   *
   * @param data The whole dataset
   * @param neighbors Valid neighbors
   * @param distances Distances to neighbors
   # @param centroid Store calculated centroid
   */
  template<bool ApplyKernel = UseKernel>
  typename std::enable_if<!ApplyKernel, bool>::type
  CalculateCentroid(const MatType& data,
                    const std::vector<size_t>& neighbors,
                    const std::vector<double>&, /*unused*/
                    arma::colvec& centroid);

  /**
   * If distance of two centroids is less than radius, one will be removed.
   * Points with distance to current centroid less than radius will be used
   * to calculate new centroid.
   */
  double radius;

  //! Maximum number of iterations before giving up.
  size_t maxIterations;

  //! Instantiated kernel.
  KernelType kernel;
};

} // namespace meanshift
} // namespace mlpack

// Include implementation.
#include "mean_shift_impl.hpp"

#endif // MLPACK_METHODS_MEAN_SHIFT_MEAN_SHIFT_HPP


#include "TriMesh.h"
#include <Eigen/Core>

namespace geomlib {
TriMesh::TriMesh(const Eigen::MatrixXf& vertices, const Eigen::MatrixXi& faces)
    : vertices_{vertices}, faces_{faces} {}

const Eigen::MatrixXf& TriMesh::GetVertexNormals() const {
  if (!vertex_normals_) {
    CalculateVertexNormals();
  }
  return *vertex_normals_;
}

const Eigen::MatrixXf& TriMesh::GetFaceNormals() const {
  if (!face_normals_) {
    CalculateFaceNormals();
  }
  return *face_normals_;
}

const Eigen::VectorXf& TriMesh::GetFaceAreas() const {
  if (!face_areas_) {
    CalculateFaceAreas();
  }
  return *face_areas_;
}

const Eigen::MatrixXf& TriMesh::GetCotangentWeights() const {
  if (!cotangent_weights_) {
    CalculateCotangentWeights();
  }
  return *cotangent_weights_;
}

const Eigen::VectorXf& TriMesh::GetVertexAreas() const {
  if (!vertex_areas_) {
    CalculateVertexAreas();
  }
  return *vertex_areas_;
}

const Eigen::MatrixXi& TriMesh::GetAdjacentFacePairs() const {
  if (!adjacent_face_pairs_) {
    BuildAdjacentFacePairs();
  }
  return *adjacent_face_pairs_;
}

void TriMesh::CalculateVertexNormals() const {
  CalculateFaceNormals();
  vertex_normals_ = std::make_unique<Eigen::MatrixXf>(
      Eigen::MatrixXf::Zero(GetNumVertices(), 3));
  for (int i = 0; i < static_cast<int>(GetNumFaces()); i++) {
    int v1 = faces_(i, 0);
    int v2 = faces_(i, 1);
    int v3 = faces_(i, 2);
    Eigen::Vector3f n = face_normals_->row(i);
    vertex_normals_->row(v1) += n;
    vertex_normals_->row(v2) += n;
    vertex_normals_->row(v3) += n;
  }

  for (int i = 0; i < static_cast<int>(GetNumVertices()); i++) {
    vertex_normals_->row(i).normalize();
  }
}

void TriMesh::CalculateFaceNormals() const {
  face_normals_ = std::make_unique<Eigen::MatrixXf>(GetNumFaces(), 3);
  for (int i = 0; i < GetNumFaces(); i++) {
    int v1 = faces_(i, 0);
    int v2 = faces_(i, 1);
    int v3 = faces_(i, 2);
    Eigen::Vector3f p1 = vertices_.row(v1);
    Eigen::Vector3f p2 = vertices_.row(v2);
    Eigen::Vector3f p3 = vertices_.row(v3);
    face_normals_->row(i) = (p2 - p1).cross(p3 - p1);
  }
}

void TriMesh::CalculateFaceAreas() const {
  auto& face_normals = GetFaceNormals();
  face_areas_ = std::make_unique<Eigen::VectorXf>(GetNumFaces());
  for (int k = 0; k < GetNumFaces(); k++) {
    (*face_areas_)(k) = face_normals.row(k).norm() / 2;
  }
}

void TriMesh::CalculateCotangentWeights() const {
  Eigen::MatrixXf p1(faces_.rows(), 3);
  Eigen::MatrixXf p2(faces_.rows(), 3);
  Eigen::MatrixXf p3(faces_.rows(), 3);
  for (int i = 0; i < faces_.rows(); i++) {
    p1.row(i) = vertices_.row(faces_(i, 0));
    p2.row(i) = vertices_.row(faces_(i, 1));
    p3.row(i) = vertices_.row(faces_(i, 2));
  }

  Eigen::VectorXf l1 = (p2 - p3).rowwise().norm();
  Eigen::VectorXf l2 = (p1 - p3).rowwise().norm();
  Eigen::VectorXf l3 = (p1 - p2).rowwise().norm();
  Eigen::VectorXf s = (l1 + l2 + l3) / 2;
  Eigen::VectorXf r = (s - l1)
                          .cwiseProduct(s - l2)
                          .cwiseProduct(s - l3)
                          .cwiseQuotient(s)
                          .cwiseSqrt();

  auto fn = [&](const Eigen::VectorXf& l) -> Eigen::VectorXf {
    return ((s - l).cwiseAbs2() - r.cwiseAbs2())
        .cwiseQuotient(2 * (s - l).cwiseProduct(r));
  };

  cotangent_weights_ = std::make_unique<Eigen::MatrixXf>(GetNumFaces(), 3);
  cotangent_weights_->col(0) = fn(l1);
  cotangent_weights_->col(1) = fn(l2);
  cotangent_weights_->col(2) = fn(l3);
}

void TriMesh::CalculateVertexAreas() const {
  vertex_areas_ = std::make_unique<Eigen::VectorXf>(
      Eigen::VectorXf::Zero(GetNumVertices()));
  for (int k = 0; k < GetNumFaces(); k++) {
    Eigen::Vector3f n = face_normals_->row(k);
    float a = n.norm() / 6;
    for (int i = 0; i < 3; i++) {
      (*vertex_areas_)(faces_(k, i)) += a;
    }
  }
}

void TriMesh::BuildAdjacentFacePairs() const {
  std::vector<Vector2i> face_pairs;
  std::unordered_map<Vector2i, int, Vector2iHasher> edge_neighbor_;

  for (int i = 0; i < GetNumFaces(); i++) {
    for (int k = 0; k < 3; k++) {
      int u = faces_(i, k);
      int v = faces_(i, (k + 1) % 3);

      if (edge_neighbor_.count({v, u})) {
        face_pairs.emplace_back(i, edge_neighbor_[{v, u}]);
      } else {
        edge_neighbor_.emplace(Vector2i{u, v}, i);
      }
    }
  }

  adjacent_face_pairs_ =
      std::make_unique<Eigen::MatrixXi>(ArrayVector2iToMatrixXi(face_pairs));
}
}  // namespace geomlib


#include <iostream>
#include <stdlib.h>
#include <string>
#include <vector>

#include <Eigen/Dense>

using namespace std;

class Node {
 public:
  Node(string name) : name_(name) { }
  virtual ~Node() { cout << "Node destructor" << endl; }
  void AddParent(Node *parent) {
    parents_.push_back(parent);
    parent->children_.push_back(this);
    parent->pindex_.push_back(parents_.size() - 1);
  }
  Node *Parent(size_t i) const { return parents_[i]; }
  Node *Child(size_t i) const { return children_[i]; }
  size_t NumParents() const { return parents_.size(); }
  size_t NumChildren() const { return children_.size(); }
  string name() const { return name_; }
  size_t pindex(size_t i) const { return pindex_[i]; }
  void NullifyParent(size_t i) { parents_[i] = nullptr; }
  void DeleteAscendantsAndSelf() { DeleteAscendantsAndThis(this); }
  void DeleteAscendantsAndThis(Node *node) {
    for (int i = node->NumParents() - 1; i >= 0; --i) {
      if (node->Parent(i) != nullptr) {
        DeleteAscendantsAndThis(node->Parent(i));
      }
    }
    cout << "Nullifying " << node->name() << " from its children: ";
    for (size_t i = 0; i < node->NumChildren(); ++i) {
      cout <<  node->Child(i)->name() << " ";
      node->Child(i)->NullifyParent(node->pindex(i));
    }
    cout << endl;

    delete node;
  }
  vector<size_t> pindex_;
 protected:
  string name_;
  vector<Node *> parents_;
  vector<Node *> children_;

};

class Variable: public Node {
 public:
  Variable(string name) : Node(name) { }
  ~Variable() { }
  Variable *Parent(size_t i) {
    return static_cast<Variable *>(Node::Parent(i));
  }
  virtual void forward() = 0;
  virtual void backward() = 0;
  size_t NumRows() { return gradient_.rows(); }
  size_t NumColumns() { return gradient_.cols(); }
  virtual Eigen::MatrixXd *value() { return &value_; }
  virtual Eigen::MatrixXd *gradient() { return &gradient_; }
  void make_final() { gradient_ = Eigen::MatrixXd::Ones(1, 1); }
 protected:
  Eigen::MatrixXd value_;
  Eigen::MatrixXd gradient_;
};

struct Input: public Variable {
  Input(string name, Eigen::MatrixXd *input) : Variable(name) {
    input_ = input;
    gradient_ = Eigen::MatrixXd::Zero(input->rows(), input->cols());
  }
  ~Input() { cout << "Deleting " << name_ << endl; }
  Eigen::MatrixXd *value() override { return input_; }
  void forward() override { }
  void backward() override { }
 protected:
  Eigen::MatrixXd *input_;
};

struct Add: public Variable {
  Add(string name, Variable *X, Variable *Y) : Variable(name) {
    AddParent(X);
    AddParent(Y);
    gradient_ = Eigen::MatrixXd::Zero(X->NumRows(), X->NumColumns());

  }
  ~Add() { cout << "Deleting " << name_ << endl; }
  void forward() override {
    Parent(0)->forward();
    Parent(1)->forward();
    value_ = *Parent(0)->value() + *Parent(1)->value();
  }
  void backward() override {
    cout << name_ << " has parents " << Parent(0)->name() << " and "
         << Parent(1)->name() << ", adding "  << gradient_
         << " to each" << endl;
    *Parent(0)->gradient() += gradient_;  // dA = dC
    *Parent(1)->gradient() += gradient_;  // dB = dC
    cout << "recursively calling backward from " << name_
         << " on its first parent " << Parent(0)->name() << endl;
    Parent(0)->backward();
    if (Parent(0) != Parent(1)) {
    cout << "recursively calling backward from " << name_
         << " on its second parent " << Parent(1)->name() << endl;
      Parent(1)->backward();
    }
  }
};

int main() {
  Eigen::MatrixXd x_value(1, 1);
  x_value << 1.0;
  Eigen::MatrixXd y_value(1, 1);
  y_value << 2.0;
  Input *x = new Input("x", &x_value);
  Input *y = new Input("y", &y_value);
  Add *z = new Add("z", x, y);

  Add *q = new Add("q", z, x);
  Add *l = new Add("l", q, q);
  l->make_final();
  l->forward();
  l->backward();
  cout << endl;
  cout << "x = " << *x->value() << endl;
  cout << "dx = " << *x->gradient() << endl;
  cout << endl;
  cout << "y = " << *y->value() << endl;
  cout << "dy = " << *y->gradient() << endl;
  cout << endl;
  cout << "z = " << *z->value() << endl;
  cout << "dz = " << *z->gradient() << endl;
  cout << endl;
  cout << "q = " << *q->value() << endl;
  cout << "dq = " << *q->gradient() << endl;
  cout << endl;
  cout << "l = " << *l->value() << endl;
  cout << "dl = " << *l->gradient() << endl;
  cout << endl;

  x_value += 0.1 * *x->gradient();
  y_value += 0.1 * *y->gradient();

  l->DeleteAscendantsAndSelf();

  cout << endl;
  cout << "x_value is updated to " << x_value << endl;
  cout << "y_value is updated to " << y_value << endl;
  cout << endl;

  return 0;
}


#include "GaussSeq.h"
#include "utils.h"
#include <boost/random/mersenne_twister.hpp>
using utils::normal;
using utils::my_float;

/*
 *  Member variables
 */
boost::random::mt19937 GaussSeq::gen {};        // define static generator

/*
 *  Constructors and destructors
 */
GaussSeq::GaussSeq(my_float mean, my_float std) {
    normal new_n(mean, std);
    this->n = new_n;
}


#pragma once

#include <defines.h>

#include <stdint.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <unsupported/Eigen/MatrixFunctions>

// CUDA runtime
#include <cuda_runtime.h>
// Utilities and system includes
//#include <helper_functions.h>
#include <nvidia/helper_cuda.h>
#include <timer.hpp>

#include "sphereSimple.hpp"
#include "optimizationSO3.hpp"

using namespace Eigen;

class OptSO3Approx : public OptSO3
{
  public:
  OptSO3Approx(float sigma, float *d_q, int w, int h, float *d_weights =NULL):
    OptSO3(sigma,d_q,w,h,d_weights), Ss_(6,Matrix2f::Identity())
  {
    checkCudaErrors(cudaMalloc((void **)&d_mu_karch_, 6*4*sizeof(float)));
    checkCudaErrors(cudaMalloc((void **)&d_p_, 6*3*sizeof(float)));
    checkCudaErrors(cudaMalloc((void **)&d_SSs_, 6*7*sizeof(float)));
    checkCudaErrors(cudaMalloc((void **)&d_Rnorths_, 6*6*sizeof(float)));
    Matrix2f S = Matrix2f::Identity()*(15.0*M_PI/180.0)*(15.0*M_PI/180.0);
    invSigma_ = S.inverse();
    t_max_ = 5.0f;
    dt_ = 0.05f; // 0.1
  };

  ~OptSO3Approx()
  {
    checkCudaErrors(cudaFree(d_mu_karch_));
    checkCudaErrors(cudaFree(d_p_));
    checkCudaErrors(cudaFree(d_SSs_));
    checkCudaErrors(cudaFree(d_Rnorths_));
  };
  Matrix<float,3,6> karcherMeans(const Matrix<float,3,6>& p0, float thresh,
      uint32_t maxIter);
  void computeSuffcientStatistics();
  Matrix<float,3,6> qKarch_; // karcher means for all axes

protected:
  SphereSimple S2_;
  float *d_mu_karch_, *d_p_;
  float *d_Rnorths_, *d_SSs_;
  Matrix2f invSigma_; // inverse of the sigma in the tangent space
  Matrix<float,1,6> Ns_; // number of normals for each axis
  Matrix<float,2,6> xSums_; // sum over all vectors for each axis
  vector<Matrix2f> Ss_; // sum over outer products of data in tangent spaces

  virtual void conjugateGradientPostparation_impl(Matrix3f& R);
  virtual float conjugateGradientPreparation_impl(Matrix3f& R, uint32_t& N);
  /* evaluate cost function for a given assignment of npormals to axes */
  virtual float evalCostFunction(Matrix3f& R);
  /* compute Jacobian */
  virtual void computeJacobian(Matrix3f&J, Matrix3f& R, float N);
  /* recompute assignment based on rotation R and return residual as well */
  //float computeAssignment(Matrix3f& R, int& N);
  /* compute all karcher means (for 6 axis) */
  Matrix<float,3,6> meanInTpS2_GPU(Matrix<float,3,6>& p);
};


#include <frovedis.hpp>
#include <frovedis/matrix/pblas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // dot product of two distributed std::vectors'
    std::vector<float> x = {1,2,3,4};
    std::vector<float> y = {1,2,3,4};
    auto xbm = vec_to_bcm<float> (x); // distributed vector x
    auto ybm = vec_to_bcm<float> (y); // distrubuted vector y
    auto d = dot<float> (xbm,ybm); // d = xbm.ybm
    BOOST_CHECK (d == 30);

    auto bm = make_blockcyclic_matrix_load<float> ("./sample_4x4");

    // checking dot() operation
    auto row1 = make_row_vector<float> (bm,1);
    auto row2 = make_row_vector<float> (bm,2);

    // checking whether the dot operation successfully taken place
    auto r = dot<float>(row1, row2); // r = row1 . row2
    BOOST_CHECK (r == 2);
}


#include <iostream>
#include <boost/timer/timer.hpp>
#include "strided_array.h"
#include "linalg.h"

template<class T>
double test()
{
    int n = 700;
    int m = 10;
    StridedArray<T> a(n, n, T(0.0));
    for (int i=0; i<n; i++)
        a.setitem(i, i, i);

    StridedArray<T> b(n, n, T(0.0));
    for (int i=0; i<n; i++)
        b.setitem(i, i, 1);

    StridedArray<T> c(n, n, T(-1.0));

    boost::timer::cpu_timer t;
    t.start();
    for (int i=0; i<m; i++)
       MM<T>(&a, &b, &c);
    t.stop();

    double dt = double(t.elapsed().wall) / 1.0e9;
    double flops = 2 * m * pow(n, 3) / dt;
    return flops;
}

int main(void)
{
    //printf("int:    %f GFLOPS\n", 1.0e-9 * test<int>());
    printf("float:  %f GFLOPS\n", 1.0e-9 * test<float>());
    //printf("double: %f GFLOPS\n", 1.0e-9 * test<double>());
    return 0;
}


#include <iostream>
#include <ratio>
#include <boost/type_index.hpp>
using namespace std;
using boost::typeindex::type_id_with_cvr;

// 如果使用 string 保留单位名称会导致自定义字符串字面值出错，因为 string 有一个 non_trivial 的构造函数
template <class Rep, class Rat = std::ratio<1>>
struct Distance{
	using Ratio = Rat;
	Rep dis;
};

template <class Rep, class Rat = std::ratio<1>>
auto operator+(Distance<Rep, Rat> lhs, Distance<Rep, Rat> rhs){
	return Distance<Rep, Rat>{lhs.dis + rhs.dis, };
}

template <class Rep, class Rat = std::ratio<1>>
auto operator-(Distance<Rep, Rat> lhs, Distance<Rep, Rat> rhs){
	return Distance<Rep, Rat>{lhs.dis - rhs.dis};
}

template <class Rep>
using kilometers = Distance<Rep, std::kilo>;

template <class Rep>
using meters = Distance<Rep>;

template <class Rep>
using decimeters = Distance<Rep, std::deci>;

template <class Rep>
using centimeters = Distance<Rep, std::centi>;

template <class Rep>
using miles = Distance<Rep, std::ratio<1609344,1000>>;

template <class Rep>
using yards = Distance<Rep, std::ratio<9144, 10000>>;

template <class Rep>
using feet = Distance<Rep, std::ratio<3048, 10000>>;

template <class Rep>
using inches = Distance<Rep, std::ratio<354, 10000>>;

constexpr kilometers<long double> operator""_km(long double dist){
	return kilometers<long double>{dist};
}

constexpr meters<long double> operator""_m(long double dist){
	return meters<long double>{dist};
}

constexpr decimeters<long double> operator""_dm(long double dist){
	return decimeters<long double>{dist};
}

constexpr centimeters<long double> operator""_cm(long double dist){
	return centimeters<long double>{dist};
}

constexpr miles<long double> operator""_mi(long double dist){
	return miles<long double>{dist};
}

constexpr yards<long double> operator""_yd(long double dist){
	return yards<long double>{dist};
}

constexpr feet<long double> operator""_ft(long double dist){
	return feet<long double>{dist};
}

constexpr inches<long double> operator""_in(long double dist){
	return inches<long double>{dist};
}

template <class Rep>
ostream& operator<<(ostream& os, kilometers<Rep> len){
	return os << len.dis << "km";
}

template <class Rep>
ostream& operator<<(ostream& os, meters<Rep> len){
	return os << len.dis << "m";
}

template <class Rep>
ostream& operator<<(ostream& os, decimeters<Rep> len){
	return os << len.dis << "dm";
}

template <class Rep>
ostream& operator<<(ostream& os, centimeters<Rep> len){
	return os << len.dis << "cm";
}

template <class Rep>
ostream& operator<<(ostream& os, miles<Rep> len){
	return os << len.dis << "mi";
}

template <class Rep>
ostream& operator<<(ostream& os, yards<Rep> len){
	return os << len.dis << "yd";
}

template <class Rep>
ostream& operator<<(ostream& os, feet<Rep> len){
	return os << len.dis << "ft";
}

template <class Rep>
ostream& operator<<(ostream& os, inches<Rep> len){
	return os << len.dis << "in";
}

template <class To, class From>
constexpr To unit_cast(From from){
	using to_ratio = typename To::Ratio;
	using from_ratio = typename From::Ratio;
	using cast_ratio = std::ratio_divide<from_ratio, to_ratio>;
	return To{from.dis * cast_ratio::num / cast_ratio::den};
}

int main(int argc, char const *argv[])
{
	auto kms_1 = 10.24_km;
	auto kms_2 = 10.24_km;
	auto kms_3 = kms_1 + kms_2;
	auto mil_1 = 1.0_mi;
	auto mil_2 = 1.0_mi;
	auto mil_3 = mil_1 + mil_2;
	cout << type_id_with_cvr<decltype(kms_1)>().pretty_name() << endl;
	cout << type_id_with_cvr<decltype(mil_1)>().pretty_name() << endl;
	cout << kms_3 << endl;
	cout << mil_3 << endl;
	cout << unit_cast<kilometers<long double>>(mil_3) << endl;
	cout << unit_cast<meters<long double>>(mil_3) << endl;
	cout << unit_cast<decimeters<long double>>(mil_3) << endl;
	cout << unit_cast<centimeters<long double>>(mil_3) << endl;
	cout << unit_cast<miles<long double>>(kms_3) << endl;
	cout << unit_cast<yards<long double>>(kms_3) << endl;
	cout << unit_cast<feet<long double>>(kms_3) << endl;
	cout << unit_cast<inches<long double>>(kms_3) << endl;
	cout << unit_cast<yards<long double>>(1.0_mi) << endl;
	return 0;
}


/**
 * \file libs/numeric/ublasx/test/rank.cpp
 *
 * \brief Test suite for the \c rank operation.
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/detail/debug.hpp>
#include <boost/numeric/ublasx/operation/rank.hpp>
#include <iostream>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = boost::numeric::ublas;
namespace ublasx = boost::numeric::ublasx;

BOOST_UBLASX_TEST_DEF( rank_deficient )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Rank Deficient matrix");

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;
	typedef ublas::matrix_traits<matrix_type>::size_type size_type;

	const std::size_t m = 3;
	const std::size_t n = 3;

	matrix_type A(m,n);
	A(0,0) = 3; A(0,1) = 1; A(0,2) = 2;
	A(1,0) = 2; A(1,1) = 0; A(1,2) = 5;
	A(2,0) = 5; A(2,1) = 1; A(2,2) = 7;

	size_type r = ublasx::rank(A);
	size_type expect_r = n-1;
	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("rank = " << r);
	BOOST_UBLASX_TEST_CHECK( r == expect_r );
}

BOOST_UBLASX_TEST_DEF( full_rank )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Full Rank matrix");

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;
	typedef ublas::matrix_traits<matrix_type>::size_type size_type;

	const std::size_t m = 3;
	const std::size_t n = 3;

	matrix_type A(m,n);
	A(0,0) = 3; A(0,1) = 1; A(0,2) = 2;
	A(1,0) = 2; A(1,1) = 0; A(1,2) = 5;
	A(2,0) = 1; A(2,1) = 2; A(2,2) = 3;

	size_type r = ublasx::rank(A);
	size_type expect_r = n;
	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("rank = " << r);
	BOOST_UBLASX_TEST_CHECK( r == expect_r );
}

int main()
{
	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( rank_deficient );
	BOOST_UBLASX_TEST_DO( full_rank );

	BOOST_UBLASX_TEST_END();
}


/** \file gauss_distribution.hpp
    \brief Gaussian probability distribution */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

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

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

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef __GAUSS_DISTRIBUTION_HPP__
#define __GAUSS_DISTRIBUTION_HPP__

#include <boost/math/distributions/normal.hpp>
#include "prob_distribution.hpp"

namespace bayesopt
{

  class GaussianDistribution: public ProbabilityDistribution
  {
  public:
    GaussianDistribution(randEngine& eng);
    virtual ~GaussianDistribution();

    /**
     * \brief Sets the mean and std of the distribution
     */
    void setMeanAndStd(double mean, double std)
    { mean_ = mean; std_ = std; };

    /**
     * \brief Probability density function
     * @param x query point
     * @return probability
     */
    double pdf(double x)
    {
      x = (x - mean_) / std_;
      return boost::math::pdf(d_,x);
    };

    /**
     * \brief Expected Improvement algorithm for minimization
     * @param min  minimum value found
     * @param g exponent (used for annealing)
     *
     * @return negative value of the expected improvement
     */
    double negativeExpectedImprovement(double min, size_t g);

    /**
     * \brief Lower confindence bound. Can be seen as the inverse of the Upper
     * confidence bound
     * @param beta std coefficient (used for annealing)
     * @return value of the lower confidence bound
     */
    double lowerConfidenceBound(double beta);

    /**
     * Probability of improvement algorithm for minimization
     * @param min  minimum value found
     * @param epsilon minimum improvement margin
     *
     * @return negative value of the probability of improvement
     */
    double negativeProbabilityOfImprovement(double min,
					    double epsilon);

    /**
     * Sample outcome acording to the marginal distribution at the query point.
     * @return outcome
     */
    double sample_query();

    double getMean() { return mean_; };
    double getStd()  { return std_; };

  private:
    boost::math::normal d_;
    double mean_;
    double std_;
  };

} //namespace bayesopt

#endif


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/sigma_point.h>
#include <BayesFilters/directional_statistics.h>

#include <Eigen/SVD>

using namespace bfl;
using namespace bfl::directional_statistics;
using namespace bfl::sigma_point;
using namespace Eigen;

bfl::sigma_point::UTWeight::UTWeight
(
    std::size_t n,
    const double alpha,
    const double beta,
    const double kappa
) :
    mean((2 * n) + 1),
    covariance((2 * n) + 1)
{
    unscented_weights(n, alpha, beta, kappa, mean, covariance, c);
}

void bfl::sigma_point::unscented_weights
(
    const std::size_t n,
    const double alpha,
    const double beta,
    const double kappa,
    Ref<VectorXd> weight_mean,
    Ref<VectorXd> weight_covariance,
    double& c
)
{
    double lambda = std::pow(alpha, 2.0) * (n + kappa) - n;

    for (int j = 0; j < ((2 * n) + 1); ++j)
    {
        if (j == 0)
        {
            weight_mean(j)       = lambda / (n + lambda);
            weight_covariance(j) = lambda / (n + lambda) + (1 - std::pow(alpha, 2.0) + beta);
        }
        else
        {
            weight_mean(j)       = 1 / (2 * (n + lambda));
            weight_covariance(j) = weight_mean(j);
        }
    }

    c = n + lambda;
}

MatrixXd bfl::sigma_point::sigma_point(const GaussianMixture& state, const double c)
{
    MatrixXd sigma_points(state.dim, ((state.dim * 2) + 1) * state.components);

    for (std::size_t i = 0; i < state.components; i++)
    {
        JacobiSVD<MatrixXd> svd = state.covariance(i).jacobiSvd(ComputeThinU);

        MatrixXd A = svd.matrixU() * svd.singularValues().cwiseSqrt().asDiagonal();

        Ref<MatrixXd> sp = sigma_points.middleCols(((state.dim * 2) + 1) * i, ((state.dim * 2) + 1));

        sp << VectorXd::Zero(state.dim), std::sqrt(c) * A, -std::sqrt(c) * A;

        if (state.dim_linear > 0)
            sp.topRows(state.dim_linear).colwise() += state.mean(i).topRows(state.dim_linear);

        if (state.dim_circular > 0)
            sp.middleRows(state.dim_linear, state.dim_circular) = directional_add(sp.middleRows(state.dim_linear, state.dim_circular), state.mean(i).middleRows(state.dim_linear, state.dim_circular));

        if (state.dim_noise > 0)
            sp.bottomRows(state.dim_noise).colwise() += state.mean(i).bottomRows(state.dim_noise);
    }

    return sigma_points;
}

std::tuple<bool, GaussianMixture, MatrixXd> bfl::sigma_point::unscented_transform
(
    const GaussianMixture& input,
    const UTWeight& weight,
    FunctionEvaluation function
)
{
    /* Sample sigma points. */
    MatrixXd input_sigma_points = sigma_point::sigma_point(input, weight.c);

    /* Propagate sigma points */
    Data fun_data;
    bool valid_fun_data;
    bfl::sigma_point::OutputSize output_size;
    std::tie(valid_fun_data, fun_data, output_size) = function(input_sigma_points);

    /* Stop here if function evaluation failed. */
    if (!valid_fun_data)
        return std::make_tuple(false, GaussianMixture(), MatrixXd(0, 0));

    /* For now casting Data to MatrixXd. */
    MatrixXd prop_sigma_points = bfl::any::any_cast<MatrixXd&&>(std::move(fun_data));

    /* Initialize transformed gaussian. */
    GaussianMixture output(input.components, output_size.first, output_size.second);

    /* Initialize cross covariance matrix. */
    MatrixXd cross_covariance(input.dim, output.dim * output.components);

    /* Process all the components of the mixture. */
    std::size_t base = ((input.dim * 2) + 1);
    for (std::size_t i = 0; i < input.components; i++)
    {
        Ref<MatrixXd> input_sigma_points_i = input_sigma_points.middleCols(base * i, base);
        Ref<MatrixXd> prop_sigma_points_i = prop_sigma_points.middleCols(base * i, base);

        /* Evaluate the mean. */
        output.mean(i).topRows(output_size.first).noalias() = prop_sigma_points_i.topRows(output_size.first) * weight.mean;
        output.mean(i).bottomRows(output_size.second) = directional_mean(prop_sigma_points_i.bottomRows(output_size.second), weight.mean);

        /* Evaluate the covariance. */
        prop_sigma_points_i.topRows(output_size.first).colwise() -= output.mean(i).topRows(output_size.first);
        prop_sigma_points_i.bottomRows(output_size.second) = directional_sub(prop_sigma_points_i.bottomRows(output_size.second), output.mean(i).bottomRows(output_size.second));
        output.covariance(i).noalias() = prop_sigma_points_i * weight.covariance.asDiagonal() * prop_sigma_points_i.transpose();

        /* Evaluate the input-output cross covariance matrix
           (noise components in the input are not considered). */
        Ref<MatrixXd> cross_covariance_i = cross_covariance.middleCols(output.dim * i, output.dim);
        input_sigma_points_i.topRows(input.dim_linear).colwise() -= input.mean(i).topRows(input.dim_linear);
        input_sigma_points_i.middleRows(input.dim_linear, input.dim_circular) = directional_sub(input_sigma_points_i.middleRows(input.dim_linear, input.dim_circular), input.mean(i).middleRows(input.dim_linear, input.dim_circular));
        cross_covariance_i.noalias() = input_sigma_points_i.topRows(input.dim_linear + input.dim_circular) * weight.covariance.asDiagonal() * prop_sigma_points_i.transpose();
    }

    return std::make_tuple(true, output, cross_covariance);
}

std::pair<GaussianMixture, MatrixXd> bfl::sigma_point::unscented_transform
(
    const GaussianMixture& state,
    const UTWeight& weight,
    StateModel& state_model
)
{
    FunctionEvaluation f = [&state_model](const Ref<const MatrixXd>& state)
                           {
                               MatrixXd tmp(state.rows(), state.cols());

                               state_model.motion(state, tmp);

                               return std::make_tuple(true, std::move(tmp), state_model.getOutputSize());
                           };
    MatrixXd cross_covariance;
    GaussianMixture output;
    std::tie(std::ignore, output, cross_covariance) = unscented_transform(state, weight, f);

    return std::make_pair(output, cross_covariance);
}

std::pair<GaussianMixture, MatrixXd> bfl::sigma_point::unscented_transform
(
    const GaussianMixture& state,
    const UTWeight& weight,
    AdditiveStateModel& state_model
)
{
    FunctionEvaluation f = [&state_model](const Ref<const MatrixXd>& state)
                           {
                               MatrixXd tmp(state.rows(), state.cols());

                               state_model.propagate(state, tmp);

                               return std::make_tuple(true, std::move(tmp), state_model.getOutputSize());
                           };

    MatrixXd cross_covariance;
    GaussianMixture output;
    std::tie(std::ignore, output, cross_covariance) = unscented_transform(state, weight, f);

    /* In the additive case the covariance matrix is augmented with the noise
       covariance matrix. */
    for(std::size_t i = 0; i < state.components; i++)
        output.covariance(i) += state_model.getNoiseCovarianceMatrix();

    return std::make_pair(output, cross_covariance);
}

std::tuple<bool, GaussianMixture, MatrixXd> bfl::sigma_point::unscented_transform
(
    const GaussianMixture& state,
    const UTWeight& weight,
    MeasurementModel& meas_model
)
{
    FunctionEvaluation f = [&meas_model](const Ref<const MatrixXd>& state)
                           {
                               bool valid_prediction;
                               bfl::Data prediction;

                               std::tie(valid_prediction, prediction) = meas_model.predictedMeasure(state);

                               return std::make_tuple(valid_prediction, std::move(prediction), meas_model.getOutputSize());
                           };

    bool valid;
    MatrixXd cross_covariance;
    GaussianMixture output;
    std::tie(valid, output, cross_covariance) = unscented_transform(state, weight, f);

    return std::make_tuple(valid, output, cross_covariance);
}

std::tuple<bool, GaussianMixture, MatrixXd> bfl::sigma_point::unscented_transform
(
    const GaussianMixture& state,
    const UTWeight& weight,
    AdditiveMeasurementModel& meas_model
)
{
    FunctionEvaluation f = [&meas_model](const Ref<const MatrixXd>& state)
                           {
                               bool valid_prediction;
                               bfl::Data prediction;

                               std::tie(valid_prediction, prediction) = meas_model.predictedMeasure(state);

                               return std::make_tuple(valid_prediction, std::move(prediction), meas_model.getOutputSize());
                           };

    bool valid;
    MatrixXd cross_covariance;
    GaussianMixture output;
    std::tie(valid, output, cross_covariance) = unscented_transform(state, weight, f);

    /* In the additive case the covariance matrix is augmented with the noise
       covariance matrix. */
    MatrixXd noise_cov;
    std::tie(std::ignore, noise_cov) = meas_model.getNoiseCovarianceMatrix();
    for (std::size_t i = 0; i < state.components; i++)
        output.covariance(i) += noise_cov;

    return std::make_tuple(valid, output, cross_covariance);
}


#include<iostream>
#include<stdio.h>
#include<vector>
#include<cmath>
#include "ray.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <fstream>
using namespace std;
using namespace Eigen;

Vector3d operator*(double s, const Vector3d &v) {
	return Vector3d(s*v(0), s*v(1), s*v(2));
}

//Triangle with verticies pos, pos + p and pos + q.
class triangle: public shape {
	Vector3d p, q, normal;
	public:
	Vector3d pos;
	triangle(Vector3d np, Vector3d nq, Vector3d npos, colour &ncol, surface &s):
		p(np), q(nq), pos(npos) {
		col = ncol;
		surf = s;
		normal = p.cross(q);
	}

	Vector3d get_normal(const Vector3d ignore) const {
		return normal;
	}

	//Moller-Trumbore intersection algorithm
	double intersect(ray r) {
		Vector3d vec, T;
		double det, u, v, t;
		vec = r.dir.cross(q);
		det = p.dot(vec);
		//std::cout << "det = " << det << std::endl;
		//If det is zero then ray in plane of triangle
		if(det < INTERSECT_EPSILON && det > -INTERSECT_EPSILON) return 0.0;
		//Get ray from pos to ray origin
		T = r.origin - pos;
		u = T.dot(vec)/det;
		//Test u
		if(u < 0 || u > 1.0) return 0.0;
		//and v (+u)
		vec = T.cross(p);
		v = r.dir.dot(vec)/det;
		if(v < 0 || v + u > 1.0) return 0.0;
		//Intersect at origin + t*dir
		t = q.dot(vec)/det;
		//Check we're not behind the ray origin
		if(t < INTERSECT_EPSILON) return 0.0;
		//return ray intersect parameter: point = Origin + t*dir
		return t;
	}

};

class sphere: public shape {
	public:
	Vector3d pos;
	double radius;
	sphere(double nr, Vector3d np, colour &ncol, surface &s):
		pos(np), radius(nr){
		col = ncol;
		surf = s;
	}

	Vector3d get_normal(const Vector3d p) const {
		return p - pos;
	}

	double intersect(ray r) {
		double a, b, c, d, dsq, s1, s2;
		c = r.origin.dot(r.origin) + pos.dot(pos) - 2*r.origin.dot(pos) - radius*radius;
		b = 2 * r.dir.dot(r.origin - pos);
		a = r.dir.dot(r.dir);
		if(a < INTERSECT_EPSILON) {
			fprintf(stderr, "Warn: sphere::intersect: a < ITERSECT_EPSILON\n");
			return 0.0;
		}
		dsq = b*b - 4*a*c;
		if(dsq <= 0) {
			return 0;
		}
		d = std::sqrt(dsq);
		s1 = (-b+d)/(2*a);
		s2 = (-b-d)/(2*a);
		if(s1 < s2) {
			if(s1 > INTERSECT_EPSILON) {
				return s1;
			}
		}
		if(s2 > INTERSECT_EPSILON) {
			return s2;
		} else {
			return s1 > INTERSECT_EPSILON ? s1 : 0.0;
		}
	}

};

std::vector<shape*>* getMeshWorld() {
	std::vector<shape*> *w = new std::vector<shape*>();

	colour red  = colour(255, 0, 0, 1.0);
	colour blue = colour(0, 0, 255, 1.0);
	colour green = colour(0, 255.0, 0, 1.0);

	surface shiny;
	shiny.reflection = 0.8;
	shiny.specular = 1.0;
	shiny.diffusion = 0.9;

	// w->push_back((shape*) (new sphere(2, Vector3d(0, 3, 15), red, shiny)));

		ifstream fin ;
					    // fin.open("/Users/Rachit/Documents/iqra_cg/tracer3/bumpy_cube.off");
					    fin.open("/Users/Rachit/Documents/iqra_cg/tracer3/bunny.off");

					    int nrows,nrows2;
					    string output;
					    if(fin.is_open())
					    {
					        fin >> output;
					        fin >> output;
					        nrows=std::stoi(output);
					        fin >> output;
					        nrows2 = std::stoi(output);
					        fin >> output;

					    }

					    Eigen::ArrayXXf X = Eigen::ArrayXXf::Zero(nrows,3);
					    Eigen::ArrayXXd Y = Eigen::ArrayXXd::Zero(nrows2,4);

					    if (fin.is_open())
					    {
					        for (int row = 0; row < nrows; row++)
					            for (int col = 0; col < 3; col++)
					            {
					                float item = 0.0;
					                fin >> item;
					                X(row, col) = item;
					            }

					    }
					   // cerr << "X = " << endl << X << endl;
					    if (fin.is_open())
					    {
					        for (int row = 0; row < nrows2; row++)
					            for (int col = 0; col < 4; col++)
					            {
					                float item = 0.0;
					                fin >> item;
					                Y(row, col) = item;
					            }
					        fin.close();
					    }
					    // cerr<<"Y ="<< endl << Y <<endl;

					    for(unsigned k=0 ; k < Y.rows();k++) {
					    	// if(k%100==0)
					    	// cerr<<"at row-->"<<k<<endl;
					    	w->push_back((shape*) (new triangle(
					    		   Vector3d(X(Y(k, 1), 0), X(Y(k, 1), 1), X(Y(k, 1), 2)),
                Vector3d(X(Y(k, 2), 0), X(Y(k, 2), 1), X(Y(k, 2), 2)),
                Vector3d(X(Y(k, 3), 0), X(Y(k, 3), 1), X(Y(k, 3), 2)),
                red,shiny
					    		)));

}

	//One sphere
	// w->push_back((shape*) (new sphere(3, Vector3d(5, 5, 10), red, shiny)));
	// w->push_back((shape*) (new sphere(1.5, Vector3d(0, 0, -5), red, shiny)));
	return w;

}

std::vector<shape*>* partAWorld() {
	std::vector<shape*> *w = new std::vector<shape*>();

	colour red  = colour(255, 0, 0, 1.0);
	colour blue = colour(0, 0, 255, 1.0);
	colour green = colour(0, 255.0, 0, 1.0);

	//SETTING ONLY DIFFUSE as using imple Lambertian shading
	surface lambert;
	lambert.reflection = 0.0;
	lambert.specular = 0.0;
	lambert.diffusion = 0.9;

	w->push_back((shape*) (new sphere(5, Vector3d(0, 3, 15), red, lambert)));
	w->push_back((shape*) (new sphere(5, Vector3d(10, 10, 15), red, lambert)));

	// w->push_back((shape*) (new sphere(3, Vector3d(5, 5, 10), red, shiny)));
	// w->push_back((shape*) (new sphere(1.5, Vector3d(0, 0, -5), red, shiny)));
	return w;

}
std::vector<shape*>* partBWorld() {
	std::vector<shape*> *w = new std::vector<shape*>();

	colour red  = colour(255, 0, 0, 1.0);
	colour blue = colour(0, 0, 255, 1.0);
	colour green = colour(0, 255.0, 0, 1.0);

	//SETTING ONLY DIFFUSE as using imple Lambertian shading
	surface lambert;
	lambert.reflection = 0.0;
	lambert.specular = 0.0;
	lambert.diffusion = 0.9;
	//Diffuse+specular
	surface specular;
	specular.reflection = 0.0;
	specular.specular = 1.0;
	specular.diffusion = 0.9;

	w->push_back((shape*) (new sphere(10, Vector3d(0, 3, 15), red, lambert)));
	w->push_back((shape*) (new sphere(5, Vector3d(10, 10, 15), blue, specular)));

	// w->push_back((shape*) (new sphere(3, Vector3d(5, 5, 10), red, shiny)));
	// w->push_back((shape*) (new sphere(1.5, Vector3d(0, 0, -5), red, shiny)));
	return w;

}
std::vector<shape*>* partCWorld() {
	std::vector<shape*> *w = new std::vector<shape*>();

	colour red  = colour(255, 0, 0, 1.0);
	colour blue = colour(0, 0, 255, 1.0);
	colour green = colour(0, 255.0, 0, 1.0);

	//SETTING ONLY DIFFUSE as using imple Lambertian shading
	surface lambert;
	lambert.reflection = 0.0;
	lambert.specular = 0.0;
	lambert.diffusion = 0.9;
	//Diffuse+specular
	surface specular;
	specular.reflection = 0.0;
	specular.specular = 1.0;
	specular.diffusion = 0.9;

	w->push_back((shape*) (new sphere(10, Vector3d(0, 3, 15), red, lambert)));
	w->push_back((shape*) (new sphere(5, Vector3d(10, 10, 15), blue, specular)));

	// w->push_back((shape*) (new sphere(3, Vector3d(5, 5, 10), red, shiny)));
	// w->push_back((shape*) (new sphere(1.5, Vector3d(0, 0, -5), red, shiny)));
	return w;

}
std::vector<shape*>* partEWorld(int n_spheres) {

	surface shiny;
	shiny.reflection = 0.4;
	shiny.specular = 1.0;
	shiny.diffusion = 0.9;
	std::vector<shape*> *w = new std::vector<shape*>();
	int i, j, k;
	for(i=0; i<3; i++) {
	for(j=0; j<3; j++) {
	for(k=0; k<3; k++) {
		if(n_spheres > 0) {
			colour c = colour(i*127.5, 255-j*127.5, ((int) (127.5+(k*127.5)))%382);
			w->push_back((shape*) (new sphere(1, Vector3d(i*4-5.5, j*4-6, k*4+14), c, shiny)));
			n_spheres -= 1;
		}
	}
	}
	}
	return w;

}

void freeWorld(std::vector<shape*> *w) {
	while(w->size() > 0) {
		delete (w->back());
		w->pop_back();
	}
	delete w;
}

std::vector<light*>* getLights() {
	std::vector<light*> *l = new std::vector<light*>();
	l->push_back(new light(Vector3d(15, 15, 5), colour(255, 255, 255)));
	return l;
}
std::vector<light*>* partALights() {
	std::vector<light*> *l = new std::vector<light*>();
	l->push_back(new light(Vector3d(15, 15, 5), colour(255, 255, 255)));
	return l;
}
std::vector<light*>* partBLights() {
	std::vector<light*> *l = new std::vector<light*>();
	l->push_back(new light(Vector3d(15, 15, 5), colour(255, 255, 255)));
	l->push_back(new light(Vector3d(-15, -15, 5), colour(255, 255, 255)));

	return l;
}
std::vector<light*>* partELights() {
	std::vector<light*> *l = new std::vector<light*>();
	l->push_back(new light(Vector3d(-5, 3, 2), colour(255, 255, 255)));
	l->push_back(new light(Vector3d(15, 15, 5), colour(255, 255, 255)));

	return l;
}

void freeLights(std::vector<light*> *l) {
	while(l->size() > 0) {
		delete (l->back());
		l->pop_back();
	}
	delete l;
}


#ifndef CPPMATH_MATRIX_PSEUDOINVERSESVD_HPP_
#define CPPMATH_MATRIX_PSEUDOINVERSESVD_HPP_

#include <Eigen/SVD>

namespace cppmath
{
    /**
     * Calculates a SVD-based pseudo inverse matrix.
     * See:
     * - http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
     * - http://eigen.tuxfamily.org/index.php?title=FAQ#Is_there_a_method_to_compute_the_.28Moore-Penrose.29_pseudo_inverse_.3F
     *
     * \author cpieloth
     * \copyright Copyright 2014 Christof Pieloth, Licensed under the Apache License, Version 2.0
     */
    template< typename T >
    class PseudoInverseSVD: private Eigen::JacobiSVD< T >
    {
    public:
        /**
         * Constructor.
         *
         * \param matrix Matrix to compute the pseudo inverse from.
         * \param pinvThreshold Threshold to keep value as zero/nonzero (default: 1.0e-6).
         */
        PseudoInverseSVD( const T& matrix, float threshold = 1.0e-6 );

        virtual ~PseudoInverseSVD();

        /**
         * Computes the pseudo inverse.
         * The pseudo inverse is internally stored for further calculations.
         *
         * \return Reference to the internally stored pseudo inverse matrix.
         */
        const T& compute();

        /**
         * Computes the pseudo inverse.
         *
         * \param pinvmat Holds the pseudo inverse matrix after computation.
         */
        void compute( T* const pinvmat ) const;

        /**
         * Multiplies the pseudo inverse with a matrix.
         * The pseudo inverse is internally stored for further calculations.
         *
         * \param m Matrix
         * \return Result of pinv*m
         */
        T operator*( const T& m );

        /**
         *  Multiplies the pseudo inverse with a matrix.
         *
         * \param m Matrix
         * \return Result of pinv*m
         */
        T operator*( const T& m ) const;

    private:
        bool m_hasInverse; /**< Indicates if the internal inverse matrix is available. */

        T m_inverse; /**< Stores a computed inverse matrix. */

        const float m_threshold;
    };
} /* namespace cppmath */

// Load the implementation
#include "PseudoInverseSVD-impl.hpp"

#endif  // CPPMATH_MATRIX_PSEUDOINVERSESVD_HPP_


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

  Copyright 2016 NumScale SAS

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

#ifndef BOOST_SIMD_NO_INVALIDS
STF_CASE_TPL(" remquo invalid", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::remquo;

  using iT =  bd::as_integer_t<T,signed>;

  STF_EXPR_IS( (remquo(T(), T()))
                  , (std::pair<T,iT>)
                  );

  T inf_  = bs::Inf<T>();
  T nan_  = bs::Nan<T>();
  T zero_ = bs::Zero<T>();
  T one_  = bs::One<T>();

  // n is unspecified by the standard so we don't test it
  std::pair<T,iT> p;

  p =  remquo(one_,nan_);
  STF_IEEE_EQUAL(p.first, nan_);

  p =  remquo(one_,inf_);
  STF_IEEE_EQUAL(p.first, nan_);

  p =  remquo(one_,zero_);
  STF_IEEE_EQUAL(p.first, nan_);

  p =  remquo(inf_,zero_);
  STF_IEEE_EQUAL(p.first, nan_);

  p =  remquo(nan_,zero_);
  STF_IEEE_EQUAL(p.first, nan_);

  p =  remquo(nan_,one_);
  STF_IEEE_EQUAL(p.first, nan_);

  p =  remquo(nan_,nan_);
  STF_IEEE_EQUAL(p.first, nan_);

  p =  remquo(nan_,inf_);
  STF_IEEE_EQUAL(p.first, nan_);
}
#endif

STF_CASE_TPL(" remquo valid", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::remquo;

  using iT =  bd::as_integer_t<T,signed>;

  STF_EXPR_IS( (remquo(T(), T()))
             , (std::pair<T,iT>)
             );

  T a0[] = { T(1), T(2), T(3), T(4.5) };
  T a1[] = { T(2), T(1), T(7), T(3.2) };

  std::size_t nb = sizeof(a0)/sizeof(T);

  std::pair<T,iT> p;

  for(std::size_t i=0;i<nb;++i)
  {
    p = remquo(a0[i],a1[i]);
    STF_EQUAL(p.second, iT(a0[i] / a1[i]));
    STF_EQUAL(p.first, a0[i] - p.second*a1[i]);
  }
}