//
// Copyright (c) 2015-2016,2018 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include <iostream>

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unbounded.hpp"
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
#include "pinocchio/multibody/joint/joint-prismatic.hpp"
#include "pinocchio/multibody/joint/joint-prismatic-unaligned.hpp"
#include "pinocchio/multibody/joint/joint-translation.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"

#include <boost/test/unit_test.hpp>

//#define VERBOSE

using namespace pinocchio;

template <typename JoinData_t>
void printOutJointData(
#ifdef VERBOSE
  const Eigen::VectorXd & q,
  const Eigen::VectorXd & q_dot,
  const JoinData_t & joint_data)
{
  using namespace std;

  cout << "q: " << q.transpose () << endl;
  cout << "q_dot: " << q_dot.transpose () << endl;
  cout << "Joint configuration:" << endl << joint_data.M << endl;
  cout << "v_J:\n" << (Motion) joint_data.v << endl;
  cout << "c_J:\n" << (Motion) joint_data.c << endl;
}
#else
const Eigen::VectorXd &,
const Eigen::VectorXd &,
const JoinData_t &)
{}
#endif

template<typename D>
void addJointAndBody(Model & model, const JointModelBase<D> & jmodel, const Model::JointIndex parent_id, const SE3 & joint_placement, const std::string & joint_name, const Inertia & Y)
{
  Model::JointIndex idx;

  idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
  model.appendBodyToJoint(idx,Y);
}

BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned)

BOOST_AUTO_TEST_CASE(vsRX)
{
  using namespace pinocchio;
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Eigen::Vector3d axis;
  axis << 1.0, 0.0, 0.0;

  Model modelRX, modelRevoluteUnaligned;

  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  JointModelRevoluteUnaligned joint_model_RU(axis);

  addJointAndBody(modelRX,JointModelRX(),0,pos,"rx",inertia);
  addJointAndBody(modelRevoluteUnaligned,joint_model_RU,0,pos,"revolute-unaligned",inertia);

  Data dataRX(modelRX);
  Data dataRevoluteUnaligned(modelRevoluteUnaligned);

  Eigen::VectorXd q = Eigen::VectorXd::Ones (modelRX.nq);
  Eigen::VectorXd v = Eigen::VectorXd::Ones (modelRX.nv);
  Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRX.nv);
  Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones (modelRevoluteUnaligned.nv);
  Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRX.nv);
  Eigen::VectorXd aRevoluteUnaligned(aRX);

  forwardKinematics(modelRX, dataRX, q, v);
  forwardKinematics(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);

  computeAllTerms(modelRX, dataRX, q, v);
  computeAllTerms(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);

  BOOST_CHECK(dataRevoluteUnaligned.oMi[1].isApprox(dataRX.oMi[1]));
  BOOST_CHECK(dataRevoluteUnaligned.liMi[1].isApprox(dataRX.liMi[1]));
  BOOST_CHECK(dataRevoluteUnaligned.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix()));
  BOOST_CHECK(dataRevoluteUnaligned.f[1].toVector().isApprox(dataRX.f[1].toVector()));

  BOOST_CHECK(dataRevoluteUnaligned.nle.isApprox(dataRX.nle));
  BOOST_CHECK(dataRevoluteUnaligned.com[0].isApprox(dataRX.com[0]));

  // InverseDynamics == rnea
  tauRX = rnea(modelRX, dataRX, q, v, aRX);
  tauRevoluteUnaligned = rnea(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v, aRevoluteUnaligned);

  BOOST_CHECK(tauRX.isApprox(tauRevoluteUnaligned));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaRX = aba(modelRX,dataRX, q, v, tauRX);
  Eigen::VectorXd aAbaRevoluteUnaligned = aba(modelRevoluteUnaligned,dataRevoluteUnaligned, q, v, tauRevoluteUnaligned);

  BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnaligned));

  // CRBA
  crba(modelRX, dataRX,q);
  crba(modelRevoluteUnaligned, dataRevoluteUnaligned, q);

  BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnaligned.M));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRX;jacobianRX.resize(6,1); jacobianRX.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRevoluteUnaligned;jacobianRevoluteUnaligned.resize(6,1);jacobianRevoluteUnaligned.setZero();
  computeJointJacobians(modelRX, dataRX, q);
  computeJointJacobians(modelRevoluteUnaligned, dataRevoluteUnaligned, q);
  getJointJacobian(modelRX, dataRX, 1, LOCAL, jacobianRX);
  getJointJacobian(modelRevoluteUnaligned, dataRevoluteUnaligned, 1, LOCAL, jacobianRevoluteUnaligned);

  BOOST_CHECK(jacobianRX.isApprox(jacobianRevoluteUnaligned));
}
BOOST_AUTO_TEST_SUITE_END ()

BOOST_AUTO_TEST_SUITE (JointPrismaticUnaligned)

  BOOST_AUTO_TEST_CASE(spatial)
  {
    SE3 M(SE3::Random());
    Motion v(Motion::Random());

    MotionPrismaticUnaligned mp(MotionPrismaticUnaligned::Vector3(1.,2.,3.),6.);
    Motion mp_dense(mp);

    BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
    BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

    BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
  }

BOOST_AUTO_TEST_CASE (vsPX)
{
  using namespace pinocchio;
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Eigen::Vector3d axis;
  axis << 1.0, 0.0, 0.0;

  Model modelPX, modelPrismaticUnaligned;

  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  JointModelPrismaticUnaligned joint_model_PU(axis);

  addJointAndBody(modelPX,JointModelPX(),0,pos,"px",inertia);
  addJointAndBody(modelPrismaticUnaligned,joint_model_PU,0,pos,"prismatic-unaligned",inertia);

  Data dataPX(modelPX);
  Data dataPrismaticUnaligned(modelPrismaticUnaligned);

  Eigen::VectorXd q = Eigen::VectorXd::Ones (modelPX.nq);
  Eigen::VectorXd v = Eigen::VectorXd::Ones (modelPX.nv);
  Eigen::VectorXd tauPX = Eigen::VectorXd::Ones (modelPX.nv);
  Eigen::VectorXd tauPrismaticUnaligned = Eigen::VectorXd::Ones (modelPrismaticUnaligned.nv);
  Eigen::VectorXd aPX = Eigen::VectorXd::Ones (modelPX.nv);
  Eigen::VectorXd aPrismaticUnaligned(aPX);

  forwardKinematics(modelPX, dataPX, q, v);
  forwardKinematics(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);

  computeAllTerms(modelPX, dataPX, q, v);
  computeAllTerms(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);

  BOOST_CHECK(dataPrismaticUnaligned.oMi[1].isApprox(dataPX.oMi[1]));
  BOOST_CHECK(dataPrismaticUnaligned.liMi[1].isApprox(dataPX.liMi[1]));
  BOOST_CHECK(dataPrismaticUnaligned.Ycrb[1].matrix().isApprox(dataPX.Ycrb[1].matrix()));
  BOOST_CHECK(dataPrismaticUnaligned.f[1].toVector().isApprox(dataPX.f[1].toVector()));

  BOOST_CHECK(dataPrismaticUnaligned.nle.isApprox(dataPX.nle));
  BOOST_CHECK(dataPrismaticUnaligned.com[0].isApprox(dataPX.com[0]));

  // InverseDynamics == rnea
  tauPX = rnea(modelPX, dataPX, q, v, aPX);
  tauPrismaticUnaligned = rnea(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v, aPrismaticUnaligned);

  BOOST_CHECK(tauPX.isApprox(tauPrismaticUnaligned));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaPX = aba(modelPX,dataPX, q, v, tauPX);
  Eigen::VectorXd aAbaPrismaticUnaligned = aba(modelPrismaticUnaligned,dataPrismaticUnaligned, q, v, tauPrismaticUnaligned);

  BOOST_CHECK(aAbaPX.isApprox(aAbaPrismaticUnaligned));

  // crba
  crba(modelPX, dataPX,q);
  crba(modelPrismaticUnaligned, dataPrismaticUnaligned, q);

  BOOST_CHECK(dataPX.M.isApprox(dataPrismaticUnaligned.M));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;jacobianPX.resize(6,1); jacobianPX.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
  computeJointJacobians(modelPX, dataPX, q);
  computeJointJacobians(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
  getJointJacobian(modelPX, dataPX, 1, LOCAL, jacobianPX);
  getJointJacobian(modelPrismaticUnaligned, dataPrismaticUnaligned, 1, LOCAL, jacobianPrismaticUnaligned);

  BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
}
BOOST_AUTO_TEST_SUITE_END ()

BOOST_AUTO_TEST_SUITE (JointSpherical)

  BOOST_AUTO_TEST_CASE(spatial)
  {
    SE3 M(SE3::Random());
    Motion v(Motion::Random());

    MotionSpherical mp(MotionSpherical::Vector3(1.,2.,3.));
    Motion mp_dense(mp);

    BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
    BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

    BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
  }

BOOST_AUTO_TEST_CASE (vsFreeFlyer)
{
  using namespace pinocchio;
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Model modelSpherical, modelFreeflyer;

  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelSpherical,JointModelSpherical(),0,pos,"spherical",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,pos,"free-flyer",inertia);

  Data dataSpherical(modelSpherical);
  Data dataFreeFlyer(modelFreeflyer);

  Eigen::VectorXd q = Eigen::VectorXd::Ones (modelSpherical.nq);q.normalize();
  VectorFF qff; qff << 0, 0, 0, q[0], q[1], q[2], q[3];
  Eigen::VectorXd v = Eigen::VectorXd::Ones (modelSpherical.nv);
  Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
  Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones (modelSpherical.nv);
  Eigen::VectorXd tauff; tauff.resize(7); tauff << 0,0,0,1,1,1,1;
  Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones (modelSpherical.nv);
  Eigen::VectorXd aff(vff);

  forwardKinematics(modelSpherical, dataSpherical, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelSpherical, dataSpherical, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataSpherical.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataSpherical.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSpherical.Ycrb[1].matrix()));
  BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataSpherical.f[1].toVector()));

  Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[3],
                                                         dataFreeFlyer.nle[4],
                                                         dataFreeFlyer.nle[5]
                                                         ;
  BOOST_CHECK(nle_expected_ff.isApprox(dataSpherical.nle));
  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSpherical.com[0]));

  // InverseDynamics == rnea
  tauSpherical = rnea(modelSpherical, dataSpherical, q, v, aSpherical);
  tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);

  Vector3 tau_expected; tau_expected << tauff(3), tauff(4), tauff(5);
  BOOST_CHECK(tauSpherical.isApprox(tau_expected));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaSpherical = aba(modelSpherical,dataSpherical, q, v, tauSpherical);
  Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
  Vector3 a_expected; a_expected << aAbaFreeFlyer[3],
                                    aAbaFreeFlyer[4],
                                    aAbaFreeFlyer[5]
                                    ;
  BOOST_CHECK(aAbaSpherical.isApprox(a_expected));

  // crba
  crba(modelSpherical, dataSpherical,q);
  crba(modelFreeflyer, dataFreeFlyer, qff);

  Eigen::Matrix<double, 3, 3> M_expected(dataFreeFlyer.M.bottomRightCorner<3,3>());

  BOOST_CHECK(dataSpherical.M.isApprox(M_expected));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
  computeJointJacobians(modelSpherical, dataSpherical, q);
  computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
  getJointJacobian(modelSpherical, dataSpherical, 1, LOCAL, jacobian_planar);
  getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);

  Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(3),
                                                                      jacobian_ff.col(4),
                                                                      jacobian_ff.col(5)
                                                                      ;

  BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));

}
BOOST_AUTO_TEST_SUITE_END ()

BOOST_AUTO_TEST_SUITE (JointSphericalZYX)

  BOOST_AUTO_TEST_CASE(spatial)
  {
    SE3 M(SE3::Random());
    Motion v(Motion::Random());

    MotionSpherical mp(MotionSpherical::Vector3(1.,2.,3.));
    Motion mp_dense(mp);

    BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
    BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

    BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
  }

