/*
 * Copyright (C) 2021 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>

#include <Eigen/Eigen>

#include <ignition/plugin/Register.hh>

#include "ignition/gazebo/components/AngularVelocity.hh"
#include "ignition/gazebo/components/LinearVelocity.hh"
#include "ignition/gazebo/components/Pose.hh"
#include "ignition/gazebo/components/World.hh"
#include "ignition/gazebo/Link.hh"
#include "ignition/gazebo/Model.hh"
#include "ignition/gazebo/System.hh"
#include "ignition/gazebo/Util.hh"

#include "Hydrodynamics.hh"

using namespace ignition;
using namespace gazebo;
using namespace systems;

/// \brief Private Hydrodynamics data class.
class ignition::gazebo::systems::HydrodynamicsPrivateData
{
  /// \brief Values to set via Plugin Parameters.
  /// Plugin Parameter: Added mass in surge, X_\dot{u}.
  public: double paramXdotU;

  /// \brief Plugin Parameter: Added mass in sway, Y_\dot{v}.
  public: double paramYdotV;

  /// \brief Plugin Parameter: Added mass in heave, Z_\dot{w}.
  public: double paramZdotW;

  /// \brief Plugin Parameter: Added mass in roll, K_\dot{p}.
  public: double paramKdotP;

  /// \brief Plugin Parameter: Added mass in pitch, M_\dot{q}.
  public: double paramMdotQ;

  /// \brief Plugin Parameter: Added mass in yaw, N_\dot{r}.
  public: double paramNdotR;

  /// \brief Plugin Parameter: Linear drag in surge.
  public: double paramXu;

  /// \brief Plugin Parameter: Quadratic drag in surge.
  public: double paramXuu;

  /// \brief Plugin Parameter: Linear drag in sway.
  public: double paramYv;

  /// \brief Plugin Parameter: Quadratic drag in sway.
  public: double paramYvv;

  /// \brief Plugin Parameter: Linear drag in heave.
  public: double paramZw;

  /// \brief Plugin Parameter: Quadratic drag in heave.
  public: double paramZww;

  /// \brief Plugin Parameter: Linear drag in roll.
  public: double paramKp;

  /// \brief Plugin Parameter: Quadratic drag in roll.
  public: double paramKpp;

  /// \brief Plugin Parameter: Linear drag in pitch.
  public: double paramMq;

  /// \brief Plugin Parameter: Quadratic drag in pitch.
  public: double paramMqq;

  /// \brief Plugin Parameter: Linear drag in yaw.
  public: double paramNr;

  /// \brief Plugin Parameter: Quadratic drag in yaw.
  public: double paramNrr;

  /// \brief Water density [kg/m^3].
  public: double waterDensity;

  /// \brief Added mass of vehicle;
  /// See: https://en.wikipedia.org/wiki/Added_mass
  Eigen::MatrixXd Ma;

  /// \brief Previous state.
  public: Eigen::VectorXd prevState;

  /// Link entity
  public: ignition::gazebo::Entity linkEntity;
};

/////////////////////////////////////////////////
void AddAngularVelocityComponent(
  const ignition::gazebo::Entity &_entity,
  ignition::gazebo::EntityComponentManager &_ecm)
{
  if (!_ecm.Component<ignition::gazebo::components::AngularVelocity>(_entity))
  {
    _ecm.CreateComponent(_entity,
      ignition::gazebo::components::AngularVelocity());
  }

  // Create an angular velocity component if one is not present.
  if (!_ecm.Component<ignition::gazebo::components::WorldAngularVelocity>(
      _entity))
  {
    _ecm.CreateComponent(_entity,
      ignition::gazebo::components::WorldAngularVelocity());
  }
}

/////////////////////////////////////////////////
void AddWorldPose(
  const ignition::gazebo::Entity &_entity,
  ignition::gazebo::EntityComponentManager &_ecm)
{
  if (!_ecm.Component<ignition::gazebo::components::WorldPose>(_entity))
  {
    _ecm.CreateComponent(_entity, ignition::gazebo::components::WorldPose());
  }
}

/////////////////////////////////////////////////
void AddWorldLinearVelocity(
  const ignition::gazebo::Entity &_entity,
  ignition::gazebo::EntityComponentManager &_ecm)
{
  if (!_ecm.Component<ignition::gazebo::components::WorldLinearVelocity>(
      _entity))
  {
    _ecm.CreateComponent(_entity,
      ignition::gazebo::components::WorldLinearVelocity());
  }
}

/////////////////////////////////////////////////
double SdfParamDouble(
    const std::shared_ptr<const sdf::Element> &_sdf,
    const std::string& _field,
    double _default)
{
  return _sdf->Get<double>(_field, _default).first;
}

/////////////////////////////////////////////////
Hydrodynamics::Hydrodynamics()
{
  this->dataPtr = std::make_unique<HydrodynamicsPrivateData>();
}

/////////////////////////////////////////////////
Hydrodynamics::~Hydrodynamics()
{
  // Do nothing
}

/////////////////////////////////////////////////
void Hydrodynamics::Configure(
  const ignition::gazebo::Entity &_entity,
  const std::shared_ptr<const sdf::Element> &_sdf,
  ignition::gazebo::EntityComponentManager &_ecm,
  ignition::gazebo::EventManager &/*_eventMgr*/
)
{
  this->dataPtr->waterDensity     = SdfParamDouble(_sdf, "waterDensity", 998);
  this->dataPtr->paramXdotU       = SdfParamDouble(_sdf, "xDotU"       , 5);
  this->dataPtr->paramYdotV       = SdfParamDouble(_sdf, "yDotV"       , 5);
  this->dataPtr->paramZdotW       = SdfParamDouble(_sdf, "zDotW"       , 0.1);
  this->dataPtr->paramKdotP       = SdfParamDouble(_sdf, "kDotP"       , 0.1);
  this->dataPtr->paramMdotQ       = SdfParamDouble(_sdf, "mDotQ"       , 0.1);
  this->dataPtr->paramNdotR       = SdfParamDouble(_sdf, "nDotR"       , 1);
  this->dataPtr->paramXu          = SdfParamDouble(_sdf, "xU"          , 20);
  this->dataPtr->paramXuu         = SdfParamDouble(_sdf, "xUU"         , 0);
  this->dataPtr->paramYv          = SdfParamDouble(_sdf, "yV"          , 20);
  this->dataPtr->paramYvv         = SdfParamDouble(_sdf, "yVV"         , 0);
  this->dataPtr->paramZw          = SdfParamDouble(_sdf, "zW"          , 20);
  this->dataPtr->paramZww         = SdfParamDouble(_sdf, "zWW"         , 0);
  this->dataPtr->paramKp          = SdfParamDouble(_sdf, "kP"          , 20);
  this->dataPtr->paramKpp         = SdfParamDouble(_sdf, "kPP"         , 0);
  this->dataPtr->paramMq          = SdfParamDouble(_sdf, "mQ"          , 20);
  this->dataPtr->paramMqq         = SdfParamDouble(_sdf, "mQQ"         , 0);
  this->dataPtr->paramNr          = SdfParamDouble(_sdf, "nR"          , 20);
  this->dataPtr->paramNrr         = SdfParamDouble(_sdf, "nRR"         , 0);

  // Create model object, to access convenient functions
  auto model = ignition::gazebo::Model(_entity);
  if(!_sdf->HasElement("link_name"))
  {
    ignerr << "You musk specify a <link_name> for the hydrodynamic"
      << " plugin to act upon";
    return;
  }
  auto linkName = _sdf->Get<std::string>("link_name");
  this->dataPtr->linkEntity = model.LinkByName(_ecm, linkName);
  if(!_ecm.HasEntity(this->dataPtr->linkEntity))
  {
    ignerr << "Link name" << linkName << "does not exist";
    return;
  }

  this->dataPtr->prevState = Eigen::VectorXd::Zero(6);

  AddWorldPose(this->dataPtr->linkEntity, _ecm);
  AddAngularVelocityComponent(this->dataPtr->linkEntity, _ecm);
  AddWorldLinearVelocity(this->dataPtr->linkEntity, _ecm);

  // Added mass according to Fossen's equations (p 37)
  this->dataPtr->Ma = Eigen::MatrixXd::Zero(6, 6);

  this->dataPtr->Ma(0, 0) = this->dataPtr->paramXdotU;
  this->dataPtr->Ma(1, 1) = this->dataPtr->paramYdotV;
  this->dataPtr->Ma(2, 2) = this->dataPtr->paramZdotW;
  this->dataPtr->Ma(3, 3) = this->dataPtr->paramKdotP;
  this->dataPtr->Ma(4, 4) = this->dataPtr->paramMdotQ;
  this->dataPtr->Ma(5, 5) = this->dataPtr->paramNdotR;
}

