/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#ifndef KINDR_ROTATIONS_EIGEN_ROTATIONQUATERNION_HPP_
#define KINDR_ROTATIONS_EIGEN_ROTATIONQUATERNION_HPP_

#include <cmath>

#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/quaternions/QuaternionEigen.hpp"
#include "kindr/rotations/RotationBase.hpp"
#include "kindr/rotations/eigen/RotationEigenFunctions.hpp"

namespace kindr {
namespace rotations {
namespace eigen_impl {

/*!  \class RotationQuaternion
 *  \brief Implementation of quaternion rotation based on Eigen::Quaternion
 *
 *  The following four typedefs are provided for convenience:
 *   - \ref eigen_impl::RotationQuaternionAD "RotationQuaternionAD" for active rotation and primitive type double
 *   - \ref eigen_impl::RotationQuaternionAF "RotationQuaternionAF" for active rotation and primitive type float
 *   - \ref eigen_impl::RotationQuaternionPD "RotationQuaternionPD" for passive rotation and primitive type double
 *   - \ref eigen_impl::RotationQuaternionPF "RotationQuaternionPF" for passive rotation and primitive type float
 *
 *  \tparam PrimType_ the primitive type of the data (double or float)
 *  \tparam Usage_ the rotation usage which is either active or passive
 *
 *  \ingroup rotations
 */
template<typename PrimType_, enum RotationUsage Usage_>
class RotationQuaternion : public RotationQuaternionBase<RotationQuaternion<PrimType_, Usage_>, Usage_> {
 private:
  /*! \brief The base type.
   */
  typedef quaternions::eigen_impl::UnitQuaternion<PrimType_> Base;

  /*! \brief The data container
   */
  Base rotationQuaternion_;
 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef typename Base::Implementation Implementation;

  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;

  //! the imaginary type, i.e., Eigen::Quaternion<>
  typedef Eigen::Matrix<PrimType_,3,1> Imaginary;

  //! quaternion as 4x1 matrix: [w; x; y; z]
  typedef Eigen::Matrix<PrimType_,4,1> Vector4;

  /*! \brief Default constructor using identity rotation.
   */
  RotationQuaternion()
    : rotationQuaternion_(Implementation::Identity()) {
  }

  /*! \brief Constructor using four scalars.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param w     first entry of the quaternion = cos(phi/2)
   *  \param x     second entry of the quaternion = n1*sin(phi/2)
   *  \param y     third entry of the quaternion = n2*sin(phi/2)
   *  \param z     fourth entry of the quaternion = n3*sin(phi/2)
   */
  RotationQuaternion(Scalar w, Scalar x, Scalar y, Scalar z)
    : rotationQuaternion_(w,x,y,z) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using real and imaginary part.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param real   real part (PrimType_)
   *  \param imag   imaginary part (Eigen::Matrix<PrimType_,3,1>)
   */
  RotationQuaternion(Scalar real, const Imaginary& imag)
    : rotationQuaternion_(real,imag(0),imag(1),imag(2)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using Eigen::Matrix<PrimType_,4,1>.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param other   Eigen::Matrix<PrimType_,4,1>
   */
  RotationQuaternion(const Vector4 & vec)
    : rotationQuaternion_(vec(0),vec(1),vec(2),vec(3)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), 1e-4, "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using Eigen::Quaternion<PrimType_>.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param other   Eigen::Quaternion<PrimType_>
   */
  explicit RotationQuaternion(const Implementation& other)
    : rotationQuaternion_(other.w(), other.x(), other.y(), other.z()) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), 1e-4, "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using quaternions::UnitQuaternion.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param other   quaternions::UnitQuaternion
   */
  explicit RotationQuaternion(const Base& other)
    : rotationQuaternion_(other.w(), other.x(), other.y(), other.z()) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), 1e-4, "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using another rotation.
   *  \param other   other rotation
   */
  template<typename OtherDerived_>
  inline explicit RotationQuaternion(const RotationBase<OtherDerived_, Usage_>& other)
    : rotationQuaternion_(internal::ConversionTraits<RotationQuaternion, OtherDerived_>::convert(other.derived()).toImplementation()) {
  }

  inline Scalar w() const {
    return rotationQuaternion_.w();
  }

  inline Scalar x() const {
    return rotationQuaternion_.x();
  }

  inline Scalar y() const {
    return rotationQuaternion_.y();
  }

  inline Scalar z() const {
    return rotationQuaternion_.z();
  }

  inline Scalar real() const {
    return this->toUnitQuaternion().real();
  }

  inline Imaginary imaginary() const {
    return this->toUnitQuaternion().imaginary();
  }

  inline Vector4 vector() const {
    Vector4 vector4;
    vector4 << w(), x(), y(), z();
    return vector4;
  }

  inline void setValues(Scalar w, Scalar x, Scalar y, Scalar z) {
    rotationQuaternion_.w() = w;
    rotationQuaternion_.x() = x;
    rotationQuaternion_.y() = y;
    rotationQuaternion_.z() = z;
  }

