/********************************************************************************
 * Copyright 2017 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "QuadraticSurface.hpp"

#include <rw/geometry/Delaunay.hpp>
#include <rw/geometry/PlainTriMesh.hpp>
#include <rw/geometry/Polygon.hpp>
#include <rw/geometry/PolygonUtil.hpp>
#include <rw/geometry/analytic/AnalyticUtil.hpp>
#include <rw/geometry/analytic/ImplicitTorus.hpp>
#include <rw/math/Vector2D.hpp>

#include <Eigen/Eigenvalues>
#include <list>

using rw::core::ownedPtr;
using namespace rw::geometry;
using namespace rw::math;

QuadraticSurface::QuadraticSurface (const Eigen::Diagonal< Eigen::Matrix3d >& A,
                                    const Eigen::Vector3d& a, double u,
                                    const std::vector< TrimmingRegion >& conditions) :
    _A (Eigen::DiagonalMatrix< double, 3, 3 > (A)),
    _a (a), _u (u), _determinantA (A.determinant ()), _diagonal (true), _conditions (conditions),
    _stepsPerRevolution (10)
{}

QuadraticSurface::QuadraticSurface (const Eigen::DiagonalMatrix< double, 3, 3 >& A,
                                    const Eigen::Vector3d& a, double u,
                                    const std::vector< TrimmingRegion >& conditions) :
    _A (A),
    _a (a), _u (u), _determinantA (A.diagonal ().determinant ()), _diagonal (true),
    _conditions (conditions), _stepsPerRevolution (10)
{}

QuadraticSurface::QuadraticSurface (
    const Eigen::SelfAdjointView< const Eigen::Matrix3d, Eigen::Upper >& A,
    const Eigen::Vector3d& a, double u, const std::vector< TrimmingRegion >& conditions) :
    _A (A),
    _a (a), _u (u), _determinantA (_A.determinant ()), _diagonal (false), _conditions (conditions),
    _stepsPerRevolution (10)
{}

QuadraticSurface::QuadraticSurface (
    const Eigen::SelfAdjointView< Eigen::Matrix3d, Eigen::Upper >& A, const Eigen::Vector3d& a,
    double u, const std::vector< TrimmingRegion >& conditions) :
    _A (A),
    _a (a), _u (u), _determinantA (_A.determinant ()), _diagonal (false), _conditions (conditions),
    _stepsPerRevolution (10)
{}

QuadraticSurface::QuadraticSurface (
    const Eigen::SelfAdjointView< const Eigen::Matrix3d, Eigen::Lower >& A,
    const Eigen::Vector3d& a, double u, const std::vector< TrimmingRegion >& conditions) :
    _A (A),
    _a (a), _u (u), _determinantA (_A.determinant ()), _diagonal (false), _conditions (conditions),
    _stepsPerRevolution (10)
{}

QuadraticSurface::QuadraticSurface (
    const Eigen::SelfAdjointView< Eigen::Matrix3d, Eigen::Lower >& A, const Eigen::Vector3d& a,
    double u, const std::vector< TrimmingRegion >& conditions) :
    _A (A),
    _a (a), _u (u), _determinantA (_A.determinant ()), _diagonal (false), _conditions (conditions),
    _stepsPerRevolution (10)
{}

QuadraticSurface::QuadraticSurface (const Eigen::Matrix3d& A, bool diagonal, double determinantA,
                                    const Eigen::Vector3d& a, double u,
                                    const std::vector< TrimmingRegion >& conditions,
                                    double stepsPerRevolution) :
    _A (A),
    _a (a), _u (u), _determinantA (determinantA), _diagonal (diagonal), _conditions (conditions),
    _stepsPerRevolution (stepsPerRevolution)
{}

QuadraticSurface::~QuadraticSurface ()
{}

QuadraticSurface::Ptr QuadraticSurface::transform (const Transform3D<>& T) const
{
    const Eigen::Matrix3d R    = T.R ().e ();
    const Eigen::Matrix3d Rinv = inverse (T.R ()).e ();
    const Eigen::Vector3d p    = T.P ().e ();
    std::vector< TrimmingRegion > conditions;
    for (std::size_t i = 0; i < _conditions.size (); i++)
        conditions.push_back (_conditions[i]->transform (T));
    const Eigen::Matrix3d Anew = R * _A * Rinv;
    return ownedPtr (new QuadraticSurface (Anew,
                                           false,
                                           _determinantA,
                                           R * _a - Anew * p,
                                           (Anew * p - 2 * (R * _a)).transpose () * p + _u,
                                           conditions,
                                           _stepsPerRevolution));
}

QuadraticSurface::Ptr QuadraticSurface::transform (const Vector3D<>& P) const
{
    const Eigen::Vector3d p = P.e ();
    std::vector< TrimmingRegion > conditions;
    for (std::size_t i = 0; i < _conditions.size (); i++)
        conditions.push_back (_conditions[i]->transform (Transform3D<> (P)));
    return ownedPtr (new QuadraticSurface (_A,
                                           _diagonal,
                                           _determinantA,
                                           _a - _A * p,
                                           (_A * p - 2 * _a).transpose () * p + _u,
                                           conditions,
                                           _stepsPerRevolution));
}

QuadraticSurface::Ptr QuadraticSurface::scale (double factor) const
{
    std::vector< TrimmingRegion > conditions;
    for (std::size_t i = 0; i < _conditions.size (); i++) {
        conditions.push_back (_conditions[i]->scale (factor));
    }
    return ownedPtr (new QuadraticSurface (_A,
                                           _diagonal,
                                           _determinantA,
                                           _a * factor,
                                           _u * factor * factor,
                                           conditions,
                                           _stepsPerRevolution));
}

QuadraticSurface::Ptr QuadraticSurface::clone () const
{
    std::vector< TrimmingRegion > conditions;
    for (std::size_t i = 0; i < _conditions.size (); i++) {
        conditions.push_back (_conditions[i]->clone ());
    }
    return ownedPtr (new QuadraticSurface (
        _A, _diagonal, _determinantA, _a, _u, conditions, _stepsPerRevolution));
}

std::pair< double, double > QuadraticSurface::extremums (const Vector3D<>& dir) const
{
    if (_diagonal)
        return extremumsDiagonal (dir);

    std::pair< QuadraticSurface, Rotation3D<> > diagonalized = diagonalize ();
    const Rotation3D<>& R                                    = diagonalized.second;
    QuadraticSurface& s                                      = diagonalized.first;

    return s.extremumsDiagonal (R * dir);
}

TriMesh::Ptr QuadraticSurface::getTriMesh (const std::vector< Vector3D<> >& border) const
{
    if (_diagonal)
        return getTriMeshDiagonal (border);

    std::pair< QuadraticSurface, Rotation3D<> > diagonalized = diagonalize ();
    const Rotation3D<>& R                                    = diagonalized.second;
    QuadraticSurface& s                                      = diagonalized.first;

    std::vector< Vector3D<> > borderRot (border.size ());
    for (std::size_t i = 0; i < border.size (); i++)
        borderRot[i] = R * border[i];
    return s.getTriMeshDiagonal (borderRot, inverse (R));
}

bool QuadraticSurface::equals (const Surface& surface, double threshold) const
{
    const QuadraticSurface* const qsurf = dynamic_cast< const QuadraticSurface* > (&surface);
    if (qsurf != nullptr) {
        if ((_A - qsurf->_A).cwiseAbs ().maxCoeff () > threshold)
            return false;
        if ((_a - qsurf->_a).cwiseAbs ().maxCoeff () > threshold)
            return false;
        if (std::fabs (_u - qsurf->_u) > threshold)
            return false;
        std::vector< bool > omatched (qsurf->_conditions.size (), false);
        for (const QuadraticSurface::TrimmingRegion treg : _conditions) {
            bool tmatched = false;
            for (std::size_t i = 0; i < qsurf->_conditions.size (); i++) {
                if (treg->equals (*qsurf->_conditions[i], threshold)) {
                    tmatched    = true;
                    omatched[i] = true;
                }
            }
            if (!tmatched)
                return false;
        }
        for (const bool match : omatched) {
            if (!match)
                return false;
        }
        return true;
    }
    else {
        return false;
    }
}

double QuadraticSurface::operator() (const Vector3D<>& p) const
{
    return p.e ().transpose () * _A * p.e () + _a.dot (p.e ()) * 2 + _u;
}

bool QuadraticSurface::insideTrimmingRegion (const Vector3D<>& P) const
{
    for (std::size_t i = 0; i < _conditions.size (); i++) {
        if ((*_conditions[i]) (P) > 0)
            return false;
    }
    return true;
}

Vector3D<> QuadraticSurface::normal (const Vector3D<>& point) const
{
    return Vector3D<> (_A * point.e () + _a) * 2;
}

Vector3D<> QuadraticSurface::gradient (const Vector3D<>& point) const
{
    return Vector3D<> (_A * point.e () + _a) * 2;
}

void QuadraticSurface::reuseTrimmingRegions (const ImplicitSurface::Ptr surface) const
{
    const QuadraticSurface::Ptr qsurf = surface.cast< QuadraticSurface > ();
    if (!qsurf.isNull ()) {
        for (std::size_t i = 0; i < qsurf->_conditions.size (); i++) {
            const QuadraticSurface::TrimmingRegion reg = qsurf->_conditions[i];
            for (const QuadraticSurface::TrimmingRegion treg : _conditions) {
                if (reg->equals (*treg, 1e-15)) {
                    qsurf->_conditions[i] = treg;
                    break;
                }
            }
        }
    }
    const ImplicitTorus::Ptr tsurf = surface.cast< ImplicitTorus > ();
    if (!tsurf.isNull ()) {
        std::vector< ImplicitTorus::TrimmingRegion > regs = tsurf->getTrimmingConditions ();
        for (std::size_t i = 0; i < regs.size (); i++) {
            const ImplicitTorus::TrimmingRegion reg = regs[i];
            for (const ImplicitTorus::TrimmingRegion treg : _conditions) {
                if (reg->equals (*treg, 1e-15)) {
                    regs[i] = treg;
                    break;
                }
            }
        }
        tsurf->setTrimmingConditions (regs);
    }
}

QuadraticSurface::Ptr QuadraticSurface::normalize () const
{
    std::vector< TrimmingRegion > conditions;
    for (std::size_t i = 0; i < _conditions.size (); i++) {
        conditions.push_back (_conditions[i]->clone ());
    }
    const double normFactor =
        std::max (_A.cwiseAbs ().maxCoeff (), std::max (_a.cwiseAbs ().maxCoeff (), std::abs (_u)));
    return ownedPtr (new QuadraticSurface (_A / normFactor,
                                           _diagonal,
                                           _determinantA / std::pow (normFactor, 3),
                                           _a / normFactor,
                                           _u / normFactor,
                                           conditions,
                                           _stepsPerRevolution));
}

std::pair< QuadraticSurface, Rotation3D<> > QuadraticSurface::diagonalize () const
{
    if (_diagonal)
        return std::make_pair (*this, Rotation3D<>::identity ());
    const Eigen::SelfAdjointEigenSolver< Eigen::Matrix3d > eigen (_A);
    Rotation3D<> R (eigen.eigenvectors ().transpose ());
    if (!R.isProperRotation (2 * std::numeric_limits< double >::epsilon ())) {
        R (2, 0) *= -1;
        R (2, 1) *= -1;
        R (2, 2) *= -1;
    }
    if (!R.isProperRotation (2 * std::numeric_limits< double >::epsilon ()))
        RW_THROW ("Could not create proper rotation from eigenvalues!");
    std::vector< TrimmingRegion > conditions;
    for (std::size_t i = 0; i < _conditions.size (); i++)
        conditions.push_back (_conditions[i]->transform (Transform3D<> (R)));
    return std::make_pair (
        QuadraticSurface (Eigen::DiagonalMatrix< double, 3, 3 > (eigen.eigenvalues ()),
                          _diagonal,
                          _determinantA,
                          _a.transpose () * inverse (R).e (),
                          _u,
                          conditions,
                          _stepsPerRevolution),
        R);
}

namespace {
static const double EPS = std::numeric_limits< double >::epsilon () * 5;
}

TriMesh::Ptr QuadraticSurface::getTriMeshDiagonal (const std::vector< Vector3D<> >& border,
                                                   const Rotation3D<>& R) const
{
    const PlainTriMeshN1D::Ptr mesh = ownedPtr (new PlainTriMeshN1D ());

    // Choose indices
    std::size_t u = 3;
    std::size_t v = 3;
    std::size_t e = 3;
    if (std::fabs (_A (2, 2)) < EPS && std::fabs (_a[2]) > EPS) {
        u = 0;
        v = 1;
        e = 2;
    }
    else if (std::fabs (_A (1, 1)) < EPS && std::fabs (_a[1]) > EPS) {
        u = 2;
        v = 0;
        e = 1;
    }
    else if (std::fabs (_A (0, 0)) < EPS && std::fabs (_a[0]) > EPS) {
        u = 1;
        v = 2;
        e = 0;
    }
    else if (std::fabs (_A (2, 2)) >= EPS) {
        u = 0;
        v = 1;
        e = 2;
    }
    else if (std::fabs (_A (1, 1)) >= EPS) {
        u = 2;
        v = 0;
        e = 1;
    }
    else if (std::fabs (_A (0, 0)) >= EPS) {
        u = 1;
        v = 2;
        e = 0;
    }
    if (u > 2 || v > 2 || e > 2)
        RW_THROW ("Error happened. Could not choose explicit variable!");

    // Split border polygon in front and back (when seen in uv plane)
    if (std::fabs (_A (e, e)) >= EPS) {
        const double eSplit = -_a[e] / _A (e, e);

        std::list< std::vector< std::size_t > > borderFront;
        std::list< std::vector< std::size_t > > borderBack;
        // Front
        {
            Vector3D<> last = border.front ();
            std::vector< std::size_t > cur;
            if (last[e] >= eSplit - EPS)
                cur.push_back (0);
            for (std::size_t i = 1; i < border.size (); i++) {
                if (last[e] >= eSplit - EPS && border[i][e] >= eSplit - EPS) {
                    cur.push_back (i);
                }
                else if (last[e] < eSplit - EPS) {
                    if (cur.size () > 0) {
                        borderFront.push_back (cur);
                        cur.clear ();
                    }
                    if (border[i][e] >= eSplit - EPS)
                        cur.push_back (i);
                }
                last = border[i];
            }
            if (last[e] >= eSplit - EPS) {
                if (border.front ()[e] >= eSplit - EPS && borderFront.size () > 0) {
                    borderFront.front ().insert (
                        borderFront.front ().begin (), cur.begin (), cur.end ());
                    cur.clear ();
                }
            }
            if (cur.size () > 0) {
                borderFront.push_back (cur);
            }
        }
        // Back
        {
            Vector3D<> last = border.front ();
            std::vector< std::size_t > cur;
            if (last[e] <= eSplit + EPS)
                cur.push_back (0);
            for (std::size_t i = 1; i < border.size (); i++) {
                if (last[e] <= eSplit + EPS && border[i][e] <= eSplit + EPS) {
                    cur.push_back (i);
                }
                else if (last[e] > eSplit + EPS) {
                    if (cur.size () > 0) {
                        borderBack.push_back (cur);
                        cur.clear ();
                    }
                    if (border[i][e] <= eSplit + EPS)
                        cur.push_back (i);
                }
                last = border[i];
            }
            if (last[e] <= eSplit + EPS) {
                if (border.front ()[e] <= eSplit + EPS && borderBack.size () > 0) {
                    borderBack.front ().insert (
                        borderBack.front ().begin (), cur.begin (), cur.end ());
                    cur.clear ();
                }
            }
            if (cur.size () > 0) {
                borderBack.push_back (cur);
            }
        }

        // If there are front border segments lying completely inside the +/- EPS region, remove
        // them if they are part of a larger back segment.
        {
            std::list< std::vector< std::size_t > >& listA       = borderFront;
            const std::list< std::vector< std::size_t > >& listB = borderBack;

            for (std::list< std::vector< std::size_t > >::iterator it = listA.begin ();
                 it != listA.end ();) {    // do not use const_iterator (first supported with
                                           // list.erase() from GCC 4.9)
                bool within = true;
                for (std::size_t i = 0; i < (*it).size () && within; i++) {
                    if (std::abs (border[(*it)[i]][e] - eSplit) > EPS) {
                        within = false;
                    }
                }
                if (within) {
                    bool match = false;
                    for (std::list< std::vector< std::size_t > >::const_iterator itB =
                             listB.begin ();
                         itB != listB.end () && !match;
                         itB++) {
                        bool outsideB = false;
                        for (std::size_t i = 0; i < (*itB).size () && !outsideB; i++) {
                            if (std::abs (border[(*itB)[i]][e] - eSplit) > EPS) {
                                outsideB = true;
                            }
                        }
                        if (outsideB) {
                            bool firstMatched = false;
                            std::size_t i     = 0;
                            for (std::size_t j = 0; j <= (*itB).size () && !match; j++) {
                                if (j == (*itB).size ()) {
                                    if (firstMatched)
                                        j = 0;
                                    else
                                        break;
                                }
                                if (!firstMatched) {
                                    if ((*itB)[j] == (*it)[i]) {
                                        firstMatched = true;
                                        i++;
                                    }
                                }
                                else {
                                    if (i >= (*it).size ()) {
                                        match = true;
                                    }
                                    else if ((*itB)[j] != (*it)[i]) {
                                        match = false;
                                        break;
                                    }
                                    else if ((*itB)[j] == (*it)[i]) {
                                        i++;
                                    }
                                }
                            }
                        }
                    }
                    if (match) {
                        it = listA.erase (it);
                    }
                    else {
                        it++;
                    }
                }
                else {
                    it++;
                }
            }
        }
        // If there are back border segments lying completely inside the +/- EPS region, remove them
        // if they are part of a larger front segment.
        {
            std::list< std::vector< std::size_t > >& listA       = borderBack;
            const std::list< std::vector< std::size_t > >& listB = borderFront;

            for (std::list< std::vector< std::size_t > >::iterator it = listA.begin ();
                 it != listA.end ();) {    // do not use const_iterator (first supported with
                                           // list.erase() from GCC 4.9)
                bool within = true;
                for (std::size_t i = 0; i < (*it).size () && within; i++) {
                    if (std::abs (border[(*it)[i]][e] - eSplit) > EPS) {
                        within = false;
                    }
                }
                if (within) {
                    bool match = false;
                    for (std::list< std::vector< std::size_t > >::const_iterator itB =
                             listB.begin ();
                         itB != listB.end () && !match;
                         itB++) {
                        bool outsideB = false;
                        for (std::size_t i = 0; i < (*itB).size () && !outsideB; i++) {
                            if (std::abs (border[(*itB)[i]][e] - eSplit) > EPS) {
                                outsideB = true;
                            }
                        }
                        if (outsideB) {
                            bool firstMatched = false;
                            std::size_t i     = 0;
                            for (std::size_t j = 0; j <= (*itB).size () && !match; j++) {
                                if (j == (*itB).size ()) {
                                    if (firstMatched)
                                        j = 0;
                                    else
                                        break;
                                }
                                if (!firstMatched) {
                                    if ((*itB)[j] == (*it)[i]) {
                                        firstMatched = true;
                                        i++;
                                    }
                                }
                                else {
                                    if (i >= (*it).size ()) {
                                        match = true;
                                    }
                                    else if ((*itB)[j] != (*it)[i]) {
                                        match = false;
                                        break;
                                    }
                                    else if ((*itB)[j] == (*it)[i]) {
                                        i++;
                                    }
                                }
                            }
                        }
                    }
                    if (match) {
                        it = listA.erase (it);
                    }
                    else {
                        it++;
                    }
                }
                else {
                    it++;
                }
            }
        }

        // Find silhouette of surface
        std::vector< QuadraticCurve > silhouette = findSilhouette (u, v, e, eSplit);

        // Try to connect polygons if open
        const std::list< std::vector< Vector3D<> > > fullPolygonFront =
            AnalyticUtil::combinePolygons (border, borderFront, silhouette, _stepsPerRevolution);
        const std::list< std::vector< Vector3D<> > > fullPolygonBack =
            AnalyticUtil::combinePolygons (border, borderBack, silhouette, _stepsPerRevolution);

        for (std::list< std::vector< Vector3D<> > >::const_iterator it = fullPolygonFront.begin ();
             it != fullPolygonFront.end ();
             it++) {
            makeSurface (*it, u, v, e, eSplit, FRONT, R, mesh);
        }
        for (std::list< std::vector< Vector3D<> > >::const_iterator it = fullPolygonBack.begin ();
             it != fullPolygonBack.end ();
             it++) {
            makeSurface (*it, u, v, e, eSplit, BACK, R, mesh);
        }
    }
    else {
        makeSurface (border, u, v, e, 0, BOTH, R, mesh);
    }

    return mesh;
}

std::vector< QuadraticCurve > QuadraticSurface::findSilhouette (std::size_t u, std::size_t v,
                                                                std::size_t e, double eSplit) const
{
    // Find silhouette of surface
    std::vector< QuadraticCurve > silhouette;
    if (std::fabs (_A (u, u)) < EPS && std::fabs (_a[u]) < EPS) {
        const double s = _a[v] * _a[v] - (_u - _a[e] * _a[e] / _A (e, e)) * _A (v, v);
        if (s >= 0) {
            const double vVal1 = (-_a[v] + std::sqrt (s)) / _A (v, v);
            const double vVal2 = (-_a[v] - std::sqrt (s)) / _A (v, v);
            const Vector3D<> c1 ((u == 0) ? 0 : (v == 0) ? vVal1 : eSplit,
                                 (u == 1) ? 0 : (v == 1) ? vVal1 : eSplit,
                                 (u == 2) ? 0 : (v == 2) ? vVal1 : eSplit);
            const Vector3D<> c2 ((u == 0) ? 0 : (v == 0) ? vVal2 : eSplit,
                                 (u == 1) ? 0 : (v == 1) ? vVal2 : eSplit,
                                 (u == 2) ? 0 : (v == 2) ? vVal2 : eSplit);
            const Vector3D<> uDir ((u == 0) ? 1 : 0, (u == 1) ? 1 : 0, (u == 2) ? 1 : 0);
            const Vector3D<> vDir ((v == 0) ? 1 : 0, (v == 1) ? 1 : 0, (v == 2) ? 1 : 0);
            silhouette.push_back (QuadraticCurve (c1, uDir, vDir, QuadraticCurve::Line));
            if (s != 0)
                silhouette.push_back (QuadraticCurve (c2, uDir, vDir, QuadraticCurve::Line));
        }
    }
    else if (std::fabs (_A (v, v)) < EPS && std::fabs (_a[v]) < EPS) {
        const double s = _a[u] * _a[u] - (_u - _a[e] * _a[e] / _A (e, e)) * _A (u, u);
        if (s >= 0) {
            const double uVal1 = (-_a[u] + std::sqrt (s)) / _A (u, u);
            const double uVal2 = (-_a[u] - std::sqrt (s)) / _A (u, u);
            const Vector3D<> c1 ((u == 0) ? uVal1 : (v == 0) ? 0 : eSplit,
                                 (u == 1) ? uVal1 : (v == 1) ? 0 : eSplit,
                                 (u == 2) ? uVal1 : (v == 2) ? 0 : eSplit);
            const Vector3D<> c2 ((u == 0) ? uVal2 : (v == 0) ? 0 : eSplit,
                                 (u == 1) ? uVal2 : (v == 1) ? 0 : eSplit,
                                 (u == 2) ? uVal2 : (v == 2) ? 0 : eSplit);
            const Vector3D<> uDir ((u == 0) ? 1 : 0, (u == 1) ? 1 : 0, (u == 2) ? 1 : 0);
            const Vector3D<> vDir ((v == 0) ? 1 : 0, (v == 1) ? 1 : 0, (v == 2) ? 1 : 0);
            silhouette.push_back (QuadraticCurve (c1, vDir, uDir, QuadraticCurve::Line));
            if (s != 0)
                silhouette.push_back (QuadraticCurve (c2, vDir, uDir, QuadraticCurve::Line));
        }
    }
    else if (std::fabs (_A (u, u)) < EPS) {
        RW_THROW ("Polynomial not yet implemented!");
    }
    else if (std::fabs (_A (v, v)) < EPS) {
        RW_THROW ("Polynomial not yet implemented!");
    }
    else if (_A (u, u) > 0 && _A (v, v) > 0) {
        // Ellipse
        const std::size_t ru = (_A (u, u) > 0) ? u : v;
        const std::size_t rv = (_A (u, u) > 0) ? v : u;
        const double ruValC  = -_a[ru] / _A (ru, ru);
        const double rvValC  = -_a[rv] / _A (rv, rv);
        const double Ke      = _A (e, e) * eSplit * eSplit + 2. * _a[e] * eSplit + _u;
        const double b       = std::sqrt (
            (_a[ru] * _a[ru] / _A (ru, ru) * 2 + _a[rv] * _a[rv] / _A (rv, rv) - Ke) / _A (rv, rv));
        const double a = std::sqrt (b * b * _A (rv, rv) / _A (ru, ru));
        Vector3D<> c;
        Vector3D<> uDir;
        Vector3D<> vDir;
        if (_A (u, u) > 0) {
            c    = Vector3D<> ((u == 0) ? ruValC : (v == 0) ? rvValC : eSplit,
                            (u == 1) ? ruValC : (v == 1) ? rvValC : eSplit,
                            (u == 2) ? ruValC : (v == 2) ? rvValC : eSplit);
            uDir = Vector3D<> ((v == 0) ? 1 : 0, (v == 1) ? 1 : 0, (v == 2) ? 1 : 0) * a;
            vDir = Vector3D<> ((u == 0) ? 1 : 0, (u == 1) ? 1 : 0, (u == 2) ? 1 : 0) * b;
        }
        else {
            c    = Vector3D<> ((u == 0) ? rvValC : (v == 0) ? ruValC : eSplit,
                            (u == 1) ? rvValC : (v == 1) ? ruValC : eSplit,
                            (u == 2) ? rvValC : (v == 2) ? ruValC : eSplit);
            uDir = Vector3D<> ((u == 0) ? 1 : 0, (u == 1) ? 1 : 0, (u == 2) ? 1 : 0) * b;
            vDir = Vector3D<> ((v == 0) ? 1 : 0, (v == 1) ? 1 : 0, (v == 2) ? 1 : 0) * a;
        }
        silhouette.push_back (QuadraticCurve (c, uDir, vDir, QuadraticCurve::Elliptic));
        silhouette.push_back (QuadraticCurve (c, uDir, -vDir, QuadraticCurve::Elliptic));
    }
    else {
        // Hyperbola
        const std::size_t ru = (_A (u, u) > 0) ? u : v;
        const std::size_t rv = (_A (u, u) > 0) ? v : u;
        const double ruValC  = -_a[ru] / _A (ru, ru);
        const double rvValC  = -_a[rv] / _A (rv, rv);
        const double Ke      = _A (e, e) * eSplit * eSplit + 2. * _a[e] * eSplit + _u;
        const double b = std::sqrt ((_a[ru] - _a[rv] * _a[rv] / _A (rv, rv) + Ke) / _A (rv, rv));
        const double a = std::sqrt (-b * b * _A (rv, rv) / _A (ru, ru));
        Vector3D<> c;
        Vector3D<> uDir;
        Vector3D<> vDir;
        if (_A (u, u) > 0) {
            c    = Vector3D<> ((u == 0) ? ruValC : (v == 0) ? rvValC : eSplit,
                            (u == 1) ? ruValC : (v == 1) ? rvValC : eSplit,
                            (u == 2) ? ruValC : (v == 2) ? rvValC : eSplit);
            uDir = Vector3D<> ((v == 0) ? 1 : 0, (v == 1) ? 1 : 0, (v == 2) ? 1 : 0) * a;
            vDir = Vector3D<> ((u == 0) ? 1 : 0, (u == 1) ? 1 : 0, (u == 2) ? 1 : 0) * b;
        }
        else {
            c    = Vector3D<> ((u == 0) ? rvValC : (v == 0) ? ruValC : eSplit,
                            (u == 1) ? rvValC : (v == 1) ? ruValC : eSplit,
                            (u == 2) ? rvValC : (v == 2) ? ruValC : eSplit);
            uDir = Vector3D<> ((u == 0) ? 1 : 0, (u == 1) ? 1 : 0, (u == 2) ? 1 : 0) * b;
            vDir = Vector3D<> ((v == 0) ? 1 : 0, (v == 1) ? 1 : 0, (v == 2) ? 1 : 0) * a;
        }
        silhouette.push_back (QuadraticCurve (c, uDir, vDir, QuadraticCurve::Hyperbola));
        silhouette.push_back (QuadraticCurve (c, uDir, -vDir, QuadraticCurve::Hyperbola));
    }
    return silhouette;
}

void QuadraticSurface::makeSurface (const std::vector< Vector3D<> > fullPolygon, std::size_t u,
                                    std::size_t v, std::size_t e, double eSplit, Place place,
                                    const Rotation3D<>& R, PlainTriMeshN1D::Ptr mesh) const
{
    // Project border
    Polygon< Vector2D<> > polygon;
    for (std::size_t i = 0; i < fullPolygon.size (); i++) {
        polygon.addVertex (Vector2D<> (fullPolygon[i][u], fullPolygon[i][v]));
    }

    // Do convex decomposition
    if (polygon.size () < 3)
        RW_THROW ("Can not decompose polygon that has " << polygon.size () << " vertices.");
    const std::vector< std::vector< std::size_t > > convexPolygonsIndexed =
        PolygonUtil::convexDecompositionIndexed (polygon);

    std::vector< Polygon< Vector2D<> > > convexPolygons (convexPolygonsIndexed.size ());
    {
        // Construct plain polygon
        std::size_t i = 0;
        for (std::vector< std::vector< std::size_t > >::const_iterator it =
                 convexPolygonsIndexed.begin ();
             it != convexPolygonsIndexed.end ();
             it++) {
            const std::vector< std::size_t >& subPoly = *it;
            for (std::size_t k = 0; k < subPoly.size (); k++) {
                convexPolygons[i].addVertex (polygon[subPoly[k]]);
                // Check if edge is shared with other polygon
                const std::size_t idxNext = subPoly[(k + 1) % subPoly.size ()];
                bool inOther              = false;
                for (std::vector< std::vector< std::size_t > >::const_iterator itB =
                         convexPolygonsIndexed.begin ();
                     itB != convexPolygonsIndexed.end () && inOther == false;
                     itB++) {
                    if (itB == it)
                        continue;
                    const std::vector< std::size_t >& subPolyB = *itB;
                    for (std::size_t kB = 0; kB < subPolyB.size () && inOther == false; kB++) {
                        const std::size_t idxNextB = subPolyB[(kB + 1) % subPolyB.size ()];
                        if (subPolyB[kB] == subPoly[k] && idxNextB == idxNext)
                            inOther = true;
                        else if (subPolyB[kB] == idxNext && idxNextB == subPoly[k])
                            inOther = true;
                    }
                }
                if (inOther) {
                    // Sample inner border
                    const Vector2D<> dp = polygon[idxNext] - polygon[subPoly[k]];
                    std::size_t samples = 20;
                    if (_A (e, e) < EPS && _A (u, u) < EPS && _A (v, v) < EPS)
                        samples = 0;    // no samples if surface is a plane!
                    for (std::size_t sI = 1; sI < samples; sI++) {
                        convexPolygons[i].addVertex (polygon[subPoly[k]] +
                                                     dp * ((double) sI / samples));
                    }
                }
            }
            i++;
        }
    }

    // Sample surface points
    double minU = convexPolygons[0][0][0];
    double maxU = convexPolygons[0][0][0];
    double minV = convexPolygons[0][0][1];
    double maxV = convexPolygons[0][0][1];
    for (std::size_t i = 0; i < convexPolygons.size (); i++) {
        const Polygon< Vector2D<> >& convexPoly = convexPolygons[i];
        for (std::size_t k = 0; k < convexPoly.size (); k++) {
            if (convexPoly[k][0] < minU)
                minU = convexPoly[k][0];
            if (convexPoly[k][0] > maxU)
                maxU = convexPoly[k][0];
            if (convexPoly[k][1] < minV)
                minV = convexPoly[k][1];
            if (convexPoly[k][1] > maxV)
                maxV = convexPoly[k][1];
        }
    }

    static const std::size_t nrPoints = 20;
    std::size_t nrPointsU             = 1;
    std::size_t nrPointsV             = 1;
    if (std::abs (_A (u, u)) >= EPS || std::abs (_A (v, v)) >= EPS) {
        nrPointsU = 2;
        nrPointsV = 2;
    }
    if (std::abs (_A (u, u)) >= EPS)
        nrPointsU = nrPoints;
    if (std::abs (_A (v, v)) >= EPS)
        nrPointsV = nrPoints;
    const double dU = (maxU - minU) / nrPointsU;
    const double dV = (maxV - minV) / nrPointsV;
    std::vector< Vector2D<> > surfacePoints ((nrPointsU - 1) * (nrPointsV - 1));
    for (std::size_t ui = 1; ui < nrPointsU; ui++) {
        const double u = minU + dU * ui;
        for (std::size_t vi = 1; vi < nrPointsV; vi++) {
            const double v                                        = minV + dV * vi;
            surfacePoints[(ui - 1) * (nrPointsV - 1) + vi - 1][0] = u;
            surfacePoints[(ui - 1) * (nrPointsV - 1) + vi - 1][1] = v;
        }
    }

    for (std::size_t i = 0; i < convexPolygons.size (); i++) {
        const Polygon< Vector2D<> >& convexPoly = convexPolygons[i];

        std::vector< Vector2D<> > points2d (convexPoly.size ());
        for (std::size_t pi = 0; pi < convexPoly.size (); pi++) {
            points2d[pi] = convexPoly[pi];
        }

        // Take the surface points in the convex region
        for (std::size_t k = 0; k < surfacePoints.size (); k++) {
            if (PolygonUtil::isInsideConvex (surfacePoints[k], convexPoly, 1e-1)) {
                points2d.push_back (surfacePoints[k]);
            }
        }

        // Triangulate with Delaunay
        const IndexedTriMesh<>::Ptr meshProj = Delaunay::triangulate (points2d);

        if (std::fabs (_A (2, 2)) < EPS && std::fabs (_A (1, 1)) < EPS &&
            std::fabs (_A (0, 0)) < EPS) {
            // Unproject
            for (std::size_t triI = 0; triI < meshProj->size (); triI++) {
                Triangle< double > tri = meshProj->getTriangle (triI);
                bool br                = false;
                for (std::size_t pi = 0; pi < 3; pi++) {
                    const double uVal = tri[pi][0];
                    const double vVal = tri[pi][1];
                    double eVal       = -_a[u] * 2 * uVal - _a[v] * 2 * vVal - _u;
                    eVal /= _a[e] * 2;
                    if ((place == FRONT && eVal < eSplit) || (place == BACK && eVal > eSplit)) {
                        br = true;
                        break;
                    }
                    tri[pi] = R * Vector3D<> ((u == 0) ? uVal : (v == 0) ? vVal : eVal,
                                              (u == 1) ? uVal : (v == 1) ? vVal : eVal,
                                              (u == 2) ? uVal : (v == 2) ? vVal : eVal);
                }
                if (br)
                    continue;
                const double nu         = _a[u];
                const double nv         = _a[v];
                const double ne         = _a[e];
                const Vector3D<> normal = R * Vector3D<> ((u == 0) ? nu : (v == 0) ? nv : ne,
                                                          (u == 1) ? nu : (v == 1) ? nv : ne,
                                                          (u == 2) ? nu : (v == 2) ? nv : ne);
                if (dot (tri.calcFaceNormal (), normal) < 0) {
                    mesh->add (TriangleN1<> (tri[0], tri[2], tri[1], normal));
                }
                else {
                    mesh->add (TriangleN1<> (tri[0], tri[1], tri[2], normal));
                }
            }
        }
        else if (std::fabs (_A (e, e)) < EPS && std::fabs (_a[e]) > EPS) {
            // Unproject
            for (std::size_t triI = 0; triI < meshProj->size (); triI++) {
                Triangle< double > tri = meshProj->getTriangle (triI);
                Vector2D<> avg (0, 0);
                bool br = false;
                for (std::size_t pi = 0; pi < 3; pi++) {
                    const double uVal = tri[pi][0];
                    const double vVal = tri[pi][1];
                    double eVal       = -_A (u, u) * uVal * uVal - _A (v, v) * vVal * vVal -
                                  _a[u] * 2 * uVal - _a[v] * 2 * vVal - _u;
                    eVal /= _a[e] * 2;
                    if ((place == FRONT && eVal < eSplit) || (place == BACK && eVal > eSplit)) {
                        br = true;
                        break;
                    }
                    tri[pi] = R * Vector3D<> ((u == 0) ? uVal : (v == 0) ? vVal : eVal,
                                              (u == 1) ? uVal : (v == 1) ? vVal : eVal,
                                              (u == 2) ? uVal : (v == 2) ? vVal : eVal);
                    avg += Vector2D<> (uVal, vVal);
                }
                if (br)
                    continue;
                avg /= 3;
                const double nu         = _A (u, u) * avg[0] + _a[u];
                const double nv         = _A (v, v) * avg[1] + _a[v];
                const double ne         = _a[e];
                const Vector3D<> normal = R * Vector3D<> ((u == 0) ? nu : (v == 0) ? nv : ne,
                                                          (u == 1) ? nu : (v == 1) ? nv : ne,
                                                          (u == 2) ? nu : (v == 2) ? nv : ne);
                if (dot (tri.calcFaceNormal (), normal) < 0) {
                    mesh->add (TriangleN1<> (tri[0], tri[2], tri[1], normal));
                }
                else {
                    mesh->add (TriangleN1<> (tri[0], tri[1], tri[2], normal));
                }
            }
        }
        else if (std::fabs (_A (e, e)) > EPS) {
            // Unproject
            for (std::size_t triI = 0; triI < meshProj->size (); triI++) {
                Triangle< double > tri = meshProj->getTriangle (triI);
                Vector3D<> avg (0, 0, 0);
                bool br   = false;
                bool zero = false;
                for (std::size_t pi = 0; pi < 3; pi++) {
                    const double uVal = tri[pi][0];
                    const double vVal = tri[pi][1];
                    double eVal       = _a[e] * _a[e] / _A (e, e) / _A (e, e) -
                                  (_A (u, u) * uVal * uVal + _A (v, v) * vVal * vVal +
                                   _a[u] * uVal * 2 + _a[v] * vVal * 2 + _u) /
                                      _A (e, e);
                    if (eVal >= 0) {
                        if (place == FRONT)
                            eVal = -_a[e] / _A (e, e) + std::sqrt (eVal);
                        else if (place == BACK)
                            eVal = -_a[e] / _A (e, e) - std::sqrt (eVal);
                    }
                    else if (eVal >= -EPS) {
                        eVal = -_a[e] / _A (e, e);
                    }
                    else {
                        // RW_WARN("For triangle " << tri << ", the explicit value under square root
                        // was negative (" << eVal << ") assuming split value!");
                        zero = true;
                        // br = true;
                        eVal = eSplit;
                    }
                    if ((place == FRONT && eVal < eSplit) || (place == BACK && eVal > eSplit)) {
                        // RW_WARN("For triangle " << tri << ", the explicit value was not on right
                        // side front/back (" << eVal << ")!");
                        eVal = eSplit;
                        br   = true;
                        break;
                    }
                    tri[pi] = R * Vector3D<> ((u == 0) ? uVal : (v == 0) ? vVal : eVal,
                                              (u == 1) ? uVal : (v == 1) ? vVal : eVal,
                                              (u == 2) ? uVal : (v == 2) ? vVal : eVal);
                    avg += Vector3D<> (uVal, vVal, eVal);
                }
                if (br)
                    continue;
                avg /= 3;
                const double nu = _A (u, u) * avg[0] + _a[u];
                const double nv = _A (v, v) * avg[1] + _a[v];
                const double ne = _A (e, e) * avg[2] + _a[e];
                Vector3D<> normal;
                if (!zero)
                    normal = R * Vector3D<> ((u == 0) ? nu : (v == 0) ? nv : ne,
                                             (u == 1) ? nu : (v == 1) ? nv : ne,
                                             (u == 2) ? nu : (v == 2) ? nv : ne);
                else if (place == FRONT)
                    normal = R * Vector3D<> ((e == 0) ? 1 : 0, (e == 1) ? 1 : 0, (e == 2) ? 1 : 0);
                else if (place == BACK)
                    normal =
                        R * Vector3D<> ((e == 0) ? -1 : 0, (e == 1) ? -1 : 0, (e == 2) ? -1 : 0);
                if (dot (tri.calcFaceNormal (), normal) < 0) {
                    mesh->add (TriangleN1<> (tri[0], tri[2], tri[1], normal));
                }
                else {
                    mesh->add (TriangleN1<> (tri[0], tri[1], tri[2], normal));
                }
            }
        }
    }
}

std::pair< double, double > QuadraticSurface::extremumsDiagonal (const Vector3D<>& dir) const
{
    std::size_t u = 3;
    std::size_t v = 3;
    std::size_t e = 3;
    if (std::fabs (_A (2, 2)) < EPS && std::fabs (_a[2]) > EPS) {
        u = 0;
        v = 1;
        e = 2;
    }
    else if (std::fabs (_A (1, 1)) < EPS && std::fabs (_a[1]) > EPS) {
        u = 2;
        v = 0;
        e = 1;
    }
    else if (std::fabs (_A (0, 0)) < EPS && std::fabs (_a[0]) > EPS) {
        u = 1;
        v = 2;
        e = 0;
    }
    else if (std::fabs (_A (2, 2)) >= EPS) {
        u = 0;
        v = 1;
        e = 2;
    }
    else if (std::fabs (_A (1, 1)) >= EPS) {
        u = 2;
        v = 0;
        e = 1;
    }
    else if (std::fabs (_A (0, 0)) >= EPS) {
        u = 1;
        v = 2;
        e = 0;
    }
    if (u > 2 || v > 2 || e > 2)
        RW_THROW ("Error happened. Could not choose explicit variable!");

    std::pair< double, double > extremums;
    extremums.first  = -std::numeric_limits< double >::max ();
    extremums.second = std::numeric_limits< double >::max ();
    if (std::fabs (_A (e, e)) < EPS && std::fabs (_a[e]) > EPS) {
        if (std::fabs (dir[e]) < EPS) {
            // Skipping due to e zero element.
            return extremums;
        }
        if (std::fabs (_A (u, u)) < EPS || std::fabs (_A (v, v)) < EPS) {
            // - No extremum
        }
        else {
            if (_A (u, u) * _A (v, v) < 0) {
                // sadle - not extremum
            }
            else {
                const double xu = (dir[u] * _a[e] - dir[e] * _a[u]) / (dir[e] * _A (u, u));
                const double xv = (dir[v] * _a[e] - dir[e] * _a[v]) / (dir[e] * _A (v, v));
                const double xe = -(_A (u, u) * xu * xu + _A (v, v) * xv * xv + _a[u] * xu * 2 +
                                    _a[v] * xv * 2 + _u) /
                                  (_a[e] * 2);
                const Vector3D<> P = Vector3D<> ((u == 0) ? xu : (v == 0) ? xv : xe,
                                                 (u == 1) ? xu : (v == 1) ? xv : xe,
                                                 (u == 2) ? xu : (v == 2) ? xv : xe);
                if (insideTrimmingRegion (P)) {
                    const double extremum = dir[u] * xu + dir[v] * xv + dir[e] * xe;
                    if (dir[e] * _A (u, u) / _a (e) > 0) {
                        extremums.second = extremum;
                    }
                    else {
                        extremums.first = extremum;
                    }
                }
            }
        }
    }
    else if (std::fabs (_A (e, e)) >= EPS) {
        std::vector< Vector3D<> > x;
        if (((_A (u, u) >= EPS) || (_A (u, u) < EPS && _a[u] < EPS)) &&
            ((_A (v, v) >= EPS) || (_A (v, v) < EPS && _a[v] < EPS))) {
            if (std::fabs (dir[u]) < EPS && std::fabs (dir[v]) < EPS) {
                const double xu = (_A (u, u) < EPS) ? 0 : -_a[u] / _A (u, u);
                const double xv = (_A (v, v) < EPS) ? 0 : -_a[v] / _A (v, v);
                double s        = _a[e] / _A (e, e) * _a[e] / _A (e, e) -
                           (_A (u, u) * xu * xu + _A (v, v) * xv * xv + _a[u] * xu * 2 +
                            _a[v] * xv * 2 + _u) /
                               _A (e, e);
                if (s < 0)
                    RW_THROW ("What should happen here?");
                const double xe1 = -_a[e] / _A (e, e) + std::sqrt (s);
                const double xe2 = -_a[e] / _A (e, e) - std::sqrt (s);
                x.push_back (Vector3D<> (xu, xv, xe1));
                x.push_back (Vector3D<> (xu, xv, xe2));
            }
            else {
                const double Auv = _A (u, u) * _A (v, v);
                const double K1 =
                    _a[e] * _a[e] * Auv +
                    _A (e, e) * (_A (u, u) * _a[v] * _a[v] + _a[u] * _a[u] * _A (v, v) - Auv * _u);
                const double K2 = Auv * dir[e] * dir[e] + _A (e, e) * _A (v, v) * dir[u] * dir[u] +
                                  _A (e, e) * _A (u, u) * dir[v] * dir[v];
                const double su =
                    (_A (u, u) < EPS) ? 0 : dir[u] * dir[u] * K1 / (_A (u, u) * _A (u, u) * K2);
                const double sv =
                    (_A (v, v) < EPS) ? 0 : dir[v] * dir[v] * K1 / (_A (v, v) * _A (v, v) * K2);
                if (su >= 0 && sv >= 0) {
                    const double xu1 = (_A (u, u) < EPS) ? 0 : -_a[u] / _A (u, u) + std::sqrt (su);
                    const double xu2 = (_A (u, u) < EPS) ? 0 : -_a[u] / _A (u, u) - std::sqrt (su);
                    const double xv1 = (_A (v, v) < EPS) ? 0 : -_a[v] / _A (v, v) + std::sqrt (sv);
                    const double xv2 = (_A (v, v) < EPS) ? 0 : -_a[v] / _A (v, v) - std::sqrt (sv);
                    const double s1  = _a[e] / _A (e, e) * _a[e] / _A (e, e) -
                                      (_A (u, u) * xu1 * xu1 + _A (v, v) * xv1 * xv1 +
                                       _a[u] * xu1 * 2 + _a[v] * xv1 * 2 + _u) /
                                          _A (e, e);
                    const double s2 = _a[e] / _A (e, e) * _a[e] / _A (e, e) -
                                      (_A (u, u) * xu2 * xu2 + _A (v, v) * xv2 * xv2 +
                                       _a[u] * xu2 * 2 + _a[v] * xv2 * 2 + _u) /
                                          _A (e, e);
                    if (s1 > 0) {
                        const double xe1a = -_a[e] / _A (e, e) + std::sqrt (s1);
                        const double xe1b = -_a[e] / _A (e, e) - std::sqrt (s1);
                        x.push_back (Vector3D<> (xu1, xv1, xe1a));
                        x.push_back (Vector3D<> (xu1, xv1, xe1b));
                    }
                    if (s2 > 0) {
                        const double xe2a = -_a[e] / _A (e, e) + std::sqrt (s2);
                        const double xe2b = -_a[e] / _A (e, e) - std::sqrt (s2);
                        x.push_back (Vector3D<> (xu2, xv2, xe2a));
                        x.push_back (Vector3D<> (xu2, xv2, xe2b));
                    }
                }
            }
        }
        for (std::size_t i = 0; i < x.size (); i++) {
            const double xu    = x[i][0];
            const double xv    = x[i][1];
            const double xe    = x[i][2];
            const Vector3D<> P = Vector3D<> ((u == 0) ? xu : (v == 0) ? xv : xe,
                                             (u == 1) ? xu : (v == 1) ? xv : xe,
                                             (u == 2) ? xu : (v == 2) ? xv : xe);
            if (insideTrimmingRegion (P)) {
                const double extremum = dir[u] * xu + dir[v] * xv + dir[e] * xe;
                if (dir[e] * _A (u, u) / _a (e) > 0) {
                    extremums.second = extremum;
                }
                else {
                    extremums.first = extremum;
                }
            }
        }
    }
    return extremums;
}

QuadraticSurface::Ptr QuadraticSurface::makeEllipsoid (const double a, const double b,
                                                       const double c)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b * c * c, a * a * c * c, a * a * b * b);
    static const Eigen::Vector3d avec = Eigen::Vector3d::Zero ();
    const double u                    = -a * a * b * b * c * c;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeSpheroid (const double a, const double b)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b, b * b, a * a);
    static const Eigen::Vector3d avec = Eigen::Vector3d::Zero ();
    const double u                    = -a * a * b * b;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeSphere (const double radius)
{
    static const Eigen::DiagonalMatrix< double, 3 > A (1, 1, 1);
    static const Eigen::Vector3d a = Eigen::Vector3d::Zero ();
    const double u                 = -radius * radius;
    return ownedPtr (new QuadraticSurface (A, a, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeEllipticParaboloid (const double a, const double b)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b, a * a, 0);
    const Eigen::Vector3d avec (0, 0, -a * a * b * b);
    static const double u = 0;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeCircularParaboloid (const double a)
{
    static const Eigen::DiagonalMatrix< double, 3 > A (1, 1, 0);
    const Eigen::Vector3d avec (0, 0, -a * a);
    static const double u = 0;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeHyperbolicParaboloid (const double a, const double b)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b, -a * a, 0);
    const Eigen::Vector3d avec (0, 0, -a * a * b * b);
    static const double u = 0;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr
QuadraticSurface::makeEllipticHyperboloidOneSheet (const double a, const double b, const double c)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b * c * c, a * a * c * c, -a * a * b * b);
    static const Eigen::Vector3d avec = Eigen::Vector3d::Zero ();
    const double u                    = -a * a * b * b * c * c;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeCircularHyperboloidOneSheet (const double a,
                                                                         const double b)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b, a * a, -a * a);
    static const Eigen::Vector3d avec = Eigen::Vector3d::Zero ();
    static const double u             = -a * a * b * b;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr
QuadraticSurface::makeEllipticHyperboloidTwoSheets (const double a, const double b, const double c)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b * c * c, a * a * c * c, -a * a * b * b);
    static const Eigen::Vector3d avec = Eigen::Vector3d::Zero ();
    static const double u             = a * a * b * b * c * c;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeCircularHyperboloidTwoSheets (const double a,
                                                                          const double b)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b, a * a, -a * a);
    static const Eigen::Vector3d avec = Eigen::Vector3d::Zero ();
    static const double u             = a * a * b * b;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeEllipticCone (const double a, const double b,
                                                          const double c)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b * c * c, a * a * c * c, -a * a * b * b);
    static const Eigen::Vector3d avec = Eigen::Vector3d::Zero ();
    static const double u             = 0;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeCircularCone (const double a, const double b)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b, b * b, -a * a);
    static const Eigen::Vector3d avec = Eigen::Vector3d::Zero ();
    static const double u             = 0;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeEllipticCylinder (const double a, const double b)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b, a * a, 0);
    static const Eigen::Vector3d avec = Eigen::Vector3d::Zero ();
    static const double u             = -a * a * b * b;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeCircularCylinder (const double radius,
                                                              const bool outward)
{
    static const Eigen::DiagonalMatrix< double, 3 > Aout (1, 1, 0);
    static const Eigen::DiagonalMatrix< double, 3 > Ain (-1, -1, 0);
    static const Eigen::Vector3d a = Eigen::Vector3d::Zero ();
    const double u                 = -radius * radius;
    if (outward)
        return ownedPtr (new QuadraticSurface (Aout, a, u));
    else
        return ownedPtr (new QuadraticSurface (Ain, a, -u));
}

QuadraticSurface::Ptr QuadraticSurface::makeHyperbolicCylinder (const double a, const double b)
{
    const Eigen::DiagonalMatrix< double, 3 > A (b * b, -a * a, 0);
    static const Eigen::Vector3d avec = Eigen::Vector3d::Zero ();
    static const double u             = -a * a * b * b;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makeParabolicCylinder (const double a)
{
    static const Eigen::DiagonalMatrix< double, 3 > A (1, 0, 0);
    const Eigen::Vector3d avec (0, a * 2, 0);
    static const double u = 0;
    return ownedPtr (new QuadraticSurface (A, avec, u));
}

QuadraticSurface::Ptr QuadraticSurface::makePlane (const Vector3D<>& n, double d)
{
    static const Eigen::DiagonalMatrix< double, 3 > A (0, 0, 0);
    return ownedPtr (new QuadraticSurface (A, Eigen::Vector3d (n[0], n[1], n[2]), d * 2));
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <string>
#include <thread>

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"

#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/EstimationSetup/createNumericalSimulator.h"
#include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_exact_termination )

//! Test exact termination conditions, to see if the propagation stops exactly (within tolerance) when it's supposed to.
//! The test is run for an RK4 and RK7(8) integrator, with otherwise identical settings.
//! Five types of termination conditions are used:
//! 0) Termination on exact time
//! 1) Termination on exact altitude
//! 2) Termination when _either_ an exact altitude _or_ an exact time is reached (whichever comes first), with the two occuring
//! very near one another
//! 3) Termination when _both_ an exact altitude _and_ an exact time are reached, with the two occuring very near one another
//! 4) Termination when _either_ an exact altitude _or_ an exact time is reached (whichever comes first), with the two _not_
//! occuring very near one another
//!
//! The tests are run for forward and backward propagation
BOOST_AUTO_TEST_CASE( testEnckePopagatorForSphericalHarmonicCentralBodies )
{
    for( unsigned int integratorCase = 0; integratorCase < 2; integratorCase++ )
    {
        for( unsigned int direction = 0; direction < 2; direction++ )
        {
            for( unsigned int simulationCase = 0; simulationCase < 5; simulationCase++ )
            {
                std::cout<<integratorCase<<" "<<direction<<" "<<simulationCase<<std::endl;
                using namespace tudat;
                using namespace simulation_setup;
                using namespace propagators;
                using namespace numerical_integrators;
                using namespace orbital_element_conversions;
                using namespace basic_mathematics;
                using namespace gravitation;

                // Load Spice kernels.
                spice_interface::loadStandardSpiceKernels( );

                // Set simulation time settings.
                double simulationStartEpoch;
                double simulationEndEpoch;

                double directionMultiplier = 1.0;
                if( direction == 0 )
                {
                    simulationStartEpoch = 0.0;
                    simulationEndEpoch = 0.2 * tudat::physical_constants::JULIAN_DAY;
                }
                else
                {
                    simulationStartEpoch = 0.2 * tudat::physical_constants::JULIAN_DAY;
                    simulationEndEpoch = 0.0;
                    directionMultiplier = -1.0;
                }

                // Define body settings for simulation.
                std::vector< std::string > bodiesToCreate;
                bodiesToCreate.push_back( "Sun" );
                bodiesToCreate.push_back( "Earth" );
                bodiesToCreate.push_back( "Moon" );

                // Create body objects.
                std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
                if( direction == 0 )
                {
                    bodySettings =
                            getDefaultBodySettings( bodiesToCreate, simulationStartEpoch - 300.0, simulationEndEpoch + 300.0 );
                }
                else
                {
                    bodySettings =
                            getDefaultBodySettings( bodiesToCreate, simulationEndEpoch - 300.0, simulationStartEpoch + 300.0 );
                }
                NamedBodyMap bodyMap = createBodies( bodySettings );

                // Create spacecraft object.
                bodyMap[ "Vehicle" ] = std::make_shared< simulation_setup::Body >( );
                bodyMap[ "Vehicle" ]->setConstantBodyMass( 400.0 );
                bodyMap[ "Vehicle" ]->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                                        std::shared_ptr< interpolators::OneDimensionalInterpolator
                                                        < double, Eigen::Vector6d  > >( ), "Earth", "ECLIPJ2000" ) );

                // Finalize body creation.
                setGlobalFrameBodyEphemerides( bodyMap, "Earth", "ECLIPJ2000" );

                // Define propagator settings variables.
                SelectedAccelerationMap accelerationMap;
                std::vector< std::string > bodiesToPropagate;
                std::vector< std::string > centralBodies;

                // Define propagation settings.
                std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;

                {
                    accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                                     basic_astrodynamics::central_gravity ) );
                    accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                                   basic_astrodynamics::central_gravity ) );
                    accelerationsOfVehicle[ "Moon" ].push_back( std::make_shared< AccelerationSettings >(
                                                                    basic_astrodynamics::central_gravity ) );
                }

                accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;
                bodiesToPropagate.push_back( "Vehicle" );
                centralBodies.push_back( "Earth" );
                basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                            bodyMap, accelerationMap, bodiesToPropagate, centralBodies );

                // Set Keplerian elements for Vehicle.
                Eigen::Vector6d vehicleInitialStateInKeplerianElements;
                vehicleInitialStateInKeplerianElements( semiMajorAxisIndex ) = 8000.0E3;
                vehicleInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
                vehicleInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 );
                vehicleInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
                        = unit_conversions::convertDegreesToRadians( 235.7 );
                vehicleInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
                        = unit_conversions::convertDegreesToRadians( 23.4 );
                vehicleInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 );

                double earthGravitationalParameter = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
                const Eigen::Vector6d vehicleInitialState = convertKeplerianToCartesianElements(
                            vehicleInitialStateInKeplerianElements, earthGravitationalParameter );

                // Define propagator settings (Cowell)
                std::shared_ptr< PropagationTerminationSettings > terminationSettings;
                std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables;
                dependentVariables.push_back(
                            std::make_shared< SingleDependentVariableSaveSettings >( relative_distance_dependent_variable,
                                                                                "Vehicle", "Earth" ) );
                double finalTestTime;
                double secondFinalTestTime;

                if( direction == 0 )
                {
                    finalTestTime = 322.5;
                    secondFinalTestTime = 501.0;
                }
                else
                {
                    finalTestTime = 11737.5;
                    secondFinalTestTime = 11701.0;
                }
                if( simulationCase == 0 )
                {
                    terminationSettings = std::make_shared< PropagationTimeTerminationSettings >(
                                simulationEndEpoch - directionMultiplier * 4.5, true );
                }
                else if( simulationCase == 1 )
                {
                    terminationSettings = std::make_shared< PropagationDependentVariableTerminationSettings >(
                                dependentVariables.at( 0 ), 8.7E6, false, true,
                                std::make_shared< root_finders::RootFinderSettings >(
                                    root_finders::bisection_root_finder, 1.0E-6, 100 ) );
                }
                else if( simulationCase == 2 )
                {
                    std::vector< std::shared_ptr< PropagationTerminationSettings > > terminationSettingsList;
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationTimeTerminationSettings >( finalTestTime, true ) );
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationDependentVariableTerminationSettings >(
                                    dependentVariables.at( 0 ), 8.7E6, false, true,
                                    std::make_shared< root_finders::RootFinderSettings >(
                                        root_finders::bisection_root_finder, 1.0E-6, 100 ) ) );
                    terminationSettings = std::make_shared< PropagationHybridTerminationSettings >(
                                terminationSettingsList, true );
                }
                else if( simulationCase == 3 )
                {
                    std::vector< std::shared_ptr< PropagationTerminationSettings > > terminationSettingsList;
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationTimeTerminationSettings >( finalTestTime, true ) );
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationDependentVariableTerminationSettings >(
                                    dependentVariables.at( 0 ), 8.7E6, false, true,
                                    std::make_shared< root_finders::RootFinderSettings >(
                                        root_finders::bisection_root_finder, 1.0E-6, 100 ) ) );
                    terminationSettings = std::make_shared< PropagationHybridTerminationSettings >(
                                terminationSettingsList, false );
                }
                else if( simulationCase == 4 )
                {
                    std::vector< std::shared_ptr< PropagationTerminationSettings > > terminationSettingsList;
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationTimeTerminationSettings >( secondFinalTestTime, true ) );
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationDependentVariableTerminationSettings >(
                                    dependentVariables.at( 0 ), 8.7E6, false, true,
                                    std::make_shared< root_finders::RootFinderSettings >(
                                        root_finders::bisection_root_finder, 1.0E-6, 100 ) ) );
                    terminationSettings = std::make_shared< PropagationHybridTerminationSettings >(
                                terminationSettingsList, false );
                }

                std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
                        std::make_shared< TranslationalStatePropagatorSettings< double > >
                        ( centralBodies, accelerationModelMap, bodiesToPropagate, vehicleInitialState, terminationSettings, cowell,
                          std::make_shared< DependentVariableSaveSettings >( dependentVariables ) );

                // Define integrator settings.
                const double fixedStepSize = 5.0;
                std::shared_ptr< IntegratorSettings< > > integratorSettings;
                if( integratorCase == 0 )
                {
                    integratorSettings = std::make_shared< IntegratorSettings< > >
                            ( rungeKutta4, simulationStartEpoch, directionMultiplier * fixedStepSize );
                }
                else
                {
                    integratorSettings = std::make_shared< RungeKuttaVariableStepSizeSettings< double > >
                            ( simulationStartEpoch, directionMultiplier * fixedStepSize,
                              RungeKuttaCoefficients::CoefficientSets::rungeKuttaFehlberg45,
                              1.0E-3, 1.0E3, 1.0E-12, 1.0E-12 );
                }

                // Propagate orbit with Cowell method
                SingleArcDynamicsSimulator< double > dynamicsSimulator(
                            bodyMap, integratorSettings, propagatorSettings, true, false, false );
                std::map< double, Eigen::VectorXd > stateHistory = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
                std::map< double, Eigen::VectorXd > dependentVariableHistory = dynamicsSimulator.getDependentVariableHistory( );

                // Sanity check: altitude limit not violated on first step
                BOOST_CHECK_EQUAL( ( vehicleInitialState.segment( 0, 3 ).norm( ) - 8.7E6 ) < 100.0, true );

                if( simulationCase == 0 )
                {
                    // Check if propagation terminated exactly on final time
                    if( direction == 0 )
                    {
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.rbegin( )->first -
                                                      ( simulationEndEpoch - 4.5 ) ), 1.0E-10 );
                    }
                    else
                    {
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.begin( )->first -
                                                      ( simulationEndEpoch + 4.5 ) ), 1.0E-10 );
                    }
                }
                else if( simulationCase == 1 )
                {
                    // Check if propagation terminated exactly on final altitude
                    if( direction == 0 )
                    {
                        BOOST_CHECK_SMALL( std::fabs( dependentVariableHistory.rbegin( )->second( 0 ) - 8.7E6 ), 0.01 );
                    }
                    else
                    {
                        BOOST_CHECK_SMALL( std::fabs( dependentVariableHistory.begin( )->second( 0 ) - 8.7E6 ), 0.01 );
                    }
                }
                else if( simulationCase == 2 )
                {
                    // Check if propagation terminated exactly on final altitude  or final time (whichever came first)
                    // Determine by inspection: for both forward propagation, altitude condition reached first; for
                    // backward propagation, time condition reaced first
                    if( direction == 0 )
                    {
                        // Check if termination on final altitude
                        BOOST_CHECK_SMALL( std::fabs( dependentVariableHistory.rbegin( )->second( 0 ) - 8.7E6 ), 0.01 );

                        // Check if final time indeed not yet reached
                        BOOST_CHECK_EQUAL( ( stateHistory.rbegin( )->first - finalTestTime ) < 1.0, true );
                    }
                    else
                    {
                        // Check if termination on final time
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.begin( )->first - finalTestTime ), 1.0E-10 );

                        // Check if final altitude indeed not yet reached
                        BOOST_CHECK_EQUAL( ( dependentVariableHistory.begin( )->second( 0 ) - 8.7E6 ) < -100.0, true );
                    }
                }
                else if( simulationCase == 3 )
                {
                    // Check if propagation terminated exactly on final altitude  or final time (both must be attained, see
                    // comment on previous test.
                    if( direction == 0 )
                    {
                        // Check if termination on final time
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.rbegin( )->first - finalTestTime ), 0.01 );

                        // Check if final altitude indeed already exceeded
                        BOOST_CHECK_EQUAL( ( dependentVariableHistory.rbegin( )->second( 0 ) - 8.7E6 ) > 100.0, true );
                    }
                    else
                    {
                        // Check if termination on final altitude
                        BOOST_CHECK_SMALL( std::fabs( dependentVariableHistory.begin( )->second( 0 ) - 8.7E6  ), 0.01 );

                        // Check if final time indeed already exceeded
                        BOOST_CHECK_EQUAL( ( stateHistory.begin( )->first - finalTestTime ) < -0.1, true );
                    }
                }
                else if( simulationCase == 4 )
                {
                    // Check if propagation terminated on final time
                    if( direction == 0 )
                    {
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.rbegin( )->first - secondFinalTestTime ), 0.01 );
                        BOOST_CHECK_EQUAL( ( dependentVariableHistory.rbegin( )->second( 0 ) - 8.7E6 ) > 100.0, true );
                    }
                    else
                    {
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.begin( )->first - secondFinalTestTime ), 0.01 );
                        BOOST_CHECK_EQUAL( ( dependentVariableHistory.begin( )->second( 0 ) - 8.7E6 ) > 100.0, true );
                    }
                }
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


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

#ifndef BOOST_STATS_rayleigh_HPP
#define BOOST_STATS_rayleigh_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/config/no_tr1/cmath.hpp>

#ifdef BOOST_MSVC
# pragma warning(push)
# pragma warning(disable: 4702) // unreachable code (return after domain_error throw).
#endif

#include <utility>

namespace boost{ namespace math{

namespace detail
{ // Error checks:
  template <class RealType, class Policy>
  inline bool verify_sigma(const char* function, RealType sigma, RealType* presult, const Policy& pol)
  {
     if(sigma <= 0)
     {
        *presult = policies::raise_domain_error<RealType>(
           function,
           "The scale parameter \"sigma\" must be > 0, but was: %1%.", sigma, pol);
        return false;
     }
     return true;
  } // bool verify_sigma

  template <class RealType, class Policy>
  inline bool verify_rayleigh_x(const char* function, RealType x, RealType* presult, const Policy& pol)
  {
     if(x < 0)
     {
        *presult = policies::raise_domain_error<RealType>(
           function,
           "The random variable must be >= 0, but was: %1%.", x, pol);
        return false;
     }
     return true;
  } // bool verify_rayleigh_x
} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class rayleigh_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   rayleigh_distribution(RealType sigma = 1)
      : m_sigma(sigma)
   {
      RealType err;
      detail::verify_sigma("boost::math::rayleigh_distribution<%1%>::rayleigh_distribution", sigma, &err, Policy());
   } // rayleigh_distribution

   RealType sigma()const
   { // Accessor.
     return m_sigma;
   }

private:
   RealType m_sigma;
}; // class rayleigh_distribution

typedef rayleigh_distribution<double> rayleigh;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const rayleigh_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const rayleigh_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>((0),  max_value<RealType>());
}

template <class RealType, class Policy>
inline RealType pdf(const rayleigh_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std function exp.

   RealType sigma = dist.sigma();
   RealType result;
   static const char* function = "boost::math::pdf(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   if(false == detail::verify_rayleigh_x(function, x, &result, Policy()))
   {
      return result;
   }
   RealType sigmasqr = sigma * sigma;
   result = x * (exp(-(x * x) / ( 2 * sigmasqr))) / sigmasqr;
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const rayleigh_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   RealType result;
   RealType sigma = dist.sigma();
   static const char* function = "boost::math::cdf(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   if(false == detail::verify_rayleigh_x(function, x, &result, Policy()))
   {
      return result;
   }
   result = -boost::math::expm1(-x * x / ( 2 * sigma * sigma), Policy());
   return result;
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const rayleigh_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   RealType result;
   RealType sigma = dist.sigma();
   static const char* function = "boost::math::quantile(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
      return result;
   if(false == detail::check_probability(function, p, &result, Policy()))
      return result;

   if(p == 0)
   {
      return 0;
   }
   if(p == 1)
   {
     return policies::raise_overflow_error<RealType>(function, 0, Policy());
   }
   result = sqrt(-2 * sigma * sigma * boost::math::log1p(-p, Policy()));
   return result;
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<rayleigh_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   RealType result;
   RealType sigma = c.dist.sigma();
   static const char* function = "boost::math::cdf(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   RealType x = c.param;
   if(false == detail::verify_rayleigh_x(function, x, &result, Policy()))
   {
      return result;
   }
   result =  exp(-x * x / ( 2 * sigma * sigma));
   return result;
} // cdf complement

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<rayleigh_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions, log & sqrt.

   RealType result;
   RealType sigma = c.dist.sigma();
   static const char* function = "boost::math::quantile(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   RealType q = c.param;
   if(false == detail::check_probability(function, q, &result, Policy()))
   {
      return result;
   }
   if(q == 1)
   {
      return 0;
   }
   if(q == 0)
   {
     return policies::raise_overflow_error<RealType>(function, 0, Policy());
   }
   result = sqrt(-2 * sigma * sigma * log(q));
   return result;
} // quantile complement

template <class RealType, class Policy>
inline RealType mean(const rayleigh_distribution<RealType, Policy>& dist)
{
   RealType result;
   RealType sigma = dist.sigma();
   static const char* function = "boost::math::mean(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   using boost::math::constants::root_half_pi;
   return sigma * root_half_pi<RealType>();
} // mean

template <class RealType, class Policy>
inline RealType variance(const rayleigh_distribution<RealType, Policy>& dist)
{
   RealType result;
   RealType sigma = dist.sigma();
   static const char* function = "boost::math::variance(const rayleigh_distribution<%1%>&, %1%)";
   if(false == detail::verify_sigma(function, sigma, &result, Policy()))
   {
      return result;
   }
   using boost::math::constants::four_minus_pi;
   return four_minus_pi<RealType>() * sigma * sigma / 2;
} // variance

template <class RealType, class Policy>
inline RealType mode(const rayleigh_distribution<RealType, Policy>& dist)
{
   return dist.sigma();
}

template <class RealType, class Policy>
inline RealType median(const rayleigh_distribution<RealType, Policy>& dist)
{
   using boost::math::constants::root_ln_four;
   return root_ln_four<RealType>() * dist.sigma();
}

template <class RealType, class Policy>
inline RealType skewness(const rayleigh_distribution<RealType, Policy>& /*dist*/)
{
  // using namespace boost::math::constants;
  return static_cast<RealType>(0.63111065781893713819189935154422777984404221106391L);
  // Computed using NTL at 150 bit, about 50 decimal digits.
  // return 2 * root_pi<RealType>() * pi_minus_three<RealType>() / pow23_four_minus_pi<RealType>();
}