/////////////////////////////////////////////////
void Hydrodynamics::PreUpdate(
      const ignition::gazebo::UpdateInfo &_info,
      ignition::gazebo::EntityComponentManager &_ecm)
{
  if (_info.paused)
    return;

  // These variables follow Fossen's scheme in "Guidance and Control
  // of Ocean Vehicles." The `state` vector contains the ship's current velocity
  // in the formate [x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel].
  // `stateDot` consists of the first derivative in time of the state vector.
  // `Cmat` corresponds to the Centripetal matrix
  // `Dmat` is the drag matrix
  // `Ma` is the added mass.
  Eigen::VectorXd stateDot = Eigen::VectorXd(6);
  Eigen::VectorXd state    = Eigen::VectorXd(6);
  Eigen::MatrixXd Cmat     = Eigen::MatrixXd::Zero(6, 6);
  Eigen::MatrixXd Dmat     = Eigen::MatrixXd::Zero(6, 6);

  // Get vehicle state
  ignition::gazebo::Link baseLink(this->dataPtr->linkEntity);
  auto linearVelocity =
    _ecm.Component<components::WorldLinearVelocity>(this->dataPtr->linkEntity);
  auto rotationalVelocity = baseLink.WorldAngularVelocity(_ecm);

  if (!linearVelocity)
  {
    ignerr << "no linear vel" <<"\n";
    return;
  }

  // Transform state to local frame
  auto pose = baseLink.WorldPose(_ecm);
  // Since we are transforming angular and linear velocity we only care about
  // rotation
  auto localLinearVelocity = pose->Rot().Inverse() * linearVelocity->Data();
  auto localRotationalVelocity = pose->Rot().Inverse() * *rotationalVelocity;

  state(0) = localLinearVelocity.X();
  state(1) = localLinearVelocity.Y();
  state(2) = localLinearVelocity.Z();

  state(3) = localRotationalVelocity.X();
  state(4) = localRotationalVelocity.Y();
  state(5) = localRotationalVelocity.Z();

  auto dt = static_cast<double>(_info.dt.count())/1e9;
  stateDot = (state - this->dataPtr->prevState)/dt;

  this->dataPtr->prevState = state;

  // The added mass
  const Eigen::VectorXd kAmassVec = this->dataPtr->Ma * stateDot;

  // Coriolis and Centripetal forces for under water vehicles (Fossen P. 37)
  // Note: this is significantly different from VRX because we need to account
  // for the under water vehicle's additional DOF
  Cmat(0, 4) = - this->dataPtr->paramZdotW * state(2);
  Cmat(0, 5) = - this->dataPtr->paramYdotV * state(1);
  Cmat(1, 3) =   this->dataPtr->paramZdotW * state(2);
  Cmat(1, 5) = - this->dataPtr->paramXdotU * state(0);
  Cmat(2, 3) = - this->dataPtr->paramYdotV * state(1);
  Cmat(2, 4) =   this->dataPtr->paramXdotU * state(0);
  Cmat(3, 1) = - this->dataPtr->paramZdotW * state(2);
  Cmat(3, 2) =   this->dataPtr->paramYdotV * state(1);
  Cmat(3, 4) = - this->dataPtr->paramNdotR * state(5);
  Cmat(3, 5) =   this->dataPtr->paramMdotQ * state(4);
  Cmat(4, 0) =   this->dataPtr->paramZdotW * state(2);
  Cmat(4, 2) = - this->dataPtr->paramXdotU * state(0);
  Cmat(4, 3) =   this->dataPtr->paramNdotR * state(5);
  Cmat(4, 5) = - this->dataPtr->paramKdotP * state(3);
  Cmat(5, 0) =   this->dataPtr->paramZdotW * state(2);
  Cmat(5, 1) =   this->dataPtr->paramXdotU * state(0);
  Cmat(5, 3) = - this->dataPtr->paramMdotQ * state(4);
  Cmat(5, 4) =   this->dataPtr->paramKdotP * state(3);
  const Eigen::VectorXd kCmatVec = - Cmat * state;

  // Damping forces (Fossen P. 43)
  Dmat(1, 1)
    = - this->dataPtr->paramYv - this->dataPtr->paramYvv * abs(state(1));
  Dmat(0, 0)
    = - this->dataPtr->paramXu - this->dataPtr->paramXuu * abs(state(0));
  Dmat(2, 2)
    = - this->dataPtr->paramZw - this->dataPtr->paramZww * abs(state(2));
  Dmat(3, 3)
    = - this->dataPtr->paramKp - this->dataPtr->paramKpp * abs(state(3));
  Dmat(4, 4)
    = - this->dataPtr->paramMq - this->dataPtr->paramMqq * abs(state(4));
  Dmat(5, 5)
    = - this->dataPtr->paramNr - this->dataPtr->paramNrr * abs(state(5));

  const Eigen::VectorXd kDvec = Dmat * state;

  const Eigen::VectorXd kTotalWrench = kAmassVec + kDvec + kCmatVec;

  ignition::math::Vector3d
    totalForce(-kTotalWrench(0), -kTotalWrench(1), -kTotalWrench(2));
  ignition::math::Vector3d
    totalTorque(-kTotalWrench(3), -kTotalWrench(4), -kTotalWrench(5));

  baseLink.AddWorldWrench(
    _ecm,
    pose->Rot()*(totalForce),
    pose->Rot()*totalTorque);
}

IGNITION_ADD_PLUGIN(
  Hydrodynamics, System,
  Hydrodynamics::ISystemConfigure,
  Hydrodynamics::ISystemPreUpdate
)

IGNITION_ADD_PLUGIN_ALIAS(
  Hydrodynamics,
  "ignition::gazebo::systems::Hydrodynamics")


#include <rbdl/rbdl.h>

#include "urdfreader.h"

#include <assert.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <map>
#include <stack>

#ifdef RBDL_USE_ROS_URDF_LIBRARY
  #include <urdf_model/model.h>
  #include <urdf_parser/urdf_parser.h>
  #include <boost/shared_ptr.hpp>

  typedef urdf::LinkSharedPtr LinkPtr;
  typedef const urdf::LinkConstSharedPtr ConstLinkPtr;
  typedef urdf::JointSharedPtr JointPtr;
  typedef urdf::ModelInterfaceSharedPtr ModelPtr;
  typedef urdf::Joint UrdfJointType;

  #define LINKMAP links_
  #define JOINTMAP joints_
  #define PARENT_TRANSFORM parent_to_joint_origin_transform
  #define RPY getRPY
#else
  #include <urdf/model.h>
  #include <urdf/link.h>
  #include <urdf/joint.h>

  typedef std::shared_ptr<urdf::Link> LinkPtr;
  typedef std::shared_ptr<urdf::Link> ConstLinkPtr;
  typedef std::shared_ptr<urdf::Joint> JointPtr;
  typedef std::shared_ptr<urdf::UrdfModel> ModelPtr;
  typedef urdf::JointType UrdfJointType;

  #define LINKMAP link_map
  #define JOINTMAP joint_map
  #define PARENT_TRANSFORM parent_to_joint_transform
  #define RPY getRpy
#endif

using namespace std;

namespace RigidBodyDynamics
{

  namespace Addons
  {

    using namespace Math;
    using namespace Errors;

    typedef vector<LinkPtr> URDFLinkVector;
    typedef vector<JointPtr> URDFJointVector;
    typedef map<string, LinkPtr> URDFLinkMap;
    typedef map<string, JointPtr> URDFJointMap;

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

void construct_model(Model *rbdl_model, ModelPtr urdf_model,
                     bool floating_base, bool verbose) {

  LinkPtr urdf_root_link;

  URDFLinkMap link_map = urdf_model->LINKMAP;
  URDFJointMap joint_map = urdf_model->JOINTMAP;

  vector<string> joint_names;

  // Holds the links that we are processing in our depth first traversal
  // with the top element being the current link.
  stack<LinkPtr> link_stack;
  // Holds the child joint index of the current link
  stack<int> joint_index_stack;

  // add the bodies in a depth-first order of the model tree
  link_stack.push(link_map[(urdf_model->getRoot()->name)]);

  // add the root body
  ConstLinkPtr root = urdf_model->getRoot();
  Vector3d root_inertial_position;
  Matrix3d root_inertial_inertia;
  double root_inertial_mass;

  Body root_link = Body();

#ifdef RBDL_USE_ROS_URDF_LIBRARY
  if (root->inertial != nullptr) {
    auto I = root->inertial;
#else
  if (root->inertial.has_value()) {
    auto I = &root->inertial.value();
#endif
    root_inertial_mass = I->mass;

    root_inertial_position.set(
      I->origin.position.x,
      I->origin.position.y,
      I->origin.position.z);

    root_inertial_inertia(0, 0) = I->ixx;
    root_inertial_inertia(0, 1) = I->ixy;
    root_inertial_inertia(0, 2) = I->ixz;

    root_inertial_inertia(1, 0) = I->ixy;
    root_inertial_inertia(1, 1) = I->iyy;
    root_inertial_inertia(1, 2) = I->iyz;

    root_inertial_inertia(2, 0) = I->ixz;
    root_inertial_inertia(2, 1) = I->iyz;
    root_inertial_inertia(2, 2) = I->izz;

    if (root_inertial_mass == 0. && root_inertial_inertia != Matrix3d::Zero()) {
      std::ostringstream error_msg;
      error_msg << "Error creating rbdl model! Urdf root link ("
                << root->name
                << ") has inertial but no mass!";
      throw RBDLFileParseError(error_msg.str());
    }

    root_link.mMass = root_inertial_mass;
    root_link.mInertia = root_inertial_inertia;
    root_link.mCenterOfMass = root_inertial_position;
  }

  Joint root_joint(JointTypeFixed);
  if (floating_base) {
    root_joint = JointTypeFloatingBase;
  }

  SpatialTransform root_joint_frame = SpatialTransform();

  if (verbose) {
    cout << "+ Adding Root Body " << endl;
    cout << "  joint frame: " << root_joint_frame << endl;
    if (floating_base) {
      cout << "  joint type : floating" << endl;
    } else {
      cout << "  joint type : fixed" << endl;
    }
    cout << "  body inertia: " << endl
         << root_link.mInertia << endl;
    cout << "  body mass   : " << root_link.mMass << endl;
    cout << "  body name   : " << root->name << endl;
  }

  rbdl_model->AppendBody(root_joint_frame,
                         root_joint,
                         root_link,
                         root->name);

  // depth first traversal: push the first child onto our joint_index_stack
  joint_index_stack.push(0);

  while (link_stack.size() > 0) {
    LinkPtr cur_link = link_stack.top();

    unsigned int joint_idx = joint_index_stack.top();

    // Add any child bodies and increment current joint index if we still
    // have child joints to process.
    if (joint_idx < cur_link->child_joints.size()) {
      JointPtr cur_joint = cur_link->child_joints[joint_idx];

      // increment joint index
      joint_index_stack.pop();
      joint_index_stack.push(joint_idx + 1);

      link_stack.push(link_map[cur_joint->child_link_name]);
      joint_index_stack.push(0);

      if (verbose) {
        for (unsigned int i = 1; i < joint_index_stack.size() - 1; i++) {
          cout << "  ";
        }
        cout << "joint '" << cur_joint->name << "' child link '" <<
          link_stack.top()->name << "' type = " << cur_joint->type << endl;
      }

      joint_names.push_back(cur_joint->name);
    } else {
      link_stack.pop();
      joint_index_stack.pop();
    }
  }

  unsigned int j;
  for (j = 0; j < joint_names.size(); j++) {
    JointPtr urdf_joint = joint_map[joint_names[j]];
    LinkPtr urdf_parent = link_map[urdf_joint->parent_link_name];
    LinkPtr urdf_child = link_map[urdf_joint->child_link_name];

    // determine where to add the current joint and child body
    unsigned int rbdl_parent_id = 0;

    if (urdf_parent->name != "base_link") {
      rbdl_parent_id = rbdl_model->GetBodyId(urdf_parent->name.c_str());
    }

    if (rbdl_parent_id == std::numeric_limits<unsigned int>::max()) {
      ostringstream error_msg;
      error_msg << "Error while processing joint '" << urdf_joint->name
                << "': parent link '" << urdf_parent->name
                << "' could not be found." << endl;
      throw RBDLFileParseError(error_msg.str());
    }

    // create the joint
    Joint rbdl_joint;
    if (urdf_joint->type == UrdfJointType::REVOLUTE ||
        urdf_joint->type == UrdfJointType::CONTINUOUS) {
      rbdl_joint = Joint(SpatialVector(urdf_joint->axis.x, urdf_joint->axis.y,
                                       urdf_joint->axis.z, 0., 0., 0.));
    } else if (urdf_joint->type == UrdfJointType::PRISMATIC) {
      rbdl_joint = Joint(SpatialVector(0., 0., 0., urdf_joint->axis.x,
                                       urdf_joint->axis.y, urdf_joint->axis.z));
    } else if (urdf_joint->type == UrdfJointType::FIXED) {
      rbdl_joint = Joint(JointTypeFixed);
    } else if (urdf_joint->type == UrdfJointType::FLOATING) {
      // todo: what order of DoF should be used?
      rbdl_joint = Joint(
        SpatialVector(0., 0., 0., 1., 0., 0.),
        SpatialVector(0., 0., 0., 0., 1., 0.),
        SpatialVector(0., 0., 0., 0., 0., 1.),
        SpatialVector(1., 0., 0., 0., 0., 0.),
        SpatialVector(0., 1., 0., 0., 0., 0.),
        SpatialVector(0., 0., 1., 0., 0., 0.));
    } else if (urdf_joint->type == UrdfJointType::PLANAR) {
      // todo: which two directions should be used that are perpendicular
      // to the specified axis?
      ostringstream error_msg;
      error_msg << "Error while processing joint '" << urdf_joint->name
                << "': planar joints not yet supported!" << endl;
      throw RBDLFileParseError(error_msg.str());
    }

    // compute the joint transformation
    Vector3d joint_rpy;
    Vector3d joint_translation;
    urdf_joint->PARENT_TRANSFORM.rotation.RPY(joint_rpy[0],
                                              joint_rpy[1], joint_rpy[2]);
    joint_translation.set(
      urdf_joint->PARENT_TRANSFORM.position.x,
      urdf_joint->PARENT_TRANSFORM.position.y,
      urdf_joint->PARENT_TRANSFORM.position.z);
     SpatialTransform rbdl_joint_frame =
       Xrotx(joint_rpy[0])
       * Xroty(joint_rpy[1])
       * Xrotz(joint_rpy[2])
       * Xtrans(joint_translation);
     //rbdl_joint_frame = Xtrans(joint_translation);

    // assemble the body
    Vector3d link_inertial_position;
    Vector3d link_inertial_rpy;
    Matrix3d link_inertial_inertia = Matrix3d::Zero();
    double link_inertial_mass = 0.;

    // but only if we actually have inertial data
#ifdef RBDL_USE_ROS_URDF_LIBRARY
    if (urdf_child->inertial) {
      auto I_child = urdf_child->inertial;
#else
    if (urdf_child->inertial.has_value()) {
      auto I_child = &urdf_child->inertial.value();
#endif
      link_inertial_mass = I_child->mass;

      link_inertial_position.set(
        I_child->origin.position.x,
        I_child->origin.position.y,
        I_child->origin.position.z);
      I_child->origin.rotation.RPY(link_inertial_rpy[0],
                                   link_inertial_rpy[1],
                                   link_inertial_rpy[2]);

      link_inertial_inertia(0, 0) = I_child->ixx;
      link_inertial_inertia(0, 1) = I_child->ixy;
      link_inertial_inertia(0, 2) = I_child->ixz;

      link_inertial_inertia(1, 0) = I_child->ixy;
      link_inertial_inertia(1, 1) = I_child->iyy;
      link_inertial_inertia(1, 2) = I_child->iyz;

      link_inertial_inertia(2, 0) = I_child->ixz;
      link_inertial_inertia(2, 1) = I_child->iyz;
      link_inertial_inertia(2, 2) = I_child->izz;

      if (link_inertial_rpy != Vector3d(0., 0., 0.)) {
        ostringstream error_msg;
        error_msg << "Error while processing body '" << urdf_child->name
                  << "': rotation of body frames not yet supported."
                  << " Please rotate the joint frame instead." << endl;
        throw RBDLFileParseError(error_msg.str());
      }
    }

    Body rbdl_body = Body(link_inertial_mass, link_inertial_position,
                            link_inertial_inertia);

    if (verbose) {
      cout << "+ Adding Body: " << urdf_child->name << endl;
      cout << "  parent_id  : " << rbdl_parent_id << endl;
      cout << "  joint frame: " << rbdl_joint_frame << endl;
      cout << "  joint dofs : " << rbdl_joint.mDoFCount << endl;
      for (unsigned int j = 0; j < rbdl_joint.mDoFCount; j++) {
        cout << "    " << j << ": "
             << rbdl_joint.mJointAxes[j].transpose() << endl;
      }
      cout << "  body inertia: " << endl
           << rbdl_body.mInertia << endl;
      cout << "  body mass   : " << rbdl_body.mMass << endl;
      cout << "  body name   : " << urdf_child->name << endl;
    }

    if (urdf_joint->type == UrdfJointType::FLOATING) {
      Matrix3d zero_matrix = Matrix3d::Zero();
      Body null_body(0., Vector3d::Zero(3), zero_matrix);
      Joint joint_txtytz(JointTypeTranslationXYZ);
      string trans_body_name = urdf_child->name + "_Translate";
      rbdl_model->AddBody(rbdl_parent_id, rbdl_joint_frame,
                          joint_txtytz, null_body,
                          trans_body_name);

      Joint joint_euler_zyx(JointTypeEulerXYZ);
      rbdl_model->AppendBody(SpatialTransform(), joint_euler_zyx,
                             rbdl_body, urdf_child->name);
    } else {
      rbdl_model->AddBody(rbdl_parent_id, rbdl_joint_frame, rbdl_joint,
                          rbdl_body, urdf_child->name);
    }
  }
}
// =============================================================================

RBDL_ADDON_DLLAPI bool URDFReadFromFile(const char *filename, Model *model,
                                        bool floating_base, bool verbose)
{
  ifstream model_file(filename);
  if (!model_file) {
    cerr << "Error opening file '" << filename << "'." << endl;
    abort();
  }

  // reserve memory for the contents of the file
  string model_xml_string;
  model_file.seekg(0, std::ios::end);
  model_xml_string.reserve(model_file.tellg());
  model_file.seekg(0, std::ios::beg);
  model_xml_string.assign((std::istreambuf_iterator<char>(model_file)),
                          std::istreambuf_iterator<char>());

  model_file.close();

  return URDFReadFromString(model_xml_string.c_str(), model, floating_base,
                            verbose);
}

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

RBDL_ADDON_DLLAPI bool URDFReadFromString(const char *model_xml_string,
                                          Model *model,
                                          bool floating_base,
                                          bool verbose)
{
    assert(model);

#ifdef RBDL_USE_ROS_URDF_LIBRARY
    ModelPtr urdf_model = urdf::parseURDF(model_xml_string);
#else
    ModelPtr urdf_model = urdf::UrdfModel::fromUrdfStr(model_xml_string);
#endif

    construct_model(model, urdf_model, floating_base, verbose);

    model->gravity.set(0., 0., -9.81);

    return true;
  }

} // namespace Addons

} // namespace RigidBodyDynamics


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file joint_transition_id.hpp
 * \date Febuary 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <tuple>
#include <memory>

#include <fl/util/traits.hpp>
#include <fl/util/meta.hpp>

#include <fl/model/transition/interface/transition_function.hpp>

namespace fl
{

// Forward declarations
template <typename...Models> class JointTransition;

/**
 * Traits of JointTransition
 */
template <typename...Models>
struct Traits<JointTransition<Models...>>
{
    enum : signed int
    {
        StateDim = JoinSizes<Traits<Models>::State::SizeAtCompileTime...>::Size,
        NoiseDim = JoinSizes<Traits<Models>::Noise::SizeAtCompileTime...>::Size,
        InputDim = JoinSizes<Traits<Models>::Input::SizeAtCompileTime...>::Size
    };

