/**
 * \file      rigid-body-kinematics.hpp
 * \author    Mehdi Benallegue
 * \date       2013
 * \brief      Implements integrators for the kinematics, in terms or rotations
 *             and translations.
 *
 * \details
 *
 *
 */

#ifndef StATEOBSERVATIONRIGIDBODYKINEMATICS_H
#define StATEOBSERVATIONRIGIDBODYKINEMATICS_H

#include <Eigen/SVD>

#include <state-observation/api.h>
#include <state-observation/tools/definitions.hpp>
#include <state-observation/tools/miscellaneous-algorithms.hpp>
#include <state-observation/tools/probability-law-simulation.hpp>

namespace stateObservation
{
namespace kine
{
inline void integrateKinematics(Vector3 & position, const Vector3 & velocity, double dt);

inline void integrateKinematics(Vector3 & position, Vector3 & velocity, const Vector3 & acceleration, double dt);

inline void integrateKinematics(Matrix3 & orientation, const Vector3 & rotationVelocity, double dt);

inline void integrateKinematics(Matrix3 & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

inline void integrateKinematics(Quaternion & orientation, const Vector3 & rotationVelocity, double dt);

inline void integrateKinematics(Quaternion & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

/// integrates the position/orientation and their time derivatives, given the
/// accelerations, and initial velocities and positions. The rotations are
/// expressed by rotation matrix
inline void integrateKinematics(Vector3 & position,
                                Vector3 & velocity,
                                const Vector3 & acceleration,
                                Matrix3 & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

/// integrates the position/orientation and their time derivatives, given the
/// accelerations, and initial velocities and positions. The orientations are
/// expressed by quaternions
inline void integrateKinematics(Vector3 & position,
                                Vector3 & velocity,
                                const Vector3 & acceleration,
                                Quaternion & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

/// integrates the postition/orientation given the velocities
inline void integrateKinematics(Vector3 & position,
                                const Vector3 & velocity,
                                Matrix3 & orientation,
                                const Vector3 & rotationVelocity,
                                double dt);

/// integrates the postition/orientation given the velocities
inline void integrateKinematics(Vector3 & position,
                                const Vector3 & velocity,
                                Quaternion & orientation,
                                const Vector3 & rotationVelocity,
                                double dt);

/// Puts the orientation vector norm between 0 and Pi if it
/// gets close to 2pi
inline Vector regulateRotationVector(const Vector3 & v);

/// Transform the rotation vector into angle axis
inline AngleAxis rotationVectorToAngleAxis(const Vector3 & v);

/// Tranbsform the rotation vector into rotation matrix
inline Matrix3 rotationVectorToRotationMatrix(const Vector3 & v);

/// Tranbsform the rotation vector into quaternion
inline Quaternion rotationVectorToQuaternion(const Vector3 & v);

/// Tranbsform the rotation matrix into rotation vector
inline Vector3 rotationMatrixToRotationVector(const Matrix3 & R);

/// Tranbsform a quaternion into rotation vector
inline Vector3 quaternionToRotationVector(const Quaternion & q);

/// Tranbsform a quaternion into rotation vector
inline Vector3 quaternionToRotationVector(const Vector4 & v);

/// scalar component of a quaternion
inline double scalarComponent(const Quaternion & q);

/// vector part of the quaternion
inline Vector3 vectorComponent(const Quaternion & q);

/// Transform the rotation matrix into roll pitch yaw
///(decompose R into Ry*Rp*Rr)
inline Vector3 rotationMatrixToRollPitchYaw(const Matrix3 & R, Vector3 & v);

inline Vector3 rotationMatrixToRollPitchYaw(const Matrix3 & R);

/// Transform the roll pitch yaw into rotation matrix
///( R = Ry*Rp*Rr)
inline Matrix3 rollPitchYawToRotationMatrix(double roll, double pitch, double yaw);

inline Matrix3 rollPitchYawToRotationMatrix(const Vector3 & rpy);

/// Transform the roll pitch yaw into rotation matrix
///( R = Ry*Rp*Rr)
inline Quaternion rollPitchYawToQuaternion(double roll, double pitch, double yaw);

inline Quaternion rollPitchYawToQuaternion(const Vector3 & rpy);

/// Projects the Matrix to so(3)
inline Matrix3 orthogonalizeRotationMatrix(const Matrix3 & M);

/// transform a 3d vector into a skew symmetric 3x3 matrix
inline Matrix3 skewSymmetric(const Vector3 & v, Matrix3 & R);

/// transform a 3d vector into a skew symmetric 3x3 matrix
inline Matrix3 skewSymmetric(const Vector3 & v);

/// transform a 3d vector into a squared skew symmetric 3x3 matrix
inline Matrix3 skewSymmetric2(const Vector3 & v, Matrix3 & R);

/// transform a 3d vector into a squared skew symmetric 3x3 matrix
inline Matrix3 skewSymmetric2(const Vector3 & v);

/// transforms a homogeneous matrix into 6d vector (position theta mu)
inline Vector6 homogeneousMatrixToVector6(const Matrix4 & M);

/// transforms a 6d vector (position theta mu) into a homogeneous matrix
inline Matrix4 vector6ToHomogeneousMatrix(const Vector6 & v);

/// @brief Builds the smallest angle matrix allowing to get from a NORMALIZED vector v1 to its imahe Rv1
/// This is based on Rodrigues formula
///
/// @param v1 the NORMALIZED vector
/// @param Rv1 the NORMALIZED image of this vector by the rotation matrix R
/// @return Matrix3 the rotation matrix R
inline Matrix3 twoVectorsToRotationMatrix(const Vector3 & v1, const Vector3 Rv1);

/// @brief checks if this matrix is a pure yaw matrix or not
///
/// @param R the rotation matrix
/// @return true is pure yaw
/// @return false is not pure yaw
inline bool isPureYaw(const Matrix3 & R);

/// @brief Gets a vector that remains horizontal with this rotation. This vector is NOT normalized
/// @details There is a general version in getInvariantOrthogonalVector(). This can be used to extract yaw angle from
/// a rotation matrix without needing to specify an order in the tils (e.g. roll then pich).
///
/// @param R the input rotation
/// @return Vector3 the output horizontal vector
inline Vector3 getInvariantHorizontalVector(const Matrix3 & R);

/// @brief Gets a vector \f$v\f$ that is orthogonal to \f$e_z\f$ and such that \f$\hat{R}^T e_z\f$ is orthogonal to
/// the tilt \f$R^T e_z\f$. This vector is NOT normalized.
/// @details This is a generalization of getInvariantHorizontalVector() which corresponds to no tilt
/// \f$\hat{R}^T e_z=e_z\f$. This function is useful to merge the yaw from the rotation matrix with the tilt.
///
/// @param Rhat the input rotation matrix \f$\hat{R}^T\f$
/// @param Rtez the input tilt \f$\hat{R}^T e_z\f$
/// @return Vector3 the output horizontal vector
inline Vector3 getInvariantOrthogonalVector(const Matrix3 & Rhat, const Vector3 & Rtez);

/// @brief Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the
/// deviation of the v vector)
/// @details throws exception when the orientation is singlular (likely gimbal lock)
/// to avoid these issues, we recommend to use mergeTiltWithYawAxisAgnostic()
/// @param Rtez the tilt \f$R_1^T e_z\f$ (the local image of \f$e_z\f$ unit vector)
/// @param R2 is the second rotation matrix from which the "yaw" needs to be extracted
/// @param v is the vector to use as reference it must be horizontal and normalized (for a traditional yaw v is by
/// deftault \f$e_x\f$)
/// @return Matrix3 the merged rotation matrix
inline Matrix3 mergeTiltWithYaw(const Vector3 & Rtez,
                                const Matrix3 & R2,
                                const Vector3 & v = Vector3::UnitX()) noexcept(false);

/// @brief Merge the roll and pitch with the yaw from a rotation matrix (minimizes the deviation of the v vector)
///
/// @param R1 is the first rotation to get the roll and pitch
/// @param R2 is the second rotation matrix from which the "yaw" needs to be extracted
/// @param v is the vector to use as reference (for a traditional yaw v is initialized to \f$e_x\f$)
/// @return Matrix3 the merged rotation matrix
inline Matrix3 mergeRoll1Pitch1WithYaw2(const Matrix3 & R1, const Matrix3 & R2, const Vector3 & v = Vector3::UnitX());

/// @brief Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the deviation
/// of the v vector)
/// @param Rtez the tilt \f$R_1^T e_z\f$ (the local image of \f$e_z\f$ unit vector)
/// @param R2 is the second rotation matrix from which the "yaw" needs to be extracted
/// @param v is the vector to use as reference (for a traditional yaw v is initialized to \f$e_x\f$)
/// @return Matrix3 the merged rotation matrix
inline Matrix3 mergeTiltWithYawAxisAgnostic(const Vector3 & Rtez, const Matrix3 & R2);

/// @brief Merge the roll and pitch with the yaw from a rotation matrix with optimal reference vector
///
/// @param R1 is the first rotation to get the roll and pitch
/// @param R2 is the second rotation matrix from which the "yaw" needs to be extracted
/// @param v is the vector to use as reference (for a traditional yaw v is initialized to \f$e_x\f$)
/// @return Matrix3 the merged rotation matrix
inline Matrix3 mergeRoll1Pitch1WithYaw2AxisAgnostic(const Matrix3 & R1, const Matrix3 & R2);

/// @brief take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the axis with this
/// rotation
/// @param rotation The 3x3 rotation matrix
/// @param axis the axis of rotation (must be normalized)
/// @param v the vector that is rotated with the rotation (must be orthogonal to axis and normalized)
/// @return double the angle
inline double rotationMatrixToAngle(const Matrix3 & rotation, const Vector3 & axis, const Vector3 & v);

/// @brief take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the upward vertical
/// axis with this rotation
/// @details this is a generalization of yaw extraction (yaw is equivalent to v = Matrix3::UnitX(), but it is more
/// efficiant to calll the dedicated  rotationMatrixToYaw() without vector parameter).
/// @param rotation The 3x3 rotation matrix
/// @param v the rotated vector (expressed in the horizontal plane, must be normalized)
/// @return double the angle
inline double rotationMatrixToYaw(const Matrix3 & rotation, const Vector2 & v);

/// @brief take 3x3 matrix represeting a rotation and gives the yaw angle from roll pitch yaw representation
/// @param rotation The 3x3 rotation matrix
/// @return double the angle
inline double rotationMatrixToYaw(const Matrix3 & rotation);

/// @brief take 3x3 matrix represeting a rotation and gives a corresponding angle around upward vertical axis
/// @details This is similar to yaw angle but here we identify a horizontal vector that stays horizontal after rotation.
/// this can be called axis agnostic yaw extraction.
/// and get the angle between them
/// @param rotation The 3x3 rotation matrix
/// @return double the angle
inline double rotationMatrixToYawAxisAgnostic(const Matrix3 & rotation);

/// @brief Get the Identity Quaternion
///
/// @return Quaternion
inline Quaternion zeroRotationQuaternion();

/// @brief Get a uniformly random Quaternion
///
/// @return Quaternion
inline Quaternion randomRotationQuaternion();

/// @brief get a randomAngle between -pi and pu
///
/// @return double the random angle
inline double randomAngle();

/// @brief Checks if it is a rotation matrix (right-hand orthonormal) or not
/// @param precision the absolute precision of the test
/// @return true when it is a rotation matrix
/// @return false when not
inline bool isRotationMatrix(const Matrix3 &, double precision = 2 * cst::epsilon1);

/// transforms a rotation into translation given a constraint of a fixed point
inline void fixedPointRotationToTranslation(const Matrix3 & R,
                                            const Vector3 & rotationVelocity,
                                            const Vector3 & rotationAcceleration,
                                            const Vector3 & fixedPoint,
                                            Vector3 & outputTranslation,
                                            Vector3 & outputLinearVelocity,
                                            Vector3 & outputLinearAcceleration);

/// derivates a quaternion using finite difference to get a angular velocity vector
inline Vector3 derivateRotationFD(const Quaternion & q1, const Quaternion & q2, double dt);

/// derivates a rotation vector using finite difference to get a angular velocity vector
inline Vector3 derivateRotationFD(const Vector3 & o1, const Vector3 & o2, double dt);

inline Vector6 derivateHomogeneousMatrixFD(const Matrix4 & m1, const Matrix4 & m2, double dt);

inline Vector6 derivatePoseThetaUFD(const Vector6 & v1, const Vector6 & v2, double dt);

/// Computes the "multiplicative Jacobian" for
/// Kalman filtering for example
/// orientation is the current orientation
/// dR is the rotation delta between the current orientation and the orientation
/// at the next step.
/// dRdR is the "multiplicative" Jacobian with regard to variations of orientation
/// dRddeltaR is the "multiplicative" Jacobian with regard to variations of deltaR
inline void derivateRotationMultiplicative(const Vector3 & deltaR, Matrix3 & dRdR, Matrix3 & dRddeltaR);

/// Computes the "multiplicative Jacobian" for
/// a function R^T.v giving a vector v expressed in a local frame
/// with regard to Rotations of this local frame
inline Matrix3 derivateRtvMultiplicative(const Matrix3 & R, const Vector3 & v);

/// uses the derivation to reconstruct the velocities and accelerations given
/// trajectories in positions and orientations only
inline IndexedVectorArray reconstructStateTrajectory(const IndexedVectorArray & positionOrientation, double dt);

inline Vector invertState(const Vector & state);

inline Matrix4 invertHomoMatrix(const Matrix4 & m);

enum rotationType
{
  matrix = 0,
  rotationVector = 1,
  quaternion = 2,
  angleaxis = 3
};

template<rotationType = rotationVector>
struct indexes
{
};

template<>
struct indexes<rotationVector>
{
  /// indexes of the different components of a vector of the kinematic state
  /// when the orientation is represented using a 3D rotation vector
  static const unsigned pos = 0;
  static const unsigned ori = 3;
  static const unsigned linVel = 6;
  static const unsigned angVel = 9;
  static const unsigned linAcc = 12;
  static const unsigned angAcc = 15;
  static const unsigned size = 18;
};

template<>
struct indexes<quaternion>
{
  /// indexes of the different components of a vector of the kinematic state
  /// when the orientation is represented using a quaternion
  static const unsigned pos = 0;
  static const unsigned ori = 3;
  static const unsigned linVel = 7;
  static const unsigned angVel = 10;
  static const unsigned linAcc = 13;
  static const unsigned angAcc = 16;
  static const unsigned size = 19;
};

/// relative tolereance to the square of quaternion norm.
constexpr double quatNormTol = 1e-6;

class Orientation
{
public:
  /// The parameter initialize should be set to true except when it is
  /// certain that the initial value will not be used
  /// And that the first operation would be to set its value
  explicit Orientation(bool initialize = true);

  /// this is the rotation vector and NOT Euler angles
  explicit Orientation(const Vector3 & v);

  explicit Orientation(const Quaternion & q);

  explicit Orientation(const Matrix3 & m);

  explicit Orientation(const AngleAxis & aa);

  Orientation(const Quaternion & q, const Matrix3 & m);

  Orientation(const double & roll, const double & pitch, const double & yaw);

  Orientation(const Orientation & multiplier1, const Orientation & multiplier2);

  inline Orientation & operator=(const Vector3 & v);

  inline Orientation & operator=(const Quaternion & q);

  inline Orientation & operator=(const Matrix3 & m);

  inline Orientation & operator=(const AngleAxis & aa);

  inline Orientation & setValue(const Quaternion & q, const Matrix3 & m);

  inline Orientation & fromVector4(const Vector4 & v);

  inline Orientation & setRandom();

  template<typename t>
  inline Orientation & setZeroRotation();

  inline Orientation & setZeroRotation();

  /// get a const reference on the matrix or the quaternion
  inline const Matrix3 & toMatrix3() const;
  inline const Quaternion & toQuaternion() const;

  inline operator const Matrix3 &() const;
  inline operator const Quaternion &() const;

  inline Vector4 toVector4() const;

  inline Vector3 toRotationVector() const;
  inline Vector3 toRollPitchYaw() const;
  inline AngleAxis toAngleAxis() const;

  /// Multiply the rotation (orientation) by another rotation R2
  /// the non const versions allow to use more optimized methods

  inline Orientation operator*(const Orientation & R2) const;

  /// Noalias versions of the operator*
  inline const Orientation & setToProductNoAlias(const Orientation & R1, const Orientation & R2);

  inline Orientation inverse() const;

  /// use the vector dt_x_omega as the increment of rotation expressed in the
  /// world frame. Which gives R_{k+1}=\exp(S(dtxomega))R_k
  inline const Orientation & integrate(Vector3 dt_x_omega);

  /// gives the log (rotation vector) of the difference of orientation
  /// gives log of (*this).inverse()*R_k1
  inline Vector3 differentiate(Orientation R_k1) const;

  /// Rotate a vector
  inline Vector3 operator*(const Vector3 & v) const;

  inline bool isSet() const;
  inline void reset();

  inline bool isMatrixSet() const;
  inline bool isQuaternionSet() const;

  /// switch the state of the Matrix or quaternion to set or not
  /// this can be used for forward initialization
  inline void setMatrix(bool b = true);
  inline void setQuaternion(bool b = true);

  /// no checks are performed for these functions, use with caution

  inline CheckedMatrix3 & getMatrixRefUnsafe();
  inline CheckedQuaternion & getQuaternionRefUnsafe();

  /// synchronizes the representations (quaternion and rotation matrix)
  inline void synchronize();

  /// retruns a zero rotation
  static inline Orientation zeroRotation();

  /// Returns a uniformly distributed random rotation
  static inline Orientation randomRotation();

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  void check_() const;

  inline const Matrix3 & quaternionToMatrix_() const;
  inline const Quaternion & matrixToQuaternion_() const;

  mutable CheckedQuaternion q_;
  mutable CheckedMatrix3 m_;
};

struct Kinematics
{
  struct Flags
  {
    typedef unsigned char Byte;

    static const Byte position = BOOST_BINARY(000001);
    static const Byte orientation = BOOST_BINARY(000010);
    static const Byte linVel = BOOST_BINARY(000100);
    static const Byte angVel = BOOST_BINARY(001000);
    static const Byte linAcc = BOOST_BINARY(010000);
    static const Byte angAcc = BOOST_BINARY(100000);

    static const Byte all = position | orientation | linVel | angVel | linAcc | angAcc;
  };

  Kinematics() {}

  /// Constructor from a vector
  /// the flags show which parts of the kinematics to be loaded from the vector
  /// the order of the vector is
  /// position orientation (quaternion) linevel angvel linAcc angAcc
  /// use the flags to define the structure of the vector
  Kinematics(const Vector & v, Flags::Byte = Flags::all);

  Kinematics(const Kinematics & multiplier1, const Kinematics & multiplier2);

  /// Fills from vector
  /// the flags show which parts of the kinematics to be loaded from the vector
  /// the order of the vector is
  /// position orientation (quaternion) linevel angvel linAcc angAcc
  /// use the flags to define the structure of the vector
  Kinematics & fromVector(const Vector & v, Flags::Byte = Flags::all);

  /// initializes at zero all the flagged fields
  /// the typename allows to set if the prefered type for rotation
  /// is a Matrix3 or a Quaternion (Quaternion by default)
  template<typename t>
  Kinematics & setZero(Flags::Byte = Flags::all);

  Kinematics & setZero(Flags::Byte = Flags::all);

  inline const Kinematics & integrate(double dt);

  inline const Kinematics & update(const Kinematics & newValue, double dt, Flags::Byte = Flags::all);

  inline Kinematics getInverse() const;

  /// converts the object to a vector
  /// the order of the vector is
  /// position orientation (quaternion) linevel angvel linAcc angAcc
  /// use the flags to define the structure of the vector
  inline Vector toVector(Flags::Byte) const;
  inline Vector toVector() const;

  /// composition of transformation
  inline Kinematics operator*(const Kinematics &)const;

  inline Kinematics setToProductNoAlias(const Kinematics & operand1, const Kinematics & operand2);

  inline void reset();

  CheckedVector3 position;
  Orientation orientation;

  CheckedVector3 linVel;
  CheckedVector3 angVel;

  CheckedVector3 linAcc;
  CheckedVector3 angAcc;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

protected:
  inline const Kinematics & update_deprecated(const Kinematics & newValue, double dt, Flags::Byte = Flags::all);

  Vector3 tempVec_;
};

} // namespace kine
} // namespace stateObservation

inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::Kinematics & k);

#include <state-observation/tools/rigid-body-kinematics.hxx>

#endif // StATEOBSERVATIONRIGIDBODYKINEMATICS_H


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2011.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/config.hpp>

#ifdef BOOST_NO_CXX11_CONSTEXPR
#include <iostream>

int main()
{
  std::cout << "Please use a compiler that supports constexpr" << std::endl;
}
#else

#define BOOST_MPL_LIMIT_STRING_SIZE 64
#define BOOST_METAPARSE_LIMIT_STRING_SIZE BOOST_MPL_LIMIT_STRING_SIZE

#include <boost/metaparse/grammar.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/build_parser.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/string.hpp>
#include <boost/metaparse/util/digit_to_int.hpp>

#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/char.hpp>
#include <boost/mpl/int.hpp>

using boost::metaparse::build_parser;
using boost::metaparse::entire_input;
using boost::metaparse::token;
using boost::metaparse::grammar;

using boost::metaparse::util::digit_to_int;

using boost::mpl::apply_wrap1;
using boost::mpl::fold;
using boost::mpl::front;
using boost::mpl::back;
using boost::mpl::plus;
using boost::mpl::minus;
using boost::mpl::times;
using boost::mpl::divides;
using boost::mpl::eval_if;
using boost::mpl::equal_to;
using boost::mpl::_1;
using boost::mpl::_2;
using boost::mpl::char_;
using boost::mpl::lambda;
using boost::mpl::int_;

#ifdef _STR
  #error _STR already defined
#endif
#define _STR BOOST_METAPARSE_STRING

template <class A, class B>
struct lazy_plus : plus<typename A::type, typename B::type> {};

template <class A, class B>
struct lazy_minus : minus<typename A::type, typename B::type> {};

template <class A, class B>
struct lazy_times : times<typename A::type, typename B::type> {};

template <class A, class B>
struct lazy_divides : divides<typename A::type, typename B::type> {};

template <class C, class T, class F>
struct lazy_eval_if : eval_if<typename C::type, T, F> {};

template <class A, class B>
struct lazy_equal_to : equal_to<typename A::type, typename B::type> {};

template <class Sequence, class State, class ForwardOp>
struct lazy_fold :
  fold<typename Sequence::type, typename State::type, typename ForwardOp::type>
{};

typedef
  lazy_fold<
    back<_1>,
    front<_1>,
    lambda<
      lazy_eval_if<
        lazy_equal_to<front<_2>, char_<'*'>>,
        lazy_times<_1, back<_2>>,
        lazy_divides<_1, back<_2>>
      >
    >::type
  >
  prod_action;

typedef
  lazy_fold<
    back<_1>,
    front<_1>,
    lambda<
      lazy_eval_if<
        lazy_equal_to<front<_2>, char_<'+'>>,
        lazy_plus<_1, back<_2>>,
        lazy_minus<_1, back<_2>>
      >
    >::type
  >
  plus_action;

typedef
  lambda<
    lazy_fold<
      _1,
      int_<0>,
      lambda<
        lazy_plus<lazy_times<_1, int_<10>>, apply_wrap1<digit_to_int<>, _2>>
      >::type
    >
  >::type
  int_action;

typedef
  grammar<_STR("plus_exp")>

    ::rule<_STR("int ::= ('0'|'1'|'2'|'3'|'4'|'5'|'6'|'7'|'8'|'9')+"), int_action>::type
    ::rule<_STR("ws ::= (' ' | '\n' | '\r' | '\t')*")>::type
    ::rule<_STR("int_token ::= int ws"), front<_1>>::type
    ::rule<_STR("plus_token ::= '+' ws"), front<_1>>::type
    ::rule<_STR("minus_token ::= '-' ws"), front<_1>>::type
    ::rule<_STR("mult_token ::= '*' ws"), front<_1>>::type
    ::rule<_STR("div_token ::= '/' ws"), front<_1>>::type
    ::rule<_STR("plus_token ::= '+' ws")>::type
    ::rule<_STR("plus_exp ::= prod_exp ((plus_token | minus_token) prod_exp)*"), plus_action>::type
    ::rule<_STR("prod_exp ::= int_token ((mult_token | div_token) int_token)*"), prod_action>::type
  expression;

typedef build_parser<entire_input<expression>> calculator_parser;

int main()
{
  using std::cout;
  using std::endl;

  cout
    << apply_wrap1<calculator_parser, _STR("13")>::type::value << endl
    << apply_wrap1<calculator_parser, _STR("1+ 2*4-6/2")>::type::value << endl
    ;
}
#endif


#include <iostream>
#include <boost/graph/adjacency_list.hpp>

using namespace std;
using namespace boost;
typedef boost::adjacency_list<listS, vecS, bidirectionalS> Graph;
typedef Graph::vertex_descriptor Vertex;
typedef Graph::vertex_iterator VertexI;
typedef Graph::in_edge_iterator inEdgeI;
typedef Graph::out_edge_iterator outEdgeI;

int main() {
  Graph g;
  Vertex u = add_vertex(g);
  Vertex v = add_vertex(g);
  Vertex w = add_vertex(g);
  Vertex x = add_vertex(g);
  add_edge(u, v, g); add_edge(u, w, g); add_edge(u, x, g);
  cout << "Number of edges: " << num_edges(g) << "\n";
  cout << "Number of vertices: " << num_vertices(g) << "\n";
  VertexI vertexIt, vertexEnd;
  inEdgeI inedgeIt, inedgeEnd;
  outEdgeI outedgeIt, outedgeEnd;
  tie(vertexIt, vertexEnd) = vertices(g);
  for (; vertexIt != vertexEnd; ++vertexIt) {
    cout << "incoming edges for " << *vertexIt << ": ";
    tie(inedgeIt, inedgeEnd) = in_edges(*vertexIt, g);
    for(; inedgeIt != inedgeEnd; ++inedgeIt) {
      cout << *inedgeIt << " ";
    }
    cout << "; in degree: " << in_degree(*vertexIt, g) << "\n";
    cout << "outgoing edges for " << *vertexIt << ": ";
    tie(outedgeIt, outedgeEnd) = out_edges(*vertexIt, g);
    for(; outedgeIt != outedgeEnd; ++outedgeIt) {
      cout << *outedgeIt << " ";
    }
    cout << "; out degree: " << out_degree(*vertexIt, g) << "\n";
  }
  return 0;
}


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

#include <boost/numeric/odeint.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/feedback/mpc.hpp>

#include <chrono>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

using namespace std::chrono_literals;
using namespace boost::numeric::odeint;

using Time = std::chrono::duration<double>;

template<typename T>
using X = Eigen::Vector2<T>;
template<typename T>
using U = Eigen::Matrix<T, 1, 1>;

using Gd = X<double>;
using Ud = U<double>;

int main()
{
  using std::sin;
  std::srand(5);

  // system variables
  Gd g = Gd::Random();
  Ud u;

  // dynamics
  auto f = []<typename S>(const X<S> & x, const U<S> u) -> smooth::Tangent<X<S>> {
    return {x(1), u(0)};
  };

  // running constraints
  auto cr = []<typename S>(const X<S> &, const U<S> & u) -> Eigen::Vector<S, 1> { return u; };
  Eigen::Vector<double, 1> crl{-0.5}, cru{0.5};

  // create MPC object and set input bounds, and desired trajectories
  smooth::feedback::MPC<Time, Gd, Ud, decltype(f), decltype(cr)> mpc{
    f,
    cr,
    crl,
    cru,
    {
      .K  = 20,
      .tf = 5,
      .qp = {.scaling = false, .polish = false},
    },
  };

  mpc.set_weights({
    .Q   = Eigen::Matrix2d::Identity(),
    .Qtf = 0.1 * Eigen::Matrix2d::Identity(),
    .R   = 0.1 * Eigen::Matrix<double, 1, 1>::Identity(),
  });
  mpc.set_xdes_rel([]<typename T>(T t) -> X<T> { return X<T>{-0.5 * sin(0.3 * t), 0}; });
  mpc.set_udes_rel([]<typename T>(T) -> U<T> { return U<T>::Zero(); });

  // prepare for integrating the closed-loop system
  runge_kutta4<Gd, double, smooth::Tangent<Gd>, double, vector_space_algebra> stepper{};
  const auto ode = [&f, &u](const Gd & x, smooth::Tangent<Gd> & d, double) { d = f(x, u); };
  std::vector<double> tvec, xvec, vvec, uvec;

  // integrate closed-loop system
  const auto t0 = std::chrono::high_resolution_clock::now();

  for (std::chrono::milliseconds t = 0s; t < 60s; t += 50ms) {
    // compute MPC input
    auto [u_mpc, code] = mpc(t, g);
    u                  = u_mpc;
    if (code != smooth::feedback::QPSolutionStatus::Optimal) {
      std::cerr << "Solver failed with code " << static_cast<int>(code) << std::endl;
    }

    // store data
    tvec.push_back(duration_cast<Time>(t).count());
    xvec.push_back(g.x());
    vvec.push_back(g.y());
    uvec.push_back(u(0));

    // step dynamics
    stepper.do_step(ode, g, 0, 0.05);
  }

  const auto tf = std::chrono::high_resolution_clock::now();

  std::cout << "MPC loop time: "
            << std::chrono::duration_cast<std::chrono::microseconds>(tf - t0).count() << "us\n";

#if ENABLE_PLOTTING
  matplot::figure();
  matplot::hold(matplot::on);

  matplot::plot(tvec, xvec)->line_width(2);
  matplot::plot(tvec, matplot::transform(tvec, [](auto t) { return -0.5 * sin(0.3 * t); }), "k--")
    ->line_width(2);
  matplot::plot(tvec, vvec)->line_width(2);
  matplot::plot(tvec, uvec)->line_width(2);
  matplot::legend({"x", "x_{des}", "v", "u"});

  matplot::show();
#else
  std::cout << "TRAJECTORY:" << std::endl;
  for (auto i = 0u; i != tvec.size(); ++i) {
    std::cout << "t=" << tvec[i] << ": x=" << xvec[i] << ", v=" << vvec[i] << std::endl;
  }
#endif
}


#pragma once

#include "Faddeeva/Faddeeva.hh"
#include "util.hpp"
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <iostream>
#include <memory>
#include <random>

#include <fstream>
#include <sstream>

namespace myFM {
template <typename Real> struct OprobitSampler {

  using DenseVector = types::Vector<Real>;
  using DenseMatrix = types::DenseMatrix<Real>;
  using IntVector = Eigen::Matrix<int, Eigen::Dynamic, 1>;
  static constexpr Real SQRT2 = 1.4142135623730951;
  static constexpr Real SQRTPI = 1.7724538509055159;
  static constexpr Real SQRT2PI = SQRT2 * SQRTPI;
  static constexpr Real PI = 3.141592653589793;

  OprobitSampler(DenseVector &x, const DenseVector &y, int K,
                 const std::vector<size_t> &indices, std::mt19937 &rng,
                 Real reg, Real nu)
      : x_(x), y_(y), K(K), indices_(indices), reg(reg), nu(nu), rng(rng),
        zmins(K), zmaxs(K), histogram(K), accept_count(0) {
    this->alpha_now = DenseVector::Zero(K - 1);
    this->gamma_now = DenseVector::Zero(K - 1);
    this->alpha_to_gamma(gamma_now, alpha_now);
    this->H = DenseMatrix::Zero(K - 1, K - 1);
    for (auto i : indices_) {
      int y_label = static_cast<int>(y_(i));
      if (std::abs(y_label - y(i)) > 1e-3) {
        throw std::invalid_argument("y has a floating-point element.");
      }
      if (y_label < 0) {
        throw std::invalid_argument("y has a negative element.");
      }
      if (y_label >= K) {
        std::stringstream ss;
        ss << "y[ " << i << "] is greater than " << (K - 1) << ".";
        throw std::invalid_argument(ss.str());
      }
      histogram[y_label]++;
    }
  }

  inline Real log_p_mvt(const DenseMatrix &SigmaInverse, const DenseVector mu,
                        Real nu, const DenseVector &x) {
    Real log_p = (x - mu).transpose() * SigmaInverse * (x - mu);
    return std::log(1 + log_p / nu) * (-nu - SigmaInverse.rows()) / 2;
  }

  inline DenseVector sample_mvt(const DenseMatrix &SigmaInverse, Real nu) {
    /*Sample From multivariate t-distribution*/
    DenseVector result(SigmaInverse.rows());
    std::normal_distribution<Real> base_dist(0, 1);
    std::gamma_distribution<Real> chi_gen(nu / 2);
    for (int i = 0; i < result.rows(); i++) {
      result(i) = base_dist(rng);
    }
    Eigen::LLT<DenseMatrix, Eigen::Upper> L(SigmaInverse);
    result = L.matrixU().solve(result);
    result /= std::sqrt(chi_gen(rng) * 2 / nu);
    if (fix_gamma0) {
      result(0) = 0;
    }
    return result;
  }

  static inline void jacobian_dgamma_dalpha(DenseMatrix &J,
                                            const DenseVector &alpha) {
    /*
    J_{ij} with i=> alpha, j=>gamma
    */
    J.array() = 0;
    J(0, 0) = 1;
    if (!fix_gamma0) {
      for (int j = 1; j < alpha.rows(); j++) {
        J(0, j) = 1;
      }
    }
    for (int i = 1; i < alpha.rows(); i++) {
      Real ed = std::exp(alpha(i));
      for (int j = i; j < alpha.rows(); j++) {
        J(i, j) = ed;
      }
    }
    // d f / d alpha_0 = (df / d gamma_i) (d gamma_i / d alpha_0 )
  }

  static inline void alpha_to_gamma(DenseVector &target,
                                    const DenseVector &alpha) {
    target(0) = alpha(0);
    for (int i = 1; i < alpha.rows(); i++) {
      target(i) = target(i - 1) + std::exp(alpha(i));
    }
  }

  static inline void gamma_to_alpha(DenseVector &target,
                                    const DenseVector &gamma) {
    target(0) = gamma(0);
    for (int i = 1; i < gamma.rows(); i++) {
      target(i) = std::log(gamma(i) - gamma(i - 1));
    }
  }

  static inline void safe_ldiff(Real x, Real y, Real &loss, Real &dx, Real &dy,
                                DenseMatrix *HessianTarget = nullptr,
                                int label = 0) {
    // assert(x >= y);
    Real denominator;
    Real exp_factor;
    if (y > 0) {
      // both positive
      // erfcy = erfc * exp( y**2 / 2)
      exp_factor = std::exp((y * y - x * x) / 2);
      denominator =
          Faddeeva::erfcx(y / SQRT2) - exp_factor * Faddeeva::erfcx(x / SQRT2);

      loss -= y * y / 2;
      loss += std::log(denominator / 2);
      dx += (2 / SQRT2PI) * exp_factor / denominator;
      dy -= (2 / SQRT2PI) / denominator;
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label, label) +=
            -(SQRT2PI * x * denominator * std::exp((y * y - x * x) / 2) +
              2 * std::exp(y * y - x * x)) /
            denominator / denominator / PI;
        (*HessianTarget)(label - 1, label - 1) +=
            (SQRT2PI * y * denominator - 2) / denominator / denominator / PI;
        Real off_diag =
            2 * std::exp((y * y - x * x) / 2) / PI / denominator / denominator;
        (*HessianTarget)(label, label - 1) += off_diag;
        (*HessianTarget)(label - 1, label) += off_diag;
      }
    } else if (x < 0) {
      // both negative
      loss -= x * x / 2;

      exp_factor = std::exp((x * x - y * y) / 2);
      denominator = Faddeeva::erfcx(-x / SQRT2) -
                    exp_factor * Faddeeva::erfcx(-y / SQRT2);
      loss += std::log(denominator / 2);
      dx += (2 / SQRT2PI) / denominator;
      dy -= (2 / SQRT2PI) * exp_factor / denominator;
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label, label) +=
            -(SQRT2PI * x * denominator + 2) / PI / denominator / denominator;
        (*HessianTarget)(label - 1, label - 1) +=
            (SQRT2PI * y * exp_factor * denominator -
             2 * (exp_factor * exp_factor)) /
            PI / denominator / denominator;
        Real off_diag = 2 * exp_factor / PI / denominator / denominator;
        (*HessianTarget)(label, label - 1) += off_diag;
        (*HessianTarget)(label - 1, label) += off_diag;
      }
    } else {
      // x positive, y negative. safe to use erf
      denominator = Faddeeva::erf(x / SQRT2) - Faddeeva::erf(y / SQRT2);
      Real expxx = std::exp(-x * x / 2);
      Real expyy = std::exp(-y * y / 2);
      dx += 2 * expxx / denominator / SQRT2PI;
      dy -= 2 * expyy / denominator / SQRT2PI;
      loss += std::log(denominator / 2);
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label, label) +=
            -(SQRT2PI * x * denominator * expxx + 2 * expxx * expxx) / PI /
            denominator / denominator;
        (*HessianTarget)(label - 1, label - 1) +=
            -(-SQRT2PI * y * denominator * expyy + 2 * expyy * expyy) / PI /
            denominator / denominator;
        Real off_diag = 2 * expxx * expyy / PI / denominator / denominator;
        (*HessianTarget)(label, label - 1) += off_diag;
        (*HessianTarget)(label - 1, label) += off_diag;
      }
    }
  }

  static inline void safe_lcdf(Real x, Real &loss, Real &dx,
                               DenseMatrix *HessianTarget = nullptr,
                               int label = 0) {
    Real denominator;
    Real exp_factor;
    if (x > 1) {
      exp_factor = std::exp(-x * x / 2);
      denominator = 1 + Faddeeva::erf(x / SQRT2);
      dx += (2 / SQRT2PI) * exp_factor / denominator;
      loss += std::log(denominator / 2);
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label, label) +=
            -(SQRT2PI * x * denominator * exp_factor +
              2 * exp_factor * exp_factor) /
            PI / denominator / denominator;
      }
    } else {
      denominator = Faddeeva::erfcx(-x / SQRT2);
      dx += (2 / SQRT2PI) / denominator;
      loss -= x * x / 2;
      loss += std::log(denominator / 2);
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label, label) +=
            -(SQRT2PI * x * denominator + 2) / PI / denominator / denominator;
      }
    }
  }

  inline void safe_lccdf(Real x, Real &loss, Real &dx,
                         DenseMatrix *HessianTarget, int label = 0) {
    Real denominator;
    if (x > -1) {
      denominator = Faddeeva::erfcx(x / SQRT2);
      dx -= (2 / SQRT2PI) / denominator;
      loss += std::log(denominator / 2);
      loss -= x * x / 2;
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label - 1, label - 1) +=
            (SQRT2PI * x * denominator - 2) / denominator / denominator / PI;
      }
    } else {
      // safe to use erf
      denominator = 1 - Faddeeva::erf(x / SQRT2);
      dx -= (2 / SQRT2PI) * std::exp(-x * x / 2) / denominator;
      loss += std::log(denominator / 2);
      if (HessianTarget != nullptr) {
        Real exp_factor = std::exp(-(x * x) / 2);
        (*HessianTarget)(label - 1, label - 1) +=
            -(-SQRT2PI * x * denominator * exp_factor +
              2 * exp_factor * exp_factor) /
            PI / denominator / denominator;
      }
    }
  }

  inline void sample_z_given_cutpoint() {
    zmins.array() = std::numeric_limits<Real>::max();
    zmaxs.array() = std::numeric_limits<Real>::lowest();
    Real deviation = 1;

    for (int train_data_index : indices_) {
      int class_index = static_cast<int>(y_(train_data_index));
      Real pred_score = x_(train_data_index);
      Real z_new;

      if (class_index == 0) {
        z_new = deviation * sample_truncated_normal_right(
                                rng, (gamma_now(class_index) - pred_score) /
                                         deviation) +
                pred_score;
        zmaxs(0) = std::max(zmaxs(0), z_new);
      } else if (class_index == (K - 1)) {
        z_new =
            deviation * sample_truncated_normal_left(
                            rng, (gamma_now(K - 2) - pred_score) / deviation) +
            pred_score;
        zmins(K - 1) = std::min(zmins(K - 1), z_new);
      } else {
        z_new =
            deviation *
                sample_truncated_normal_twoside(
                    rng, (gamma_now(class_index - 1) - pred_score) / deviation,
                    (gamma_now(class_index) - pred_score) / deviation) +
            pred_score;
        zmins(class_index) = std::min(zmins(class_index), z_new);
        zmaxs(class_index) = std::max(zmaxs(class_index), z_new);
      }
      x_(train_data_index) -= z_new;
    }
  }

  inline void start_sample() {
    DenseVector alpha_hat = DenseVector::Zero(K - 1);
    find_minimum(alpha_hat);
    alpha_now = alpha_hat;
    alpha_to_gamma(gamma_now, alpha_now);
  }

  inline void sample_cutpoint_given_z() {
    for (int i = 1; i <= (K - 3); i++) {
      Real lower = zmaxs(i);
      Real upper = zmins(i + 1);
      gamma_now(i) = std::uniform_real_distribution<Real>(lower, upper)(rng);
    }
  }

  inline void find_minimum(DenseVector &alpha_hat, bool verbose = false) {
    int max_iter = 10000;
    Real epsilon = 1e-5;
    Real epsilon_rel = 1e-5;
    Real delta = 1e-5;
    int past = 3;
    DenseVector history(past);
    DenseVector alpha_new(alpha_hat);
    DenseVector dalpha(alpha_hat);
    DenseVector direction(alpha_hat);
    Real ll_current;
    bool first = true;
    int i = 0;
    while (true) {
      if (first) {
        ll_current = (*this)(alpha_hat, dalpha, &H);
        if (verbose) {
          print_to_stream(std::cout, "ll_current = ", ll_current,
                          "\ndalpha = ", dalpha);
          std::cout << std::endl;
        }
      }
      {

        Real alpha2 = alpha_hat.norm();
        Real dalpha2 = dalpha.norm();
        if (verbose) {
          print_to_stream(std::cout, "ll = ", ll_current,
                          "\nalpha_hat =", alpha_hat);
          std::cout << std::endl;

          print_to_stream(std::cout, "dalpha2 = ", dalpha2);
          std::cout << std::endl;
        }

        if (dalpha2 < epsilon || dalpha2 < epsilon_rel * alpha2) {
          break;
        }
      }

      direction = -H.llt().solve(dalpha);
      if (verbose) {
        print_to_stream(std::cout, "H = ", H);
        std::cout << std::endl;

        print_to_stream(std::cout, "direction = ", direction);
        std::cout << std::endl;
      }

      Real step_size = 1;
      int lsc = 0;
      while (true) {
        alpha_new = alpha_hat + step_size * direction;
        Real ll_new;
        try {
          ll_new = (*this)(alpha_new, dalpha, &H);
        } catch (std::runtime_error) {
          step_size /= 2;
          continue;
        }

        if (ll_new >= (ll_current * (1 + delta))) {
          step_size /= 2;
        } else {
          alpha_hat = alpha_new;
          ll_current = ll_new;
          break;
        }
        if (++lsc > 1000)
          break;
      }
      first = false;
      if (i >= past) {
        Real past_loss = history(i % past);
        if (std::abs(past_loss - ll_current) <=
            delta *
                std::max(std::max(abs(ll_current), abs(past_loss)), Real(1))) {
          break;
        }
      }
      history(i % past) = ll_current;
      i++;
      if (i >= max_iter)
        break;
    }
    if (i == max_iter) {
      throw std::runtime_error("Failed to converge. See fail-log.txt");
    }
  }

  inline bool step(bool verbose = false) {
    DenseVector alpha_hat = alpha_now;
    DenseVector gamma(alpha_hat);
    find_minimum(alpha_hat, verbose);
    DenseVector alpha_candidate = sample_mvt(H, nu) + alpha_hat;

    Real ll_candidate, ll_old;
    try {
      ll_candidate = -(*this)(alpha_candidate, gamma);
      ll_old = -(*this)(alpha_now, gamma);
    } catch (std::runtime_error e) {
      // should be NaN encounter
      return false;
    }
    Real log_p_transition_candidate =
        log_p_mvt(H, alpha_hat, nu, alpha_candidate);
    Real log_p_transition_old = log_p_mvt(H, alpha_hat, nu, alpha_now);
    Real test_ratio = std::exp(ll_candidate - log_p_transition_candidate -
                               ll_old + log_p_transition_old);
    Real u = std::uniform_real_distribution<Real>{0, 1}(rng);
    if (u < test_ratio) {
      alpha_now = alpha_candidate;
      alpha_to_gamma(gamma_now, alpha_now);
      accept_count++;
      return true;
    } else {
      return false;
    }
  }

  inline Real operator()(const DenseVector &alpha, DenseVector &dalpha,
                         DenseMatrix *HessianTarget = nullptr) {
    DenseVector gamma = DenseVector::Zero(alpha.rows());
    dalpha.array() = 0;
    alpha_to_gamma(gamma, alpha);

    DenseMatrix dGammadAlpha = DenseMatrix(alpha.rows(), alpha.rows());
    jacobian_dgamma_dalpha(dGammadAlpha, alpha);
    Real ll = 0;
    if (HessianTarget != nullptr) {

      (*HessianTarget).array() = 0;
    }
    for (auto i : indices_) {
      int label = y_(i);
      if (label == 0) {
        safe_lcdf(gamma(0) - x_(i), ll, dalpha(0), HessianTarget, label);
      } else if (label == (K - 1)) {
        safe_lccdf(gamma(K - 2) - x_(i), ll, dalpha(K - 2), HessianTarget,
                   label);
      } else {
        safe_ldiff(gamma(label) - x_(i), gamma(label - 1) - x_(i), ll,
                   dalpha(label), dalpha(label - 1), HessianTarget, label);
      }
    }

    if (HessianTarget != nullptr) {
      DenseMatrix &H = (*HessianTarget);
      DenseVector expAlpha(alpha.array().exp().matrix());
      H = dGammadAlpha * H * dGammadAlpha.transpose();
      {
        // m = 0
        // gamma 0 = alpha_0 does not contribute
        // since \partial^2 gamma_0 / \partial alpha_i \partial alpha_j = 0 for
        // all gamma_m = alpha_0 + \sum _{s=1}^{m}(exp\alpha_s)
        for (int m = 1; m < (K - 1); m++) {
          { // i =0, j > 0
            for (int j = 1; j <= m; j++) {
              H(j, j) += dalpha(m) * expAlpha(j);
            }
          }
        }
      }
      H(0, 0) -= reg;
      for (int m = 1; m < (K - 1); m++) {
        H(m, m) -= reg;
      }
      H.array() *= -1;
      if (H.hasNaN()) {
        fail_dump();
        throw std::runtime_error(print_to_string(
            __FILE__, ":", __LINE__, " H has NaN, alpha = ", alpha));
      }
    }
    if (fix_gamma0) {
      dalpha(0) = 0;
      if (HessianTarget != nullptr) {
        (*HessianTarget).row(0).array() = 0;
        (*HessianTarget).col(0).array() = 0;
        (*HessianTarget)(0, 0) = 1;
      }
    }
    dalpha = -dGammadAlpha * dalpha;
    if (dalpha.hasNaN()) {
      fail_dump();
      throw std::runtime_error(print_to_string(
          __FILE__, ":", __LINE__, " dalpha has NaN, alpha = ", alpha));
    }

    dalpha(0) += reg * alpha(0);
    ll -= 0.5 * reg * alpha(0) * alpha(0);
    for (int m = 1; m < (K - 1); m++) {
      dalpha(m) += reg * alpha(m);
      ll -= 0.5 * reg * alpha(m) * alpha(m);
    }
    return -ll;
  }

  template <class ostype> inline void show_info(ostype &os) {
    os << "{\"xs\": [";
    bool first = true;
    for (auto i : indices_) {
      if (!first)
        os << ", ";
      os << x_[i];
      first = false;
    }
    os << "], \"ys\":[";
    first = true;
    for (auto i : indices_) {
      if (!first)
        os << ", ";
      os << y_[i];
      first = false;
    }
    os << "]}";
  }

  inline void fail_dump() {
    std::ofstream fail_log("fail-log.json");
    show_info(fail_log);
  }

  DenseVector &x_;
  const DenseVector &y_;

  int K;
  const std::vector<size_t> indices_;
  Real tune = 1;
  Real reg;
  Real nu;
  std::mt19937 &rng;
  DenseVector alpha_now;
  DenseVector gamma_now;
  DenseMatrix H;
  static constexpr bool fix_gamma0 = false;
  DenseVector zmins, zmaxs;
  std::vector<size_t> histogram;
  size_t accept_count;
};

} // namespace myFM


