#include "rice/Class.hpp"
#include "rice/String.hpp"
#include "rice/Constructor.hpp"
#include "rice/Enum.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>

using namespace Rice;

typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign>     Vector3d;
typedef Eigen::Matrix<double, 4, 4, Eigen::DontAlign>     Matrix4d;
typedef Eigen::Quaternion<double, Eigen::DontAlign>    Quaterniond;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign>
                                                       MatrixXd;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::DontAlign>
                                                       VectorXd;
typedef Eigen::Transform< double, 3, Eigen::Isometry > Isometry3d;
typedef Eigen::Transform< double, 3, Eigen::Affine > Affine3d;
typedef Eigen::AngleAxis<double> AngleAxisd;

/*
 * Document-class: Eigen::Vector3
 *
 * A 3-vector holding floating-point numbers
 *
 * @!method initialize(x = 0, y = 0, z = 0)
 *   Creates a new vector
 *   @param [Numeric] x
 *   @param [Numeric] y
 *   @param [Numeric] z
 * @!method norm
 *   The vector's norm
 *   @return [Numeric]
 * @!method normalize!
 *   Normalizes self
 *   @return [void]
 * @!method normalize
 *   Returns a normalized self
 *   @return [Vector3]
 * @!method [](index)
 *   Returns an element
 *   @param [Integer] index the element index (0, 1 or 2)
 *   @return [Numeric]
 * @!method x
 *   Returns X
 *   @return [Numeric]
 * @!method y
 *   Returns Y
 *   @return [Numeric]
 * @!method z
 *   Returns Z
 *   @return [Numeric]
 * @!method x=(value)
 *   Sets X
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method y=(value)
 *   Sets Y
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method z=(value)
 *   Sets Z
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method []=(index, value)
 *   Sets an element
 *   @param [Integer] index the element index (0, 1 or 2)
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method +(v)
 *    Sum
 *    @param [Vector3] v
 *    @return [Vector3] the sum
 * @!method -(v)
 *    Subtracts
 *    @param [Vector3] v
 *    @return [Vector3] the subtraction
 * @!method *(scalar)
 *    Multiplies by a scalar
 *    @param [Numeric] scalar the scalar
 *    @return [Vector3] the result
 * @!method /(scalar)
 *    Divides by a scalar
 *    @param [Numeric] scalar the scalar
 *    @return [Vector3] the result
 * @!method -@()
 *    Negation
 *    @return [Vector3] the result
 * @!method cross(v)
 *    Cross product
 *    @param [VectorX] v
 *    @return [VectorX] the result
 * @!method dot(v)
 *    Dot product
 *    @param [VectorX] v
 *    @return [VectorX] the result
 * @!method approx?(v, threshold = dummy_precision)
 *    Verifies that two vectors are within threshold of each other, elementwise
 *    @param [Vector3]
 *    @return [Boolean]
 */

struct Vector3
{
    Vector3d* v;

    Vector3(double x, double y, double z)
        : v(new Vector3d(x, y, z)) {}
    Vector3(Vector3d const& _v)
        : v(new Vector3d(_v)) {}
    ~Vector3()
    { delete v; }

    double x() const { return v->x(); }
    double y() const { return v->y(); }
    double z() const { return v->z(); }
    void setX(double value) { v->x() = value; }
    void setY(double value) { v->y() = value; }
    void setZ(double value) { v->z() = value; }

    double norm() const { return v->norm(); }
    Vector3* normalize() const { return new Vector3(v->normalized()); }
    void normalizeBang() const { v->normalize(); }

    double get(int i) const { return (*v)[i]; }
    void set(int i, double value) { (*v)[i] = value; }

    Vector3* operator + (Vector3 const& other) const
    { return new Vector3(*v + *other.v); }
    Vector3* operator - (Vector3 const& other) const
    { return new Vector3(*v - *other.v); }

    Vector3* operator / (double scalar) const
    { return new Vector3(*v / scalar); }

    Vector3* negate() const
    { return new Vector3(-*v); }
    Vector3* scale(double value) const
    { return new Vector3(*v * value); }
    double dot(Vector3 const& other) const
    { return this->v->dot(*other.v); }
    Vector3* cross(Vector3 const& other) const
    { return new Vector3(this->v->cross(*other.v)); }
    bool operator ==(Vector3 const& other) const
    { return (*this->v) == (*other.v); }
    bool isApprox(Vector3 const& other, double tolerance)
    { return v->isApprox(*other.v, tolerance); }
};

/*
 * Document-class: Eigen::VectorX
 *
 * A variable-length vector holding floating-point numbers
 *
 * @!method initialize(size = 0)
 *   Creates a new vector
 *   @param [Numeric] size
 * @!method resize(new_size)
 *   Changes the vector's size
 *   @param [Integer] new_size
 *   @return [Numeric]
 * @!method norm
 *   The vector's norm
 *   @return [Numeric]
 * @!method normalize!
 *   Normalizes self
 *   @return [void]
 * @!method normalize
 *   Returns a normalized self
 *   @return [VectorX]
 * @!method size
 *   Returns the vector's size
 *   @return [Integer]
 * @!method [](index)
 *   Returns an element
 *   @param [Integer] index the element index (0, 1 or 2)
 *   @return [Numeric]
 * @!method []=(index, value)
 *   Sets an element
 *   @param [Integer] index the element index (0, 1 or 2)
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method +(v)
 *    Sum
 *    @param [VectorX] v
 *    @return [VectorX] the sum
 * @!method -(v)
 *    Subtracts
 *    @param [VectorX] v
 *    @return [VectorX] the subtraction
 * @!method *(scalar)
 *    Multiplies by a scalar
 *    @param [Numeric] scalar the scalar
 *    @return [VectorX] the result
 * @!method /(scalar)
 *    Divides by a scalar
 *    @param [Numeric] scalar the scalar
 *    @return [VectorX] the result
 * @!method -@()
 *    Negation
 *    @return [VectorX] the result
 * @!method dot(v)
 *    Dot product
 *    @param [VectorX] v
 *    @return [VectorX] the result
 * @!method approx?(v, threshold = dummy_precision)
 *    Verifies that two vectors are within threshold of each other, elementwise
 *    @param [VectorX]
 *    @return [Boolean]
 */
struct VectorX {

    VectorXd* v;

    VectorX()
        : v(new VectorXd()) {}
    VectorX(VectorX const& v)
        : v(new VectorXd(*v.v)) {}
    VectorX(int n)
        : v(new VectorXd(n)) {}
    VectorX(VectorXd const& _v)
        : v(new VectorXd(_v)) {}
    ~VectorX()
    { delete v; }

    void resize(int n) { v->resize(n); }
    void conservativeResize(int n) { v->conservativeResize(n); }

    double norm() const { return v->norm(); }
    VectorX* normalize() const { return new VectorX(v->normalized()); }
    void normalizeBang() const { v->normalize(); }

    unsigned int size() { return v->size(); }

    double get(int i) const { return (*v)[i]; }
    void set(int i, double value) { (*v)[i] = value; }

    VectorX* operator + (VectorX const& other) const
    { return new VectorX(*v + *other.v); }
    VectorX* operator - (VectorX const& other) const
    { return new VectorX(*v - *other.v); }

    VectorX* operator / (double scalar) const
    { return new VectorX(*v / scalar); }

    VectorX* negate() const
    { return new VectorX(-*v); }

    VectorX* scale(double value) const
    { return new VectorX(*v * value); }

    double dot(VectorX const& other) const
    { return this->v->dot(*other.v); }

    bool operator ==(VectorX const& other) const
    { return (*this->v) == (*other.v); }

    bool isApprox(VectorX const& other, double tolerance)
    { return v->isApprox(*other.v, tolerance); }

};

/*
 * Document-class: Eigen::Matrix4
 *
 * A 4x4 matrix holding floating-point numbers
 *
 * @!method rows
 *    @return [Integer] the number of rows
 * @!method cols
 *    @return [Numeric] the number of columns
 * @!method size
 *    @return [Numeric] the number of elements
 * @!method [](row, col)
 *    Accesses an element
 *    @param [Integer] row the element's row
 *    @param [Integer] col the element's column
 *    @return [Numeric] the required element
 * @!method []=(row, col, value)
 *    Sets an element
 *    @param [Integer] row the element's row
 *    @param [Integer] col the element's column
 *    @param [Numeric] value the new value
 *    @return [Numeric] the value
 * @!method +(m)
 *    Sums two matrices
 *    @param [Matrix4] m the matrix to add
 *    @return [Matrix4] the sum
 * @!method -(m)
 *    Subtracts two matrices
 *    @param [Matrix4] m the matrix to subtract to self
 *    @return [Matrix4] the subtraction
 * @!method *(v)
 *    Multiplies by a scalar
 *    @param [Numeric] v the scalar
 *    @return [Matrix4] the result
 * @!method /(v)
 *    Divides this matrix by a scalar
 *    @param [Numeric] v the scalar
 *    @return [Matrix4] the result
 * @!method -@(v)
 *    Returns this matrix' negation
 *    @return [Matrix4] the result
 * @!method T
 *    Returns the transposed matrix
 *    @return [Matrix4]
 * @!method norm
 *    Returns the matrix' norm
 *    @return [Numeric]
 * @!method dotM(m)
 *    Matrix multiplication
 *    @param [Matrix4]
 *    @return [Numeric]
 * @!method approx?(m, threshold = dummy_precision)
 *    Verifies that two matrices are within threshold of each other, elementwise
 *    @param [Matrix4]
 *    @return [Boolean]
 */
struct Matrix4
{
    Matrix4d* mx;

    Matrix4() : mx(new Matrix4d()) {}

    Matrix4(Matrix4d const& _mx)
        : mx(new Matrix4d(_mx)) {}

    ~Matrix4()
    { delete mx; }

    double norm() const { return mx->norm(); }

    int rows() const { return mx->rows(); }
    int cols() const { return mx->cols(); }
    int size() const { return mx->size(); }

    double get(int i, int j ) const { return (*mx)(i,j); }
    void set(int i, int j, double value) { (*mx)(i,j) = value; }

    Matrix4* transpose() const
    { return new Matrix4(mx->transpose()); }

    Matrix4* operator + (Matrix4 const& other) const
    { return new Matrix4(*mx + *other.mx); }

    Matrix4* operator - (Matrix4 const& other) const
    { return new Matrix4(*mx - *other.mx); }

    Matrix4* operator / (double scalar) const
    { return new Matrix4(*mx / scalar); }

    Matrix4* negate() const
    { return new Matrix4(-*mx); }

    Matrix4* scale(double value) const
    { return new Matrix4(*mx * value); }

    Matrix4* dotM (Matrix4 const& other) const
    { return new Matrix4(*mx * (*other.mx)); }

    bool operator ==(Matrix4 const& other) const
    { return (*this->mx) == (*other.mx); }

    bool isApprox(Matrix4 const& other, double tolerance)
    { return mx->isApprox(*other.mx, tolerance); }
};

/*
 * Document-class: Eigen::JacobiSVD
 *
 * Linear problem solver
 *
 * This is not constructed directly. Use {Eigen::MatrixX#jacobiSvd} instead.
 *
 * @!method solve(vector)
 *   Solves the linear problem for a given vector
 *   @param [VectorX] vector
 *   @return [VectorX] the result
 */
struct JacobiSVD {
    typedef Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject> EigenT;
    EigenT* j;

    JacobiSVD( EigenT const& j )
    : j(new EigenT(j)) {}
    JacobiSVD( JacobiSVD const& j )
    : j(new EigenT(*j.j)) {}
    ~JacobiSVD()
    { delete j; }

    VectorX* solve(VectorX* y)
    { return new VectorX(j->solve(*y->v)); }
};