    typedef typename Traits<
                         typename FirstTypeIn<Models...>::Type
                     >::State::Scalar Scalar;

    typedef Eigen::Matrix<Scalar, StateDim, 1> State;
    typedef Eigen::Matrix<Scalar, NoiseDim, 1> Noise;
    typedef Eigen::Matrix<Scalar, InputDim, 1> Input;

    typedef TransitionInterface<
                State,
                Noise,
                Input
            > TransitionBase;
};

/**
 * \ingroup transitions
 */
template <typename ... Models>
class JointTransition
    : public Traits<
                 JointTransition<Models...>
             >::TransitionBase
{
private:
    /** Typdef of \c This for #from_traits(TypeName) helper */
    typedef JointTransition<Models...> This;

public:
    typedef from_traits(State);
    typedef from_traits(Noise);
    typedef from_traits(Input);

public:
    /**
     * Constructor a JointTransition which is a composition of mixture of
     * fixed and dynamic-size process models.
     *
     * \param models    Variadic list of shared pointers of the models
     */
    JointTransition(const Models& ... models)
        : models_(models...)
    { }

    /**
     * \copydoc TransitionInterface::predict_state
     */
    virtual State predict_state(double delta_time,
                                const State& state,
                                const Noise& noise,
                                const Input& input)
    {
        State prediction = State(state_dimension(), 1);

        predict_state<sizeof...(Models)>(
            models_,
            delta_time,
            state,
            noise,
            input,
            prediction);

        return prediction;
    }

    /**
     * \brief Overridable default destructor
     */
    virtual ~JointTransition() noexcept { }

    /**
     * \copydoc TransitionInterface::state_dimension
     *
     * Determines the joint size of the state vector of all models
     */
    virtual int state_dimension() const
    {
        return expand_state_dimension(CreateIndexSequence<sizeof...(Models)>());
    }

    /**
     * \copydoc TransitionInterface::noise_dimension
     *
     * Determines the joint size of the noise vector of all models
     */
    virtual int noise_dimension() const
    {
        return expand_noise_dimension(CreateIndexSequence<sizeof...(Models)>());
    }

    /**
     * \copydoc TransitionInterface::input_dimension
     *
     * Determines the joint size of the input vector of all models
     */
    virtual int input_dimension() const
    {
        return expand_input_dimension(CreateIndexSequence<sizeof...(Models)>());
    }

    std::tuple<Models...>& models()
    {
        return models_;
    }

    const std::tuple<Models...>& models() const
    {
        return models_;
    }

protected:
    /**
     * \brief Contains the points to the sub-models which this joint model
     *        is composed of.
     */
    std::tuple<Models...> models_;

private:
    /** \cond internal */
    template <int...Indices>
    constexpr int expand_state_dimension(IndexSequence<Indices...>) const
    {
        const auto& dims = { std::get<Indices>(models_).state_dimension()... };

        int joint_dim = 0;
        for (auto dim : dims) { joint_dim += dim; }

        return joint_dim;
    }

    template <int...Indices>
    constexpr int expand_noise_dimension(IndexSequence<Indices...>) const
    {
        const auto& dims = { std::get<Indices>(models_).noise_dimension()... };

        int joint_dim = 0;
        for (auto dim : dims) { joint_dim += dim; }

        return joint_dim;
    }

    template <int...Indices>
    constexpr int expand_input_dimension(IndexSequence<Indices...>) const
    {
        const auto& dims = { std::get<Indices>(models_).input_dimension()... };

        int joint_dim = 0;
        for (auto dim : dims) { joint_dim += dim; }

        return joint_dim;
    }

    template <
        int Size,
        int k = 0,
        typename Tuple,
        typename State,
        typename Noise,
        typename Input
    >
    void predict_state(Tuple& models_tuple,
                       double delta_time,
                       const State& state,
                       const Noise& noise,
                       const Input& input,
                       State& prediction,
                       const int state_offset = 0,
                       const int noise_offset = 0,
                       const int input_offset = 0)
    {
        auto&& model = std::get<k>(models_tuple);

        const auto state_dim = model.state_dimension();
        const auto noise_dim = model.noise_dimension();
        const auto input_dim = model.input_dimension();

        prediction.middleRows(state_offset, state_dim) =
            model.predict_state(
                delta_time,
                state.middleRows(state_offset, state_dim),
                noise.middleRows(noise_offset, noise_dim),
                input.middleRows(input_offset, input_dim)
            );

        if (Size == k + 1) return;

        predict_state<Size, k + (k + 1 < Size ? 1 : 0)>(
                    models_tuple,
                    delta_time,
                    state,
                    noise,
                    input,
                    prediction,
                    state_offset + state_dim,
                    noise_offset + noise_dim,
                    input_offset + input_dim);
    }
    /** \endcond */
};

}


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

#include "bempp/common/config_ahmed.hpp"

#ifdef WITH_AHMED

#include "../type_template.hpp"
#include "../type_combinations.hpp"
#include "../check_arrays_are_close.hpp"

#include "assembly/context.hpp"
#include "assembly/discrete_boundary_operator.hpp"
#include "assembly/laplace_3d_adjoint_double_layer_boundary_operator.hpp"
#include "assembly/laplace_3d_double_layer_boundary_operator.hpp"
#include "assembly/laplace_3d_hypersingular_boundary_operator.hpp"
#include "assembly/laplace_3d_single_layer_boundary_operator.hpp"
#include "assembly/helmholtz_3d_hypersingular_boundary_operator.hpp"
#include "assembly/modified_helmholtz_3d_adjoint_double_layer_boundary_operator.hpp"
#include "assembly/modified_helmholtz_3d_double_layer_boundary_operator.hpp"
#include "assembly/modified_helmholtz_3d_single_layer_boundary_operator.hpp"
#include "assembly/modified_helmholtz_3d_hypersingular_boundary_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"

#include "grid/grid_factory.hpp"

#include "space/piecewise_constant_scalar_space.hpp"
#include "space/piecewise_linear_continuous_scalar_space.hpp"
#include "space/piecewise_linear_discontinuous_scalar_space.hpp"
#include "space/piecewise_polynomial_continuous_scalar_space.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/type_traits/is_complex.hpp>

using namespace Bempp;

namespace Bempp
{

namespace
{

template <typename T> T initWaveNumber();
template <> float initWaveNumber() { return 1.2f; }
template <> double initWaveNumber(){ return 1.2; }
template <> std::complex<float> initWaveNumber()
{ return std::complex<float>(1.2f, 0.7f); }
template <> std::complex<double> initWaveNumber()
{ return std::complex<double>(1.2, 0.7); }

}

}

// Tests

BOOST_AUTO_TEST_SUITE(IntegralEvaluationWithBlas)

BOOST_AUTO_TEST_CASE_TEMPLATE(blas_works_for_modified_helmholtz_3d_double_layer_operator_spaces_a,
                              Traits, basis_kernel_result_combinations)
{
    typedef typename Traits::BasisFunctionType BFT;
    typedef typename Traits::KernelType KT;
    typedef typename Traits::ResultType RT;
    typedef typename ScalarTraits<RT>::RealType RealType;

    KT waveNumber = initWaveNumber<KT>();

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
//        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);
    params, "meshes/cube-12-reoriented.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearDiscontinuousScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseQuads(
        new PiecewisePolynomialContinuousScalarSpace<BFT>(grid, 2));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsNoBlas;
    assemblyOptionsNoBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextNoBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsNoBlas));

    BoundaryOperator<BFT, RT> opNoBlas =
            modifiedHelmholtz3dDoubleLayerBoundaryOperator<BFT, KT, RT>(
                contextNoBlas, pwiseQuads, pwiseQuads, pwiseLinears, waveNumber);
    arma::Mat<RT> weakFormNoBlas = opNoBlas.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsBlas;
    assemblyOptionsBlas.enableBlasInQuadrature();
    assemblyOptionsBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsBlas));

    BoundaryOperator<BFT, RT> opBlas =
            modifiedHelmholtz3dDoubleLayerBoundaryOperator<BFT, KT, RT>(
                contextBlas, pwiseQuads, pwiseQuads, pwiseLinears, waveNumber);
    arma::Mat<RT> weakFormBlas = opBlas.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<RT>(
                    weakFormNoBlas, weakFormBlas,
                    100 * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(blas_works_for_modified_helmholtz_3d_double_layer_operator_spaces_b,
                              Traits, basis_kernel_result_combinations)
{
    typedef typename Traits::BasisFunctionType BFT;
    typedef typename Traits::KernelType KT;
    typedef typename Traits::ResultType RT;
    typedef typename ScalarTraits<RT>::RealType RealType;

    KT waveNumber = initWaveNumber<KT>();

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
//        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);
    params, "meshes/cube-12-reoriented.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearDiscontinuousScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseQuads(
        new PiecewisePolynomialContinuousScalarSpace<BFT>(grid, 2));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsNoBlas;
    assemblyOptionsNoBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextNoBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsNoBlas));

    BoundaryOperator<BFT, RT> opNoBlas =
            modifiedHelmholtz3dDoubleLayerBoundaryOperator<BFT, KT, RT>(
                contextNoBlas, pwiseLinears, pwiseLinears, pwiseQuads, waveNumber);
    arma::Mat<RT> weakFormNoBlas = opNoBlas.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsBlas;
    assemblyOptionsBlas.enableBlasInQuadrature();
    assemblyOptionsBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsBlas));

    BoundaryOperator<BFT, RT> opBlas =
            modifiedHelmholtz3dDoubleLayerBoundaryOperator<BFT, KT, RT>(
                contextBlas, pwiseLinears, pwiseLinears, pwiseQuads, waveNumber);
    arma::Mat<RT> weakFormBlas = opBlas.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<RT>(
                    weakFormNoBlas, weakFormBlas,
                    100 * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(blas_works_for_laplace_3d_hypersingular_operator_spaces_a,
                              Traits, basis_result_combinations)
{
    typedef typename Traits::BasisFunctionType BFT;
    typedef typename Traits::ResultType RT;
    typedef typename ScalarTraits<RT>::RealType RealType;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
    //        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);
                params, "meshes/cube-12-reoriented.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearDiscontinuousScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseQuads(
        new PiecewisePolynomialContinuousScalarSpace<BFT>(grid, 2));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsNoBlas;
    assemblyOptionsNoBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextNoBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsNoBlas));

    BoundaryOperator<BFT, RT> opNoBlas =
            laplace3dHypersingularBoundaryOperator<BFT, RT>(
                contextNoBlas, pwiseQuads, pwiseQuads, pwiseLinears);
    arma::Mat<RT> weakFormNoBlas = opNoBlas.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsBlas;
    assemblyOptionsBlas.enableBlasInQuadrature();
    assemblyOptionsBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsBlas));

    BoundaryOperator<BFT, RT> opBlas =
            laplace3dHypersingularBoundaryOperator<BFT, RT>(
                contextBlas, pwiseQuads, pwiseQuads, pwiseLinears);
    arma::Mat<RT> weakFormBlas = opBlas.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<RT>(
                    weakFormNoBlas, weakFormBlas,
                    100 * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(blas_works_for_laplace_3d_hypersingular_operator_spaces_b,
                              Traits, basis_result_combinations)
{
    typedef typename Traits::BasisFunctionType BFT;
    typedef typename Traits::ResultType RT;
    typedef typename ScalarTraits<RT>::RealType RealType;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
    //        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);
                params, "meshes/cube-12-reoriented.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearDiscontinuousScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseQuads(
        new PiecewisePolynomialContinuousScalarSpace<BFT>(grid, 2));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsNoBlas;
    assemblyOptionsNoBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextNoBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsNoBlas));

    BoundaryOperator<BFT, RT> opNoBlas =
            laplace3dHypersingularBoundaryOperator<BFT, RT>(
                contextNoBlas, pwiseLinears, pwiseQuads, pwiseQuads);
    arma::Mat<RT> weakFormNoBlas = opNoBlas.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsBlas;
    assemblyOptionsBlas.enableBlasInQuadrature();
    assemblyOptionsBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsBlas));

    BoundaryOperator<BFT, RT> opBlas =
            laplace3dHypersingularBoundaryOperator<BFT, RT>(
                contextBlas, pwiseLinears, pwiseQuads, pwiseQuads);
    arma::Mat<RT> weakFormBlas = opBlas.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<RT>(
                    weakFormNoBlas, weakFormBlas,
                    100 * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(blas_works_for_modified_helmholtz_3d_hypersingular_operator_spaces_a,
                              Traits, basis_kernel_result_combinations)
{
    typedef typename Traits::BasisFunctionType BFT;
    typedef typename Traits::KernelType KT;
    typedef typename Traits::ResultType RT;
    typedef typename ScalarTraits<RT>::RealType RealType;

    KT waveNumber = initWaveNumber<KT>();

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
//        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);
    params, "meshes/cube-12-reoriented.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearDiscontinuousScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseQuads(
        new PiecewisePolynomialContinuousScalarSpace<BFT>(grid, 2));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsNoBlas;
    assemblyOptionsNoBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextNoBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsNoBlas));

    BoundaryOperator<BFT, RT> opNoBlas =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, KT, RT>(
                contextNoBlas, pwiseQuads, pwiseQuads, pwiseLinears, waveNumber);
    arma::Mat<RT> weakFormNoBlas = opNoBlas.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsBlas;
    assemblyOptionsBlas.enableBlasInQuadrature();
    assemblyOptionsBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsBlas));

    BoundaryOperator<BFT, RT> opBlas =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, KT, RT>(
                contextBlas, pwiseQuads, pwiseQuads, pwiseLinears, waveNumber);
    arma::Mat<RT> weakFormBlas = opBlas.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<RT>(
                    weakFormNoBlas, weakFormBlas,
                    100 * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(blas_works_for_modified_helmholtz_3d_hypersingular_operator_spaces_b,
                              Traits, basis_kernel_result_combinations)
{
    typedef typename Traits::BasisFunctionType BFT;
    typedef typename Traits::KernelType KT;
    typedef typename Traits::ResultType RT;
    typedef typename ScalarTraits<RT>::RealType RealType;

    KT waveNumber = initWaveNumber<KT>();

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
//        params, "../../meshes/sphere-h-0.4.msh", false /* verbose */);
    params, "meshes/cube-12-reoriented.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearDiscontinuousScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseQuads(
        new PiecewisePolynomialContinuousScalarSpace<BFT>(grid, 2));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsNoBlas;
    assemblyOptionsNoBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextNoBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsNoBlas));

    BoundaryOperator<BFT, RT> opNoBlas =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, KT, RT>(
                contextNoBlas, pwiseLinears, pwiseLinears, pwiseQuads, waveNumber);
    arma::Mat<RT> weakFormNoBlas = opNoBlas.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsBlas;
    assemblyOptionsBlas.enableBlasInQuadrature();
    assemblyOptionsBlas.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextBlas(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsBlas));

    BoundaryOperator<BFT, RT> opBlas =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, KT, RT>(
                contextBlas, pwiseLinears, pwiseLinears, pwiseQuads, waveNumber);
    arma::Mat<RT> weakFormBlas = opBlas.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<RT>(
                    weakFormNoBlas, weakFormBlas,
                    100 * std::numeric_limits<RealType>::epsilon()));
}

