//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 20012 - 2012   LRI    UMR 12623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_FACT_12_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_FACT_12_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/constant/register.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_fact_12 Fact_12
 *
 * \par Description
 * Constant Fact_12, 12! = 479001600, the saturated factorial value is
 * min(Valmax<T>(),479001600)
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/fact_12.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::fact_12_(A0)>::type
 *     Fact_12();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Fact_12
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Fact_12 of functor Fact_12
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    BOOST_SIMD_CONSTANT_REGISTER(Fact_12,int,479001600,0x4de467e0, 0x41bc8cfc00000000ll)
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Fact_12, Fact_12)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif


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

int main()
{
  short a = 42;
  int b = 42;

  if (boost::numeric_cast<int>(a) != b) {
    return 1;
  }

  return 0;
}


//------------------------------------------------------------------------------
/// \file ReaderMonad_tests.cpp
/// \ref Ivan Čukić, Functional Programming in C++,  Manning Publications;
/// 1st edition (November 19, 2018). ISBN-13: 978-1617293818
//------------------------------------------------------------------------------
#include "Categories/Monads/ReaderMonad.h"

#include <boost/test/unit_test.hpp>
#include <cassert>
#include <cstdio> // std::tmpfile
#include <filesystem> // std::filesystem::temp_directory_path;
#include <fstream>
#include <iostream>
#include <optional>
#include <regex>
#include <string>

using Categories::Monads::ReaderMonad::Local;
using Categories::Monads::ReaderMonad::MultiplicationEndomorphismComposed;
using Categories::Monads::ReaderMonad::Unit;
using Categories::Monads::ReaderMonad::apply_morphism;
using Categories::Monads::ReaderMonad::ask;
using Categories::Monads::ReaderMonad::bind;
using Categories::Monads::ReaderMonad::bind_;
using Categories::Monads::ReaderMonad::return_;
using Categories::Monads::ReaderMonad::unit;

BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(Monads)
BOOST_AUTO_TEST_SUITE(ReaderMonad_tests)

class TemporaryDirectory
{
  public:

    TemporaryDirectory(const std::string& temporary_directory):
      temporary_directory_{temporary_directory}
    {
      std::filesystem::create_directory(temporary_directory);
    }

    class TemporaryFile
    {
      public:

        TemporaryFile(
          const std::filesystem::path directory,
          const std::string& filename
          ):
          filename_path_{directory / filename}
        {}

        std::filesystem::path filename_path() const
        {
          return filename_path_;
        }

      private:

        std::filesystem::path filename_path_;
    };

    ~TemporaryDirectory()
    {
      std::filesystem::remove_all(temporary_directory_);
    }

    TemporaryFile temporary_file(const std::string& filename)
    {
      return TemporaryFile{temporary_directory_, filename};
    }

  private:

    std::filesystem::path temporary_directory_;
};

// cf. http://www.cplusplus.com/doc/tutorial/files/
// https://stackoverflow.com/questions/3379956/how-to-create-a-temporary-directory-in-c

void set_fake_state(const std::filesystem::path& filepath, const int value)
{
  std::ofstream output_stream {filepath, std::ios::binary};
  output_stream.seekp(0, std::ios::beg);

  output_stream << std::to_string(value) << "\n";

  output_stream.close();
}

std::optional<int> read_fake_state(const std::filesystem::path& filepath)
{
  std::ifstream input_stream {filepath, std::ios::binary};
  std::string s;
  input_stream.seekg(0, std::ios::beg);

  std::getline(input_stream, s);

  if (s.empty())
  {
    return std::nullopt;
  }

  return std::make_optional<int>(std::stoi(s));
}

struct TestEnvironment
{
  bool yes_;
  double x_;
  std::string s_;
};

TestEnvironment initial_test_environment {false, 42.0, "Start"};
const TestEnvironment test_environment {true, 1.616, "Middle"};

// Test morphisms.

double r(const TestEnvironment& environment)
{
  if (environment.yes_)
  {
    return environment.x_;
  }
  return 0.0;
}

std::string s(const TestEnvironment& environment)
{
  if (environment.yes_)
  {
    return environment.s_;
  }
  return "Go back.";
}

// test_morphisms belongs to Hom(E, X).
// Example from https://nbviewer.jupyter.org/github/dbrattli/OSlash/blob/master/notebooks/Reader.ipynb

// Auxiliary function to test morphism.
std::string transform_text(const std::string text)
{
  return std::regex_replace(text, std::regex("Hi"), "Hello");
}

// Serves as a morphism in (E \to X)
auto test_string_morphism = [](const std::string& name) -> std::string
{
  return "Hi " + name + "!";
};

// Serves as a morphism f : X \to T(Y) that will have an endomorphism applied to
// it.
auto another_string_morphism(const std::string& text)
{
  const std::string replaced_text {transform_text(text)};

  return unit<std::string, std::string>(replaced_text);
}

// Modified example.
auto test_double_string_morphism = [](const double value) -> std::string
{
  return "Value : " + std::to_string(value) + ", ";
};