template <class RealType, class Policy>
inline RealType kurtosis(const rayleigh_distribution<RealType, Policy>& /*dist*/)
{
  // using namespace boost::math::constants;
  return static_cast<RealType>(3.2450893006876380628486604106197544154170667057995L);
  // Computed using NTL at 150 bit, about 50 decimal digits.
  // return 3 - (6 * pi<RealType>() * pi<RealType>() - 24 * pi<RealType>() + 16) /
  // (four_minus_pi<RealType>() * four_minus_pi<RealType>());
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const rayleigh_distribution<RealType, Policy>& /*dist*/)
{
  //using namespace boost::math::constants;
  // Computed using NTL at 150 bit, about 50 decimal digits.
  return static_cast<RealType>(0.2450893006876380628486604106197544154170667057995L);
  // return -(6 * pi<RealType>() * pi<RealType>() - 24 * pi<RealType>() + 16) /
  //   (four_minus_pi<RealType>() * four_minus_pi<RealType>());
} // kurtosis

} // namespace math
} // namespace boost

#ifdef BOOST_MSVC
# pragma warning(pop)
#endif

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_rayleigh_HPP


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_MIN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MIN_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-arithmetic
    This function object computes the smallest of its parameter.

    @par Header <boost/simd/function/min.hpp>

    @par Notes
    For any given value @c x and @c y of type @c T:

    Using `min(x, y)` is similar to `(x <  y) ? x : y` which is the standard behaviour.

    With this definition min(x, @ref Nan) should return x...

    On some systems the intrinsic used returns Nan as soon x or y is a nan.
    So the real definition of our min function must add: but if y is Nan the result is system dependent.

    This can be corrected using the pedantic_ decorator that ensures the standard behaviour at a cost.

    @par Decorators

     - pedantic_ ensures the standard behaviour.
     - std_ call std::min.

    @see min,  minnum, minnummag, minmag, minmod

    @par Example:

      @snippet min.cpp min

    @par Possible output:

      @snippet min.txt min

  **/
  Value min(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/min.hpp>
#include <boost/simd/function/simd/min.hpp>

#endif


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>

#include <Eigen/Dense>

#include <kontiki/trajectories/uniform_so3_spline_trajectory.h>
#include "trajectory_helper.h"
#include "spline_helpers.h"

namespace py = pybind11;

namespace TT = kontiki::trajectories;

static Eigen::Quaterniond vec_to_quat(const Eigen::Vector4d& qv) {
  return Eigen::Quaterniond(qv(0), qv(1), qv(2), qv(3));
}

static Eigen::Vector4d quat_to_vec(const Eigen::Quaterniond& q) {
  return Eigen::Vector4d(q.w(), q.x(), q.y(), q.z());
}

struct PySO3SplineHelper {
  using PyControlPointType = Eigen::Vector4d;

  static Eigen::Vector4d ConvertCppToPy(const Eigen::Quaterniond& cp) {
    return quat_to_vec(cp);
  }

  static Eigen::Quaterniond ConvertPyToCpp(const Eigen::Vector4d& cp) {
    return vec_to_quat(cp);
  }
};

PYBIND11_MODULE(_uniform_so3_spline_trajectory, m) {
  m.doc() = "Uniform splined trajectory in SO3";

  using Class = TT::UniformSO3SplineTrajectory;
  using Helper = PySO3SplineHelper;
  auto cls = py::class_<Class, std::shared_ptr<Class>>(m, "UniformSO3SplineTrajectory");
  cls.doc() = R"pbdoc( A spline with control points in SO(3)

  Control points are unit quaternions q=(w, x, y, z).
 )pbdoc";

  // Common attributes
  declare_spline_common<Class, Helper>(cls);
  declare_trajectory_common<Class>(cls);

} // PYBIND11_MODULE