BOOST_AUTO_TEST_CASE (vsFreeFlyer)
{
  // WARNIG : Dynamic algorithm's results cannot be compared to FreeFlyer's ones because
  // of the representation of the rotation and the ConstraintSubspace difference.
  using namespace pinocchio;
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Model modelSphericalZYX, modelFreeflyer;

  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelSphericalZYX,JointModelSphericalZYX(),0,pos,"spherical-zyx",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,pos,"free-flyer",inertia);

  Data dataSphericalZYX(modelSphericalZYX);
  Data dataFreeFlyer(modelFreeflyer);

  Eigen::AngleAxisd rollAngle(1, Eigen::Vector3d::UnitZ());
  Eigen::AngleAxisd yawAngle(1, Eigen::Vector3d::UnitY());
  Eigen::AngleAxisd pitchAngle(1, Eigen::Vector3d::UnitX());
  Eigen::Quaterniond q_sph = rollAngle * yawAngle * pitchAngle;

  Eigen::VectorXd q = Eigen::VectorXd::Ones (modelSphericalZYX.nq);
  VectorFF qff; qff << 0, 0, 0, q_sph.x(), q_sph.y(), q_sph.z(), q_sph.w();
  Eigen::VectorXd v = Eigen::VectorXd::Ones (modelSphericalZYX.nv);
  Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
  Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones (modelSphericalZYX.nv);
  Eigen::VectorXd tauff; tauff.resize(6); tauff << 0,0,0,1,1,1;
  Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones (modelSphericalZYX.nv);
  Eigen::VectorXd aff(vff);

  forwardKinematics(modelSphericalZYX, dataSphericalZYX, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelSphericalZYX, dataSphericalZYX, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataSphericalZYX.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataSphericalZYX.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSphericalZYX.Ycrb[1].matrix()));

  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSphericalZYX.com[0]));
}

BOOST_AUTO_TEST_CASE ( test_rnea )
{
  using namespace pinocchio;
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Model model;
  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());

  addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);

  Data data (model);

  Eigen::VectorXd q = Eigen::VectorXd::Zero (model.nq);
  Eigen::VectorXd v = Eigen::VectorXd::Zero (model.nv);
  Eigen::VectorXd a = Eigen::VectorXd::Zero (model.nv);

  rnea (model, data, q, v, a);
  Vector3 tau_expected (0., -4.905, 0.);

  BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-14));

  q = Eigen::VectorXd::Ones (model.nq);
  v = Eigen::VectorXd::Ones (model.nv);
  a = Eigen::VectorXd::Ones (model.nv);

  rnea (model, data, q, v, a);
  tau_expected << -0.53611600195085, -0.74621832606188, -0.38177329067604;

  BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));

  q << 3, 2, 1;
  v = Eigen::VectorXd::Ones (model.nv);
  a = Eigen::VectorXd::Ones (model.nv);

  rnea (model, data, q, v, a);
  tau_expected << 0.73934458094049,  2.7804530848031, 0.50684940972146;

  BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));
}

BOOST_AUTO_TEST_CASE ( test_crba )
{
  using namespace pinocchio;
  using namespace std;
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Model model;
  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());

  addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);

  Data data (model);

  Eigen::VectorXd q (Eigen::VectorXd::Zero (model.nq));
  Eigen::MatrixXd M_expected (model.nv,model.nv);

  crba (model, data, q);
  M_expected <<
  1.25,    0,    0,
  0, 1.25,    0,
  0,    0,    1;

  BOOST_CHECK (M_expected.isApprox(data.M, 1e-14));

  q = Eigen::VectorXd::Ones (model.nq);

  crba (model, data, q);
  M_expected <<
  1.0729816454316, -5.5511151231258e-17,     -0.8414709848079,
  -5.5511151231258e-17,                 1.25,                    0,
  -0.8414709848079,                    0,                    1;

  BOOST_CHECK (M_expected.isApprox(data.M, 1e-12));

  q << 3, 2, 1;

  crba (model, data, q);
  M_expected <<
  1.043294547392, 2.7755575615629e-17,   -0.90929742682568,
  0,                1.25,                   0,
  -0.90929742682568,                   0,                  1;

  BOOST_CHECK (M_expected.isApprox(data.M, 1e-10));
}

BOOST_AUTO_TEST_SUITE_END ()

BOOST_AUTO_TEST_SUITE ( JointPrismatic )

BOOST_AUTO_TEST_CASE(spatial)
{
  typedef TransformPrismaticTpl<double,0,0> TransformX;
  typedef TransformPrismaticTpl<double,0,1> TransformY;
  typedef TransformPrismaticTpl<double,0,2> TransformZ;

  typedef SE3::Vector3 Vector3;

  const double displacement = 0.2;
  SE3 Mplain, Mrand(SE3::Random());

  TransformX Mx(displacement);
  Mplain = Mx;
  BOOST_CHECK(Mplain.translation().isApprox(Vector3(displacement,0,0)));
  BOOST_CHECK(Mplain.rotation().isIdentity());
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mx));

  TransformY My(displacement);
  Mplain = My;
  BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,displacement,0)));
  BOOST_CHECK(Mplain.rotation().isIdentity());
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*My));

  TransformZ Mz(displacement);
  Mplain = Mz;
  BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,0,displacement)));
  BOOST_CHECK(Mplain.rotation().isIdentity());
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mz));

  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionPrismaticTpl<double,0,0> mp_x(2.);
  Motion mp_dense_x(mp_x);

  BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
  BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));

  BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));

  MotionPrismaticTpl<double,0,1> mp_y(2.);
  Motion mp_dense_y(mp_y);

  BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
  BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));

  BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));

  MotionPrismaticTpl<double,0,2> mp_z(2.);
  Motion mp_dense_z(mp_z);

  BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
  BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));

  BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
}

BOOST_AUTO_TEST_CASE ( test_kinematics )
{
  using namespace pinocchio;

  Motion expected_v_J (Motion::Zero ());
  Motion expected_c_J (Motion::Zero ());

  SE3 expected_configuration (SE3::Identity ());

  JointDataPX joint_data;
  JointModelPX joint_model;

  joint_model.setIndexes (0, 0, 0);

  Eigen::VectorXd q (Eigen::VectorXd::Zero (1));
  Eigen::VectorXd q_dot (Eigen::VectorXd::Zero (1));

  // -------
  q << 0. ;
  q_dot << 0.;

  joint_model.calc (joint_data, q, q_dot);

  printOutJointData <JointDataPX> (q, q_dot, joint_data);

  BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
  BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
  BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
  BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));

  // -------
  q << 1.;
  q_dot << 1.;

  joint_model.calc (joint_data, q, q_dot);

  printOutJointData <JointDataPX> (q, q_dot, joint_data);

  expected_configuration.translation () << 1, 0, 0;

  expected_v_J.linear () << 1., 0., 0.;

  BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
  BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
  BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
  BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
}

BOOST_AUTO_TEST_CASE ( test_rnea )
{
  using namespace pinocchio;
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Model model;
  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());

  addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);

  Data data (model);

  Eigen::VectorXd q (Eigen::VectorXd::Zero (model.nq));
  Eigen::VectorXd v (Eigen::VectorXd::Zero (model.nv));
  Eigen::VectorXd a (Eigen::VectorXd::Zero (model.nv));

  rnea (model, data, q, v, a);

  Eigen::VectorXd tau_expected (Eigen::VectorXd::Zero (model.nq));
  tau_expected  << 0;

  BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-14));

  // -----
  q = Eigen::VectorXd::Ones (model.nq);
  v = Eigen::VectorXd::Ones (model.nv);
  a = Eigen::VectorXd::Ones (model.nv);

  rnea (model, data, q, v, a);
  tau_expected << 1;

  BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));

  q << 3;
  v = Eigen::VectorXd::Ones (model.nv);
  a = Eigen::VectorXd::Ones (model.nv);

  rnea (model, data, q, v, a);
  tau_expected << 1;

  BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));
}

BOOST_AUTO_TEST_CASE ( test_crba )
{
  using namespace pinocchio;
  using namespace std;
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Model model;
  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());

  addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);

  Data data (model);

  Eigen::VectorXd q (Eigen::VectorXd::Zero (model.nq));
  Eigen::MatrixXd M_expected (model.nv,model.nv);

  crba (model, data, q);
  M_expected << 1.0;

  BOOST_CHECK (M_expected.isApprox(data.M, 1e-14));

  q = Eigen::VectorXd::Ones (model.nq);

  crba (model, data, q);

  BOOST_CHECK (M_expected.isApprox(data.M, 1e-12));

  q << 3;

  crba (model, data, q);

  BOOST_CHECK (M_expected.isApprox(data.M, 1e-10));
}

BOOST_AUTO_TEST_SUITE_END ()

BOOST_AUTO_TEST_SUITE (JointPlanar)

BOOST_AUTO_TEST_CASE(spatial)
{
  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionPlanar mp(1.,2.,3.);
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_CASE (vsFreeFlyer)
{
  using namespace pinocchio;
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 4, 1> VectorPl;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Model modelPlanar, modelFreeflyer;

  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelPlanar,JointModelPlanar(),0,SE3::Identity(),"planar",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,SE3::Identity(),"free-flyer",inertia);

  Data dataPlanar(modelPlanar);
  Data dataFreeFlyer(modelFreeflyer);

  VectorPl q; q << 1, 1, 0, 1; // Angle is PI /2;
  VectorFF qff; qff << 1, 1, 0, 0, 0, sqrt(2)/2, sqrt(2)/2 ;
  Eigen::VectorXd v = Eigen::VectorXd::Ones (modelPlanar.nv);
  Vector6 vff; vff << 1, 1, 0, 0, 0, 1;
  Eigen::VectorXd tauPlanar = Eigen::VectorXd::Ones (modelPlanar.nv);
  Eigen::VectorXd tauff = Eigen::VectorXd::Ones (modelFreeflyer.nv);
  Eigen::VectorXd aPlanar = Eigen::VectorXd::Ones (modelPlanar.nv);
  Eigen::VectorXd aff(vff);

  forwardKinematics(modelPlanar, dataPlanar, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelPlanar, dataPlanar, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataPlanar.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataPlanar.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataPlanar.Ycrb[1].matrix()));
  BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataPlanar.f[1].toVector()));

  Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[0],
                                                         dataFreeFlyer.nle[1],
                                                         dataFreeFlyer.nle[5]
                                                         ;
  BOOST_CHECK(nle_expected_ff.isApprox(dataPlanar.nle));
  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataPlanar.com[0]));

  // InverseDynamics == rnea
  tauPlanar = rnea(modelPlanar, dataPlanar, q, v, aPlanar);
  tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);

  Vector3 tau_expected; tau_expected << tauff(0), tauff(1), tauff(5);
  BOOST_CHECK(tauPlanar.isApprox(tau_expected));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaPlanar = aba(modelPlanar,dataPlanar, q, v, tauPlanar);
  Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
  Vector3 a_expected; a_expected << aAbaFreeFlyer[0],
                                    aAbaFreeFlyer[1],
                                    aAbaFreeFlyer[5]
                                    ;
  BOOST_CHECK(aAbaPlanar.isApprox(a_expected));

  // crba
  crba(modelPlanar, dataPlanar,q);
  crba(modelFreeflyer, dataFreeFlyer, qff);

  Eigen::Matrix<double, 3, 3> M_expected;
  M_expected.block<2,2>(0,0) = dataFreeFlyer.M.block<2,2>(0,0);
  M_expected.block<1,2>(2,0) = dataFreeFlyer.M.block<1,2>(5,0);
  M_expected.block<2,1>(0,2) = dataFreeFlyer.M.col(5).head<2>();
  M_expected.block<1,1>(2,2) = dataFreeFlyer.M.col(5).tail<1>();

  BOOST_CHECK(dataPlanar.M.isApprox(M_expected));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
  computeJointJacobians(modelPlanar, dataPlanar, q);
  computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
  getJointJacobian(modelPlanar, dataPlanar, 1, LOCAL, jacobian_planar);
  getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);

  Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
                                                                      jacobian_ff.col(1),
                                                                      jacobian_ff.col(5)
                                                                      ;

  BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
}
BOOST_AUTO_TEST_SUITE_END ()

BOOST_AUTO_TEST_SUITE (JointTranslation)

