// Boost.Geometry Index
// Compare performance with std::set using 1-dimensional object
// (i.e. angle, or number line coordiante)

// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.

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

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <boost/chrono.hpp>
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <set>

int main()
{
    namespace bg = boost::geometry;
    namespace bgi = bg::index;
    typedef boost::chrono::thread_clock clock_t;
    typedef boost::chrono::duration<float> dur_t;

    size_t values_count = 1001;
    size_t count_start = 10;
    size_t count_stop = 1000;
    size_t count_step = 10;
    size_t insrem_count = 3000000;

    typedef bg::model::point<float, 1, bg::cs::cartesian> P;
    //typedef bgi::rtree<P, bgi::linear<8, 3> > RT;
    typedef bgi::rtree<P, bgi::quadratic<8, 3> > RT;
    //typedef bgi::rtree<P, bgi::rstar<8, 3> > RT;

    RT t;
    std::set<float> s;
    size_t val_i = 0;
    for ( size_t curr_count = count_start ; curr_count < count_stop ; curr_count += count_step )
    {
        // inserting test
        {
            for (; val_i < curr_count ; ++val_i )
            {
                float v = val_i / 100.0f;
                P p(v);
                t.insert(p);
                s.insert(v);
            }

            float v = (val_i+1) / 100.0f;
            P test_p(v);

            std::cout << t.size() << ' ';

            clock_t::time_point start = clock_t::now();

            for (size_t i = 0 ; i < insrem_count ; ++i )
            {
                t.insert(test_p);
                t.remove(test_p);
            }

            dur_t time = clock_t::now() - start;
            std::cout << time.count() << ' ';

            start = clock_t::now();

            for (size_t i = 0 ; i < insrem_count ; ++i )
            {
                s.insert(v);
                s.erase(v);
            }

            time = clock_t::now() - start;
            std::cout << time.count() << ' ';
        }

        std::cout << '\n';
    }

    return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 5, 1> test_double_5_1(const Eigen::Matrix<double, 5, 1> & M)
{
	return M;
}
void export_double_5_1()
{
	boost::python::def("test_double_5_1",test_double_5_1);
}


#pragma once

#include <boost/pending/disjoint_sets.hpp>
#include <xtensor/xtensor.hpp>

namespace nifty {
namespace ufd {

    template<class T = uint64_t>
    class BoostUfd {

    public:
        typedef T value_type;

        // initialize from number of elements
        // for consecutive elements
        BoostUfd(const value_type size) : n_elements_(size),
                                          upper_bound_(size),
                                          ranks_(n_elements_),
                                          parents_(n_elements_),
                                          sets_(&ranks_[0], &parents_[0]) {
            for(value_type elem = 0; elem < n_elements_; ++elem) {
                sets_.make_set(elem);
            }
        }

        // initialize from element list for non-consecutive elements
        // NOTE we need to set the ranks and the elements to the number of elements we would
        // have if the elements were continuous (i.e. max + 1)
        template<class ELEMENTS>
        BoostUfd(const xt::xexpression<ELEMENTS> & elements, const std::size_t upper_bound) : n_elements_(elements.derived_cast().size()),
                                                                                upper_bound_(upper_bound),
                                                                                ranks_(upper_bound_),
                                                                                parents_(upper_bound_),
                                                                                sets_(&ranks_[0], &parents_[0]) {
            const auto & elems = elements.derived_cast();
            for(const value_type elem : elems) {
                sets_.make_set(elem);
            }
        }

        // TODO make const and not const version for path compression ?
        inline value_type find(const value_type elem) {
            return sets_.find_set(elem);
        }

        inline void merge(const value_type elem1, const value_type elem2) {
            // std::cout << "Merge elems " << elem1 << " " << elem2 << std::endl;
            sets_.link(elem1, elem2);
        }

        inline std::size_t numberOfElements() const {
            return n_elements_;
        }

    private:
        std::size_t n_elements_;
        std::size_t upper_bound_;
        std::vector<value_type> ranks_;
        std::vector<value_type> parents_;
        boost::disjoint_sets<value_type*, value_type*> sets_;
    };

}
}


#define BOOST_TEST_MODULE "C++ Unit Tests for metaLBM"
#include <boost/test/included/unit_test.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
namespace tt = boost::test_tools;

#include <iostream>
#include <vector>
template<class T, std::size_t Alignment = 1>
#ifdef ALIGN_MEMORY
using vector = std::vector<T,
                           boost::alignment::aligned_allocator<T, Alignment> >;
#else
using vector = std::vector<T>;
#endif

#define NPROCS 1
#include "input.h"
#include "commons.h"
#include "structure.h"
#include "calculate.h"

using namespace lbm;

BOOST_AUTO_TEST_SUITE(TestCalculate)

BOOST_AUTO_TEST_CASE(TestComputeDensity) {
  vector<valueType> fPop(L::dimQ
                         *(L::lX_l+2*L::hX)
                         *(L::lY_l+2*L::hY)
                         *(L::lZ_l+2*L::hZ));
  vector<valueType> density_r(L::lX_g *L::lY_g*L::lZ_g);

  for(int idx = 0; idx < L::lX_g *L::lY_g*L::lZ_g; ++idx) {
    for(int iQ = 0; iQ < L::dimQ; ++iQ) {
      fPop[idxPop(idx, iQ)] = (valueType) (idx + iQ);
    }
    density_r[idx] = computeDensity<valueType>(fPop.data(), idx);
  }

  for(int idx = 0; idx < L::lX_g *L::lY_g*L::lZ_g; ++idx) {
    BOOST_TEST(density_r[idx] == (valueType)(L::dimQ*idx + (L::dimQ-1)*L::dimQ/2),
               tt::tolerance(1e-15));
  }
}

BOOST_AUTO_TEST_CASE(TestComputeVelocity) {
  vector<valueType> fPop(L::dimQ*sizeX_l()*sizeY_l()*sizeZ_l(), (valueType) 0);
  vector<MathVector<valueType, L::dimD>> velocity_r(L::lX_g *L::lY_g*L::lZ_g,
                                                    MathVector<valueType, L::dimD>{0});

  for(int idx = 0; idx < L::lX_g *L::lY_g*L::lZ_g; ++idx) {
    for(int iQ = 0; iQ < L::dimQ; ++iQ) {
      fPop[idxPop(idx, iQ)] = (valueType) L::weight()[iQ];
    }
    velocity_r[idx] = computeVelocity<valueType>(fPop.data(), idx,
                                                 (valueType) idx+1);
  }

  for(int idx = 0; idx < L::lX_g *L::lY_g*L::lZ_g; ++idx) {
    for(int d = 0; d < L::dimD; ++d) {
      BOOST_TEST(velocity_r[idx][d] == (valueType) 0,
                 tt::tolerance(1e-15));
    }
  }
}

BOOST_AUTO_TEST_CASE(TestComputeEquilibrium) {
  vector<valueType> fEq(L::dimQ*sizeX_l()*sizeY_l()*sizeZ_l(), (valueType) 0);
  vector<valueType> density_r(L::lX_g *L::lY_g*L::lZ_g);
  vector<MathVector<valueType, L::dimD>> velocity_r(L::lX_g *L::lY_g*L::lZ_g,
                                                    MathVector<valueType, L::dimD>{0});

  for(int idx = 0; idx < L::lX_g *L::lY_g*L::lZ_g; ++idx) {
    MathVector<valueType, L::dimD> velocity{(valueType)idx/(valueType) (L::lX_g*L::lY_g*L::lZ_g)};
    valueType density = (valueType)(1+idx)/(valueType) (L::lX_g*L::lY_g*L::lZ_g);
    for(int iQ = 0; iQ < L::dimQ; ++iQ) {
      fEq[idxPop(idx, iQ)] = computeEquilibrium<valueType>(iQ, density, velocity,
                                                           velocity.norm2());
    }
    density_r[idx] = computeDensity<valueType>(fEq.data(), idx);
    velocity_r[idx] = computeVelocity<valueType>(fEq.data(), idx,
                                                 density_r[idx]);

  }

  for(int idx = 0; idx < L::lX_g *L::lY_g*L::lZ_g; ++idx) {
    BOOST_TEST(density_r[idx] == (valueType)(1+idx)/(valueType) (L::lX_g*L::lY_g*L::lZ_g),
             tt::tolerance(1e-15));
  for(int d = 0; d < L::dimD; ++d) {
    BOOST_TEST(velocity_r[idx][d] == (valueType)idx/(valueType) (L::lX_g*L::lY_g*L::lZ_g),
               tt::tolerance(1e-15));
  }
 }
}

BOOST_AUTO_TEST_SUITE_END()


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix2i a; a << 1, 2, 3, 4;
cout << "Here is the matrix a:\n" << a << endl;

a = a.transpose(); // !!! do NOT do this !!!
cout << "and the result of the aliasing effect:\n" << a << endl;
  return 0;
}


#include "fbstab/components/mpc_data.h"

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

#include "tools/matrix_sequence.h"

namespace fbstab {

using MatrixXd = Eigen::MatrixXd;
using VectorXd = Eigen::VectorXd;
using Map = Eigen::Map<Eigen::MatrixXd>;
using ConstMap = Eigen::Map<const Eigen::MatrixXd>;

void MpcData::gemvH(const Eigen::VectorXd& x, double a, double b,
                    Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::gemvH: y input is null.");
  }
  if (x.size() != nz_ || y->size() != nz_) {
    throw std::runtime_error("Size mismatch in MpcData::gemvH.");
  }
  if (b == 0.0) {
    y->fill(0.0);
  } else if (b != 1.0) {
    (*y) *= b;
  }

  // Create reshaped views of input and output vectors.
  Map w(y->data(), nx_ + nu_,
        N_ + 1);  // w = reshape(y, [nx + nu, N + 1]);
  ConstMap v(x.data(), nx_ + nu_,
             N_ + 1);  // v = reshape(x, [nx + nu, N + 1]);
  for (int i = 0; i < N_ + 1; i++) {
    const auto& Q = Q_(i);
    const auto& S = S_(i);
    const auto& R = R_(i);

    // These variables alias w.
    auto yx = w.block(0, i, nx_, 1);
    auto yu = w.block(nx_, i, nu_, 1);

    // These variables alias v.
    const auto vx = v.block(0, i, nx_, 1);
    const auto vu = v.block(nx_, i, nu_, 1);

    // [yx] += a * [Q(i) S(i)'] [vx]
    // [yu]        [S(i) R(i) ] [vu]
    // Using lazyProduct is inefficient so should be avoided when possible.
    if (a == 1.0) {
      yx.noalias() += Q * vx + S.transpose() * vu;
      yu.noalias() += S * vx + R * vu;
    } else if (a == -1.0) {
      yx.noalias() -= Q * vx + S.transpose() * vu;
      yu.noalias() -= S * vx + R * vu;
    } else {
      yx += a * Q.lazyProduct(vx);
      yx += a * S.transpose().lazyProduct(vu);
      yu += a * S.lazyProduct(vx);
      yu += a * R.lazyProduct(vu);
    }
  }
}

