/*
 * (C) Copyright 2020 UCAR
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 */
#include <Eigen/Dense>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <vector>

#include "ioda/Engines/Factory.h"
#include "ioda/Exception.h"
#include "ioda/Group.h"

// These tests really need a better check system.
// Boost unit tests would have been excellent here.

template <class E, typename Container>
void test_eigen_regular_attributes(Container g, const E& eigen_data, bool is2D = true) {
  g.atts.addWithEigenRegular("data", eigen_data, is2D);

  // check_data can't be purely of type E, here, since we might be
  // feeding in a map or a block.
  typedef typename E::Scalar ScalarType;
  Eigen::Array<ScalarType, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> check_data;
  g.atts.readWithEigenRegular("data", check_data);

  // Check dimensions, then check values.
  if (is2D) {
    if (eigen_data.rows() != check_data.rows()) throw;
    if (eigen_data.cols() != check_data.cols()) throw;
    for (Eigen::Index i = 0; i < eigen_data.rows(); ++i)
      for (Eigen::Index j = 0; j < eigen_data.cols(); ++j)
        if (eigen_data(i, j) != check_data(i, j)) throw;
  } else {
    // Data are read as a column vector. No real way to get
    // around this, since we are storing only a 1-D object.
    if (eigen_data.size() != check_data.size()) throw;
    if (eigen_data.rows() != check_data.rows()) check_data.transposeInPlace();
    for (Eigen::Index i = 0; i < eigen_data.rows(); ++i)
      for (Eigen::Index j = 0; j < eigen_data.cols(); ++j)
        if (eigen_data(i, j) != check_data(i, j)) throw;
  }
}

template <class E, typename Container>
void test_eigen_tensor_attributes(Container g, const E& eigen_data) {
  g.atts.addWithEigenTensor("data", eigen_data);

  // TODO(rhoneyager): change to make a tensor from whatever
  // the input type is.
  E check_data(eigen_data.dimensions());
  g.atts.readWithEigenTensor("data", check_data);

  const ioda::Dimensions dims = ioda::detail::EigenCompat::getTensorDimensions(eigen_data);

  for (ioda::Dimensions_t i = 0; i < dims.numElements; ++i)
    if (eigen_data.data()[i] != check_data.data()[i]) throw;
}

template <typename T>
bool test_equal(const T& a, const T& b) {
  return a == b;
}

template <>
bool test_equal(const float& a, const float& b) {
  return std::abs(a - b) < 0.00001f;
}

template <>
bool test_equal(const double& a, const double& b) {
  return std::abs(a - b) < 0.00001f;
}

template <>
bool test_equal(const long double& a, const long double& b) {
  return std::abs(a - b) < 0.00001f;
}

template <typename T, typename Container>
void test_attribute_functions(Container g, std::initializer_list<T> values,
                              std::initializer_list<ioda::Dimensions_t> dimensions) {
  // Add attribute using initializer lists
  g.atts.template add<T>("initializer_lists", values, dimensions);

  // Add attribute using gsl spans
  using namespace std;
  vector<T> vals{values};
  g.atts.template add<T>("gsl_spans", vals, dimensions);

  // Open an attribute
  ioda::Attribute a_spans = g.atts["gsl_spans"];
  ioda::Attribute a_ilist = g.atts.open("initializer_lists");

  // Verify dimensionality
  auto adims = a_spans.getDimensions();
  if (adims.dimensionality != gsl::narrow<ioda::Dimensions_t>(dimensions.size())) throw;
  // Verify dimensions
  vector<ioda::Dimensions_t> dims{dimensions};
  size_t numElems = (dims.empty()) ? 0 : 1;
  for (size_t i = 0; i < dims.size(); ++i) {
    if (dims[i] != adims.dimsCur[i]) throw;
    if (dims[i] != adims.dimsMax[i]) throw;
    numElems *= dims[i];
  }
  if (gsl::narrow<ioda::Dimensions_t>(numElems) != adims.numElements) throw;

  // Read the attribute and check values
  vector<T> v_at1;
  vector<T> v_at1_presized(numElems);
  a_ilist.read(v_at1);
  // int32_t c_arr_at1[6];
  // at1.read(gsl::make_span(c_arr_at1, 6));

  a_ilist.read(gsl::make_span(v_at1_presized));

  if (v_at1.size() != numElems) throw;

  for (size_t i = 0; i < numElems; ++i) {
    if (!test_equal(v_at1[i], vals[i])) throw;
    if (!test_equal(v_at1_presized[i], vals[i])) throw;
  }

  // TODO(rhoneyager): Verify data type.
}

template <class E, typename Container>
void test_eigen_regular_variable(Container g, const E& eigen_data) {
  typedef typename E::Scalar ScalarType;
  // g.vars.create("data", )
  ioda::VariableCreationParameters params_1;
  params_1.setFillValue<ScalarType>(0);
  params_1.chunk = true;
  params_1.compressWithGZIP();

  auto v_double =
    g.vars.template create<ScalarType>("var", {eigen_data.rows(), eigen_data.cols()}, {}, params_1);
  v_double.writeWithEigenRegular(eigen_data);

  /*
  std::vector<double> check_v_double;
  v_double.read<double>(check_v_double);
  if (check_v_double.size() != 4) throw;
  if (check_v_double[0] > 9.9 || check_v_double[0] < 9.7) throw;
  if (check_v_double[1] > 9.9 || check_v_double[1] < 9.7) throw;
  if (check_v_double[2] > 2.3 || check_v_double[2] < 2.1) throw;
  if (check_v_double[3] > 1.7 || check_v_double[3] < 1.5) throw;
  */
}

template <class E, typename Container>
void test_eigen_tensor_variable(Container g, const E& eigen_data) {
  typedef typename E::Scalar ScalarType;
  const ioda::Dimensions dims = ioda::detail::EigenCompat::getTensorDimensions(eigen_data);
  auto v = g.vars.template create<ScalarType>("data", dims.dimsCur, dims.dimsMax);
  v.writeWithEigenTensor(eigen_data);

  // TODO(rhoneyager): change to make a tensor from whatever
  // the input type is.
  E check_data(eigen_data.dimensions());
  v.readWithEigenTensor(check_data);

  for (ioda::Dimensions_t i = 0; i < dims.numElements; ++i)
    if (eigen_data.data()[i] != check_data.data()[i]) throw;
}

/// Run a series of tests on the input group.
void test_group_backend_engine(ioda::Group g) {
  // Can we make child groups?
  // Can we nest groups?
  g.create("Test_group_1");
  auto g2 = g.create("Test_group_2");
  g2.create("Child 1");

  // Can we check for group existence?
  if (!g.exists("Test_group_1")) throw;
  // Can we verify that nested groups exist?
  if (!g.exists("Test_group_2/Child 1")) throw;

  // Can we list groups?
  auto g_list = g.list();
  if (g_list.size() != 2) throw;
  // Can we list another way?
  auto g_list2 = g.listObjects();
  if (g_list2.empty()) throw;

  // Can we open groups?
  auto g3 = g2.open("Child 1");
  // Can we open nested groups?
  auto g4 = g.open("Test_group_2/Child 1");

  // Attribute tests
  auto gatt = g.create("Attribute Tests");

  // Let's template these tests and run them in separate groups to prevent name clashes.
  test_attribute_functions<double>(gatt.create("double_single"), {3.14159}, {1});
  test_attribute_functions<double>(gatt.create("double_vector"), {0.1, 0.2, 0.3, 0.4}, {4});
  test_attribute_functions<double>(gatt.create("double_array_2x3"), {1.2, 2.4, 3.6, 4.8, 5.9, 6.3}, {2, 3});
  test_attribute_functions<float>(gatt.create("float_array_2x3"), {1.1f, 2.2f, 3.3f, 4.4f, 5.5f, 6.6f},
                                  {2, 3});
  // test_attribute_functions<int8_t>(gatt.create("int8_t_3x2"), { -1, 1, -2, 2, -3, 3 }, { 3,2 });
  // test_attribute_functions<uint8_t>(gatt.create("uint8_t_3x2"), { 1, 1, 2, 2, 3, 3 }, { 3,2 });
  test_attribute_functions<int16_t>(gatt.create("int16_t_2x2"), {1, -4, 9, -16}, {2, 2});
  test_attribute_functions<uint16_t>(gatt.create("uint16_t_2x2"), {1, 4, 9, 16}, {2, 2});
  test_attribute_functions<int32_t>(gatt.create("int32_t_2x2"), {1, -4, 9, -16}, {2, 2});
  test_attribute_functions<uint32_t>(gatt.create("uint32_t_2x2"), {1, 4, 9, 16}, {2, 2});
  test_attribute_functions<int64_t>(gatt.create("int64_t_2"), {32768, -131072}, {2});
  test_attribute_functions<uint64_t>(gatt.create("uint64_t_2"), {1073741824, 1099511627776}, {2});
  test_attribute_functions<long double>(gatt.create("ld_1"), {1}, {1});
  test_attribute_functions<unsigned long>(gatt.create("ul_1"), {1}, {1});
  test_attribute_functions<size_t>(gatt.create("size_t"), {1}, {1});
  // test_attribute_functions<wchar_t>(gatt.create("wchar_t"), {L'A'}, {1});
  // test_attribute_functions<char16_t>(gatt.create("char16_t"), {u'A'}, {1});
  // test_attribute_functions<char32_t>(gatt.create("char32_t"), {U'A'}, {1});
  // test_attribute_functions<signed char>(gatt.create("schar_t"), {'a'}, {1});
  // test_attribute_functions<unsigned char>(gatt.create("uchar_t"), {'a'}, {1});
  test_attribute_functions<char>(gatt.create("char_t"), {'a'}, {1});

  test_attribute_functions<std::string>(gatt.create("string_t"), {"Hi Steve!", "This is a test."}, {2});
  // BUG: char gets auto-detected as an 8-bit integer type. Need to re-introduce the type-overriding options
  //   when creating, reading and writing attributes.
  // test_attribute_functions<char>(gatt.create("char_t_2"), { 'a', 'b' }, { 2 });
  // BUG: Can't make a span of bools!
  // test_attribute_functions<bool>(gatt.create("bool_t_2"), { true, false }, { 2 });

  Eigen::ArrayXXi int_array_1(3, 3);
  int_array_1 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  test_eigen_regular_attributes(gatt.create("eigen_ints_3x3"), int_array_1);
  test_eigen_regular_attributes(gatt.create("eigen_ints_2x3"), int_array_1.block(1, 0, 2, 3));
  test_eigen_regular_attributes(gatt.create("eigen_ints_1x3_2D"), int_array_1.block(2, 0, 1, 3));
  test_eigen_regular_attributes(gatt.create("eigen_ints_1x3_scalar"), int_array_1.block(2, 0, 1, 3), false);

  Eigen::Tensor<int, 3> int_tensor_1(2, 3, 4);  // 24 elements in a 2x3x4 tensor.
  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 4; ++k) {
        int_tensor_1(i, j, k) = (3 * i) + (2 * j) + k;
      }
    }
  }
  test_eigen_tensor_attributes(gatt.create("eigen_tensor_ints_2x3x4"), int_tensor_1);

  // Now let's create some variables.
  // We can also populate variables with data.

  ioda::VariableCreationParameters params_1;
  params_1.setFillValue<double>(-999);
  params_1.chunk = true;
  params_1.compressWithGZIP();
  auto gvar = g.create("Variable Tests");
  auto v_double = gvar.vars.create<double>("Double_var", {2, 2}, {2, 2}, params_1);
  v_double.write<double>({9.8, 9.8, 2.2, 1.6});

  test_attribute_functions<int16_t>(v_double, {1, -1, 2, 4}, {2, 2});

  std::vector<double> check_v_double;
  v_double.read<double>(check_v_double);
  if (check_v_double.size() != 4) throw;
  if (check_v_double[0] > 9.9 || check_v_double[0] < 9.7) throw;
  if (check_v_double[1] > 9.9 || check_v_double[1] < 9.7) throw;
  if (check_v_double[2] > 2.3 || check_v_double[2] < 2.1) throw;
  if (check_v_double[3] > 1.7 || check_v_double[3] < 1.5) throw;

  // Resizable variable tests
  ioda::VariableCreationParameters params_2;
  params_2.setFillValue<double>(-999);
  params_2.chunk = true;
  params_2.chunks = {30};
  auto v_d2 = gvar.vars.create<double>("d2_var", {30}, {90}, params_2);
  v_d2.write<double>({1,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11, 12, 13, 14, 15,
                      16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30});
  v_d2.resize({60});

  // Eigen tests

  Eigen::MatrixXi mat_int_4x4(4, 4);
  mat_int_4x4 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16;
  test_eigen_regular_variable(gvar.create("eigen_matrix_ints_4x4"), mat_int_4x4);

  test_eigen_tensor_variable(gvar.create("eigen_tensor_ints"), int_tensor_1);

  // Dimension scale tests
  auto dim_1 = gvar.vars.create<int>("dim_1", {1}, {1});
  dim_1.setIsDimensionScale("dim_1");
  auto var_a = gvar.vars.create<int>("var_a_dim_1", {1}, {1});
  var_a.attachDimensionScale(0, dim_1);
  if (!var_a.isDimensionScaleAttached(0, dim_1)) throw;
  var_a.detachDimensionScale(0, dim_1);
  if (var_a.isDimensionScaleAttached(0, dim_1)) throw;
}