BOOST_AUTO_TEST_CASE(spatial)
{
  typedef TransformTranslationTpl<double,0> TransformTranslation;
  typedef SE3::Vector3 Vector3;

  const Vector3 displacement(Vector3::Random());
  SE3 Mplain, Mrand(SE3::Random());

  TransformTranslation Mtrans(displacement);
  Mplain = Mtrans;
  BOOST_CHECK(Mplain.translation().isApprox(displacement));
  BOOST_CHECK(Mplain.rotation().isIdentity());
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mtrans));

  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionTranslation mp(MotionTranslation::Vector3(1.,2.,3.));
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_CASE (vsFreeFlyer)
{
  using namespace pinocchio;
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Model modelTranslation, modelFreeflyer;

  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelTranslation,JointModelTranslation(),0,SE3::Identity(),"translation",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,SE3::Identity(),"free-flyer",inertia);

  Data dataTranslation(modelTranslation);
  Data dataFreeFlyer(modelFreeflyer);

  Eigen::VectorXd q = Eigen::VectorXd::Ones (modelTranslation.nq);               VectorFF qff; qff << 1, 1, 1, 0, 0, 0, 1 ;
  Eigen::VectorXd v = Eigen::VectorXd::Ones (modelTranslation.nv);               Vector6 vff; vff << 1, 1, 1, 0, 0, 0;
  Eigen::VectorXd tauTranslation = Eigen::VectorXd::Ones (modelTranslation.nv);       Eigen::VectorXd tauff(6); tauff << 1, 1, 1, 0, 0, 0;
  Eigen::VectorXd aTranslation = Eigen::VectorXd::Ones (modelTranslation.nv);         Eigen::VectorXd aff(vff);

  forwardKinematics(modelTranslation, dataTranslation, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelTranslation, dataTranslation, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataTranslation.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataTranslation.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataTranslation.Ycrb[1].matrix()));
  BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataTranslation.f[1].toVector()));

  Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[0],
                                                         dataFreeFlyer.nle[1],
                                                         dataFreeFlyer.nle[2]
                                                         ;
  BOOST_CHECK(nle_expected_ff.isApprox(dataTranslation.nle));
  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataTranslation.com[0]));

  // InverseDynamics == rnea
  tauTranslation = rnea(modelTranslation, dataTranslation, q, v, aTranslation);
  tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);

  Vector3 tau_expected; tau_expected << tauff(0), tauff(1), tauff(2);
  BOOST_CHECK(tauTranslation.isApprox(tau_expected));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaTranslation = aba(modelTranslation,dataTranslation, q, v, tauTranslation);
  Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
  Vector3 a_expected; a_expected << aAbaFreeFlyer[0],
                                    aAbaFreeFlyer[1],
                                    aAbaFreeFlyer[2]
                                    ;
  BOOST_CHECK(aAbaTranslation.isApprox(a_expected));

  // crba
  crba(modelTranslation, dataTranslation,q);
  crba(modelFreeflyer, dataFreeFlyer, qff);

  Eigen::Matrix<double, 3, 3> M_expected(dataFreeFlyer.M.topLeftCorner<3,3>());

  BOOST_CHECK(dataTranslation.M.isApprox(M_expected));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
  computeJointJacobians(modelTranslation, dataTranslation, q);
  computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
  getJointJacobian(modelTranslation, dataTranslation, 1, LOCAL, jacobian_planar);
  getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);

  Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
                                                                      jacobian_ff.col(1),
                                                                      jacobian_ff.col(2)
                                                                      ;

  BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
}
BOOST_AUTO_TEST_SUITE_END ()

BOOST_AUTO_TEST_SUITE (JointRevoluteUnbounded)

  BOOST_AUTO_TEST_CASE(spatial)
  {
    SE3 M(SE3::Random());
    Motion v(Motion::Random());

    MotionRevoluteTpl<double,0,0> mp_x(2.);
    Motion mp_dense_x(mp_x);

    BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
    BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));

    BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));

    MotionRevoluteTpl<double,0,1> mp_y(2.);
    Motion mp_dense_y(mp_y);

    BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
    BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));

    BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));

    MotionRevoluteTpl<double,0,2> mp_z(2.);
    Motion mp_dense_z(mp_z);

    BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
    BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));

    BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
  }

BOOST_AUTO_TEST_CASE (vsRX)
{
  typedef Eigen::Matrix <double, 3, 1> Vector3;
  typedef Eigen::Matrix <double, 3, 3> Matrix3;

  Model modelRX, modelRevoluteUnbounded;

  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  JointModelRUBX joint_model_RUX;
  addJointAndBody(modelRX,JointModelRX(),0,SE3::Identity(),"rx",inertia);
  addJointAndBody(modelRevoluteUnbounded,joint_model_RUX,0,SE3::Identity(),"revolute unbounded x",inertia);

  Data dataRX(modelRX);
  Data dataRevoluteUnbounded(modelRevoluteUnbounded);

  Eigen::VectorXd q_rx = Eigen::VectorXd::Ones (modelRX.nq);
  Eigen::VectorXd q_rubx = Eigen::VectorXd::Ones (modelRevoluteUnbounded.nq);
  double ca, sa; double alpha = q_rx(0); SINCOS (alpha, &sa, &ca);
  q_rubx(0) = ca;
  q_rubx(1) = sa;
  Eigen::VectorXd v_rx = Eigen::VectorXd::Ones (modelRX.nv);
  Eigen::VectorXd v_rubx = v_rx;
  Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRX.nv);       Eigen::VectorXd tauRevoluteUnbounded = Eigen::VectorXd::Ones (modelRevoluteUnbounded.nv);
  Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRX.nv);         Eigen::VectorXd aRevoluteUnbounded = aRX;

  forwardKinematics(modelRX, dataRX, q_rx, v_rx);
  forwardKinematics(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx);

  computeAllTerms(modelRX, dataRX, q_rx, v_rx);
  computeAllTerms(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx);

  BOOST_CHECK(dataRevoluteUnbounded.oMi[1].isApprox(dataRX.oMi[1]));
  BOOST_CHECK(dataRevoluteUnbounded.liMi[1].isApprox(dataRX.liMi[1]));
  BOOST_CHECK(dataRevoluteUnbounded.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix()));
  BOOST_CHECK(dataRevoluteUnbounded.f[1].toVector().isApprox(dataRX.f[1].toVector()));

  BOOST_CHECK(dataRevoluteUnbounded.nle.isApprox(dataRX.nle));
  BOOST_CHECK(dataRevoluteUnbounded.com[0].isApprox(dataRX.com[0]));

  // InverseDynamics == rnea
  tauRX = rnea(modelRX, dataRX, q_rx, v_rx, aRX);
  tauRevoluteUnbounded = rnea(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx, aRevoluteUnbounded);

  BOOST_CHECK(tauRX.isApprox(tauRevoluteUnbounded));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaRX= aba(modelRX,dataRX, q_rx, v_rx, tauRX);
  Eigen::VectorXd aAbaRevoluteUnbounded = aba(modelRevoluteUnbounded,dataRevoluteUnbounded, q_rubx, v_rubx, tauRevoluteUnbounded);

  BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnbounded));

  // crba
  crba(modelRX, dataRX,q_rx);
  crba(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);

  BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnbounded.M));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;jacobianPX.resize(6,1); jacobianPX.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
  computeJointJacobians(modelRX, dataRX, q_rx);
  computeJointJacobians(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);
  getJointJacobian(modelRX, dataRX, 1, LOCAL, jacobianPX);
  getJointJacobian(modelRevoluteUnbounded, dataRevoluteUnbounded, 1, LOCAL, jacobianPrismaticUnaligned);

  BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));

}
BOOST_AUTO_TEST_SUITE_END ()