void MpcData::gemvA(const Eigen::VectorXd& x, double a, double b,
                    Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::gemvA: y input is null.");
  }
  if (x.size() != nz_ || y->size() != nv_) {
    throw std::runtime_error("Size mismatch in MpcData::gemvA.");
  }
  if (b == 0.0) {
    y->fill(0.0);
  } else if (b != 1.0) {
    (*y) *= b;
  }
  // Create reshaped views of input and output vectors.
  ConstMap z(x.data(), nx_ + nu_, N_ + 1);
  Map w(y->data(), nc_, N_ + 1);

  for (int i = 0; i < N_ + 1; i++) {
    const auto& E = E_(i);
    const auto& L = L_(i);

    // This aliases w.
    auto yi = w.col(i);

    // These alias z.
    const auto xi = z.block(0, i, nx_, 1);
    const auto ui = z.block(nx_, i, nu_, 1);

    // yi += a*(E*vx + L*vu)
    if (a == 1.0) {
      yi.noalias() += E * xi + L * ui;
    } else if (a == -1.0) {
      yi.noalias() -= E * xi + L * ui;
    } else {
      yi += a * E.lazyProduct(xi);
      yi += a * L.lazyProduct(ui);
    }
  }
}

void MpcData::gemvG(const Eigen::VectorXd& x, double a, double b,
                    Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::gemvG: y input is null.");
  }
  if (x.size() != nz_ || y->size() != nl_) {
    throw std::runtime_error("Size mismatch in MpcData::gemvG.");
  }
  if (b == 0.0) {
    y->fill(0.0);
  } else if (b != 1.0) {
    (*y) *= b;
  }
  // Create reshaped views of input and output vectors.
  ConstMap z(x.data(), nx_ + nu_, N_ + 1);
  Map w(y->data(), nx_, N_ + 1);

  w.col(0).noalias() += -a * z.block(0, 0, nx_, 1);

  for (int i = 1; i < N_ + 1; i++) {
    const auto& A = A_(i - 1);
    const auto& B = B_(i - 1);

    // Alias for the output at stage i.
    auto yi = w.col(i);
    // Aliases for the state and control at stage i - 1.
    const auto xm1 = z.block(0, i - 1, nx_, 1);
    const auto um1 = z.block(nx_, i - 1, nu_, 1);
    // Alias for the state at stage i.
    const auto xi = z.block(0, i, nx_, 1);

    // y(i) += a*(A(i-1)*x(i-1) + B(i-1)u(i-1) - x(i))
    if (a == 1.0) {
      yi.noalias() += A * xm1 + B * um1;
      yi.noalias() -= xi;
    } else if (a == -1.0) {
      yi.noalias() -= A * xm1 + B * um1;
      yi.noalias() += xi;
    } else {
      yi += a * A.lazyProduct(xm1);
      yi += a * B.lazyProduct(um1);
      yi -= a * xi;
    }
  }
}

void MpcData::gemvGT(const Eigen::VectorXd& x, double a, double b,
                     Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::gemvGT: y input is null.");
  }
  if (x.size() != nl_ || y->size() != nz_) {
    throw std::runtime_error("Size mismatch in MpcData::gemvGT.");
  }
  if (b == 0.0) {
    y->fill(0.0);
  } else if (b != 1.0) {
    (*y) *= b;
  }

  // Create reshaped views of input and output vectors.
  ConstMap v(x.data(), nx_, N_ + 1);
  Map w(y->data(), nx_ + nu_, N_ + 1);

  for (int i = 0; i < N_; i++) {
    const auto& A = A_(i);
    const auto& B = B_(i);

    // Aliases for the dual variables at stage i and i+1;
    const auto vi = v.col(i);
    const auto vp1 = v.col(i + 1);

    // Aliases for the state and control at stage i.
    auto xi = w.block(0, i, nx_, 1);
    auto ui = w.block(nx_, i, nu_, 1);

    // x(i) += a*(-v(i) + A(i)' * v(i+1))
    // u(i) += a*B(i)' * v(i+1)
    xi.noalias() += -a * vi;
    if (a == 1.0) {
      xi.noalias() += A.transpose() * vp1;
      ui.noalias() += B.transpose() * vp1;
    } else if (a == -1.0) {
      xi.noalias() -= A.transpose() * vp1;
      ui.noalias() -= B.transpose() * vp1;
    } else {
      xi.noalias() += a * A.transpose().lazyProduct(vp1);
    }
  }
  // The i = N step of the recursion.
  w.block(0, N_, nx_, 1).noalias() += -a * v.col(N_);
}

void MpcData::gemvAT(const Eigen::VectorXd& x, double a, double b,
                     Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::gemvAT: y input is null.");
  }
  if (x.size() != nv_ || y->size() != nz_) {
    throw std::runtime_error("Size mismatch in MpcData::gemvAT.");
  }
  if (b == 0.0) {
    y->fill(0.0);
  } else if (b != 1.0) {
    (*y) *= b;
  }
  // Create reshaped views of input and output vectors.
  ConstMap v(x.data(), nc_, N_ + 1);
  Map w(y->data(), nx_ + nu_, N_ + 1);

  for (int i = 0; i < N_ + 1; i++) {
    const auto& E = E_(i);
    const auto& L = L_(i);

    auto xi = w.block(0, i, nx_, 1);
    auto ui = w.block(nx_, i, nu_, 1);

    const auto vi = v.col(i);
    // x(i) += a*E(i)' * v(i)
    // u(i) += a*L(i)' * v(i)
    if (a == 1.0) {
      xi.noalias() += E.transpose() * vi;
      ui.noalias() += L.transpose() * vi;
    } else if (a == -1.0) {
      xi.noalias() -= E.transpose() * vi;
      ui.noalias() -= L.transpose() * vi;
    } else {
      ui.noalias() += a * L.transpose().lazyProduct(vi);
      xi.noalias() += a * E.transpose().lazyProduct(vi);
    }
  }
}

void MpcData::axpyf(double a, Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::axpyf: y input is null.");
  }
  if (y->size() != nz_) {
    throw std::runtime_error("Size mismatch in MpcData::axpyf.");
  }

  // Create reshaped view of the input vector.
  Map w(y->data(), nx_ + nu_, N_ + 1);

  for (int i = 0; i < N_ + 1; i++) {
    auto xi = w.block(0, i, nx_, 1);
    auto ui = w.block(nx_, i, nu_, 1);

    xi.noalias() += a * q_(i);
    ui.noalias() += a * r_(i);
  }
}

void MpcData::axpyh(double a, Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::axpyh: y input is null.");
  }
  if (y->size() != nl_) {
    throw std::runtime_error("Size mismatch in MpcData::axpyh.");
  }
  // Create reshaped view of the input vector.
  Map w(y->data(), nx_, N_ + 1);
  w.col(0) += -a * x0_;

  for (int i = 1; i < N_ + 1; i++) {
    w.col(i) += -a * c_(i - 1);
  }
}

void MpcData::axpyb(double a, Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::axpyb: y input is null.");
  }
  if (y->size() != nv_) {
    throw std::runtime_error("Size mismatch in MpcData::axpyb.");
  }
  // Create reshaped view of the input vector.
  Map w(y->data(), nc_, N_ + 1);

  for (int i = 0; i < N_ + 1; i++) {
    w.col(i).noalias() += -a * d_(i);
  }
}

void MpcData::ValidateInputs() const {
  bool OK = true;
  const int N = Q_.length();
  if (N <= 0) {
    throw std::runtime_error("Horizon length must be at least 1.");
  }

  OK = OK && N == R_.length();
  OK = OK && N == S_.length();
  OK = OK && N == q_.length();
  OK = OK && N == r_.length();
  OK = OK && (N - 1) == A_.length();
  OK = OK && (N - 1) == B_.length();
  OK = OK && (N - 1) == c_.length();
  OK = OK && N == E_.length();
  OK = OK && N == L_.length();
  OK = OK && N == d_.length();
  if (!OK) {
    throw std::runtime_error(
        "Sequence length mismatch in input data to MpcData.");
  }

  const int nx = Q_.rows();
  if (x0_.size() != nx) {
    throw std::runtime_error("Size mismatch in x0 input to MpcData.");
  }
  if (Q_.cols() != nx) {
    throw std::runtime_error("Size mismatch in Q input to MpcData.");
  }
  if (S_.cols() != nx) {
    throw std::runtime_error("Size mismatch in S input to MpcData.");
  }
  if (q_.rows() != nx) {
    throw std::runtime_error("Size mismatch in q input to MpcData.");
  }
  if (E_.cols() != nx) {
    throw std::runtime_error("Size mismatch in E input to MpcData.");
  }
  if (A_.rows() != nx || A_.cols() != nx) {
    throw std::runtime_error("Size mismatch in A input to MpcData.");
  }
  if (B_.rows() != nx) {
    throw std::runtime_error("Size mismatch in B input to MpcData.");
  }
  if (c_.rows() != nx) {
    throw std::runtime_error("Size mismatch in c input to MpcData.");
  }

  const int nu = R_.rows();
  if (R_.cols() != nu) {
    throw std::runtime_error("Size mismatch in R input to MpcData.");
  }
  if (S_.rows() != nu) {
    throw std::runtime_error("Size mismatch in S input to MpcData.");
  }
  if (r_.rows() != nu) {
    throw std::runtime_error("Size mismatch in r input to MpcData.");
  }
  if (L_.cols() != nu) {
    throw std::runtime_error("Size mismatch in L input to MpcData.");
  }
  if (B_.cols() != nu) {
    throw std::runtime_error("Size mismatch in B input to MpcData.");
  }

  const int nc = E_.rows();
  if (L_.rows() != nc) {
    throw std::runtime_error("Size mismatch in L input to MpcData.");
  }
  if (d_.rows() != nc) {
    throw std::runtime_error("Size mismatch in d input to MpcData.");
  }
}

}  // namespace fbstab


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "sparse_solver.h"
#include <Eigen/SparseLU>
#include <Eigen/MetisSupport>
#include <unsupported/Eigen/SparseExtra>