int main(int argc, char** argv) {
  using namespace ioda;
  using namespace std;
  try {
    auto f = Engines::constructFromCmdLine(argc, argv, "test-tempates.hdf5");

    test_group_backend_engine(f);

  } catch (const std::exception& e) {
    ioda::unwind_exception_stack(e, cout);
    return 1;
  }
  return 0;
}


// Copyright (C) 2019 Vicente J. Botet Escriba
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
// Based on https://github.com/akrzemi1/explicit/blob/master/test/test_explicit.cpp

// <experimental/numerics/v1/charconv.hpp>

//#define JASEL_CONFIG_CONTRACT_LEVEL_MASK 0x111
#define JASEL_CONFIG_CONTRACT_VIOLATION_THROWS_V 1

#include <iostream>
#include <experimental/numerics/charsconv.hpp>
#include <string>
#include <array>

#include <boost/detail/lightweight_test.hpp>

namespace stdex = std::experimental;

int main()
{
#if __cplusplus > 201402L
    {
        std::array<char, 10> str{"42"};
        int result;
        auto [p, ec] = stdex::from_chars(str.data(), str.data()+2, result);
        //(void)p;
        BOOST_TEST_EQ(p, str.data()+2);
        BOOST_TEST(std::errc{} == ec);
        BOOST_TEST_EQ(42, result);
    }
#endif
    {
        std::array<char, 10> str{"42"};
        int result=0;
        auto p = stdex::from_chars_noerr(str.data(), str.data()+2, result);
        BOOST_TEST_EQ(p, str.data()+2);
        BOOST_TEST_EQ(42, result);
    }
#if __cplusplus > 201402L
    {
        std::array<char, 10> str{""};
        int result;
        auto [p, ec] = stdex::from_chars(str.data(), str.data()+0, result);
        BOOST_TEST_EQ(p, str.data());
        BOOST_TEST(std::errc::invalid_argument == ec);
    }
    {
        BOOST_TEST(! stdex::can_narrow_to<signed char>(300));
        std::array<char, 10> str{"300"};
        signed char result;
        auto [p, ec] = stdex::from_chars(str.data(), str.data()+10, result);
        BOOST_TEST_EQ(p, str.data());
        BOOST_TEST(std::errc::result_out_of_range == ec);
    }
#endif
    {
        std::array<char, 10> str{"300"};
        char* tmp;
        auto via = stdex::strto_via<unsigned int>(str.data(), &tmp, 10);
        BOOST_TEST_EQ(tmp , str.data()+3);
        BOOST_TEST_EQ(300 , via);
    }
    {
        std::array<char, 10> str{"300"};
        char* tmp;
        auto via = stdex::strto_via<unsigned short>(str.data(), &tmp, 10);
        BOOST_TEST_EQ(tmp , str.data()+3);
        BOOST_TEST_EQ(300 , via);
    }
    {
        std::array<char, 10> str{"200"};
        char* tmp;
        auto via = stdex::strto_via<unsigned char>(str.data(), &tmp, 10);
        BOOST_TEST_EQ(tmp , str.data()+3);
        BOOST_TEST_EQ(200 , via);
    }
    {
        std::array<char, 10> str{"300"};
        char* tmp;
        auto via = stdex::strto_via<unsigned char>(str.data(), &tmp, 10);
        BOOST_TEST_EQ(tmp , str.data());
        BOOST_TEST_EQ(0 , via);
    }
    {
        std::array<char, 10> str{"300"};
        char* tmp;
        auto via = stdex::strto_via<int>(str.data(), &tmp, 10);
        BOOST_TEST_EQ(tmp , str.data()+3);
        BOOST_TEST_EQ(300 , via);
    }
    {
        std::array<char, 10> str{"300"};
        char* tmp;
        auto via = stdex::strto<unsigned long>(str.data(), &tmp, 10);
        BOOST_TEST_EQ(tmp , str.data()+3);
        BOOST_TEST_EQ(300 , via);
        BOOST_TEST(! stdex::can_narrow_to<signed char>(via));
    }
    {
        std::array<char, 10> str{"300"};
        char* tmp;
        auto via = stdex::strto<long>(str.data(), &tmp, 10);
        BOOST_TEST_EQ(tmp , str.data()+3);
        BOOST_TEST_EQ(300 , via);
        BOOST_TEST(! stdex::can_narrow_to<signed char>(via));
    }
    {
        std::array<char, 10> str{"-30"};
        char* tmp;
        auto via = stdex::strto<long>(str.data(), &tmp, 10);
        BOOST_TEST_EQ(tmp , str.data()+3);
        BOOST_TEST_EQ(-30 , via);
        BOOST_TEST(stdex::can_narrow_to<signed char>(via));
    }
    {
        std::array<char, 10> str{"300"};
        char* tmp;
        auto via = stdex::strto<unsigned long long>(str.data(), &tmp, 10);
        BOOST_TEST_EQ(tmp , str.data()+3);
        BOOST_TEST_EQ(300 , via);
    }
    {
        std::array<char, 10> str{"-30"};
        char* tmp;
        auto via = stdex::strto<long long>(str.data(), &tmp, 10);
        BOOST_TEST_EQ(tmp , str.data()+3);
        BOOST_TEST_EQ(-30 , via);
    }
#if __cplusplus > 201402L
    {
        BOOST_TEST(! stdex::can_narrow_to<unsigned char>(300));
        std::array<char, 10> str{"300"};
        unsigned char result;
        auto [p, ec] = stdex::from_chars(str.data(), str.data()+10, result);
        BOOST_TEST_EQ(p, str.data());
        BOOST_TEST(std::errc::result_out_of_range == ec);
    }
    {
        errno = 0;
        std::array<char, 10> str{"AB"};
        int result;
        auto [p, ec] = stdex::from_chars(str.data(), str.data()+10, result);
        BOOST_TEST_EQ(p, str.data());
        BOOST_TEST(std::errc::invalid_argument == ec);
    }
#endif
    {
        std::array<char, 100> arr;
        (void) stdex::to_chars(arr.data(), arr.data()+100, 10);
        BOOST_TEST_EQ(std::strcmp("10", arr.data()), 0);
    }
#if __cplusplus > 201402L
    {
        std::array<char, 10> str;
        int value = 44;
        auto [p, ec] = stdex::to_chars(str.data(), str.data()+2, value);
        BOOST_TEST(std::errc{} == ec);
        BOOST_TEST(p == str.data()+2);
        BOOST_TEST_EQ(std::strcmp("44", str.data()), 0);
    }
    {
        std::array<char, 10> str;
        int value = -43;
        auto [p, ec] = stdex::to_chars(str.data(), str.data()+9, value);
        BOOST_TEST(std::errc{} == ec);
        BOOST_TEST_EQ(p, str.data()+3);
        BOOST_TEST_EQ(std::strcmp("-43", str.data()), 0);
    }
#endif
    {
        std::array<char, 10> str;
        int value = 45;
        auto p = stdex::to_chars_noerr(str.data(), str.data()+9, value);
        BOOST_TEST(p == str.data()+2);
        BOOST_TEST_EQ(std::strcmp("45", str.data()), 0);
    }
    return boost::report_errors();
}


#include <boost/random/random_device.hpp>

int main()
{
	boost::random::random_device gen;
}


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

#include "model/game/gameparameters.h"
#include <iostream>
#include <string>

BOOST_AUTO_TEST_SUITE( gameproperties_suite )

BOOST_AUTO_TEST_CASE(nodeCount)
{
    GameParameters p;
    BOOST_CHECK(p.setGridSideSize(10));
    BOOST_CHECK(p.setRatio_NodeCountVsGridSpotCount(0.5));
    BOOST_CHECK_EQUAL(p.nodeCount(),50);
    BOOST_CHECK(p.setRatio_NodeCountVsGridSpotCount(1));
    BOOST_CHECK_EQUAL(p.nodeCount(),100);

    BOOST_CHECK(p.setGridSideSize(5));
    BOOST_CHECK(p.setRatio_NodeCountVsGridSpotCount(0.5));
    BOOST_CHECK_EQUAL(p.nodeCount(),13);

    BOOST_CHECK(p.setGridSideSize(150) == false);
    BOOST_CHECK(p.setRatio_NodeCountVsGridSpotCount(2) == false);
    BOOST_CHECK(p.setRatio_NodeCountVsGridSpotCount(0) == false);
    BOOST_CHECK_EQUAL(p.nodeCount(),13);
}
BOOST_AUTO_TEST_CASE(edgeTargetCount)
{
    GameParameters p;
    BOOST_CHECK(p.setGridSideSize(10));
    BOOST_CHECK(p.setRatio_EdgeCountVsGridSpotCount(2));
    BOOST_CHECK_EQUAL(p.edgeTargetCount(),200);
    BOOST_CHECK(p.setRatio_EdgeCountVsGridSpotCount(0.5));
    BOOST_CHECK_EQUAL(p.edgeTargetCount(),50);

    BOOST_CHECK(p.setGridSideSize(5));
    BOOST_CHECK(p.setRatio_EdgeCountVsGridSpotCount(0.5));
    BOOST_CHECK_EQUAL(p.edgeTargetCount(),13);

    BOOST_CHECK(p.setRatio_EdgeCountVsGridSpotCount(0) == false);
    BOOST_CHECK_EQUAL(p.edgeTargetCount(),13);
}
BOOST_AUTO_TEST_CASE(itemCount)
{
    GameParameters p;
    p.setGridSideSize(10);
    p.setRatio_NodeCountVsGridSpotCount(2);
    BOOST_CHECK(p.setRatio_ItemCountVsNodeCount(2));
    BOOST_CHECK_EQUAL(p.itemCount(),100);

    BOOST_CHECK(p.setRatio_ItemCountVsNodeCount(0.5));
    BOOST_CHECK_EQUAL(p.itemCount(),25);

    BOOST_CHECK(p.setRatio_ItemCountVsNodeCount(0) == false);
    BOOST_CHECK_EQUAL(p.itemCount(),25);
}

BOOST_AUTO_TEST_SUITE_END()


#include "opq.h"

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

#include "externals/cxx/namespace.hpp"
#include "externals/cxx/pretty_function.hpp"

#include <stdexcept>

BEGIN_NAMESPACE(rysq, asymptotic)

template<size_t N, typename T>
struct asymptotic_ {
    static void roots(T X, T *R, T *W) {
	boost::call_once(once_flag, &asymptotic_<N,T>::initialize);
	T r = 1/X;
	T w = 1/sqrt(X);
	for (size_t i = 0; i < N; ++i) {
	    R[i] = r*R_[i];
	    W[i] = w*W_[i];
	}
    }
private:
    static T R_[N], W_[N];
    static boost::once_flag once_flag;
    static void initialize() {
	static const size_t n = 2*N;
	T beta[n], alpha[n] = { 0 };
	beta[0] = boost::math::constants::root_pi<T>();
	for (size_t i = 1; i < n; ++i) {
	    beta[i] = T(i)/2;
	}
	T r[n], w[n];
	int status = opq::coefficients(n, alpha, beta, r, w);
	if (status != 0)  {
	    throw std::runtime_error(PRETTY_FUNCTION("opq::coefficients returned ",
						     status));
	}
	// CALL RYSGW_(N,ALPHA,BETA,EPS,RTS,W_TS,IERR,W_RK) for
	for (size_t i = 0; i < N; ++i) {
	    size_t j = i + N;
	    R_[i] = r[j]*r[j];
	    W_[i] = w[j];
	}
    }
};
template<size_t N, typename T> T asymptotic_<N,T>::R_[N];
template<size_t N, typename T> T asymptotic_<N,T>::W_[N];

template<size_t N, typename T>
boost::once_flag asymptotic_<N,T>::once_flag = BOOST_ONCE_INIT;

template<size_t N, typename T>
void roots(T X, T *R, T *W) {
    asymptotic_<N,T>::roots(X, R, W);
}

END_NAMESPACE(rysq, asymptotic)


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// Generalized Pose-and-Scale Estimation using 4-Point Congruence Constraints
//
// Victor Fragoso and Sudipta Sinha.
// In Proc. of the IEEE International Conf. on 3D Vision (3DV) 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include <stdlib.h>
#include <cmath>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <gp4pc/camera_feature_correspondence_2d_3d.h>
#include <gp4pc/estimate_similarity_transformation.h>
#include <gp4pc/pinhole_camera.h>
#include <math/utils.h>
#include <utility>
#include <vector>