/*
 * Document-class: Eigen::MatrixX
 *
 * A variable-size matrix holding floating-point numbers
 *
 * @!method resize(rows, cols)
 *    Resizes the matrix
 *    @param [Integer] rows the new number of rows
 *    @param [Integer] cols the new number of columns
 * @!method rows
 *    @return [Integer] the number of rows
 * @!method cols
 *    @return [Numeric] the number of columns
 * @!method size
 *    @return [Numeric] the number of elements
 * @!method [](row, col)
 *    Accesses an element
 *    @param [Integer] row the element's row
 *    @param [Integer] col the element's column
 *    @return [Numeric] the required element
 * @!method []=(row, col, value)
 *    Sets an element
 *    @param [Integer] row the element's row
 *    @param [Integer] col the element's column
 *    @param [Numeric] value the new value
 *    @return [Numeric] the value
 * @!method setRow(row, vector)
 *    Sets a whole matrix row
 *    @param [Integer] row the row index
 *    @param [VectorX] vector the row values
 * @!method setColumn(column, vector)
 *    Sets a whole matrix column
 *    @param [Integer] row the column index
 *    @param [VectorX] vector the column values
 * @!method +(m)
 *    Sums two matrices
 *    @param [MatrixX] m the matrix to add
 *    @return [MatrixX] the sum
 * @!method -(m)
 *    Subtracts two matrices
 *    @param [MatrixX] m the matrix to subtract to self
 *    @return [MatrixX] the subtraction
 * @!method *(v)
 *    Multiplies by a scalar
 *    @param [Numeric] v the scalar
 *    @return [MatrixX] the result
 * @!method /(v)
 *    Divides this matrix by a scalar
 *    @param [Numeric] v the scalar
 *    @return [MatrixX] the result
 * @!method -@(v)
 *    Returns this matrix' negation
 *    @return [MatrixX] the result
 * @!method T
 *    Returns the transposed matrix
 *    @return [MatrixX]
 * @!method norm
 *    Returns the matrix' norm
 *    @return [Numeric]
 * @!method dotV(m)
 *    Matrix/vector multiplication
 *    @param [VectorX]
 *    @return [Numeric]
 * @!method dotM(m)
 *    Matrix multiplication
 *    @param [Matrix4]
 *    @return [Numeric]
 * @!method approx?(m, threshold = dummy_precision)
 *    Verifies that two matrices are within threshold of each other, elementwise
 *    @param [Matrix4]
 *    @return [Boolean]
 * @!method jacobiSvd(flags = 0)
 *    Returns a SVD solver to find V from W in self.dotV(V) = W
 *    @param [Integer] flags solver flags, as OR-ed values of Eigen::ComputeFullU,
 *      Eigen::ComputeThinU and Eigen::ComputeThinV. See Eigen documentation
 *    @return [JacobiSVD]
 */
struct MatrixX {

    MatrixXd* m;

    MatrixX() : m(new MatrixXd()) {}
    MatrixX(const MatrixX& m) : m(new MatrixXd(*m.m)) {}
    MatrixX(int rows, int cols) : m(new MatrixXd(rows,cols)) {}
    MatrixX(const MatrixXd& _m) : m(new MatrixXd(_m)) {}
    ~MatrixX() { delete m; }

    void resize(int rows, int cols) { m->resize(rows,cols); }
    void conservativeResize(int rows, int cols) { m->conservativeResize(rows,cols); }

    double norm() const { return m->norm(); }

    unsigned int rows() const { return m->rows(); }
    unsigned int cols() const { return m->cols(); }
    unsigned int size() const { return m->size(); }

    double get(int i, int j ) const { return (*m)(i,j); }
    void set(int i, int j, double value) { (*m)(i,j) = value; }

    VectorX* getRow(int i) const { return new VectorX(m->row(i)); }
    void setRow(int i, const VectorX& v) { m->row(i) = *(v.v); }

    VectorX* getColumn(int j) const { return new VectorX(m->col(j)); }
    void setColumn(int j, const VectorX& v) { m->col(j) = *(v.v); }

    MatrixX* transpose() const
    { return new MatrixX(m->transpose()); }

    MatrixX* operator + (MatrixX const& other) const
    { return new MatrixX(*m + *other.m); }

    MatrixX* operator - (MatrixX const& other) const
    { return new MatrixX(*m - *other.m); }

    MatrixX* operator / (double scalar) const
    { return new MatrixX(*m / scalar); }

    MatrixX* negate() const
    { return new MatrixX(-*m); }

    MatrixX* scale (double scalar) const
    { return new MatrixX(*m * scalar); }

    VectorX* dotV (VectorX const& other) const
    { return new VectorX(*m * *other.v); }

    MatrixX* dotM (MatrixX const& other) const
    { return new MatrixX(*m * (*other.m)); }

    JacobiSVD* jacobiSvd(int flags = 0) const
    { return new JacobiSVD(m->jacobiSvd(flags)); }

    bool operator ==(MatrixX const& other) const
    { return (*this->m) == (*other.m); }

    bool isApprox(MatrixX const& other, double tolerance)
    { return m->isApprox(*other.m, tolerance); }
};

/*
 * Document-class: Eigen::Quaternion
 *
 * A floating-point valued quaternion
 *
 * @!method w
 *   The real part of the quaternion
 *   @return [Numeric]
 * @!method x
 *   The first element of the imaginary part of the quaternion
 *   @return [Numeric]
 * @!method y
 *   The second element of the imaginary part of the quaternion
 *   @return [Numeric]
 * @!method z
 *   The third element of the imaginary part of the quaternion
 *   @return [Numeric]
 * @!method w=(value)
 *   Sets the real part of the quaternion
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method x=(value)
 *   Sets the first element of the imaginary part of the quaternion
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method y=(value)
 *   Sets the second element of the imaginary part of the quaternion
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method z=(value)
 *   Sets the third element of the imaginary part of the quaternion
 *   @param [Numeric] value
 *   @return [Numeric]
 * @!method norm
 *   The norm
 *   @return [Numeric]
 * @!method concatenate(q)
 *   Quaternion multiplication
 *   @param [Quaternion] q
 *   @return [Quaternion] self * q
 * @!method inverse
 *   The quaternion inverse
 *   @return [Quaternion]
 * @!method transform(v)
 *   Transform a vector-3 by this quaternion
 *   @param [Vector3] v
 *   @return [Vector3]
 * @!method matrix
 *   The rotation matrix equivalent to this unit quaternion
 *   @return [MatrixX]
 * @!method normalize!
 *   Normalizes self
 *   @return [void]
 * @!method normalize
 *   Returns the self normalized
 *   @return [Quaternion]
 * @!method approx?(q, threshold = dummy_precision)
 *   Verifies that two quaternions are within threshold of each other, elementwise
 *   @param [Quaternion]
 *   @return [Boolean]
 * @!method to_euler
 *   Converts this quaternion into Euler-Bryant angles
 *   @return [Vector3] a 3-vector where .x is roll (rotation around X), .y is
 *     pitch and .z yaw
 * @!method from_euler(v)
 *   Initializes from euler angles
 *   @param [Vector3] v a 3-vector where .x is roll (rotation around X), .y is
 *     pitch and .z yaw
 *   @return [void]
 * @!method from_angle_axis(angle_axis)
 *   Initializes from an angle-axis representation
 *   @param [AngleAxis] an angle-axis representation of a rotation
 *   @return [void]
 * @!method from_matrix(matrix)
 *   Initializes from a rotation matrix
 *   @param [MatrixX]
 *   @return [void]
 */
struct Quaternion
{
    Quaterniond* q;
    Quaternion(double w, double x, double y, double z)
        : q(new Quaterniond(w, x, y, z)) { }
    Quaternion(Quaternion const& q)
        : q(new Quaterniond(*q.q)) { }
    Quaternion(Quaterniond const& _q)
        : q(new Quaterniond(_q)) {}

    ~Quaternion()
    { delete q; }

    double w() const { return q->w(); }
    double x() const { return q->x(); }
    double y() const { return q->y(); }
    double z() const { return q->z(); }
    void setW(double value) { q->w() = value; }
    void setX(double value) { q->x() = value; }
    void setY(double value) { q->y() = value; }
    void setZ(double value) { q->z() = value; }

    double norm() const { return q->norm(); }

    bool operator ==(Quaternion const& other) const
    { return x() == other.x() && y() == other.y() && z() == other.z() && w() == other.w(); }

    Quaternion* concatenate(Quaternion const& other) const
    { return new Quaternion((*q) * (*other.q)); }
    Vector3* transform(Vector3 const& v) const
    { return new Vector3((*q) * (*v.v)); }
    Quaternion* inverse() const
    { return new Quaternion(q->inverse()); }
    void normalizeBang()
    { q->normalize(); }
    Quaternion* normalize() const
    {
        Quaterniond q = *this->q;
        q.normalize();
        return new Quaternion(q);
    }
    MatrixX* matrix() const
    {
        return new MatrixX(q->matrix());
    }

    void fromAngleAxis(double angle, Vector3 const& axis)
    {
	*(this->q) =
            Eigen::AngleAxisd(angle, *axis.v);
    }

    void fromEuler(Vector3 const& angles, int axis0, int axis1, int axis2)
    {
        *(this->q) =
            Eigen::AngleAxisd(angles.x(), Eigen::Vector3d::Unit(axis0)) *
            Eigen::AngleAxisd(angles.y(), Eigen::Vector3d::Unit(axis1)) *
            Eigen::AngleAxisd(angles.z(), Eigen::Vector3d::Unit(axis2));
    }

    void fromMatrix(MatrixX const& matrix)
    {
	*(this->q) =
            Quaterniond(Eigen::Matrix3d(*matrix.m));
    }

    bool isApprox(Quaternion const& other, double tolerance)
    {
        return q->isApprox(*other.q, tolerance);
    }

    Vector3* toEuler()
    {
        const Eigen::Matrix3d m = q->toRotationMatrix();
        double i = Eigen::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm();
        double y = atan2(-m.coeff(2,0), i);
        double x=0,z=0;
        if (i > Eigen::NumTraits<double>::dummy_precision()){
            x = ::atan2(m.coeff(1,0), m.coeff(0,0));
            z = ::atan2(m.coeff(2,1), m.coeff(2,2));
        }else{
            z = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1));
        }
        return new Vector3(x,y,z);
    }
};

/*
 * Document-class: Eigen::AngleAxis
 *
 * A rotation represented by an axis and angle
 *
 * @!method angle
 *   The angle (in radians)
 *   @return [Numeric]
 * @!method axis
 *   The rotation axis
 *   @return [Vector3]
 * @!method concatenate(aa)
 *   Combine this rotation with another rotation
 *   @param [AngleAxis] aa
 *   @return [AngleAxis]
 * @!method inverse
 *   The inverse rotation
 *   @return [AngleAxis]
 * @!method transform(v)
 *   Apply this rotation on a 3-vector
 *   @param [Vector3] v
 *   @return [Vector3]
 * @!method matrix
 *   The rotation matrix equivalent to this unit quaternion
 *   @return [MatrixX]
 * @!method approx?(aa, threshold = dummy_precision)
 *    Verifies that two angle-axis are within threshold of each other, elementwise
 *    @param [AngleAxis]
 *    @return [Boolean]
 * @!method to_euler
 *    Converts this quaternion into Euler-Bryant angles
 *    @return [Vector3] a 3-vector where .x is roll (rotation around X), .y is
 *      pitch and .z yaw
 * @!method from_euler(v)
 *    Initializes from euler angles
 *    @param [Vector3] v a 3-vector where .x is roll (rotation around X), .y is
 *      pitch and .z yaw
 * @!method from_quaternion(q)
 *    Initializes from a quaternion
 *    @param [Quaternion] q
 * @!method from_matrix(matrix)
 *    Initializes from a rotation matrix
 *    @param [MatrixX]
 */
struct AngleAxis
{
    AngleAxisd* aa;
    AngleAxis(double angle, Vector3 const& axis)
        : aa(new AngleAxisd(angle, Eigen::Vector3d(*axis.v))){}
    AngleAxis(AngleAxis const& aa)
        : aa(new AngleAxisd(*aa.aa)) { }
    AngleAxis(AngleAxisd const& _aa)
        : aa(new AngleAxisd(_aa)) {}

    ~AngleAxis()
    { delete aa; }

    bool operator ==(AngleAxis const& other) const
    { return angle() == other.angle() && axis() == other.axis(); }

    double angle() const { return aa->angle(); }
    Vector3* axis() const { return new Vector3(aa->axis()); }

    AngleAxis* concatenate(AngleAxis const& other) const
    { return new AngleAxis(static_cast<AngleAxisd>((*aa) * (*other.aa))); }

    Vector3* transform(Vector3 const& v) const
    { return new Vector3((*aa) * (*v.v)); }

    AngleAxis* inverse() const
    { return new AngleAxis(aa->inverse()); }

    MatrixX* matrix() const
    {
        return new MatrixX(aa->matrix());
    }

    void fromQuaternion(Quaternion const& q)
    {
        *(this->aa) = Eigen::Quaterniond(q.w(), q.x(), q.y(), q.z());
    }

    void fromEuler(Vector3 const& angles, int axis0, int axis1, int axis2)
    {
        *(this->aa) =
            Eigen::AngleAxisd(angles.x(), Eigen::Vector3d::Unit(axis0)) *
            Eigen::AngleAxisd(angles.y(), Eigen::Vector3d::Unit(axis1)) *
            Eigen::AngleAxisd(angles.z(), Eigen::Vector3d::Unit(axis2));
    }

    void fromMatrix(MatrixX const& matrix)
    {
	    *(this->aa) =
            AngleAxisd(Eigen::Matrix3d(*matrix.m));
    }

    bool isApprox(AngleAxis const& other, double tolerance)
    {
        return aa->isApprox(*other.aa, tolerance);
    }

    Vector3* toEuler()
    {
        const Eigen::Matrix3d m = aa->toRotationMatrix();
        double i = Eigen::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm();
        double y = atan2(-m.coeff(2,0), i);
        double x=0,z=0;
        if (i > Eigen::NumTraits<double>::dummy_precision()){
            x = ::atan2(m.coeff(1,0), m.coeff(0,0));
            z = ::atan2(m.coeff(2,1), m.coeff(2,2));
        }else{
            z = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1));
        }
        return new Vector3(x,y,z);
    }
};