template<typename T> void test_metis_T()
{
  SparseLU<SparseMatrix<T, ColMajor>, MetisOrdering<int> > sparselu_metis;

  check_sparse_square_solving(sparselu_metis);
}

EIGEN_DECLARE_TEST(metis_support)
{
  CALL_SUBTEST_1(test_metis_T<double>());
}


// It is extremely important to use this header
// if you are using the numpy_eigen interface
#include <numpy_eigen/boost_python_headers.hpp>
#include <sparse_block_matrix/sparse_block_matrix.h>
#include <Eigen/Dense>

using namespace boost::python;
using namespace sparse_block_matrix;

template<typename SBM>
void clearSBM(SBM * sbm) { sbm->clear(); }

template<typename SBM>
typename SBM::SparseMatrixBlock blockSBM(const SBM * sbm,int r, int c) {
  if(r < 0 || r >= sbm->bRows())
    throw std::runtime_error("Row index out of bounds");
  if(c < 0 || c >= sbm->bCols())
    throw std::runtime_error("Column index out of bounds");

  const typename SBM::SparseMatrixBlock * mat = sbm->block(r,c);
  if(mat)
    {
      return *mat;
    }
  else
    {
      typename SBM::SparseMatrixBlock rval;
      rval.resize(sbm->rowsOfBlock(r), sbm->colsOfBlock(c));
      rval.setZero();
      return rval;
    }
}

template<typename SBM>
void setBlockSBM(SBM * sbm, int r, int c, const typename SBM::SparseMatrixBlock & mat) {
  if(r < 0 || r >= sbm->bRows())
    throw std::runtime_error("Row index out of bounds");
  if(c < 0 || c >= sbm->bCols())
    throw std::runtime_error("Column index out of bounds");

  typename SBM::SparseMatrixBlock * blk = sbm->block(r,c,true);
  if(!blk)
    throw std::runtime_error("Unexpected error: block is null");

  *blk = mat;

}

template<typename SBM>
Eigen::MatrixXd toDenseSymmetric(SBM * sbm)
{
  //return sbm->toDense().selfadjointView<Eigen::Upper>();
  return sbm->toDense(). template selfadjointView<Eigen::Upper>();
}

template<typename SBM>
double at(SBM * sbm, int r, int c)
{
    return (*sbm)(r,c);
}

void exportSBM()
{
  typedef SparseBlockMatrix<Eigen::MatrixXd> mat_t;

  boost::python::class_< mat_t > ("SparseBlockMatrixXd", init<>())
    .def(init<const Eigen::VectorXi &, const Eigen::VectorXi &, bool>( ("SparseBlockMatrixXd(rbi, cbi, hasStorage)\n\nrbi:  a vector of ints containing the row layout of the blocks. The component i of the array should contain the index of the first row of the block i+1\ncbi:  a vector of ints containing the column layout of the blocks. The component i of the array should contain the index of the first column of the block i+1")) )
    .def("rows",&mat_t::rows)
    .def("cols",&mat_t::cols)
    .def("bRows",&mat_t::bRows)
    .def("bCols",&mat_t::bCols)
    .def("clear",&clearSBM<mat_t>)
    .def("clear",&mat_t::clear)
    .def("isBlockSet",&mat_t::isBlockSet)
    .def("getBlock",&blockSBM<mat_t>)
    .def("setBlock",&setBlockSBM<mat_t>)
    .def("rowsOfBlock",&mat_t::rowsOfBlock)
    .def("colsOfBlock",&mat_t::colsOfBlock)
    .def("rowBaseOfBlock",&mat_t::rowBaseOfBlock)
    .def("colBaseOfBlock",&mat_t::colBaseOfBlock)
    .def("nonZeros",&mat_t::nonZeros)
    .def("nonZeroBlocks",&mat_t::nonZeroBlocks)
    .def("clone", &mat_t::clone, return_value_policy<manage_new_object>())
    .def("toDense", &mat_t::toDense)
    .def("toDenseSymmetric", &toDenseSymmetric<mat_t>)
      .def("at", &at<mat_t>)
    // Still more to do here, but this is the easy stuff that will allow us to look
    // at one of these things in Python.
    ;

}


/*
Copyright 2020 Standard Cyborg

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

#pragma once

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdocumentation"
#ifndef DEBUG
#define EIGEN_NO_DEBUG  1
#endif
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#pragma clang diagnostic pop


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 * Nicolas Mansard
 * Joseph Mirabel
 *
 * CNRS/AIST
 *
 */

#include <boost/function.hpp>

#include <sot/core/binary-op.hh>
#include <sot/core/unary-op.hh>
#include <sot/core/variadic-op.hh>

#include <sot/core/matrix-geometry.hh>

#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>

#include <boost/numeric/conversion/cast.hpp>
#include <deque>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>

#include "../tools/type-name-helper.hh"

namespace dg = ::dynamicgraph;

/* ---------------------------------------------------------------------------*/
/* ------- GENERIC HELPERS -------------------------------------------------- */
/* ---------------------------------------------------------------------------*/

#define ADD_COMMAND(name, def) commandMap.insert(std::make_pair(name, def))

namespace dynamicgraph {
namespace sot {
template <typename TypeIn, typename TypeOut> struct UnaryOpHeader {
  typedef TypeIn Tin;
  typedef TypeOut Tout;
  static inline std::string nameTypeIn(void) {
    return TypeNameHelper<Tin>::typeName();
  }
  static inline std::string nameTypeOut(void) {
    return TypeNameHelper<Tout>::typeName();
  }
  inline void addSpecificCommands(Entity &, Entity::CommandMap_t &) {}
  inline std::string getDocString() const {
    return std::string("Undocumented unary operator\n"
                       "  - input  ") +
           nameTypeIn() +
           std::string("\n"
                       "  - output ") +
           nameTypeOut() + std::string("\n");
  }
};

/* ---------------------------------------------------------------------- */
/* --- ALGEBRA SELECTORS ------------------------------------------------ */
/* ---------------------------------------------------------------------- */
struct VectorSelecter : public UnaryOpHeader<dg::Vector, dg::Vector> {
  inline void operator()(const Tin &m, Vector &res) const {
    res.resize(size);
    Vector::Index r = 0;
    for (std::size_t i = 0; i < idxs.size(); ++i) {
      const Vector::Index &R = idxs[i].first;
      const Vector::Index &nr = idxs[i].second;
      assert((nr >= 0) && (R + nr <= m.size()));
      res.segment(r, nr) = m.segment(R, nr);
      r += nr;
    }
    assert(r == size);
  }

  typedef std::pair<Vector::Index, Vector::Index> segment_t;
  typedef std::vector<segment_t> segments_t;
  segments_t idxs;
  Vector::Index size;

  inline void setBounds(const int &m, const int &M) {
    idxs = segments_t(1, segment_t(m, M - m));
    size = M - m;
  }
  inline void addBounds(const int &m, const int &M) {
    idxs.push_back(segment_t(m, M - m));
    size += M - m;
  }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    boost::function<void(const int &, const int &)> setBound =
        boost::bind(&VectorSelecter::setBounds, this, _1, _2);
    doc = docCommandVoid2("Set the bound of the selection [m,M[.", "int (min)",
                          "int (max)");
    ADD_COMMAND("selec", makeCommandVoid2(ent, setBound, doc));
    boost::function<void(const int &, const int &)> addBound =
        boost::bind(&VectorSelecter::addBounds, this, _1, _2);
    doc = docCommandVoid2("Add a segment to be selected [m,M[.", "int (min)",
                          "int (max)");
    ADD_COMMAND("addSelec", makeCommandVoid2(ent, addBound, doc));
  }
  VectorSelecter() : size(0) {}
};

/* ---------------------------------------------------------------------- */
struct VectorComponent : public UnaryOpHeader<dg::Vector, double> {
  inline void operator()(const Tin &m, double &res) const {
    assert(index < m.size());
    res = m(index);
  }

  int index;
  inline void setIndex(const int &m) { index = m; }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    std::string doc;

    boost::function<void(const int &)> callback =
        boost::bind(&VectorComponent::setIndex, this, _1);
    doc = command::docCommandVoid1("Set the index of the component.",
                                   "int (index)");
    ADD_COMMAND("setIndex", command::makeCommandVoid1(ent, callback, doc));
  }
  inline std::string getDocString() const {
    std::string docString("Select a component of a vector\n"
                          "  - input  vector\n"
                          "  - output double");
    return docString;
  }
};

/* ---------------------------------------------------------------------- */
struct MatrixSelector : public UnaryOpHeader<dg::Matrix, dg::Matrix> {
  inline void operator()(const Matrix &m, Matrix &res) const {
    assert((imin <= imax) && (imax <= m.rows()));
    assert((jmin <= jmax) && (jmax <= m.cols()));
    res.resize(imax - imin, jmax - jmin);
    for (int i = imin; i < imax; ++i)
      for (int j = jmin; j < jmax; ++j)
        res(i - imin, j - jmin) = m(i, j);
  }

public:
  int imin, imax;
  int jmin, jmax;

  inline void setBoundsRow(const int &m, const int &M) {
    imin = m;
    imax = M;
  }
  inline void setBoundsCol(const int &m, const int &M) {
    jmin = m;
    jmax = M;
  }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    boost::function<void(const int &, const int &)> setBoundsRow =
        boost::bind(&MatrixSelector::setBoundsRow, this, _1, _2);
    boost::function<void(const int &, const int &)> setBoundsCol =
        boost::bind(&MatrixSelector::setBoundsCol, this, _1, _2);

    doc = docCommandVoid2("Set the bound on rows.", "int (min)", "int (max)");
    ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc));

    doc = docCommandVoid2("Set the bound on cols [m,M[.", "int (min)",
                          "int (max)");
    ADD_COMMAND("selecCols", makeCommandVoid2(ent, setBoundsCol, doc));
  }
};

/* ---------------------------------------------------------------------- */
struct MatrixColumnSelector : public UnaryOpHeader<dg::Matrix, dg::Vector> {
public:
  inline void operator()(const Tin &m, Tout &res) const {
    assert((imin <= imax) && (imax <= m.rows()));
    assert(jcol < m.cols());

    res.resize(imax - imin);
    for (int i = imin; i < imax; ++i)
      res(i - imin) = m(i, jcol);
  }