namespace msft {
namespace {

// Random seed.
constexpr size_t kSeed = 67;
// Focal length.
static const double kFocalLength = 1000.0;
// Number of cameras.
static const int kNumCameras = 4;
// Number of points.
static const int kNumPoints = 100;
// Min. angle in degrees.
static const double kMinAngleDeg = 5.0;
// Max. angle in degrees.
static const double kMaxAngleDeg = 40.0;
// Max. num of trials.
static const int kNumTrials = 8;
// Expected success rate.
static const double kSuccessRate = 0.8;
// Minimum and maximum limits for generating 3D points.
static const double kMinXY = -5.0;
static const double kMaxXY = 5.0;
static const double kMinZ = 10;
static const double kMaxZ = 20;

// Similarity transformation.
struct SimilarityTransformation {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  Eigen::Quaterniond rotation = Eigen::Quaterniond::Identity();
  Eigen::Vector3d translation = Eigen::Vector3d::Zero();
  double scale = 1.0;
};

struct TestParameters {
  // Num. of points for the test.
  int num_points = 4;
  // Noise level to points in 3d.
  double noise_std_dev = 0.0;
  // Inlier ratio. 1.0: All inliers, 0.0: All outliers.
  double inlier_ratio = 1.0;
  // Rotation error threshold in radians.
  double rotation_thresh = DegToRad(1.0);
  // Translation error threshold.
  double translation_thresh = 0.5;
  // Scale error threshold.
  double scale_thresh = 0.1;
  // Rotation generation parameters.
  double min_angle_deg = 1.0;
  double max_angle_deg = 45.0;
  // Translation generation parameters.
  double min_translation_value = 0.0;
  double max_translation_value = 10.0;
  // Scale generation parameters.
  double min_scale_value = 0.5;
  double max_scale_value = 5.0;
  // Ransac parameters.
  Gp4pcRobustEstimator::RansacParameters ransac_params;
};

class EstimateSimilarityTransformationTest : public ::testing::Test {
 public:
  static void SetUpTestCase() {
    rng = new std::mt19937(kSeed);
    srand(kSeed);
  }

  static void TearDownTestCase() {
    delete rng;
  }

  double GenerateRandomScale(const double min_value,
                             const double max_value) {
    CHECK_LT(min_value, max_value);
    std::mt19937& prng = *rng;
    std::uniform_real_distribution<double> dist(min_value, max_value);
    return dist(prng);
  }

  Eigen::Vector3d GenerateRandomVector3d(const double min_value,
                                         const double max_value) {
    CHECK_LT(min_value, max_value);
    std::mt19937& prng = *rng;
    std::uniform_real_distribution<double> dist(min_value, max_value);
    const Eigen::Vector3d point(dist(prng),
                                dist(prng),
                                dist(prng));
    return point;
  }

  Eigen::Quaterniond GenerateRandomRotation(const double min_angle_deg,
                                            const double max_angle_deg) {
    CHECK_LT(min_angle_deg, max_angle_deg);
    std::uniform_real_distribution<double> angle_dist(min_angle_deg,
                                                      max_angle_deg);
    std::mt19937& prng = *rng;
    const Eigen::Quaterniond rotation(
        Eigen::AngleAxisd(DegToRad(angle_dist(prng)),
                          Eigen::Vector3d::Random().normalized()));
    return rotation;
  }

  PinholeCamera GenerateRandomCamera() {
    const Eigen::Vector2d principal_point(kFocalLength / 2.0,
                                          kFocalLength / 2.0);
    const Eigen::Quaterniond rotation = GenerateRandomRotation(kMinAngleDeg,
                                                               kMaxAngleDeg);
    const Eigen::Vector3d translation = GenerateRandomVector3d(-10, 10);
    PinholeCamera camera(kFocalLength, principal_point, rotation, translation);
    return camera;
  }

  Eigen::Vector3d GenerateRandomPoint3d(const double min_xy,
                                        const double max_xy,
                                        const double min_z,
                                        const double max_z) {
    std::mt19937& prng = *rng;
    std::uniform_real_distribution<double> xy_dist(min_xy, max_xy);
    std::uniform_real_distribution<double> z_dist(min_z, max_z);
    Eigen::Vector3d point(xy_dist(prng), xy_dist(prng), z_dist(prng));
    return point;
  }

  Eigen::Vector3d AddNoiseToPoint3d(const double noise_std_dev,
                                    Eigen::Vector3d* point) {
    std::mt19937& prng = *rng;
    std::normal_distribution<double> noise(0.0, noise_std_dev);
    *CHECK_NOTNULL(point) += Eigen::Vector3d(noise(prng),
                                             noise(prng),
                                             noise(prng));
    return *point;
  }

  SimilarityTransformation GenerateRandomSimilarityTransformation(
      const TestParameters& test_params) {
    SimilarityTransformation transformation;
    transformation.rotation = GenerateRandomRotation(test_params.min_angle_deg,
                                                     test_params.max_angle_deg);
    transformation.translation =
        GenerateRandomVector3d(test_params.min_translation_value,
                               test_params.max_translation_value);
    transformation.scale = GenerateRandomScale(test_params.min_scale_value,
                                               test_params.max_scale_value);
    return transformation;
  }

  std::vector<CameraFeatureCorrespondence2D3D> GenerateCorrespondences(
      const TestParameters& test_params,
      const SimilarityTransformation& expected_transformation) {
    CHECK_GT(test_params.inlier_ratio, 0.0);
    CHECK_LE(test_params.inlier_ratio, 1.0);
    std::vector<CameraFeatureCorrespondence2D3D> correspondences(
        test_params.num_points);

    // Generate cameras.
    std::vector<PinholeCamera> cameras(kNumCameras);
    for (int i = 0; i < kNumCameras; ++i) {
      cameras[i] = GenerateRandomCamera();
    }

    // Generate synthetic correspondences.
    for (int i = 0; i < test_params.num_points; ++i) {
      CameraFeatureCorrespondence2D3D& correspondence = correspondences[i];
      correspondence.camera = cameras[i % cameras.size()];

      // Set up random 3D point and reproject it onto the image. Make sure the
      // point is in front of the camera.
      double depth = -1;
      do {
        // Create a random 3D point that is in front of the camera.
        correspondence.point =
            GenerateRandomPoint3d(kMinXY, kMaxXY, kMinZ, kMaxZ);
        // Project point and see if it is infront of camera.
        depth = correspondence.camera.ProjectPoint(correspondence.point,
                                                   &correspondence.observation);
      } while(depth < 0);

      // Add zero-mean Gaussian noise.
      if (test_params.noise_std_dev > 0.0) {
        AddNoiseToPoint3d(test_params.noise_std_dev, &correspondence.point);
      }
    }

    // Add outliers and apply inv. transformation.
    for (int i = 0; i < correspondences.size(); ++i) {
      // Add outlier.
      if (i > test_params.inlier_ratio * correspondences.size()) {
        correspondences[i].observation =
            kFocalLength * Eigen::Vector2d::Random();
      }

      const Eigen::Vector3d old_point = correspondences[i].point;
      // Since the 3d point is in front of the camera, we need to calculate the
      // final 3d point to estimate the rigid transformation.
      const Eigen::Vector3d new_point =
          expected_transformation.rotation.conjugate() *
          (old_point - expected_transformation.translation) /
          expected_transformation.scale;
      correspondences[i].point = new_point;
    }

    return correspondences;
  }

  bool EvaluateSolution(const TestParameters& test_params,
                        const SimilarityTransformation& expected_transformation,
                        const Gp4pc::Solution& estimated_transformations) {
    // Check that we get a single solution.
    EXPECT_GE(estimated_transformations.rotations.size(), 1);
    EXPECT_GE(estimated_transformations.rotations.size(),
              estimated_transformations.translations.size());
    EXPECT_GE(estimated_transformations.rotations.size(),
              estimated_transformations.scales.size());

    // Estimated solution.
    const int num_solutions = estimated_transformations.rotations.size();
    const Eigen::Quaterniond& expected_rotation =
        expected_transformation.rotation;
    const Eigen::Vector3d& expected_translation =
        expected_transformation.translation;
    const double& expected_scale = expected_transformation.scale;
    int num_trials_passed = 0;
    // Evaluation of solutions.
    for (int i = 0; i < num_solutions; ++i) {
      const Eigen::Quaterniond& estimated_rotation =
          estimated_transformations.rotations[i];
      const Eigen::Vector3d& estimated_translation =
          estimated_transformations.translations[i];
      const double& estimated_scale = estimated_transformations.scales[i];
      // Compute errors.
      const double rotation_error =
          expected_rotation.angularDistance(estimated_rotation);
      const double translation_error =
          (expected_translation - estimated_translation).norm();
      const double scale_error = std::abs(expected_scale - estimated_scale);

      const bool good_rotation = rotation_error < test_params.rotation_thresh;
      const bool good_translation =
           translation_error < test_params.translation_thresh;
      const bool good_scale = scale_error < test_params.scale_thresh;
      const bool good_run = good_rotation && good_translation && good_scale;

      VLOG_IF(2, !good_run) << "Solution: " << (i + 1) << "/" << num_solutions
                            << "\nRotation error: " << rotation_error
                            << " thresh: " << test_params.rotation_thresh
                            << " good: " << good_rotation
                            << "\nTranslation error: " << translation_error
                            << " thresh: " << test_params.translation_thresh
                            << " good: " << good_translation
                            << "\nScale error: " << scale_error
                            << " thresh: " << test_params.scale_thresh
                            << " good: " << good_scale;

      if (good_run) {
        ++num_trials_passed;
      }
    }

    return num_trials_passed > 0;
  }

  bool ExecuteRandomTest(
      const TestParameters& test_params,
      const SimilarityTransformation& expected_transformation) {
    // Compute 2D-3D correspondences.
    const std::vector<CameraFeatureCorrespondence2D3D> correspondences =
        GenerateCorrespondences(test_params, expected_transformation);

    // Estimate similarity transformation.
    const Gp4pc::Solution solution =
        EstimateSimilarityTransformation(correspondences);

    // Evaluate solution.
    return EvaluateSolution(test_params, expected_transformation, solution);
  }

  bool ExecuteRansacRandomTest(
      const TestParameters& test_params,
      const SimilarityTransformation& expected_transformation) {
    // Compute 2D-3D correspondences.
    const std::vector<CameraFeatureCorrespondence2D3D> correspondences =
        GenerateCorrespondences(test_params, expected_transformation);

    // Estimate similarity transformation.
    Gp4pcRobustEstimator::RansacSummary ransac_summary;
    const Gp4pc::Solution solution =
        EstimateSimilarityTransformation(test_params.ransac_params,
                                         correspondences,
                                         &ransac_summary);

    // Evaluate solution.
    return EvaluateSolution(test_params, expected_transformation, solution);
  }

  // Random number generator.
  static std::mt19937* rng;
};

std::mt19937* EstimateSimilarityTransformationTest::rng = nullptr;

// Minimal estimation tests.
TEST_F(EstimateSimilarityTransformationTest,
       MinimalBasicEstimationNoNoise) {
  TestParameters test_params;
  int num_successful_runs = 0;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    if (ExecuteRandomTest(test_params, expected_transformation)) {
      ++num_successful_runs;
    }
  }
  const double success_rate =
      static_cast<double>(num_successful_runs) / kNumTrials;
  EXPECT_TRUE(success_rate > kSuccessRate) << "Success rate: " << success_rate;
}

TEST_F(EstimateSimilarityTransformationTest,
       MinimalBasicEstimationWithNoise) {
  TestParameters test_params;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  int num_successful_runs = 0;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    if (ExecuteRandomTest(test_params, expected_transformation)) {
      ++num_successful_runs;
    }
  }
  const double success_rate =
      static_cast<double>(num_successful_runs) / kNumTrials;
  EXPECT_TRUE(success_rate > kSuccessRate) << "Success rate: " << success_rate;
}

// Test RANSAC estimators all inliers.
TEST_F(EstimateSimilarityTransformationTest, AllInliersWithNoise) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  int num_successful_runs = 0;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    if (ExecuteRansacRandomTest(test_params, expected_transformation)) {
      ++num_successful_runs;
    }
  }
  const double success_rate =
      static_cast<double>(num_successful_runs) / kNumTrials;
  EXPECT_TRUE(success_rate > kSuccessRate) << "Success rate: " << success_rate;
}

// RANSAC w/ inliers and outliers and noise.
TEST_F(EstimateSimilarityTransformationTest,
       InliersAndOutliersWithNoise) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  test_params.inlier_ratio = 0.8;
  int num_successful_runs = 0;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    if (ExecuteRansacRandomTest(test_params, expected_transformation)) {
      ++num_successful_runs;
    }
  }
  const double success_rate =
      static_cast<double>(num_successful_runs) / kNumTrials;
  EXPECT_TRUE(success_rate > kSuccessRate) << "Success rate: " << success_rate;
}

}  // namespace
}  // namespace msft


// File: rich_vector_expr.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    typedef std::complex<double>  cdouble;
    dense_vector<cdouble>         u(10), v(10);
    dense_vector<double>          w(10), x(10, 4.0);

    for (unsigned i= 0; i < size(v); i++)
	v[i]= cdouble(i+1, 10-i), w[i]= 2 * i + 2;

    // Increment w by x
    // and assign the sum of--the incremented--w and v to u
    u= v + (w+= x);
    std::cout << "u is " << u << "\n";

    // w= w * 3; x= 2; v= v + w + x; u= u + v;
    u+= v+= (w*= 3) + (x= 2);
    std::cout << "u is " << u << "w is " << w << "\n";

    return 0;
}


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include "Molassembler/Shapes/PointGroupElements.h"

#include "Molassembler/Temple/Functional.h"
#include "boost/optional.hpp"
#include <Eigen/Geometry>