#include <iostream>

/*
 * Document-class: Eigen::Isometry3
 *
 * An isometry
 *
 * @!method approx?(v, threshold = dummy_precision)
 *    Verifies that two isometries are within threshold of each other, elementwise
 *    @param [Isometry3]
 *    @return [Boolean]
 * @!method inverse
 *    The inverse transformation
 *    @return [Isometry3]
 * @!method translation
 *    The translation part of this transformation
 *    @return [Vector3]
 * @!method rotation
 *    The rotation part of this transformation
 *    @return [Quaternion]
 * @!method concatenate(is)
 *    Concatenate self and another transformation
 *    @param [Isometry3] is
 *    @return [Isometry3]
 * @!method translate(v)
 *    Add a new translation after the rotation
 *    @param [Vector3] v the translation
 *    @return [void]
 * @!method rotate(q)
 *    Add a new rotation after the translation
 *    @param [Quaternion] q the rotation
 *    @return [void]
 * @!method pretranslate(v)
 *    Add a new translation before the rotation
 *    @param [Vector3] v the translation
 *    @return [void]
 * @!method prerotate(q)
 *    Add a new rotation before the translation
 *    @param [Quaternion] q the rotation
 *    @return [void]
 * @!method matrix
 *    The transformation matrix equivalent to self
 *    @return [MatrixX]
 */
struct Isometry3
{
    Isometry3d *t;

    Isometry3() : t(new Isometry3d()) { t->setIdentity(); }
    Isometry3(const Isometry3& _m) : t(new Isometry3d(*_m.t)) {}
    Isometry3(const Isometry3d& _m) : t(new Isometry3d(_m)) {}
    ~Isometry3() { delete t; }

    Isometry3* inverse() const
    { return new Isometry3( t->inverse() ); }

    Vector3* translation() const
    { return new Vector3( t->translation() ); }

    Quaternion* rotation() const
    { return new Quaternion( Eigen::Quaterniond(t->linear()) ); }

    Isometry3* concatenate(Isometry3 const& other) const
    { return new Isometry3( *t * *other.t ); }

    Vector3* transform(Vector3 const& other) const
    { return new Vector3( *t * *other.v ); }

    MatrixX* matrix() const
    { return new MatrixX( t->matrix() ); }

    void translate( Vector3 const& other ) const
    { t->translate( *other.v ); }

    void pretranslate( Vector3 const& other ) const
    { t->pretranslate( *other.v ); }

    void rotate( Quaternion const& other ) const
    { t->rotate( *other.q ); }

    void prerotate( Quaternion const& other ) const
    { t->prerotate( *other.q ); }

    bool operator ==(Isometry3 const& other) const
    { return (*this->t).matrix() == (*other.t).matrix(); }

    bool isApprox(Isometry3 const& other, double tolerance)
    { return t->isApprox(*other.t, tolerance); }
};

/*
 * Document-class: Eigen::Affine3
 *
 * An affine transformation
 *
 * @!method approx?(v, threshold = dummy_precision)
 *    Verifies that two transformations are within threshold of each other, elementwise
 *    @param [Isometry3]
 *    @return [Boolean]
 * @!method inverse
 *    The inverse transformation
 *    @return [Affine3]
 * @!method translation
 *    The translation part of this transformation
 *    @return [Vector3]
 * @!method rotation
 *    The rotation part of this transformation
 *    @return [Quaternion]
 * @!method concatenate(is)
 *    Concatenate self and another transformation
 *    @param [Affine3] is
 *    @return [Affine3]
 * @!method translate(v)
 *    Add a new translation after the rotation
 *    @param [Vector3] v the translation
 *    @return [void]
 * @!method rotate(q)
 *    Add a new rotation after the translation
 *    @param [Quaternion] q the rotation
 *    @return [void]
 * @!method pretranslate(v)
 *    Add a new translation before the rotation
 *    @param [Vector3] v the translation
 *    @return [void]
 * @!method prerotate(q)
 *    Add a new rotation before the translation
 *    @param [Quaternion] q the rotation
 *    @return [void]
 * @!method matrix
 *    The transformation matrix equivalent to self
 *    @return [MatrixX]
 */
struct Affine3
{
    Affine3d *t;

    Affine3() : t(new Affine3d()) { t->setIdentity(); }
    Affine3(const Affine3& _m) : t(new Affine3d(*_m.t)) {}
    Affine3(const Affine3d& _m) : t(new Affine3d(_m)) {}
    ~Affine3() { delete t; }

    Affine3* inverse() const
    { return new Affine3( t->inverse() ); }

    Vector3* translation() const
    { return new Vector3( t->translation() ); }

    Quaternion* rotation() const
    { return new Quaternion( Eigen::Quaterniond(t->linear()) ); }

    Affine3* concatenate(Affine3 const& other) const
    { return new Affine3( *t * *other.t ); }

    Vector3* transform(Vector3 const& other) const
    { return new Vector3( *t * *other.v ); }

    MatrixX* matrix() const
    { return new MatrixX( t->matrix() ); }

    void translate( Vector3 const& other ) const
    { t->translate( *other.v ); }

    void pretranslate( Vector3 const& other ) const
    { t->pretranslate( *other.v ); }

    void rotate( Quaternion const& other ) const
    { t->rotate( *other.q ); }

    void prerotate( Quaternion const& other ) const
    { t->prerotate( *other.q ); }

    bool operator ==(Affine3 const& other) const
    { return (*this->t).matrix() == (*other.t).matrix(); }

    bool isApprox(Affine3 const& other, double tolerance)
    { return t->isApprox(*other.t, tolerance); }
};

