#include "aslam-tsvd-solver/aslam-tsvd-solver.h"

#include <algorithm>
#include <cmath>

#include <aslam/backend/CompressedColumnMatrix.hpp>
#include <cholmod.h>
#include <Eigen/Dense>
#include <glog/logging.h>
#include <sm/PropertyTree.hpp>
#include <truncated-svd-solver/cholmod-helpers.h>
#include <truncated-svd-solver/linear-algebra-helpers.h>

namespace aslam {
namespace backend {

AslamTruncatedSvdSolver::Options createTsvdOptionsFromPropertyTree(
    const sm::PropertyTree& config) {
  AslamTruncatedSvdSolver::Options tsvd_options;
  tsvd_options.columnScaling =
      config.getBool("columnScaling", tsvd_options.columnScaling);
  tsvd_options.epsNorm = config.getDouble("epsNorm", tsvd_options.epsNorm);
  tsvd_options.epsSVD = config.getDouble("epsSVD", tsvd_options.epsSVD);
  tsvd_options.epsQR = config.getDouble("epsQR", tsvd_options.epsQR);
  tsvd_options.svdTol = config.getDouble("svdTol", tsvd_options.svdTol);
  tsvd_options.qrTol = config.getDouble("qrTol", tsvd_options.qrTol);
  tsvd_options.verbose = config.getBool("verbose", tsvd_options.verbose);
  return tsvd_options;
}

AslamTruncatedSvdSolver::AslamTruncatedSvdSolver(const Options& options)
    : truncated_svd_solver::TruncatedSvdSolver(options) {}

AslamTruncatedSvdSolver::AslamTruncatedSvdSolver(const sm::PropertyTree& config)
    : AslamTruncatedSvdSolver(createTsvdOptionsFromPropertyTree(config)) {}

AslamTruncatedSvdSolver::~AslamTruncatedSvdSolver() {}

void AslamTruncatedSvdSolver::buildSystem(size_t numThreads,
                                          bool useMEstimator) {
  jacobian_builder_.buildSystem(numThreads, useMEstimator);
}

bool AslamTruncatedSvdSolver::solveSystem(Eigen::VectorXd& dx) {
  aslam::backend::CompressedColumnMatrix<std::ptrdiff_t>& Jt =
    jacobian_builder_.J_transpose();
  cholmod_sparse Jt_CS;
  Jt.getView(&Jt_CS);
  cholmod_sparse* J_CS = cholmod_l_transpose(&Jt_CS, 1, &cholmod_);
  if (J_CS == NULL)
    return false;
  cholmod_dense e_CD;
  truncated_svd_solver::eigenDenseToCholmodDenseView(_e, &e_CD);
  bool status = true;
  solve(J_CS, &e_CD, margStartIndex_, dx);
  if (tsvd_options_.verbose) {
    std::cout << "SVD rank: " << getSVDRank() << std::endl;
    std::cout << "SVD rank deficiency: " << getSVDRankDeficiency()
      << std::endl;
    std::cout << "QR rank: " << getQRRank() << std::endl;
    std::cout << "QR rank deficiency: " << getQRRankDeficiency()
      << std::endl;
    std::cout.precision(3);
    std::cout.width(4);
    std::cout << "Singular values:\n" << getSingularValues().transpose()
      << std::endl;
    std::cout << "V-matrix (observability basis, column vectors correspond to singular values)\n" << getMatrixV()
      << std::endl;
  }
  cholmod_l_free_sparse(&J_CS, &cholmod_);
  return status;
}

double AslamTruncatedSvdSolver::rhsJtJrhs() {
  return 0.0;
}

void AslamTruncatedSvdSolver::initMatrixStructureImplementation(const
    std::vector<aslam::backend::DesignVariable*>& dvs, const
    std::vector<aslam::backend::ErrorTerm*>& errors, bool
    useDiagonalConditioner) {
  CHECK(!useDiagonalConditioner) << "useDiagonalConditioner not supported in AslamTruncatedSvdSolver";
  clear();
  jacobian_builder_.initMatrixStructure(dvs, errors);
}

bool AslamTruncatedSvdSolver::analyzeMarginal() {
  aslam::backend::CompressedColumnMatrix<std::ptrdiff_t>& Jt =
      jacobian_builder_.J_transpose();
  cholmod_sparse Jt_CS;
  Jt.getView(&Jt_CS);
  truncated_svd_solver::SelfFreeingCholmodPtr<cholmod_sparse> J_CS(
      cholmod_l_transpose(&Jt_CS, 1, &cholmod_), cholmod_);
  if (J_CS == nullptr) {
    return false;
  }
  truncated_svd_solver::TruncatedSvdSolver::analyzeMarginal(
      J_CS, margStartIndex_);
  return true;
}

const aslam::backend::CompressedColumnMatrix<std::ptrdiff_t>&
  AslamTruncatedSvdSolver::getJacobianTranspose() const {
  return jacobian_builder_.J_transpose();
}

}  // namespace backend
}  // namespace aslam