namespace Scine {
namespace Molassembler {
namespace Shapes {
namespace Elements {
namespace {

template<typename EnumType>
constexpr auto underlying(const EnumType e) {
  return static_cast<std::underlying_type_t<EnumType>>(e);
}

inline bool collinear(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
  return std::fabs(std::fabs(a.dot(b) / (a.norm() * b.norm())) - 1) <= 1e-8;
}

inline bool orthogonal(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
  return std::fabs(a.dot(b) / (a.norm() * b.norm())) <= 1e-8;
}

template<typename T>
inline std::unique_ptr<SymmetryElement> wrap(T&& element) {
  using Decayed = std::decay_t<T>;
  return std::make_unique<Decayed>(std::forward<T>(element));
}

void addProperAxisElements(ElementsList& list, const Eigen::Vector3d& axis, const unsigned n) {
  // C2 gives only a C2, but C3 should also give a C3², etc.
  const Rotation element = Rotation::Cn(axis, n);
  Rotation composite = element;
  for(unsigned i = n; i > 1; --i) {
    list.push_back(wrap(composite));
    composite = element * composite;
  }
}

void addImproperAxisElements(ElementsList& list, const Eigen::Vector3d& axis, const unsigned n) {
  const Rotation element = Rotation::Sn(axis, n);
  Rotation composite = element;
  for(unsigned i = n; i > 1; --i) {
    list.push_back(wrap(composite));
    composite = element * composite;
  }
}

ElementsList Cnh(const unsigned n) {
  const auto sigma_xy = Reflection::sigma_xy();

  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  elements.push_back(wrap(sigma_xy));
  std::vector<Rotation> rotations;
  const Rotation element = Rotation::Cn_z(n);
  Rotation composite = element;
  for(unsigned i = n; i > 1; --i) {
    rotations.push_back(composite);
    composite = element * composite;
  }
  const unsigned S = rotations.size();
  // Add sigma_xy modified Cn axes
  for(unsigned i = 0; i < S; ++i) {
    rotations.push_back(sigma_xy * rotations.at(i));
  }
  for(auto& rotation : rotations) {
    elements.emplace_back(
      wrap(std::move(rotation))
    );
  }
  assert(elements.size() == 2 * n);
  return elements;
}

ElementsList Cnv(const unsigned n) {
  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  addProperAxisElements(elements, Eigen::Vector3d::UnitZ(), n);
  // Reflection planes include z and increment by pi/n along z
  const auto rotation = Rotation::Cn_z(2 * n);
  Eigen::Vector3d planeNormal = Eigen::Vector3d::UnitY();
  for(unsigned i = 0; i < n; ++i) {
    elements.push_back(
      wrap(Reflection(planeNormal))
    );
    planeNormal = rotation.matrix() * planeNormal;
  }
  assert(elements.size() == 2 * n);
  return elements;
}

ElementsList Dn(const unsigned n) {
  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  addProperAxisElements(elements, Eigen::Vector3d::UnitZ(), n);
  // Dn groups have C2 axes along pi/n increments in the xy plane
  const auto rotation = Rotation::Cn_z(2 * n);
  Eigen::Vector3d c2axis = Eigen::Vector3d::UnitX();
  for(unsigned i = 0; i < n; ++i) {
    elements.push_back(wrap(Rotation::Cn(c2axis, 2)));
    c2axis = rotation.matrix() * c2axis;
  }
  assert(elements.size() == 2 * n);
  return elements;
}

ElementsList Dnh(const unsigned n) {
  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  elements.push_back(wrap(Reflection::sigma_xy()));
  elements.reserve(4 * n);
  std::vector<Rotation> rotations;
  const Rotation element = Rotation::Cn_z(n);
  Rotation composite = element;
  for(unsigned i = n; i > 1; --i) {
    rotations.push_back(composite);
    composite = element * composite;
  }
  const unsigned S = rotations.size();
  // Generate the S_n axes from the sigma_xy * C_n
  for(unsigned i = 0; i < S; ++i) {
    rotations.push_back(Reflection::sigma_xy() * rotations.at(i));
  }
  for(auto& rotation : rotations) {
    elements.emplace_back(
      wrap(std::move(rotation))
    );
  }

  /* Dnh groups have C2 axes along pi/n increments in the xy plane
   * and sigma_v planes perpendicular to those C2 axes
   */
  const auto rotation = Rotation::Cn_z(2 * n);
  Eigen::Vector3d c2axis = Eigen::Vector3d::UnitX();
  for(unsigned i = 0; i < n; ++i) {
    elements.push_back(wrap(Rotation::Cn(c2axis, 2)));
    elements.push_back(
      wrap(Reflection(
        Eigen::Vector3d::UnitZ().cross(c2axis)
      ))
    );
    c2axis = rotation.matrix() * c2axis;
  }

  assert(elements.size() == 4 * n);
  return elements;
}

ElementsList Dnd(const unsigned n) {
  const Eigen::Vector3d e_x = Eigen::Vector3d::UnitX();
  const Eigen::Vector3d e_z = Eigen::Vector3d::UnitZ();

  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  addImproperAxisElements(elements, e_z, 2 * n);
  /* C2 axes */
  const auto rotationMatrix = Rotation::Cn_z(2 * n).matrix();
  Eigen::Vector3d c2axis = e_x;
  for(unsigned i = 0; i < n; ++i) {
    elements.push_back(wrap(Rotation::Cn(c2axis, 2)));
    c2axis = rotationMatrix * c2axis;
  }
  /* sigma_ds */
  Eigen::Vector3d planeNormal = (e_x + rotationMatrix * e_x).normalized().cross(e_z);
  for(unsigned i = 0; i < n; ++i) {
    elements.push_back(wrap(Reflection(planeNormal)));
    planeNormal = rotationMatrix * planeNormal;
  }
  assert(elements.size() == 4 * n);
  return elements;
}

ElementsList I() {
  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  elements.reserve(60);

  const double phi = (1 + std::sqrt(5)) / 2;
  Eigen::Matrix<double, 3, 6> axes;
  axes << 0.0, 0.0, phi, -phi, 1.0, 1.0,
          1.0, 1.0, 0.0, 0.0, phi, -phi,
          phi, -phi, 1.0, 1.0, 0.0, 0.0;

  /* 12 C5, 12 C5^2 */
  for(unsigned i = 0; i < 6; ++i) {
    addProperAxisElements(elements, axes.col(i), 5);
  }

  /* 15 C2 along sums of two positions*/
  const Eigen::Matrix3d C5 = Eigen::AngleAxisd(2 * M_PI / 5, axes.col(0).normalized()).toRotationMatrix();
  Eigen::Matrix3d twoAxesBases;
  twoAxesBases.col(0) = (axes.col(0) + axes.col(2)) / 2;
  twoAxesBases.col(1) = (axes.col(2) + axes.col(4)) / 2;
  twoAxesBases.col(2) = (axes.col(2) - axes.col(3)) / 2;
  for(unsigned i = 0; i < 3; ++i) {
    Eigen::Vector3d compoundAxis = twoAxesBases.col(i);
    for(unsigned j = 0; j < 5; ++j) {
      elements.push_back(wrap(Rotation::Cn(compoundAxis, 2)));
      compoundAxis = C5 * compoundAxis;
    }
  }

  /* 20 C3 along sums of three positions*/
  Eigen::Matrix<double, 3, 2> threeAxesBases;
  threeAxesBases.col(0) = (axes.col(0) + axes.col(2) + axes.col(4)) / 3;
  threeAxesBases.col(1) = (axes.col(2) + axes.col(4) - axes.col(3)) / 3;
  for(unsigned i = 0; i < 2; ++i) {
    Eigen::Vector3d compoundAxis = threeAxesBases.col(i);
    for(unsigned j = 0; j < 5; ++j) {
      elements.push_back(wrap(Rotation::Cn(compoundAxis, 3)));
      elements.push_back(wrap(Rotation::Cn(-compoundAxis, 3)));
      compoundAxis = C5 * compoundAxis;
    }
  }

  assert(elements.size() == 60);
  return elements;
}

ElementsList Ih() {
  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  elements.reserve(120);
  /* i */
  elements.push_back(wrap(Inversion::i()));
  const double phi = (1 + std::sqrt(5)) / 2;
  Eigen::Matrix<double, 3, 6> axes;
  axes << 0.0, 0.0, phi, -phi, 1.0, 1.0,
          1.0, 1.0, 0.0, 0.0, phi, -phi,
          phi, -phi, 1.0, 1.0, 0.0, 0.0;

  /* 12 S10, 12 S10^3, 12 C5, 12 C5^2 */
  for(unsigned i = 0; i < 6; ++i) {
    const auto& axis = axes.col(i);
    elements.push_back(wrap(Rotation::Sn(axis, 10)));
    elements.push_back(wrap(Rotation::Sn(-axis, 10)));
    elements.push_back(wrap(Rotation::Sn(axis, 10, 3)));
    elements.push_back(wrap(Rotation::Sn(-axis, 10, 3)));
    // C5, C5^2, C5^3 = -C5^2, C5^4 = -C5
    addProperAxisElements(elements, axis, 5);
  }

  /* 15 C2, 15 sigma along sums of two positions */
  const Eigen::Matrix3d C5 = Eigen::AngleAxisd(2 * M_PI / 5, axes.col(0).normalized()).toRotationMatrix();
  Eigen::Matrix3d twoAxesBases;
  twoAxesBases.col(0) = (axes.col(0) + axes.col(2)) / 2;
  twoAxesBases.col(1) = (axes.col(2) + axes.col(4)) / 2;
  twoAxesBases.col(2) = (axes.col(2) - axes.col(3)) / 2;
  for(unsigned i = 0; i < 3; ++i) {
    Eigen::Vector3d compoundAxis = twoAxesBases.col(i);
    for(unsigned j = 0; j < 5; ++j) {
      elements.push_back(wrap(Rotation::Cn(compoundAxis, 2)));
      elements.push_back(wrap(Reflection(compoundAxis)));
      compoundAxis = C5 * compoundAxis;
    }
  }

  /* 20 S6, 20 C3 along sums of three positions */
  Eigen::Matrix<double, 3, 2> threeAxesBases;
  threeAxesBases.col(0) = (axes.col(0) + axes.col(2) + axes.col(4)) / 3;
  threeAxesBases.col(1) = (axes.col(2) + axes.col(4) - axes.col(3)) / 3;
  for(unsigned i = 0; i < 2; ++i) {
    Eigen::Vector3d compoundAxis = threeAxesBases.col(i);
    for(unsigned j = 0; j < 5; ++j) {
      elements.push_back(wrap(Rotation::Sn(compoundAxis, 6)));
      elements.push_back(wrap(Rotation::Sn(-compoundAxis, 6)));
      elements.push_back(wrap(Rotation::Cn(compoundAxis, 3)));
      elements.push_back(wrap(Rotation::Cn(-compoundAxis, 3)));
      compoundAxis = C5 * compoundAxis;
    }
  }
  assert(elements.size() == 120);
  return elements;
}

} // namespace

Identity Identity::E() {
  return {};
}

SymmetryElement::Matrix Identity::matrix() const {
  return Matrix::Identity();
}

boost::optional<SymmetryElement::Vector> Identity::vector() const {
  return boost::none;
}

std::string Identity::name() const {
  return "E";
}

Inversion Inversion::i() {
  return {};
}

SymmetryElement::Matrix Inversion::matrix() const {
  return -Matrix::Identity();
}

boost::optional<SymmetryElement::Vector> Inversion::vector() const {
  return boost::none;
}

std::string Inversion::name() const {
  return "i";
}

Rotation::Rotation(
  const Eigen::Vector3d& passAxis,
  const unsigned passN,
  const unsigned passPower,
  const bool passReflect
) : axis(passAxis.normalized()),
    n(passN),
    power(passPower),
    reflect(passReflect)
{}

Rotation Rotation::Cn(const Eigen::Vector3d& axis, const unsigned n, const unsigned power) {
  return Rotation(axis, n, power, false);
}

Rotation Rotation::Sn(const Eigen::Vector3d& axis, const unsigned n, const unsigned power) {
  return Rotation(axis, n, power, true);
}

Rotation Rotation::operator * (const Rotation& rhs) const {
  if(collinear(axis, rhs.axis)) {
    if(n == rhs.n) {
      return Rotation(axis, n, power + rhs.power, reflect xor rhs.reflect);
    }

    throw std::logic_error("Rotation data model cannot handle collinear multiplication of axes of different order n");
  }

  if(orthogonal(axis, rhs.axis)) {
    // Rotate rhs' axis by *this, but keep everything else
    return Rotation(matrix() * rhs.axis, rhs.n, rhs.power, rhs.reflect);
  }

  throw std::logic_error("Rotation data model cannot handle non-orthogonal multiplication of rotations");
}

SymmetryElement::Matrix Rotation::matrix() const {
  if(!reflect) {
    return Eigen::AngleAxisd(2 * M_PI * power / n, axis).toRotationMatrix();
  }

  const double angle = 2 * M_PI * power / n;
  const double sine = std::sin(angle);
  const double cosine = std::cos(angle);
  const double onePlusCosine = 1 + cosine;

  const double xx = cosine - axis(0) * axis(0) * onePlusCosine;
  const double yy = cosine - axis(1) * axis(1) * onePlusCosine;
  const double zz = cosine - axis(2) * axis(2) * onePlusCosine;

  const double xy = - axis(0) * axis(1) * onePlusCosine;
  const double xz = - axis(0) * axis(2) * onePlusCosine;
  const double yz = - axis(1) * axis(2) * onePlusCosine;

  const double x = axis(0) * sine;
  const double y = axis(1) * sine;
  const double z = axis(2) * sine;

  Eigen::Matrix3d rotationMatrix;

  rotationMatrix <<
        xx, xy - z, xz + y,
    xy + z,     yy, yz - x,
    xz - y, yz + x,     zz;

  return rotationMatrix;
}

boost::optional<SymmetryElement::Vector> Rotation::vector() const {
  return axis;
}

std::string Rotation::name() const {
  std::string composite = (reflect ? "S" : "C");
  composite += std::to_string(n);
  if(power > 1) {
    composite += "^" + std::to_string(power);
  }
  if(std::fabs(axis.z()) < 1e-8) {
    composite += "'";
  } else if(std::fabs(axis.x()) + std::fabs(axis.y()) > 1e-8) {
    composite += (
      " along {"
      + std::to_string(axis.x()) + ", "
      + std::to_string(axis.y()) + ", "
      + std::to_string(axis.z()) + "}"
    );
  }
  return composite;
}

Reflection::Reflection(const Eigen::Vector3d& passNormal) : normal(passNormal.normalized()) {}

Reflection Reflection::sigma_xy() {
  return Reflection(Eigen::Vector3d::UnitZ());
}

Reflection Reflection::sigma_xz() {
  return Reflection(Eigen::Vector3d::UnitY());
}

Reflection Reflection::sigma_yz() {
  return Reflection(Eigen::Vector3d::UnitX());
}

SymmetryElement::Matrix Reflection::matrix() const {
  Eigen::Matrix3d reflection;

  const double normalSquareNorm = normal.squaredNorm();

  for(unsigned i = 0; i < 3; ++i) {
    for(unsigned j = 0; j < 3; ++j) {
      reflection(i, j) = (i == j ? 1 : 0) - 2 * normal(i) * normal(j) / normalSquareNorm;
    }
  }

  return reflection;
}

boost::optional<SymmetryElement::Vector> Reflection::vector() const {
  if(orthogonal(normal, Eigen::Vector3d::UnitZ())) {
    return normal.cross(Eigen::Vector3d::UnitZ());
  }

  if(orthogonal(normal, Eigen::Vector3d::UnitX())) {
    return normal.cross(Eigen::Vector3d::UnitX());
  }

  if(orthogonal(normal, Eigen::Vector3d::UnitY())) {
    return normal.cross(Eigen::Vector3d::UnitY());
  }

  return boost::none;
}

std::string Reflection::name() const {
  std::string composite = "sigma";

  if(normal.cwiseAbs().isApprox(Eigen::Vector3d::UnitZ(), 1e-8)) {
    composite += "h_";
  } else if(orthogonal(normal, Eigen::Vector3d::UnitZ())) {
    composite += "v_";
  } else {
    composite += (
      " w/ normal {"
      + std::to_string(normal.x()) + ", "
      + std::to_string(normal.y()) + ", "
      + std::to_string(normal.z()) + "}"
    );
  }

  if(normal.cwiseAbs().isApprox(Eigen::Vector3d::UnitX(), 1e-8)) {
    composite += " (yz)";
  } else if(normal.cwiseAbs().isApprox(Eigen::Vector3d::UnitY(), 1e-8)) {
    composite += " (xz)";
  }

  return composite;
}

Rotation operator * (const Rotation& rot, const Reflection& reflection) {
  if(!collinear(rot.axis, reflection.normal)) {
    throw std::logic_error("Cannot handle off-axis Rotation / Reflection combination");
  }

  return Rotation(rot.axis, rot.n, rot.power, !rot.reflect);
}

Rotation operator * (const Reflection& reflection, const Rotation& rot) {
  return rot * reflection;
}

Eigen::Matrix3d improperRotationMatrix(
  const Eigen::Vector3d& axis,
  const double angle
) {
  const double sine = std::sin(angle);
  const double cosine = std::cos(angle);
  const double onePlusCosine = 1 + cosine;

  const double xx = cosine - axis(0) * axis(0) * onePlusCosine;
  const double yy = cosine - axis(1) * axis(1) * onePlusCosine;
  const double zz = cosine - axis(2) * axis(2) * onePlusCosine;

  const double xy = - axis(0) * axis(1) * onePlusCosine;
  const double xz = - axis(0) * axis(2) * onePlusCosine;
  const double yz = - axis(1) * axis(2) * onePlusCosine;

  const double x = axis(0) * sine;
  const double y = axis(1) * sine;
  const double z = axis(2) * sine;

  Eigen::Matrix3d rotationMatrix;

  rotationMatrix <<
        xx, xy - z, xz + y,
    xy + z,     yy, yz - x,
    xz - y, yz + x,     zz;

  return rotationMatrix;
}

Eigen::Matrix3d properRotationMatrix(
  const Eigen::Vector3d& axis,
  const double angle
) {
  return Eigen::AngleAxisd(angle, axis).toRotationMatrix();
}

Eigen::Matrix3d reflectionMatrix(const Eigen::Vector3d& planeNormal) {
  Eigen::Matrix3d reflection;

  const double normalSquareNorm = planeNormal.squaredNorm();

  for(unsigned i = 0; i < 3; ++i) {
    for(unsigned j = 0; j < 3; ++j) {
      reflection(i, j) = (i == j ? 1 : 0) - 2 * planeNormal(i) * planeNormal(j) / normalSquareNorm;
    }
  }

  return reflection;
}

//! Returns all symmetry elements of a point group
ElementsList symmetryElements(PointGroup group) noexcept {
  if(group == PointGroup::Cinfv) {
    group = PointGroup::C8v;
  }

  if(group == PointGroup::Dinfh) {
    group = PointGroup::D8h;
  }

  const Identity E {};
  const Inversion inversion {};

  const auto e_x = Eigen::Vector3d::UnitX();
  const auto e_y = Eigen::Vector3d::UnitY();
  const auto e_z = Eigen::Vector3d::UnitZ();

  const Reflection sigma_xy {e_z};
  const Reflection sigma_xz {e_y};
  const Reflection sigma_yz {e_x};

  const double tetrahedronAngle = 2 * std::atan(std::sqrt(2));

  ElementsList elements;
  elements.push_back(wrap(E));

  switch(group) {
    case(PointGroup::C1):
      return elements;

    case(PointGroup::Ci):
      {
        elements.push_back(wrap(inversion));
        return elements;
      }

    case(PointGroup::Cs):
      {
        elements.push_back(wrap(sigma_xy));
        return elements;
      }

    case(PointGroup::C2):
    case(PointGroup::C3):
    case(PointGroup::C4):
    case(PointGroup::C5):
    case(PointGroup::C6):
    case(PointGroup::C7):
    case(PointGroup::C8):
      {
        const unsigned n = 2 + underlying(group) - underlying(PointGroup::C2);
        addProperAxisElements(elements, e_z, n);
        assert(elements.size() == n);
        return elements;
      }

    case(PointGroup::C2h):
    case(PointGroup::C3h):
    case(PointGroup::C4h):
    case(PointGroup::C5h):
    case(PointGroup::C6h):
    case(PointGroup::C7h):
    case(PointGroup::C8h):
      {
        const unsigned n = 2 + underlying(group) - underlying(PointGroup::C2h);
        std::vector<Rotation> rotations;
        const Rotation element = Rotation::Cn(e_z, n);
        Rotation composite = element;
        for(unsigned i = n; i > 1; --i) {
          rotations.push_back(composite);
          composite = element * composite;
        }
        const unsigned S = rotations.size();
        // Add sigma_xy modified Cn axes
        for(unsigned i = 0; i < S; ++i) {
          rotations.push_back(sigma_xy * rotations.at(i));
        }
        return Cnh(2 + underlying(group) - underlying(PointGroup::C2h));
      }

    case(PointGroup::C2v):
    case(PointGroup::C3v):
    case(PointGroup::C4v):
    case(PointGroup::C5v):
    case(PointGroup::C6v):
    case(PointGroup::C7v):
    case(PointGroup::C8v):
      {
        return Cnv(2 + underlying(group) - underlying(PointGroup::C2v));
      }

    case(PointGroup::S4):
    case(PointGroup::S6):
    case(PointGroup::S8):
      {
        const unsigned n = 4 + 2 * (underlying(group) - underlying(PointGroup::S4));
        addImproperAxisElements(elements, e_z, n);
        assert(elements.size() == n);
        return elements;
      }

    case(PointGroup::D2):
    case(PointGroup::D3):
    case(PointGroup::D4):
    case(PointGroup::D5):
    case(PointGroup::D6):
    case(PointGroup::D7):
    case(PointGroup::D8):
      {
        return Dn(2 + underlying(group) - underlying(PointGroup::D2));
      }

    case(PointGroup::D2h):
    case(PointGroup::D3h):
    case(PointGroup::D4h):
    case(PointGroup::D5h):
    case(PointGroup::D6h):
    case(PointGroup::D7h):
    case(PointGroup::D8h):
      {
        return Dnh(2 + underlying(group) - underlying(PointGroup::D2h));
      }

    case(PointGroup::D2d):
    case(PointGroup::D3d):
    case(PointGroup::D4d):
    case(PointGroup::D5d):
    case(PointGroup::D6d):
    case(PointGroup::D7d):
    case(PointGroup::D8d):
      {
        return Dnd(2 + underlying(group) - underlying(PointGroup::D2d));
      }

    /* Cubic groups */
    case(PointGroup::T):
      {
        elements.reserve(12);
        const Eigen::Vector3d axis_2 = properRotationMatrix(e_y, tetrahedronAngle) * e_z;
        const Eigen::Vector3d axis_3 = Rotation::Cn(e_z, 3).matrix() * axis_2;
        const Eigen::Vector3d axis_4 = Rotation::Cn(e_z, 3).matrix() * axis_3;
        addProperAxisElements(elements, e_z, 3);
        addProperAxisElements(elements, axis_2, 3);
        addProperAxisElements(elements, axis_3, 3);
        addProperAxisElements(elements, axis_4, 3);
        elements.push_back(wrap(Rotation::Cn((e_z + axis_2).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_z + axis_3).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_z + axis_4).normalized(), 2)));
        assert(elements.size() == 12);
        return elements;
      }