extern "C" void Init_eigen()
{
     Rice::Module rb_mEigen = define_module("Eigen");

     Data_Type<Vector3> rb_Vector3 = define_class_under<Vector3>(rb_mEigen, "Vector3")
       .define_constructor(Constructor<Vector3,double,double,double>(),
               (Arg("x") = static_cast<double>(0),
               Arg("y") = static_cast<double>(0),
               Arg("z") = static_cast<double>(0)))
       .define_method("__equal__",  &Vector3::operator ==)
       .define_method("norm",  &Vector3::norm)
       .define_method("normalize!",  &Vector3::normalizeBang)
       .define_method("normalize",  &Vector3::normalize)
       .define_method("[]",  &Vector3::get)
       .define_method("[]=",  &Vector3::set)
       .define_method("x",  &Vector3::x)
       .define_method("y",  &Vector3::y)
       .define_method("z",  &Vector3::z)
       .define_method("x=", &Vector3::setX)
       .define_method("y=", &Vector3::setY)
       .define_method("z=", &Vector3::setZ)
       .define_method("+",  &Vector3::operator +)
       .define_method("-",  &Vector3::operator -)
       .define_method("/",  &Vector3::operator /)
       .define_method("-@", &Vector3::negate)
       .define_method("*",  &Vector3::scale)
       .define_method("cross", &Vector3::cross)
       .define_method("dot",  &Vector3::dot)
       .define_method("approx?", &Vector3::isApprox, (Arg("v"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));

     Data_Type<Quaternion> rb_Quaternion = define_class_under<Quaternion>(rb_mEigen, "Quaternion")
       .define_constructor(Constructor<Quaternion,double,double,double,double>())
       .define_method("__equal__", &Quaternion::operator ==)
       .define_method("w",  &Quaternion::w)
       .define_method("x",  &Quaternion::x)
       .define_method("y",  &Quaternion::y)
       .define_method("z",  &Quaternion::z)
       .define_method("w=", &Quaternion::setW)
       .define_method("x=", &Quaternion::setX)
       .define_method("y=", &Quaternion::setY)
       .define_method("z=", &Quaternion::setZ)
       .define_method("norm", &Quaternion::norm)
       .define_method("concatenate", &Quaternion::concatenate)
       .define_method("inverse", &Quaternion::inverse)
       .define_method("transform", &Quaternion::transform)
       .define_method("matrix", &Quaternion::matrix)
       .define_method("normalize!", &Quaternion::normalizeBang)
       .define_method("normalize", &Quaternion::normalize)
       .define_method("approx?", &Quaternion::isApprox, (Arg("q"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
       .define_method("to_euler", &Quaternion::toEuler)
       .define_method("from_euler", &Quaternion::fromEuler)
       .define_method("from_angle_axis", &Quaternion::fromAngleAxis)
       .define_method("from_matrix", &Quaternion::fromMatrix);

     Data_Type<AngleAxis> rb_AngleAxis = define_class_under<AngleAxis>(rb_mEigen, "AngleAxis")
       .define_constructor(Constructor<AngleAxis,double,Vector3 const&>())
       .define_method("__equal__", &AngleAxis::operator ==)
       .define_method("angle",  &AngleAxis::angle)
       .define_method("axis",  &AngleAxis::axis)
       .define_method("concatenate", &AngleAxis::concatenate)
       .define_method("inverse", &AngleAxis::inverse)
       .define_method("transform", &AngleAxis::transform)
       .define_method("matrix", &AngleAxis::matrix)
       .define_method("approx?", &AngleAxis::isApprox, (Arg("q"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
       .define_method("to_euler", &AngleAxis::toEuler)
       .define_method("from_euler", &AngleAxis::fromEuler)
       .define_method("from_quaternion", &AngleAxis::fromQuaternion)
       .define_method("from_matrix", &AngleAxis::fromMatrix);

     Data_Type<VectorX> rb_VectorX = define_class_under<VectorX>(rb_mEigen, "VectorX")
       .define_constructor(Constructor<VectorX,int>(),
               (Arg("rows") = static_cast<int>(0)))
       .define_method("resize", &VectorX::resize)
       .define_method("__equal__",  &VectorX::operator ==)
       .define_method("norm",  &VectorX::norm)
       .define_method("normalize!",  &VectorX::normalizeBang)
       .define_method("normalize",  &VectorX::normalize)
       .define_method("size", &VectorX::size)
       .define_method("[]",  &VectorX::get)
       .define_method("[]=",  &VectorX::set)
       .define_method("+",  &VectorX::operator +)
       .define_method("-",  &VectorX::operator -)
       .define_method("/",  &VectorX::operator /)
       .define_method("-@", &VectorX::negate)
       .define_method("*",  &VectorX::scale)
       .define_method("dot",  &VectorX::dot)
       .define_method("approx?", &VectorX::isApprox, (Arg("v"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));

     Data_Type<Matrix4> rb_Matrix4 = define_class_under<Matrix4>(rb_mEigen, "Matrix4")
       .define_constructor(Constructor<Matrix4>())
       .define_method("__equal__",  &Matrix4::operator ==)
       .define_method("T", &Matrix4::transpose)
       .define_method("norm",  &Matrix4::norm)
       .define_method("rows", &Matrix4::rows)
       .define_method("cols", &Matrix4::cols)
       .define_method("size", &Matrix4::size)
       .define_method("[]",  &Matrix4::get)
       .define_method("[]=",  &Matrix4::set)
       .define_method("+",  &Matrix4::operator +)
       .define_method("-",  &Matrix4::operator -)
       .define_method("/",  &Matrix4::operator /)
       .define_method("-@", &Matrix4::negate)
       .define_method("*",  &Matrix4::scale)
       .define_method("dotM",  &Matrix4::dotM)
       .define_method("approx?", &Matrix4::isApprox, (Arg("m"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));

     rb_mEigen.const_set("ComputeFullU", INT2FIX(Eigen::ComputeFullU));
     rb_mEigen.const_set("ComputeThinU", INT2FIX(Eigen::ComputeThinU));
     rb_mEigen.const_set("ComputeThinV", INT2FIX(Eigen::ComputeThinV));

     Data_Type<JacobiSVD> rb_JacobiSVD = define_class_under<JacobiSVD>(rb_mEigen, "JacobiSVD")
        .define_method("solve", &JacobiSVD::solve);

     Data_Type<MatrixX> rb_MatrixX = define_class_under<MatrixX>(rb_mEigen, "MatrixX")
       .define_constructor(Constructor<MatrixX,int,int>(),
               (Arg("rows") = static_cast<int>(0),
                Arg("cols") = static_cast<int>(0)))
       .define_method("resize", &MatrixX::resize)
       .define_method("__equal__",  &MatrixX::operator ==)
       .define_method("T", &MatrixX::transpose)
       .define_method("norm",  &MatrixX::norm)
       .define_method("rows", &MatrixX::rows)
       .define_method("cols", &MatrixX::cols)
       .define_method("size", &MatrixX::size)
       .define_method("[]",  &MatrixX::get)
       .define_method("[]=",  &MatrixX::set)
       .define_method("row", &MatrixX::getRow)
       .define_method("setRow", &MatrixX::setRow)
       .define_method("col", &MatrixX::getColumn)
       .define_method("setCol", &MatrixX::setColumn)
       .define_method("+",  &MatrixX::operator +)
       .define_method("-",  &MatrixX::operator -)
       .define_method("/",  &MatrixX::operator /)
       .define_method("-@", &MatrixX::negate)
       .define_method("*",  &MatrixX::scale)
       .define_method("dotV",  &MatrixX::dotV)
       .define_method("dotM",  &MatrixX::dotM)
       .define_method("jacobiSvd", &MatrixX::jacobiSvd, (Arg("flags") = 0))
       .define_method("approx?", &MatrixX::isApprox, (Arg("m"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));

     Data_Type<Isometry3> rb_Isometry3 = define_class_under<Isometry3>(rb_mEigen, "Isometry3")
       .define_constructor(Constructor<Isometry3>())
       .define_method("__equal__",  &Isometry3::operator ==)
       .define_method("approx?", &Isometry3::isApprox, (Arg("i"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
       .define_method("inverse", &Isometry3::inverse)
       .define_method("translation", &Isometry3::translation)
       .define_method("rotation", &Isometry3::rotation)
       .define_method("concatenate", &Isometry3::concatenate)
       .define_method("transform", &Isometry3::transform)
       .define_method("matrix", &Isometry3::matrix)
       .define_method("translate", &Isometry3::translate)
       .define_method("pretranslate", &Isometry3::pretranslate)
       .define_method("rotate", &Isometry3::rotate)
       .define_method("prerotate", &Isometry3::prerotate);

     Data_Type<Affine3> rb_Affine3 = define_class_under<Affine3>(rb_mEigen, "Affine3")
       .define_constructor(Constructor<Affine3>())
       .define_method("__equal__",  &Affine3::operator ==)
       .define_method("approx?", &Affine3::isApprox, (Arg("i"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
       .define_method("inverse", &Affine3::inverse)
       .define_method("translation", &Affine3::translation)
       .define_method("rotation", &Affine3::rotation)
       .define_method("concatenate", &Affine3::concatenate)
       .define_method("transform", &Affine3::transform)
       .define_method("matrix", &Affine3::matrix)
       .define_method("translate", &Affine3::translate)
       .define_method("pretranslate", &Affine3::pretranslate)
       .define_method("rotate", &Affine3::rotate)
       .define_method("prerotate", &Affine3::prerotate);
}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/fold_left.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/string.hpp>
#include <boost/hana/value.hpp>
namespace hana = boost::hana;

int main() {
    auto sum_string = [](auto str) {
        return hana::fold_left(str, hana::int_c<0>, [](auto sum, auto c) {
            constexpr int i = hana::value(c) - 48; // convert character to decimal
            return sum + hana::int_c<i>;
        });
    };

    BOOST_HANA_CONSTANT_CHECK(
        sum_string(BOOST_HANA_STRING("1234")) == hana::int_c<1 + 2 + 3 + 4>
    );
}


#include <Eigen/Eigen>
#include <gtest/gtest.h>

#include <aslam/common/entrypoint.h>
#include <aslam/common/memory.h>
#include <aslam/common/pose-types.h>
#include <aslam/triangulation/test/triangulation-fixture.h>

constexpr size_t kNumObservations = 20;

TYPED_TEST(TriangulationFixture, LinearTriangulateFromNViews) {
  this->setLandmark(kGPoint);
  this->fillMeasurements(kNumObservations);
  this->expectSuccess();
}

class TriangulationMultiviewTest : public TriangulationFixture<Vector2dList> {};

TEST_F(TriangulationMultiviewTest, linearTriangulateFromNViewsMultiCam) {
  Aligned<std::vector, Eigen::Vector2d> measurements;
  Aligned<std::vector, aslam::Transformation> T_G_I;
  Aligned<std::vector, aslam::Transformation> T_I_C;
  std::vector<size_t> measurement_camera_indices;
  Eigen::Vector3d G_point;

  // To make the test simple to write, create 2 cameras and fill observations
  // for both, then simply append the vectors together.
  const unsigned int num_cameras = 2;
  T_I_C.resize(num_cameras);

  for (size_t i = 0; i < T_I_C.size(); ++i) {
    T_I_C[i].setRandom(0.2, 0.1);
    Aligned<std::vector, Eigen::Vector2d> cam_measurements;
    Aligned<std::vector, aslam::Transformation> cam_T_G_I;
    fillObservations(kNumObservations, T_I_C[i], &cam_measurements, &cam_T_G_I);

    // Append to the end of the vectors.
    measurements.insert(measurements.end(), cam_measurements.begin(),
                        cam_measurements.end());
    T_G_I.insert(T_G_I.end(), cam_T_G_I.begin(), cam_T_G_I.end());

    // Fill in the correct size of camera indices also.
    measurement_camera_indices.resize(measurements.size(), i);
  }

  aslam::linearTriangulateFromNViewsMultiCam(measurements,
      measurement_camera_indices, T_G_I, T_I_C, &G_point);

  EXPECT_TRUE(EIGEN_MATRIX_NEAR(kGPoint, G_point, kDoubleTolerance));
}

TEST_F(TriangulationMultiviewTest, iterativeGaussNewtonTriangulateFromNViews) {
  Aligned<std::vector, Eigen::Vector2d> measurements;
  Aligned<std::vector, aslam::Transformation> T_G_B;
  Aligned<std::vector, aslam::Transformation> T_B_C;
  std::vector<size_t> measurement_camera_indices;
  Eigen::Vector3d G_point;

  // To make the test simple to write, create 1 camera and fill observations,
  // then simply append the vectors together.
  const unsigned int num_cameras = 1;
  T_B_C.resize(num_cameras);

  for (size_t i = 0; i < T_B_C.size(); ++i) {
    T_B_C[i].setRandom(0.2, 0.1);
    Aligned<std::vector, Eigen::Vector2d> cam_measurements;
    Aligned<std::vector, aslam::Transformation> cam_T_G_B;
    fillObservations(kNumObservations, T_B_C[i], &cam_measurements, &cam_T_G_B);

    // Append to the end of the vectors.
    measurements.insert(measurements.end(), cam_measurements.begin(),
                        cam_measurements.end());
    T_G_B.insert(T_G_B.end(), cam_T_G_B.begin(), cam_T_G_B.end());

    // Fill in the correct size of camera indices also.
    measurement_camera_indices.resize(measurements.size(), i);
  }

  aslam::iterativeGaussNewtonTriangulateFromNViews(measurements, T_G_B, T_B_C[0], &G_point);

  EXPECT_TRUE(EIGEN_MATRIX_NEAR(kGPoint, G_point, kDoubleTolerance));
}

TYPED_TEST(TriangulationFixture, RandomPoses) {
  constexpr size_t kNumCameraPoses = 5;
  this->setNMeasurements(kNumCameraPoses);

  // Create a landmark.
  const double depth = 5.0;
  this->setLandmark(Eigen::Vector3d(1.0, 1.0, depth));

  // Generate some random camera poses and project the landmark into it.
  constexpr double kRandomTranslationNorm = 0.1;
  constexpr double kRandomRotationAngleRad = 20 / 180.0 * M_PI;

  for (size_t pose_idx = 0; pose_idx < kNumCameraPoses; ++pose_idx) {
    this->T_G_I_[pose_idx].setRandom(kRandomTranslationNorm, kRandomRotationAngleRad);
  }

  this->inferMeasurements();
  this->expectSuccess();
}

TYPED_TEST(TriangulationFixture, TwoParallelRays) {
  constexpr size_t kNumCameraPoses = 3;
  this->setNMeasurements(kNumCameraPoses);

  // Create a landmark.
  const double depth = 5.0;
  this->setLandmark(Eigen::Vector3d(1.0, 1.0, depth));

  for (size_t pose_idx = 0; pose_idx < kNumCameraPoses; ++pose_idx) {
    this->T_G_I_[pose_idx].setIdentity();
  }

  this->inferMeasurements();
  this->expectFailue();
}

TYPED_TEST(TriangulationFixture, TwoNearParallelRays) {
  constexpr size_t kNumCameraPoses = 2;
  this->setNMeasurements(kNumCameraPoses);

  // Create a landmark.
  const double depth = 5.0;
  this->setLandmark(Eigen::Vector3d(1.0, 1.0, depth));

  // Create near parallel rays.
  aslam::Transformation noise;
  const double disparity_angle_rad = 0.1 / 180.0 * M_PI;

  const double camera_shift = std::atan(disparity_angle_rad) * depth;
  noise.setRandom(camera_shift, 0.0);
  this->T_G_I_[1] = this->T_G_I_[1] * noise;

  this->inferMeasurements();
  this->expectFailue();
}

TYPED_TEST(TriangulationFixture, CombinedParallelAndGoodRays) {
  constexpr size_t kNumCameraPoses = 3;
  this->setNMeasurements(kNumCameraPoses);

  // Create near parallel rays.
  aslam::Transformation noise;
  noise.setRandom(0.01, 0.1);
  this->T_G_I_[1] = this->T_G_I_[1] * noise;

  this->T_G_I_[2].setRandom(0.5, 0.2);

  // Create a landmark.
  const double depth = 5.0;
  this->setLandmark(Eigen::Vector3d(1.0, 1.0, depth));

  this->inferMeasurements();
  this->expectSuccess();
}

ASLAM_UNITTEST_ENTRYPOINT


#include <Eigen/Dense>

#include <ancse/config.hpp>
#include <ancse/cfl_condition.hpp>
#include <ancse/fvm_rate_of_change.hpp>
#include <ancse/snapshot_writer.hpp>
#include <ancse/time_loop.hpp>

static int n_vars = 3;

template<class F>
Eigen::MatrixXd ic(const F &f, const Grid &grid) {
    Eigen::MatrixXd u0(n_vars, grid.n_cells);
    for(int i = 0; i < grid.n_cells; ++i) {
        u0.col(i) = f(cell_center(grid, i));
    }

    return u0;
}

TimeLoop make_fvm(const nlohmann::json &config,
                  const Grid &grid,
                  std::shared_ptr<Model> &model)
{
    double t_end = config["t_end"];
    double cfl_number = config["cfl_number"];

    auto n_ghost = grid.n_ghost;
    auto n_cells = grid.n_cells;

    auto n_vars = model->get_nvars();

    auto simulation_time = std::make_shared<SimulationTime>(t_end);
    auto fvm_rate_of_change
            = make_fvm_rate_of_change(config, grid, model,
                                      simulation_time);
    auto boundary_condition
            = make_boundary_condition(n_ghost,
                                      config["boundary_condition"]);
    auto time_integrator = make_runge_kutta(config,
                                            fvm_rate_of_change,
                                            boundary_condition,
                                            n_vars, n_cells);
    auto cfl_condition = make_cfl_condition(grid, model, cfl_number);
    auto snapshot_writer = std::make_shared< JSONSnapshotWriter<FVM> >
            (grid, model, simulation_time,
             std::string(config["output_dir"]),
             std::string(config["output_file"]));

    return TimeLoop(simulation_time, time_integrator,
                    cfl_condition, snapshot_writer);
}

void sod_shock_tube_test(const nlohmann::json &config)
{
    double gamma = 7./5.;
    std::shared_ptr<Model> model = std::make_shared<Euler>();
    auto model_euler = dynamic_cast<Euler*>(model.get());
    model_euler->set_gamma(gamma);

    auto fn = [gamma](double x) {
        Eigen::VectorXd u(n_vars);
        if (x <= 0.5) { // left state
            u(0) = 1;
            u(1) = 0;
            u(2) = 1/(gamma-1);
        } else {        // right state
            u(0) = 0.125;
            u(1) = 0;
            u(2) = 0.1/(gamma-1);
        }
        return u;
    };

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({0.0, 1.0}, n_cells, n_ghost);
    auto u0 = ic(fn, grid);

    auto fvm = make_fvm(config, grid, model);
    fvm(u0);
}

void vacuum_test(const nlohmann::json &config)
{
    double gamma = 7./5.;
    std::shared_ptr<Model> model = std::make_shared<Euler>();
    auto model_euler = dynamic_cast<Euler*>(model.get());
    model_euler->set_gamma(gamma);

    auto fn = [](double x) {
        Eigen::VectorXd u(n_vars);
        if (x <= 0.5) { // left state
            u(0) = 1;
            u(1) = -2;
            u(2) = 4.5;
        } else {        // right state
            u(0) = 1;
            u(1) = 2;
            u(2) = 4.5;
        }
        return u;
    };

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({0.0, 1.0}, n_cells, n_ghost);
    auto u0 = ic(fn, grid);

    auto fvm = make_fvm(config, grid, model);
    fvm(u0);
}

void lax_shock_tube_test(const nlohmann::json &config)
{
    double gamma = 7./5.;
    std::shared_ptr<Model> model = std::make_shared<Euler>();
    auto model_euler = dynamic_cast<Euler*>(model.get());
    model_euler->set_gamma(gamma);

    auto fn = [](double x) {
        Eigen::VectorXd u(n_vars);
        if (x <= 0.5) { // left state
            u(0) = 0.445;
            u(1) = 0.311;
            u(2) = 8.928;
        } else {        // right state
            u(0) = 0.5;
            u(1) = 0;
            u(2) = 1.4275;
        }
        return u;
    };

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({0.0, 1.0}, n_cells, n_ghost);
    auto u0 = ic(fn, grid);

    auto fvm = make_fvm(config, grid, model);
    fvm(u0);
}

void smooth_test(const nlohmann::json &config)
{
    double gamma = 7./5.;
    std::shared_ptr<Model> model = std::make_shared<Euler>();
    auto model_euler = dynamic_cast<Euler*>(model.get());
    model_euler->set_gamma(gamma);

    double a = 0.1;
    double sigma = 0.1;
    auto fn = [gamma, a, sigma](double x) {
        Eigen::VectorXd u(n_vars);
        u(0) = 1;
        u(1) = 0;
        u(2) = (1 + a*exp(-x*x/(sigma*sigma)))/(gamma-1);
        return u;
    };

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({-1.0, +1.0}, n_cells, n_ghost);
    auto u0 = ic(fn, grid);

    auto fvm = make_fvm(config, grid, model);
    fvm(u0);
}

int main(int argc, char* const argv[])
{
    nlohmann::json config;
    std::string fileName;
    if (argc == 2) {
        fileName = argv[1];
    } else {
        fileName = "../config.json";
    }
    config = get_config (fileName);

    std::string ic_key = config["initial_conditions"];
    if (ic_key == "sod_shock_tube") {
        sod_shock_tube_test(config);
    } else if (ic_key == "vacuum") {
    vacuum_test(config);
    } else if (ic_key == "lax_shock_tube") {
        lax_shock_tube_test(config);
    } else if (ic_key == "smooth") {
        smooth_test(config);
    }

    return 0;
}


//
//  ComputeHausdorffDistance.cpp
//  Elasticity
//
//  Created by Wim van Rees on 12/28/16.
//  Copyright © 2016 Wim van Rees. All rights reserved.
//

#include "ComputeHausdorffDistance.hpp"

#include <igl/hausdorff.h>
#include <Eigen/Geometry>
#include "WriteVTK.hpp"

Real ComputeHausdorffDistance::compute(const Eigen::Ref<const Eigen::MatrixXd> vertices_A, const Eigen::Ref<const Eigen::MatrixXd> vertices_B, const Eigen::Ref<const Eigen::MatrixXi> faces, const Real rescale) const
{
    // compute the least-squares transformation between two point sets
    //https://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gab3f5a82a24490b936f8694cf8fef8e60

    const Eigen::MatrixXd trafo = Eigen::umeyama(vertices_A.transpose(), vertices_B.transpose(), rescale);
    const Eigen::Matrix3d rotmat = trafo.block<3,3>(0,0);
    const Eigen::Vector3d transv = trafo.block<3,1>(0,3);
    // now we have the rotation and scaling matrices to transform vertices_A into vertices_B

    // apply the transform
    const int nVertices = vertices_A.rows();
    Eigen::MatrixXd vertices_A_trafo(nVertices,3);
    for(int i=0;i<nVertices;++i)
    {
        const Eigen::Vector3d vertA = vertices_A.row(i);
        const Eigen::Vector3d vertA_trafo = rotmat * vertA + transv;
        vertices_A_trafo.row(i) = vertA_trafo;
    }

    if(bDump)
    {
        WriteVTK writerA(vertices_A, faces);
        writerA.write("haussdorf_A");

        WriteVTK writerB(vertices_B, faces);
        writerB.write("haussdorf_B");

        WriteVTK writerA_trafo(vertices_A_trafo, faces);
        writerA_trafo.write("haussdorf_A_trafo");
    }

    // compute the distance
    Real retval;
//    igl::hausdorff<Eigen::MatrixXd, Eigen::MatrixXi, Eigen::MatrixXd, Eigen::MatrixXi, Real>(vertices_A_trafo, faces, vertices_B, faces, retval);

    // IGL does not handle Eigen::Ref well -- need to define a reference explicitly
    const Eigen::MatrixXd & ref_vertices_B = vertices_B;
    const Eigen::MatrixXi & ref_faces = faces;
    igl::hausdorff(vertices_A_trafo, ref_faces, ref_vertices_B, ref_faces, retval);

    return retval;
}


#ifndef __POLYNOMIAL_H__
#define __POLYNOMIAL_H__

#include <iostream>
#include <vector>
#include <math.h>
#include <NTL/ZZ_p.h>

#include "src/utils/math.hpp"

// TODO : REFACTOR -> use recursive polynomials
// TODO : Templatize the class

typedef std::vector<unsigned int> monomial;

class MultiVariatePolynomial
{
public:
    MultiVariatePolynomial(unsigned int arity, unsigned int degree, unsigned int order) : arity(arity), degree(degree), order(order), maxNbElements(binomialCoefficient(degree + arity, degree)), coefficients(maxNbElements)
    {
        NTL::ZZ p((long)order);
        NTL::ZZ_p::init(p);
    }
    MultiVariatePolynomial(unsigned int arity, unsigned int degree, unsigned int order, std::vector<NTL::ZZ_p> values) : MultiVariatePolynomial(arity, degree, order)
    {
        if (values.size() != this->getMaxNbElements())
            throw std::invalid_argument("Incorrect number of values provided");
        coefficients = values;
    }
    MultiVariatePolynomial() = delete;

    unsigned int getArity() const
    {
        return this->arity;
    };
    void setArity(unsigned int arity)
    {
        this->arity = arity;
    };
    unsigned int getDegree() const
    {
        return this->degree;
    };
    void setDegree(unsigned int degree)
    {
        this->degree = degree;
    };
    unsigned int getOrder() const
    {
        return this->order;
    };
    void setOrder(unsigned int order)
    {
        this->order = order;
    };

    unsigned int getMaxNbElements() const
    {
        return this->maxNbElements;
    };

    void setElement(std::vector<unsigned int>& monomial, NTL::ZZ_p value);

    void setElement(unsigned int index, NTL::ZZ_p value);

    NTL::ZZ_p getElement(std::vector<unsigned int>& monomial) const;

    NTL::ZZ_p getElement(unsigned int index) const;

    void print() const;

    std::string to_string() const;

    MultiVariatePolynomial add(const MultiVariatePolynomial& other) const;
    MultiVariatePolynomial operator+(const MultiVariatePolynomial& other) const;

    MultiVariatePolynomial sub(const MultiVariatePolynomial& other) const;
    MultiVariatePolynomial operator-(const MultiVariatePolynomial& other) const;

    NTL::ZZ_p evaluate(std::vector<NTL::ZZ_p>& point) const;

    static MultiVariatePolynomial interpolate(std::vector<std::vector<NTL::ZZ_p>>& known_values, int degree, int arity, int order);

private:
    unsigned int arity;
    unsigned int degree;
    unsigned int order;
    unsigned int maxNbElements;
    std::vector<NTL::ZZ_p> coefficients;
};

#endif // __POLYNOMIAL_H__


/* -*-c++-*--------------------------------------------------------------------
 * 2018 Bernd Pfrommer bernd.pfrommer@gmail.com
 */

#include "tagslam/yaml_utils.h"
#include <boost/range/irange.hpp>
#include <XmlRpcException.h>
#include <Eigen/Geometry>
#include <cmath>

namespace tagslam {
  using boost::irange;
  using std::sqrt;
  using std::fixed;
  using std::setw;
  using std::setprecision;

#define FMT(X, Y) fixed << setw(X) << setprecision(Y)

  namespace yaml_utils {
    static void write_vec(std::ostream &of,
                          const string &prefix,
                          double x, double y, double z) {
      const int p(8);
      of.precision(p);
      of << prefix << "x: " << std::fixed << x << std::endl;
      of << prefix << "y: " << std::fixed << y << std::endl;
      of << prefix << "z: " << std::fixed << z << std::endl;
    }

    void write_pose(std::ostream &of, const string &prefix,
                    const Transform &pose,
                    const PoseNoise &n, bool writeNoise) {
      Eigen::AngleAxisd aa(pose.linear());
      Point3d r = aa.angle() * aa.axis();
      Point3d t(pose.translation());
      const string pps = prefix + "  ";
      of << prefix << "position:" << std::endl;
      write_vec(of, pps, t(0), t(1), t(2));
      of << prefix << "rotation:" << std::endl;
      write_vec(of, pps, r(0), r(1), r(2));
      if (writeNoise) {
        Eigen::Matrix<double, 6, 1> diag = n.getDiagonal();
        of << prefix << "position_noise:" << std::endl;
        write_vec(of, pps, sqrt(diag(3)),sqrt(diag(4)),sqrt(diag(5)));
        of << prefix << "rotation_noise:" << std::endl;
        write_vec(of, pps, sqrt(diag(0)),sqrt(diag(1)),sqrt(diag(2)));
      }
    }

    void write_matrix(std::ostream &of, const string &prefix,
                      const Transform &pose) {
      const auto m = pose.matrix();
      for (const auto i: irange(0, 4)) {
        of << prefix << "- [";
        for (const auto j: irange(0, 3)) {
          of << FMT(12,8) << m(i, j) << ",";
        }
        of << FMT(12,8) << m(i, 3) << "]" << std::endl;
      }
    }

    void write_pose_with_covariance(std::ostream &of,
                                    const string &prefix,
                                    const Transform &pose,
                                    const PoseNoise &n) {
      Eigen::AngleAxisd aa;
      aa.fromRotationMatrix(pose.rotation());
      Eigen::Vector3d r = aa.angle() * aa.axis();
      Eigen::Vector3d t = pose.translation();
      const string pps = prefix + "  ";
      of << prefix << "position:" << std::endl;
      write_vec(of, pps, t(0), t(1), t(2));
      of << prefix << "rotation:" << std::endl;
      write_vec(of, pps, r(0), r(1), r(2));
      of << prefix << "R:" << std::endl;
      const auto R = n.convertToR();
      of << prefix << "  [ ";
      for (const auto i: irange(0l, R.rows())) {
        for (const auto j: irange(0l, R.cols())) {
          of << R(i, j);
          if (i != R.rows() - 1 || j != R.cols() - 1) {
            of << ", ";
          }
        }
        if (i != R.rows() - 1) {
          of << std::endl << prefix << "    ";
        }
      }
      of << " ]" << std::endl;
    }
  }
}  // namespace


#include <stdlib.h>
#include <assert.h>
#include <stdint.h>
#include <string.h>
#include <iostream>

#include <boost/array.hpp>

#include "tools/debug.hpp"
#include "tools/get_char.hpp"
#include "tools/insertion_sort.hpp"
#include "tools/losertree.hpp"
#include "../tools/contest.hpp"

namespace rantala {

void mergesort_4way(unsigned char**, size_t, unsigned char**);

template <unsigned K>
static void
mergesort_losertree(unsigned char** strings, size_t n, unsigned char** tmp)
{
	if (n < 0x10000) {
		mergesort_4way(strings, n, tmp);
		return;
	}
	debug() << __func__ << "(), n="<<n<<"\n";
	const size_t split = size_t(double(n) / double(K));
	boost::array<std::pair<unsigned char**, size_t>, K> ranges;
	for (unsigned i=0; i < K-1; ++i) {
		ranges[i] = std::make_pair(strings+i*split, split);
	}
	ranges[K-1] = std::make_pair(strings+(K-1)*split, n-(K-1)*split);
	for (unsigned i=0; i < K; ++i) {
		mergesort_losertree<K>(ranges[i].first, ranges[i].second,
				tmp+(ranges[i].first-strings));
	}
	unsigned char** result = tmp;
	loser_tree<unsigned char*> tree(ranges.begin(), ranges.end());
	while (tree._nonempty_streams) { *result++ = tree.min(); }
	(void) memcpy(strings, tmp, n*sizeof(unsigned char*));
}

void mergesort_losertree_64way(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree<64>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_128way(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree<128>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_256way(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree<256>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_512way(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree<512>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_1024way(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree<1024>(strings, n, tmp);
	free(tmp);
}

PSS_CONTESTANT(mergesort_losertree_64way,
               "rantala/mergesort_losertree_64way",
               "mergesort 64way loser tree based")
PSS_CONTESTANT(mergesort_losertree_128way,
               "rantala/mergesort_losertree_128way",
               "mergesort 128way loser tree based")
PSS_CONTESTANT(mergesort_losertree_256way,
               "rantala/mergesort_losertree_256way",
               "mergesort 256way loser tree based")
PSS_CONTESTANT(mergesort_losertree_512way,
               "rantala/mergesort_losertree_512way",
               "mergesort 512way loser tree based")
PSS_CONTESTANT(mergesort_losertree_1024way,
               "rantala/mergesort_losertree_1024way",
               "mergesort 1024way loser tree based")

void mergesort_4way_parallel(unsigned char**, size_t, unsigned char**);

template <unsigned K>
static void
mergesort_losertree_parallel(unsigned char** strings, size_t n, unsigned char** tmp)
{
	if (n < 0x10000) {
		mergesort_4way_parallel(strings, n, tmp);
		return;
	}
	debug() << __func__ << "(), n="<<n<<"\n";
	const size_t split = size_t(double(n) / double(K));
	boost::array<std::pair<unsigned char**, size_t>, K> ranges;
	for (unsigned i=0; i < K-1; ++i) {
		ranges[i] = std::make_pair(strings+i*split, split);
	}
	ranges[K-1] = std::make_pair(strings+(K-1)*split, n-(K-1)*split);
#pragma omp parallel for
	for (unsigned i=0; i < K; ++i) {
		mergesort_losertree_parallel<K>(ranges[i].first, ranges[i].second,
				tmp+(ranges[i].first-strings));
	}
	unsigned char** result = tmp;
	loser_tree<unsigned char*> tree(ranges.begin(), ranges.end());
	while (tree._nonempty_streams) { *result++ = tree.min(); }
	(void) memcpy(strings, tmp, n*sizeof(unsigned char*));
}

void mergesort_losertree_64way_parallel(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree_parallel<64>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_128way_parallel(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree_parallel<128>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_256way_parallel(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree_parallel<256>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_512way_parallel(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree_parallel<512>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_1024way_parallel(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree_parallel<1024>(strings, n, tmp);
	free(tmp);
}

PSS_CONTESTANT_PARALLEL(mergesort_losertree_64way_parallel,
                        "rantala/mergesort_losertree_64way_parallel",
                        "mergesort parallel 64way loser tree based")
PSS_CONTESTANT_PARALLEL(mergesort_losertree_128way_parallel,
                        "rantala/mergesort_losertree_128way_parallel",
                        "mergesort parallel 128way loser tree based")
PSS_CONTESTANT_PARALLEL(mergesort_losertree_256way_parallel,
                        "rantala/mergesort_losertree_256way_parallel",
                        "mergesort parallel 256way loser tree based")
PSS_CONTESTANT_PARALLEL(mergesort_losertree_512way_parallel,
                        "rantala/mergesort_losertree_512way_parallel",
                        "mergesort parallel 512way loser tree based")
PSS_CONTESTANT_PARALLEL(mergesort_losertree_1024way_parallel,
                        "rantala/mergesort_losertree_1024way_parallel",
                        "mergesort parallel 1024way loser tree based")

}


/*
 * fclayer.cpp
 *
 * Feed-forward:
 *    z(l) = a(l-1) * w + b // where * (vector, matrix) multiplication
 *    a(l) = activation(z(l))
 *
 * Back propagation:
 *    delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
 *    dC/db(l) = delta(l)
 *    dC/dw(l) = a(l-1) * delta(l)
 *    gradient(C, a(l - 1)) = transp(w) * delta(l)
 */
#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/random.h"
#include "core/functions.h"
#include "fclayer.h"

using namespace std;
using namespace boost;
using namespace yann;

namespace yann {

////////////////////////////////////////////////////////////////////////////////////////////////
//
// FullyConnectedLayer_Context implementation
//
class FullyConnectedLayer_Context :
    public Layer::Context
{
  typedef Layer::Context Base;

  friend class FullyConnectedLayer;

public:
  FullyConnectedLayer_Context(const MatrixSize & output_size,
                              const MatrixSize & batch_size) :
    Base(output_size, batch_size)
  {
    YANN_CHECK_GT(output_size, 0);
    YANN_CHECK_GT(batch_size, 0);
    _zz.resize(get_batch_size(), get_output_size());
  }

  FullyConnectedLayer_Context(const RefVectorBatch & output) :
    Base(output)
  {
    YANN_CHECK_GT(yann::get_batch_size(output), 0);
    YANN_CHECK_GT(yann::get_batch_item_size(output), 0);

    _zz.resize(get_batch_size(), get_output_size());
  }

protected:
  VectorBatch              _zz;
}; // class FullyConnectedLayer_Context

////////////////////////////////////////////////////////////////////////////////////////////////
//
// FullyConnectedLayer_TrainingContext implementation
//
class FullyConnectedLayer_TrainingContext :
    public FullyConnectedLayer_Context
{
  typedef FullyConnectedLayer_Context Base;

  typedef vector<pair<MatrixSize, Value>> SamplingSortingVector;
  typedef vector<MatrixSize> SamplingCounterVector;

  friend class FullyConnectedLayer;

public:
  FullyConnectedLayer_TrainingContext(
      const MatrixSize & input_size,
      const MatrixSize & output_size,
      const MatrixSize & batch_size,
      const unique_ptr<Layer::Updater> & updater,
      bool is_sampled) :
    Base(output_size, batch_size),
    _ww_updater(updater->copy()),
    _bb_updater(updater->copy())
  {
    YANN_CHECK_GT(input_size, 0);

    _delta_ww.resize(input_size, get_output_size());
    _delta_bb.resize(get_output_size());
    _delta.resizeLike(_zz);

    _sigma_derivative_zz.resizeLike(_zz);
    _collapse_vector = Vector::Ones(get_batch_size());

    _ww_updater->init(input_size, get_output_size());
    _bb_updater->init(1, get_output_size()); // RowMajor

    if(is_sampled) {
      _sampling_gradient_out_sorting = SamplingSortingVector(get_output_size());
      _sampling_counter = SamplingCounterVector(get_output_size(), 0);
    }
  }

  FullyConnectedLayer_TrainingContext(
      const MatrixSize & input_size,
      const RefVectorBatch & output,
      const unique_ptr<Layer::Updater> & updater,
      bool is_sampled) :
    Base(output),
    _ww_updater(updater->copy()),
    _bb_updater(updater->copy())
  {
    YANN_CHECK_GT(input_size, 0);

    _delta_ww.resize(input_size, get_output_size());
    _delta_bb.resize(get_output_size());
    _delta.resizeLike(_zz);

    _sigma_derivative_zz.resizeLike(_zz);
    _collapse_vector = Vector::Ones(get_batch_size());

    _ww_updater->init(input_size, get_output_size());
    _bb_updater->init(1, get_output_size()); // RowMajor

    if(is_sampled) {
      _sampling_gradient_out_sorting = SamplingSortingVector(get_output_size());
      _sampling_counter = SamplingCounterVector(get_output_size(), 0);
    }
  }

  // Layer::Context overwrites
  virtual void start_epoch()
  {
    YANN_SLOW_CHECK(_ww_updater);
    YANN_SLOW_CHECK(_bb_updater);

    Base::start_epoch();

    _ww_updater->start_epoch();
    _bb_updater->start_epoch();
  }

  virtual void reset_state()
  {
    YANN_SLOW_CHECK(_ww_updater);
    YANN_SLOW_CHECK(_bb_updater);

    Base::reset_state();

    _delta_ww.setZero();
    _delta_bb.setZero();

    _ww_updater->reset();
    _bb_updater->reset();

    if(_sampling_counter) {
      fill(_sampling_counter->begin(), _sampling_counter->end(), 0);
    }
  }

private:
  Matrix _delta_ww;
  Vector _delta_bb;
  VectorBatch _delta;

  VectorBatch _sigma_derivative_zz;
  Vector _collapse_vector;

  unique_ptr<Layer::Updater> _ww_updater;
  unique_ptr<Layer::Updater> _bb_updater;

  // sampling support
  optional<SamplingSortingVector> _sampling_gradient_out_sorting;
  optional<SamplingCounterVector> _sampling_counter;
}; // class FullyConnectedLayer_TrainingContext

}; // namespace yann

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::FullyConnectedLayer implementation
//
yann::FullyConnectedLayer::FullyConnectedLayer(const MatrixSize & input_size,
                                               const MatrixSize & output_size) :
    _ww(input_size, output_size),
    _bb(output_size),
    _fixed_bias(false),
    _sampling_rate(2.0), // bigger than 1.0
    _activation_function(new SigmoidFunction())
{
}

yann::FullyConnectedLayer::~FullyConnectedLayer()
{
}

void yann::FullyConnectedLayer::set_activation_function(
    const unique_ptr<ActivationFunction> & activation_function)
{
  YANN_CHECK(activation_function);
  _activation_function = activation_function->copy();
}

void yann::FullyConnectedLayer::set_values(const Matrix & ww, const Vector & bb)
{
  YANN_CHECK(is_same_size(ww, _ww));
  YANN_CHECK(is_same_size(bb, _bb));
  _ww = ww;
  _bb = bb;
}

void yann::FullyConnectedLayer::set_fixed_bias(const Value & val)
{
  _bb.setConstant(val);
  _fixed_bias = true;
}

void yann::FullyConnectedLayer::set_sampling_rate(const double & sampling_rate)
{
  YANN_CHECK_GE(sampling_rate, 0.0);
  YANN_CHECK_LE(sampling_rate, 1.0);
  _sampling_rate = sampling_rate;
}

bool yann::FullyConnectedLayer::is_sampled() const
{
  return 0 < _sampling_rate && _sampling_rate < 1.0;
}

// Layer overwrites
bool yann::FullyConnectedLayer::is_valid() const
{
  if(!Base::is_valid()) {
    return false;
  }
  if(!_activation_function) {
    return false;
  }
  if(_ww.cols() != _bb.size()) {
    return false;
  }
  return true;
}

std::string yann::FullyConnectedLayer::get_name() const
{
  return "FullyConnectedLayer";
}

string yann::FullyConnectedLayer::get_info() const
{
  YANN_CHECK(is_valid());

  ostringstream oss;
  oss << Base::get_info()
      << " activation: " << _activation_function->get_info()
      ;
  return oss.str();
}

bool yann::FullyConnectedLayer::is_equal(const Layer & other, double tolerance) const
{
  if(!Base::is_equal(other, tolerance)) {
    return false;
  }
  auto the_other = dynamic_cast<const FullyConnectedLayer*>(&other);
  if(the_other == nullptr) {
    return false;
  }
  // TOOD: add deep compare
  if(_activation_function->get_info() != the_other->_activation_function->get_info()) {
    return false;
  }
  if(_fixed_bias != the_other->_fixed_bias) {
    return false;
  }
  if(!_ww.isApprox(the_other->_ww, tolerance)) {
    return false;
  }
  if(!_bb.isApprox(the_other->_bb, tolerance)) {
    return false;
  }
  return true;
}

MatrixSize yann::FullyConnectedLayer::get_input_size() const
{
  return _ww.rows();
}

MatrixSize yann::FullyConnectedLayer::get_output_size() const
{
  return _ww.cols();
}

unique_ptr<Layer::Context> yann::FullyConnectedLayer::create_context(const MatrixSize & batch_size) const
{
  YANN_CHECK(is_valid());
  return make_unique<FullyConnectedLayer_Context>(get_output_size(), batch_size);
}
unique_ptr<Layer::Context> yann::FullyConnectedLayer::create_context(const RefVectorBatch & output) const
{
  YANN_CHECK(is_valid());
  return make_unique<FullyConnectedLayer_Context>(output);
}
unique_ptr<Layer::Context> yann::FullyConnectedLayer::create_training_context(
    const MatrixSize & batch_size, const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  YANN_CHECK(updater);
  return make_unique<FullyConnectedLayer_TrainingContext>(
      get_input_size(), get_output_size(), batch_size, updater, is_sampled());
}
unique_ptr<Layer::Context> yann::FullyConnectedLayer::create_training_context(
    const RefVectorBatch & output, const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  YANN_CHECK(updater);
  return make_unique<FullyConnectedLayer_TrainingContext>(
      get_input_size(), output, updater, is_sampled());
}

template<typename InputType>
void yann::FullyConnectedLayer::feedforward_internal(
    const InputType & input,
    Context * context,
    enum OperationMode mode) const
{
  auto ctx = dynamic_cast<FullyConnectedLayer_Context *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_EQ(get_batch_size(input), ctx->get_batch_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());

  // z(l) = a(l-1)*w + b
  ctx->_zz.noalias() = MatrixFunctions<InputType>::product(input, _ww);
  plus_batches(ctx->_zz, _bb);

  // a(l) = activation(z(l))
  YANN_CHECK(is_same_size(ctx->_zz, ctx->get_output()));
  _activation_function->f(ctx->_zz, ctx->get_output(), mode);
}

void yann::FullyConnectedLayer::feedforward(
    const RefConstVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  feedforward_internal(input, context, mode);
}

void yann::FullyConnectedLayer::feedforward(
    const RefConstSparseVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  feedforward_internal(input, context, mode);
}

template<typename InputType>
void yann::FullyConnectedLayer::backprop_internal(
    const RefConstVectorBatch & gradient_output,
    const InputType & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  auto ctx = dynamic_cast<FullyConnectedLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_GT(get_batch_size(gradient_output), 0);
  YANN_CHECK_EQ(get_batch_item_size(gradient_output), get_output_size());
  YANN_CHECK_EQ(get_batch_size(input), get_batch_size(gradient_output));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK(!gradient_input || is_same_size(input, *gradient_input));

  // just to make it easier to read
  const auto & zz = ctx->_zz;
  auto & sigma_derivative_zz = ctx->_sigma_derivative_zz;
  auto & delta = ctx->_delta;
  const auto & collapse_vector = ctx->_collapse_vector;
  auto & delta_ww = ctx->_delta_ww;
  auto & delta_bb = ctx->_delta_bb;
  const auto batch_size = get_batch_size(input);

  // delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
  YANN_CHECK(is_same_size(zz, sigma_derivative_zz));
  YANN_CHECK(is_same_size(zz, gradient_output));
  _activation_function->derivative(zz, sigma_derivative_zz);
  delta.array() = gradient_output.array() * sigma_derivative_zz.array();

  // update deltas
  // dC/db(l) = delta(l)
  // dC/dw(l) = a(l-1) * delta(l)
  YANN_CHECK_EQ(collapse_vector.size(), get_batch_size(delta));
  YANN_CHECK_EQ(delta_bb.size(), get_output_size());
  YANN_CHECK_EQ(batch_size,  get_batch_size(gradient_output));
  YANN_CHECK(is_same_size(delta_ww, _ww));
  YANN_CHECK(is_same_size(delta_bb, _bb));
  delta_ww.noalias() += MatrixFunctions<InputType>::product(input.transpose(), delta);
  if(!_fixed_bias) {
    delta_bb.noalias() += MatrixFunctions<InputType>::product(collapse_vector, delta);
  }

  // we don't need to calculate the gradient(C, a(l)) for the "first" layer (actual inputs)
  // gradient(C, a(l - 1)) = transp(w) * delta(l)
  if(gradient_input) {
    (*gradient_input).noalias() = MatrixFunctions<InputType>::product(delta, _ww.transpose());
  }
}

template<typename InputType>
void yann::FullyConnectedLayer::backprop_with_sampling_internal(
    const RefConstVectorBatch & gradient_output,
    const InputType & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  auto ctx = dynamic_cast<FullyConnectedLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(ctx->_sampling_gradient_out_sorting);
  YANN_CHECK(ctx->_sampling_counter);
  YANN_CHECK(is_valid());
  YANN_CHECK(is_sampled());
  YANN_CHECK_GT(get_batch_size(gradient_output), 0);
  YANN_CHECK_EQ(get_batch_item_size(gradient_output), get_output_size());
  YANN_CHECK_EQ(get_batch_size(input), get_batch_size(gradient_output));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK(!gradient_input || is_same_size(input, *gradient_input));

  // one row at a time
  const auto batch_size = get_batch_size(input);
  const auto output_size = get_output_size();
  const MatrixSize sampled_num = get_output_size() * _sampling_rate;
  auto & delta_ww = ctx->_delta_ww;
  auto & delta_bb = ctx->_delta_bb;
  YANN_SLOW_CHECK_EQ(delta_bb.size(), get_output_size());
  auto & sampling_gradient_out_sorting = *(ctx->_sampling_gradient_out_sorting);
  auto & sampling_counter = *(ctx->_sampling_counter);

  YANN_SLOW_CHECK(is_same_size(delta_ww, _ww));
  YANN_SLOW_CHECK(is_same_size(delta_bb, _bb));

  if(gradient_input) {
    gradient_input->setZero();
  }

  for(MatrixSize batch_pos = 0; batch_pos < batch_size; ++batch_pos) {
    // just to make it easier to read
    const auto in = get_batch(input, batch_pos);
    const auto gradient_out = get_batch(gradient_output, batch_pos);
    const auto zz = get_batch(ctx->_zz, batch_pos);
    const auto sigma_derivative_zz = get_batch(ctx->_sigma_derivative_zz, batch_pos);

    YANN_CHECK(is_same_size(zz, sigma_derivative_zz));
    YANN_CHECK(is_same_size(zz, gradient_out));

    // prepare sampling
    for(MatrixSize ii = 0, out_size = output_size; ii < out_size; ++ii) {
      sampling_gradient_out_sorting[ii].first = ii;
      sampling_gradient_out_sorting[ii].second = std::abs(gradient_out(ii));
    }
    sort(sampling_gradient_out_sorting.begin(), sampling_gradient_out_sorting.end(),
        [](const auto & left, const auto & right) -> bool
        {
          return left.second > right.second; // sort in descending order
        }
    );
    // TODO: we don't need to calculate it for all values
    _activation_function->derivative(zz, sigma_derivative_zz);

    for(MatrixSize ii = 0; ii < sampled_num; ++ii) {
      const auto out_pos = sampling_gradient_out_sorting[ii].first;
      YANN_SLOW_CHECK_GE(out_pos, 0);
      YANN_SLOW_CHECK_LT(out_pos, output_size);
      ++sampling_counter[out_pos];

      // delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
      const auto delta = gradient_out(out_pos) * sigma_derivative_zz(out_pos);

      // update deltas
      // dC/db(l) = delta(l)
      // dC/dw(l) = a(l-1) * delta(l)
      delta_ww.col(out_pos) += in * delta;
      if(!_fixed_bias) {
        delta_bb(out_pos) += delta;
      }
      // we don't need to calculate the gradient(C, a(l)) for the "first" layer (actual inputs)
      // // gradient(C, a(l - 1)) = transp(w) * delta(l)
      if(gradient_input) {
        auto gradient_in = get_batch(*gradient_input, batch_pos);
        gradient_in.noalias() += delta * _ww.col(out_pos);
      }
    }
  }
}

void yann::FullyConnectedLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  if(is_sampled()) {
    backprop_with_sampling_internal(gradient_output, input, gradient_input, context);
  } else {
    backprop_internal(gradient_output, input, gradient_input, context);
  }
}

void yann::FullyConnectedLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstSparseVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  // <RefConstSparseMatrix> is required for MatrixFunctions<>::product
  if(is_sampled()) {
    backprop_with_sampling_internal<RefConstSparseMatrix>(gradient_output, input, gradient_input, context);
  } else {
    backprop_internal<RefConstSparseMatrix>(gradient_output, input, gradient_input, context);
  }
}

void yann::FullyConnectedLayer::init(enum InitMode mode, boost::optional<InitContext> init_context)
{
  switch (mode) {
  case InitMode_Zeros:
    _ww.setZero();
    if(!_fixed_bias) {
      _bb.setZero();
    }
    break;
  case InitMode_Random:
    {
      unique_ptr<RandomGenerator> gen01 = RandomGenerator::normal_distribution(0, 1,
          init_context ? optional<Value>(init_context->seed()) : boost::none);
      gen01->generate(_ww);
      if(!_fixed_bias) {
        gen01->generate(_bb);
      }
    }
    break;
  }
}

void yann::FullyConnectedLayer::update(Context * context, const size_t & tests_num)
{
  auto ctx = dynamic_cast<FullyConnectedLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(ctx->_ww_updater);
  YANN_CHECK(ctx->_bb_updater);
  YANN_CHECK(is_same_size(_ww, ctx->_delta_ww));
  YANN_CHECK(is_same_size(_bb, ctx->_delta_bb));

  if(!is_sampled()) {
    ctx->_ww_updater->update(ctx->_delta_ww, tests_num, _ww);
    if(!_fixed_bias) {
      ctx->_bb_updater->update(ctx->_delta_bb, tests_num, _bb);
    }
  } else {
    const auto & sampling_counter = *(ctx->_sampling_counter);
    const auto & delta_ww = ctx->_delta_ww;
    const auto & delta_bb = ctx->_delta_bb;
    const auto rows = delta_ww.rows();

    YANN_CHECK_EQ((MatrixSize)sampling_counter.size(), delta_ww.cols());
    for(MatrixSize ii = 0; ii < delta_ww.cols(); ++ii) {
      if(sampling_counter[ii] <= 0) {
        continue;
      }
      // can't use delta_ww.col(ii) and _ww.col(ii) here
      ctx->_ww_updater->update(
          delta_ww.block(0, ii, rows, 1),
          sampling_counter[ii],
          _ww.block(0, ii, rows, 1));
      if(!_fixed_bias) {
        ctx->_bb_updater->update(delta_bb(ii), sampling_counter[ii], _bb(ii));
      }
    }
  }
}

// the format is (w:<weights>,b:<biases>)
void yann::FullyConnectedLayer::read(std::istream & is)
{
  Base::read(is);

  read_char(is, '(');
  read_object(is, "w", _ww);
  read_char(is, ',');
  read_object(is, "b", _bb);
  read_char(is, ')');
}

// the format is (w:<weights>,b:<biases>)
void yann::FullyConnectedLayer::write(std::ostream & os) const
{
  Base::write(os);

  os << "(";
  write_object(os, "w", _ww);
  os << ",";
  write_object(os, "b", _bb);
  os << ")";
}


#pragma once

#include <boost/iterator/counting_iterator.hpp>

namespace nifty{
    namespace graph{

        ///\cond
        namespace detail_complete_graph{
            template<class NODE_TYPE>
            uint64_t n_complete_dgraph_edges(const NODE_TYPE && n_nodes, const bool self_loops){
                if(n_nodes == 0){
                    return 0;
                }
                else{
                    const auto from_self_loop = static_cast<NODE_TYPE>(self_loops) * n_nodes;
                    return ((n_nodes*(n_nodes-1))/2) + from_self_loop;
                }
            }
        }
        ///\endcond

        template<class NODE_INTEGER_TYPE, bool SELF_LOOPS = false>
        class CompleteGraph{
        public:

            typedef NODE_INTEGER_TYPE   NodeType;
            typedef uint64_t            EdgeType;

            typedef NodeType NodeConstRef;
            typedef EdgeType EdgeConstRef;

            boost::counting_iterator<NodeType> NodeIter;
            boost::counting_iterator<EdgeType> EdgeIter;

            CompleteGraph(const NodeType & n_nodes = 0)
            :   n_nodes_(n_nodes),
                n_edges_(detail_complete_graph::n_complete_dgraph_edges(n_nodes, SELF_LOOPS))
            {
            }

            NodeType n_edges()const{
                return n_edges_;
            }
            NodeType n_nodes()const{
                return n_nodes_;
            }

            NodeIter nodes_begin()const{
                return NodeIter(NodeType(0));
            }
            NodeIter nodes_end()const{
                return NodeIter(NodeType(n_nodes_));
            }
            EdgeIter edges_begin()const{
                return EdgeIter(EdgeType(0));
            }
            EdgeIter edges_end()const{
                return EdgeIter(EdgeType(n_edges_));
            }

        private:
            NodeType n_nodes_;
            EdgeType n_edges_;

        };

    }
}


#include <boost/test/unit_test.hpp>

#include <crave/utils/Evaluator.hpp>
#include <crave/ir/UserConstraint.hpp>
#include <crave/ir/UserExpression.hpp>

#include <set>
#include <iostream>

// using namespace std;
using namespace crave;

BOOST_FIXTURE_TEST_SUITE(Evaluations_t, Context_Fixture)

BOOST_AUTO_TEST_CASE(logical_not_t1) {
  crv_variable<unsigned int> a;
  Evaluator evaluator;

  evaluator.assign(a(), 0u);

  BOOST_REQUIRE(evaluator.evaluate(!(a() != 0)));
  BOOST_REQUIRE(evaluator.result<bool>());

  evaluator.assign(a(), 42u);

  BOOST_REQUIRE(evaluator.evaluate(!(a() == 0)));
  BOOST_REQUIRE(evaluator.result<bool>());
}

BOOST_AUTO_TEST_CASE(logical_not_t2) {
  crv_variable<unsigned char> a;
  crv_variable<unsigned int> b;
  Evaluator eval;
  expression expr = make_expression(if_then_else(!(a() % 2 == 0), b() > 0 && b() <= 50, b() > 50 && b() <= 100));

  eval.assign(a(), 1u);

  BOOST_REQUIRE(!eval.evaluate(expr));

  eval.assign(b(), 35u);

  BOOST_REQUIRE(eval.evaluate(expr));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(a(), 2u);
  eval.assign(b(), 75u);

  BOOST_REQUIRE(eval.evaluate(expr));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(a(), 1u);

  BOOST_REQUIRE(eval.evaluate(expr));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(logical_and_t1) {
  crv_variable<bool> a;
  crv_variable<bool> b;
  crv_variable<bool> c;
  Evaluator eval;

  eval.assign(a(), true);
  eval.assign(b(), true);

  BOOST_REQUIRE(eval.evaluate(a() && b()));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(c(), false);

  BOOST_REQUIRE(eval.evaluate(c() == (a() && b())));
  BOOST_REQUIRE(!eval.result<bool>());

  eval.assign(a(), false);
  eval.assign(b(), false);

  BOOST_REQUIRE(eval.evaluate(c() == (a() && b())));
  BOOST_REQUIRE(eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(logical_or_t1) {
  crv_variable<bool> a;
  crv_variable<bool> b;
  crv_variable<bool> c;
  Evaluator eval;

  eval.assign(a(), false);
  eval.assign(b(), false);

  BOOST_REQUIRE(eval.evaluate(a() || b()));
  BOOST_REQUIRE(!eval.result<bool>());

  eval.assign(c(), false);

  BOOST_REQUIRE(eval.evaluate(c() == (a() || b())));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(a(), true);
  eval.assign(c(), true);

  BOOST_REQUIRE(eval.evaluate(c() == (a() || b())));
  BOOST_REQUIRE(eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(equal_t1) {
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a(), 65535);

  BOOST_REQUIRE(eval.evaluate(a()));
  BOOST_REQUIRE_EQUAL(eval.result<unsigned int>(), 65535);
  BOOST_REQUIRE(eval.evaluate(a() == 65535));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(b(), 5);

  BOOST_REQUIRE(eval.evaluate(a() == b()));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(not_equal_t1) {
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a(), 25u);

  for (unsigned int i = 0; i < 50u; ++i) {
    eval.assign(b(), i);
    BOOST_REQUIRE(eval.evaluate(a() != b()));

    if (i != 25u)
      BOOST_REQUIRE(eval.result<bool>());
    else
      BOOST_REQUIRE(!eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(less) {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  Evaluator eval;

  for (unsigned int i = 0u; i < 50u; ++i) {
    eval.assign(a(), i);
    BOOST_REQUIRE(eval.evaluate(a() < 50u));
    BOOST_REQUIRE(eval.result<bool>());

    eval.assign(b(), i);
    BOOST_REQUIRE(eval.evaluate(b() < 50u));
    BOOST_REQUIRE(eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(less_equal) {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  Evaluator eval;

  for (unsigned int i = 0u; i <= 50u; ++i) {
    eval.assign(a(), i);
    BOOST_REQUIRE(eval.evaluate(a() <= 50u));
    BOOST_REQUIRE(eval.result<bool>());

    eval.assign(b(), i);
    BOOST_REQUIRE(eval.evaluate(b() <= 50u));
    BOOST_REQUIRE(eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(greater) {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  Evaluator eval;

  for (int i = 50; i > 0; --i) {
    eval.assign(a(), i);
    BOOST_REQUIRE(eval.evaluate(a() > 0u));
    BOOST_REQUIRE(eval.result<bool>());

    eval.assign(b(), i);
    BOOST_REQUIRE(eval.evaluate(b() > 0u));
    BOOST_REQUIRE(eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(greater_equal) {
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  Evaluator eval;

  for (int i = 50; i >= 0; --i) {
    eval.assign(a(), i);
    BOOST_REQUIRE(eval.evaluate(a() >= 0u));
    BOOST_REQUIRE(eval.result<bool>());

    eval.assign(b(), i);
    BOOST_REQUIRE(eval.evaluate(b() >= 0u));
    BOOST_REQUIRE(eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(neg_t1) {
  crv_variable<int> a;
  crv_variable<int> b;
  Evaluator eval;

  eval.assign(a(), 1337);
  BOOST_REQUIRE(eval.evaluate(-a() == 1337));
  BOOST_REQUIRE(!eval.result<bool>());

  eval.assign(b(), -1337);
  BOOST_REQUIRE(eval.evaluate(a() == -b()));
  BOOST_REQUIRE(eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(neg_t2) {
  int a = 1337;
  crv_variable<int> b;
  Evaluator eval;

  eval.assign(b(), -a);
  BOOST_REQUIRE(eval.evaluate(b()));
  BOOST_REQUIRE_EQUAL(eval.result<int>(), -1337);
}

BOOST_AUTO_TEST_CASE(complement_t1) {
  crv_variable<int> a;
  crv_variable<int> b;
  Evaluator eval;

  eval.assign(a(), 0);
  eval.assign(b(), -1);
  BOOST_REQUIRE(eval.evaluate(~a() == b()));
  BOOST_REQUIRE(eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(complement_t2) {
  int a = 42;
  crv_variable<int> b;
  Evaluator eval;

  eval.assign(b(), a);
  BOOST_REQUIRE(eval.evaluate(~b()));
  BOOST_REQUIRE_EQUAL(eval.result<int>(), -43);
}

BOOST_AUTO_TEST_CASE(bitwise_and_t1) {
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a(), 42);
  eval.assign(b(), 1337);

  BOOST_REQUIRE(eval.evaluate(a() & b()));
  BOOST_REQUIRE_EQUAL(eval.result<unsigned int>(), 40);
}

BOOST_AUTO_TEST_CASE(bitwise_or_t1) {
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a(), 42);
  eval.assign(b(), 1337);

  BOOST_REQUIRE(eval.evaluate(a() | b()));
  BOOST_REQUIRE_EQUAL(eval.result<unsigned int>(), 1339);
}

BOOST_AUTO_TEST_CASE(xor_t1) {
  crv_variable<bool> a;
  crv_variable<bool> b;
  Evaluator eval;

  eval.assign(a(), false);
  eval.assign(b(), false);

  BOOST_REQUIRE(eval.evaluate(a() ^ b()));
  BOOST_REQUIRE_EQUAL(eval.result<bool>(), false);

  eval.assign(b(), true);

  BOOST_REQUIRE(eval.evaluate(a() ^ b()));
  BOOST_REQUIRE_EQUAL(eval.result<bool>(), true);
}

BOOST_AUTO_TEST_CASE(xor_t2) {
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a(), 65535);
  eval.assign(b(), 4080);

  BOOST_REQUIRE(eval.evaluate(a() ^ b()));
  BOOST_REQUIRE_EQUAL(eval.result<unsigned int>(), 61455);
}

BOOST_AUTO_TEST_CASE(shiftleft) {
  crv_variable<unsigned> a;
  crv_variable<char> b;
  Evaluator eval;

  int count = 0;
  while (++count < 256) {
    eval.assign(a(), count);
    eval.assign(b(), count % (sizeof(unsigned) << 3u));

    BOOST_REQUIRE(eval.evaluate(a() << b()));
    BOOST_REQUIRE_EQUAL(eval.result<unsigned>(), count << (count % (sizeof(unsigned) << 3u)));
  }
}

BOOST_AUTO_TEST_CASE(shiftright) {
  crv_variable<unsigned> a;
  crv_variable<char> b;
  Evaluator eval;

  int count = 0;
  while (256 > ++count) {
    eval.assign(a(), count + 256);
    eval.assign(b(), count % 8);

    BOOST_REQUIRE(eval.evaluate(a() >> b()));
    BOOST_REQUIRE_EQUAL(eval.result<unsigned>(), (count + 256) >> (count % 8));
  }
}

BOOST_AUTO_TEST_CASE(plus_minus) {
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  Evaluator eval;

  unsigned int cnt = 0;
  while (cnt++ < 300) {
    eval.assign(a(), cnt * cnt);
    eval.assign(b(), cnt + cnt);

    BOOST_REQUIRE(eval.evaluate(a() + b()));
    BOOST_REQUIRE_EQUAL(eval.result<unsigned>(), (cnt * cnt) + (cnt + cnt));

    BOOST_REQUIRE(eval.evaluate(a() - b()));
    BOOST_REQUIRE_EQUAL(eval.result<unsigned>(), (cnt * cnt) - (cnt + cnt));
  }
}

BOOST_AUTO_TEST_CASE(mult_mod) {
  crv_variable<int> a;
  crv_variable<int> b;
  Evaluator eval;

  for (int i = -3; i <= 3; i++) {
    for (int j = -3; j <= 3; j++) {
      eval.assign(a(), i);
      eval.assign(b(), j);

      BOOST_REQUIRE(eval.evaluate(a() * b() % 6));
      BOOST_REQUIRE_EQUAL(eval.result<int>(), i * j % 6);
    }
  }

  eval.assign(b(), 0);

  BOOST_REQUIRE(!eval.evaluate(a() % b()));
}

BOOST_AUTO_TEST_CASE(divide) {
  crv_variable<short> a;
  crv_variable<short> b;
  Evaluator eval;

  unsigned int cnt = 1;
  while (cnt++ < 256) {
    eval.assign(a(), cnt * cnt);
    eval.assign(b(), cnt + cnt);

    BOOST_REQUIRE(eval.evaluate(a() / b()));
    BOOST_REQUIRE_EQUAL(eval.result<short>(), (cnt * cnt) / (cnt + cnt));

    BOOST_REQUIRE(eval.evaluate(a() % b()));
    BOOST_REQUIRE_EQUAL(eval.result<short>(), (cnt * cnt) % (cnt + cnt));
  }

  eval.assign(b(), 0u);
  BOOST_REQUIRE(!eval.evaluate(a() / b()));
}

BOOST_AUTO_TEST_CASE(element_inside_set) {
  std::set<unsigned> s;
  s.insert(1);
  s.insert(7);
  s.insert(9);

  crv_variable<unsigned> x;
  Evaluator eval;

  eval.assign(x(), 1);

  BOOST_REQUIRE(eval.evaluate(inside(x(), s)));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(x(), 5);

  BOOST_REQUIRE(eval.evaluate(inside(x(), s)));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(element_inside_vec) {
  std::vector<unsigned> v;
  v.push_back(1);
  v.push_back(7);
  v.push_back(9);

  crv_variable<unsigned> x;
  Evaluator eval;

  eval.assign(x(), 7u);

  BOOST_REQUIRE(eval.evaluate(inside(x(), v)));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(x(), 5u);

  BOOST_REQUIRE(eval.evaluate(inside(x(), v)));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(element_inside_array) {
  unsigned a[3];
  a[0] = 1;
  a[1] = 7;
  a[2] = 9;

  crv_variable<unsigned> x;
  Evaluator eval;

  eval.assign(x(), 9);

  BOOST_REQUIRE(eval.evaluate(inside(x(), a)));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(x(), 5u);

  BOOST_REQUIRE(eval.evaluate(inside(x(), a)));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(element_inside_list) {
  std::list<unsigned> l;
  l.push_back(1);
  l.push_back(7);
  l.push_back(9);

  crv_variable<unsigned> x;
  Evaluator eval;

  eval.assign(x(), 7u);

  BOOST_REQUIRE(eval.evaluate(inside(x(), l)));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(x(), 5u);

  BOOST_REQUIRE(eval.evaluate(inside(x(), l)));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(element_not_inside) {
  Evaluator eval;
  {
    std::set<unsigned> s;
    crv_variable<unsigned> x;
    eval.assign(x(), 1u);

    BOOST_REQUIRE(eval.evaluate(inside(x(), s)));
    BOOST_REQUIRE(!eval.result<bool>());

    s.insert(1u);

    BOOST_REQUIRE(eval.evaluate(inside(x(), s)));
    BOOST_REQUIRE(eval.result<bool>());
  }
  {
    std::vector<unsigned> v;

    crv_variable<unsigned> x;
    eval.assign(x(), 1u);

    BOOST_REQUIRE(eval.evaluate(inside(x(), v)));
    BOOST_REQUIRE(!eval.result<bool>());

    v.push_back(1u);

    BOOST_REQUIRE(eval.evaluate(inside(x(), v)));
    BOOST_REQUIRE(eval.result<bool>());
  }
  {
    std::vector<unsigned> l;
    crv_variable<unsigned> x;
    eval.assign(x(), 1u);

    BOOST_REQUIRE(eval.evaluate(inside(x(), l)));
    BOOST_REQUIRE(!eval.result<bool>());

    l.push_back(1u);

    BOOST_REQUIRE(eval.evaluate(inside(x(), l)));
    BOOST_REQUIRE(eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(if_then_else_t1) {
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  Evaluator eval;
  expression expr = make_expression(if_then_else(a()<5, b()> 0 && b() <= 50, b() > 50 && b() <= 100));

  for (int i = 0; i < 10; ++i) {
    eval.assign(a(), i);
    eval.assign(b(), 25);
    BOOST_REQUIRE(eval.evaluate(expr));

    if (i < 5) {
      BOOST_REQUIRE(eval.result<bool>());
    } else {
      BOOST_REQUIRE(!eval.result<bool>());
    }
    eval.assign(b(), 75);
    BOOST_REQUIRE(eval.evaluate(expr));

    if (i < 5) {
      BOOST_REQUIRE(!eval.result<bool>());
    } else {
      BOOST_REQUIRE(eval.result<bool>());
    }
  }
}

BOOST_AUTO_TEST_CASE(if_then_t1) {
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  Evaluator eval;
  expression expr = make_expression(if_then(a()<5, b()> 0 && b() <= 100));

  for (int i = 0; i < 10; ++i) {
    eval.assign(a(), i);
    eval.assign(b(), 25u);
    BOOST_REQUIRE(eval.evaluate(expr));
    BOOST_REQUIRE(eval.result<bool>());

    eval.assign(b(), 705u);
    BOOST_REQUIRE(eval.evaluate(expr));

    if (i < 5) {
      BOOST_REQUIRE(!eval.result<bool>());
    } else {
      BOOST_REQUIRE(eval.result<bool>());
    }
  }
}

BOOST_AUTO_TEST_CASE(equal_t2) {
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a(), 1);
  eval.assign(b(), 2);

  BOOST_REQUIRE(eval.evaluate(a() == b()));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(equal_t3) {
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  crv_variable<unsigned int> c;
  Evaluator eval;

  eval.assign(a(), 1);
  eval.assign(b(), 2);
  eval.assign(c(), 3);

  BOOST_REQUIRE(eval.evaluate(a() + b() == c()));
  BOOST_REQUIRE(eval.result<bool>());
}

BOOST_AUTO_TEST_SUITE_END()  // Evaluations