////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/// @project        Open Space Toolkit ▸ Mathematics
/// @file           OpenSpaceToolkit/Mathematics/Geometry/3D/Objects/Ellipsoid.cpp
/// @author         Lucas Brémond <lucas@loftorbital.com>
/// @license        Apache License 2.0

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Transformations/Rotations/RotationMatrix.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Transformation.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Intersection.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Objects/Cone.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Objects/Pyramid.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Objects/Ellipsoid.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Objects/Sphere.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Objects/Segment.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Objects/Ray.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Objects/Line.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/3D/Objects/PointSet.hpp>
#include <OpenSpaceToolkit/Mathematics/Objects/Interval.hpp>

#include <OpenSpaceToolkit/Core/Error.hpp>
#include <OpenSpaceToolkit/Core/Utilities.hpp>

#include <Gte/Mathematics/GteIntrEllipsoid3Ellipsoid3.h>
// #include <Gte/Mathematics/GteIntrHalfspace3Ellipsoid3.h>
#include <Gte/Mathematics/GteIntrPlane3Ellipsoid3.h>
#include <Gte/Mathematics/GteIntrSegment3Ellipsoid3.h>
#include <Gte/Mathematics/GteIntrRay3Ellipsoid3.h>
#include <Gte/Mathematics/GteIntrLine3Ellipsoid3.h>

// Disable Eigen warnings

#pragma GCC diagnostic push // Save diagnostic state

#pragma GCC diagnostic ignored "-Wshadow"
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wint-in-bool-context"

#include <Eigen/Eigenvalues>

#pragma GCC diagnostic pop // Turn the warnings back on

#include <math.h>

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

namespace ostk
{
namespace math
{
namespace geom
{
namespace d3
{
namespace objects
{

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

gte::Vector3<double>            EllipsoidGteVectorFromPoint                 (   const   Point&                      aPoint                                      )
{
    return { aPoint.x(), aPoint.y(), aPoint.z() } ;
}

gte::Vector3<double>            EllipsoidGteVectorFromVector3d              (   const   Vector3d&                   aVector                                     )
{
    return { aVector.x(), aVector.y(), aVector.z() } ;
}

Point                           EllipsoidPointFromGteVector                 (   const   gte::Vector3<double>&       aVector                                     )
{
    return { aVector[0], aVector[1], aVector[2] } ;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                                Ellipsoid::Ellipsoid                        (   const   Point&                      aCenter,
                                                                                const   Real&                       aFirstPrincipalSemiAxis,
                                                                                const   Real&                       aSecondPrincipalSemiAxis,
                                                                                const   Real&                       aThirdPrincipalSemiAxis,
                                                                                const   Quaternion&                 anOrientation                               )
                                :   Object(),
                                    center_(aCenter),
                                    a_(aFirstPrincipalSemiAxis),
                                    b_(aSecondPrincipalSemiAxis),
                                    c_(aThirdPrincipalSemiAxis),
                                    q_(anOrientation)
{

    if (a_.isDefined() && (a_ < 0.0))
    {
        throw ostk::core::error::RuntimeError("First principal semi-axis is negative.") ;
    }

    if (b_.isDefined() && (b_ < 0.0))
    {
        throw ostk::core::error::RuntimeError("Second principal semi-axis is negative.") ;
    }

    if (c_.isDefined() && (c_ < 0.0))
    {
        throw ostk::core::error::RuntimeError("Third principal semi-axis is negative.") ;
    }

}

Ellipsoid*                      Ellipsoid::clone                            ( ) const
{
    return new Ellipsoid(*this) ;
}

bool                            Ellipsoid::operator ==                      (   const   Ellipsoid&                  anEllipsoid                                 ) const
{

    if ((!this->isDefined()) || (!anEllipsoid.isDefined()))
    {
        return false ;
    }

    if (center_ == anEllipsoid.center_)
    {

        if ((a_ == anEllipsoid.a_) && (b_ == anEllipsoid.b_) && (c_ == anEllipsoid.c_) && (q_ == anEllipsoid.q_))
        {
            return true ;
        }

        return this->getMatrix().isNear(anEllipsoid.getMatrix(), Real::Epsilon()) ;

    }

    return false ;

}

bool                            Ellipsoid::operator !=                      (   const   Ellipsoid&                  anEllipsoid                                 ) const
{
    return !((*this) == anEllipsoid) ;
}

bool                            Ellipsoid::isDefined                        ( ) const
{
    return center_.isDefined() && a_.isDefined() && b_.isDefined() && c_.isDefined() && q_.isDefined() ;
}

bool                            Ellipsoid::intersects                       (   const   Point&                      aPoint                                      ) const
{
    return this->contains(aPoint) ;
}

bool                            Ellipsoid::intersects                       (   const   PointSet&                   aPointSet                                   ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    return (!aPointSet.isEmpty()) && std::any_of(aPointSet.begin(), aPointSet.end(), [this] (const Point& aPoint) -> bool { return this->contains(aPoint) ; }) ;

}

bool                            Ellipsoid::intersects                       (   const   Line&                       aLine                                       ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    if (!aLine.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Line") ;
    }

    // Line

    const gte::Line3<double> line = { EllipsoidGteVectorFromPoint(aLine.getOrigin()), EllipsoidGteVectorFromVector3d(aLine.getDirection()) } ;

    // Ellipsoid

    const gte::Vector3<double> center = EllipsoidGteVectorFromPoint(center_) ;
    const std::array<gte::Vector3<double>, 3> axes = { EllipsoidGteVectorFromVector3d(this->getFirstAxis()), EllipsoidGteVectorFromVector3d(this->getSecondAxis()), EllipsoidGteVectorFromVector3d(this->getThirdAxis()) } ;
    const gte::Vector3<double> extent = { a_, b_, c_ } ;

    const gte::Ellipsoid3<double> ellipsoid = { center, axes, extent } ;

    // Intersection

    gte::TIQuery<double, gte::Line3<double>, gte::Ellipsoid3<double>> intersectionQuery ;

    auto intersectionResult = intersectionQuery(line, ellipsoid) ;

    return intersectionResult.intersect ;

}

bool                            Ellipsoid::intersects                       (   const   Ray&                        aRay                                        ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    if (!aRay.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ray") ;
    }

    // Ray

    const gte::Ray3<double> ray = { EllipsoidGteVectorFromPoint(aRay.getOrigin()), EllipsoidGteVectorFromVector3d(aRay.getDirection()) } ;

    // Ellipsoid

    const gte::Vector3<double> center = EllipsoidGteVectorFromPoint(center_) ;
    const std::array<gte::Vector3<double>, 3> axes = { EllipsoidGteVectorFromVector3d(this->getFirstAxis()), EllipsoidGteVectorFromVector3d(this->getSecondAxis()), EllipsoidGteVectorFromVector3d(this->getThirdAxis()) } ;
    const gte::Vector3<double> extent = { a_, b_, c_ } ;

    const gte::Ellipsoid3<double> ellipsoid = { center, axes, extent } ;

    // Intersection

    gte::TIQuery<double, gte::Ray3<double>, gte::Ellipsoid3<double>> intersectionQuery ;

    auto intersectionResult = intersectionQuery(ray, ellipsoid) ;

    return intersectionResult.intersect ;

}

bool                            Ellipsoid::intersects                       (   const   Segment&                    aSegment                                    ) const
{

    using ostk::math::obj::Interval ;

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    if (!aSegment.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Segment") ;
    }

    // // Does not work for some reason... most likely a bug w/ gte::TIQuery<double, gte::Segment3<double>, gte::Ellipsoid3<double>>

    // // Segment

    // const gte::Segment3<double> segment = { EllipsoidGteVectorFromVector3d(aSegment.getFirstPoint()), EllipsoidGteVectorFromVector3d(aSegment.getSecondPoint()) } ;

    // // Ellipsoid

    // const gte::Vector3<double> center = EllipsoidGteVectorFromVector3d(center_) ;
    // const std::array<gte::Vector3<double>, 3> axes = { EllipsoidGteVectorFromVector3d(this->getFirstAxis()), EllipsoidGteVectorFromVector3d(this->getSecondAxis()), EllipsoidGteVectorFromVector3d(this->getThirdAxis()) } ;
    // const gte::Vector3<double> extent = { a_, b_, c_ } ;

    // const gte::Ellipsoid3<double> ellipsoid = { center, axes, extent } ;

    // // Intersection

    // gte::TIQuery<double, gte::Segment3<double>, gte::Ellipsoid3<double>> intersectionQuery ;

    // auto intersectionResult = intersectionQuery(segment, ellipsoid) ;

    // return intersectionResult.intersect ;

    if (aSegment.isDegenerate())
    {
        return this->contains(aSegment.getFirstPoint()) ;
    }

    // https://www.geometrictools.com/GTEngine/Include/Mathematics/GteIntrSegment3Ellipsoid3.h

    const Vector3d segmentDirection = aSegment.getDirection() ;
    const Vector3d segmentCenter = aSegment.getCenter().asVector() ;
    const Real segmentHalfLength = aSegment.getLength() / 2.0 ;

    const Matrix3d M = this->getMatrix() ;

    const Vector3d diff = segmentCenter - center_.asVector() ;
    const Vector3d matDir = M * segmentDirection ;
    const Vector3d matDiff = M * diff ;

    const Real a2 = segmentDirection.dot(matDir) ;
    const Real a1 = segmentDirection.dot(matDiff) ;
    const Real a0 = diff.dot(matDiff) - 1.0 ;

    const Real discriminant = (a1 * a1) - (a0 * a2) ;

    static const Real tolerance = 1e-25 ; // [TBM] Tolerance parameter should be dynamic

    if (discriminant < -tolerance) // No real roots
    {
        return false ;
    }
    else if (discriminant > tolerance) // Two real roots
    {

        const Real discriminantRoot = std::sqrt(discriminant) ;
        const Real a2_inverse = 1.0 / a2 ;

        const Real t0 = (-a1 - discriminantRoot) * a2_inverse ;
        const Real t1 = (-a1 + discriminantRoot) * a2_inverse ;

        const Interval<Real> resultInterval = Interval<Real>::Open(t0, t1) ;
        const Interval<Real> segmentInterval = Interval<Real>::Closed(-segmentHalfLength, +segmentHalfLength) ;

        if (!resultInterval.contains(segmentInterval))
        {

            if (resultInterval.intersects(segmentInterval))
            {
                return true ;
            }
            else // No intersection
            {
                return false ;
            }

        }
        else
        {
            return false ;
        }

    }
    else // One real root
    {

        const Real t0 = -a1 / a2 ;

        if (std::abs(t0) <= segmentHalfLength) // Single intersection
        {
            return true ;
        }
        else // No intersection
        {
            return false ;
        }

    }

    return false ;

}

bool                            Ellipsoid::intersects                       (   const   Plane&                      aPlane                                      ) const
{

    // https://file.scirp.org/pdf/AM20121100009_89014420.pdf
    // https://www.researchgate.net/publication/312384498_Intersection_of_an_Ellipsoid_and_a_Plane

    if (!aPlane.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Plane") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    // Plane

    const gte::Vector3<double> normal = EllipsoidGteVectorFromVector3d(aPlane.getNormalVector()) ;
    const gte::Vector3<double> point = EllipsoidGteVectorFromPoint(aPlane.getPoint()) ;

    const gte::Plane3<double> plane = { normal, point } ;

    // Ellipsoid

    const gte::Vector3<double> center = EllipsoidGteVectorFromPoint(center_) ;
    const std::array<gte::Vector3<double>, 3> axes = { EllipsoidGteVectorFromVector3d(this->getFirstAxis()), EllipsoidGteVectorFromVector3d(this->getSecondAxis()), EllipsoidGteVectorFromVector3d(this->getThirdAxis()) } ;
    const gte::Vector3<double> extent = { a_, b_, c_ } ;

    const gte::Ellipsoid3<double> ellipsoid = { center, axes, extent } ;

    // Intersection

    gte::TIQuery<double, gte::Plane3<double>, gte::Ellipsoid3<double>> intersectionQuery ;

    auto intersectionResult = intersectionQuery(plane, ellipsoid) ;

    return intersectionResult.intersect ;

}

// bool                            Ellipsoid::intersects                       (   const   Sphere&                     aSphere                                     ) const
// {

//     if (!aSphere.isDefined())
//     {
//         throw ostk::core::error::runtime::Undefined("Sphere") ;
//     }

//     if (!this->isDefined())
//     {
//         throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
//     }

//     return this->intersects(Ellipsoid(aSphere.getCenter(), aSphere.getRadius(), aSphere.getRadius(), aSphere.getRadius())) ;

// }

// bool                            Ellipsoid::intersects                       (   const   Ellipsoid&                  anEllipsoid                                 ) const
// {

//     if (!anEllipsoid.isDefined())
//     {
//         throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
//     }

//     if (!this->isDefined())
//     {
//         throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
//     }

//     // Ellipsoid

//     const gte::Vector3<double> center = EllipsoidGteVectorFromPoint(center_) ;
//     const std::array<gte::Vector3<double>, 3> axes = { EllipsoidGteVectorFromVector3d(this->getFirstAxis()), EllipsoidGteVectorFromVector3d(this->getSecondAxis()), EllipsoidGteVectorFromVector3d(this->getThirdAxis()) } ;
//     const gte::Vector3<double> extent = { a_, b_, c_ } ;

//     const gte::Ellipsoid3<double> ellipsoid = { center, axes, extent } ;

//     // Another ellipsoid

//     const gte::Vector3<double> anotherCenter = EllipsoidGteVectorFromPoint(anEllipsoid.center_) ;
//     const std::array<gte::Vector3<double>, 3> anotherAxes = { EllipsoidGteVectorFromVector3d(anEllipsoid.getFirstAxis()), EllipsoidGteVectorFromVector3d(anEllipsoid.getSecondAxis()), EllipsoidGteVectorFromVector3d(anEllipsoid.getThirdAxis()) } ;
//     const gte::Vector3<double> anotherExtent = { anEllipsoid.a_, anEllipsoid.b_, anEllipsoid.c_ } ;

//     const gte::Ellipsoid3<double> anotherEllipsoid = { anotherCenter, anotherAxes, anotherExtent } ;

//     // Intersection

//     gte::TIQuery<double, gte::Ellipsoid3<double>, gte::Ellipsoid3<double>> intersectionQuery ;

//     auto intersectionResult = intersectionQuery(ellipsoid, anotherEllipsoid) ;

//     return intersectionResult.intersect ;

// }

bool                            Ellipsoid::intersects                       (   const   Pyramid&                    aPyramid                                    ) const
{
    return aPyramid.intersects(*this) ;
}

bool                            Ellipsoid::intersects                       (   const   Cone&                       aCone                                       ) const
{
    return aCone.intersects(*this) ;
}

bool                            Ellipsoid::contains                         (   const   Point&                      aPoint                                      ) const
{

    using ostk::math::obj::Vector3d ;

    if (!aPoint.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Point") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    Matrix3d dcm ;

    dcm.row(0) = q_ * Vector3d::X() ;
    dcm.row(1) = q_ * Vector3d::Y() ;
    dcm.row(2) = q_ * Vector3d::Z() ;

    const Vector3d point = dcm * (aPoint - center_) ;

    const Real& x = point.x() ;
    const Real& y = point.y() ;
    const Real& z = point.z() ;

    return std::abs((x * x) / (a_ * a_) + (y * y) / (b_ * b_) + (z * z) / (c_ * c_) - 1.0) < Real::Epsilon() ;

}

bool                            Ellipsoid::contains                         (   const   PointSet&                   aPointSet                                   ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    return (!aPointSet.isEmpty()) && std::all_of(aPointSet.begin(), aPointSet.end(), [this] (const Point& aPoint) -> bool { return this->contains(aPoint) ; }) ;

}

bool                            Ellipsoid::contains                         (   const   Segment&                    aSegment                                    ) const
{
    return this->contains(aSegment.getFirstPoint()) && this->contains(aSegment.getSecondPoint()) ;
}

Point                           Ellipsoid::getCenter                        ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    return center_ ;

}

Real                            Ellipsoid::getFirstPrincipalSemiAxis        ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    return a_ ;

}

Real                            Ellipsoid::getSecondPrincipalSemiAxis       ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    return b_ ;

}

Real                            Ellipsoid::getThirdPrincipalSemiAxis        ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    return c_ ;

}

Vector3d                        Ellipsoid::getFirstAxis                     ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    return q_.toConjugate() * Vector3d::X() ;

}

Vector3d                        Ellipsoid::getSecondAxis                    ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    return q_.toConjugate() * Vector3d::Y() ;

}

Vector3d                        Ellipsoid::getThirdAxis                     ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    return q_.toConjugate() * Vector3d::Z() ;

}

Quaternion                      Ellipsoid::getOrientation                   ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    return q_ ;

}