//Authors: Dario Cattaruzza, Alessandro Abate, Peter Schrammel, Daniel Kroening
//University of Oxford 2016
//This code is supplied under the BSD license agreement (see license.txt)

#include <math.h>

#include <Eigen/Eigenvalues>

#include <boost/timer.hpp>

#include "JordanMatrix.h"
#include "MatrixToString.h"

namespace abstract{

template <class scalar>
scalar  JordanMatrix<scalar>::ms_half(0.5);

template <class scalar>
scalar  JordanMatrix<scalar>::ms_one(1);

template <class scalar>
scalar  JordanMatrix<scalar>::ms_two(2);

template <class scalar>
typename JordanMatrix<scalar>::complexS JordanMatrix<scalar>::ms_complexOne(1,0);

template <class scalar>
MatToStr<scalar>  JordanMatrix<scalar>::ms_logger(true);

template <class scalar>
MatToStr<scalar>  JordanMatrix<scalar>::ms_decoder(false);

template <class scalar>
typename JordanMatrix<scalar>::MatrixS JordanMatrix<scalar>::ms_emptyMatrix(0,0);

template <class scalar>
traceDynamics_t JordanMatrix<scalar>::ms_trace_dynamics=eTraceNoDynamics;

template <class scalar>
bool JordanMatrix<scalar>::ms_trace_time=false;

/// Constructs an empty matrix
template <class scalar>
JordanMatrix<scalar>::JordanMatrix(int dimension) :
  m_dimension(dimension),
  m_zero(func::ms_weakZero),
  m_largeZero(dimension*dimension*func::ms_weakZero),
  m_dynamics(dimension,dimension),
  m_refDynamics(dimension,dimension),
  m_eigenSpace(dimension),
  m_minSeparation(dimension,1),
  m_jordanTime(0)
{
  m_pseudoEigenVectors=MatrixS(m_dimension,m_dimension);
  m_invPseudoEigenVectors=MatrixS(m_dimension,m_dimension);
}

/// Changes the default dimension of the system
template <class scalar>
void JordanMatrix<scalar>::changeDimensions(const int dimension)
{
  if (dimension!=m_dimension) {
    m_dimension=dimension;
    m_dynamics.conservativeResize(dimension,dimension);
    m_minSeparation.conservativeResize(dimension,1);
    m_refDynamics.resize(dimension,dimension);
    m_pseudoEigenVectors.resize(dimension,dimension);
    m_invPseudoEigenVectors.resize(dimension,dimension);
  }
}

/// Loads a matrix from a given description
template <class scalar>
int JordanMatrix<scalar>::load(const std::string &data,size_t pos)
{
  int result=ms_logger.StringToMat(m_dynamics,data,pos);
  //changeDimensions(m_dynamics.rows());
  if ((result>0) && !calculateJordanForm()) return -1;
  return result;
}

template <class scalar>
bool JordanMatrix<scalar>::loadFromRef(const MatrixR &matrix)
{
  changeDimensions(matrix.rows());
  for (int row=0;row<matrix.rows();row++) {
    for (int col=0;col<matrix.cols();col++) {
      m_dynamics.coeffRef(row,col)=matrix.coeff(row,col);
    }
  }
  return calculateJordanForm();
}

template <class scalar>
bool JordanMatrix<scalar>::load(const MatrixS &dynamics)
{
  changeDimensions(dynamics.rows());
  m_dynamics=dynamics;
  return calculateJordanForm();
}

template <class scalar>
bool JordanMatrix<scalar>::loadJordan(const MatrixS &matrix)
{
  boost::timer timer;
  changeDimensions(matrix.rows());
  m_dynamics=matrix;
  m_zero=calculateEpsilon(m_dynamics);
  func::setZero(m_zero);
  m_largeZero=m_zero*m_dimension*m_dimension;
  interToRef(m_refDynamics,m_dynamics);
  m_pseudoEigenValues=m_dynamics;
  m_pseudoEigenVectors=MatrixS::Identity(m_dimension,m_dimension);
  m_invPseudoEigenVectors=m_pseudoEigenVectors;
  m_hasOnes=false;
  m_hasMultiplicities=false;
  m_isOne.resize(2*m_dimension);
  m_conjugatePair.resize(2*m_dimension);
  m_jordanIndex.resize(2*m_dimension);
  for (int i=0;i<m_dimension;i++) {
    m_conjugatePair[i]=-1;
    m_conjugatePair[i+m_dimension]=-1;
    if ((i<m_dimension-1) && !func::isZero(m_dynamics.coeff(i,i+1))) m_conjugatePair[i]=i+1;
    else if ((i>0) && !func::isZero(m_dynamics.coeff(i-1,i)))        m_conjugatePair[i]=i-1;
    int mult=(m_conjugatePair[i]<0) ? 1 : 2;
    m_jordanIndex[i]=0;
    if ((i>=mult) && !func::isZero(m_dynamics.coeff(i,i-mult))) m_jordanIndex[i-mult]+1;
    m_jordanIndex[i+m_dimension]=0;
    if (m_jordanIndex[i]>0) m_hasMultiplicities=true;
    m_isOne[i]=func::isZero(ms_one-m_dynamics.coeff(i,i)) && (m_conjugatePair[i]<0);
    m_hasOnes|=m_isOne[i];
  }
  m_eigenValues=pseudoToJordan(m_pseudoEigenValues,eToEigenValues);
  m_eigenNorms.resize(m_eigenValues.rows(),1);
  for (int i=0;i<m_eigenValues.rows();i++) m_eigenNorms.coeffRef(i,0)=func::norm2(m_eigenValues.coeff(i,i));
  m_eigenVectors=MatrixC::Identity(m_dimension,m_dimension);
  m_invEigenVectors=m_eigenVectors;
  m_error=func::ms_hardZero;
  m_jordanTime=timer.elapsed()*1000;
  if (ms_trace_time) ms_logger.logData(m_jordanTime,"Pole Extraction time:",true);
  return true;
}

/// calculates the estimated roundoff error of a matrix operation
template <class scalar>
template <class MatrixType> inline typename JordanMatrix<scalar>::refScalar JordanMatrix<scalar>::calculateEpsilon(const MatrixType &matrix)
{
  if (matrix.rows()>0) {
    refScalar max=func::toUpper(func::norm2(matrix.coeff(0,0)));
    refScalar min=func::toLower(func::norm2(matrix.coeff(0,0)));
    for (int row=0;row<matrix.rows();row++) {
      for (int col=0;col<matrix.cols();col++) {
        refScalar upper=func::toUpper(func::norm2(matrix.coeff(row,col)));
        refScalar lower=func::toLower(func::norm2(matrix.coeff(row,col)));
        if (upper>max) max=upper;
        if (lower<min) min=lower;
      }
    }
    //scalar max=matrix.maxCoeff();
    //scalar min=matrix.minCoeff();
    if (-min>max) max=-min;
    return max*func::ms_weakEpsilon;
  }
  return 0;
}

/// Loads the transformation matrix for the state space
template <class scalar>
bool JordanMatrix<scalar>::calculateJordanForm(bool includeSvd)
{
  boost::timer timer;
  m_zero=calculateEpsilon(m_dynamics);
  func::setZero(m_zero);
  m_largeZero=m_zero*m_dimension*m_dimension;
  interToRef(m_refDynamics,m_dynamics);

  m_eigenSpace.computeJordan(m_refDynamics);
  if (ms_trace_time) ms_logger.logData(timer.elapsed()*1000,"Jordan Form:",true);
  if (m_eigenSpace.info()!=Eigen::Success) {
    if (ms_trace_dynamics>=eTraceDynamics) ms_logger.logData("Failed to find Jordan Form");
    return false;
  }
  refToInter(m_eigenValues,m_eigenSpace.getEigenValues());
  refToInter(m_eigenVectors,m_eigenSpace.getEigenVectors());

  m_jordanIndex=m_eigenSpace.getJordanIndeces();
  m_conjugatePair=m_eigenSpace.getConjugatePairs();
  m_isOne=m_eigenSpace.getOnes();
  m_hasOnes=m_eigenSpace.hasOnes();
  m_hasMultiplicities=m_eigenSpace.hasMultiplicities();
  m_eigenNorms.resize(m_eigenValues.rows(),1);
  for (int i=0;i<m_eigenValues.rows();i++) m_eigenNorms.coeffRef(i,0)=func::norm2(m_eigenValues.coeff(i,i));
  try {
    m_invEigenVectors=m_eigenVectors.inverse();
  }
  catch(...) {
    refToInter(m_invEigenVectors,m_eigenSpace.getEigenVectors().inverse());
  }
  if (ms_trace_dynamics>=eTraceDynamics) {
    ms_logger.logData(m_dynamics,"Dynamics:");
    ms_logger.logData(m_eigenValues,"EigenValues:");
    ms_logger.logData(m_eigenVectors,"EigenVectors:");
    ms_logger.logData(m_invEigenVectors,"InvEigenVectors:");
  }

  m_pseudoEigenValues=jordanToPseudoJordan(m_eigenValues,eToEigenValues);
  m_pseudoEigenVectors=jordanToPseudoJordan(m_eigenVectors,eToEigenVectors);
  m_invPseudoEigenVectors=m_pseudoEigenVectors.inverse();
  if (ms_trace_dynamics>=eTraceDynamics) {
    MatrixS pseudoCalculated=m_pseudoEigenVectors*m_pseudoEigenValues*m_invPseudoEigenVectors;
    ms_logger.logData(m_pseudoEigenValues,"PseudoEigenValues");
    ms_logger.logData(m_pseudoEigenVectors,"PseudoEigenVectors");
    ms_logger.logData(m_invPseudoEigenVectors,"InvPseudoEigenVectors");
    ms_logger.logData(pseudoCalculated,"PseudoCalc");
  }
  if (includeSvd) {
    calculateBlockSVD();
    m_minSigma=func::toLower(m_blockSingularValues.coeff(0,0));
    m_maxSigma=func::toUpper(m_blockSingularValues.coeff(0,0));
    for (int row=1;row<m_blockSingularValues.rows();row++) {
      if (func::toUpper(m_blockSingularValues.coeff(row,0))>m_maxSigma) {
        m_maxSigma=func::toUpper(m_blockSingularValues.coeff(row,0));
      }
      if (func::toLower(m_blockSingularValues.coeff(row,0))<m_minSigma) {
        m_minSigma=func::toLower(m_blockSingularValues.coeff(row,0));
      }
    }
    m_jordanTime=timer.elapsed()*1000;
    if (ms_trace_time) ms_logger.logData(m_jordanTime,"SVD time:",true);
  }
  calculateEigenError();
  m_jordanTime=timer.elapsed()*1000;
  if (ms_trace_time) ms_logger.logData(m_jordanTime,"Jordan Error time:",true);
  for (int row=0;row<m_invPseudoEigenVectors.rows();row++) {
    for (int col=0;col<m_invPseudoEigenVectors.cols();col++) {
      if (func::isNan(m_invPseudoEigenVectors.coeff(row,col))) {
        return false;
      }
    }
  }
  return true;
}

/// Retrieves an equivalent real Jordan from a complex representation
template <class scalar>
typename JordanMatrix<scalar>::MatrixS JordanMatrix<scalar>::jordanToPseudoJordan(const MatrixC &source,const pseudoType_t conversionType)
{
  MatrixS result=source.real();
  if (conversionType==eToEigenValues) {
    for (int col=0;col<source.rows();col++) {
      if (m_conjugatePair[col]>col) {
        result.coeffRef(col+1,col)=-source.coeff(col,col).imag();
        result.coeffRef(col,col+1)=source.coeff(col,col).imag();
        for (int offset=1;offset<=m_jordanIndex[col];offset++) {
          int row=col-2*offset;
          result.coeffRef(row+1,col)=-source.coeff(row,col).imag();
          result.coeffRef(row,col+1)=source.coeff(row,col).imag();
        }
        col++;
      }
    }
  }
  else if (conversionType==eToEigenVectors) {
    for (int col=0;col<source.cols();col++) {
      if (m_conjugatePair[col]>col) {
        result.col(col+1)=source.col(col).imag();
        col++;
      }
    }
  }
  else {
      for (int row=0;row<source.rows();row++) {
        if (m_conjugatePair[row]>row) {
          result.row(row+1)=source.row(row).imag();
          row++;
        }
      }
  }
  return result;
}

/// Retrieves an equivalent complex Jordan from a real representation
template <class scalar>
typename JordanMatrix<scalar>::MatrixC JordanMatrix<scalar>::pseudoToJordan(const MatrixS &source,const pseudoType_t conversionType)
{
  MatrixC result=MatrixC::Zero(source.rows(),source.cols());
  if (conversionType==eToEigenValues) {
    int mult=1;
    for (int row=0;row<source.rows();row+=mult) {
      mult=(m_conjugatePair[row]<0) ? 1 : 2;
      if (m_jordanIndex[row+mult]>0) {
        if (m_conjugatePair[row]<0) {
          result.coeffRef(row,row)=complexS(source.coeff(row,row),func::ms_hardZero);
        }
        else {
          result.coeffRef(row,row)=complexS(source.coeff(row,row),source.coeff(row,row+1));
          result.coeffRef(row+1,row+mult+1)=ms_complexOne;
        }
        result.coeffRef(row,row+mult)=ms_complexOne;
      }
      else {
        result.coeffRef(row,row)=complexS(source.coeff(row,row),source.coeff(row,row+1));
      }
    }
  }
  else if (conversionType==eToEigenVectors) {
    for (int col=0;col<source.cols();col++) {
      result.col(col).real()=source.col(col);
      if (m_conjugatePair[col]>col) {
        result.col(col).imag()=source.col(col+1);
        col++;
        result.col(col).real()=source.col(col-1);
        result.col(col).imag()=-source.col(col);
      }
    }
  }
  else {
    for (int row=0;row<source.rows();row++) {
      result.row(row).real()=source.row(row);
      if (m_conjugatePair[row]>row) {
        result.row(row).imag()=source.row(row+1);
        row++;
        result.row(row).real()=source.row(row-1);
        result.row(row).imag()=-source.row(row);
      }
    }
  }
  return result;
}

/// Retrieves a scalar matrix from a refScalar one
template <class scalar>
void JordanMatrix<scalar>::interToRef(SolverMatrixType &dest,const MatrixS &source)
{
  dest.conservativeResize(source.rows(),source.cols());
  for (int row=0;row<source.rows();row++) {
    for (int col=0;col<source.cols();col++) {
      refScalar coef=func::toCentre(source.coeff(row,col));
      dest.coeffRef(row,col)=coef;
    }
  }
}

/// Retrieves a scalar matrix from a refScalar one
template <class scalar>
void JordanMatrix<scalar>::refToInter(MatrixC &dest,const SolverComplexMatrixType &source)
{
  dest.conservativeResize(source.rows(),source.cols());
  for (int row=0;row<source.rows();row++) {
    for (int col=0;col<source.cols();col++) {
      dest.coeffRef(row,col)=func::toScalar(source.coeff(row,col));
    }
  }
}

/// Retrieves a scalar matrix from a refScalar one
template <class scalar>
void JordanMatrix<scalar>::refToInter(MatrixS &dest,const SolverMatrixType &source)
{
  dest.conservativeResize(source.rows(),source.cols());
  for (int row=0;row<source.rows();row++) {
    for (int col=0;col<source.cols();col++) {
      dest.coeffRef(row,col)=source.coeff(row,col);
    }
  }
}

/// Transforms the matrix to Reduced Row Echelon Form
template <class scalar>
int JordanMatrix<scalar>::toRREF(MatrixC &matrix)
{
  int rank=m_dimension;
  int col=0;
  for (int row=0;col<matrix.rows();row++,col++) {
    while (func::isZero(func::norm2(matrix.coeff(row,col)),m_zero)) {
      for (int row2=row+1;row2<matrix.rows();row2++) {
        if (!func::isZero(func::norm2(matrix.coeff(row2,col)),m_zero)) {
          matrix.row(row)+=matrix.row(row2);
          break;
        }
      }
      if (func::isZero(func::norm2(matrix.coeff(row,col)),m_zero)) {
        col++;
        if (col==matrix.rows()) break;
      }
    }
    if (col==matrix.rows()) break;
    complexR multiplier=func::toCentre(matrix.coeff(row,col));
    if (!func::isZero(func::norm2(multiplier),m_zero)) {
      for (int col2=0;col2<matrix.cols();col2++) matrix.coeffRef(row,col2)/=multiplier;
      rank--;
    }
    for (int row2=row+1;row2<matrix.rows();row2++) {
      complexS multiplier=matrix.coeff(row2,col);
      matrix.row(row2)-=multiplier*matrix.row(row);
    }
  }
  col=0;
  for (int row=1;(row<matrix.rows()) && (col<matrix.cols());row++) {
    while ((col<matrix.cols()) && func::isZero(func::norm2(matrix.coeff(row,col)),m_zero)) col++;
    if (col<matrix.cols()) {
      for (int row2=0;row2<row;row2++) {
        if (!func::isZero(func::norm2(matrix.coeff(row2,col)),m_zero)) {
          complexS multiplier=matrix.coeff(row2,col);
          matrix.row(row2)-=multiplier*matrix.row(row);
        }
      }
    }
  }
  return rank;
}

/// Returns the description of a complex matrix
template <class scalar>
std::string JordanMatrix<scalar>::getMatrix(const MatrixC &matrix,bool brackets)
{
  if (brackets) return ms_logger.MatToString(matrix);
  return ms_decoder.MatToString(matrix);
}

/// Returns the description of a matrix
template <class scalar>
std::string JordanMatrix<scalar>::getMatrix(const MatrixS &matrix,bool brackets)
{
  if (brackets) return ms_logger.MatToString(matrix);
  return ms_decoder.MatToString(matrix);
}

/// Returns the complex eigenvector matrix (S)
template <class scalar>
std::string JordanMatrix<scalar>::getEigenVectorsDesc(bool pseudo)
{
  if (pseudo) return getMatrix(m_pseudoEigenVectors,false);
  return getMatrix(m_eigenVectors,false);
}

/// Returns the singular values of the matrix
template <class scalar>
std::string JordanMatrix<scalar>::getSingularValuesDesc()
{
  return getMatrix(m_blockSingularValues,false);
}

/// Returns the inverse complex eigenvector matrix (S^-1)
template <class scalar>
std::string JordanMatrix<scalar>::getInvEigenVectorsDesc(bool pseudo)
{
  if (pseudo) return getMatrix(m_invPseudoEigenVectors,false);
  return getMatrix(m_invEigenVectors,false);
}

/// Returns the schur decomposition of the dynamics
template <class scalar>
std::string JordanMatrix<scalar>::getSJinvS()
{
  MatrixC matrix=m_eigenVectors;
  std::string result=getMatrix(matrix);
  result+=getMatrix(m_eigenValues);
  matrix=m_eigenVectors.inverse();
  result+=getMatrix(matrix);
  return result;
}

/// Retrieves the inverse of the dynamics
template <class scalar>
typename JordanMatrix<scalar>::MatrixS JordanMatrix<scalar>::getPseudoInverse(const MatrixS &matrix,bool &hasInverse)
{
  load(matrix);
  MatrixC diag=m_eigenValues;
  hasInverse=true;
  for (int i=0;i<m_dimension;i++) {
    char sign=func::hardSign(func::norm2(diag.coeff(i,i)));
    if (sign!=0) {
      diag.coeffRef(i,i)=ms_complexOne/diag.coeff(i,i);
      for (int j=1;j<=m_jordanIndex[i];j++) {
        diag.coeffRef(i-j,i)=-func::c_pow(-diag.coeffRef(i,i),j+1);
      }
    }
    else hasInverse=false;
  }
  if (hasInverse) return m_dynamics.inverse();
  diag=m_invEigenVectors*diag*m_eigenVectors;
  return diag.real();
}

/// Calculates the pseudoinverse of a matrix
template <class scalar>
typename JordanMatrix<scalar>::MatrixS JordanMatrix<scalar>::getSVDpseudoInverse(const MatrixS &matrix,bool &hasInverse)
{
  interToRef(m_refDynamics,matrix);
  m_svdSpace.compute(m_refDynamics, Eigen::ComputeFullU | Eigen::ComputeFullV);
  SolverMatrixType d=m_svdSpace.singularValues().asDiagonal();
  SolverMatrixType u=m_svdSpace.matrixU();
  SolverMatrixType v=m_svdSpace.matrixV();
  MatrixS diag;
  MatrixS matrixU;
  MatrixS matrixV;
  refToInter(diag,d);
  refToInter(matrixU,u);
  refToInter(matrixV,v);
  if (ms_trace_dynamics>=eTraceDynamics) {
    ms_logger.logData(matrix,"Inverse:");
    ms_logger.logData(matrixU);
    ms_logger.logData(diag);
    ms_logger.logData(matrixV);
  }
  for (int i=0;i<matrix.rows();i++) {
    if (func::isZero(diag.coeff(i,i))) hasInverse=false;
    else diag.coeffRef(i,i)=scalar(1)/diag.coeff(i,i);
  }
  if (matrix.rows()!=matrix.cols()) {
    diag.conservativeResize(matrixV.cols(),matrixU.rows());
    if (matrixV.cols()>matrixU.rows()) {
      diag.block(matrixU.rows(),0,matrixV.cols()-matrixU.rows(),matrixU.rows())=MatrixS::Zero(matrixV.cols()-matrixU.rows(),matrixU.rows());
    }
    else {
      diag.block(0,matrixV.cols(),matrixV.cols(),matrixU.rows()-matrixV.cols())=MatrixS::Zero(matrixV.cols(),matrixU.rows()-matrixV.cols());
    }
    hasInverse=false;
  }
  if (hasInverse) return matrix.inverse();
  return matrixV*diag*matrixU.adjoint();
}

/// Calculates a lower bound for the minimum separation between any two jordan blocks
template <class scalar>
typename JordanMatrix<scalar>::refScalar JordanMatrix<scalar>::calculateMinSeparation()
{
  MatrixS base;
  refToInter(base,m_eigenSpace.getSchur());
  scalar nonDiagNorm2=0;
  for (int row=0;row<m_dimension-1;row++) {
    if (m_conjugatePair[row]<row) nonDiagNorm2+=func::squared(base.coeff(row,row+1));//*base.coeff(row,row+1);
    for (int col=row+2;col<m_dimension;col++) {
      nonDiagNorm2+=func::squared(base.coeff(row,col));//*base.coeff(row,col);
    }
  }
  scalar dimension=m_dimension;
  scalar factor=sqrt(func::pow((dimension-ms_one)/dimension,m_dimension-1));
  factor*=sqrt(func::pow(ms_two,(m_dimension-1)*(m_dimension+1)));//The smallest col/row is ignored

  for (int i=0;i<m_dimension;i++) {
    scalar diagNorm2=0;
    scalar det=ms_one;
    int mult=(m_conjugatePair[i]<0) ? 1 : 2;
    m_minSeparation.coeffRef(i,0)=func::toLower(func::norm2(m_eigenValues.coeff(i,i)));
    for (int j=0;j<m_dimension;j++) {
      if (i+mult*m_jordanIndex[j]==j) continue;
      scalar sep=func::norm2(m_eigenValues.coeff(i,i)-m_eigenValues.coeff(j,j));
      det*=sep;
      diagNorm2+=func::squared(sep);//*sep;
    }
    if (func::toLower(det)==0) func::imprecise(det,func::ms_hardZero);
    diagNorm2+=nonDiagNorm2;
    scalar maxColOrRowProd=func::pow(sqrt(diagNorm2),m_dimension-1);
    while (m_jordanIndex[i+mult]>0) i+=mult;
    if (m_jordanIndex[i]>0) {
      maxColOrRowProd*=func::pow(dimension,m_dimension);
      m_minSeparation.coeffRef(i,0)=func::toLower(factor*func::pow(det,m_jordanIndex[i])/maxColOrRowProd);
      for (int j=0;j<m_jordanIndex[i]*mult;j++) m_minSeparation.coeffRef(i-j,0)=m_minSeparation.coeffRef(i,0);
    }
    else m_minSeparation.coeffRef(i,0)=func::toLower(factor*det/maxColOrRowProd);
    if (m_conjugatePair[i]>i) {
      m_minSeparation.coeffRef(i+1,0)=m_minSeparation.coeffRef(i,0);
      i++;
    }
  }
  return m_minSeparation.minCoeff();
}

/// Calculates the maximum error for the numerical approximation of the eigencvalues
template <class scalar>
scalar JordanMatrix<scalar>::calculateEigenError()
{
  scalar kP=m_pseudoEigenVectors.norm()*m_invPseudoEigenVectors.norm();
  MatrixS calculated=m_pseudoEigenVectors*m_pseudoEigenValues*m_invPseudoEigenVectors;
  if (ms_trace_dynamics>=eTraceErrors) ms_logger.logData(calculated,"Calculated:");
  calculated-=m_dynamics;
  scalar errorNorm=calculated.norm();
  m_error=errorNorm*kP;
  if (func::toUpper(m_error)>m_zero) func::imprecise(m_error,m_zero);

  m_boundForError=0;
  if (m_hasMultiplicities) return m_error;

  if (ms_trace_dynamics>=eTraceErrors) ms_logger.logData(m_error,"Error:",true);
  m_error=func::setpm(m_error);
  complexS complexError=m_error;
  for (int i=0;i<m_dimension;i++) {
    m_eigenValues.coeffRef(i,i)+=complexError;
//    m_eigenValues.coeffRef(i,i).real()+=m_error;
//    if (m_conjugatePair[i]>=0) m_eigenValues.coeffRef(i,i).imag()+=m_error;
  }
  m_pseudoEigenValues=jordanToPseudoJordan(m_eigenValues,eToEigenValues);

  calculateMinSeparation();
  for (int i=0;i<m_dimension;i++) {
    if (m_minSeparation.coeff(i,0)>0) {
      scalar angleError=errorNorm/m_minSeparation.coeff(i,0);
      scalar cosTheta=func::toLower(func::cosine(angleError));
      scalar invCosTheta=ms_one/cosTheta;
      scalar vError=func::getHull(cosTheta,invCosTheta);
      m_eigenVectors.col(i)*=vError;
    }
  }
  m_pseudoEigenVectors=jordanToPseudoJordan(m_eigenVectors,eToEigenVectors);
  m_invPseudoEigenVectors=m_pseudoEigenVectors.inverse();//jordanToPseudoJordan(m_invEigenVectors,eToInvEigenVectors);
  if (ms_trace_dynamics>=eTraceDynamics) {
    ms_logger.logData(m_pseudoEigenValues,"PseudoEigenValues");
    ms_logger.logData(m_pseudoEigenVectors,"PseudoEigenVectors");
    ms_logger.logData(m_invPseudoEigenVectors,"InvPseudoEigenVectors");
  }
  return m_error;
}

/// Calculates the maximum error for the numerical approximation of the matrix to the nth power
/// @return the maximum variation of the eigenvalues
template <class scalar>
scalar JordanMatrix<scalar>::calculateBoundedEigenError(scalar iteration)
{
  if (func::isNegative(iteration)) iteration=-iteration;
  if (func::isZero(iteration-m_boundForError)) return m_error;
  m_pseudoEigenVectors=jordanToPseudoJordan(m_eigenVectors,eToEigenVectors);
  m_invPseudoEigenVectors=m_pseudoEigenVectors.inverse();//jordanToPseudoJordan(m_invEigenVectors,eToInvEigenVectors);
  if (ms_trace_dynamics>=eTraceDynamics) {
      ms_logger.logData(m_pseudoEigenVectors,"S");
      ms_logger.logData(m_pseudoEigenValues,"J");
      ms_logger.logData(m_invPseudoEigenVectors,"invS");
  }
  MatrixS calculated=m_pseudoEigenVectors*m_pseudoEigenValues*m_invPseudoEigenVectors;
  if (ms_trace_dynamics>=eTraceErrors) ms_logger.logData(calculated,"Calculated");
  MatrixS jordanError=m_invPseudoEigenVectors*m_dynamics*m_pseudoEigenVectors;
  if (ms_trace_dynamics>=eTraceErrors) ms_logger.logData(jordanError,"Calculated Jordan");
  jordanError-=m_pseudoEigenValues;
  if (ms_trace_dynamics>=eTraceErrors) ms_logger.logData(jordanError,"Jordan Error");
  m_error=jordanError.norm();
  scalar theta=acos((ms_one-m_error)/(ms_one+m_error));
  scalar nTheta=iteration*theta;
  if (func::toUpper(nTheta)>m_zero) func::imprecise(nTheta,m_zero);
  scalar cosn=func::toLower(func::cosine(nTheta));
  scalar invCosN=ms_one/cosn;
  scalar vError=func::getHull(cosn,invCosN);
  m_pseudoEigenVectors*=vError;
  m_invPseudoEigenVectors=m_pseudoEigenVectors.inverse();
  return m_error;
}

/// Calculates the singular values
template <class scalar>
bool JordanMatrix<scalar>::calculateSVD()
{
  boost::timer timer;
  MatrixS dynamicsSq=m_dynamics*m_dynamics.transpose();
  interToRef(m_refDynamics,dynamicsSq);
  m_eigenSpace.computeJordan(m_refDynamics);
  if (ms_trace_time) ms_logger.logData(timer.elapsed()*1000,"Full Svd:",true);
  if (m_eigenSpace.info()!=Eigen::Success) return false;
  MatrixS singularValues,singularVectors,inverseVectors;
  refToInter(singularValues,m_eigenSpace.getEigenValues().real());
  refToInter(singularVectors,m_eigenSpace.getEigenVectors().real());
  inverseVectors=singularVectors.inverse();
  scalar kP=singularVectors.norm()*inverseVectors.norm();
  dynamicsSq-=singularVectors*singularValues*singularVectors.inverse();
  scalar error=dynamicsSq.norm()*kP;
  if (func::toUpper(m_error)>m_zero) func::imprecise(m_error,m_zero);
  error=func::setpm(error);
  for (int i=0;i<m_dimension;i++) {
    singularValues.coeffRef(i,i)+=error;
    singularValues.coeffRef(i,i)=sqrt(singularValues.coeff(i,i));
  }
  return true;
}

/// Calculates the singular values for each Jordan Block
template <class scalar>
void JordanMatrix<scalar>::calculateBlockSVD()
{
  m_blockSingularValues.resize(m_dimension,2);
  for (int row=m_dimension-1;row>=0;row--) {
    if (m_jordanIndex[row]>0) {
      int blockSize=(m_jordanIndex[row]+1);
      MatrixR jordanBlock=MatrixR::Zero(m_dimension,m_dimension);
      jordanBlock.coeffRef(0,0)=func::toUpper(m_eigenNorms.coeff(row,0));
      for (int i=1;i<blockSize;i++) {
        jordanBlock.coeffRef(i,i)=jordanBlock.coeff(0,0);
        jordanBlock.coeffRef(i,i-1)=1;
      }
      m_svdSpace.compute(jordanBlock);
      scalar norm=m_svdSpace.singularValues().coeff(0)*(ms_one+m_dimension*m_dimension*Eigen::NumTraits<refScalar>::epsilon());
      if (m_conjugatePair[row]>=0) blockSize*=2;
      for (int j=0;j<blockSize;j++) m_blockSingularValues.coeffRef(row--,0)=norm;
      row++;
    }
    else {
      m_blockSingularValues.coeffRef(row,0)=m_eigenNorms.coeff(row,0);
    }
  }
  m_blockSingularValues.col(1)=MatrixS::Ones(m_dimension,1);
  for (int row=1;row<m_dimension;row++) {
    if (m_jordanIndex[row]>0)
    {
      int mult=(m_conjugatePair[row]>=0) ? 2 : 1;
      m_blockSingularValues.coeffRef(row,1)=m_blockSingularValues.coeff(row-mult,1)/(this->ms_one-norm(m_eigenValues.coeff(row,row)));
      m_blockSingularValues.coeffRef(row-mult*m_jordanIndex[row],1)+=m_blockSingularValues.coeff(row,1);
    }
  }
}

#ifdef USE_LDOUBLE
  #ifdef USE_SINGLES
    template class JordanMatrix<long double>;
  #endif
  #ifdef USE_INTERVALS
    template class JordanMatrix<ldinterval>;
  #endif
#endif
#ifdef USE_MPREAL
  #ifdef USE_SINGLES
    template class JordanMatrix<mpfr::mpreal>;
  #endif
  #ifdef USE_INTERVALS
    template class JordanMatrix<mpinterval>;
  #endif
#endif
}