BOOST_AUTO_TEST_SUITE_END()

#endif // WITH_AHMED


#define BOOST_TEST_MODULE parsers

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <vector>
#include <string>
#include <fstream>
#include <sstream>

#include <mla/matrix/all.h++>
#include <mla/parsers/MatrixMarket.h++>

#include "config_paths.h"

typedef boost::mpl::list<
	mla::matrix::DenseRowMajor<float>,
	mla::matrix::DenseRowMajor<double>,
	mla::matrix::SparseDOK<float>,
	mla::matrix::SparseDOK<double>
> matrix_type_list;

typedef boost::mpl::list<
//	float,
	double
> scalar_list;

template <typename Scalar>
using MatrixType = mla::matrix::SparseDOK<Scalar>;

BOOST_AUTO_TEST_SUITE(parser_MatrixMarket)

BOOST_AUTO_TEST_CASE_TEMPLATE( test_parser_coordinate_real_general, Scalar, scalar_list )
{
	using namespace mla;

	MatrixMarket parser;
	std::stringstream ss;
	ss.str("%%MatrixMarket matrix coordinate real general\n\
5 5 2\n\
1 2 3.0\n\
4 5 6.0\n\
");

	std::vector<MatrixMarket::Symbol> expected_token_vector = {
		MatrixMarket::TS_COMMENT,
		MatrixMarket::TS_COMMENT,
		MatrixMarket::TS_STRING_MATRIX_MARKET,
		MatrixMarket::TS_STRING_MATRIX,
		MatrixMarket::TS_STRING_COORDINATE,
		MatrixMarket::TS_STRING_REAL,
		MatrixMarket::TS_STRING_GENERAL,
		MatrixMarket::TS_EOL
	};

	// test the tokens
	for(MatrixMarket::Symbol expected_token: expected_token_vector)
	{
		MatrixMarket::Symbol received_token = parser.lexer_header(ss);
		BOOST_CHECK_EQUAL( received_token, expected_token);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_lexer_body, Scalar, scalar_list )
{
	using namespace mla;

	MatrixMarket parser;
	std::stringstream ss;
	ss.str("0\n1\n%some crap\n1.0\n-1\n1.234e-56\n");

	std::vector<MatrixMarket::Symbol> expected_token_vector = {
		MatrixMarket::TS_INTEGER_NUMBER,
		MatrixMarket::TS_EOL,
		MatrixMarket::TS_INTEGER_NUMBER,
		MatrixMarket::TS_EOL,
		MatrixMarket::TS_COMMENT,
		MatrixMarket::TS_EOL,
		MatrixMarket::TS_REAL_NUMBER,
		MatrixMarket::TS_EOL,
		MatrixMarket::TS_REAL_NUMBER,
		MatrixMarket::TS_EOL,
		MatrixMarket::TS_REAL_NUMBER,
		MatrixMarket::TS_EOL
	};

	// test the tokens
	for(MatrixMarket::Symbol expected_token: expected_token_vector)
	{
		MatrixMarket::Symbol received_token = parser.lexer_body(ss);
		BOOST_CHECK_EQUAL( received_token, expected_token);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_lexer_body_integer_number, Scalar, scalar_list )
{
	using namespace mla;

	MatrixMarket parser;
	std::stringstream ss;
	ss.str("0 1 2");

	std::vector<unsigned long> expected_integer_number_vector = {
		0,
		1,
		2,
	};

	// test the tokens
	for(unsigned int expected_number: expected_integer_number_vector)
	{
		parser.lexer_body(ss);
		BOOST_CHECK_EQUAL( expected_number, parser.integer_number);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_lexer_body_real_number, Scalar, scalar_list )
{
	using namespace mla;

	MatrixMarket parser;
	std::stringstream ss;
	ss.str("0.0 1.0 2.0    3.1   ");

	std::vector<double> expected_integer_number_vector = {
		0.0,
		1.0,
		2.0,
		3.1
	};

	// test the tokens
	for(double expected_number: expected_integer_number_vector)
	{
		parser.lexer_body(ss);
		BOOST_CHECK_CLOSE( expected_number, parser.real_number, 1e-5);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( basic_coordinate_real_general_matrix, MatrixType, matrix_type_list )
{
	using namespace mla;

	MatrixMarket parser;
	MatrixType matrix;
	std::stringstream ss;

	ss.str("%%MatrixMarket matrix coordinate real general \n\
8 9 2 \n\
1 3 4.0 \n\
5 6 7.0 \n\
");

	parser.parse(ss, matrix);

	// test
	BOOST_CHECK_EQUAL( matrix.rows(), 8 );
	BOOST_CHECK_EQUAL( matrix.columns(), 9 );
	BOOST_CHECK_CLOSE( matrix.getValue(0, 2), 4.0, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(4, 5), 7.0, 1e-5);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( basic_coordinate_real_symmetric_matrix, MatrixType, matrix_type_list )
{
	using namespace mla;

	MatrixMarket parser;
	MatrixType matrix;
	std::stringstream ss;

	ss.str("%%MatrixMarket matrix coordinate real symmetric \n\
9 9 2 \n\
1 3 4.0 \n\
5 6 7.0 \n\
");

	parser.parse(ss, matrix);

	// test
	BOOST_CHECK_EQUAL( matrix.rows(), 9 );
	BOOST_CHECK_EQUAL( matrix.columns(), 9 );
	BOOST_CHECK_CLOSE( matrix.getValue(0, 2), 4.0, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(2, 0), 4.0, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(4, 5), 7.0, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(5, 4), 7.0, 1e-5);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( basic_coordinate_real_skew_symmetric_matrix, MatrixType, matrix_type_list )
{
	using namespace mla;

	MatrixMarket parser;
	MatrixType matrix;
	std::stringstream ss;

	ss.str("%%MatrixMarket matrix coordinate real skew-symmetric \n\
9 9 2 \n\
1 3 4.0 \n\
5 6 7.0 \n\
");

	parser.parse(ss, matrix);

	// test
	BOOST_CHECK_EQUAL( matrix.rows(), 9 );
	BOOST_CHECK_EQUAL( matrix.columns(), 9 );
	BOOST_CHECK_CLOSE( matrix.getValue(0, 2),  4.0, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(2, 0), -4.0, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(4, 5),  7.0, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(5, 4), -7.0, 1e-5);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( parse_coordinate_test01, MatrixType, matrix_type_list )
{
	using namespace mla;

	MatrixMarket parser;
	MatrixType matrix;

	std::string file_path = FULL_MATRIX_MARKET_FILES_PATH "coordinate/test01.mtx";

	std::ifstream file(file_path, std::ifstream::in);

	BOOST_CHECK( file.is_open() );

	// parse
	parser.parse(file, matrix);

	BOOST_CHECK_EQUAL( matrix.rows(), 5 );
	BOOST_CHECK_EQUAL( matrix.columns(), 5 );

	BOOST_CHECK_CLOSE( matrix.getValue(0, 0),  1.0, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(1, 1),  10.5, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(3, 1),  250.5, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(2, 2),  0.015, 1e-5);

	BOOST_CHECK_CLOSE( matrix.getValue(0, 3),  6.0, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(3, 3),  -280.0, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(3, 4),  33.32, 1e-5);
	BOOST_CHECK_CLOSE( matrix.getValue(4, 4),  12.0, 1e-5);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( parse_coordinate_bcsstk14, MatrixType, matrix_type_list )
{
	using namespace mla;

	/**
	 * Parse file taken from:
	 * http://math.nist.gov/MatrixMarket/data/Harwell-Boeing/bcsstruc2/bcsstk14.html
	 **/

	MatrixMarket parser;
	MatrixType matrix;

	std::string file_path = FULL_MATRIX_MARKET_FILES_PATH "coordinate/bcsstk14.mtx";

	std::ifstream file(file_path, std::ifstream::in);

	BOOST_CHECK( file.is_open() );

	// parse
	parser.parse(file, matrix);

	BOOST_CHECK_EQUAL( matrix.rows(), 1806 );
	BOOST_CHECK_EQUAL( matrix.columns(), 1806 );
}

BOOST_AUTO_TEST_SUITE_END()


#include "ValueWithError.hpp"
#include "precompiled.hpp"

#define TEST_COMPATIBLE_TYPE
#include "common.hpp"

#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wglobal-constructors"
#endif // __clang__

#if defined __GNUC__ \
            && ( __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7) ) \
            && !defined __clang__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

#include <boost/test/unit_test.hpp>

using namespace error_propagation;

namespace {

  typedef boost::multiprecision::cpp_dec_float_100 MPH;
  typedef boost::multiprecision::cpp_dec_float_50  MPF;
  typedef ValueWithError<MPH> VD;
  typedef ValueWithError<MPF> VF;

  struct Fixture
  {
   VD pvd;
   VD pvf;
  };

} // anonymous namespace

BOOST_FIXTURE_TEST_SUITE(Test_ValueWithError_multiprecision,Fixture)

BOOST_AUTO_TEST_CASE(default_ctor)
{
  BOOST_CHECK_PV(VD(),MPH(),MPH());
  BOOST_CHECK_PV(VD(MPH(1.0)),MPH(1.0),MPH());
  BOOST_CHECK_PV(VD(MPH(1.0),MPH(2.0)),MPH(1.0),MPH(2.0));
}

BOOST_AUTO_TEST_CASE(assign)
{
  VD a, b;
  a = b;
  BOOST_CHECK_PV(a,b.GetValue(),b.GetError());
}

TEST_OP(times,*)
TEST_OP(plus,+)
TEST_OP(minus,-)
TEST_OP(div,/)

TEST_OP_COMPARE(equal,==)
TEST_OP_COMPARE(not_equal,!=)
TEST_OP_COMPARE(greater,>)
TEST_OP_COMPARE(greater_than,>=)
TEST_OP_COMPARE(smaller,<)
TEST_OP_COMPARE(smaller_than,<=)

TEST_MATH_ONE_ARG(abs)
TEST_MATH_ONE_ARG(acos)
TEST_MATH_ONE_ARG(asin)
TEST_MATH_ONE_ARG(atan)
TEST_MATH_ONE_ARG(cos)
TEST_MATH_ONE_ARG(cosh)
  // TEST_MATH_ONE_ARG(cbrt)
  // TEST_MATH_ONE_ARG(asinh)
  // TEST_MATH_ONE_ARG(acosh)
  // TEST_MATH_ONE_ARG(atanh)
  // TEST_MATH_ONE_ARG(erf)
  // TEST_MATH_ONE_ARG(erfc)
  // TEST_MATH_ONE_ARG(lgamma)
  // TEST_MATH_ONE_ARG(tgamma)
  // TEST_MATH_TWO_ARGS(hypot)
TEST_MATH_ONE_ARG(exp)
TEST_MATH_ONE_ARG(fabs)
TEST_MATH_ONE_ARG(log)
TEST_MATH_TWO_ARGS(pow)
TEST_MATH_ONE_ARG(sin)
TEST_MATH_ONE_ARG(sinh)
TEST_MATH_ONE_ARG(sqrt)
TEST_MATH_ONE_ARG(tan)
TEST_MATH_ONE_ARG(tanh)

BOOST_AUTO_TEST_SUITE_END() // Test_ValueWithError_multiprecision

#ifdef __clang__
#pragma clang diagnostic pop
#endif // __clang__

#if defined __GNUC__ \
            && ( __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7) ) \
            && !defined __clang__
#pragma GCC diagnostic pop
#endif


#include "fruchterman.hpp"
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <array>
#include <utility>
#include <boost/format.hpp>

std::pair<std::vector<std::array<double, 3>>, std::vector<std::array<std::size_t, 3>>>
    read_obj(const std::string& path)
{
    std::vector<std::array<double, 3>> vertex;
    std::vector<std::array<std::size_t, 3>> face;

    std::ifstream ifs{path};
    if (ifs.fail())
    {
        throw std::runtime_error("ERROR: cannot open obj file: " + path + ".");
    }

    std::string buf;
    while (ifs >> buf)
    {
        if (buf == "v")
        {
            double x, y, z;
            ifs >> x >> y >> z;
            vertex.push_back(std::array<double, 3>{x, y, z});
        }
        else if (buf == "f")
        {
            std::size_t v[3] = {};
            for (int i = 0; i < 3; ++i)
            {
                std::string s;
                ifs >> s;
                std::size_t pos{s.find_first_of("/")};
                if (pos == std::string::npos)
                {
                    pos = s.size();
                }
                std::sscanf(s.substr(0, pos).c_str(), "%zu", &v[i]);
            }
            assert (v[0] != 0 && v[1] != 0 && v[2] != 0); // each face has three vetices
            face.push_back(std::array<std::size_t, 3>{--v[0], --v[1], --v[2]});
        }
        // not implemeted
        else if (buf == "#") {}
        else if (buf == "vt") {}
        else if (buf == "vn") {}
        else if (buf == "o") {}
        else if (buf == "g") {}
        else if (buf == "s") {}
        else if (buf == "mtllib") {}
        else if (buf == "usemtl") {}
        else {}

        // discard the rest of the line
        ifs.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
    }

    std::size_t nV{vertex.size()};
    std::size_t nF{face.size()};
    std::size_t nE = nF * 3 / 2; // triangular mesh
    if (nV - nE + nF - 2 != 0)
    {
        std::cerr << boost::format("nV: %zu, nE: %zu, nF: %zu, g = 1 - 0.5 * (nV - nE + nF) = %d")
                         % nV % nE % nF % (1 - (nV - nE + nF) * 0.5)
                  << std::endl;
        throw std::runtime_error("ERROR: input mesh is not 2-manifold with genus 0: " + path + ".");
    }

    return {vertex, face};
}

void write_obj(const std::vector<std::array<double, 3>>& vertex,
    const std::vector<std::array<std::size_t, 3>> face, const std::string& path)
{
    std::ofstream ofs;
    try
    {
        ofs.open(path);
    }
    catch (std::ios_base::failure& e)
    {
        std::cerr << e.what() << std::endl;
    }

    for (const auto& v : vertex)
    {
        ofs << "v " << v[0]
            << " "  << v[1]
            << " "  << v[2] << "\n";
    }
    for (const auto& f : face)
    {
        ofs << "f " << f[0] + 1
            << " "  << f[1] + 1
            << " "  << f[2] + 1 << "\n";
    }
    ofs.close();
}

int main()
{
    const auto [v, f] = read_obj("bunny.obj");
    write_obj(layout_with_fr_3d(v, f, .5, .1, 1), f, "test.obj");
    return 0;
}


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

#include <disparity_graph.hpp>
#include <image.hpp>
#include <indexing.hpp>
#include <indexing_checks.hpp>
#include <pgm_io.hpp>

BOOST_AUTO_TEST_SUITE(DisparityGraphTest)

using sp::graph::disparity::DisparityGraph;
using sp::image::Image;
using sp::image::PGM_IO;
using sp::indexing::checks::edge_exists;
using sp::indexing::checks::neighborhood_exists;
using sp::indexing::checks::node_exists;
using sp::indexing::reparametrization_index;
using sp::indexing::reparametrization_index_fast;
using sp::types::ULONG;

BOOST_AUTO_TEST_CASE(check_nodes_existence)
{
    PGM_IO pgm_io;
    std::istringstream image_content{R"image(
    P2
    3 2
    2
    0 0 0
    0 0 0
    )image"};

    image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image image{*pgm_io.get_image()};
    struct DisparityGraph disparity_graph{image, image, 2, 1, 1};

    for (ULONG y = 0; y < 2; ++y)
    {
        BOOST_CHECK(node_exists(&disparity_graph, {{0, y}, 0}));
        BOOST_CHECK(node_exists(&disparity_graph, {{1, y}, 0}));
        BOOST_CHECK(node_exists(&disparity_graph, {{2, y}, 0}));

        BOOST_CHECK(node_exists(&disparity_graph, {{0, y}, 1}));
        BOOST_CHECK(node_exists(&disparity_graph, {{1, y}, 1}));

        BOOST_CHECK(!node_exists(&disparity_graph, {{0, y}, 2}));
        BOOST_CHECK(!node_exists(&disparity_graph, {{1, y}, 2}));

        BOOST_CHECK(!node_exists(&disparity_graph, {{2, y}, 1}));
    }
    BOOST_CHECK(!node_exists(&disparity_graph, {{0, 2}, 0}));
    BOOST_CHECK(!node_exists(&disparity_graph, {{3, 0}, 2}));
    BOOST_CHECK(!node_exists(&disparity_graph, {{3, 2}, 2}));
}

BOOST_AUTO_TEST_CASE(check_reparametrization_indexing)
{
    PGM_IO pgm_io;
    std::istringstream image_content{R"image(
    P2
    3 2
    2
    0 0 0
    0 0 0
    )image"};

    image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{image, image, 3, 1, 1};

    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 0}, 0}, 0), 0);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 1}, 0}, 0), 1);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 0}, 1}, 0), 2);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 0}, 2}, 0), 4);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 1}, 2}, 0), 5);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 0}, 0}, 1), 6);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 1}, 0}, 1), 7);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 0}, 1}, 1), 8);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 1}, 1}, 1), 9);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 0}, 2}, 1), 10);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 1}, 2}, 1), 11);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 0}, 0}, 2), 12);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 0}, 0}, 3), 18);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{0, 1}, 2}, 3), 23);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{1, 0}, 0}, 0), 24);
    BOOST_CHECK_EQUAL(reparametrization_index_fast(&disparity_graph, {{1, 1}, 2}, 3), 47);
}