    case(PointGroup::Td):
      {
        elements.reserve(24);
        Eigen::Matrix<double, 3, Eigen::Dynamic> positions(3, 4);
        positions.col(0) = e_z;
        positions.col(1) = properRotationMatrix(e_y, tetrahedronAngle) * e_z;
        positions.col(2) = Rotation::Cn(e_z, 3).matrix() * positions.col(1);
        positions.col(3) = Rotation::Cn(e_z, 3).matrix() * positions.col(2);
        // C3 axes
        addProperAxisElements(elements, e_z, 3);
        addProperAxisElements(elements, positions.col(1), 3);
        addProperAxisElements(elements, positions.col(2), 3);
        addProperAxisElements(elements, positions.col(3), 3);
        const Eigen::Vector3d axis_12 = (e_z + positions.col(1)).normalized();
        const Eigen::Vector3d axis_13 = (e_z + positions.col(2)).normalized();
        const Eigen::Vector3d axis_14 = (e_z + positions.col(3)).normalized();
        // S4, C2, S4^3
        addImproperAxisElements(elements, axis_12, 4);
        addImproperAxisElements(elements, axis_13, 4);
        addImproperAxisElements(elements, axis_14, 4);
        // Sigma d
        for(unsigned i = 0; i < 3; ++i) {
          for(unsigned j = i + 1; j < 4; ++j) {
            elements.push_back(
              wrap(Reflection(
                positions.col(i).cross(positions.col(j))
              ))
            );
          }
        }
        assert(elements.size() == 24);
        return elements;
      }
    case(PointGroup::Th):
      {
        elements.reserve(24);
        /* i */
        elements.push_back(wrap(inversion));
        /* 4 S6, 4 C3, 4 C3^2, 4 S6^5 along lin. comb. of three axes */
        { // +++ <-> ---
          const Eigen::Vector3d axis_ppp = (  e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_ppp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_ppp, 6)));
          elements.push_back(wrap(Rotation::Sn(axis_ppp, 6, 5)));
        }
        { // ++- <-> --+
          const Eigen::Vector3d axis_ppm = (  e_x + e_y - e_z).normalized();
          addProperAxisElements(elements, axis_ppm, 3);
          elements.push_back(wrap(Rotation::Sn(axis_ppm, 6)));
          elements.push_back(wrap(Rotation::Sn(axis_ppm, 6, 5)));
        }
        { // +-+ <-> -+-
          const Eigen::Vector3d axis_pmp = (  e_x - e_y + e_z).normalized();
          addProperAxisElements(elements, axis_pmp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_pmp, 6)));
          elements.push_back(wrap(Rotation::Sn(axis_pmp, 6, 5)));
        }
        { // -++ <-> +--
          const Eigen::Vector3d axis_mpp = (- e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_mpp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_mpp, 6)));
          elements.push_back(wrap(Rotation::Sn(axis_mpp, 6, 5)));
        }
        /* 3 C2 along axes */
        elements.push_back(wrap(Rotation::Cn(e_x, 2)));
        elements.push_back(wrap(Rotation::Cn(e_y, 2)));
        elements.push_back(wrap(Rotation::Cn(e_z, 2)));
        /* 3 sigma_h with normals along axes */
        elements.push_back(wrap(Reflection(e_x)));
        elements.push_back(wrap(Reflection(e_y)));
        elements.push_back(wrap(Reflection(e_z)));
        assert(elements.size() == 24);
        return elements;
      }

    case(PointGroup::O):
      {
        elements.reserve(24);
        /* 6 C4, 3 C2 (C4^2) == C4, C2 and C4^3 along the coordinate axes */
        addProperAxisElements(elements, e_x, 4);
        addProperAxisElements(elements, e_y, 4);
        addProperAxisElements(elements, e_z, 4);
        /* 8 C3 along linear combinations of three axes */
        { // +++ <-> ---
          const Eigen::Vector3d axis_ppp = (  e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_ppp, 3);
        }
        { // ++- <-> --+
          const Eigen::Vector3d axis_ppm = (  e_x + e_y - e_z).normalized();
          addProperAxisElements(elements, axis_ppm, 3);
        }
        { // +-+ <-> -+-
          const Eigen::Vector3d axis_pmp = (  e_x - e_y + e_z).normalized();
          addProperAxisElements(elements, axis_pmp, 3);
        }
        { // -++ <-> +--
          const Eigen::Vector3d axis_mpp = (- e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_mpp, 3);
        }
        /* 6 C2' along combinations of two axes */
        elements.push_back(wrap(Rotation::Cn((e_x + e_y).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x - e_y).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x + e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x - e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_y + e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_y - e_z).normalized(), 2)));
        assert(elements.size() == 24);
        return elements;
      }
    case(PointGroup::Oh):
      {
        elements.push_back(wrap(inversion));
        elements.reserve(48);
        /* 8 C3 and 8 S6 share the linear combinations of three axes */
        { // +++ <-> ---
          const Eigen::Vector3d axis_ppp = (  e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_ppp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_ppp, 6)));
          elements.push_back(wrap(Rotation::Sn(-axis_ppp, 6)));
        }
        { // ++- <-> --+
          const Eigen::Vector3d axis_ppm = (  e_x + e_y - e_z).normalized();
          addProperAxisElements(elements, axis_ppm, 3);
          elements.push_back(wrap(Rotation::Sn(axis_ppm, 6)));
          elements.push_back(wrap(Rotation::Sn(-axis_ppm, 6)));
        }
        { // +-+ <-> -+-
          const Eigen::Vector3d axis_pmp = (  e_x - e_y + e_z).normalized();
          addProperAxisElements(elements, axis_pmp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_pmp, 6)));
          elements.push_back(wrap(Rotation::Sn(-axis_pmp, 6)));
        }
        { // -++ <-> +--
          const Eigen::Vector3d axis_mpp = (- e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_mpp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_mpp, 6)));
          elements.push_back(wrap(Rotation::Sn(-axis_mpp, 6)));
        }
        /* 6 C2 along linear combinations of two axes */
        elements.push_back(wrap(Rotation::Cn((e_x + e_y).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x - e_y).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x + e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x - e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_y + e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_y - e_z).normalized(), 2)));
        /* 6 C4 and 3 C2 (C4^2) along axes */
        addProperAxisElements(elements, e_x, 4);
        addProperAxisElements(elements, e_y, 4);
        addProperAxisElements(elements, e_z, 4);
        /* 6 S4 along axes */
        elements.push_back(wrap(Rotation::Sn(  e_x, 4)));
        elements.push_back(wrap(Rotation::Sn(- e_x, 4)));
        elements.push_back(wrap(Rotation::Sn(  e_y, 4)));
        elements.push_back(wrap(Rotation::Sn(- e_y, 4)));
        elements.push_back(wrap(Rotation::Sn(  e_z, 4)));
        elements.push_back(wrap(Rotation::Sn(- e_z, 4)));
        /* 3 sigma h along combinations of two axes */
        elements.push_back(wrap(sigma_xy));
        elements.push_back(wrap(sigma_xz));
        elements.push_back(wrap(sigma_yz));
        /* 6 sigma d along linear combinations of three axes */
        elements.push_back(wrap(Reflection((e_x + e_y).cross(e_z))));
        elements.push_back(wrap(Reflection((e_x - e_y).cross(e_z))));
        elements.push_back(wrap(Reflection((e_x + e_z).cross(e_y))));
        elements.push_back(wrap(Reflection((e_x - e_z).cross(e_y))));
        elements.push_back(wrap(Reflection((e_y + e_z).cross(e_x))));
        elements.push_back(wrap(Reflection((e_y - e_z).cross(e_x))));
        assert(elements.size() == 48);
        return elements;
      }

    /* Icosahedral groups */
    case(PointGroup::I):
        return I();

    case(PointGroup::Ih):
        return Ih();

    default:
      return {};
  }
}

