/**
 * The MIT License (MIT)
 *
 * Copyright (c) 2015 Frank Astier
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "../include/quaternion.h"
#include "../include/quaternion_io.h"

#include <set>
#include <unordered_set>
#include <chrono> // definitely needed on some platforms, but maybe not with clang
#include <random>
#include <iomanip>

#include <boost/math/quaternion.hpp>
#include <boost/rational.hpp>
#include <map>

using namespace std;
using namespace boost;
using namespace quaternion;

//----------------------------------------------------------------------------------------------------------------------
// Test utilities
//----------------------------------------------------------------------------------------------------------------------
/**
 * Prints out arrays to a stream - sometimes useful when debugging unit tests
 */
template <typename T, size_t n>
inline ostream& operator<<(ostream& out, const array<T,n>& x) {
  out << "{";
  for (size_t i = 0; i < n; ++i) {
    out << x[i];
    if (i < n-1)
      out << ",";
  }
  return out << "}";
}

/**
 * Prints out a vector of elements of type T to a stream
 */
template <typename T>
inline ostream& operator<<(ostream& out, const std::vector<T>& x) {
  out << "(";
  for (size_t i = 0; i < x.size(); ++i) {
    out << x[i];
    if (i < x.size()-1)
      out << ",";
  }
  return out << ")";
}

/**
 * Compare a boost quaternion to quaternion, within epsilon.
 * Boost is required only for testing, to compare against a known implementation,
 * so this method is not provided alongside the other nearly_equal in quaternion.h.
 */
template <typename T>
inline bool operator==(const quaternion::Quaternion<T>& us, const boost::math::quaternion<T>& them) {
  return us.a() == them.R_component_1()
         && us.b() == them.R_component_2()
         && us.c() == them.R_component_3()
         && us.d() == them.R_component_4();
}

template <typename T, typename T1>
inline bool nearly_equal(const quaternion::Quaternion<T>& us, const boost::math::quaternion<T>& them, T1 eps) {
  return is_nearly_equal(us.a(), them.R_component_1(), eps)
         && is_nearly_equal(us.b(), them.R_component_2(), eps)
         && is_nearly_equal(us.c(), them.R_component_3(), eps)
         && is_nearly_equal(us.d(), them.R_component_4(), eps);
}

/**
 * This used to test e.g. the polar representation of the quaternions.
 */
template <typename T, size_t n>
inline bool nearly_equal(const array<T,n>& x, const array<T,n>& y, T eps) {
  for (size_t i = 0; i < n; ++i)
    if (!is_nearly_equal(x[i], y[i], eps))
      return false;
  return true;
};

/**
 * This used to test e.g. the rotation matrices.
 */
template <typename T, size_t n1, size_t n2>
inline bool
nearly_equal(const array<array<T,n1>,n2>& x, const array<array<T,n1>,n2>& y, T eps) {
  for (size_t i = 0; i < n1; ++i)
    for (size_t j = 0; j < n2; ++j)
      if (!is_nearly_equal(x[i][j], y[i][j], eps))
        return false;
  return true;
};

/**
 * Transpose of a square matrix.
 */
template <typename T, size_t n>
inline array<array<T,n>,n> transpose(const array<array<T,n>,n>& a) {
  array<array<T,n>,n> r;
  for (size_t i = 0; i < n; ++i)
    for (size_t j = 0; j < n; ++j)
      r[i][j] = a[j][i];
  return r;
}

template <typename T>
inline T det(const array<array<T,2>,2>& m) {
  return m[0][0] * m[1][1] - m[0][1] * m[1][0];
}

template <typename T, size_t n>
inline T det(const array<array<T,n>,n>& m) {
  T d = 0;
  for (size_t j = 0; j < n; ++j) {
    array<array<T,n-1>,n-1> sub;
    for (size_t k = 1; k < n; ++k)
      for (size_t l = 0; l < n; ++l)
        if (l != j)
          sub[k-1][l - (l > j)] = m[k][l];
    d += ((j % 2) ? -1 : 1) * m[0][j] * det(sub);
  }
  return d;
}

/**
 * Conjugate of a square matrix.
 * Using "conj" because it is the std:: name for the conjugate of a std::complex.
 * TODO: need 1 function for both real and complex matrices.
 */
template <typename T, size_t n>
inline array<array<T,n>,n> conj(const array<array<T,n>,n>& a) {
  array<array<T,n>,n> r;
  for (size_t i = 0; i < n; ++i)
    for (size_t j = 0; j < n; ++j)
      r[i][j] = conj(a[i][j]); // std::conj will transform reals into complex here
  return r;
}

/**
 * Initialize a m x n matrix from a list of values of size m * n.
 */
template <size_t m, size_t n = m, typename T =double>
inline array<array<T,n>,m> make_mat(std::initializer_list<T> l) {
  array<array<T,n>,m> mr;
  const double* it = l.begin();
  for (size_t i = 0; i < m; ++i)
    for (size_t j = 0; j < n; ++j)
      mr[i][j] = *it++;
  return mr;
};

/**
 * Square matrix addition.
 */
template <typename T, size_t n>
inline array<array<T,n>,n>
operator+(const array<array<T,n>,n>& a, const array<array<T,n>,n>& b) {
  array<array<T,n>,n> r;
  for (size_t i = 0; i < n; ++i)
    for (size_t j = 0; j < n; ++j)
      r[i][j] = a[i][j] + b[i][j];
  return r;
}

/**
 * Square matrix multiplication.
 * TODO: move to mat utils lib
 */
template <typename T, size_t n>
inline array<array<T,n>,n>
operator*(const array<array<T,n>,n>& a, const array<array<T,n>,n>& b) {
  array<array<T,n>,n> r;
  for (size_t i = 0; i < n; ++i)
    for (size_t j = 0; j < n; ++j) {
      T val = 0;
      for (size_t k = 0; k < n; ++k)
        val += a[i][k] * b[k][j];
      r[i][j] = val;
  }
  return r;
}

/**
 * A random number generator, for the speed tests.
 */
std::default_random_engine generator;
std::uniform_real_distribution<float> distribution(0.0,1.0);
auto rng = bind(distribution, generator);

/**
 * A function to generate random quaternions.
 */
template <typename T, typename G>
inline Quaternion<T> random_quaternion(G& g) {
  return {g(), g(), g(), g()};
}

/**
 * For convenience and ease of reading the testing code.
 */
typedef std::complex<float> Cf;
typedef std::complex<double> Cd;
typedef std::complex<long double> Cld;

typedef boost::math::quaternion<float> qf;
typedef boost::math::quaternion<double> qd;

void test_nearly_equal() {
  cout << "Testing nearly equal for array" << endl;

  {
    array<array<float,3>,3> A;
    A[0] = {{1, 0, 0}};
    A[1] = {{0, 1, 0}};
    A[2] = {{0, 0, 1}};
    assert(nearly_equal(A, A, 0.0f));
  }

  {
    array<array<float,3>,3> A, B;
    A[0] = {{1, 0, 0}};
    A[1] = {{0, 1, 0}};
    A[2] = {{0, 0, 1}};
    B[0] = {{1, 0, 0}};
    B[1] = {{0, 1, 0}};
    B[2] = {{0, 0, 2}};
    assert(!nearly_equal(A, B, 1e-6f));
  }
}

void test_det() {
  cout << "Testing det" << endl;
  {
    auto mat22 = make_mat<2,2,double>;
    assert(det(mat22({0,0,0,0})) == 0);
    assert(det(mat22({1,0,0,0})) == 0);
    assert(det(mat22({1,1,0,0})) == 0);
    assert(det(mat22({1,0,0,1})) == 1);
    assert(det(mat22({1,0,1,0})) == 0);
    assert(det(mat22({1,0,1,1})) == 1);
    assert(det(mat22({0,1,1,0})) == -1);
    assert(det(mat22({1,2,3,4})) == -2);
  }
  {
    auto mat33 = make_mat<3,3,double>;
    assert(det(mat33({0,0,0, 0,0,0, 0,0,0})) == 0);
    assert(det(mat33({1,0,0, 0,1,0, 0,0,1})) == 1);
    assert(det(mat33({1,0,0, 0,1,2, 0,3,4})) == -2);
    assert(det(mat33({1,2,3, 4,5,6, 7,8,9})) == 0);
    assert(det(mat33({1,8,3, 2,5,7, 4,6,9})) == 59);
  }
}

//----------------------------------------------------------------------------------------------------------------------
// Unit tests
//----------------------------------------------------------------------------------------------------------------------
void test_constructors() {
  cout << "Testing constructors" << endl;
  {
    Qf x;
    assert(x.a() == 0 && x.b() == 0 && x.c() == 0 && x.d() == 0);
    assert(x.real() == 0);
    assert(x.unreal() == Qf_0);
  }

  {
    Qf x(0,0,0,0);
    assert(x.a() == 0 && x.b() == 0 && x.c() == 0 && x.d() == 0);
  }

  {
    Qf x(1);
    assert(x.a() == 1 && x.b() == 0 && x.c() == 0 && x.d() == 0);
  }

  {
    Qf x(1, 2);
    assert(x.a() == 1 && x.b() == 2 && x.c() == 0 && x.d() == 0);
  }

  {
    Qf x(1, 2, 3);
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 0);
  }

  {
    Qf x(1, 2, 3, 4);
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);

  }

  {
    Qf x(1,2);
    assert(x.a() == 1 && x.b() == 2 && x.c() == 0 && x.d() == 0);
  }

  {
    Qf x(1,2,3,4);
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
  }

  {
    Qf x(Cf(1,2));
    assert(x.a() == 1 && x.b() == 2 && x.c() == 0 && x.d() == 0);
  }

  {
    Qf x(Cf(1,2), Cf(3,4));
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
    assert(x.c1() == Cf(1,2));
    assert(x.c2() == Cf(3,4));
  }

  {
    Qd x(Cf(1,2), Cf(3,4));
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
  }

  {
    Qd y(Cf(1,2), Cf(3,4));
    Qd x(y);
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
  }

  {
    Qf y(1,2,3,4);
    Qd x(y);
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
  }

  {
    Qd y(Cf(1,2), Cf(3,4));
    Qd x = y;
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
  }

  {
    Qf y(1,2,3,4);
    Qd x = y;
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
    assert(x.c1() == complex<double>(1,2));
    assert(x.c2() == complex<double>(3,4));
  }

  {
    int d[4] = {1,2,3,4};
    Qf x(d);
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
  }

  {
    int d[] = {1,2,3,4, 5,6,7,8};
    int* it = d;
    Qf x(it), y(it + 4);
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
    assert(y.a() == 5 && y.b() == 6 && y.c() == 7 && y.d() == 8);
  }

  {
    vector<int> d{1,2,3,4};
    Qf x(d.begin());
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
  }

  {
    set<int> d{1,2,3,4};
    Qf x(d.begin());
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
  }

  {
    Qf x{1,0,2,0};
    assert(x.a() == 1 && x.b() == 0 && x.c() == 2 && x.d() == 0);
  }

  {
    Qd x({1,2,3,4});
    assert(x.a() == 1 && x.b() == 2 && x.c() == 3 && x.d() == 4);
  }

  {
    Qf x(1,2,3,4);
    Qf y;
    y += 3;
    y = x;
    assert(x == y);
  }
}