Matrix3d                        Ellipsoid::getMatrix                        ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    const Vector3d firstRatio = this->getFirstAxis() / a_ ;
	const Vector3d secondRatio = this->getSecondAxis() / b_ ;
	const Vector3d thirdRatio = this->getThirdAxis() / c_ ;

    auto tensorProduct = [] (const Vector3d& aFirstVector, const Vector3d& aSecondVector) -> Matrix3d
    {

        Matrix3d tensorProductMatrix ;

        tensorProductMatrix <<  aFirstVector(0) * aSecondVector(0), aFirstVector(0) * aSecondVector(1), aFirstVector(0) * aSecondVector(2),
                                aFirstVector(1) * aSecondVector(0), aFirstVector(1) * aSecondVector(1), aFirstVector(1) * aSecondVector(2),
                                aFirstVector(2) * aSecondVector(0), aFirstVector(2) * aSecondVector(1), aFirstVector(2) * aSecondVector(2) ;

        return tensorProductMatrix ;

    } ;

	return tensorProduct(firstRatio, firstRatio) + tensorProduct(secondRatio, secondRatio) + tensorProduct(thirdRatio, thirdRatio) ;

}

Intersection                    Ellipsoid::intersectionWith                 (   const   Line&                       aLine                                       ) const
{

    using ostk::math::geom::d3::objects::PointSet ;

    if (!aLine.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Line") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    // Line

    const gte::Line3<double> segment = { EllipsoidGteVectorFromPoint(aLine.getOrigin()), EllipsoidGteVectorFromVector3d(aLine.getDirection()) } ;

    // Ellipsoid

    const gte::Vector3<double> center = EllipsoidGteVectorFromPoint(center_) ;
    const std::array<gte::Vector3<double>, 3> axes = { EllipsoidGteVectorFromVector3d(this->getFirstAxis()), EllipsoidGteVectorFromVector3d(this->getSecondAxis()), EllipsoidGteVectorFromVector3d(this->getThirdAxis()) } ;
    const gte::Vector3<double> extent = { a_, b_, c_ } ;

    const gte::Ellipsoid3<double> ellipsoid = { center, axes, extent } ;

    // Intersection

    gte::FIQuery<double, gte::Line3<double>, gte::Ellipsoid3<double>> intersectionQuery ;

    auto intersectionResult = intersectionQuery(segment, ellipsoid) ;

    if (intersectionResult.intersect)
    {

        if (intersectionResult.numIntersections == 1)
        {
            return Intersection::Point(EllipsoidPointFromGteVector(intersectionResult.point[0])) ;
        }
        else if (intersectionResult.numIntersections == 2)
        {
            return Intersection::PointSet(PointSet({ Point(EllipsoidPointFromGteVector(intersectionResult.point[0])), Point(EllipsoidPointFromGteVector(intersectionResult.point[1])) })) ;
        }
        else
        {
            throw ostk::core::error::RuntimeError("Intersection algorithm has failed.") ;
        }

    }

    return Intersection::Empty() ;

}

Intersection                    Ellipsoid::intersectionWith                 (   const   Ray&                        aRay,
                                                                                const   bool                        onlyInSight                                 ) const
{

    using ostk::math::geom::d3::objects::PointSet ;

    if (!aRay.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ray") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    // Ray

    const gte::Ray3<double> ray = { EllipsoidGteVectorFromPoint(aRay.getOrigin()), EllipsoidGteVectorFromVector3d(aRay.getDirection()) } ;

    // Ellipsoid

    const gte::Vector3<double> center = EllipsoidGteVectorFromPoint(center_) ;
    const std::array<gte::Vector3<double>, 3> axes = { EllipsoidGteVectorFromVector3d(this->getFirstAxis()), EllipsoidGteVectorFromVector3d(this->getSecondAxis()), EllipsoidGteVectorFromVector3d(this->getThirdAxis()) } ;
    const gte::Vector3<double> extent = { a_, b_, c_ } ;

    const gte::Ellipsoid3<double> ellipsoid = { center, axes, extent } ;

    // Intersection

    gte::FIQuery<double, gte::Ray3<double>, gte::Ellipsoid3<double>> intersectionQuery ;

    auto intersectionResult = intersectionQuery(ray, ellipsoid) ;

    if (intersectionResult.intersect)
    {

        if (intersectionResult.numIntersections == 1)
        {

            const Point point = EllipsoidPointFromGteVector(intersectionResult.point[0]) ;

            if ((point == aRay.getOrigin()) && (!this->contains(point))) // Discard ray origin, if returned by Gte
            {
                return Intersection::Empty() ;
            }

            return Intersection::Point(point) ;

        }
        else if (intersectionResult.numIntersections == 2)
        {

            const Point firstPoint = EllipsoidPointFromGteVector(intersectionResult.point[0]) ;
            const Point secondPoint = EllipsoidPointFromGteVector(intersectionResult.point[1]) ;

            if ((firstPoint == aRay.getOrigin()) || (secondPoint == aRay.getOrigin()))
            {

                if ((firstPoint == aRay.getOrigin()) && (!this->contains(firstPoint))) // Discard ray origin, if returned by Gte
                {
                    return Intersection::Point(secondPoint) ;
                }

                if ((secondPoint == aRay.getOrigin()) && (!this->contains(secondPoint))) // Discard ray origin, if returned by Gte
                {
                    return Intersection::Point(firstPoint) ;
                }

            }

            if ((firstPoint - secondPoint).norm() < Real::Epsilon())
            {
                return Intersection::Point(firstPoint) ;
            }

            const PointSet pointSet = { { firstPoint, secondPoint } } ;

            return onlyInSight ? Intersection::Point(pointSet.getPointClosestTo(aRay.getOrigin())) : Intersection::PointSet(pointSet) ;

        }
        else
        {
            throw ostk::core::error::RuntimeError("Intersection algorithm has failed.") ;
        }

    }

    return Intersection::Empty() ;

}

Intersection                    Ellipsoid::intersectionWith                 (   const   Segment&                    aSegment                                    ) const
{

    using ostk::math::geom::d3::objects::PointSet ;

    if (!aSegment.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Segment") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    if (aSegment.isDegenerate() && this->contains(aSegment.getFirstPoint()))
    {
        return Intersection::Point(aSegment.getFirstPoint()) ;
    }

    // Segment

    const gte::Segment3<double> segment = { EllipsoidGteVectorFromPoint(aSegment.getFirstPoint()), EllipsoidGteVectorFromPoint(aSegment.getSecondPoint()) } ; ;

    // Ellipsoid

    const gte::Vector3<double> center = EllipsoidGteVectorFromPoint(center_) ;
    const std::array<gte::Vector3<double>, 3> axes = { EllipsoidGteVectorFromVector3d(this->getFirstAxis()), EllipsoidGteVectorFromVector3d(this->getSecondAxis()), EllipsoidGteVectorFromVector3d(this->getThirdAxis()) } ;
    const gte::Vector3<double> extent = { a_, b_, c_ } ;

    const gte::Ellipsoid3<double> ellipsoid = { center, axes, extent } ;

    // Intersection

    gte::FIQuery<double, gte::Segment3<double>, gte::Ellipsoid3<double>> intersectionQuery ;

    auto intersectionResult = intersectionQuery(segment, ellipsoid) ;

    if (intersectionResult.intersect)
    {

        if (intersectionResult.numIntersections == 1)
        {

            const Point point = EllipsoidPointFromGteVector(intersectionResult.point[0]) ;

            if (isnan(point.x()) || isnan(point.y()) || isnan(point.z()))
            {
                return Intersection::Empty() ;
            }

            if ((!point.isDefined()) || ((point == aSegment.getFirstPoint() || (point == aSegment.getSecondPoint())) && (!this->contains(point)))) // Discard segment points, if returned by Gte
            {
                return Intersection::Empty() ;
            }

            return Intersection::Point(point) ;

        }
        else if (intersectionResult.numIntersections == 2)
        {

            const Point firstPoint = EllipsoidPointFromGteVector(intersectionResult.point[0]) ;
            const Point secondPoint = EllipsoidPointFromGteVector(intersectionResult.point[1]) ;

            if ((firstPoint == aSegment.getFirstPoint()) || (secondPoint == aSegment.getFirstPoint()) || (firstPoint == aSegment.getSecondPoint()) || (secondPoint == aSegment.getSecondPoint())) // Discard segment points, if returned by Gte
            {

                if ((firstPoint == aSegment.getFirstPoint()) && (!this->contains(firstPoint)))
                {

                    if ((secondPoint == aSegment.getSecondPoint()) && (!this->contains(secondPoint)))
                    {
                        return Intersection::Empty() ;
                    }

                    return Intersection::Point(secondPoint) ;

                }

                if ((firstPoint == aSegment.getSecondPoint()) && (!this->contains(firstPoint)))
                {

                    if ((secondPoint == aSegment.getFirstPoint()) && (!this->contains(secondPoint)))
                    {
                        return Intersection::Empty() ;
                    }

                    return Intersection::Point(secondPoint) ;

                }

                if ((secondPoint == aSegment.getFirstPoint()) && (!this->contains(secondPoint)))
                {

                    if ((firstPoint == aSegment.getSecondPoint()) && (!this->contains(firstPoint)))
                    {
                        return Intersection::Empty() ;
                    }

                    return Intersection::Point(firstPoint) ;

                }

                if ((secondPoint == aSegment.getSecondPoint()) && (!this->contains(secondPoint)))
                {

                    if ((firstPoint == aSegment.getFirstPoint()) && (!this->contains(firstPoint)))
                    {
                        return Intersection::Empty() ;
                    }

                    return Intersection::Point(firstPoint) ;

                }

            }

            return Intersection::PointSet(PointSet({ firstPoint, secondPoint })) ;

        }
        else
        {
            throw ostk::core::error::RuntimeError("Intersection algorithm has failed.") ;
        }

    }

    return Intersection::Empty() ;

}

Intersection                    Ellipsoid::intersectionWith                 (   const   Pyramid&                    aPyramid,
                                                                                const   bool                        onlyInSight                                 ) const
{
    return aPyramid.intersectionWith(*this, onlyInSight) ;
}

Intersection                    Ellipsoid::intersectionWith                 (   const   Cone&                       aCone,
                                                                                const   bool                        onlyInSight                                 ) const
{
    return aCone.intersectionWith(*this, onlyInSight) ;
}

void                            Ellipsoid::print                            (           std::ostream&               anOutputStream,
                                                                                bool                        displayDecorators                           ) const
{

    displayDecorators ? ostk::core::utils::Print::Header(anOutputStream, "Ellipsoid") : void () ;

    ostk::core::utils::Print::Line(anOutputStream) << "Center:"              << (center_.isDefined() ? center_.toString() : "Undefined") ;

    ostk::core::utils::Print::Line(anOutputStream) << "First principal semi-axis:" << (a_.isDefined() ? a_.toString() : "Undefined") ;
    ostk::core::utils::Print::Line(anOutputStream) << "Second principal semi-axis:" << (b_.isDefined() ? b_.toString() : "Undefined") ;
    ostk::core::utils::Print::Line(anOutputStream) << "Third principal semi-axis:" << (c_.isDefined() ? c_.toString() : "Undefined") ;

    ostk::core::utils::Print::Line(anOutputStream) << "First axis:"          << (q_.isDefined() ? this->getFirstAxis().toString() : "Undefined") ;
    ostk::core::utils::Print::Line(anOutputStream) << "Second axis:"         << (q_.isDefined() ? this->getSecondAxis().toString() : "Undefined") ;
    ostk::core::utils::Print::Line(anOutputStream) << "Third axis:"          << (q_.isDefined() ? this->getThirdAxis().toString() : "Undefined") ;

    ostk::core::utils::Print::Line(anOutputStream) << "Orientation:"         << (q_.isDefined() ? q_.toString() : "Undefined") ;

    displayDecorators ? ostk::core::utils::Print::Footer(anOutputStream) : void () ;

}

void                            Ellipsoid::applyTransformation              (   const   Transformation&             aTransformation                             )
{

    using ostk::math::geom::d3::trf::rot::RotationMatrix ;

    if (!aTransformation.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Transformation") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Ellipsoid") ;
    }

    if (aTransformation.isIdentity())
    {
        return ;
    }

    center_.applyTransformation(aTransformation) ;

    const Matrix3d rotationMatrix = aTransformation.getMatrix().block<3, 3>(0, 0).inverse() ;
    const Matrix3d transformedMatrix = rotationMatrix.transpose() * this->getMatrix() * rotationMatrix ;

    Eigen::SelfAdjointEigenSolver<Matrix3d> eigenSolver(transformedMatrix) ;

    if (eigenSolver.info() != Eigen::Success)
    {
        throw ostk::core::error::RuntimeError("Eigen vector calculation has failed.") ;
    }

    a_ = std::sqrt(1.0 / eigenSolver.eigenvalues()(0)) ;
    b_ = std::sqrt(1.0 / eigenSolver.eigenvalues()(1)) ;
    c_ = std::sqrt(1.0 / eigenSolver.eigenvalues()(2)) ;

    const Vector3d firstAxis = eigenSolver.eigenvectors().col(0) ;
    const Vector3d secondAxis = eigenSolver.eigenvectors().col(1) ;
    const Vector3d thirdAxis = firstAxis.cross(secondAxis) ;

    q_ = Quaternion::RotationMatrix(RotationMatrix::Columns(firstAxis, secondAxis, thirdAxis)).conjugate() ;

}

Ellipsoid                       Ellipsoid::Undefined                        ( )
{
    return { Point::Undefined(), Real::Undefined(), Real::Undefined(), Real::Undefined(), Quaternion::Undefined() } ;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

}
}
}
}
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


