/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   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.
 */

#ifndef DART_MATH_GEOMETRY_HPP_
#define DART_MATH_GEOMETRY_HPP_

#include <Eigen/Dense>

#include "dart/common/Deprecated.hpp"
#include "dart/math/Constants.hpp"
#include "dart/math/MathTypes.hpp"

namespace dart {
namespace math {

/// \brief
Eigen::Matrix3s makeSkewSymmetric(const Eigen::Vector3s& _v);

/// \brief
Eigen::Vector3s fromSkewSymmetric(const Eigen::Matrix3s& _m);

//------------------------------------------------------------------------------
/// \brief
Eigen::Quaternion_s expToQuat(const Eigen::Vector3s& _v);

/// \brief
Eigen::Vector3s quatToExp(const Eigen::Quaternion_s& _q);

/// \brief
Eigen::Vector3s rotatePoint(
    const Eigen::Quaternion_s& _q, const Eigen::Vector3s& _pt);

/// \brief
Eigen::Vector3s rotatePoint(
    const Eigen::Quaternion_s& _q, s_t _x, s_t _y, s_t _z);

/// \brief
Eigen::Matrix3s quatDeriv(const Eigen::Quaternion_s& _q, int _el);

/// \brief
Eigen::Matrix3s quatSecondDeriv(
    const Eigen::Quaternion_s& _q, int _el1, int _el2);

//------------------------------------------------------------------------------
/// \brief Given Euler XYX angles, return a 3x3 rotation matrix, which is
/// equivalent to RotX(angle(0)) * RotY(angle(1)) * RotX(angle(2)).
Eigen::Matrix3s eulerXYXToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerXYZ angles, return a 3x3 rotation matrix, which is
/// equivalent to RotX(angle(0)) * RotY(angle(1)) * RotZ(angle(2)).
Eigen::Matrix3s eulerXYZToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerXZX angles, return a 3x3 rotation matrix, which is
/// equivalent to RotX(angle(0)) * RotZ(angle(1)) * RotX(angle(2)).
Eigen::Matrix3s eulerXZXToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerXZY angles, return a 3x3 rotation matrix, which is
/// equivalent to RotX(angle(0)) * RotZ(angle(1)) * RotY(angle(2)).
Eigen::Matrix3s eulerXZYToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerYXY angles, return a 3x3 rotation matrix, which is
/// equivalent to RotY(angle(0)) * RotX(angle(1)) * RotY(angle(2)).
Eigen::Matrix3s eulerYXYToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerYXZ angles, return a 3x3 rotation matrix, which is
/// equivalent to RotY(angle(0)) * RotX(angle(1)) * RotZ(angle(2)).
Eigen::Matrix3s eulerYXZToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerYZX angles, return a 3x3 rotation matrix, which is
/// equivalent to RotY(angle(0)) * RotZ(angle(1)) * RotX(angle(2)).
Eigen::Matrix3s eulerYZXToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerYZY angles, return a 3x3 rotation matrix, which is
/// equivalent to RotY(angle(0)) * RotZ(angle(1)) * RotY(angle(2)).
Eigen::Matrix3s eulerYZYToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerZXY angles, return a 3x3 rotation matrix, which is
/// equivalent to RotZ(angle(0)) * RotX(angle(1)) * RotY(angle(2)).
Eigen::Matrix3s eulerZXYToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerZYX angles, return a 3x3 rotation matrix, which is
/// equivalent to RotZ(angle(0)) * RotY(angle(1)) * RotX(angle(2)).
/// singularity : angle[1] = -+ 0.5*PI
Eigen::Matrix3s eulerZYXToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerZXZ angles, return a 3x3 rotation matrix, which is
/// equivalent to RotZ(angle(0)) * RotX(angle(1)) * RotZ(angle(2)).
Eigen::Matrix3s eulerZXZToMatrix(const Eigen::Vector3s& _angle);

/// \brief Given EulerZYZ angles, return a 3x3 rotation matrix, which is
/// equivalent to RotZ(angle(0)) * RotY(angle(1)) * RotZ(angle(2)).
/// singularity : angle[1] = 0, PI
Eigen::Matrix3s eulerZYZToMatrix(const Eigen::Vector3s& _angle);

//------------------------------------------------------------------------------
/// \brief get the Euler XYX angle from R
Eigen::Vector3s matrixToEulerXYX(const Eigen::Matrix3s& _R);

/// \brief get the Euler XYZ angle from R
Eigen::Vector3s matrixToEulerXYZ(const Eigen::Matrix3s& _R);

///// \brief get the Euler XZX angle from R
// Eigen::Vector3s matrixToEulerXZX(const Eigen::Matrix3s& R);

/// \brief get the Euler XZY angle from R
Eigen::Vector3s matrixToEulerXZY(const Eigen::Matrix3s& _R);

///// \brief get the Euler YXY angle from R
// Eigen::Vector3s matrixToEulerYXY(const Eigen::Matrix3s& R);

/// \brief get the Euler YXZ angle from R
Eigen::Vector3s matrixToEulerYXZ(const Eigen::Matrix3s& _R);

/// \brief get the Euler YZX angle from R
Eigen::Vector3s matrixToEulerYZX(const Eigen::Matrix3s& _R);

///// \brief get the Euler YZY angle from R
// Eigen::Vector3s matrixToEulerYZY(const Eigen::Matrix3s& R);

/// \brief get the Euler ZXY angle from R
Eigen::Vector3s matrixToEulerZXY(const Eigen::Matrix3s& _R);

/// \brief get the Euler ZYX angle from R
Eigen::Vector3s matrixToEulerZYX(const Eigen::Matrix3s& _R);

///// \brief get the Euler ZXZ angle from R
// Eigen::Vector3s matrixToEulerZXZ(const Eigen::Matrix3s& R);

///// \brief get the Euler ZYZ angle from R
// Eigen::Vector3s matrixToEulerZYZ(const Eigen::Matrix3s& R);

//------------------------------------------------------------------------------

/// Returns the Jacobian of an SO(3) element w.r.t. its exponential
/// coordinates where the Jacobian maps the time derivative of the exponential
/// coordinates to the angular velocity in the world frame.
Eigen::Matrix3s so3LeftJacobian(const Eigen::Vector3s& w);

/// Returns the Jacobian of an SO(3) element w.r.t. its exponential
/// coordinates where the Jacobian maps the time derivative of the exponential
/// coordinates to the angular velocity in the body frame.
Eigen::Matrix3s so3RightJacobian(const Eigen::Vector3s& w);

/// Returns the time derivative of the left Jacobian of SO(3).
Eigen::Matrix3s so3LeftJacobianTimeDeriv(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq);

/// Returns the time derivative of the right Jacobian of SO(3).
Eigen::Matrix3s so3RightJacobianTimeDeriv(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq);

/// \brief Exponential mapping
Eigen::Isometry3s expMap(const Eigen::Vector6s& _S);

/// \brief Exponential mapping, DART style. This treats the exponentiation
/// operation as a rotation, and then a translation, rather than an integration
/// of a screw.
Eigen::Isometry3s expMapDart(const Eigen::Vector6s& _S);

/// \brief fast version of Exp(se3(s, 0))
/// \todo This expAngular() can be replaced by Eigen::AngleAxis() but we need
/// to verify that they have exactly same functionality.
/// See: https://github.com/dartsim/dart/issues/88
Eigen::Isometry3s expAngular(const Eigen::Vector3s& _s);

/// \brief Computes the Rotation matrix from a given expmap vector.
Eigen::Matrix3s expMapRot(const Eigen::Vector3s& _expmap);

/// \brief Computes the Jacobian of the expmap
Eigen::Matrix3s expMapJac(const Eigen::Vector3s& _expmap);

/// Returns the Jacobian of an SO(3) element w.r.t. its exponential
/// coordinates where the Jacobian maps the time derivative of the exponential
/// coordinates to the angular velocity in the world frame.
Eigen::Matrix3s so3LeftJacobian(const Eigen::Vector3s& w);

/// Returns the Jacobian of an SO(3) element w.r.t. its exponential
/// coordinates where the Jacobian maps the time derivative of the exponential
/// coordinates to the angular velocity in the body frame.
Eigen::Matrix3s so3RightJacobian(const Eigen::Vector3s& w);

/// \brief Computes the Jacobian of the logMap(R * expMapRot(expMap))
Eigen::Matrix3s expMapJacAt(
    const Eigen::Vector3s& _expmap, const Eigen::Matrix3s& R);

/// \brief Computes the time derivative of the expmap Jacobian.
Eigen::Matrix3s expMapJacDot(
    const Eigen::Vector3s& _expmap, const Eigen::Vector3s& _qdot);

Eigen::Matrix3s so3LeftJacobianTimeDeriv(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq);

Eigen::Matrix3s so3RightJacobianTimeDeriv(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq);

Eigen::Matrix3s so3RightJacobianTimeDerivDeriv(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq, int index);

Eigen::Matrix3s so3RightJacobianTimeDerivDeriv2(
    const Eigen::Vector3s& q, const Eigen::Vector3s& dq, int index);

/// \brief computes the derivative of the Jacobian of the expmap wrt to _qi
/// indexed dof; _qi \f$ \in \f$ {0,1,2}
Eigen::Matrix3s expMapJacDeriv(const Eigen::Vector3s& _expmap, int _qi);

/// \brief computes the gradient of logMap(expMapRot()) wrt to _qi
/// indexed dof; _qi \f$ \in \f$ {0,1,2}
Eigen::Vector3s expMapGradient(const Eigen::Vector3s& pos, int _qi);

/// \brief computes the gradient of logMap(expMapRot(screw * eps) *
/// expMapRot(original)) wrt to eps
Eigen::Vector3s expMapNestedGradient(
    const Eigen::Vector3s& original, const Eigen::Vector3s& screw);

Eigen::Vector3s finiteDifferenceExpMapNestedGradient(
    const Eigen::Vector3s& original,
    const Eigen::Vector3s& screw,
    bool useRidders = true);

Eigen::Vector3s finiteDifferenceRiddersExpMapNestedGradient(
    const Eigen::Vector3s& original, const Eigen::Vector3s& screw);

/// \brief Log mapping
/// \note When @f$|Log(R)| = @pi@f$, Exp(LogR(R) = Exp(-Log(R)).
/// The implementation returns only the positive one.
Eigen::Vector3s logMap(const Eigen::Matrix3s& _R);

/// \brief Log mapping
Eigen::Vector6s logMap(const Eigen::Isometry3s& _T);

/// This takes a screw axis and a point, and gives us the direction that the
/// point will move if we increase theta by an infinitesimal amount.
Eigen::Vector3s gradientWrtTheta(
    const Eigen::Vector6s& screwAxis, const Eigen::Vector3s& point, s_t theta);

/// This takes a rotation axis and a point, and gives us the direction that the
/// point will move if we increase the theta by an infinitesimal amount.
Eigen::Vector3s gradientWrtThetaPureRotation(
    const Eigen::Vector3s& omega, const Eigen::Vector3s& point, s_t theta);

/// This returns the average of the points on edge A and edge B closest to each
/// other.
Eigen::Vector3s getContactPoint(
    const Eigen::Vector3s& edgeAPoint,
    const Eigen::Vector3s& edgeADir,
    const Eigen::Vector3s& edgeBPoint,
    const Eigen::Vector3s& edgeBDir,
    s_t radiusA = 1.0,
    s_t radiusB = 1.0);

/// This returns gradient of the average of the points on edge A and edge B
/// closest to each other, allowing all the inputs to change.
Eigen::Vector3s getContactPointGradient(
    const Eigen::Vector3s& edgeAPoint,
    const Eigen::Vector3s& edgeAPointGradient,
    const Eigen::Vector3s& edgeADir,
    const Eigen::Vector3s& edgeADirGradient,
    const Eigen::Vector3s& edgeBPoint,
    const Eigen::Vector3s& edgeBPointGradient,
    const Eigen::Vector3s& edgeBDir,
    const Eigen::Vector3s& edgeBDirGradient,
    s_t radiusA = 1.0,
    s_t radiusB = 1.0);

Eigen::VectorXs dampedPInv(
    const Eigen::MatrixXs& J, const Eigen::VectorXs& x, s_t damping = 0.05);

bool hasTinySingularValues(
    const Eigen::MatrixXs& J, s_t clippingThreshold = 1e-4);

Eigen::MatrixXs clippedSingularsPinv(
    const Eigen::MatrixXs& J, s_t clippingThreshold = 1e-4);

//------------------------------------------------------------------------------
/// \brief Rectify the rotation part so as that it satifies the orthogonality
/// condition.
///
/// It is one step of @f$R_{i_1}=1/2(R_i + R_i^{-T})@f$.
/// Hence by calling this function iterativley, you can make the rotation part
/// closer to SO(3).
// SE3 Normalize(const SE3& T);

/// \brief reparameterize such as ||s'|| < M_PI and Exp(s) == Epx(s')
// Axis Reparameterize(const Axis& s);

//------------------------------------------------------------------------------
/// \brief adjoint mapping
/// \note @f$Ad_TV = ( Rw@,, ~p @times Rw + Rv)@f$,
/// where @f$T=(R,p)@in SE(3), @quad V=(w,v)@in se(3) @f$.
Eigen::Vector6s AdT(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _V);

/// \brief Get linear transformation matrix of Adjoint mapping
Eigen::Matrix6s getAdTMatrix(const Eigen::Isometry3s& T);

// TODO(JS): Rename and add documentation
Eigen::Matrix6s AdTMatrix(const Eigen::Isometry3s& T);
Eigen::Matrix6s AdInvTMatrix(const Eigen::Isometry3s& T);

// TODO(JS): Rename and add documentation
Eigen::Matrix6s dAdTMatrix(const Eigen::Isometry3s& T);
Eigen::Matrix6s dAdInvTMatrix(const Eigen::Isometry3s& T);

/// Adjoint mapping for dynamic size Jacobian
template <typename Derived>
typename Derived::PlainObject AdTJac(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  // Compute AdT column by column
  for (int i = 0; i < _J.cols(); ++i)
    ret.col(i) = AdT(_T, _J.col(i));

  return ret;
}

/// Adjoint mapping for fixed size Jacobian
template <typename Derived>
typename Derived::PlainObject AdTJacFixed(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  // Check if _J is fixed size Jacobian
  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);

  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  // Compute AdT
  ret.template topRows<3>().noalias() = _T.linear() * _J.template topRows<3>();
  ret.template bottomRows<3>().noalias()
      = -ret.template topRows<3>().colwise().cross(_T.translation())
        + _T.linear() * _J.template bottomRows<3>();