void test_trigonometric_constructors() {
  cout << "Testing trigonometric constructors" << endl;
  {
    Qf x = spherical(0.0f,0.0f,0.0f,0.0f);
    assert(x.a() == 0 && x.b() == 0 && x.c() == 0 && x.d() == 0);
  }

  {
    Qd x = spherical(10.0, 3.1415, 3.1415/2.0, 3.1415/4.0);
    assert(nearly_equal(x, boost::math::spherical(10.0, 3.1415, 3.1415 / 2, 3.1415 / 4), 1e-6));
  }

  {
    Qd x = semipolar(10.0, 3.1415, 3.1415/2, 3.1415/4);
    assert(nearly_equal(x, boost::math::semipolar(10.0, 3.1415, 3.1415 / 2, 3.1415 / 4), 1e-6));
  }

  {
    Qd x = multipolar(10.0, 3.1415, 3.1415/2, 3.1415/4);
    assert(nearly_equal(x, boost::math::multipolar(10.0, 3.1415, 3.1415 / 2, 3.1415 / 4), 1e-6));
  }

  {
    Qd x = cylindrospherical(-2.0, -3.0, 3.1415/2, 3.1415/4);
    assert(nearly_equal(x, boost::math::cylindrospherical(-2.0, -3.0, 3.1415 / 2, 3.1415 / 4), 1e-6));
  }

  {
    Qd x = cylindrical(-2.0, 3.1415/2, 3.0, 4.0);
    assert(nearly_equal(x, boost::math::cylindrical(-2.0, 3.1415 / 2, 3.0, 4.0), 1e-6));
  }
}

void test_IJK() {
  cout << "Testing IJK" << endl;
  assert(Qf_0 == 0);
  assert(Qf_1 == 1);
  assert(Qf_i * Qf_i == -1);
  assert(Qf_j * Qf_j == -1);
  assert(Qf_k * Qf_k == -1);
  assert(Qf_i * Qf_j * Qf_k == -1);

  assert(Qd_0 == 0);
  assert(Qd_1 == 1);
  assert(Qd_i * Qd_i == -1);
  assert(Qd_j * Qd_j == -1);
  assert(Qd_k * Qd_k == -1);
  assert(Qd_i * Qd_j * Qd_k == -1);

  assert(Qld_0 == 0);
  assert(Qld_1 == 1);
  assert(Qld_i * Qld_i == -1);
  assert(Qld_j * Qld_j == -1);
  assert(Qld_k * Qld_k == -1);
  assert(Qld_i * Qld_j * Qld_k == -1);

  assert(Qf_i/Qf_j == -Qf_k);
  assert(Qf_j/Qf_k == -Qf_i);
  assert(Qf_k/Qf_i == -Qf_j);

  assert(1.0f/Qf_i == -Qf_i);
  assert(1.0f/Qf_j == -Qf_j);
  assert(1.0f/Qf_k == -Qf_k);

  assert(Qf_i*(Qf_k/Qf_i) == -Qf_k);
  assert((Qf_k/Qf_i)*Qf_i == Qf_k);
}

void test_accessors() {
  cout << "Testing accessors" << endl;
  {
    Qd x;
    assert(x.real() == 0);
    assert(x.unreal() == 0);
    assert(x.unreal() == Qd());
    assert(x.is_zero());
    assert(x.is_zero(1e-6));
    assert(!x.is_non_zero());
    assert(!x.is_non_zero(1e-6));
    assert(x.is_finite());
    assert(!x.is_inf());
    assert(!x.is_nan());
    assert(x.is_real());
    assert(x.is_complex());
    assert(!x.is_unreal());
    assert(!x.is_unit());
    assert(-x == x);
  }
  {
    Qd x(1);
    assert(x.real() == 1);
    assert(x.unreal() == 0);
    assert(x.unreal() == Qd());
    assert(!x.is_zero());
    assert(!x.is_zero(1e-6));
    assert(x.is_non_zero());
    assert(x.is_non_zero(1e-6));
    assert(x.is_finite());
    assert(!x.is_inf());
    assert(!x.is_nan());
    assert(x.is_real());
    assert(x.is_complex());
    assert(!x.is_unreal());
    assert(x.is_unit());
    assert(-x == Qd(-1));
  }
  {
    Qd x(0,1);
    assert(x.real() == 0);
    assert(x.unreal() == Qd(0,1));
    assert(!x.is_zero());
    assert(!x.is_zero(1e-6));
    assert(x.is_non_zero());
    assert(x.is_non_zero(1e-6));
    assert(x.is_finite());
    assert(!x.is_inf());
    assert(!x.is_nan());
    assert(!x.is_real());
    assert(x.is_complex());
    assert(x.is_unreal());
    assert(x.is_unit());
    assert(-x == Qd(0,-1));
  }
  {
    Qd x(1e-12);
    assert(x.real() == 1e-12);
    assert(x.unreal() == 0);
    assert(!x.is_zero());
    assert(x.is_zero(1e-6));
    assert(x.is_non_zero());
    assert(!x.is_non_zero(1e-6));
    assert(x.is_finite());
    assert(!x.is_inf());
    assert(!x.is_nan());
    assert(x.is_real());
    assert(x.is_complex());
    assert(!x.is_unreal());
    assert(!x.is_unit());
  }
  {
    Qf x(1, 2, 3, 4);
    assert(x.real() == 1);
    assert(x.unreal() != 0);
    assert(x.unreal() == Qf(0, 2, 3, 4));
    assert(!x.is_zero());
    assert(!x.is_zero(1e-6));
    assert(x.is_non_zero());
    assert(x.is_non_zero(1e-6));
    assert(x.is_finite());
    assert(!x.is_inf());
    assert(!x.is_nan());
    assert(!x.is_real());
    assert(!x.is_complex());
    assert(!x.is_unreal());
    assert(!x.is_unit());
    assert(-x == Qf(-1,-2,-3,-4));
  }
  {
    assert(is_inf(Qd_i/0));
    assert(is_nan(Qd_i/Qd_0)); // TODO: this should be inf, but it slows down operator/
    assert(is_nan(Qd_0/Qd_0));
  }

  {
    Qf x(3.14);
    assert(x.real() == 3.14f);
  }

  {
    Qf x(3.14, 2.71);
    assert(x.c1().real() == 3.14f && x.c1().imag() == 2.71f);
  }

  {
    Qf x(1,2,3,4);
    array<float,4> a = x.to_array();
    array<float,4> r{{1,2,3,4}};
    assert(a == r); // exact equality expected (operator== is in std)
  }
}

void test_conjugate() {
  cout << "Testing conjugate" << endl;

  {
    assert(conj(Qf(0)) == 0);
    assert(conj(Qf(1)) == 1);
    assert(conj(Qf_i) == -Qf_i);
    assert(conj(Qf_j) == -Qf_j);
    assert(conj(Qf_k) == -Qf_k);
  }
  {
    Qf x(1, 2, 3, 4);
    assert(conj(x) == Qf(1, -2, -3, -4));
    assert(conj(conj(x)) == x);
    assert(conj(x) + x == Qf(2, 0, 0, 0));
    assert(x - conj(x) == Qf(0, 4, 6, 8));
  }
}

void test_complex_matrix_2d_representation() {
  cout << "Testing complex matrix 2d representation" << endl;
  using MR = complex_matrix_2d<double>;

  {
    assert(to_complex_matrix_2d(Qd()) == MR());
    assert(from_complex_matrix_2d(MR()) == 0);
  }
  {
    MR r;
    r[0] = {{Cd(1,0), Cd()}};
    r[1] = {{Cd(), Cd(1,0)}};
    assert(to_complex_matrix_2d(Qd_1) == r);
    assert(from_complex_matrix_2d(r) == Qd_1);
  }
  {
    MR r;
    r[0] = {{Cd(0,1), Cd()}};
    r[1] = {{Cd(), Cd(0,-1)}};
    assert(to_complex_matrix_2d(Qd_i) == r);
    assert(from_complex_matrix_2d(r) == Qd_i);
  }
  {
    MR r;
    r[0] = {{Cd(), Cd(1,0)}};
    r[1] = {{Cd(-1,0), Cd()}};
    assert(to_complex_matrix_2d(Qd_j) == r);
    assert(from_complex_matrix_2d(r) == Qd_j);
  }
  {
    MR r;
    r[0] = {{Cd(), Cd(0,1)}};
    r[1] = {{Cd(0,1), Cd()}};
    assert(to_complex_matrix_2d(Qd_k) == r);
    assert(from_complex_matrix_2d(r) == Qd_k);
  }
  {
    MR r;
    r[0] = {{Cd(1, 2), Cd(3, 4)}};
    r[1] = {{Cd(-3, 4), Cd(1, -2)}};
    assert(to_complex_matrix_2d(Qd(1,2,3,4)) == r);
    assert(from_complex_matrix_2d(r) == Qd(1,2,3,4));
  }
  {
    Qd x(1,2,3,4); MR m = to_complex_matrix_2d(x);
    assert(from_complex_matrix_2d(to_complex_matrix_2d(x)) == x);
    assert(to_complex_matrix_2d(from_complex_matrix_2d(m)) == m);
    assert(norm_squared(x) == det(m));
    assert(to_complex_matrix_2d(conj(x)) == transpose(conj(m)));
  }
  {
    Qd x(1,2,3,4), y(5,6,7,8);
    MR mx = to_complex_matrix_2d(x);
    MR my = to_complex_matrix_2d(y);
    assert(to_complex_matrix_2d(x + y) == mx + my);
    assert(to_complex_matrix_2d(x * y) == mx * my);
  }
}

void test_real_matrix_4d_representation() {
  cout << "Testing real matrix 4d representation" << endl;
  using MR = real_matrix_4d<double>;
  auto mat4d = make_mat<4,4,double>;
  {
    assert(to_real_matrix_4d(Qd_0) == mat4d({0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0}));
    assert(to_real_matrix_4d(Qd_1) == mat4d({1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1}));
    assert(to_real_matrix_4d(Qd_i) == mat4d({0,1,0,0, -1,0,0,0, 0,0,0,-1, 0,0,1,0}));
    assert(to_real_matrix_4d(Qd_j) == mat4d({0,0,1,0, 0,0,0,1, -1,0,0,0, 0,-1,0,0}));
    assert(to_real_matrix_4d(Qd_k) == mat4d({0,0,0,1, 0,0,-1,0, 0,1,0,0, -1,0,0,0}));
  }
  {
    Qd x(1,2,3,4); MR m = to_real_matrix_4d(x);
    assert(from_real_matrix_4d(to_real_matrix_4d(x)) == x);
    assert(to_real_matrix_4d(from_real_matrix_4d(m)) == m);
    assert(norm_squared(x) == std::sqrt(det(m)));
    assert(to_real_matrix_4d(conj(x)) == transpose(m)); // std::conj puts us in complex, but we are reals
  }
  {
    Qd x(1,2,3,4), y(5,6,7,8);
    MR mx = to_real_matrix_4d(x);
    MR my = to_real_matrix_4d(y);
    assert(to_real_matrix_4d(x + y) == mx + my);
    assert(to_real_matrix_4d(x * y) == mx * my);
  }
}

void test_to_polar_representation() {
  cout << "Testing polar representation" << endl;
  typedef polar_representation<double> PR;
  {
    PR expected{{0,0,0,0,0}};
    assert(to_polar_representation(Qd()) == expected);

    expected = {{1,1,1,1,1}};
    assert(!nearly_equal(to_polar_representation(Qd(1)), expected, 1e-6));

    expected = {{3,3.141592,0,0,0}};
    assert(nearly_equal(to_polar_representation(Qd(-3)), expected, 1e-6));

    expected = {{1,3.1415926f/2,1,0,0}};
    assert(nearly_equal(to_polar_representation(Qd(0, 1)), expected, 1e-6));

    expected = {{3,3.1415926f/2,-1,0,0}};
    assert(nearly_equal(to_polar_representation(Qd(0, -3)), expected, 1e-6));

    expected = {{1,3.1415926f/2,0,1,0}};
    assert(nearly_equal(to_polar_representation(Qd(0, 0, 1)), expected, 1e-6));

    expected = {{2.5,3.1415926f/2,0,-1,0}};
    assert(nearly_equal(to_polar_representation(Qd(0, 0, -2.5)), expected, 1e-6));

    expected = {{1,3.1415926f/2,0,0,1}};
    assert(nearly_equal(to_polar_representation(Qd(0, 0, 0, 1)), expected, 1e-6));

    expected = {{3.5,3.1415926f/2,0,0,-1}};
    assert(nearly_equal(to_polar_representation(Qd(0, 0, 0, -3.5)), expected, 1e-6));
  }
}