unsigned order(const PointGroup group) {
  const std::vector<unsigned> orders {
    1, 2, 2, // C1, Ci, Cs
    2, 3, 4, 5, 6, 7, 8, // Cn
    4, 6, 8, 10, 12, 14, 16, // Cnh
    4, 6, 8, 10, 12, 14, 16, // Cnv
    4, 6, 8, // S4, S6, S8
    4, 6, 8, 10, 12, 14, 16, // Dn
    8, 12, 16, 20, 24, 28, 32, // Dnh
    8, 12, 16, 20, 24, 28, 32, // Dnd
    12, 24, 24, // T, Td, Th
    24, 48, // O, Oh
    60, 120, // I, Ih
    16, 32 // Cinfv, Dinfh (forwarded to C8v and D8h)
  };
  return orders.at(underlying(group));
}

NpGroupingsMapType npGroupings(const ElementsList& elements) {
  assert(elements.front()->matrix() == Elements::Identity().matrix());
  const unsigned E = elements.size();

  NpGroupingsMapType npGroupings;

  auto testVector = [&](const Eigen::Vector3d& v) {
    // Check if there is already a grouping for this vector
    for(const auto& iterPair : npGroupings) {
      if(Temple::any_of(
        iterPair.second,
        [&v](const ElementGrouping& grouping) -> bool {
          return grouping.probePoint.isApprox(v, 1e-8);
        }
      )) {
        return;
      }
    }

    Eigen::Matrix<double, 3, Eigen::Dynamic> mappedPoints(3, E);
    mappedPoints.col(0) = v;
    unsigned np = 1;
    std::vector<
      std::vector<unsigned>
    > groups {
      {0}
    };

    for(unsigned i = 1; i < E; ++i) {
      Eigen::Vector3d mapped = elements.at(i)->matrix() * mappedPoints.col(0);
      bool found = false;
      for(unsigned j = 0; j < np; ++j) {
        if(mappedPoints.col(j).isApprox(mapped, 1e-8)) {
          found = true;
          groups.at(j).push_back(i);
          break;
        }
      }

      if(!found) {
        mappedPoints.col(np) = mapped;
        ++np;
        groups.push_back(std::vector<unsigned> {i});
      }
    }

    assert(std::is_sorted(std::begin(groups), std::end(groups)));

    ElementGrouping grouping;
    grouping.probePoint = mappedPoints.col(0);
    grouping.groups = std::move(groups);
    auto findIter = npGroupings.find(np);
    if(findIter == std::end(npGroupings)) {
      npGroupings.emplace(np, std::vector<ElementGrouping> {std::move(grouping)});
    } else {
      auto& groupingsList = findIter->second;
      if(
        !Temple::any_of(
          groupingsList,
          [&grouping](const ElementGrouping& group) -> bool {
            return grouping.groups == group.groups;
          }
        )
      ) {
        groupingsList.push_back(std::move(grouping));
      }
    }
  };

  testVector(Eigen::Vector3d::UnitZ());
  testVector(Eigen::Vector3d::UnitZ() + 0.1 * Eigen::Vector3d::UnitX());
  testVector(Eigen::Vector3d::UnitX());
  testVector(Eigen::Vector3d::UnitY());
  testVector(Eigen::Vector3d::Zero());

  for(const auto& elementPtr : elements) {
    if(auto axisOption = elementPtr->vector()) {
      testVector(*axisOption);
    }
  }

  return npGroupings;
}

} // namespace Elements
} // namespace Shapes
} // namespace Molassembler
} // namespace Scine


/*
* Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
*/

/*
* \author Guillaume Labbe-Morissette
*/

#ifndef DATACLEANING_CPP
#define DATACLEANING_CPP

#ifdef _WIN32
#include "../utils/getopt.h"
#pragma comment(lib, "Ws2_32.lib")
#endif

#include <cstdio>
#include <string>
#include <iostream>
#include <list>
#include <iostream>
#include <Eigen/Dense>
#include <fstream>
#include "../math/Interpolation.hpp"
#include "../filter/QualityFilter.hpp"
#include "../filter/IntensityFilter.hpp"
#include "../filter/InsanePositionFilter.hpp"

using namespace std;

/**Shows the usage information about data-cleaning*/
void printUsage(){
	std::cerr << "\n\
  NAME\n\n\
     data-cleaning - Filtre les points d'un nuage\n\n\
  SYNOPSIS\n \
	   data-cleaning [-q QualityFilter] [-i IntensityFilter]\n\n\
  DESCRIPTION\n\n \
  Copyright 2017-2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés" << std::endl;
	exit(1);
}

/**
 * Filter all points received on standard input
 *
 * @param argc number of parameter
 * @param argv value of the parameters
 */
int main(int argc,char** argv){

	std::string line;

	//Filter chain
	std::list<PointFilter *> filters;
        filters.push_back(new InsanePositionFilter());

	//TODO: load desired filters and parameters from command line
        int index;
        int quality;
        int intensity;
        while((index=getopt(argc,argv,"q:i:"))!=-1)
        {
            switch(index)
            {
                case 'q':
                    if(sscanf(optarg,"%d", &quality) != 1)
                    {
                        std::cerr << "Error: -q invalid quality parameter" << std::endl;
                        printUsage();
                    }
                    else
                    {
                        filters.push_back(new QualityFilter(quality));
                    }
                break;

                case 'i':
                    if(sscanf(optarg,"%d", &intensity) != 1)
                    {
                        std::cerr << "Error: -i invalid intensity parameter" << std::endl;
                        printUsage();
                    }
                    else
                    {
                        filters.push_back(new IntensityFilter(intensity));
                    }
                break;
            }
        }
        unsigned int lineCount = 1;
        while((std::getline(std::cin,line))&&(line!="0")){
            double x,y,z;
            uint32_t quality;
            uint32_t intensity;

            if(sscanf(line.c_str(),"%lf %lf %lf %d %d",&x,&y,&z,&quality,&intensity)==5){
		bool doFilter = false;

			//Apply filter chain
		for(auto i = filters.begin();i!= filters.end();i++){
                    if( (*i)->filterPoint(0,x,y,z,quality,intensity) ){
			doFilter = true;
                        break;
                    }
		}

		if(!doFilter){
                    printf("%.6lf %.6lf %.6lf %d %d\n",x,y,z,quality,intensity);
		}
            }
            else{
		std::cerr << "Error at line " << lineCount << std::endl;
            }
            lineCount++;
        }
    }
#endif


#include <boost/circular_buffer.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <range/v3/algorithm.hpp>
#include <range/v3/numeric.hpp>
#include <range/v3/view.hpp>
#include <../aoc2020/combinations_algo.h>
namespace views = ranges::views;

int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    std::string line;
    int n = 0;
    while (std::getline(ifs, line)) {
      int t = 0;
      auto f = [&t](auto b, auto e) {
        if ((*b % *(b + 1)) == 0) {
          t += (*b) / *(b + 1);
        }
        return false;
      };
      std::istringstream iss(line);
      std::istream_iterator<int> b(iss);
      std::istream_iterator<int> e;
      std::vector<int> v;
      std::copy(b, e, std::back_inserter(v));
      for_each_permutation(v.begin(), v.begin() + 2, v.end(), f);
      n += t;
    }
    std::cout << n << std::endl;
  }
}


/*
 * sat.cpp
 *
 *  Created on: Feb 3, 2014
 *      Author: algomorph
 */