#include <boost/multiprecision/cpp_dec_float.hpp>
//#include <cmath>
#include <iostream>

const int PRECISION = 10000;
typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<PRECISION>> arbFloat;

enum returnID {success = 0, precisionExceeded = 1};

int main(){
	arbFloat k = 6,
		threeK = pow(3, k),
		twoK = pow(2, k),
		floorK;
	bool isSolution = false;

	for(; !isSolution; ++k){
		threeK *= 3, twoK *= 2, floorK = floor(threeK / twoK);
		isSolution = threeK - twoK * floorK > twoK - floorK - 2;
	}

	if(threeK - twoK * floorK <= twoK - floorK - 2){
		std::cout << "Solution at k = " << k << "\n";
		return returnID::success;
	} else {
		std::cout << "Error: Precision exceeded at k = " << k << " with " << PRECISION << " digits\n";
		return returnID::precisionExceeded;
	}
}


#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include "nn.cpp"

int main(){
    auto s = std::vector <int> {2,5,3};
    auto a = NeuralNetwork(s); // NN with 2,5,3 layers. 2 inputs, 3 outputs

    auto inp = Eigen::MatrixXd::Random(2,1); // Supply random input
    auto ans = a.predict(inp);
    std::cout<< ans << std::endl;

    return 0;
}