void test_to_rotation_matrix() {
  cout << "Testing rotation matrix" << endl;
  typedef rotation_matrix<double> RM;
  auto rm = make_mat<3,3,double>;

  {
    Qd x(1);
    RM expected = rm({1,0,0, 0,1,0, 0,0,1});
    assert(nearly_equal(to_rotation_matrix(x), expected, 1e-16));
    assert(from_rotation_matrix(expected) == x);
    assert(from_rotation_matrix(to_rotation_matrix(x)) == x);
    assert(to_rotation_matrix(from_rotation_matrix(expected)) == expected);
  }
  {
    Qd x(0,1);
    RM expected = rm({1,0,0, 0,-1,0, 0,0,-1});
    assert(nearly_equal(to_rotation_matrix(x), expected, 1e-16));
    assert(from_rotation_matrix(expected) == x);
    assert(from_rotation_matrix(to_rotation_matrix(x)) == x);
    assert(to_rotation_matrix(from_rotation_matrix(expected)) == expected);
  }
  {
    Qd x(0,0,1);
    RM expected = rm({-1,0,0, 0,1,0, 0,0,-1});
    assert(nearly_equal(to_rotation_matrix(x), expected, 1e-16));
    assert(from_rotation_matrix(expected) == x);
    assert(from_rotation_matrix(to_rotation_matrix(x)) == x);
    assert(to_rotation_matrix(from_rotation_matrix(expected)) == expected);
  }
  {
    Qd x(0,0,0,1);
    RM expected = rm({-1,0,0, 0,-1,0, 0,0,1});
    assert(nearly_equal(to_rotation_matrix(x), expected, 1e-16));
    assert(from_rotation_matrix(expected) == x);
    assert(from_rotation_matrix(to_rotation_matrix(x)) == x);
    assert(to_rotation_matrix(from_rotation_matrix(expected)) == expected);
  }
  {
    // TODO: more tests here
  }
}

void test_euler_angles() {
  cout << "Testing Euler angles" << endl;
  const double pi = 3.14159265358979323846;
  using A = array<double,3>;
  {
    //assert((to_euler(Qd_0) == array<double,3>{{0,0,0}})); only unit quaternions
    assert((to_euler(Qd_i) == A{{0,0,pi}}));
    assert((to_euler(Qd_j) == A{{pi,0,0}}));
    assert((to_euler(Qd_k) == A{{pi,0,pi}}));
  }
  {
    assert(nearly_equal(from_euler(A{{pi,0,0}}), Qd_i, 1e-16));
    assert(nearly_equal(from_euler(A{{0,pi,0}}), Qd_j, 1e-16));
    assert(nearly_equal(from_euler(A{{0,0,pi}}), -Qd_i + Qd_k, 1e-16));
    assert(nearly_equal(from_euler(A{{pi,0,pi}}), Qd_j, 1e-1));
  }
}

void test_norms() {
  cout << "Testing norms" << endl;
  {
    assert(abs(Qld_0) == 0);
    assert(abs(Qld_1) == 1);
    assert(abs(Qld_i) == 1);
    assert(abs(Qld_j) == 1);
    assert(abs(Qld_k) == 1);
  }

  {
    Qf x;
    assert(norm_squared(x) == 0);
    assert(std::abs(abs(x)) < 1e-6);
    assert(norm_l0(x) == 0);
    assert(norm_l1(x) == 0);
    assert(norm_sup(x) == 0);
    assert(norm_lk(x, 0.5) == 0);
    assert(unreal_norm_squared(x) == 0);
    assert(is_real(x));
    assert(is_complex(x));
    assert(!is_unreal(x));
  }

  {
    Qd x{1,2,3,4};
    assert(norm_squared(x) == 1 + 4 + 9 + 16);
    assert(std::abs(abs(x) - std::sqrt(1 + 4 + 9 + 16)) < 1e-6);
    assert(normalize(x).is_unit(1e-6));
    assert(nearly_equal(normalize(x), x / std::sqrt(1 + 4 + 9 + 16), 1e-6));
    assert(unreal_norm_squared(x) == 29);
    assert(norm_l0(x) == 4);
    assert(norm_l1(x) == 10);
    assert(norm_sup(x) == 4);
    assert(is_nearly_equal(norm_lk(x, .5), pow(1 + sqrt(2) + sqrt(3) + sqrt(4), 2), 1e-6));
  }

  {
    Qd x{1,-4,3,2};
    assert(norm_l0(x) == 4);
    assert(norm_l1(x) == 1+2+3+4);
    assert(norm_sup(x) == 4);
    assert(unreal_norm_squared(x) == 29);
    assert(!is_unit(x));
    assert(is_unit(normalize(x), 1e-6));
    assert(!is_unreal(x));
    assert(is_nearly_equal(norm_lk(x, .5), pow(1 + sqrt(2) + sqrt(3) + sqrt(4), 2), 1e-6));
    x -= 1;
    assert(is_unreal(x, 1e-6));
  }
}

void test_equality() {
  cout << "Testing equality" << endl;
  assert(Qf(1) == 1);
  assert(1 == Qf(1));
  assert(Qf(1) != 2);
  assert(2 != Qf(1));
  assert(Qf(1,2) != 1);
  assert(Qf(1,2) == Cf(1,2));
  assert(Cf(1,2) == Qf(1,2));
  assert(Qf(1,2) != Cf(3,4));
  assert(Cf(3,4) != Qf(1,2));
  assert(Qf(1,2,3) == Qf(1,2,3));
  assert(Qf(1,2,3) != Qf(4,5,6));
  assert(Qf(1,2,3,4) == Qf(1, 2, 3, 4));
  assert(Qf(1,2,3,4) != Qf(4,3,2,1));
  assert(nearly_equal(Qf(1, 2, 3, 4), Qf(1, 2, 3, 4), 0));
  assert(nearly_equal(Qf(1, 2, 3, 4), Qf(1, 2, 3, 4), 1e-6));
  assert(nearly_equal(Qf(1, 2, 3, 4), Qf(1, 2, 3, 3.9999999f), 1e-6));
  assert(!nearly_equal(Qf(1, 2, 3, 4), Qf(0, 2, 3, 3.9999999f), 1e-6));
  assert(!nearly_equal(Qf(1, 2, 3, 4), Qf(1), 1e-6));
  assert(!nearly_equal(Qf(1, 2, 3, 4), Qf(1, 2), 1e-6));
  assert(!nearly_equal(Qf(1, 2, 3, 4), Qf(1, 2, 3), 1e-6));
  assert(!nearly_equal(Qf(1, 2, 3, 4), Cf(1), 1e-6));
  assert(!nearly_equal(Qf(1, 2, 3, 4), Cf(1, 2), 1e-6));
  assert(nearly_equal(Qf(1), Cf(1), 1e-6));
  assert(!nearly_equal(Qf(1), Cf(2), 1e-6));
  assert(nearly_equal(Qf(1, 2), Cf(1, 2), 1e-6));
  assert(!nearly_equal(Qf(1), Cf(1, 2), 1e-6));
  assert(!nearly_equal(Qf(3, 4), Cf(1, 2), 1e-6));
}

void test_plus_minus() {
  cout << "Testing +/-" << endl;
  assert(-Qf(1,2,3,4) == Qf(-1,-2,-3,-4));
  assert(- -Qf(1,2,3,4) == Qf(1,2,3,4));
  assert(- - -Qf(1,2,3,4) == Qf(-1,-2,-3,-4));
  assert(+Qf(1,2,3,4) == Qf(1,2,3,4));
  assert(+ +Qf(1,2,3,4) == Qf(1,2,3,4));
  assert(+ + +Qf(1,2,3,4) == Qf(1,2,3,4));
}

void test_unary_w_scalar() {
  cout << "Testing unary operators with scalar" << endl;
  {
    Qd x(1,2,3,4);
    x += (long double) 3;
    assert(x == Qd(4,2,3,4));
  }

  {
    Qd x(1,2,3,4);
    x -= 3;
    assert(x == Qd(-2,2,3,4));
  }

  {
    Qd x(1,2,3,4);
    x *= -3.5;
    assert(x == Qd(-3.5,-3.5*2,-3.5*3,-3.5*4));
  }

  {
    Qd x(1,2,3,4);
    x /= 2;
    assert(x == Qd(1.0/2,2.0/2,3.0/2,4.0/2));
  }

  {
    Qd x(1,2);
    x /= 3;
    assert(x == complex<double>(1.0/3, 2.0/3));
  }
}

void test_unary_w_complex() {
  cout << "Testing unary operators with complex" << endl;
  {
    Qf x(1); Cf y(3.14, 2.718);
    x += y;
    assert(nearly_equal(x, Cf(4.14, 2.718), 1e-6));
  }

  {
    Qf x(1,2), y(3.14, 2.718);
    x += y;
    assert(nearly_equal(x, Cf(4.14, 4.718), 1e-6));
  }

  {
    Qf x(1,2), y(3.14, 2.718);
    x -= y;
    assert(nearly_equal(x, Cf(-2.14f, -0.718f), 1e-6));
  }

  {
    Qf x(1,2,3,4); Cf y(5,6);
    x *= y;
    assert(x == Qf(1,2,3,4) * Qf(5,6));
  }

  {
    Qf x(1,2,3,4); Cf y(5,6);
    x /= y;
    assert(nearly_equal(x, Qf(1, 2, 3, 4) / Qf(5, 6), 1e-6));
  }
}

void test_unary_w_quaternion() {
  cout << "Testing unary operators with quaternion" << endl;
  {
    Qf x(1), y(3.14);
    x += y;
    assert(std::abs(x.real() - 4.14f) < 1e-6);
  }

  {
    Qf x(1,2), y(3.14, 2.718);
    x += y;
    assert(nearly_equal(x, Cf(4.14, 4.718), 1e-6));
  }

  {
    Qf x(1,2,3,4), y(5,6,7,8);
    x += y;
    assert(x == Qf(6,8,10,12));
    Qd z(1,1,1,1);
    x += z;
    assert(x == Qf(7,9,11,13));
  }

  {
    Qf x(1,2,3,4), y(5,6,7,8);
    qf q1(1,2,3,4), q2(5,6,7,8);
    x *= y;
    assert(x == Qf(-60,12,30,24));
    assert(x == q1 * q2);
    Qd z(1,1,1,1);
    x *= z;
    assert(x == Qf(-126,-42,-18,-54));
  }

  {
    Qf x(1,2,3,4), y(5,6,7,8);
    qf q1(1,2,3,4), q2(5,6,7,8);
    x /= y;
    assert(nearly_equal(x, q1 / q2, 1e-6));
    Qd z(1,1,1,1);
    x /= z;
    assert(nearly_equal(x, (q1 / q2) / qf(1, 1, 1, 1), 1e-6));
  }
}