#pragma once

#include <algorithm>
#include <cassert>
#include <vector>

#include <Eigen/Core>

#include "cho_util/core/random.hpp"

namespace cho {
namespace core {

void GenerateConvexPolygon(
    const int n, std::vector<Eigen::Vector2f>* const points,
    cho::core::RNG& rng = cho::core::RNG::GetInstance()) {
  assert(n > 2);

  std::vector<float> xv, yv;
  for (auto& v_out : {&xv, &yv}) {
    float v0{1}, v1{1};
    v_out->clear();
    v_out->reserve(n);
    for (int i = 0; i < n - 2; ++i) {
      const bool sel = rng.Uniform(0, 1) < 0.5f;
      const float v = rng.Uniform(0, 1);
      if (sel) {
        v_out->emplace_back(v - v0);
        v0 = v;
      } else {
        v_out->emplace_back(v1 - v);
        v1 = v;
      }
    }
    v_out->emplace_back(v1 - 0);
    v_out->emplace_back(0 - v0);
  }
  std::shuffle(yv.begin(), yv.end(), rng.generator);

  // Compute angles of each cumulative vector.
  std::vector<float> hv(n);
  for (int i = 0; i < n; ++i) {
    hv[i] = std::atan2(yv[i], xv[i]);
  }

  // Argsort by angle.
  std::vector<int> indices(n);
  std::iota(indices.begin(), indices.end(), 0);
  std::sort(indices.begin(), indices.end(),
            [&hv](const int i0, const int i1) { return hv[i0] < hv[i1]; });

  // Accumulative vector sum into output.
  Eigen::Vector2f point{0, 0};
  points->clear();
  points->reserve(n);
  for (int i = 0; i < xv.size(); ++i) {
    point.x() += xv[indices[i]];
    point.y() += yv[indices[i]];
    points->emplace_back(point);
  }
}

}  // namespace core
}  // namespace cho


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2019 Prashant K. Jha
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
////////////////////////////////////////////////////////////////////////////////

#ifndef UTILS_H
#define UTILS_H

#include "utilCompare.hpp"
#include "utilIO.hpp"
#include "utilLibs.hpp"
#include <boost/random.hpp>

//#include "../../external/gmm/gmm.h"

#define X_POSITIVE_BOUNDARY 0
#define X_NEGATIVE_BOUNDARY 1
#define Y_POSITIVE_BOUNDARY 2
#define Y_NEGATIVE_BOUNDARY 3
#define Z_POSITIVE_BOUNDARY 4
#define Z_NEGATIVE_BOUNDARY 5

#define OUTER_BOUNDARY_NODE 0
#define INNER_BOUNDARY_NODE 1
#define INNER_NETWORK_NODE 2