/*
 * Copyright (C) 2014-2016 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 <string.h>
#include <boost/algorithm/string.hpp>

#include "gazebo/math/Vector3Stats.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/test/ServerFixture.hh"
#include "gazebo/test/helper_physics_generator.hh"

using namespace gazebo;

const double g_tolerance = 1e-4;

class PhysicsLinkTest : public ServerFixture,
                        public testing::WithParamInterface<const char*>
{
  /// \brief Test force adding functions.
  /// \param[in] _physicsEngine Type of physics engine to use.
  public: void AddForce(const std::string &_physicsEngine);

  /// \brief Use AddLinkForce on the given direction and then the opposite
  /// direction so they cancel out.
  /// \param[in] _world World pointer.
  /// \param[in] _link Link pointer.
  /// \param[in] _force Force expressed in link frame.
  /// \param[in] _offset Offset expressed in link frame, defaults to link
  /// origin.
  public: void AddLinkForceTwoWays(physics::WorldPtr _world,
      physics::LinkPtr _link, math::Vector3 _force,
      math::Vector3 _offset = math::Vector3::Zero);

  /// \brief Test GetWorldAngularMomentum.
  /// \param[in] _physicsEngine Type of physics engine to use.
  public: void GetWorldAngularMomentum(const std::string &_physicsEngine);

  /// \brief Test GetWorldEnergy* functions.
  /// \param[in] _physicsEngine Type of physics engine to use.
  public: void GetWorldEnergy(const std::string &_physicsEngine);

  /// \brief Test Link::GetWorldInertia* functions.
  /// \param[in] _physicsEngine Physics engine to use.
  public: void GetWorldInertia(const std::string &_physicsEngine);

  /// \brief Test wrench subscriber.
  /// \param[in] _physicsEngine Type of physics engine to use.
  public: void OnWrenchMsg(const std::string &_physicsEngine);

  /// \brief Test velocity setting functions.
  /// \param[in] _physicsEngine Type of physics engine to use.
  public: void SetVelocity(const std::string &_physicsEngine);
};

/////////////////////////////////////////////////
void PhysicsLinkTest::AddLinkForceTwoWays(physics::WorldPtr _world,
    physics::LinkPtr _link, math::Vector3 _force,
    math::Vector3 _offset)
{
  // Get state before adding force
  math::Vector3 linearVelWorld0 = _link->GetWorldCoGLinearVel();
  math::Vector3 angularVelWorld0 = _link->GetWorldAngularVel();
  math::Pose poseWorld0 = _link->GetWorldPose();

  // Add Link Force
  if (_offset == math::Vector3::Zero)
    _link->AddLinkForce(_force);
  else
    _link->AddLinkForce(_force, _offset);

  double dt = _world->GetPhysicsEngine()->GetMaxStepSize();
  _world->Step(1);

  int moreThanOneStep = 2;

  // Check force and torque (at CoG?) in world frame
  math::Vector3 forceWorld = poseWorld0.rot.RotateVector(_force);
  EXPECT_EQ(forceWorld, _link->GetWorldForce());

  math::Vector3 worldOffset = poseWorld0.rot.RotateVector(
      _offset - _link->GetInertial()->GetCoG());
  math::Vector3 torqueWorld = worldOffset.Cross(forceWorld);
  EXPECT_EQ(torqueWorld, _link->GetWorldTorque());

  // Check acceleration in world frame
  math::Vector3 oneStepLinearAccel =
      forceWorld/_link->GetInertial()->GetMass();
  EXPECT_EQ(oneStepLinearAccel, _link->GetWorldLinearAccel());

  // Compute angular accel by multiplying world torque
  // by inverse of world inertia matrix.
  // In this case, the gyroscopic coupling terms are zero
  // since the model is a unit box.
  math::Vector3 oneStepAngularAccel =
      _link->GetWorldInertiaMatrix().Inverse() * torqueWorld;
  EXPECT_EQ(oneStepAngularAccel, _link->GetWorldAngularAccel());

  // Check velocity in world frame
  math::Vector3 oneStepLinearVel = linearVelWorld0 + dt*oneStepLinearAccel;
  EXPECT_EQ(oneStepLinearVel, _link->GetWorldCoGLinearVel());

  math::Vector3 oneStepAngularVel = angularVelWorld0 + dt*oneStepAngularAccel;
  EXPECT_EQ(oneStepAngularVel, _link->GetWorldAngularVel());

  // Step forward and check again
  _world->Step(moreThanOneStep);

  // Check that force and torque are zero
  EXPECT_EQ(math::Vector3::Zero, _link->GetWorldForce());
  EXPECT_EQ(math::Vector3::Zero, _link->GetWorldTorque());

  // Check that acceleration is zero
  EXPECT_EQ(math::Vector3::Zero, _link->GetWorldLinearAccel());
  EXPECT_EQ(math::Vector3::Zero, _link->GetWorldAngularAccel());

  // Check that velocity hasn't changed
  EXPECT_EQ(oneStepLinearVel, _link->GetWorldCoGLinearVel());
  EXPECT_EQ(oneStepAngularVel, _link->GetWorldAngularVel());

  // Add opposing force in link frame and check that link is back to initial
  // velocity
  if (_offset == math::Vector3::Zero)
    _link->AddLinkForce(-_force);
  else
    _link->AddLinkForce(-_force, _offset);

  _world->Step(moreThanOneStep);
  EXPECT_EQ(math::Vector3::Zero, _link->GetWorldForce());
  EXPECT_EQ(math::Vector3::Zero, _link->GetWorldTorque());
  EXPECT_EQ(linearVelWorld0, _link->GetWorldCoGLinearVel());
  EXPECT_EQ(angularVelWorld0, _link->GetWorldAngularVel());
  EXPECT_EQ(math::Vector3::Zero, _link->GetWorldLinearAccel());
  EXPECT_EQ(math::Vector3::Zero, _link->GetWorldAngularAccel());
}

/////////////////////////////////////////////////
void PhysicsLinkTest::AddForce(const std::string &_physicsEngine)
{
  // TODO bullet, dart and simbody currently fail this test
  if (_physicsEngine != "ode")
  {
    gzerr << "Aborting AddForce test for Bullet, DART and Simbody. "
          << "See issues #1476, #1477, and #1478."
          << std::endl;
    return;
  }

  Load("worlds/blank.world", true, _physicsEngine);
  physics::WorldPtr world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

  // check the physics engine
  physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
  ASSERT_TRUE(physics != NULL);
  EXPECT_EQ(physics->GetType(), _physicsEngine);
  double dt = physics->GetMaxStepSize();
  EXPECT_GT(dt, 0);

  // disable gravity
  world->SetGravity(ignition::math::Vector3d::Zero);

  // Spawn a box
  math::Vector3 size(1, 1, 1);
  SpawnBox("box", size, math::Vector3::Zero, math::Vector3::Zero, false);
  physics::ModelPtr model = world->GetModel("box");
  ASSERT_TRUE(model != NULL);
  physics::LinkPtr link = model->GetLink();
  ASSERT_TRUE(link != NULL);

  // Check that link is at rest
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearVel());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularVel());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearAccel());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularAccel());

  // Add force at link frame
  gzdbg << "World == link == inertial frames, no offset" << std::endl;
  EXPECT_EQ(math::Pose(), link->GetWorldPose());
  EXPECT_EQ(math::Pose(), link->GetWorldInertialPose());
  this->AddLinkForceTwoWays(world, link, math::Vector3(1, 20, 31));

  gzdbg << "World != link == inertial frames, no offset" << std::endl;
  model->SetLinkWorldPose(math::Pose(math::Vector3(2, 3, 4),
                          math::Vector3(0, M_PI/2.0, 1)), link);
  EXPECT_NE(math::Pose(), link->GetWorldPose());
  EXPECT_EQ(link->GetWorldPose(), link->GetWorldInertialPose());
  this->AddLinkForceTwoWays(world, link, math::Vector3(-1, 10, 5));

  gzdbg << "World == link == inertial frames, with offset" << std::endl;
  model->SetLinkWorldPose(math::Pose(), link);
  EXPECT_EQ(math::Pose(), link->GetWorldPose());
  EXPECT_EQ(math::Pose(), link->GetWorldInertialPose());
  this->AddLinkForceTwoWays(world, link, math::Vector3(5, 4, 3),
      math::Vector3(-2, 1, 0));

  gzdbg << "World == link != inertial frames, no offset" << std::endl;
  model->SetLinkWorldPose(math::Pose(), link);
  math::Pose inertialPose = math::Pose(math::Vector3(1, 5, 8),
      math::Vector3(M_PI/3.0, M_PI*1.5, M_PI/4));
  link->GetInertial()->SetCoG(inertialPose);
  EXPECT_EQ(math::Pose(), link->GetWorldPose());
  EXPECT_EQ(inertialPose, link->GetWorldInertialPose());
  this->AddLinkForceTwoWays(world, link, math::Vector3(1, 2, 1));

  gzdbg << "World != link != inertial frames, with offset" << std::endl;
  model->SetLinkWorldPose(math::Pose(math::Vector3(5, 10, -4),
                          math::Vector3(0, M_PI/2.0, M_PI/6)), link);
  inertialPose = math::Pose(math::Vector3(0, -5, 10),
      math::Vector3(0, 2.0*M_PI, M_PI/3));
  link->GetInertial()->SetCoG(inertialPose);
  this->AddLinkForceTwoWays(world, link, math::Vector3(1, 2, 1),
      math::Vector3(-2, 0.5, 1));

  gzdbg << "World != link != inertial frames, with offset and initial vel"
      << std::endl;
  model->SetLinkWorldPose(math::Pose(math::Vector3(-1.5, 0.8, 3),
                          math::Vector3(-M_PI/4.5, M_PI/3.0, M_PI*1.2)), link);
  inertialPose = math::Pose(math::Vector3(1, 0, -5.6),
      math::Vector3(M_PI/9, 0, M_PI*3));
  link->GetInertial()->SetCoG(inertialPose);
  link->SetLinearVel(math::Vector3(2, -0.1, 5));
  link->SetAngularVel(math::Vector3(-M_PI/10, 0, 0.0001));
  this->AddLinkForceTwoWays(world, link, math::Vector3(-3, 2.5, -15),
      math::Vector3(-6, -1, -0.2));
}

/////////////////////////////////////////////////
// GetWorldAngularMomentum:
// Spawn box and verify Link::GetWorldAngularMomentum functions
// Make dimensions unequal and give angular velocity that causes
// gyroscopic tumbling.
void PhysicsLinkTest::GetWorldAngularMomentum(const std::string &_physicsEngine)
{
  // Load a blank world (no ground plane)
  Load("worlds/blank.world", true, _physicsEngine);
  auto world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

  // Verify physics engine type
  auto physics = world->GetPhysicsEngine();
  ASSERT_TRUE(physics != NULL);
  EXPECT_EQ(physics->GetType(), _physicsEngine);

  // disable gravity
  world->SetGravity(ignition::math::Vector3d::Zero);

  physics::ModelPtr model;
  {
    // Box size
    const double dx = 0.1;
    const double dy = 0.4;
    const double dz = 0.9;
    const double mass = 10.0;

    msgs::Model msgModel;
    msgModel.set_name(this->GetUniqueString("model"));
    msgs::AddBoxLink(msgModel, mass, ignition::math::Vector3d(dx, dy, dz));
    model = this->SpawnModel(msgModel);
  }
  ASSERT_TRUE(model != NULL);

  // inertia matrix, recompute if dimensions change
  const double Ixx = 0.80833333;
  const double Iyy = 0.68333333;
  const double Izz = 0.14166667;
  const math::Matrix3 I0(Ixx, 0.0, 0.0
                       , 0.0, Iyy, 0.0
                       , 0.0, 0.0, Izz);

  // Since Ixx > Iyy > Izz,
  // angular velocity with large y component
  // will cause gyroscopic tumbling
  const math::Vector3 w0(1e-3, 1.5e0, 1.5e-2);
  model->SetAngularVel(w0);

  // Get link and verify inertia and initial velocity
  auto link = model->GetLink();
  ASSERT_TRUE(link != NULL);
  ASSERT_EQ(w0, link->GetWorldAngularVel());
  ASSERT_EQ(I0, link->GetWorldInertiaMatrix());

  // Compute initial angular momentum
  const math::Vector3 H0(I0 * w0);
  ASSERT_EQ(H0, link->GetWorldAngularMomentum());
  const double H0mag = H0.GetLength();

  math::Vector3Stats angularMomentumError;
  const std::string stat("maxAbs");
  EXPECT_TRUE(angularMomentumError.InsertStatistic(stat));
  const int steps = 5000;
  for (int i = 0; i < steps; ++i)
  {
    world->Step(1);
    math::Vector3 H = link->GetWorldAngularMomentum();
    angularMomentumError.InsertData((H - H0) / H0mag);
  }
  if (_physicsEngine == "dart")
  {
    gzdbg << "dart has higher error for this test (see #1487), "
          << "so a larger tolerance is used."
          << std::endl;
    EXPECT_LT(angularMomentumError.Mag().Map()[stat], g_tolerance * 1e3);
  }
  else
  {
    EXPECT_LT(angularMomentumError.Mag().Map()[stat], g_tolerance * 10);
  }

  RecordProperty("engine", _physicsEngine);
  this->Record("angularMomentumError", angularMomentumError);
}

/////////////////////////////////////////////////
void PhysicsLinkTest::GetWorldEnergy(const std::string &_physicsEngine)
{
  Load("worlds/empty.world", true, _physicsEngine);
  physics::WorldPtr world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

  // check the physics engine
  physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
  ASSERT_TRUE(physics != NULL);
  EXPECT_EQ(physics->GetType(), _physicsEngine);
  double dt = physics->GetMaxStepSize();
  EXPECT_GT(dt, 0);

  // Get gravity magnitude
  double g = physics->GetGravity().GetLength();

  // Spawn a box
  double z0 = 10.0;
  math::Vector3 size(1, 1, 1);
  math::Vector3 pos0(0, 0, z0 + size.z / 2);
  SpawnBox("box", size, pos0, math::Vector3::Zero, false);
  physics::ModelPtr model = world->GetModel("box");
  ASSERT_TRUE(model != NULL);
  physics::LinkPtr link = model->GetLink();
  ASSERT_TRUE(link != NULL);

  // Get initial energy
  double energy0 = link->GetWorldEnergy();
  EXPECT_NEAR(link->GetWorldEnergyKinetic(), 0, g_tolerance);

  double totalTime = sqrt(2*z0/g)*0.95;
  unsigned int stepSize = 10;
  unsigned int steps = floor(totalTime / (dt*stepSize));
  for (unsigned int i = 0; i < steps; ++i)
  {
    world->Step(stepSize);
    double energy = link->GetWorldEnergy();
    EXPECT_NEAR(energy / energy0, 1.0, g_tolerance * 10);
  }
}

/////////////////////////////////////////////////
// GetWorldInertia:
// Spawn boxes and verify Link::GetWorldInertia* functions
void PhysicsLinkTest::GetWorldInertia(const std::string &_physicsEngine)
{
  // Load a blank world (no ground plane)
  Load("worlds/blank.world", true, _physicsEngine);
  auto world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

  // Verify physics engine type
  auto physics = world->GetPhysicsEngine();
  ASSERT_TRUE(physics != NULL);
  EXPECT_EQ(physics->GetType(), _physicsEngine);

  // disable gravity
  world->SetGravity(ignition::math::Vector3d::Zero);

  // Box size
  const double dx = 1.0;
  const double dy = 4.0;
  const double dz = 9.0;
  const double mass = 10.0;
  const double angle = M_PI / 3.0;

  const unsigned int testCases = 4;
  for (unsigned int i = 0; i < testCases; ++i)
  {
    // Use msgs::AddBoxLink
    msgs::Model msgModel;
    math::Pose modelPose, linkPose, inertialPose;

    msgModel.set_name(this->GetUniqueString("model"));
    msgs::AddBoxLink(msgModel, mass, ignition::math::Vector3d(dx, dy, dz));
    modelPose.pos.x = i * dz;
    modelPose.pos.z = dz;

    // i=0: rotated model pose
    //  expect inertial pose to match model pose
    if (i == 0)
    {
      modelPose.rot.SetFromEuler(0.0, 0.0, angle);
    }
    // i=1: rotated link pose
    //  expect inertial pose to match link pose
    else if (i == 1)
    {
      linkPose.rot.SetFromEuler(0.0, 0.0, angle);
    }
    // i=2: rotated inertial pose
    //  expect inertial pose to differ from link pose
    else if (i == 2)
    {
      inertialPose.rot.SetFromEuler(0.0, 0.0, angle);
    }
    // i=3: offset inertial pose
    //  expect inertial pose to differ from link pose
    else if (i == 3)
    {
      inertialPose.pos.Set(1, 1, 1);
    }

    {
      auto msgLink = msgModel.mutable_link(0);
      auto msgInertial = msgLink->mutable_inertial();

      msgs::Set(msgModel.mutable_pose(), modelPose.Ign());
      msgs::Set(msgLink->mutable_pose(), linkPose.Ign());
      msgs::Set(msgInertial->mutable_pose(), inertialPose.Ign());
    }

    auto model = this->SpawnModel(msgModel);
    ASSERT_TRUE(model != NULL);

    auto link = model->GetLink();
    ASSERT_TRUE(link != NULL);

    EXPECT_EQ(model->GetWorldPose(), modelPose);
    EXPECT_EQ(link->GetWorldPose(), linkPose + modelPose);
    EXPECT_EQ(link->GetWorldInertialPose(),
              inertialPose + linkPose + modelPose);

    // i=0: rotated model pose
    //  expect inertial pose to match model pose
    if (i == 0)
    {
      EXPECT_EQ(model->GetWorldPose(),
                link->GetWorldInertialPose());
    }
    // i=1: rotated link pose
    //  expect inertial pose to match link pose
    else if (i == 1)
    {
      EXPECT_EQ(link->GetWorldPose(),
                link->GetWorldInertialPose());
    }
    // i=2: offset and rotated inertial pose
    //  expect inertial pose to differ from link pose
    else if (i == 2)
    {
      EXPECT_EQ(link->GetWorldPose().pos,
                link->GetWorldInertialPose().pos);
    }
    // i=3: offset inertial pose
    //  expect inertial pose to differ from link pose
    else if (i == 3)
    {
      EXPECT_EQ(link->GetWorldPose().pos + inertialPose.pos,
                link->GetWorldInertialPose().pos);
    }

    // Expect rotated inertia matrix
    math::Matrix3 inertia = link->GetWorldInertiaMatrix();
    if (i == 3)
    {
      EXPECT_NEAR(inertia[0][0], 80.8333, 1e-4);
      EXPECT_NEAR(inertia[1][1], 68.3333, 1e-4);
      EXPECT_NEAR(inertia[2][2], 14.1667, 1e-4);
      for (int row = 0; row < 3; ++row)
        for (int col = 0; col < 3; ++col)
          if (row != col)
            EXPECT_NEAR(inertia[row][col], 0.0, g_tolerance);
    }
    else
    {
      EXPECT_NEAR(inertia[0][0], 71.4583, 1e-4);
      EXPECT_NEAR(inertia[1][1], 77.7083, 1e-4);
      EXPECT_NEAR(inertia[2][2], 14.1667, 1e-4);
      EXPECT_NEAR(inertia[0][1],  5.4126, 1e-4);
      EXPECT_NEAR(inertia[1][0],  5.4126, 1e-4);
      EXPECT_NEAR(inertia[0][2], 0, g_tolerance);
      EXPECT_NEAR(inertia[2][0], 0, g_tolerance);
      EXPECT_NEAR(inertia[1][2], 0, g_tolerance);
      EXPECT_NEAR(inertia[2][1], 0, g_tolerance);
    }

    // For 0-2, apply torque and expect equivalent response
    if (i <= 2)
    {
      for (int step = 0; step < 50; ++step)
      {
        link->SetTorque(math::Vector3(100, 0, 0));
        world->Step(1);
      }
      if (_physicsEngine.compare("dart") == 0)
      {
        gzerr << "Dart fails this portion of the test (#1090)" << std::endl;
      }
      else
      {
        math::Vector3 vel = link->GetWorldAngularVel();
        EXPECT_NEAR(vel.x,  0.0703, g_tolerance);
        EXPECT_NEAR(vel.y, -0.0049, g_tolerance);
        EXPECT_NEAR(vel.z,  0.0000, g_tolerance);
      }
    }
  }
}

/////////////////////////////////////////////////
void PhysicsLinkTest::OnWrenchMsg(const std::string &_physicsEngine)
{
  // TODO bullet, dart and simbody currently fail this test
  if (_physicsEngine != "ode")
  {
    gzerr << "Aborting OnWrenchMsg test for Bullet, DART and Simbody. "
          << "Because of issues #1476, #1477, and #1478."
          << std::endl;
    return;
  }

  Load("worlds/blank.world", true, _physicsEngine);
  physics::WorldPtr world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

  // check the physics engine
  physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
  ASSERT_TRUE(physics != NULL);
  EXPECT_EQ(physics->GetType(), _physicsEngine);
  double dt = physics->GetMaxStepSize();
  EXPECT_GT(dt, 0);

  // disable gravity
  world->SetGravity(ignition::math::Vector3d::Zero);

  // Spawn a box
  math::Vector3 size(1, 1, 1);
  SpawnBox("box", size, math::Vector3::Zero, math::Vector3::Zero, false);
  physics::ModelPtr model = world->GetModel("box");
  ASSERT_TRUE(model != NULL);
  physics::LinkPtr link = model->GetLink();
  ASSERT_TRUE(link != NULL);

  // Check that link is at rest
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearVel());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularVel());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearAccel());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularAccel());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldForce());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldTorque());

  // Publish wrench message
  std::string topicName = "~/" + link->GetScopedName() + "/wrench";
  boost::replace_all(topicName, "::", "/");
  transport::PublisherPtr wrenchPub =
    this->node->Advertise<msgs::Wrench>(topicName);

  msgs::Wrench msg;

  std::vector<math::Vector3> forces;
  std::vector<math::Vector3> torques;
  std::vector<math::Vector3> forceOffsets;

  // Only force
  forces.push_back(math::Vector3(1, 0, 0));
  torques.push_back(math::Vector3::Zero);
  forceOffsets.push_back(math::Vector3::Zero);

  // Only force, with an offset
  forces.push_back(math::Vector3(5.2, 0.1, 10));
  torques.push_back(math::Vector3::Zero);
  forceOffsets.push_back(math::Vector3(2.1, 1, -0.6));

  // Only torque
  forces.push_back(math::Vector3::Zero);
  torques.push_back(math::Vector3(-0.2, 5, 0));
  forceOffsets.push_back(math::Vector3::Zero);

  // All fields set
  forces.push_back(math::Vector3(5, 6, -0.9));
  torques.push_back(math::Vector3(-0.2, 5, 0));
  forceOffsets.push_back(math::Vector3(-1, -4, -0.8));

  for (unsigned int i = 0; i < forces.size(); ++i)
  {
    gzdbg << "Testing force: " << forces[i].x << ", "
                               << forces[i].y << ", "
                               << forces[i].z <<
                   " torque: " << torques[i].x << ", "
                               << torques[i].y << ", "
                               << torques[i].z <<
             " force offset: " << forceOffsets[i].x << ", "
                               << forceOffsets[i].y << ", "
                               << forceOffsets[i].z << std::endl;

    // Publish message
    msgs::Set(msg.mutable_force(), forces[i].Ign());
    msgs::Set(msg.mutable_torque(), torques[i].Ign());
    // Leave optional field unset if it's zero
    if (forceOffsets[i] != math::Vector3::Zero)
      msgs::Set(msg.mutable_force_offset(), forceOffsets[i].Ign());

    wrenchPub->Publish(msg);

    // Calculate expected values
    math::Vector3 forceWorld = forces[i];
    math::Vector3 worldOffset = forceOffsets[i] - link->GetInertial()->GetCoG();
    math::Vector3 torqueWorld = worldOffset.Cross(forces[i]) + torques[i];

    // Wait for message to be received
    while (link->GetWorldForce() != forceWorld ||
           link->GetWorldTorque() != torqueWorld)
    {
      world->Step(1);
      common::Time::MSleep(1);
    }

    // Check force and torque (at CoG?) in world frame
    EXPECT_EQ(link->GetWorldForce(), forceWorld);
    EXPECT_EQ(link->GetWorldTorque(), torqueWorld);

    // Reset link's physics states
    link->ResetPhysicsStates();
    link->SetWorldPose(math::Pose());
    world->Step(1);

    EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearVel());
    EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularVel());
    EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearAccel());
    EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularAccel());
    EXPECT_EQ(math::Vector3::Zero, link->GetWorldForce());
    EXPECT_EQ(math::Vector3::Zero, link->GetWorldTorque());
  }
}

/////////////////////////////////////////////////
void PhysicsLinkTest::SetVelocity(const std::string &_physicsEngine)
{
  Load("worlds/blank.world", true, _physicsEngine);
  physics::WorldPtr world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

  // check the physics engine
  physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
  ASSERT_TRUE(physics != NULL);
  EXPECT_EQ(physics->GetType(), _physicsEngine);
  double dt = physics->GetMaxStepSize();
  EXPECT_GT(dt, 0);

  // disable gravity
  world->SetGravity(ignition::math::Vector3d::Zero);

  // Spawn a box
  math::Vector3 size(1, 1, 1);
  math::Vector3 pos0(0, 0, 1);
  SpawnBox("box", size, pos0, math::Vector3::Zero, false);
  physics::ModelPtr model = world->GetModel("box");
  ASSERT_TRUE(model != NULL);
  physics::LinkPtr link = model->GetLink();
  ASSERT_TRUE(link != NULL);

  // Set upward velocity and check
  math::Vector3 vel(0, 0, 1);
  link->SetLinearVel(vel);
  world->Step(1);
  EXPECT_EQ(vel, link->GetWorldLinearVel());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularVel());

  // Step forward and check velocity again
  world->Step(44);
  double time = world->GetSimTime().Double();
  EXPECT_EQ(vel, link->GetWorldLinearVel());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularVel());

  // check position
  math::Vector3 pos = link->GetWorldPose().pos;
  EXPECT_EQ(pos0 + time*vel, pos);

  // Set velocity to zero
  link->SetLinearVel(math::Vector3::Zero);
  world->Step(1);
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearVel());
  EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularVel());
  EXPECT_EQ(pos0 + time*vel, pos);

  // Start translating and rotating
  vel.Set(1, 1, 0);
  math::Vector3 vel2(0, 2.0, 0);
  link->SetLinearVel(vel);
  link->SetAngularVel(vel2);

  // Step once
  world->Step(1);
  EXPECT_EQ(vel, link->GetWorldLinearVel());
  EXPECT_EQ(vel2, link->GetWorldAngularVel());

  // test linear velocity at specific point in space
  math::Vector3 offset(0, 0, -0.5);
  math::Vector3 vel3 = link->GetWorldLinearVel(offset, math::Quaternion());
  EXPECT_NEAR(vel3.x, 0.0, g_tolerance);
  EXPECT_NEAR(vel3.y, 1.0, g_tolerance);
  EXPECT_NEAR(vel3.z, 0.0, g_tolerance);

  // check rotation
  math::Vector3 rpy = link->GetWorldPose().rot.GetAsEuler();
  EXPECT_NEAR(rpy.x, 0.0, g_tolerance);
  EXPECT_NEAR(rpy.y, vel2.y*dt, g_tolerance);
  EXPECT_NEAR(rpy.z, 0.0, g_tolerance);
}

/////////////////////////////////////////////////
TEST_P(PhysicsLinkTest, AddForce)
{
  AddForce(GetParam());
}

/////////////////////////////////////////////////
TEST_P(PhysicsLinkTest, GetWorldAngularMomentum)
{
  GetWorldAngularMomentum(GetParam());
}

/////////////////////////////////////////////////
TEST_P(PhysicsLinkTest, GetWorldEnergy)
{
  GetWorldEnergy(GetParam());
}

/////////////////////////////////////////////////
TEST_P(PhysicsLinkTest, GetWorldInertia)
{
  GetWorldInertia(GetParam());
}

/////////////////////////////////////////////////
TEST_P(PhysicsLinkTest, OnWrenchMsg)
{
  OnWrenchMsg(GetParam());
}

/////////////////////////////////////////////////
TEST_P(PhysicsLinkTest, SetVelocity)
{
  SetVelocity(GetParam());
}

INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsLinkTest,
                        PHYSICS_ENGINE_VALUES);

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


// [[Rcpp::depends(BH)]]

#include <iostream>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <Rcpp.h>
#include <vector>
#include <algorithm>    // std::max/min
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;
using namespace Rcpp;


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 * Created on February 17, 2021, 4:22 PM
 */

