#include "catch.hpp"

#include "libirc/connectivity.h"

#include "config.h"
#include "libirc/atom.h"
#include "libirc/conversion.h"
#include "libirc/io.h"
#include "libirc/molecule.h"
#include "libirc/transformation.h"

#include <iostream>

#ifdef HAVE_ARMA
#include <armadillo>
using vec3 = arma::vec3;
using vec = arma::vec;
using mat = arma::mat;

template<typename T>
using Mat = arma::Mat<T>;
#elif HAVE_EIGEN3
#include <Eigen/Dense>
using vec3 = Eigen::Vector3d;
using vec = Eigen::VectorXd;
using mat = Eigen::MatrixXd;

template<typename T>
using Mat = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
#else
#error
#endif

using namespace irc;

// TODO (Peter) Extend for linear angles
template<typename Vector3, typename Vector, typename Matrix>
std::tuple<std::vector<connectivity::Bond>,
           std::vector<connectivity::Angle>,
           std::vector<connectivity::Dihedral>,
           std::vector<connectivity::LinearAngle<Vector3>>,
           std::vector<connectivity::OutOfPlaneBend>>
badla_from_molecule(const molecule::Molecule<Vector3>& mol) {

  using namespace connectivity;

  // Compute interatomic distance for formaldehyde molecule
  Matrix dd{distances<Vector3, Matrix>(mol)};

  // Build graph based on the adjacency matrix
  UGraph adj{adjacency_matrix(dd, mol)};

  // Compute distance matrix and predecessor matrix
  Matrix dist{distance_matrix<Matrix>(adj)};

  // Compute bonds
  std::vector<Bond> B{bonds(dist, mol)};

  // Compute angles
  std::vector<Angle> A{angles(dist, mol)};

  // Compute dihedral angles
  std::vector<Dihedral> D{dihedrals(dist, mol)};

  // Compute linear angles
  std::vector<LinearAngle<Vector3>> LA{linear_angles(dist, mol)};

  // Compute out of plane bends
  std::vector<OutOfPlaneBend> OoPB{out_of_plane_bends(dist, mol)};

  // Return bonds, angles and dihedral angles
  return std::make_tuple(B, A, D, LA, OoPB);
}

TEST_CASE("Bonds, angles and dihedral angles") {
  using namespace connectivity;

  CHECK_THROWS(Bond{0, 0});

  Bond b1{0, 1};
  Bond b2{1, 0};
  Bond b3{b1};
  Bond b4{0, 2};

  CHECK(b1 == b2);
  CHECK(b1 == b3);
  CHECK(b1 != b4);

  CHECK_THROWS(Angle{0, 0, 1});
  CHECK_THROWS(Angle{0, 1, 0});
  CHECK_THROWS(Angle{1, 0, 0});
  CHECK_THROWS(Angle{0, 0, 0});

  Angle a1{0, 1, 2};
  Angle a2{2, 1, 0};
  Angle a3{a1};
  Angle a4{1, 2, 3};

  CHECK(a1 == a2);
  CHECK(a1 == a3);
  CHECK(a1 != a4);

  CHECK_THROWS(Dihedral{0, 0, 1, 2});
  CHECK_THROWS(Dihedral{0, 1, 0, 2});
  CHECK_THROWS(Dihedral{0, 1, 2, 0});
  CHECK_THROWS(Dihedral{1, 0, 0, 2});
  CHECK_THROWS(Dihedral{1, 0, 2, 0});
  CHECK_THROWS(Dihedral{1, 2, 0, 0});

  CHECK_THROWS(Dihedral{1, 0, 0, 0});
  CHECK_THROWS(Dihedral{0, 1, 0, 0});
  CHECK_THROWS(Dihedral{0, 0, 1, 0});
  CHECK_THROWS(Dihedral{0, 0, 0, 1});

  CHECK_THROWS(Dihedral{0, 0, 1, 1});
  CHECK_THROWS(Dihedral{0, 1, 0, 1});
  CHECK_THROWS(Dihedral{0, 1, 1, 0});
  CHECK_THROWS(Dihedral{1, 0, 1, 0});
  CHECK_THROWS(Dihedral{1, 1, 0, 0});

  CHECK_THROWS(Dihedral{0, 0, 0, 0});

  Dihedral d1{0, 1, 2, 3};
  Dihedral d2{3, 2, 1, 0};
  Dihedral d3{d1};
  Dihedral d4{1, 2, 3, 4};

  CHECK(d1 == d2);
  CHECK(d1 == d3);
  CHECK(d1 != d4);
}

TEST_CASE("Distance, angle and dihedral angle") {
  using namespace connectivity;

  // Define four points in 3D space
  const auto p1 = vec3{0.00, 0.00, -0.25};
  const auto p2 = vec3{0.00, 0.00, 1.50};
  const auto p3 = vec3{0.00, 1.00, 1.50};
  const auto p4 = vec3{5.00, 1.00, 1.50};

  CHECK(distance(p1, p2) == Approx(1.75));

  CHECK(angle(p1, p2, p3) == Approx(tools::constants::pi / 2.));

  CHECK(dihedral(p1, p2, p3, p4) == Approx(-tools::constants::pi / 2.));
}

// Regular bond
TEST_CASE("Connectivity for compressed H2") {
  using namespace std;

  using namespace tools::conversion;
  using namespace molecule;
  using namespace connectivity;

  double d{0.5};

  // Define compressed H2 molecule
  Molecule<vec3> molecule{{"H", {0.0, 0.0, d}}, {"H", {0.0, 0.0, 0.0}}};

  // Transform molecular coordinates from angstrom to bohr
  multiply_positions(molecule, angstrom_to_bohr);

  // Get bonds, angles and dihedrals
  std::vector<Bond> B;
  std::vector<Angle> A;
  std::vector<Dihedral> D;
  std::vector<LinearAngle<vec3>> LA;
  std::vector<OutOfPlaneBend> OoPB;
  std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

  CHECK(B.size() == 1);
  CHECK(A.empty());
  CHECK(D.empty());
  CHECK(LA.empty());
  CHECK(OoPB.empty());

  // Compute IRC
  vec q{connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(molecule), B, {}, {}, {}, {})};

  // Check number of IRC
  REQUIRE(linalg::size<vec>(q) == 1);

  // Check bond length
  CHECK(q(0) == Approx(d * angstrom_to_bohr));
}

// Interfragment bond
TEST_CASE("Connectivity for stretched H2") {
  using namespace std;

  using namespace tools::conversion;
  using namespace molecule;
  using namespace connectivity;

  double d{2.5};

  // Define compressed H2 molecule
  Molecule<vec3> molecule{{"H", {0.0, 0.0, d}}, {"H", {0.0, 0.0, 0.0}}};

  // Transform molecular coordinates from angstrom to bohr
  multiply_positions(molecule, angstrom_to_bohr);

  // Get bonds, angles and dihedrals
  std::vector<Bond> B;
  std::vector<Angle> A;
  std::vector<Dihedral> D;
  std::vector<LinearAngle<vec3>> LA;
  std::vector<OutOfPlaneBend> OoPB;
  std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

  CHECK(B.size() == 1);
  CHECK(A.empty());
  CHECK(D.empty());
  CHECK(LA.empty());
  CHECK(OoPB.empty());

  // Compute IRC
  vec q{connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(molecule), B, A, D, {}, {})};

  // Check number of IRC
  REQUIRE(linalg::size<vec>(q) == 1);

  // Check bond length
  CHECK(q(0) == Approx(d * angstrom_to_bohr));
}

// Angle
TEST_CASE("Connectivity for compressed H2O") {
  using namespace std;

  using namespace tools::conversion;
  using namespace molecule;
  using namespace connectivity;

  double d1{0.8};
  double d2{0.6};
  double angle{134};

  double angle_rad{angle * deg_to_rad};
  double a{(180. - angle) / 2.};
  double a_rad{a * deg_to_rad};

  // Define compressed H2 molecule
  Molecule<vec3> molecule{
      {"O", {0, 0, 0}},
      {"H", {d1 * std::cos(a_rad), -d1 * std::sin(a_rad), 0}},
      {"H", {-d2 * std::cos(a_rad), -d2 * std::sin(a_rad), 0}}};

  // Transform molecular coordinates from angstrom to bohr
  multiply_positions(molecule, angstrom_to_bohr);

  // Get bonds, angles and dihedrals
  std::vector<Bond> B;
  std::vector<Angle> A;
  std::vector<Dihedral> D;
  std::vector<LinearAngle<vec3>> LA;
  std::vector<OutOfPlaneBend> OoPB;
  std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

  CHECK(B.size() == 2);
  CHECK(A.size() == 1);
  CHECK(D.empty());
  CHECK(LA.empty());
  CHECK(OoPB.empty());

  // Compute IRC
  vec q{connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(molecule), B, A, D, {}, {})};

  // Check number of IRC
  REQUIRE(linalg::size<vec>(q) == 3);

  // Check bond 1
  CHECK(q(0) == Approx(d1 * angstrom_to_bohr));

  // Check bond 2
  CHECK(q(1) == Approx(d2 * angstrom_to_bohr));

  // Check angle
  CHECK(q(2) == Approx(angle_rad));
}