namespace util {

/*!
 * @brief Return the Square of numbers and ignores values lower than 0 and
 * higher than 1
 *
 * @param x  Number
 * @return value Value between 0 and 1
 */
double square(double x);

/*!
 * @brief Ignores values lower than 0 and higher than 1
 *
 * @param x  Number
 * @return value Value between 0 and 1
 */
double linear(double x);

/*!
 * @brief Regularized Heaviside function
 *
 * @param x  Number
 * @return value Heaveside function
 */
double heaviside(double x);

/*!
 * @brief Modulus of a vector (norm)
 *
 * @param xg X coordinate
 * @param yg Y coordinate
 * @return value Norm
 */
double modulus(double xg, double yg);

/*!
 * @brief Determinant of matrix
 *
 * @param M Matrix
 * @return value Determinant of M
 */
double determinant(const std::vector<std::vector<double>> &M);

/*!
 * @brief Inverse of matrix
 *
 * @param M Matrix
 * @return Minv Inverse of matrix M
 */
std::vector<std::vector<double>> inverse(const std::vector<std::vector<double>> &M);

/*!
 * @brief Transpose of matrix
 *
 * @param M Matrix
 * @return Mt Transpose of matrix M
 */
std::vector<std::vector<double>> transpose(const std::vector<std::vector<double>> &M);

/*!
 * @brief Add element to list
 *
 * @param i Element to be added
 * @param data List
 * @return true True if i is added to list
 */
bool addToList(unsigned int i, std::vector<unsigned int> *data);

/*!
 * @brief compute the mass of the qoi
 *
 * @param es Equation system
 * @param system_name Name of system
 * @param var_name Name of variable
 * @param value_mass Value of mass
 */
void computeMass(EquationSystems &es, const std::string &system_name,
                 const std::string &var_name, double &value_mass);

/*! @brief Position index of given point in 2d
   * @param x X coordinate of point
   * @param y Y coordinate of point
   * @param Nelx Number of elements in along x-axis (and y-axis)
   * @return Index Unique integer corresponding to the point
 */
int positionIndex(double x, double y, int Nelx);

/*!
 * @brief Projects concentration to physical range i.e. [0,1]
 *
 * @param x Number
 * @return value Value between 0 and 1
 */
double project_concentration(double x);

/*!
 * @brief Find node closer to given point
 *
 * @param x Point
 * @param mesh Mesh
 * @return id Id of node
 */
unsigned int locate_node(const Point &x, const MeshBase &mesh);

/*!
 * @brief Returns point in line formed by points p1 and p2
 *
 * @param p1 Point 1
 * @param p2 Point 2
 * @param s Parametric coordinate
 * @return p Point
 */
Point point_on_line(const Point &p1, const Point &p2, const double &s);

/*!
 * @brief Returns direction from point 1 to 2
 *
 * @param p1 Point 1
 * @param p2 Point 2
 * @return p Unit vector pointing at point 2 from point 1
 */
Point get_direction(const Point &p1, const Point &p2);

/*!
 * @brief Returns true if point is inside the box
 *
 * @param p Point
 * @param box Box
 * @return True If point inside box otherwise false
 */
bool is_inside_box(const Point &p, const std::pair<Point, Point> &box, double tol = 0.);
bool is_inside_box(const Point &p, const double &box_size, double tol = 0.);

/*!
 * @brief Returns true if point is inside the cylinder
 *
 * @param p Point
 * @param length Length of cylinder
 * @param radius Radius of cylinder
 * @param axis Axis of cylinder
 * @return True If point inside cylinder otherwise false
 */
bool is_inside_cylinder(const Point &p, const double &length, const double &radius, const Point &axis);

/*!
 * @brief Returns true if point is inside the cylinder
 *
 * @param p Point
 * @param R Radius of cylinder
 * @param x1 Point at the center of cross-section at s=0
 * @param x2 Point at the center of cross-section at s=L
 * @param
 * @return True If point inside cylinder otherwise false
 */
bool is_inside_cylinder(const Point &p, const double &radius, const Point &x1,
                        const Point &x2);

/*!
 * @brief Returns true if point is inside the ellipsoid
 *
 * @param p Point
 * @param center Center of ellipse
 * @param radius_vec Vector of radius describing ellipse
 * @param dim Dimension
 * @param d
 * @return True If point inside otherwise false
 */
bool is_inside_ellipse(const Point &p, const Point &center, const std::vector<double> &radius_vec, unsigned int dim);

/*!
 * @brief Returns true if point is inside the ellipsoid
 *
 * Also computes
 * d = x^2 / r1^2 + y^2 / r2^2 + z^2 / r3^2
 *
 * @param p Point
 * @param center Center of ellipse
 * @param radius_vec Vector of radius describing ellipse
 * @param dim Dimension
 * @param d
 * @return True If point inside otherwise false
 */
bool is_inside_ellipse(const Point &p, const Point &center, const std::vector<double> &radius_vec, unsigned int dim, double &d);

/*!
 * @brief Transforms point in ellipse to point in ball of given radius
 *
 * @param p Point
 * @param center Center of ellipse
 * @param radius_vec Vector of radius describing ellipse
 * @param dim Dimension
 * @param ball_r Radius of ball
 * @return Point Point in ball
 */
Point ellipse_to_ball(const Point &p, const Point &center, const std::vector<double> &radius_vec, unsigned int dim, const double &ball_r);

/*!
 * @brief Returns the vector after rotating by desired angle
 *
 * @param p Vector
 * @param theta Angle of rotation
 * @param axis Axis of rotation
 */
Point rotate(const Point &p, const double &theta, const Point &axis);

/*!
 * @brief Returns the unit vector corresponding to cross product
 *
 * Note: Better to use inbuilt function in Point - p1.cross(p2)
 *
 * @param p1 Vector 1
 * @param p2 Vector 2
 * @return vector Unit vector along the cross product
 */
Point cross_product(const Point &p1, const Point &p2);

/*!
 * @brief Computes angle between two vectors
 * @param a Vector 1
 * @param b Vector 2
 */
double angle(Point a, Point b);

/*!
 * @brief Computes angle between two vectors
 * @param a Vector 1
 * @param b Vector 2
 * @param axis Axis of rotation
 * @param is_axis If true then axis is the axis of orientation, otherwise
 * axis specifies the +ve side of the plane in which a and b are
 */
double angle(Point a, Point b, Point axis, bool is_axis = true);

/*!
 * @brief Computes following function
 *
 * exp[1 - 1 / (1 - r^a)]
 *
 * @param r Argument of function
 * @param exponent Value of power a in above function
 */
double exp_decay_function(double r, double exponent = 4.);

std::vector<Point> discretize_ball_surface(const unsigned int
                                             &disc_num,
                                           const double
                                             &ball_r,
                                           unsigned int dim);

std::vector<Point> discretize_cube(const unsigned int
                                     &disc_num,
                                   const double
                                     &cube_size,
                                   unsigned int dim);

/*!
 * @brief Do lines intersect
 *
 * @param line_1 Line 1
 * @param line_2 Line 2
 * @return True If lines intersect, else false
 */
bool lines_intersect(const std::pair<Point, Point> &line_1, const std::pair<Point,
                                                                            Point> &line_2);

/*!
 * @brief Compute distance between lines
 *
 * @param line_1 Line 1
 * @param line_2 Line 2
 * @return Value Distance
 */
double distance_between_lines(const std::pair<Point, Point> &line_1,
                              const std::pair<Point, Point> &line_2);
double distance_between_segments(const std::pair<Point, Point> &line_1,
                                 const std::pair<Point, Point> &line_2);

/*!
 * @brief Compute distance between planes
 *
 * @param plane_1 Plane 1 given by pair of normal and one point which it
 * contains
 * @param plane_2 Plane 2 given by pair of normal and one point which it
 * contains
 * @return Value Distance
 */
double distance_between_planes(const std::pair<Point, Point> &plane_1,
                               const std::pair<Point, Point> &plane_2);

/*!
 * @brief Compute distance between point and line
 *
 * @param p Point
 * @param line Line
 * @return Value Distance
 */
double point_distance_line(const Point &p,
                           const std::pair<Point, Point> &line);
double point_distance_segment(const Point &p,
                              const std::pair<Point, Point> &line);

/*!
 * @brief Compute distance between point and plane
 *
 * @param p Point
 * @param plane Plane given by pair of normal and one point which it
 * contains
 * @return Value Distance
 */
double point_distance_plane(const Point &p,
                            const std::pair<Point, Point> &plane);

/*!
 * @brief Get list of elements which have this node as vertex
 *
 * @param node_id Node id
 * @param mesh Mesh
 * @return List
 */
std::vector<unsigned int> find_elems(const unsigned int node_id,
                                     const MeshBase &mesh);

/*!
 * @brief Get list of elements which have this node as vertex
 *
 * @param node_id Node id
 * @param mesh Mesh
 * @return List
 */
void add_unique(unsigned int dof, Real val,
                std::vector<unsigned int> &list, std::vector<Real> &list_val);

/*!
 * @brief Get the major axis which is parallel to given vector within some
 * tolerance
 *
 * @param vec Given vector
 * @return axis Parallel to vec
 */
std::string get_vec_major_axis(Point vec);

/*!
 * @brief Get the length between two points
 *
 * @param p1
 * @param p2
 * @return distance
 */

double dist_between_points(const std::vector<double> &p1, const std::vector<double> &p2);

void get_unit_vector(std::vector<double> &p1);

unsigned int get_elem_id(const std::vector<double> &p, const double &mesh_size,
                         const unsigned int &num_elems, const unsigned int &dim);
unsigned int get_elem_id(const Point &p, const double &mesh_size,
                         const unsigned int &num_elems, const unsigned int &dim);

Point to_point(const std::vector<double> &p);

std::vector<double> cross_prod(std::vector<double> &p1, std::vector<double> &p2);

std::vector<double> rotate(std::vector<double> &p, double theta, std::vector<double> &axis);

Point determineRotator(const Point &dir);
std::vector<double> determineRotator(const std::vector<double> &dir);

inline float time_diff(std::chrono::steady_clock::time_point begin,
                       std::chrono::steady_clock::time_point end) {

  return std::chrono::duration_cast<std::chrono::microseconds>(end -
                                                               begin)
    .count();
}

template<class T>
inline int locate_in_set(const T &key, const std::vector<T> &set) {

  for (int i = 0; i < set.size(); i++)
    if (set[i] == key)
      return i;

  return -1;
}

inline void clear_oss(std::ostringstream &oss) {
  oss.str("");
  oss.clear();
}

template<class T>
inline T get_avg(const std::vector<T> &list) {

  if (list.size() == 0)
    return T(0);

  T avg = 0.;
  for (const auto &l : list)
    avg += l;
  avg = avg / (double(list.size()));

  return avg;
}

template<class T>
inline T get_std_dev(const std::vector<T> &list, const T mean) {

  if (list.size() == 0)
    return T(0);

  T dev = 0.;
  for (const auto &l : list)
    dev += (l - mean) * (l - mean);
  dev = dev / (T(list.size()));

  return std::sqrt(dev);
}

template<class T>
inline T get_std_dev(const std::vector<T> &list) {

  if (list.size() == 0)
    return T(0);

  // get mean
  auto mean = get_avg(list);

  T dev = 0.;
  for (const auto &l : list)
    dev += (l - mean) * (l - mean);
  dev = dev / (T(list.size()));

  return std::sqrt(dev);
}

inline std::mt19937 get_random_generator(int seed) {
  if (seed < 0) {
    std::random_device rd;
    return std::mt19937(rd());
  } else {
    return std::mt19937(seed);
  }
}

inline boost::mt19937 get_random_generator_boost(int seed) {

  if (seed < 0) {
    auto rd = boost::mt19937();
    return rd;
  } else {
    auto rd = boost::mt19937(seed);
    return rd;
  }
}

inline double proj(double x) {
  auto y = 1. < x ? 1. : x;
  return 0. > y ? 0. : y;
}

} // namespace util

#endif // UTILS_H


#include <iostream>
#include <boost/numeric/interval.hpp>

int main()
{
	boost::numeric::interval<int> range1(0, 100);
	std::cout << boost::numeric::median(range1) << std::endl;
	std::cout << boost::numeric::width(range1) << std::endl;

	boost::numeric::interval<int> range2(6, 7);
	boost::numeric::interval<int> range3(4, 8);

	auto range_min = boost::numeric::min(range2, range3);

	std::cout << range_min.lower() << " ~ "
		<< range_min.upper() << std::endl;

	auto range_max = boost::numeric::max(range2, range3);

	std::cout << range_max.lower() << " ~ "
		<< range_max.upper() << std::endl;

	boost::numeric::interval<int> range4(-4, 8);

	auto range_abs = boost::numeric::abs(range4);

	std::cout << range_abs.lower() << " ~ "
		<< range_abs.upper() << std::endl;

	return 0;
}