#include <gtest/gtest.h>

#include <Eigen/Eigen>
#include <cmath>
#include <functional>
#include <limits>
#include <memory>
#include <vector>

#include "BaseLib/ConfigTree.h"
#include "MaterialLib/MPL/Medium.h"
#include "MaterialLib/MPL/Properties/ThermalConductivity/CreateSoilThermalConductivitySomerton.h"
#include "MaterialLib/MPL/Properties/ThermalConductivity/SoilThermalConductivitySomerton.h"
#include "MaterialLib/MPL/Utils/FormEigenTensor.h"
#include "ParameterLib/ConstantParameter.h"
#include "ParameterLib/CoordinateSystem.h"
#include "Tests/TestTools.h"

namespace MPL = MaterialPropertyLib;

std::unique_ptr<MaterialPropertyLib::Property>
createTestSoilThermalConductivitySomertonProperty(
    const char xml[], int const geometry_dimension,
    std::vector<std::unique_ptr<ParameterLib::ParameterBase>> const& parameters,
    ParameterLib::CoordinateSystem const* const local_coordinate_system,
    std::function<std::unique_ptr<MaterialPropertyLib::Property>(
        int const geometry_dimension,
        BaseLib::ConfigTree const& config,
        std::vector<std::unique_ptr<ParameterLib::ParameterBase>> const&
            parameters,
        ParameterLib::CoordinateSystem const* const local_coordinate_system)>
        createProperty)
{
    auto ptree = Tests::readXml(xml);
    BaseLib::ConfigTree conf(std::move(ptree), "", BaseLib::ConfigTree::onerror,
                             BaseLib::ConfigTree::onwarning);
    auto const& sub_config = conf.getConfigSubtree("property");
    // Parsing the property name:
    auto const property_name =
        sub_config.getConfigParameter<std::string>("name");

    return createProperty(geometry_dimension, sub_config, parameters,
                          local_coordinate_system);
}