BOOST_AUTO_TEST_CASE(check_neighborhood)
{
    PGM_IO pgm_io;
    std::istringstream left_image_content{R"left_image(
    P2
    3 2
    2
    0 1 2
    2 0 1
    )left_image"};
    std::istringstream right_image_content{R"right_image(
    P2
    3 2
    2
    1 0 2
    2 0 1
    )right_image"};

    left_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image left_image{*pgm_io.get_image()};

    right_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image right_image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{left_image, right_image, 3, 1, 1};

    BOOST_CHECK(neighborhood_exists(&disparity_graph, {0, 0}, {1, 0}));
    BOOST_CHECK(neighborhood_exists(&disparity_graph, {0, 0}, {0, 1}));

    BOOST_CHECK(neighborhood_exists(&disparity_graph, {1, 0}, {2, 0}));

    BOOST_CHECK(neighborhood_exists(&disparity_graph, {1, 0}, {0, 0}));
    BOOST_CHECK(neighborhood_exists(&disparity_graph, {1, 0}, {1, 1}));

    BOOST_CHECK(!neighborhood_exists(&disparity_graph, {0, 0}, {0, 0}));
    BOOST_CHECK(!neighborhood_exists(&disparity_graph, {0, 0}, {1, 1}));
    BOOST_CHECK(!neighborhood_exists(&disparity_graph, {0, 0}, {0, 2}));
    BOOST_CHECK(!neighborhood_exists(&disparity_graph, {0, 0}, {2, 0}));

    BOOST_CHECK(!neighborhood_exists(&disparity_graph, {1, 0}, {1, 0}));
    BOOST_CHECK(!neighborhood_exists(&disparity_graph, {1, 0}, {0, 1}));
    BOOST_CHECK(!neighborhood_exists(&disparity_graph, {1, 0}, {2, 1}));
}