#pragma once
#include "mtao/types.hpp"
#include <Eigen/Sparse>

namespace mtao::geometry {
    //A collection of simplicies, indices into simplices, and related barycentric coordinates
    template <typename SimplexType, typename BType, typename SIType>
        Eigen::SparseMatrix<typename BType::Scalar>
        barycentric_matrix(int num_vertices, const Eigen::MatrixBase<SimplexType>& S, const Eigen::MatrixBase<SIType>& SI, const Eigen::MatrixBase<BType>& B) {
            assert(SI.size() == B.cols());

            using Scalar = typename BType::Scalar;
            Eigen::SparseMatrix<Scalar> A(B.cols(), num_vertices);

            std::vector<Eigen::Triplet<Scalar>> trips;
            trips.reserve(S.rows()*SI.size());

            for(int i = 0; i < B.cols(); ++i)
            {
                auto s = S.col(SI(i));
                auto b = B.col(i);
                for(int j = 0; j < b.rows(); ++j)
                {
                    trips.emplace_back(i,s(j),b(j));
                }
            }
            A.setFromTriplets(trips.begin(),trips.end());
            return A;
        }
    //A collection of simplicies, indices into simplices, and related barycentric coordinates
    //piecewise constant per-face values
    template <typename Scalar, typename SIType>
        Eigen::SparseMatrix<Scalar>
        barycentric_matrix_face(int num_faces, const Eigen::MatrixBase<SIType>& SI) {

            Eigen::SparseMatrix<Scalar> A(SI.rows(), num_faces);

            std::vector<Eigen::Triplet<Scalar>> trips;
            trips.reserve(SI.size());

            for(int i = 0; i < SI.cols(); ++i)
            {
                trips.emplace_back(i,SI(i),1);
            }
            A.setFromTriplets(trips.begin(),trips.end());
            return A;
        }
}


// Boost.Geometry

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014, 2016, 2017.
// Modifications copyright (c) 2014-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP
#define BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/formulas/differential_quantities.hpp>
#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/result_direct.hpp>

#ifndef BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS
#define BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 1000
#endif

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The solution of the direct problem of geodesics on latlong coordinates, after Vincenty, 1975
\author See
    - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
    - http://www.icsm.gov.au/gda/gdav2.3.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template <
    typename CT,
    bool EnableCoordinates = true,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false
>
class vincenty_direct
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcCoordinates = EnableCoordinates || CalcQuantities;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;

public:
    typedef result_direct<CT> result_type;

    template <typename T, typename Dist, typename Azi, typename Spheroid>
    static inline result_type apply(T const& lo1,
                                    T const& la1,
                                    Dist const& distance,
                                    Azi const& azimuth12,
                                    Spheroid const& spheroid)
    {
        result_type result;

        CT const lon1 = lo1;
        CT const lat1 = la1;

        if ( math::equals(distance, Dist(0)) || distance < Dist(0) )
        {
            result.lon2 = lon1;
            result.lat2 = lat1;
            return result;
        }

        CT const radius_a = CT(get_radius<0>(spheroid));
        CT const radius_b = CT(get_radius<2>(spheroid));
        CT const flattening = formula::flattening<CT>(spheroid);

        CT const sin_azimuth12 = sin(azimuth12);
        CT const cos_azimuth12 = cos(azimuth12);

        // U: reduced latitude, defined by tan U = (1-f) tan phi
        CT const one_min_f = CT(1) - flattening;
        CT const tan_U1 = one_min_f * tan(lat1);
        CT const sigma1 = atan2(tan_U1, cos_azimuth12); // (1)

        // may be calculated from tan using 1 sqrt()
        CT const U1 = atan(tan_U1);
        CT const sin_U1 = sin(U1);
        CT const cos_U1 = cos(U1);

        CT const sin_alpha = cos_U1 * sin_azimuth12; // (2)
        CT const sin_alpha_sqr = math::sqr(sin_alpha);
        CT const cos_alpha_sqr = CT(1) - sin_alpha_sqr;

        CT const b_sqr = radius_b * radius_b;
        CT const u_sqr = cos_alpha_sqr * (radius_a * radius_a - b_sqr) / b_sqr;
        CT const A = CT(1) + (u_sqr/CT(16384)) * (CT(4096) + u_sqr*(CT(-768) + u_sqr*(CT(320) - u_sqr*CT(175)))); // (3)
        CT const B = (u_sqr/CT(1024))*(CT(256) + u_sqr*(CT(-128) + u_sqr*(CT(74) - u_sqr*CT(47)))); // (4)

        CT s_div_bA = distance / (radius_b * A);
        CT sigma = s_div_bA; // (7)

        CT previous_sigma;
        CT sin_sigma;
        CT cos_sigma;
        CT cos_2sigma_m;
        CT cos_2sigma_m_sqr;

        int counter = 0; // robustness

        do
        {
            previous_sigma = sigma;

            CT const two_sigma_m = CT(2) * sigma1 + sigma; // (5)

            sin_sigma = sin(sigma);
            cos_sigma = cos(sigma);
            CT const sin_sigma_sqr = math::sqr(sin_sigma);
            cos_2sigma_m = cos(two_sigma_m);
            cos_2sigma_m_sqr = math::sqr(cos_2sigma_m);

            CT const delta_sigma = B * sin_sigma * (cos_2sigma_m
                                        + (B/CT(4)) * ( cos_sigma * (CT(-1) + CT(2)*cos_2sigma_m_sqr)
                                            - (B/CT(6) * cos_2sigma_m * (CT(-3)+CT(4)*sin_sigma_sqr) * (CT(-3)+CT(4)*cos_2sigma_m_sqr)) )); // (6)

            sigma = s_div_bA + delta_sigma; // (7)

            ++counter; // robustness

        } while ( geometry::math::abs(previous_sigma - sigma) > CT(1e-12)
               //&& geometry::math::abs(sigma) < pi
               && counter < BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS ); // robustness

        if (BOOST_GEOMETRY_CONDITION(CalcCoordinates))
        {
            result.lat2
                = atan2( sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_azimuth12,
                         one_min_f * math::sqrt(sin_alpha_sqr + math::sqr(sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_azimuth12))); // (8)

            CT const lambda = atan2( sin_sigma * sin_azimuth12,
                                     cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_azimuth12); // (9)
            CT const C = (flattening/CT(16)) * cos_alpha_sqr * ( CT(4) + flattening * ( CT(4) - CT(3) * cos_alpha_sqr ) ); // (10)
            CT const L = lambda - (CT(1) - C) * flattening * sin_alpha
                            * ( sigma + C * sin_sigma * ( cos_2sigma_m + C * cos_sigma * ( CT(-1) + CT(2) * cos_2sigma_m_sqr ) ) ); // (11)

            result.lon2 = lon1 + L;
        }

        if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
        {
            result.reverse_azimuth
                = atan2(sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_azimuth12); // (12)
        }

        if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
        {
            typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
            quantities::apply(lon1, lat1, result.lon2, result.lat2,
                              azimuth12, result.reverse_azimuth,
                              radius_b, flattening,
                              result.reduced_length, result.geodesic_scale);
        }

        return result;
    }

};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP


/**
 * Copyright (c) 2018, The Akatsuki(Jacob.lsx). All rights reserved.
 */

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <ctime>
#include <climits>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <Eigen/Core>

#include <ceres/ceres.h>
#include <ceres/rotation.h>

using namespace std;
using namespace cv;

// TODO Warning!!!!! The result of using this method for bundle adjustment optimization is incorrect!!!!!!!

/**
 * TODO
 * The result of using this method for bundle adjustment optimization is incorrect,
 * and I don’t understand it for the time being.
 */
#define USED_ISOMETRY3D_IN_BA       0

/**
 * 一次测量的值，包括一个世界坐标系下三维点与一个灰度值
 */
struct Measurement {
    Measurement(Eigen::Vector3d p, float g) : pos_world(p), grayscale(g) {}
    Eigen::Vector3d pos_world;
    float grayscale;
};

/**
 * 从像素坐标系转换成世界坐标系
 */
inline Eigen::Vector3d project2Dto3D(int x, int y, int d, float fx, float fy, float cx, float cy, float scale)
{
    float zz = float(d) / scale;
    float xx = zz * (x-cx) / fx;
    float yy = zz * (y-cy) / fy;
    return Eigen::Vector3d(xx, yy, zz);
}

/**
 * 从世界坐标系转换成像素坐标系
 */
inline Eigen::Vector2d project3Dto2D(float x, float y, float z, float fx, float fy, float cx, float cy)
{
    float u = fx * x / z + cx;
    float v = fy * y / z + cy;
    return Eigen::Vector2d(u, v);
}

/**
 * 直接法估计位姿
 * input:
 *   测量值(空间点的灰度), 新的灰度图, 相机内参;
 * output:
 *   相机位姿
 * return:
 *   true为成功, false失败
 */
bool poseEstimationDirect(const vector<Measurement>&, cv::Mat*, Eigen::Matrix3f&, Eigen::Isometry3d&);

#if USED_ISOMETRY3D_IN_BA
/**
 * project a 3d point into an image plane, the error is photometric error
 */
class EdgeSE3ProjectDirect
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeSE3ProjectDirect(const Eigen::Vector3d &p_world, const float &grayscale, cv::Mat* const image)
     : x_world_(p_world), grayscale_(grayscale), image_(image) {}

    static void addCameraIntrinsics(const Eigen::Matrix3f &K)
    {
        fx_ = K(0, 0);
        fy_ = K(1, 1);
        cx_ = K(0, 2);
        cy_ = K(1, 2);
    }

    template<typename T>
    bool operator() (const T* const T_, T* residuals) const;

    static ceres::CostFunction* create(const Eigen::Vector3d &p_world, const float &grayscale, cv::Mat* image)
    {
//         return (new ceres::AutoDiffCostFunction<EdgeSE3ProjectDirect, 1, 16>(new EdgeSE3ProjectDirect(p_world, grayscale, image)));
        return (new ceres::NumericDiffCostFunction<EdgeSE3ProjectDirect, ceres::CENTRAL, 1, 16>(new EdgeSE3ProjectDirect(p_world, grayscale, image)));
    }

protected:
    /* get a gray scale value from reference image (bilinear interpolated) */
    inline float getPixelValue(float x, float y) const
    {
        uchar* data = &image_->data[(int)(y) * image_->step + (int)(x)];
        float xx = x - floor(x);
        float yy = y - floor(y);

        /* 求取分布在集中相邻４个像素点覆盖面积的像素灰度值 */
        return float((1 - xx) * (1 - yy) * data[0] + \
                     xx * (1 - yy) * data[1] + \
                     (1 - xx) * yy * data[image_->step] + \
                     xx * yy * data[image_->step + 1]);
    }

private:
    Eigen::Vector3d x_world_;                   // 3D point in world frame
    float grayscale_;                           // Measurement grayscale
    static float cx_, cy_, fx_, fy_;            // Camera intrinsics
    cv::Mat* image_ = nullptr;                  // reference image
};

float EdgeSE3ProjectDirect::cx_ = 0.f;
float EdgeSE3ProjectDirect::cy_ = 0.f;
float EdgeSE3ProjectDirect::fx_ = 0.f;
float EdgeSE3ProjectDirect::fy_ = 0.f;