TEST(MaterialPropertyLib, SoilThermalConductivitySomerton1D)
{
    const char xml[] =
        "<property>"
        "   <name>thermal_conductivity</name>"
        "   <type>SoilThermalConductivitySomerton</type>"
        "   <dry_thermal_conductivity>k_T_dry</dry_thermal_conductivity>"
        "   <wet_thermal_conductivity>k_T_wet</wet_thermal_conductivity>"
        "</property>";

    double const k_T_dry = 0.1;
    double const k_T_wet = 0.3;
    std::vector<std::unique_ptr<ParameterLib::ParameterBase>> parameters;
    parameters.push_back(
        std::make_unique<ParameterLib::ConstantParameter<double>>("k_T_dry",
                                                                  k_T_dry));
    parameters.push_back(
        std::make_unique<ParameterLib::ConstantParameter<double>>("k_T_wet",
                                                                  k_T_wet));

    const int dimemsion = 1;
    std::unique_ptr<MPL::Property> const k_T_ptr =
        createTestSoilThermalConductivitySomertonProperty(
            xml, dimemsion, parameters, nullptr,
            MPL::createSoilThermalConductivitySomerton);

    MPL::Property const& k_T_property = *k_T_ptr;

    MPL::VariableArray variable_array;
    ParameterLib::SpatialPosition const pos;
    double const t = std::numeric_limits<double>::quiet_NaN();
    double const dt = std::numeric_limits<double>::quiet_NaN();

    // For S <= 0.0
    {
        double const S_less_than_zero[] = {-1, 0.0};
        for (int i = 0; i < 2; i++)
        {
            variable_array[static_cast<int>(MPL::Variable::liquid_saturation)] =
                S_less_than_zero[i];

            double const computed_k_T =
                k_T_property.template value<double>(variable_array, pos, t, dt);
            ASSERT_LE(std::fabs(k_T_dry - computed_k_T), 1e-10)
                << "for expected thermal conductivity " << k_T_dry
                << " and for computed thermal conductivity." << computed_k_T;

            double const computed_dk_T_dS =
                k_T_property.template dValue<double>(
                    variable_array,
                    MaterialPropertyLib::Variable::liquid_saturation, pos, t,
                    dt);

            ASSERT_LE(std::fabs(0.0 - computed_dk_T_dS), 1e-10)
                << "for expected derivative of thermal conductivity with "
                   "respect to saturation "
                << 0.0
                << " and for computed derivative of thermal conductivity with "
                   "respect to saturation."
                << computed_dk_T_dS;
        }
    }
    // For S > 1.0
    {
        variable_array[static_cast<int>(MPL::Variable::liquid_saturation)] =
            2.0;

        double const computed_k_T =
            k_T_property.template value<double>(variable_array, pos, t, dt);
        ASSERT_LE(std::fabs(k_T_wet - computed_k_T), 1e-10)
            << "for expected thermal conductivity " << k_T_wet
            << " and for computed thermal conductivity." << computed_k_T;

        double const computed_dk_T_dS = k_T_property.template dValue<double>(
            variable_array, MaterialPropertyLib::Variable::liquid_saturation,
            pos, t, dt);

        ASSERT_LE(std::fabs(0.0 - computed_dk_T_dS), 1e-10)
            << "for expected derivative of thermal conductivity with respect "
               "to saturation "
            << 0.0
            << " and for computed derivative of thermal conductivity with "
               "respect to saturation."
            << computed_dk_T_dS;
    }

    // For S in (0, 1)
    std::vector<double> const S_L = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8};
    std::vector<double> const expected_k_T = {
        1.632456e-01, 1.894427e-01, 2.095445e-01, 2.264911e-01,
        2.414214e-01, 2.549193e-01, 2.673320e-01, 2.788854e-01};

    double const perturbation = 1.e-6;
    for (std::size_t i = 0; i < S_L.size(); i++)
    {
        variable_array[static_cast<int>(MPL::Variable::liquid_saturation)] =
            S_L[i];

        double const k_T_i =
            k_T_property.template value<double>(variable_array, pos, t, dt);
        ASSERT_LE(std::fabs(expected_k_T[i] - k_T_i), 1e-7)
            << "for expected thermal conductivity " << expected_k_T[i]
            << " and for computed thermal conductivity." << k_T_i;

        variable_array[static_cast<int>(MPL::Variable::liquid_saturation)] =
            S_L[i] - perturbation;
        double const k_T_i_0 =
            k_T_property.template value<double>(variable_array, pos, t, dt);

        variable_array[static_cast<int>(MPL::Variable::liquid_saturation)] =
            S_L[i] + perturbation;

        double const k_T_i_1 =
            k_T_property.template value<double>(variable_array, pos, t, dt);

        double const approximated_dk_T_dS =
            0.5 * (k_T_i_1 - k_T_i_0) / perturbation;
        double const analytic_dk_T_dS = k_T_property.template dValue<double>(
            variable_array, MaterialPropertyLib::Variable::liquid_saturation,
            pos, t, dt);

        ASSERT_LE(std::fabs(analytic_dk_T_dS - approximated_dk_T_dS), 1e-5)
            << "for expected derivative of thermal conductivity with respect "
               "to saturation "
            << analytic_dk_T_dS
            << " and for computed derivative of thermal conductivity with "
               "respect to saturation."
            << approximated_dk_T_dS;
    }
}

TEST(MaterialPropertyLib, SoilThermalConductivitySomerton3D)
{
    const char xml[] =
        "<property>"
        "   <name>thermal_conductivity</name>"
        "   <type>SoilThermalConductivitySomerton</type>"
        "   <dry_thermal_conductivity>k_T_dry</dry_thermal_conductivity>"
        "   <wet_thermal_conductivity>k_T_wet</wet_thermal_conductivity>"
        "</property>";

    std::vector<double> k_T_dry{0.1, 0.1, 0.1};
    std::vector<double> k_T_wet{0.3, 0.3, 0.3};
    std::vector<std::unique_ptr<ParameterLib::ParameterBase>> parameters;
    parameters.push_back(
        std::make_unique<ParameterLib::ConstantParameter<double>>("k_T_dry",
                                                                  k_T_dry));
    parameters.push_back(
        std::make_unique<ParameterLib::ConstantParameter<double>>("k_T_wet",
                                                                  k_T_wet));

    const int dimemsion = 3;
    std::unique_ptr<MPL::Property> const k_T_ptr =
        createTestSoilThermalConductivitySomertonProperty(
            xml, dimemsion, parameters, nullptr,
            MPL::createSoilThermalConductivitySomerton);

    MPL::Property const& k_T_property = *k_T_ptr;

    MPL::VariableArray variable_array;
    ParameterLib::SpatialPosition const pos;
    double const t = std::numeric_limits<double>::quiet_NaN();
    double const dt = std::numeric_limits<double>::quiet_NaN();

    // For S <= 0.0
    {
        double const S_less_than_zero[] = {-1, 0.0};
        for (int i = 0; i < 2; i++)
        {
            variable_array[static_cast<int>(MPL::Variable::liquid_saturation)] =
                S_less_than_zero[i];

            auto const computed_k_T = MPL::formEigenTensor<3>(
                k_T_property.value(variable_array, pos, t, dt));

            for (std::size_t k = 0; k < k_T_dry.size(); k++)
            {
                ASSERT_LE(std::fabs(k_T_dry[k] - computed_k_T(k, k)), 1e-10)
                    << "for expected thermal conductivity " << k_T_dry[k]
                    << " and for computed thermal conductivity."
                    << computed_k_T(k, k);
            }

            auto const computed_dk_T_dS =
                MPL::formEigenTensor<3>(k_T_property.dValue(
                    variable_array,
                    MaterialPropertyLib::Variable::liquid_saturation, pos, t,
                    dt));

            for (std::size_t k = 0; k < k_T_dry.size(); k++)
            {
                ASSERT_LE(std::fabs(0.0 - computed_dk_T_dS(k, k)), 1e-10)
                    << "for expected derivative of thermal conductivity with "
                       "respect to saturation "
                    << 0.0
                    << " and for computed derivative of thermal conductivity "
                       "with respect to saturation."
                    << computed_dk_T_dS(k, k);
            }
        }
    }
    // For S > 1.0
    {
        variable_array[static_cast<int>(MPL::Variable::liquid_saturation)] =
            2.0;

        auto const computed_k_T = MPL::formEigenTensor<3>(
            k_T_property.value(variable_array, pos, t, dt));

        for (std::size_t k = 0; k < k_T_dry.size(); k++)
        {
            ASSERT_LE(std::fabs(k_T_wet[k] - computed_k_T(k, k)), 1e-10)
                << "for expected thermal conductivity " << k_T_wet[k]
                << " and for computed thermal conductivity."
                << computed_k_T(k, k);
        }

        auto const computed_dk_T_dS =
            MPL::formEigenTensor<3>(k_T_property.dValue(
                variable_array,
                MaterialPropertyLib::Variable::liquid_saturation, pos, t, dt));

        for (std::size_t k = 0; k < k_T_dry.size(); k++)
        {
            ASSERT_LE(std::fabs(0.0 - computed_dk_T_dS(k, k)), 1e-10)
                << "for expected derivative of thermal conductivity with "
                   "respect to saturation "
                << 0.0
                << " and for computed derivative of thermal conductivity with "
                   "respect to saturation."
                << computed_dk_T_dS(k, k);
        }
    }

    // For S in (0, 1)
    std::vector<double> const S_L = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8};
    std::vector<double> const expected_k_T = {
        1.632456e-01, 1.894427e-01, 2.095445e-01, 2.264911e-01,
        2.414214e-01, 2.549193e-01, 2.673320e-01, 2.788854e-01};

    double const perturbation = 1.e-6;
    for (std::size_t i = 0; i < S_L.size(); i++)
    {
        variable_array[static_cast<int>(MPL::Variable::liquid_saturation)] =
            S_L[i];

        auto const k_T_i = MPL::formEigenTensor<3>(
            k_T_property.value(variable_array, pos, t, dt));

        for (std::size_t k = 0; k < k_T_dry.size(); k++)
        {
            ASSERT_LE(std::fabs(expected_k_T[i] - k_T_i(k, k)), 1e-7)
                << "for expected thermal conductivity " << expected_k_T[i]
                << " and for computed thermal conductivity." << k_T_i(k, k);
        }

        variable_array[static_cast<int>(MPL::Variable::liquid_saturation)] =
            S_L[i] - perturbation;
        auto const k_T_i_0 = MPL::formEigenTensor<3>(
            k_T_property.value(variable_array, pos, t, dt));

        variable_array[static_cast<int>(MPL::Variable::liquid_saturation)] =
            S_L[i] + perturbation;

        auto const k_T_i_1 = MPL::formEigenTensor<3>(
            k_T_property.value(variable_array, pos, t, dt));

        auto const analytic_dk_T_dS =
            MPL::formEigenTensor<3>(k_T_property.dValue(
                variable_array,
                MaterialPropertyLib::Variable::liquid_saturation, pos, t, dt));

        for (std::size_t k = 0; k < k_T_dry.size(); k++)
        {
            auto const approximated_dk_T_dS =
                0.5 * (k_T_i_1(k, k) - k_T_i_0(k, k)) / perturbation;
            ASSERT_LE(std::fabs(analytic_dk_T_dS(k, k) - approximated_dk_T_dS),
                      1e-5)
                << "for expected derivative of thermal conductivity with "
                   "respect to saturation "
                << analytic_dk_T_dS(k, k)
                << " and for computed derivative of thermal conductivity with "
                   "respect to saturation."
                << approximated_dk_T_dS;
        }
    }
}


#include <boost/make_shared.hpp>
#include <aslam/backend/RotationExpression.hpp>
#include <aslam/backend/RotationExpressionNode.hpp>
#include <aslam/backend/EuclideanExpressionNode.hpp>
#include <sm/boost/null_deleter.hpp>

namespace aslam {
  namespace backend {

    /// \brief the base case is to initialize an expression from a design variable.
    RotationExpression::RotationExpression(RotationExpressionNode * rotationDesignVariable)
    {
      _root.reset(rotationDesignVariable,sm::null_deleter());
      SM_ASSERT_TRUE(Exception, _root.get() != NULL, "It is illegal to initialize a rotation expression with a null pointer");
    }