BOOST_AUTO_TEST_SUITE(JointRevolute)

  BOOST_AUTO_TEST_CASE(spatial)
  {
    typedef TransformRevoluteTpl<double,0,0> TransformX;
    typedef TransformRevoluteTpl<double,0,1> TransformY;
    typedef TransformRevoluteTpl<double,0,2> TransformZ;

    typedef SE3::Vector3 Vector3;

    const double alpha = 0.2;
    double sin_alpha, cos_alpha; SINCOS(alpha,&sin_alpha,&cos_alpha);
    SE3 Mplain, Mrand(SE3::Random());

    TransformX Mx(sin_alpha,cos_alpha);
    Mplain = Mx;
    BOOST_CHECK(Mplain.translation().isZero());
    BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitX()).toRotationMatrix()));
    BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mx));

    TransformY My(sin_alpha,cos_alpha);
    Mplain = My;
    BOOST_CHECK(Mplain.translation().isZero());
    BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitY()).toRotationMatrix()));
    BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*My));

    TransformZ Mz(sin_alpha,cos_alpha);
    Mplain = Mz;
    BOOST_CHECK(Mplain.translation().isZero());
    BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitZ()).toRotationMatrix()));
    BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mz));

    SE3 M(SE3::Random());
    Motion v(Motion::Random());

    MotionRevoluteTpl<double,0,0> mp_x(2.);
    Motion mp_dense_x(mp_x);

    BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
    BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));

    BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));

    MotionRevoluteTpl<double,0,1> mp_y(2.);
    Motion mp_dense_y(mp_y);

    BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
    BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));

    BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));

    MotionRevoluteTpl<double,0,2> mp_z(2.);
    Motion mp_dense_z(mp_z);

    BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
    BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));

    BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
  }

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned)

  BOOST_AUTO_TEST_CASE(spatial)
  {
    SE3 M(SE3::Random());
    Motion v(Motion::Random());

    MotionRevoluteUnaligned mp(MotionRevoluteUnaligned::Vector3(1.,2.,3.),6.);
    Motion mp_dense(mp);

    BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
    BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

    BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
  }

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(JointModelBase_test)

  struct TestJointModelIsEqual
  {
    template<typename JointModel>
    void operator()(const pinocchio::JointModelBase<JointModel> &) const
    {
      JointModel jmodel;
      jmodel.setIndexes(0,0,0);

      test(jmodel);
    }

    template<typename Scalar, int Options>
    void operator()(const JointModelRevoluteUnalignedTpl<Scalar,Options> & ) const
    {
      typedef JointModelRevoluteUnalignedTpl<Scalar,Options> JointModelRevoluteUnaligned;
      typedef typename JointModelRevoluteUnaligned::Vector3 Vector3;
      JointModelRevoluteUnaligned jmodel(Vector3::Random().normalized());
      jmodel.setIndexes(0,0,0);

      test(jmodel);
    }

    template<typename Scalar, int Options>
    void operator()(const JointModelPrismaticUnalignedTpl<Scalar,Options> & ) const
    {
      typedef JointModelPrismaticUnalignedTpl<Scalar,Options> JointModelPrismaticUnaligned;
      typedef typename JointModelPrismaticUnaligned::Vector3 Vector3;
      JointModelPrismaticUnaligned jmodel(Vector3::Random().normalized());
      jmodel.setIndexes(0,0,0);

      test(jmodel);
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollection>
    void operator()(const JointModelTpl<Scalar,Options,JointCollection> & ) const
    {
      typedef JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
      typedef JointModelTpl<Scalar,Options,JointCollection> JointModel;
      JointModel jmodel((JointModelRX()));
      jmodel.setIndexes(0,0,0);

      test(jmodel);
    }

    template<typename JointModel>
    static void test(const JointModelBase<JointModel> & jmodel)
    {
      JointModel jmodel_copy = jmodel.derived();
      BOOST_CHECK(jmodel_copy == jmodel.derived());

      JointModel jmodel_any;
      BOOST_CHECK(jmodel_any != jmodel.derived());
      BOOST_CHECK(!jmodel_any.isEqual(jmodel.derived()));
    }
  };

  BOOST_AUTO_TEST_CASE(isEqual)
  {
    typedef JointCollectionDefault::JointModelVariant JointModelVariant;
    boost::mpl::for_each<JointModelVariant::types>(TestJointModelIsEqual());

    JointModelRX joint_revolutex;
    JointModelRY joint_revolutey;

    BOOST_CHECK(joint_revolutex != joint_revolutey);

    JointModel jmodelx(joint_revolutex);
    jmodelx.setIndexes(0,0,0);
    TestJointModelIsEqual()(JointModel());

    JointModel jmodel_any;
    BOOST_CHECK(jmodel_any != jmodelx);
  }

  struct TestJointModelCast
  {
    template<typename JointModel>
    void operator()(const pinocchio::JointModelBase<JointModel> &) const
    {
      JointModel jmodel;
      jmodel.setIndexes(0,0,0);

      test(jmodel);
    }

    template<typename Scalar, int Options>
    void operator()(const JointModelRevoluteUnalignedTpl<Scalar,Options> & ) const
    {
      typedef JointModelRevoluteUnalignedTpl<Scalar,Options> JointModelRevoluteUnaligned;
      typedef typename JointModelRevoluteUnaligned::Vector3 Vector3;
      JointModelRevoluteUnaligned jmodel(Vector3::Random().normalized());
      jmodel.setIndexes(0,0,0);

      test(jmodel);
    }

    template<typename Scalar, int Options>
    void operator()(const JointModelPrismaticUnalignedTpl<Scalar,Options> & ) const
    {
      typedef JointModelPrismaticUnalignedTpl<Scalar,Options> JointModelPrismaticUnaligned;
      typedef typename JointModelPrismaticUnaligned::Vector3 Vector3;
      JointModelPrismaticUnaligned jmodel(Vector3::Random().normalized());
      jmodel.setIndexes(0,0,0);

      test(jmodel);
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollection>
    void operator()(const JointModelTpl<Scalar,Options,JointCollection> & ) const
    {
      typedef JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
      typedef JointModelTpl<Scalar,Options,JointCollection> JointModel;
      JointModel jmodel((JointModelRX()));
      jmodel.setIndexes(0,0,0);

      test(jmodel);
    }

    template<typename JointModel>
    static void test(const JointModelBase<JointModel> & jmodel)
    {
      typedef typename JointModel::Scalar Scalar;
      BOOST_CHECK(jmodel.template cast<Scalar>() == jmodel);
      BOOST_CHECK(jmodel.template cast<long double>().template cast<double>() == jmodel);
    }
  };

  BOOST_AUTO_TEST_CASE(cast)
  {
    typedef JointCollectionDefault::JointModelVariant JointModelVariant;
    boost::mpl::for_each<JointModelVariant::types>(TestJointModelCast());

    TestJointModelCast()(JointModel());
  }

BOOST_AUTO_TEST_SUITE_END()


#include <algorithm>
#include <cmath>
#include <iostream>
#include <stdlib.h>
#include <string>
#include <tuple>
#include <vector>
#include <limits>
#include <algorithm>
#include <boost/numeric/odeint.hpp>

#include "ode.h"
#include <ebit-ode-messages.pb.h>

using namespace std;

typedef std::vector<double> state_type;

typedef ::google::protobuf::RepeatedPtrField<::EbitODEMessages::MatrixValue>
matrix;
typedef ::google::protobuf::RepeatedPtrField<::EbitODEMessages::InitialValue>
initValue;
typedef ::google::protobuf::RepeatedPtrField<::EbitODEMessages::Nuclide>
nuclides;

typedef ::google::protobuf::RepeatedField<double> times;

double *create_matrix(int dimension, const matrix &sparse) {
    double *result = new double[dimension * dimension];

    for (int i = 0; i < 2 * dimension; ++i) result[i] = 0.0;
    for (auto ptr = sparse.begin(); ptr < sparse.end(); ++ptr) {
	// to keep it in sync with julia, decf 1 on array offsets
	auto i = ptr->row() - 1;
	auto j = ptr->column() - 1;
	result[dimension * i + j] += ptr->value();
	// std::cout << "R_{" << i << "," << j << "}: " << ptr->value() << std::endl;
    }
    // std::cout << endl;

    return result;
}

state_type &create_init_values(int dimension, const initValue &init_values) {
    auto result =
	new state_type(2 * dimension);
    for (auto ptr = init_values.begin(); ptr < init_values.end(); ++ptr) {

	auto i = ptr->index() - 1;
	(*result)[i] = ptr->number_of_particles();
	(*result)[dimension + i] = ptr->temperature_in_ev();
    }
    return *result;
}

class ebit_ode {
    const double *qV_e;
    const double *qV_t;
    const double *A;
    const double *phi;
    const double *qVe_over_Vol_x_kT;
    const double *source;

    const double *Xi_ij;
    const double *eta_ij;
    const double *CX_ij;

    unsigned int no_dimensions;
    double min_N;

public:
    ~ebit_ode() {}
    ebit_ode(const EbitODEMessages::DiffEqParameters &p) {
	no_dimensions = p.no_dimensions();
	qV_e = p.qve().data();
	qV_t = p.qvt().data();
	A = p.mass_number().data();
	phi = p.spitzer_divided_by_overlap().data();
	qVe_over_Vol_x_kT = p.qve_over_vol_x_kt().data();
	source = p.source_terms().data();
	min_N = p.minimum_n();
	Xi_ij = create_matrix(no_dimensions, p.inverted_collision_constant());
	eta_ij = create_matrix(no_dimensions, p.rate_of_change_divided_by_n());
	CX_ij = create_matrix(no_dimensions, p.dcharge_ex_divided_by_n_times_tau());
    }

    void operator()(const state_type &x, state_type &dxdt, const double) {
	auto tau = [&](int i) { return std::max(0.0, x[no_dimensions + i]); };
	auto N = [&](int i) { return std::max(0.0, x[i]); };

	auto dN = [&](int i, double incf) { dxdt[i] += incf; };
	auto dtau = [&](int i, double incf) { dxdt[i + no_dimensions] += incf; };

	auto set_dN = [&](int i, double set_point) { dxdt[i] = set_point; };
	auto set_dtau = [&](int i, double set_point) { dxdt[i + no_dimensions] = set_point; };

	auto CX = [&](int i, int j) { return CX_ij[no_dimensions*i + j]; };
	auto eta = [&](int i, int j) { return eta_ij[no_dimensions*i + j]; };
	auto Xi = [&](int i, int j) { return Xi_ij[no_dimensions*i + j]; };

	for (int i = 0; i < no_dimensions; ++i) {
	    double R_esc_sum_j = 0.0;
	    double R_exchange_sum_j = 0.0;

	    set_dN(i, source[i]);
	    set_dtau(i, 0.0);

	    for (int j = 0; j < no_dimensions; ++j) {

		if (N(i) > min_N && N(j) > min_N && tau(j) > 0.0 && tau(i) > 0.0) {
		    double f_ij = std::min((tau(i) * qV_e[j]) / (tau(j) * qV_e[i]), 1.0);
		    double n_j = N(j) * qVe_over_Vol_x_kT[j] / tau(j);
		    double arg = (tau(i) / A[i] + tau(j) / A[j]);
		    double Sigma = Xi(i,j) * n_j * pow(arg, -1.5);
		    R_esc_sum_j += f_ij * Sigma;
		    R_exchange_sum_j += f_ij * Sigma * (tau(j) - tau(i));

		    dN(i,  CX(i,j) * N(j) * sqrt(tau(j)));
		}

		dN(i, eta(i,j) * N(j));
	    }

	    dtau(i, R_exchange_sum_j);
	    if (N(i) > min_N) {
		double R_esc = 3 / sqrt(3) * R_esc_sum_j * (tau(i) / qV_t[i]) *
		    exp(-qV_t[i] / tau(i));
		dtau(i, (std::min(qV_e[i] / tau(i), 1.0) * phi[i] ) - (tau(i) + qV_t[i]) * R_esc);
		dN(i, -N(i) * R_esc);
	    }
	}
    }
};

void publish(const nuclides& m_nuclides, const state_type &x, double time){
    std::cout << "time: " << time << " ";
    for (auto n = m_nuclides.begin(); n < m_nuclides.end(); ++n) {
	std::cout << ", N(A=" << n->a() << ",Z=" << n->z() << ",q=" << n->q() << "+): "
		  << x[n->i() - 1];
    }
    std::cout << endl;
}

class write_state {
    const nuclides& m_nuclides;
public:
    write_state(const nuclides& nuclides) : m_nuclides(nuclides) {}

    void operator()(const state_type &x, double time) const {
	publish(m_nuclides, x, time);
    }
};

EbitODEMessages::Result *prepare_result(const nuclides &nuclides) {
    auto result = new EbitODEMessages::Result();

    for (auto p = nuclides.begin(); p < nuclides.end(); ++p) {
	auto n = result->add_n();
	auto kt = result->add_kt();

	n->set_allocated_nuclide(new EbitODEMessages::Nuclide(*p));
	kt->set_allocated_nuclide(new EbitODEMessages::Nuclide(*p));
    }
    return result;
}

bool save_state(const state_type &x, EbitODEMessages::Result &result,
                const nuclides &nuclides, int no_dimensions, double last_time,
		double only_if_larger = 0.0) {
    if (last_time < only_if_larger)
	return false;

    result.add_times(last_time);
    for (auto n = nuclides.begin(); n < nuclides.end(); ++n) {
	auto i = n->i() - 1;
	result.mutable_n(i)->add_values(x[i]);
	result.mutable_kt(i)->add_values(x[no_dimensions + i]);
    }

    return true;
}

class push_back_state_and_time {
    EbitODEMessages::Result &m_result;
    const nuclides &m_nuclides;
    int m_no_dimensions;
    int m_last_time_i = 0;
    const times& m_times;
public:
    push_back_state_and_time(EbitODEMessages::Result &result, const nuclides &nuclides,
			     int no_dimensions, const times& times)
	: m_result(result), m_nuclides(nuclides),
	  m_no_dimensions(no_dimensions), m_times(times)
	{}

    void operator()(const state_type &x, double t) {
	// publish(m_nuclides, x, t);
	auto saved = save_state(x, m_result, m_nuclides,
				m_no_dimensions, t,
				m_times.Get(m_last_time_i));
	if (saved && m_last_time_i + 1 < m_times.size())
	    ++m_last_time_i;
    }
};

EbitODEMessages::Result* do_solve(const ebit_ode &ode,
				  const EbitODEMessages::SolverParameters &solver_params,
				  const EbitODEMessages::DiffEqParameters &diff_params,
				  const EbitODEMessages::ProblemParameters& problem_params,
				  const nuclides &nuclides) {
    using namespace boost::numeric::odeint;

    std::cout << "Start solving problem of size: " << diff_params.no_dimensions()
	      << std::endl;

    auto saveat = solver_params.saveat();
    auto x = create_init_values(diff_params.no_dimensions(),
				diff_params.initial_values());

    typedef runge_kutta_fehlberg78<state_type> error_stepper_type;
    typedef controlled_runge_kutta<error_stepper_type> controlled_stepper_type;
    controlled_stepper_type controlled_stepper;

    auto result = prepare_result(nuclides);

    auto ptr = saveat.begin();
    double last_time = *ptr++;

    integrate_adaptive(controlled_stepper, ode, x,
		       problem_params.time_span().start(),
		       problem_params.time_span().stop(), 1e-1
		       //write_state(nuclides)
		       ,push_back_state_and_time(*result, nuclides,
		       				diff_params.no_dimensions(),
		       				solver_params.saveat())
	);

    result->set_return_code(EbitODEMessages::Success);

    return result;
}

void solve_ode(const char *msg_buffer, unsigned int size, char **answer_buffer,
               unsigned int *answer_size) {

    EbitODEMessages::Message msg;
    if (!msg.ParseFromArray(msg_buffer, size))
	throw "Couldn't parse message.";

    auto problem = msg.ode_problem();
    auto diff_params = problem.diff_eq_parameters();
    auto ode = ebit_ode(diff_params);

    auto answer = new EbitODEMessages::Message();

    auto result = do_solve(ode, problem.solver_parameters(), diff_params,
			   problem.problem_parameters(), problem.nuclides());
    result->set_allocated_problem(&problem);
    result->set_start_time(0.0);
    result->set_stop_time(0.0);

    answer->set_msg_type(EbitODEMessages::ODEResult);
    answer->set_allocated_ode_result(result);

    *answer_size = answer->ByteSize();
    *answer_buffer = static_cast<char *>(malloc(*answer_size));

    answer->SerializeToArray(*answer_buffer, *answer_size);
}


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

#include <iostream>
#include <cmath>
#include <complex>

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

using namespace std;

template <typename T>
T inline my_value(std::size_t i, T) { return T(i); }

std::complex<double>
inline my_value(std::size_t i, std::complex<double>)
{ return std::complex<double>(i, i+1); }

template <typename VectorU>
void test(VectorU& u, const char* name)
{
    using std::abs;

    typedef typename mtl::Collection<VectorU>::value_type value_type;
    typedef typename mtl::Collection<VectorU>::size_type  size_type;
    for (size_type i= 0; i < size(u); i++)
	u[i]= my_value(i+1, value_type());

    value_type dot_cmp= dot(u, u);

    bool wrong= abs(unary_dot(u) - dot_cmp) > 0.001,
	 not_two_norm= abs(two_norm(u) * two_norm(u) - abs(unary_dot(u))) > 0.001;
    std::cout << name << ": u = " << u << "\n unary_dot(u) = " << unary_dot(u) << " is "
	      << (wrong ? "not" : "") << "equal with dot(u, u)\n";
    if (wrong)
	throw "unary_dot product wrong";

    std::cout << " two_norm(u) = " << two_norm(u) << ", unary_dot(u) is " << (not_two_norm ? "not" : "") << "equal with square of it\n";
    if (not_two_norm)
	throw "different from square of two_norm";

    std::cout << " unary_dot<2>(u) = " << mtl::unary_dot<2>(u) << "\n"; std::cout.flush();
    if (abs(mtl::unary_dot<2>(u) - dot_cmp) > 0.001)
	throw "unary_dot product wrong";

    std::cout << " unary_dot<6>(u) = " << mtl::unary_dot<6>(u) << "\n"; std::cout.flush();
    if (abs(mtl::unary_dot<6>(u) - dot_cmp) > 0.001)
	throw "unary_dot product wrong";
}

int main(int ,char**)
{
    using mtl::vec::parameters;
    const int size= 9;

    mtl::dense_vector<float>   u(size);
    mtl::dense_vector<double>  x(size);
    mtl::dense_vector<std::complex<double> >  xc(size);

    std::cout << "Testing vector operations\n";

    test(u, "test float");
    test(x, "test double");
    test(xc, "test complex<double>");

    mtl::dense_vector<float, parameters<mtl::row_major> >   ur(size);
    test(ur, "test float in row vector");

    return 0;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "simple_witness_complex"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <gudhi/Simplex_tree.h>

#include <gudhi/Witness_complex.h>

#include <iostream>
#include <vector>
#include <utility>

BOOST_AUTO_TEST_CASE(simple_witness_complex) {
  using Nearest_landmark_range = std::vector<std::pair<std::size_t, double>>;
  using Nearest_landmark_table = std::vector<Nearest_landmark_range>;
  using Witness_complex = Gudhi::witness_complex::Witness_complex<Nearest_landmark_table>;
  using Simplex_tree = Gudhi::Simplex_tree<>;

  Simplex_tree stree;
  Nearest_landmark_table nlt;

  // Example contains 5 witnesses and 5 landmarks
  Nearest_landmark_range w0 = {std::make_pair(0, 0), std::make_pair(1, 1), std::make_pair(2, 2),
                               std::make_pair(3, 3), std::make_pair(4, 4)}; nlt.push_back(w0);
  Nearest_landmark_range w1 = {std::make_pair(1, 0), std::make_pair(2, 1), std::make_pair(3, 2),
                               std::make_pair(4, 3), std::make_pair(0, 4)}; nlt.push_back(w1);
  Nearest_landmark_range w2 = {std::make_pair(2, 0), std::make_pair(3, 1), std::make_pair(4, 2),
                               std::make_pair(0, 3), std::make_pair(1, 4)}; nlt.push_back(w2);
  Nearest_landmark_range w3 = {std::make_pair(3, 0), std::make_pair(4, 1), std::make_pair(0, 2),
                               std::make_pair(1, 3), std::make_pair(2, 4)}; nlt.push_back(w3);
  Nearest_landmark_range w4 = {std::make_pair(4, 0), std::make_pair(0, 1), std::make_pair(1, 2),
                               std::make_pair(2, 3), std::make_pair(3, 4)}; nlt.push_back(w4);

  Witness_complex witness_complex(nlt);
  BOOST_CHECK(witness_complex.create_complex(stree, 4.1));

  std::clog << "Number of simplices: " << stree.num_simplices() << std::endl;
  BOOST_CHECK(stree.num_simplices() == 31);

  // Check when complex not empty
  BOOST_CHECK(!witness_complex.create_complex(stree, 4.1));

  // Check when max_alpha_square negative
  Simplex_tree stree2;
  BOOST_CHECK(!witness_complex.create_complex(stree2, -0.02));

  witness_complex.create_complex(stree2, 4.1, 2);
  std::clog << "Number of simplices: " << stree2.num_simplices() << std::endl;
  BOOST_CHECK(stree2.num_simplices() == 25);

}


#pragma once

#include <vector>
#include <fstream>
#include <array>
#include <Eigen/Dense>

#include "common.hpp"

namespace Discregrid
{

class DiscreteGrid
{
public:

	using CoefficientVector = Eigen::Matrix<Real, 32, 1>;
	using ContinuousFunction = std::function<Real(Vector3r const&)>;
	using MultiIndex = std::array<unsigned int, 3>;
	using Predicate = std::function<bool(Vector3r const&, Real)>;
	using SamplePredicate = std::function<bool(Vector3r const&)>;

	DiscreteGrid() = default;
	DiscreteGrid(AlignedBox3r const& domain, std::array<unsigned int, 3> const& resolution)
		: m_domain(domain), m_resolution(resolution), m_n_fields(0u)
	{
		auto n = Eigen::Matrix<unsigned int, 3, 1>::Map(resolution.data());
		m_cell_size = domain.diagonal().cwiseQuotient(n.cast<Real>());
		m_inv_cell_size = m_cell_size.cwiseInverse();
		m_n_cells = n.prod();
	}
	virtual ~DiscreteGrid() = default;

	virtual void save(std::string const& filename) const = 0;
	virtual void load(std::string const& filename) = 0;

	virtual unsigned int addFunction(ContinuousFunction const& func, bool verbose = false,
		SamplePredicate const& pred = nullptr) = 0;

	Real interpolate(Vector3r const& xi, Vector3r* gradient = nullptr) const
	{
		return interpolate(0u, xi, gradient);
	}

	virtual Real interpolate(unsigned int field_id, Vector3r const& xi,
		Vector3r* gradient = nullptr) const = 0;

	/**
	 * @brief Determines the shape functions for the discretization with ID field_id at point xi.
	 *
	 * @param field_id Discretization ID
	 * @param x Location where the shape functions should be determined
	 * @param cell cell of x
	 * @param c0 vector required for the interpolation
	 * @param N	shape functions for the cell of x
	 * @param dN (Optional) derivatives of the shape functions, required to compute the gradient
	 * @return Success of the function.
	 */
	virtual bool determineShapeFunctions(unsigned int field_id, Vector3r const &x,
		std::array<unsigned int, 32> &cell, Vector3r &c0, Eigen::Matrix<Real, 32, 1> &N,
		Eigen::Matrix<Real, 32, 3> *dN = nullptr) const = 0;

	/**
	 * @brief Evaluates the given discretization with ID field_id at point xi.
	 *
	 * @param field_id Discretization ID
	 * @param xi Location where the discrete function is evaluated
	 * @param cell cell of xi
	 * @param c0 vector required for the interpolation
	 * @param N	shape functions for the cell of xi
	 * @param gradient (Optional) if a pointer to a vector is passed the gradient of the discrete function will be evaluated
	 * @param dN (Optional) derivatives of the shape functions, required to compute the gradient
	 * @return Real Results of the evaluation of the discrete function at point xi
	 */
	virtual Real interpolate(unsigned int field_id, Vector3r const& xi, const std::array<unsigned int, 32> &cell, const Vector3r &c0, const Eigen::Matrix<Real, 32, 1> &N,
		Vector3r* gradient = nullptr, Eigen::Matrix<Real, 32, 3> *dN = nullptr) const = 0;

	virtual void reduceField(unsigned int field_id, Predicate pred) {}

	MultiIndex singleToMultiIndex(unsigned int i) const;
	unsigned int multiToSingleIndex(MultiIndex const& ijk) const;

	AlignedBox3r subdomain(MultiIndex const& ijk) const;
	AlignedBox3r subdomain(unsigned int l) const;

	AlignedBox3r const& domain() const { return m_domain; }
	std::array<unsigned int, 3> const& resolution() const { return m_resolution; };
	Vector3r const& cellSize() const { return m_cell_size;}
	Vector3r const& invCellSize() const { return m_inv_cell_size;}

protected:

	AlignedBox3r m_domain;
	std::array<unsigned int, 3> m_resolution;
	Vector3r m_cell_size;
	Vector3r m_inv_cell_size;
	std::size_t m_n_cells;
	std::size_t m_n_fields;
};
}


/*
 * padding.hpp
 *
 *  Created on: Apr 8, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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.
 */

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace math{

/**
 * Pad tensor by repeating its border values.
 * Same border behavior as mode='edge' for NumPy or BORDER_REPLICATE for OpenCV
 * NB: For simple padding with zeros, use the Tensor::pad method provided by Eigen.
 * @param tensor input tensor
 * @return padded tensor
 */
template<typename Scalar>
Eigen::Tensor<Scalar,3,Eigen::ColMajor> pad_replicate(const Eigen::Tensor<Scalar,3,Eigen::ColMajor>& tensor, int border_width=1);

//TODO: provide Eigen::Matrix overload (if there is no Eigen equivalent)

} //end namespace math


// 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 <boost/numeric/mtl/mtl.hpp>

using namespace std;
using mtl::generate_mask; using mtl::row_major; using mtl::col_major;

template <typename Matrix>
void print_matrix(Matrix& matrix)
{
    using std::cout;
    typedef typename mtl::Collection<Matrix>::size_type size_type;
    for (size_type i= 0 ; i < num_rows(matrix); i++ ){
	for(size_type j=0; j < num_cols(matrix);  j++ ){
	    cout.fill (' '); cout.width (8); cout.precision (5); cout.flags (ios_base::left);
	    cout << showpoint <<  matrix[i][j] <<"  ";
	}
	cout << endl;
    }
}

template <typename Matrix>
void test(Matrix& matrix, const char* name)
{
    namespace with_bracket = mtl::mat::with_bracket;
    // namespace with_iterator = mtl::mat::with_iterator;

    using mtl::mat::recursive_cholesky_visitor_t;
    using mtl::mat::detail::mult_schur_update_t;

    std::cout << "Test " << name << "\n-----\n\n";
    fill_matrix_for_cholesky(matrix);

    recursive_cholesky(matrix);
    if (matrix.num_cols() <= 10) {
	print_matrix(matrix); std::cout << "\n";
    }

    fill_matrix_for_cholesky(matrix);

#if 0
    with_iterator::recursive_cholesky_base_visitor_t  iter_vis;
    recursive_cholesky(matrix, iter_vis);
    if (matrix.num_cols() <= 10) {
	print_matrix(matrix); std::cout << "\n";
    }

    fill_matrix_for_cholesky(matrix);
#endif

    recursive_cholesky_visitor_t<mtl::recursion::bound_test_static<2>, with_bracket::cholesky_base_t, with_bracket::tri_solve_base_t,
                                 with_bracket::tri_schur_base_t, with_bracket::schur_update_base_t>
        iter_vis2;
    recursive_cholesky(matrix, iter_vis2);
    if (matrix.num_cols() <= 10) {
	print_matrix(matrix); std::cout << "\n";
    }

    fill_matrix_for_cholesky(matrix);

#if 0 // ITERATOR VERSION CURRENTLY NOT SUPPORTED -- CAUSES SEGFAULT, e.g. with icc 11.0 in r8536 !!!!

    recursive_cholesky_visitor_t<mtl::recursion::bound_test_static<2>, with_iterator::cholesky_base_t, with_iterator::tri_solve_base_t,
                                 with_iterator::tri_schur_base_t, with_iterator::schur_update_base_t>
        iter_vis3;

    recursive_cholesky(matrix, iter_vis3);
    if (matrix.num_cols() <= 10) {
	print_matrix(matrix); std::cout << "\n";
    }

    fill_matrix_for_cholesky(matrix);

    typedef mult_schur_update_t<mtl::gen_tiling_22_dmat_dmat_mult_t<mtl::assign::minus_sum> > schur_update_22_t;
    recursive_cholesky_visitor_t<mtl::recursion::bound_test_static<2>, with_iterator::cholesky_base_t, with_iterator::tri_solve_base_t,
                                 with_iterator::tri_schur_base_t, schur_update_22_t>
        iter_vis4;

    recursive_cholesky(matrix, iter_vis4);
    if (matrix.num_cols() <= 10) {
	print_matrix(matrix); std::cout << "\n";
    }

    typedef detail::mult_schur_update_t<gen_tiling_44_dmat_dmat_mult_t<minus_mult_assign_t> > schur_update_44_t;

#endif
}

int main(int argc, char* argv[])
{
    mtl::vampir_trace<9999> tracer;

    using namespace mtl;
    unsigned size= 13;
    if (argc > 1) size= atoi(argv[1]);

    dense2D<double>                                dr(size, size);
    dense2D<double, mat::parameters<col_major> > dc(size, size);
    morton_dense<double,  morton_mask>             md(size, size);
    morton_dense<double,  morton_z_mask>           mzd(size, size);
    morton_dense<double,  doppled_2_row_mask>      d2r(size, size);
    morton_dense<double,  doppled_2_col_mask>      d2c(size, size);
    morton_dense<double,  doppled_16_row_mask>     d16r(size, size);
    morton_dense<double,  doppled_32_row_mask>     d32r(size, size);
    morton_dense<double,  doppled_64_row_mask>     d64r(size, size);
    morton_dense<double,  doppled_64_col_mask>     d64c(size, size);
    morton_dense<double,  doppled_128_col_mask>    d128r(size, size);
    size= 9;
    dense2D<double>                                dr2(size, size);

    test(dr2, "Dense row major");
    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(md, "Morton N-order");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(d2c, "Hybrid 2 column-major");
    test(d16r, "Hybrid 16 row-major");

    return 0;
}


#include "BaseSpectrumCode.h"
#include "SlabSpectrumCode.h"
#include "numeric_tools.h"

#include <boost/math/special_functions/expint.hpp>
#include <iostream>

using namespace Eigen;
using namespace Numerics;
using namespace PrintFuncs;
using namespace boost::math;

std::pair<Tensor3d, Tensor4d> SlabSpectrumCode::calcTracks()
{
    Tensor4d tau = Tensor4d(m_cells, m_cells, m_rays, m_energies);
	tau.setZero();

    Tensor3d zz(m_cells, m_cells, m_rays);
    zz.setZero();

	std::vector<double> delta(m_radii.size() - 1, 0.0);

	for(size_t i = 0; i < delta.size(); i++)
	{
		delta[i] = m_radii[i + 1] - m_radii[i];
	}

	for(int h = 0; h < m_energies; h++)
		for(int i = 0; i < m_cells; i++)
          for(int j = i; j < m_cells; j++)
	      {
	          if (i == j)
                  tau(i, j, 0, h) = delta[i] * m_totalXS(h, i);
              else
              {
                  if (i == 0)
					  tau(i, j, 0, h) = delta[j] * m_totalXS(h, j) + tau(0, j - 1, 0, h);
                  else
                      tau(i, j, 0, h) = delta[j] * m_totalXS(h, j)
                      + tau(0, j - 1, 0, h) - tau(0, i - 1, 0, h);
              }
		  }

	for(int h = 0; h < m_energies; h++)
		for(int i = 0; i < m_cells; i++)
          for(int j = i; j < m_cells; j++)
	      {
	        if (i != j) tau(i, j, 0, h) -= delta[j] * m_totalXS(h, j) +  delta[i] * m_totalXS(h, i);
			if (tau(i, j, 0, h) < 0.0) tau(i, j, 0, h) = 0.0;

			out.print(TraceLevel::TRACE, "Tau: {:3d} {:3d} {:3d} {:5.4f}", h, i, j, tau(i, j,  0, h));
		  }

    std::pair<Tensor3d, Tensor4d> trackData = std::make_pair(zz, tau);
	return trackData;
}

Tensor3d SlabSpectrumCode::calcCPs(std::pair<Tensor3d, Tensor4d> &trackData)
{
	Tensor4d tau = trackData.second;

	Tensor3d gcpm = Tensor3d(m_cells, m_cells, m_energies);
	gcpm.setZero();

	Tensor3d CSet1(m_cells, m_cells, m_cells);
	Tensor3d CSet2(m_cells, m_cells, m_cells);

	MatrixXd P = MatrixXd::Zero(m_cells, m_cells);

    for(int h = 0; h < m_energies; h++)
	{
	  for(int i = 0; i < m_cells; i++)
        for(int j = i; j < m_cells; j++)
		{
          if (j == i)
            P(i, j) = 1.0 - (1.0 / (2.0 * tau(i, i, 0, h))) * (1.0 - 2.0 * expint(3, tau(i, i, 0, h)));
          else
            P(i, j) = (1.0 / (2.0 * tau(i, i, 0, h))) * (expint(3, tau(i, j, 0, h))
			          - expint(3, tau(i, j, 0, h) + tau(i, i, 0, h))
					  - expint(3, tau(i, j, 0, h) + tau(j, j, 0, h))
					  + expint(3, tau(i, j, 0, h) + tau(j, j, 0, h) + tau(i, i, 0, h)));
		}

	for(int i = 0; i < m_cells; i++)
	  for(int j = i + 1; j < m_cells; j++)
          P(j, i) = P(i, j) * (m_totalXS(h, i) / m_totalXS(h, j)) * (m_volumes(i) / m_volumes(j));

	for(int i = 0; i < m_cells; i++)
	  for(int j = 0; j < m_cells; j++)
		gcpm(i, j, h) = P(i, j);
	}

	out.print(TraceLevel::INFO, "P matrix (vacuum BC)");
    printMatrix(gcpm, out, TraceLevel::INFO, "Group");
	return gcpm;
}

void SlabSpectrumCode::applyBoundaryConditions(Tensor3d &gcpm)
{
	for(int h = 0; h < m_energies; h++)
	{
		out.print(TraceLevel::INFO, "Apply boundary conditions Group {}", h + 1);

		double albedo = m_solverData.getAlbedo()[0];

		VectorXd Pis   = VectorXd::Zero(m_cells);
	    VectorXd psi   = VectorXd::Zero(m_cells);

		Pis.setZero();
		psi.setZero();

		for(int i = 0; i < m_cells; i++)
		{
			for(int j = 0; j < m_cells; j++)
			{
				gcpm(i, j, h) /= m_totalXS(h, j);  //reduced
			}
		}

		for(int i = 0; i < m_cells; i++)
		{
			for(int j = 0; j < m_cells; j++)
			{
				Pis(i) += gcpm(i, j, h) * m_totalXS(h, j);
			}

			Pis(i) = 1.0 - Pis(i);
			psi(i) = (4.0 * m_volumes(i) / m_surface) * Pis(i);

			out.print(TraceLevel::INFO, "Cell n: {} Pis: {:7.6e}  psi [cm]: {:7.6e}", i, Pis(i), psi(i));
		}

		double Pss = 0.0;

		for(int i = 0; i < m_cells; i++)
        {
			Pss += psi(i) * m_totalXS(h, i);
		}

		Pss = 1.0 - Pss;

		out.print(TraceLevel::INFO, "Pss [cm2]: {:7.6e} \n", Pss);

		for(int i = 0; i < m_cells; i++)
		{
			for(int j = 0; j < m_cells; j++)
			{
				gcpm(i, j, h) += Pis(i) * psi(j) * (albedo / (1.0 - Pss * albedo));
				gcpm(i, j, h) *= m_totalXS(h, j);
			}
		}

	}

	out.print(TraceLevel::INFO, "P matrix (white BC)");
    printMatrix(gcpm, out, TraceLevel::INFO, "Group");

    for(int h = 0; h < m_energies; h++)
	{
	    for(int i = 0; i < m_cells; i++)
		{
			for(int j = 0; j < m_cells; j++)
			{
				gcpm(i, j, h) /= m_totalXS(h, j);
			}
		}
	}
}


/**********************************************************************
*  Copyright (c) 2008-2014, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include <gtest/gtest.h>
#include "GeometryFixture.hpp"

#include "../Transformation.hpp"
#include "../Point3d.hpp"
#include "../Vector3d.hpp"
#include "../EulerAngles.hpp"

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

using namespace std;
using namespace boost;
using namespace openstudio;

TEST_F(GeometryFixture, RotationTransformations)
{
  double tol = 1.0E-12;
  Vector3d z(0,0,1);
  Point3d point1(1,0,0);
  Transformation T;
  Point3d temp;

  // identity transformation
  temp = T*point1;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(0.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // rotation 30 degrees about z
  T = Transformation::rotation(z, degToRad(30));
  temp = T*point1;
  EXPECT_NEAR(cos(degToRad(30)), temp.x(), tol);
  EXPECT_NEAR(sin(degToRad(30)), temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // rotation -30 degrees about z
  temp = T.inverse()*point1;
  EXPECT_NEAR(cos(degToRad(30)), temp.x(), tol);
  EXPECT_NEAR(-sin(degToRad(30)), temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // rotation 0 degrees about z
  temp = T.inverse()*T*point1;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(0.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // rotation 0 degrees about z
  temp = T*T.inverse()*point1;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(0.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // rotation 90 degrees about z
  T = Transformation::rotation(z, degToRad(90));
  temp = T*point1;
  EXPECT_NEAR(0.0, temp.x(), tol);
  EXPECT_NEAR(1.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);
}

TEST_F(GeometryFixture, RotationAboutPointTransformations)
{
  double tol = 1.0E-6;
  Vector3d z(0,0,1);
  Point3d point1(1,0,0);
  Transformation T;
  Point3d temp;

  // rotation around origin, 30 degrees about z
  T = Transformation::rotation(Point3d(0,0,0), z, degToRad(30));
  temp = T*point1;
  EXPECT_NEAR(0.866025, temp.x(), tol);
  EXPECT_NEAR(0.5, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // rotation around 1*x, 30 degrees about z
  T = Transformation::rotation(Point3d(1,0,0), z, degToRad(30));
  temp = T*point1;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(0.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // rotation around 1*y, 30 degrees about z
  T = Transformation::rotation(Point3d(0,1,0), z, degToRad(30));
  temp = T*point1;
  EXPECT_NEAR(1.366025, temp.x(), tol);
  EXPECT_NEAR(0.633975, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // rotation around 1*z, 30 degrees about z
  T = Transformation::rotation(Point3d(0,0,1), z, degToRad(30));
  temp = T*point1;
  EXPECT_NEAR(0.866025, temp.x(), tol);
  EXPECT_NEAR(0.5, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // rotation around 1*x + 1*y + 1*z, 30 degrees about z
  T = Transformation::rotation(Point3d(1,1,1), z, degToRad(30));
  temp = T*point1;
  EXPECT_NEAR(1.5, temp.x(), tol);
  EXPECT_NEAR(0.133975, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);
}

TEST_F(GeometryFixture, TranslationTransformations)
{
  double tol = 1.0E-12;
  Vector3d trans(1,1,1);
  Point3d point1(1,0,0);
  Transformation T;
  Point3d temp;

  // identity transformation
  temp = T*point1;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(0.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // move by 1, 1, 1
  T = Transformation::translation(trans);
  temp = T*point1;
  EXPECT_NEAR(2.0, temp.x(), tol);
  EXPECT_NEAR(1.0, temp.y(), tol);
  EXPECT_NEAR(1.0, temp.z(), tol);

  // move by -1, -1, -1
  temp = T.inverse()*point1;
  EXPECT_NEAR(0.0, temp.x(), tol);
  EXPECT_NEAR(-1.0, temp.y(), tol);
  EXPECT_NEAR(-1.0, temp.z(), tol);

  // identity transformation
  temp = T.inverse()*T*point1;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(0.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  // identity transformation
  temp = T*T.inverse()*point1;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(0.0, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);
}

TEST_F(GeometryFixture, AlignZPrimeTransformations)
{
  Vector3d xAxis(1,0,0);
  Vector3d yAxis(0,1,0);
  Vector3d zAxis(0,0,1);
  Transformation T;

  Vector3d outwardNormal(0, -1, 0);
  T = Transformation::alignZPrime(outwardNormal);
  EXPECT_TRUE(xAxis == T*xAxis);
  EXPECT_TRUE(zAxis == T*yAxis);
  EXPECT_TRUE(outwardNormal == T*zAxis);

  outwardNormal = Vector3d(1, 0, 0);
  T = Transformation::alignZPrime(outwardNormal);
  EXPECT_TRUE(yAxis == T*xAxis);
  EXPECT_TRUE(zAxis == T*yAxis);
  EXPECT_TRUE(outwardNormal == T*zAxis);

  outwardNormal = Vector3d(0, 1, 0);
  T = Transformation::alignZPrime(outwardNormal);
  EXPECT_TRUE(-xAxis == T*xAxis);
  EXPECT_TRUE(zAxis == T*yAxis);
  EXPECT_TRUE(outwardNormal == T*zAxis);

  outwardNormal = Vector3d(-1, 0, 0);
  T = Transformation::alignZPrime(outwardNormal);
  EXPECT_TRUE(-yAxis == T*xAxis);
  EXPECT_TRUE(zAxis == T*yAxis);
  EXPECT_TRUE(outwardNormal == T*zAxis);

  outwardNormal = Vector3d(0, 0, 1);
  T = Transformation::alignZPrime(outwardNormal);
  EXPECT_TRUE(-xAxis == T*xAxis);
  EXPECT_TRUE(-yAxis == T*yAxis);
  EXPECT_TRUE(outwardNormal == T*zAxis);

  outwardNormal = Vector3d(0, 0, -1);
  T = Transformation::alignZPrime(outwardNormal);
  EXPECT_TRUE(-xAxis == T*xAxis);
  EXPECT_TRUE(yAxis == T*yAxis);
  EXPECT_TRUE(outwardNormal == T*zAxis);
}

TEST_F(GeometryFixture, AlignFaceTransformations)
{
  double tol = 1.0E-12;

  Point3dVector vertices(4);
  vertices[0] = Point3d(1, 0, 1);
  vertices[1] = Point3d(1, 0, 0);
  vertices[2] = Point3d(2, 0, 0);
  vertices[3] = Point3d(2, 0, 1);
  Point3dVector testVertices;
  Point3dVector tempVertices;
  Transformation T;

  // rotate 0 degrees about z
  testVertices = Transformation::rotation(Vector3d(0,0,1), 0)*vertices;
  T = Transformation::alignFace(testVertices);
  tempVertices = T.inverse()*testVertices;
  EXPECT_NEAR(0, tempVertices[0].x(), tol);
  EXPECT_NEAR(1, tempVertices[0].y(), tol);
  EXPECT_NEAR(0, tempVertices[0].z(), tol);
  EXPECT_NEAR(0, tempVertices[1].x(), tol);
  EXPECT_NEAR(0, tempVertices[1].y(), tol);
  EXPECT_NEAR(0, tempVertices[1].z(), tol);
  EXPECT_NEAR(1, tempVertices[2].x(), tol);
  EXPECT_NEAR(0, tempVertices[2].y(), tol);
  EXPECT_NEAR(0, tempVertices[2].z(), tol);
  EXPECT_NEAR(1, tempVertices[3].x(), tol);
  EXPECT_NEAR(1, tempVertices[3].y(), tol);
  EXPECT_NEAR(0, tempVertices[3].z(), tol);

  // rotate 30 degrees about z
  testVertices = Transformation::rotation(Vector3d(0,0,1), degToRad(30))*vertices;
  T = Transformation::alignFace(testVertices);
  tempVertices = T.inverse()*testVertices;
  EXPECT_NEAR(0, tempVertices[0].x(), tol);
  EXPECT_NEAR(1, tempVertices[0].y(), tol);
  EXPECT_NEAR(0, tempVertices[0].z(), tol);
  EXPECT_NEAR(0, tempVertices[1].x(), tol);
  EXPECT_NEAR(0, tempVertices[1].y(), tol);
  EXPECT_NEAR(0, tempVertices[1].z(), tol);
  EXPECT_NEAR(1, tempVertices[2].x(), tol);
  EXPECT_NEAR(0, tempVertices[2].y(), tol);
  EXPECT_NEAR(0, tempVertices[2].z(), tol);
  EXPECT_NEAR(1, tempVertices[3].x(), tol);
  EXPECT_NEAR(1, tempVertices[3].y(), tol);
  EXPECT_NEAR(0, tempVertices[3].z(), tol);

  // rotate -30 degrees about z
  testVertices = Transformation::rotation(Vector3d(0,0,1), -degToRad(30))*vertices;
  T = Transformation::alignFace(testVertices);
  tempVertices = T.inverse()*testVertices;
  EXPECT_NEAR(0, tempVertices[0].x(), tol);
  EXPECT_NEAR(1, tempVertices[0].y(), tol);
  EXPECT_NEAR(0, tempVertices[0].z(), tol);
  EXPECT_NEAR(0, tempVertices[1].x(), tol);
  EXPECT_NEAR(0, tempVertices[1].y(), tol);
  EXPECT_NEAR(0, tempVertices[1].z(), tol);
  EXPECT_NEAR(1, tempVertices[2].x(), tol);
  EXPECT_NEAR(0, tempVertices[2].y(), tol);
  EXPECT_NEAR(0, tempVertices[2].z(), tol);
  EXPECT_NEAR(1, tempVertices[3].x(), tol);
  EXPECT_NEAR(1, tempVertices[3].y(), tol);
  EXPECT_NEAR(0, tempVertices[3].z(), tol);

  // rotate -30 degrees about x
  testVertices = Transformation::rotation(Vector3d(1,0,0), -degToRad(30))*vertices;
  T = Transformation::alignFace(testVertices);
  tempVertices = T.inverse()*testVertices;
  EXPECT_NEAR(0, tempVertices[0].x(), tol);
  EXPECT_NEAR(1, tempVertices[0].y(), tol);
  EXPECT_NEAR(0, tempVertices[0].z(), tol);
  EXPECT_NEAR(0, tempVertices[1].x(), tol);
  EXPECT_NEAR(0, tempVertices[1].y(), tol);
  EXPECT_NEAR(0, tempVertices[1].z(), tol);
  EXPECT_NEAR(1, tempVertices[2].x(), tol);
  EXPECT_NEAR(0, tempVertices[2].y(), tol);
  EXPECT_NEAR(0, tempVertices[2].z(), tol);
  EXPECT_NEAR(1, tempVertices[3].x(), tol);
  EXPECT_NEAR(1, tempVertices[3].y(), tol);
  EXPECT_NEAR(0, tempVertices[3].z(), tol);

  // rotate -90 degrees about x
  testVertices = Transformation::rotation(Vector3d(1,0,0), -degToRad(90))*vertices;
  T = Transformation::alignFace(testVertices);
  tempVertices = T.inverse()*testVertices;
  EXPECT_NEAR(1, tempVertices[0].x(), tol);
  EXPECT_NEAR(0, tempVertices[0].y(), tol);
  EXPECT_NEAR(0, tempVertices[0].z(), tol);
  EXPECT_NEAR(1, tempVertices[1].x(), tol);
  EXPECT_NEAR(1, tempVertices[1].y(), tol);
  EXPECT_NEAR(0, tempVertices[1].z(), tol);
  EXPECT_NEAR(0, tempVertices[2].x(), tol);
  EXPECT_NEAR(1, tempVertices[2].y(), tol);
  EXPECT_NEAR(0, tempVertices[2].z(), tol);
  EXPECT_NEAR(0, tempVertices[3].x(), tol);
  EXPECT_NEAR(0, tempVertices[3].y(), tol);
  EXPECT_NEAR(0, tempVertices[3].z(), tol);

  // rotate 30 degrees about x
  testVertices = Transformation::rotation(Vector3d(1,0,0), degToRad(30))*vertices;
  T = Transformation::alignFace(testVertices);
  tempVertices = T.inverse()*testVertices;
  EXPECT_NEAR(0, tempVertices[0].x(), tol);
  EXPECT_NEAR(1, tempVertices[0].y(), tol);
  EXPECT_NEAR(0, tempVertices[0].z(), tol);
  EXPECT_NEAR(0, tempVertices[1].x(), tol);
  EXPECT_NEAR(0, tempVertices[1].y(), tol);
  EXPECT_NEAR(0, tempVertices[1].z(), tol);
  EXPECT_NEAR(1, tempVertices[2].x(), tol);
  EXPECT_NEAR(0, tempVertices[2].y(), tol);
  EXPECT_NEAR(0, tempVertices[2].z(), tol);
  EXPECT_NEAR(1, tempVertices[3].x(), tol);
  EXPECT_NEAR(1, tempVertices[3].y(), tol);
  EXPECT_NEAR(0, tempVertices[3].z(), tol);

  // rotate 90 degrees about x
  testVertices = Transformation::rotation(Vector3d(1,0,0), degToRad(90))*vertices;
  T = Transformation::alignFace(testVertices);
  tempVertices = T.inverse()*testVertices;
  EXPECT_NEAR(1, tempVertices[0].x(), tol);
  EXPECT_NEAR(0, tempVertices[0].y(), tol);
  EXPECT_NEAR(0, tempVertices[0].z(), tol);
  EXPECT_NEAR(1, tempVertices[1].x(), tol);
  EXPECT_NEAR(1, tempVertices[1].y(), tol);
  EXPECT_NEAR(0, tempVertices[1].z(), tol);
  EXPECT_NEAR(0, tempVertices[2].x(), tol);
  EXPECT_NEAR(1, tempVertices[2].y(), tol);
  EXPECT_NEAR(0, tempVertices[2].z(), tol);
  EXPECT_NEAR(0, tempVertices[3].x(), tol);
  EXPECT_NEAR(0, tempVertices[3].y(), tol);
  EXPECT_NEAR(0, tempVertices[3].z(), tol);
}

TEST_F(GeometryFixture, AlignFaceTransformations_TrapezoidFloor)
{
  double tol = 1.0E-12;

  Point3dVector vertices(4);
  vertices[0] = Point3d(27.69, 0, 0);
  vertices[1] = Point3d(0, 0, 0);
  vertices[2] = Point3d(5, 5, 0);
  vertices[3] = Point3d(22.69, 5, 0);
  Point3dVector tempVertices;
  Transformation T;

  T = Transformation::alignFace(vertices);
  tempVertices = T.inverse()*vertices;
  EXPECT_NEAR(0, tempVertices[0].x(), tol);
  EXPECT_NEAR(0, tempVertices[0].y(), tol);
  EXPECT_NEAR(0, tempVertices[0].z(), tol);
  EXPECT_NEAR(27.69, tempVertices[1].x(), tol);
  EXPECT_NEAR(0, tempVertices[1].y(), tol);
  EXPECT_NEAR(0, tempVertices[1].z(), tol);
  EXPECT_NEAR(22.69, tempVertices[2].x(), tol);
  EXPECT_NEAR(5, tempVertices[2].y(), tol);
  EXPECT_NEAR(0, tempVertices[2].z(), tol);
  EXPECT_NEAR(5, tempVertices[3].x(), tol);
  EXPECT_NEAR(5, tempVertices[3].y(), tol);
  EXPECT_NEAR(0, tempVertices[3].z(), tol);

}

TEST_F(GeometryFixture, Transformations)
{

  /* SketchUp implementation of this test:
      origin = Geom::Point3d.new(0,0,0)
      x_axis = Geom::Vector3d.new(0,0,1)
      y_axis = Geom::Vector3d.new(0,0,1)
      z_axis = Geom::Vector3d.new(0,0,1)
      trans = Geom::Vector3d.new(1,1,1)

      point = Geom::Point3d.new(1,0,0)

      temp = Geom::Transformation.rotation(origin, z_axis, 30.degrees)*point
      puts temp

      temp = Geom::Transformation.translation(trans )*temp
      puts temp

      temp = Geom::Transformation.rotation(origin, x_axis, 30.degrees)*temp
      puts temp

      temp = Geom::Transformation.translation(trans )*temp
      puts temp

      temp = Geom::Transformation.rotation(origin, y_axis, 30.degrees)*temp
      puts temp

      temp = Geom::Transformation.translation(trans )*temp
      puts temp
  */

  double tol = 1.0E-6;
  Vector3d xAxis(0,0,1);
  Vector3d yAxis(0,0,1);
  Vector3d zAxis(0,0,1);
  Vector3d trans(1,1,1);
  Point3d point(1,0,0);
  Point3d temp;

  temp = Transformation::rotation(zAxis, degToRad(30))*point;
  EXPECT_NEAR(0.866025, temp.x(), tol);
  EXPECT_NEAR(0.5, temp.y(), tol);
  EXPECT_NEAR(0.0, temp.z(), tol);

  temp = Transformation::translation(trans)*temp;
  EXPECT_NEAR(1.866025, temp.x(), tol);
  EXPECT_NEAR(1.5, temp.y(), tol);
  EXPECT_NEAR(1.0, temp.z(), tol);

  temp = Transformation::rotation(xAxis, degToRad(30))*temp;
  EXPECT_NEAR(0.866025, temp.x(), tol);
  EXPECT_NEAR(2.232051, temp.y(), tol);
  EXPECT_NEAR(1.0, temp.z(), tol);

  temp = Transformation::translation(trans)*temp;
  EXPECT_NEAR(1.866025, temp.x(), tol);
  EXPECT_NEAR(3.232051, temp.y(), tol);
  EXPECT_NEAR(2.0, temp.z(), tol);

  temp = Transformation::rotation(yAxis, degToRad(30))*temp;
  EXPECT_NEAR(0.0, temp.x(), tol);
  EXPECT_NEAR(3.732051, temp.y(), tol);
  EXPECT_NEAR(2.0, temp.z(), tol);

  temp = Transformation::translation(trans)*temp;
  EXPECT_NEAR(1.0, temp.x(), tol);
  EXPECT_NEAR(4.732051, temp.y(), tol);
  EXPECT_NEAR(3.0, temp.z(), tol);
}

TEST_F(GeometryFixture, EulerAngles)
{
  Transformation transformation;
  EulerAngles angles = transformation.eulerAngles();
  EXPECT_EQ(0.0, angles.psi());
  EXPECT_EQ(0.0, angles.theta());
  EXPECT_EQ(0.0, angles.phi());

  angles = EulerAngles(0,0,0);
  transformation = Transformation::rotation(angles);
  Matrix rotationMatrix = transformation.rotationMatrix();
  ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size1());
  ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size2());
  EXPECT_EQ(1.0, rotationMatrix(0,0));
  EXPECT_EQ(0.0, rotationMatrix(0,1));
  EXPECT_EQ(0.0, rotationMatrix(0,2));
  EXPECT_EQ(0.0, rotationMatrix(1,0));
  EXPECT_EQ(1.0, rotationMatrix(1,1));
  EXPECT_EQ(0.0, rotationMatrix(1,2));
  EXPECT_EQ(0.0, rotationMatrix(2,0));
  EXPECT_EQ(0.0, rotationMatrix(2,1));
  EXPECT_EQ(1.0, rotationMatrix(2,2));

  transformation = Transformation::translation(Vector3d(1,1,1));
  angles = transformation.eulerAngles();
  EXPECT_EQ(0.0, angles.psi());
  EXPECT_EQ(0.0, angles.theta());
  EXPECT_EQ(0.0, angles.phi());

  transformation = Transformation::rotation(Vector3d(1,0,0), 1.0);
  angles = transformation.eulerAngles();
  EXPECT_EQ(1.0, angles.psi());
  EXPECT_EQ(0.0, angles.theta());
  EXPECT_EQ(0.0, angles.phi());

  angles = EulerAngles(1,0,0);
  transformation = Transformation::rotation(angles);
  rotationMatrix = transformation.rotationMatrix();
  ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size1());
  ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size2());
  EXPECT_NEAR(1.0, rotationMatrix(0,0), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(0,1), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(0,2), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(1,0), 0.0001);
  EXPECT_NEAR(cos(1.0), rotationMatrix(1,1), 0.0001);
  EXPECT_NEAR(-sin(1.0), rotationMatrix(1,2), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(2,0), 0.0001);
  EXPECT_NEAR(sin(1.0), rotationMatrix(2,1), 0.0001);
  EXPECT_NEAR(cos(1.0), rotationMatrix(2,2), 0.0001);

  transformation = Transformation::rotation(Vector3d(0,1,0), 1.0);
  angles = transformation.eulerAngles();
  EXPECT_EQ(0.0, angles.psi());
  EXPECT_EQ(1.0, angles.theta());
  EXPECT_EQ(0.0, angles.phi());

  angles = EulerAngles(0,1,0);
  transformation = Transformation::rotation(angles);
  rotationMatrix = transformation.rotationMatrix();
  ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size1());
  ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size2());
  EXPECT_NEAR(cos(1.0), rotationMatrix(0,0), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(0,1), 0.0001);
  EXPECT_NEAR(sin(1.0), rotationMatrix(0,2), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(1,0), 0.0001);
  EXPECT_NEAR(1.0, rotationMatrix(1,1), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(1,2), 0.0001);
  EXPECT_NEAR(-sin(1.0), rotationMatrix(2,0), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(2,1), 0.0001);
  EXPECT_NEAR(cos(1.0), rotationMatrix(2,2), 0.0001);

  transformation = Transformation::rotation(Vector3d(0,0,1), 1.0);
  angles = transformation.eulerAngles();
  EXPECT_EQ(0.0, angles.psi());
  EXPECT_EQ(0.0, angles.theta());
  EXPECT_EQ(1.0, angles.phi());

  angles = EulerAngles(0,0,1);
  transformation = Transformation::rotation(angles);
  rotationMatrix = transformation.rotationMatrix();
  ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size1());
  ASSERT_EQ(static_cast<unsigned>(3), rotationMatrix.size2());
  EXPECT_NEAR(cos(1.0), rotationMatrix(0,0), 0.0001);
  EXPECT_NEAR(-sin(1.0), rotationMatrix(0,1), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(0,2), 0.0001);
  EXPECT_NEAR(sin(1.0), rotationMatrix(1,0), 0.0001);
  EXPECT_NEAR(cos(1.0), rotationMatrix(1,1), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(1,2), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(2,0), 0.0001);
  EXPECT_NEAR(0.0, rotationMatrix(2,1), 0.0001);
  EXPECT_NEAR(1.0, rotationMatrix(2,2), 0.0001);

  Matrix matrix(4,4);
  matrix(0,0) = 0.5;
  matrix(0,1) = -0.1464;
  matrix(0,2) = 0.8536;
  matrix(1,0) = 0.5;
  matrix(1,1) = 0.8536;
  matrix(1,2) = -0.1464;
  matrix(2,0) = -0.7071;
  matrix(2,1) = 0.5;
  matrix(2,2) = 0.5;
  matrix(3,3) = 1.0;

  transformation = Transformation(matrix);
  angles = transformation.eulerAngles();
  EXPECT_NEAR(boost::math::constants::pi<double>()/4.0, angles.psi(), 0.0001);
  EXPECT_NEAR(boost::math::constants::pi<double>()/4.0, angles.theta(), 0.0001);
  EXPECT_NEAR(boost::math::constants::pi<double>()/4.0, angles.phi(), 0.0001);

  angles = EulerAngles(boost::math::constants::pi<double>()/4.0,
                       boost::math::constants::pi<double>()/4.0,
                       boost::math::constants::pi<double>()/4.0);
  transformation = Transformation::rotation(angles);
  rotationMatrix = transformation.rotationMatrix();
  EXPECT_NEAR(0.5, rotationMatrix(0,0), 0.0001);
  EXPECT_NEAR(-0.1464, rotationMatrix(0,1), 0.0001);
  EXPECT_NEAR(0.8536, rotationMatrix(0,2), 0.0001);
  EXPECT_NEAR(0.5, rotationMatrix(1,0), 0.0001);
  EXPECT_NEAR(0.8536, rotationMatrix(1,1), 0.0001);
  EXPECT_NEAR(-0.1464, rotationMatrix(1,2), 0.0001);
  EXPECT_NEAR(-0.7071, rotationMatrix(2,0), 0.0001);
  EXPECT_NEAR(0.5, rotationMatrix(2,1), 0.0001);
  EXPECT_NEAR(0.5, rotationMatrix(2,2), 0.0001);
}