BOOST_AUTO_TEST_CASE(check_edges_existence)
{
    PGM_IO pgm_io;
    std::istringstream left_image_content{R"left_image(
    P2
    3 2
    2
    0 1 2
    2 0 1
    )left_image"};
    std::istringstream right_image_content{R"right_image(
    P2
    3 2
    2
    1 0 2
    2 0 1
    )right_image"};

    left_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image left_image{*pgm_io.get_image()};

    right_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image right_image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{left_image, right_image, 2, 1, 1};

    BOOST_CHECK(edge_exists(&disparity_graph, {{{0, 0}, 0}, {{1, 0}, 0}}));
    BOOST_CHECK(edge_exists(&disparity_graph, {{{0, 0}, 0}, {{1, 0}, 1}}));

    BOOST_CHECK(edge_exists(&disparity_graph, {{{1, 0}, 0}, {{0, 0}, 0}}));
    BOOST_CHECK(edge_exists(&disparity_graph, {{{1, 0}, 1}, {{0, 0}, 0}}));

    BOOST_CHECK(!edge_exists(&disparity_graph, {{{1, 0}, 2}, {{1, 0}, 1}}));
    BOOST_CHECK(!edge_exists(&disparity_graph, {{{0, 0}, 1}, {{1, 0}, 2}}));

    BOOST_CHECK(!edge_exists(&disparity_graph, {{{0, 0}, 2}, {{1, 0}, 0}}));
    BOOST_CHECK(!edge_exists(&disparity_graph, {{{1, 0}, 0}, {{1, 0}, 2}}));
}

BOOST_AUTO_TEST_CASE(check_continuity_constraint)
{
    PGM_IO pgm_io;
    std::istringstream left_image_content{R"left_image(
    P2
    3 2
    2
    0 1 2
    2 0 1
    )left_image"};
    std::istringstream right_image_content{R"right_image(
    P2
    3 2
    2
    1 0 2
    2 0 1
    )right_image"};

    left_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image left_image{*pgm_io.get_image()};

    right_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image right_image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{left_image, right_image, 3, 1, 1};

    BOOST_CHECK(!edge_exists(&disparity_graph, {{{0, 0}, 2}, {{1, 0}, 0}}));
    BOOST_CHECK(!edge_exists(&disparity_graph, {{{1, 0}, 0}, {{0, 0}, 2}}));

    BOOST_CHECK(!edge_exists(&disparity_graph, {{{0, 1}, 2}, {{1, 1}, 0}}));
    BOOST_CHECK(!edge_exists(&disparity_graph, {{{1, 1}, 0}, {{0, 1}, 2}}));
}

BOOST_AUTO_TEST_CASE(check_initial_edges_penalties)
{
    PGM_IO pgm_io;
    std::istringstream left_image_content{R"left_image(
    P2
    3 2
    2
    0 1 2
    2 0 1
    )left_image"};
    std::istringstream right_image_content{R"right_image(
    P2
    3 2
    2
    1 0 2
    2 0 1
    )right_image"};

    left_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image left_image{*pgm_io.get_image()};

    right_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image right_image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{left_image, right_image, 3, 1, 1};

    BOOST_CHECK_CLOSE(
        edge_penalty(&disparity_graph, {{{0, 0}, 0}, {{1, 0}, 0}}),
        0.0,
        0.5
    );
    BOOST_CHECK_CLOSE(
        edge_penalty(&disparity_graph, {{{0, 0}, 0}, {{0, 1}, 1}}),
        1.0,
        0.5
    );

    BOOST_CHECK_CLOSE(
        edge_penalty(&disparity_graph, {{{1, 0}, 1}, {{2, 0}, 0}}),
        1.0,
        0.5
    );

    BOOST_CHECK_CLOSE(
        edge_penalty(&disparity_graph, {{{1, 0}, 0}, {{0, 0}, 2}}),
        4.0,
        0.5
    );
    BOOST_CHECK_CLOSE(
        edge_penalty(&disparity_graph, {{{1, 0}, 2}, {{0, 1}, 0}}),
        4.0,
        0.5
    );
    BOOST_CHECK_CLOSE(
        edge_penalty(&disparity_graph, {{{1, 0}, 2}, {{1, 1}, 2}}),
        0.0,
        0.5
    );
}