    RotationExpression::~RotationExpression()
    {
    }

    RotationExpression::RotationExpression(boost::shared_ptr<RotationExpressionNode> node) :
      _root(node)
    {
    }

    RotationExpression::RotationExpression(const Eigen::Matrix3d & C) :
      _root(boost::make_shared<ConstantRotationExpressionNode>(C))
    {
    }

    /// \brief Evaluate the rotation matrix.
    Eigen::Matrix3d RotationExpression::toRotationMatrix() const
    {
      if(isEmpty()) return Eigen::Matrix3d::Identity();
      return _root->toRotationMatrix();
    }

    /// \brief return the expression that inverts the rotation.
    RotationExpression RotationExpression::inverse() const
    {
      if(isEmpty()) return *this;
      boost::shared_ptr<RotationExpressionNode> newRoot( new RotationExpressionNodeInverse(_root) );
      return RotationExpression(newRoot);
    }

    /// \brief Evaluate the Jacobians
    void RotationExpression::evaluateJacobians(JacobianContainer & outJacobians) const
    {
      if(!isEmpty())
        _root->evaluateJacobians(outJacobians);
    }

    RotationExpression RotationExpression::operator*(const RotationExpression & p) const
    {
      if(p.isEmpty())
        return *this;
      if(this->isEmpty())
        return p;
      boost::shared_ptr<RotationExpressionNode> newRoot( new RotationExpressionNodeMultiply(_root, p._root));
      return RotationExpression(newRoot);
    }

    EuclideanExpression RotationExpression::operator*(const EuclideanExpression & p) const
    {
      if(p.isEmpty())
        return EuclideanExpression();
      if(this->isEmpty())
        return p;
      boost::shared_ptr<EuclideanExpressionNode> newRoot( new EuclideanExpressionNodeMultiply(_root, p._root));
      return EuclideanExpression(newRoot);

    }

    HomogeneousExpression RotationExpression::operator*(const HomogeneousExpression & /* p */) const
    {
      // \todo
        SM_THROW(Exception, "Not implemented yet")
      return HomogeneousExpression();
    }

    TransformationExpression RotationExpression::toTransformationExpression() const
    {
      // \todo
        SM_THROW(Exception, "Not implemented yet")
      return TransformationExpression();
    }

    void RotationExpression::getDesignVariables(DesignVariable::set_t & designVariables) const
    {
      if(!isEmpty())
        _root->getDesignVariables(designVariables);
    }

    EuclideanExpression RotationExpression::toParameters(sm::kinematics::RotationalKinematics::Ptr rk) const {
      assert(!isEmpty());
      boost::shared_ptr<EuclideanExpressionNode> een( new EuclideanExpressionNodeRotationParameters(_root, rk));
      return EuclideanExpression(een);
    }

  } // namespace backend
} // namespace aslam


#pragma once

#include "sco/modeling.hpp"
#include "sco/modeling_utils.hpp"
#include "sco/sco_fwd.hpp"
#include <Eigen/Core>
#include "trajopt/common.hpp"
#include <openrave/openrave.h>
namespace trajopt {

using namespace sco;
typedef BasicArray<Var> VarArray;

#if 0
void makeTrajVariablesAndBounds(int n_steps, const RobotAndDOF& manip, OptProb& prob_out, VarArray& vars_out);

class FKFunc {
public:
  virtual OpenRAVE::Transform operator()(const VectorXd& x) const = 0;
  virtual ~FKFunc() {}
};

class FKPositionJacobian {
public:
  virtual Eigen::MatrixXd operator()(const VectorXd& x) const = 0;
  virtual ~FKPositionJacobian() {}
};
#endif

struct CartPoseErrCalculator : public VectorOfVector {
  OR::Transform pose_inv_;
  ConfigurationPtr manip_;
  OR::KinBody::LinkPtr link_;
  CartPoseErrCalculator(const OR::Transform& pose, ConfigurationPtr manip, OR::KinBody::LinkPtr link) :
    pose_inv_(pose.inverse()),
    manip_(manip),
    link_(link) {}
  VectorXd operator()(const VectorXd& dof_vals) const;
};

struct CartPoseErrorPlotter : public Plotter {
  boost::shared_ptr<void> m_calc; //actually points to a CartPoseErrCalculator = CartPoseCost::f_
  VarVector m_vars;
  CartPoseErrorPlotter(boost::shared_ptr<void> calc, const VarVector& vars) : m_calc(calc), m_vars(vars) {}
  void Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles);
};

struct CartVelJacCalculator : MatrixOfVector {
  ConfigurationPtr manip_;
  KinBody::LinkPtr link_;
  double limit_;
  CartVelJacCalculator(ConfigurationPtr manip, KinBody::LinkPtr link, double limit) :
    manip_(manip), link_(link), limit_(limit) {}

  MatrixXd operator()(const VectorXd& dof_vals) const;
};

struct CartVelCalculator : VectorOfVector {
  ConfigurationPtr manip_;
  KinBody::LinkPtr link_;
  double limit_;
  CartVelCalculator(ConfigurationPtr manip, KinBody::LinkPtr link, double limit) :
    manip_(manip), link_(link), limit_(limit) {}

  VectorXd operator()(const VectorXd& dof_vals) const;
};

#if 0
class CartPoseCost : public CostFromErrFunc {
public:
  CartPoseCost(const VarVector& vars, const OR::Transform& pose, RobotAndDOFPtr manip, KinBody::LinkPtr link, const VectorXd& coeffs);
};

class CartPoseConstraint : public ConstraintFromFunc {
public:
  CartPoseConstraint(const VarVector& vars, const OR::Transform& pose, RobotAndDOFPtr manip, KinBody::LinkPtr link, const VectorXd& coeffs);
};

class CartVelConstraint : public ConstraintFromFunc {
public:
  CartVelConstraint(const VarVector& step0vars, const VarVector& step1vars, RobotAndDOFPtr manip, KinBody::LinkPtr link, double distlimit);
};
#endif

}


//==================================================================================================
/*!
  @file

    @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_NEZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NEZ_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-predicates

    This function object returns @ref True if x is not equal to @ref Zero else returns @ref False.

    @par Header <boost/simd/function/is_nez.hpp>

    @par Note

     Using `is_nez(x)` is similar to: `x != 0`

    @par Example:

      @snippet is_nez.cpp is_nez

    @par Possible output:

      @snippet is_nez.txt is_nez

  **/
  as_logical_t<Value> is_nez(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/is_nez.hpp>
#include <boost/simd/function/simd/is_nez.hpp>

#endif


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)
//
// This program will evaluate the change in relative translation error produced
// by the relative translation optimization. The angular relative translation
// errors (compared the to ground truth relative translation) are shown before
// the optimization and after the optimization. The change in error is also
// output (negative and lower values mean the error decreased).xs

#include <Eigen/Core>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <theia/theia.h>

#include <memory>
#include <string>

DEFINE_string(matches, "", "Matches database directory (generated by RocksDB)");
DEFINE_string(reconstruction, "", "Reconstruction to use as ground truth.");

using theia::Reconstruction;
using theia::TrackId;
using theia::ViewId;
using theia::ViewIdPair;

// Accumulate all two view feature matches between the input views. The features
// are normalized according to the camera intrinsics.
void GetFeatureCorrespondences(
    const theia::View& view1,
    const theia::View& view2,
    std::vector<theia::FeatureCorrespondence>* matches) {
  const theia::Camera& camera1 = view1.Camera();
  const theia::Camera& camera2 = view2.Camera();
  const std::vector<theia::TrackId>& tracks = view1.TrackIds();
  for (const theia::TrackId track_id : tracks) {
    const theia::Feature* feature2 = view2.GetFeature(track_id);
    // If view 2 does not contain the current track then it cannot be a
    // correspondence.
    if (feature2 == nullptr) {
      continue;
    }

    theia::FeatureCorrespondence match;
    const theia::Feature* feature1 = view1.GetFeature(track_id);
    match.feature1 = *feature1;
    match.feature2 = *feature2;

    // Normalize for camera intrinsics.
    match.feature1 =
        camera1.PixelToNormalizedCoordinates(match.feature1).hnormalized();
    match.feature2 =
        camera2.PixelToNormalizedCoordinates(match.feature2).hnormalized();
    matches->emplace_back(match);
  }
}

double ComputeRelativeTranslationError(
    const Eigen::Vector3d& position1,
    const Eigen::Vector3d& position2,
    const Eigen::Matrix3d& rotation1,
    const Eigen::Vector3d& relative_translation) {
  const Eigen::Vector3d world_translation =
      rotation1 * (position2 - position1).normalized();
  return theia::RadToDeg(acos(
      theia::Clamp(relative_translation.dot(world_translation), -1.0, 1.0)));
}

void EvaluateTranslationOptimization(
    const std::string& matches_directory,
    const theia::Reconstruction& reconstruction) {
  const std::vector<double> histogram_bins = {
      2, 5, 10, 15, 25, 50, 90, 135, 180, 225, 270, 316};
  const std::vector<double> change_bins = {-50, -20, -10, -5, 0, 5, 10, 20, 50};
  theia::Histogram<double> before_hist(histogram_bins),
      after_hist(histogram_bins), change_hist(change_bins);

  // Open the DB containing match data.
  theia::RocksDbFeaturesAndMatchesDatabase database(matches_directory);

  // Collect relative translations.
  const auto match_keys = database.ImageNamesOfMatches();
  for (const auto& match_key : match_keys) {
    const theia::ImagePairMatch& match =
        database.GetImagePairMatch(match_key.first, match_key.second);
    // Get the ViewIds from the names.
    const theia::ViewId view_id1 = reconstruction.ViewIdFromName(match.image1);
    const theia::ViewId view_id2 = reconstruction.ViewIdFromName(match.image2);
    const theia::View* view1 = reconstruction.View(view_id1);
    const theia::View* view2 = reconstruction.View(view_id2);
    if (view1 == nullptr || view2 == nullptr) {
      continue;
    }
    const theia::Camera& camera1 = view1->Camera();
    const theia::Camera& camera2 = view2->Camera();

    Eigen::Vector3d relative_position = match.twoview_info.position_2;
    const double translation_angular_error = ComputeRelativeTranslationError(
        camera1.GetPosition(),
        camera2.GetPosition(),
        camera1.GetOrientationAsRotationMatrix(),
        relative_position);
    before_hist.Add(translation_angular_error);

    // Optimize the relative position and recompute the error.
    std::vector<theia::FeatureCorrespondence> correspondences;
    GetFeatureCorrespondences(*view1, *view2, &correspondences);
    CHECK(theia::OptimizeRelativePositionWithKnownRotation(
        correspondences,
        camera1.GetOrientationAsAngleAxis(),
        camera2.GetOrientationAsAngleAxis(),
        &relative_position));

    const double optimized_translation_angular_error =
        ComputeRelativeTranslationError(
            camera1.GetPosition(),
            camera2.GetPosition(),
            camera1.GetOrientationAsRotationMatrix(),
            relative_position);
    after_hist.Add(optimized_translation_angular_error);

    change_hist.Add(optimized_translation_angular_error -
                    translation_angular_error);
  }
  LOG(INFO) << "Before histogram:\n"
            << before_hist.PrintString() << "\n\nAfter histogram:\n"
            << after_hist.PrintString() << "\n\nChange histogram:\n"
            << change_hist.PrintString();
}

int main(int argc, char* argv[]) {
  google::InitGoogleLogging(argv[0]);
  THEIA_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);

  // Get the g.t. reconstruction first.
  std::unique_ptr<theia::Reconstruction> reconstruction(
      new theia::Reconstruction());
  CHECK(theia::ReadReconstruction(FLAGS_reconstruction, reconstruction.get()))
      << "Could not read reconstruction from " << FLAGS_reconstruction;

  LOG(INFO) << "Reading the matches.";
  EvaluateTranslationOptimization(FLAGS_matches, *reconstruction);

  return 0;
}


#pragma once

#include "../stateobject.hh"
#include "../../JsonWriter/jsonwriter.hh"