#include "Sorter.h"
#include <algorithm>
#include <boost/dynamic_bitset.hpp>
#include <bitset>
#include <stack>
#include <vector>

void Sorter::Shuffle(int *vals, int n)
{
	for (int ii = 0; ii < n; ++ii)
		Swap(vals + ii, vals + (rand() % n));
}

void Sorter::Bubble(int *vals, int n)
{
	for (; n > 1; --n)
		for (int ii = 1; ii < n; ++ii)
			if (vals[ii - 1] > vals[ii])
				Swap(vals + ii - 1, vals + ii);
}

void Sorter::Cocktail(int *vals, int n)
{
	int lo = 0, hi = n - 1;
	while (hi - lo > 1)
	{
		for(int ii = lo; ii < hi; ++ii)
			if (vals[ii] > vals[ii + 1])
				Swap(vals + ii, vals + ii + 1);
		--hi;
		for (int ii = hi; ii > lo; --ii)
			if (vals[ii] < vals[ii - 1])
				Swap(vals + ii, vals + ii - 1);
		++lo;
	}
}

void Sorter::Gnome(int *vals, int n)
{
	int ii = 1;
	while (ii < n)
	{
		if (ii == 0)
			++ii;
		if (vals[ii] < vals[ii - 1])
		{
			Swap(vals + ii, vals + ii - 1);
			--ii;
		}
		else
			++ii;

	}
}