#include <sat.h>
#include <cstdlib>
#include <cstring>
#include <sstream>
#include <string>
#include <iostream>
#include <oclUtils.h>
#include <ClState.hpp>
#include <gpudefs.h>
#include <boost/lexical_cast.hpp>
#include <test.hpp>
namespace sat {

void configureSat(satConfig& config, const int& w, const int& h) {
	//pad so the matrix dimensions are multiples of 32
	config.width = w;
	config.height = h;
	config.carryWidth = w + (w % WARP_SIZE > 0) * (WARP_SIZE - (w % WARP_SIZE));
	config.carryHeight = h
			+ (w % WARP_SIZE > 0) * (WARP_SIZE - (h % WARP_SIZE));
	config.colGroupCount = config.carryWidth / WARP_SIZE;
	config.rowGroupCount = config.carryHeight / WARP_SIZE;
	config.iLastColGroup = config.colGroupCount - 1;
	config.iLastRowGroup = config.rowGroupCount - 1;
	config.inputStride = SCHEDULE_OPTIMIZED_N_WARPS * config.carryWidth;
	config.border = 0;
	config.invWidth = 1.f / (float) w;
	config.invHeight = 1.f / (float) h;
}

#define DEF_SYMBOL(name,value) " -D " + std::string(name) + "=" + value

std::string genSatClDefineOptions(const satConfig& config) {
	return
	DEF_SYMBOL("WIDTH", boost::lexical_cast<std::string>(config.width))
			+ DEF_SYMBOL("HEIGHT", boost::lexical_cast<std::string>(config.height))
			+ DEF_SYMBOL("N_COLUMNS",
					boost::lexical_cast<std::string>(config.colGroupCount))
			+ DEF_SYMBOL("N_ROWS",
					boost::lexical_cast<std::string>(config.rowGroupCount))
			+ DEF_SYMBOL("LAST_M",
					boost::lexical_cast<std::string>(config.iLastColGroup))
			+ DEF_SYMBOL("LAST_N",
					boost::lexical_cast<std::string>(config.iLastRowGroup))
			+ DEF_SYMBOL("BORDER", boost::lexical_cast<std::string>(config.border))
			+ DEF_SYMBOL("CARRY_WIDTH",
					boost::lexical_cast<std::string>(config.carryWidth))
			+ DEF_SYMBOL("CARRY_HEIGHT",
					boost::lexical_cast<std::string>(config.carryHeight))
			+ DEF_SYMBOL("INPUT_STRIDE",
					boost::lexical_cast<std::string>(config.inputStride))
			+ DEF_SYMBOL("INV_WIDTH",
					boost::lexical_cast<std::string>(config.invWidth))
			+ DEF_SYMBOL("INV_HEIGHT",
					boost::lexical_cast<std::string>(config.invHeight));
}

void runKernel(const cl_kernel& kernel, const cl_command_queue& queue,
		const cl_uint& argCount, cl_mem* args, const cl_uint& dim,
		size_t* globalSize, size_t* localSize) {
	cl_int errCode = CL_SUCCESS;
	char kernelName[128];
	for (cl_uint iMem = 0; iMem < argCount; iMem++) {
		errCode = clSetKernelArg(kernel, iMem, sizeof(cl_mem),
				(void *) (args + iMem));
		if (errCode != CL_SUCCESS) {
			cl_int err2 = clGetKernelInfo(kernel, CL_KERNEL_FUNCTION_NAME,128,&kernelName,NULL);
			shrLog("clSetKernelArg failed to set argument for kernel %s with error %d.\n",kernelName,
					errCode);
		}
	}
	errCode = clEnqueueNDRangeKernel(queue, kernel, dim, 0, globalSize,
			localSize, 0, 0, 0);
	if (errCode != CL_SUCCESS) {
		shrLog(
				"clEnqueueNDRangeKernel failed to enqueue the kernel with error %d.\n",
				errCode);
	}
}

void padMatrixLR(float* origMat, float* paddedMat, const int& width, const int& height,
		const int& carryWidth) {
	const size_t origRowSize = width*sizeof(float);
	for(int iRow = 0; iRow < height; iRow++){
		memcpy(paddedMat,origMat,origRowSize);
		paddedMat += carryWidth;
		origMat += width;
	}
}
void unpadMatrixLR(float* destMat, float* paddedMat, const int& width, const int& height,
		const int& carryWidth) {
	const size_t destRowSize = width*sizeof(float);
	for(int iRow = 0; iRow < height; iRow++){
		memcpy(destMat,paddedMat,destRowSize);
		paddedMat += carryWidth;
		destMat += width;
	}
}

void computeSummedAreaTable(float* inOutMatrix, const int& w, const int& h,
		CLState& state) {
	satConfig config;
	configureSat(config, w, h);
	std::string defineOptions = genSatClDefineOptions(config);
	std::cout << "Define options:\n " << defineOptions << std::endl;
	cl_program program = state.compileOCLProgram("sat.cl", defineOptions);
	cl_int errCode;
	float* preppedInOutMatrix;
	bool paddingEnabled = config.carryWidth != config.width || config.carryHeight != config.height;
	if(paddingEnabled){
		preppedInOutMatrix = new float[config.carryWidth*config.carryHeight]();
		padMatrixLR(inOutMatrix, preppedInOutMatrix, config.width, config.height, config.carryWidth);
	}else{
		preppedInOutMatrix = inOutMatrix;
	}

	const size_t matrixBufferSize = config.carryWidth * config.carryHeight
			* sizeof(float);

	//create matrix buffer and copy the host contents
	cl_mem matrix = clCreateBuffer(state.context,
	CL_MEM_READ_WRITE, matrixBufferSize, NULL, &errCode);

	if (errCode != CL_SUCCESS) {
		shrLog(
				"Failed to create matrix buffer & copy the contents to device, error %d.\n",
				errCode);
	}

	//create buffers in GPU memory for intermediate results
	cl_mem yBar = clCreateBuffer(state.context, CL_MEM_READ_WRITE,
			config.rowGroupCount * config.carryWidth * sizeof(float), NULL,
			&errCode);

	if (errCode != CL_SUCCESS) {
		shrLog("Failed to create buffer, error %d.\n", errCode);
	}

	cl_mem vHat = clCreateBuffer(state.context, CL_MEM_READ_WRITE,
			config.colGroupCount * config.carryHeight * sizeof(float), NULL,
			&errCode);
	if (errCode != CL_SUCCESS) {
		shrLog("Failed to create buffer, error %d.\n", errCode);
	}

	cl_mem ySum = clCreateBuffer(state.context, CL_MEM_READ_WRITE,
			config.rowGroupCount * config.colGroupCount * sizeof(float), NULL,
			&errCode);

	if (errCode != CL_SUCCESS) {
		shrLog("Failed to create buffer, error %d.\n", errCode);
	}

	cl_command_queue queue = clCreateCommandQueue(state.context,
			state.devices[0],
			(state.enableProfiling ? CL_QUEUE_PROFILING_ENABLE : 0), &errCode);

	//push back elements to be released after the program has run
	state.deferredReleaseQueues.push_back(queue);
	state.deferredReleaseMem.push_back(matrix);
	state.deferredReleaseMem.push_back(yBar);
	state.deferredReleaseMem.push_back(vHat);
	state.deferredReleaseMem.push_back(ySum);

	//create kernels
	cl_kernel stage1 = clCreateKernel(program, "computeBlockAggregates", &errCode);
	cl_kernel stage2 = clCreateKernel(program, "verticalAggregate", &errCode);
	cl_kernel stage3 = clCreateKernel(program, "horizontalAggregate", &errCode);
	cl_kernel stage4 = clCreateKernel(program, "redistributeSAT",
			&errCode);

	size_t group_size_factor;
	errCode = clGetKernelWorkGroupInfo(stage1, state.devices[0],
	CL_KERNEL_PREFERRED_WORK_GROUP_SIZE_MULTIPLE, sizeof(size_t),
			&group_size_factor, NULL);
	std::cout << "Sugguested work group size multiple: " << group_size_factor
			<< std::endl << std::flush;

	size_t max_kernel_group_size;
	errCode = clGetKernelWorkGroupInfo(stage1, state.devices[0],
	CL_KERNEL_WORK_GROUP_SIZE, sizeof(size_t), &max_kernel_group_size, NULL);
	std::cout << "Maximum work group size: " << max_kernel_group_size
			<< std::endl << std::flush;

	const int nWm = (config.width + MAX_N_THREADS - 1) / MAX_N_THREADS, nHm =
			(config.height + MAX_N_THREADS - 1) / MAX_N_THREADS;

// load data:
	errCode = clEnqueueWriteBuffer(queue, matrix, CL_TRUE, 0, matrixBufferSize,
			preppedInOutMatrix, 0, NULL, NULL);

//===stage 1===

//debug array & buffer
	const int debugBufCount = WARP_SIZE * (WARP_SIZE + 1);
	size_t debugBufSize = debugBufCount * sizeof(float);
	float* debugArray = new float[debugBufCount];
	for (int iFloat; iFloat < debugBufCount; iFloat++) {
		debugArray[iFloat] = 0.0f;
	}
	//prettyPrintMat(debugArray, WARP_SIZE, WARP_SIZE + 1);

	cl_mem debugBuf = clCreateBuffer(state.context,
	CL_MEM_READ_WRITE, debugBufSize, NULL, &errCode);
	errCode = clEnqueueWriteBuffer(queue, debugBuf, CL_TRUE, 0, debugBufSize,
			debugArray, 0, NULL, NULL);

	if (errCode != CL_SUCCESS) {
		shrLog("clCreateBuffer failed to create buffer with error %d.\n",
				errCode);
	}
	state.deferredReleaseMem.push_back(debugBuf);

	const cl_uint workDim = 2;
	const cl_uint stage1argCount = 3;
	size_t stage1GlobalSize[] = { static_cast<size_t>(config.carryWidth),
			static_cast<size_t>(config.colGroupCount
					* SCHEDULE_OPTIMIZED_N_WARPS) };
	size_t stage1LocalSize[] = { WARP_SIZE, SCHEDULE_OPTIMIZED_N_WARPS };
	cl_mem stage1Args[stage1argCount] = { matrix, yBar, vHat};
	runKernel(stage1, queue, stage1argCount, stage1Args, workDim,
			stage1GlobalSize, stage1LocalSize);
	//get data back from GPU
	//cl_event gpudone_event;
//	errCode = clEnqueueReadBuffer(queue, debugBuf, CL_TRUE, 0, debugBufSize,
//			debugArray, 0, NULL, NULL);

	//prettyPrintMat(debugArray, WARP_SIZE, WARP_SIZE + 1);
	if (errCode != CL_SUCCESS) {
		shrLog("clEnqueueReadBuffer failed to read buffer with error %d.\n",
				errCode);
	}

	//===stage 2===
	//initiate global & group sizes
	const cl_uint stage2argCount = 2;
	size_t stage2GlobalSize[] = { static_cast<size_t>(nWm * WARP_SIZE),
			static_cast<size_t>(MAX_WARPS) };
	size_t stage2LocalSize[] = { WARP_SIZE, MAX_WARPS };
	cl_mem stage2Args[stage2argCount] = { yBar, ySum };
	runKernel(stage2, queue, stage2argCount, stage2Args, workDim,
			stage2GlobalSize, stage2LocalSize);

	//===stage 3===
	//initiate global & group sizes
	const cl_uint stage3argCount = 2;
	size_t stage3GlobalSize[] = { static_cast<size_t>(WARP_SIZE),
			static_cast<size_t>(nHm * MAX_WARPS) };
	size_t stage3LocalSize[] = { WARP_SIZE, MAX_WARPS };
	cl_mem stage3Args[stage3argCount] = { ySum, vHat };
	runKernel(stage3, queue, stage3argCount, stage3Args, workDim,
			stage3GlobalSize, stage3LocalSize);

	//===stage 4===
	//initiate global & group sizes
	const cl_uint stage4argCount = 3;
	size_t stage4GlobalSize[] = { static_cast<size_t>(config.carryWidth),
			static_cast<size_t>(config.colGroupCount
					* SCHEDULE_OPTIMIZED_N_WARPS) };
	size_t stage4LocalSize[] = { WARP_SIZE, SCHEDULE_OPTIMIZED_N_WARPS };
	cl_mem stage4Args[stage4argCount] = { matrix, yBar, vHat };
	runKernel(stage4, queue, stage4argCount, stage4Args, workDim,
			stage4GlobalSize, stage4LocalSize);
	errCode = clFlush(queue);
	if (errCode != CL_SUCCESS) {
		shrLog("clFlush failed to with error %d.\n", errCode);
	}

	//get data back from GPU
	//cl_event gpudone_event;
	errCode = clEnqueueReadBuffer(queue, matrix, CL_TRUE, 0,
			static_cast<size_t>(matrixBufferSize), preppedInOutMatrix, 0, NULL,
			NULL);
	if (errCode != CL_SUCCESS) {
		shrLog("clEnqueueReadBuffer failed to read buffer with error %d.\n",
				errCode);
	}
	if(paddingEnabled){
		unpadMatrixLR(inOutMatrix,preppedInOutMatrix,config.width,config.height,config.carryWidth);
	}
	delete[] debugArray;
} //end computeSummedAreaTable
} //end namespace sat

int main(int argc, char** argv) {
// start the logs
	shrSetLogFileName("clsat.log");
	const int width = 32, height = 32;
	float* inOutMatrix = new float[width * height];
	std::cout << "[clsat] Generating random input image (" << width << "x"
			<< height << ") ... " << std::flush;
	for (int i = 0; i < width * height; ++i)
		inOutMatrix[i] = 1; //rand() % 256;
	std::cout << "done!" << std::endl
			<< "[clsat] Computing summed-area table in the GPU ... "
			<< std::endl << std::flush;
	CLState state(true, true, true);
	sat::computeSummedAreaTable(inOutMatrix, width, height, state);
	prettyPrintMatrix(inOutMatrix, width, height);
	delete[] inOutMatrix;
	return 0;
}


#define _USE_MATH_DEFINES

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

#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <igvc_msgs/velocity_pair.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/MarkerArray.h>

#include <tf/transform_datatypes.h>

#include <igvc_utils/robot_state.h>
#include <parameter_assertions/assertions.h>

#include "path_follower.h"