  return ret;
}

/// \brief Fast version of Ad([R 0; 0 1], V)
Eigen::Vector6s AdR(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _V);

/// \brief fast version of Ad(T, se3(w, 0))
Eigen::Vector6s AdTAngular(
    const Eigen::Isometry3s& _T, const Eigen::Vector3s& _w);

/// \brief fast version of Ad(T, se3(0, v))
Eigen::Vector6s AdTLinear(
    const Eigen::Isometry3s& _T, const Eigen::Vector3s& _v);

///// \brief fast version of Ad([I p; 0 1], V)
// se3 AdP(const Vec3& p, const se3& s);

/// \brief Change coordinate Frame of a Jacobian
template <typename Derived>
typename Derived::PlainObject AdRJac(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  ret.template topRows<3>().noalias() = _T.linear() * _J.template topRows<3>();

  ret.template bottomRows<3>().noalias()
      = _T.linear() * _J.template bottomRows<3>();

  return ret;
}

template <typename Derived>
typename Derived::PlainObject AdRInvJac(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  ret.template topRows<3>().noalias()
      = _T.linear().transpose() * _J.template topRows<3>();

  ret.template bottomRows<3>().noalias()
      = _T.linear().transpose() * _J.template bottomRows<3>();

  return ret;
}

template <typename Derived>
typename Derived::PlainObject adJac(
    const Eigen::Vector6s& _V, const Eigen::MatrixBase<Derived>& _J)
{
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  ret.template topRows<3>().noalias()
      = -_J.template topRows<3>().colwise().cross(_V.head<3>());

  ret.template bottomRows<3>().noalias()
      = -_J.template bottomRows<3>().colwise().cross(_V.head<3>())
        - _J.template topRows<3>().colwise().cross(_V.tail<3>());

  return ret;
}

/// \brief fast version of Ad(Inv(T), V)
Eigen::Vector6s AdInvT(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _V);

/// Adjoint mapping for dynamic size Jacobian
template <typename Derived>
typename Derived::PlainObject AdInvTJac(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  // Compute AdInvT column by column
  for (int i = 0; i < _J.cols(); ++i)
    ret.col(i) = AdInvT(_T, _J.col(i));

  return ret;
}

/// Adjoint mapping for fixed size Jacobian
template <typename Derived>
typename Derived::PlainObject AdInvTJacFixed(
    const Eigen::Isometry3s& _T, const Eigen::MatrixBase<Derived>& _J)
{
  // Check if _J is fixed size Jacobian
  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);

  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(
      Derived::RowsAtCompileTime == 6,
      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename Derived::PlainObject ret(_J.rows(), _J.cols());

  // Compute AdInvT
  ret.template topRows<3>().noalias()
      = _T.linear().transpose() * _J.template topRows<3>();
  ret.template bottomRows<3>().noalias()
      = _T.linear().transpose()
        * (_J.template bottomRows<3>()
           + _J.template topRows<3>().colwise().cross(_T.translation()));

  return ret;
}

///// \brief fast version of Ad(Inv(T), se3(Eigen_Vec3(0), v))
// Eigen::Vector3s AdInvTLinear(const Eigen::Isometry3s& T,
//                             const Eigen::Vector3s& v);

///// \brief fast version of Ad(Inv(T), se3(w, Eigen_Vec3(0)))
// Axis AdInvTAngular(const SE3& T, const Axis& w);

///// \brief Fast version of Ad(Inv([R 0; 0 1]), V)
// se3 AdInvR(const SE3& T, const se3& V);

/// \brief Fast version of Ad(Inv([R 0; 0 1]), se3(0, v))
Eigen::Vector6s AdInvRLinear(
    const Eigen::Isometry3s& _T, const Eigen::Vector3s& _v);

/// \brief dual adjoint mapping
/// \note @f$Ad^{@,*}_TF = ( R^T (m - p@times f)@,,~ R^T f)@f$,
/// where @f$T=(R,p)@in SE(3), F=(m,f)@in se(3)^*@f$.
Eigen::Vector6s dAdT(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _F);

///// \brief fast version of Ad(Inv(T), dse3(Eigen_Vec3(0), F))
// dse3 dAdTLinear(const SE3& T, const Vec3& F);

/// \brief fast version of dAd(Inv(T), F)
Eigen::Vector6s dAdInvT(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _F);

/// \brief fast version of dAd(Inv([R 0; 0 1]), F)
Eigen::Vector6s dAdInvR(const Eigen::Isometry3s& _T, const Eigen::Vector6s& _F);

///// \brief fast version of dAd(Inv(SE3(p)), dse3(Eigen_Vec3(0), F))
// dse3 dAdInvPLinear(const Vec3& p, const Vec3& F);

/// \brief adjoint mapping
/// \note @f$ad_X Y = ( w_X @times w_Y@,,~w_X @times v_Y - w_Y @times v_X),@f$,
/// where @f$X=(w_X,v_X)@in se(3), @quad Y=(w_Y,v_Y)@in se(3) @f$.
Eigen::Vector6s ad(const Eigen::Vector6s& _X, const Eigen::Vector6s& _Y);

// TODO(JS): Rename and add documentation
Eigen::Matrix6s adMatrix(const Eigen::Vector6s& X);

/// \brief fast version of ad(se3(Eigen_Vec3(0), v), S)
// Vec3 ad_Vec3_se3(const Vec3& v, const se3& S);

/// \brief fast version of ad(se3(w, 0), se3(v, 0)) -> check
// Axis ad_Axis_Axis(const Axis& w, const Axis& v);

/// \brief dual adjoint mapping
/// \note @f$ad^{@,*}_V F = (m @times w + f @times v@,,~ f @times w),@f$
/// , where @f$F=(m,f)@in se^{@,*}(3), @quad V=(w,v)@in se(3) @f$.
Eigen::Vector6s dad(const Eigen::Vector6s& _s, const Eigen::Vector6s& _t);

/// \brief
Inertia transformInertia(const Eigen::Isometry3s& _T, const Inertia& _AI);

/// Use the Parallel Axis Theorem to compute the moment of inertia of a body
/// whose center of mass has been shifted from the origin
Eigen::Matrix3s parallelAxisTheorem(
    const Eigen::Matrix3s& _original,
    const Eigen::Vector3s& _comShift,
    s_t _mass);

enum AxisType
{
  AXIS_X = 0,
  AXIS_Y = 1,
  AXIS_Z = 2
};

/// Compute a rotation matrix from a vector. One axis of the rotated coordinates
/// by the rotation matrix matches the input axis where the axis is specified
/// by axisType.
Eigen::Matrix3s computeRotation(
    const Eigen::Vector3s& axis, AxisType axisType = AxisType::AXIS_X);

/// Compute a transform from a vector and a position. The rotation of the result
/// transform is computed by computeRotationMatrix(), and the translation is
/// just the input translation.
Eigen::Isometry3s computeTransform(
    const Eigen::Vector3s& axis,
    const Eigen::Vector3s& translation,
    AxisType axisType = AxisType::AXIS_X);

/// Generate frame given origin and z-axis
DART_DEPRECATED(6.0)
Eigen::Isometry3s getFrameOriginAxisZ(
    const Eigen::Vector3s& _origin, const Eigen::Vector3s& _axisZ);

/// \brief Check if determinant of _R is equat to 1 and all the elements are not
/// NaN values.
bool verifyRotation(const Eigen::Matrix3s& _R);

/// \brief Check if determinant of the rotational part of _T is equat to 1 and
/// all the elements are not NaN values.
bool verifyTransform(const Eigen::Isometry3s& _T);

/// Compute the angle (in the range of -pi to +pi) which ignores any full
/// rotations
#ifdef DART_USE_ARBITRARY_PRECISION
inline s_t wrapToPi(s_t angle)
{
  s_t pi = constantsd::pi();
  return fmod(angle + pi, 2 * pi) - pi;
}
#else
inline s_t wrapToPi(s_t angle)
{
  constexpr auto pi = constantsd::pi();

  return std::fmod(angle + pi, 2 * pi) - pi;
}
#endif

template <typename MatrixType, typename ReturnType>
void extractNullSpace(const Eigen::JacobiSVD<MatrixType>& _SVD, ReturnType& _NS)
{
  int rank = 0;
  // TODO(MXG): Replace this with _SVD.rank() once the latest Eigen is released
  if (_SVD.nonzeroSingularValues() > 0)
  {
    s_t thresh = max(
        _SVD.singularValues().coeff(0) * 1e-10,
        std::numeric_limits<s_t>::min());
    int i = _SVD.nonzeroSingularValues() - 1;
    while (i >= 0 && _SVD.singularValues().coeff(i) < thresh)
      --i;
    rank = i + 1;
  }

  int cols = _SVD.matrixV().cols(), rows = _SVD.matrixV().rows();
  _NS = _SVD.matrixV().block(0, rank, rows, cols - rank);
}

template <typename MatrixType, typename ReturnType>
void computeNullSpace(const MatrixType& _M, ReturnType& _NS)
{
  Eigen::JacobiSVD<MatrixType> svd(_M, Eigen::ComputeFullV);
  extractNullSpace(svd, _NS);
}

typedef std::vector<Eigen::Vector3s> SupportGeometry;

typedef common::aligned_vector<Eigen::Vector2s> SupportPolygon;

/// Project the support geometry points onto a plane with the given axes
/// and then compute their convex hull, which will take the form of a polgyon.
/// _axis1 and _axis2 must both have unit length for this function to work
/// correctly.
SupportPolygon computeSupportPolgyon(
    const SupportGeometry& _geometry,
    const Eigen::Vector3s& _axis1 = Eigen::Vector3s::UnitX(),
    const Eigen::Vector3s& _axis2 = Eigen::Vector3s::UnitY());

/// Same as computeSupportPolgyon, except you can pass in a
/// std::vector<std::size_t> which will have the same size as the returned
/// SupportPolygon, and each entry will contain the original index of each point
/// in the SupportPolygon
SupportPolygon computeSupportPolgyon(
    std::vector<std::size_t>& _originalIndices,
    const SupportGeometry& _geometry,
    const Eigen::Vector3s& _axis1 = Eigen::Vector3s::UnitX(),
    const Eigen::Vector3s& _axis2 = Eigen::Vector3s::UnitY());

/// Computes the convex hull of a set of 2D points
SupportPolygon computeConvexHull(const SupportPolygon& _points);

/// Computes the convex hull of a set of 2D points and fills in _originalIndices
/// with the original index of each entry in the returned SupportPolygon
SupportPolygon computeConvexHull(
    std::vector<std::size_t>& _originalIndices, const SupportPolygon& _points);

/// Compute the centroid of a polygon, assuming the polygon is a convex hull
Eigen::Vector2s computeCentroidOfHull(const SupportPolygon& _convexHull);

/// Intersection_t is returned by the computeIntersection() function to indicate
/// whether there was a valid intersection between the two line segments
enum IntersectionResult
{