void Sorter::OddEven(int *vals, int n)
{
	bool sorted = false;
	while (!sorted)
	{
		sorted = true;
		Swapper(sorted, vals, n, 1);
		Swapper(sorted, vals, n, 0);
	}
}

void Sorter::Comb(int *vals, int n)
{
	int gap = n >> 1;
	bool sorted = false;
	while (!sorted || gap != 1)
	{
		sorted = true;
		gap = gap == 1 ? 1 : gap >> 1;
		for (int ii = 0; ii + gap < n; ++ii)
		{
			if (vals[ii] > vals[ii + gap])
			{
				Swap(vals + ii, vals + ii + gap);
				sorted = false;
			}
		}
	}
}

void Sorter::Selection(int *vals, int n)
{
	for (int min = 0, base = 0; base < n; min = ++base)
	{
		for (int ii = min + 1; ii < n; ++ii)
			if (vals[ii] < vals[min])
				min = ii;
		Swap(vals + base, vals + min);
	}
}

void Sorter::Pancake(int *vals, int n)
{
	for (int top = n - 1, max = 0; top > 1; --top, max = 0)
	{
		for (int ii = 0; ii <= top; ++ii)
			if (vals[max] < vals[ii])
				max = ii;
		if (max != top)
		{
			Flip(vals, max);
			Flip(vals, top);
		}
	}
}