// Angle from interfragment bonds
TEST_CASE("Connectivity for stretched H2O") {
  using namespace std;

  using namespace tools::conversion;
  using namespace molecule;
  using namespace connectivity;

  double d1{1.3};
  double d2{1.8};
  double angle{85};

  double angle_rad{angle * deg_to_rad};
  double a{(180. - angle) / 2.};
  double a_rad{a * deg_to_rad};

  // Define compressed H2 molecule
  Molecule<vec3> molecule{
      {"O", {0, 0, 0}},
      {"H", {d1 * std::cos(a_rad), -d1 * std::sin(a_rad), 0}},
      {"H", {-d2 * std::cos(a_rad), -d2 * std::sin(a_rad), 0}}};

  // Transform molecular coordinates from angstrom to bohr
  multiply_positions(molecule, angstrom_to_bohr);

  // Get bonds, angles and dihedrals
  std::vector<Bond> B;
  std::vector<Angle> A;
  std::vector<Dihedral> D;
  std::vector<LinearAngle<vec3>> LA;
  std::vector<OutOfPlaneBend> OoPB;
  std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

  CHECK(B.size() == 2);
  CHECK(A.size() == 1);
  CHECK(D.empty());
  CHECK(LA.empty());
  CHECK(OoPB.empty());

  // Compute IRC
  vec q{connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(molecule), B, A, D, {}, {})};

  // Check number of IRC
  REQUIRE(linalg::size<vec>(q) == 3);

  // Check bond 1
  CHECK(q(0) == Approx(d1 * angstrom_to_bohr));

  // Check bond 2
  CHECK(q(1) == Approx(d2 * angstrom_to_bohr));

  // Check angle
  CHECK(q(2) == Approx(angle_rad));
}

// Dihedral
TEST_CASE("Connectivity for compressed H2O2") {
  using namespace std;

  using namespace tools::conversion;
  using namespace molecule;
  using namespace connectivity;

  // Define H2O2 molecule
  Molecule<vec3> molecule{{"O", {0.000000, 0.734058, -0.052750}},
                          {"O", {0.000000, -0.734058, -0.052750}},
                          {"H", {0.839547, 0.880752, 0.422001}},
                          {"H", {-0.839547, -0.880752, 0.422001}}};

  // Transform molecular coordinates from angstrom to bohr
  multiply_positions(molecule, angstrom_to_bohr);

  // Get bonds, angles and dihedrals
  std::vector<Bond> B;
  std::vector<Angle> A;
  std::vector<Dihedral> D;
  std::vector<LinearAngle<vec3>> LA;
  std::vector<OutOfPlaneBend> OoPB;
  std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

  // Check number of bonds
  CHECK(B.size() == 3);

  // Check number of angles
  CHECK(A.size() == 2);

  // Check number of dihedral angles
  CHECK(D.size() == 1);

  // Compute IRC
  vec q{connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(molecule), B, A, D, {}, {})};

  // Check number of IRC
  REQUIRE(linalg::size<vec>(q) == 6);

  // Atomic positions
  vec3 p1{molecule[0].position};
  vec3 p2{molecule[1].position};
  vec3 p3{molecule[2].position};
  vec3 p4{molecule[3].position};

  // Check bond 1
  CHECK(q(0) == Approx(distance(p1, p2)));

  // Check bond 2
  CHECK(q(1) == Approx(distance(p1, p3)));

  // Check bond 3
  CHECK(q(2) == Approx(distance(p2, p4)));

  // Check angle 1
  CHECK(q(3) == Approx(angle(p2, p1, p3)));

  // Check angle 2
  CHECK(q(4) == Approx(angle(p1, p2, p4)));

  // Check dihedral
  CHECK(q(5) == Approx(dihedral(p3, p1, p2, p4)));
}

// Dihedral from interfragment bonds
TEST_CASE("Connectivity for stretched H2O2") {
  using namespace std;

  using namespace tools::conversion;
  using namespace molecule;
  using namespace connectivity;

  // Define H2O2 molecule
  Molecule<vec3> molecule{{"O", {0.000000, 0.734058, -0.052750}},
                          {"O", {0.000000, -0.734058, -0.052750}},
                          {"H", {1.839547, 1.880752, 0.422001}},
                          {"H", {-1.839547, -1.880752, 0.422001}}};

  // Transform molecular coordinates from angstrom to bohr
  multiply_positions(molecule, angstrom_to_bohr);

  // Get bonds, angles and dihedrals
  std::vector<Bond> B;
  std::vector<Angle> A;
  std::vector<Dihedral> D;
  std::vector<LinearAngle<vec3>> LA;
  std::vector<OutOfPlaneBend> OoPB;
  std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

  // Check number of bonds
  CHECK(B.size() == 3);

  // Check number of angles
  CHECK(A.size() == 2);

  // Check number of dihedral angles
  CHECK(D.size() == 1);

  // Compute IRC
  vec q{connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(molecule), B, A, D, {}, {})};

  // Check number of IRC
  REQUIRE(linalg::size<vec>(q) == 6);

  // Atomic positions
  vec3 p1{molecule[0].position};
  vec3 p2{molecule[1].position};
  vec3 p3{molecule[2].position};
  vec3 p4{molecule[3].position};

  // Check bond 1
  CHECK(q(0) == Approx(distance(p1, p2)));

  // Check bond 2
  CHECK(q(1) == Approx(distance(p1, p3)));

  // Check bond 3
  CHECK(q(2) == Approx(distance(p2, p4)));

  // Check angle 1
  CHECK(q(3) == Approx(angle(p2, p1, p3)));

  // Check angle 2
  CHECK(q(4) == Approx(angle(p1, p2, p4)));

  // Check dihedral
  CHECK(q(5) == Approx(dihedral(p3, p1, p2, p4)));
}

// Recursive interfragment bond
TEST_CASE("Connectivity for two stretched H2 molecules") {
  using namespace std;

  using namespace tools::conversion;
  using namespace molecule;
  using namespace connectivity;

  // Define H2O2 molecule
  Molecule<vec3> molecule{{"H", {0.0, 0.0, 0.0}},
                          {"H", {2.0, 0.0, 0.0}},
                          {"H", {5.0, 0.0, 1.0}},
                          {"H", {7.0, 0.0, 1.0}}};

  // Transform molecular coordinates from angstrom to bohr
  multiply_positions(molecule, angstrom_to_bohr);

  // Get bonds, angles and dihedrals
  std::vector<Bond> B;
  std::vector<Angle> A;
  std::vector<Dihedral> D;
  std::vector<LinearAngle<vec3>> LA;
  std::vector<OutOfPlaneBend> OoPB;
  std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

  CHECK(B.size() == 3);
  CHECK(A.size() == 2);
  CHECK(D.size() == 1);
  CHECK(LA.size() == 0);
  CHECK(OoPB.size() == 0);

  // Compute IRC
  vec q{connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(molecule), B, A, D, {}, {})};

  // Check number of IRC
  REQUIRE(linalg::size<vec>(q) == 6);

  // Atomic positions
  vec3 p1{molecule[0].position};
  vec3 p2{molecule[1].position};
  vec3 p3{molecule[2].position};
  vec3 p4{molecule[3].position};

  // Check bond 1
  CHECK(q(0) == Approx(distance(p1, p2)));

  // Check bond 2
  CHECK(q(1) == Approx(distance(p2, p3)));

  // Check bond 3
  CHECK(q(2) == Approx(distance(p3, p4)));

  // Check angle 1
  CHECK(q(3) == Approx(angle(p1, p2, p3)));

  // Check angle 2
  CHECK(q(4) == Approx(angle(p2, p3, p4)));

  // Check dihedral
  CHECK(q(5) == Approx(dihedral(p1, p2, p3, p4)));
}

// Hydrogen bond (without quasi-linear angles)
TEST_CASE("Connectivity for bent water dimer") {
  using namespace std;

  using namespace tools::conversion;
  using namespace molecule;
  using namespace connectivity;

  // Define water dimer
  Molecule<vec3> molecule{{"O", {-1.464, 0.099, -0.300}},
                          {"H", {-1.956, 0.624, -0.340}},
                          {"H", {-1.797, -0.799, 0.206}},
                          {"O", {1.369, 0.146, -0.395}},
                          {"H", {1.894, 0.486, 0.335}},
                          {"H", {0.451, 0.165, -0.083}}};

  // Transform molecular coordinates from angstrom to bohr
  multiply_positions(molecule, angstrom_to_bohr);

  // Get bonds, angles and dihedrals
  std::vector<Bond> B;
  std::vector<Angle> A;
  std::vector<Dihedral> D;
  std::vector<LinearAngle<vec3>> LA;
  std::vector<OutOfPlaneBend> OoPB;
  std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

  // Check number of bonds
  CHECK(B.size() == 5);

  // Check number of angles
  CHECK(A.size() == 5);

  // TODO: Check wit other codes (where dihedral 2-1-3-6 is added).
  // Check number of dihedral angles
  CHECK(D.size() == 3);

  // Compute IRC
  vec q{cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(molecule), B, A, D, {}, {})};

  // Check number of IRC
  CHECK(linalg::size<vec>(q) == 13);

  // Atomic positions
  vec3 p1{molecule[0].position};
  vec3 p2{molecule[1].position};
  vec3 p3{molecule[2].position};
  vec3 p4{molecule[3].position};
  vec3 p5{molecule[4].position};
  vec3 p6{molecule[5].position};

  // Check bond 1
  CHECK(q(0) == Approx(distance(p1, p2)));

  // Check bond 2
  CHECK(q(1) == Approx(distance(p1, p3)));

  // Check bond 3
  CHECK(q(2) == Approx(distance(p4, p5)));

  // Check bond 4
  CHECK(q(3) == Approx(distance(p1, p6)));

  // Check bond 5
  CHECK(q(4) == Approx(distance(p4, p6)));

  // Check angle 1
  CHECK(q(5) == Approx(angle(p2, p1, p3)));

  // Check angle 2
  CHECK(q(6) == Approx(angle(p1, p6, p4)));

  // Check angle 3
  CHECK(q(7) == Approx(angle(p2, p1, p6)));

  // Check angle 4
  CHECK(q(8) == Approx(angle(p3, p1, p6)));

  // Check angle 5
  CHECK(q(9) == Approx(angle(p5, p4, p6)));

  // Check dihedral 1
  CHECK(q(10) == Approx(dihedral(p2, p1, p6, p4)));

  // Check dihedral 2
  CHECK(q(11) == Approx(dihedral(p3, p1, p6, p4)));

  // Check dihedral 3
  CHECK(q(12) == Approx(dihedral(p1, p6, p4, p5)));
}