void test_operators() {
  cout << "Testing operators" << endl;
  {
    assert(Qd(1,2,3,4) + 4 == Qd(5,2,3,4));
    assert(Qd(1,2,3,4) + 4 == 4 + Qd(1,2,3,4));
  }
  {
    assert(Qd(1,2,3,4) - 4 == Qd(-3,2,3,4));
    assert(Qd(1,2,3,4) - 4 == -4 + Qd(1,2,3,4));
  }
  {
    assert(Qd(1,2,3,4) * 4 == Qd(4,8,12,16));
    assert(Qd(1,2,3,4) * 4 == 4 * Qd(1,2,3,4));
  }
  {
    assert(Qd(1,2,3,4) / 4 == Qd(1.0/4,2.0/4,3.0/4,1.0));
    assert(4 / Qd(1,2,3,4) == 4 * inverse(Qd(1,2,3,4)));
  }

  {
    assert(Qd(1,2,3,4) + Cd(1,2) == Qd(2,4,3,4));
    assert(Qd(1,2,3,4) + Cd(1,2) == Cd(1,2) + Qd(1,2,3,4));
  }
  {
    assert(Qd(1,2,3,4) - Cd(1,2) == Qd(0,0,3,4));
    assert(Qd(1,2,3,4) - Cd(1,2) == -Cd(1,2) + Qd(1,2,3,4));
  }
  {

    assert(Qd(1,2,3,4) * Cd(1,2) == qd(1,2,3,4) * Cd(1,2));
    assert(Qd(1,2,3,4) * Cd(1,2) == Qd(-3,4,11,-2));
    assert(Qd(1,2,3,4) * Cd(1,2) == Cd(1,2) * Qd(1,2,3,4));
  }
  {
    assert(Qd(1,2,3,4) / Cd(1,2) == qd(1,2,3,4) / Cd(1,2));
    assert(Qd(1,2,3,4) / Cd(1,2) == Qd(1,0,-1,2));
    assert(Cd(1,2) / Qd(1,2,3,4) == Cd(1,2) * inverse(Qd(1,2,3,4)));
  }
  {
    assert(Qf(1,2,3,4) + Qf(4,5,6,7) == Qf(5,7,9,11));
    assert(Qf(1,2,3,4) - Qf(4,5,6,7) == Qf(-3,-3,-3,-3));
  }
  {
    assert(Qf(1,2,3,4) * Qf(4,-5,6,-7) == qf(1,2,3,4) * qf(4,-5,6,-7));
    assert(nearly_equal(Qf(1,2,3,4) / Qf(4,-5,6,-7), qf(1,2,3,4) / qf(4,-5,6,-7), 1e-6));
  }
}

void test_pow2() {
  cout << "Testing pow2" << endl;

  assert(pow2(Qf_0) == 0);
  assert(pow2(Qf_1) == 1);
  assert(pow2(Qf_1) == Qf_1 * Qf_1);
  assert(pow2(Qf_i) == -1);
  assert(pow2(Qf_j) == -1);
  assert(pow2(Qf_k) == -1);
  assert(pow2(Qf(1,2,3,4)) == Qf(1,2,3,4) * Qf(1,2,3,4));
  assert(pow2(Qf(3.14)) == 3.14f * 3.14f);
  assert(pow2(Qf(1,2)) == Cf(1,2) * Cf(1,2));
}

void test_pow3() {
  cout << "Testing pow3" << endl;

  assert(pow3(Qd_0) == 0);
  assert(pow3(Qd_1) == 1);
  assert(pow3(Qd_i) == -Qd_i);
  assert(pow3(Qd_j) == -Qd_j);
  assert(pow3(Qd_k) == -Qd_k);

  assert(pow3(Qld_0) == 0);
  assert(pow3(Qld_1) == 1);
  assert(pow3(Qld_i) == -Qld_i);
  assert(pow3(Qld_j) == -Qld_j);
  assert(pow3(Qld_k) == -Qld_k);
  assert(pow3(Qf(1,2,3,4)) == Qf(1,2,3,4) * Qf(1,2,3,4) * Qf(1,2,3,4));
}

void test_pow() {
  cout << "Testing pow" << endl;

  { // pow should work as is for various combinations of floats/ints.
    // The library should figure out which actual method to use.
    // We don't want to have to write pow(x, (int) 3) or pow(x, (float)3.5),
    // as it is not convenient.
    Quaternion<int> x(1,2,3,4);
    Quaternion<long> y(1,2,3,4);
    Quaternion<float> z(1,2,3,4);
    Quaternion<double> t(1,2,3,4);
    assert(pow(x,3) == pow(y,3));
    assert(pow(x,3) == pow(z,3));
    assert(pow(x,3) == pow(t,3));
    assert(pow(x,3.5) == pow(y,3.5));
    assert(pow(x,3.5) == pow(z,3.5));
    assert(pow(x,3.5) == pow(t,3.5));
  }

  assert(pow(Qf_0,2) == 0);
  assert(pow(Qf_1,2) == 1);
  assert(pow(Qf_i,2) == -1);
  assert(pow(Qf_j,2) == -1);
  assert(pow(Qf_k,2) == -1);

  assert(pow(Qf_1, 0) == 1);
  assert(pow(Qf_i, 0) == 1);
  assert(pow(Qf_j, 0) == 1);
  assert(pow(Qf_k, 0) == 1);

  assert(nearly_equal(pow(Qf(1), -1), Qf(1), 1e-6));
  assert(nearly_equal(pow(Qf(2), -3), Qf(1.0f / 8), 1e-6));
  assert(nearly_equal(pow(Qf(-2), -3), Qf(-1.0f / 8), 1e-6));
  assert(nearly_equal(pow(Qd_i, -2), pow(Cd(0, 1), -2.0f), 1e-15));
  assert(pow(Qd_i, -2) == -1);
  assert(pow(Qd_j, -2) == -1);
  assert(pow(Qd_k, -2) == -1);

  assert(pow(Qf_1, 0.5f) == 1);
  assert(nearly_equal(pow(-Qf_1, 0.5f), Qf_i, 1e-6));
  assert(nearly_equal(pow(Qf_i, 0.5f), sqrt(Cf(0, 1)), 1e-6));
  assert(nearly_equal(pow(-Qf_i, 0.5f), sqrt(Cf(0, -1)), 1e-6));
  assert(nearly_equal(pow(Qf_j, 0.5f), Qf(1.0f / sqrt(2.0f), 0, 1.0f / sqrt(2.0f)), 1e-6));
  assert(nearly_equal(pow(-Qf_j, 0.5f), Qf(1.0f / sqrt(2.0f), 0, -1.0f / sqrt(2.0f)), 1e-6));
  assert(nearly_equal(pow(Qf_k, 0.5f), Qf(1.0f / sqrt(2.0f), 0, 0, 1.0f / sqrt(2.0f)), 1e-6));
  assert(nearly_equal(pow(-Qf_k, 0.5f), Qf(1.0f / sqrt(2.0f), 0, 0, -1.0f / sqrt(2.0f)), 1e-6));
  assert(pow(Qf_1, -0.33f) == 1);
  assert(nearly_equal(pow(-Qf_1, -0.33f), pow(Cf(-1, 0), -0.33f), 1e-6));
  assert(nearly_equal(pow(Qf_i, -0.33f), pow(Cf(0, 1), -0.33f), 1e-6));
  assert(nearly_equal(pow(-Qf_i, -0.33f), pow(Cf(0, -1), -0.33f), 1e-6));

  for (size_t i = 0; i < 1000; ++i) {
    int n = (int) random() % 20;
    Qld x(1+rand()%5,rand()%5,rand()%5,rand()%5);
    Qld y = Qld_1;
    for (int j = 0; j < n; ++j)
      y *= x;
    assert(norm_squared(y - pow(x, n)) < 1e-10);
  }

  // Need Qld for precision, and even with Qld, not better than 1e-5...
  for (size_t i = 0; i < 1000; ++i) {
    long double n = ((long double)(random() % 20))/(1 + (long double)(random() % 15));
    Qld x(1+rand()%5,rand()%5,rand()%5,rand()%5);
    assert(nearly_equal(pow(x, n), exp(n * log(x)), 1e-5));
  }
}

void test_q_pow() {
  cout << "Testing q pow" << endl;

  assert(pow(Qd_0,Qd(2)) == 0);
  assert(pow(Qd_1,Qd(2)) == 1);
  assert(pow(Qd_i,Qd(2)) == -1);
  assert(pow(Qd_j,Qd(2)) == -1);
  assert(pow(Qd_k,Qd(2)) == -1);

  for (size_t i = 0; i < 10; ++i) {
    Qld x(rand()%5,rand()%5,rand()%5,rand()%5);
    Qld y(rand()%5,rand()%5,rand()%5,rand()%5);
    assert(nearly_equal(pow(x, y), exp(y * log(x)), 1e-6));
  }
}

void test_log() {
  cout << "Testing log" << endl;

  { // Make sure it works for reals
    Qld x(2,0,0,0);
    assert(std::abs(log(x).real() - std::log(2)) < 1e-10);
  }

  size_t N = 10;
  for (size_t i = 0; i < N; ++i) {
    Qld x(rand() % 5, rand() % 5, rand() % 5, rand() % 5);
    assert(nearly_equal(x, exp(log(x)), 1e-6)); // but not the other way around!
  }
}

void test_exp() {
  cout << "Testing exp" << endl;

  { // Make sure it works for reals
    Qld x(2,0,0,0);
    assert(std::abs(exp(x).real() - std::exp(2)) < 1e-10);
  }

  {
    size_t N = 1000;
    for (size_t i = 0; i < N; ++i) {
      Qd x(rand() % 5, rand() % 5, rand() % 5, rand() % 5);
      qd bx(x.a(), x.b(), x.c(), x.d());
      Qd y = exp(x);
      qd by = exp(bx);
      assert(nearly_equal(y, by, 1e-6));
    }
  }
}

void test_dot() {
  cout << "Testing dot product" << endl;
  assert(dot(Qf(1, 2, 3, 4), Qf(2, 2, 2, 2)) == 20);
  assert(dot(Qf(-1, 2, -3, 4), Qf(-1, 2, -3, 4)) == norm_squared(Qf(1, 2, 3, 4)));
  // TODO: verify this works in general
  Qf a(1,2,3,4), b(5,6,7,8);
  Qf d1 = dot(a,b);
  Qf d2 = .5*(conj(b) * a + conj(a) * b);
  assert(d1 == d2);
}

void test_cross() {
  cout << "Testing cross product" << endl;
  // TODO: verify
  assert(Qf(0,-2,4,-2) == cross(Qf(1, 2, 3, 4), Qf(2, 2, 2, 2)));
  // TODO: verify this works in general
  Qf a(0,2,3,4), b(0,6,7,8);
  Qf p1 = cross(a,b);
  Qf p2 = .5*(a * b - conj(b) * conj(a));
  assert(p1 == p2);
}

void test_commutator() {
  cout << "Testing commutator" << endl;
  Qf c = commutator(Qf(1,2,3,4), Qf(2,2,2,2));
  assert(c == Qf(0,-4,8,-4));
  assert(c == Qf(1,2,3,4) * Qf(2,2,2,2) - Qf(2,2,2,2) * Qf(1,2,3,4));
  // TODO: verify this works in general
  Qf a(1,2,3,4), b(5,6,7,8);
  Qf c1 = commutator(a,b);
  Qf c2 = 2 * cross(a,b);
  assert(c1 == c2);
}

void test_trigo() {
  cout << "Testing trigonometric functions" << endl;
  // TODO: verify ordinary real and complex trigo against std
  {
    Qd x{1,2,3,4}; qd qx{1,2,3,4};
    assert(nearly_equal(sin(x), sin(qx), 1e-6));
  }
  {
    Qd x{1,2,3,4}; qd qx{1,2,3,4};
    assert(nearly_equal(cos(x), cos(qx), 1e-6));
  }
  {
    Qd x{1,2,3,4};
    assert(nearly_equal(tan(x), sin(x) / cos(x), 1e-6));
  }
  {
    for (double t = 0; t < 6.28; t += .01) {
      assert(nearly_equal(sin(Qd(t)), sin(t), 1e-6));
      assert(nearly_equal(cos(Qd(t)), cos(t), 1e-6));
      assert(nearly_equal(tan(Qd(t)), tan(t), 1e-6));
    }
  }
  {
    for (double t = 0; t < 6.28; t += .01) {
      assert(nearly_equal(sin(Qd(0, t)), sin(Cd(0, t)), 1e-6));
      assert(nearly_equal(cos(Qd(0, t)), cos(Cd(0, t)), 1e-6));
      assert(nearly_equal(tan(Qd(0, t)), tan(Cd(0, t)), 1e-6));
    }
  }
  {
    for (size_t i = 0; i < 1000; ++i) {
      Qd x = random_quaternion<float>(rng);
      qd qx(x.a(), x.b(), x.c(), x.d());
      assert(nearly_equal(sin(x), sin(qx), 1e-6));
    }
  }
  {
    for (size_t i = 0; i < 1000; ++i) {
      Qd x = random_quaternion<float>(rng);
      qd qx(x.a(), x.b(), x.c(), x.d());
      assert(nearly_equal(cos(x), cos(qx), 1e-6));
    }
  }
  {
    for (size_t i = 0; i < 1000; ++i) {
      Qd x = random_quaternion<float>(rng);
      qd qx(x.a(), x.b(), x.c(), x.d());
      assert(nearly_equal(tan(x), tan(qx), 1e-6));
    }
  }
}