void Sorter::Insertion(int *vals, int n)
{
	for (int ii = 1; ii < n; ++ii)
	{
		int in = BinarySearch(vals, vals[ii], 0, ii - 1);
		for (int jj = ii - 1; jj >= in && vals[jj] != vals[jj + 1]; --jj)
			Swap(vals + jj, vals + jj + 1);
	}
}

void Sorter::Shell(int *vals, int n)
{
	for (int gap = n >> 1; gap > 0; gap >>= 1)
	{
		for (int ii = gap; ii < n; ++ii)
		{
			int trace, min = vals[ii];
			for (trace = ii; trace >= gap && vals[trace - gap] > min; trace -= gap)
				vals[trace] = vals[trace - gap];
			vals[trace] = min;
		}
	}
}

void Sorter::Merge(int *vals, int lo, int hi)
{
	if (lo < hi)
	{
		int mid = lo + ((hi - lo) >> 1);
		Merge(vals, lo, mid);
		Merge(vals, mid + 1, hi);
		Merger(vals, lo, mid, hi);
	}
}

void Sorter::ItMerge(int *vals, int n)
{
	for (int size = 1; size < n; size >>= 1)
	{
		for (int left = 0; left < n; left += size >> 1)
		{
			int mid = std::min(left + size - 1, n - 1);
			int right = std::min(left + (size >> 1) - 1, n - 1);
			Merger(vals, left, mid, right);
		}
	}
}