  INTERSECTING = 0, ///< An intersection was found
  PARALLEL,         ///< The line segments are parallel
  BEYOND_ENDPOINTS  ///< There is no intersection because the end points do not
                    ///< expand far enough

};

/// Compute the intersection between a line segment that goes from a1 -> a2 and
/// a line segment that goes from b1 -> b2.
IntersectionResult computeIntersection(
    Eigen::Vector2s& _intersectionPoint,
    const Eigen::Vector2s& a1,
    const Eigen::Vector2s& a2,
    const Eigen::Vector2s& b1,
    const Eigen::Vector2s& b2);

/// Compute a 2D cross product
s_t cross(const Eigen::Vector2s& _v1, const Eigen::Vector2s& _v2);

/// Returns true if the point _p is inside the support polygon
bool isInsideSupportPolygon(
    const Eigen::Vector2s& _p,
    const SupportPolygon& _support,
    bool _includeEdge = true);

/// Returns the point which is closest to _p that also lays on the line segment
/// that goes from _s1 -> _s2
Eigen::Vector2s computeClosestPointOnLineSegment(
    const Eigen::Vector2s& _p,
    const Eigen::Vector2s& _s1,
    const Eigen::Vector2s& _s2);

/// Returns the point which is closest to _p that also lays on the edge of the
/// support polygon
Eigen::Vector2s computeClosestPointOnSupportPolygon(
    const Eigen::Vector2s& _p, const SupportPolygon& _support);

/// Same as closestPointOnSupportPolygon, but also fills in _index1 and _index2
/// with the indices of the line segment
Eigen::Vector2s computeClosestPointOnSupportPolygon(
    std::size_t& _index1,
    std::size_t& _index2,
    const Eigen::Vector2s& _p,
    const SupportPolygon& _support);

/// This computes and returns the closest point on a line, given by a point and
/// a direction, to a goal point
Eigen::Vector3s closestPointOnLine(
    const Eigen::Vector3s& pointOnLine,
    const Eigen::Vector3s& lineDirection,
    const Eigen::Vector3s& goalPoint);

/// This computes and returns the gradient of closestPointOnLine(), given the
/// gradients of its inputs
Eigen::Vector3s closestPointOnLineGradient(
    const Eigen::Vector3s& pointOnLine,
    const Eigen::Vector3s& pointOnLineGradient,
    const Eigen::Vector3s& lineDirection,
    const Eigen::Vector3s& lineDirectionGradient,
    const Eigen::Vector3s& goalPoint,
    const Eigen::Vector3s& goalPointGradient);

// Represents a bounding box with minimum and maximum coordinates.
class BoundingBox
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  BoundingBox();
  BoundingBox(const Eigen::Vector3s& min, const Eigen::Vector3s& max);

  inline const Eigen::Vector3s& getMin() const
  {
    return mMin;
  }
  inline const Eigen::Vector3s& getMax() const
  {
    return mMax;
  }

  inline void setMin(const Eigen::Vector3s& min)
  {
    mMin = min;
  }
  inline void setMax(const Eigen::Vector3s& max)
  {
    mMax = max;
  }

  // \brief Centroid of the bounding box (i.e average of min and max)
  inline Eigen::Vector3s computeCenter() const
  {
    return (mMax + mMin) * 0.5;
  }
  // \brief Coordinates of the maximum corner with respect to the centroid.
  inline Eigen::Vector3s computeHalfExtents() const
  {
    return (mMax - mMin) * 0.5;
  }
  // \brief Length of each of the sides of the bounding box.
  inline Eigen::Vector3s computeFullExtents() const
  {
    return (mMax - mMin);
  }

protected:
  // \brief minimum coordinates of the bounding box
  Eigen::Vector3s mMin;
  // \brief maximum coordinates of the bounding box
  Eigen::Vector3s mMax;
};

} // namespace math
} // namespace dart

#endif // DART_MATH_GEOMETRY_HPP_


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/meta/real_c.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test value of real_c constants for every base real types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE( real_c )
{
  using boost::simd::meta::real_c;

  real_c< boost::simd::uint32_t, 0x3F800000UL>          one_fc;
  real_c< boost::simd::uint64_t, 0x3FF0000000000000ULL> one_dc;

  float  one_f = 1.f;
  double one_d = 1.;

  NT2_TEST_EQUAL( one_f, float(one_fc)  );
  NT2_TEST_EQUAL( one_d, double(one_dc) );
}


#ifndef LBP_XCSLBP_HPP
#define LBP_XCSLBP_HPP

#include <lbp/defs.hpp>
#include <lbp/utils.hpp>
#include <lbp/detail/neighborhoods.hpp>
#include <lbp/detail/sampling.hpp>

#include <opencv2/core.hpp>

#include <boost/hana/fold.hpp>
#include <boost/hana/tuple.hpp>

#include <boost/integer.hpp>

//
// @inproceedings{silva2015extended,
//   title={An eXtended center-symmetric local binary pattern for background modeling and subtraction in videos},
//   author={Silva, Caroline and Bouwmans, Thierry and Fr{\'e}licot, Carl},
//   booktitle={International Joint Conference on Computer Vision, Imaging and Computer Graphics Theory and Applications, VISAPP 2015},
//   year={2015}
// }
//

namespace lbp {
namespace xcslbp_detail {

template< typename T >
auto xcslbp = [](auto N, auto S) {
    return [=](const cv::Mat& src, size_t i, size_t j, const T& epsilon) {
        using namespace cv;
        using namespace hana::literals;

        const auto c = S (src, i, j);

        return boost::hana::fold (
            N, 0, [&, shift = 0](auto accum, auto x) mutable {
                const auto a = S (src, i + x [0_c], j + x [1_c]);
                const auto b = S (src, i - x [0_c], j - x [1_c]);

                const auto g1 = a - b + c;
                const auto g2 = (a - c) * (b - c);

                return accum | (((g1 + g2) >= epsilon) << shift++);
            });
    };
};

} // namespace xcslbp_detail

template< typename T, size_t R, size_t P >
auto xcslbp = [](const cv::Mat& src, const T& epsilon = T { }) {
    using value_type = typename boost::uint_t< P/2 >::least;

    cv::Mat dst (src.size (), opencv_type< value_type >, cv::Scalar (0));

    auto op = xcslbp_detail::xcslbp< T > (
        detail::semicircular_neighborhood< R, P >,
        detail::nearest_sampler< T >);

#pragma omp parallel for
    for (size_t i = R; i < src.rows - R; ++i) {
        for (size_t j = R; j < src.cols - R; ++j) {
            dst.at< value_type > (i, j) = op (src, i, j, epsilon);
        }
    }

    return dst;
};

} // namespace lbp

#endif // LBP_XCSLBP_HPP


#include "Scene_polyhedron_item.h"
#include <CGAL/Three/Viewer_interface.h>
#include <CGAL/AABB_intersections.h>
#include "Kernel_type.h"
#include <CGAL/IO/Polyhedron_iostream.h>

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/AABB_face_graph_triangle_primitive.h>

#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_with_info_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_triangulation_plus_2.h>
#include <CGAL/Triangulation_2_filtered_projection_traits_3.h>
#include <CGAL/Polygon_mesh_processing/compute_normal.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/Polygon_mesh_processing/measure.h>
#include <CGAL/Polygon_mesh_processing/self_intersections.h>
#include <CGAL/Polygon_mesh_processing/repair.h>
#include <CGAL/Polygon_mesh_processing/polygon_soup_to_polygon_mesh.h>
#include <CGAL/Polygon_mesh_processing/orient_polygon_soup.h>
#include <CGAL/property_map.h>
#include <CGAL/statistics_helpers.h>

#include <list>
#include <queue>
#include <iostream>
#include <limits>

#include <QVariant>
#include <QDebug>
#include <QDialog>

#include <boost/foreach.hpp>
#include <boost/container/flat_map.hpp>

namespace PMP = CGAL::Polygon_mesh_processing;

typedef CGAL::AABB_face_graph_triangle_primitive<Polyhedron> Primitive;
typedef CGAL::AABB_traits<Kernel, Primitive> AABB_traits;
typedef CGAL::AABB_tree<AABB_traits> Input_facets_AABB_tree;
const char* aabb_property_name = "Scene_polyhedron_item aabb tree";

Input_facets_AABB_tree* get_aabb_tree(Scene_polyhedron_item* item)
{
    QVariant aabb_tree_property = item->property(aabb_property_name);
    if(aabb_tree_property.isValid()) {
        void* ptr = aabb_tree_property.value<void*>();
        return static_cast<Input_facets_AABB_tree*>(ptr);
    }
    else {
        Polyhedron* poly = item->polyhedron();
        if(poly) {
            Input_facets_AABB_tree* tree =
                    new Input_facets_AABB_tree(faces(*poly).first,
                                               faces(*poly).second,
                                               *poly);
            item->setProperty(aabb_property_name,
                              QVariant::fromValue<void*>(tree));
            return tree;
        }
        else return 0;
    }
}

void delete_aabb_tree(Scene_polyhedron_item* item)
{
    QVariant aabb_tree_property = item->property(aabb_property_name);
    if(aabb_tree_property.isValid()) {
        void* ptr = aabb_tree_property.value<void*>();
        Input_facets_AABB_tree* tree = static_cast<Input_facets_AABB_tree*>(ptr);
        if(tree) {
            delete tree;
            tree = 0;
        }
        item->setProperty(aabb_property_name, QVariant());
    }
}

template<typename TypeWithXYZ, typename ContainerWithPushBack>
void push_back_xyz(const TypeWithXYZ& t,
                   ContainerWithPushBack& vector)
{
  vector.push_back(t.x());
  vector.push_back(t.y());
  vector.push_back(t.z());
}

typedef Polyhedron::Traits Traits;
typedef Polyhedron::Facet Facet;
typedef CGAL::Triangulation_2_filtered_projection_traits_3<Traits>   P_traits;
typedef Polyhedron::Halfedge_handle Halfedge_handle;
struct Face_info {
    Polyhedron::Halfedge_handle e[3];
    bool is_external;
};
typedef CGAL::Triangulation_vertex_base_with_info_2<Halfedge_handle,
P_traits>        Vb;
typedef CGAL::Triangulation_face_base_with_info_2<Face_info,
P_traits>          Fb1;
typedef CGAL::Constrained_triangulation_face_base_2<P_traits, Fb1>   Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>                  TDS;
typedef CGAL::No_intersection_tag                                    Itag;
typedef CGAL::Constrained_Delaunay_triangulation_2<P_traits,
TDS,
Itag>             CDTbase;
typedef CGAL::Constrained_triangulation_plus_2<CDTbase>              CDT;

//Make sure all the facets are triangles

template<typename FaceNormalPmap, typename VertexNormalPmap>
void
Scene_polyhedron_item::triangulate_facet(Facet_iterator fit,
                                         const FaceNormalPmap& fnmap,
                                         const VertexNormalPmap& vnmap,
                                         const bool colors_only) const
{
    //Computes the normal of the facet
    Traits::Vector_3 normal = get(fnmap, fit);

    //check if normal contains NaN values
    if (normal.x() != normal.x() || normal.y() != normal.y() || normal.z() != normal.z())
    {
        qDebug()<<"Warning : normal is not valid. Facet not displayed";
        return;
    }
    P_traits cdt_traits(normal);
    CDT cdt(cdt_traits);

    Facet::Halfedge_around_facet_circulator
            he_circ = fit->facet_begin(),
            he_circ_end(he_circ);

    // Iterates on the vector of facet handles
    typedef boost::graph_traits<Polyhedron>::vertex_descriptor vertex_descriptor;
    boost::container::flat_map<CDT::Vertex_handle, vertex_descriptor> v2v;
    CDT::Vertex_handle previous, first;
    do {
        CDT::Vertex_handle vh = cdt.insert(he_circ->vertex()->point());
        v2v.insert(std::make_pair(vh, he_circ->vertex()));
        if(first == 0) {
            first = vh;
        }
        vh->info() = he_circ;
        if(previous != 0 && previous != vh) {
            cdt.insert_constraint(previous, vh);
        }
        previous = vh;
    } while( ++he_circ != he_circ_end );
    cdt.insert_constraint(previous, first);
    // sets mark is_external
    for(CDT::All_faces_iterator
        fit2 = cdt.all_faces_begin(),
        end = cdt.all_faces_end();
        fit2 != end; ++fit2)
    {
        fit2->info().is_external = false;
    }
    //check if the facet is external or internal
    std::queue<CDT::Face_handle> face_queue;
    face_queue.push(cdt.infinite_vertex()->face());
    while(! face_queue.empty() ) {
        CDT::Face_handle fh = face_queue.front();
        face_queue.pop();
        if(fh->info().is_external) continue;
        fh->info().is_external = true;
        for(int i = 0; i <3; ++i) {
            if(!cdt.is_constrained(std::make_pair(fh, i)))
            {
                face_queue.push(fh->neighbor(i));
            }
        }
    }
    //iterates on the internal faces to add the vertices to the positions
    //and the normals to the appropriate vectors
    const int this_patch_id = fit->patch_id();
    for(CDT::Finite_faces_iterator
        ffit = cdt.finite_faces_begin(),
        end = cdt.finite_faces_end();
        ffit != end; ++ffit)
    {
        if(ffit->info().is_external)
            continue;

        if (!is_monochrome)
        {
          for (int i = 0; i<3; ++i)
          {
            color_facets.push_back(colors_[this_patch_id].redF());
            color_facets.push_back(colors_[this_patch_id].greenF());
            color_facets.push_back(colors_[this_patch_id].blueF());

            color_facets.push_back(colors_[this_patch_id].redF());
            color_facets.push_back(colors_[this_patch_id].greenF());
            color_facets.push_back(colors_[this_patch_id].blueF());
          }
        }
        if (colors_only)
          continue;

        push_back_xyz(ffit->vertex(0)->point(), positions_facets);
        positions_facets.push_back(1.0);

        push_back_xyz(ffit->vertex(1)->point(), positions_facets);
        positions_facets.push_back(1.0);

        push_back_xyz(ffit->vertex(2)->point(), positions_facets);
        positions_facets.push_back(1.0);

        push_back_xyz(normal, normals_flat);
        push_back_xyz(normal, normals_flat);
        push_back_xyz(normal, normals_flat);

        Traits::Vector_3 ng = get(vnmap, v2v[ffit->vertex(0)]);
        push_back_xyz(ng, normals_gouraud);

        ng = get(vnmap, v2v[ffit->vertex(1)]);
        push_back_xyz(ng, normals_gouraud);

        ng = get(vnmap, v2v[ffit->vertex(2)]);
        push_back_xyz(ng, normals_gouraud);
    }
}