void test_hyper_trigo() {
  cout << "Testing hyperbolic trigonometric functions" << endl;
  {
    Qd x{1,2,3,4};
    assert(nearly_equal(sinh(x), (exp(x) - exp(-x)) / 2, 1e-6));
  }
  {
    Qd x{1,2,3,4};
    assert(nearly_equal(cosh(x), (exp(x) + exp(-x)) / 2, 1e-6));
  }
  {
    Qd x{1,2,3,4};
    assert(nearly_equal(tanh(x), sinh(x) / cosh(x), 1e-6));
  }
  {
    for (size_t i = 0; i < 1000; ++i) {
      Qd x = random_quaternion<float>(rng);
      qd qx(x.a(), x.b(), x.c(), x.d());
      assert(nearly_equal(sinh(x), sinh(qx), 1e-6));
    }
  }
  {
    for (size_t i = 0; i < 1000; ++i) {
      Qd x = random_quaternion<float>(rng);
      qd qx(x.a(), x.b(), x.c(), x.d());
      assert(nearly_equal(cosh(x), cosh(qx), 1e-6));
    }
  }
  {
    for (size_t i = 0; i < 1000; ++i) {
      Qd x = random_quaternion<float>(rng);
      qd qx(x.a(), x.b(), x.c(), x.d());
      assert(nearly_equal(tanh(x), tanh(qx), 1e-6));
    }
  }
  { // TODO: relative precision around 0 is not too good
    for (double t = -4; t < 4; t += .1) {
      assert(nearly_equal(sinh(Qd(t)), sinh(t), 1e-1));
      assert(nearly_equal(cosh(Qd(t)), cosh(t), 1e-1));
      assert(nearly_equal(tanh(Qd(t)), tanh(t), 1e-1));
    }
  }
  {
    for (double t = -4; t < 4; t += .1) {
      assert(nearly_equal(sinh(Qd(0, t)), sinh(Cd(0, t)), 1e-6));
      assert(nearly_equal(cosh(Qd(0, t)), cosh(Cd(0, t)), 1e-6));
      assert(nearly_equal(tanh(Qd(0, t)), tanh(Cd(0, t)), 1e-6));
    }
  }
}

void test_axby() {
  cout << "Testing axby" << endl;
  {
    Qf x(1,2,3,4), y(5,6,7,8);
    assert(axby(0,x,0,y) == 0);
  }
  {
    Qf x(1,2,3,4), y(5,6,7,8);
    assert(axby(1,x,0,y) == x);
  }
  {
    Qf x(1,2,3,4), y(5,6,7,8);
    assert(axby(0,x,1,y) == y);
  }
  {
    Qf x(1,2,3,4), y(5,6,7,8);
    assert(axby(-1,x,3,y) == -x + 3 * y);
  }
}

void test_swap() {
  cout << "Testing swap" << endl;
  {
    Qd x(1,2,3,4), y(5,6,7,8);
    std::swap(x,y);
    assert(x == Qd(5,6,7,8));
    assert(y == Qd(1,2,3,4));
  }
}

void test_lexicographic_order() {
  cout << "Testing lexicographic order" << endl;
  quaternion::lexicographic_order<double> lt;
  {
    assert(lt(Qd_0, Qd_1));
    assert(lt(Qd_1, Qd(2)));
    assert(!lt(Qd_1, Qd_i));
    assert(lt(Qd_i, Qd_1));
    assert(lt(Qd_j, Qd_i));
    assert(lt(Qd_j, Qd_1));
    assert(lt(Qd_k, Qd_j));
    assert(lt(Qd_k, Qd_i));
    assert(lt(Qd_k, Qd_1));
    assert(!lt(Qd(1,2,3,4), Qd(1,2,3,4)));
    assert(lt(Qd(1,2,3,4), Qd(5,6,7,8)));
    assert(lt(Qd(1,2,3,4), Qd(1,5,7,8)));
    assert(lt(Qd(1,2,3,4), Qd(1,2,7,8)));
    assert(lt(Qd(1,2,3,4), Qd(1,2,3,8)));
  }
}

void test_hash() {
  cout << "Testing hash" << endl;
  {
    Qf x(1,2,3,4);
    quaternion::hash<Qf::value_type> h;
    assert(h(x) == 4040899601354814717);
  }
}

void test_stl() {
  cout << "Testing STL" << endl;
  {
    vector<Qf> qs{{1, 2, 3, 4},
                  {5, 6, 7, 8},
                  {1, 3, 5, 7},
                  {2, 4, 6, 8}};
    auto v = accumulate(qs.begin(), qs.end(), Qf_1, multiplies<Qf>());
    assert(v == Qf_1 * Qf(1, 2, 3, 4) * Qf(5, 6, 7, 8) * Qf(1, 3, 5, 7) * Qf(2, 4, 6, 8));
  }

  {
    unordered_set<Qd, quaternion::hash<double>> q_set = {{1, 2, 3, 4}, {5, 6, 7, 8}, {1, 2, 3, 4}};
    assert(q_set.size() == 2);
    auto v = accumulate(q_set.begin(), q_set.end(), Qd_0, plus<Qd>());
    assert(v == Qd(1,2,3,4) + Qd(5,6,7,8));
  }
  {
    using Qi = Quaternion<int>;
    map<Qi, int, quaternion::lexicographic_order<int>> m = {{Qi(2), 3}, {Qi(3,4,5), 1}, {Qi(3,4,6), 2}};
    assert(m.size() == 3);
    auto t = accumulate(m.begin(), m.end(), 0, [](int tt, const pair<Qi,int>& q){ return tt + q.second; });
    assert(t == 6);
  }
}

void test_binary_quaternions() {
  cout << "Testing binary quaternions" << endl;
  {
    using Qb = Quaternion<bool>;
    Qb x(0,1,0,1), y(1,0,1,0);
    assert(x * y == Qb(0,0,0,1));
    //cout << pow(x, 5) << endl; this doesn't compile, requires the "negative" of a binary number
    //cout << exp(x) << endl; // this doesn't compile for now
  }
}

void test_integer_quaternions() {
  cout << "Testing integer quaternions" << endl;
  {
    using Qi = Quaternion<int>;
    Qi x(1, 2, 3, 4), y(5, 6, 7, 8);
    assert(x + y == Qi(6, 8, 10, 12));
    //cout << pow(x, 5) << endl;
    //cout << pow(x, 3.5) << endl;
    //cout << exp(x) << endl; // this doesn't compile for now
  }
  {
    Cd x(1,1);
  }
  // TODO: more tests here
}

// TODO: try quaternion with complex anyway

void test_quaternion_matrix() {
  cout << "Testing quaternion matrix: ";
  {
    constexpr size_t n = 8; // increasing n quickly becomes a perf test
    array<array<Qd,n>,n> M;
    for (size_t i = 0; i < n; ++i)
      for (size_t j = 0; j < n; ++j)
        M[i][j] = random_quaternion<double>(rng);
    cout << det(M);
  }
  cout << " OK" << endl;
}

void test_io() {
  stringstream s;
  s << Qf() << Qf(1) << Qf(-1) << Qf(0,1) << Qf(0,-1) << Qf(0,0,1) << Qf(0,0,-1);
  s << Qf(0,0,0,1) << Qf(0,0,0,-1) << Qf(1,2,3,4) << Qf(-1,2,-3,4);
  assert(s.str() == "01-1i-ij-jk-k1+2i+3j+4k-1+2i-3j+4k");
}

void test_io_eps() {
  cout << "Testing io/eps" << endl;
  Qd x(1e-1, 1e-2, 1e-3, 1e-4);
  stringstream s;
  s << set_display_eps(1e-6) << x;
  s << set_display_eps(1e-4) << x;
  s << set_display_eps(1e-3) << x;
  s << set_display_eps(1e-2) << x;
  s << set_display_eps(1e-1) << x;
  s << set_display_eps(1) << x;
  s << set_display_eps(1e-6);
  assert(s.str() == "0.1+0.01i+0.001j+0.0001k0.1+0.01i+0.001j0.1+0.01i0.1");
}

void test_io_style() {
  cout << "Testing io/style" << endl;
  Qd x(1,2,3,4);
  stringstream s;
  s << set_display_eps(1e-6);
  s << set_display_style(q_nice) << x << " ";
  s << set_display_style(q_compact) << x ;
  assert(s.str() == "1+2i+3j+4k {1,2,3,4}");
}

/**
 * Some operations don't compile with boost rational, in particular
 * those that involve e.g. std::fabs (like operator<< does!).
 * This works fine, but I'm disabling it for now so that Quaternion.h does
 * not depend on boost at all.
 */
//void test_boost_rational() {
//  cout << "Testing with boost::rational" << endl;
//  typedef rational<long> Rl;
//  typedef Quaternion<Rl> Qrl;
//  {
//    Qrl x({1,2},{3,4},{5,6},{7,8}), y({2,3},{4,5},{6,7},{8,9});
//    Qrl z = x * y;
//    assert(z.a() == Rl(-554,315));
//    assert(z.b() == Rl(481,540));
//    assert(z.c() == Rl(641,630));
//    assert(z.d() == Rl(253,252));
//  }
//}

void test_precision() {
  cout << "Testing precision" << endl;
  {
    Qd x(0,1,0,0);
    x = Qd_i * (2.0 + exp(log(x)) / x - 2.0);
    assert(nearly_equal(x, Qd_i, 1e-16));
  }

  {
    Qd x(0.0001,0.002,3.5,3.1415926535);
    assert(nearly_equal(exp(log(x)), x, 1e-8));
  }

  {
    Qf x(0.0001,0.002,3.5,3.1415926535);
    assert(abs(exp(log(x)) - x) < 1e-6);
  }
}

/**
 * Between standard C++, boost and vectorclass which uses intrinsics, no difference.
 * The compiler is probably optimizing well enough, or the intrinsics are not used properly.
 * Whoever is in first position (boot or quaternion) in this micro-benchmark, is twice as
 * fast as the second.... But only on the Mac/clang or gcc. That doesn't happen on Linux/gcc.
 */
void test_multiplication_speed() {
  cout << "Testing multiplication speed" << endl;
  size_t N = 100000;

  Qf q1 = random_quaternion<float>(rng), q2 = random_quaternion<float>(rng);

  {
    float certificate = 0.0;
    auto start = std::chrono::system_clock::now();
    for (size_t i = 0; i < N; ++i) {
      Qf r = q1 * (q2 + (float)i);
      certificate += r.a() + r.b() + r.c() + r.d();
    }
    auto end = std::chrono::system_clock::now();
    std::chrono::nanoseconds diff = end - start;
    cout << "Quaternion: " << (diff.count() / N) << "ns" << endl;
    cout << "Certificate=" << certificate << endl;
  }

  { // With Boost
    qf a(q1.a(),q1.b(),q1.c(),q1.d()), b(q2.a(),q2.b(),q2.c(),q2.d());
    float certificate = 0.0;
    auto start = std::chrono::system_clock::now();
    for (size_t i = 0; i < N; ++i) {
      qf r = a * (b + (float)i);
      certificate += r.R_component_1() + r.R_component_2() + r.R_component_3() + r.R_component_4();
    }
    auto end = std::chrono::system_clock::now();
    std::chrono::nanoseconds diff = end - start;
    cout << "Boost: " << (diff.count() / N) << "ns" << endl;
    cout << "Certificate=" << certificate << endl;
  }
}