BOOST_AUTO_TEST_SUITE(Morphisms_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TestMorphismsMapEnvironmentToCategoryObjectType)
{
  BOOST_TEST(r(initial_test_environment) == 0.0);
  BOOST_TEST(r(test_environment) == 1.616);
  BOOST_TEST(s(initial_test_environment) == "Go back.");
  BOOST_TEST(s(test_environment) == "Middle");
  BOOST_TEST(test_string_morphism("Jean Talon") == "Hi Jean Talon!");
  BOOST_TEST(test_double_string_morphism(2.998) == "Value : 2.998000, ");
  BOOST_TEST(transform_text("Hi there") == "Hello there");
  BOOST_TEST(another_string_morphism("Hi Hi Hi you")("42") ==
    "Hello Hello Hello you");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(UnitReturnsConstantFunctions)
{
  {
    auto unit_component = unit<double, TestEnvironment>(69.0);
    BOOST_TEST(unit_component(initial_test_environment) == 69.0);
  }
  {
    auto unit_component = unit<double, TestEnvironment>(1.616);
    BOOST_TEST(unit_component(initial_test_environment) == 1.616);
  }
  // Example from https://nbviewer.jupyter.org/github/dbrattli/OSlash/blob/master/notebooks/Reader.ipynb
  {
    auto unit_component = unit<int, std::string>(42);
    BOOST_TEST(unit_component("Ignored") == 42);
  }
  {
    auto unit_component = unit<std::string, std::string>("Hello there");
    BOOST_TEST(unit_component("Bonjour") == "Hello there");
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(LambdaVersionOfUnitReturnsConstantFunctions)
{
  const auto unit_component = return_(10.0);
  BOOST_TEST(unit_component(initial_test_environment) == 10.0);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(AskReturnsEnvironmentAsIdentity)
{
  const auto result = ask(initial_test_environment);
  BOOST_TEST(!result.yes_);
  BOOST_TEST(result.x_ == 42.0);
  BOOST_TEST(result.s_ == "Start");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ApplyMorphismReturnsValueOfTypeX)
{
  {
    const auto result = apply_morphism(r, initial_test_environment);
    BOOST_TEST(result == 0.0);
  }
  {
    const auto result = apply_morphism(r, test_environment);
    BOOST_TEST(result == 1.616);
  }
}

BOOST_AUTO_TEST_SUITE_END() // Morphisms_tests

BOOST_AUTO_TEST_SUITE(Bind_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TestingStepsInBind)
{
  const std::string test_environment_string {"Sieur de la Salle"};
  {
    auto f_on_X =
      another_string_morphism(test_string_morphism(test_environment_string));
    BOOST_TEST(f_on_X(test_environment_string) == "Hello Sieur de la Salle!");
  }
  {
    auto f_on_X = another_string_morphism(
      apply_morphism(test_string_morphism, test_environment_string));

    BOOST_TEST(apply_morphism(f_on_X, test_environment_string) ==
      "Hello Sieur de la Salle!");
  }
}

// TODO: Make bind work with templates.
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BindWorksOnStringMorphisms)
{
  auto result = bind(test_string_morphism, another_string_morphism);
  //result("Le Royer");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(MultiplicationEndomorphismComposedWorksOnStringMorphisms)
{
  // \mu_Y \circ Tf : [E, X] \to [E, Y]
  // another_string_morphism = f: std::string -> [str, str]
  // test_string_morphism, [E,X], (str -> str)
  // test_string_morphism is a typical morphism. But, f, or another_string_
  // morphism is how to "configure" the output behavior of test_string_morphism.

  auto mu_Tf = MultiplicationEndomorphismComposed{another_string_morphism};
  auto result = mu_Tf(test_string_morphism);
  BOOST_TEST(result("Le Royer") == "Hello Le Royer!");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BindAsLambdaWorksOnStringMorphisms)
{
  auto result = bind_(test_string_morphism, another_string_morphism);

  BOOST_TEST(result("Le Royer") == "Hello Le Royer!");
}

BOOST_AUTO_TEST_SUITE_END() // Bind_tests

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TemporaryDirectoryAndFStreamMakesReadWriteEnvironment)
{
  {
    std::filesystem::path temporary_path {
      std::filesystem::temp_directory_path()};

    std::string temporary_directory {"TestValues"};

    std::string temporary_file_name {"TestValue"};
    //std::filesystem::path new_path {temporary_path /= temporary_file_name};
    std::filesystem::path new_path {temporary_path /= temporary_directory};

    //std::cout << temporary_file_name << " " << new_path << "\n";

    std::ofstream ofs {new_path / temporary_file_name, std::ios::binary};

    ofs << "1" << "\n";

    //ofs.close();

    // read back
    std::ifstream istream {new_path / temporary_file_name, std::ios::binary};

    std::string s;

    // Doesnt work.
    //istream >> s;

    std::getline(istream, s);

    //std::cout << " s : " << s << std::endl;
    //int s_int {std::stoi(s)};

    //BOOST_TEST(s_int == 1);

    //BOOST_TEST(s == "1");

    // Remove the directory and its contents.
    std::filesystem::remove_all(new_path);
  }
  {
    std::ofstream ofs {"TestValueTest", std::ios::binary};
    ofs << "1" << "\n";
    ofs.close();

    std::ifstream istream {"TestValueTest", std::ios::binary};

    std::string s;

    std::getline(istream, s);

    //istream >> s;

    //std::cout << " s : " << s << "\n";

    int s_int {std::stoi(s)};

    BOOST_TEST(s_int == 1);
  }
  {

    std::filesystem::path temporary_directory {"TestValues"};
    std::filesystem::create_directory(temporary_directory);

    std::string temporary_file_name {"TestValue"};
    std::filesystem::path new_path {temporary_directory / temporary_file_name};

    //std::cout << new_path << "\n";

    std::ofstream ofs {new_path, std::ios::binary};
    ofs << "1" << "\n";
    ofs.close();

    std::ifstream istream {new_path, std::ios::binary};

    std::string s;

    std::getline(istream, s);

    BOOST_TEST((!s.empty()));

    //std::cout << " s : " << s << "\n";

    int s_int {std::stoi(s)};

    BOOST_TEST(s_int == 1);

    istream.close();

    ofs.open(new_path, std::ios::binary);

    ofs.seekp(0, std::ios::beg);
    ofs << "0" << "\n";
    ofs.close();

    istream.open(new_path, std::ios::binary);

    istream.seekg(0, std::ios::beg);

    std::getline(istream, s);

    s_int = std::stoi(s);

    BOOST_TEST(s_int == 0);

    std::filesystem::remove_all(temporary_directory);
  }
  {
    TemporaryDirectory temp_dir {"TestValues"};
    auto temp_file = temp_dir.temporary_file("TestValue");

    set_fake_state(temp_file.filename_path(), 1);

    auto fake_state = read_fake_state(temp_file.filename_path());

    BOOST_TEST(static_cast<bool>(fake_state));
    BOOST_TEST(*fake_state == 1);

    set_fake_state(temp_file.filename_path(), 0);

    fake_state = read_fake_state(temp_file.filename_path());

    BOOST_TEST(static_cast<bool>(fake_state));
    BOOST_TEST(*fake_state == 0);

  }
}

BOOST_AUTO_TEST_SUITE_END() // ReaderMonad_tests
BOOST_AUTO_TEST_SUITE_END() // Monads
BOOST_AUTO_TEST_SUITE_END() // Categories


#include <pokerstove/peval/Card.h>
#include <pokerstove/peval/CardSet.h>
#include <pokerstove/util/combinations.h>
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <boost/shared_ptr.hpp>
#include <iostream>
#include <string>
#include <vector>

using namespace std;
namespace po = boost::program_options;
using namespace pokerstove;

#if 0
std::set<CardSet>
expandRankSet(size_t numCards)
{
    combinations cards(52,numCards);
    do
    {
        CardSet hand;
        for (size_t i=0; i<num_cards; i++)
        {
            hand.insert (Card(cards[i]));
        }
        collection.insert(hand.canonize());
        rankHands[hand.rankstr()] = hand.rankColex();
    }
    while (cards.next());
}
#endif

int main(int argc, char** argv) {
  try {
    // set up the program options, handle the help case, and extract the
    // values
    po::options_description desc(
        "pscolex, a utility which prints all combinations\n"
        "of poker hands, using canonical suits, or only ranks\n");

    // clang-format off
    desc.add_options()
        ("help,?", "produce help message")
        ("num-cards,n", po::value<size_t>()->default_value(2),
            "number of cards in hands")
        ("ranks", "print the set of rank values");
    // clang-format on

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv)
                  .style(po::command_line_style::unix_style)
                  .options(desc)
                  .run(),
              vm);
    po::notify(vm);

    // check for help
    if (vm.count("help") || argc == 1) {
      cout << desc << endl;
      return 1;
    }

    // extract the options
    size_t num_cards = vm["num-cards"].as<size_t>();

    set<CardSet> canonicalHands;
    map<string, size_t> rankHands;
    combinations cards(52, num_cards);
    do {
      CardSet hand;
      for (size_t i = 0; i < num_cards; i++) {
        hand.insert(Card(cards[i]));
      }
      canonicalHands.insert(hand.canonize());
      rankHands[hand.rankstr()] = hand.rankColex();
    } while (cards.next());

    if (vm.count("ranks") > 0) {
      for (auto it = rankHands.begin(); it != rankHands.end(); it++)
        cout << boost::format("%s: %d\n") % it->first % it->second;
    } else {
      for (auto it = canonicalHands.begin(); it != canonicalHands.end(); it++)
        cout << boost::format("%s: %d\n") % it->str() % it->colex();
    }
  } catch (std::exception& e) {
    cerr << "-- caught exception--\n" << e.what() << "\n";
    return 1;
  } catch (...) {
    cerr << "Exception of unknown type!\n";
    return 1;
  }
  return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/matrix/element_matrix.hpp>
#include <boost/numeric/mtl/matrix/element_array.hpp>
#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/operation/print.hpp>

using namespace std;

template <typename Matrix>
void test(Matrix& matrix, const char* name)
{
    cout << "\n" << name << "\n";

    using mtl::mat::inserter;
    typedef typename mtl::Collection<Matrix>::value_type value_type;

    mtl::compressed2D<double> block(2, 2);
    {
	inserter<mtl::compressed2D<double> > ins(block);
	ins[0][1] << 2.0;
	ins[1][0] << 6.0;
    }

    set_to_zero(matrix); // dense matrices are not automatically set to zero
    {
	inserter<Matrix, mtl::operations::update_plus<value_type> > ins(matrix);

	ins[0][0] << 1.0; ins[0][2] << 7.0;
	ins[1][3] << 3.0;
	ins[2][4] << 4.0;
	ins[3][1] << 5.0;

	ins[1][2] << block;
    }

    cout << "Filled matrix:\n" << matrix << "\n";
    MTL_THROW_IF(matrix[0][1] != 0.0, mtl::runtime_error("wrong zero-element"));
    MTL_THROW_IF(matrix[0][0] != 1.0, mtl::runtime_error("wrong insertion (single value)"));
    MTL_THROW_IF(matrix[1][3] != 5.0, mtl::runtime_error("wrong summation"));
    MTL_THROW_IF(matrix[2][2] != 6.0, mtl::runtime_error("wrong insertion (single value)"));
}

int main(int, char**)
{
    using namespace mtl;
    unsigned size= 5;

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    morton_dense<double, recursion::morton_z_mask>       mzd(size, size);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(size, size);
    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<complex<double> >                            drc(size, size);
    compressed2D<complex<double> >                       crc(size, size);

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(drc, "Dense row major complex");
    test(crc, "Compressed row major complex");
    test(cc, "Compressed column major");

    return 0;
}


// Copyright (C) 2020 T. Zachary Laine
//
// 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)
//[ roman_numeral_example
#include <boost/parser/parser.hpp>

#include <iostream>
#include <string>

namespace bp = boost::parser;

int main()
{
    std::cout << "Enter a number using Roman numerals. ";
    std::string input;
    std::getline(std::cin, input);

    //[ roman_numeral_symbol_tables
    bp::symbols<int> const ones = {
        {"I", 1},
        {"II", 2},
        {"III", 3},
        {"IV", 4},
        {"V", 5},
        {"VI", 6},
        {"VII", 7},
        {"VIII", 8},
        {"IX", 9}};

    bp::symbols<int> const tens = {
        {"X", 10},
        {"XX", 20},
        {"XXX", 30},
        {"XL", 40},
        {"L", 50},
        {"LX", 60},
        {"LXX", 70},
        {"LXXX", 80},
        {"XC", 90}};

    bp::symbols<int> const hundreds = {
        {"C", 100},
        {"CC", 200},
        {"CCC", 300},
        {"CD", 400},
        {"D", 500},
        {"DC", 600},
        {"DCC", 700},
        {"DCCC", 800},
        {"CM", 900}};
    //]

    //[ roman_numeral_actions
    int result = 0;
    auto const add_1000 = [&result](auto & ctx) { result += 1000; };
    auto const add = [&result](auto & ctx) { result += _attr(ctx); };
    //]

    //[ roman_numeral_parser
    using namespace bp::literals;
    auto const parser =
        *'M'_l[add_1000] >> -hundreds[add] >> -tens[add] >> -ones[add];
    //]

    if (bp::parse(input, parser) && result != 0)
        std::cout << "That's " << result << " in Arabic numerals.\n";
    else
        std::cout << "That's not a Roman number.\n";
}
//]


#pragma once

#include <boost/graph/directed_graph.hpp>
#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/bipartite.hpp>

namespace taiton {

template<class Graph>
using vertex_t = typename boost::graph_traits<Graph>::vertex_descriptor;

template<class Graph>
using edge_t = typename boost::graph_traits<Graph>::edge_descriptor;

template<class Graph>
void get_distance(const Graph& g,std::vector<int>& dist,typename Graph::vertex_descriptor s = 0){
        boost::breadth_first_search(g,s,boost::visitor(boost::make_bfs_visitor(boost::record_distances(dist.data(),boost::on_tree_edge{}))));
}

}


// Copyright 2015-2019 Hans Dembinski
//
// 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)

#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/axis/ostream.hpp>
#include <boost/histogram/axis/regular.hpp>
#include <limits>
#include <sstream>
#include <type_traits>
#include "is_close.hpp"
#include "std_ostream.hpp"
#include "throw_exception.hpp"
#include "utility_axis.hpp"
#include "utility_str.hpp"

int main() {
  using namespace boost::histogram;
  using def = use_default;
  namespace tr = axis::transform;
  namespace op = axis::option;

  BOOST_TEST(std::is_nothrow_move_assignable<axis::regular<>>::value);
  BOOST_TEST(std::is_nothrow_move_constructible<axis::regular<>>::value);

  // bad_ctors
  {
    BOOST_TEST_THROWS(axis::regular<>(1, 0, 0), std::invalid_argument);
    BOOST_TEST_THROWS(axis::regular<>(0, 0, 1), std::invalid_argument);
  }

  // ctors and assignment
  {
    axis::regular<> a{4, -2, 2};
    axis::regular<> b;
    BOOST_TEST_NE(a, b);
    b = a;
    BOOST_TEST_EQ(a, b);
    axis::regular<> c = std::move(b);
    BOOST_TEST_EQ(c, a);
    axis::regular<> d;
    BOOST_TEST_NE(c, d);
    d = std::move(c);
    BOOST_TEST_EQ(d, a);
  }

  // input, output
  {
    axis::regular<> a{4, -2, 2, "foo"};
    BOOST_TEST_EQ(a.metadata(), "foo");
    const auto& cref = a;
    BOOST_TEST_EQ(cref.metadata(), "foo");
    cref.metadata() = "bar"; // this is allowed
    BOOST_TEST_EQ(cref.metadata(), "bar");
    BOOST_TEST_EQ(a.value(0), -2);
    BOOST_TEST_EQ(a.value(1), -1);
    BOOST_TEST_EQ(a.value(2), 0);
    BOOST_TEST_EQ(a.value(3), 1);
    BOOST_TEST_EQ(a.value(4), 2);
    BOOST_TEST_EQ(a.bin(-1).lower(), -std::numeric_limits<double>::infinity());
    BOOST_TEST_EQ(a.bin(-1).upper(), -2);
    BOOST_TEST_EQ(a.bin(a.size()).lower(), 2);
    BOOST_TEST_EQ(a.bin(a.size()).upper(), std::numeric_limits<double>::infinity());
    BOOST_TEST_EQ(a.index(-10.), -1);
    BOOST_TEST_EQ(a.index(-2.1), -1);
    BOOST_TEST_EQ(a.index(-2.0), 0);
    BOOST_TEST_EQ(a.index(-1.1), 0);
    BOOST_TEST_EQ(a.index(0.0), 2);
    BOOST_TEST_EQ(a.index(0.9), 2);
    BOOST_TEST_EQ(a.index(1.0), 3);
    BOOST_TEST_EQ(a.index(10.), 4);
    BOOST_TEST_EQ(a.index(-std::numeric_limits<double>::infinity()), -1);
    BOOST_TEST_EQ(a.index(std::numeric_limits<double>::infinity()), 4);
    BOOST_TEST_EQ(a.index(std::numeric_limits<double>::quiet_NaN()), 4);

    BOOST_TEST_EQ(str(a),
                  "regular(4, -2, 2, metadata=\"bar\", options=underflow | overflow)");
  }

  // with inverted range
  {
    axis::regular<> a{2, 1, -2};
    BOOST_TEST_EQ(a.bin(-1).lower(), std::numeric_limits<double>::infinity());
    BOOST_TEST_EQ(a.bin(0).lower(), 1);
    BOOST_TEST_EQ(a.bin(1).lower(), -0.5);
    BOOST_TEST_EQ(a.bin(2).lower(), -2);
    BOOST_TEST_EQ(a.bin(2).upper(), -std::numeric_limits<double>::infinity());
    BOOST_TEST_EQ(a.index(2), -1);
    BOOST_TEST_EQ(a.index(1.001), -1);
    BOOST_TEST_EQ(a.index(1), 0);
    BOOST_TEST_EQ(a.index(0), 0);
    BOOST_TEST_EQ(a.index(-0.499), 0);
    BOOST_TEST_EQ(a.index(-0.5), 1);
    BOOST_TEST_EQ(a.index(-1), 1);
    BOOST_TEST_EQ(a.index(-2), 2);
    BOOST_TEST_EQ(a.index(-20), 2);
  }

  // with log transform
  {
    auto a = axis::regular<double, tr::log>{2, 1e0, 1e2};
    BOOST_TEST_EQ(a.bin(-1).lower(), 0.0);
    BOOST_TEST_IS_CLOSE(a.bin(0).lower(), 1.0, 1e-9);
    BOOST_TEST_IS_CLOSE(a.bin(1).lower(), 10.0, 1e-9);
    BOOST_TEST_IS_CLOSE(a.bin(2).lower(), 100.0, 1e-9);
    BOOST_TEST_EQ(a.bin(2).upper(), std::numeric_limits<double>::infinity());

    BOOST_TEST_EQ(a.index(-1), 2); // produces NaN in conversion
    BOOST_TEST_EQ(a.index(0), -1);
    BOOST_TEST_EQ(a.index(1), 0);
    BOOST_TEST_EQ(a.index(9), 0);
    BOOST_TEST_EQ(a.index(10), 1);
    BOOST_TEST_EQ(a.index(90), 1);
    BOOST_TEST_EQ(a.index(100), 2);
    BOOST_TEST_EQ(a.index(std::numeric_limits<double>::infinity()), 2);

    BOOST_TEST_THROWS((axis::regular<double, tr::log>{2, -1, 0}), std::invalid_argument);

    BOOST_TEST_CSTR_EQ(
        str(a).c_str(),
        "regular(transform::log{}, 2, 1, 100, options=underflow | overflow)");
  }

  // with sqrt transform
  {
    axis::regular<double, tr::sqrt> a(2, 0, 4);
    // this is weird, but -inf * -inf = inf, thus the lower bound
    BOOST_TEST_EQ(a.bin(-1).lower(), std::numeric_limits<double>::infinity());
    BOOST_TEST_IS_CLOSE(a.bin(0).lower(), 0.0, 1e-9);
    BOOST_TEST_IS_CLOSE(a.bin(1).lower(), 1.0, 1e-9);
    BOOST_TEST_IS_CLOSE(a.bin(2).lower(), 4.0, 1e-9);
    BOOST_TEST_EQ(a.bin(2).upper(), std::numeric_limits<double>::infinity());

    BOOST_TEST_EQ(a.index(-1), 2); // produces NaN in conversion
    BOOST_TEST_EQ(a.index(0), 0);
    BOOST_TEST_EQ(a.index(0.99), 0);
    BOOST_TEST_EQ(a.index(1), 1);
    BOOST_TEST_EQ(a.index(3.99), 1);
    BOOST_TEST_EQ(a.index(4), 2);
    BOOST_TEST_EQ(a.index(100), 2);
    BOOST_TEST_EQ(a.index(std::numeric_limits<double>::infinity()), 2);

    BOOST_TEST_EQ(str(a),
                  "regular(transform::sqrt{}, 2, 0, 4, options=underflow | overflow)");
  }

  // with pow transform
  {
    axis::regular<double, tr::pow> a(tr::pow{0.5}, 2, 0, 4);
    // this is weird, but -inf * -inf = inf, thus the lower bound
    BOOST_TEST_EQ(a.bin(-1).lower(), std::numeric_limits<double>::infinity());
    BOOST_TEST_IS_CLOSE(a.bin(0).lower(), 0.0, 1e-9);
    BOOST_TEST_IS_CLOSE(a.bin(1).lower(), 1.0, 1e-9);
    BOOST_TEST_IS_CLOSE(a.bin(2).lower(), 4.0, 1e-9);
    BOOST_TEST_EQ(a.bin(2).upper(), std::numeric_limits<double>::infinity());

    BOOST_TEST_EQ(a.index(-1), 2); // produces NaN in conversion
    BOOST_TEST_EQ(a.index(0), 0);
    BOOST_TEST_EQ(a.index(0.99), 0);
    BOOST_TEST_EQ(a.index(1), 1);
    BOOST_TEST_EQ(a.index(3.99), 1);
    BOOST_TEST_EQ(a.index(4), 2);
    BOOST_TEST_EQ(a.index(100), 2);
    BOOST_TEST_EQ(a.index(std::numeric_limits<double>::infinity()), 2);

    BOOST_TEST_EQ(str(a),
                  "regular(transform::pow{0.5}, 2, 0, 4, options=underflow | overflow)");
  }

  // with step
  {
    axis::regular<> a(axis::step(0.5), 1, 3);
    BOOST_TEST_EQ(a.size(), 4);
    BOOST_TEST_EQ(a.bin(-1).lower(), -std::numeric_limits<double>::infinity());
    BOOST_TEST_EQ(a.value(0), 1);
    BOOST_TEST_EQ(a.value(1), 1.5);
    BOOST_TEST_EQ(a.value(2), 2);
    BOOST_TEST_EQ(a.value(3), 2.5);
    BOOST_TEST_EQ(a.value(4), 3);
    BOOST_TEST_EQ(a.bin(4).upper(), std::numeric_limits<double>::infinity());

    axis::regular<> b(axis::step(0.5), 1, 3.1);
    BOOST_TEST_EQ(a, b);
  }

  // with circular option
  {
    axis::circular<> a{4, 0, 1};
    BOOST_TEST_EQ(a.bin(-1).lower(), a.bin(a.size() - 1).lower() - 1);
    BOOST_TEST_EQ(a.index(-1.0 * 3), 0);
    BOOST_TEST_EQ(a.index(0.0), 0);
    BOOST_TEST_EQ(a.index(0.25), 1);
    BOOST_TEST_EQ(a.index(0.5), 2);
    BOOST_TEST_EQ(a.index(0.75), 3);
    BOOST_TEST_EQ(a.index(1.0), 0);
    BOOST_TEST_EQ(a.index(std::numeric_limits<double>::infinity()), 4);
    BOOST_TEST_EQ(a.index(-std::numeric_limits<double>::infinity()), 4);
    BOOST_TEST_EQ(a.index(std::numeric_limits<double>::quiet_NaN()), 4);
  }

  // with growth
  {
    using pii_t = std::pair<axis::index_type, axis::index_type>;
    axis::regular<double, def, def, op::growth_t> a{1, 0, 1};
    BOOST_TEST_EQ(a.size(), 1);
    BOOST_TEST_EQ(a.update(0), pii_t(0, 0));
    BOOST_TEST_EQ(a.size(), 1);
    BOOST_TEST_EQ(a.update(1), pii_t(1, -1));
    BOOST_TEST_EQ(a.size(), 2);
    BOOST_TEST_EQ(a.value(0), 0);
    BOOST_TEST_EQ(a.value(2), 2);
    BOOST_TEST_EQ(a.update(-1), pii_t(0, 1));
    BOOST_TEST_EQ(a.size(), 3);
    BOOST_TEST_EQ(a.value(0), -1);
    BOOST_TEST_EQ(a.value(3), 2);
    BOOST_TEST_EQ(a.update(-10), pii_t(0, 9));
    BOOST_TEST_EQ(a.size(), 12);
    BOOST_TEST_EQ(a.value(0), -10);
    BOOST_TEST_EQ(a.value(12), 2);
    BOOST_TEST_EQ(a.update(std::numeric_limits<double>::infinity()), pii_t(a.size(), 0));
    BOOST_TEST_EQ(a.update(std::numeric_limits<double>::quiet_NaN()), pii_t(a.size(), 0));
    BOOST_TEST_EQ(a.update(-std::numeric_limits<double>::infinity()), pii_t(-1, 0));
  }

  // axis with overflow bin represents open interval
  {
    axis::regular<double, def, def, op::overflow_t> a{2, 0, 1};
    BOOST_TEST_EQ(a.index(0), 0);
    BOOST_TEST_EQ(a.index(0.49), 0);
    BOOST_TEST_EQ(a.index(0.50), 1);
    BOOST_TEST_EQ(a.index(0.99), 1);
    BOOST_TEST_EQ(a.index(1), 2);   // overflow bin
    BOOST_TEST_EQ(a.index(1.1), 2); // overflow bin
  }

  // axis without overflow bin represents a closed interval
  {
    axis::regular<double, def, def, op::none_t> a{2, 0, 1};
    BOOST_TEST_EQ(a.index(0), 0);
    BOOST_TEST_EQ(a.index(0.49), 0);
    BOOST_TEST_EQ(a.index(0.50), 1);
    BOOST_TEST_EQ(a.index(0.99), 1);
    BOOST_TEST_EQ(a.index(1), 1);   // last ordinary bin
    BOOST_TEST_EQ(a.index(1.1), 2); // out of range
  }

  // iterators
  {
    test_axis_iterator(axis::regular<>(5, 0, 1), 0, 5);
    test_axis_iterator(axis::regular<double, def, def, op::none_t>(5, 0, 1), 0, 5);
    test_axis_iterator(axis::circular<>(5, 0, 1), 0, 5);
  }

  // bin_type streamable
  {
    auto test = [](const auto& x, const char* ref) {
      std::ostringstream os;
      os << x;
      BOOST_TEST_EQ(os.str(), std::string(ref));
    };

    auto a = axis::regular<>(2, 0, 1);
    test(a.bin(0), "[0, 0.5)");
  }

  // null_type streamable
  {
    auto a = axis::regular<float, def, axis::null_type>(2, 0, 1);
    BOOST_TEST_EQ(str(a), "regular(2, 0, 1, options=underflow | overflow)");
  }

  // shrink and rebin
  {
    using A = axis::regular<>;
    auto a = A(5, 0, 5);
    auto b = A(a, 1, 4, 1);
    BOOST_TEST_EQ(b.size(), 3);
    BOOST_TEST_EQ(b.value(0), 1);
    BOOST_TEST_EQ(b.value(3), 4);
    auto c = A(a, 0, 4, 2);
    BOOST_TEST_EQ(c.size(), 2);
    BOOST_TEST_EQ(c.value(0), 0);
    BOOST_TEST_EQ(c.value(2), 4);
    auto e = A(a, 1, 5, 2);
    BOOST_TEST_EQ(e.size(), 2);
    BOOST_TEST_EQ(e.value(0), 1);
    BOOST_TEST_EQ(e.value(2), 5);
  }

  // shrink and rebin with circular option
  {
    using A = axis::circular<>;
    auto a = A(4, 1, 5);
    BOOST_TEST_THROWS(A(a, 1, 4, 1), std::invalid_argument);
    BOOST_TEST_THROWS(A(a, 0, 3, 1), std::invalid_argument);
    auto b = A(a, 0, 4, 2);
    BOOST_TEST_EQ(b.size(), 2);
    BOOST_TEST_EQ(b.value(0), 1);
    BOOST_TEST_EQ(b.value(2), 5);
  }

  return boost::report_errors();
}


#pragma once

#include <array>
#include <tuple>
#include <utility>

#include <Eigen/Core>

namespace test_util {
namespace internal {
template <std::size_t CurDim_, std::size_t N_>
struct LinspaceImpl {
    template <typename Func>
    static void apply(const std::array<int, N_>& counts, Eigen::Matrix<double, N_, 1>& val, Func func) {
        for (int i = 0; i < counts[CurDim_]; ++i) {
            val[CurDim_] = double(i) / (counts[CurDim_] - 1);
            LinspaceImpl<CurDim_ + 1, N_>::apply(counts, val, func);
        }
    }
};

template <std::size_t N_>
struct LinspaceImpl<N_, N_> {
    template <typename Func>
    static void apply(const std::array<int, N_>& /*counts*/, Eigen::Matrix<double, N_, 1>& val, Func func) {
        func(val);
    }
};
} // namespace internal

template <std::size_t N, typename Func>
void linspace(const std::array<int, N>& counts, Func func) {
    Eigen::Matrix<double, N, 1> val;
    internal::LinspaceImpl<0, N>::apply(counts, val, func);
}

namespace internal {
template <class F, class Tuple, std::size_t... I>
constexpr decltype(auto) applyImpl(F&& f, Tuple&& t, std::index_sequence<I...> /*indexSequence*/) {
    return f(std::get<I>(std::forward<Tuple>(t))...);
}
} // namespace internal

template <class F, class Tuple>
constexpr decltype(auto) apply(F&& f, Tuple&& t) {
    return internal::applyImpl(std::forward<F>(f),
                               std::forward<Tuple>(t),
                               std::make_index_sequence<std::tuple_size<std::remove_reference_t<Tuple>>::value>{});
}

namespace internal {
template <typename T, size_t N, size_t... Is>
inline auto asTuple(std::array<T, N> const& arr, std::index_sequence<Is...> /*indexSequence*/) {
    return std::make_tuple(arr[Is]...);
}
} // namespace internal

template <typename T, size_t N>
inline auto asTuple(std::array<T, N> const& arr) {
    return internal::asTuple(arr, std::make_index_sequence<N>{});
}

} // namespace test_util


#include "aikido/trajectory/BSpline.hpp"

#include <sstream>

#include <Eigen/Core>

#include "aikido/common/StepSequence.hpp"
#include "aikido/common/memory.hpp"

namespace aikido {
namespace trajectory {

namespace {

//==============================================================================
void throwIfInvalidIndex(
    const std::vector<BSpline::SplineType>& splines, std::size_t index)
{
  if (index >= splines.size())
  {
    std::stringstream ss;
    ss << "The index '" << index << "' is out of the range [0, "
       << splines.size() << "].\n";
    throw std::invalid_argument(ss.str());
  }
}

} // namespace

//==============================================================================
BSpline::BSpline(
    statespace::ConstStateSpacePtr stateSpace,
    const BSpline::KnotVectorType& knots,
    const BSpline::ControlPointVectorType& controlPoints)
  : mStateSpace(std::move(stateSpace))
{
  if (mStateSpace == nullptr)
  {
    throw std::invalid_argument("StateSpace is null.");
  }

  if (knots.size() < 3)
  {
    std::stringstream ss;
    ss << "Number of knots '" << knots.size() << "' should be at least two.";
    throw std::invalid_argument(ss.str());
  }

  if (controlPoints.size() < 1)
  {
    std::stringstream ss;
    ss << "Number of control points '" << knots.size()
       << "' should be at least one.";
    throw std::invalid_argument(ss.str());
  }

  if (knots.size() - controlPoints.size() - 1 < 0)
  {
    std::stringstream ss;
    ss << "Degree '" << knots.size() - controlPoints.size() - 1
       << "' (# of knots ' " << knots.size() << "' - # of control points '"
       << controlPoints.size() << "' - 1) should be at least zero.";
    throw std::invalid_argument(ss.str());
  }

  mStartTime = knots[0];
  mEndTime = knots[knots.size() - 1];

  mSplines.reserve(mStateSpace->getDimension());
  for (auto i = 0u; i < mStateSpace->getDimension(); ++i)
    mSplines.emplace_back(knots, controlPoints);
}

//==============================================================================
BSpline::BSpline(
    statespace::ConstStateSpacePtr stateSpace,
    std::size_t degree,
    const BSpline::ControlPointVectorType& controlPoints,
    double startTime,
    double endTime)
  : BSpline(
        std::move(stateSpace),
        computeUniformKnots(
            degree,
            static_cast<std::size_t>(controlPoints.size()),
            startTime,
            endTime),
        controlPoints)
{
  if (static_cast<std::size_t>(controlPoints.size()) <= degree)
  {
    std::stringstream ss;
    ss << "The number of control points '" << controlPoints.size()
       << "' should be greater than the degree '" << degree << "'.";
    throw std::invalid_argument(ss.str());
  }
}

//==============================================================================
BSpline::BSpline(
    statespace::ConstStateSpacePtr stateSpace,
    std::size_t degree,
    std::size_t numControlPoints,
    double startTime,
    double endTime)
  : BSpline(
        std::move(stateSpace),
        degree,
        ControlPointVectorType::Zero(
            static_cast<ControlPointVectorType::Index>(numControlPoints)),
        startTime,
        endTime)
{
  if (startTime >= endTime)
  {
    std::stringstream ss;
    ss << "The start-time '" << startTime << "' should be less than the "
       << "end-time '" << endTime << "'.";
    throw std::invalid_argument(ss.str());
  }
}

//==============================================================================
BSpline::BSpline(const BSpline& other)
  : mStateSpace(other.mStateSpace)
  , mSplines(other.mSplines)
  , mStartTime(other.mStartTime)
  , mEndTime(other.mEndTime)
{
  // Do nothing
}

//==============================================================================
BSpline::BSpline(BSpline&& other)
  : mStateSpace(std::move(other.mStateSpace))
  , mSplines(std::move(other.mSplines))
  , mStartTime(std::move(other.mStartTime))
  , mEndTime(std::move(other.mEndTime))
{
  // Do nothing
}

//==============================================================================
BSpline::~BSpline()
{
  // Do nothing
}

//==============================================================================
BSpline& BSpline::operator=(const BSpline& other)
{
  BSpline newBSpline(other);
  *this = std::move(newBSpline);

  return *this;
}

//==============================================================================
BSpline& BSpline::operator=(BSpline&& other)
{
  mStateSpace = std::move(other.mStateSpace);
  mSplines = std::move(other.mSplines);
  mStartTime = std::move(other.mStartTime);
  mEndTime = std::move(other.mEndTime);

  return *this;
}

//==============================================================================
std::unique_ptr<Trajectory> BSpline::clone() const
{
  return ::aikido::common::make_unique<BSpline>(*this);
}

//==============================================================================
std::size_t BSpline::getDegree() const
{
  if (mStateSpace->getDimension() == 0u)
    return 0; // TODO: Better idea?

  assert(!mSplines.empty());

  return static_cast<std::size_t>(mSplines[0].degree());
}

//==============================================================================
std::size_t BSpline::getOrder() const
{
  return getDegree() + 1u;
}

//==============================================================================
std::size_t BSpline::getNumKnots() const
{
  if (mStateSpace->getDimension() == 0u)
    return 0; // TODO: Better idea?

  assert(!mSplines.empty());

  return static_cast<std::size_t>(mSplines[0].knots().size());
}

//==============================================================================
std::size_t BSpline::getNumControlPoints() const
{
  if (mStateSpace->getDimension() == 0u)
    return 0; // TODO: Better idea?

  assert(!mSplines.empty());

  return static_cast<std::size_t>(mSplines[0].ctrls().size());
}

//==============================================================================
void BSpline::setStartPoint(std::size_t stateSpaceIndex, double value)
{
  throwIfInvalidIndex(mSplines, stateSpaceIndex);

  mSplines[stateSpaceIndex].ctrls()[0] = value;
}

//==============================================================================
void BSpline::setStartPoint(const Eigen::VectorXd& point)
{
  const auto stateSpaceDim = mStateSpace->getDimension();

  if (stateSpaceDim != static_cast<std::size_t>(point.size()))
    throw std::invalid_argument("Invalid dimension");

  for (auto i = 0u; i < stateSpaceDim; ++i)
    mSplines[i].ctrls()[0] = point[i];
}

//==============================================================================
void BSpline::setStartPoint(const statespace::StateSpace::State* state)
{
  Eigen::VectorXd values;
  mStateSpace->logMap(state, values);
  setStartPoint(values);
}

//==============================================================================
void BSpline::setEndPoint(std::size_t stateSpaceIndex, double value)
{
  throwIfInvalidIndex(mSplines, stateSpaceIndex);

  const std::size_t numCtrPts = getNumControlPoints();

  if (numCtrPts == 0)
    return;

  mSplines[stateSpaceIndex].ctrls()[static_cast<int>(numCtrPts) - 1] = value;
}

//==============================================================================
void BSpline::setEndPoint(const Eigen::VectorXd& point)
{
  const std::size_t numCtrPts = getNumControlPoints();

  if (numCtrPts == 0)
    return;

  const auto stateSpaceDim = mStateSpace->getDimension();

  if (stateSpaceDim != static_cast<std::size_t>(point.size()))
    throw std::invalid_argument("Invalid dimension");

  for (auto i = 0u; i < stateSpaceDim; ++i)
    mSplines[i].ctrls()[static_cast<int>(numCtrPts) - 1] = point[i];
}

//==============================================================================
void BSpline::setEndPoint(const statespace::StateSpace::State* state)
{
  if (getNumControlPoints() == 0)
    return;

  Eigen::VectorXd values;
  mStateSpace->logMap(state, values);
  setEndPoint(values);
}

//==============================================================================
void BSpline::setControlPoints(
    std::size_t stateSpaceIndex,
    const BSpline::ControlPointVectorType& controlPoints,
    bool withStartControlPoint,
    bool withEndControlPoint)
{
  throwIfInvalidIndex(mSplines, stateSpaceIndex);

  const auto numControlPoints
      = static_cast<ControlPointVectorType::Index>(getNumControlPoints());

  if (numControlPoints == 0)
    return;

  if (withStartControlPoint)
  {
    if (withEndControlPoint)
    {
      if (controlPoints.size() != numControlPoints)
        throw std::invalid_argument("Invalid number of control points");

      mSplines[stateSpaceIndex].ctrls() = controlPoints;
    }
    else
    {
      if (controlPoints.size() - 1 != numControlPoints)
        throw std::invalid_argument("Invalid number of control points");

      mSplines[stateSpaceIndex].ctrls().head(numControlPoints - 1u)
          = controlPoints;
    }
  }
  else
  {
    if (withEndControlPoint)
    {
      if (controlPoints.size() - 1 != numControlPoints)
        throw std::invalid_argument("Invalid number of control points");

      mSplines[stateSpaceIndex].ctrls().tail(numControlPoints - 1u)
          = controlPoints;
    }
    else
    {
      if (controlPoints.size() - 2 != numControlPoints)
        throw std::invalid_argument("Invalid number of control points");

      mSplines[stateSpaceIndex].ctrls().segment(1, numControlPoints - 2u)
          = controlPoints;
    }
  }
}

//==============================================================================
void BSpline::setControlPoints(
    std::size_t stateSpaceIndex,
    double value,
    bool withStartControlPoint,
    bool withEndControlPoint)
{
  throwIfInvalidIndex(mSplines, stateSpaceIndex);

  const auto numControlPoints
      = static_cast<ControlPointVectorType::Index>(getNumControlPoints());

  if (numControlPoints == 0)
    return;

  if (withStartControlPoint)
  {
    if (withEndControlPoint)
    {
      mSplines[stateSpaceIndex].ctrls().setConstant(value);
    }
    else
    {
      mSplines[stateSpaceIndex]
          .ctrls()
          .head(numControlPoints - 1)
          .setConstant(value);
    }
  }
  else
  {
    if (withEndControlPoint)
    {
      mSplines[stateSpaceIndex]
          .ctrls()
          .tail(numControlPoints - 1)
          .setConstant(value);
    }
    else
    {
      mSplines[stateSpaceIndex]
          .ctrls()
          .segment(1, numControlPoints - 2)
          .setConstant(value);
    }
  }
}

//==============================================================================
const BSpline::ControlPointVectorType& BSpline::getControlPoints(
    std::size_t stateSpaceIndex) const
{
  throwIfInvalidIndex(mSplines, stateSpaceIndex);

  return mSplines[stateSpaceIndex].ctrls();
}

//==============================================================================
BSpline::ControlPointVectorType BSpline::getControlPoints(
    std::size_t stateSpaceIndex,
    bool withStartControlPoint,
    bool withEndControlPoint) const
{
  throwIfInvalidIndex(mSplines, stateSpaceIndex);

  const auto numControlPoints
      = static_cast<ControlPointVectorType::Index>(getNumControlPoints());

  if (withStartControlPoint)
  {
    if (withEndControlPoint)
      return getControlPoints(stateSpaceIndex);
    else
      return mSplines[stateSpaceIndex].ctrls().head(numControlPoints - 1);
  }
  else
  {
    if (withEndControlPoint)
      return mSplines[stateSpaceIndex].ctrls().tail(numControlPoints - 1);
    else
      return mSplines[stateSpaceIndex].ctrls().segment(1, numControlPoints - 2);
  }
}

//==============================================================================
statespace::ConstStateSpacePtr BSpline::getStateSpace() const
{
  return mStateSpace;
}

//==============================================================================
std::size_t BSpline::getNumDerivatives() const
{
  return getDegree();
}

//==============================================================================
double BSpline::getStartTime() const
{
  return mStartTime;
}

//==============================================================================
double BSpline::getEndTime() const
{
  return mEndTime;
}

//==============================================================================
double BSpline::getDuration() const
{
  return mEndTime - mStartTime;
}

//==============================================================================
void throwIfInvalidTime(const BSpline& spline, double t)
{
  const auto startTime = spline.getStartTime();
  const auto endTime = spline.getEndTime();

  if (t < startTime || endTime < t)
  {
    std::stringstream ss;
    ss << "The evaluating time '" << t << "' is out of the duration ["
       << startTime << ", " << endTime << "], which isn't allowed.";
    throw std::invalid_argument(ss.str());
  }
}

//==============================================================================
void BSpline::evaluate(double t, statespace::StateSpace::State* state) const
{
  throwIfInvalidTime(*this, t);

  Eigen::VectorXd values(mStateSpace->getDimension());

  for (auto i = 0u; i < mStateSpace->getDimension(); ++i)
    values[i] = mSplines[i](t)[0];

  mStateSpace->expMap(values, state);
}

//==============================================================================
void BSpline::evaluateDerivative(
    double t, int derivative, Eigen::VectorXd& /*tangentVector*/) const
{
  throwIfInvalidTime(*this, t);

  if (derivative < 1)
    throw std::invalid_argument("Derivative must be positive.");

  // TODO(JS): Not implemented
}

//==============================================================================
double BSpline::computeArcLength(
    const distance::DistanceMetric& distanceMetric, double resolution) const
{
  const common::StepSequence steps(
      resolution, true, true, getStartTime(), getEndTime());

  if (steps.getLength() == 0u)
    return 0.0;

  auto state0 = mStateSpace->createState();
  auto state1 = mStateSpace->createState();

  double arcLength = 0.0;

  auto it = steps.begin();
  evaluate(*it, state0);
  for (; it != steps.end(); ++it)
  {
    evaluate(*it, state1);
    arcLength += distanceMetric.distance(state0, state1);

    mStateSpace->copyState(state1, state0);
  }

  return arcLength;
}

//==============================================================================
std::size_t BSpline::computeNumKnots(
    std::size_t degree, std::size_t numControlPoints)
{
  return degree + numControlPoints + 1u;
}

//==============================================================================
BSpline::KnotVectorType BSpline::computeUniformKnots(
    std::size_t degree,
    std::size_t numControlPoints,
    double startTime,
    double endTime)
{
  if (numControlPoints <= degree)
  {
    throw std::invalid_argument(
        "Number of control points should be greater than degree");
  }

  const auto numKnots = computeNumKnots(degree, numControlPoints);

  KnotVectorType knots(numKnots);
  knots.head(static_cast<ControlPointVectorType::Index>(degree))
      .setConstant(startTime);
  knots.tail(static_cast<ControlPointVectorType::Index>(degree))
      .setConstant(endTime);

  const common::StepSequence internalKnots(
      startTime, endTime, numControlPoints + 1u - degree, true);
  ControlPointVectorType::Index index = static_cast<int>(degree);
  assert(
      degree + internalKnots.getLength() + degree
      == static_cast<std::size_t>(knots.size()));
  for (double knot : internalKnots)
    knots[index++] = knot;

  return knots;
}

} // namespace trajectory
} // namespace aikido


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/algorithm/count_if.hpp>
#include <numeric>
#include <vector>
#include <algorithm>
#include <simd_test.hpp>

using namespace boost::simd;
using namespace boost::alignment;

STF_CASE_TPL( "Check unary simd::count_if", STF_NUMERIC_TYPES )
{
  namespace bs = boost::simd;
  static const int N = pack<T>::static_size;

  std::vector<T> values(2*N+1);
  std::iota(values.begin(), values.end(),T(1));

  auto c = std::count_if(values.begin(), values.end(), bs::is_odd);

  STF_EQUAL ( (boost::simd::count_if(values.data(), values.data()+2*N+1, bs::is_odd))
            , c
            );

}


/**
 * Copyright 2021 Huawei Technologies Co., Ltd
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "backend/kernel_compiler/cpu/eigen/eigh_cpu_kernel.h"
#include <Eigen/Eigenvalues>
#include <type_traits>
#include "utils/ms_utils.h"

namespace mindspore {
namespace kernel {

namespace {
constexpr size_t kInputsNum = 2;
constexpr size_t kOutputsNum = 2;
constexpr size_t kDefaultShape = 1;
constexpr auto kAMatrixDimNum = 2;

}  // namespace
using Eigen::Dynamic;
using Eigen::EigenSolver;
using Eigen::Lower;
using Eigen::Map;
using Eigen::MatrixBase;
using Eigen::RowMajor;
using Eigen::Upper;

template <typename T>
using MatrixSquare = Eigen::Matrix<T, Dynamic, Dynamic, RowMajor>;

template <typename T>
using ComplexMatrixSquare = Eigen::Matrix<std::complex<T>, Dynamic, Dynamic, RowMajor>;

template <typename T>
void EighCPUKernel<T>::InitKernel(const CNodePtr &kernel_node) {
  dtype_ = AnfAlgo::GetInputDeviceDataType(kernel_node, 0);

  compute_eigen_vectors = AnfAlgo::GetNodeAttr<bool>(kernel_node, C_EIEH_VECTOR);

  auto A_shape = AnfAlgo::GetPrevNodeOutputInferShape(kernel_node, 0);
  CHECK_KERNEL_INPUTS_NUM(A_shape.size(), kAMatrixDimNum, AnfAlgo::GetCNodeName(kernel_node));

  if (A_shape.size() != kShape2dDims || A_shape[0] != A_shape[1]) {
    MS_LOG(EXCEPTION) << "wrong array shape, A should be a matrix, but got [" << A_shape[0] << " X " << A_shape[1]
                      << "]";
  }
  m_ = A_shape[0];
}

template <typename T>
void EighCPUKernel<T>::InitInputOutputSize(const CNodePtr &kernel_node) {
  CPUKernel::InitInputOutputSize(kernel_node);
  (void)workspace_size_list_.template emplace_back(m_ * m_ * sizeof(T));
}

template <typename T>
bool SolveSelfAdjointMatrix(const Map<MatrixSquare<T>> &A, Map<MatrixSquare<T>> *output, Map<MatrixSquare<T>> *outputv,
                            bool compute_eigen_vectors) {
  Eigen::SelfAdjointEigenSolver<MatrixSquare<T>> solver(A);
  output->noalias() = solver.eigenvalues();
  if (compute_eigen_vectors) {
    outputv->noalias() = solver.eigenvectors();
  }
  return true;
}

template <typename T>
bool SolveComplexMatrix(const Map<MatrixSquare<T>> &A, Map<MatrixSquare<T>> *output, Map<MatrixSquare<T>> *outputv,
                        bool compute_eigen_vectors) {
  Eigen::ComplexEigenSolver<MatrixSquare<T>> solver(A);
  output->noalias() = solver.eigenvalues();
  if (compute_eigen_vectors) {
    outputv->noalias() = solver.eigenvectors();
  }
  return true;
}

template <typename T>
bool EighCPUKernel<T>::Launch(const std::vector<AddressPtr> &inputs, const std::vector<AddressPtr> &workspace,
                              const std::vector<AddressPtr> &outputs) {
  CHECK_KERNEL_INPUTS_NUM(inputs.size(), kInputsNum, kernel_name_);
  CHECK_KERNEL_OUTPUTS_NUM(outputs.size(), kOutputsNum, kernel_name_);

  auto A_addr = reinterpret_cast<T *>(inputs[0]->addr);
  // is the Matrix a symmetric matrix(0, all, general matxi, -1 lower triangle, 1 upper triangle)
  auto symmetric_type = reinterpret_cast<bool *>(inputs[1]->addr);
  auto output_addr = reinterpret_cast<T *>(outputs[0]->addr);
  auto output_v_addr = reinterpret_cast<T *>(outputs[1]->addr);
  Map<MatrixSquare<T>> A(A_addr, m_, m_);
  Map<MatrixSquare<T>> A_(A_addr, m_, m_);
  Map<MatrixSquare<T>> output(output_addr, m_, 1);
  Map<MatrixSquare<T>> outputv(output_v_addr, m_, m_);
  // selfadjoint matrix
  if (*symmetric_type) {
    A_ = A.template selfadjointView<Lower>();
  } else {
    A_ = A.template selfadjointView<Upper>();
  }
  // Real scalar eigen solver
  if constexpr (std::is_same_v<T, float>) {
    SolveSelfAdjointMatrix(A_, &output, &outputv, compute_eigen_vectors);
  } else if constexpr (std::is_same_v<T, double>) {
    SolveSelfAdjointMatrix(A_, &output, &outputv, compute_eigen_vectors);
  } else {
    // complex eigen solver
    SolveComplexMatrix(A_, &output, &outputv, compute_eigen_vectors);
  }
  return true;
}
}  // namespace kernel
}  // namespace mindspore


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"  // Precompiled headers

#include <mrpt/bayes/CParticleFilterCapable.h>
#include <mrpt/math/distributions.h>
#include <mrpt/math/matrix_serialization.h>
#include <mrpt/poses/CPointPDFSOG.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/os.h>
#include <Eigen/Dense>

using namespace mrpt::poses;
using namespace mrpt::math;

using namespace mrpt::bayes;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CPointPDFSOG, CPosePDF, mrpt::poses)

/*---------------------------------------------------------------
	Constructor
  ---------------------------------------------------------------*/
CPointPDFSOG::CPointPDFSOG(size_t nModes) : m_modes(nModes) {}
/*---------------------------------------------------------------
			clear
  ---------------------------------------------------------------*/
void CPointPDFSOG::clear() { m_modes.clear(); }
/*---------------------------------------------------------------
	Resize
  ---------------------------------------------------------------*/
void CPointPDFSOG::resize(const size_t N) { m_modes.resize(N); }
/*---------------------------------------------------------------
						getMean
  Returns an estimate of the pose, (the mean, or mathematical expectation of the
 PDF)
 ---------------------------------------------------------------*/
void CPointPDFSOG::getMean(CPoint3D& p) const
{
	size_t N = m_modes.size();
	double X = 0, Y = 0, Z = 0;

	if (N)
	{
		CListGaussianModes::const_iterator it;
		double sumW = 0;

		for (it = m_modes.begin(); it != m_modes.end(); ++it)
		{
			double w;
			sumW += w = exp(it->log_w);
			X += it->val.mean.x() * w;
			Y += it->val.mean.y() * w;
			Z += it->val.mean.z() * w;
		}
		if (sumW > 0)
		{
			X /= sumW;
			Y /= sumW;
			Z /= sumW;
		}
	}

	p.x(X);
	p.y(Y);
	p.z(Z);
}

std::tuple<CMatrixDouble33, CPoint3D> CPointPDFSOG::getCovarianceAndMean() const
{
	size_t N = m_modes.size();

	CMatrixDouble33 estCov;
	CPoint3D p;
	getMean(p);
	estCov.setZero();

	if (N)
	{
		// 1) Get the mean:
		double sumW = 0;
		auto estMean = CMatrixDouble31(p);

		CListGaussianModes::const_iterator it;

		for (it = m_modes.begin(); it != m_modes.end(); ++it)
		{
			double w;
			sumW += w = exp(it->log_w);

			auto estMean_i = CMatrixDouble31(it->val.mean);
			estMean_i -= estMean;

			auto partCov =
				CMatrixDouble33(estMean_i.asEigen() * estMean_i.transpose());
			partCov += it->val.cov;
			partCov *= w;
			estCov += partCov;
		}

		if (sumW != 0) estCov *= (1.0 / sumW);
	}

	return {estCov, p};
}

uint8_t CPointPDFSOG::serializeGetVersion() const { return 1; }
void CPointPDFSOG::serializeTo(mrpt::serialization::CArchive& out) const
{
	uint32_t N = m_modes.size();
	out << N;
	for (const auto& m : m_modes)
	{
		out << m.log_w;
		out << m.val.mean;
		mrpt::math::serializeSymmetricMatrixTo(m.val.cov, out);
	}
}
void CPointPDFSOG::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		{
			uint32_t N;
			in >> N;
			this->resize(N);
			for (auto& m : m_modes)
			{
				in >> m.log_w;

				// In version 0, weights were linear!!
				if (version == 0) m.log_w = log(max(1e-300, m.log_w));

				in >> m.val.mean;
				mrpt::math::deserializeSymmetricMatrixFrom(m.val.cov, in);
			}
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

void CPointPDFSOG::copyFrom(const CPointPDF& o)
{
	MRPT_START

	if (this == &o) return;  // It may be used sometimes

	if (o.GetRuntimeClass() == CLASS_ID(CPointPDFSOG))
	{
		m_modes = dynamic_cast<const CPointPDFSOG*>(&o)->m_modes;
	}
	else
	{
		// Approximate as a mono-modal gaussian pdf:
		this->resize(1);
		m_modes[0].log_w = 0;
		o.getCovarianceAndMean(m_modes[0].val.cov, m_modes[0].val.mean);
	}

	MRPT_END
}

/*---------------------------------------------------------------
						saveToTextFile
  ---------------------------------------------------------------*/
bool CPointPDFSOG::saveToTextFile(const std::string& file) const
{
	FILE* f = os::fopen(file.c_str(), "wt");
	if (!f) return false;

	for (const auto& m_mode : m_modes)
		os::fprintf(
			f, "%e %e %e %e %e %e %e %e %e %e\n", exp(m_mode.log_w),
			m_mode.val.mean.x(), m_mode.val.mean.y(), m_mode.val.mean.z(),
			m_mode.val.cov(0, 0), m_mode.val.cov(1, 1), m_mode.val.cov(2, 2),
			m_mode.val.cov(0, 1), m_mode.val.cov(0, 2), m_mode.val.cov(1, 2));
	os::fclose(f);
	return true;
}

/*---------------------------------------------------------------
						changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointPDFSOG::changeCoordinatesReference(const CPose3D& newReferenceBase)
{
	for (auto& m : m_modes) m.val.changeCoordinatesReference(newReferenceBase);
}

/*---------------------------------------------------------------
					drawSingleSample
 ---------------------------------------------------------------*/
void CPointPDFSOG::drawSingleSample(CPoint3D& outSample) const
{
	MRPT_START

	ASSERT_(m_modes.size() > 0);

	// 1st: Select a mode with a probability proportional to its weight:
	vector<double> logWeights(m_modes.size());
	vector<size_t> outIdxs;
	vector<double>::iterator itW;
	CListGaussianModes::const_iterator it;
	for (it = m_modes.begin(), itW = logWeights.begin(); it != m_modes.end();
		 ++it, ++itW)
		*itW = it->log_w;

	CParticleFilterCapable::computeResampling(
		CParticleFilter::prMultinomial,  // Resampling algorithm
		logWeights,  // input: log weights
		outIdxs  // output: indexes
	);

	// we need just one: take the first (arbitrary)
	size_t selectedIdx = outIdxs[0];
	ASSERT_(selectedIdx < m_modes.size());
	const CPointPDFGaussian* selMode = &m_modes[selectedIdx].val;

	// 2nd: Draw a position from the selected Gaussian:
	CVectorDouble vec;
	getRandomGenerator().drawGaussianMultivariate(vec, selMode->cov);

	ASSERT_(vec.size() == 3);
	outSample.x(selMode->mean.x() + vec[0]);
	outSample.y(selMode->mean.y() + vec[1]);
	outSample.z(selMode->mean.z() + vec[2]);

	MRPT_END
}

/*---------------------------------------------------------------
					bayesianFusion
 ---------------------------------------------------------------*/
void CPointPDFSOG::bayesianFusion(
	const CPointPDF& p1_, const CPointPDF& p2_,
	const double minMahalanobisDistToDrop)
{
	MRPT_START

	// p1: CPointPDFSOG, p2: CPosePDFGaussian:

	ASSERT_(p1_.GetRuntimeClass() == CLASS_ID(CPointPDFSOG));
	ASSERT_(p2_.GetRuntimeClass() == CLASS_ID(CPointPDFSOG));

	const auto* p1 = dynamic_cast<const CPointPDFSOG*>(&p1_);
	const auto* p2 = dynamic_cast<const CPointPDFSOG*>(&p2_);

	// Compute the new kernel means, covariances, and weights after multiplying
	// to the Gaussian "p2":
	CPointPDFGaussian auxGaussianProduct, auxSOG_Kernel_i;

	float minMahalanobisDistToDrop2 = square(minMahalanobisDistToDrop);

	this->m_modes.clear();
	bool is2D =
		false;  // to detect & avoid errors in 3x3 matrix inversions of range=2.

	for (const auto& m : p1->m_modes)
	{
		CMatrixDouble33 c = m.val.cov;

		// Is a 2D covariance??
		if (c(2, 2) == 0)
		{
			is2D = true;
			c(2, 2) = 1;
		}

		ASSERT_(c(0, 0) != 0 && c(0, 0) != 0);

		const CMatrixDouble33 covInv = c.inverse_LLt();

		Eigen::Vector3d eta = covInv * CMatrixDouble31(m.val.mean);

		// Normal distribution canonical form constant:
		// See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
		double a = -0.5 * (3 * log(M_2PI) - log(covInv.det()) +
						   (eta.transpose() * c.asEigen() * eta)(0, 0));

		for (const auto& m2 : p2->m_modes)
		{
			auxSOG_Kernel_i = m2.val;
			if (auxSOG_Kernel_i.cov(2, 2) == 0)
			{
				auxSOG_Kernel_i.cov(2, 2) = 1;
				is2D = true;
			}
			ASSERT_(
				auxSOG_Kernel_i.cov(0, 0) > 0 && auxSOG_Kernel_i.cov(1, 1) > 0);

			// Should we drop this product term??
			bool reallyComputeThisOne = true;
			if (minMahalanobisDistToDrop > 0)
			{
				// Approximate (fast) mahalanobis distance (square):
				float mahaDist2;

				float stdX2 = max(auxSOG_Kernel_i.cov(0, 0), m.val.cov(0, 0));
				mahaDist2 =
					square(auxSOG_Kernel_i.mean.x() - m.val.mean.x()) / stdX2;

				float stdY2 = max(auxSOG_Kernel_i.cov(1, 1), m.val.cov(1, 1));
				mahaDist2 +=
					square(auxSOG_Kernel_i.mean.y() - m.val.mean.y()) / stdY2;

				if (!is2D)
				{
					float stdZ2 =
						max(auxSOG_Kernel_i.cov(2, 2), m.val.cov(2, 2));
					mahaDist2 +=
						square(auxSOG_Kernel_i.mean.z() - m.val.mean.z()) /
						stdZ2;
				}

				reallyComputeThisOne = mahaDist2 < minMahalanobisDistToDrop2;
			}

			if (reallyComputeThisOne)
			{
				auxGaussianProduct.bayesianFusion(auxSOG_Kernel_i, m.val);

				// ----------------------------------------------------------------------
				// The new weight is given by:
				//
				//   w'_i = w_i * exp( a + a_i - a' )
				//
				//      a = -1/2 ( dimensionality * log(2pi) - log(det(Cov^-1))
				//      + (Cov^-1 * mu)^t * Cov^-1 * (Cov^-1 * mu) )
				//
				// ----------------------------------------------------------------------
				TGaussianMode newKernel;

				newKernel.val = auxGaussianProduct;  // Copy mean & cov

				CMatrixDouble33 covInv_i = auxSOG_Kernel_i.cov.inverse_LLt();
				Eigen::Vector3d eta_i =
					CMatrixDouble31(auxSOG_Kernel_i.mean).asEigen();
				eta_i = covInv_i.asEigen() * eta_i;

				CMatrixDouble33 new_covInv_i = newKernel.val.cov.inverse_LLt();
				Eigen::Vector3d new_eta_i =
					CMatrixDouble31(newKernel.val.mean).asEigen();
				new_eta_i = new_covInv_i.asEigen() * new_eta_i;

				double a_i =
					-0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
							(eta_i.transpose() * auxSOG_Kernel_i.cov.asEigen() *
							 eta_i)(0, 0));
				double new_a_i =
					-0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
							(new_eta_i.transpose() *
							 newKernel.val.cov.asEigen() * new_eta_i)(0, 0));

				newKernel.log_w = m.log_w + m2.log_w + a + a_i - new_a_i;

				// Fix 2D case:
				if (is2D) newKernel.val.cov(2, 2) = 0;

				// Add to the results (in "this") the new kernel:
				this->m_modes.push_back(newKernel);
			}  // end if reallyComputeThisOne
		}  // end for it2

	}  // end for it1

	normalizeWeights();

	MRPT_END
}

/*---------------------------------------------------------------
						enforceCovSymmetry
 ---------------------------------------------------------------*/
void CPointPDFSOG::enforceCovSymmetry()
{
	MRPT_START
	// Differences, when they exist, appear in the ~15'th significant
	//  digit, so... just take one of them arbitrarily!
	for (auto& m_mode : m_modes)
	{
		m_mode.val.cov(0, 1) = m_mode.val.cov(1, 0);
		m_mode.val.cov(0, 2) = m_mode.val.cov(2, 0);
		m_mode.val.cov(1, 2) = m_mode.val.cov(2, 1);
	}

	MRPT_END
}

/*---------------------------------------------------------------
						normalizeWeights
 ---------------------------------------------------------------*/
void CPointPDFSOG::normalizeWeights()
{
	MRPT_START

	if (!m_modes.size()) return;

	CListGaussianModes::iterator it;
	double maxW = m_modes[0].log_w;
	for (it = m_modes.begin(); it != m_modes.end(); ++it)
		maxW = max(maxW, it->log_w);

	for (it = m_modes.begin(); it != m_modes.end(); ++it) it->log_w -= maxW;

	MRPT_END
}

/*---------------------------------------------------------------
						ESS
 ---------------------------------------------------------------*/
double CPointPDFSOG::ESS() const
{
	MRPT_START
	CListGaussianModes::const_iterator it;
	double cum = 0;

	/* Sum of weights: */
	double sumLinearWeights = 0;
	for (it = m_modes.begin(); it != m_modes.end(); ++it)
		sumLinearWeights += exp(it->log_w);

	/* Compute ESS: */
	for (it = m_modes.begin(); it != m_modes.end(); ++it)
		cum += square(exp(it->log_w) / sumLinearWeights);

	if (cum == 0)
		return 0;
	else
		return 1.0 / (m_modes.size() * cum);
	MRPT_END
}

/*---------------------------------------------------------------
						evaluatePDFInArea
 ---------------------------------------------------------------*/
void CPointPDFSOG::evaluatePDFInArea(
	float x_min, float x_max, float y_min, float y_max, float resolutionXY,
	float z, CMatrixD& outMatrix, bool sumOverAllZs)
{
	MRPT_START

	ASSERT_(x_max > x_min);
	ASSERT_(y_max > y_min);
	ASSERT_(resolutionXY > 0);

	const auto Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
	const auto Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
	outMatrix.setSize(Ny, Nx);

	for (size_t i = 0; i < Ny; i++)
	{
		const float y = y_min + i * resolutionXY;
		for (size_t j = 0; j < Nx; j++)
		{
			float x = x_min + j * resolutionXY;
			outMatrix(i, j) = evaluatePDF(CPoint3D(x, y, z), sumOverAllZs);
		}
	}

	MRPT_END
}

/*---------------------------------------------------------------
						evaluatePDF
 ---------------------------------------------------------------*/
double CPointPDFSOG::evaluatePDF(const CPoint3D& x, bool sumOverAllZs) const
{
	if (!sumOverAllZs)
	{
		// Normal evaluation:
		auto X = CMatrixDouble31(x);
		double ret = 0;

		CMatrixDouble31 MU;

		for (const auto& m_mode : m_modes)
		{
			MU = CMatrixDouble31(m_mode.val.mean);
			ret += exp(m_mode.log_w) * math::normalPDF(X, MU, m_mode.val.cov);
		}

		return ret;
	}
	else
	{
		// Only X,Y:
		CMatrixD X(2, 1), MU(2, 1), COV(2, 2);
		double ret = 0;

		X(0, 0) = x.x();
		X(1, 0) = x.y();

		for (const auto& m_mode : m_modes)
		{
			MU(0, 0) = m_mode.val.mean.x();
			MU(1, 0) = m_mode.val.mean.y();

			COV(0, 0) = m_mode.val.cov(0, 0);
			COV(1, 1) = m_mode.val.cov(1, 1);
			COV(0, 1) = COV(1, 0) = m_mode.val.cov(0, 1);

			ret += exp(m_mode.log_w) * math::normalPDF(X, MU, COV);
		}

		return ret;
	}
}

/*---------------------------------------------------------------
						getMostLikelyMode
 ---------------------------------------------------------------*/
void CPointPDFSOG::getMostLikelyMode(CPointPDFGaussian& outVal) const
{
	if (this->empty())
	{
		outVal = CPointPDFGaussian();
	}
	else
	{
		auto it_best = m_modes.end();
		for (auto it = m_modes.begin(); it != m_modes.end(); ++it)
			if (it_best == m_modes.end() || it->log_w > it_best->log_w)
				it_best = it;

		outVal = it_best->val;
	}
}

/*---------------------------------------------------------------
						getAs3DObject
 ---------------------------------------------------------------*/
// void  CPointPDFSOG::getAs3DObject( mrpt::opengl::CSetOfObjects::Ptr	&outObj
// )
// const
//{
//	// For each gaussian node
//	for (CListGaussianModes::const_iterator it = m_modes.begin(); it!=
// m_modes.end();++it)
//	{
//		opengl::CEllipsoid::Ptr obj =
// std::make_shared<opengl::CEllipsoid>();
//
//		obj->setPose( it->val.mean);
//		obj->setCovMatrix(it->val.cov,  it->val.cov(2,2)==0  ?  2:3);
//
//		obj->setQuantiles(3);
//		obj->enableDrawSolid3D(false);
//		obj->setColor(1,0,0, 0.5);
//
//		outObj->insert( obj );
//	} // end for each gaussian node
//}


#pragma once

#include <deal.II/base/numbers.h>
// // system includes --------------------------------------------------------
#include <cmath>

#include "base/numbers.hpp"
#include "enum/enum.hpp"
#include "moment_base.hpp"
#include "quadrature/qmidpoint.hpp"
#include "quadrature/quad_handler1d.hpp"
#include "quadrature/quadrature_handler.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"

namespace boltzmann {

class Energy : public MomentBase
{
 public:
  Energy() {}

  // ------------------------------------------------------------
  template <typename SPECTRAL_BASIS>
  Energy(const SPECTRAL_BASIS& spectral_basis)
  {
    init(spectral_basis);
  }

  // ------------------------------------------------------------
  template <typename SPECTRAL_BASIS>
  void init(const SPECTRAL_BASIS& spectral_basis);

  /**
   * @brief compute energy
   *
   * @param dst
   * @param src
   * @param count #physical dofs
   */
  void compute(double* dst, const double* src, int count) const;

  // ------------------------------------------------------------
  double compute(const double* src) const;

  // ------------------------------------------------------------
  template <typename VEC_IN, typename VEC_OUT, typename INDEXER>
  void compute(VEC_OUT& dst,
               const VEC_IN& src,
               const dealii::IndexSet& relevant_phys_dofs,
               const INDEXER& indexer) const
  {
    // some g++ cannot see this function inherited from MomentBase
    MomentBase::compute(dst, src, relevant_phys_dofs, indexer);
  }

 private:
  using MomentBase::entry_t;
  using MomentBase::n_velo_dofs;
  using MomentBase::contributions;
};

// ------------------------------------------------------------
template <typename SPECTRAL_BASIS>
void
Energy::init(const SPECTRAL_BASIS& spectral_basis)
{
  n_velo_dofs = spectral_basis.n_dofs();

  typedef typename std::tuple_element<1, typename SPECTRAL_BASIS::elem_t::container_t>::type
      radial_elem_t;

  typedef typename std::tuple_element<0, typename SPECTRAL_BASIS::elem_t::container_t>::type
      angular_elem_t;

  auto& QR_handler = QuadHandler<MaxwellQuadrature>::GetInstance();
  typedef QuadAdaptor<MaxwellQuadrature> qr_adapt;

  unsigned int nqR = spectral::get_max_k(spectral_basis) + 2;
  Eigen::VectorXd pts_qR(nqR);
  Eigen::VectorXd wts_qR(nqR);

  qr_adapt::apply(pts_qR.data(), wts_qR.data(), QR_handler.get(nqR), 0.5);

  typedef typename SPECTRAL_BASIS::elem_t elem_t;
  for (auto it = spectral_basis.begin(); it != spectral_basis.end(); ++it) {
    typename elem_t::Acc::template get<radial_elem_t> getter;
    typename elem_t::Acc::template get<angular_elem_t> geta;

    if (geta(*it).get_id().l != 0) continue;

    const auto& rr = getter(*it);
    const double w = rr.w();

    if (std::abs(w - 0.5) > 1e-10)
      throw std::runtime_error("Error: wrong element weight in Energy::init");

    double sum = 0;
    for (unsigned int q = 0; q < pts_qR.size(); ++q) {
      const double r = pts_qR[q];
      sum += rr.evaluate(r) * r * r * wts_qR[q];
    }
    const unsigned int i = it - spectral_basis.begin();
    contributions.push_back(std::make_pair(i, 2 * numbers::PI * sum));
  }
}

// ------------------------------------------------------------
void
Energy::compute(double* dst, const double* src, int count) const
{
#pragma omp parallel for
  for (int i = 0; i < count; ++i) {
    const double* local_src = src + i * n_velo_dofs;
    double sum = 0;
    for (unsigned int j = 0; j < contributions.size(); ++j) {
      sum += contributions[j].second * local_src[contributions[j].first];
    }
    dst[i] = sum;
  }
}

// ------------------------------------------------------------
double
Energy::compute(const double* src) const
{
  double sum = 0.0;
  for (unsigned int j = 0; j < contributions.size(); ++j) {
    sum += contributions[j].second * src[contributions[j].first];
  }
  return sum;
}

}  // end namespace boltzmann


//=======================================================================
// Copyright 2009 Trustees of Indiana University.
// Authors: Michael Hansen
//
// 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)
//=======================================================================

#include <iostream>
#include <map>
#include <vector>

#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/random.hpp>
#include <boost/test/minimal.hpp>
#include <boost/graph/iteration_macros.hpp>

#define INITIALIZE_VERTEX 0
#define DISCOVER_VERTEX 1
#define EXAMINE_VERTEX 2
#define EXAMINE_EDGE 3
#define EDGE_RELAXED 4
#define EDGE_NOT_RELAXED 5
#define FINISH_VERTEX 6

template <typename Graph>
void run_dijkstra_test(const Graph& graph)
{
  using namespace boost;

  // Set up property maps
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

  typedef typename std::map<vertex_t, vertex_t> vertex_map_t;
  typedef associative_property_map<vertex_map_t> predecessor_map_t;
  vertex_map_t default_vertex_map, no_color_map_vertex_map;
  predecessor_map_t default_predecessor_map(default_vertex_map),
                    no_color_map_predecessor_map(no_color_map_vertex_map);

  typedef typename std::map<vertex_t, double> vertex_double_map_t;
  typedef associative_property_map<vertex_double_map_t> distance_map_t;
  vertex_double_map_t default_vertex_double_map, no_color_map_vertex_double_map;
  distance_map_t default_distance_map(default_vertex_double_map),
                 no_color_map_distance_map(no_color_map_vertex_double_map);

  // Run dijkstra algoirthms
  dijkstra_shortest_paths(graph, vertex(0, graph),
                          predecessor_map(default_predecessor_map)
                          .distance_map(default_distance_map));

  dijkstra_shortest_paths_no_color_map(graph, vertex(0, graph),
                                       predecessor_map(no_color_map_predecessor_map)
                                       .distance_map(no_color_map_distance_map));

  // Verify that predecessor maps are equal
  BOOST_CHECK(std::equal(default_vertex_map.begin(), default_vertex_map.end(),
              no_color_map_vertex_map.begin()));

  // Verify that distance maps are equal
  BOOST_CHECK(std::equal(default_vertex_double_map.begin(), default_vertex_double_map.end(),
              no_color_map_vertex_double_map.begin()));
}

int test_main(int argc, char* argv[])
{
  using namespace boost;

  int vertices_to_create = 10;
  int edges_to_create = 500;
  std::size_t random_seed = time(0);

  if (argc > 1) {
    vertices_to_create = lexical_cast<int>(argv[1]);
  }

  if (argc > 2) {
    edges_to_create = lexical_cast<int>(argv[2]);
  }

  if (argc > 3) {
    random_seed = lexical_cast<std::size_t>(argv[3]);
  }

  minstd_rand generator(random_seed);

  // Set up graph
  typedef adjacency_list<listS, listS, directedS,
    property<vertex_index_t, int >,
    property<edge_weight_t, double> > graph_t;

  typedef graph_traits<graph_t>::vertex_descriptor vertex_t;
  typedef graph_traits<graph_t>::edge_descriptor edge_t;

  graph_t graph;
  generate_random_graph(graph, vertices_to_create, edges_to_create, generator);

  // Set up property maps
  typedef property_map<graph_t, vertex_index_t>::type index_map_t;
  index_map_t index_map = get(vertex_index, graph);
  int vertex_index = 0;

  BGL_FORALL_VERTICES(current_vertex, graph, graph_t) {
    put(index_map, current_vertex, vertex_index++);
  }

  typedef property_map<graph_t, edge_weight_t>::type weight_map_t;
  weight_map_t weight_map = get(edge_weight, graph);
  randomize_property<edge_weight_t>(graph, generator);

  // Run comparison test with original dijkstra_shortest_paths
  std::cout << "Running dijkstra shortest paths test with " << num_vertices(graph) <<
    " vertices and " << num_edges(graph) << " edges " << std::endl;

  run_dijkstra_test(graph);

  return 0;
}


#pragma once
/**

   @file FishSchool.hpp
   @brief Fish school simulator with deterministic motion equations.

*/

#include <atomic>
#include <chrono>
#include <cinttypes>
#include <memory>

#include <boost/statechart/detail/memory.hpp>
#include <boost/statechart/fifo_scheduler.hpp>
#include <boost/statechart/event_base.hpp>

#include <yaml-cpp/yaml.h>

#ifdef _MSC_VER
#pragma warning(push, 0)
#endif
#include <dds/pub/Publisher.hpp>
#include <dds/sub/Subscriber.hpp>
#ifdef _MSC_VER
#pragma warning(pop)
#endif

#include <mimir/IAlgorithm.hpp>

namespace mimir
{
  namespace algorithm
  {
    /**
       @brief Kinematic fish school simulation model.

       \rst

       This class implements a simple kinematic fish school, which can be controlled by
       its inputs. The fish school is modelled as a 5 degrees of freedom kinematic
       body. It has a three dimensional position represented in an inertial reference
       frame, \{NED\}, with axes pointing North, East, and Down, respectively. The body
       can rotate about its y-axis -- pitch: :math:`\theta`, and z-axis -- yaw:
       :math:`\psi`. The fish school is modeled as an under-actuated entity, meaning that
       it can only change forward speed (surge) and its angular velocities. The input to
       the simulation model are surge :math:`u`, rate of turn :math:`r`, and desired depth
       :math:`D_d`. The desired depth is achieved with a first order response using pitch
       angular velocity as manipulated variable. The commanded surge and rate of turn is
       immidiate, i.e. there is no dynamic response. Let us define some quantities to
       describe the fish school with an ordinary differential equation.

       .. math::
          :nowrap:

          \begin{align}
            \boldsymbol{p} = \begin{bmatrix}N\\E\\D\\\end{bmatrix} &\in \mathbb{R}^3 &\text{position}\\
            \boldsymbol{\Theta} = \begin{bmatrix}\theta\\\psi\end{bmatrix} &\in \mathbb{S}^2 &\text{attitude}\\
            \boldsymbol{v} = \begin{bmatrix}u\\v\\w\end{bmatrix} &\in \mathbb{R}^3 &\text{linear velocity}\\
            \boldsymbol{w} = \begin{bmatrix}p\\q\\r\\\end{bmatrix} &\in \mathbb{R}^3 &\text{angular velocity}\\
            D_d &\in \mathbb{R} &\text{desired depth} \\
            \boldsymbol{\eta}(t) = \begin{bmatrix}\boldsymbol{p}\\\Theta\end{bmatrix}, &\quad\mathbb{R} \to \mathbb{R}^3 \times \mathbb{S}^2&\text{state vector}
          \end{align}

       Let :math:`R_y(\theta), R_z(\psi) \in SO(3)` be rotation matrixes, so that
       :math:`R(\boldsymbol{\eta})` is the chained rotation defined as follows:

       .. math::
          :nowrap:

          \begin{align*}
            R_y(\theta) &= \left[\begin{array}{ccc}\cos(\theta)&0 &\sin(\theta)\\0&1&0\\-\sin(\theta)&0&\cos(\theta) \end{array} \right] \\
            R_z(\psi) &= \left[\begin{array}{ccc}\cos(\psi)&-\sin(\psi)&0\\\sin(\psi)&\cos(\psi)&0\\0&0&1 \end{array} \right]\\
            R(\boldsymbol{\eta}) &= R_z(\psi)R_y(\theta) \\
          \end{align*}

       We then use the angular velocity transformation :math:`T(\theta)` as defined in
       :cite:`Fossen2011` and the combined state vector transformation
       :math:`J(\boldsymbol{\eta})` for our 5-dimensional system becomes:

       .. math::
          :nowrap:

          \begin{align*}
            T(\theta) &= \left[\begin{array}{cc}1&0\\0& \frac{1}{\cos(\theta)}\end{array} \right] \\
            J(\boldsymbol{\eta}) &= \left[\begin{array}{cc}R(\boldsymbol{\eta})&0\\0& T(\theta)\end{array} \right] \\
          \end{align*}

       The depth response has a proportional feedback on depth error and a stabilizing
       term to level out the pitch: :math:`q_d = 0.1(D - D_d)\cos(2\theta) - \theta`. The
       resulting ordinary differential equation and input vector are, respectively,

       .. math::
          :nowrap:

          \begin{align*}
            \dat \eta(t) &= J(\boldsymbol{\eta}) \begin{bmatrix}v\\q_d\\q\end{bmatrix} \\
            u(t_d) &= \begin{bmatrix}v_d\\r_d\\Z_d\end{bmatrix},
          \end{align*}

       where :math:`t_d` indicates discrete time points indicated by ``time_step_ms`` in
       the YAML config.

       The table below describes the inputs, outputs and parameters of the
       algorithm. These variables are specified using the YAML configuration file defined
       further below.

       +----------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-----------------+
       | Name           | Symbol                     | Description                         | Causality     | Variability    | Default              | Unit            |
       +================+============================+=====================================+===============+================+======================+=================+
       | ``fish_ctrl``  | :math:`u`                  | Desired: Surge, rate of turn, depth | ``input``     | ``discrete``   | \                    | [m/s, rad/s, m] |
       +----------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-----------------+
       | ``kinematics`` | :math:`y`                  | ``fkin::Kinematics6D``              | ``output``    | ``continuous`` | \                    | \               |
       +----------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-----------------+
       | ``position``   | :math:`p_{p,0}`            | Initial; North, East, Down          | ``parameter`` | ``fixed``      | :math:`[0, 100, 50]` | m               |
       +----------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-----------------+
       | ``euler``      | :math:`[\theta_0, \psi_0]` | Initial; Pitch, Yaw                 | ``parameter`` | ``fixed``      | :math:`[0,0]`        | rad             |
       +----------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-----------------+

       .. warning::

          This numerical model is DEPRECATED in favor of a more detailed model with
          stochastic behavior defined using FMI :cite:`fmi2`. Nevertheless, it may serve
          as an example on how to implement a simple :cpp:class:`mimir::IAlgorithm`.

       \endrst

    */
    class FishSchool : public IAlgorithm
    {
    public:
      /**
         @brief FishSchool constructor.

         The constructor parses the specification from the given YAML
         node. It establishes data structures and sets up DDS readers
         and writers according to the input/output scheme of the
         algorithm. The following YAML code block shows the expected
         layout of the ``FishSchool`` map of a input config file.  The
         inputs, outputs, and initial conditions are communicated with
         DDS communication. Common is their DDS topic and DDS
         identifier. The specification is deemed self-explanatory.

         \rst

         .. code-block:: yaml

             time_step_ms: 200
             inputs:
               fish_ctrl:                # DDS type: fkin::IdVec3d
                 topic: fkinFishCtrl
                 id: Fish
                 max_age_ms: -1
             outputs:                    # DDS type: fkin::Kinematics6D
               kinematics:
                 topic: fkinKinematics6D
                 id: Fish
             initial_conditions:
               position:                 # DDS type: fkin::IdVec3d
                 topic: fkinPosition
                 id: FishInit
                 max_wait_ms: 80
                 fallback: [0, 100, 50]
               euler:                    # DDS type: fkin::IdVec3d
                 topic: fkinEuler
                 id: FishInit
                 max_wait_ms: 80
                 fallback: [0, 0, 0]

         \endrst

         @param [in] config YAML configuration from input file.
         @param [in] scheduler State machine scheduler, needed to post events to state machine.
         @param [in] machine State machine processor handle, needed to post events to state machine.
         @param publisher DDS data writer to send data.
         @param subscriber DDS data reader to receive data.
      */
      explicit FishSchool(
          const YAML::Node& config,
          boost::statechart::fifo_scheduler<>& scheduler,
          boost::statechart::fifo_scheduler<>::processor_handle machine,
          dds::pub::Publisher publisher,
          dds::sub::Subscriber subscriber);
      /// Destructor.
      virtual ~FishSchool();
      /**
         See base class for details.

         \rst
         The algorithm uses CVODES from SUNDIALS :cite:`Hindmarsh2005sundials`.
         \endrst
      */
      virtual void solve(const std::atomic<bool>& cancel_token);
      /// See base class.
      virtual void initialize(const std::atomic<bool>& cancel_token);
      /// See base class.
      virtual void timer(const std::atomic<bool>& cancel_token);
      /// Name identifier of algorithm.
      virtual inline const char* name() { return "FishSchool"; }
      /// Helper function that queues event to state machine.
      void event(boost::statechart::event_base * const event);

    private:
      /// March simulation time one time step ahead.
      inline void step_time() { m_next_step += m_time_step; }
      FishSchool() = delete;
      class Impl;
      std::unique_ptr<Impl> m_impl; ///< Holds the implementation of the algorithm.
      boost::statechart::fifo_scheduler<> & m_scheduler; ///< Members for state machine.
      boost::statechart::fifo_scheduler<>::processor_handle m_stateMachine; ///< Member for state machine.
      const std::chrono::milliseconds m_time_step; ///< Discrete time step.
      std::chrono::steady_clock::time_point m_next_step; ///< Simulation time point.
      const YAML::Node m_config; ///< YAML configuration for algorithm.
    };

  }
}


#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

// test that weak references work ok for various types of arrays
// including nonunit strides and strange orderings resulting from
// slicing.

#define test_expr(d1,d2) BZTEST(all((d1)==(d2)));

int main()
{
  Array<double, 2> a(10,10);
  Array<double, 3> aa(10,10,10);
  Array<double,2> b;
  Array<double,1> c;

  a=tensor::i+10*tensor::j;
  aa=tensor::i+10*tensor::j+100*tensor::k;

  b.weakReference(a);
  test_expr(a,b);
  c.weakReference(a(1,Range::all()));
  test_expr(a(1,Range::all()), c);
  c.weakReference(a(Range::all(),2));
  test_expr(a(Range::all(),2), c);
  a.weakReference(aa(Range(2,6), Range::all(), 7));
  test_expr(aa(Range(2,6), Range::all(), 7), a);

  return 0;
}


// Copyright (C) 2015 National ICT Australia (NICTA)
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
// -------------------------------------------------------------------
//
// Written by Conrad Sanderson - http://conradsanderson.id.au

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("init_fill_1")
  {
  mat Z( 5,  6, fill::zeros);
  mat O( 5,  6, fill::ones);
  mat I( 5,  6, fill::eye);
  mat U(50, 60, fill::randu);
  mat N(50, 60, fill::randn);

  REQUIRE( accu(Z != 0) == 0   );
  REQUIRE( accu(O != 0) == 5*6 );
  REQUIRE( accu(I != 0) == 5   );

  REQUIRE(   mean(vectorise(U)) == Approx(0.500).epsilon(0.05) );
  REQUIRE( stddev(vectorise(U)) == Approx(0.288).epsilon(0.05) );

  REQUIRE(   mean(vectorise(N)) == Approx(0.0).epsilon(0.05) );
  REQUIRE( stddev(vectorise(N)) == Approx(1.0).epsilon(0.05) );

  mat X(5, 6, fill::none);   // only to test instantiation
  }

TEST_CASE("init_fill_2")
  {
  cube Z( 5,  6, 2, fill::zeros);
  cube O( 5,  6, 2, fill::ones);
  cube U(50, 60, 2, fill::randu);
  cube N(50, 60, 2, fill::randn);

  REQUIRE( accu(Z != 0) == 0     );
  REQUIRE( accu(O != 0) == 5*6*2 );

  REQUIRE(   mean(vectorise(U)) == Approx(0.500).epsilon(0.05) );
  REQUIRE( stddev(vectorise(U)) == Approx(0.288).epsilon(0.05) );

  REQUIRE(   mean(vectorise(N)) == Approx(0.0).epsilon(0.05) );
  REQUIRE( stddev(vectorise(N)) == Approx(1.0).epsilon(0.05) );

  cube X(5, 6, 2, fill::none);   // only to test instantiation

  cube I;  REQUIRE_THROWS( I = cube(5, 6, 2, fill::eye) );
  }


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/constants/properties.hpp>
#include <nt2/include/constants/real_splat.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include "../constant.hpp"

 #ifdef BOOST_MSVC
  #pragma warning(disable: 4146)  // unary minus applied to unsigned
#endif

NT2_TEST_CASE_TPL(allbits_real, BOOST_SIMD_REAL_TYPES)
{
  NT2_TEST_EXPR_TYPE( (nt2::Allbits<T>())
                    , boost::mpl::_
                    , T
                    );

  NT2_TEST( check_is_nan(nt2::Allbits<T>()) );
}

NT2_TEST_CASE_TPL(allbits_unsigned, BOOST_SIMD_INTEGRAL_TYPES)
{
  NT2_CHECK_CONSTANT(Allbits , ~T(0UL) ,T);
}

NT2_TEST_CASE_TPL(bitincrement_real, BOOST_SIMD_REAL_TYPES)
{
  NT2_TEST_EXPR_TYPE( (nt2::Bitincrement<T>())
                    , boost::mpl::_
                    , T
                    );

  NT2_TEST_EQUAL( nt2::Bitincrement<T>()
                , (nt2::real_constant<T,1,1>())
                );
}

NT2_TEST_CASE_TPL(bitincrement_unsigned, BOOST_SIMD_INTEGRAL_TYPES)
{
  NT2_CHECK_CONSTANT(Bitincrement , T(1) ,T);
}

NT2_TEST_CASE(signmask_real)
{
  NT2_CHECK_CONSTANT(Signmask , -0. ,double);
  NT2_CHECK_CONSTANT(Signmask , -0.f,float);
}

NT2_TEST_CASE_TPL(signmask_unsigned, BOOST_SIMD_UNSIGNED_TYPES)
{
  NT2_CHECK_CONSTANT(Signmask , T(0) ,T);
}

NT2_TEST_CASE_TPL(signmask_signed, BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{
  NT2_CHECK_CONSTANT(Signmask, (T(1) << (CHAR_BIT*sizeof(T)-1)) ,T);
}

NT2_TEST_CASE(sqrtvalmax_real)
{
  NT2_CHECK_CONSTANT(Sqrtvalmax , 1.3407807929942600077e+154, double);
  NT2_CHECK_CONSTANT(Sqrtvalmax , 18446744073709551616.f    , float);
}

NT2_TEST_CASE_TPL(sqrtvalmax_unsigned, BOOST_SIMD_UNSIGNED_TYPES)
{
  NT2_CHECK_CONSTANT(Sqrtvalmax , T( (T(1) << (CHAR_BIT*sizeof(T)/2))-1) ,T);
}

NT2_TEST_CASE(sqrtvalmax_signed)
{
  NT2_CHECK_CONSTANT(Sqrtvalmax, 3037000499ll , nt2::int64_t);
  NT2_CHECK_CONSTANT(Sqrtvalmax,      46340   , nt2::int32_t);
  NT2_CHECK_CONSTANT(Sqrtvalmax,        181   , nt2::int16_t);
  NT2_CHECK_CONSTANT(Sqrtvalmax,         11   , nt2::int8_t );
}

NT2_TEST_CASE(valmax_real)
{
  NT2_CHECK_CONSTANT(Valmax , 1.7976931348623157e+308 , double);
  NT2_CHECK_CONSTANT(Valmax , float(3.4028235e+38)    , float);
}

NT2_TEST_CASE_TPL(valmax_unsigned, BOOST_SIMD_UNSIGNED_TYPES)
{
  NT2_CHECK_CONSTANT(Valmax , T(~(T(0))) ,T);
}

NT2_TEST_CASE_TPL(valmax_signed, BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{
  NT2_CHECK_CONSTANT(Valmax, ~(T(1) << (CHAR_BIT*sizeof(T)-1)),T);
}

NT2_TEST_CASE(valmin_real)
{
  NT2_CHECK_CONSTANT(Valmin , -1.7976931348623157e+308 , double);
  NT2_CHECK_CONSTANT(Valmin , float(-3.4028235e+38)    , float);
}

NT2_TEST_CASE_TPL(valmin_unsigned, BOOST_SIMD_UNSIGNED_TYPES)
{
  NT2_CHECK_CONSTANT(Valmin , T(0) ,T);
}

NT2_TEST_CASE_TPL(valmin_signed, BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{
  typedef typename boost::dispatch::meta::as_unsigned<T>::type u_t;

  NT2_CHECK_CONSTANT(Valmin, T( -u_t(T(1) << (CHAR_BIT*sizeof(T)-1)) ),T);
}

NT2_TEST_CASE_TPL(Maxleftshift, BOOST_SIMD_TYPES)
{
  using boost::dispatch::meta::as_integer;

  NT2_TEST_EXPR_TYPE( nt2::Maxleftshift<T>()
                    , boost::mpl::_
                    , (typename as_integer<T,signed>::type)
                    );

  NT2_TEST_EQUAL( nt2::Maxleftshift<T>()
                , (typename as_integer<T,signed>::type(CHAR_BIT*sizeof(T)-1))
                );
}

NT2_TEST_CASE(ieee)
{
  NT2_TEST_EQUAL( nt2::Nbmantissabits<double>() , 52);
  NT2_TEST_EQUAL( nt2::Nbmantissabits<float>()  , 23);

  NT2_TEST_EQUAL( nt2::Nbexponentbits<double>() , 11);
  NT2_TEST_EQUAL( nt2::Nbexponentbits<float>()  , 8  );

  NT2_TEST_EQUAL( nt2::Maxexponent<double>(), 1023  );
  NT2_TEST_EQUAL( nt2::Maxexponent<float>()  , 127    );

  NT2_TEST_EQUAL( nt2::Minexponent<double>(), -1022  );
  NT2_TEST_EQUAL( nt2::Minexponent<float>()  , -126  );

  NT2_TEST_EQUAL( nt2::Ldexpmask<double>(), 0x7FF0000000000000ll  );
  NT2_TEST_EQUAL( nt2::Ldexpmask<float>() , 0x7F800000            );

  NT2_TEST_EQUAL( nt2::Nbdigits<double>(), 53 );
  NT2_TEST_EQUAL( nt2::Nbdigits<float>() , 24 );
}

NT2_TEST_CASE_TPL(ieee_ints, BOOST_SIMD_INTEGRAL_TYPES)
{
  NT2_CHECK_CONSTANT(Nbmantissabits , T(sizeof(T)*CHAR_BIT) , T);
  NT2_CHECK_CONSTANT(Nbexponentbits , T(0)                  , T);
  NT2_CHECK_CONSTANT(Maxexponent    , T(0)                  , T);
  NT2_CHECK_CONSTANT(Minexponent    , T(0)                  , T);
  NT2_CHECK_CONSTANT(Ldexpmask      , T(0)                  , T);
  NT2_CHECK_CONSTANT(Nbdigits       , T(0)                  , T);
}