#include <QObject>
#include <QMenu>
#include <QAction>

void
Scene_polyhedron_item::initialize_buffers(CGAL::Three::Viewer_interface* viewer) const
{
    //vao containing the data for the facets
    {
        program = getShaderProgram(PROGRAM_WITH_LIGHT, viewer);
        program->bind();
        //flat
        vaos[Facets]->bind();
        buffers[Facets_vertices].bind();
        buffers[Facets_vertices].allocate(positions_facets.data(),
                            static_cast<int>(positions_facets.size()*sizeof(float)));
        program->enableAttributeArray("vertex");
        program->setAttributeBuffer("vertex",GL_FLOAT,0,4);
        buffers[Facets_vertices].release();

        buffers[Facets_normals_flat].bind();
        buffers[Facets_normals_flat].allocate(normals_flat.data(),
                            static_cast<int>(normals_flat.size()*sizeof(float)));
        program->enableAttributeArray("normals");
        program->setAttributeBuffer("normals",GL_FLOAT,0,3);
        buffers[Facets_normals_flat].release();

        if(!is_monochrome)
        {
            buffers[Facets_color].bind();
            buffers[Facets_color].allocate(color_facets.data(),
                                static_cast<int>(color_facets.size()*sizeof(float)));
            program->enableAttributeArray("colors");
            program->setAttributeBuffer("colors",GL_FLOAT,0,3);
            buffers[Facets_color].release();
        }
        vaos[Facets]->release();
        //gouraud
        vaos[Gouraud_Facets]->bind();
        buffers[Facets_vertices].bind();
        program->enableAttributeArray("vertex");
        program->setAttributeBuffer("vertex",GL_FLOAT,0,4);
        buffers[Facets_vertices].release();

        buffers[Facets_normals_gouraud].bind();
        buffers[Facets_normals_gouraud].allocate(normals_gouraud.data(),
                            static_cast<int>(normals_gouraud.size()*sizeof(float)));
        program->enableAttributeArray("normals");
        program->setAttributeBuffer("normals",GL_FLOAT,0,3);
        buffers[Facets_normals_gouraud].release();
        if(!is_monochrome)
        {
            buffers[Facets_color].bind();
            program->enableAttributeArray("colors");
            program->setAttributeBuffer("colors",GL_FLOAT,0,3);
            buffers[Facets_color].release();
        }
        else
        {
            program->disableAttributeArray("colors");
        }
        vaos[Gouraud_Facets]->release();

        program->release();

    }
    //vao containing the data for the lines
    {
        program = getShaderProgram(PROGRAM_WITHOUT_LIGHT, viewer);
        program->bind();
        vaos[Edges]->bind();

        buffers[Edges_vertices].bind();
        buffers[Edges_vertices].allocate(positions_lines.data(),
                            static_cast<int>(positions_lines.size()*sizeof(float)));
        program->enableAttributeArray("vertex");
        program->setAttributeBuffer("vertex",GL_FLOAT,0,4);
        buffers[Edges_vertices].release();

        buffers[Edges_color].bind();
        buffers[Edges_color].allocate(color_lines.data(),
                            static_cast<int>(color_lines.size()*sizeof(float)));
       if(!is_monochrome)
       {
           program->enableAttributeArray("colors");
           program->setAttributeBuffer("colors",GL_FLOAT,0,3);
           buffers[Edges_color].release();
       }
       else
       {
           program->disableAttributeArray("colors");
       }
        program->release();

        vaos[Edges]->release();

    }
  //vao containing the data for the feature_edges
  {
      program = getShaderProgram(PROGRAM_NO_SELECTION, viewer);
      program->bind();
      vaos[Feature_edges]->bind();

      buffers[Feature_edges_vertices].bind();
      buffers[Feature_edges_vertices].allocate(positions_feature_lines.data(),
                          static_cast<int>(positions_feature_lines.size()*sizeof(float)));
      program->enableAttributeArray("vertex");
      program->setAttributeBuffer("vertex",GL_FLOAT,0,4);
      buffers[Feature_edges_vertices].release();
      program->disableAttributeArray("colors");
      program->release();

      vaos[Feature_edges]->release();

  }
    nb_f_lines = positions_feature_lines.size();
    positions_feature_lines.resize(0);
    std::vector<float>(positions_feature_lines).swap(positions_feature_lines);
    nb_lines = positions_lines.size();
    positions_lines.resize(0);
    std::vector<float>(positions_lines).swap(positions_lines);
    nb_facets = positions_facets.size();
    positions_facets.resize(0);
    std::vector<float>(positions_facets).swap(positions_facets);

    color_lines.resize(0);
    std::vector<float>(color_lines).swap(color_lines);
    color_facets.resize(0);
    std::vector<float>(color_facets).swap(color_facets);
    normals_flat.resize(0);
    std::vector<float>(normals_flat).swap(normals_flat);
    normals_gouraud.resize(0);
    std::vector<float>(normals_gouraud).swap(normals_gouraud);
    are_buffers_filled = true;
}

void
Scene_polyhedron_item::compute_normals_and_vertices(const bool colors_only) const
{
    positions_facets.resize(0);
    positions_lines.resize(0);
    positions_feature_lines.resize(0);
    normals_flat.resize(0);
    normals_gouraud.resize(0);
    color_lines.resize(0);
    color_facets.resize(0);

    //Facets
    typedef Polyhedron::Traits	    Kernel;
    typedef Kernel::Point_3	    Point;
    typedef Kernel::Vector_3	    Vector;
    typedef Polyhedron::Facet_iterator Facet_iterator;
    typedef Polyhedron::Halfedge_around_facet_circulator HF_circulator;
    typedef boost::graph_traits<Polyhedron>::face_descriptor   face_descriptor;
    typedef boost::graph_traits<Polyhedron>::vertex_descriptor vertex_descriptor;

    boost::container::flat_map<face_descriptor, Vector> face_normals_map;
    boost::associative_property_map< boost::container::flat_map<face_descriptor, Vector> >
      nf_pmap(face_normals_map);
    boost::container::flat_map<vertex_descriptor, Vector> vertex_normals_map;
    boost::associative_property_map< boost::container::flat_map<vertex_descriptor, Vector> >
      nv_pmap(vertex_normals_map);

    PMP::compute_normals(*poly, nv_pmap, nf_pmap);

    Facet_iterator f = poly->facets_begin();
    for(f = poly->facets_begin();
        f != poly->facets_end();
        f++)
    {
      if (f == boost::graph_traits<Polyhedron>::null_face())
        continue;

      if(is_triangle(f->halfedge(),*poly))
      {
          const int this_patch_id = f->patch_id();
          Vector n = get(nf_pmap, f);
          HF_circulator he = f->facet_begin();
          HF_circulator end = he;
          CGAL_For_all(he,end)
          {
            if (!is_monochrome)
            {
              color_facets.push_back(colors_[this_patch_id].redF());
              color_facets.push_back(colors_[this_patch_id].greenF());
              color_facets.push_back(colors_[this_patch_id].blueF());
            }
            if (colors_only)
              continue;

            // If Flat shading:1 normal per polygon added once per vertex
            push_back_xyz(n, normals_flat);

            //// If Gouraud shading: 1 normal per vertex
            Vector nv = get(nv_pmap, he->vertex());
            push_back_xyz(nv, normals_gouraud);

            //position
            const Point& p = he->vertex()->point();
            push_back_xyz(p, positions_facets);
            positions_facets.push_back(1.0);
         }
      }
      else if (is_quad(f->halfedge(), *poly))
      {
        if (!is_monochrome)
        {
          const int this_patch_id = f->patch_id();
          for (unsigned int i = 0; i < 6; ++i)
          { //6 "halfedges" for the quad, because it is 2 triangles
            color_facets.push_back(colors_[this_patch_id].redF());
            color_facets.push_back(colors_[this_patch_id].greenF());
            color_facets.push_back(colors_[this_patch_id].blueF());
          }
        }
        if (colors_only)
          continue;

        Vector nf = get(nf_pmap, f);

        //1st half-quad
        Point p0 = f->halfedge()->vertex()->point();
        Point p1 = f->halfedge()->next()->vertex()->point();
        Point p2 = f->halfedge()->next()->next()->vertex()->point();

        push_back_xyz(p0, positions_facets);
        positions_facets.push_back(1.0);

        push_back_xyz(p1, positions_facets);
        positions_facets.push_back(1.0);

        push_back_xyz(p2, positions_facets);
        positions_facets.push_back(1.0);

        push_back_xyz(nf, normals_flat);
        push_back_xyz(nf, normals_flat);
        push_back_xyz(nf, normals_flat);

        Vector nv = get(nv_pmap, f->halfedge()->vertex());
        push_back_xyz(nv, normals_gouraud);

        nv = get(nv_pmap, f->halfedge()->next()->vertex());
        push_back_xyz(nv, normals_gouraud);

        nv = get(nv_pmap, f->halfedge()->next()->next()->vertex());
        push_back_xyz(nv, normals_gouraud);

        //2nd half-quad
        p0 = f->halfedge()->next()->next()->vertex()->point();
        p1 = f->halfedge()->prev()->vertex()->point();
        p2 = f->halfedge()->vertex()->point();

        push_back_xyz(p0, positions_facets);
        positions_facets.push_back(1.0);

        push_back_xyz(p1, positions_facets);
        positions_facets.push_back(1.0);

        push_back_xyz(p2, positions_facets);
        positions_facets.push_back(1.0);

        push_back_xyz(nf, normals_flat);
        push_back_xyz(nf, normals_flat);
        push_back_xyz(nf, normals_flat);

        nv = get(nv_pmap, f->halfedge()->next()->next()->vertex());
        push_back_xyz(nv, normals_gouraud);

        nv = get(nv_pmap, f->halfedge()->prev()->vertex());
        push_back_xyz(nv, normals_gouraud);

        nv = get(nv_pmap, f->halfedge()->vertex());
        push_back_xyz(nv, normals_gouraud);
      }
      else
      {
        triangulate_facet(f, nf_pmap, nv_pmap, colors_only);
      }

    }
    //Lines
    typedef Kernel::Point_3		Point;
    typedef Polyhedron::Edge_iterator	Edge_iterator;
    Edge_iterator he;
    for(he = poly->edges_begin();
        he != poly->edges_end();
        he++)
    {
        const Point& a = he->vertex()->point();
        const Point& b = he->opposite()->vertex()->point();
        if ( he->is_feature_edge())
        {
          if (colors_only)
            continue;

          push_back_xyz(a, positions_feature_lines);
          positions_feature_lines.push_back(1.0);

          push_back_xyz(b, positions_feature_lines);
          positions_feature_lines.push_back(1.0);
        }
        else
        {
          if (!is_monochrome)
          {
            color_lines.push_back(this->color().lighter(50).redF());
            color_lines.push_back(this->color().lighter(50).greenF());
            color_lines.push_back(this->color().lighter(50).blueF());

            color_lines.push_back(this->color().lighter(50).redF());
            color_lines.push_back(this->color().lighter(50).greenF());
            color_lines.push_back(this->color().lighter(50).blueF());
          }
          if (colors_only)
            continue;

          push_back_xyz(a, positions_lines);
          positions_lines.push_back(1.0);

          push_back_xyz(b, positions_lines);
          positions_lines.push_back(1.0);
        }
    }
}

Scene_polyhedron_item::Scene_polyhedron_item()
    : Scene_item(NbOfVbos,NbOfVaos),
      poly(new Polyhedron),
      show_only_feature_edges_m(false),
      show_feature_edges_m(false),
      facet_picking_m(false),
      erase_next_picked_facet_m(false),
      plugin_has_set_color_vector_m(false)
{
    cur_shading=FlatPlusEdges;
    is_selected = true;
    nb_facets = 0;
    nb_lines = 0;
    nb_f_lines = 0;
    init();
}