// TODO: warm up tests correctly, and get some variance
void test_pow_speed() {
  cout << "Testing pow speed" << endl;
  size_t N = 1000000;

  std::default_random_engine generator;
  std::lognormal_distribution<float> distribution(0.0,1.0);
  auto rng = bind(distribution, generator);
  Qf q1 = random_quaternion<float>(rng);

  { // With Boost
    qf a(q1.a(),q1.b(),q1.c(),q1.d());
    float certificate = 0.0;
    auto start = std::chrono::system_clock::now();
    for (size_t i = 0; i < N; ++i) {
      qf r = pow(a, 15);
      certificate += r.R_component_1() + r.R_component_2() + r.R_component_3() + r.R_component_4();
    }
    auto end = std::chrono::system_clock::now();
    std::chrono::nanoseconds diff = end - start;
    cout << "Boost: " << (diff.count() / N) << "ns" << endl;
    cout << "Certificate=" << certificate << endl;
  }

  {
    float certificate = 0.0;
    auto start = std::chrono::system_clock::now();
    for (size_t i = 0; i < N; ++i) {
      Qf r = pow(q1, 15);
      certificate += r.a() + r.b() + r.c() + r.d();
    }
    auto end = std::chrono::system_clock::now();
    std::chrono::nanoseconds diff = end - start;
    cout << "quaternion: " << (diff.count() / N) << "ns" << endl;
    cout << "Certificate=" << certificate << endl;
  }

  // TODO: match results
}

void test_exp_speed() {
  cout << "Testing exp speed" << endl;

  size_t N = 100000;

  {
    float certificate = 0.0;
    auto start = std::chrono::system_clock::now();
    for (size_t i = 0; i < N; ++i) {
      Qld x(rand() % 5, rand() % 5, rand() % 5, rand() % 5);
      Qld r = exp(x);
      certificate += r.a() + r.b() + r.c() + r.d();
    }
    auto end = std::chrono::system_clock::now();
    std::chrono::nanoseconds diff = end - start;
    cout << "quaternion: " << (diff.count() / N) << "ns" << endl;
    cout << "Certificate=" << certificate << endl;
  }

  {
    float certificate = 0.0;
    auto start = std::chrono::system_clock::now();
    for (size_t i = 0; i < N; ++i) {
      qd x(rand() % 5, rand() % 5, rand() % 5, rand() % 5);
      qd r = exp(x);
      certificate += r.R_component_1() + r.R_component_2() + r.R_component_3() + r.R_component_4();
    }
    auto end = std::chrono::system_clock::now();
    std::chrono::nanoseconds diff = end - start;
    cout << "Boost: " << (diff.count() / N) << "ns" << endl;
    cout << "Certificate=" << certificate << endl;
  }
}

void test_axby_speed() {
  cout << "Testing axby speed" << endl;
  size_t N = 100000;

  Qf q1 = random_quaternion<float>(rng), q2 = random_quaternion<float>(rng);

  { // With Boost
    qf a(q1.a(),q1.b(),q1.c(),q1.d()), b(q2.a(),q2.b(),q2.c(),q2.d());
    float certificate = 0.0;
    auto start = std::chrono::system_clock::now();
    for (size_t i = 0; i < N; ++i) {
      qf r = ((float) i) * a + ((float) i+1) * b;
      certificate += r.R_component_1() + r.R_component_2() + r.R_component_3() + r.R_component_4();
    }
    auto end = std::chrono::system_clock::now();
    std::chrono::nanoseconds diff = end - start;
    cout << "Boost: " << (diff.count() / N) << "ns" << endl;
    cout << "Certificate=" << certificate << endl;
  }

  {
    float certificate = 0.0;
    auto start = std::chrono::system_clock::now();
    for (size_t i = 0; i < N; ++i) {
      Qf r = axby(i, q1, i + 1, q2);
      certificate += r.a() + r.b() + r.c() + r.d();
    }
    auto end = std::chrono::system_clock::now();
    std::chrono::nanoseconds diff = end - start;
    cout << "Quaternion: " << (diff.count() / N) << "ns" << endl;
    cout << "Certificate=" << certificate << endl;
  }
}

void test_tan_speed() {
  cout << "Testing tan speed" << endl;
  size_t N = 100000;

  Qf q1 = random_quaternion<float>(rng);

  { // With Boost
    qf a(q1.a(),q1.b(),q1.c(),q1.d());
    float certificate = 0.0;
    auto start = std::chrono::system_clock::now();
    for (size_t i = 0; i < N; ++i) {
      qf r = tan(a);
      certificate += r.R_component_1() + r.R_component_2() + r.R_component_3() + r.R_component_4();
    }
    auto end = std::chrono::system_clock::now();
    std::chrono::nanoseconds diff = end - start;
    cout << "Boost: " << (diff.count() / N) << "ns" << endl;
    cout << "Certificate=" << certificate << endl;
  }

  {
    float certificate = 0.0;
    auto start = std::chrono::system_clock::now();
    for (size_t i = 0; i < N; ++i) {
      Qf r = tan(q1);
      certificate += r.a() + r.b() + r.c() + r.d();
    }
    auto end = std::chrono::system_clock::now();
    std::chrono::nanoseconds diff = end - start;
    cout << "Quaternion: " << (diff.count() / N) << "ns" << endl;
    cout << "Certificate=" << certificate << endl;
  }
}

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

  test_nearly_equal();
  test_det();

  test_constructors();
  test_trigonometric_constructors();
  test_IJK();
  test_accessors();
  test_conjugate();
  test_complex_matrix_2d_representation();
  test_real_matrix_4d_representation();
  test_to_polar_representation();
  test_to_rotation_matrix();
  test_euler_angles();
  test_norms();
  test_equality();
  test_plus_minus();
  test_unary_w_scalar();
  test_unary_w_complex();
  test_unary_w_quaternion();
  test_operators();
  //test_boost_rational(); disabled for now, but keep, works fine
  test_pow2();
  test_pow3();
  test_pow();
  test_q_pow();
  test_log();
  test_exp();
  test_dot();
  test_cross();
  test_commutator();
  test_trigo();
  test_hyper_trigo();
  test_axby();
  test_swap();
  test_lexicographic_order();
  test_hash();
  test_stl();
  test_binary_quaternions(); // really requires specialized implementation
  test_integer_quaternions(); // some operations don't work (transcendentals)
  test_quaternion_matrix();
  test_io();
  test_io_eps();
  test_io_style();

  cout << endl;
  test_precision();

  cout << endl;
  test_exp_speed();
  test_multiplication_speed();
  test_pow_speed();
  test_axby_speed();
  test_tan_speed();

  return 0;
}


// File: Matrix3x3.hpp
#ifndef _Matrix3x3_hpp
#define _Matrix3x3_hpp

#include <iostream>
#include <boost/python.hpp>
#include <LinearMath/btMatrix3x3.h>

using namespace boost::python;
using namespace std;

ostream& operator<<(ostream& os, const btMatrix3x3& m);

void defineMatrix3x3();

#endif // _Matrix3x3_hpp


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "latbuilder/Util.h"
#include "netbuilder/Helpers/Path.h"
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <boost/filesystem.hpp>

#include <NTL/ZZ.h>

namespace LatBuilder
{

//===============================================================================
Polynomial PolynomialFromInt(uInteger x)
{
   Polynomial P;
   P.SetLength(LENGTH_UINTEGER);
   uInteger y=x;
   long i=0;
    do{
       P[i++] = y%2;
   } while(y >>= 1);

    P.normalize();
    return P;
}

//================================================================================

uInteger IndexOfPolynomial(Polynomial P)
{
   uInteger x=0;
   uInteger q=1;

   for (int i = 0; i <= deg(P); i++)
    {
        if(IsOne(coeff(P,i))){
            x+= q;
       }
       q <<= 1;
    }
    return x;
}

//================================================================================

std::vector<uInteger> primeFactors(uInteger n, bool raise)
{
  NTL::PrimeSeq s;
   unsigned long p;
   p = s.next();
   std::vector<uInteger> factors;
   while (p <= n) {
      ldiv_t qr = std::ldiv(n, p);
      uInteger factor = 1;
      while (qr.rem == 0) {
         n = qr.quot;
         factor *= p;
         if (!raise)
            break;
         qr = std::ldiv(n, p);
      }
      if (factor > 1)
         factors.push_back(factor);
      p = s.next();
   }
   return factors;
}

//================================================================================

std::map<uInteger, uInteger> primeFactorsMap(uInteger n)
{
  NTL::PrimeSeq s;
   unsigned long factor;
   factor = s.next();
   std::map<uInteger, uInteger> factors;
   while (factor <= n) {
      ldiv_t qr = std::ldiv(n, factor);
      while (qr.rem == 0) {
         n = qr.quot;
         factors[factor]++;
         qr = std::ldiv(n, factor);
      }
      factor = s.next();
   }
   return factors;
}

//================================================================================

std::pair<long long, long long> egcd(uInteger a, uInteger b)
{
   typedef std::pair<long long, long long> result_type;
   result_type cur{0, 1};
   result_type last{1, 0};
   while (b != 0) {
      ldiv_t qr = std::ldiv(a, b);
      a = b;
      b = qr.rem;
      std::swap(cur, last);
      cur.first = cur.first - qr.quot * last.first;
      cur.second = cur.second - qr.quot * last.second;
   }
   return last;
}

//================================================================================

uInteger Vm(const Polynomial& h, const Polynomial& P)
{

   long m = deg(P);
   NTL::vector<NTL::GF2> w;
   w.resize(m);
   uInteger res = 0;
   for(long i=0; i<m; i++){
      w[i] = (m-i-1>deg(h)) ? NTL::GF2(0) : coeff(h,m-i-1);
      for(long j=0; j<i; j++){
         w[i] += w[j]* coeff(P,m-(i-j));
      }
   }
   uInteger q = 1;
   for(long i=m-1; i>=0; i--){
      res += IsOne(w[i])*q;
      q *= 2;
   }

   return res;

}

uInteger log2Int(unsigned int n){
    unsigned int res=0;
    while (n>1){
    n = n >> 1;
    res ++;
    }
    return res;
}

//================================================================================

const char* ws = " \t\n\r\f\v";

// trim from end (right)
inline std::string& rtrim(std::string& s, const char* t = ws)
{
      s.erase(s.find_last_not_of(t) + 1);
      return s;
}

// trim from beginning (left)
inline std::string& ltrim(std::string& s, const char* t = ws)
{
      s.erase(0, s.find_first_not_of(t));
      return s;
}

// trim from both ends (left & right)
inline std::string& trim(std::string& s, const char* t = ws)
{
      return ltrim(rtrim(s, t), t);
}

std::string getDefaultPolynomial(unsigned int degree)
{
    if (degree <= 32)
    {
        std::string path = NetBuilder::PATH_TO_LATNETBUILDER_DIR + "/../share/latnetbuilder/data/default_polys.csv";
        if (boost::filesystem::exists(path)){
            std::ifstream file(path);
            std::string sent;
            do
            {
            getline(file,sent);
            trim(sent);
            }
            while (sent != "###");

            getline(file,sent);

            for(unsigned int d = 0; d <= degree; ++d)
            {
                getline(file,sent);
            }
            return sent;
        }
        else{
            throw std::runtime_error("Unable to locate data folder. The value of PATH_TO_LATNETBUILDER_DIR is probably incorrect. See netbuilder/Path.h.");
        }
    }
    return "";
}

std::string to_string(LatticeType LT){

    static const char * LatticeTypeStrings[] = { "Ordinary", "Polynomial", "Digital" };

    return LatticeTypeStrings[(int) LT];
}

} // namespace LatBuilder


// Copyright 2004, 2005 The Trustees of Indiana University.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

//  Authors: Nick Edmonds
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_RMAT_GENERATOR_FASTER_HPP
#define BOOST_GRAPH_RMAT_GENERATOR_FASTER_HPP

