/*
 * 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_ROTATIONMATRIX_HPP_
#define KINDR_ROTATIONS_EIGEN_ROTATIONMATRIX_HPP_

#include <cmath>

#include <Eigen/Geometry>

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

namespace kindr {
namespace rotations {
namespace eigen_impl {

/*! \class RotationMatrix
 *  \brief Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
 *
 *  The following four typedefs are provided for convenience:
 *   - \ref eigen_impl::RotationMatrixAD "RotationMatrixAD" for active rotation and double primitive type
 *   - \ref eigen_impl::RotationMatrixAF "RotationMatrixAF" for active rotation and float primitive type
 *   - \ref eigen_impl::RotationMatrixPD "RotationMatrixPD" for passive rotation and double primitive type
 *   - \ref eigen_impl::RotationMatrixPF "RotationMatrixPF" for passive rotation and float primitive type
 *
 *  \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 RotationMatrix : public RotationMatrixBase<RotationMatrix<PrimType_, Usage_>, Usage_>, private Eigen::Matrix<PrimType_, 3, 3> {
 private:
  /*! \brief The base type.
   */
  typedef Eigen::Matrix<PrimType_, 3, 3> Base;
 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef Base Implementation;
  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;

  /*! \brief Default constructor using identity rotation.
   */
  RotationMatrix()
    : Base(Base::Identity()) {
  }

  /*! \brief Constructor using nine scalars.
   *  In debug mode, an assertion is thrown if the matrix is not a rotation matrix.
   *  \param r11     entry in row 1, col 1
   *  \param r12     entry in row 1, col 2
   *  \param r13     entry in row 1, col 3
   *  \param r21     entry in row 2, col 1
   *  \param r22     entry in row 2, col 2
   *  \param r23     entry in row 2, col 3
   *  \param r31     entry in row 3, col 1
   *  \param r32     entry in row 3, col 2
   *  \param r33     entry in row 3, col 3
   */
  RotationMatrix(Scalar r11, Scalar r12, Scalar r13,
                 Scalar r21, Scalar r22, Scalar r23,
                 Scalar r31, Scalar r32, Scalar r33) {
    if(Usage_ == RotationUsage::ACTIVE)
    {
      *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;
    } else {
      *this << r11,r21,r31,r12,r22,r32,r13,r23,r33;
    }
    KINDR_ASSERT_MATRIX_NEAR_DBG(std::runtime_error, this->toImplementation() * this->toImplementation().transpose(), Base::Identity(), static_cast<Scalar>(1e-4), "Input matrix is not orthogonal.");
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->determinant(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input matrix determinant is not 1.");
  }

  /*! \brief Constructor using Eigen::Matrix.
   *  In debug mode, an assertion is thrown if the rotation vector has not unit length.
   *  \param other   Eigen::Matrix<PrimType_,3,3>
   */
  explicit RotationMatrix(const Base& other)
  // : Base(other)
  {
    if(Usage_ == RotationUsage::ACTIVE)
    {
      this->toImplementation() = other;
    } else {
      this->toImplementation() = other.transpose();
    }
    KINDR_ASSERT_MATRIX_NEAR_DBG(std::runtime_error, other * other.transpose(), Base::Identity(), static_cast<Scalar>(1e-4), "Input matrix is not orthogonal.");
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, other.determinant(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input matrix determinant is not 1.");
  }

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

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

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

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  RotationMatrix inverted() const {
    RotationMatrix matrix;
    matrix.toImplementation() = this->toImplementation().transpose();
    return matrix;
  }

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

  /*! \brief Returns the transpose of the rotation matrix.
   *  \returns the inverse of the rotation
   */
  RotationMatrix transposed() const {
    RotationMatrix matrix;
    matrix.toImplementation() = this->toImplementation().transpose();
    return matrix;
  }

  /*! \brief Transposes the rotation matrix.
   *  \returns reference
   */
  RotationMatrix& transpose() {
    *this = this->transposed();
    return *this;
  }

  /*! \brief Returns the determinant of the rotation matrix.
   *  \returns determinant of the rotation matrix
   */
  Scalar determinant() const {
  return toImplementation().determinant();
  }

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

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

  /*! \brief Reading access to the rotation matrix.
   *  \returns rotation matrix (matrix) with reading access
   */
  inline Implementation matrix() const {
    if(Usage_ == RotationUsage::ACTIVE)
    {
      return this->toImplementation();
    } else {
      return this->toImplementation().transpose();
    }
  }

  /*! \brief  Writing access to the rotation matrix.
   */
  inline void setMatrix(const Implementation & input) {
    if(Usage_ == RotationUsage::ACTIVE)
    {
      this->toImplementation() = input;
    } else {
      this->toImplementation() = input.transpose();
    }
  }

  /*! \brief  Writing access to the rotation matrix.
   */
  inline void setMatrix(Scalar r11, Scalar r12, Scalar r13,
                        Scalar r21, Scalar r22, Scalar r23,
                        Scalar r31, Scalar r32, Scalar r33) {
    if(Usage_ == RotationUsage::ACTIVE)
    {
      *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;
    } else {
      *this << r11,r21,r31,r12,r22,r32,r13,r23,r33;
    }
  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  RotationMatrix& setIdentity() {
    this->Implementation::setIdentity();
    return *this;
  }

  /*! \brief Returns a unique matrix rotation.
   *  A rotation matrix is always unique.
   *  This function is used to compare different rotations.
   *  \returns copy of the matrix rotation which is unique
   */
  RotationMatrix getUnique() const {
    return *this;
  }

  /*! \brief Modifies the matrix rotation such that it becomes unique.
   *  A rotation matrix is always unique.
   *  \returns reference
   */
  RotationMatrix& setUnique() {
    return *this;
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using RotationMatrixBase<RotationMatrix<PrimType_, Usage_>, Usage_>::operator*; // otherwise ambiguous RotationBase and Eigen

  /*! \brief Equivalence operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator==.
   *  \returns true if two rotations are similar.
   */
  using RotationMatrixBase<RotationMatrix<PrimType_, Usage_>, Usage_>::operator==; // otherwise ambiguous RotationBase and Eigen

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const RotationMatrix& rotationMatrix) {
    if(Usage_ == RotationUsage::ACTIVE) {
      out << rotationMatrix.toImplementation();
    } else {
      out << rotationMatrix.inverted().toImplementation();
    }
    return out;
  }
};

//! \brief Active matrix rotation with double primitive type
typedef RotationMatrix<double, RotationUsage::ACTIVE>  RotationMatrixAD;
//! \brief Active matrix rotation with float primitive type
typedef RotationMatrix<float,  RotationUsage::ACTIVE>  RotationMatrixAF;
//! \brief Passive matrix rotation with double primitive type
typedef RotationMatrix<double, RotationUsage::PASSIVE> RotationMatrixPD;
//! \brief Passive matrix rotation with float primitive type
typedef RotationMatrix<float,  RotationUsage::PASSIVE> RotationMatrixPF;

} // namespace eigen_impl

namespace internal {

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

template<typename PrimType_, enum RotationUsage Usage_>
class get_matrix3X<eigen_impl::RotationMatrix<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::RotationMatrix<PrimType_, RotationUsage::ACTIVE>> {
 public:
  typedef eigen_impl::RotationMatrix<PrimType_, RotationUsage::PASSIVE> OtherUsage;
};

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

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

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationMatrix<DestPrimType_, Usage_>, eigen_impl::RotationVector<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationMatrix<DestPrimType_, Usage_> convert(const eigen_impl::RotationVector<SourcePrimType_, Usage_>& rotationVector) {
    typename eigen_impl::RotationMatrix<DestPrimType_, Usage_>::Implementation matrixdata;
    typedef typename eigen_impl::RotationVector<SourcePrimType_, Usage_>::Scalar Scalar;
    const  typename eigen_impl::RotationVector<DestPrimType_, Usage_>::Implementation rv = rotationVector.toImplementation().template cast<DestPrimType_>();
    const SourcePrimType_ v1 = rv.x();
    const SourcePrimType_ v2 = rv.y();
    const SourcePrimType_ v3 = rv.z();
    const SourcePrimType_ v = rv.norm();

    if (v < common::internal::NumTraits<Scalar>::dummy_precision())  {

      // active to passive
//      matrixdata << 1.0,  v3, -v2,
//                        -v3, 1.0,  v1,
//                          v2, -v1, 1.0;
      // active to active
      matrixdata << 1.0,  -v3, v2,
                    v3, 1.0,  -v1,
                   -v2, v1, 1.0;
    } else {
      // active rotation vector to active matrix // not matlab code (is transposed)
      const DestPrimType_ t3 = v*(1.0/2.0);
      const DestPrimType_ t2 = sin(t3);
      const DestPrimType_ t4 = cos(t3);
      const DestPrimType_ t5 = 1.0/(v*v);
      const DestPrimType_ t6 = t4*v*v3;
      const DestPrimType_ t7 = t2*v1*v2;
      const DestPrimType_ t8 = t2*t2;
      const DestPrimType_ t9 = v1*v1;
      const DestPrimType_ t10 = v2*v2;
      const DestPrimType_ t11 = v3*v3;
      const DestPrimType_ t12 = v*v;
      const DestPrimType_ t13 = t4*t4;
      const DestPrimType_ t14 = t12*t13;
      const DestPrimType_ t15 = t2*v1*v3;
      const DestPrimType_ t16 = t4*v*v1;
      const DestPrimType_ t17 = t2*v2*v3;
      matrixdata(0,0) = t5*(t14-t8*(-t9+t10+t11));
      matrixdata(1,0) = t2*t5*(t6+t7)*2.0;
      matrixdata(2,0) = t2*t5*(t15-t4*v*v2)*2.0;
      matrixdata(0,1) = t2*t5*(t6-t7)*-2.0;
      matrixdata(1,1) = t5*(t14-t8*(t9-t10+t11));
      matrixdata(2,1) = t2*t5*(t16+t17)*2.0;
      matrixdata(0,2) = t2*t5*(t15+t4*v*v2)*2.0;
      matrixdata(1,2) = t2*t5*(t16-t17)*-2.0;
      matrixdata(2,2) = t5*(t14-t8*(t9+t10-t11));

    }

    eigen_impl::RotationMatrix<DestPrimType_, Usage_> matrix;
    matrix.toImplementation() = matrixdata;
    return matrix;

  }
};

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

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

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

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

/*! \brief Multiplication of two rotation matrices
 */
template<typename PrimType_, enum RotationUsage Usage_>
class MultiplicationTraits<RotationBase<eigen_impl::RotationMatrix<PrimType_, Usage_>, Usage_>, RotationBase<eigen_impl::RotationMatrix<PrimType_, Usage_>, Usage_>> {
 public:
  inline static eigen_impl::RotationMatrix<PrimType_, Usage_> mult(const eigen_impl::RotationMatrix<PrimType_, Usage_>& lhs, const eigen_impl::RotationMatrix<PrimType_, Usage_>& rhs) {
    if(Usage_ == RotationUsage::ACTIVE) {
      eigen_impl::RotationMatrix<PrimType_, Usage_> result;
      result.toImplementation() = lhs.toImplementation() * rhs.toImplementation();
      return result;
    } else {
      eigen_impl::RotationMatrix<PrimType_, Usage_> result;
      result.toImplementation() = rhs.toImplementation() * lhs.toImplementation();
      return result;
    }
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
//template<typename PrimType_, enum RotationUsage Usage_>
//class RotationTraits<eigen_impl::RotationMatrix<PrimType_, Usage_>> {
// public:
//  template<typename get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>::IndexType Cols>
//  inline static typename get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>::template Matrix3X<Cols> rotate(const eigen_impl::RotationMatrix<PrimType_, Usage_>& R, const typename get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>::template Matrix3X<Cols>& m){
//    return R.toImplementation() * m;
//  }
//};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Usage Conversion Traits - required?
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

template<typename PrimType_>
class UsageConversionTraits<eigen_impl::RotationMatrix<PrimType_, RotationUsage::PASSIVE>,RotationUsage::PASSIVE> {
 public:
  inline static typename get_other_usage<eigen_impl::RotationMatrix<PrimType_, RotationUsage::PASSIVE>>::OtherUsage getActive(const eigen_impl::RotationMatrix<PrimType_,RotationUsage::PASSIVE>& in) {
    return typename get_other_usage<eigen_impl::RotationMatrix<PrimType_, RotationUsage::PASSIVE>>::OtherUsage(in.inverted().toImplementation());
  }

  // getPassive() does not exist (on purpose)
};

template<typename PrimType_>
class UsageConversionTraits<eigen_impl::RotationMatrix<PrimType_, RotationUsage::ACTIVE>,RotationUsage::ACTIVE> {
 public:
  inline static typename get_other_usage<eigen_impl::RotationMatrix<PrimType_, RotationUsage::ACTIVE>>::OtherUsage getPassive(const eigen_impl::RotationMatrix<PrimType_, RotationUsage::ACTIVE>& in) {
    return typename get_other_usage<eigen_impl::RotationMatrix<PrimType_, RotationUsage::ACTIVE>>::OtherUsage(in.toImplementation());
  }

  // getActive() does not exist (on purpose)
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Box Operations - required?
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

//template<typename LeftPrimType_, typename RightPrimType_, enum RotationUsage Usage_>
//class BoxOperationTraits<RotationBase<eigen_impl::RotationMatrix<LeftPrimType_, Usage_>, Usage_>, RotationBase<eigen_impl::RotationMatrix<RightPrimType_, Usage_>, Usage_>> {
// public:
//  inline static typename internal::get_matrix3X<eigen_impl::RotationMatrix<LeftPrimType_, Usage_>>::template Matrix3X<1> box_minus(const eigen_impl::RotationMatrix<LeftPrimType_, Usage_>& lhs, const eigen_impl::RotationMatrix<RightPrimType_, Usage_>& rhs) {
//    return (lhs*rhs.inverted()).getLogarithmicMap();
//  }
//
//  inline static  eigen_impl::RotationMatrix<LeftPrimType_, Usage_> box_plus(const eigen_impl::RotationMatrix<RightPrimType_, Usage_>& rotation, const typename internal::get_matrix3X<eigen_impl::RotationMatrix<RightPrimType_, Usage_>>::template Matrix3X<1>& vector) {
//    return eigen_impl::RotationMatrix<LeftPrimType_, Usage_>((MapTraits<RotationBase<eigen_impl::RotationMatrix<RightPrimType_,Usage_>, Usage_>>::set_exponential_map(vector)).toImplementation()*rotation.toImplementation());
//  }
//};

//template<typename PrimType_, enum RotationUsage Usage_>
//class MapTraits<eigen_impl::RotationMatrix<PrimType_, Usage_>> {
// public:
//
//  inline static eigen_impl::RotationMatrix<PrimType_, Usage_> set_exponential_map(const typename internal::get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>::template Matrix3X<1>& vector) {
//    typedef typename get_scalar<eigen_impl::RotationMatrix<PrimType_, Usage_>>::Scalar Scalar;
//    if (Usage_ == RotationUsage::ACTIVE) {
//      return eigen_impl::RotationMatrix<PrimType_, Usage_>(eigen_impl::RotationVector<Scalar, Usage_>(vector));
//    }
//    if (Usage_ == RotationUsage::PASSIVE) {
//      return eigen_impl::RotationMatrix<PrimType_, Usage_>(eigen_impl::RotationVector<Scalar, Usage_>(vector));
//    }
//  }
//
////  inline static typename internal::get_matrix3X<Rotation_>::template Matrix3X<1> get_logarithmic_map(const Rotation_& rotation) {
////    typedef typename get_scalar<Rotation_>::Scalar Scalar;
////    eigen_impl::RotationVector<Scalar, Rotation_::Usage> rotationVector(rotation);
////    return rotationVector.getUnique().toImplementation();
////  }
//
//  inline static typename internal::get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>::template Matrix3X<1> get_logarithmic_map(const eigen_impl::RotationMatrix<PrimType_, Usage_>& rotation) {
//    typedef typename get_scalar<eigen_impl::RotationMatrix<PrimType_, Usage_>>::Scalar Scalar;
//
//
//
//    if (Usage_ == RotationUsage::ACTIVE) {
//      return eigen_impl::RotationVector<Scalar, Usage_>(rotation).toImplementation();
//    }
//    if (Usage_ == RotationUsage::PASSIVE) {
//      return eigen_impl::RotationVector<Scalar, Usage_>(rotation).toImplementation();
//    }
//  }
//
//};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Fixing Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_, enum RotationUsage Usage_>
class FixingTraits<eigen_impl::RotationMatrix<PrimType_, Usage_>> {
 public:
  inline static void fix(eigen_impl::RotationMatrix<PrimType_, Usage_>& R) {
    const PrimType_ factor = 1/pow(R.determinant(), 1.0/3.0);
    R.setMatrix(factor*R.matrix()(0,0),
                factor*R.matrix()(0,1),
                factor*R.matrix()(0,2),
                factor*R.matrix()(1,0),
                factor*R.matrix()(1,1),
                factor*R.matrix()(1,2),
                factor*R.matrix()(2,0),
                factor*R.matrix()(2,1),
                factor*R.matrix()(2,2));
  }
};

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

#endif /* KINDR_ROTATIONS_EIGEN_ROTATIONMATRIX_HPP_ */


// Localization display for RViz
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include "loc_display.h"

#include <pluginlib/class_list_macros.h>
#include <rviz/ogre_helpers/shape.h>
#include <rviz/display_context.h>
#include <ros/package.h>
#include <rviz/frame_manager.h>
#include <rviz/ogre_helpers/arrow.h>

#include <field_model/field_model.h>
#include <OGRE/OgreEntity.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreMaterialManager.h>

#include <boost/foreach.hpp>

namespace loc_display
{

LocDisplay::LocDisplay()
{
	ROS_INFO("Adding ogre path: %s", (ros::package::getPath("loc_display") + "/ogre/scripts").c_str());
	Ogre::ResourceGroupManager::getSingleton().addResourceLocation(ros::package::getPath("loc_display") + "/ogre/scripts", "FileSystem", "loc_display");
	Ogre::ResourceGroupManager::getSingleton().initialiseResourceGroup("loc_display");

	m_frame_property = new rviz::TfFrameProperty(
		"Reference Frame", rviz::TfFrameProperty::FIXED_FRAME_STRING,
		"The TF frame the field will use for its origin.",
		this, 0, true
	);
}

LocDisplay::~LocDisplay()
{
}

void LocDisplay::onInitialize()
{
	rviz::Display::onInitialize();

	m_frame_property->setFrameManager(context_->getFrameManager());

	field_model::FieldModel* field = field_model::FieldModel::getInstance();

	rviz::Shape* plane = new rviz::Shape(rviz::Shape::Cube, scene_manager_, scene_node_);

	const double MARGIN = field->boundary();
	plane->setScale(Ogre::Vector3(field->length() + 2.0*MARGIN, field->width() + 2.0*MARGIN, 0.01));
	plane->setPosition(Ogre::Vector3(0.0, 0.0, -0.005));
	Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().getByName("Soccer/Field", "loc_display");
	if(mat.isNull())
		ROS_ERROR("Could not find field material");

	plane->getEntity()->setMaterial(mat);

	const std::vector<field_model::WorldObject>& lines = field->objects(field_model::WorldObject::Type_FieldLine);

	for(size_t i = 0; i < lines.size(); ++i)
	{
		const field_model::WorldObject& obj = lines[i];

		Eigen::Vector2d mid = (obj.points()[0] + obj.points()[1]) / 2.0;
		Eigen::Vector2d dir = obj.points()[1] - obj.points()[0];

		Ogre::Quaternion rot;
		rot.FromAngleAxis(Ogre::Radian(atan2(dir.y(), dir.x())), Ogre::Vector3::UNIT_Z);

		rviz::Shape* line = new rviz::Shape(rviz::Shape::Cube, scene_manager_, scene_node_);
		line->setPosition(Ogre::Vector3(mid.x(), mid.y(), -0.005+0.001));
		line->setOrientation(rot);
		line->setScale(Ogre::Vector3(dir.norm() + 0.05, 0.05, 0.01));
		line->getEntity()->setMaterialName("Soccer/Field/Line", "loc_display");
	}

	const std::vector<field_model::WorldObject>& posts = field->objects(field_model::WorldObject::Type_GoalPost);
	Ogre::Quaternion post_rot;
	post_rot.FromAngleAxis(Ogre::Radian(M_PI/2.0), Ogre::Vector3::UNIT_X);
	for(size_t i = 0; i < posts.size(); ++i)
	{
		const field_model::WorldObject& obj = posts[i];

		const double HEIGHT = 1.0;

		rviz::Shape* post = new rviz::Shape(rviz::Shape::Cylinder, scene_manager_, scene_node_);
		post->setPosition(Ogre::Vector3(obj.pose().x(), obj.pose().y(), HEIGHT/2.0));
		post->setScale(Ogre::Vector3(0.1, HEIGHT, 0.1));
		post->getEntity()->setMaterialName("Soccer/Field/GoalPost", "loc_display");
		post->setOrientation(post_rot);
	}

	// Center circle
	int CIRCLE_LINE_COUNT = 200;
	double r = field->centerCircleDiameter()/2.0;
	for(int i = 0; i < CIRCLE_LINE_COUNT; ++i)
	{
		rviz::Shape* line = new rviz::Shape(rviz::Shape::Cube, scene_manager_, scene_node_);

		double angle = i * 2.0 * M_PI / CIRCLE_LINE_COUNT;
		line->setPosition(Ogre::Vector3(cos(angle)*r, sin(angle)*r, -0.005+0.001));
		line->setScale(Ogre::Vector3(0.04, r * 2.0*M_PI / CIRCLE_LINE_COUNT + 0.001, 0.01));

		Ogre::Quaternion rot;
		rot.FromAngleAxis(Ogre::Radian(angle), Ogre::Vector3::UNIT_Z);
		line->setOrientation(rot);

		line->getEntity()->setMaterialName("Soccer/Field/Line", "loc_display");
	}

	addMarkers(field_model::WorldObject::Type_GoalPost);
	addMarkers(field_model::WorldObject::Type_LineXingL);
	addMarkers(field_model::WorldObject::Type_LineXingT);
	addMarkers(field_model::WorldObject::Type_LineXingX);
	addMarkers(field_model::WorldObject::Type_XMarker);

	// Coordinate system arrows
	const double ARROW_OFFSET = 0.1;
	const Ogre::Vector3 ARROW_ORIGIN(-field->length()/2.0 - MARGIN, -field->width()/2.0 - MARGIN - ARROW_OFFSET, 0.0);
	Ogre::Quaternion rot;

	rviz::Arrow* x_arrow = new rviz::Arrow(scene_manager_, scene_node_, field->length() + 2.0*MARGIN-0.3, 0.05, 0.27, 0.12);
	x_arrow->setPosition(ARROW_ORIGIN);
	rot.FromAngleAxis(Ogre::Radian(-M_PI/2.0), Ogre::Vector3::UNIT_Y);
	x_arrow->setOrientation(rot);
	x_arrow->setColor(1.0, 0.0, 0.0, 1.0);

// 	rviz::Arrow* y_arrow = new rviz::Arrow(scene_manager_, scene_node_, field->width());
// 	x_arrow->setPosition(ARROW_ORIGIN);
// 	rot.FromAngleAxis(Ogre::Radian(-M_PI/2.0), Ogre::Vector3::UNIT_Y);
// 	x_arrow->setOrientation(rot);

	// Table
	rviz::Shape* table = new rviz::Shape(rviz::Shape::Cube, scene_manager_, scene_node_);
	table->setScale(Ogre::Vector3(field->length() + 2.0 * MARGIN, 0.5, 0.6));
	table->setPosition(Ogre::Vector3(0.0, -field->width()/2.0 - MARGIN - 0.2 - 0.25, 0.3));
	table->setColor(0.2, 0.2, 0.2, 1.0);

	m_manual_object = scene_manager_->createManualObject();
	m_manual_object->setDynamic( true );
	scene_node_->attachObject( m_manual_object );

	scene_node_->setVisible(isEnabled());
}

void LocDisplay::addMarkers(field_model::WorldObject::Type type)
{
	const std::vector<field_model::WorldObject>& objs = field_model::FieldModel::getInstance()->objects(type);

	BOOST_FOREACH(const field_model::WorldObject& obj, objs)
	{
		rviz::Shape* cube = new rviz::Shape(rviz::Shape::Cube, scene_manager_, scene_node_);

		cube->setScale(Ogre::Vector3(0.05, 0.05, 0.05));
		cube->setPosition(Ogre::Vector3(obj.pose().x(), obj.pose().y(), 0.0));
		cube->getEntity()->setMaterialName("Soccer/Field/FloorMarker", "loc_display");
	}
}

void LocDisplay::update(float wall_dt, float ros_dt)
{
	QString qframe = m_frame_property->getFrame();
	std::string frame = qframe.toStdString();

	Ogre::Vector3 position;
	Ogre::Quaternion orientation;
	if( context_->getFrameManager()->getTransform( frame, ros::Time(), position, orientation ))
	{
		scene_node_->setPosition( position + Ogre::Vector3(0, 0, -0.001) );
		scene_node_->setOrientation( orientation );
		setStatus( rviz::StatusProperty::Ok, "Transform", "Transform OK" );
	}
	else
	{
		std::string error;
		if( context_->getFrameManager()->transformHasProblems( frame, ros::Time(), error ))
		{
			setStatus( rviz::StatusProperty::Error, "Transform", QString::fromStdString( error ));
		}
		else
		{
			setStatus( rviz::StatusProperty::Error, "Transform",
					   "Could not transform from [" + qframe + "] to [" + fixed_frame_ + "]" );
		}
	}
}

}

PLUGINLIB_EXPORT_CLASS(loc_display::LocDisplay, rviz::Display)


/*
    BSD 3-Clause License
    Copyright (c) 2018, Roboy
            All rights reserved.
    Redistribution and use in source and binary forms, with or without
            modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright notice, this
    list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.
    * Neither the name of the copyright holder nor the names of its
    contributors may be used to endorse or promote products derived from
    this software without specific prior written permission.
    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
            IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
            FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
            DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
            SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
            CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
    OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
    OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
    author: Simon Trendel ( st@gi.ai ), 2018
    description: A custom ros control command interface for CARDSflow robots
*/

#pragma once

#include <hardware_interface/internal/hardware_resource_manager.h>
#include <cassert>
#include <string>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

namespace hardware_interface {

    class CardsflowStateHandle {
    public:
        CardsflowStateHandle();

        /**
         * @param name The name of the joint
         * @param joint_index index of the joint in the robot model
         * @param pos joint position
         * @param vel joint velocity
         * @param acc joint acceleration
         * @param L pointer to the L matrix
         * @param M pointer to the mass matrix
         * @param CG pointer to the Coriolis+Gravity vector
        */
        CardsflowStateHandle(const std::string &name, int joint_index, const double *pos, const double *vel,
                             const double *acc, const MatrixXd *L, const MatrixXd *M, const VectorXd *CG);

        /**
         * Returns the joint name
         * @return joint name
         */
        std::string getName() const;

        /**
         * Returns the joint index in the robot model
         * @return joint index
         */
        int getJointIndex() const;

        /**
         * Returns the currect joint position
         * @return joint position
         */
        double getPosition() const;

        /**
         * Returns the currect joint velocity
         * @return joint velocity
         */
        double getVelocity() const;

        /**
         * Returns the currect joint acceleration
         * @return joint acceleration
         */
        double getAcceleration() const;

        /**
         * Returns the cable model L matrix
         * @return L
         */
        MatrixXd getL() const;

        /**
         * Returns the robot Mass matrix
         * @return M
         */
        MatrixXd getM() const;

        /**
         * Returns the robot Coriolis+Gravity vector
         * @return CG
         */
        VectorXd getCG() const;

    private:
        std::string name_; /// joint name
        int joint_index_; /// joint index
        const double *pos_; /// joint position
        const double *vel_; /// joint velocity
        const double *acc_; /// joint acceleration
        const VectorXd *CG_; /// Coriolis+Gravity vector
        const MatrixXd *L_, *M_; /// L/M matrices
    };

    class CardsflowStateInterface : public HardwareResourceManager<CardsflowStateHandle> {
    };

}


#ifndef __SNE_JLA__
#define __SNE_JLA__

#include <string>
#include <armadillo>
#include <imcmc/imcmc.hpp>
#include <imcmc/parser++.hpp>

#include "ParamList.hpp"

/////////////////////////////////////////////////////
//  JLA data structure is actually defined in jla.h
/////////////////////////////////////////////////////

double Likelihood_SNe_JLA(  imcmc::imcmc_double&    param,
                            double&                 lndet,
                            double&                 chisq,
                            void*                   model,
                            void*                   data,
                            imcmc::istate&          state );

struct SNe_JLA_Mock{
    DataInfo    data_info;
    int         sne_num;
    bool        has_err;
    bool        use_full_cov;
	bool		invert_err;	// used to check randomness of w transition
    arma::vec   z;
    arma::vec   mb, dmb;
    arma::mat   covmat_inv;
    SNe_JLA_Mock();
    ~SNe_JLA_Mock();
    void Init( std::string& dataset );
};

double Likelihood_SNe_JLA_Mock(	imcmc::imcmc_double&	param,
								double&					lndet,
								double&					chisq,
								void*					model,
								void*					data,
								imcmc::istate&			state );

#endif  //  __SNE__


#include "laserproc/door_depth_detector.h"
#include <geometry_msgs/PoseWithCovariance.h>
#include <tuw_object_msgs/ObjectWithCovariance.h>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/plot.hpp>
#include <tuw_geometry/figure.h>
#include <tuw_geometry/point2d.h>
#include <tuw_geometry/linesegment2d_detector.h>
#include <tuw_measurement_utils/contour.h>
#include <random>
#include <boost/uuid/uuid_io.hpp>

using namespace tuw;
using namespace tuw::door_laser_proc;

DoorDepthDetector::ParametersNode::ParametersNode() : node( "~" )
{
  node.param<std::string>( "source_frame", source_frame, "/base_link" );
  node.param<std::string>( "world_frame", world_frame, "/map" );
  node.param<bool>( "debug", debug, false );
  node.param<std::string>( "publisher_topic", publisher_topic, "/door_detections" );
  node.param<std::string>( "internal_mode", internal_mode, "contour" );
}

DoorDepthDetector::ReconfigureParams::ReconfigureParams()
{
  contour_cut_thresh = 0.25;
  min_door_len = 0.4;
  max_door_len = 1.1;
}

DoorDepthDetector::DoorDepthDetector( ros::NodeHandle &_nh ) : params_( new DoorDepthDetector::ParametersNode()),
                                                               DoorDetectorBase( _nh ),
                                                               figure_local_( "KernelResponses" )
{
  init( params_->publisher_topic );

  contour_container_ = ContourContainer( ContourContainer::Order::RIGHT2LEFT );

  wm_config_.reset( new WorldscopedMapConfig());

  figure_local_.init( wm_config_->map_pix_x, wm_config_->map_pix_y,
                      -2, 25,
                      wm_config_->map_min_y, wm_config_->map_max_y,
                      wm_config_->map_rotation + M_PI / 2.0,
                      wm_config_->map_grid_x, wm_config_->map_grid_y );

  ws_map_.init( wm_config_->map_pix_x, wm_config_->map_pix_y,
                -2, 25,
                wm_config_->map_min_y, wm_config_->map_max_y,
                wm_config_->map_rotation + M_PI / 2.0 );

  std::mt19937 gen( 19937 );
  std::uniform_int_distribution<> dist( 20, 255 );

  for ( size_t i = 0; i < 1000; ++i )
  {
    colorMap_[i] = cv::Scalar( dist( gen ), dist( gen ), dist( gen ));
  }

  cb_type_ = boost::bind( &DoorDepthDetector::reconfigureCallback, this, _1, _2 );
  cb_server_.setCallback( cb_type_ );
}

DoorDepthDetector::~DoorDepthDetector() = default;

//void DoorDepthDetector::callbackConfig(tuw_door_detection::DepthDetectorConfig &_config, uint32_t level)
//{
//	config_.reset(new tuw_door_detection::DepthDetectorConfig(_config));
//}

void DoorDepthDetector::reconfigureCallback( tuw_door_detection::DepthDetectorConfig &_config, uint32_t level )
{
  detector_config_.min_door_len = _config.min_door_len;
  detector_config_.max_door_len = _config.max_door_len;
  detector_config_.contour_cut_thresh = _config.contour_cut_thresh;
  line_segment_detector_.config_.threshold_split = _config.line_detection_split_threshold;
  line_segment_detector_.config_.threshold_split_neighbor = _config.line_detection_split_neighbor;
  line_segment_detector_.config_.min_length = _config.line_detection_min_length;
  line_segment_detector_.config_.min_points_per_line = _config.line_detection_min_points_per_line;
  line_segment_detector_.config_.min_points_per_unit = _config.line_detection_min_points_per_unit;
}

void DoorDepthDetector::plot( const std::vector<DoorDetectionPtr> &_detections )
{
  figure_local_.clear();

  for ( const auto d : _detections )
  {
    double radius = 1.0;
    cv::Scalar color;
    if ( d->validDetection())
    {
      radius = 1.0 + (d->responseNormalized() * 2.0);
      color = cv::Scalar( 255.0 * d->responseNormalized(), 0, 0 );
    } else
    {
      color = cv::Scalar( 0, 0, 0 );
    }
    figure_local_.circle( d->operator[]( 0 ).point, radius, color, 1.0 );
  }
  cv::imshow( figure_local_.title(), figure_local_.view());
  cv::waitKey( 1 );
}

void DoorDepthDetector::plot( const std::vector<double> &_responses )
{
  cv::Mat plotResponses;
  std::vector<double> resp_copy( _responses );
  cv::Mat responseMat( resp_copy );
  responseMat.convertTo( responseMat, CV_64F );
  cv::Ptr<cv::plot::Plot2d> plot = cv::plot::createPlot2d( responseMat );
  plot->render( plotResponses );
  cv::imshow( "plot", plotResponses );
}

template <typename T>
std::vector<T> DoorDepthDetector::normalize( std::vector<T> &_v )
{
  std::vector<T> c( _v );
  T min_resp = std::numeric_limits<T>::max();
  T max_resp = -std::numeric_limits<T>::max();

  for ( const T r : c )
  {
    min_resp = std::min( r, min_resp );
    max_resp = std::max( r, max_resp );
  }

  auto it = c.begin();
  for ( ; it != c.end(); ++it )
  {
    *it = ((*it) - min_resp) / (max_resp - min_resp);
  }

  return std::move( c );
}

std::vector<std::shared_ptr<tuw::Contour>> DoorDepthDetector::contourMode( const sensor_msgs::LaserScan &_scan )
{

  double contour_thresh = 0.35;
  cv::Mat img = cv::Mat::zeros( ws_map_.height(), ws_map_.width(), CV_8UC3);
  cv::Mat corner_img = cv::Mat::zeros( ws_map_.height(), ws_map_.width(), CV_8U );
  const size_t N = _scan.ranges.size();
  size_t contour_cnt = 1;

  size_t i = 0;
  double last_range = _scan.ranges[0];
  size_t last_range_idx = 0;
  size_t last_angle = _scan.angle_min;
  while ( !isfinite( last_range ) && i < N )
  {
    i++;
    last_range = _scan.ranges[i];
    last_angle = _scan.angle_min + (_scan.angle_increment * i);
    last_range_idx = i;
  }

  std::vector<std::shared_ptr<Contour>> contours;
  contours.push_back( std::make_shared<Contour>( uuid_generator_()));

  for ( size_t i = last_range_idx; i < N; i++ )
  {

    if ( std::isfinite( _scan.ranges[i] ) && _scan.ranges[i] < _scan.range_max )
    {

      Eigen::Vector2d v = range2Eigen( _scan, last_range_idx );
      Eigen::Vector2d vnext = range2Eigen( _scan, i );
      Point2D pt = Point2D( v[0], v[1] );
      Point2D ptnext = Point2D( vnext[0], vnext[1] );
      const double angle = _scan.angle_min + (_scan.angle_increment * i);

      if ( abs( _scan.ranges[i] - last_range ) < detector_config_.contour_cut_thresh )
      {
        //const int color_idx = jet_sampler[contour_cnt % jet_sampler.size()];
        contours.back()->push_back( Beam::make_beam( i, _scan.ranges[i], angle, ptnext ));
      } else
      {
        contour_cnt += 1;
        const auto beam = Beam::make_beam( i, _scan.ranges[i], angle, ptnext );
        std::shared_ptr<Contour> contr = std::make_shared<Contour>( uuid_generator_());
        contours.push_back( contr );
        contr->push_back( beam );
      }

      last_range = _scan.ranges[i];
      last_angle = angle;
      last_range_idx = i;
    }
  }

  contours.erase( std::remove_if( contours.begin(),
                                  contours.end(),
                                  [this]( std::shared_ptr<Contour> &c )
                                  {
                                    return c->length() < detector_config_.min_door_len;
                                  } ),
                  contours.end());

  contour_container_.clear();
  contour_container_.insert( contours );

  contour_cnt = 0;
  for ( size_t c_idx = 0; c_idx < contour_container_.size(); ++c_idx )
  {
    contour_cnt += 1;
    cv::Scalar color = colorMap_[contour_cnt];
    std::shared_ptr<Contour> elem = contour_container_.getContours()[c_idx];

    elem->renderInternal( ws_map_ );
    elem->cvDetectCorners();
    elem->detectLines( line_segment_detector_ );
    contour_container_.sortLines( elem );
    elem->calculateBoundingBoxObjSpace();
    //elem->optimizeLines( 10 );
    elem->set_door_candidate( false );
    elem->candidateLikelyhood( 0.0 );

    //elem->cvConvexityDefects(ws_map_);

    //Children are automatically sorted
    isDoorCandidate( elem );

    if ( elem->is_door_candidate())
    {
      //@ToDo: Strategy missing
      //determineHandle( elem );
    }
    if ( !elem->getChildCandidates().empty())
    {
      //for ( auto chld : elem->getChildCandidates())
      //{
      //  determineHandle( chld );
      //}
      determineHandle( elem );
    }

    //Only length requirement satisfied -> likelyhood is low
    if ( elem->is_door_candidate() && elem->getLineSegments().empty())
    {
      elem->candidateLikelyhood( 0.5 );
    } else if ( elem->is_door_candidate()) // has single line and fullfills length requirement seems good
    {
      elem->candidateLikelyhood( 1.0 );
    }

    elem->render( ws_map_, img, color, 2, true );
  }

  cv::imshow( "lines_as_img", img );
  cv::waitKey( 1 );

  return std::move( contours );
  //cv::imshow("corners", corner_img);
}

void DoorDepthDetector::determineHandle( const std::shared_ptr<Contour> &contour )
{
  for ( std::shared_ptr<Contour> chld : contour->getChildCandidates())
  {
    std::shared_ptr<Contour> neighbor_right = nullptr;
    std::shared_ptr<Contour> neighbor_left = nullptr;

    if ( contour->getChildren().size() > 1 )
    {
      //Search the child nodes for a wall
      unsigned int counter = 0;
      for ( std::shared_ptr<Contour> sibling : contour->getChildren())
      {
        if ( boost::lexical_cast<std::string>( chld->id()) == boost::lexical_cast<std::string>( sibling->id()))
        {
          if ( counter == 0 )
          {
            neighbor_left = contour->getChildren()[counter + 1];
          } else if ( counter == (contour->getChildren().size() - 1))
          {
            neighbor_right = contour->getChildren()[counter - 1];
          } else
          {
            if ( contour->getChildren().size() > 2 )
            {
              neighbor_right = contour->getChildren()[counter + 1];
              neighbor_left = contour->getChildren()[counter - 1];
            }
          }
        }
        counter++;
      }
    } else
    {
      //Search the neighboring contours for child nodes with a wall
    }

    if ( neighbor_left && neighbor_right )
    {
      ROS_WARN( "door has two neighbors, should not happen" );
    }

    if ( neighbor_left ) //post is on the left
    {
      ROS_INFO( "door has left neighbor" );

      chld->doorPostLeft( true );
      //Reminder: indices start right, end left
      //auto ls = neighbor_left->getLineSegments().front();
      //std::vector<cv::Point2d> v_ls_right_left = {
      //    neighbor_left->beams().back()->end_point.cv() - neighbor_left->beams().front()->end_point.cv()
      //};
      //
      //std::vector<cv::Point2d> v_chld_ls_left_right = {
      //    chld->beams().front()->end_point.cv() - chld->beams().back()->end_point.cv()
      //};
      //
      //v_ls_right_left[0] = v_ls_right_left[0] / cv::norm( v_ls_right_left, cv::NORM_L2SQR );
      //v_chld_ls_left_right[0] = v_chld_ls_left_right[0] / cv::norm( v_chld_ls_left_right, cv::NORM_L2SQR );
      //
      //double angle = std::acos( v_ls_right_left[0].dot( v_chld_ls_left_right[0] ));
    }
    if ( neighbor_right ) //post is on the right
    {
      ROS_INFO( "door has right neighbor" );

      chld->doorPostLeft( false );
      //auto ls = neighbor_right->getLineSegments().front();
      //std::vector<cv::Point2d> v_ls_right_left = {
      //    neighbor_right->beams().back()->end_point.cv() - neighbor_right->beams().front()->end_point.cv()
      //};
      //
      //std::vector<cv::Point2d> v_chld_ls_left_right = {
      //    chld->beams().front()->end_point.cv() - chld->beams().back()->end_point.cv()
      //};
      //
      //v_ls_right_left[0] = v_ls_right_left[0] / cv::norm( v_ls_right_left, cv::NORM_L2SQR );
      //v_chld_ls_left_right[0] = v_chld_ls_left_right[0] / cv::norm( v_chld_ls_left_right, cv::NORM_L2SQR );
      //
      //double angle = std::acos( v_ls_right_left[0].dot( v_chld_ls_left_right[0] ));
    }
  }
}

const bool DoorDepthDetector::isDoorCandidate( const std::shared_ptr<Contour> &contour )
{
  //@ToDo: move child construction to separate function?!
  if ( contour->length() > detector_config_.min_door_len
       && contour->length() < detector_config_.max_door_len )
  {
    contour->set_door_candidate( true );
    return true;
  }

  auto lSegs = contour->getLineSegments();
  //std::cout << boost::lexical_cast<std::string>( contour->id()) << " has lsegs: " << lSegs.size() << std::endl;
  for ( int ii = 0; ii < lSegs.size(); ii++ )
  {
    auto lSeg = lSegs[ii];
    unsigned int idx0 = lSeg.idx0_;
    unsigned int idx1 = lSeg.idx1_;
    std::shared_ptr<Contour> cChild = std::make_shared<Contour>( uuid_generator_());

    for ( ; idx0 < (idx1 - 1); idx0++ )
    {
      cChild->push_back( contour->beams()[idx0] );
    }

    if ( cChild->beams().size() > 1 )
    {
      if ( isDoorCandidate( cChild ))
      {
        cChild->getLineSegments() = {lSeg};
        cChild->getLineSegments().front().idx0_ = 0;
        cChild->getLineSegments().front().idx1_ = cChild->beams().size();
        cChild->candidateLikelyhood( 1.0 ); //fullfills length requirement and has line segment by construction
        contour->addChildCandidate( cChild );
      }
      contour->addChild( cChild );
    }
  }

  contour->set_door_candidate( false );
  return false;
}

//Border mode only same supported
bool DoorDepthDetector::kernelMode( const sensor_msgs::LaserScan &_scan, std::vector<DoorDetectionPtr> &_detections )
{
  assert ( KERNEL_SIZE % 2 == 0 );

  std::vector<double> ranges;
  ranges.resize( _scan.ranges.size());
  std::copy( _scan.ranges.begin(), _scan.ranges.end(), ranges.begin());

  std::vector<double> kernel( KERNEL_SIZE );
  size_t half_size = static_cast<std::size_t>(KERNEL_SIZE / 2.0);

  //TODO: preallocate
  //Same border mode
  for ( size_t i = 0; i < KERNEL_SIZE; ++i )
  {
    if ( i < half_size )
    {
      kernel[i] = -1.0f;
      //slow
      ranges.insert( ranges.begin(), ranges[0] );
    } else
    {
      kernel[i] = 1.0f;
      ranges.push_back( ranges.back());
    }
  }
  size_t N = ranges.size() - half_size;
  std::vector<double> responses;

  for ( size_t i = half_size; i < N; ++i )
  {
    double length = ranges[i];

    double sum = 0.0;
    size_t total_inner_loop = KERNEL_SIZE;
    for ( int j = 0; j < KERNEL_SIZE; ++j )
    {
      double curr_range = ranges[(i - half_size) + j];
      //HOW TO DEAL WITH THIS APPROPRIATELY?
      if ( !isfinite( curr_range ) || length >= _scan.range_max )
      {
        total_inner_loop--;
        continue;
      }
      sum += (kernel[j] * curr_range);
    }
    sum /= static_cast<double>(total_inner_loop);
    if ( !isfinite( sum ))
    {
      sum = 0;
    }
    responses.push_back( sum );
  }

  plot( responses );
  std::vector<double> responses_normalized = normalize( responses );

  _detections.clear();
  N = _scan.ranges.size();
  for ( int i = 0; i < N; ++i )
  {
    if ( std::isfinite( _scan.ranges[i] ) && _scan.ranges[i] < _scan.range_max )
    {
      Eigen::Vector2d point_meas = range2Eigen( _scan, i );
      auto d = std::make_shared<DoorDetection>();
      d->resize( _scan.ranges.size());
      d->operator[]( 0 ).point = Point2D( point_meas.x(), point_meas.y());
      d->operator[]( 0 ).length = _scan.ranges[i];
      d->response() = responses[i];
      d->responseNormalized() = responses_normalized[i];
      d->validDetection() = false;
      if ( d->responseNormalized() > 0.7 || d->responseNormalized() < 0.2 )
      {
        d->validDetection() = true;
        if ( d->responseNormalized() < 0.2 )
        {
          d->responseNormalized() = 1.0 - d->responseNormalized();
        }
        if ( !_detections.empty())
        {
          _detections.back()->link( _detections.back(), d );
        }
        _detections.push_back( d );
      }
    }
  }
  plot( _detections );
}

bool
DoorDepthDetector::structureMode( const sensor_msgs::LaserScan &_scan, std::vector<DoorDetectionPtr> &_detections )
{
  //Eigen::Matrix<double,4,4> tf_laser2world = tf2EigenMat(tf_laser);
  //std::cout << "robot pose: (" << tf_laser2world(0,3) << ", " << tf_laser2world(1,3) << ")" << std::endl;

  size_t N = _scan.ranges.size();

  int i = 0;
  double last_range = _scan.ranges[0];
  int last_range_idx = 0;
  while ( !isfinite( last_range ) && i < N )
  {
    i++;
    last_range = _scan.ranges[i];
    last_range_idx = i;
  }

  std::vector<Eigen::Vector2d> detected_door_ranges;
  std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> candidates;
  std::vector<std::pair<unsigned int, unsigned int>> candidates_idx;

  for ( ; i < N; ++i )
  {
    double length = _scan.ranges[i];
    double abs_diff = fabs( length - last_range );

    if ((length < _scan.range_max) && isfinite( length ) && abs_diff > thresh_ )
    {

      Eigen::Vector2d meas1 = range2Eigen( _scan, i );
      Eigen::Vector2d meas0 = range2Eigen( _scan, last_range_idx );

      candidates_idx.push_back( std::make_pair( last_range_idx, i ));
      candidates.push_back( std::make_pair( meas0, meas1 ));
    }

    if ( isfinite( length ))
    {
      last_range = length;
      last_range_idx = i;
    }
  }

  std::size_t idx_start = candidates_idx[0].second;
  std::size_t idx_candidates = 1;
  for ( ; idx_candidates < candidates_idx.size(); ++idx_candidates )
  {
  }

  //
  std::set<std::size_t> idx_set;
  i = 0;
  for ( const auto &c : candidates_idx )
  {
    if ( idx_set.count( c.first ) == 0 )
    {
      auto d = std::make_shared<DoorDetection>();
      d->resize( 1 );
      d->operator[]( 0 ).point = Point2D( candidates[i].first.x(), candidates[i].first.y());
      d->validDetection() = true;
      _detections.push_back( d );
      idx_set.insert( c.first );
    }

    if ( idx_set.count( c.second ) == 0 )
    {
      auto d = std::make_shared<DoorDetection>();
      d->operator[]( 0 ).point = Point2D( candidates[i].second.x(), candidates[i].second.y());
      d->validDetection() = true;
      _detections.push_back( d );
      idx_set.insert( c.second );
    }
    i++;
  }
}

bool DoorDepthDetector::processLaser( const sensor_msgs::LaserScan &_scan )
{

  last_header_.reset( new std_msgs::Header( _scan.header ));
  objects_.clear();

  tf::StampedTransform tf_laser;
  if ( !getTF( params()->world_frame, _scan.header.frame_id, tf_laser, params()->debug ))
  {
    return false;
  }

  door_detection_.clear();
  if ( params()->internal_mode == "structure" )
  {
    structureMode( _scan, door_detection_ );
  } else if ( params()->internal_mode == "kernel" )
  {
    kernelMode( _scan, door_detection_ );
  } else if ( params()->internal_mode == "contour" )
  {
    contours_ = contourMode( _scan );
  } else
  {
    ROS_ERROR( "Mode not supported, cant process laser" );
  }

  //t = tf_laser2world * t;
  for ( DoorDetectionPtr d : door_detection_ )
  {
    if ( !d->validDetection())
    {
      //TODO: cleanup
      continue;
    }
    Eigen::Quaterniond q( 1, 0, 0, 0 );
    tuw_object_msgs::ObjectWithCovariance out_obj;
    const auto point = d->operator[]( 0 ).point;

    out_obj.object.pose.position.x = point.x();
    out_obj.object.pose.position.y = point.y();
    out_obj.object.pose.position.z = 0;//v[2];

    out_obj.object.pose.orientation.x = q.x();
    out_obj.object.pose.orientation.y = q.y();
    out_obj.object.pose.orientation.z = q.z();
    out_obj.object.pose.orientation.w = q.w();

    out_obj.object.shape = tuw_object_msgs::Object::SHAPE_TRAFFIC_CONE;
    out_obj.object.ids = {
        static_cast<int>(objects_.size())
    };
    out_obj.object.ids_confidence = {0};
    out_obj.object.shape_variables = {0.1, 1, 0.5};
    objects_.push_back( std::move( out_obj ));
  }

  return true;
}


#include <boost/test/unit_test.hpp>

#include <random>

#include <memory>

#include "rfr/data_containers/default_data_container.hpp"
#include "rfr/splits/binary_split_one_feature_rss_loss.hpp"
#include "rfr/trees/k_ary_tree.hpp"
#include "rfr/trees/k_ary_mondrian_tree.hpp"
#include "rfr/forests/regression_forest.hpp"
#include "rfr/forests/mondrian_forest.hpp"
#include "rfr/forests/quantile_regression_forest.hpp"

#include <sstream>
#include <cereal/archives/xml.hpp>
#include <cereal/archives/portable_binary.hpp>
#include <cereal/archives/binary.hpp>
typedef cereal::PortableBinaryInputArchive iarch_type;
typedef cereal::PortableBinaryOutputArchive oarch_type;

typedef double num_t;
typedef double response_t;
typedef unsigned int index_t;
typedef std::default_random_engine rng_t;

typedef rfr::data_containers::default_container<num_t, response_t, index_t> data_container_type;
typedef rfr::nodes::k_ary_mondrian_node_full<2, num_t, response_t, index_t, rng_t> node_type;
typedef rfr::nodes::temporary_node<num_t, index_t> tmp_node_type;/**/
typedef rfr::trees::k_ary_mondrian_tree<2, node_type, num_t, response_t, index_t, rng_t> tree_type;
typedef rfr::forests::mondrian_forest< tree_type, num_t, response_t, index_t, rng_t> forest_type;

data_container_type load_diabetes_data(){
	//data_container_type data;
	data_container_type data(10);

    std::string feature_file, response_file;

    feature_file  = std::string(boost::unit_test::framework::master_test_suite().argv[1]) + "diabetes_features.csv";
    response_file = std::string(boost::unit_test::framework::master_test_suite().argv[1]) + "diabetes_responses.csv";

    data.import_csv_files(feature_file, response_file);
    return(data);
}

BOOST_AUTO_TEST_CASE( mondrian_forest_compile_tests ){

    auto data = load_diabetes_data();

	rfr::trees::tree_options<num_t, response_t, index_t> tree_opts;
	tree_opts.min_samples_to_split = 4;
	tree_opts.min_samples_in_leaf = 1;
	tree_opts.hierarchical_smoothing = false;
	tree_opts.max_features = data.num_data_points()*3/4;
	tree_opts.life_time = 5;

	rfr::forests::forest_options<num_t, response_t, index_t> forest_opts(tree_opts);

	forest_opts.num_data_points_per_tree = data.num_data_points();
	forest_opts.num_trees = 8;
	forest_opts.do_bootstrapping = true;
	forest_opts.compute_oob_error= true;

	forest_type the_forest(forest_opts);

	rng_t rng;

	BOOST_REQUIRE(std::isnan(the_forest.out_of_bag_error()));

	the_forest.fit(data, rng);
	BOOST_REQUIRE(!std::isnan(the_forest.out_of_bag_error()));

	response_t s_d, pred_mean;
    auto tmp = the_forest.predict(data.retrieve_data_point(5));

	std::ostringstream oss;
	{
		oarch_type oarchive(oss);
		oarchive(the_forest);
	}

	the_forest.save_to_binary_file("mondrian_forest_test.bin");

	forest_type the_forest2;

	auto s = the_forest.ascii_string_representation();

	the_forest2.load_from_ascii_string(s);

	forest_type the_forest3;
	the_forest3.load_from_binary_file("mondrian_forest_test.bin");

}

BOOST_AUTO_TEST_CASE( mondrian_forest_partial_fit ){

    auto data = load_diabetes_data();

	rfr::trees::tree_options<num_t, response_t, index_t> tree_opts;
	tree_opts.min_samples_to_split = 4;
	tree_opts.min_samples_in_leaf = 1;
	tree_opts.hierarchical_smoothing = false;
	tree_opts.max_features = data.num_data_points()*3/4;
	tree_opts.life_time = 5;//1000

	rfr::forests::forest_options<num_t, response_t, index_t> forest_opts(tree_opts);

	forest_opts.num_data_points_per_tree = data.num_data_points();
	forest_opts.num_trees = 8;
	forest_opts.do_bootstrapping = true;
	forest_opts.compute_oob_error= true;

	forest_type the_forest(forest_opts);

	rng_t rng;
	response_t s_d, pred_mean;
	BOOST_REQUIRE(std::isnan(the_forest.out_of_bag_error()));

	// TODO check prediction again with new API
	//BOOST_REQUIRE(std::isnan(the_forest.predict(data.retrieve_data_point(5), s_d, pred_mean, rng)));

	//response_t pre;
	//pre = the_forest.predict(data.retrieve_data_point(5), s_d, pred_mean, rng);

	//std::cout << "UNIT_TEST:: " << pre << std::endl;

	for(int i =0; i<data.num_data_points(); i++){
		the_forest.partial_fit(data, rng, i);
		//BOOST_REQUIRE(!std::isnan(the_forest.predict(data.retrieve_data_point(5), s_d, pred_mean, rng)));
	}

    auto tmp = the_forest.predict(data.retrieve_data_point(5));

	std::ostringstream oss;
	{
		oarch_type oarchive(oss);
		oarchive(the_forest);
	}

	the_forest.save_to_binary_file("mondrian_forest_partial_fit_test.bin");

	forest_type the_forest2;

	auto s = the_forest.ascii_string_representation();

	the_forest2.load_from_ascii_string(s);

	forest_type the_forest3;
	the_forest3.load_from_binary_file("mondrian_forest_partial_fit_test.bin");

}

BOOST_AUTO_TEST_CASE( mondrian_forest_predict_median_test ){

    auto data = load_diabetes_data();

	rfr::trees::tree_options<num_t, response_t, index_t> tree_opts;
	tree_opts.min_samples_to_split = 4;
	tree_opts.min_samples_in_leaf = 1;
	tree_opts.hierarchical_smoothing = false;
	tree_opts.max_features = data.num_data_points()*3/4;
	tree_opts.life_time = 5;//1000

	rfr::forests::forest_options<num_t, response_t, index_t> forest_opts(tree_opts);

	forest_opts.num_data_points_per_tree = data.num_data_points();
	forest_opts.num_trees = 8;
	forest_opts.do_bootstrapping = true;
	forest_opts.compute_oob_error= true;

	forest_type the_forest(forest_opts);

	rng_t rng;

	BOOST_REQUIRE(std::isnan(the_forest.out_of_bag_error()));

	the_forest.fit(data, rng);
	BOOST_REQUIRE(!std::isnan(the_forest.out_of_bag_error()));

	response_t s_d, pred_mean;
    auto tmp = the_forest.predict_median(data.retrieve_data_point(5));

	std::ostringstream oss;
	{
		oarch_type oarchive(oss);
		oarchive(the_forest);
	}

	the_forest.save_to_binary_file("mondrian_forest_test.bin");

	forest_type the_forest2;

	auto s = the_forest.ascii_string_representation();

	the_forest2.load_from_ascii_string(s);

	forest_type the_forest3;
	the_forest3.load_from_binary_file("mondrian_forest_test.bin");

}

BOOST_AUTO_TEST_CASE( mondrian_forest_exceptions_tests ){

    auto data = load_diabetes_data();

	rfr::trees::tree_options<num_t, response_t, index_t> tree_opts;
	tree_opts.min_samples_to_split = 2;
	tree_opts.min_samples_in_leaf = 1;
	tree_opts.max_features = data.num_data_points()*3/4;
	tree_opts.life_time = 5;

	rfr::forests::forest_options<num_t, response_t, index_t> forest_opts(tree_opts);

	forest_opts.num_data_points_per_tree = data.num_data_points();
	forest_opts.num_trees = 8;
	forest_opts.do_bootstrapping = false;
	forest_opts.compute_oob_error= true;

	forest_type the_forest(forest_opts);
	rng_t rng;

	// no trees in the forest
	the_forest.options.num_trees = 0;
	BOOST_REQUIRE_THROW(the_forest.fit(data, rng), std::runtime_error);

	// no datapoints in any trees
	the_forest.options.num_trees = 8;
	the_forest.options.num_data_points_per_tree = 0;
	BOOST_REQUIRE_THROW(the_forest.fit(data, rng), std::runtime_error);

	// not enough datapoints and no bootstrapping
	the_forest.options.num_data_points_per_tree = 32768;
	BOOST_REQUIRE_THROW(the_forest.fit(data, rng), std::runtime_error);

	// a successful training without bootstrapping for covarage
	the_forest.options.num_data_points_per_tree = 128;
	the_forest.fit(data, rng);

	// no features to split
	the_forest.options.num_data_points_per_tree = 128;
	the_forest.options.tree_opts.max_features = 0;
	BOOST_REQUIRE_THROW(the_forest.fit(data, rng), std::runtime_error);

}