TEST_CASE("Linear angles") {
  using irc::connectivity::angle;

  SECTION("orthogonal to X and Y") {
    const vec3 p1 = {0, 0, 0};
    const vec3 p2 = {1, 1, 1};
    const vec3 p3 = {2, 2, 2};

    CHECK(angle(p1, p2, p3) == tools::constants::pi);
  };

  using namespace tools::conversion;
  using namespace molecule;
  using namespace connectivity;

  Molecule<vec3> molecule{{"C", {0.0, 1.0, 2.0}},
                          {"C", {2.0, 1.0, 2.0}},
                          {"C", {4.0, 1.0, 2.0}},
                          {"C", {6.0, 1.0, 2.0}}};

  // Transform molecular coordinates from angstrom to bohr
  multiply_positions(molecule, angstrom_to_bohr);

  // Get bonds, angles and dihedrals
  std::vector<Bond> B;
  std::vector<Angle> A;
  std::vector<Dihedral> D;
  std::vector<LinearAngle<vec3>> LA;
  std::vector<OutOfPlaneBend> OoPB;
  std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

  CHECK(B.size() == 3);
  CHECK(A.size() == 0);
  CHECK(D.size() == 0);
  CHECK(LA.size() == 2 * 2);
  CHECK(OoPB.size() == 0);

  vec q{connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(molecule), B, A, D, LA, OoPB)};

  REQUIRE(linalg::size<vec>(q) == 7);

  CHECK(q(3) == Approx(tools::constants::pi));
  CHECK(q(4) == Approx(tools::constants::pi));
  CHECK(q(5) == Approx(tools::constants::pi));
  CHECK(q(6) == Approx(tools::constants::pi));
}

TEST_CASE("Ignore invalid dihedral") {
  using namespace molecule;
  using namespace connectivity;

  SECTION("Linear molecule") {
    Molecule<vec3> molecule{
        {"C", {0, 0, 0}}, {"C", {0, 0, 2}}, {"C", {0, 0, 4}}, {"C", {0, 0, 6}}};

    std::vector<Bond> B;
    std::vector<Angle> A;
    std::vector<Dihedral> D;
    std::vector<LinearAngle<vec3>> LA;
    std::vector<OutOfPlaneBend> OoPB;
    std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

    CHECK(B.size() == 3);
    CHECK(A.size() == 0);
    CHECK(D.size() == 0);
    CHECK(LA.size() == 2 * 2);
    CHECK(OoPB.size() == 0);
  }

  SECTION("Single bend molecule") {
    Molecule<vec3> molecule{
        {"C", {0, 0, 0}}, {"C", {0, 0, 2}}, {"C", {0, 0, 4}}, {"C", {0, 1, 6}}};

    std::vector<Bond> B;
    std::vector<Angle> A;
    std::vector<Dihedral> D;
    std::vector<LinearAngle<vec3>> LA;
    std::vector<OutOfPlaneBend> OoPB;
    std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

    CHECK(B.size() == 3);
    CHECK(A.size() == 1);
    CHECK(D.size() == 0);
    CHECK(LA.size() == 2 * 1);
    CHECK(OoPB.size() == 0);
  }

  SECTION("Two bends molecule") {
    Molecule<vec3> molecule{
        {"C", {0, 1, 0}}, {"C", {0, 0, 2}}, {"C", {0, 0, 4}}, {"C", {0, 1, 6}}};

    std::vector<Bond> B;
    std::vector<Angle> A;
    std::vector<Dihedral> D;
    std::vector<LinearAngle<vec3>> LA;
    std::vector<OutOfPlaneBend> OoPB;
    std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(molecule);

    CHECK(B.size() == 3);
    CHECK(A.size() == 2);
    CHECK(D.size() == 1);
    CHECK(LA.size() == 0);
    CHECK(OoPB.size() == 0);
  }
}

// Quasi-linear angles
TEST_CASE("Connectivity for water dimer") {
  // TODO
}

// Interfragment bonds
TEST_CASE("Connectivity for benzene dimer") {
  // TODO
}

TEST_CASE("Connectivity test for CH2O") {
  // TODO
  /*
  using namespace std;

  using namespace tools::conversion;
  using namespace molecule;
  using namespace connectivity;

  double d1{1.3};
  double d2{1.4};
  double a_deg{102.03};

  double a{(180. - a_deg) * deg_to_rad};

  double cos_a{std::cos(a)};
  double sin_a{std::sin(a)};

  // Define water dimer
  Molecule<vec3> molecule{{"H", { 1.0686, -0.1411,  1.0408}},
                          {"C", { 0.5979,  0.0151,  0.0688}},
                          {"H", { 1.2687,  0.2002, -0.7717}},
                          {"O", {-0.5960, -0.0151, -0.0686}}};

  // Transform molecular coordinates from angstrom to bohr
  multiply_positions(molecule, angstrom_to_bohr);

  // Get bonds, angles and dihedrals
  std::vector<Bond> B;
  std::vector<Angle> A;
  std::vector<Dihedral> D;
  std::tie(B, A, D) = bad_from_molecule<vec3, vec, mat>(molecule);

  // Check number of bonds
  CHECK(B.size() == 3);

  // Check number of angles
  CHECK(A.size() == 3);

  // Check number of dihedral angles
  CHECK(D.size() == 1);

  // Compute IRC
  vec q{
      cartesian_to_irc<vec3, vec>(to_cartesian<vec3, vec>(molecule), B, A, D)};

  // Check number of IRC
  REQUIRE(linalg::size<vec>(q) == 7);
   */
}

TEST_CASE("Connectivity test for 2-butyne") {
  // TODO
}

TEST_CASE("Connectivity of molecule database") {
  using namespace io;

  using namespace connectivity;
  using namespace molecule;
  using namespace tools;

  struct ConnectivityResult {
    std::string filename;
    std::size_t n_bonds;
    std::size_t n_angles;
    std::size_t n_dihedrals;
    std::size_t n_linear_angles;
    std::size_t n_out_of_plane_bends;
  };

  auto results =
      std::vector<ConnectivityResult>{{"carbon_dioxide.xyz", 2, 0, 0, 2, 0},
                                      {"hydrogen_peroxide.xyz", 3, 2, 1, 0, 0},
                                      {"ethanol.xyz", 8, 13, 12, 0, 0},
                                      {"glycerol.xyz", 13, 21, 27, 0, 0},
                                      {"octane.xyz", 25, 48, 63, 0, 0},
                                      {"phenol.xyz", 13, 19, 26, 0, 6},
                                      {"indene.xyz", 18, 30, 44, 0, 8},
                                      {"toluene.xyz", 15, 24, 30, 0, 6},
                                      {"caffeine.xyz", 25, 43, 54, 0, 6}};

  for (const auto& molecule_parameters : results) {
    CAPTURE(molecule_parameters.filename);

    const auto mol =
        load_xyz<vec3>(config::molecules_dir + molecule_parameters.filename);

    std::vector<Bond> B;
    std::vector<Angle> A;
    std::vector<Dihedral> D;
    std::vector<LinearAngle<vec3>> LA;
    std::vector<OutOfPlaneBend> OoPB;
    std::tie(B, A, D, LA, OoPB) = badla_from_molecule<vec3, vec, mat>(mol);

    CHECK(B.size() == molecule_parameters.n_bonds);
    CHECK(A.size() == molecule_parameters.n_angles);
    CHECK(D.size() == molecule_parameters.n_dihedrals);
    CHECK(LA.size() == molecule_parameters.n_linear_angles);
    CHECK(OoPB.size() == molecule_parameters.n_out_of_plane_bends);
  }
}


/*
 * test_LIUTONGPAN.cpp
 *
 *  Created on: 2019-3-29
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <fstream>
#include <hikyuu/StockManager.h>
#include <hikyuu/indicator/crt/LIUTONGPAN.h>

using namespace hku;

/**
 * @defgroup test_indicator_LIUTONGPAN test_indicator_LIUTONGPAN
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_LIUTONGPAN ) {
    StockManager& sm = StockManager::instance();

    Stock stk = getStock("SH600004");
    KQuery query = KQueryByDate(Datetime(200301010000), Datetime(200708250000));
    KData k = stk.getKData(query);

    Indicator liutong = LIUTONGPAN(k);
    BOOST_CHECK(liutong.size() == k.size());
    size_t total = k.size();
    for (int i = 0; i < total; i++) {
        if (k[i].datetime < Datetime(200512200000)) {
            BOOST_CHECK(liutong[i] == 40000);
        } else if (k[i].datetime >= Datetime(200512200000)
                && k[i].datetime < Datetime(200612200000)) {
            BOOST_CHECK(liutong[i] == 47600);
        } else {
            BOOST_CHECK(liutong[i] == 49696);
        }
    }
}

//-----------------------------------------------------------------------------
// test export
//-----------------------------------------------------------------------------
#if HKU_SUPPORT_SERIALIZATION

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_LIUTONGPAN_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/LIUTONGPAN.xml";

    KData k = getStock("SH600000").getKData(KQuery(-10));
    Indicator x1 = LIUTONGPAN(k);
    x1.setContext(k);

    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(x1);
    }

    Indicator x2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(x2);
    }

    BOOST_CHECK(x2.name() == "LIUTONGPAN");
    BOOST_CHECK(x1.size() == x2.size());
    BOOST_CHECK(x1.discard() == x2.discard());
    BOOST_CHECK(x1.getResultNumber() == x2.getResultNumber());
    for (size_t i = 0; i < x1.size(); ++i) {
        BOOST_CHECK_CLOSE(x1[i], x2[i], 0.00001);
    }
}
#endif /* #if HKU_SUPPORT_SERIALIZATION */