#include <math.h>
#include <iterator>
#include <utility>
#include <vector>
#include <queue>
#include <map>
#include <boost/assert.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/type_traits/is_base_and_derived.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/test/floating_point_comparison.hpp>

using boost::shared_ptr;
using boost::uniform_01;

// Returns floor(log_2(n)), and -1 when n is 0
template <typename IntegerType>
inline int int_log2(IntegerType n) {
  int l = 0;
  while (n > 0) {++l; n >>= 1;}
  return l - 1;
}

struct keep_all_edges {
  template <typename T>
  bool operator()(const T&, const T&) { return true; }
};

template <typename Distribution, typename ProcessId>
struct keep_local_edges {

  keep_local_edges(const Distribution& distrib, const ProcessId& id)
    : distrib(distrib), id(id)
  { }

  template <typename T>
  bool operator()(const T& x, const T& y)
  { return distrib(x) == id || distrib(y) == id; }

private:
  const Distribution& distrib;
  const ProcessId&    id;
};

template <typename RandomGenerator, typename T>
void
generate_permutation_vector(RandomGenerator& gen, std::vector<T>& vertexPermutation, T n)
{
  using boost::uniform_int;

  vertexPermutation.resize(n);

  // Generate permutation map of vertex numbers
  uniform_int<T> rand_vertex(0, n-1);
  for (T i = 0; i < n; ++i)
    vertexPermutation[i] = i;

  // Can't use std::random_shuffle unless we create another (synchronized) PRNG
  for (T i = 0; i < n; ++i)
    std::swap(vertexPermutation[i], vertexPermutation[rand_vertex(gen)]);
}

template <typename RandomGenerator, typename T>
std::pair<T,T>
generate_edge_faster(shared_ptr<uniform_01<RandomGenerator> > prob, T n,
                     unsigned int SCALE, double a, double b, double c, double /*d*/)
{
  T u = 0, v = 0;
  T step = n/2;
  for (unsigned int j = 0; j < SCALE; ++j) {
    double p = (*prob)();

    if (p < a)
      ;
    else if (p >= a && p < a + b)
      v += step;
    else if (p >= a + b && p < a + b + c)
      u += step;
    else { // p > a + b + c && p < a + b + c + d
      u += step;
      v += step;
    }

    step /= 2;
  }

  return std::make_pair(u, v);
}

namespace boost {

  /*
    Chakrabarti's R-MAT scale free generator.

    For all flavors of the R-MAT iterator a+b+c+d must equal 1 and for the
    unique_rmat_iterator 'm' << 'n^2'.  If 'm' is too close to 'n^2' the
    generator may be unable to generate sufficient unique edges

    To get a true scale free distribution {a, b, c, d : a > b, a > c, a > d}
  */

  template<typename RandomGenerator, typename Graph>
  class rmat_iterator_faster
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    // No argument constructor, set to terminating condition
    rmat_iterator_faster()
      : gen(), edge(0) { }

    // Initialize for edge generation
    rmat_iterator_faster(RandomGenerator& gen, vertices_size_type n,
                         edges_size_type m, double a, double b, double c,
                         double d, bool permute_vertices = true)
      : gen(), n(n), a(a), b(b), c(c), d(d), edge(m),
        permute_vertices(permute_vertices),
        SCALE(int_log2(n))

    {
      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., boost::test_tools::fraction_tolerance(1.e-5)));

      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      // TODO: Generate the entire adjacency matrix then "Clip and flip" if undirected graph

      // Generate the first edge
      vertices_size_type u, v;
      tie(u, v) = generate_edge_faster(this->gen, n, SCALE, a, b, c, d);

      if (permute_vertices)
        current = std::make_pair(vertexPermutation[u],
                                 vertexPermutation[v]);
      else
        current = std::make_pair(u, v);

      --edge;
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    rmat_iterator_faster& operator++()
    {
      vertices_size_type u, v;
      tie(u, v) = generate_edge_faster(this->gen, n, SCALE, a, b, c, d);

      if (permute_vertices)
        current = std::make_pair(vertexPermutation[u],
                                 vertexPermutation[v]);
      else
        current = std::make_pair(u, v);

      --edge;

      return *this;
    }

    rmat_iterator_faster operator++(int)
    {
      rmat_iterator_faster temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const rmat_iterator_faster& other) const
    {
      return edge == other.edge;
    }

    bool operator!=(const rmat_iterator_faster& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;
    vertices_size_type n;
    double a, b, c, d;
    int edge;
    bool permute_vertices;
    int SCALE;

    // Internal data structures
    std::vector<vertices_size_type> vertexPermutation;
    value_type current;
  };

  // Sorted version for CSR
  template <typename T>
  struct sort_pair {
    bool operator() (const std::pair<T,T>& x, const std::pair<T,T>& y)
    {
      if (x.first == y.first)
        return x.second > y.second;
      else
        return x.first > y.first;
    }
  };

  template<typename RandomGenerator, typename Graph,
           typename EdgePredicate = keep_all_edges>
  class sorted_rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    // No argument constructor, set to terminating condition
    sorted_rmat_iterator()
      : gen(), values(sort_pair<vertices_size_type>()), done(true)
    { }

    // Initialize for edge generation
    sorted_rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                         edges_size_type m, double a, double b, double c,
                         double d, bool permute_vertices = true,
                         EdgePredicate ep = keep_all_edges())
      : gen(), permute_vertices(permute_vertices),
        values(sort_pair<vertices_size_type>()), done(false)

    {
      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., boost::test_tools::fraction_tolerance(1.e-5)));

      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      std::vector<vertices_size_type> vertexPermutation;
      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      // TODO: "Clip and flip" if undirected graph
      int SCALE = int_log2(n);

      for (edges_size_type i = 0; i < m; ++i) {

        vertices_size_type u, v;
        tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

        if (permute_vertices) {
          if (ep(vertexPermutation[u], vertexPermutation[v]))
            values.push(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
        } else {
          if (ep(u, v))
            values.push(std::make_pair(u, v));
        }

      }

      current = values.top();
      values.pop();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    sorted_rmat_iterator& operator++()
    {
      if (!values.empty()) {
        current = values.top();
        values.pop();
      } else
        done = true;

      return *this;
    }

    sorted_rmat_iterator operator++(int)
    {
      sorted_rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const sorted_rmat_iterator& other) const
    {
      return values.empty() && other.values.empty() && done && other.done;
    }

    bool operator!=(const sorted_rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;
    bool permute_vertices;

    // Internal data structures
    std::priority_queue<value_type, std::vector<value_type>, sort_pair<vertices_size_type> > values;
    value_type current;
    bool       done;
  };

  // This version is slow but guarantees unique edges
  template<typename RandomGenerator, typename Graph,
           typename EdgePredicate = keep_all_edges>
  class unique_rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    // No argument constructor, set to terminating condition
    unique_rmat_iterator()
        : gen(), done(true)
    { }

    // Initialize for edge generation
    unique_rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                         edges_size_type m, double a, double b, double c,
                         double d, bool permute_vertices = true,
                         EdgePredicate ep = keep_all_edges())
      : gen(), done(false)

    {
      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., boost::test_tools::fraction_tolerance(1.e-5)));

      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      std::vector<vertices_size_type> vertexPermutation;
      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      int SCALE = int_log2(n);

      std::map<value_type, bool> edge_map;

      edges_size_type edges = 0;
      do {
        vertices_size_type u, v;
        tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

        // Lowest vertex number always comes first
        // (this means we don't have to worry about i->j and j->i being in the edge list)
        if (u > v && is_same<directed_category, undirected_tag>::value)
          std::swap(u, v);

        if (edge_map.find(std::make_pair(u, v)) == edge_map.end()) {
          edge_map[std::make_pair(u, v)] = true;

          if (permute_vertices) {
            if (ep(vertexPermutation[u], vertexPermutation[v]))
              values.push_back(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
          } else {
            if (ep(u, v))
              values.push_back(std::make_pair(u, v));
          }

          edges++;
        }
      } while (edges < m);
      // NGE - Asking for more than n^2 edges will result in an infinite loop here
      //       Asking for a value too close to n^2 edges may as well

      current = values.back();
      values.pop_back();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    unique_rmat_iterator& operator++()
    {
      if (!values.empty()) {
        current = values.back();
        values.pop_back();
      } else
        done = true;

      return *this;
    }

    unique_rmat_iterator operator++(int)
    {
      unique_rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const unique_rmat_iterator& other) const
    {
      return values.empty() && other.values.empty() && done && other.done;
    }

    bool operator!=(const unique_rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;

    // Internal data structures
    std::vector<value_type> values;
    value_type              current;
    bool                    done;
  };

  // This version is slow but guarantees unique edges
  template<typename RandomGenerator, typename Graph,
           typename EdgePredicate = keep_all_edges>
  class sorted_unique_rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    // No argument constructor, set to terminating condition
    sorted_unique_rmat_iterator()
      : gen(), values(sort_pair<vertices_size_type>()), done(true) { }

    // Initialize for edge generation
    sorted_unique_rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                                edges_size_type m, double a, double b, double c,
                                double d, bool bidirectional = false,
                                bool permute_vertices = true,
                                EdgePredicate ep = keep_all_edges())
      : gen(), bidirectional(bidirectional),
        values(sort_pair<vertices_size_type>()), done(false)

    {
      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., boost::test_tools::fraction_tolerance(1.e-5)));

      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      std::vector<vertices_size_type> vertexPermutation;
      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      int SCALE = int_log2(n);

      std::map<value_type, bool> edge_map;

      edges_size_type edges = 0;
      do {

        vertices_size_type u, v;
        tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

        if (bidirectional) {
          if (edge_map.find(std::make_pair(u, v)) == edge_map.end()) {
            edge_map[std::make_pair(u, v)] = true;
            edge_map[std::make_pair(v, u)] = true;

            if (ep(u, v)) {
              if (permute_vertices) {
                values.push(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
                values.push(std::make_pair(vertexPermutation[v], vertexPermutation[u]));
              } else {
                values.push(std::make_pair(u, v));
                values.push(std::make_pair(v, u));
              }
           }

            ++edges;
          }
        } else {
          // Lowest vertex number always comes first
          // (this means we don't have to worry about i->j and j->i being in the edge list)
          if (u > v && is_same<directed_category, undirected_tag>::value)
            std::swap(u, v);

          if (edge_map.find(std::make_pair(u, v)) == edge_map.end()) {
            edge_map[std::make_pair(u, v)] = true;

            if (permute_vertices) {
              if (ep(vertexPermutation[u], vertexPermutation[v]))
                values.push(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
            } else {
              if (ep(u, v))
                values.push(std::make_pair(u, v));
            }

            ++edges;
          }
        }

      } while (edges < m);
      // NGE - Asking for more than n^2 edges will result in an infinite loop here
      //       Asking for a value too close to n^2 edges may as well

      current = values.top();
      values.pop();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    sorted_unique_rmat_iterator& operator++()
    {
      if (!values.empty()) {
        current = values.top();
        values.pop();
      } else
        done = true;

      return *this;
    }

    sorted_unique_rmat_iterator operator++(int)
    {
      sorted_unique_rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const sorted_unique_rmat_iterator& other) const
    {
      return values.empty() && other.values.empty() && done && other.done;
    }

    bool operator!=(const sorted_unique_rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;
    bool             bidirectional;

    // Internal data structures
    std::priority_queue<value_type, std::vector<value_type>,
                        sort_pair<vertices_size_type> > values;
    value_type current;
    bool       done;
  };

} // end namespace boost

#endif // BOOST_GRAPH_RMAT_GENERATOR_FASTER_HPP


#include <Eigen/Dense>
#include <iostream>
#include <memory>
#include <mutex>
#include <sstream>
#include <string>
#include <vector>

#include "boost/python.hpp"
#include "boost/python/stl_iterator.hpp"
#include "boost/shared_ptr.hpp"

std::mutex mutex;

template <typename T>
inline std::vector<T> py_list_to_std_vector(
    const boost::python::object& iterable) {
  return std::vector<T>(boost::python::stl_input_iterator<T>(iterable),
                        boost::python::stl_input_iterator<T>());
}

template <class T>
inline boost::python::list std_vector_to_py_list(std::vector<T> vector) {
  typename std::vector<T>::iterator iter;
  boost::python::list list;
  for (iter = vector.begin(); iter != vector.end(); ++iter) {
    list.append(*iter);
  }
  return list;
}

class Scene {
 public:
  Scene() {}

  void set_strain(std::vector<double>& strain) {
    strain_.resize(strain.size());
    // std::lock_guard<std::mutex> lock(mutex);
    for (int i = 0; i < strain.size(); i++) {
      auto a = strain.at(i);
      strain_(i) = a;
    }
  }

  Eigen::VectorXd& strain() {
    // std::lock_guard<std::mutex> lock(mutex);
    return strain_;
  }

  Eigen::VectorXd stress() {
    // std::lock_guard<std::mutex> lock(mutex);
    return stress_;
  }

  void compute_stress() {
    // std::lock_guard<std::mutex> lock(mutex);
    stress_ = 100 * strain_;
  }

 private:
  Eigen::VectorXd stress_;
  Eigen::VectorXd strain_;
};

class Omega {
 public:
  std::shared_ptr<Scene> get_scene() { return scene_; }

  int creat_scene() {
    scene_ = std::make_shared<Scene>();
    my_scenes_.push_back(scene_);
    return my_scenes_.size() - 1;
  }

  void checkout_scene(int i) {
    std::lock_guard<std::mutex> lock(mutex);
    scene_ = my_scenes_.at(i);
  }
  void load_scene(std::shared_ptr<Scene> scene) { scene_ = scene; }

  boost::python::list get_stress() {
    scene_->compute_stress();
    auto stress = scene_->stress();
    std::vector<double> a;
    for (int i = 0; i < stress.size(); i++) {
      a.push_back(stress(i));
    }
    return std_vector_to_py_list(a);
  }

  void set_dstrain(boost::python::list dstrain) {
    std::lock_guard<std::mutex> lock(mutex);
    std::vector<double> a;

    a = py_list_to_std_vector<double>(dstrain);
    scene_->set_strain(a);
    // std::lock_guard<std::mutex> unlock(mutex);
  }

 private:
  std::vector<std::shared_ptr<Scene>> my_scenes_;
  std::shared_ptr<Scene> scene_;
  int currentSceneNb;
};

BOOST_PYTHON_MODULE(Omega) {
  //   boost::python::class_<Scene>("Scene")
  //       //.def(init<double, double>())
  //       .def("set_strain", &Scene::set_strain)
  //       .def("strain", &Scene::strain)
  //       .def("compute_stress", &Scene::compute_stress);

  boost::python::class_<Omega>("Omega")
      //.def(init<double, double>())
      .def("creat_scene", &Omega::creat_scene)
      .def("get_scene", &Omega::get_scene)
      .def("checkout_scene", &Omega::checkout_scene)
      .def("get_stress", &Omega::get_stress)
      .def("set_dstrain", &Omega::set_dstrain)
      .def("load_scene", &Omega::load_scene);
}


#include <boost/bimap.hpp>

int main()
{
	boost::bimap<int, int> test;
}


/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

// Algorithm details and publications: http://www.golems.org/node/1570

#ifndef DART_PLANNING_PATHFOLLOWINGTRAJECTORY_HPP_
#define DART_PLANNING_PATHFOLLOWINGTRAJECTORY_HPP_

#include <Eigen/Core>
#include "dart/planning/Path.hpp"
#include "dart/planning/Trajectory.hpp"

namespace dart {
namespace planning {

class PathFollowingTrajectory : public Trajectory
{
public:
	PathFollowingTrajectory(const Path &path, const Eigen::VectorXd &maxVelocity, const Eigen::VectorXd &maxAcceleration);
	~PathFollowingTrajectory(void);

	bool isValid() const;
	double getDuration() const;
	Eigen::VectorXd getPosition(double time) const;
	Eigen::VectorXd getVelocity(double time) const;
	double getMaxAccelerationError();

private:
	struct TrajectoryStep {
		TrajectoryStep() {}
		TrajectoryStep(double pathPos, double pathVel) :
			pathPos(pathPos),
      pathVel(pathVel),
      time(0.0)
		{}
		double pathPos;
		double pathVel;
		double time;
	};

	bool getNextSwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration);
	bool getNextAccelerationSwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration);
	bool getNextVelocitySwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration);
	bool integrateForward(std::list<TrajectoryStep> &trajectory, double acceleration);
	void integrateBackward(std::list<TrajectoryStep> &trajectory, std::list<TrajectoryStep> &startTrajectory, double acceleration);
	double getMinMaxPathAcceleration(double pathPosition, double pathVelocity, bool max);
	double getMinMaxPhaseSlope(double pathPosition, double pathVelocity, bool max);
	double getAccelerationMaxPathVelocity(double pathPos);
	double getVelocityMaxPathVelocity(double pathPos);
	double getAccelerationMaxPathVelocityDeriv(double pathPos);
	double getVelocityMaxPathVelocityDeriv(double pathPos);

	TrajectoryStep getIntersection(const std::list<TrajectoryStep> &trajectory, std::list<TrajectoryStep>::iterator &it, const TrajectoryStep &linePoint1, const TrajectoryStep &linePoint2);
	inline double getSlope(const TrajectoryStep &point1, const TrajectoryStep &point2);
	inline double getSlope(std::list<TrajectoryStep>::const_iterator lineEnd);

	std::list<TrajectoryStep>::const_iterator getTrajectorySegment(double time) const;

	Path path;
	Eigen::VectorXd maxVelocity;
	Eigen::VectorXd maxAcceleration;
	unsigned int n;
	bool valid;
	std::list<TrajectoryStep> trajectory;

	static const double eps;
	static const double timeStep;

	mutable double cachedTime;
	mutable std::list<TrajectoryStep>::const_iterator cachedTrajectorySegment;
};

} // namespace planning
} // namespace dart