template<typename T>
bool EdgeSE3ProjectDirect::operator() (const T* const T_,  T* residuals) const
{
    Eigen::Isometry3d transformationMatrix;
    transformationMatrix.matrix() << T_[0],  T_[1],  T_[2],  T_[3],
                                     T_[4],  T_[5],  T_[6],  T_[7],
                                     T_[8],  T_[9],  T_[10], T_[11],
                                     T_[12], T_[13], T_[14], T_[15];

    Eigen::Vector3d p3d = transformationMatrix * x_world_;

    Eigen::Vector2d p2d = project3Dto2D(p3d.x(), p3d.y(), p3d.z(), fx_, fy_, cx_, cy_);

    residuals[0] = (T)grayscale_ - (T)getPixelValue(p2d.x(), p2d.y());

    return true;
}

#else

/**
 * project a 3d point into an image plane, the error is photometric error
 */
class EdgeSE3ProjectDirect
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeSE3ProjectDirect(const Eigen::Vector3d &p_world, const float &grayscale, cv::Mat* const image)
     : x_world_(p_world), grayscale_(grayscale), image_(image) {}

    static void addCameraIntrinsics(const Eigen::Matrix3f &K)
    {
        fx_ = K(0, 0);
        fy_ = K(1, 1);
        cx_ = K(0, 2);
        cy_ = K(1, 2);
    }

    template<typename T>
    bool operator() (const T* const r_vec, const T* const t, T* residuals) const;

    static ceres::CostFunction* create(const Eigen::Vector3d &p_world, const float &grayscale, cv::Mat* image)
    {
//         return (new ceres::AutoDiffCostFunction<EdgeSE3ProjectDirect, 1, 16>(new EdgeSE3ProjectDirect(p_world, grayscale, image)));
        return (new ceres::NumericDiffCostFunction<EdgeSE3ProjectDirect, ceres::CENTRAL, 1, 3, 3>(new EdgeSE3ProjectDirect(p_world, grayscale, image)));
    }

protected:
    /* get a gray scale value from reference image (bilinear interpolated) */
    inline float getPixelValue(float x, float y) const
    {
        uchar* data = &image_->data[(int)(y) * image_->step + (int)(x)];
        float xx = x - floor(x);
        float yy = y - floor(y);

        /* 求取分布在集中相邻４个像素点覆盖面积的像素灰度值 */
        return float((1 - xx) * (1 - yy) * data[0] + \
                     xx * (1 - yy) * data[1] + \
                     (1 - xx) * yy * data[image_->step] + \
                     xx * yy * data[image_->step + 1]);
    }

private:
    Eigen::Vector3d x_world_;                   // 3D point in world frame
    float grayscale_;                           // Measurement grayscale
    static float cx_, cy_, fx_, fy_;            // Camera intrinsics
    cv::Mat* image_ = nullptr;                  // reference image
};

float EdgeSE3ProjectDirect::cx_ = 0.f;
float EdgeSE3ProjectDirect::cy_ = 0.f;
float EdgeSE3ProjectDirect::fx_ = 0.f;
float EdgeSE3ProjectDirect::fy_ = 0.f;

template<typename T>
bool EdgeSE3ProjectDirect::operator() (const T* const r_vec, const T* const t, T* residuals) const
{
    T p_world[3] = {(T)x_world_.x(), (T)x_world_.y(), (T)x_world_.z()};
    T p_cam[3];
    ceres::AngleAxisRotatePoint(r_vec, p_world, p_cam);
    p_cam[0] += t[0];
    p_cam[1] += t[1];
    p_cam[2] += t[2];

    Eigen::Vector2d p2d = project3Dto2D(p_cam[0], p_cam[1], p_cam[2], fx_, fy_, cx_, cy_);

    residuals[0] = (T)grayscale_ - (T)getPixelValue(p2d.x(), p2d.y());

    return true;
}
#endif

int main(int argc, char** argv)
{
    if (argc != 2) {
        cout << "usage: direct_sparse path_to_dataset" << endl;
        return 1;
    }

    srand((unsigned int) time(0));
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";

    ifstream fin(associate_file);

    string rgb_file, depth_file, time_rgb, time_depth;
    cv::Mat color, depth, gray;
    std::vector<Measurement> measurements;

    // Camera intrinsics
    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    float depth_scale = 1000.0;
    Eigen::Matrix3f K;
    K << fx, 0.f, cx, 0.f, fy, cy, 0.f, 0.f, 1.0f;

    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();

    cv::Mat prev_color;

    // 我们以第一个图像为参考，对后续图像和参考图像做直接法
    for (int index = 0; index < 10; index ++) {
        cout << "**************** loop " << index << " **************** \r\n";
        fin >> time_rgb >> rgb_file >> time_depth >> depth_file;
        color = cv::imread(path_to_dataset + "/" + rgb_file);
        depth = cv::imread(path_to_dataset + "/" + depth_file, cv::IMREAD_UNCHANGED);
        if (color.data == nullptr || depth.data == nullptr) {
            continue;
        }
        cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);  // change rgb photo to gray photo
        if (index == 0) {
            // detect Oriented FAST KeyPoint for 1st photo
            std::vector<cv::KeyPoint> keypoints;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect(color, keypoints);
            for (auto kp : keypoints) {
                // 去掉邻近边缘处的点
                if (kp.pt.x < 20 || kp.pt.y < 20 || (kp.pt.x + 20) > color.cols || (kp.pt.y + 20) > color.rows) {
                    continue;
                }
                ushort d = depth.ptr<ushort>(cvRound(kp.pt.y))[cvRound(kp.pt.x)];
                if (d == 0) {
                    continue;
                }
                Eigen::Vector3d p3d = project2Dto3D(kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale);
                float grayscale = float(gray.ptr<uchar>(cvRound(kp.pt.y))[cvRound(kp.pt.x)]);
                measurements.push_back(Measurement(p3d, grayscale));
            }
            prev_color = color.clone();
            continue;
        }

        // 使用直接法计算相机运动
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        poseEstimationDirect(measurements, &gray, K, Tcw);
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
        cout << "direct method costs time: " << time_used.count() << "secound. \r\n";
        cout << "Tcw = " << Tcw.matrix() << endl;

        // plot the feature points
        cv::Mat img_show(color.rows * 2, color.cols, CV_8UC3);
        prev_color.copyTo(img_show(cv::Rect(0, 0, color.cols, color.rows)));
        color.copyTo(img_show(cv::Rect(0, color.rows, color.cols, color.rows)));

        for (Measurement m : measurements) {
            if (rand() > RAND_MAX / 5) {
                continue;
            }
            Eigen::Vector3d p = m.pos_world;
            Eigen::Vector2d pixel_prev = project3Dto2D(p(0, 0), p(1, 0), p(2, 0), fx, fy, cx, cy);
            Eigen::Vector3d p2 = Tcw * m.pos_world;
            Eigen::Vector2d pixel_now = project3Dto2D(p2(0, 0), p2(1, 0), p2(2, 0), fx, fy, cx, cy);
            if (pixel_now(0, 0) < 0 || pixel_now(0, 0) >= color.cols || pixel_now(1, 0) < 0 || pixel_now(1, 0) >= color.rows) {
                continue;
            }

            float b = 255 * float(rand()) / RAND_MAX;
            float g = 255 * float(rand()) / RAND_MAX;
            float r = 255 * float(rand()) / RAND_MAX;

            cv::circle(img_show, cv::Point2d(pixel_prev(0, 0), pixel_prev(1, 0)), 8, cv::Scalar(b, g, r), 2);
            cv::circle(img_show, cv::Point2d(pixel_now(0, 0), pixel_now(1, 0) + color.rows), 8, cv::Scalar(b, g, r), 2);
            cv::line(img_show, cv::Point2d(pixel_prev(0, 0), pixel_prev(1, 0)), cv::Point2d(pixel_now(0, 0), pixel_now(1, 0) + color.rows), cv::Scalar(b, g, r), 1);
        }

        cv::imshow("result", img_show);
        cv::waitKey(0);
    }

    return 0;
}

/**
 * 直接法估计位姿
 */