/** @} */


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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.
*/
#ifndef __BlockVectorTest__
#define __BlockVectorTest__

#include <boost/numeric/ublas/vector_sparse.hpp>
#include <cppunit/extensions/HelperMacros.h>
#include "SiconosVector.hpp"
#include <cmath>
#include <vector>

class BlockVectorTest : public CppUnit::TestFixture
{

private:
  /** serialization hooks
  */
  ACCEPT_SERIALIZATION(BlockVectorTest);

  // Name of the tests suite
  CPPUNIT_TEST_SUITE(BlockVectorTest);

  // tests to be done ...

  //  CPPUNIT_TEST(testBuildBlockVector);
  CPPUNIT_TEST(testConstructor1);
  CPPUNIT_TEST(testConstructor2);
  CPPUNIT_TEST(testConstructor3);
  CPPUNIT_TEST(testConstructor4);
  CPPUNIT_TEST(testConstructor5);
  CPPUNIT_TEST(testZero);
  CPPUNIT_TEST(testFill);
  CPPUNIT_TEST(testNorm);
  CPPUNIT_TEST(testAssignment);
  CPPUNIT_TEST(testOperators1);
  CPPUNIT_TEST(testOperators2);
  CPPUNIT_TEST(testOperators3);
  CPPUNIT_TEST(testOperators4);
  CPPUNIT_TEST(testInsert);
  CPPUNIT_TEST(End);

  CPPUNIT_TEST_SUITE_END();

  // \todo exception test

  void testConstructor1();
  void testConstructor2();
  void testConstructor3();
  void testConstructor4();
  void testConstructor5();
  void testZero();
  void testFill();
  void testNorm();
  void testAssignment();
  void testOperators1();
  void testOperators2();
  void testOperators3();
  void testOperators4();
  void testInsert();
  void End();
  // Members

  SP::BlockVector ref;
  std::vector<double> vq;
  SP::DenseVect dv;
  SP::SparseVect sv;
  double tol;

public:
  void setUp();
  void tearDown();

};

#endif


#include <boost/test/unit_test.hpp>
#include <vector>
#include <numeric>
#include <limits>
#include <list>
#include <set>
#include <map>
#include <stack>
#include <math.h>
#include <stdio.h>
#include <unordered_map>

using namespace std;
using namespace boost;

// make hash table that
// hash value from adding up characters
// comparison test - equal if all character can be found in other word

typedef set<string> Anagrams;
typedef boost::shared_ptr<Anagrams> AnagramsPtr;

class AnagramHash
{
public:
	size_t operator()( const string& w ) const
	{
		size_t r = 0;
		for( string::const_iterator I=w.begin(); I!=w.end(); ++I)
		{
			r += *I;
		}
		return r;
	}
};

class AnagramCompare
{
public:
	bool operator()( const string& x, const string& y ) const
	{
		if( x.length() != y.length() )
			return false;

		for( string::const_iterator I=x.begin(); I!=x.end(); ++I)
		{
			if( y.find( *I ) ==  string::npos )
				return false;
		}
		return true;
	}
};

typedef unordered_map< string, AnagramsPtr, AnagramHash, AnagramCompare > AnagramHashTable;

class AnagramFinder
{
public:
	void Find( string words[], int wordsCount )
	{
		AnagramHashTable table;
		for ( int i=0; i<wordsCount; ++i)
		{
			AnagramHashTable::iterator I = table.find( words[i] );
			AnagramsPtr anagrams = NULL;

			if( I == table.end() )
			{
				anagrams = AnagramsPtr( new Anagrams() );
				table.emplace( pair<string,AnagramsPtr>( words[i], anagrams) );
			}
			else
			{
				anagrams = I->second;
			}

			anagrams->insert( words[i] );
		}

		AnagramHashTable::iterator A;
		for( A = table.begin(); A!=table.end(); ++A )
		{
			AnagramsPtr an = A->second;
			for( Anagrams::iterator W= an->begin(); W!=an->end();++W )
			{
				const string& word( *W );
				printf( "%s,", word.c_str() );
			}
			printf( "\n" );
		}
	}
};

BOOST_AUTO_TEST_CASE( TestQuiz4 )
{
	string words[] = { "CAT", "ACT", "TAC", "ACT", "GAC" };

	AnagramFinder finder;
	finder.Find( words, 5 );
}


//
// Created by robotmws on 2021/2/21.
//

#ifndef UR_ADMITTANCE_CONTROLLER_UTILITIES_HPP
#define UR_ADMITTANCE_CONTROLLER_UTILITIES_HPP

#define PACKAGE_NAME "ur_admittance_controller"

#include <csignal>
#include <fstream>
#include <iostream>
#include <cstring>
#include <numeric>
#include <cmath>

#include "spline_interpolation/spline.h" /* https://kluge.in-chemnitz.de/opensource/spline/ */

#include <ros/ros.h>
#include <ros/package.h>

#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/String.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/Pose.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <sensor_msgs/JointState.h>

#include "ur_admittance_controller/parameter_msg.h"
#include "ur_admittance_controller/joint_trajectory.h"
#include "ur_admittance_controller/parameter_srv.h"

#include <std_srvs/Trigger.h>
#include <std_srvs/SetBool.h>
#include <controller_manager_msgs/SwitchController.h>
#include <controller_manager_msgs/ListControllers.h>

#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#include <Eigen/Eigen>

using namespace Eigen;

typedef Matrix<double, 7, 1> Vector7d;
typedef Matrix<double, 6, 1> Vector6d;
typedef Matrix<double, 6, 6> Matrix6d;
typedef Array<double, 6, 1> Array6d;

struct extra_data_keypoint {
    double time_keypoint;
    double data_value;
};

template<typename T> void loadParam(const ros::NodeHandle &nodeHandle, const std::string &key, T &var) {
    if (!nodeHandle.getParam(key, var)) {
        ROS_ERROR("param %s cannot be loaded.", key.c_str());
        ros::shutdown();
    }
    std::ostringstream oss;
    oss << var;
    ROS_DEBUG("param %s is %s", key.c_str(), oss.str().c_str());
}

template<typename T> void loadParam(const ros::NodeHandle &nodeHandle, const std::string &key, std::vector<T> &var) {
    if (!nodeHandle.getParam(key, var)) {
        ROS_ERROR("param %s cannot be loaded.", key.c_str());
        ros::shutdown();
    }
    std::ostringstream oss;
    for(size_t i = 0; i < var.size(); ++i) {
        if(i != 0) oss << ",";
        oss << var[i];
    }
    ROS_DEBUG("param %s is %s", key.c_str(), oss.str().c_str());
}

template<typename T> void loadParam(const ros::NodeHandle &nodeHandle, const std::string &key, T &var, const T &defaultVal) {
    nodeHandle.param(key, var, defaultVal);
    std::ostringstream oss;
    oss << var;
    ROS_DEBUG("param %s is %s", key.c_str(), oss.str().c_str());
}

inline void csv_debug(const std::vector<double>& vector, const std::string& name) {
    std::string package_path = ros::package::getPath(PACKAGE_NAME);
    std::string save_file = package_path + "/debug/" + name + "_debug.csv";
    std::ofstream vector_debug = std::ofstream(save_file);
    vector_debug << name << "\n";
    for (double i : vector) {
        vector_debug << i << "\n";
    }
    vector_debug.close();

}

inline void csv_debug(std::vector<Vector6d> vector6d, const std::string& name) {
    std::string package_path = ros::package::getPath(PACKAGE_NAME);
    std::string save_file = package_path + "/debug/" + name + "_debug.csv";
    std::ofstream vector_debug = std::ofstream(save_file);
    vector_debug << name << "\n";
    for (auto & i : vector6d) {
        for (unsigned int joint_n = 0; joint_n < 6; joint_n++) {
            vector_debug << i[joint_n] << ",";
        }
        vector_debug << "\n";
    }
    vector_debug.close();
}

inline void csv_debug(std::vector<tk::spline> spline6d, std::vector<double> s, const std::vector<Vector6d>& data_vector, const std::string& name) {
    std::string package_path = ros::package::getPath(PACKAGE_NAME);
    std::string save_file = package_path + "/debug/" + name + "_spline6d_debug.csv";
    std::ofstream spline6d_debug = std::ofstream(save_file);
    spline6d_debug << "s, ,Joint1,Joint2,Joint3,Joint4,Joint5,Joint6\n\n";
    for (unsigned int i = 0; i < data_vector.size(); i++) {
        spline6d_debug << s[i] << ", ,";
        for (unsigned int joint_n = 0; joint_n < 6; joint_n++) { spline6d_debug << spline6d[joint_n](s[i]) << ","; }
        spline6d_debug << "\n";
    }
    spline6d_debug.close();
}