#endif // DART_PLANNING_PATHFOLLOWINGTRAJECTORY_HPP_


/***********************************************************************************************************************
*  OpenStudio(R), Copyright (c) 2008-2021, Alliance for Sustainable Energy, LLC, and other contributors. All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
*  following conditions are met:
*
*  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
*  disclaimer.
*
*  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
*  disclaimer in the documentation and/or other materials provided with the distribution.
*
*  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote products
*  derived from this software without specific prior written permission from the respective party.
*
*  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative works
*  may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without specific prior
*  written permission from Alliance for Sustainable Energy, LLC.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER(S) AND ANY CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
*  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER(S), ANY CONTRIBUTORS, THE UNITED STATES GOVERNMENT, OR THE UNITED
*  STATES DEPARTMENT OF ENERGY, NOR ANY OF THEIR EMPLOYEES, BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
*  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
*  USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
*  STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
*  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***********************************************************************************************************************/

#ifndef UTILITIES_DATA_MATRIX_HPP
#define UTILITIES_DATA_MATRIX_HPP

#include "Vector.hpp"
#include "../UtilitiesAPI.hpp"
#include "../core/Logger.hpp"

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

namespace openstudio {

/// Matrix
typedef boost::numeric::ublas::matrix<double> Matrix;

//////////////////////////////////////////////////////////////////////////
// Begin SWIG'able, copy and paste into Matrix.i
//////////////////////////////////////////////////////////////////////////

/// new operators

UTILITIES_API bool operator==(const Matrix& lhs, const Matrix& rhs);
UTILITIES_API bool operator!=(const Matrix& lhs, const Matrix& rhs);

/// common methods

/// linear interpolation of the function v = f(x, y) at point xi, yi
/// assumes that x and y are strictly increasing
UTILITIES_API double interp(const Vector& x, const Vector& y, const Matrix& v, double xi, double yi, InterpMethod interpMethod = LinearInterp,
                            ExtrapMethod extrapMethod = NoneExtrap);

/// linear interpolation of the function v = f(x, y) at points xi, yi
/// assumes that x and y are strictly increasing
UTILITIES_API Vector interp(const Vector& x, const Vector& y, const Matrix& v, const Vector& xi, double yi, InterpMethod interpMethod = LinearInterp,
                            ExtrapMethod extrapMethod = NoneExtrap);

/// linear interpolation of the function v = f(x, y) at points xi, yi
/// assumes that x and y are strictly increasing
UTILITIES_API Vector interp(const Vector& x, const Vector& y, const Matrix& v, double xi, const Vector& yi, InterpMethod interpMethod = LinearInterp,
                            ExtrapMethod extrapMethod = NoneExtrap);

/// linear interpolation of the function v = f(x, y) at points xi, yi
/// assumes that x and y are strictly increasing
UTILITIES_API Matrix interp(const Vector& x, const Vector& y, const Matrix& v, const Vector& xi, const Vector& yi,
                            InterpMethod interpMethod = LinearInterp, ExtrapMethod extrapMethod = NoneExtrap);

/// matrix product
UTILITIES_API Matrix prod(const Matrix& lop, const Matrix& rop);

/// vector product
UTILITIES_API Vector prod(const Matrix& m, const Vector& v);

/// outer product
UTILITIES_API Matrix outerProd(const Vector& lhs, const Vector& rhs);

/// take the natural logarithm of Matrix elements, componentwise
UTILITIES_API Matrix log(const Matrix& v);

/// take the logarithm of Matrix elements with respect to base, componentwise
UTILITIES_API Matrix log(const Matrix& v, double base);

/// generates a M x N Matrix whose elements come from the uniform distribution on [a,b].
UTILITIES_API Matrix randMatrix(double a, double b, unsigned M, unsigned N);

/// sum of all elements
UTILITIES_API double sum(const Matrix& matrix);

/// maximum of all elements
UTILITIES_API double maximum(const Matrix& matrix);

/// minimum of all elements
UTILITIES_API double minimum(const Matrix& matrix);

/// mean of all elements
UTILITIES_API double mean(const Matrix& matrix);

/// get the connected components from an NxN adjacency matrix (1.0 for i-j connected, 0.0 for i-j not connected)
UTILITIES_API std::vector<std::vector<unsigned>> findConnectedComponents(const Matrix& matrix);

// from the boost vault:
// The following code inverts the matrix input using LU-decomposition with backsubstitution of unit vectors. Reference: Numerical Recipes in C, 2nd ed., by Press, Teukolsky, Vetterling & Flannery.
/// Matrix inversion routine, using lu_factorize and lu_substitute in uBLAS to invert a matrix */
template <class T>
bool invert(const boost::numeric::ublas::matrix<T>& input, boost::numeric::ublas::matrix<T>& inverse) {

  // create a working copy of the input
  boost::numeric::ublas::matrix<T> A(input);

  // create a permutation matrix for the LU-factorization
  boost::numeric::ublas::permutation_matrix<std::size_t> pm(A.size1());

  // perform LU-factorization
  typename boost::numeric::ublas::matrix<T>::size_type res = boost::numeric::ublas::lu_factorize(A, pm);
  if (res != 0) {
    LOG_FREE(Info, "boost.ublas",
             "boost::numeric::ublas::lu_factorize returned res = " << res << ", A = " << A << ", pm = " << pm << " for input = " << input);
    return false;
  }

  // create identity matrix of "inverse"
  inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));

  // backsubstitute to get the inverse
  try {
    boost::numeric::ublas::lu_substitute(A, pm, inverse);
  } catch (std::exception& e) {
    LOG_FREE(Info, "boost.ublas", "boost::numeric::ublas::lu_substitute threw exception '" << e.what() << "' for A = " << A << ", pm = " << pm);
    return false;
  }

  return true;
}

}  // namespace openstudio

#endif  //UTILITIES_DATA_MATRIX_HPP