#include <Eigen/Geometry>

namespace bold
{
  class OrientationState : public StateObject
  {
  public:
    OrientationState(Eigen::Quaterniond const& quaternion);

    Eigen::Quaterniond const& getQuaternion() const { return d_quaternion; };

    double getPitchAngle() const { return d_pitch; }
    double getRollAngle() const { return d_roll; }
    double getYawAngle() const { return d_yaw; }

    Eigen::Affine3d withoutYaw() const;

    void writeJson(rapidjson::Writer<rapidjson::StringBuffer>& writer) const override { writeJsonInternal(writer); }
    void writeJson(rapidjson::Writer<WebSocketBuffer>& writer) const override { writeJsonInternal(writer); }
    void writeJson(rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer) const override { writeJsonInternal(writer); }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  private:
    template<typename TBuffer>
    void writeJsonInternal(rapidjson::Writer<TBuffer> &writer) const;

    Eigen::Quaterniond d_quaternion;
    double d_pitch;
    double d_roll;
    double d_yaw;
  };

  template<typename TBuffer>
  inline void OrientationState::writeJsonInternal(rapidjson::Writer<TBuffer> &writer) const
  {
    writer.StartObject();
    {
      writer.String("quaternion");
      writer.StartArray();
      JsonWriter::swapNaN(writer, d_quaternion.x(), "%.6f");
      JsonWriter::swapNaN(writer, d_quaternion.y(), "%.6f");
      JsonWriter::swapNaN(writer, d_quaternion.z(), "%.6f");
      JsonWriter::swapNaN(writer, d_quaternion.w(), "%.6f");
      writer.EndArray();

      writer.String("pitch"); JsonWriter::swapNaN(writer, d_pitch, "%.3f");
      writer.String("roll");  JsonWriter::swapNaN(writer, d_roll, "%.3f");
      writer.String("yaw");   JsonWriter::swapNaN(writer, d_yaw, "%.3f");
    }
    writer.EndObject();
  }
}


#pragma once

#include <utility>
#include <vector>

#include <Eigen/Dense>

#include "GNAObject.hh"

class GaussLegendre2d: public GNAObject,
                       public TransformationBind<GaussLegendre2d> {
  friend class GaussLegendre2dHist;
public:
  GaussLegendre2d(std::vector<double> xedges,
                  std::vector<int> xorders,
                  double ymin, double ymax, int yorder)
  : m_xedges(std::move(xedges)), m_xorders(std::move(xorders)),
    m_ymin(ymin), m_ymax(ymax), m_yorder(yorder)
  {
    init();
  }
  GaussLegendre2d(const double *xedges, const size_t *xorders, size_t xcnt,
                double ymin, double ymax, int yorder)
    : m_xedges(xcnt+1), m_xorders(xcnt),
      m_ymin(ymin), m_ymax(ymax), m_yorder(yorder)
  {
    std::copy(xedges, xedges+xcnt+1, m_xedges.begin());
    std::copy(xorders, xorders+xcnt, m_xorders.begin());
    init();
  }
  GaussLegendre2d(const double *xedges, size_t xorder, size_t xcnt,
                double ymin, double ymax, int yorder)
    : m_xedges(xcnt+1), m_xorders(xcnt, xorder),
      m_ymin(ymin), m_ymax(ymax), m_yorder(yorder)
  {
    std::copy(xedges, xedges+xcnt+1, m_xedges.begin());
    init();
  }
protected:
  void init();

  void pointsTypes(TypesFunctionArgs& fargs);
  void points(FunctionArgs& fargs);

  void histTypes(TypesFunctionArgs& fargs);
  void hist(FunctionArgs& fargs);

  std::vector<double> m_xedges;
  std::vector<int> m_xorders;
  double m_ymin, m_ymax;
  int m_yorder;

  std::vector<double> m_xpoints;
  Eigen::ArrayXd m_xweights;

  std::vector<double> m_ypoints;
  Eigen::ArrayXd m_yweights;
};

class GaussLegendre2dHist: public GNASingleObject,
                           public TransformationBind<GaussLegendre2dHist> {
public:
  GaussLegendre2dHist(const GaussLegendre2d *base);
protected:
  const GaussLegendre2d *m_base;
};


/*
 * Modified version of code at:
 * https://gist.github.com/Mathiasb17/5fb84d6be9745270f66a4dec6be9b9da
 */

#include <boost/filesystem.hpp>
#include <cstdio>
#include <fstream>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <string>
#include <vector>
#include <openvdb/openvdb.h>
#include <openvdb/tools/ParticlesToLevelSet.h>
#include <openvdb/Exceptions.h>
#include <openvdb/Types.h>
#include <openvdb/tree/LeafNode.h>
#include <openvdb/tools/Filter.h>
#include <openvdb/tools/LevelSetFilter.h>
#include <openvdb/tools/VolumeToMesh.h>

DEFINE_string(input, "", "Input file containing particle data");
DEFINE_string(output, "", "Output file prefix to save signed distance to");

namespace {
  typedef openvdb::Real Real;
  typedef openvdb::Vec3R  Vec3R;
  typedef openvdb::Vec3d  Vec3d;
  typedef openvdb::Coord Coord;
  typedef openvdb::CoordBBox CoordBBox;
}  // namespace anonymous

class ParticlesList {
  public:
    typedef Vec3R PosType;

  protected:
    // Each particle has a position, velocity and radius.
    struct Particle
    {
      Vec3R p, v;
      Real r;
    };

    // Parameters to control output.
    Real radius_scale_  = 1.0;
    Real velocity_scale_ = 1.0;

    // Particle list stored in a vector.
    std::vector<Particle> particle_list_;

  public:
    ParticlesList(Real r_scale=1, Real v_scale=1)
      : radius_scale_(r_scale), velocity_scale_(v_scale) {}

    // Add particle with given position and radius.
    void AddParticle(const Vec3R &p, const Vec3R &v, const Real r = 3.0) {
      Particle part;
      part.p = p;
      part.v = v;
      part.r = r;
      particle_list_.push_back(part);
    }

    // Methods required by OpenVDB
    size_t size() const { return particle_list_.size(); }
    void getPos(size_t n,  Vec3R& pos) const { pos = particle_list_[n].p; }
    void getPosRad(size_t n,  Vec3R& pos, Real& rad) const {
      pos = particle_list_[n].p;
      rad = radius_scale_*particle_list_[n].r;
    }
    void getPosRadVel(size_t n,  Vec3R& pos, Real& rad, Vec3R& vel) const {
      pos = particle_list_[n].p;
      rad = radius_scale_*particle_list_[n].r;
      vel = velocity_scale_*particle_list_[n].v;
    }
    void getAtt(size_t n, openvdb::Index32& att) const { att = openvdb::Index32(n); }
};

int main(int argc, char *argv[]) {
  google::InitGoogleLogging(argv[0]);
  gflags::ParseCommandLineFlags(&argc, &argv, true);

  CHECK_NE(FLAGS_input, "") << "Must supply an input file name";
  CHECK_NE(FLAGS_output, "") << "Must supply an output file name";

  openvdb::initialize();

  const std::string input = FLAGS_input;
  const std::string output = FLAGS_output;

  /* Point cloud loading */
  CHECK(boost::filesystem::exists(input)) << "File " << input << " not found";
  FILE *fp = fopen(input.c_str(), "r");
  CHECK(fp != NULL) << "Error reading file " << input;

  // First read the header.
  Coord start, end, dims;
  int num_particles;
  CHECK(fscanf(fp, "%d %d %d\n", &start[0], &start[1], &start[2]) > 0);
  CHECK(fscanf(fp, "%d %d %d\n", &end[0], &end[1], &end[2]) > 0);
  dims = end - start + Coord(1);
  CHECK(fscanf(fp, "%d\n", &num_particles) > 0);

  // Now read particle data and add particles.
  ParticlesList pl(1, 1);
  for (int i = 0; i < num_particles; ++i) {
    Vec3R pos, vel;
    int p;
    CHECK(fscanf(fp, "%d : %lf %lf %lf, %lf %lf %lf\n", &p,
                 &pos[0], &pos[1], &pos[2], &vel[0], &vel[1], &vel[2]) > 0);
    pl.AddParticle(pos, vel);
  }
  fclose(fp);

  LOG(INFO) << "Loaded " << num_particles << " particles";
  CHECK_EQ(int(pl.size()), num_particles);

  /* OpenVDB processing */

  const float voxelSize = 1.f;
  openvdb::FloatGrid::Ptr ls = openvdb::createLevelSet<openvdb::FloatGrid>(voxelSize);

  openvdb::tools::ParticlesToLevelSet<openvdb::FloatGrid> raster(*ls);
  //raster.rasterizeTrails(pl, 0.75);
  raster.rasterizeSpheres(pl);

  openvdb::io::File unfiltered_output(output+"_unfiltered.vdb");
  openvdb::GridPtrVec unfiltered;
  unfiltered.push_back(ls);
  unfiltered_output.write(unfiltered);
  unfiltered_output.close();

  openvdb::tools::LevelSetFilter<openvdb::FloatGrid> filterer(*ls);
  filterer.mean();

  openvdb::io::File filtered_output(output+"_filtered.vdb");
  openvdb::GridPtrVec filtered;
  filtered.push_back(ls);
  filtered_output.write(unfiltered);
  filtered_output.close();

  /*
  std::vector<openvdb::Vec3s> points;
  std::vector<openvdb::Vec4I> quads;
  std::vector<openvdb::Vec3I> triangles;

  openvdb::tools::volumeToMesh(*ls, points, triangles, quads, 0.005, 0);

  std::cout << "points " << points.size() << std::endl;
  std::cout << "triangles " << triangles.size() << std::endl;
  std::cout << "quads " << quads.size() << std::endl;
  */

  return 0;
}


#pragma once

#include <crest/geometry/indexed_mesh.hpp>
#include <crest/geometry/patch.hpp>
#include <crest/geometry/mesh_algorithms.hpp>

#include <boost/range/iterator_range.hpp>

#include <cassert>

namespace crest
{
    namespace detail
    {
        template <typename Index>
        class DescendantMap
        {
        public:
            typedef typename std::vector<Index>::const_iterator          ConstIndexIterator;
            typedef typename boost::iterator_range<ConstIndexIterator>   ConstIndexIteratorRange;

            template <typename Scalar>
            explicit DescendantMap(const IndexedMesh<Scalar, Index> & coarse,
                                   const IndexedMesh<Scalar, Index> & fine)
            {
                _map = std::vector<std::vector<Index>>(coarse.num_elements());
                for (Index k = 0; k < fine.num_elements(); ++k)
                {
                    assert(fine.ancestor_for(k) < coarse.num_elements());
                    _map[fine.ancestor_for(k)].push_back(k);
                }
            }

            ConstIndexIteratorRange descendants_for(Index coarse_element) const
            {
                assert(coarse_element >= Index(0) && coarse_element <= static_cast<Index>(_map.size()));
                return boost::make_iterator_range(_map[coarse_element].begin(), _map[coarse_element].end());
            }

        private:
            std::vector<std::vector<Index>> _map;
        };
    }

    /**
     * Represents a triangulated domain by a "coarse" (usually quasi-uniform) mesh and
     * a "fine" mesh that is formed from the coarse mesh by bisection.
     */
    template <typename Scalar, typename Index>
    class BiscaleMesh
    {
    public:
        struct CoarseTag {};
        struct FineTag {};

        typedef Patch<Scalar, Index, CoarseTag> CoarsePatch;
        typedef Patch<Scalar, Index, FineTag>   FinePatch;

        typedef typename detail::DescendantMap<Index>::ConstIndexIteratorRange ConstIndexIteratorRange;

        explicit BiscaleMesh(IndexedMesh<Scalar, Index> coarse_mesh,
                             IndexedMesh<Scalar, Index> fine_mesh);

        CoarsePatch coarse_element_patch(Index coarse_element, unsigned int max_distance) const;
        FinePatch fine_patch_from_coarse(const CoarsePatch & coarse_patch) const;