  int imin, imax;
  int jcol;
  inline void selectCol(const int &m) { jcol = m; }
  inline void setBoundsRow(const int &m, const int &M) {
    imin = m;
    imax = M;
  }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    boost::function<void(const int &, const int &)> setBoundsRow =
        boost::bind(&MatrixColumnSelector::setBoundsRow, this, _1, _2);
    boost::function<void(const int &)> selectCol =
        boost::bind(&MatrixColumnSelector::selectCol, this, _1);

    doc = docCommandVoid2("Set the bound on rows.", "int (min)", "int (max)");
    ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc));

    doc = docCommandVoid1("Select the col to copy.", "int (col index)");
    ADD_COMMAND("selecCols", makeCommandVoid1(ent, selectCol, doc));
  }
};

/* ---------------------------------------------------------------------- */
struct MatrixTranspose : public UnaryOpHeader<dg::Matrix, dg::Matrix> {
  inline void operator()(const Tin &m, Tout &res) const { res = m.transpose(); }
};

/* ---------------------------------------------------------------------- */
struct Diagonalizer : public UnaryOpHeader<Vector, Matrix> {
  inline void operator()(const dg::Vector &r, dg::Matrix &res) {
    res = r.asDiagonal();
  }

public:
  Diagonalizer(void) : nbr(0), nbc(0) {}
  unsigned int nbr, nbc;
  inline void resize(const int &r, const int &c) {
    nbr = r;
    nbc = c;
  }
  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    boost::function<void(const int &, const int &)> resize =
        boost::bind(&Diagonalizer::resize, this, _1, _2);

    doc = docCommandVoid2("Set output size.", "int (row)", "int (col)");
    ADD_COMMAND("resize", makeCommandVoid2(ent, resize, doc));
  }
};

/* ---------------------------------------------------------------------- */
/* --- INVERSION -------------------------------------------------------- */
/* ---------------------------------------------------------------------- */

template <typename matrixgen>
struct Inverser : public UnaryOpHeader<matrixgen, matrixgen> {
  typedef typename UnaryOpHeader<matrixgen, matrixgen>::Tin Tin;
  typedef typename UnaryOpHeader<matrixgen, matrixgen>::Tout Tout;
  inline void operator()(const Tin &m, Tout &res) const { res = m.inverse(); }
};

struct Normalize : public UnaryOpHeader<dg::Vector, double> {
  inline void operator()(const dg::Vector &m, double &res) const {
    res = m.norm();
  }

  inline std::string getDocString() const {
    std::string docString("Computes the norm of a vector\n"
                          "  - input  vector\n"
                          "  - output double");
    return docString;
  }
};

struct InverserRotation : public UnaryOpHeader<MatrixRotation, MatrixRotation> {
  inline void operator()(const Tin &m, Tout &res) const { res = m.transpose(); }
};

struct InverserQuaternion
    : public UnaryOpHeader<VectorQuaternion, VectorQuaternion> {
  inline void operator()(const Tin &m, Tout &res) const { res = m.conjugate(); }
};

/* ----------------------------------------------------------------------- */
/* --- SE3/SO3 conversions ----------------------------------------------- */
/* ----------------------------------------------------------------------- */

struct MatrixHomoToPoseUTheta
    : public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
  inline void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
    res.resize(6);
    VectorUTheta r(M.linear());
    res.head<3>() = M.translation();
    res.tail<3>() = r.angle() * r.axis();
  }
};

struct SkewSymToVector : public UnaryOpHeader<Matrix, Vector> {
  inline void operator()(const Matrix &M, Vector &res) {
    res.resize(3);
    res(0) = M(7);
    res(1) = M(2);
    res(2) = M(3);
  }
};

struct PoseUThetaToMatrixHomo
    : public UnaryOpHeader<Vector, MatrixHomogeneous> {
  inline void operator()(const dg::Vector &v, MatrixHomogeneous &res) {
    assert(v.size() >= 6);
    res.translation() = v.head<3>();
    double theta = v.tail<3>().norm();
    if (theta > 0)
      res.linear() = Eigen::AngleAxisd(theta, v.tail<3>() / theta).matrix();
    else
      res.linear().setIdentity();
  }
};

struct SE3VectorToMatrixHomo
    : public UnaryOpHeader<dg::Vector, MatrixHomogeneous> {
  void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
    Mres.translation() = vect.head<3>();
    Mres.linear().row(0) = vect.segment(3, 3);
    Mres.linear().row(1) = vect.segment(6, 3);
    Mres.linear().row(2) = vect.segment(9, 3);
  }
};

struct MatrixHomoToSE3Vector
    : public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
  void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
    res.resize(12);
    res.head<3>() = M.translation();
    res.segment(3, 3) = M.linear().row(0);
    res.segment(6, 3) = M.linear().row(1);
    res.segment(9, 3) = M.linear().row(2);
  }
};

struct PoseQuaternionToMatrixHomo
    : public UnaryOpHeader<Vector, MatrixHomogeneous> {
  void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
    Mres.translation() = vect.head<3>();
    Mres.linear() = VectorQuaternion(vect.tail<4>()).toRotationMatrix();
  }
};

struct MatrixHomoToPoseQuaternion
    : public UnaryOpHeader<MatrixHomogeneous, Vector> {
  inline void operator()(const MatrixHomogeneous &M, Vector &res) {
    res.resize(7);
    res.head<3>() = M.translation();
    Eigen::Map<VectorQuaternion> q(res.tail<4>().data());
    q = M.linear();
  }
};

struct MatrixHomoToPoseRollPitchYaw
    : public UnaryOpHeader<MatrixHomogeneous, Vector> {
  inline void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
    VectorRollPitchYaw r = (M.linear().eulerAngles(2, 1, 0)).reverse();
    dg::Vector t(3);
    t = M.translation();
    res.resize(6);
    for (unsigned int i = 0; i < 3; ++i)
      res(i) = t(i);
    for (unsigned int i = 0; i < 3; ++i)
      res(i + 3) = r(i);
  }
};

struct PoseRollPitchYawToMatrixHomo
    : public UnaryOpHeader<Vector, MatrixHomogeneous> {
  inline void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {

    VectorRollPitchYaw r;
    for (unsigned int i = 0; i < 3; ++i)
      r(i) = vect(i + 3);
    MatrixRotation R = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
                        Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
                        Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
                           .toRotationMatrix();

    dg::Vector t(3);
    for (unsigned int i = 0; i < 3; ++i)
      t(i) = vect(i);

    // buildFrom(R,t);
    Mres = Eigen::Translation3d(t) * R;
  }
};

struct PoseRollPitchYawToPoseUTheta : public UnaryOpHeader<Vector, Vector> {
  inline void operator()(const dg::Vector &vect, dg::Vector &vectres) {
    VectorRollPitchYaw r;
    for (unsigned int i = 0; i < 3; ++i)
      r(i) = vect(i + 3);
    MatrixRotation R = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
                        Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
                        Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
                           .toRotationMatrix();

    VectorUTheta rrot(R);

    vectres.resize(6);
    for (unsigned int i = 0; i < 3; ++i) {
      vectres(i) = vect(i);
      vectres(i + 3) = rrot.angle() * rrot.axis()(i);
    }
  }
};

struct HomoToMatrix : public UnaryOpHeader<MatrixHomogeneous, Matrix> {
  inline void operator()(const MatrixHomogeneous &M, dg::Matrix &res) {
    res = M.matrix();
  }
};

struct MatrixToHomo : public UnaryOpHeader<Matrix, MatrixHomogeneous> {
  inline void operator()(const Eigen::Matrix<double, 4, 4> &M,
                         MatrixHomogeneous &res) {
    res = M;
  }
};

struct HomoToTwist : public UnaryOpHeader<MatrixHomogeneous, MatrixTwist> {
  inline void operator()(const MatrixHomogeneous &M, MatrixTwist &res) {
    Eigen::Vector3d _t = M.translation();
    MatrixRotation R(M.linear());
    Eigen::Matrix3d Tx;
    Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;

    Eigen::Matrix3d sk;
    sk = Tx * R;
    res.block<3, 3>(0, 0) = R;
    res.block<3, 3>(0, 3) = sk;
    res.block<3, 3>(3, 0) = Eigen::Matrix3d::Zero();
    res.block<3, 3>(3, 3) = R;
  }
};

struct HomoToRotation
    : public UnaryOpHeader<MatrixHomogeneous, MatrixRotation> {
  inline void operator()(const MatrixHomogeneous &M, MatrixRotation &res) {
    res = M.linear();
  }
};

struct MatrixHomoToPose : public UnaryOpHeader<MatrixHomogeneous, Vector> {
  inline void operator()(const MatrixHomogeneous &M, Vector &res) {
    res.resize(3);
    res = M.translation();
  }
};

struct RPYToMatrix : public UnaryOpHeader<VectorRollPitchYaw, MatrixRotation> {
  inline void operator()(const VectorRollPitchYaw &r, MatrixRotation &res) {
    res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
           Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
           Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
              .toRotationMatrix();
  }
};

struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> {
  inline void operator()(const MatrixRotation &r, VectorRollPitchYaw &res) {
    res = (r.eulerAngles(2, 1, 0)).reverse();
  }
};

struct RPYToQuaternion
    : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaternion> {
  inline void operator()(const VectorRollPitchYaw &r, VectorQuaternion &res) {
    res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
           Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
           Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
              .toRotationMatrix();
  }
};

struct QuaternionToRPY
    : public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> {
  inline void operator()(const VectorQuaternion &r, VectorRollPitchYaw &res) {
    res = (r.toRotationMatrix().eulerAngles(2, 1, 0)).reverse();
  }
};

struct QuaternionToMatrix
    : public UnaryOpHeader<VectorQuaternion, MatrixRotation> {
  inline void operator()(const VectorQuaternion &r, MatrixRotation &res) {
    res = r.toRotationMatrix();
  }
};

struct MatrixToQuaternion
    : public UnaryOpHeader<MatrixRotation, VectorQuaternion> {
  inline void operator()(const MatrixRotation &r, VectorQuaternion &res) {
    res = r;
  }
};

struct MatrixToUTheta : public UnaryOpHeader<MatrixRotation, VectorUTheta> {
  inline void operator()(const MatrixRotation &r, VectorUTheta &res) {
    res = r;
  }
};

struct UThetaToQuaternion
    : public UnaryOpHeader<VectorUTheta, VectorQuaternion> {
  inline void operator()(const VectorUTheta &r, VectorQuaternion &res) {
    res = r;
  }
};