Scene_polyhedron_item::Scene_polyhedron_item(Polyhedron* const p)
    : Scene_item(NbOfVbos,NbOfVaos),
      poly(p),
      show_only_feature_edges_m(false),
      show_feature_edges_m(false),
      facet_picking_m(false),
      erase_next_picked_facet_m(false),
      plugin_has_set_color_vector_m(false)
{
    cur_shading=FlatPlusEdges;
    is_selected = true;
    nb_facets = 0;
    nb_lines = 0;
    nb_f_lines = 0;
    init();
    invalidateOpenGLBuffers();
}

Scene_polyhedron_item::Scene_polyhedron_item(const Polyhedron& p)
    : Scene_item(NbOfVbos,NbOfVaos),
      poly(new Polyhedron(p)),
      show_only_feature_edges_m(false),
      show_feature_edges_m(false),
      facet_picking_m(false),
      erase_next_picked_facet_m(false),
      plugin_has_set_color_vector_m(false)
{
    //setItemIsMulticolor(true);
    cur_shading=FlatPlusEdges;
    is_selected=true;
    init();
    nb_facets = 0;
    nb_lines = 0;
    nb_f_lines = 0;
    invalidateOpenGLBuffers();
}

Scene_polyhedron_item::~Scene_polyhedron_item()
{
    delete_aabb_tree(this);
    delete poly;
}

#include "Color_map.h"

void
Scene_polyhedron_item::
init()
{
  typedef Polyhedron::Facet_iterator Facet_iterator;

  if ( !plugin_has_set_color_vector_m )
  {
    // Fill indices map and get max subdomain value
    int max = 0;
    for(Facet_iterator fit = poly->facets_begin(), end = poly->facets_end() ;
        fit != end; ++fit)
    {
      max = (std::max)(max, fit->patch_id());
    }

    colors_.resize(0);
    compute_color_map(this->color(), max + 1,
                      std::back_inserter(colors_));
  }
  invalidate_stats();
}

void
Scene_polyhedron_item::
invalidate_stats()
{
  number_of_degenerated_faces = (unsigned int)(-1);
  number_of_null_length_edges = (unsigned int)(-1);
  volume = -std::numeric_limits<double>::infinity();
  area = -std::numeric_limits<double>::infinity();
  self_intersect = false;

}

Scene_polyhedron_item*
Scene_polyhedron_item::clone() const {
    return new Scene_polyhedron_item(*poly);}

// Load polyhedron from .OFF file
bool
Scene_polyhedron_item::load(std::istream& in)
{

    in >> *poly;

    if ( in && !isEmpty() )
    {
        invalidateOpenGLBuffers();
        return true;
    }
    return false;
}
// Load polyhedron from .obj file
bool
Scene_polyhedron_item::load_obj(std::istream& in)
{
  typedef Polyhedron::Vertex::Point Point;
  std::vector<Point> points;
  std::vector<std::vector<std::size_t> > faces;

  Point p;
  std::string line;
  std::size_t i, j, k;
  bool failed = false;
  while(getline(in, line)){
    if(line[0] == 'v'){
      std::istringstream iss(line.substr(1));
      iss >> p;
      if(! iss) failed = true;
      points.push_back(p);
    } else if(line[0] == 'f'){
      std::istringstream iss(line.substr(1));
      iss >> i >> j >> k;
      if(! iss) failed = true;
      std::vector<std::size_t> face;
      face.push_back(i-1);
      face.push_back(j-1);
      face.push_back(k-1);
      faces.push_back(face);
    }
  }
  if(CGAL::Polygon_mesh_processing::orient_polygon_soup(points,faces)){
    CGAL::Polygon_mesh_processing::polygon_soup_to_polygon_mesh( points,faces,*poly);
  }else{
    std::cerr << "not orientable"<< std::endl;
    return false;
  }
    if ( (! failed) && !isEmpty() )
    {
        invalidateOpenGLBuffers();
        return true;
    }
    return false;
}

// Write polyhedron to .OFF file
bool
Scene_polyhedron_item::save(std::ostream& out) const
{
  out.precision(17);
    out << *poly;
    return (bool) out;
}

QString
Scene_polyhedron_item::toolTip() const
{
    if(!poly)
        return QString();

  QString str =
         QObject::tr("<p>Polyhedron <b>%1</b> (mode: %5, color: %6)</p>"
                       "<p>Number of vertices: %2<br />"
                       "Number of edges: %3<br />"
                     "Number of facets: %4")
            .arg(this->name())
            .arg(poly->size_of_vertices())
            .arg(poly->size_of_halfedges()/2)
            .arg(poly->size_of_facets())
            .arg(this->renderingModeName())
            .arg(this->color().name());
  str += QString("<br />Number of isolated vertices : %1<br />").arg(getNbIsolatedvertices());
  return str;
}

QMenu* Scene_polyhedron_item::contextMenu()
{
  const char* prop_name = "Menu modified by Scene_polyhedron_item.";

  QMenu* menu = Scene_item::contextMenu();

  // Use dynamic properties:
  // http://doc.qt.io/qt-5/qobject.html#property
  bool menuChanged = menu->property(prop_name).toBool();

  if(!menuChanged) {

    QAction* actionShowOnlyFeatureEdges =
        menu->addAction(tr("Show Only &Feature Edges"));
    actionShowOnlyFeatureEdges->setCheckable(true);
    actionShowOnlyFeatureEdges->setChecked(show_only_feature_edges_m);
    actionShowOnlyFeatureEdges->setObjectName("actionShowOnlyFeatureEdges");
    connect(actionShowOnlyFeatureEdges, SIGNAL(toggled(bool)),
            this, SLOT(show_only_feature_edges(bool)));

    QAction* actionShowFeatureEdges =
        menu->addAction(tr("Show Feature Edges"));
    actionShowFeatureEdges->setCheckable(true);
    actionShowFeatureEdges->setChecked(show_feature_edges_m);
    actionShowFeatureEdges->setObjectName("actionShowFeatureEdges");
    connect(actionShowFeatureEdges, SIGNAL(toggled(bool)),
            this, SLOT(show_feature_edges(bool)));

    QAction* actionPickFacets =
        menu->addAction(tr("Facets Picking"));
    actionPickFacets->setCheckable(true);
    actionPickFacets->setObjectName("actionPickFacets");
    connect(actionPickFacets, SIGNAL(toggled(bool)),
            this, SLOT(enable_facets_picking(bool)));

    QAction* actionEraseNextFacet =
        menu->addAction(tr("Erase Next Picked Facet"));
    actionEraseNextFacet->setCheckable(true);
    actionEraseNextFacet->setObjectName("actionEraseNextFacet");
    connect(actionEraseNextFacet, SIGNAL(toggled(bool)),
            this, SLOT(set_erase_next_picked_facet(bool)));
    menu->setProperty(prop_name, true);
  }

  QAction* action = menu->findChild<QAction*>("actionShowOnlyFeatureEdges");
  if(action) action->setChecked(show_only_feature_edges_m);
  action = menu->findChild<QAction*>("actionShowFeatureEdges");
  if(action) action->setChecked(show_feature_edges_m);
  action = menu->findChild<QAction*>("actionPickFacets");
  if(action) action->setChecked(facet_picking_m);
  action = menu->findChild<QAction*>("actionEraseNextFacet");
  if(action) action->setChecked(erase_next_picked_facet_m);
  return menu;
}

void Scene_polyhedron_item::show_only_feature_edges(bool b)
{
    show_only_feature_edges_m = b;
    invalidateOpenGLBuffers();
    Q_EMIT itemChanged();
}

void Scene_polyhedron_item::show_feature_edges(bool b)
{
  show_feature_edges_m = b;
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}

void Scene_polyhedron_item::enable_facets_picking(bool b)
{
    facet_picking_m = b;
}

void Scene_polyhedron_item::set_erase_next_picked_facet(bool b)
{
    if(b) { facet_picking_m = true; } // automatically activate facet_picking
    erase_next_picked_facet_m = b;
}

void Scene_polyhedron_item::draw(CGAL::Three::Viewer_interface* viewer) const {
    if(!are_buffers_filled)
    {
        compute_normals_and_vertices();
        initialize_buffers(viewer);
        compute_bbox();
    }

    if(renderingMode() == Flat || renderingMode() == FlatPlusEdges)
        vaos[Facets]->bind();
    else
    {
        vaos[Gouraud_Facets]->bind();
    }
    attrib_buffers(viewer, PROGRAM_WITH_LIGHT);
    program = getShaderProgram(PROGRAM_WITH_LIGHT);
    program->bind();
    if(is_monochrome)
    {
            program->setAttributeValue("colors", this->color());
    }
    if(is_selected)
            program->setUniformValue("is_selected", true);
    else
            program->setUniformValue("is_selected", false);
    viewer->glDrawArrays(GL_TRIANGLES, 0, static_cast<GLsizei>(nb_facets/4));
    program->release();
    if(renderingMode() == Flat || renderingMode() == FlatPlusEdges)
        vaos[Facets]->release();
    else
        vaos[Gouraud_Facets]->release();
}

// Points/Wireframe/Flat/Gouraud OpenGL drawing in a display list
void Scene_polyhedron_item::draw_edges(CGAL::Three::Viewer_interface* viewer) const
{
    if (!are_buffers_filled)
    {
        compute_normals_and_vertices();
        initialize_buffers(viewer);
        compute_bbox();
    }

    if(!show_only_feature_edges_m)
    {
        vaos[Edges]->bind();

        attrib_buffers(viewer, PROGRAM_WITHOUT_LIGHT);
        program = getShaderProgram(PROGRAM_WITHOUT_LIGHT);
        program->bind();
        //draw the edges
        if(is_monochrome)
        {
            program->setAttributeValue("colors", this->color().lighter(50));
            if(is_selected)
                program->setUniformValue("is_selected", true);
            else
                program->setUniformValue("is_selected", false);
        }
        viewer->glDrawArrays(GL_LINES, 0, static_cast<GLsizei>(nb_lines/4));
        program->release();
        vaos[Edges]->release();
    }

    //draw the feature edges
    vaos[Feature_edges]->bind();
    attrib_buffers(viewer, PROGRAM_NO_SELECTION);
    program = getShaderProgram(PROGRAM_NO_SELECTION);
    program->bind();
    if(show_feature_edges_m || show_only_feature_edges_m)
        program->setAttributeValue("colors", Qt::red);
    else
    {
        if(!is_selected)
            program->setAttributeValue("colors", this->color().lighter(50));
        else
            program->setAttributeValue("colors",QColor(0,0,0));
    }
    viewer->glDrawArrays(GL_LINES, 0, static_cast<GLsizei>(nb_f_lines/4));
    program->release();
    vaos[Feature_edges]->release();
    }

void
Scene_polyhedron_item::draw_points(CGAL::Three::Viewer_interface* viewer) const {
    if(!are_buffers_filled)
    {
        compute_normals_and_vertices();
        initialize_buffers(viewer);
        compute_bbox();
    }

    vaos[Edges]->bind();
    attrib_buffers(viewer, PROGRAM_WITHOUT_LIGHT);
    program = getShaderProgram(PROGRAM_WITHOUT_LIGHT);
    program->bind();
    //draw the points
    viewer->glDrawArrays(GL_POINTS, 0, static_cast<GLsizei>(nb_lines/4));
    // Clean-up
    program->release();
    vaos[Edges]->release();
}

Polyhedron*
Scene_polyhedron_item::polyhedron()       { return poly; }
const Polyhedron*
Scene_polyhedron_item::polyhedron() const { return poly; }

bool
Scene_polyhedron_item::isEmpty() const {
    return (poly == 0) || poly->empty();
}

void Scene_polyhedron_item::compute_bbox() const {
    const Kernel::Point_3& p = *(poly->points_begin());
    CGAL::Bbox_3 bbox(p.x(), p.y(), p.z(), p.x(), p.y(), p.z());
    for(Polyhedron::Point_iterator it = poly->points_begin();
        it != poly->points_end();
        ++it) {
        bbox = bbox + it->bbox();
    }
    _bbox = Bbox(bbox.xmin(),bbox.ymin(),bbox.zmin(),
                bbox.xmax(),bbox.ymax(),bbox.zmax());
}

void
Scene_polyhedron_item::
invalidateOpenGLBuffers()
{
  Q_EMIT item_is_about_to_be_changed();
    delete_aabb_tree(this);
    init();
    Base::invalidateOpenGLBuffers();
    are_buffers_filled = false;

    invalidate_stats();
}

void
Scene_polyhedron_item::selection_changed(bool p_is_selected)
{
    if(p_is_selected != is_selected)
    {
        is_selected = p_is_selected;
    }

}