inline void trajectory_debug_csv(std::vector<sensor_msgs::JointState> trajectory, const std::string& trajectory_name) {
    std::string package_path = ros::package::getPath(PACKAGE_NAME);
    std::string save_file = package_path + "/debug/" + trajectory_name + "_debug.csv";
    std::ofstream trajectory_debug = std::ofstream(save_file);
    trajectory_debug
            << "frame_id,seq,sec,nsec,     ,pos_joint1,pos_joint2,pos_joint2,pos_joint4,pos_joint5,pos_joint6,     ,vel_joint1,vel_joint2,vel_joint3,vel_joint4,vel_joint5,vel_joint6\n\n";
    for (auto & i : trajectory) {
        trajectory_debug << i.header.frame_id << "," << i.header.seq << ","
                         << i.header.stamp.sec << "," << i.header.stamp.nsec << ", ,";
        for (unsigned int joint_n = 0; joint_n < 6; joint_n++) {
            trajectory_debug << i.position[joint_n] << ",";
        }
        trajectory_debug << " ,";
        for (unsigned int joint_n = 0; joint_n < 6; joint_n++) {
            trajectory_debug << i.velocity[joint_n] << ",";
        }
        trajectory_debug << "\n";
    }
    trajectory_debug.close();
}

inline int sign(double num) {
    if (num >= 0) return 1;
    else return -1;
}

inline std::vector<tk::spline> spline_interpolation(std::vector<Vector6d> data_vector, double spline_lenght, const std::string& output_file, bool debug=false) {
    // Creation of Spline6d Vector -> Usage: spline6d[joint_number](s) = q(s)
    std::vector<tk::spline> spline6d;
    // Creation of s € [0,spline_lenght] vector
    std::vector<double> s;
    for (unsigned i = 0; i < data_vector.size(); i++) {
        double s_i = spline_lenght * (i / (double(data_vector.size()) - 1));
        s.push_back(s_i);
    }
    // Compute Spline for each Joint
    for (unsigned joint_number = 0; joint_number < 6; joint_number++) {
        // Create a Single-Joint Vector
        std::vector<double> waypoints_1d;
        for (auto & i : data_vector) {
            waypoints_1d.push_back(i[joint_number]);
        }
        // Compute Cubic Spline [Q(s), s € [0,T]]
        tk::spline spline1d;
        spline1d.set_points(s, waypoints_1d);
        // Add Results to "spline6d" Vector
        spline6d.push_back(spline1d);
    }
    // ---- DEBUG ---- //
    if (debug) {
        csv_debug(spline6d, s, data_vector, output_file);
    }
    return spline6d;
}

inline std::vector<tk::spline> spline_interpolation(std::vector<Array6d> data_vector, double spline_lenght, const std::string& output_file) {
    // Converting Array into Vector
    std::vector<Vector6d> data_vector_temp;
    for (auto & i : data_vector) {
        data_vector_temp.emplace_back(i.matrix());
    }
    return spline_interpolation(data_vector_temp, spline_lenght, output_file);
}

inline Vector6d get_spline_value(std::vector<tk::spline> spline6d, double s) {
    Vector6d spline_value;
    spline_value.resize(6);

    // Get spline1d value for each joint
    for (unsigned int i = 0; i < 6; i++) {
        spline_value[i] = spline6d[i](s);
    }
    return spline_value;
}

inline extra_data_keypoint new_extra_data_keypoint(double data_value, double time_keypoint) {
    extra_data_keypoint temp{};
    temp.data_value = data_value;
    temp.time_keypoint = time_keypoint;
    return temp;
}

inline Vector6d new_vector_6d(double x, double y, double z, double roll, double pitch, double yaw) {
    Vector6d temp;
    temp.setZero();
    temp[0] = x;
    temp[1] = y;
    temp[2] = z;
    temp[3] = roll;
    temp[4] = pitch;
    temp[5] = yaw;
    return temp;
}

#endif //UR_ADMITTANCE_CONTROLLER_UTILITIES_HPP


// (C) Copyright Renaud Detry   2007-2008.
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/** @file */

#ifndef NUKLEI_TRSL_IS_PICKED_SYSTEMATIC_HPP
#define NUKLEI_TRSL_IS_PICKED_SYSTEMATIC_HPP

#include <nuklei/trsl/common.hpp>
#include <nuklei/trsl/weight_accessor.hpp>

#include <algorithm>
#include <functional>
#include <utility>
#include <cstdlib> // rand & random
#include <limits>
#include <cassert>
#include <boost/static_assert.hpp>

/** @brief Public namespace. */
namespace nuklei_trsl {

  /**
   * @brief Functor to use with persistent_filter_iterator for
   * systematic sampling of a range.
   *
   * The sampling method is systematic sampling [1, 2].
   *
   * This class is intended to be used as a predicate functor to
   * trsl::persistent_filter_iterator to form a sample
   * iterator. The sample iterator accesses a population of
   * elements through a range defined by a pair of Forward Iterators
   * (begin/end), and provides on-the-fly iteration through a sample
   * of the population.
   *
   * Systematic sampling may perform very badly if the order in which
   * the input population is presented is following a pattern. If a
   * pattern is likely to occur in the population, the user may want to
   * pipe the sample iterator after a
   * trsl::random_permutation_iterator, or use is_picked_systematic with
   * ppfilter_iterator.  The resulting iterator effectively implements
   * probability sampling.  The price to pay for probability
   * sampling is a Random Access Iterator. While
   * persistent_filter_iterator will work e.g. with <tt>std::list</tt>,
   * ppfilter_iterator requires e.g. <tt>std::vector</tt> or
   * <tt>std::deque</tt>.
   *
   * @param ElementType Type of the elements in the
   * population. Constness and
   * reference modifiers are handled internally; this parameter should be
   * a bare type, e.g. <tt>Particle</tt> (not <tt>const Particle&</tt>).
   *
   * @param WeightType Element weight type, should be a floating point type.
   * Defaults to <tt>double</tt>.
   *
   * @param WeightAccessor Type of the accessor that will allow to
   * extract weights from elements. Defaults to mp_weight_accessor,
   * see @ref accessor for further details on accessors.
   *
   * References:
   *
   * - [1] R. Douc, O. Cappe, and E. Moulines. Comparison of
   * resampling schemes for particle filtering. International
   * Symposium on Parallel and Distributed Processing and
   * Applications, 2005:64, 2005.
   *
   * - [2] J. Hol, T. Schön, and F. Gustafsson. On resampling
   * algorithms for particle filters. In Nonlinear Statistical Signal
   * Processing Workshop, 2006.
   */
  template<
    typename ElementType,
    typename WeightType = double,
    typename WeightAccessor = mp_weight_accessor<WeightType, ElementType>
  > class is_picked_systematic
  {
  private:
    BOOST_STATIC_ASSERT((std::numeric_limits<WeightType>::is_integer == false));
  public:
    typedef ElementType element_type;
    typedef WeightType weight_type;
    typedef WeightAccessor weight_accessor_type;

    /**
     * @brief Default constructor, shoud not be used explicitely.
     *
     * This constructor makes an invalid predicate. It should only be used in
     * cases where the predicate is never used.
     */
    is_picked_systematic() :
      sampleSize_(0),
      populationWeight_(0)
      {
        initialize( 0 );
      }

    /**
     * @brief Construction with system-provided random number.
     *
     * The systematic sampling predicate initialization needs a random
     * number in <tt>[0,1[</tt>.  This constructor uses
     * trsl::rand_gen::uniform_01 to generate that number.  See @ref
     * random for more details.
     *
     * @param sampleSize Number of elements in the sample, within
     * <tt>[0, infinity[</tt>.
     *
     * @param populationWeight Total weight of the
     * population, within <tt>]0, infinity[</tt>. Generally equal to 1.
     *
     * @param wac Weight accessor. Defaults to
     * mp_weight_accessor. Note that if you don't pass explicitly a
     * mp_weight_accessor(<tt>&ElementType::GETWEIGHTMETHOD</tt>), the
     * default constructor for mp_weight_accessor will setup to always
     * return 1, ignoring element weights. See @ref accessor for
     * further details.
     *
     * The population weight has to be strictly larger than 0. Also,
     * @p WeightType should be fine enough to allow the sum of all
     * element weights to be close to @p populationWeight.
     */
    is_picked_systematic(size_t sampleSize,
                         WeightType populationWeight,
                         WeightAccessor const& wac = WeightAccessor()) :
      wac_(wac), sampleSize_(sampleSize),
      populationWeight_(populationWeight)
      {
        initialize( rand_gen::uniform_01<WeightType>() );
      }

    /**
     * @brief Construction with user-provided random number.
     *
     * The systematic sampling predicate initialization needs a random
     * number in <tt>[0,1[</tt>.  This constructor allows the user to
     * provide that number directly; the user is free to choose a
     * fancy random number generator, such as the Boost
     * Random Number Library or GSL.
     *
     * @param sampleSize Number of elements in the sample, within
     * <tt>[0, infinity[</tt>.
     *
     * @param populationWeight Total weight of the
     * population, within <tt>]0, infinity[</tt>. Generally equal to 1.
     *
     * @param uniform01 Random number in <tt>[0,1[</tt>.
     *
     * @param wac Weight accessor. Defaults to
     * mp_weight_accessor. Note that if you don't pass explicitly a
     * mp_weight_accessor(<tt>&ElementType::GETWEIGHTMETHOD</tt>), the
     * default constructor for mp_weight_accessor will setup to always
     * return 1, ignoring element weights. See @ref accessor for
     * further details.
     *
     * The population weight has to be strictly larger than 0. Also,
     * @p WeightType should be fine enough to allow the sum of all
     * element weights to be close to @p populationWeight.
     */
    is_picked_systematic(size_t sampleSize,
                         WeightType populationWeight,
                         WeightType uniform01,
                         WeightAccessor const& wac = WeightAccessor()) :
      wac_(wac), sampleSize_(sampleSize),
      populationWeight_(populationWeight)
      {
        initialize(uniform01);
      }

    /**
     * @brief Decides whether <tt>e</tt> should be picked or not (used
     * by persistent_filter_iterator).
     *
     * Part of the requirements for persistent_filter_iterator
     * predicates.
     */
    bool operator()(const ElementType & e)
      {
        if (sampleSize_ == 0) return false;

#ifdef NUKLEI_TRSL_USE_SYSTEMATIC_INTUITIVE_ALGORITHM
        // This algorithm is the intuitive implementation of
        // systematic sampling, where one pictures a wheel with
        // N_SAMPLE spokes and distributes the population around the
        // wheel as segments of tire of length proportional to their
        // weight; the spokes point to picked elements.
        WeightType arrow = k_*step_;
        assert(cumulative_ <= arrow);
        if (arrow < cumulative_ + wac_(e))
        {
          k_++;
          return true;
        }
        cumulative_ += wac_(e);
        return false;
#else
        // This algorithm is a massaged version of the intuitive
        // algorithm.  Both algorithms are conceptually identical, but
        // this version is faster.
        assert(position_ >= 0);
        if (position_ < wac_(e))
        {
          position_ += step_;
          return true;
        }
        position_ -= wac_(e);
        return false;
#endif
      }

    /**
     * @brief Return whether @p e has been picked already.
     *
     * This method will return a coherent value only if the last
     * call of operator()(const ElementType&) was on @p e and returned true.
     * When is_picked_systematic is used with a presistent_filter_iterator @p i,
     * this assumption is always valid for @p *i as long as @p i is not the end.
     *
     * This method is awkward to use directly; it is meant to be called by
     * trsl::is_first_pick.
     */
    bool is_first_pick(const ElementType & e) const
    {
      if (wac_(e) <= step_) return true;
#ifdef NUKLEI_TRSL_USE_SYSTEMATIC_INTUITIVE_ALGORITHM
      return k_*step_ - cumulative_ < 2*step_;
#else
      return position_ < 2*step_;
#endif
    }

    /**
     * @brief Returns whether two predicates are at the same sampling
     * advancement.
     *
     * Part of the requirements for persistent_filter_iterator
     * predicates.
     */
    bool operator== (const is_picked_systematic<ElementType, WeightType, WeightAccessor> &p) const
      {
        if (sampleSize_ != p.sampleSize_ ||
            populationWeight_ != p.populationWeight_) return false;

#ifdef NUKLEI_TRSL_USE_SYSTEMATIC_INTUITIVE_ALGORITHM
        if (cumulative_ != p.cumulative_ ||
            k_ != p.k_) return false;
#else
        if (position_ != p.position_) return false;
#endif

        return true;
      }

  private:
    void initialize(WeightType randomReal)
      {
        // If one could rely on
        //
        //   (x!=0.0)/0.0 == inf (with the sign of x)
        //
        // and on
        //
        //   0.0 * inf == NaN,
        //
        // neither of these throwing exceptions, then the following
        // block could be avoided, and the case sampleSize_ == 0 would
        // be implicitly handled by inf/NaN arithmetic.
        //
        // However, it would be necessary to maintain two different
        // implementations depending on
        // std::numeric_limits<double>::is_iec559, and even then I'm
        // not sure if inf/NaN arithmetic is reliable.
        if (sampleSize_ != 0)
          step_ = populationWeight_ / sampleSize_;
        else
          // A semantically correct value for step_ would be inf, but
          // we don't want to get into has_infinity trouble...
          // When sampleSize_==0, step_ should be considered undefined.
          step_ = 0;

        WeightType randomWeight = randomReal * step_;
#ifdef NUKLEI_TRSL_USE_SYSTEMATIC_INTUITIVE_ALGORITHM
        cumulative_ = -randomWeight;
        k_ = 0;
#else
        position_ = randomWeight;
#endif
      }

  private:
    WeightAccessor wac_;
    size_t sampleSize_;
    WeightType populationWeight_;
    WeightType step_;

#ifdef NUKLEI_TRSL_USE_SYSTEMATIC_INTUITIVE_ALGORITHM
    WeightType cumulative_;
    size_t k_;
#else
    WeightType position_;
#endif
  };