template <typename TypeIn1, typename TypeIn2, typename TypeOut>
struct BinaryOpHeader {
  typedef TypeIn1 Tin1;
  typedef TypeIn2 Tin2;
  typedef TypeOut Tout;
  inline static std::string nameTypeIn1(void) {
    return TypeNameHelper<Tin1>::typeName();
  }
  inline static std::string nameTypeIn2(void) {
    return TypeNameHelper<Tin2>::typeName();
  }
  inline static std::string nameTypeOut(void) {
    return TypeNameHelper<Tout>::typeName();
  }
  inline void addSpecificCommands(Entity &, Entity::CommandMap_t &) {}
  inline std::string getDocString() const {
    return std::string("Undocumented binary operator\n"
                       "  - input  ") +
           nameTypeIn1() +
           std::string("\n"
                       "  -        ") +
           nameTypeIn2() +
           std::string("\n"
                       "  - output ") +
           nameTypeOut() + std::string("\n");
  }
};

} /* namespace sot */
} /* namespace dynamicgraph */

/* ---------------------------------------------------------------------------*/
/* ---------------------------------------------------------------------------*/
/* ---------------------------------------------------------------------------*/

namespace dynamicgraph {
namespace sot {

/* --- MULTIPLICATION --------------------------------------------------- */

template <typename F, typename E>
struct Multiplier_FxE__E : public BinaryOpHeader<F, E, E> {
  inline void operator()(const F &f, const E &e, E &res) const { res = f * e; }
};

template <>
inline void
Multiplier_FxE__E<dynamicgraph::sot::MatrixHomogeneous, dynamicgraph::Vector>::
operator()(const dynamicgraph::sot::MatrixHomogeneous &f,
           const dynamicgraph::Vector &e, dynamicgraph::Vector &res) const {
  res = f.matrix() * e;
}

template <>
inline void Multiplier_FxE__E<double, dynamicgraph::Vector>::
operator()(const double &x, const dynamicgraph::Vector &v,
           dynamicgraph::Vector &res) const {
  res = v;
  res *= x;
}

typedef Multiplier_FxE__E<double, dynamicgraph::Vector>
    Multiplier_double_vector;
typedef Multiplier_FxE__E<dynamicgraph::Matrix, dynamicgraph::Vector>
    Multiplier_matrix_vector;
typedef Multiplier_FxE__E<MatrixHomogeneous, dynamicgraph::Vector>
    Multiplier_matrixHomo_vector;
typedef Multiplier_FxE__E<MatrixTwist, dynamicgraph::Vector>
    Multiplier_matrixTwist_vector;

/* --- SUBSTRACTION ----------------------------------------------------- */
template <typename T> struct Substraction : public BinaryOpHeader<T, T, T> {
  inline void operator()(const T &v1, const T &v2, T &r) const {
    r = v1;
    r -= v2;
  }
};

/* --- STACK ------------------------------------------------------------ */
struct VectorStack
    : public BinaryOpHeader<dynamicgraph::Vector, dynamicgraph::Vector,
                            dynamicgraph::Vector> {
public:
  int v1min, v1max;
  int v2min, v2max;
  inline void operator()(const dynamicgraph::Vector &v1,
                         const dynamicgraph::Vector &v2,
                         dynamicgraph::Vector &res) const {
    assert((v1max >= v1min) && (v1.size() >= v1max));
    assert((v2max >= v2min) && (v2.size() >= v2max));

    const int v1size = v1max - v1min, v2size = v2max - v2min;
    res.resize(v1size + v2size);
    for (int i = 0; i < v1size; ++i) {
      res(i) = v1(i + v1min);
    }
    for (int i = 0; i < v2size; ++i) {
      res(v1size + i) = v2(i + v2min);
    }
  }

  inline void selec1(const int &m, const int M) {
    v1min = m;
    v1max = M;
  }
  inline void selec2(const int &m, const int M) {
    v2min = m;
    v2max = M;
  }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    boost::function<void(const int &, const int &)> selec1 =
        boost::bind(&VectorStack::selec1, this, _1, _2);
    boost::function<void(const int &, const int &)> selec2 =
        boost::bind(&VectorStack::selec2, this, _1, _2);

    ADD_COMMAND(
        "selec1",
        makeCommandVoid2(ent, selec1,
                         docCommandVoid2("set the min and max of selection.",
                                         "int (imin)", "int (imax)")));
    ADD_COMMAND(
        "selec2",
        makeCommandVoid2(ent, selec2,
                         docCommandVoid2("set the min and max of selection.",
                                         "int (imin)", "int (imax)")));
  }
};

/* ---------------------------------------------------------------------- */

struct Composer
    : public BinaryOpHeader<dynamicgraph::Matrix, dynamicgraph::Vector,
                            MatrixHomogeneous> {
  inline void operator()(const dynamicgraph::Matrix &R,
                         const dynamicgraph::Vector &t,
                         MatrixHomogeneous &H) const {
    H.linear() = R;
    H.translation() = t;
  }
};

/* --- CONVOLUTION PRODUCT ---------------------------------------------- */
struct ConvolutionTemporal
    : public BinaryOpHeader<dynamicgraph::Vector, dynamicgraph::Matrix,
                            dynamicgraph::Vector> {
  typedef std::deque<dynamicgraph::Vector> MemoryType;
  MemoryType memory;

  inline void convolution(const MemoryType &f1, const dynamicgraph::Matrix &f2,
                          dynamicgraph::Vector &res) {
    const Vector::Index nconv = (Vector::Index)f1.size(), nsig = f2.rows();
    sotDEBUG(15) << "Size: " << nconv << "x" << nsig << std::endl;
    if (nconv > f2.cols())
      return; // TODO: error, this should not happen

    res.resize(nsig);
    res.fill(0);
    unsigned int j = 0;
    for (MemoryType::const_iterator iter = f1.begin(); iter != f1.end();
         iter++) {
      const dynamicgraph::Vector &s_tau = *iter;
      sotDEBUG(45) << "Sig" << j << ": " << s_tau;
      if (s_tau.size() != nsig)
        return; // TODO: error throw;
      for (int i = 0; i < nsig; ++i) {
        res(i) += f2(i, j) * s_tau(i);
      }
      j++;
    }
  }
  inline void operator()(const dynamicgraph::Vector &v1,
                         const dynamicgraph::Matrix &m2,
                         dynamicgraph::Vector &res) {
    memory.push_front(v1);
    while ((Vector::Index)memory.size() > m2.cols())
      memory.pop_back();
    convolution(memory, m2, res);
  }
};

/* --- BOOLEAN REDUCTION ------------------------------------------------ */

template <typename T> struct Comparison : public BinaryOpHeader<T, T, bool> {
  inline void operator()(const T &a, const T &b, bool &res) const {
    res = (a < b);
  }
  inline std::string getDocString() const {
    typedef BinaryOpHeader<T, T, bool> Base;
    return std::string("Comparison of inputs:\n"
                       "  - input  ") +
           Base::nameTypeIn1() +
           std::string("\n"
                       "  -        ") +
           Base::nameTypeIn2() +
           std::string("\n"
                       "  - output ") +
           Base::nameTypeOut() +
           std::string("\n"
                       "  sout = ( sin1 < sin2 )\n");
  }
};

template <typename T1, typename T2 = T1>
struct MatrixComparison : public BinaryOpHeader<T1, T2, bool> {
  // TODO T1 or T2 could be a scalar type.
  inline void operator()(const T1 &a, const T2 &b, bool &res) const {
    if (equal && any)
      res = (a.array() <= b.array()).any();
    else if (equal && !any)
      res = (a.array() <= b.array()).all();
    else if (!equal && any)
      res = (a.array() < b.array()).any();
    else if (!equal && !any)
      res = (a.array() < b.array()).all();
  }
  inline std::string getDocString() const {
    typedef BinaryOpHeader<T1, T2, bool> Base;
    return std::string("Comparison of inputs:\n"
                       "  - input  ") +
           Base::nameTypeIn1() +
           std::string("\n"
                       "  -        ") +
           Base::nameTypeIn2() +
           std::string("\n"
                       "  - output ") +
           Base::nameTypeOut() +
           std::string("\n"
                       "  sout = ( sin1 < sin2 ).op()\n") +
           std::string("\n"
                       "  where op is either any (default) or all. The "
                       "comparison can be made <=.\n");
  }
  MatrixComparison() : any(true), equal(false) {}
  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    ADD_COMMAND(
        "setTrueIfAny",
        makeDirectSetter(ent, &any, docDirectSetter("trueIfAny", "bool")));
    ADD_COMMAND(
        "getTrueIfAny",
        makeDirectGetter(ent, &any, docDirectGetter("trueIfAny", "bool")));
    ADD_COMMAND("setEqual", makeDirectSetter(ent, &equal,
                                             docDirectSetter("equal", "bool")));
    ADD_COMMAND("getEqual", makeDirectGetter(ent, &equal,
                                             docDirectGetter("equal", "bool")));
  }
  bool any, equal;
};

} /* namespace sot */
} /* namespace dynamicgraph */

namespace dynamicgraph {
namespace sot {

template <typename T> struct WeightedAdder : public BinaryOpHeader<T, T, T> {
public:
  double gain1, gain2;
  inline void operator()(const T &v1, const T &v2, T &res) const {
    res = v1;
    res *= gain1;
    res += gain2 * v2;
  }

  inline void addSpecificCommands(Entity &ent,
                                  Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    std::string doc;

    ADD_COMMAND(
        "setGain1",
        makeDirectSetter(ent, &gain1, docDirectSetter("gain1", "double")));
    ADD_COMMAND(
        "setGain2",
        makeDirectSetter(ent, &gain2, docDirectSetter("gain2", "double")));
    ADD_COMMAND(
        "getGain1",
        makeDirectGetter(ent, &gain1, docDirectGetter("gain1", "double")));
    ADD_COMMAND(
        "getGain2",
        makeDirectGetter(ent, &gain2, docDirectGetter("gain2", "double")));
  }

  inline std::string getDocString() const {
    return std::string("Weighted Combination of inputs : \n - gain{1|2} gain.");
  }
};

} // namespace sot
} // namespace dynamicgraph