void Sorter::Quick(int *vals, int lo, int hi)
{
	if (lo < hi)
	{
		int p = Partition(vals, lo, hi);
		Quick(vals, lo, p - 1);
		Quick(vals, p + 1, hi);
	}
}

void Sorter::ItQuick(int *vals, int n)
{
	std::stack<int> stack;
	stack.push(0);
	stack.push(n - 1);
	while (!stack.empty())
	{
		int hi = stack.top();
		stack.pop();
		int lo = stack.top();
		stack.pop();
		int p = Partition(vals, lo, hi);
		if (p - 1 > lo)
		{
			stack.push(lo);
			stack.push(p - 1);
		}
		if (p + 1 < hi)
		{
			stack.push(p + 1);
			stack.push(hi);
		}
	}
}

void Sorter::TQuick(int *vals, int lo, int hi)
{
	if (lo < hi)
	{
		int left, right;
		TPartition(vals, lo, hi, left, right);
		TQuick(vals, lo, left);
		TQuick(vals, right, hi);
	}
}

void Sorter::Pigeonhole(int *vals, int n)
{
	int min = *std::min_element(vals, vals + n), max = *std::max_element(vals, vals + n);
	std::vector<std::vector<int>> holes(max - min - 1);
	for (int ii = 0; ii < n; ++ii)
		holes[ii - min].push_back(vals[ii]);
	int in = 0;
	for (std::vector<int> hole : holes)
		for (int val : hole)
			vals[in++] = val;
}

void Sorter::Bucket(int *vals, int n, int b)
{
	int max = *std::max_element(vals, vals + n);
	std::vector<std::vector<int>> buckets(b);
	for (int ii = 0; ii < n; ++ii)
		buckets[b * vals[ii] / max].push_back(vals[ii]);
	for (std::vector<int> bucket : buckets)
		Insertion(&bucket[0], bucket.size()); // maybe make recursive, find base case.
	int in = 0;
	for (std::vector<int> bucket : buckets)
		for (int val : bucket)
			vals[in++] = val;
}

void Sorter::Counting(int *vals, int n)
{
	int min = *std::min_element(vals, vals + n), max = *std::max_element(vals, vals + n);
	std::vector<int> count(max - min + 1), sorted(n);
	for (int ii = 0; ii < n; ++ii)
		++count[vals[ii] - min];
	for (int ii = n - 1; ii >= 0; --ii)
	{
		sorted[count[vals[ii] - min] - 1] = vals[ii];
		--count[vals[ii] - min];
	}
	for (int ii = 0; ii < n; ++ii)
		vals[ii] = sorted[ii];
}

void Sorter::Radix(int *vals, int n, int b)
{
	int runs = 0, d = 1, max = *std::max_element(vals, vals + n);
	while (max > 0)
	{
		max /= b;
		++runs;
	}
	std::vector<std::vector<int>> digits(b);
	while (runs-- > 0)
	{
		for (int ii = 0; ii < n; ++ii)
			digits[vals[ii] / d % b].push_back(vals[ii]);
		d *= b;
		int in = 0;
		for (std::vector<int> digit : digits)
		{
			for (int val : digit)
				vals[in++] = val;
			digit.resize(0);
		}
	}
}

void Sorter::Gravity(int *vals, int n)
{
	int max = *std::max_element(vals, vals + n);
	std::vector<boost::dynamic_bitset<>> abacus(n, boost::dynamic_bitset<>(max));
	for (int ii = 0; ii < n; ++ii)
	{
		abacus[ii].flip();
		abacus[ii] >>= max - vals[ii];
	}
	for (int jj = 0, sum = 0; jj < max; ++jj, sum = 0)
	{
		for (int ii = 0; ii < n; ++ii)
		{
			sum += abacus[ii].test(jj) ? 1 : 0;
			abacus[ii].set(jj, 0);
		}
		for (int ii = n - 1; ii >= n - sum; --ii)
			abacus[ii].set(jj, 1);
	}
	for (int ii = 0; ii < n; ++ii)
		vals[ii] = abacus[ii].count();
}

void Sorter::Swap(int *a, int *b)
{
	int tmp = *a;
	*a = *b;
	*b = tmp;
}

void Sorter::Swapper(bool &sorted, int *vals, int n, int parity)
{
	for (int ii = parity; ii < n - 2; ii += 2)
	{
		if (vals[ii] > vals[ii + 1])
		{
			Swap(vals + ii, vals + ii + 1);
			sorted = false;
		}
	}
}

void Sorter::Flip(int *vals, int n)
{
	int lo = 0, hi = n;
	while (lo < hi)
		Swap(vals + lo++, vals + hi--);
}

int Sorter::BinarySearch(int *vals, int key, int lo, int hi)
{
	int mid;
	while (lo <= hi)
	{
		mid = lo + ((hi - lo) >> 1);
		if (vals[mid] == key)
			return mid + 1;
		if (vals[mid] < key)
			lo = mid + 1;
		else
			hi = mid - 1;
	}
	return vals[mid] < key ? mid + 1 : mid;
}