PathFollower::PathFollower()
{
  ros::NodeHandle nh;
  ros::NodeHandle pNh("~");

  // load controller parameters
  using seconds = double;

  double target_velocity;
  double axle_length;
  double k1;
  double k2;
  double simulation_frequency;
  double lookahead_dist;
  seconds simulation_horizon;

  assertions::param(pNh, "target_v", target_velocity, 1.0);
  assertions::param(pNh, "axle_length", axle_length, 0.52);
  assertions::param(pNh, "k1", k1, 1.0);
  assertions::param(pNh, "k2", k2, 3.0);
  assertions::param(pNh, "simulation_frequency", simulation_frequency, 100.0);
  assertions::param(pNh, "lookahead_dist", lookahead_dist, 2.0);
  assertions::param(pNh, "simulation_horizon", simulation_horizon, 5.0);

  controller_ = std::unique_ptr<SmoothControl>(new SmoothControl{
      k1, k2, axle_length, simulation_frequency, target_velocity, lookahead_dist, simulation_horizon });

  // load global parameters
  assertions::getParam(pNh, "maximum_vel", maximum_vel_, { assertions::NumberAssertionType::POSITIVE });
  assertions::param(pNh, "stop_dist", stop_dist_, 0.9);

  ros::Subscriber path_sub = nh.subscribe("/path", 1, &PathFollower::pathCallback, this);
  ros::Subscriber pose_sub = nh.subscribe("/odometry/filtered", 1, &PathFollower::positionCallback, this);
  ros::Subscriber waypoint_sub = nh.subscribe("/waypoint", 1, &PathFollower::waypointCallback, this);

  cmd_pub_ = nh.advertise<igvc_msgs::velocity_pair>("/motors", 1);
  target_pub_ = nh.advertise<geometry_msgs::PointStamped>("/target_point", 1);
  trajectory_pub_ = nh.advertise<nav_msgs::Path>("/trajectory", 1);

  ros::spin();
}

void PathFollower::pathCallback(const nav_msgs::PathConstPtr& msg)
{
  ROS_DEBUG_STREAM("Follower got path. Size: " << msg->poses.size());
  //  path_ = msg;
  // TODO: Remove patch when motion planning correctly incorporates heading
  path_ = getPatchedPath(msg);
}

void PathFollower::waypointCallback(const geometry_msgs::PointStampedConstPtr& msg)
{
  waypoint_ = msg;
}

/**
Constructs a new trajectory to follow using the current path msg and publishes
the first velocity command from this trajectory.
*/
void PathFollower::positionCallback(const nav_msgs::OdometryConstPtr& msg)
{
  if (path_.get() == nullptr || path_->poses.empty() || path_->poses.size() < 2)
  {
    ROS_INFO_THROTTLE(1, "Path empty.");
    igvc_msgs::velocity_pair vel;
    vel.left_velocity = 0.;
    vel.right_velocity = 0.;
    cmd_pub_.publish(vel);
    path_.reset();
    return;
  }

  // Current pose
  RobotState cur_pos(msg);

  double goal_dist = cur_pos.distTo(waypoint_->point.x, waypoint_->point.y);

  ROS_DEBUG_STREAM("Distance to waypoint: " << goal_dist << "(m.)");

  ros::Time time = msg->header.stamp;
  igvc_msgs::velocity_pair vel;  // immediate velocity command
  vel.header.stamp = time;

  if (goal_dist <= stop_dist_)
  {
    /**
    Stop when the robot is a set distance away from the waypoint
    */
    ROS_INFO_STREAM(">>>WAYPOINT REACHED...STOPPING<<<");
    vel.right_velocity = 0;
    vel.left_velocity = 0;
    // make path null to stop planning until path generated
    // for new waypoint
    path_ = nullptr;
  }
  else
  {
    /**
    Obtain smooth control law from the controller. This includes a smooth
    trajectory for visualization purposes and an immediate velocity command.
    */
    nav_msgs::Path trajectory_msg;
    trajectory_msg.header.stamp = time;
    trajectory_msg.header.frame_id = "/odom";

    RobotState target;
    controller_->getTrajectory(vel, path_, trajectory_msg, cur_pos, target);
    // publish trajectory
    trajectory_pub_.publish(trajectory_msg);

    // publish target position
    geometry_msgs::PointStamped target_point;
    target_point.header.frame_id = "/odom";
    target_point.header.stamp = time;
    tf::pointTFToMsg(target.transform.getOrigin(), target_point.point);
    target_pub_.publish(target_point);

    ROS_DEBUG_STREAM("Distance to target: " << cur_pos.distTo(target) << "(m.)");
  }

  // make sure maximum velocity not exceeded
  if (std::max(std::abs(vel.right_velocity), std::abs(vel.left_velocity)) > maximum_vel_)
  {
    ROS_ERROR_STREAM_THROTTLE(5, "Maximum velocity exceeded. Right: "
                                     << vel.right_velocity << "(m/s), Left: " << vel.left_velocity
                                     << "(m/s), Max: " << maximum_vel_ << "(m/s) ... Stopping robot...");
    vel.right_velocity = 0;
    vel.left_velocity = 0;
  }

  cmd_pub_.publish(vel);  // pub velocity command
}

nav_msgs::PathConstPtr PathFollower::getPatchedPath(const nav_msgs::PathConstPtr& msg) const
{
  nav_msgs::PathPtr new_path = boost::make_shared<nav_msgs::Path>(*msg);
  size_t num_poses = new_path->poses.size();
  for (size_t i = 0; i < num_poses; i++)
  {
    double delta_x = new_path->poses[i + 1].pose.position.x - new_path->poses[i].pose.position.x;
    double delta_y = new_path->poses[i + 1].pose.position.y - new_path->poses[i].pose.position.y;
    double heading = atan2(delta_y, delta_x);
    new_path->poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(heading);
  }
  new_path->poses.back().pose.orientation = new_path->poses[num_poses - 2].pose.orientation;
  return new_path;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "path_follower");
  PathFollower path_follower;
  return 0;
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;
using arma::Col;
using arma::Row;
using arma::uword;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenMatRowIndsGenRowVec : public Expected {
    public:
      ExpectedInPlaceGenMatRowIndsGenRowVec() {
        cout << "Compute ExpectedInPlaceGenMatRowIndsGenRowVec(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
          InputClass::GenMat,
          InputClass::RowInds,
          InputClass::GenRowVec
        });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _genMat = *static_cast<Mat<double>*>(value.second);
                break;
              case 1:
                _fileSuffix += "," + value.first;
                _rowInds = *static_cast<Col<uword>*>(value.second);
                break;
              case 2:
                _fileSuffix += "," + value.first;
                _genRowVec = *static_cast<Row<double>*>(value.second);
                break;
            }
            ++n;
          }

          cout << "Using input: " << _fileSuffix << endl;

          _copyOfGenMat = _genMat;
          _copyOfRowInds = _rowInds;
          _copyOfGenRowVec = _genRowVec;

          expectedRowsEqual();

          _genMat = _copyOfGenMat;
          _rowInds = _copyOfRowInds;
          _genRowVec = _copyOfGenRowVec;
          expectedRowsPlus();

          _genMat = _copyOfGenMat;
          _rowInds = _copyOfRowInds;
          _genRowVec = _copyOfGenRowVec;
          expectedRowsMinus();

          _genMat = _copyOfGenMat;
          _rowInds = _copyOfRowInds;
          _genRowVec = _copyOfGenRowVec;
          expectedRowsElemTimes();

          _genMat = _copyOfGenMat;
          _rowInds = _copyOfRowInds;
          _genRowVec = _copyOfGenRowVec;
          expectedRowsElemDivide();

          _genMat = _copyOfGenMat;
          _rowInds = _copyOfRowInds;
          _genRowVec = _copyOfGenRowVec;
          expectedEach_rowEqual();

          _genMat = _copyOfGenMat;
          _rowInds = _copyOfRowInds;
          _genRowVec = _copyOfGenRowVec;
          expectedEach_rowPlus();

          _genMat = _copyOfGenMat;
          _rowInds = _copyOfRowInds;
          _genRowVec = _copyOfGenRowVec;
          expectedEach_rowMinus();

          _genMat = _copyOfGenMat;
          _rowInds = _copyOfRowInds;
          _genRowVec = _copyOfGenRowVec;
          expectedEach_rowElemTimes();

          _genMat = _copyOfGenMat;
          _rowInds = _copyOfRowInds;
          _genRowVec = _copyOfGenRowVec;
          expectedEach_rowElemDivide();
        }

        cout << "done." << endl;
      }

    protected:
      Mat<double> _genMat;
      Mat<double> _copyOfGenMat;

      Col<uword> _rowInds;
      Col<uword> _copyOfRowInds;

      Row<double> _genRowVec;
      Row<double> _copyOfGenRowVec;

      void expectedRowsEqual() {
        for(int i = 0; i < _rowInds.n_elem; i++) {
          if(_rowInds.at(i) >= _genMat.n_rows) {
            return;
          }
        }

        if(_genRowVec.n_cols != _genMat.n_cols) {
          return;
        }

        if(_rowInds.n_elem != 1) {
          return;
        }

        cout << "- Compute expectedRowsEqual() ... ";

        _genMat.rows(_rowInds) = _genRowVec;
        save<double>("Mat.rowsEqual", _genMat);

        cout << "done." << endl;
      }

      void expectedRowsPlus() {
        for(int i = 0; i < _rowInds.n_elem; i++) {
          if(_rowInds.at(i) >= _genMat.n_rows) {
            return;
          }
        }

        if(_genRowVec.n_cols != _genMat.n_cols) {
          return;
        }

        if(_rowInds.n_elem != 1) {
          return;
        }

        cout << "- Compute expectedRowsPlus() ... ";

        _genMat.rows(_rowInds) += _genRowVec;
        save<double>("Mat.rowsPlus", _genMat);

        cout << "done." << endl;
      }

      void expectedRowsMinus() {
        for(int i = 0; i < _rowInds.n_elem; i++) {
          if(_rowInds.at(i) >= _genMat.n_rows) {
            return;
          }
        }

        if(_genRowVec.n_cols != _genMat.n_cols) {
          return;
        }

        if(_rowInds.n_elem != 1) {
          return;
        }

        cout << "- Compute expectedRowsMinus() ... ";

        _genMat.rows(_rowInds) -= _genRowVec;
        save<double>("Mat.rowsMinus", _genMat);

        cout << "done." << endl;
      }

      void expectedRowsElemTimes() {
        for(int i = 0; i < _rowInds.n_elem; i++) {
          if(_rowInds.at(i) >= _genMat.n_rows) {
            return;
          }
        }

        if(_genRowVec.n_cols != _genMat.n_cols) {
          return;
        }

        if(_rowInds.n_elem != 1) {
          return;
        }

        cout << "- Compute expectedRowsElemTimes() ... ";

        _genMat.rows(_rowInds) %= _genRowVec;
        save<double>("Mat.rowsElemTimes", _genMat);

        cout << "done." << endl;
      }

      void expectedRowsElemDivide() {
        for(int i = 0; i < _rowInds.n_elem; i++) {
          if(_rowInds.at(i) >= _genMat.n_rows) {
            return;
          }
        }

        if(_genRowVec.n_cols != _genMat.n_cols) {
          return;
        }

        if(_rowInds.n_elem != 1) {
          return;
        }

        cout << "- Compute expectedRowsElemDivide() ... ";

        _genMat.rows(_rowInds) /= _genRowVec;
        save<double>("Mat.rowsElemDivide", _genMat);

        cout << "done." << endl;
      }

      void expectedEach_rowEqual() {
        for(int i = 0; i < _rowInds.n_elem; i++) {
          if(_rowInds.at(i) >= _genMat.n_rows) {
            return;
          }
        }

        if(_genRowVec.n_cols != _genMat.n_cols) {
          return;
        }

        cout << "- Compute expectedEach_rowEqual() ... ";

        _genMat.each_row(_rowInds) = _genRowVec;
        save<double>("Mat.each_rowEqual", _genMat);

        cout << "done." << endl;
      }

      void expectedEach_rowPlus() {
        for(int i = 0; i < _rowInds.n_elem; i++) {
          if(_rowInds.at(i) >= _genMat.n_rows) {
            return;
          }
        }

        if(_genRowVec.n_cols != _genMat.n_cols) {
          return;
        }

        cout << "- Compute expectedEach_rowPlus() ... ";

        _genMat.each_row(_rowInds) += _genRowVec;
        save<double>("Mat.each_rowPlus", _genMat);

        cout << "done." << endl;
      }

      void expectedEach_rowMinus() {
        for(int i = 0; i < _rowInds.n_elem; i++) {
          if(_rowInds.at(i) >= _genMat.n_rows) {
            return;
          }
        }

        if(_genRowVec.n_cols != _genMat.n_cols) {
          return;
        }

        cout << "- Compute expectedEach_rowMinus() ... ";

        _genMat.each_row(_rowInds) -= _genRowVec;
        save<double>("Mat.each_rowMinus", _genMat);

        cout << "done." << endl;
      }

      void expectedEach_rowElemTimes() {
        for(int i = 0; i < _rowInds.n_elem; i++) {
          if(_rowInds.at(i) >= _genMat.n_rows) {
            return;
          }
        }

        if(_genRowVec.n_cols != _genMat.n_cols) {
          return;
        }

        cout << "- Compute expectedEach_rowElemTimes() ... ";

        _genMat.each_row(_rowInds) %= _genRowVec;
        save<double>("Mat.each_rowElemTimes", _genMat);

        cout << "done." << endl;
      }

      void expectedEach_rowElemDivide() {
        for(int i = 0; i < _rowInds.n_elem; i++) {
          if(_rowInds.at(i) >= _genMat.n_rows) {
            return;
          }
        }

        if(_genRowVec.n_cols != _genMat.n_cols) {
          return;
        }

        cout << "- Compute expectedEach_rowElemDivide() ... ";

        _genMat.each_row(_rowInds) /= _genRowVec;
        save<double>("Mat.each_rowElemDivide", _genMat);

        cout << "done." << endl;
      }

  };
}