namespace dynamicgraph {
namespace sot {
template <typename Tin, typename Tout, typename Time>
std::string VariadicAbstract<Tin, Tout, Time>::getTypeInName(void) {
  return TypeNameHelper<Tin>::typeName();
}
template <typename Tin, typename Tout, typename Time>
std::string VariadicAbstract<Tin, Tout, Time>::getTypeOutName(void) {
  return TypeNameHelper<Tout>::typeName();
}

template <typename TypeIn, typename TypeOut> struct VariadicOpHeader {
  typedef TypeIn Tin;
  typedef TypeOut Tout;
  inline static std::string nameTypeIn(void) {
    return TypeNameHelper<Tin>::typeName();
  }
  inline static std::string nameTypeOut(void) {
    return TypeNameHelper<Tout>::typeName();
  }
  template <typename Op>
  inline void initialize(VariadicOp<Op> *, Entity::CommandMap_t &) {}
  inline void updateSignalNumber(const int &) {}
  inline std::string getDocString() const {
    return std::string("Undocumented variadic operator\n"
                       "  - input  " +
                       nameTypeIn() +
                       "\n"
                       "  - output " +
                       nameTypeOut() + "\n");
  }
};

/* --- VectorMix ------------------------------------------------------------ */
struct VectorMix : public VariadicOpHeader<Vector, Vector> {
public:
  typedef VariadicOp<VectorMix> Base;
  struct segment_t {
    Vector::Index index, size, input;
    std::size_t sigIdx;
    segment_t(Vector::Index i, Vector::Index s, std::size_t sig)
        : index(i), size(s), sigIdx(sig) {}
  };
  typedef std::vector<segment_t> segments_t;
  Base *entity;
  segments_t idxs;
  inline void operator()(const std::vector<const Vector *> &vs,
                         Vector &res) const {
    res = *vs[0];
    for (std::size_t i = 0; i < idxs.size(); ++i) {
      const segment_t &s = idxs[i];
      if (s.sigIdx >= vs.size())
        throw std::invalid_argument("Index out of range in VectorMix");
      res.segment(s.index, s.size) = *vs[s.sigIdx];
    }
  }

  inline void addSelec(const int &sigIdx, const int &i, const int &s) {
    idxs.push_back(segment_t(i, s, sigIdx));
  }

  inline void initialize(Base *ent, Entity::CommandMap_t &commandMap) {
    using namespace dynamicgraph::command;
    entity = ent;

    ent->addSignal("default");

    boost::function<void(const int &, const int &, const int &)> selec =
        boost::bind(&VectorMix::addSelec, this, _1, _2, _3);

    commandMap.insert(std::make_pair(
        "addSelec", makeCommandVoid3<Base, int, int, int>(
                        *ent, selec,
                        docCommandVoid3("add selection from a vector.",
                                        "int (signal index >= 1)",
                                        "int (index)", "int (size)"))));
  }
};

/* --- ADDITION --------------------------------------------------------- */
template <typename T> struct AdderVariadic : public VariadicOpHeader<T, T> {
  typedef VariadicOp<AdderVariadic> Base;

  Base *entity;
  Vector coeffs;

  AdderVariadic() : coeffs() {}
  inline void operator()(const std::vector<const T *> &vs, T &res) const {
    assert(vs.size() == (std::size_t)coeffs.size());
    if (vs.size() == 0)
      return;
    res = coeffs[0] * (*vs[0]);
    for (std::size_t i = 1; i < vs.size(); ++i)
      res += coeffs[i] * (*vs[i]);
  }

  inline void setCoeffs(const Vector &c) {
    if (entity->getSignalNumber() != c.size())
      throw std::invalid_argument("Invalid coefficient size.");
    coeffs = c;
  }
  inline void updateSignalNumber(const int &n) { coeffs = Vector::Ones(n); }

  inline void initialize(Base *ent, Entity::CommandMap_t &) {
    entity = ent;
    entity->setSignalNumber(2);
  }

  inline std::string getDocString() const {
    return "Linear combination of inputs\n"
           "  - input  " +
           VariadicOpHeader<T, T>::nameTypeIn() +
           "\n"
           "  - output " +
           VariadicOpHeader<T, T>::nameTypeOut() +
           "\n"
           "  sout = sum ([coeffs[i] * sin[i] for i in range(n) ])\n"
           "  Coefficients are set by commands, default value is 1.\n";
  }
};

/* --- MULTIPLICATION --------------------------------------------------- */
template <typename T> struct Multiplier : public VariadicOpHeader<T, T> {
  typedef VariadicOp<Multiplier> Base;

  inline void operator()(const std::vector<const T *> &vs, T &res) const {
    if (vs.size() == 0)
      setIdentity(res);
    else {
      res = *vs[0];
      for (std::size_t i = 1; i < vs.size(); ++i)
        res *= *vs[i];
    }
  }

  inline void setIdentity(T &res) const { res.setIdentity(); }

  inline void initialize(Base *ent, Entity::CommandMap_t &) {
    ent->setSignalNumber(2);
  }
};
template <> inline void Multiplier<double>::setIdentity(double &res) const {
  res = 1;
}
template <>
inline void Multiplier<MatrixHomogeneous>::
operator()(const std::vector<const MatrixHomogeneous *> &vs,
           MatrixHomogeneous &res) const {
  if (vs.size() == 0)
    setIdentity(res);
  else {
    res = *vs[0];
    for (std::size_t i = 1; i < vs.size(); ++i)
      res = res * *vs[i];
  }
}
template <>
inline void Multiplier<Vector>::
operator()(const std::vector<const Vector *> &vs, Vector &res) const {
  if (vs.size() == 0)
    res.resize(0);
  else {
    res = *vs[0];
    for (std::size_t i = 1; i < vs.size(); ++i)
      res.array() *= vs[i]->array();
  }
}

/* --- BOOLEAN --------------------------------------------------------- */
template <int operation> struct BoolOp : public VariadicOpHeader<bool, bool> {
  typedef VariadicOp<BoolOp> Base;

  inline void operator()(const std::vector<const bool *> &vs, bool &res) const {
    // TODO computation could be optimized with lazy evaluation of the
    // signals. When the output result is know, the remaining signals are
    // not computed.
    if (vs.size() == 0)
      return;
    res = *vs[0];
    for (std::size_t i = 1; i < vs.size(); ++i)
      switch (operation) {
      case 0:
        if (!res)
          return;
        res = *vs[i];
        break;
      case 1:
        if (res)
          return;
        res = *vs[i];
        break;
      }
  }
};

} // namespace sot
} // namespace dynamicgraph

/* --- TODO ------------------------------------------------------------------*/
// The following commented lines are sot-v1 entities that are still waiting
//   for conversion. Help yourself!

// /* --------------------------------------------------------------------------
// */

// struct WeightedDirection
// {
// public:
//   void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector&
//   v2,dynamicgraph::Vector& res ) const
//   {
//     const double norm1 = v1.norm();
//     const double norm2 = v2.norm();
//     res=v2; res*=norm1;
//     res*= (1/norm2);
//   }
// };
// typedef BinaryOp< Vector,Vector,Vector,WeightedDirection > weightdir;
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(weightdir,vector,weight_dir,"WeightDir")

// /* --------------------------------------------------------------------------
// */

// struct Nullificator
// {
// public:
//   void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector&
//   v2,dynamicgraph::Vector& res ) const
//   {
//     const unsigned int s = std::max( v1.size(),v2.size() );
//     res.resize(s);
//     for( unsigned int i=0;i<s;++i )
//       {
// 	if( v1(i)>v2(i) ) res(i)=v1(i)-v2(i);
// 	else 	if( v1(i)<-v2(i) ) res(i)=v1(i)+v2(i);
// 	else res(i)=0;
//       }
//   }
// };
// typedef BinaryOp< Vector,Vector,Vector,Nullificator > vectNil;
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(vectNil,vector,vectnil_,"Nullificator")

// /* --------------------------------------------------------------------------
// */

// struct VirtualSpring
// {
// public:
//   double spring;

//   void operator()( const dynamicgraph::Vector& pos,const
//   dynamicgraph::Vector& ref,dynamicgraph::Vector& res ) const
//   {
//     double norm = ref.norm();
//     double dist = ref.scalarProduct(pos) / (norm*norm);

//     res.resize( ref.size() );
//     res = ref;  res *= dist; res -= pos;
//     res *= spring;
//   }
// };
// typedef BinaryOp< Vector,Vector,Vector,VirtualSpring > virtspring;
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E_CMD
// (virtspring,vector,virtspring_,
//  "VirtualSpring"
//  ,else if( cmdLine=="spring" ){  CMDARGS_INOUT(op.spring); }
//  ,"VirtualSpring<pos,ref> compute the virtual force of a spring attache "
//  "to the reference line <ref>. The eq is: k.(<ref|pos>/<ref|ref>.ref-pos)"
//  "Params:\n  - spring: get/set the spring factor.")