bool poseEstimationDirect(const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw)
{
    EdgeSE3ProjectDirect::addCameraIntrinsics(K);

#if USED_ISOMETRY3D_IN_BA
    ceres::Problem problem;
    for (auto m : measurements) {
        ceres::CostFunction *costFunction = EdgeSE3ProjectDirect::create(m.pos_world, m.grayscale, gray);
        problem.AddResidualBlock(costFunction, nullptr, Tcw.data());
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
#else
    double r_vec[3] = {0.f}, t[3] = {0.f};
    ceres::Problem problem;
    for (auto m : measurements) {
        ceres::CostFunction *costFunction = EdgeSE3ProjectDirect::create(m.pos_world, m.grayscale, gray);
        problem.AddResidualBlock(costFunction, nullptr, r_vec, t);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    cv::Mat R_vec = (cv::Mat_<double>(3, 1) << r_vec[0], r_vec[1], r_vec[2]);
    cv::Mat R_matrix;
    cv::Rodrigues(R_vec, R_matrix);

    Eigen::Matrix3d R;
    R << R_matrix.at<double>(0, 0), R_matrix.at<double>(0, 1), R_matrix.at<double>(0, 2),
         R_matrix.at<double>(1, 0), R_matrix.at<double>(1, 1), R_matrix.at<double>(1, 2),
         R_matrix.at<double>(2, 0), R_matrix.at<double>(2, 1), R_matrix.at<double>(2, 2);

    Tcw.prerotate(R);
    Tcw.pretranslate(Eigen::Vector3d(t[0], t[1], t[2]));
#endif

    return true;
}


// Run like this
// ./loglikelihood4 competition/S1a/dataset13_training.csv  --use_ldl
// This code will forecast 4 unknown parameters of the Matern covariance matrix
// Developed by Alexander Litvinenko (RWTH Aachen) and Ronald Kriemann (MIS MPG Leipzig)
// Based on the HLIBPro library (v. 2.9) www.hlibpro.com
// No warranties.
#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <stdlib.h>

#include <boost/format.hpp>
#include <boost/program_options.hpp>

#include <gsl/gsl_multimin.h>

#include "hlib.hh"

using namespace HLIB;
using namespace boost::program_options;

enum {
    IDX_SIGMA  = 0,
    IDX_LENGTH = 1,
    IDX_NU     = 2,
    IDX_TAU     = 3
};

// global options
int        nmin      = CFG::Cluster::nmin;
double     eps       = 1e-6;
double     fac_eps   = 1e-6;
double     shift     = 1e-8;
bool       use_ldl   = false;

//
// read dataset from file
//
void
read_data ( const std::string &       datafile,
            std::vector< T2Point > &  vertices,
            BLAS::Vector< double > &  Z_data )
{
    std::ifstream  in( datafile );

    if ( ! in ) // error
        exit( 1 );

    size_t  N_vtx = 0;

    #if 1

    std::string  line;

    std::getline( in, line );

    if ( line == "x,y,values" )
    {
        std::list< T2Point >  pos;
        std::list< double >   vals;

        while ( std::getline( in, line ) )
        {
            auto    parts = split( line, "," );
            double  x = atof( parts[0].c_str() );
            double  y = atof( parts[1].c_str() );
            double  v = atof( parts[2].c_str() );

            pos.push_back( T2Point( x, y ) );
            vals.push_back( v );
        }// while

        N_vtx = pos.size();

        std::cout << N_vtx << std::endl;

        vertices.resize( N_vtx );
        Z_data = BLAS::Vector< double >( N_vtx );

        int  i = 0;

        for ( auto  p : pos )
            vertices[ i++ ] = p;

        i = 0;

        for ( auto  v : vals )
            Z_data( i++ ) = v;
    }// if
    else
        HERROR( ERR_NOT_IMPL, "", "" );

    #else

    in >> N_vtx;

    std::cout << "reading " << N_vtx << " datapoints" << std::endl;

    vertices.resize( N_vtx );
    Z_data = BLAS::Vector< double >( N_vtx );

    for ( idx_t  i = 0; i < idx_t(N_vtx); ++i )
    {
        int     index = i;
        double  x, y, z;
        double  v     = 0.0;

        in >> index >> x >> y >> z >> v;

        vertices[ index ] = T2Point( x, y );
        Z_data( index )   = v;
    }// for

    #endif

    //
    // for visualization of data, export 2D points with v value in csv file
    //

    std::ofstream  out( "data.csv" );

    out << "x,y,z,v" << std::endl;

    for ( uint  i = 0; i < N_vtx; ++i )
        out << vertices[i].x() << "," << vertices[i].y() << ",0," << Z_data( i ) << std::endl;
}

//
// define LogLikeliHood Problem to be evaluated at theta by minimization function
//
struct LogLikeliHoodProblem
{
    std::vector< T2Point >                vertices;
    std::unique_ptr< TCoordinate >        coord;
    std::unique_ptr< TClusterTree >       ct;
    std::unique_ptr< TBlockClusterTree >  bct;
    std::unique_ptr< TVector >            Z;

    LogLikeliHoodProblem ( const std::string &  datafile )
    {
        init( datafile );
    }

    void
    init ( const std::string &  datafile )
    {
        BLAS::Vector< double >  Z_data;

        read_data( datafile, vertices, Z_data );

        coord = std::make_unique< TCoordinate >( vertices );

        TAutoBSPPartStrat  part_strat;
        TBSPCTBuilder      ct_builder( & part_strat, nmin );

        ct = ct_builder.build( coord.get() );

        TStdGeomAdmCond    adm_cond( 2.0, use_min_diam );
        TBCBuilder         bct_builder;

        bct = bct_builder.build( ct.get(), ct.get(), & adm_cond );

        Z   = std::make_unique< TScalarVector >( *ct->root(), std::move( Z_data ) );

        ct->perm_e2i()->permute( Z.get() );
    }

    double
    eval ( const double  sigma,
           const double  length,
           const double  nu,
           const double tau )
    {
        TMaternCovCoeffFn< T2Point >  matern_coefffn( 2.0/pow(1.1,sigma), 1.0/pow(1.5,length), 1.0/pow(1.2, nu),  vertices );
       // TMaternCovCoeffFn< T2Point >  matern_coefffn( sigma, length, nu,  vertices );
        TPermCoeffFn< double >        coefffn( & matern_coefffn, ct->perm_i2e(), ct->perm_i2e() );

        TACAPlus< double >            aca( & coefffn );
        auto                          acc = fixed_prec( eps );
        TDenseMatBuilder< double >    h_builder( & coefffn, & aca );

        auto                          C        = h_builder.build( bct.get(), acc );

        if ( shift != 0.0 )
        {
           // std::cout << "Diagonal is added!!!!!!!!!!" << std::endl;
            add_identity( C.get(), 1/pow(2.0,tau) );
        }

        auto                          fac_acc  = fixed_prec( fac_eps );
        auto                          C_fac    = C->copy();
        auto                          fac_opts = fac_options_t{ point_wise, CFG::Arith::storage_type, false };

        if ( use_ldl )
            ldl( C_fac.get(), fac_acc, fac_opts );
        else
            chol( C_fac.get(), fac_acc );

        std::unique_ptr< TFacInvMatrix >   C_inv;

        if ( use_ldl )
            C_inv = std::make_unique< TLDLInvMatrix >( C_fac.get(), symmetric, point_wise );
        else
            C_inv = std::make_unique< TLLInvMatrix >( C_fac.get(), symmetric );

        const size_t                  N     = vertices.size();
        double                        log_det_C = 0.0;

        for ( idx_t  i = 0; i < idx_t(N); ++i )
        {
            if ( use_ldl ) log_det_C +=   std::log( C_fac->entry( i, i ) );
            else           log_det_C += 2*std::log( C_fac->entry( i, i ) ); // two factors L!
        }// if

        TStopCriterion                sstop( 250, 1e-16, 0.0 );
        TCG                           solver( sstop );
        auto                          sol = C->row_vector();

        solver.solve( C.get(), sol.get(), Z.get(), C_inv.get() );

        auto                          ZdotCZ = std::real( Z->dot( sol.get() ) );
        const double                  log2pi = std::log( 2.0 * Math::pi<double>() );
        auto                          LL     = -0.5 * ( N * log2pi + log_det_C + ZdotCZ );

        return LL;
    }
};

//
// wrapper from GSL to LogLikeliHoodProblem
//
double
eval_logli ( const gsl_vector *  param,
             void *              data )
{
    double sigma  = gsl_vector_get( param, IDX_SIGMA );
    double length = gsl_vector_get( param, IDX_LENGTH );
    double nu     = gsl_vector_get( param, IDX_NU );
    double tau    = gsl_vector_get( param, IDX_TAU );

    LogLikeliHoodProblem *  problem = static_cast< LogLikeliHoodProblem * >( data );

    return - problem->eval( sigma, length, nu, tau );
}

//
// optimization function using GSL
//
double
maximize_likelihood ( double &                sigma,
                      double &                length,
                      double &                nu,
                      double &                tau,
                      LogLikeliHoodProblem &  problem )
{
    int        status   = 0;
    const int  max_iter = 500;

    const gsl_multimin_fminimizer_type *  T = gsl_multimin_fminimizer_nmsimplex2;
    gsl_multimin_fminimizer *             s = NULL;
    gsl_vector *                          ss;
    gsl_vector *                          x;
    gsl_multimin_function                 minex_func;
    double                                size;

    x = gsl_vector_alloc( 4 );        // start value
    gsl_vector_set( x, IDX_SIGMA,  sigma );
    gsl_vector_set( x, IDX_LENGTH, length );
    gsl_vector_set( x, IDX_NU,     nu );
    gsl_vector_set( x, IDX_TAU,    tau );

    ss = gsl_vector_alloc( 4 );       // step sizes
    gsl_vector_set( ss, IDX_SIGMA,  1.0 );
    gsl_vector_set( ss, IDX_LENGTH, 1.0 );
    gsl_vector_set( ss, IDX_NU,     1.0 );
    gsl_vector_set( ss, IDX_TAU,    1.0 );
    //gsl_vector_set( ss, IDX_SIGMA,  0.05 );
    //gsl_vector_set( ss, IDX_LENGTH, 0.005 );
    //gsl_vector_set( ss, IDX_NU,     0.01 );
    //gsl_vector_set( ss, IDX_TAU,    1 );

    // Initialize method and iterate
    minex_func.n      = 4;
    minex_func.f      = & eval_logli;
    minex_func.params = & problem;

    s = gsl_multimin_fminimizer_alloc( T, 4 );
    gsl_multimin_fminimizer_set(s, & minex_func, x, ss );

    int     iter = 0;
    double  LL = 0;

    do
    {
        iter++;
        status = gsl_multimin_fminimizer_iterate( s );

        if ( status != 0 )
            break;

        size   = gsl_multimin_fminimizer_size( s );    // return eps for stopping criteria
        status = gsl_multimin_test_size( size, 1e-3 ); // This function tests the minimizer specific characteristic size

        if ( status == GSL_SUCCESS )
        {
            std::cout << "converged to minimum" << std::endl;
            FILE* f4 = fopen( "111_S2b_results.txt", "a+");
           // fprintf(f4, "sigma, ell, nu, tau\n"); //2.0/pow(1.1,sigma), 1.0/pow(1.5,length), 1.0/pow(1.2, nu),
            fprintf(f4, "& %.6f & %.6f & %.6f & %.6f & %.6f \\\\ \\hline \n", 2.0/pow(1.1, sigma), 1.0/pow(1.5,length), 1.0/pow(1.2,nu), 1.0/pow(2.0, tau), LL );
            fclose(f4);
        }

        sigma  = gsl_vector_get( s->x, IDX_SIGMA );
        length = gsl_vector_get( s->x, IDX_LENGTH );
        nu     = gsl_vector_get( s->x, IDX_NU );
        tau    = gsl_vector_get( s->x, IDX_TAU );
        LL     = -s->fval;

        std::cout << "  loglikelihood at" //2.0/pow(1.1,sigma), 1.0/pow(1.5,length), 1.0/pow(1.2, nu),
                  << "  σ = " << 2.0/pow(1.1, sigma)
                  << ", ℓ = " << 1.0/pow(1.5, length)
                  << ", ν = " << 1.0/pow(1.2, nu)
                  << ", tau = " << 1.0/pow(2.0, tau)
                  << "   is " << LL
                  << std::endl;
    } while (( status == GSL_CONTINUE ) && ( iter < max_iter ));

    gsl_multimin_fminimizer_free( s );
    gsl_vector_free( ss );
    gsl_vector_free( x );

    return LL;
}

//
// main function
//
int
main ( int      argc,
       char **  argv )
{
    INIT();

    std::string  datafile = "datafile.txt";
    //Important ! sigma= 2.0/pow(1.1,sigma), length = 1.0/pow(1.5,length), 1.0/pow(1.2, nu),
    double  sigma  = 1.0; //take these values from previous experiments (Part 1a)
    double  length = 7.0; //means, cov length = 1/pow(1.5, 7)
    double  nu     = 4.0; //means, nu = 1/pow(1.2, 2)
    double  tau    = 20;

    //
    // define command line options
    //

    options_description             all_opts;
    options_description             vis_opts( "usage: loglikelihood [options] datafile\n  where options include" );
    options_description             hid_opts( "Hidden options" );
    positional_options_description  pos_opts;
    variables_map                   vm;

    // standard options
    vis_opts.add_options()
        ( "help,h",                       ": print this help text" )
        ( "threads,t",   value<int>(),    ": number of parallel threads" )
        ( "verbosity,v", value<int>(),    ": verbosity level" )
        ( "nmin",        value<int>(),    ": set minimal cluster size" )
        ( "eps,e",       value<double>(), ": set H accuracy" )
        ( "epslu",       value<double>(), ": set only H factorization accuracy" )
        ( "shift",       value<double>(), ": regularization parameter" )
        ( "ldl",                          ": use LDL factorization" )
        ( "sigma",       value<double>(), ": sigma parameter" )
        ( "nu",          value<double>(), ": nu parameter" )
        ( "length",      value<double>(), ": length parameter" )
        ( "tau",         value<double>(), ": tau paramater" )
        ;

    hid_opts.add_options()
        ( "data",        value<std::string>(), ": datafile defining problem" )
        ;

    // options for command line parsing
    all_opts.add( vis_opts ).add( hid_opts );

    // all "non-option" arguments should be "--data" arguments
    pos_opts.add( "data", -1 );

    //
    // parse command line options
    //

    try
    {
        store( command_line_parser( argc, argv ).options( all_opts ).positional( pos_opts ).run(), vm );
        notify( vm );
    }// try
    catch ( required_option &  e )
    {
        std::cout << e.get_option_name() << " requires an argument, try \"-h\"" << std::endl;
        exit( 1 );
    }// catch
    catch ( unknown_option &  e )
    {
        std::cout << e.what() << ", try \"-h\"" << std::endl;
        exit( 1 );
    }// catch

    //
    // eval command line options
    //

    if ( vm.count( "help") )
    {
        std::cout << vis_opts << std::endl;
        exit( 1 );
    }// if

    if ( vm.count( "nmin"      ) ) nmin     = vm["nmin"].as<int>();
    if ( vm.count( "eps"       ) ) eps      = vm["eps"].as<double>();
    if ( vm.count( "epslu"     ) ) fac_eps  = vm["epslu"].as<double>();
    if ( vm.count( "shift"     ) ) shift    = vm["shift"].as<double>();
    if ( vm.count( "threads"   ) ) CFG::set_nthreads( vm["threads"].as<int>() );
    if ( vm.count( "verbosity" ) ) CFG::set_verbosity( vm["verbosity"].as<int>() );
    if ( vm.count( "ldl"       ) ) use_ldl  = true;
    if ( vm.count( "sigma"     ) ) sigma    = vm["sigma"].as<double>();
    if ( vm.count( "nu"        ) ) nu       = vm["nu"].as<double>();
    if ( vm.count( "length"    ) ) length   = vm["length"].as<double>();
    if ( vm.count( "tau"       ) ) tau      = vm["tau"].as<double>();

    // default to general eps
    if ( fac_eps == -1 )
        fac_eps = eps;

    if ( vm.count( "data" ) )
        datafile = vm["data"].as<std::string>();
    else
    {
        std::cout << "usage: loglikelihood [options] datafile" << std::endl;
        exit( 1 );
    }// if

    std::cout << "initial parameters : " << sigma << ", " << nu << ", " << length << ", " << tau << std::endl;

    LogLikeliHoodProblem  problem( datafile );

    auto  LL = maximize_likelihood( sigma, length, nu, tau, problem );

    DONE();

}


/*

Copyright (c) 2020   Michael Borinsky

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

*/

#pragma once

#include <vector>
#include <Eigen/Dense>

#include "graph.hpp"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::LDLT;

double get_laplacian(
        MatrixXd& L,
        const graph& g,
        const vector<double>& X,
        const edge_subgraph_type& contracted_subgraph,
        int num_components,
        const vector<int>& contracted_subgraph_components_map
        )
{
    assert( X.size() == g._E );
    assert( contracted_subgraph_components_map.size() == g._V );

    int E = g._E;
    int V = num_components;

    assert( L.rows() == V-1 && L.cols() == V-1 );

    L.setZero( V-1, V-1 );

    double Lambda = 1.;
    for ( int j = 0; j < E; j++ )
    {
        if( contracted_subgraph[j] )
            continue;

        pair<int, int> edge;
        int k,l,c;
        tie(edge, c) = g._edges[j];
        tie(k,l) = edge;

        Lambda *= X[j];

        k = contracted_subgraph_components_map[k];
        l = contracted_subgraph_components_map[l];

        if( k == l )
            continue;

        if ( k < l )
            tie(k, l) = make_pair(l, k);

        double x = 1. / X[j];

        L(l,l) += x;

        if( k == V - 1 ) // delete last row
            continue;

        L(k,k) +=  x;
        L(k,l) += -x;
        // only lower triangular part of the laplacian matters for ldlt. => Fill only lower triangular part.
    }

    return Lambda;
}

void get_P_matrix(
        MatrixXd& P,
        const graph& g,
        const vector< VectorXd >& momenta,
        int num_components,
        const vector<int>& contracted_subgraph_components_map
        )
{
    assert( contracted_subgraph_components_map.size() == g._V );
    if( momenta.size() != g._V )
    {
        stringstream s;
        s << "P-matrix calculation - the graph " << g << " has " << g._V << " vertices, but only " << momenta.size() << " momenta were provided. An incoming momentum for each vertex is needed. Give a zero incoming momentum for internal vertices.)";
        throw domain_error(s.str());
    }

    int V = num_components;
    P.setZero( V - 1, momenta[0].size() );

    for( int i = 0; i < g._V; i++ )
    {
        int v = contracted_subgraph_components_map[i];

        if( v == V-1 )
            continue;

        P.row(v) += momenta[i].transpose();
    }
}

double eval_psi_polynomial(
        double Lambda,
        const LDLT< MatrixXd >& ldlt
        )
{
    double detL = ldlt.matrixL().determinant();
    double detD = ldlt.vectorD().prod();

    return detL * detL * detD * Lambda;
}

double eval_pphi_polynomial(
        const MatrixXd& P,
        const LDLT< MatrixXd >& ldlt
        )
{
    return ( P.transpose() * ldlt.solve( P ) ).trace();
}

double eval_M_polynomial(
        const graph& g,
        const vector<double>& masses_sqr,
        const vector<double>& X,
        const edge_subgraph_type& contracted_subgraph
        )
{
    if( masses_sqr.size() != g._E )
    {
        stringstream s;
        s << "m polynomial calculation - the graph " << g << " has " << g._E << " edges, but only " << masses_sqr.size() << " squared masses were given. A mass for each edge is needed. (It can be zero.)";
        throw domain_error(s.str());
    }

    assert( g._E == X.size() );

    double M = 0.;

    for( int j = 0; j < g._E; j++ )
    {
        if( !contracted_subgraph[j] )
            M += X[j] * masses_sqr[j];
    }

    return M;
}


/*
 * This file is part of the Geneva library collection.
 *
 * See the NOTICE file in the top-level directory of the Geneva library
 * collection for a list of contributors and copyright information.
 *
 * The following license applies to the code IN THIS FILE:
 *
 * ***************************************************************************
 *
 * Boost Software License - Version 1.0 - August 17th, 2003
 *
 * Permission is hereby granted, free of charge, to any person or organization
 * obtaining a copy of the software and accompanying documentation covered by
 * this license (the "Software") to use, reproduce, display, distribute,
 * execute, and transmit the Software, and to prepare derivative works of the
 * Software, and to permit third-parties to whom the Software is furnished to
 * do so, all subject to the following:
 *
 * The copyright notices in the Software and this entire statement, including
 * the above license grant, this restriction and the following disclaimer,
 * must be included in all copies of the Software, in whole or in part, and
 * all derivative works of the Software, unless such copies or derivative
 * works are solely in the form of machine-executable object code generated by
 * a source language processor.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
 * FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 *
 * ***************************************************************************
 *
 * NOTE THAT THE BOOST-LICENSE DOES NOT APPLY TO ANY OTHER FILES OF THE
 * GENEVA LIBRARY, UNLESS THIS IS EXPLICITLY STATED IN THE CORRESPONDING FILE!
 */

#pragma once

// Global checks, defines and includes needed for all of Geneva
#include "common/GGlobalDefines.hpp"

// Standard headers go here
#include <cmath>
#include <cstdlib>

// Boost headers go here
#include <boost/math/special_functions.hpp>

// Geneva headers go here
#include "common/GExceptions.hpp"
#include "common/GLogger.hpp"
#include "common/GErrorStreamer.hpp"

namespace Gem {
namespace Common {

/******************************************************************************/

/** @brief Calculates the acos value of a float value */
G_API_COMMON float gacos(const float &);
/** @brief Calculates the acos value of a double value */
G_API_COMMON double gacos(const double &);

/** @brief Calculates the asin value of a float value */
G_API_COMMON float gasin(const float &);
/** @brief Calculates the asin value of a double value */
G_API_COMMON double gasin(const double &);

/** @brief Calculates the tan value of a float value */
G_API_COMMON float gtan(const float &);
/** @brief Calculates the tan value of a double value */
G_API_COMMON double gtan(const double &);

/** @brief Calculates the tanh value of a float value */
G_API_COMMON float gtanh(const float &);
/** @brief Calculates the tanh value of a double value */
G_API_COMMON double gtanh(const double &);

/** @brief Calculates the atan value of a float value */
G_API_COMMON float gatan(const float &);
/** @brief Calculates the atan value of a double value */
G_API_COMMON double gatan(const double &);

/** @brief Calculates the sinh value of a float value */
G_API_COMMON float gsinh(const float &);
/** @brief Calculates the sinh value of a double value */
G_API_COMMON double gsinh(const double &);

/** @brief Calculates the cosh value of a float value */
G_API_COMMON float gcosh(const float &);
/** @brief Calculates the cosh value of a double value */
G_API_COMMON double gcosh(const double &);

/** @brief Calculates the ceil value of a float value */
G_API_COMMON float gceil(const float &);
/** @brief Calculates the ceil value of a double value */
G_API_COMMON double gceil(const double &);

/** @brief Calculates the maximum value of two float values */
G_API_COMMON float gmax(const float &, const float &);
/** @brief Calculates the maximum value of two double values */
G_API_COMMON double gmax(const double &, const double &);

/** @brief Calculates the minimum value of two float values */
G_API_COMMON float gmin(const float &, const float &);
/** @brief Calculates the minimum value of two double values */
G_API_COMMON double gmin(const double &, const double &);

/** @brief Calculates the floor value of a float value */
G_API_COMMON float gfloor(const float &);
/** @brief Calculates the floor value of a double value */
G_API_COMMON double gfloor(const double &);
/** @brief Calculates the floor value of a double value */
G_API_COMMON long double gfloor(const long double &);

/** @brief Calculates the fabs value of a float value */
G_API_COMMON float gfabs(const float &);
/** @brief Calculates the fabs value of a double value */
G_API_COMMON double gfabs(const double &);
/** @brief Calculates the fabs value of a long double value */
G_API_COMMON long double gfabs(const long double &);

/** @brief Calculates the abs value of an int value */
G_API_COMMON int giabs(const int &);
/** @brief Calculates the abs value of a long int value */
G_API_COMMON long giabs(const long &);

/** @brief Calculates the sqrt value of a float value */
G_API_COMMON float gsqrt(const float &);
/** @brief Calculates the sqrt value of a double value */
G_API_COMMON double gsqrt(const double &);

/** @brief Calculates the sin value of a float value */
G_API_COMMON float gsin(const float &);
/** @brief Calculates the sin value of a double value */
G_API_COMMON double gsin(const double &);

/** @brief Calculates the cos value of a float value */
G_API_COMMON float gcos(const float &);
/** @brief Calculates the cos value of a double value */
G_API_COMMON double gcos(const double &);

/** @brief Calculates the log value of a float value */
G_API_COMMON float glog(const float &);
/** @brief Calculates the log value of a double value */
G_API_COMMON double glog(const double &);

/** @brief Calculates the log10 value of a float value */
G_API_COMMON float glog10(const float &);
/** @brief Calculates the log10 value of a double value */
G_API_COMMON double glog10(const double &);

/** @brief Calculates the pow value of a float value */
G_API_COMMON float gpow(const float &, const float &);
/** @brief Calculates the pow value of a double value */
G_API_COMMON double gpow(const double &, const double &);

/** @brief Calculates the hypot value of a float value */
G_API_COMMON float ghypot(const float &, const float &);
/** @brief Calculates the hypot value of a double value */
G_API_COMMON double ghypot(const double &, const double &);

/** @brief Performs alpha blending for floats */
G_API_COMMON float gmix(const float &, const float &, const float &);
/** @brief Performs alpha blending for doubles */
G_API_COMMON double gmix(const double &, const double &, const double &);

/** @brief Calculates the exp value of a float value */
G_API_COMMON float gexp(const float &);
/** @brief Calculates the exp value of a double value */
G_API_COMMON double gexp(const double &);

/** @brief Calculates the sign value of a float value */
G_API_COMMON float gsign(const float &);
/** @brief Calculates the sign value of a double value */
G_API_COMMON double gsign(const double &);

/** @brief A sigmoid function with user-defined minimum / maximum values (float version) */
G_API_COMMON float gsigmoid(const float &, const float &, const float &);
/** @brief A sigmoid function with user-defined minimum / maximum values (double version) */
G_API_COMMON double gsigmoid(const double &, const double &, const double &);

} /* namespace Common */
} /* namespace Gem */