        Index                       ancestor_for(Index fine_element) const;
        ConstIndexIteratorRange     descendants_for(Index coarse_element) const;

        const IndexedMesh<Scalar, Index> & coarse_mesh() const;
        const IndexedMesh<Scalar, Index> & fine_mesh() const;

    private:
        IndexedMesh<Scalar, Index>      _coarse;
        IndexedMesh<Scalar, Index>      _fine;
        detail::DescendantMap<Index>    _descendants;
    };

    template <typename Scalar, typename Index>
    BiscaleMesh<Scalar, Index>::BiscaleMesh(IndexedMesh<Scalar, Index> coarse_mesh,
                                            IndexedMesh<Scalar, Index> fine_mesh)
            :   _descendants(detail::DescendantMap<Index>(coarse_mesh, fine_mesh))
    {
        _coarse = std::move(coarse_mesh);
        _fine = std::move(fine_mesh);
        // Check that all coarse elements have at least one descendant, otherwise
        // the fine mesh cannot correspond to a refinement of the coarse mesh.
        for (Index k = 0; k < _coarse.num_elements(); ++k)
        {
            // Since this is a no-op in release mode, the compiler will optimize it away
            assert(!_descendants.descendants_for(k).empty());
        }
    };

    template <typename Scalar, typename Index>
    typename BiscaleMesh<Scalar, Index>::CoarsePatch
    BiscaleMesh<Scalar, Index>::coarse_element_patch(Index coarse_element, unsigned int max_distance) const
    {
        return patch_for_element<CoarseTag>(coarse_mesh(), coarse_element, max_distance);
    };

    template <typename Scalar, typename Index>
    typename BiscaleMesh<Scalar, Index>::FinePatch
    BiscaleMesh<Scalar, Index>::fine_patch_from_coarse(
            const BiscaleMesh<Scalar, Index>::CoarsePatch & coarse_patch) const
    {
        std::vector<Index> fine_patch;
        for (const auto coarse_element : coarse_patch)
        {
            for (const auto fine_element : descendants_for(coarse_element))
            {
                fine_patch.push_back(fine_element);
            }
        }
        fine_patch = algo::sorted_unique(std::move(fine_patch));
        return FinePatch(fine_mesh(), std::move(fine_patch));
    };

    template <typename Scalar, typename Index>
    Index BiscaleMesh<Scalar, Index>::ancestor_for(Index fine_element) const
    {
        return _fine.ancestor_for(fine_element);
    };

    template <typename Scalar, typename Index>
    typename BiscaleMesh<Scalar, Index>::ConstIndexIteratorRange
    BiscaleMesh<Scalar, Index>::descendants_for(Index coarse_element) const
    {
        return _descendants.descendants_for(coarse_element);
    };

    template <typename Scalar, typename Index>
    const IndexedMesh<Scalar, Index> & BiscaleMesh<Scalar, Index>::coarse_mesh() const
    {
        return _coarse;
    };

    template <typename Scalar, typename Index>
    const IndexedMesh<Scalar, Index> & BiscaleMesh<Scalar, Index>::fine_mesh() const
    {
        return _fine;
    };

}


// Copyright (c) 2005 - 2014 Marc de Kamps
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//    * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software
//      without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
// USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//      If you use this software in work leading to a scientific publication, you should include a reference there to
//      the 'currently valid reference', which can be found at http://miind.sourceforge.net
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <GeomLib.hpp>

using namespace GeomLib;
using namespace MPILib;

BOOST_AUTO_TEST_CASE(BinEstimatorTest)
{
    Number n_bins     = 5;

    Time t_mem        =  10e-3;
    Time t_ref        =  0.0;
    Potential V_min   = -10.0;
    Potential V_peak  =  10.0;
    Potential V_reset =   0.0;
    Potential gamma   =  0.5;

    NeuronParameter par_neuron(V_peak, V_reset, V_reset, t_ref, t_mem);
    QifParameter
    par_qif
    (
        gamma
    );

    OdeParameter
    par_ode
    (
    	n_bins,
		V_min,
		par_neuron,
        InitialDensityParameter(V_reset, 0.0)
    );

    SpikingQifNeuralDynamics dyn(par_ode,par_qif);
    QifOdeSystem sys(dyn);

    vector<double> array_interpretation(n_bins, 0);
    vector<double> array_density(n_bins);

    sys.PrepareReport(&array_interpretation[0], &array_density[0]);

    BinEstimator
		estimator
		(
			array_interpretation,
			par_ode
		);

   Index i_new = estimator.SearchBin(0.0);
    BOOST_CHECK(i_new == 2);
    i_new = estimator.SearchBin(9.64);
    BOOST_CHECK(i_new == 4);
    i_new = estimator.SearchBin(par_ode._V_min + 1e-10);
    BOOST_CHECK(i_new == 0);
    i_new = estimator.SearchBin(par_ode._V_min);
    BOOST_CHECK(i_new == 0);
    bool b_catch = false;

    try {
        i_new = estimator.SearchBin(par_ode._V_min - 1e-10);
    } catch (GeomLibException&) {
        b_catch = true;
    }

    BOOST_REQUIRE(b_catch);
}

BOOST_AUTO_TEST_CASE(ExcitatoryBinEstimatorCoverTest)
{

    // test values in this routine are generated by master.py
    Number n_bins = 5;

    Time t_mem        =  10e-3;
    Time t_ref        =  0.0;
    Potential V_min   = -10.0;
    Potential V_peak  =  10.0;
    Potential V_reset = -10.0;
    Potential gamma   =  0.5;

    QifParameter
    par_qif
    (
        gamma
    );

    NeuronParameter par_neuron(V_peak, V_reset, V_reset, t_ref, t_mem);
    OdeParameter par_ode
    (
    	n_bins,
		V_min,
		par_neuron,
        InitialDensityParameter(V_reset, 0.0)
    );

    Potential h = 0.1;

    SpikingQifNeuralDynamics dyn(par_ode, par_qif);
    QifOdeSystem sys(dyn);

    vector<double> array_interpretation(n_bins, 0);
    vector<double> array_density(n_bins);

    sys.PrepareReport(&array_interpretation[0], &array_density[0]);

    BinEstimator estimator(array_interpretation, par_ode);
    Index i = estimator.SearchBin(0.0);
    BinEstimator::CoverPair pair = estimator.CalculateBinCover(i, -h);

    BOOST_CHECK(pair.first._index  == 1);
    BOOST_CHECK(pair.second._index == 2);

    BOOST_CHECK_CLOSE(pair.first._alpha,  0.14869352, 0.01);
    BOOST_CHECK_CLOSE(pair.second._alpha, 1 - 0.22855551, 0.01); // master.py subtracts -1 from diagonal

    i = estimator.SearchBin(V_min);
    pair = estimator.CalculateBinCover(i, -h);
    BOOST_CHECK(pair.first._index == -1);
    BOOST_CHECK(pair.second._index == 0);

    BOOST_CHECK(pair.first._alpha == 0.0);
    BOOST_CHECK_CLOSE(pair.second._alpha, 1 - 0.0109785 , 1e-4); // master.py subtracts 1 from diagonal

    i = estimator.SearchBin(V_peak);
    pair = estimator.CalculateBinCover(i, -h);

    BOOST_CHECK(pair.first._index  == 3);
    BOOST_CHECK(pair.second._index == 4);
    pair = estimator.CalculateBinCover(i, -h);

    BOOST_CHECK_CLOSE(pair.first._alpha,  0.14869352, 1e-4);
    BOOST_CHECK_CLOSE(pair.second._alpha, 1 - 0.0109785 , 1e-4);

    h = 9.5;
    i = estimator.SearchBin(V_min);
    pair = estimator.CalculateBinCover(i, -h);

    BOOST_CHECK(pair.first._index == -1);
    BOOST_CHECK(pair.second._index == -1);

    i = 1;
    pair = estimator.CalculateBinCover(i, -h);

    BOOST_CHECK(pair.first._index == -1);
    BOOST_CHECK(pair.second._index == 0);

    BOOST_CHECK_CLOSE(pair.second._alpha, 0.03087537, 1e-4);

    i = estimator.SearchBin(V_peak);
    pair = estimator.CalculateBinCover(i, -h);

    BOOST_CHECK(pair.first._index  == 0);
    BOOST_CHECK(pair.second._index == 3);

    BOOST_CHECK_CLOSE(pair.first._alpha,  0.84725725, 1e-4);
    BOOST_CHECK_CLOSE(pair.second._alpha, 0.41817786 , 1e-4);

    h = 19.5;
    i = 3;
    pair = estimator.CalculateBinCover(i, -h);
    BOOST_CHECK(pair.first._index == -1);
    BOOST_CHECK(pair.second._index == -1);

    i = estimator.SearchBin(V_peak);
    pair = estimator.CalculateBinCover(i, -h);
    BOOST_CHECK(pair.second._index == 0);
    BOOST_CHECK_CLOSE(pair.second._alpha, 0.05489251 , 1e-4);
}

BOOST_AUTO_TEST_CASE(InhibitoryBinEstimatorCoverTest)
{
    // test values in this routine are generated by master.py
    Number n_bins = 5;

    Time t_mem        =  10e-3;
    Time t_ref        =  0.0;
    Potential V_min   = -10.0;
    Potential V_peak  =  10.0;
    Potential V_reset = -10.0;
    Potential gamma   =  0.5;

    QifParameter
    par_qif
    (
        gamma
    );

    NeuronParameter par_neuron(V_peak, V_reset, V_reset, t_ref, t_mem);
    OdeParameter
    par_ode
    (
    	n_bins,
		V_min,
		par_neuron,
        InitialDensityParameter(V_reset, 0.0)
    );

    Potential h = 0.1;

    SpikingQifNeuralDynamics dyn(par_ode, par_qif);
    QifOdeSystem sys(dyn);

    vector<double> array_interpretation(n_bins, 0);
    vector<double> array_density(n_bins);

    sys.PrepareReport(&array_interpretation[0], &array_density[0]);

    BinEstimator estimator(array_interpretation, par_ode);
    Index i = estimator.SearchBin(V_min);
    BinEstimator::CoverPair pair = estimator.CalculateBinCover(i, h);

    BOOST_CHECK(pair.first._index  == 0);
    BOOST_CHECK(pair.second._index == 1);

    BOOST_CHECK_CLOSE(pair.first._alpha, 0.9890214 , 1e-4);
    BOOST_CHECK_CLOSE(pair.second._alpha, 0.14869352, 1e-4);

    i = estimator.SearchBin(V_peak);
    pair = estimator.CalculateBinCover(i, h);

    BOOST_CHECK(pair.first._index == 4);
    BOOST_CHECK(pair.second._index == 5);

    BOOST_CHECK_CLOSE(pair.first._alpha, 1 - 0.0109785, 1e-4);
    BOOST_CHECK_CLOSE(pair.second._alpha, 0.0, 1e-4);

    h = 19.5;

    i = estimator.SearchBin(V_min);
    pair = estimator.CalculateBinCover(i, h);

    BOOST_CHECK(pair.first._index  == 4);
    BOOST_CHECK(pair.second._index == 5);

    BOOST_CHECK_CLOSE(pair.first._alpha, 0.05489251, 1e-4);
    BOOST_CHECK(pair.second._alpha == 0.0);
}