  /**
   * @brief Return whether @p *i has been picked already.
   *
   * This function calls <tt>i.predicate().is_first_pick(*i)</tt>.
   *
   * When sampling from a population, elements with a large weight may be selected several times.
   * This function checks if the element pointed by @p *i has been selected already, e.g. at <tt>*(i-1)</tt>.
   *
   * @param Iterator Will typically be trsl::pp_filter_iterator or trsl::persitent_filter_iterator.
   *
   * @param i Instance of pp_filter_iterator or persitent_filter_iterator. Should not be the end.
   */
  template<class Iterator>
  bool is_first_pick(const Iterator& i)
  {
    return i.predicate().is_first_pick(*i);
  }

}

#endif // include guard


/*
@copyright Louis Dionne 2014
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/hana/map.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>
#include <boost/hana/detail/minimal/product.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

template <int i>
constexpr auto key = detail::minimal::comparable<>(i);

template <int i>
constexpr auto value = detail::minimal::comparable<>(i + 100);

template <int i, int j>
BOOST_HANA_CONSTEXPR_LAMBDA auto p = detail::minimal::product<>(key<i>, value<j>);

int main() {
    BOOST_HANA_CONSTANT_ASSERT(keys(map()) == list());
    BOOST_HANA_CONSTEXPR_ASSERT(keys(map(p<1, 1>)) == list(key<1>));
    BOOST_HANA_CONSTEXPR_ASSERT(keys(map(p<1, 1>, p<2, 2>)) == list(key<1>, key<2>));
    BOOST_HANA_CONSTEXPR_ASSERT(keys(map(p<1, 1>, p<2, 2>, p<3, 3>)) == list(key<1>, key<2>, key<3>));
}


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

  Copyright 2015 NumScale SAS

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

STF_CASE_TPL (" if_allbits_else real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::if_allbits_else;
  using bs::logical;
  using bs::Nan;

  // return type conformity test
  STF_EXPR_IS( if_allbits_else(T(), T()), T );

  // specific values tests
  STF_EQUAL(if_allbits_else(T(0), T(1)), T(1));
  STF_IEEE_EQUAL(if_allbits_else(T(1), T(1)), Nan<T>());
  STF_IEEE_EQUAL(if_allbits_else(bs::Inf<T>(), T(1)) , Nan<T>());
  STF_IEEE_EQUAL(if_allbits_else(bs::Minf<T>(), T(1)), Nan<T>());
  STF_IEEE_EQUAL(if_allbits_else(bs::Nan<T>(), T(1)) , Nan<T>());
  STF_EQUAL(if_allbits_else(bs::Zero<T>(), T(1)), T(1));

} // end of test for floating_

STF_CASE_TPL (" if_allbits_elsesigned_ int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::if_allbits_else;
  using bs::logical;
  using bs::Allbits;

  // return type conformity test
  STF_EXPR_IS( if_allbits_else(T(), T()), T );

  // specific values tests
  STF_EQUAL(if_allbits_else(T(0), T(1)), T(1));
  STF_EQUAL(if_allbits_else(T(1), T(1)), Allbits<T>());
} // end of test for signed_int_


// Copyright 2005 The Trustees of Indiana University.

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

//  Authors: Ben Martin
//           Douglas Gregor
//           Andrew Lumsdaine
#include <boost/graph/sloan_ordering.hpp>
#include "graph_types.hpp"
#include <boost/python.hpp>
#include <vector>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/python/list_append_iterator.hpp>
#include <utility>

namespace boost { namespace graph { namespace python {

template<typename Graph>
boost::python::list
sloan_ordering
  (Graph& g,
   vector_property_map<
     default_color_type,
     typename property_map<Graph, vertex_index_t>::const_type
   >* in_color,
   vector_property_map<
     float,
     typename property_map<Graph, vertex_index_t>::const_type
   >* in_priority,
   float weight1,
   float weight2)
{
  typedef typename graph_traits<Graph>::degree_size_type degree_size_type;

  typedef typename property_map<Graph, vertex_index_t>::const_type
    VertexIndexMap;
  typedef vector_property_map<default_color_type, VertexIndexMap> ColorMap;
  typedef vector_property_map<float, VertexIndexMap> PriorityMap;
  typedef vector_property_map<degree_size_type, VertexIndexMap>
    OutDegreeMap;

  // Out-degree map
  OutDegreeMap out_degree_map(num_vertices(g), get(vertex_index, g));
  BGL_FORALL_VERTICES_T(v, g, Graph)
    put(out_degree_map, v, out_degree(v, g));

  ColorMap color = in_color ? *in_color
                            : ColorMap(num_vertices(g), get(vertex_index, g));

  PriorityMap priority = in_priority ? *in_priority
                           : PriorityMap(num_vertices(g), get(vertex_index, g));

  boost::python::list result;
  boost::sloan_ordering(g, list_append_iterator(result),
                        color, out_degree_map, priority, weight1, weight2);
  return result;
}

template<typename Graph>
boost::python::list
sloan_ordering_se
  (Graph& g,
   typename graph_traits<Graph>::vertex_descriptor start,
   typename graph_traits<Graph>::vertex_descriptor end,
   vector_property_map<
     default_color_type,
     typename property_map<Graph, vertex_index_t>::const_type
   >* in_color,
   vector_property_map<
     float,
     typename property_map<Graph, vertex_index_t>::const_type
   >* in_priority,
   float weight1,
   float weight2)
{
  typedef typename graph_traits<Graph>::degree_size_type degree_size_type;

  typedef typename property_map<Graph, vertex_index_t>::const_type
    VertexIndexMap;
  typedef vector_property_map<default_color_type, VertexIndexMap> ColorMap;
  typedef vector_property_map<float, VertexIndexMap> PriorityMap;
  typedef vector_property_map<degree_size_type, VertexIndexMap>
    OutDegreeMap;

  // Out-degree map
  OutDegreeMap out_degree_map(num_vertices(g), get(vertex_index, g));
  BGL_FORALL_VERTICES_T(v, g, Graph)
    put(out_degree_map, v, out_degree(v, g));

  ColorMap color = in_color ? *in_color
                            : ColorMap(num_vertices(g), get(vertex_index, g));

  PriorityMap priority = in_priority ? *in_priority
                           : PriorityMap(num_vertices(g), get(vertex_index, g));

  boost::python::list result;
  boost::sloan_ordering(g, start, end, list_append_iterator(result),
                        color, out_degree_map, priority, weight1, weight2);
  return result;
}

template<typename Graph>
boost::python::tuple
sloan_se(Graph &g,
         typename graph_traits<Graph>::vertex_descriptor s)
{
  typedef typename graph_traits<Graph>::degree_size_type degree_size_type;

  typedef typename property_map<Graph, vertex_index_t>::const_type
    VertexIndexMap;
  typedef vector_property_map<default_color_type, VertexIndexMap> ColorMap;
  typedef vector_property_map<degree_size_type, VertexIndexMap>
    OutDegreeMap;

  typename graph_traits<Graph>::vertex_descriptor s2 = s;
  typename graph_traits<Graph>::vertex_descriptor e;

  // Out-degree map
  OutDegreeMap out_degree_map(num_vertices(g), get(vertex_index, g));
  BGL_FORALL_VERTICES_T(v, g, Graph)
    put(out_degree_map, v, out_degree(v, g));

  ColorMap color = ColorMap(num_vertices(g), get(vertex_index, g));

  e = boost::sloan_start_end_vertices(g, s2, color, out_degree_map);

  return boost::python::make_tuple(s, e);
}

void export_sloan_ordering()
{
  using boost::python::arg;
  using boost::python::def;

#define UNDIRECTED_GRAPH(Name,Type)                                     \
  {                                                                     \
    typedef property_map<Graph, vertex_index_t>::const_type VertexIndexMap; \
    typedef vector_property_map<default_color_type, VertexIndexMap> ColorMap; \
    typedef vector_property_map<float, VertexIndexMap> PriorityMap;     \
                                                                        \
    def("sloan_ordering",                                               \
        &sloan_ordering<Type>,                                          \
        (arg("graph"),                                                  \
         arg("color_map") = static_cast<ColorMap*>(0),                  \
         arg("priority_map") = static_cast<PriorityMap*>(0),            \
         arg("weight1") = 1.0,                                          \
         arg("weight2") = 2.0));                                        \
                                                                        \
    def("sloan_ordering",                                               \
        &sloan_ordering_se<Type>,                                       \
        (arg("graph"),                                                  \
         arg("start"),                                                  \
         arg("end"),                                                    \
         arg("color_map") = static_cast<ColorMap*>(0),                  \
         arg("priority_map") = static_cast<PriorityMap*>(0),            \
         arg("weight1") = 1.0,                                          \
         arg("weight2") = 2.0));                                        \
                                                                        \
    def("sloan_start_end_vertices",					    \
        &sloan_se<Type>,                                                \
        (arg("graph"),                                                  \
         arg("start_vertex")));                                         \
                                                                        \
  }
#define DIRECTED_GRAPH(Name,Type)
#include "graphs.hpp"
}

} } } // end namespace boost::graph::python


#pragma once
#include <boost/accumulators/accumulators.hpp>
#include <boost/circular_buffer.hpp>
#include <manipulate_topics/msg_operators.hpp>
#include <ros/ros.h>

/**
 * @brief  Base class for topic filters (convulutional filters)
 *
 * |Subscribed Topic|Description|
 * |-----|-----|
 * |input|The topic the value to be filtered is published at|
 * |output|The topic the filtered value is published at|
 *
 *
 * @tparam T Tpye of the Message to be filtered. Operators for the derived filteres must be implemented.
 */