/* cpuid_test.cc
   Jeremy Barnes, 21 February 2007
   Copyright (c) 2007 Jeremy Barnes.  All rights reserved.

   Test of the CPUID detection code.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include "jml/arch/simd_vector.h"
#include "jml/arch/demangle.h"

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <vector>
#include <set>
#include <iostream>
#include <cmath>

using namespace ML;
using namespace std;

using boost::unit_test::test_suite;

BOOST_AUTO_TEST_CASE( test1 )
{
    BOOST_CHECK(cpuid_flags() != 0);
}

void vec_scale_k_test_case(int nvals)
{
    cerr << "testing vec_scale with nvals: " << nvals << endl;

    double x[nvals], r[nvals], r2[nvals];

    double k = 3.0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        r2[i] = x[i] * k;
    }

    SIMD::vec_scale(x, k, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_scale_k_mixed_test_case )
{
    vec_scale_k_test_case(1);
    vec_scale_k_test_case(2);
    vec_scale_k_test_case(3);
    vec_scale_k_test_case(4);
    vec_scale_k_test_case(5);
    vec_scale_k_test_case(12);
    vec_scale_k_test_case(16);
    vec_scale_k_test_case(123);
}

void vec_add_k_mixed_test_case(int nvals)
{
    cerr << "testing " << nvals << endl;

    double x[nvals], r[nvals], r2[nvals];
    float y[nvals];

    double k = 3.0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] + k * y[i];
    }

    SIMD::vec_add(x, k, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_add_k_mixed_test )
{
    vec_add_k_mixed_test_case(1);
    vec_add_k_mixed_test_case(2);
    vec_add_k_mixed_test_case(3);
    vec_add_k_mixed_test_case(4);
    vec_add_k_mixed_test_case(5);
    vec_add_k_mixed_test_case(8);
    vec_add_k_mixed_test_case(9);
    vec_add_k_mixed_test_case(12);
    vec_add_k_mixed_test_case(16);
    vec_add_k_mixed_test_case(123);
}

void vec_add_k_double_test_case(int nvals)
{
    cerr << "testing " << nvals << endl;

    double x[nvals], r[nvals], r2[nvals], y[nvals];

    double k = 3.0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] + k * y[i];
    }

    SIMD::vec_add(x, k, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_add_k_double_test )
{
    vec_add_k_double_test_case(1);
    vec_add_k_double_test_case(2);
    vec_add_k_double_test_case(3);
    vec_add_k_double_test_case(4);
    vec_add_k_double_test_case(5);
    vec_add_k_double_test_case(8);
    vec_add_k_double_test_case(9);
    vec_add_k_double_test_case(12);
    vec_add_k_double_test_case(16);
    vec_add_k_double_test_case(123);
}

void vec_add_k_float_test_case(int nvals)
{
    cerr << "testing " << nvals << endl;

    float x[nvals], r[nvals], r2[nvals], y[nvals];

    float k = 3.0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] + k * y[i];
    }

    SIMD::vec_add(x, k, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_add_k_float_test )
{
    vec_add_k_float_test_case(1);
    vec_add_k_float_test_case(2);
    vec_add_k_float_test_case(3);
    vec_add_k_float_test_case(4);
    vec_add_k_float_test_case(5);
    vec_add_k_float_test_case(8);
    vec_add_k_float_test_case(9);
    vec_add_k_float_test_case(12);
    vec_add_k_float_test_case(16);
    vec_add_k_float_test_case(123);
}

void vec_prod_double_test_case(int nvals)
{
    cerr << "testing " << nvals << endl;

    double x[nvals], r[nvals], r2[nvals], y[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] * y[i];
    }

    SIMD::vec_prod(x, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_prod_double_test )
{
    vec_prod_double_test_case(1);
    vec_prod_double_test_case(2);
    vec_prod_double_test_case(3);
    vec_prod_double_test_case(4);
    vec_prod_double_test_case(5);
    vec_prod_double_test_case(8);
    vec_prod_double_test_case(9);
    vec_prod_double_test_case(12);
    vec_prod_double_test_case(16);
    vec_prod_double_test_case(123);
}

void vec_prod_float_test_case(int nvals)
{
    cerr << "testing " << nvals << endl;

    float x[nvals], r[nvals], r2[nvals], y[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] * y[i];
    }

    SIMD::vec_prod(x, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_prod_float_test )
{
    vec_prod_float_test_case(1);
    vec_prod_float_test_case(2);
    vec_prod_float_test_case(3);
    vec_prod_float_test_case(4);
    vec_prod_float_test_case(5);
    vec_prod_float_test_case(8);
    vec_prod_float_test_case(9);
    vec_prod_float_test_case(12);
    vec_prod_float_test_case(16);
    vec_prod_float_test_case(123);
}

float get_eps(float)
{
    return 1e-7;
}

double get_eps(double)
{
    return 1e-10;
}

template<typename T>
void vec_dotprod_test_case(int nvals)
{
    cerr << "nvals = " << nvals << endl;

    T x[nvals], y[nvals], r = 0.0, r2;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r += x[i] * y[i];
    }

    r2 = SIMD::vec_dotprod(x, y, nvals);

    T eps = get_eps(T());
    BOOST_CHECK(fabs(r - r2) / max(fabs(r), fabs(r2)) < eps);
}

template<typename T>
void vec_dotprod_dp_test_case(int nvals)
{
    T x[nvals], y[nvals];
    double r = 0.0, r2;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r += x[i] * y[i];
    }

    r2 = SIMD::vec_dotprod_dp(x, y, nvals);

    T eps = get_eps(T());
    if (fabs(r - r2) / max(fabs(r), fabs(r2)) >= eps) {
        cerr << "difference = " << (abs(r - r2) / max(abs(r), abs(r2)))
             << endl;
        BOOST_CHECK_EQUAL(r, r2);
    }
    BOOST_CHECK(fabs(r - r2) / max(fabs(r), fabs(r2)) < eps);
}

void vec_dotprod_dp_mixed_test_case(int nvals)
{
    double x[nvals];
    float y[nvals];
    double r = 0.0, r2;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r += x[i] * y[i];
    }

    r2 = SIMD::vec_dotprod_dp(x, y, nvals);

    double eps = get_eps(0.0);
    if (fabs(r - r2) / max(fabs(r), fabs(r2)) >= eps) {
        cerr << "difference = " << (abs(r - r2) / max(abs(r), abs(r2)))
             << endl;
        BOOST_CHECK_EQUAL(r, r2);
    }
    BOOST_CHECK(fabs(r - r2) / max(fabs(r), fabs(r2)) < eps);
}

BOOST_AUTO_TEST_CASE(vec_dotprod_test)
{
    cerr << "float" << endl;
    vec_dotprod_test_case<float>(1);
    vec_dotprod_test_case<float>(2);
    vec_dotprod_test_case<float>(3);
    vec_dotprod_test_case<float>(4);
    vec_dotprod_test_case<float>(5);
    vec_dotprod_test_case<float>(8);
    vec_dotprod_test_case<float>(9);
    vec_dotprod_test_case<float>(12);
    vec_dotprod_test_case<float>(16);
    vec_dotprod_test_case<float>(123);

    cerr << "float dp" << endl;
    vec_dotprod_dp_test_case<float>(1);
    vec_dotprod_dp_test_case<float>(2);
    vec_dotprod_dp_test_case<float>(3);
    vec_dotprod_dp_test_case<float>(4);
    vec_dotprod_dp_test_case<float>(5);
    vec_dotprod_dp_test_case<float>(8);
    vec_dotprod_dp_test_case<float>(9);
    vec_dotprod_dp_test_case<float>(12);
    vec_dotprod_dp_test_case<float>(16);
    vec_dotprod_dp_test_case<float>(123);

    cerr << "double" << endl;
    vec_dotprod_test_case<double>(1);
    vec_dotprod_test_case<double>(2);
    vec_dotprod_test_case<double>(3);
    vec_dotprod_test_case<double>(4);
    vec_dotprod_test_case<double>(5);
    vec_dotprod_test_case<double>(8);
    vec_dotprod_test_case<double>(9);
    vec_dotprod_test_case<double>(12);
    vec_dotprod_test_case<double>(16);
    vec_dotprod_test_case<double>(123);

    cerr << "mixed" << endl;
    vec_dotprod_dp_mixed_test_case(1);
    vec_dotprod_dp_mixed_test_case(2);
    vec_dotprod_dp_mixed_test_case(3);
    vec_dotprod_dp_mixed_test_case(4);
    vec_dotprod_dp_mixed_test_case(5);
    vec_dotprod_dp_mixed_test_case(8);
    vec_dotprod_dp_mixed_test_case(9);
    vec_dotprod_dp_mixed_test_case(12);
    vec_dotprod_dp_mixed_test_case(16);
    vec_dotprod_dp_mixed_test_case(123);
}

template<typename T1, typename T2>
void vec_accum_prod3_test_case(int nvals)
{
    cerr << "testing vec_accum_prod3 nvals " << nvals << " T1 "
         << demangle(typeid(T1).name())
         << " T2 " << demangle(typeid(T2).name())
         << endl;

    T1 x[nvals], y[nvals];
    T2 z[nvals];
    double r = 0.0, r2;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        z[i] = rand() / 16384.0;
        r += x[i] * y[i] * z[i];
    }

    r2 = SIMD::vec_accum_prod3(x, y, z, nvals);

    T1 eps = get_eps(T1());
    if (fabs(r - r2) / max(fabs(r), fabs(r2)) >= eps) {
        cerr << "difference = " << (abs(r - r2) / max(abs(r), abs(r2)))
             << endl;
        BOOST_CHECK_EQUAL(r, r2);
    }
    BOOST_CHECK(fabs(r - r2) / max(fabs(r), fabs(r2)) < eps);
}

template<typename Float1, typename Float2>
void vec_accum_prod3_test_cases()
{
    vec_accum_prod3_test_case<Float1, Float2>(1);
    vec_accum_prod3_test_case<Float1, Float2>(2);
    vec_accum_prod3_test_case<Float1, Float2>(3);
    vec_accum_prod3_test_case<Float1, Float2>(4);
    vec_accum_prod3_test_case<Float1, Float2>(5);
    vec_accum_prod3_test_case<Float1, Float2>(8);
    vec_accum_prod3_test_case<Float1, Float2>(9);
    vec_accum_prod3_test_case<Float1, Float2>(12);
    vec_accum_prod3_test_case<Float1, Float2>(16);
    vec_accum_prod3_test_case<Float1, Float2>(123);
}

BOOST_AUTO_TEST_CASE(vec_accum_prod3_test)
{
    vec_accum_prod3_test_cases<float, float>();
    vec_accum_prod3_test_cases<float, double>();
    vec_accum_prod3_test_cases<double, float>();
    vec_accum_prod3_test_cases<double, double>();
}

template<typename Float1, typename Float2>
void vec_add_sqr_test_case(int nvals)
{
    cerr << "testing vec_add_sqr nvals " << nvals << " float1 "
         << demangle(typeid(Float1).name())
         << " float2 " << demangle(typeid(Float2).name())
         << endl;

    Float1 x[nvals], r[nvals], r2[nvals];
    Float2 y[nvals];

    Float1 k = 3.0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] + k * (y[i] * y[i]);
    }

    SIMD::vec_add_sqr(x, k, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        if (r[i] != r2[i]) ;

        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

template<typename Float1, typename Float2>
void vec_add_sqr_test_cases()
{
    vec_add_sqr_test_case<Float1, Float2>(1);
    vec_add_sqr_test_case<Float1, Float2>(2);
    vec_add_sqr_test_case<Float1, Float2>(3);
    vec_add_sqr_test_case<Float1, Float2>(4);
    vec_add_sqr_test_case<Float1, Float2>(5);
    vec_add_sqr_test_case<Float1, Float2>(8);
    vec_add_sqr_test_case<Float1, Float2>(9);
    vec_add_sqr_test_case<Float1, Float2>(12);
    vec_add_sqr_test_case<Float1, Float2>(16);
    vec_add_sqr_test_case<Float1, Float2>(123);
}

BOOST_AUTO_TEST_CASE( vec_add_sqr_test )
{
    vec_add_sqr_test_cases<float, float>();
    vec_add_sqr_test_cases<float, double>();
    vec_add_sqr_test_cases<double, float>();
    vec_add_sqr_test_cases<double, double>();
}

template<typename Float1, typename Float2, typename Float3>
void vec_add_3array_test_case(int nvals)
{
    cerr << "testing vec_add_sqr nvals " << nvals << " float1 "
         << demangle(typeid(Float1).name())
         << " float2 " << demangle(typeid(Float2).name())
         << " float3 " << demangle(typeid(Float3).name())
         << endl;

    Float1 x[nvals], r[nvals], r2[nvals];
    Float2 y[nvals];
    Float3 k[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        k[i] = rand() / 16384.0;
        r2[i] = x[i] + k[i] * y[i];
    }

    SIMD::vec_add(x, k, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        if (r[i] != r2[i]) cerr << "difference on element " << i << " of "
                                << nvals << endl;;

        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

template<typename Float1, typename Float2, typename Float3>
void vec_add_3array_test_cases()
{
    vec_add_3array_test_case<Float1, Float2, Float3>(1);
    vec_add_3array_test_case<Float1, Float2, Float3>(2);
    vec_add_3array_test_case<Float1, Float2, Float3>(3);
    vec_add_3array_test_case<Float1, Float2, Float3>(4);
    vec_add_3array_test_case<Float1, Float2, Float3>(5);
    vec_add_3array_test_case<Float1, Float2, Float3>(8);
    vec_add_3array_test_case<Float1, Float2, Float3>(9);
    vec_add_3array_test_case<Float1, Float2, Float3>(12);
    vec_add_3array_test_case<Float1, Float2, Float3>(15);
    vec_add_3array_test_case<Float1, Float2, Float3>(16);
    vec_add_3array_test_case<Float1, Float2, Float3>(17);
    vec_add_3array_test_case<Float1, Float2, Float3>(123);
}

BOOST_AUTO_TEST_CASE( vec_add_3array_test )
{
    vec_add_3array_test_cases<float, float, float>();
    vec_add_3array_test_cases<float, double, float>();
    vec_add_3array_test_cases<double, float, float>();
    vec_add_3array_test_cases<double, double, float>();
    vec_add_3array_test_cases<float, float, double>();
    vec_add_3array_test_cases<float, double, double>();
    vec_add_3array_test_cases<double, float, double>();
    vec_add_3array_test_cases<double, double, double>();
}

template<typename Float, typename Precision>
void vec_exp_test_case(int nvals)
{
    cerr << "testing vec_exp " << nvals << " float "
         << demangle(typeid(Float).name())
         << " precision " << demangle(typeid(Precision).name())
         << endl;

    Float x[nvals];
    Precision r[nvals], r2[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0 / 1024.0;
        r2[i] = exp(Precision(x[i]));
    }

    SIMD::vec_exp(x, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        if (r[i] != r2[i]) cerr << "difference on element " << i << " of "
                                << nvals << endl;;

        // ./arch/testing/simd_vector_test.cc(515): error in "vec_exp_test": check r[i] == r2[i] failed [1.21850755e+20 != 1.21850746e+20]
        //BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

template<typename Float, typename Precision>
void vec_exp_test_cases()
{
    vec_exp_test_case<Float, Precision>(1);
    vec_exp_test_case<Float, Precision>(2);
    vec_exp_test_case<Float, Precision>(3);
    vec_exp_test_case<Float, Precision>(4);
    vec_exp_test_case<Float, Precision>(5);
    vec_exp_test_case<Float, Precision>(8);
    vec_exp_test_case<Float, Precision>(9);
    vec_exp_test_case<Float, Precision>(12);
    vec_exp_test_case<Float, Precision>(15);
    vec_exp_test_case<Float, Precision>(16);
    vec_exp_test_case<Float, Precision>(17);
    vec_exp_test_case<Float, Precision>(123);
}

BOOST_AUTO_TEST_CASE( vec_exp_test )
{
    vec_exp_test_cases<float, float>();
    vec_exp_test_cases<float, double>();
    vec_exp_test_cases<double, double>();
}

template<typename Float, typename Precision>
void vec_exp_k_test_case(int nvals)
{
    cerr << "testing vec_exp with k " << nvals << " float "
         << demangle(typeid(Float).name())
         << " precision " << demangle(typeid(Precision).name())
         << endl;

    Float x[nvals];
    Float k = rand();
    Precision r[nvals], r2[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0 / 1024.0;
        r2[i] = exp((double)(k * Precision(x[i])));
    }

    SIMD::vec_exp(x, k, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

template<typename Float, typename Precision>
void vec_exp_k_test_cases()
{
    vec_exp_k_test_case<Float, Precision>(1);
    vec_exp_k_test_case<Float, Precision>(2);
    vec_exp_k_test_case<Float, Precision>(3);
    vec_exp_k_test_case<Float, Precision>(4);
    vec_exp_k_test_case<Float, Precision>(5);
    vec_exp_k_test_case<Float, Precision>(8);
    vec_exp_k_test_case<Float, Precision>(9);
    vec_exp_k_test_case<Float, Precision>(12);
    vec_exp_k_test_case<Float, Precision>(15);
    vec_exp_k_test_case<Float, Precision>(16);
    vec_exp_k_test_case<Float, Precision>(17);
    vec_exp_k_test_case<Float, Precision>(123);
}

BOOST_AUTO_TEST_CASE( vec_exp_k_test )
{
    vec_exp_k_test_cases<float, float>();
    vec_exp_k_test_cases<float, double>();
    vec_exp_k_test_cases<double, double>();
}

template<typename T>
void vec_add_mixed_test_case(int nvals)
{
    cerr << "testing vec_add_mixed " << demangle(typeid(T).name()) <<
        " with " << nvals << endl;

    double x[nvals], r[nvals], r2[nvals];
    T y[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] + y[i];
    }

    SIMD::vec_add(x, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_add_mixed_test )
{
    for(auto x : {1, 2, 3, 4, 5, 6, 8, 9, 12, 16, 123}) {
        vec_add_mixed_test_case<float>(x);
        vec_add_mixed_test_case<double>(x);
    }
}

void vec_sum_dp_test_case(int nvals)
{
    cerr << "testing vec_sum_dp " << " with " << nvals << endl;

    float x[nvals];
    double r2 = 0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        r2 += x[i];
    }

    double r = SIMD::vec_sum_dp(x, nvals);

    BOOST_CHECK_EQUAL(r, r2);
}

BOOST_AUTO_TEST_CASE( vec_sum_dp )
{
    for(auto x : {1, 2, 3, 4, 5, 6, 8, 9, 12, 16, 123}) {
        vec_sum_dp_test_case(x);
    }
}

template<typename T>
void vec_k1_x_plus_k2_y_z_test_case(int nvals)
{
    cerr << "testing vec_k1_x_plus_k2_y_z_test_case " <<
        demangle(typeid(T).name()) << " with " << nvals << endl;

    T x[nvals], r[nvals], r2[nvals], y[nvals], z[nvals];

    T k1 = 3.9;
    T k2 = 8.2;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        z[i] = rand() / 16384.0;
        r2[i] = k1* x[i] + k2 * y[i] * z[i];
    }

    SIMD::vec_k1_x_plus_k2_y_z(k1, x, k2, y, z, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_k1_x_plus_k2_y_z )
{
    for(auto x : {1, 2, 3, 4, 5, 6, 8, 9, 12, 16, 123}) {
        vec_k1_x_plus_k2_y_z_test_case<float>(x);
        vec_k1_x_plus_k2_y_z_test_case<double>(x);
    }
}

template<typename T>
void vec_twonorm_sqr_test_case(int nvals)
{
    cerr << "testing vec_twonorm_sqr " << demangle(typeid(T).name()) <<
        " with nvals: " << nvals << endl;

    T x[nvals];
    T r2 = 0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        r2 += x[i] * x[i];
    }

    T r = SIMD::vec_twonorm_sqr(x, nvals);

    BOOST_CHECK_EQUAL(r, r2);
}

BOOST_AUTO_TEST_CASE( vec_twonorm_sqr_test )
{
    for(auto x : {1, 2, 3, 4, 5, 6, 8, 9, 12, 16, 123}) {
        vec_twonorm_sqr_test_case<float>(x);
        vec_twonorm_sqr_test_case<double>(x);
    }
}

void vec_twonorm_sqr_dp_test_case(int nvals)
{
    cerr << "testing vec_twonorm_sqr_dp " << " with nvals: " << nvals << endl;

    float x[nvals];
    double r2 = 0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        double xx = x[i];
        r2 += xx * xx;
    }

    double r = SIMD::vec_twonorm_sqr_dp(x, nvals);

    BOOST_CHECK_EQUAL(r, r2);
}

BOOST_AUTO_TEST_CASE( vec_twonorm_sqr_dp_test )
{
    for(auto x : {1, 2, 3, 4, 5, 6, 8, 9, 12, 16, 123}) {
        vec_twonorm_sqr_dp_test_case(x);
    }
}

void vec_kl_test_case(int nvals)
{
    cerr << "testing vec_kl " << " with " << nvals << endl;

    float p[nvals], q[nvals];
    double r2 = 0;

    for (unsigned i = 0; i < nvals;  ++i) {
        p[i] = rand() / 16384.0;
        q[i] = rand() / 16384.0;
        r2 += p[i] * logf(p[i] / q[i]);
    }

    double r = SIMD::vec_kl(p, q, nvals);

    BOOST_CHECK_CLOSE(r, r2, 0.00001);
}

BOOST_AUTO_TEST_CASE( vec_kl_test )
{
    //for(auto x : {1, 2, 3, 4, 5, 6, 8, 9, 12, 16, 123}) {
    for(int x = 1; x<100; x++) {
        vec_kl_test_case(x);
    }
}

void vec_min_max_el_test_case(int nvals)
{
    cerr << "testing vec_min_max_el with " << nvals << endl;

    float x[nvals], mins[nvals], maxs[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        mins[i] = rand() / 16384.0;
        maxs[i] = rand() / 16384.0;
    }

    SIMD::vec_min_max_el(x, mins, maxs, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK( mins[i] <= x[i] );
        BOOST_CHECK( maxs[i] >= x[i] );
    }
}

BOOST_AUTO_TEST_CASE( vec_min_max_el_test )
{
    for(auto x : {1, 2, 3, 4, 5, 6, 8, 9, 12, 16, 123}) {
        vec_min_max_el_test_case(x);
    }
}