  inline void setParts(Scalar real, const Imaginary& imag) {
    rotationQuaternion_.w() = real;
    rotationQuaternion_.x() = imag(0);
    rotationQuaternion_.y() = imag(1);
    rotationQuaternion_.z() = imag(2);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline Implementation& toImplementation() {
    return toUnitQuaternion().toImplementation();
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline const Implementation& toImplementation() const {
    return toUnitQuaternion().toImplementation();
  }

//  Implementation toImplementation() const {
//    return Implementation(this->w(),this->x(),this->y(),this->z());
//  }

  Base& toUnitQuaternion() {
     return static_cast<Base&>(rotationQuaternion_);
   }

  const Base& toUnitQuaternion() const {
     return static_cast<const Base&>(rotationQuaternion_);
   }

  /*! \brief Assignment operator using a UnitQuaternion.
   *  \param quat   UnitQuaternion
   *  \returns reference
   */
  template<typename PrimTypeIn_>
  RotationQuaternion& operator =(const quaternions::eigen_impl::UnitQuaternion<PrimTypeIn_>& quat) {
    this->toImplementation() = Implementation(quat.w(),quat.x(),quat.y(),quat.z());
    return *this;
  }

  /*! \brief Assignment operator using another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  RotationQuaternion& operator =(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<RotationQuaternion, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Assignment operator using another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename PrimTypeIn_>
  RotationQuaternion& operator =(const RotationQuaternion<PrimTypeIn_, Usage_>& other) {
    this->toImplementation() = other.toImplementation().template cast<PrimType_>();
    return *this;
  }

  /*! \brief Bracket operator which assigns a UnitQuaternion to the RotationQuaternion.
   *  \param quat   UnitQuaternion
   *  \returns reference
   */
  template<typename PrimTypeIn_>
  RotationQuaternion& operator ()(const quaternions::eigen_impl::UnitQuaternion<PrimTypeIn_>& quat) {
    this->toImplementation() = Implementation(quat.w(),quat.x(),quat.y(),quat.z());
    return *this;
  }

  /*! \brief Bracket operator which assigns a Quaternion to the RotationQuaternion.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param quat   Quaternion
   *  \returns reference
   */
  template<typename PrimTypeIn_>
  RotationQuaternion& operator ()(const quaternions::eigen_impl::Quaternion<PrimTypeIn_>& quat) {
    this->toImplementation() = Implementation(quat.w(),quat.x(),quat.y(),quat.z());
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input quaternion has not unit length.");
    return *this;
  }

  /*! \brief Parenthesis operator to convert from another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  RotationQuaternion& operator ()(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<RotationQuaternion, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  RotationQuaternion inverted() const {
    return RotationQuaternion(this->toUnitQuaternion().inverted());
  }

  /*! \brief Inverts the rotation.
   *  \returns reference
   */
  RotationQuaternion& invert() {
    *this = inverted();
    return *this;
  }

  /*! \brief Returns the conjugated of the quaternion.
   *  \returns conjugated of the quaternion
   */
  RotationQuaternion conjugated() const {
    return RotationQuaternion(this->toUnitQuaternion().conjugated());
  }

  /*! \brief Conjugates of the quaternion.
   *  \returns reference
   */
  RotationQuaternion& conjugate() {
    *this = conjugated();
    return *this;
  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  RotationQuaternion& setIdentity() {
    rotationQuaternion_.w() = static_cast<Scalar>(1);
    rotationQuaternion_.x() = static_cast<Scalar>(0);
    rotationQuaternion_.y() = static_cast<Scalar>(0);
    rotationQuaternion_.z() = static_cast<Scalar>(0);
    return *this;
  }

  /*! \brief Returns a unique quaternion rotation with w > 0.
   *  This function is used to compare different rotations.
   *  \returns copy of the quaternion rotation which is unique
   */
  RotationQuaternion getUnique() const {
    if(this->w() > 0) {
      return *this;
    } else if (this->w() < 0){
      return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
    } else { // w == 0

      if(this->x() > 0) {
        return *this;
      } else if (this->x() < 0){
        return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
      } else { // x == 0

        if(this->y() > 0) {
          return *this;
        } else if (this->y() < 0){
          return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
        } else { // y == 0

          if(this->z() > 0) { // z must be either -1 or 1 in this case
            return *this;
          } else {
            return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
          }
        }
      }
    }
  }

  /*! \brief Returns the quaternion matrix Qleft: q*p = Qleft(q)*p
   *  This function can be used to get the derivative of the concatenation with respect to the right quaternion.
   *  \returns the quaternion matrix Qleft
   */
  Eigen::Matrix<PrimType_,4,4> getQuaternionMatrix() {
    Eigen::Matrix<PrimType_,4,4> Qleft;
    if(Usage_ == rotations::RotationUsage::ACTIVE)
    {
      Qleft(0,0) =  w();      Qleft(0,1) = -x();      Qleft(0,2) = -y();      Qleft(0,3) = -z();
      Qleft(1,0) =  x();      Qleft(1,1) =  w();      Qleft(1,2) = -z();      Qleft(1,3) =  y();
      Qleft(2,0) =  y();      Qleft(2,1) =  z();      Qleft(2,2) =  w();      Qleft(2,3) = -x();
      Qleft(3,0) =  z();      Qleft(3,1) = -y();      Qleft(3,2) =  x();      Qleft(3,3) =  w();
    }
    if(Usage_ == rotations::RotationUsage::PASSIVE)
    {
      Qleft(0,0) =  w();      Qleft(0,1) = -x();      Qleft(0,2) = -y();      Qleft(0,3) = -z();
      Qleft(1,0) =  x();      Qleft(1,1) =  w();      Qleft(1,2) =  z();      Qleft(1,3) = -y();
      Qleft(2,0) =  y();      Qleft(2,1) = -z();      Qleft(2,2) =  w();      Qleft(2,3) =  x();
      Qleft(3,0) =  z();      Qleft(3,1) =  y();      Qleft(3,2) = -x();      Qleft(3,3) =  w();
    }
    return Qleft;
  }

  /*! \brief Returns the quaternion matrix Qright: q*p = Qright(p)*q
   *  This function can be used to get the derivative of the concatenation with respect to the left quaternion.
   *  \returns the quaternion matrix Qright
   */
  Eigen::Matrix<PrimType_,4,4> getConjugateQuaternionMatrix() {
    Eigen::Matrix<PrimType_,4,4> Qright;
    if(Usage_ == rotations::RotationUsage::ACTIVE)
    {
      Qright(0,0) =  w();      Qright(0,1) = -x();      Qright(0,2) = -y();      Qright(0,3) = -z();
      Qright(1,0) =  x();      Qright(1,1) =  w();      Qright(1,2) =  z();      Qright(1,3) = -y();
      Qright(2,0) =  y();      Qright(2,1) = -z();      Qright(2,2) =  w();      Qright(2,3) =  x();
      Qright(3,0) =  z();      Qright(3,1) =  y();      Qright(3,2) = -x();      Qright(3,3) =  w();
    }
    if(Usage_ == rotations::RotationUsage::PASSIVE)
    {
      Qright(0,0) =  w();      Qright(0,1) = -x();      Qright(0,2) = -y();      Qright(0,3) = -z();
      Qright(1,0) =  x();      Qright(1,1) =  w();      Qright(1,2) = -z();      Qright(1,3) =  y();
      Qright(2,0) =  y();      Qright(2,1) =  z();      Qright(2,2) =  w();      Qright(2,3) = -x();
      Qright(3,0) =  z();      Qright(3,1) = -y();      Qright(3,2) =  x();      Qright(3,3) =  w();
    }
    return Qright;
  }

  /*! \brief Returns the global quaternion diff matrix H: GlobalAngularVelocity = 2*H*qdiff, qdiff = 0.5*H^T*GlobalAngularVelocity
   *  \returns the global quaternion diff matrix H
   */
  Eigen::Matrix<PrimType_,3,4> getGlobalQuaternionDiffMatrix() {
    Eigen::Matrix<PrimType_,3,4> H;
    if(Usage_ == rotations::RotationUsage::ACTIVE) // x, y, z * -1
    {
      H(0,0) =  -x();      H(0,1) =  w();      H(0,2) =  z();      H(0,3) = -y();
      H(1,0) =  -y();      H(1,1) = -z();      H(1,2) =  w();      H(1,3) =  x();
      H(2,0) =  -z();      H(2,1) =  y();      H(2,2) = -x();      H(2,3) =  w();
    }
    if(Usage_ == rotations::RotationUsage::PASSIVE)
    {
      H(0,0) = -x();      H(0,1) =  w();      H(0,2) = -z();      H(0,3) =  y();
      H(1,0) = -y();      H(1,1) =  z();      H(1,2) =  w();      H(1,3) = -x();
      H(2,0) = -z();      H(2,1) = -y();      H(2,2) =  x();      H(2,3) =  w();
    }
    return H;
  }

  /*! \brief Returns the local quaternion diff matrix HBar: LocalAngularVelocity = 2*HBar*qdiff, qdiff = 0.5*HBar^T*LocalAngularVelocity
   *  \returns the local quaternion diff matrix HBar
   */
  Eigen::Matrix<PrimType_,3,4> getLocalQuaternionDiffMatrix() const {
    Eigen::Matrix<PrimType_,3,4> HBar;
    if(Usage_ == rotations::RotationUsage::ACTIVE) // x, y, z * -1
    {
      HBar(0,0) =  -x();      HBar(0,1) =  w();      HBar(0,2) = -z();      HBar(0,3) =  y();
      HBar(1,0) =  -y();      HBar(1,1) =  z();      HBar(1,2) =  w();      HBar(1,3) = -x();
      HBar(2,0) =  -z();      HBar(2,1) = -y();      HBar(2,2) =  x();      HBar(2,3) =  w();
    }
    if(Usage_ == rotations::RotationUsage::PASSIVE)
    {
      HBar(0,0) = -x();      HBar(0,1) =  w();      HBar(0,2) =  z();      HBar(0,3) = -y();
      HBar(1,0) = -y();      HBar(1,1) = -z();      HBar(1,2) =  w();      HBar(1,3) =  x();
      HBar(2,0) = -z();      HBar(2,1) =  y();      HBar(2,2) = -x();      HBar(2,3) =  w();
    }
    return HBar;
  }

  /*! \brief Modifies the quaternion rotation such that w >= 0.
   *  \returns reference
   */
  RotationQuaternion& setUnique() {
    *this = getUnique();
    return *this;
  }

  /*! \brief Returns the norm of the quaternion.
   *  The RotationQuaternion should always have unit length.
   *  \returns norm of the quaternion
   */
  Scalar norm() {
    return rotationQuaternion_.norm();
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because QuaternionBase provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using RotationQuaternionBase<RotationQuaternion<PrimType_, Usage_>, Usage_> ::operator*;

  /*! \brief Equivalence operator.
   *  This is explicitly specified, because QuaternionBase provides also an operator==.
   *  \returns true if two rotations are similar.
   */
  using RotationQuaternionBase<RotationQuaternion<PrimType_, Usage_>, Usage_> ::operator==;

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const RotationQuaternion& rquat) {
    out << rquat.toUnitQuaternion();
    return out;
  }
};

//! \brief Active quaternion rotation with double primitive type
typedef RotationQuaternion<double, RotationUsage::ACTIVE>  RotationQuaternionAD;
//! \brief Active quaternion rotation with float primitive type
typedef RotationQuaternion<float,  RotationUsage::ACTIVE>  RotationQuaternionAF;
//! \brief Passive quaternion rotation with double primitive type
typedef RotationQuaternion<double, RotationUsage::PASSIVE> RotationQuaternionPD;
//! \brief Passive quaternion rotation with float primitive type
typedef RotationQuaternion<float,  RotationUsage::PASSIVE> RotationQuaternionPF;

} // namespace eigen_impl

namespace internal {

template<typename PrimType_, enum RotationUsage Usage_>
class get_scalar<eigen_impl::RotationQuaternion<PrimType_, Usage_>> {
 public:
  typedef PrimType_ Scalar;
};

template<typename PrimType_, enum RotationUsage Usage_>
class get_matrix3X<eigen_impl::RotationQuaternion<PrimType_, Usage_>>{
 public:
  typedef int IndexType;

  template <IndexType Cols>
  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::RotationQuaternion<PrimType_, RotationUsage::ACTIVE>> {
 public:
  typedef eigen_impl::RotationQuaternion<PrimType_, RotationUsage::PASSIVE> OtherUsage;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::RotationQuaternion<PrimType_, RotationUsage::PASSIVE>> {
 public:
  typedef eigen_impl::RotationQuaternion<PrimType_, RotationUsage::ACTIVE> OtherUsage;
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationQuaternion<DestPrimType_, Usage_>, eigen_impl::AngleAxis<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationQuaternion<DestPrimType_, Usage_> convert(const eigen_impl::AngleAxis<SourcePrimType_, Usage_>& aa) {
    return eigen_impl::RotationQuaternion<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getQuaternionFromAngleAxis<SourcePrimType_, DestPrimType_>(aa.toImplementation()));
  }
};

template <typename Scalar_ = double>
inline bool isLessThenEpsilons4thRoot(Scalar_ x){
  static const Scalar_ epsilon4thRoot = pow(std::numeric_limits<Scalar_>::epsilon(), 1.0/4.0);
  return x < epsilon4thRoot;
}

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationQuaternion<DestPrimType_, Usage_>, eigen_impl::RotationVector<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationQuaternion<DestPrimType_, Usage_> convert(const eigen_impl::RotationVector<SourcePrimType_, Usage_>& rotationVector) {
    typedef typename eigen_impl::RotationQuaternion<DestPrimType_, Usage_>::Scalar Scalar;
    typedef typename eigen_impl::RotationQuaternion<DestPrimType_, Usage_>::Imaginary Imaginary;
//    const Scalar v = rotationVector.toImplementation().norm();
//    Scalar real;
//    Imaginary imaginary;
//    if (v < common::internal::NumTraits<Scalar>::dummy_precision()) {
//      real = 1.0;
//      imaginary= 0.5*rotationVector.toImplementation().template cast<DestPrimType_>();
//    }
//    else {
//      real = cos(v/2);
//      imaginary = sin(v/2)/v*rotationVector.toImplementation().template cast<DestPrimType_>();
//    }
//    return eigen_impl::RotationQuaternion<DestPrimType_, Usage_>(real, imaginary);

    Scalar theta = (Scalar)rotationVector.toImplementation().norm();

    // na is 1/theta sin(theta/2)
    double na;
    if(isLessThenEpsilons4thRoot(theta))
    {
        const Scalar one_over_48 = 1.0/48.0;
        na = 0.5 + (theta * theta) * one_over_48;
    }
    else
    {
        na = sin(theta*0.5) / theta;
    }
    Imaginary axis = rotationVector.toImplementation().template cast<Scalar>()*na;
    Scalar ct = cos(theta*0.5);
    return eigen_impl::RotationQuaternion<DestPrimType_, Usage_>(ct, axis[0],axis[1],axis[2]);
//    return Eigen::Vector4d(axis[0],axis[1],axis[2],ct);

  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationQuaternion<DestPrimType_, Usage_>, eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationQuaternion<DestPrimType_, Usage_> convert(const eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>& q) {
    return eigen_impl::RotationQuaternion<DestPrimType_, Usage_>(q.toImplementation().template cast<DestPrimType_>());
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationQuaternion<DestPrimType_, Usage_>, eigen_impl::RotationMatrix<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationQuaternion<DestPrimType_, Usage_> convert(const eigen_impl::RotationMatrix<SourcePrimType_, Usage_>& rotationMatrix) {
    return eigen_impl::RotationQuaternion<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getQuaternionFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.toImplementation()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationQuaternion<DestPrimType_, Usage_>, eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationQuaternion<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>& xyz) {
    return eigen_impl::RotationQuaternion<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getQuaternionFromRpy<SourcePrimType_, DestPrimType_>(xyz.toImplementation()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationQuaternion<DestPrimType_, Usage_>, eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationQuaternion<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>& zyx) {
    return eigen_impl::RotationQuaternion<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getQuaternionFromYpr<SourcePrimType_, DestPrimType_>(zyx.toImplementation()));
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Multiplication Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_, enum RotationUsage Usage_>
class ComparisonTraits<eigen_impl::RotationQuaternion<PrimType_, Usage_>, eigen_impl::RotationQuaternion<PrimType_, Usage_>> {
 public:
   inline static bool isEqual(const eigen_impl::RotationQuaternion<PrimType_, Usage_>& a, const eigen_impl::RotationQuaternion<PrimType_, Usage_>& b){
     return a.toImplementation().w() ==  b.toImplementation().w() &&
            a.toImplementation().x() ==  b.toImplementation().x() &&
            a.toImplementation().y() ==  b.toImplementation().y() &&
            a.toImplementation().z() ==  b.toImplementation().z();
   }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Fixing Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_, enum RotationUsage Usage_>
class FixingTraits<eigen_impl::RotationQuaternion<PrimType_, Usage_>> {
 public:
  inline static void fix(eigen_impl::RotationQuaternion<PrimType_, Usage_>& q) {
    q.toImplementation().normalize();
  }
};

} // namespace internal
} // namespace rotations
} // namespace kindr

#endif /* KINDR_ROTATIONS_EIGEN_ROTATIONQUATERNION_HPP_ */


#ifndef CONTRACTOR_TERMS_PERMUTATIONGROUP_HPP_
#define CONTRACTOR_TERMS_PERMUTATIONGROUP_HPP_

#include "terms/Index.hpp"
#include "terms/IndexSubstitution.hpp"

#include <ostream>
#include <vector>

#include <boost/range/join.hpp>

namespace Contractor::Terms {

/*
 * NOTE: For a more performant solution (especially for larger groups) the
 * Schreier-Sims-Algorithm for generating a base and a strong generating set
 * (used as a representation for the group) should be used instead.
 * However as we are only expecting to treat small-ish groups with this class
 * and given that the Schreier-Sims-Algorithm involves heavy group theory
 * that I don't understand yet, we'll settle on the brute-force method here.
 */

/**
 * A class representing a permutation group (A group in the mathematical sense that consists of
 * permutation operations).
 */
class PermutationGroup {
public:
	/**
	 * An element consists of a specific index sequence along with a factor that results from converting
	 * the initial sequence into the stored one using only the allowed permutation operations.
	 */
	struct Element {
		std::vector< Index > indexSequence;
		int factor = 1.0f;

		Element(const std::vector< Index > &seq = {}, int factor = 1.0f) : indexSequence(seq), factor(factor) {}
		Element(const Element &other) = default;
		Element(Element &&other)      = default;

		Element &operator=(const Element &other) = default;
		Element &operator=(Element &&other) = default;

		friend bool operator==(const Element &lhs, const Element &rhs) {
			return lhs.indexSequence == rhs.indexSequence;
		};

		friend bool operator!=(const Element &lhs, const Element &rhs) { return lhs != rhs; }

		friend bool operator<(const Element &lhs, const Element &rhs) { return lhs.indexSequence < rhs.indexSequence; }
	};

	PermutationGroup() = default;
	PermutationGroup(const Element &startConfiguration);
	PermutationGroup(Element &&startConfiguration);
	PermutationGroup(const PermutationGroup &other) = default;
	PermutationGroup(PermutationGroup &&other)      = default;
	PermutationGroup &operator=(const PermutationGroup &other) = default;
	PermutationGroup &operator=(PermutationGroup &&other) = default;

	friend bool operator==(const PermutationGroup &lhs, const PermutationGroup &rhs);
	friend bool operator!=(const PermutationGroup &lhs, const PermutationGroup &rhs);
	friend std::ostream &operator<<(std::ostream &stream, const PermutationGroup &group);

	/**
	 * Add a generator for this group
	 *
	 * @param generator The generator operation to add
	 * @param regenerate Whether to regenerate the group after having added the generator
	 */
	void addGenerator(const IndexSubstitution &generator, bool regenerate = true);
	/**
	 * Add a generator for this group
	 *
	 * @param generator The generator operation to add
	 * @param regenerate Whether to regenerate the group after having added the generator
	 */
	void addGenerator(IndexSubstitution &&generator, bool regenerate = true);

	/**
	 * @returns A list of generator operations of this group
	 */
	const std::vector< IndexSubstitution > &getGenerators() const;
	/**
	 * @returns A list of operations of this group that are not the generators but that result
	 * by chainging and combining the generators.
	 */
	const std::vector< IndexSubstitution > &getAdditionalSymmetryOperations() const;

	/**
	 * @returns The list of permutations of the index sequence that can be reached by applying the
	 * permutation operations contained in this group.
	 */
	const std::vector< Element > &getIndexPermutations() const;

	/**
	 * Set the initial index sequence this group shall act on
	 *
	 * @param rootSequence The sequence to permute
	 */
	void setRootSequence(const Element &rootSequence);

	/**
	 * @returns Whether the given permutation operation is contained in this group
	 */
	bool contains(const IndexSubstitution &permutation) const;
	/**
	 * @returns Whether the given index sequence can be reached from the root sequence set on this group
	 * by only using the permutation operations contained in this group.
	 */
	bool contains(const std::vector< Index > &indexSequence) const;

	/**
	 * @returns The size of this group (amount of permutation operations contained in it)
	 */
	std::size_t size() const;

	/**
	 * @returns A "canonical" index sequence. This sequence will be the same no matter which root sequence
	 * has been chosen for this group, provided that the different root sequences can be converted into one another
	 * using only the allowed permutation operations on them.
	 */
	const std::vector< Index > &getCanonicalRepresentation() const;
	/**
	 * The factor that is associated with turning the set root sequence into the "canonical" one
	 */
	int getCanonicalRepresentationFactor() const;

	/**
	 * Given the generators of this group, this function will generate all permutation operations.
	 */
	void regenerateGroup();

protected:
	std::vector< Element > m_permutations;
	std::vector< IndexSubstitution > m_generators = { IndexSubstitution::identity() };
	std::vector< IndexSubstitution > m_additionalElements;

	void generateSymmetryOperations(const IndexSubstitution &preceidingOperation = IndexSubstitution::identity());
};

}; // namespace Contractor::Terms

// Provide template specialization of std::hash for the PermutationGroup class
namespace std {
template<> struct hash< Contractor::Terms::PermutationGroup > {
	std::size_t operator()(const Contractor::Terms::PermutationGroup &group) const {
		std::size_t hash = 0;

		auto it = boost::join(group.getGenerators(), group.getAdditionalSymmetryOperations());
		for (auto i = it.begin(); i != it.end(); ++i) {
			hash += std::hash< Contractor::Terms::IndexSubstitution >{}(*i);
		}

		return hash;
	}
};
}; // namespace std

#endif // CONTRACTOR_TERMS_PERMUTATIONGROUP_HPP_


/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef _GAZEBO_MATH_FUNCTIONS_HH_
#define _GAZEBO_MATH_FUNCTIONS_HH_

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/round.hpp>
#include <algorithm>
#include <cmath>
#include <limits>
#include <string>
#include <iostream>
#include <vector>
#include "gazebo/util/system.hh"

/// \brief Double maximum value
#define GZ_DBL_MAX gazebo::math::MAX_D

/// \brief Double min value
#define GZ_DBL_MIN gazebo::math::MIN_D

/// \brief Double positive infinite value
#define GZ_DBL_INF gazebo::math::INF_D

/// \brief Float maximum value
#define GZ_FLT_MAX gazebo::math::MAX_F

/// \brief Float minimum value
#define GZ_FLT_MIN gazebo::math::MIN_F

/// \brief 32bit unsigned integer maximum value
#define GZ_UINT32_MAX gazebo::math::MAX_UI32

/// \brief 32bit unsigned integer minimum value
#define GZ_UINT32_MIN gazebo::math::MIN_UI32

/// \brief 32bit integer maximum value
#define GZ_INT32_MAX gazebo::math::MAX_I32

/// \brief 32bit integer minimum value
#define GZ_INT32_MIN gazebo::math::MIN_I32

namespace gazebo
{
  namespace math
  {
    /// \addtogroup gazebo_math
    /// \brief A set of classes that encapsulate math related properties and
    ///        functions.
    /// \{

    /// \brief Double maximum value. This value will be similar to 1.79769e+308
    static const double
    GAZEBO_DEPRECATED(8.0)
    MAX_D = std::numeric_limits<double>::max();

    /// \brief Double min value. This value will be similar to 2.22507e-308
    static const double
    GAZEBO_DEPRECATED(8.0)
    MIN_D = std::numeric_limits<double>::min();

    /// \brief Double positive infinite value
    static const double
    GAZEBO_DEPRECATED(8.0)
    INF_D = std::numeric_limits<double>::infinity();

    /// \brief Returns the representation of a quiet not a number (NAN)
    static const double
    GAZEBO_DEPRECATED(8.0)
    NAN_D = std::numeric_limits<double>::quiet_NaN();

    /// \brief Float maximum value. This value will be similar to 3.40282e+38
    static const float
    GAZEBO_DEPRECATED(8.0)
    MAX_F = std::numeric_limits<float>::max();

    /// \brief Float minimum value. This value will be similar to 1.17549e-38
    static const float
    GAZEBO_DEPRECATED(8.0)
    MIN_F = std::numeric_limits<float>::min();

    /// \brief 32bit unsigned integer maximum value
    static const uint32_t
    GAZEBO_DEPRECATED(8.0)
    MAX_UI32 = std::numeric_limits<uint32_t>::max();

    /// \brief 32bit unsigned integer minimum value
    static const uint32_t
    GAZEBO_DEPRECATED(8.0)
    MIN_UI32 = std::numeric_limits<uint32_t>::min();

    /// \brief 32bit unsigned integer maximum value
    static const int32_t
    GAZEBO_DEPRECATED(8.0)
    MAX_I32 = std::numeric_limits<int32_t>::max();

    /// \brief 32bit unsigned integer minimum value
    static const int32_t
    GAZEBO_DEPRECATED(8.0)
    MIN_I32 = std::numeric_limits<int32_t>::min();

    /// \brief Returns the representation of a quiet not a number (NAN)
    static const int
    GAZEBO_DEPRECATED(8.0)
    NAN_I = std::numeric_limits<int>::quiet_NaN();

    /// \brief Simple clamping function
    /// \param[in] _v value
    /// \param[in] _min minimum
    /// \param[in] _max maximum
    /// \deprecated See ignition::math::clamp
    template<typename T>
    inline T
    GAZEBO_DEPRECATED(8.0)
    clamp(T _v, T _min, T _max)
    {
      return std::max(std::min(_v, _max), _min);
    }

    /// \brief check if a float is NaN
    /// \param[in] _v the value
    /// \return true if _v is not a number, false otherwise
    /// \deprecated See ignition::math::isnan
    inline bool
    GAZEBO_DEPRECATED(8.0)
    isnan(float _v)
    {
      return (boost::math::isnan)(_v);
    }

    /// \brief check if a double is NaN
    /// \param[in] _v the value
    /// \return true if _v is not a number, false otherwise
    /// \deprecated See ignition::math::isnan
    inline bool
    GAZEBO_DEPRECATED(8.0)
    isnan(double _v)
    {
      return (boost::math::isnan)(_v);
    }

#ifndef _WIN32
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
    /// \brief Fix a nan value.
    /// \param[in] _v Value to correct.
    /// \return 0 if _v is NaN, _v otherwise.
    /// \deprecated See ignition::math::fixnan
    inline float
    GAZEBO_DEPRECATED(8.0)
    fixnan(float _v)
    {
      return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
    }

    /// \brief Fix a nan value.
    /// \param[in] _v Value to correct.
    /// \return 0 if _v is NaN, _v otherwise.
    /// \deprecated See ignition::math::fixnan
    inline double
    GAZEBO_DEPRECATED(8.0)
    fixnan(double _v)
    {
      return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
    }
#ifndef _WIN32
#pragma GCC diagnostic pop
#endif

    /// \brief get mean of vector of values
    /// \param[in] _values the vector of values
    /// \return the mean
    /// \deprecated See ignition::math::mean
    template<typename T>
    inline T
    GAZEBO_DEPRECATED(8.0)
    mean(const std::vector<T> &_values)
    {
      T sum = 0;
      for (unsigned int i = 0; i < _values.size(); ++i)
        sum += _values[i];
      return sum / _values.size();
    }

    /// \brief get variance of vector of values
    /// \param[in] _values the vector of values
    /// \return the squared deviation
    /// \deprecated See ignition::math::variance
    template<typename T>
    inline T
    GAZEBO_DEPRECATED(8.0)
    variance(const std::vector<T> &_values)
    {
      T avg = mean<T>(_values);

      T sum = 0;
      for (unsigned int i = 0; i < _values.size(); ++i)
        sum += (_values[i] - avg) * (_values[i] - avg);
      return sum / _values.size();
    }

    /// \brief get the maximum value of vector of values
    /// \param[in] _values the vector of values
    /// \return maximum
    /// \deprecated See ignition::math::max
    template<typename T>
    inline T
    GAZEBO_DEPRECATED(8.0)
    max(const std::vector<T> &_values)
    {
      T max = std::numeric_limits<T>::min();
      for (unsigned int i = 0; i < _values.size(); ++i)
        if (_values[i] > max)
          max = _values[i];
      return max;
    }

    /// \brief get the minimum value of vector of values
    /// \param[in] _values the vector of values
    /// \return minimum
    /// \deprecated See ignition::math::min
    template<typename T>
    inline T
    GAZEBO_DEPRECATED(8.0)
    min(const std::vector<T> &_values)
    {
      T min = std::numeric_limits<T>::max();
      for (unsigned int i = 0; i < _values.size(); ++i)
        if (_values[i] < min)
          min = _values[i];
      return min;
    }

    /// \brief check if two values are equal, within a tolerance
    /// \param[in] _a the first value
    /// \param[in] _b the second value
    /// \param[in] _epsilon the tolerance
    /// \deprecated See ignition::math::equal
    template<typename T>
    inline bool
    GAZEBO_DEPRECATED(8.0)
    equal(const T &_a, const T &_b, const T &_epsilon = 1e-6)
    {
      return std::fabs(_a - _b) <= _epsilon;
    }

    /// \brief get value at a specified precision
    /// \param[in] _a the number
    /// \param[in] _precision the precision
    /// \return the value for the specified precision
    /// \deprecated See ignition::math::precision
    template<typename T>
    inline T
    GAZEBO_DEPRECATED(8.0)
    precision(const T &_a, const unsigned int &_precision)
    {
      if (!std::isinf(_a))
      {
        return boost::math::round(
          _a * pow(10, _precision)) / pow(10, _precision);
      }
      else
      {
        return _a;
      }
    }

    /// \brief is this a power of 2?
    /// \param[in] _x the number
    /// \return true if _x is a power of 2, false otherwise
    /// \deprecated See ignition::math::isPowerOfTwo
    inline bool
    GAZEBO_DEPRECATED(8.0)
    isPowerOfTwo(unsigned int _x)
    {
      return ((_x != 0) && ((_x & (~_x + 1)) == _x));
    }

    /// \brief Get the smallest power of two that is greater or equal to a given
    /// value
    /// \param[in] _x the number
    /// \return the same value if _x is already a power of two. Otherwise,
    /// it returns the smallest power of two that is greater than _x
    /// \deprecated See ignition::math::roundUpPowerOfTwo
    inline unsigned int
    GAZEBO_DEPRECATED(8.0)
    roundUpPowerOfTwo(unsigned int _x)
    {
      if (_x == 0)
        return 1;

#ifndef _WIN32
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
      if (isPowerOfTwo(_x))
        return _x;
#ifndef _WIN32
#pragma GCC diagnostic pop
#endif

      while (_x & (_x - 1))
        _x = _x & (_x - 1);

      _x = _x << 1;

      return _x;
    }

    /// \brief parse string into an integer
    /// \param[in] _input the string
    /// \return an integer, 0 or 0 and a message in the error stream
    /// \deprecated See ignition::math::parseInt
    inline int
    GAZEBO_DEPRECATED(8.0)
    parseInt(const std::string& _input)
    {
      const char *p = _input.c_str();
#ifndef _WIN32
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
      if (!*p || *p == '?')
        return NAN_I;
#ifndef _WIN32
#pragma GCC diagnostic pop
#endif

      int s = 1;
      while (*p == ' ')
        p++;

      if (*p == '-')
      {
        s = -1;
        p++;
      }

      int acc = 0;
      while (*p >= '0' && *p <= '9')
        acc = acc * 10 + *p++ - '0';

      if (*p)
      {
        std::cerr << "Invalid int numeric format[" << _input << "]\n";
        return 0.0;
      }

      return s * acc;
    }

    /// \brief parse string into float
    /// \param _input the string
    /// \return a floating point number (can be NaN) or 0 with a message in the
    /// error stream
    /// \deprecated See ignition::math::parseFloat
    inline double
    GAZEBO_DEPRECATED(8.0)
    parseFloat(const std::string& _input)
    {
      const char *p = _input.c_str();
#ifndef _WIN32
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
      if (!*p || *p == '?')
        return NAN_D;
#ifndef _WIN32
#pragma GCC diagnostic pop
#endif
      int s = 1;
      while (*p == ' ')
        p++;

      if (*p == '-')
      {
        s = -1;
        p++;
      }

      double acc = 0;
      while (*p >= '0' && *p <= '9')
        acc = acc * 10 + *p++ - '0';

      if (*p == '.')
      {
        double k = 0.1;
        p++;
        while (*p >= '0' && *p <= '9')
        {
          acc += (*p++ - '0') * k;
          k *= 0.1;
        }
      }
      if (*p == 'e')
      {
        int es = 1;
        int f = 0;
        p++;
        if (*p == '-')
        {
          es = -1;
          p++;
        }
        else if (*p == '+')
        {
          es = 1;
          p++;
        }
        while (*p >= '0' && *p <= '9')
          f = f * 10 + *p++ - '0';

        acc *= pow(10, f*es);
      }

      if (*p)
      {
        std::cerr << "Invalid double numeric format[" << _input << "]\n";
        return 0.0;
      }
      return s * acc;
    }
    /// \}
  }
}
#endif


#include <signal.h>
#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <cassert>
#include <sstream>
#include "g2o/apps/g2o_cli/dl_wrapper.h"
#include "g2o/apps/g2o_cli/output_helper.h"
#include "g2o/apps/g2o_cli/g2o_common.h"

#include "g2o/core/estimate_propagator.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/hyper_dijkstra.h"

#include "g2o/stuff/macros.h"
#include "g2o/stuff/color_macros.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/filesys_tools.h"
#include "g2o/stuff/string_tools.h"
#include "g2o/stuff/timeutil.h"

#include "edge_labeler.h"
#include "edge_creator.h"
#include "star.h"

#include "g2o/stuff/unscented.h"
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace g2o;
using namespace Eigen;

typedef SigmaPoint<VectorXd> MySigmaPoint;

void testMarginals(SparseOptimizer& optimizer){
  cerr << "Projecting marginals" << endl;
  std::vector<std::pair<int, int> > blockIndices;
  for (size_t i=0; i<optimizer.activeVertices().size(); i++) {
    OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
    if (v->hessianIndex()>=0){
      blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex()));
    }
    // if (v->hessianIndex()>0){
    //   blockIndices.push_back(make_pair(v->hessianIndex()-1, v->hessianIndex()));
    // }
  }
  SparseBlockMatrix<MatrixXd> spinv;
  if (optimizer.computeMarginals(spinv, blockIndices)) {
    for (size_t i=0; i<optimizer.activeVertices().size(); i++) {
      OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
      cerr << "Vertex id:" << v->id() << endl;
      if (v->hessianIndex()>=0){
        cerr << "increments block :" << v->hessianIndex() << ", " << v->hessianIndex()<< " covariance:" <<  endl;
        VectorXd mean(v->minimalEstimateDimension()); //HACK: need to set identity
        mean.fill(0);
        VectorXd oldMean(v->minimalEstimateDimension()); //HACK: need to set identity
        v->getMinimalEstimateData(&oldMean[0]);
        MatrixXd& cov= *(spinv.block(v->hessianIndex(), v->hessianIndex()));
        std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > spts;
        cerr << cov << endl;
        if (! sampleUnscented(spts,mean,cov) )
          continue;

        // now apply the oplus operator to the sigma points,
        // and get the points in the global space
        std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > tspts = spts;

        for (size_t j=0; j<spts.size(); j++) {
          v->push();
          // cerr << "v_before [" << j << "]" << endl;
          v->getMinimalEstimateData(&mean[0]);
          // cerr << mean << endl;
          // cerr << "sigma [" << j << "]" << endl;
          // cerr << spts[j]._sample << endl;
          v->oplus(&(spts[j]._sample[0]));
          v->getMinimalEstimateData(&mean[0]);
          tspts[j]._sample=mean;
          // cerr << "oplus [" << j << "]" << endl;
          // cerr << tspts[j]._sample << endl;
          v->pop();
        }
        MatrixXd cov2=cov;
        reconstructGaussian(mean, cov2, tspts);
        cerr << "global block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl;
        cerr << "mean: " << endl;
        cerr <<  mean << endl;
        cerr << "oldMean: " << endl;
        cerr <<  oldMean << endl;
        cerr << "cov: " << endl;
        cerr << cov2 << endl;

      }
      // if (v->hessianIndex()>0){
      //   cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< endl;
      //   cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex()));
      //   cerr << endl;
      // }
    }
  }
}

int unscentedTest(){
  MatrixXd m=MatrixXd(6,6);
  for (int i=0; i<6; i++){
    for (int j=i; j<6; j++){
      m(i,j)=m(j,i)=i*j+1;
    }
  }
  m+=MatrixXd::Identity(6,6);
  cerr << m;
  VectorXd mean(6);
  mean.fill(1);

  std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > spts;
  sampleUnscented(spts,mean,m);
  for (size_t i =0; i<spts.size(); i++){
    cerr << "Point " << i << " " << endl << "wi=" << spts[i]._wi << " wp=" << spts[i]._wp << " " << endl;
    cerr << spts[i]._sample << endl;
  }

  VectorXd recMean(6);
  MatrixXd recCov(6,6);

  reconstructGaussian(recMean, recCov, spts);

  cerr << "recMean" << endl;
  cerr << recMean << endl;

  cerr << "recCov" << endl;
  cerr << recCov << endl;

  return 0;
}


/*
 // Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
 // component of the Texas A&M University System.

 // All rights reserved.

 // The information and source code contained herein is the exclusive
 // property of TEES and may not be disclosed, examined or reproduced
 // in whole or in part without explicit written authorization from TEES.
*/

#ifndef STAPL_MAPFUNCS_LINEARIZATION_HPP
#define STAPL_MAPFUNCS_LINEARIZATION_HPP

#include <stapl/algorithms/functional.hpp>
#include <stapl/views/mapping_functions/mapping_functions.hpp>
#include <stapl/utility/tuple.hpp>
#include <stapl/utility/tuple/to_index.hpp>
#include <stapl/utility/tuple/ensure_tuple.hpp>
#include <stapl/utility/tuple/rearrange.hpp>
#include <stapl/utility/integer_sequence.hpp>

#include <boost/mpl/find_if.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/distance.hpp>

namespace stapl {

template<typename GID, typename Traversal>
struct nd_reverse_linearize;

template<typename GID, typename Traversal>
struct nd_linearize;

namespace detail {

//////////////////////////////////////////////////////////////////////
/// @brief Rearrange the contents of a tuple to be in the correct
///        traversal order.
///
/// For example, given the tuple (x, y, z) and the traversal <1, 2, 0>,
/// output the tuple (z, x, y).
///
/// @tparam Tuple Type of the run-time tuple of std::size_ts
/// @tparam Traversal Traversal ordering.
//////////////////////////////////////////////////////////////////////
template<typename Tuple, typename Traversal,
         typename Indices = make_index_sequence<tuple_size<Traversal>::value>>
struct traversal_order;

template<typename Tuple, typename... IntTypes, std::size_t... Indices>
struct traversal_order<Tuple, tuple<IntTypes...>, index_sequence<Indices...>>
{
  typedef boost::mpl::vector<IntTypes...>                  vector_t;

private:
  typedef tuple<
    typename boost::mpl::distance<
      typename boost::mpl::begin<vector_t>::type,
      typename boost::mpl::find_if<
        vector_t, type_constant_equal<Indices>>::type
    >::type...>                                            indices_t;

public:
  static Tuple apply(Tuple const& src)
  {
    return Tuple(get<tuple_element<Indices, indices_t>::type::value>(src)...);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Specialization for size_t
//////////////////////////////////////////////////////////////////////
template<typename... IntTypes, std::size_t... Indices>
struct traversal_order<std::size_t, tuple<IntTypes...>,
                       index_sequence<Indices...>>
{
  static std::size_t apply(std::size_t src)
  {
    return src;
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Rearrange the contents of a tuple to be in the reverse
///        traversal order.
/// @tparam Tuple Type of tuple to rearrange.
/// @tparam Travesal Traversal ordering.
///
/// For example, given the tuple (x, y, z) and the traversal <1, 2, 0>,
/// output the tuple (y, z, x).
//////////////////////////////////////////////////////////////////////
template<typename Tuple, typename Traversal,
         typename Indices = make_index_sequence<tuple_size<Tuple>::value>>
struct reverse_traversal_order;

template<typename Tuple, typename Traversal, std::size_t... Indices>
struct reverse_traversal_order<Tuple, Traversal, index_sequence<Indices...>>
{
  static Tuple apply(Tuple const& src)
  {
    return Tuple(get<tuple_element<Indices, Traversal>::type::value>(src)...);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Computes the modulus of a 1-dimensional number with the
///        linear size of all of the dimensions before the current
///        dimension @p N.
//////////////////////////////////////////////////////////////////////
template<int N>
struct apply_modulo
{
  template<class Tuple>
  static size_t apply(Tuple const& sizes, size_t result)
  {
    return apply_modulo<N-1>::apply(
      tuple_ops::pop_back(sizes),
      result % tuple_ops::fold(sizes, 1, stapl::multiplies<size_t>()));
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Base case @ref apply_modulo.
//////////////////////////////////////////////////////////////////////
template<>
struct apply_modulo<0>
{
  template<class Tuple>
  static size_t apply(Tuple const&, size_t result)
  { return result; }
};

//////////////////////////////////////////////////////////////////////
/// @brief Divides a given number by all of the elements of a tuple.
///
/// Start with N = N-1 and sizes in traversal order
//////////////////////////////////////////////////////////////////////
template<int N>
struct apply_divide
{
  template<class Tuple>
  static size_t apply(Tuple const& sizes, size_t result)
  {
    return apply_divide<N-1>::apply(sizes, result / get<N>(sizes));
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Base case @ref apply_divide.
//////////////////////////////////////////////////////////////////////
template<>
struct apply_divide<-1>
{
  template<class Tuple>
  static size_t apply(Tuple const&, size_t result)
  { return result; }
};

//////////////////////////////////////////////////////////////////////
/// @brief Translates a 1-dimensional linearization back to its
///        n-dimensional form.
///
/// This functor assumes that the sizes are in the correct traversal
/// order.
//////////////////////////////////////////////////////////////////////
template<int I, typename GID>
struct reverse_linearize
{
  static GID apply(GID const& m_size, size_t linear, GID gid)
  {
    size_t result =
      apply_modulo<tuple_size<GID>::value-1-I>::apply(
        tuple_ops::pop_back(m_size), linear);

    result = apply_divide<I-1>::apply(m_size, result);

    get<I>(gid) = result;

    return reverse_linearize<I-1, GID>::apply(m_size, linear, gid);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Base case of recursion for @ref reverse_linearize.
/// @note Intel fails if function operator takes @p gid by reference.
//////////////////////////////////////////////////////////////////////
template<typename GID>
struct reverse_linearize<-1, GID>
{
  static GID apply(GID const&, size_t, GID gid)
  { return gid; }
};

//////////////////////////////////////////////////////////////////////
/// @brief Small function to optimize index linearization by not
/// performing multiplication on the first element.
//////////////////////////////////////////////////////////////////////
template<int Idx>
struct conditional_multiply
{
  template<typename Tuple>
  static size_t apply(Tuple const& plane_sizes, size_t lhs)
  {
    return lhs * get<Idx>(plane_sizes);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Specialization for first element of tuple.  Do not perform
/// multiplication and assert the first element in plane_sizes is 1.
//////////////////////////////////////////////////////////////////////
template<>
struct conditional_multiply<0>
{
  template<typename Tuple>
  static size_t apply(Tuple const& plane_sizes, size_t lhs)
  {
    stapl_assert(get<0>(plane_sizes) == 1, "found last idx != 1");
    return lhs;
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Reorders input Indices according to provided traversal order,
/// decrements each by corresponding value in @p first and then multiplies
/// by corresponding value in @p plane_sizes, accumulating the results into
/// a linearized index.
//////////////////////////////////////////////////////////////////////
template <int Iterate, int LastIdx, typename Traversal>
struct reorder_localize_linearize
{
  template<typename Tuple, typename Index, typename... Indices>
  static size_t apply(Tuple const& first, Tuple const& plane_sizes,
                    Index&& i, Indices&&... is)

 {
   constexpr size_t idx = std::tuple_element<Iterate, Traversal>::type::value;

   return conditional_multiply<idx>::apply(
     plane_sizes, i - std::get<idx>(first))
     + reorder_localize_linearize<Iterate + 1, LastIdx, Traversal>::apply(
         first, plane_sizes, std::forward<Indices>(is)...);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Base case of the recursion.
/// @note Traversal of the tuple occurs from 0 to tuple_size-1 for performance
/// reasons.
//////////////////////////////////////////////////////////////////////
template<int LastIdx, typename Traversal>
struct reorder_localize_linearize<LastIdx, LastIdx, Traversal>
{
  template<typename Tuple, typename Index>
  static size_t apply(Tuple const& first, Tuple const& plane_sizes, Index&& i)
  {
    constexpr size_t idx = std::tuple_element<LastIdx, Traversal>::type::value;

    return conditional_multiply<idx>::apply(
      plane_sizes, i - std::get<idx>(first));
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Computes the factor by which a component of an n-dimensional GID
/// should be multiplied when linearizing the GID.
//////////////////////////////////////////////////////////////////////
template<int Iterate, int LastIdx>
struct compute_plane_sizes
{
  template <typename Tuple>
  static void apply(Tuple const& size, Tuple& plane_size)
  {
    // Other indices multiplied by size of all inner dimensions.
    std::get<Iterate>(plane_size) =
      std::get<Iterate-1>(size)*std::get<Iterate-1>(plane_size);
    compute_plane_sizes<Iterate+1, LastIdx>::apply(size, plane_size);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Specialization for first iteration of @ref compute_plane_sizes.
///
/// The first element represents the innermost dimension of the traversal,
/// and thus isn't multiplied by any of the other dimensions.
/// @note Traversal of the tuple occurs from 0 to tuple_size-1 for performance
/// reasons.
//////////////////////////////////////////////////////////////////////
template<int LastIdx>
struct compute_plane_sizes<0, LastIdx>
{
  template <typename Tuple>
  static void apply(Tuple const& size, Tuple& plane_size)
  {
    // Innermost dimension is multiplied by 1 when linearizing.
    std::get<0>(plane_size) = 1;
    compute_plane_sizes<1, LastIdx>::apply(size, plane_size);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Specialization for final iteration of @ref compute_plane_sizes.
///
/// The last element represents the outermost dimension of the traversal,
/// and thus is multiplied by the size of all inner dimensions when linearized.
/// @note Traversal of the tuple occurs from 0 to tuple_size-1 for performance
/// reasons.
//////////////////////////////////////////////////////////////////////
template<int LastIdx>
struct compute_plane_sizes<LastIdx, LastIdx>
{
  template <typename Tuple>
  static void apply(Tuple const& size, Tuple& plane_size)
  {
    // Plane size is size multiplied by size of all inner dimensions.
    std::get<LastIdx>(plane_size) =
      std::get<LastIdx-1>(size)*std::get<LastIdx-1>(plane_size);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Specialization for the 1-D case.
//////////////////////////////////////////////////////////////////////
template<>
struct compute_plane_sizes<0, 0>
{
  template <typename Tuple>
  static void apply(Tuple const& size, Tuple& plane_size)
  {
    // Innermost dimension is multiplied by 1 when linearizing.
    std::get<0>(plane_size) = 1;
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Type metafunction reflecting the reduced rank linearizer
/// that a slice operation returns.
//////////////////////////////////////////////////////////////////////
template<std::size_t SliceIndex, typename Traversal, typename GID>
struct slice_result
{
  using slices_tuple_t =
    typename tuple_ops::from_index_sequence<index_sequence<SliceIndex>>::type;

  using new_gid_t =
    typename tuple_ops::result_of::heterogeneous_discard<
      slices_tuple_t, GID>::type;

  using new_traversal_t =
    typename tuple_ops::result_of::heterogeneous_discard<
      slices_tuple_t, Traversal>::type;

  using type = nd_linearize<new_gid_t, new_traversal_t>;
};

template<typename Slices, typename Traversal,
         typename = make_index_sequence<tuple_size<Slices>::value>>
struct indices_of_most_significant;

//////////////////////////////////////////////////////////////////////
/// @brief Metafunction to compute the reordering of a Slices tuple (as a tuple
///        of indices) in descending order of Traversal's most significant
///        (slowest moving) dimensions.
///
///        When given multiple dimensions to slice at a time, we need to slice
///        them in the order of the largest to smallest (where largest and
///        smallest refers to their positions in the real traversal ordering).
///
///        For example, if Slices is <1,2,3> and Traversal is <0,2,4,3,1> then
///        the output would be <2,0,1>.
///
///        This is because we compute:
///          <4-Tr<Sl<0>>, 4-Tr<Sl<1>>, 4-Tr<Sl<2>>> =
///          <4-Tr<1>, 4-Tr<2>, 4-Tr<3>> =
///          <4-2, 4-4, 4-3> =
///          <2,0,1>
///
///        The meaning of <2,0,1> in this case means we need to slice position
///        1 first, then position 2, and then position 0. This boils down to
///        calling slice<2> and then slice<3> and then slice<1>. This is
///        necessary because in actuality, we are slicing the real dimensions of
///        4 then 3 then 2 of the traversal, which are the most significant
///        dimensions descending in order.
///
/// @note  The result of this metafunction only has meaning if the input slices
///        indeed refer to the last |Slices| dimensions of the linearizer and
///        it is only the order that is unknown.
//////////////////////////////////////////////////////////////////////
template<typename Slices, typename Traversal, std::size_t... Indices>
struct indices_of_most_significant<Slices, Traversal,
                                   index_sequence<Indices...>>
{
  static constexpr std::size_t traversal_size = tuple_size<Traversal>::value-1;

  using type = tuple<std::integral_constant<std::size_t,
    traversal_size - tuple_element<
      tuple_element<Indices, Slices>::type::value, Traversal>::type::value
  >...>;
};

//////////////////////////////////////////////////////////////////////
/// @brief Compute what the rest of the slices should be based on the current
///        slice. For example, if the incoming slices for slice() are
///        <0,1,2>, we would need to recursively call slice<0>.slice<0>.slice<0>
///        because we are now referring to the dimensions which are one less.
///        However, if the incoming slices are <2,1,0>, we would instead call
///        slice<2>.slice<1>.slice<0>.
//////////////////////////////////////////////////////////////////////
constexpr std::size_t adjusted_slice(std::size_t FirstSlice,
                                     std::size_t SecondSlice)
{
  return SecondSlice > FirstSlice ? SecondSlice-1 : SecondSlice;
}

} // namespace detail

//////////////////////////////////////////////////////////////////////
/// @brief Translates an n-dimensional @p gid to a 1-dimensional
///        linearization, based on the given @p Traversal order.
//////////////////////////////////////////////////////////////////////
template<typename GID, typename Traversal>
struct nd_linearize
{
public:
  using traversal_type = Traversal;
  using index_type     = GID;
  using gid_type       = size_t;
  using size_type      = index_type;
  using inverse        = nd_reverse_linearize<GID, Traversal>;
  using is_bijective   = std::true_type;

  static constexpr size_t last_idx = tuple_size<size_type>::value - 1;

  size_type         m_size;

  size_type         m_plane_sizes;
  size_type         m_first;

  /// @brief Original unreordered size kept to make slicing logic easier.
  size_type         m_original_size;

  /// @brief Original unreordered first index kept to make slicing logic easier.
  size_type         m_original_first;

public:
  nd_linearize(void) = default;

  explicit
  nd_linearize(size_type size)
    : m_size(detail::traversal_order<GID, Traversal>::apply(size)), m_first(),
      m_original_size(size), m_original_first()
  {
    detail::compute_plane_sizes<0, last_idx>::apply(m_size, m_plane_sizes);
  }

  explicit
  nd_linearize(size_type size, size_type first)
    : m_size(detail::traversal_order<GID, Traversal>::apply(size)),
      m_first(detail::traversal_order<GID, Traversal>::apply(first)),
      m_original_size(size), m_original_first(first)
  {
    detail::compute_plane_sizes<0, last_idx>::apply(m_size, m_plane_sizes);
  }

  explicit
  nd_linearize(nd_reverse_linearize<GID, Traversal> const& other)
    : m_size(other.m_size), m_first()
  {
    detail::compute_plane_sizes<0, last_idx>::apply(m_size, m_plane_sizes);
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Slice a single index of the multidimensional space, fixing
  /// its value to that of the passed @p idx parameters. Return an offset
  /// representing the effect of the slicing operation and a reduced rank
  /// linearizer for the remaining indices.
  ///
  /// @todo Consider storing offset internally in @ref nd_linearize to
  /// better abstract behavior.
  //////////////////////////////////////////////////////////////////////
  template<std::size_t SliceIndex>
  std::pair<
    std::size_t, typename detail::slice_result<SliceIndex,Traversal,GID>::type
  >
  slice(size_t const& idx)
  {
    constexpr size_t mapped_index =
      tuple_element<SliceIndex, Traversal>::type::value;

    static_assert(mapped_index == last_idx,
                  "Slicing limited to last in traversal order");

    const size_t multiplier  = get<mapped_index>(m_plane_sizes);
    const std::size_t offset = (idx - get<mapped_index>(m_first)) * multiplier;

    using slice_result_t = detail::slice_result<SliceIndex,Traversal,GID>;
    using SlicesTuple    = typename slice_result_t::slices_tuple_t;
    using NewGID         = typename slice_result_t::new_gid_t;
    using NewTraversal   = typename slice_result_t::new_traversal_t;

    return std::make_pair(
      offset,
      nd_linearize<NewGID, NewTraversal>(
        tuple_ops::heterogeneous_discard<SlicesTuple>(m_original_size),
        tuple_ops::heterogeneous_discard<SlicesTuple>(m_original_first)));
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Slice multiple indices of the multidimensional space.
  /// Recursively called with single @p slice signature above as base case.
  //////////////////////////////////////////////////////////////////////
  template<std::size_t FirstSlice, std::size_t SecondSlice,
           std::size_t... Slices, typename Fixed>
    std::pair<std::size_t,
              decltype(
                std::declval<
                  typename detail::slice_result<
                    FirstSlice, Traversal, GID>::type
                >().template slice<
                    detail::adjusted_slice(FirstSlice, SecondSlice),
                    detail::adjusted_slice(FirstSlice, Slices)...
                  >(
                  tuple_ops::heterogeneous_discard<
                    tuple<std::integral_constant<size_t, 0>>
                  >(std::declval<Fixed>())).second)>
  slice(Fixed const& fixed)
  {
    using discard_tuple_t = tuple<std::integral_constant<size_t, 0>>;

    auto p = slice<FirstSlice>(get<0>(fixed));

    auto q = (p.second).template slice<
      detail::adjusted_slice(FirstSlice, SecondSlice),
      detail::adjusted_slice(FirstSlice, Slices)...>(
        tuple_ops::heterogeneous_discard<discard_tuple_t>(fixed)
      );

    return std::make_pair(p.first + q.first, q.second);
  }

private:
  //////////////////////////////////////////////////////////////////////
  /// @brief Slice multiple indices of the multidimensional space.
  ///    Specialization when Slices is an index_sequence
  //////////////////////////////////////////////////////////////////////
  template<std::size_t... Slices, typename Fixed>
  auto slice_impl(index_sequence<Slices...>, Fixed const& fixed)
    -> decltype(this->template slice<Slices...>(fixed))
  {
    return this->template slice<Slices...>(fixed);
  }

public:

  //////////////////////////////////////////////////////////////////////
  /// @brief Slice multiple indices of the multidimensional space.
  ///        Specialization when Slices is a tuple of integral constants
  ///
  ///        When slicing multiple indices at a time, we first need to rearrange
  ///        the slice dimensions to match the most significant dimensions of
  ///        the traversal.
  ///
  /// @tparam Slices Tuple of integral constants
  /// @param fixed The indices to fix in the slice
  //////////////////////////////////////////////////////////////////////
  template<typename Slices, typename Fixed>
  auto slice(Fixed const& fixed) ->
    decltype(this->slice_impl(typename tuple_ops::to_index_sequence<
      typename tuple_ops::result_of::rearrange<Slices,
        typename detail::indices_of_most_significant<Slices, Traversal>::type
      >::type
    >::type(), fixed))
  {
    // Rearrange the tuple of fixed values
    auto reordered_fixed = detail::traversal_order<Fixed,
      typename detail::indices_of_most_significant<Slices, Traversal>::type
    >::apply(fixed);

    // Rearrange the compile-time tuple of slices
    using reordered_slices = typename tuple_ops::to_index_sequence<
      typename tuple_ops::result_of::rearrange<
        Slices,
        typename detail::indices_of_most_significant<Slices, Traversal>::type
      >::type
    >::type;

    return this->slice_impl(reordered_slices(), reordered_fixed);
  }

public:
  //////////////////////////////////////////////////////////////////////
  /// @brief Localize and then linearize the indices that are the components
  /// of an n-dimensional GID.
  //////////////////////////////////////////////////////////////////////
  template <typename... Indices>
  gid_type operator()(Indices const&... is) const
  {
    return detail::reorder_localize_linearize<0, last_idx, Traversal>::apply(
      m_first, m_plane_sizes, is...);
  }

private:
  template<std::size_t... I>
  gid_type
  linearize_impl(index_type const& gid, index_sequence<I...> const&) const
  {
    return detail::reorder_localize_linearize<0, last_idx, Traversal>::apply(
      m_first, m_plane_sizes, std::get<I>(gid)...);
  }

public:
  //////////////////////////////////////////////////////////////////////
  /// Localize and then linearize the n-dimensional GID provided.
  //////////////////////////////////////////////////////////////////////
  template<typename Indices =
             make_index_sequence<tuple_size<index_type>::value>>
  gid_type operator()(index_type const& gid) const
  {
    return linearize_impl(gid, Indices());
  }

  void define_type(typer& t)
  {
    t.member(m_size);
    t.member(m_plane_sizes);
    t.member(m_first);
    t.member(m_original_size);
    t.member(m_original_first);
  }
}; // struct nd_linearize

//////////////////////////////////////////////////////////////////////
/// @brief Base case for @ref nd_linearize for 1-dimensional GID.
///
/// @todo Possibly use of the f_ident mapping function for this.
//////////////////////////////////////////////////////////////////////
template<typename Traversal>
struct nd_linearize<size_t, Traversal>
{
  using index_type = std::size_t;
  using traversal_type = Traversal;

  size_t m_size;
  size_t m_original_size;
  size_t m_original_first;
  size_t m_first;

  nd_linearize(void)
    : m_first(0)
  { }

  explicit
  nd_linearize(size_t)
    : m_first(0)
  { }

  explicit
  nd_linearize(size_t size, size_t first)
    : m_size(size), m_original_size(size), m_original_first(first),
      m_first(first)
  { }

  size_t operator()(size_t gid) const
  { return gid - m_first; }

  void define_type(typer& t)
  {
    t.member(m_size);
    t.member(m_original_size);
    t.member(m_original_first);
    t.member(m_first);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Translates a 1-dimensional linearized GID to its
///        n-dimensional GID, based on the given @p Traversal order.
//////////////////////////////////////////////////////////////////////
template<typename GID, typename Traversal>
struct nd_reverse_linearize
{
  typedef GID                               gid_type;
  typedef gid_type                          result_type;
  typedef std::size_t                       index_type;
  typedef gid_type                          size_type;
  typedef nd_linearize<GID, Traversal>      inverse;
  typedef std::true_type is_bijective;

  enum { last_idx = tuple_size<size_type>::value - 1 };

  size_type m_size;

  //////////////////////////////////////////////////////////////////////
  /// @todo Is the default constructor needed? if it is size should be
  ///       correctly initialized (default constructed in domain.hpp).
  //////////////////////////////////////////////////////////////////////
  nd_reverse_linearize(void) = default;

  explicit
  nd_reverse_linearize(size_type const& size)
    : m_size(detail::traversal_order<gid_type, Traversal>::apply(size))
  { }

  explicit
  nd_reverse_linearize(nd_linearize<GID, Traversal> const& other)
    : m_size(other.m_size)
  { }

  //////////////////////////////////////////////////////////////////////
  /// @todo merge calls to inverse_linearizer_t::operator() and
  /// inverse_index_order_t::operator() into a single call
  /// to minimize the number of traversals of the tuple.
  //////////////////////////////////////////////////////////////////////
  gid_type operator()(index_type linear) const
  {
    return detail::reverse_traversal_order<gid_type, Traversal>::apply(
      detail::reverse_linearize<last_idx, gid_type>::apply(
        m_size, linear, gid_type())
    );
  }

  void define_type(typer& t)
  {
    t.member(m_size);
  }
}; // struct nd_reverse_linearize

//////////////////////////////////////////////////////////////////////
/// @brief Base case for @ref nd_reverse_linearize for 1-dimensional GID.
///
/// @todo Possible use of the f_ident mapping function for this.
//////////////////////////////////////////////////////////////////////
template<typename Traversal>
struct nd_reverse_linearize<size_t, Traversal>
{
  nd_reverse_linearize(void) = default;

  explicit
  nd_reverse_linearize(size_t)
  { }

  size_t operator()(size_t linear) const
  { return linear; }
};

} // namespace stapl

#endif // STAPL_MAPFUNCS_LINEARIZATION_HPP


#ifndef SKYLARK_GEMM_HPP
#define SKYLARK_GEMM_HPP

#include <boost/mpi.hpp>
#include "exception.hpp"
#include "sparse_matrix.hpp"
#include "computed_matrix.hpp"
#include "../utility/typer.hpp"

#include "Gemm_detail.hpp"

// Defines a generic Gemm function that receives both dense and sparse matrices.

namespace skylark { namespace base {

/**
 * Rename the elemental Gemm function, so that we have unified access.
 */

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::Matrix<T>& A, const elem::Matrix<T>& B,
    T beta, elem::Matrix<T>& C) {
    elem::Gemm(oA, oB, alpha, A, B, beta, C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::Matrix<T>& A, const elem::Matrix<T>& B,
    elem::Matrix<T>& C) {
    elem::Gemm(oA, oB, alpha, A, B, C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::STAR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    T beta, elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {
    elem::Gemm(oA, oB, alpha, A.LockedMatrix(), B.LockedMatrix(), beta, C.Matrix());
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::STAR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {
    elem::Gemm(oA, oB, alpha, A.LockedMatrix(), B.LockedMatrix(), C.Matrix());
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T>& A, const elem::DistMatrix<T>& B,
    T beta, elem::DistMatrix<T>& C) {
    elem::Gemm(oA, oB, alpha, A, B, beta, C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T>& A, const elem::DistMatrix<T>& B,
    elem::DistMatrix<T>& C) {
    elem::Gemm(oA, oB, alpha, A, B, C);
}

/**
 * The following combinations is not offered by Elemental, but is useful for us.
 * We implement it partially.
 */

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VC, elem::STAR>& B,
    T beta, elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {
    // TODO verify sizes etc.

    if ((oA == elem::TRANSPOSE || oA == elem::ADJOINT) && oB == elem::NORMAL) {
        boost::mpi::communicator comm(C.Grid().Comm(), boost::mpi::comm_attach);
        elem::Matrix<T> Clocal(C.Matrix());
        elem::Gemm(oA, elem::NORMAL,
            alpha, A.LockedMatrix(), B.LockedMatrix(),
            beta / T(comm.size()), Clocal);
        boost::mpi::all_reduce(comm,
            Clocal.Buffer(), Clocal.MemorySize(), C.Matrix().Buffer(),
            std::plus<T>());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VC, elem::STAR>& B,
    elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {

    int C_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    int C_width = (oB == elem::NORMAL ? B.Width() : B.Height());
    elem::Zeros(C, C_height, C_width);
    base::Gemm(oA, oB, alpha, A, B, T(0), C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    T beta, elem::DistMatrix<T, elem::VC, elem::STAR>& C) {
    // TODO verify sizes etc.

    if (oA == elem::NORMAL && oB == elem::NORMAL) {
        elem::Gemm(elem::NORMAL, elem::NORMAL,
            alpha, A.LockedMatrix(), B.LockedMatrix(),
            beta, C.Matrix());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    elem::DistMatrix<T, elem::VC, elem::STAR>& C) {

    int C_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    int C_width = (oB == elem::NORMAL ? B.Width() : B.Height());
    elem::Zeros(C, C_height, C_width);
    base::Gemm(oA, oB, alpha, A, B, T(0), C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VR, elem::STAR>& B,
    T beta, elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {
    // TODO verify sizes etc.

    if ((oA == elem::TRANSPOSE || oA == elem::ADJOINT) && oB == elem::NORMAL) {
        boost::mpi::communicator comm(C.Grid().Comm(), boost::mpi::comm_attach);
        elem::Matrix<T> Clocal(C.Matrix());
        elem::Gemm(oA, elem::NORMAL,
            alpha, A.LockedMatrix(), B.LockedMatrix(),
            beta / T(comm.size()), Clocal);
        boost::mpi::all_reduce(comm,
            Clocal.Buffer(), Clocal.MemorySize(), C.Matrix().Buffer(),
            std::plus<T>());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VR, elem::STAR>& B,
    elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {

    int C_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    int C_width = (oB == elem::NORMAL ? B.Width() : B.Height());
    elem::Zeros(C, C_height, C_width);
    base::Gemm(oA, oB, alpha, A, B, T(0), C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    T beta, elem::DistMatrix<T, elem::VR, elem::STAR>& C) {
    // TODO verify sizes etc.

    if (oA == elem::NORMAL && oB == elem::NORMAL) {
        elem::Gemm(elem::NORMAL, elem::NORMAL,
            alpha, A.LockedMatrix(), B.LockedMatrix(),
            beta, C.Matrix());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    elem::DistMatrix<T, elem::VR, elem::STAR>& C) {

    int C_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    int C_width = (oB == elem::NORMAL ? B.Width() : B.Height());
    elem::Zeros(C, C_height, C_width);
    base::Gemm(oA, oB, alpha, A, B, T(0), C);
}

/**
 * Gemm between mixed elemental, sparse input. Output is dense elemental.
 */

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::Matrix<T>& A, const sparse_matrix_t<T>& B,
    T beta, elem::Matrix<T>& C) {
    // TODO verify sizes etc.

    const int* indptr = B.indptr();
    const int* indices = B.indices();
    const T *values = B.locked_values();

    int k = A.Width();
    int n = B.width();
    int m = A.Height();

    if (oA == elem::ADJOINT && std::is_same<T, elem::Base<T> >::value)
        oA = elem::TRANSPOSE;

    if (oB == elem::ADJOINT && std::is_same<T, elem::Base<T> >::value)
        oB = elem::TRANSPOSE;

    if (oA == elem::ADJOINT || oB == elem::ADJOINT)
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());

    // NN
    if (oA == elem::NORMAL && oB == elem::NORMAL) {

        elem::Scal(beta, C);

        elem::Matrix<T> Ac;
        elem::Matrix<T> Cc;

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for private(Cc, Ac)
#       endif
        for(int col = 0; col < n; col++) {
            elem::View(Cc, C, 0, col, m, 1);
            for (int j = indptr[col]; j < indptr[col + 1]; j++) {
                int row = indices[j];
                T val = values[j];
                elem::LockedView(Ac, A, 0, row, m, 1);
                elem::Axpy(alpha * val, Ac, Cc);
            }
        }
    }

    // NT
    if (oA == elem::NORMAL && oB == elem::TRANSPOSE) {

        elem::Scal(beta, C);

        elem::Matrix<T> Ac;
        elem::Matrix<T> Cc;

        // Now, we simply think of B has being in CSR mode...
        int row = 0;
        for(int row = 0; row < n; row++) {
            elem::LockedView(Ac, A, 0, row, m, 1);
#           if SKYLARK_HAVE_OPENMP
#           pragma omp parallel for private(Cc)
#           endif
            for (int j = indptr[row]; j < indptr[row + 1]; j++) {
                int col = indices[j];
                T val = values[j];
                elem::View(Cc, C, 0, col, m, 1);
                elem::Axpy(alpha * val, Ac, Cc);
            }
        }
    }

    // TN - TODO: Not tested!
    if (oA == elem::TRANSPOSE && oB == elem::NORMAL) {
        double *c = C.Buffer();
        int ldc = C.LDim();

        const double *a = A.LockedBuffer();
        int lda = A.LDim();

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for collapse(2)
#       endif
        for (int j = 0; j < n; j++)
            for(int row = 0; row < k; row++) {
                c[j * ldc + row] *= beta;
                 for (int l = indptr[j]; l < indptr[j + 1]; l++) {
                     int rr = indices[l];
                     T val = values[l];
                     c[j * ldc + row] += val * a[j * lda + rr];
                 }
            }
    }

    // TT - TODO: Not tested!
    if (oA == elem::TRANSPOSE && oB == elem::TRANSPOSE) {
        elem::Scal(beta, C);

        double *c = C.Buffer();
        int ldc = C.LDim();

        const double *a = A.LockedBuffer();
        int lda = A.LDim();

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for
#       endif
        for(int row = 0; row < k; row++)
            for(int rb = 0; rb < n; rb++)
                for (int l = indptr[rb]; l < indptr[rb + 1]; l++) {
                    int col = indices[l];
                    c[col * ldc + row] += values[l] * a[row * lda + rb];
                }
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const sparse_matrix_t<T>& A, const elem::Matrix<T>& B,
    T beta, elem::Matrix<T>& C) {
    // TODO verify sizes etc.

    const int* indptr = A.indptr();
    const int* indices = A.indices();
    const double *values = A.locked_values();

    int k = A.width();
    int n = B.Width();
    int m = B.Height();

    if (oA == elem::ADJOINT && std::is_same<T, elem::Base<T> >::value)
        oA = elem::TRANSPOSE;

    if (oB == elem::ADJOINT && std::is_same<T, elem::Base<T> >::value)
        oB = elem::TRANSPOSE;

    // NN
    if (oA == elem::NORMAL && oB == elem::NORMAL) {

        elem::Scal(beta, C);

        double *c = C.Buffer();
        int ldc = C.LDim();

        const double *b = B.LockedBuffer();
        int ldb = B.LDim();

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for
#       endif
        for(int i = 0; i < n; i++)
            for(int col = 0; col < k; col++)
                 for (int j = indptr[col]; j < indptr[col + 1]; j++) {
                     int row = indices[j];
                     T val = values[j];
                     c[i * ldc + row] += alpha * val * b[i * ldb + col];
                 }
    }

    // NT
    if (oA == elem::NORMAL && (oB == elem::TRANSPOSE || oB == elem::ADJOINT)) {

        elem::Scal(beta, C);

        elem::Matrix<T> Bc;
        elem::Matrix<T> BTr;
        elem::Matrix<T> Cr;

        for(int col = 0; col < k; col++) {
            elem::LockedView(Bc, B, 0, col, m, 1);
            elem::Transpose(Bc, BTr, oB == elem::ADJOINT);
#           if SKYLARK_HAVE_OPENMP
#           pragma omp parallel for private(Cr)
#           endif
            for (int j = indptr[col]; j < indptr[col + 1]; j++) {
                int row = indices[j];
                T val = values[j];
                elem::View(Cr, C, row, 0, 1, m);
                elem::Axpy(alpha * val, BTr, Cr);
            }
        }
    }

    // TN - TODO: Not tested!
    if (oA == elem::TRANSPOSE && oB == elem::NORMAL) {
        double *c = C.Buffer();
        int ldc = C.LDim();

        const double *b = B.LockedBuffer();
        int ldb = B.LDim();

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for collapse(2)
#       endif
        for (int j = 0; j < n; j++)
            for(int row = 0; row < k; row++) {
                c[j * ldc + row] *= beta;
                 for (int l = indptr[row]; l < indptr[row + 1]; l++) {
                     int col = indices[l];
                     T val = values[l];
                     c[j * ldc + row] += val * b[j * ldb + col];
                 }
            }
    }

    // AN - TODO: Not tested!
    if (oA == elem::ADJOINT && oB == elem::NORMAL) {
        double *c = C.Buffer();
        int ldc = C.LDim();

        const double *b = B.LockedBuffer();
        int ldb = B.LDim();

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for collapse(2)
#       endif
        for (int j = 0; j < n; j++)
            for(int row = 0; row < k; row++) {
                c[j * ldc + row] *= beta;
                 for (int l = indptr[row]; l < indptr[row + 1]; l++) {
                     int col = indices[l];
                     T val = elem::Conj(values[l]);
                     c[j * ldc + row] += val * b[j * ldb + col];
                 }
            }
    }

    // TT - TODO: Not tested!
    if (oA == elem::TRANSPOSE && (oB == elem::TRANSPOSE || oB == elem::ADJOINT)) {

        elem::Scal(beta, C);

        elem::Matrix<T> Bc;
        elem::Matrix<T> BTr;
        elem::Matrix<T> Cr;

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for private(Cr, Bc, BTr)
#       endif
        for(int row = 0; row < k; row++) {
            elem::View(Cr, C, row, 0, 1, m);
            for (int l = indptr[row]; l < indptr[row + 1]; l++) {
                int col = indices[l];
                T val = values[l];
                elem::LockedView(Bc, B, 0, col, m, 1);
                elem::Transpose(Bc, BTr, oB == elem::ADJOINT);
                elem::Axpy(alpha * val, BTr, Cr);
            }
        }
    }

    // AT - TODO: Not tested!
    if (oA == elem::ADJOINT && (oB == elem::TRANSPOSE || oB == elem::ADJOINT)) {

        elem::Scal(beta, C);

        elem::Matrix<T> Bc;
        elem::Matrix<T> BTr;
        elem::Matrix<T> Cr;

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for private(Cr, Bc, BTr)
#       endif
        for(int row = 0; row < k; row++) {
            elem::View(Cr, C, row, 0, 1, m);
            for (int l = indptr[row]; l < indptr[row + 1]; l++) {
                int col = indices[l];
                T val = elem::Conj(values[l]);
                elem::LockedView(Bc, B, 0, col, m, 1);
                elem::Transpose(Bc, BTr, oB == elem::ADJOINT);
                elem::Axpy(alpha * val, BTr, Cr);
            }
        }
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const sparse_matrix_t<T>& A, const elem::Matrix<T>& B,
    elem::Matrix<T>& C) {
    int C_height = (oA == elem::NORMAL ? A.height() : A.width());
    int C_width = (oB == elem::NORMAL ? B.Width() : B.Height());
    elem::Zeros(C, C_height, C_width);
    base::Gemm(oA, oB, alpha, A, B, T(0), C);
}

#if SKYLARK_HAVE_COMBBLAS
/**
 * Mixed GEMM for Elemental and CombBLAS matrices. For a distributed Elemental
 * input matrix, the output has the same distribution.
 */

/// Gemm for distCombBLAS x distElemental(* / *) -> distElemental (SOMETHING / *)
template<typename index_type, typename value_type, elem::Distribution col_d>
void Gemm(elem::Orientation oA, elem::Orientation oB, double alpha,
          const SpParMat<index_type, value_type, SpDCCols<index_type, value_type> > &A,
          const elem::DistMatrix<value_type, elem::STAR, elem::STAR> &B,
          double beta,
          elem::DistMatrix<value_type, col_d, elem::STAR> &C) {

    if(oA == elem::NORMAL && oB == elem::NORMAL) {

        if(A.getnol() != B.Height())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(A.getnrow() != C.Height())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(B.Width() != C.Width())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        //XXX: simple heuristic to decide what to communicate (improve!)
        //     or just if A.getncol() < B.Width..
        if(A.getnnz() < B.Height() * B.Width())
            detail::outer_panel_mixed_gemm_impl_nn(alpha, A, B, beta, C);
        else
            detail::inner_panel_mixed_gemm_impl_nn(alpha, A, B, beta, C);
    }
}

/// Gemm for distCombBLAS x distElemental(SOMETHING / *) -> distElemental (* / *)
template<typename index_type, typename value_type, elem::Distribution col_d>
void Gemm(elem::Orientation oA, elem::Orientation oB, double alpha,
          const SpParMat<index_type, value_type, SpDCCols<index_type, value_type> > &A,
          const elem::DistMatrix<value_type, col_d, elem::STAR> &B,
          double beta,
          elem::DistMatrix<value_type, elem::STAR, elem::STAR> &C) {

    if(oA == elem::TRANSPOSE && oB == elem::NORMAL) {

        if(A.getrow() != B.Height())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(A.getncol() != C.Height())
            SKYLARK_THROW_EXCEPTION (
                    base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(B.Width() != C.Width())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        detail::outer_panel_mixed_gemm_impl_tn(alpha, A, B, beta, C);
    }

}

#endif // SKYLARK_HAVE_COMBBLAS

/* All combinations with computed matrix */

template<typename CT, typename RT, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const computed_matrix_t<CT>& A,
    const RT& B, typename utility::typer_t<OT>::value_type beta, OT& C) {
    base::Gemm(oA, oB, alpha, A.materialize(), B, beta, C);
}

template<typename CT, typename RT, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const computed_matrix_t<CT>& A,
    const RT& B, OT& C) {
    base::Gemm(oA, oB, alpha, A.materialize(), B, C);
}

template<typename CT, typename RT, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const RT& A,
    const computed_matrix_t<CT>& B,
    typename utility::typer_t<OT>::value_type beta, OT& C) {
    base::Gemm(oA, oB, alpha, A, B.materialize(), beta, C);
}

template<typename CT, typename RT, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const RT& A,
    const computed_matrix_t<CT>& B, OT& C) {
    base::Gemm(oA, oB, alpha, A, B.materialize(), C);
}

template<typename CT1, typename CT2, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const computed_matrix_t<CT1>& A,
    const computed_matrix_t<CT2>& B, typename utility::typer_t<OT>::value_type beta,
    OT& C) {
    base::Gemm(oA, oB, alpha, A.materialize(), B.materialize(), beta, C);
}

template<typename CT1, typename CT2, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const computed_matrix_t<CT1>& A,
    const computed_matrix_t<CT2>& B, OT& C) {
    base::Gemm(oA, oB, alpha, A.materialize(), B.materialize(), C);
}

} } // namespace skylark::base
#endif // SKYLARK_GEMM_HPP