template<class T>
class MessageFilterBase{
    public:
        /**
         * @brief Construct a new Message Filter Base object
         *
         * @param nh Nodehandle for handling ros namespaces
         *
         */
        MessageFilterBase(ros::NodeHandle &nh);

    protected:
        /**
         * @brief Procedure the filter algorithm is implemented in.
         *
         * Uses values from the buffer_ and filters them.
         *
         * @return T Filtered value.
         */
        virtual T filter()=0;

        std::unique_ptr<boost:: circular_buffer<T>> buffer_;    //<Circular buffer to hold the specified number of samples
    private:
        void inputCallback(T msg);
        ros::NodeHandle nh_;
        ros::Subscriber input_;
        ros::Publisher output_;

};
#include "../src/msg_filter_base.cpp"


/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * 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 <gtest/gtest.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/bind.hpp>

#include <ignition/math/Rand.hh>

#include "gazebo/sensors/Noise.hh"
#include "gazebo/sensors/GaussianNoiseModel.hh"
#include "test/util.hh"

using namespace gazebo;

class NoiseTest : public gazebo::testing::AutoLogFixture { };

const unsigned int g_applyCount = 100;

// We will use 5 sigma (4e-5 chance of failure)
const double g_sigma = 5.0;

////////////////////////////////////////////////////////////////
// Helper function that constructs sdf strings for noise element
sdf::ElementPtr NoiseSdf(const std::string &_type, double _mean,
  double _stddev, double _biasMean, double _biasStddev, double _precision)
{
  std::ostringstream noiseStream;
  noiseStream << "<sdf version='1.4'>"
              << "  <noise type='" << _type << "'>"
              << "    <mean>" << _mean << "</mean>"
              << "    <stddev>" << _stddev << "</stddev>"
              << "    <bias_mean>" << _biasMean << "</bias_mean>"
              << "    <bias_stddev>" << _biasStddev << "</bias_stddev>"
              << "    <precision>" << _precision << "</precision>"
              << "  </noise>"
              << "</sdf>";

  sdf::ElementPtr sdf(new sdf::Element);
  sdf::initFile("noise.sdf", sdf);
  sdf::readString(noiseStream.str(), sdf);

  return sdf;
}

//////////////////////////////////////////////////
// Test constructor
TEST_F(NoiseTest, Constructor)
{
  // Construct and nothing else
  {
    sensors::Noise noise(sensors::Noise::NONE);
  }

  // Construct and initialize
  {
    sensors::Noise noise(sensors::Noise::NONE);
    noise.Load(NoiseSdf("none", 0, 0, 0, 0, 0));
  }
}

//////////////////////////////////////////////////
// Test noise types
TEST_F(NoiseTest, Types)
{
  // NONE type
  {
    sensors::NoisePtr noise =
        sensors::NoiseFactory::NewNoiseModel(NoiseSdf("none", 0, 0, 0, 0, 0));
    EXPECT_EQ(noise->GetNoiseType(), sensors::Noise::NONE);
  }

  // GAUSSIAN type
  {
    sensors::NoisePtr noise =
        sensors::NoiseFactory::NewNoiseModel(
        NoiseSdf("gaussian", 0, 0, 0, 0, 0));
    EXPECT_EQ(noise->GetNoiseType(), sensors::Noise::GAUSSIAN);
  }

  // GAUSSIAN_QUANTIZED type
  {
    sensors::NoisePtr noise =
        sensors::NoiseFactory::NewNoiseModel(
        NoiseSdf("gaussian_quantized", 0, 0, 0, 0, 0));
    EXPECT_EQ(noise->GetNoiseType(), sensors::Noise::GAUSSIAN);
  }
}

//////////////////////////////////////////////////
// Helper function for testing no noise
void NoNoise(sensors::NoisePtr _noise, unsigned int _count)
{
  // Expect no change in input value
  for (unsigned int i = 0; i < _count; ++i)
  {
    double x = ignition::math::Rand::DblUniform(-1e6, 1e6);
    EXPECT_NEAR(x, _noise->Apply(x), 1e-6);
  }
}