TEST_F(GeometryFixture, Transformation_Decompose)
{
  Transformation translation = Transformation::translation(Vector3d(1, 0, 0));
  Transformation rotation = Transformation::rotation(Vector3d(0,0,1), degToRad(-90));

  Transformation transformation = translation*rotation;
  Vector3d origin = transformation.translation();
  EulerAngles angles = transformation.eulerAngles();
  Transformation test = Transformation::translation(origin)*Transformation::rotation(angles);

  EXPECT_TRUE(transformation.matrix() == test.matrix()) << transformation.matrix() << std::endl << test.matrix();

  transformation = rotation*translation;
  origin = transformation.translation();
  angles = transformation.eulerAngles();
  test = Transformation::translation(origin)*Transformation::rotation(angles);

  EXPECT_TRUE(transformation.matrix() == test.matrix()) << transformation.matrix() << std::endl << test.matrix();

}


/**
 * @file
 * @brief NPDE homework TestQuadratureRules
 * @author Erick Schulz, Liaowang Huang (refactoring)
 * @date 08/03/2019, 22/02/2020 (refactoring)
 * @copyright Developed at ETH Zurich
 */

#include "testquadraturerules.h"

#include <lf/base/base.h>
#include <lf/quad/quad.h>

#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace TestQuadratureRules {