BOOST_AUTO_TEST_CASE(check_initial_nodes_penalties)
{
    PGM_IO pgm_io;
    std::istringstream left_image_content{R"left_image(
    P2
    3 2
    2
    0 1 2
    2 0 1
    )left_image"};
    std::istringstream right_image_content{R"right_image(
    P2
    3 2
    2
    1 0 2
    2 0 1
    )right_image"};

    left_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image left_image{*pgm_io.get_image()};

    right_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image right_image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{left_image, right_image, 3, 1, 1};

    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{0, 0}, 0}), 1.0, 0.5);
    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{0, 0}, 1}), 0.0, 0.5);
    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{0, 0}, 2}), 1.0, 0.5);

    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{1, 0}, 0}), 1.0, 0.5);
    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{1, 0}, 1}), 4.0, 0.5);

    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{2, 0}, 0}), 0.0, 0.5);

    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{0, 1}, 0}), 0.0, 0.5);
    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{0, 1}, 1}), 4.0, 0.5);
    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{0, 1}, 2}), 1.0, 0.5);

    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{1, 1}, 0}), 0.0, 0.5);
    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{1, 1}, 1}), 1.0, 0.5);

    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{2, 1}, 0}), 0.0, 0.5);
}

BOOST_AUTO_TEST_CASE(check_edges_penalties)
{
    PGM_IO pgm_io;
    std::istringstream left_image_content{R"left_image(
    P2
    3 2
    2
    0 1 2
    2 0 1
    )left_image"};
    std::istringstream right_image_content{R"right_image(
    P2
    3 2
    2
    1 0 2
    2 0 1
    )right_image"};

    left_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image left_image{*pgm_io.get_image()};

    right_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image right_image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{left_image, right_image, 3, 1, 10};

    disparity_graph.reparametrization[
        reparametrization_index(&disparity_graph, {{0, 0}, 0}, {1, 0})
    ] = -2;

    BOOST_CHECK_CLOSE(
        edge_penalty(&disparity_graph, {{{0, 0}, 0}, {{1, 0}, 0}}),
        2,
        1
    );
    BOOST_CHECK_CLOSE(
        edge_penalty(&disparity_graph, {{{0, 0}, 0}, {{1, 0}, 1}}),
        12,
        1
    );

    BOOST_CHECK_CLOSE(
        edge_penalty(&disparity_graph, {{{1, 0}, 0}, {{0, 0}, 0}}),
        2,
        1
    );
    BOOST_CHECK_CLOSE(
        edge_penalty(&disparity_graph, {{{1, 0}, 0}, {{0, 0}, 1}}),
        10,
        1
    );
}

BOOST_AUTO_TEST_CASE(check_nodes_penalties)
{
    PGM_IO pgm_io;
    std::istringstream left_image_content{R"left_image(
    P2
    3 2
    2
    0 1 2
    2 0 1
    )left_image"};
    std::istringstream right_image_content{R"right_image(
    P2
    3 2
    2
    1 0 2
    2 0 1
    )right_image"};

    left_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image left_image{*pgm_io.get_image()};

    right_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image right_image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{left_image, right_image, 3, 10, 1};

    disparity_graph.reparametrization[
        reparametrization_index(&disparity_graph, {{0, 0}, 0}, {1, 0})
    ] = -2;

    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{0, 0}, 0}), 8, 1);
    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{0, 0}, 1}), 0, 1);

    BOOST_CHECK_CLOSE(node_penalty(&disparity_graph, {{1, 0}, 0}), 10, 1);
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// 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)
//
// Example: Custom coordinate system example, using transform

#include <iostream>

#include <boost/geometry/geometry.hpp>

// See also c10_custom_cs_example

// 1: declare, for example two cartesian coordinate systems
struct cart {};
struct cart_shifted5 {};

// 2: register to which coordinate system family they belong
namespace boost { namespace geometry { namespace traits
{

template<> struct cs_tag<cart> { typedef cartesian_tag type; };
template<> struct cs_tag<cart_shifted5> { typedef cartesian_tag type; };

}}} // namespaces

// 3: sample implementation of a shift
//    to convert coordinate system "cart" to "cart_shirted5"
struct shift
{
    template <typename P1, typename P2>
    inline bool apply(P1 const& p1, P2& p2) const
    {
        namespace bg = boost::geometry;
        bg::set<0>(p2, bg::get<0>(p1) + 5);
        bg::set<1>(p2, bg::get<1>(p1));
        return true;
    }
};

// 4: register the default strategy to transform any cart point to any cart_shifted5 point
namespace boost { namespace geometry  { namespace strategy { namespace transform { namespace services
{

template <typename P1, typename P2>
struct default_strategy<cartesian_tag, cartesian_tag, cart, cart_shifted5, 2, 2, P1, P2>
{
    typedef shift type;
};

}}}}} // namespaces

// 5: implement a distance strategy between the two different ones
struct shift_and_calc_distance
{
    template <typename P1, typename P2>
    inline double apply(P1 const& p1, P2 const& p2) const
    {
        P2 p1_shifted;
        boost::geometry::transform(p1, p1_shifted);
        return boost::geometry::distance(p1_shifted, p2);
    }
};

// 6: Define point types using this explicitly
typedef boost::geometry::model::point<double, 2, cart> point1;
typedef boost::geometry::model::point<double, 2, cart_shifted5> point2;

// 7: register the distance strategy
namespace boost { namespace geometry { namespace strategy { namespace distance { namespace services
{
    template <>
    struct tag<shift_and_calc_distance>
    {
        typedef strategy_tag_distance_point_point type;
    };

    template <typename P1, typename P2>
    struct return_type<shift_and_calc_distance, P1, P2>
    {
        typedef double type;
    };

    template <>
    struct default_strategy<point_tag, point1, point2, cartesian_tag, cartesian_tag>
    {
        typedef shift_and_calc_distance type;
    };

}}}}}

int main()
{
    point1 p1_a(0, 0), p1_b(5, 5);
    point2 p2_a(2, 2), p2_b(6, 6);

    // Distances run for points on the same coordinate system.
    // This is possible by default because they are cartesian coordinate systems.
    double d1 = boost::geometry::distance(p1_a, p1_b);
    double d2 = boost::geometry::distance(p2_a, p2_b);

    std::cout << d1 << " " << d2 << std::endl;

    // Transform from a to b:
    boost::geometry::model::point<double, 2, cart_shifted5> p1_shifted;
    boost::geometry::transform(p1_a, p1_shifted);

    // Of course this can be calculated now, same CS
    double d3 = boost::geometry::distance(p1_shifted, p2_a);

    // Calculate distance between them. Note that inside distance the
    // transformation is called.
    double d4 = boost::geometry::distance(p1_a, p2_a);

    // The result should be the same.
    std::cout << d3 << " " << d4 << std::endl;

    return 0;
}


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

#ifndef MTL_TFUNCTOR_TFUNCTOR_MIXED_INCLUDE
#define MTL_TFUNCTOR_TFUNCTOR_MIXED_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/concept/std_concept.hpp>
#include <cmath>

namespace mtl { namespace tfunctor {

// Currently we define only scalar concepts
// Eventually this will be expanded like "scale"

/// Plus functor that stores left summand
template <typename Value1, typename Value2>
struct left_plus
{
    typedef typename Addable<Value1, Value2>::result_type result_type;
    explicit left_plus(const Value1& v1) : v1(v1) {}

    result_type operator() (const Value2& v2) const
    {
	return v1 + v2;
    }
private:
    Value1 v1;
};

/// Plus functor that stores right summand
template <typename Value1, typename Value2>
struct right_plus
{
    typedef typename Addable<Value1, Value2>::result_type result_type;
    explicit right_plus(const Value2& v2) : v2(v2) {}

    result_type operator() (const Value1& v1) const
    {
	return v1 + v2;
    }
private:
    Value2 v2;
};

/// Minus functor that stores left summand
template <typename Value1, typename Value2>
struct left_minus
{
    typedef typename Subtractable<Value1, Value2>::result_type result_type;
    explicit left_minus(const Value1& v1) : v1(v1) {}

    result_type operator() (const Value2& v2) const
    {
	return v1 - v2;
    }
private:
    Value1 v1;
};

/// Minus functor that stores right summand
template <typename Value1, typename Value2>
struct right_minus
{
    typedef typename Subtractable<Value1, Value2>::result_type result_type;
    explicit right_minus(const Value2& v2) : v2(v2) {}

    result_type operator() (const Value1& v1) const
    {
	return v1 - v2;
    }
private:
    Value2 v2;
};

/// Minimum functor that stores left operand
/** Result type is right operand.
    This way the min of a scalar and a vector keeps the vector type. **/
template <typename Value1, typename Value2>
struct left_min
{
    typedef Value2 result_type;
    explicit left_min(const Value1& v1) : v1(v1) {}

    result_type operator() (const Value2& v2) const
    {
	return v1 < v2 ? Value2(v1) : v2;
    }
private:
    Value1 v1;
};

/// Minimum functor that stores right operand
/** Result type is left operand.
    This way the min of a vector and a scalar keeps the vector type. **/
template <typename Value1, typename Value2>
struct right_min
{
    typedef Value1 result_type;
    explicit right_min(const Value2& v2) : v2(v2) {}

    result_type operator() (const Value1& v1) const
    {
	return v1 < v2 ? v1 : Value1(v2);
    }
private:
    Value2 v2;
};

/// Maximum functor that stores left operand
/** Result type is right operand.
    This way the max of a scalar and a vector keeps the vector type. **/
template <typename Value1, typename Value2>
struct left_max
{
    typedef Value2 result_type;
    explicit left_max(const Value1& v1) : v1(v1) {}

    result_type operator() (const Value2& v2) const
    {
	return v1 < v2 ? v2 : Value2(v1);
    }
private:
    Value1 v1;
};

/// Maximum functor that stores right operand
/** Result type is left operand.
    This way the max of a vector and a scalar keeps the vector type. **/
template <typename Value1, typename Value2>
struct right_max
{
    typedef Value1 result_type;
    explicit right_max(const Value2& v2) : v2(v2) {}

    result_type operator() (const Value1& v1) const
    {
	return v1 < v2 ? Value1(v2) : v1;
    }
private:
    Value2 v2;
};

/// Raise Value1 to the power of Value2.
/** Value2 is bound in the functor and Value1 passes to the operator. **/
template <typename Value1, typename Value2>
struct pow_by
{
    typedef Value1 result_type; // not perfect but works for all C++98 overloads

    explicit pow_by(const Value2& exp) : exp(exp) {}

    result_type operator() (const Value1& base) const
    {
        using std::pow;
        return pow(base, exp);
    }

    Value2 value() const { return exp; }
private:
    Value2 exp;
};

}} // namespace mtl::tfunctor

#endif // MTL_TFUNCTOR_TFUNCTOR_MIXED_INCLUDE


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