void Sorter::Merger(int *vals, int lo, int mid, int hi)
{
	int left = lo, right = mid + 1, tmp = 0;
	std::vector<int> aux(hi - lo + 1);
	while (left <= mid && right <= hi)
	{
		if (vals[left] < vals[right])
			aux[tmp++] = vals[left++];
		else
			aux[tmp++] = vals[right++];
	}
	while (left <= mid)
		aux[tmp++] = vals[left++];
	for (int ii = 0; ii < tmp; ++ii)
		vals[ii + lo] = aux[ii];
}

int Sorter::Partition(int *vals, int lo, int hi)
{
	int ii = lo, p = vals[lo];
	for (int jj = ii + 1; jj <= hi; ++jj)
	{
		if (vals[jj] < p)
			Swap(vals + (++ii), vals + jj);
		Swap(vals + (++ii), vals + lo);
	}
	return ii;
}

void Sorter::TPartition(int *vals, int lo, int hi, int &left, int &right)
{
	int ii = lo, p = vals[lo];
	while (ii <= hi)
	{
		if (vals[ii] < p)
			Swap(vals + (lo++), vals + (ii++));
		else if (vals[ii] == p)
			++ii;
		else
			Swap(vals + ii, vals + (hi--));
	}
	left = lo - 1;
	right = hi + 1;
}


#pragma once
///@file simpleClusterization.hpp
///@brief This is a simple library that provides basic functionality to clusterize data according to either kmeans or fuzzy cmeans algorithms

#include <Eigen/Dense>
#ifndef FCM_MAX_ITERATIONS
///The maximum number of iterations to do if the algorithm doesn't otherwise converge
//@note It's wrapped in an #ifndef to allow the user to override it at compile time
#define FCM_MAX_ITERATIONS  20
#endif

#ifndef FCM_THRESHOLD
///The threshold below which real numbers are deemed identical, used to avoid infinite weights and determine when the algorithm has converged
//@note It's wrapped in an #ifndef to allow the user to override it at compile time
#define FCM_THRESHOLD   1.0E-19
#endif

#include <simpleClusterization_common.hpp>
using namespace Eigen;

///A constant used in offsetting the centroids of one-datapoint clusters in fuzzy-cmeans, to avoid infinite weights
const float offsetConstant                  = 0.05;
///The number of different seeds to try for each value of clusters number when attempting a clusterization
const int   attemptsPerClustersNumber       = 3;
///If empty clusters happen, the algorithm for that number of clusters fails, so we try up to this number of times if this happens.
const int   maxIterationPerClustersNumber   = 5;

/*!
 * @brief       This function calculates the weights of a fuzzy c-means clusterization, according to the next formula:
 *              w_ij = 1 / (norm(centroids(i), entities(j)))
 * @note        The weights are not normalized, as depending on the use case the normalization will be columns-wise or row-wise
 * @param[in]   entities    The datapoints
 * @param[in]   centroids   The centroids of the clusters
 * @param[out]  weights     The resulting weights
 * @param[in]   norm        A pointer to the norm function you want to use
*/
void calculateFuzzyWeights(
        const Ref<const MatrixXf>   &entities,
        const Ref<const MatrixXf>   &centroids,
        Ref<MatrixXfR>              weights,
        squaredNorm_t               *norm
        );

/*!
 * @brief Generates a fixed number of clusters and their fuzzy weights
 * @param[in]       entities    The datapoints
 * @param[out]      centroids   The centroids of the clusters. The number of rows are the required clusters to find
 * @param[out]      weights     The weights associated with the clusterization (array of floats)
 * @param[in]       norm        A pointer to the norm function you want to use
*/
void FCMGenerator(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        Ref<MatrixXfR>              weights,
        squaredNorm_t               *norm
    );

/*!
 * @brief       Returns a measure of the fit of the clusterization, it's strictly positive and the smaller it is, the best the fit
 * @details     The Davies-Boulding Index defines a measure of the "goodness" of a clusterization of a data population based on the following quantities:
                The scatter vector S_i= (1/T_i * sum_j (norm(C_i, X_j)))^(1/2) where T_i is the population size of the i-th cluster and the sum runs over the datapoints belonging to the i-th cluster
                The Cluster Separation Matrix M_ij = (norm(C_i, C_j))^(1/2)
                The Davies-Bouldin Matrix R_ij = (S_i + S_j)/M_ij
                The Davies-Bouldin Vector R_i = max_(j!=i) R_ij
                The Davies-Bouldin index is, in terms of the previous quantities, R = 1/N * sum_i R_i where N is the number of clusters
 * @param[in]   entities    The datapoints
 * @param[in]   centroids   The centroids of the clusters
 * @param[out]  weights     The weights associating each centroid to its cluster (it's an array of bools)
 * @param[in]   norm        A pointer to the norm function you want to use
 * @return     The Davies-Boulding index of the provided clusterization
*/
float daviesBouldinIndex(
        const Ref<const MatrixXf>    &entities,
        const Ref<const MatrixXf>    &centroids,
        const Ref<const MatrixXb>    &weights,
        squaredNorm_t                *norm
    );

/*!
 * @brief Returns a measure of how well the clusters fit the data
 * @warning TODO <b>Not implemented</b>
 * @param[in]   entities     The datapoints
 * @param[in]   clusters     The centroids of the clusters
 * @param[out]  weights      The weights associating each centroid to its cluster (it's an array of floats)
 * @param[in]   norm         A pointer to the norm function you want to use
 * @return     the fitness of the clusterization
*/
float silhouetteTest(
        const Ref<const MatrixXf>   &entities,
        const Ref<const MatrixXf>   &clusters,
        const Ref<const MatrixXfR>  &weights,
        squaredNorm_t               *norm
        );

/*!
 * @brief       Given a dataset and centroids, returns a weights matrix that is true if the j-th centroid is the closest to the i-th datapoint, and false otherwise
 * @param[in]   entities    The datapoints
 * @param[in]   centroids   The centroids of the clusters
 * @param[out]  weights     The weights associating each centroid to its cluster (it's an array of bools)
 * @param[in]   norm        A pointer to the norm function you want to use
*/
void calculateBooleanWeights(
        const Ref<const MatrixXf>   &entities,
        const Ref<const MatrixXf>   &centroids,
        Ref<MatrixXbR>              weights,
        squaredNorm_t               *norm
    );

/*!
 * @brief       Given a dataset and a centroids matrix of k rows, it tries to identify the most probable k centroids to represent the dataset
 * @param[in]   entities    The datapoints
 * @param[out]   centroids   The centroids of the clusters
 * @param[out]  weights     The weights associating each centroid to its cluster (it's an array of bools)
 * @param[in]   norm        A pointer to the norm function you want to use
*/
void kmeansGenerator(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        Ref<MatrixXbR>              weights,
        squaredNorm_t               *norm
    );

/*!
 * @brief Finds the best fitting number of clusters for the given datapoints, up to the number of rows of centroids through an approximated algorithm compared to full fuzzy c-means
 * @param[in]   entities    The datapoints
 * @param[out]  centroids   The centroids of the clusters
 * @param[out]  weights     The weights associating each centroid to its cluster (it's an array of floats)
 * @param[out]  boolWeights The weights associating each centroid to its cluster (it's an array of bools)
 * @param[in]   norm        A pointer to the norm function you want to use
 * @return     the number of clusters generated
*/
int clusterGeneratorApproximate(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        Ref<MatrixXfR>              weights,
        Ref<MatrixXbR>              boolWeights,
        squaredNorm_t               *norm
    );

/*!
 * @brief Finds the best fitting number of clusters for the given datapoints, up to the number of rows of centroids using the fuzzy c-means algorithm
 * @param[in]   entities    The datapoints
 * @param[out]  centroids   The centroids of the clusters
 * @param[out]  weights     The weights associating each centroid to its cluster (it's an array of floats)
 * @param[in]   norm        A pointer to the norm function you want to use
 * @return     the number of clusters generated
*/
int clusterGeneratorExact(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        MatrixXfR                   &weights,
        squaredNorm_t               *norm
    );


#include "line_interval.hpp"
#include "shape2d.hpp"
#include <Eigen/Dense>
#include <vector>
#include <array>
#include <iostream>
#define _USE_MATH_DEFINES
#include <math.h>

using namespace Eigen;
using namespace std;

array<double, 2> polarEquation(Vector2d v0, Vector2d v1){
    array<double, 3> line={v0[1]-v1[1], v1[0]-v0[0],-(v0[0]*v1[1]-v1[0]*v0[1])};

    double normalAngle=atan2(line[1], line[0]);
    double normalDist=line[2] / sqrt(line[0] * line[0] + line[1] * line[1]);

    if(normalDist<0){
	normalDist = -normalDist;
	normalAngle += M_PI;
	if(normalAngle>=M_PI){
	    normalAngle -= 2 * M_PI;
	}
    }

    array<double, 2> polar={normalDist, normalAngle};
    return polar;
}

LineInterval::LineInterval(Vector2d _point){
    point = _point;
    angleStart = atan2(point[1], point[0]);
    distLowerBound = 0.0;
    distUpperBound = 0.0;
    intervalMaxDists = {0.0, 0.0};
}

LineInterval::LineInterval(double _angleStart, double _angleEnd){
    angleStart=_angleStart;
    angleEnd=_angleEnd;
    distLowerBound = 0.0;
    distUpperBound = 0.0;
    intervalMaxDists = {0.0, 0.0};
}

double LineInterval::DistAt(array<double, 2> edge, int side){
    double angle=(side==0 ? angleStart : angleEnd);
    return edge[0]/cos(angle-edge[1]);
}

array<double, 3> LineInterval::Divide(){
    double mid=(angleStart+IntervalAngleEnd())/2;
    if(mid>=M_PI){
	mid -= 2 * M_PI;
    }
    return {angleStart, mid, angleEnd};
}

bool LineInterval::IntersectsEdge(double v0Angle, double v1Angle){
    double edgeAngleStart=v0Angle;
    double edgeAngleEnd=v1Angle;
    if(edgeAngleStart>edgeAngleEnd){
	if(angleStart<0){
	    edgeAngleStart -= M_PI * 2;
	} else {
	    edgeAngleEnd += M_PI * 2;
	}
    }
    return (edgeAngleStart<=angleStart && edgeAngleEnd>=IntervalAngleEnd());
}

double LineInterval::IntervalAngleEnd(){
    double intervalAngleEnd= (angleStart>angleEnd ? angleEnd + (2 * M_PI) : angleEnd);
    return intervalAngleEnd;
}

bool LineInterval::containsNormal(array<double, 2> edge){

    if(angleStart>angleEnd){
	return (edge[1]>=angleStart && edge[1]<=M_PI) || (edge[1]<=angleEnd && edge[1] >= -M_PI);
    } else {
	return (edge[1] >= angleStart && edge[1] <= angleEnd);
    }
}

array<double, 3> LineInterval::FunctionsAt(array<double, 2> edge, int side){
    double angle=(side==0 ? angleStart : angleEnd);
    //double dist=edge[0]/cos(angle-edge[1]);
    double dist=DistAt(edge, side);
    double distPrime=tan(angle-edge[1])*dist;

    double angleSin=sin(angle-edge[1]);
    double angleCos=cos(angle-edge[1]);

    double distPrime2=edge[0]*(1+angleSin*angleSin)/(angleCos*angleCos*angleCos);
    return {dist, distPrime, distPrime2};
}

double LineInterval::ApproxRoot(double distStart, double distEnd, double derivStart, double derivEnd){
    //double intervalAngleEnd= (angleStart>angleEnd ? angleEnd + (2 * M_PI) : angleEnd);

    double bStart=distStart - (angleStart * derivStart);
    double bEnd=distEnd - (IntervalAngleEnd() * derivEnd);

    Matrix2d A;
    A << derivStart, -1,   derivEnd, -1;
    Vector2d b;
    b << -bStart, -bEnd;

    return A.colPivHouseholderQr().solve(b)[1];
}

void LineInterval::SetAngleEnd(Vector2d _point){
    angleEnd = atan2(_point[1], _point[0]);
}

void LineInterval::update(double upperBound, double lowerBound, double distStart, double distEnd, unsigned long int shapeId){
    distUpperBound+=upperBound;
    distLowerBound+=lowerBound;

    if(distStart > max(intervalMaxDists[0], intervalMaxDists[1]) || distEnd > max(intervalMaxDists[0], intervalMaxDists[1])){
	intervalMaxDists={distStart, distEnd};
    }
    shapeIds.push_back(shapeId);
}

double LineInterval::MaxWidth(){
    //double intervalAngleEnd= (angleStart>angleEnd ? angleEnd + (2 * M_PI) : angleEnd);
    double b=intervalMaxDists[0];
    double c=intervalMaxDists[1];
    return sqrt( b*b + c*c - 2*b*c*cos(IntervalAngleEnd()-angleStart) );
}

array<Vector2d, 2> LineInterval::EndPoints(){
    Vector2d v0(intervalMaxDists[0]*cos(angleStart), intervalMaxDists[0]*sin(angleStart));
    Vector2d v1(intervalMaxDists[1]*cos(angleEnd), intervalMaxDists[1]*sin(angleEnd));
    return {v0, v1};
}

vector<unsigned long int> LineInterval::ShapeIds(){
    return shapeIds;
}

double LineInterval::LowerBound(){
    return distLowerBound;
}

double LineInterval::UpperBound(){
    return distUpperBound;
}

Vector2d LineInterval::Point(){
    return point;
}