double factorial(int i) { return std::tgamma(i + 1); }

/* SAM_LISTING_BEGIN_1 */
bool testQuadOrderTria(const lf::quad::QuadRule &quad_rule,
                       unsigned int order) {
  bool order_isExact = true;  // return variable
  //====================
  // Your code goes here
  //====================
  return order_isExact;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
bool testQuadOrderQuad(const lf::quad::QuadRule &quad_rule,
                       unsigned int order) {
  bool order_isExact = true;  // return variable

  //====================
  // Your code goes here
  //====================
  return order_isExact;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
unsigned int calcQuadOrder(const lf::quad::QuadRule &quad_rule) {
  unsigned int maximal_order = quad_rule.Order();

  //====================
  // Your code goes here
  //====================
  return maximal_order;
}
/* SAM_LISTING_END_3 */

}  // namespace TestQuadratureRules


//
// Created by erik on 10/1/16.
//

#include "vec.h"
#define BOOST_TEST_MODULE VEC
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(VecTest)
    using spatacs::Vec;
    BOOST_AUTO_TEST_CASE(OPS)
    {
        Vec a = {1, 2, 3};
        Vec b = {5, 4, 7};

        BOOST_CHECK_EQUAL(a + b, (Vec{6, 6, 10}));
        Vec c = a;
        c += b;
        BOOST_CHECK_EQUAL( c , a + b );

        BOOST_CHECK_EQUAL( b - a , (Vec{4, 2, 4}) );
        c -= b;
        BOOST_CHECK_EQUAL(c , a);

        float f = 2;
        BOOST_CHECK_EQUAL(1*a , a);
        BOOST_CHECK_EQUAL(f*a , a*f);
        BOOST_CHECK_EQUAL(2*a , (Vec{2, 4, 6}));
        c *= f;
        BOOST_CHECK_EQUAL(c , f*a);
    }

    BOOST_AUTO_TEST_CASE(Length)
    {
        Vec a = {1, 0, 0};
        BOOST_CHECK_EQUAL(1 , length(a));

        Vec b = {2, 2, 1};
        BOOST_CHECK_EQUAL(3 , length(b));

        BOOST_CHECK_EQUAL(length(2*b) , 2*length(b));
    }

    BOOST_AUTO_TEST_CASE(Dot)
    {
        Vec a = {1, 3, 2};
        Vec b = {2, 2, 1};
        Vec c = {0, 3, 1};
        BOOST_CHECK_EQUAL(dot(b, b) , length(b) * length(b));

        Vec ex = {1, 0, 0};
        Vec ey = {0, 1, 0};
        Vec ez = {0, 0, 1};
        BOOST_CHECK_EQUAL(dot(ex, b) , b.x);
        BOOST_CHECK_EQUAL(dot(ey, b) , b.y);
        BOOST_CHECK_EQUAL(dot(ez, b) , b.z);

        BOOST_CHECK_EQUAL( dot(a+b, c) , dot(a, c) + dot(b, c));
        BOOST_CHECK_EQUAL( dot(a, b) , dot(b, a));
        BOOST_CHECK_EQUAL( dot(2*a, b) , 2*dot(a, b));
    }

    BOOST_AUTO_TEST_CASE(Decomposition)
    {
        Vec a = {1, 3, 2};
        Vec b = {2, 2, 1};
        Vec ex = {1, 0, 0};

        BOOST_CHECK_EQUAL(parallel(a, ex) , (Vec{1, 0, 0}));
        BOOST_CHECK_EQUAL(perpendicular(a, ex) , (Vec{0, 3, 2}));

        BOOST_CHECK_EQUAL(parallel(a, b) + perpendicular(a, b) , a);
        BOOST_CHECK_EQUAL(parallel(a, b) , parallel(a, 2*b));
        BOOST_CHECK_EQUAL(perpendicular(a, b) , perpendicular(a, 2*b));

        BOOST_CHECK_SMALL( dot(parallel(a, b), perpendicular(a, b)), 1e-6 );
    }

    BOOST_AUTO_TEST_CASE(Decomposition_CornerCases)
    {
        Vec a = {1, 3, 2};
        Vec ex = {0, 0, 0};

        BOOST_CHECK_EQUAL(parallel(a, ex) , a);
        BOOST_CHECK_EQUAL(perpendicular(a, ex) , a);
    }
BOOST_AUTO_TEST_SUITE_END()


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

//! \file
//!\brief Tests round-trip for fixed_point negatable with 2 decimal digits.

#define BOOST_TEST_MODULE test_negatable_round_trip_digits10_002
#define BOOST_LIB_DIAGNOSTIC

#include <algorithm>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>

#include <boost/cstdint.hpp>
#include <boost/fixed_point/fixed_point.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  // Define a binary fixed-point type with 2 decimal digits of precision.
  typedef
  boost::fixed_point::negatable<0,
                                -8,
                                boost::fixed_point::round::nearest_even>
  fixed_point_type;

  bool round_trip(const fixed_point_type& x);
}