void
Scene_polyhedron_item::setColor(QColor c)
{
  // reset patch ids
  if (colors_.size()>2 || plugin_has_set_color_vector_m)
  {
    BOOST_FOREACH(Polyhedron::Facet_handle fh, faces(*poly))
      fh->set_patch_id(1);
    colors_[1]=c;
  }
  Scene_item::setColor(c);
}

void
Scene_polyhedron_item::select(double orig_x,
                              double orig_y,
                              double orig_z,
                              double dir_x,
                              double dir_y,
                              double dir_z)
{
    if(facet_picking_m) {
        typedef Input_facets_AABB_tree Tree;
        typedef Tree::Object_and_primitive_id Object_and_primitive_id;

        Tree* aabb_tree = get_aabb_tree(this);
        if(aabb_tree) {
            const Kernel::Point_3 ray_origin(orig_x, orig_y, orig_z);
            const Kernel::Vector_3 ray_dir(dir_x, dir_y, dir_z);
            const Kernel::Ray_3 ray(ray_origin, ray_dir);
            typedef std::list<Object_and_primitive_id> Intersections;
            Intersections intersections;
            aabb_tree->all_intersections(ray, std::back_inserter(intersections));
            Intersections::iterator closest = intersections.begin();
            if(closest != intersections.end()) {
                const Kernel::Point_3* closest_point =
                        CGAL::object_cast<Kernel::Point_3>(&closest->first);
                for(Intersections::iterator
                    it = boost::next(intersections.begin()),
                    end = intersections.end();
                    it != end; ++it)
                {
                    if(! closest_point) {
                        closest = it;
                    }
                    else {
                        const Kernel::Point_3* it_point =
                                CGAL::object_cast<Kernel::Point_3>(&it->first);
                        if(it_point &&
                                (ray_dir * (*it_point - *closest_point)) < 0)
                        {
                            closest = it;
                            closest_point = it_point;
                        }
                    }
                }
                if(closest_point) {
                    Polyhedron::Facet_handle selected_fh = closest->second;

                    // The computation of the nearest vertex may be costly.  Only
                    // do it if some objects are connected to the signal
                    // 'selected_vertex'.
                    if(QObject::receivers(SIGNAL(selected_vertex(void*))) > 0)
                    {
                        Polyhedron::Halfedge_around_facet_circulator
                                he_it = selected_fh->facet_begin(),
                                around_end = he_it;

                        Polyhedron::Vertex_handle v = he_it->vertex(), nearest_v = v;

                        Kernel::FT sq_dist = CGAL::squared_distance(*closest_point,
                                                                    v->point());
                        while(++he_it != around_end) {
                            v = he_it->vertex();
                            Kernel::FT new_sq_dist = CGAL::squared_distance(*closest_point,
                                                                            v->point());
                            if(new_sq_dist < sq_dist) {
                                sq_dist = new_sq_dist;
                                nearest_v = v;
                            }
                        }
                        //bottleneck
            Q_EMIT selected_vertex((void*)(&*nearest_v));
                    }

                    if(QObject::receivers(SIGNAL(selected_edge(void*))) > 0
                            || QObject::receivers(SIGNAL(selected_halfedge(void*))) > 0)
                    {
                        Polyhedron::Halfedge_around_facet_circulator
                                he_it = selected_fh->facet_begin(),
                                around_end = he_it;

                        Polyhedron::Halfedge_handle nearest_h = he_it;
                        Kernel::FT sq_dist = CGAL::squared_distance(*closest_point,
                                                                    Kernel::Segment_3(he_it->vertex()->point(), he_it->opposite()->vertex()->point()));

                        while(++he_it != around_end) {
                            Kernel::FT new_sq_dist = CGAL::squared_distance(*closest_point,
                                                                            Kernel::Segment_3(he_it->vertex()->point(), he_it->opposite()->vertex()->point()));
                            if(new_sq_dist < sq_dist) {
                                sq_dist = new_sq_dist;
                                nearest_h = he_it;
                            }
                        }

            Q_EMIT selected_halfedge((void*)(&*nearest_h));
            Q_EMIT selected_edge((void*)(std::min)(&*nearest_h, &*nearest_h->opposite()));
                    }

          Q_EMIT selected_facet((void*)(&*selected_fh));
                    if(erase_next_picked_facet_m) {
                        polyhedron()->erase_facet(selected_fh->halfedge());
                        polyhedron()->normalize_border();
                        //set_erase_next_picked_facet(false);
                        invalidateOpenGLBuffers();
            Q_EMIT itemChanged();
                    }
                }
            }
        }
    }
    Base::select(orig_x, orig_y, orig_z, dir_x, dir_y, dir_z);
}

void Scene_polyhedron_item::update_vertex_indices()
{
    std::size_t id=0;
    for (Polyhedron::Vertex_iterator vit = polyhedron()->vertices_begin(),
         vit_end = polyhedron()->vertices_end(); vit != vit_end; ++vit)
    {
        vit->id()=id++;
    }
}
void Scene_polyhedron_item::update_facet_indices()
{
    std::size_t id=0;
    for (Polyhedron::Facet_iterator  fit = polyhedron()->facets_begin(),
         fit_end = polyhedron()->facets_end(); fit != fit_end; ++fit)
    {
        fit->id()=id++;
    }
}
void Scene_polyhedron_item::update_halfedge_indices()
{
    std::size_t id=0;
    for (Polyhedron::Halfedge_iterator hit = polyhedron()->halfedges_begin(),
         hit_end = polyhedron()->halfedges_end(); hit != hit_end; ++hit)
    {
        hit->id()=id++;
    }
}
void Scene_polyhedron_item::invalidate_aabb_tree()
{
  delete_aabb_tree(this);
}
QString Scene_polyhedron_item::compute_stats(int type)
{
  double minl, maxl, meanl, midl;
  switch (type)
  {
  case MIN_LENGTH:
  case MAX_LENGTH:
  case MID_LENGTH:
  case MEAN_LENGTH:
  case NB_NULL_LENGTH:
    poly->normalize_border();
    edges_length(poly, minl, maxl, meanl, midl, number_of_null_length_edges);
  }

  double mini, maxi, ave;
  switch (type)
  {
  case MIN_ANGLE:
  case MAX_ANGLE:
  case MEAN_ANGLE:
    angles(poly, mini, maxi, ave);
  }

  switch(type)
  {
  case NB_VERTICES:
    return QString::number(poly->size_of_vertices());

  case NB_FACETS:
    return QString::number(poly->size_of_facets());

  case NB_CONNECTED_COMPOS:
  {
    typedef boost::graph_traits<Polyhedron>::face_descriptor face_descriptor;
    int i = 0;
    BOOST_FOREACH(face_descriptor f, faces(*poly)){
      f->id() = i++;
    }
    boost::vector_property_map<int,
      boost::property_map<Polyhedron, boost::face_index_t>::type>
      fccmap(get(boost::face_index, *poly));
    return QString::number(PMP::connected_components(*poly, fccmap));
  }
  case NB_BORDER_EDGES:
    poly->normalize_border();
    return QString::number(poly->size_of_border_halfedges());

  case NB_EDGES:
    return QString::number(poly->size_of_halfedges() / 2);

  case NB_DEGENERATED_FACES:
  {
    if (poly->is_pure_triangle())
    {
      if (number_of_degenerated_faces == (unsigned int)(-1))
        number_of_degenerated_faces = nb_degenerate_faces(poly, get(CGAL::vertex_point, *poly));
      return QString::number(number_of_degenerated_faces);
    }
    else
      return QString("n/a");
  }
  case AREA:
  {
    if (poly->is_pure_triangle())
    {
      if(area == -std::numeric_limits<double>::infinity())
        area = CGAL::Polygon_mesh_processing::area(*poly);
      return QString::number(area);
    }
    else
      return QString("n/a");
  }
  case VOLUME:
  {
    if (poly->is_pure_triangle() && poly->is_closed())
    {
      if (volume == -std::numeric_limits<double>::infinity())
        volume = CGAL::Polygon_mesh_processing::volume(*poly);
      return QString::number(volume);
    }
    else
      return QString("n/a");
  }
  case SELFINTER:
  {
    //todo : add a test about cache validity
    if (poly->is_pure_triangle())
      self_intersect = CGAL::Polygon_mesh_processing::does_self_intersect(*poly);
    if (self_intersect)
      return QString("Yes");
    else if (poly->is_pure_triangle())
      return QString("No");
    else
      return QString("n/a");
  }
  case MIN_LENGTH:
    return QString::number(minl);
  case MAX_LENGTH:
    return QString::number(maxl);
  case MID_LENGTH:
    return QString::number(midl);
  case MEAN_LENGTH:
    return QString::number(meanl);
  case NB_NULL_LENGTH:
    return QString::number(number_of_null_length_edges);

  case MIN_ANGLE:
    return QString::number(mini);
  case MAX_ANGLE:
    return QString::number(maxi);
  case MEAN_ANGLE:
    return QString::number(ave);

  case HOLES:
    return QString::number(nb_holes(poly));
  }
  return QString();
}

CGAL::Three::Scene_item::Header_data Scene_polyhedron_item::header() const
{
  CGAL::Three::Scene_item::Header_data data;
  //categories
  data.categories.append(std::pair<QString,int>(QString("Properties"),9));
  data.categories.append(std::pair<QString,int>(QString("Edges"),6));
  data.categories.append(std::pair<QString,int>(QString("Angles"),3));

  //titles
  data.titles.append(QString("#Vertices"));
  data.titles.append(QString("#Facets"));
  data.titles.append(QString("#Connected Components"));
  data.titles.append(QString("#Border Edges"));
  data.titles.append(QString("#Degenerated Faces"));
  data.titles.append(QString("Connected Components of the Boundary"));
  data.titles.append(QString("Area"));
  data.titles.append(QString("Volume"));
  data.titles.append(QString("Self-Intersecting"));
  data.titles.append(QString("#Edges"));
  data.titles.append(QString("Minimum Length"));
  data.titles.append(QString("Maximum Length"));
  data.titles.append(QString("Median Length"));
  data.titles.append(QString("Mean Length"));
  data.titles.append(QString("#Null Length"));
  data.titles.append(QString("Minimum"));
  data.titles.append(QString("Maximum"));
  data.titles.append(QString("Average"));
  return data;
}


#include <boost/container_hash/hash.hpp>

#include <algorithm>
#include <iomanip>
#include <iostream>
#include <regex>
#include <unordered_set>
#include <vector>

static std::regex RX{"^([0-9-]+),([0-9-]+),([0-9-]+),([0-9-]+)"};

struct Point {
    // Point() = default;
    // Point(int64_t _i, int64_t _j, int64_t _k, int64_t _l) noexcept : i(_i), j(_j), k(_k), l(_l) {}

    int64_t i = 0, j = 0, k = 0, l = 0;
};

int64_t star_distance(Point const& a, Point const& b) noexcept {
    return std::abs(a.i - b.i) + std::abs(a.j - b.j) + std::abs(a.k - b.k) + std::abs(a.l - b.l);
}

bool operator< (Point const& a, Point const& b) noexcept {
    return star_distance(a, {}) < star_distance(b, {});
}

bool operator== (Point const& a, Point const& b) noexcept {
    return a.i == b.i && a.j == b.j && a.k == b.k && a.l == b.l;
}

std::string to_string(Point const& p) {
    return "(" + std::to_string(p.i).append(",") + std::to_string(p.j).append(",") + std::to_string(p.k).append(",") +
           std::to_string(p.l) + ")";
}

namespace std {

template <>
struct hash<Point>
{
    size_t operator()(Point const& p) const noexcept
    {
        size_t seed = 0;
        boost::hash_combine(seed, p.i);
        boost::hash_combine(seed, p.j);
        boost::hash_combine(seed, p.k);
        boost::hash_combine(seed, p.l);
        return seed;
    }
};

}  // namespace std

using Group = std::vector<Point>;
using Constellation = std::unordered_set<Point>;

auto measure(Group const& g)
{
    long min_i = std::numeric_limits<long>::max();
    long min_j = std::numeric_limits<long>::max();
    long min_k = std::numeric_limits<long>::max();
    long min_l = std::numeric_limits<long>::max();
    long max_i = std::numeric_limits<long>::min();
    long max_j = std::numeric_limits<long>::min();
    long max_k = std::numeric_limits<long>::min();
    long max_l = std::numeric_limits<long>::min();
    for ( auto const& b : g ) {
        min_i = std::min(min_i, b.i);
        min_j = std::min(min_j, b.j);
        min_k = std::min(min_k, b.k);
        min_l = std::min(min_l, b.l);
        max_i = std::max(max_i, b.i);
        max_j = std::max(max_j, b.j);
        max_k = std::max(max_k, b.k);
        max_l = std::max(max_l, b.l);
    }
    ++max_i;
    ++max_j;
    ++max_k;
    ++max_l;
    return std::make_tuple(min_i, max_i, min_j, max_j, min_k, max_k, min_l, max_l);
}