#include <iostream>
#include <cmath>
#include <vector>

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

const unsigned sz= 5;

inline float f(float x) { return x; }
inline double f(double x) { return x; }

template <typename Vector>
void test(Vector& v, const char* name)
{
    typedef typename mtl::Collection<Vector>::value_type T;
    std::cout << "\n" << name << "\n";

    Vector w(v);
    iota(w);
    MTL_THROW_IF(w[4] != f(T(4)), mtl::runtime_error("Wrong value in w"));
    std::cout << "w == " << w << "\n";

    iota(w, T(5));
    MTL_THROW_IF(w[4] != f(T(9)), mtl::runtime_error("Wrong value in w"));
    std::cout << "w == " << w << "\n";

}

int main(int, char**)
{
    using namespace mtl;
    dense_vector<float>                                                 cf(sz, 0.0);
    dense_vector<double>                                                cd(sz, 0.0);
    dense_vector<float, mtl::vec::parameters<row_major> >            rf(sz, 0.0);

    test(cf, "dense_vector<float>");
    test(cd, "dense_vector<double>");
    test(rf, "dense_vector<float, parameters<row_major> >");

    return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Musegaas, P. (2012). Optimization of Space Trajectories Including Multiple Gravity Assists
 *          and Deep Space Maneuvers. MSc Thesis, Delft University of Technology, Delft,
 *          The Netherlands.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include <tudat/basics/testMacros.h>

#include "tudat/astro/ephemerides/constantEphemeris.h"
#include "tudat/astro/mission_segments/transferNode.h"

namespace tudat
{
namespace unit_tests
{

//! Test implementation of departure leg within MGA trajectory model.
BOOST_AUTO_TEST_SUITE( test_capture_leg )

//! Test delta-V computation for infinite parking orbit.
BOOST_AUTO_TEST_CASE( testVelocitiesInfiniteParkingOrbit )
{
    // Set tolerance.
    const double tolerance = 1.0e-13;

    // Expected test result based on the last part of the ideal Messenger trajectory as modelled by
    // GTOP software distributed and downloadable from the ESA website, or within the PaGMO
    // Astrotoolbox.
    // Note that this concerns a capture at infinite distance and hence is not the best test case.
    const double expectedDeltaV = 4632.24252029314;

    // Specify the required parameters.
    // Set the planetary positions and velocities.
    const Eigen::Vector6d planetState = ( Eigen::Vector6d( ) <<
                53627979831.9492, -5044669560.01491, -5339232305.54465,
                -4857.99954791498, 50668.339570669, 4579.44661303178 ).finished( );
    std::shared_ptr< ephemerides::Ephemeris > constantEphemeris =
            std::make_shared< ephemerides::ConstantEphemeris >( planetState );

    // Set velocity before capture body.
    const Eigen::Vector3d velocityBeforePlanet (
                -5080.6362408257, 55179.1205883308, 3549.4183219232 );

    // Set the gravitational parameters. The sun's is irrelevant for the deltaV consumption, but
    // required only for other functionality in the class.
    const double mercuryGravitationalParameter = 2.2321e13;

    // Set the capture orbit (at inifinity).
    const double semiMajorAxis = std::numeric_limits< double >::infinity( );
    const double eccentricity = 0.;

    // Set test case.
    using namespace tudat::mission_segments;
    CaptureAndInsertionNode captureNode(
                constantEphemeris,
                mercuryGravitationalParameter, semiMajorAxis, eccentricity,
                [=]( ){return velocityBeforePlanet; } );
    captureNode.updateNodeParameters( ( Eigen::VectorXd( 1 )<<0.0 ).finished( ) );

    // Prepare the variables for the results.
    double resultingDeltaV = captureNode.getNodeDeltaV( );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaV, resultingDeltaV, tolerance );
}

//! Test delta-V computation for circular parking orbit.
BOOST_AUTO_TEST_CASE( testVelocitiesCircularParkingOrbit )
{
    // Set tolerance.
    const double tolerance = 1.0e-13;

    // Expected test result based on Table 18.2 of [Wakker, 2007]. Capture velocity to arrive at
    // a parking orbit around Mars of 1.1 times the planetary radius starting from Earth. Result
    // calculated with more precision separately using Equation 18.25 of [Wakker, 2007].
    const double expectedDeltaV = 2087.1062716740798;

    // Specify the required parameters.
    // Set the planetary positions and velocities.
    const Eigen::Vector6d planetState = ( Eigen::Vector6d( ) <<
                 227936637698.942, 0.0, 0.0,
                 0.0, 24129.4836355380, 0.0 ).finished( );
    std::shared_ptr< ephemerides::Ephemeris > constantEphemeris =
            std::make_shared< ephemerides::ConstantEphemeris >( planetState );

    // Set velocity before capture body.
    const Eigen::Vector3d velocityBeforePlanet ( 0.0, 21480.6500358053, 0.0 );

    // Set the gravitational parameters. The Sun's is irrelevant for the deltaV consumption, but
    // required only for other functionality in the class.
    const double marsGravitationalParameter = 4.2830e13;

    // Set the capture orbit.
    const double semiMajorAxis = 1.1 * 3.3895e6;;
    const double eccentricity = 0.;

    // Set test case.
    using namespace tudat::mission_segments;
    CaptureAndInsertionNode captureNode(
                constantEphemeris,
                marsGravitationalParameter, semiMajorAxis, eccentricity,
                [=]( ){return velocityBeforePlanet; } );
    captureNode.updateNodeParameters( ( Eigen::VectorXd( 1 )<<0.0 ) .finished( ) );

    // Prepare the variables for the results.
    double resultingDeltaV = captureNode.getNodeDeltaV( );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaV, resultingDeltaV, tolerance );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*
* LLNS Copyright Start
* Copyright (c) 2017, Lawrence Livermore National Security
* This work was performed under the auspices of the U.S. Department
* of Energy by Lawrence Livermore National Laboratory in part under
* Contract W-7405-Eng-48 and in part under Contract DE-AC52-07NA27344.
* Produced at the Lawrence Livermore National Laboratory.
* All rights reserved.
* For details, see the LICENSE file.
* LLNS Copyright End
*/

#ifndef _MATRIX_DATA_BOOST_H_
#define _MATRIX_DATA_BOOST_H_
#pragma once

#include "utilities/matrixData.hpp"
#include <boost/numeric/ublas/matrix_sparse.hpp>

/** @brief class implementing an expandable sparse matrix based on the boost matrices geared for Jacobian entries*/
template <class type>
class matrixDataBoost : public matrixData<type>
{
  public:
    boost::numeric::ublas::mapped_matrix<type, boost::numeric::ublas::column_major>
      m;  //!<boost matrix to interface
  public:
    /** @brief constructor
    @param[in] R  number of rows
    @param[in] C  number of columns
    */
    matrixDataBoost (count_t R, count_t C) : matrixData<type> (R, C), m (R, C){};
    /**
    * function to clear the data
    */
    void clear () override { m.clear (); }
    void assign (index_t X, index_t Y, const type &num) override
    {
        m (X, Y) += num;
        // printf("insert [%d,%d]=%f\n", X, Y, num);
    }

    count_t size () const override { return static_cast<count_t> (m.nnz ()); }
    count_t capacity () const override { return m.nnz_capacity (); }
    /**
    * @brief don't use this function
    */
    matrixElement<type> element (index_t N) { return matrixElement<type> (); }
    /**
    * @brief don't use this function
    */
    matrixElement<type> start () override
    {
        return {matrixData<type>::rowLim, matrixData<type>::colLim, type (0)};
    }
    /**
    * @brief don't use this function
    */
    matrixElement<type> next () override { return {matrixData<type>::rowLim, matrixData<type>::colLim, type (0)}; }
    type at (index_t rowN, index_t colN) const override { return m (rowN, colN); }
    auto begin () const { return matriIteratorBoost (this, 0); }
    auto end () const { return matriIteratorBoost (this, size ()); }

  protected:
    class matrixIteratorBoost
    {
      public:
        explicit matrixIteratorBoost (const matrixDataBoost<type> *matrixData, index_t start = 0)
        {
            if (start == 0)
            {
                cptr = mDB->m.begin1 ();
                if (cptr == mDB->m.end1 ())
                {
                    return;
                }
                cptr2 = cptr.begin ();
                cptr2end = cptr.end ();
                while (cptr2 == cptr2end)
                {
                    ++cptr;
                    if (cptr == mDB->m.end1 ())
                    {
                        return;
                    }
                    cptr2 = cptr.begin ();
                    cptr2end = cptr.end ();
                }
            }
            else if (start == mDB->size ())
            {
                cptr = mDB->m.end1 ();
            }
        }

        matrixIteratorBoost &operator++ () override
        {
            ++cptr2;
            if (cptr2 == cptr2end)
            {
                ++cptr;
                cptr2 = cptr.begin ();
                cptr2end = cptr.end ();
                while (cptr2 == cptr2end)
                {
                    ++cptr;
                    if (cptr == mDB->m.end1 ())
                    {
                        return;
                    }
                    cptr2 = cptr.begin ();
                    cptr2end = cptr.end ();
                }
            }
        }

        virtual matrixElement<type> operator* () const override
        {
            return {cptr2.index1 (), cptr2.index2 (), *cptr2};
        }

      private:
        matrixDataBoost<type> *mDB = nullptr;
        decltype (mDB->m.begin1 ()) cptr;
        decltype (cptr->begin ()) cptr2;
        decltype (cptr->end ()) cptr2end;
    };
};

#endif


// Boost.Signals library

// Copyright Douglas Gregor 2001-2003. 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)

// For more information, see http://www.boost.org

#include <boost/signals/signal2.hpp>
#include <iostream>

template<typename T>
struct first_positive {
  typedef T result_type;

  template<typename InputIterator>
  T operator()(InputIterator first, InputIterator last) const
  {
    while (first != last && !(*first > 0)) { ++first; }
    return (first == last) ? 0
                           : *first;
  }
};

template<typename T>
struct noisy_divide {
  typedef T result_type;

  T operator()(const T& x, const T& y) const
  {
    std::cout << "Dividing " << x << " and " << y << std::endl;
    return x/y;
  }
};

int main()
{
  boost::signal2<int, int, int, first_positive<int> > sig_positive;
  sig_positive.connect(std::plus<int>());
  sig_positive.connect(std::multiplies<int>());
  sig_positive.connect(std::minus<int>());
  sig_positive.connect(noisy_divide<int>());

  assert(sig_positive(3, -5) == 8); // returns 8, but prints nothing

  return 0;
}


//==================================================================================================
/*!
  @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_REC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-arithmetic
    This function object returns the inverse (reciprocal) of the entry.

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

    @par Notes

    The call to `rec(x)` is similar to  `One(as(x))/x`

    For integral typed entries the result is always in the set \f$\{0,  \pm1, Valmax \}\f$

    @par Decorators

     - raw_ Many simd architectures provide an intrinsic that computes some bits of the inverse (at least 12)
            and don't care of denormals or limiting values. If it exists this is obtained by the raw_ decorator.
            If the intrinsic doesn't exist the regular @c rec is called.

    @par Example:

      @snippet rec.cpp rec

    @par Possible output:

      @snippet rec.txt rec
  **/
  Value rec(Value const& x);
} }
#endif

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

#endif