BOOST_AUTO_TEST_CASE(LifCoverTest)
{
    // test values in this routine are generated by master.py
    Number n_bins = 5;

    Time t_mem        =  10e-3;
    Time t_ref        =  0.0;
    Potential V_min   = -2.0;
    Potential V_peak  =  1.0;
    Potential V_reset =  0.0;

    NeuronParameter par_neuron(V_peak, V_reset, V_reset, t_ref, t_mem);
    OdeParameter
    par_ode
    (
    	n_bins,
		V_min,
		par_neuron,
        InitialDensityParameter(V_reset, 0.0)
    );

	LifNeuralDynamics dyn(par_ode, 0.01);
	LeakingOdeSystem sys(dyn);

    vector<double> array_interpretation(dyn.NumberOfBins(), 0);
    vector<double> array_density(dyn.NumberOfBins());

    sys.PrepareReport(&array_interpretation[0], &array_density[0]);

    BinEstimator estimator(array_interpretation, par_ode);
    // bin boundaries are:
    // -3.16228
    // -1
    // -0.316228
    // -0.1
    // -0.0316228
    // -0.01
    //  0
    //  0.01
    //  0.0316228
    //  0.1
    //  0.316228

    Index i = estimator.SearchBin(V_min);

    Potential h = 0.95;
    BinEstimator::CoverPair pair = estimator.CalculateBinCover(i, h);
    // the first boundary should be firmly in bin 0, but isn't
    // because -3.16228 + h is still below V_min (= -2); therefore -1
    // the second one should be in bin 3
    BOOST_CHECK( pair.first._index  == -1);
    BOOST_CHECK( pair.second._index ==  3);

    i = estimator.SearchBin(0.05);
    h = 0.25;
    pair = estimator.CalculateBinCover(i,h);

    BOOST_CHECK( pair.first._index  == 9);
    BOOST_CHECK( pair.second._index == 10);
}

BOOST_AUTO_TEST_CASE(LifEstimatorTest)
{
    // test values in this routine are generated by master.py
    Number n_bins = 5;

    Time t_mem        =  10e-3;
    Time t_ref        =  0.0;
    Potential V_min   = -2.0;
    Potential V_peak  =  1.0;
    Potential V_reset =  0.0;

    NeuronParameter par_neuron(V_peak, V_reset, V_reset, t_ref, t_mem);
    OdeParameter
    par_ode
    (
    	n_bins,
		V_min,
		par_neuron,
        InitialDensityParameter(V_reset, 0.0)
    );

	LifNeuralDynamics dyn(par_ode, 0.01);
	LeakingOdeSystem sys(dyn);
    vector<double> array_interpretation(dyn.NumberOfBins(), 0);
    vector<double> array_density(dyn.NumberOfBins());

    sys.PrepareReport(&array_interpretation[0], &array_density[0]);

    LifEstimator estimator(array_interpretation, par_ode);

    // -3.16228
    // -1
    // -0.316228
    // -0.1
    // -0.0316228
    // -0.01
    //  0
    //  0.01
    //  0.0316228
    //  0.1
    //  0.316228

    int i = estimator.SearchBin(V_min-1e-10);
    BOOST_CHECK (i == -1);
    i = estimator.SearchBin(V_min);
    BOOST_CHECK(i == 0);
    i = estimator.SearchBin(-1.1);
    BOOST_CHECK(i == 0);
    i = estimator.SearchBin(-1.0);
    // the boundary is vague, because gamma is reconstructed
    BOOST_CHECK(i == 0);
    i = estimator.SearchBin(-1.0 + 1e-9);
    BOOST_CHECK(i == 1);
    i = estimator.SearchBin(-0.316227);
    BOOST_CHECK(i == 2);
    i = estimator.SearchBin(-0.099999);
    BOOST_CHECK( i == 3);
    i = estimator.SearchBin(-0.0316227);
    BOOST_CHECK( i == 4);
    i = estimator.SearchBin(-0.0099999);
    BOOST_CHECK( i == 5);
    i = estimator.SearchBin(0.0099999);
    BOOST_CHECK( i == 6);
    i = estimator.SearchBin(0.01001);
    BOOST_CHECK( i == 7);
    i = estimator.SearchBin(0.0316229);
    BOOST_CHECK( i == 8);
    i = estimator.SearchBin(0.1001);
    BOOST_CHECK( i == 9);
    i = estimator.SearchBin(0.316229);
    BOOST_CHECK(i == 10);
    i = estimator.SearchBin(1.00000001);
    BOOST_CHECK(i == -1);

    Potential h = 0.95;
    i = estimator.SearchBin(V_min);

    LifEstimator::CoverPair pair = estimator.CalculateBinCover(i, h);
    // the first boundary should be firmly in bin 0, but isn't
    // because -3.16228 + h is still below V_min; therefore -1
    // the second one should be in bin 3
    BOOST_CHECK( pair.first._index  == -1);
    BOOST_CHECK( pair.second._index ==  3);

    i = estimator.SearchBin(0.05);
    h = 0.25;
    pair = estimator.CalculateBinCover(i,h);

    BOOST_CHECK( pair.first._index  == 9);
    BOOST_CHECK( pair.second._index == 10);

}


#include <dai/alldai.h>
using namespace dai;

#include <boost/lexical_cast.hpp>

#include <cassert>
#include <chrono>
#include <ctime>
#include <iostream>
#include <map>
#include <string>
#include <vector>
using namespace std;

static FactorGraph fg;
static PropertySet opts;
static BP bp;
static map<int, bool> clamps;

void initBP() {
    bp = BP(fg, opts);
    for (const auto& clamp : clamps) {
        int varIndex = clamp.first;
        bool varValue = clamp.second;
        bp.clamp(varIndex, varValue ? 1 : 0);
    }
    bp.init();
}

void queryVariable() {
    int varIndex;
    cin >> varIndex;
    clog << __LOGSTR__ << "Q " << varIndex << endl;

    // auto ans = bp.belief(fg.var(varIndex)).get(1);
    auto ans = bp.newBelief(varIndex);
    clog << __LOGSTR__ << "Returning " << ans << "." << endl;
    cout << ans << endl;
}

void queryFactor() {
    int factorIndex, valueIndex;
    cin >> factorIndex >> valueIndex;
    clog << __LOGSTR__ << "FQ " << factorIndex << " " << valueIndex << endl;

    auto ans = bp.beliefF(factorIndex).get(valueIndex);
    clog << __LOGSTR__ << "Returning " << ans << "." << endl;
    cout << ans << endl;
}

void runBP() {
    double tolerance;
    size_t minIters, maxIters, histLength;
    cin >> tolerance >> minIters >> maxIters >> histLength;
    clog << __LOGSTR__ << "BP " << tolerance << " " << minIters << " " << maxIters << " " << histLength << endl;

    double yetToConvergeFraction = bp.run(tolerance, minIters, maxIters, histLength);
    cout << yetToConvergeFraction << endl;
}

void clamp() {
    int varIndex;
    string varValueStr;
    cin >> varIndex >> varValueStr;
    assert(varValueStr == "true" || varValueStr == "false");
    clog << __LOGSTR__ << "O " << varIndex << " " << varValueStr << endl;

    bool varValue = (varValueStr == "true");
    clamps[varIndex] = varValue;
    initBP();
    cout << "O " << varIndex << " " << varValueStr << endl;
}

int main(int argc, char *argv[]) {
    if (argc < 2) {
        cerr << __LOGSTR__ << "Insufficient number of arguments." << endl;
        return 1;
    }
    char *factorGraphFileName = argv[1];

    clog << __LOGSTR__ << "Hello!" << endl
                       << "Bingo compiled on " << __DATE__ << " at " << __TIME__ << "." << endl;
    fg.ReadFromFile(factorGraphFileName);
    clog << __LOGSTR__ << "Finished reading factor graph." << endl;

    opts.set("maxiter", static_cast<size_t>(10000000));
    opts.set("tol", Real(1e-6));
    opts.set("verb", static_cast<size_t>(1));
    opts.set("updates", string("SEQRND")); // "SEQRND", or "PARALL", or "SEQFIX", or "SEQRNDPAR"
    opts.set("logdomain", true);

    initBP();

    string cmdType;
    while (cin >> cmdType) {
        clog << __LOGSTR__ << "Read command " << cmdType << endl;
        if (cmdType == "Q") {
            queryVariable();
        } else if (cmdType == "FQ") {
            queryFactor();
        } else if (cmdType == "BP") {
            runBP();
        } else if (cmdType == "O") {
            clamp();
        } else {
            assert(cmdType == "NL");
            cout << endl;
        }
    }

    clog << __LOGSTR__ << "Bye!" << endl;
    return 0;
}


// Copyright John Maddock 2011.

// Use, modification and distribution are 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)

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#if !defined(TEST_MPQ) && !defined(TEST_TOMMATH) && !defined(TEST_CPP_INT)
#  define TEST_MPQ
#  define TEST_TOMMATH
#  define TEST_CPP_INT

#ifdef _MSC_VER
#pragma message("CAUTION!!: No backend type specified so testing everything.... this will take some time!!")
#endif
#ifdef __GNUC__
#pragma warning "CAUTION!!: No backend type specified so testing everything.... this will take some time!!"
#endif

#endif

#if defined(TEST_MPQ)
#include <boost/multiprecision/gmp.hpp>
#endif
#if defined(TEST_TOMMATH)
#include <boost/multiprecision/tommath.hpp>
#endif
#ifdef TEST_CPP_INT
#include <boost/multiprecision/cpp_int.hpp>
#endif

#include <boost/algorithm/string/case_conv.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/multiprecision/rational_adaptor.hpp>
#include "test.hpp"
#include <iostream>
#include <iomanip>

template <class T>
T generate_random()
{
   static boost::random::uniform_int_distribution<unsigned> ui(0, 20);
   static boost::random::mt19937 gen;
   T val = T(gen());
   unsigned lim = ui(gen);
   for(unsigned i = 0; i < lim; ++i)
   {
      val *= (gen.max)();
      val += gen();
   }
   T denom = T(gen());
   lim = ui(gen);
   for(unsigned i = 0; i < lim; ++i)
   {
      denom *= (gen.max)();
      denom += gen();
   }
   return val / denom;
}

template <class T>
void do_round_trip(const T& val, std::ios_base::fmtflags f, const boost::mpl::true_&)
{
   std::stringstream ss;
#ifndef BOOST_NO_CXX11_NUMERIC_LIMITS
   ss << std::setprecision(std::numeric_limits<T>::max_digits10);
#else
   ss << std::setprecision(std::numeric_limits<T>::digits10 + 5);
#endif
   ss.flags(f);
   ss << val;
   T new_val = static_cast<T>(ss.str());
   BOOST_CHECK_EQUAL(new_val, val);
   new_val = static_cast<T>(val.str(0, f));
   BOOST_CHECK_EQUAL(new_val, val);
}

template <class T>
void do_round_trip(const T& val, std::ios_base::fmtflags f, const boost::mpl::false_&)
{
   std::stringstream ss;
   ss << std::setprecision(std::numeric_limits<T>::digits10 + 4);
   ss.flags(f);
   ss << val;
   T new_val;
   ss >> new_val;
   BOOST_CHECK_EQUAL(new_val, val);
}

template <class T>
struct is_number : public boost::mpl::false_{};
template <class T>
struct is_number<boost::multiprecision::number<T> > : public boost::mpl::true_{};

template <class T>
void do_round_trip(const T& val, std::ios_base::fmtflags f)
{
   do_round_trip(val, f, is_number<T>());
}

template <class T>
void do_round_trip(const T& val)
{
   do_round_trip(val, std::ios_base::fmtflags(0));
   if(val >= 0)
   {
      do_round_trip(val, std::ios_base::fmtflags(std::ios_base::showbase|std::ios_base::hex));
      do_round_trip(val, std::ios_base::fmtflags(std::ios_base::showbase|std::ios_base::oct));
   }
}

template <class T>
void test_round_trip()
{
   for(unsigned i = 0; i < 1000; ++i)
   {
      T val = generate_random<T>();
      do_round_trip(val);
      do_round_trip(T(-val));
   }
}

int main()
{
#ifdef TEST_MPQ
   test_round_trip<boost::multiprecision::mpq_rational>();
   test_round_trip<boost::rational<boost::multiprecision::mpz_int> >();
   test_round_trip<boost::multiprecision::number<boost::multiprecision::rational_adaptor<boost::multiprecision::gmp_int> > >();
#endif
#ifdef TEST_TOMMATH
   test_round_trip<boost::rational<boost::multiprecision::tom_int> >();
   test_round_trip<boost::multiprecision::tom_rational >();
#endif
#ifdef TEST_CPP_INT
   test_round_trip<boost::rational<boost::multiprecision::cpp_int> >();
   test_round_trip<boost::multiprecision::cpp_rational >();
#endif
   return boost::report_errors();
}