int main()
{
    Group stars;

    std::string line;
    while(std::getline(std::cin, line)) {
        if (line.empty()) break;
        std::smatch m;
        if (!std::regex_match(line, m, RX)) {
            std::cout << "ERROR on line " << line << "\n";
            exit(1);
        }
        stars.push_back(Point{std::stoll(m.str(1)), std::stoll(m.str(2)), std::stoll(m.str(3)), std::stoll(m.str(4))});
    }

    std::vector<Constellation> constellations;

    for(Point p0 : stars) {
        bool added = false;
        for (auto& c : constellations) {
            if ( std::any_of(c.begin(), c.end(), [&p0](Point const& oth) { return star_distance(p0, oth) <= 3; }) ) {
                c.insert(p0);
                added = true;
            }
        }
        if (!added) {
            constellations.push_back({});
            constellations.back().insert(std::move(p0));
        }
    }

    for (auto i1 = constellations.begin(); i1 != std::prev(constellations.end()); ++i1) {
        for (auto i2 = std::next(i1); i2 != constellations.end(); ) {
            Constellation g0 = *i1;
            std::copy(i2->begin(), i2->end(), std::inserter(g0, g0.end()));
            if ( g0.size() < (i1->size() + i2->size()) ) {
                *i1 = std::move(g0);
                constellations.erase(i2);
                i2 = std::next(i1);
            } else {
                ++i2;
            }
        }
    }

    std::cout << "1 Result=" << constellations.size() << "\n";

    return 0;
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/frexp.hpp>
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/std.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/constant/mindenormal.hpp>
#include <boost/simd/detail/constant/minexponent.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <utility>
#include <tuple>

STF_CASE_TPL(" frexp0", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::frexp;
  using iT = bd::as_integer_t<T,signed>;

  {
    namespace bs = boost::simd;
    iT e;
    T  m;
    T  a = bs::Valmax<T>();
    std::tie(m, e) = frexp(a);
    STF_ULP_EQUAL(m, bs::One<T>()-bs::Halfeps<T>(), 1);
    STF_EQUAL(e, bs::Limitexponent<T>());
    STF_EQUAL(ldexp(m,e),a);
  }

#ifndef BOOST_SIMD_NO_DENORMALS
  {
    namespace bs = boost::simd;
    iT e;
    T  m;
    T a = bs::Mindenormal<T>();
    std::tie(m, e) = frexp(a);
    STF_ULP_EQUAL(m, bs::Half<T>(), 1);
    STF_EQUAL(e, bs::Minexponent<T>()-bs::Nbmantissabits<T>()+bs::One<T>());
    STF_EQUAL(ldexp(m,e),a);
  }

  {
    namespace bs = boost::simd;
    iT e;
    T  m;
    T a = bs::Smallestposval<T>()/2;
    std::tie(m, e) = frexp(a);
    STF_ULP_EQUAL(m, bs::Half<T>(), 1);
    STF_EQUAL(e, bs::Minexponent<T>());
    STF_EQUAL(ldexp(m,e),a);
  }

  {
    namespace bs = boost::simd;
    iT e;
    T  m;
    T a = bs::Smallestposval<T>()/4;
    std::tie(m, e) = frexp(a);
    STF_ULP_EQUAL(m, bs::Half<T>(), 1);
    STF_EQUAL(e, bs::Minexponent<T>()-bs::One<T>());
    STF_EQUAL(ldexp(m,e),a);
  }
#endif

}

STF_CASE_TPL(" frexp", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::frexp;
  using iT = bd::as_integer_t<T,signed>;

  STF_EXPR_IS( (frexp(T()))
             , (std::pair<T,iT>)
             );

  {
    namespace bs = boost::simd;
    iT e;
    T  m;

    std::tie(m, e) = frexp(bs::One<T>());
    STF_EQUAL(m, bs::Half<T>());
    STF_EQUAL(e, bs::One<iT>());
  }

  {
    namespace bs = boost::simd;
    std::pair<T,iT> p;

    p = frexp(bs::One<T>());
    STF_EQUAL(p.first  , bs::Half<T>());
    STF_EQUAL(p.second , bs::One<iT>());
  }
}

STF_CASE_TPL(" frexp0", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::frexp;
  using iT = bd::as_integer_t<T,signed>;

  {
    namespace bs = boost::simd;
    iT e;
    T  m;
    T  a = bs::Valmax<T>();
    std::tie(m, e) = frexp(a);
    STF_ULP_EQUAL(m, bs::One<T>()-bs::Halfeps<T>(), 1);
    STF_EQUAL(e, bs::Limitexponent<T>());
    STF_EQUAL(ldexp(m,e),a);
  }

#ifndef BOOST_SIMD_NO_DENORMALS
  {
    namespace bs = boost::simd;
    iT e;
    T  m;
    T a = bs::Mindenormal<T>();
    std::tie(m, e) = frexp(a);
    STF_ULP_EQUAL(m, bs::Half<T>(), 1);
    STF_EQUAL(e, bs::Minexponent<T>()-bs::Nbmantissabits<T>()+bs::One<T>());
    STF_EQUAL(ldexp(m,e),a);
  }

  {
    namespace bs = boost::simd;
    iT e;
    T  m;
    T a = bs::Smallestposval<T>()/2;
    std::tie(m, e) = frexp(a);
    STF_ULP_EQUAL(m, bs::Half<T>(), 1);
    STF_EQUAL(e, bs::Minexponent<T>());
    STF_EQUAL(ldexp(m,e),a);
  }

  {
    namespace bs = boost::simd;
    iT e;
    T  m;
    T a = bs::Smallestposval<T>()/4;
    std::tie(m, e) = frexp(a);
    STF_ULP_EQUAL(m, bs::Half<T>(), 1);
    STF_EQUAL(e, bs::Minexponent<T>()-bs::One<T>());
    STF_EQUAL(ldexp(m,e),a);
  }
#endif

}

STF_CASE_TPL(" frexp fast", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::frexp;
  using bs::fast_;
  using iT = bd::as_integer_t<T,signed>;

  STF_EXPR_IS( (bs::fast_(frexp)(T()))
             , (std::pair<T,iT>)
             );

  {
    namespace bs = boost::simd;
    iT e;
    T  m;

    std::tie(m, e) = bs::fast_(frexp)(bs::One<T>());
    STF_EQUAL(m, bs::Half<T>());
    STF_EQUAL(e, bs::One<iT>());
  }

  {
    namespace bs = boost::simd;
    std::pair<T,iT> p;

    p = bs::fast_(frexp)(bs::One<T>());
    STF_EQUAL(p.first  , bs::Half<T>());
    STF_EQUAL(p.second , bs::One<iT>());
  }
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>
using std::cout;  using std::endl;
#include <cerrno> // for ::errno

//[policy_eg_6

/*`
Suppose we want a set of distributions to behave as follows:

* Return infinity on overflow, rather than throwing an exception.
* Don't perform any promotion from double to long double internally.
* Return the closest integer result from the quantiles of discrete
distributions.

We'll begin by including the needed header for all the distributions:
*/

#include <boost/math/distributions.hpp>

/*`

Open up an appropriate namespace, calling it `my_distributions`,
for our distributions, and define the policy type we want.
Any policies we don't specify here will inherit the defaults:

*/

namespace my_distributions
{
  using namespace boost::math::policies;
  // using boost::math::policies::errno_on_error; // etc.

  typedef policy<
     // return infinity and set errno rather than throw:
     overflow_error<errno_on_error>,
     // Don't promote double -> long double internally:
     promote_double<false>,
     // Return the closest integer result for discrete quantiles:
     discrete_quantile<integer_round_nearest>
  > my_policy;

/*`

All we need do now is invoke the BOOST_MATH_DECLARE_DISTRIBUTIONS
macro passing the floating point type `double` and policy types `my_policy` as arguments:

*/

BOOST_MATH_DECLARE_DISTRIBUTIONS(double, my_policy)

} // close namespace my_namespace

/*`

We now have a set of typedefs defined in namespace my_distributions
that all look something like this:

``
typedef boost::math::normal_distribution<double, my_policy> normal;
typedef boost::math::cauchy_distribution<double, my_policy> cauchy;
typedef boost::math::gamma_distribution<double, my_policy> gamma;
// etc
``

So that when we use my_distributions::normal we really end up using
`boost::math::normal_distribution<double, my_policy>`:

*/

int main()
{
   // Construct distribution with something we know will overflow
  // (using double rather than if promoted to long double):
   my_distributions::normal norm(10, 2);

   errno = 0;
   cout << "Result of quantile(norm, 0) is: "
      << quantile(norm, 0) << endl; // -infinity.
   cout << "errno = " << errno << endl;
   errno = 0;
   cout << "Result of quantile(norm, 1) is: "
      << quantile(norm, 1) << endl; // +infinity.
   cout << "errno = " << errno << endl;

   // Now try a discrete distribution.
   my_distributions::binomial binom(20, 0.25);
   cout << "Result of quantile(binom, 0.05) is: "
      << quantile(binom, 0.05) << endl; // To check we get integer results.
   cout << "Result of quantile(complement(binom, 0.05)) is: "
      << quantile(complement(binom, 0.05)) << endl;
}

/*`

Which outputs:

[pre
Result of quantile(norm, 0) is: -1.#INF
errno = 34
Result of quantile(norm, 1) is: 1.#INF
errno = 34
Result of quantile(binom, 0.05) is: 1
Result of quantile(complement(binom, 0.05)) is: 8
]

This mechanism is particularly useful when we want to define a
project-wide policy, and don't want to modify the Boost source
or set  project wide build macros (possibly fragile and easy to forget).

*/
//] //[/policy_eg_6]


#include <boost/math/distributions/find_scale.hpp>


#include <stdlib.h>
#include <iostream>
#include <memory>
#include "conex/cone_program.h"
#include "conex/constraint.h"
#include "conex/linear_constraint.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>

namespace conex {
using DenseMatrix = Eigen::MatrixXd;
using Eigen::VectorXd;

int DoRandomDenseTest(const SolverConfiguration& config, int number_of_tests,
                      int random_seed) {
  srand(random_seed);
  int total_iters = 0;
  for (int i = 0; i < number_of_tests; i++) {
    int num_variables = 5;
    int num_constraints = 6 + 2 * i;
    double eps = 1e-12;

    DenseMatrix Alinear = DenseMatrix::Random(num_constraints, num_variables);
    DenseMatrix Clinear = DenseMatrix::Random(num_constraints, 1);
    Clinear = Clinear.array().abs();

    LinearConstraint linear_constraint{num_constraints, &Alinear, &Clinear};

    Program prog(num_variables);
    prog.AddConstraint(linear_constraint);

    VectorXd x0 = VectorXd::Random(num_constraints, 1);
    x0 = x0.array().abs();
    x0 *= 0.01 / x0.norm();
    VectorXd b = Alinear.transpose() * x0;
    DenseMatrix y(num_variables, 1);
    Solve(b, prog, config, y.data());

    VectorXd x(num_constraints);
    prog.GetDualVariable(0, &x);

    VectorXd slack = Clinear - Alinear * y;
    EXPECT_LE((Alinear.transpose() * x - b).norm(), eps * b.norm());
    EXPECT_GE(slack.minCoeff(), -eps);
    EXPECT_GE(x.minCoeff(), -eps);
    EXPECT_GE(slack.dot(x), -eps);
    double mu = 1.0 / (config.inv_sqrt_mu_max * config.inv_sqrt_mu_max);
    EXPECT_LE(slack.dot(x), (mu + std::sqrt(eps)) * num_constraints);
    auto status = prog.Status();
    total_iters += status.num_iterations;
  }
  return total_iters;
}

GTEST_TEST(LP, Dense) {
  SolverConfiguration config;
  config.prepare_dual_variables = true;
  config.inv_sqrt_mu_max = 5e5;
  config.divergence_upper_bound = 1000;
  config.dinf_upper_bound = 1.35;
  config.final_centering_tolerance = 1;
  int num_tests = 50;
  int random_seed = 1;

  config.enable_line_search = 0;
  config.enable_rescaling = 1;
  int num_iter_div = DoRandomDenseTest(config, num_tests, random_seed);

  config.enable_line_search = 0;
  config.enable_rescaling = 0;
  int num_iter_div_no_rescale =
      DoRandomDenseTest(config, num_tests, random_seed);

  config.enable_line_search = 1;
  config.enable_rescaling = 1;
  int num_iter_line_search = DoRandomDenseTest(config, num_tests, random_seed);

  config.enable_line_search = 1;
  config.enable_rescaling = 0;
  int num_iter_line_search_no_rescale =
      DoRandomDenseTest(config, num_tests, random_seed);

  // Empirical evidence suggests line search is better than divergence
  // upper-bound.
  EXPECT_LE(num_iter_div, num_iter_div_no_rescale);

  // Empirical evidence suggests rescaling helps.
  EXPECT_LE(num_iter_line_search, num_iter_div_no_rescale);
  EXPECT_LE(num_iter_line_search, num_iter_line_search_no_rescale);
}
Eigen::VectorXd ExtractVars(const Eigen::VectorXd& x,
                            std::vector<int> indices) {
  Eigen::VectorXd z(indices.size());
  int cnt = 0;
  for (auto i : indices) {
    z(cnt++) = x(i);
  }
  return z;
}

using Eigen::MatrixXd;
using std::vector;
std::pair<MatrixXd, MatrixXd> Combine(vector<MatrixXd> A, vector<MatrixXd> C,
                                      vector<vector<int> > vars) {
  int n = A.at(0).rows() * A.size();
  int m = A.at(0).cols() * A.size();
  Eigen::MatrixXd Af(n, m);
  Eigen::MatrixXd Cf(n, 1);
  Af.setZero();
  Cf.setZero();
  int cnt = 0;
  int max_v = 0;
  for (size_t i = 0; i < A.size(); i++) {
    for (int k = 0; k < A.at(i).rows(); k++) {
      Cf(cnt, 0) = C.at(i)(k, 0);
      for (size_t j = 0; j < vars.at(i).size(); j++) {
        Af(cnt, vars.at(i).at(j)) = A.at(i)(k, j);
        if (vars.at(i).at(j) > max_v) {
          max_v = vars.at(i).at(j);
        }
      }
      cnt++;
    }
  }

  Eigen::MatrixXd Ac = Af.topLeftCorner(cnt, max_v + 1);
  Eigen::MatrixXd Cc = Cf.topLeftCorner(cnt, 1);
  std::pair<MatrixXd, MatrixXd> output;
  output.first = Ac;
  output.second = Cc;
  return output;
}

Eigen::VectorXd SolveSparseHelper(bool sparse) {
  double eps = 1e-8;
  using Eigen::MatrixXd;
  using std::vector;
  SolverConfiguration config;
  config.prepare_dual_variables = true;
  config.final_centering_tolerance = 1;
  config.divergence_upper_bound = 1e4;

  int number_of_constraints = 50;
  std::vector<std::vector<int> > variables(number_of_constraints);
  vector<MatrixXd> A(number_of_constraints);
  vector<MatrixXd> C(number_of_constraints);

  srand(1);
  int number_of_variables;
  int vars_per_constraint = 5;
  int rows_per_constraint = 10;
  int var_start = 0;
  for (int i = 0; i < number_of_constraints; i++) {
    for (int j = 0; j < vars_per_constraint; j++) {
      variables.at(i).push_back(var_start + j);
    }
    var_start = variables.at(i).back();
  }
  number_of_variables = variables.back().back() + 1;
  for (int i = 0; i < number_of_constraints; i++) {
    MatrixXd Ai =
        DenseMatrix::Random(rows_per_constraint, variables.at(i).size());
    MatrixXd Ci(rows_per_constraint, 1);
    Ci.setConstant(1 * (i * .01 + 1));
    C.at(i) = Ci;
    A.at(i) = Ai;
  }

  auto E = C.at(0);
  E.setConstant(1);
  Eigen::VectorXd b(number_of_variables);
  b.setZero();
  for (int i = 0; i < number_of_constraints; i++) {
    Eigen::VectorXd bi = A.at(i).transpose() * E;
    int cnt = 0;
    for (auto k : variables.at(i)) {
      b(k) += bi(cnt++);
    }
  }

  Program prog(number_of_variables);

  MatrixXd y(number_of_variables, 1);
  if (sparse) {
    for (int i = 0; i < number_of_constraints; i++) {
      prog.AddConstraint(LinearConstraint(A.at(i), C.at(i)), variables.at(i));
    }

    Solve(b, prog, config, y.data());
    MatrixXd Ax = b * 0;
    for (int i = 0; i < number_of_constraints; i++) {
      VectorXd slack = C.at(i) - A.at(i) * ExtractVars(y, variables.at(i));
      EXPECT_TRUE((slack).minCoeff() >= -eps);

      MatrixXd xi(A.at(i).rows(), 1);
      prog.GetDualVariable(i, &xi);

      VectorXd temp = A.at(i).transpose() * xi;
      int k = 0;
      for (auto vk : variables.at(i)) {
        Ax(vk) += temp(k);
        k++;
      }
    }
    EXPECT_NEAR((Ax - b).norm(), 0, 1e-8);
  } else {
    prog.AddConstraint(LinearConstraint{Combine(A, C, variables).first,
                                        Combine(A, C, variables).second});
    Solve(b, prog, config, y.data());

    auto res = b;
    auto L = Combine(A, C, variables);
    VectorXd slack = L.second - L.first * y;

    MatrixXd xi(L.second.rows(), 1);
    prog.GetDualVariable(0, &xi);
    EXPECT_NEAR((b - L.first.transpose() * xi).norm(), 0, 1e-8);
  }

  return y;
}

// Solve the same problem in dense and sparse format.
GTEST_TEST(LP, Sparse) {
  auto y1 = SolveSparseHelper(true);
  auto y2 = SolveSparseHelper(false);
  EXPECT_NEAR((y1 - y2).norm(), 0, 1e-7);
}

Eigen::VectorXd SolveFillIn(bool sparse) {
  double eps = 1e-8;
  using Eigen::MatrixXd;
  using std::vector;
  SolverConfiguration config;
  config.prepare_dual_variables = true;
  config.divergence_upper_bound = 10000;
  config.final_centering_tolerance = 1;

  int number_of_constraints = 4;
  std::vector<std::vector<int> > variables{{0, 1}, {1, 2}, {2, 3}, {0, 3}};

  int number_of_variables = 3 + 1;
  vector<MatrixXd> A(number_of_constraints);
  vector<MatrixXd> C(number_of_constraints);

  srand(1);
  int rows_per_constraint = 3;

  for (int i = 0; i < number_of_constraints; i++) {
    MatrixXd Ai =
        DenseMatrix::Random(rows_per_constraint, variables.at(i).size());
    MatrixXd Ci(rows_per_constraint, 1);
    Ci.setConstant(1 * (i * .01 + 1));
    C.at(i) = Ci;
    A.at(i) = Ai;
  }

  auto E = C.at(0);
  E.setConstant(1);
  Eigen::VectorXd b(number_of_variables);
  b.setZero();
  for (int i = 0; i < number_of_constraints; i++) {
    Eigen::VectorXd bi = A.at(i).transpose() * E;
    int cnt = 0;
    for (auto k : variables.at(i)) {
      b(k) += bi(cnt++);
    }
  }

  Program prog(number_of_variables);

  MatrixXd y(number_of_variables, 1);
  if (sparse) {
    for (int i = 0; i < number_of_constraints; i++) {
      prog.AddConstraint(LinearConstraint(A.at(i), C.at(i)), variables.at(i));
    }

    Solve(b, prog, config, y.data());
    MatrixXd Ax = b * 0;
    for (int i = 0; i < number_of_constraints; i++) {
      VectorXd slack = C.at(i) - A.at(i) * ExtractVars(y, variables.at(i));
      EXPECT_TRUE((slack).minCoeff() >= -eps);

      MatrixXd xi(A.at(i).rows(), 1);
      prog.GetDualVariable(i, &xi);
      VectorXd temp = A.at(i).transpose() * xi;
      int k = 0;
      for (auto vk : variables.at(i)) {
        Ax(vk) += temp(k);
        k++;
      }
    }
    EXPECT_NEAR((Ax - b).norm(), 0, 1e-8);
  } else {
    prog.AddConstraint(LinearConstraint{Combine(A, C, variables).first,
                                        Combine(A, C, variables).second});
    Solve(b, prog, config, y.data());

    auto res = b;
    auto L = Combine(A, C, variables);
    VectorXd slack = L.second - L.first * y;

    MatrixXd xi(L.second.rows(), 1);
    prog.GetDualVariable(0, &xi);
    EXPECT_NEAR((b - L.first.transpose() * xi).norm(), 0, 1e-8);
  }

  return y;
}

GTEST_TEST(LP, SparseWithFillIn) {
  auto y1 = SolveFillIn(true);
  auto y2 = SolveFillIn(false);
  EXPECT_NEAR((y1 - y2).norm(), 0, 1e-7);
}

void DoRandomPrimalFailsSlater(double distance_to_infeasible) {
  SolverConfiguration config;
  config.prepare_dual_variables = true;
  config.inv_sqrt_mu_max = 10000;
  config.maximum_mu = 10000000;
  config.divergence_upper_bound = 10000;
  config.infeasibility_threshold = 2000000;
  config.final_centering_steps = 5;
  config.final_centering_tolerance = 1;

  int m = 10;
  int n1 = 3;  // The number of implicit equations
  int n2 = 8;
  int n = 2 * n1 + n2;
  Eigen::MatrixXd yref = DenseMatrix::Random(m, 1);

  DenseMatrix A1 = DenseMatrix::Random(n1, m);
  DenseMatrix C1 = A1 * yref;
  DenseMatrix A2 = DenseMatrix::Random(n2, m);
  DenseMatrix C2 = A2 * yref;
  C2.array() += 2;

  DenseMatrix A = DenseMatrix::Random(n, m);
  DenseMatrix C = DenseMatrix::Random(n, 1);
  A << A1, -A1, A2;

  DenseMatrix offset(n1, 1);
  offset.setConstant(distance_to_infeasible);
  C << C1, -(C1 - offset), C2;

  LinearConstraint _constraint{n, &A, &C};

  Program prog(m);
  prog.SetNumberOfVariables(m);
  prog.AddConstraint(_constraint);

  VectorXd b(2);
  VectorXd xref = VectorXd::Random(n);
  xref = xref.array().abs();
  b = A.transpose() * xref;

  DenseMatrix y(m, 1);
  Solve(b, prog, config, y.data());
  VectorXd x(n);
  prog.GetDualVariable(0, &x);

  if (distance_to_infeasible < 0) {
    double scale = (-C.transpose() * x)(0, 0);
    EXPECT_NEAR((A.transpose() * x / scale).norm(), 0, 1e-4);
    EXPECT_GE((-C.transpose() * x)(0, 0), 0);
    EXPECT_GE(x.minCoeff() / scale, -1e-8);
  } else {
    EXPECT_NEAR((C.transpose() * x - b.transpose() * y).norm(), 0, 1e-5);
    EXPECT_GE((C - A * y).minCoeff(), -1e-5);
    EXPECT_NEAR((A.transpose() * x - b).norm(), 0, 1e-5);
    EXPECT_GE(x.minCoeff(), -1e-8);
  }
}

GTEST_TEST(LP, RandomPrimal) {
  srand(0);
  for (int i = 0; i < 3; i++) {
    DoRandomPrimalFailsSlater(.1 * (-1 + i * 1));
  }
}

void DoRandomDualFailsSlater(double distance_to_infeasible) {
  SolverConfiguration config;
  config.prepare_dual_variables = true;
  config.inv_sqrt_mu_max = 10000;
  config.divergence_upper_bound = 10000;
  config.maximum_mu = 1e7;
  config.infeasibility_threshold = 1e5;
  config.final_centering_steps = 2;
  config.final_centering_tolerance = 1;

  int m1 = 4;
  int m2 = 4;
  int m = m1 + m2;
  int n = 10;
  Eigen::MatrixXd yref = DenseMatrix::Random(m, 1);

  DenseMatrix A(n, m);
  DenseMatrix A1 = DenseMatrix::Random(n, m1);
  DenseMatrix A2 = DenseMatrix::Random(n, m2);
  A2 = A2.array().abs();
  A2.topRows(n - m2).setZero();
  A1.bottomRows(m2).setZero();
  A << A1, A2;

  DenseMatrix C(n, 1);
  C.setConstant(1);

  Program prog(m);
  prog.SetNumberOfVariables(m);

  VectorXd xref = VectorXd::Random(n);
  xref = xref.array().abs();
  A.bottomRightCorner(m2, m2) = Eigen::MatrixXd::Identity(m2, m2);
  VectorXd b = A.transpose() * xref;
  b.bottomRows(m2).setConstant(distance_to_infeasible);

  LinearConstraint constraint{n, &A, &C};
  prog.AddConstraint(constraint);

  DenseMatrix y(m, 1);
  bool solved = Solve(b, prog, config, y.data());
  VectorXd x(n);
  prog.GetDualVariable(0, &x);

  if (distance_to_infeasible < 0) {
    EXPECT_EQ(solved, 0);
    EXPECT_GE((-A * y).minCoeff(), -1e-8);
    EXPECT_GE(b.col(0).dot(y.col(0)), 0);
  } else {
    EXPECT_EQ(solved, 1);
    EXPECT_NEAR((C.transpose() * x - b.transpose() * y).norm(), 0, 1e-6);
    EXPECT_NEAR((A.transpose() * x - b).norm(), 0, 1e-8);
    EXPECT_GE((C - A * y).minCoeff(), -1e-8);
  }
}

GTEST_TEST(LP, RandomDual) {
  srand(0);
  for (int i = 0; i < 3; i++) {
    double distance_to_infeasibility = -1.0 + i * 1;
    DoRandomDualFailsSlater(distance_to_infeasibility);
  }
}
}  // namespace conex