//////////////////////////////////////////////////
// Helper function for testing Gaussian noise
void GaussianNoise(sensors::NoisePtr _noise, unsigned int _count)
{
  sensors::GaussianNoiseModelPtr noiseModel =
      std::dynamic_pointer_cast<sensors::GaussianNoiseModel>(_noise);

  ASSERT_TRUE(noiseModel != nullptr);

  // Use constant input and repeatedly add noise to it.
  double x = 42.0;

  // boost code from http://stackoverflow.com/questions/3534335
  boost::accumulators::accumulator_set<double,
    boost::accumulators::stats<boost::accumulators::tag::mean,
                               boost::accumulators::tag::variance > > acc;

  for (unsigned int i = 0; i < _count; ++i)
  {
    double y = _noise->Apply(x);
    acc(y);
  }

  // The sample mean should be near x+mean, with standard deviation of
  // stddev / sqrt(_count)
  // https://onlinecourses.science.psu.edu/stat414/node/167
  // We will use 5 sigma (4e-5 chance of failure)
  double mean = noiseModel->GetMean() + noiseModel->GetBias();
  double stddev = noiseModel->GetStdDev();
  double sampleStdDev = g_sigma*stddev / sqrt(_count);
  EXPECT_NEAR(boost::accumulators::mean(acc), x+mean, sampleStdDev);

  // The sample variance has the following variance:
  // 2 stddev^4 / (_count - 1)
  // en.wikipedia.org/wiki/Variance#Distribution_of_the_sample_variance
  // Again use 5 sigma
  double variance = stddev*stddev;
  double sampleVariance2 = 2 * variance*variance / (_count - 1);
  EXPECT_NEAR(boost::accumulators::variance(acc),
              variance, g_sigma*sqrt(sampleVariance2));
}

//////////////////////////////////////////////////
// Test noise application
TEST_F(NoiseTest, ApplyNone)
{
  sensors::NoisePtr noise = sensors::NoiseFactory::NewNoiseModel(
      NoiseSdf("none", 0, 0, 0, 0, 0));

  NoNoise(noise, g_applyCount);
}

TEST_F(NoiseTest, ApplyGaussian)
{
  double mean, stddev, biasMean, biasStddev;

  // GAUSSIAN with zero means and standard deviations
  // should be the same as NONE
  mean = 0.0;
  stddev = 0.0;
  biasMean = 0.0;
  biasStddev = 0.0;
  {
    sensors::NoisePtr noise = sensors::NoiseFactory::NewNoiseModel(
        NoiseSdf("gaussian", mean, stddev, biasMean, biasStddev, 0));
    NoNoise(noise, g_applyCount);
  }

  // GAUSSIAN with non-zero means and standard deviations, but no bias
  mean = 10.0;
  stddev = 5.0;
  biasMean = 0.0;
  biasStddev = 0.0;
  {
    sensors::NoisePtr noise = sensors::NoiseFactory::NewNoiseModel(
        NoiseSdf("gaussian", mean, stddev, biasMean, biasStddev, 0));
    sensors::GaussianNoiseModelPtr gaussianNoise =
      std::dynamic_pointer_cast<sensors::GaussianNoiseModel>(noise);
    EXPECT_NEAR(gaussianNoise->GetBias(), 0.0, 1e-6);
    GaussianNoise(noise, g_applyCount);
  }

  // GAUSSIAN with non-zero mean, exact bias, and standard deviations
  mean = 10.0;
  stddev = 5.0;
  biasMean = 100.0;
  biasStddev = 0.0;
  {
    sensors::NoisePtr noise = sensors::NoiseFactory::NewNoiseModel(
        NoiseSdf("gaussian", mean, stddev, biasMean, biasStddev, 0));
    GaussianNoise(noise, g_applyCount);
  }

  // Test bias generation
  mean = 0.0;
  stddev = 0.0;
  biasMean = 0.0;
  biasStddev = 5.0;
  {
    boost::accumulators::accumulator_set<double,
      boost::accumulators::stats<boost::accumulators::tag::mean,
                                 boost::accumulators::tag::variance > > acc;

    for (unsigned int i = 0; i < g_applyCount; ++i)
    {
      sensors::NoisePtr noise = sensors::NoiseFactory::NewNoiseModel(
          NoiseSdf("gaussian", mean, stddev, biasMean, biasStddev, 0));
      sensors::GaussianNoiseModelPtr gaussianNoise =
        std::dynamic_pointer_cast<sensors::GaussianNoiseModel>(noise);
      acc(gaussianNoise->GetBias());
    }

    // See comments in GaussianNoise function to explain these calculations.
    double sampleStdDev = g_sigma*biasStddev / sqrt(g_applyCount);
    EXPECT_NEAR(boost::accumulators::mean(acc), 0.0, sampleStdDev);

    double variance = biasStddev*biasStddev;
    double sampleVariance2 = 2 * variance*variance / (g_applyCount - 1);
    EXPECT_NEAR(boost::accumulators::variance(acc),
                variance, g_sigma*sqrt(sampleVariance2));
  }
}

TEST_F(NoiseTest, ApplyGaussianQuantized)
{
  double mean, stddev, biasMean, biasStddev, precision;

  // GAUSSIAN_QUANTIZED with zero means and standard deviations
  // should be the same as NONE
  mean = 0.0;
  stddev = 0.0;
  biasMean = 0.0;
  biasStddev = 0.0;
  precision = 0.0;
  {
    sensors::NoisePtr noise = sensors::NoiseFactory::NewNoiseModel(
        NoiseSdf("gaussian_quantized", mean, stddev, biasMean,
        biasStddev, precision));

    NoNoise(noise, g_applyCount);
  }

  // GAUSSIAN_QUANTIZED with non-zero means and standard deviations,
  // but no bias or precision
  mean = 10.0;
  stddev = 5.0;
  biasMean = 0.0;
  biasStddev = 0.0;
  precision = 0.0;
  {
    sensors::NoisePtr noise = sensors::NoiseFactory::NewNoiseModel(
        NoiseSdf("gaussian_quantized", mean, stddev, biasMean,
        biasStddev, precision));
    sensors::GaussianNoiseModelPtr gaussianNoise =
      std::dynamic_pointer_cast<sensors::GaussianNoiseModel>(noise);
    EXPECT_NEAR(gaussianNoise->GetBias(), 0.0, 1e-6);

    GaussianNoise(noise, g_applyCount);
  }

  // GAUSSIAN with non-zero mean, exact bias, and standard deviations
  // no precision specified
  mean = 10.0;
  stddev = 5.0;
  biasMean = 100.0;
  biasStddev = 0.0;
  precision = 0.0;
  {
    sensors::NoisePtr noise = sensors::NoiseFactory::NewNoiseModel(
        NoiseSdf("gaussian_quantized", mean, stddev, biasMean,
        biasStddev, precision));

    GaussianNoise(noise, g_applyCount);
  }

  // Test bias generation
  mean = 0.0;
  stddev = 0.0;
  biasMean = 0.0;
  biasStddev = 5.0;
  precision = 0.0;
  {
    boost::accumulators::accumulator_set<double,
      boost::accumulators::stats<boost::accumulators::tag::mean,
                                 boost::accumulators::tag::variance > > acc;

    for (unsigned int i = 0; i < g_applyCount; ++i)
    {
      sensors::NoisePtr noise = sensors::NoiseFactory::NewNoiseModel(
          NoiseSdf("gaussian_quantized", mean, stddev, biasMean,
          biasStddev, precision));
      sensors::GaussianNoiseModelPtr gaussianNoise =
        std::dynamic_pointer_cast<sensors::GaussianNoiseModel>(noise);
      acc(gaussianNoise->GetBias());
    }

    // See comments in GaussianNoise function to explain these calculations.
    double sampleStdDev = g_sigma*biasStddev / sqrt(g_applyCount);
    EXPECT_NEAR(boost::accumulators::mean(acc), 0.0, sampleStdDev);

    double variance = biasStddev*biasStddev;
    double sampleVariance2 = 2 * variance*variance / (g_applyCount - 1);
    EXPECT_NEAR(boost::accumulators::variance(acc),
                variance, g_sigma*sqrt(sampleVariance2));
  }

  // Test precision
  mean = 0.0;
  stddev = 0.0;
  biasMean = 0.0;
  biasStddev = 0.0;
  precision = 0.3;
  {
    sensors::NoisePtr noise = sensors::NoiseFactory::NewNoiseModel(
        NoiseSdf("gaussian_quantized", mean, stddev, biasMean,
        biasStddev, precision));

    EXPECT_NEAR(noise->Apply(0.32), 0.3, 1e-6);
    EXPECT_NEAR(noise->Apply(0.31), 0.3, 1e-6);
    EXPECT_NEAR(noise->Apply(0.30), 0.3, 1e-6);
    EXPECT_NEAR(noise->Apply(0.29), 0.3, 1e-6);
    EXPECT_NEAR(noise->Apply(0.28), 0.3, 1e-6);

    EXPECT_NEAR(noise->Apply(-12.92), -12.9, 1e-6);
    EXPECT_NEAR(noise->Apply(-12.91), -12.9, 1e-6);
    EXPECT_NEAR(noise->Apply(-12.90), -12.9, 1e-6);
    EXPECT_NEAR(noise->Apply(-12.89), -12.9, 1e-6);
    EXPECT_NEAR(noise->Apply(-12.88), -12.9, 1e-6);
  }
}

//////////////////////////////////////////////////
// Callback function for applying custom noise
double OnApplyCustomNoise(double _in)
{
  return _in*2;
}

TEST_F(NoiseTest, OnApplyNoise)
{
  // Verify that the custom callback function is called if noise type is
  // set to CUSTOM
  sensors::NoisePtr noise(new sensors::Noise(sensors::Noise::CUSTOM));
  ASSERT_TRUE(noise != nullptr);
  EXPECT_TRUE(noise->GetNoiseType() == sensors::Noise::CUSTOM);

  noise->SetCustomNoiseCallback(
    boost::bind(&OnApplyCustomNoise, _1));

  for (double i = 0; i < 100; i += 1)
  {
    double value = noise->Apply(i);
    EXPECT_DOUBLE_EQ(value, i*2);
  }
}

/////////////////////////////////////////////////
int main(int argc, char **argv)
{
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}