bool local::round_trip(const local::fixed_point_type& x)
{
  using local::fixed_point_type;

  std::stringstream ss1;

  ss1 << std::setprecision(std::numeric_limits<fixed_point_type>::digits10)
      << std::fixed
      << x;

  std::stringstream ss2(ss1.str());

  fixed_point_type y;
  ss2 >> y;

  const bool b(x == y);

  return b;
}

BOOST_AUTO_TEST_CASE(test_negatable_round_trip_digits10_002)
{
  using local::fixed_point_type;

  typedef fixed_point_type::float_type floating_point_type;

  boost::uint_fast16_t count;

  BOOST_CONSTEXPR_OR_CONST boost::uint_fast16_t number_of_test_cases = UINT16_C(99);

  bool b = true;

  // Test every single value with 2 decimal digits of precision
  // ranging from 0.01, 0.02, 0.03, ... 0.99.
  for(count = UINT16_C(1); ((count < number_of_test_cases) && b); ++count)
  {
    std::stringstream ss1;

    ss1 << count;

    std::string str(ss1.str());

    str.insert(std::string::size_type(0U),
               std::string::size_type(2U) - ((std::min)(std::string::size_type(2U), str.length())),
               char('0'));

    const fixed_point_type x(boost::lexical_cast<floating_point_type>(str.insert(std::string::size_type(0U), "0.")));

    const bool next_test_result = local::round_trip(x);

    b = (b && next_test_result);
  }

  BOOST_CHECK_EQUAL(count, number_of_test_cases);

  BOOST_CHECK_EQUAL(b, true);
}