//////////////////////////////////////////////////////////////////////////////
// cross_validation::example::k_fold.cpp                                    //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#include <algorithm>
#include <iterator>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <boost/math/tools/precision.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>

#include <boost/statistics/detail/cross_validation/estimator/adaptor/include.hpp>
#include <boost/statistics/detail/cross_validation/k_fold/include.hpp>
#include <boost/statistics/detail/cross_validation/error/include.hpp>

#include <libs/statistics/detail/cross_validation/example/k_fold.h>

void example_k_fold(std::ostream& os)
{

    os << "-> example_k_fold :" << std::endl;

    // This example shows how to perform K train/predict cycles.
    // The estimator is built upon a Boost.Accumulator for the mean.
    // The input (x) equals the input (y), in this case, and has type double

    using namespace boost;
    namespace stat = boost::statistics::detail;
    namespace cv = stat::cross_validation;
    namespace cv_es = cv::estimator;
    namespace cv_ex = cv::extractor;
    namespace cv_kf = cv::k_fold;

    typedef mt19937                                 urng_;
    typedef double                                  val_;
    typedef std::vector<val_>                       vals_;
    typedef range_iterator<vals_>::type             vals_it_;
    typedef boost::normal_distribution<val_>        nd_;
    typedef boost::variate_generator<urng_&,nd_>    vg_;

    // This comes up often, so this metafunction saves a bit of time
    typedef accumulators::tag::mean tag_;
    typedef accumulators::stats<
        tag_
    >  stat_;
    typedef accumulators::accumulator_set<val_,stat_> acc_;

    // Boost.Accumulators cannot be used directly. An adaptor is needed.
    typedef cv_es::adaptor::meta::nullary_predictor<acc_,tag_>      meta_p_;
    typedef cv_es::adaptor::joined<
        cv_es::adaptor::unary_trainer,
        meta_p_::apply,
        acc_
    > joined_;

    typedef cv_kf::partition<val_,cv_ex::identity> kf_p_;

    const unsigned n = 1e1;
    const unsigned k = 5e0;
    BOOST_ASSERT(n % k == 0);

    nd_ nd;
    urng_ urng;
    vg_ vg(urng,nd);

    vals_ vec_x;
    std::generate_n(
        std::back_inserter(vec_x),
        n,
        vg
    );

    // vec_x is duplicated in kf_p, which is intenteded in this example.
    // example_k_fold in sandbox/kernel shows how to avoid duplication

    kf_p_ kf_p(
        k,
        boost::begin(vec_x),
        boost::end(vec_x)
    );

    struct float_{

        static bool equal(const val_& a, const val_& b){
            static val_ e = boost::math::tools::epsilon<val_>();;
            return fabs(a-b)< e;
        }

    };

    // Tests that concatenating the test-data over all k increments, is
    // identical to vec_x.
    for(unsigned i = 0; i<2; i++){
        if(i>0){
            // 2nd pass to check that resets properly
            kf_p.initialize();
        }

        vals_it_ vec_x_it  = boost::begin( vec_x );
        while(kf_p.index()<kf_p.n_folds()){
            os << kf_p << std::endl;
            BOOST_FOREACH(const val_& t,kf_p.subset2())
            {
                val_ t1 = *vec_x_it;
                BOOST_ASSERT( float_::equal(t,t1) );
                ++vec_x_it;
            };
            kf_p.increment();
        }
    }
    os << std::endl;

    // Cross-Validation #1
    vals_ preds(n);
    vals_it_ preds_b = boost::begin(preds);
    vals_it_ preds_e;
    BOOST_ASSERT(n / k > 1);
    kf_p.initialize();
    while(kf_p.index()<kf_p.n_folds()){
        acc_ acc;
        joined_ joined(acc);
        preds_e = train_predict(
            kf_p,
            joined,
            preds_b
        );
        val_ tmp = *preds_b;
        BOOST_AUTO(
            r,
            make_iterator_range(boost::next(preds_b),preds_e)
        );
        BOOST_FOREACH(const val_& v, r)
        {
            // Because we use a nullary_estimator
            // a.k.a. marginal estimator in statistical terminology.
            BOOST_ASSERT(tmp == v);
        }

        preds_b = preds_e;
        kf_p.increment();
    }

    // Cross-Validation #2
    {
        vals_ output(n);
        acc_ acc;
        joined_ joined(acc);
        cross_validate(
            kf_p,
            joined,
            boost::begin(preds),
            boost::begin(output)
        );

        val_ sqrt_mse = cv::error::sqrt_mse(
            boost::begin(preds),
            boost::end(preds),
            boost::begin(output)
        );

        val_ mae = cv::error::mean_abs_error(
            boost::begin(preds),
            boost::end(preds),
            boost::begin(output)
        );

        os << "sqrt_mse = " << sqrt_mse << std::endl;
        os << "mae = " << mae << std::endl;

    }

    os << "<-" << std::endl;

}


#include "drake/common/trajectories/piecewise_polynomial.h"

#include <random>
#include <stdexcept>
#include <vector>

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

#include "drake/common/drake_assert.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/common/test_utilities/expect_throws_message.h"
#include "drake/common/trajectories/test/random_piecewise_polynomial.h"
#include "drake/math/autodiff_gradient.h"

using drake::math::autoDiffToGradientMatrix;
using drake::math::DiscardGradient;
using Eigen::Matrix;
using std::default_random_engine;
using std::normal_distribution;
using std::runtime_error;
using std::uniform_int_distribution;
using std::uniform_real_distribution;
using std::vector;

namespace drake {
namespace trajectories {
namespace {

template<typename T>
void testIntegralAndDerivative() {
  int num_coefficients = 5;
  int num_segments = 3;
  int rows = 3;
  int cols = 5;

  default_random_engine generator;
  vector<double> segment_times =
      PiecewiseTrajectory<double>::RandomSegmentTimes(num_segments, generator);
  PiecewisePolynomial<T> piecewise =
      test::MakeRandomPiecewisePolynomial<T>(
          rows, cols, num_coefficients, segment_times);

  // derivative(0) should be same as original piecewise.
  EXPECT_TRUE(
      CompareMatrices(piecewise.value(piecewise.start_time()),
                      piecewise.derivative(0).value(piecewise.start_time()),
                      1e-10, MatrixCompareType::absolute));

  // differentiate integral, get original back
  PiecewisePolynomial<T> piecewise_back = piecewise.integral().derivative();
  if (!piecewise.isApprox(piecewise_back, 1e-10)) throw runtime_error("wrong");

  // check value at start time
  MatrixX<T> desired_value_at_t0 =
      MatrixX<T>::Random(piecewise.rows(), piecewise.cols());
  PiecewisePolynomial<T> integral = piecewise.integral(desired_value_at_t0);
  auto value_at_t0 = integral.value(piecewise.start_time());
  EXPECT_TRUE(CompareMatrices(desired_value_at_t0, value_at_t0, 1e-10,
                              MatrixCompareType::absolute));

  // check continuity at sample points
  for (int i = 0; i < piecewise.get_number_of_segments() - 1; ++i) {
    EXPECT_EQ(integral.getPolynomial(i)
                  .EvaluateUnivariate(integral.duration(i)),
              integral.getPolynomial(i + 1).EvaluateUnivariate(0.0));
  }
}

template<typename T>
void testBasicFunctionality() {
  int max_num_coefficients = 6;
  int num_tests = 100;
  default_random_engine generator;
  uniform_int_distribution<> int_distribution(1, max_num_coefficients);

  for (int i = 0; i < num_tests; ++i) {
    int num_coefficients = int_distribution(generator);
    int num_segments = int_distribution(generator);
    int rows = int_distribution(generator);
    int cols = int_distribution(generator);

    vector<double> segment_times =
        PiecewiseTrajectory<double>::RandomSegmentTimes(num_segments,
                                                        generator);
    PiecewisePolynomial<T> piecewise1 = test::MakeRandomPiecewisePolynomial<T>(
        rows, cols, num_coefficients, segment_times);
    PiecewisePolynomial<T> piecewise2 = test::MakeRandomPiecewisePolynomial<T>(
        rows, cols, num_coefficients, segment_times);
    PiecewisePolynomial<T> piecewise3_not_matching_rows =
        test::MakeRandomPiecewisePolynomial<T>(rows + 1, cols, num_coefficients,
                                               segment_times);
    PiecewisePolynomial<T> piecewise4_not_matching_cols =
        test::MakeRandomPiecewisePolynomial<T>(rows, cols + 1, num_coefficients,
                                               segment_times);
    PiecewisePolynomial<T> piecewise5 = test::MakeRandomPiecewisePolynomial<T>(
        cols, rows, num_coefficients, segment_times);

    normal_distribution<double> normal;
    double shift = normal(generator);
    MatrixX<T> offset =
        MatrixX<T>::Random(piecewise1.rows(), piecewise1.cols());

    PiecewisePolynomial<T> sum = piecewise1 + piecewise2;
    PiecewisePolynomial<T> difference = piecewise2 - piecewise1;
    PiecewisePolynomial<T> piecewise1_plus_offset = piecewise1 + offset;
    PiecewisePolynomial<T> piecewise1_minus_offset = piecewise1 - offset;
    PiecewisePolynomial<T> piecewise1_shifted = piecewise1;
    piecewise1_shifted.shiftRight(shift);
    PiecewisePolynomial<T> product = piecewise1 * piecewise5;
    PiecewisePolynomial<T> unary_minus = -piecewise1;

    const double total_time = segment_times.back() - segment_times.front();
    PiecewisePolynomial<T> piecewise2_twice = piecewise2;
    PiecewisePolynomial<T> piecewise2_shifted = piecewise2;
    piecewise2_shifted.shiftRight(total_time);

    // Checks that concatenation of trajectories that are not time
    // aligned at the connecting ends is a failure.
    PiecewisePolynomial<T> piecewise2_shifted_twice = piecewise2;
    piecewise2_shifted_twice.shiftRight(2. * total_time);
    EXPECT_THROW(piecewise2_twice.ConcatenateInTime(
        piecewise2_shifted_twice), std::runtime_error);

    // Checks that concatenation of trajectories that have different
    // row counts is a failure.
    PiecewisePolynomial<T> piecewise3_not_matching_rows_shifted =
        piecewise3_not_matching_rows;
    piecewise3_not_matching_rows_shifted.shiftRight(total_time);
    EXPECT_THROW(piecewise2_twice.ConcatenateInTime(
        piecewise3_not_matching_rows_shifted), std::runtime_error);

    // Checks that concatenation of trajectories that have different
    // col counts is a failure.
    PiecewisePolynomial<T> piecewise4_not_matching_cols_shifted =
        piecewise4_not_matching_cols;
    piecewise4_not_matching_cols_shifted.shiftRight(total_time);
    EXPECT_THROW(piecewise2_twice.ConcatenateInTime(
        piecewise4_not_matching_cols_shifted), std::runtime_error);

    piecewise2_twice.ConcatenateInTime(piecewise2_shifted);

    uniform_real_distribution<double> uniform(piecewise1.start_time(),
                                              piecewise1.end_time());
    double t = uniform(generator);

    EXPECT_TRUE(CompareMatrices(sum.value(t),
                                piecewise1.value(t) + piecewise2.value(t), 1e-8,
                                MatrixCompareType::absolute));

    EXPECT_TRUE(CompareMatrices(difference.value(t),
                                piecewise2.value(t) - piecewise1.value(t), 1e-8,
                                MatrixCompareType::absolute));

    EXPECT_TRUE(CompareMatrices(piecewise1_plus_offset.value(t),
                                piecewise1.value(t) + offset, 1e-8,
                                MatrixCompareType::absolute));

    EXPECT_TRUE(CompareMatrices(piecewise1_minus_offset.value(t),
                                piecewise1.value(t) - offset, 1e-8,
                                MatrixCompareType::absolute));

    EXPECT_TRUE(CompareMatrices(piecewise1_shifted.value(t),
                                piecewise1.value(t - shift), 1e-8,
                                MatrixCompareType::absolute));

    EXPECT_TRUE(CompareMatrices(product.value(t),
                                piecewise1.value(t) * piecewise5.value(t), 1e-8,
                                MatrixCompareType::absolute));

    EXPECT_TRUE(CompareMatrices(unary_minus.value(t), -(piecewise1.value(t))));

    // Checks that `piecewise2_twice` is effectively the concatenation of
    // `piecewise2` and a copy of `piecewise2` that is shifted to the right
    // (i.e. towards increasing values of t) by an amount equal to its entire
    // time length. To this end, it verifies that R(tₓ) = R(tₓ + d), where
    // R(t) = P(t) for t0 <= t <= t1, R(t) = Q(t) for t1 <= t <= t2,
    // Q(t) = P(t - d) for t1 <= t <= t2, d = t1 - t0 = t2 - t1 and
    // t0 < tₓ < t1, with P, Q and R functions being piecewise polynomials.
    EXPECT_TRUE(CompareMatrices(
        piecewise2_twice.value(t), piecewise2_twice.value(t + total_time),
        1e-8, MatrixCompareType::absolute));
  }
}

template<typename T>
void testValueOutsideOfRange() {
  default_random_engine generator;
  vector<double> segment_times =
      PiecewiseTrajectory<double>::RandomSegmentTimes(6, generator);
  PiecewisePolynomial<T> piecewise =
      test::MakeRandomPiecewisePolynomial<T>(3, 4, 5, segment_times);

  EXPECT_TRUE(CompareMatrices(piecewise.value(piecewise.start_time()),
                              piecewise.value(piecewise.start_time() - 1.0),
                              1e-10, MatrixCompareType::absolute));

  EXPECT_TRUE(CompareMatrices(piecewise.value(piecewise.end_time()),
                              piecewise.value(piecewise.end_time() + 1.0),
                              1e-10, MatrixCompareType::absolute));
}

// Test the generation of cubic splines with first and second derivatives
// continuous between the end of the last segment and the beginning of the
// first.
GTEST_TEST(testPiecewisePolynomial, CubicSplinePeriodicBoundaryConditionTest) {
  Eigen::VectorXd breaks(5);
  breaks << 0, 1, 2, 3, 4;

  // Spline in 3d.
  Eigen::MatrixXd samples(3, 5);
  samples << 1, 1, 1,
        2, 2, 2,
        0, 3, 3,
        -2, 2, 2,
        1, 1, 1;
  const bool periodic_endpoint = true;

  PiecewisePolynomial<double> periodic_spline =
      PiecewisePolynomial<double>::CubicWithContinuousSecondDerivatives(
          breaks, samples, periodic_endpoint);

  std::unique_ptr<Trajectory<double>> spline_dt =
      periodic_spline.MakeDerivative(1);
  std::unique_ptr<Trajectory<double>> spline_ddt =
      periodic_spline.MakeDerivative(2);

  Eigen::VectorXd begin_dt = spline_dt->value(breaks(0));
  Eigen::VectorXd end_dt = spline_dt->value(breaks(breaks.size() - 1));

  Eigen::VectorXd begin_ddt = spline_ddt->value(breaks(0));
  Eigen::VectorXd end_ddt = spline_ddt->value(breaks(breaks.size() - 1));

  EXPECT_TRUE(CompareMatrices(end_dt, begin_dt, 1e-14));
  EXPECT_TRUE(CompareMatrices(end_ddt, begin_ddt, 1e-14));

  // Test that evaluating the derivative directly gives the same results.
  const double t = 1.234;
  EXPECT_TRUE(CompareMatrices(periodic_spline.EvalDerivative(t, 1),
                              spline_dt->value(t), 1e-14));
  EXPECT_TRUE(CompareMatrices(periodic_spline.EvalDerivative(t, 2),
                              spline_ddt->value(t), 1e-14));
}

// Test various exception cases.  We want to check that these throw rather
// than crash (or return potentially bad data).
GTEST_TEST(testPiecewisePolynomial, ExceptionsTest) {
  Eigen::VectorXd breaks(5);
  breaks << 0, 1, 2, 3, 4;

  // Spline in 3d.
  Eigen::MatrixXd samples(3, 5);
  samples << 1, 1, 1,
        2, 2, 2,
        0, 3, 3,
        -2, 2, 2,
        1, 1, 1;

  // No throw with monotonic breaks.
  PiecewisePolynomial<double>::CubicWithContinuousSecondDerivatives(
      breaks, samples, true);

  // Throw when breaks are not strictly monotonic.
  breaks[1] = 0;
  DRAKE_EXPECT_THROWS_MESSAGE(
      PiecewisePolynomial<double>::CubicWithContinuousSecondDerivatives(
          breaks, samples, true),
      std::runtime_error, "Times must be in increasing order.");
}

GTEST_TEST(testPiecewisePolynomial, AllTests) {
  testIntegralAndDerivative<double>();

  testBasicFunctionality<double>();

  testValueOutsideOfRange<double>();
}

GTEST_TEST(testPiecewisePolynomial, VectorValueTest) {
  // Note: Keep one negative time to confirm that negative values can work for
  // constant trajectories.
  const std::vector<double> times = {-1.5, 0, .5, 1, 1.5};
  const Eigen::Vector3d value(1, 2, 3);

  const PiecewisePolynomial<double> col(value);
  Eigen::MatrixXd out = col.vector_values(times);
  EXPECT_EQ(out.rows(), 3);
  EXPECT_EQ(out.cols(), 5);
  for (int i = 0; i < 4; i++) {
    EXPECT_TRUE(CompareMatrices(out.col(i), value, 0));
  }

  PiecewisePolynomial<double> row(value.transpose());
  out = row.vector_values(times);
  EXPECT_EQ(out.rows(), 5);
  EXPECT_EQ(out.cols(), 3);
  for (int i = 0; i < 4; i++) {
    EXPECT_TRUE(CompareMatrices(out.row(i), value.transpose(), 0));
  }

  PiecewisePolynomial<double> mat(Eigen::Matrix3d::Identity());
  DRAKE_EXPECT_THROWS_MESSAGE(
      mat.vector_values(times), std::runtime_error,
      "This method only supports vector-valued trajectories.");
}

GTEST_TEST(testPiecewisePolynomial, RemoveFinalSegmentTest) {
  Eigen::VectorXd breaks(3);
  breaks << 0, .5, 1.;
  Eigen::MatrixXd samples(2, 3);
  samples << 1, 1, 2,
             2, 0, 3;

  PiecewisePolynomial<double> pp =
      PiecewisePolynomial<double>::CubicWithContinuousSecondDerivatives(
          breaks, samples);

  EXPECT_EQ(pp.end_time(), 1.);
  EXPECT_EQ(pp.get_number_of_segments(), 2);

  pp.RemoveFinalSegment();
  EXPECT_EQ(pp.end_time(), .5);
  EXPECT_EQ(pp.get_number_of_segments(), 1);

  pp.RemoveFinalSegment();
  EXPECT_TRUE(pp.empty());
}

std::unique_ptr<Trajectory<double>> TestReverseTime(
    const PiecewisePolynomial<double>& pp_orig) {
  std::unique_ptr<Trajectory<double>> pp_ptr = pp_orig.Clone();
  PiecewisePolynomial<double>* pp =
      dynamic_cast<PiecewisePolynomial<double>*>(pp_ptr.get());

  pp->ReverseTime();
  // Start time and end time have been switched.
  EXPECT_NEAR(pp->start_time(), -pp_orig.end_time(), 1e-14);
  EXPECT_NEAR(pp->end_time(), -pp_orig.start_time(), 1e-14);

  for (const double t : {0.1, .2, .52, .77}) {
    EXPECT_TRUE(CompareMatrices(pp->value(t), pp_orig.value(-t), 1e-14));
  }
  return pp_ptr;
}

void TestScaling(const PiecewisePolynomial<double>& pp_orig,
                 const double scale) {
  std::unique_ptr<Trajectory<double>> pp_ptr = pp_orig.Clone();
  PiecewisePolynomial<double>* pp =
      dynamic_cast<PiecewisePolynomial<double>*>(pp_ptr.get());

  pp->ScaleTime(scale);
  EXPECT_NEAR(pp->start_time(), scale * pp_orig.start_time(), 1e-14);
  EXPECT_NEAR(pp->end_time(), scale * pp_orig.end_time(), 1e-14);
  for (const double trel : {0.1, .2, .52, .77}) {
    const double t = pp_orig.start_time() +
                     trel * (pp_orig.end_time() - pp_orig.start_time());
    EXPECT_TRUE(CompareMatrices(pp->value(scale * t), pp_orig.value(t), 1e-14));
  }
}

GTEST_TEST(testPiecewisePolynomial, ReverseAndScaleTimeTest) {
  Eigen::VectorXd breaks(3);
  breaks << 0, .5, 1.;
  Eigen::MatrixXd samples(2, 3);
  samples << 1, 1, 2,
             2, 0, 3;

  const PiecewisePolynomial<double> zoh =
      PiecewisePolynomial<double>::ZeroOrderHold(breaks, samples);
  auto reversed_zoh = TestReverseTime(zoh);
  // Confirm that the documentation is correct about the subtle behavior at the
  // break-points due to the switch in the half-open interval (since zoh is
  // discontinuous at the breaks).
  EXPECT_FALSE(
      CompareMatrices(reversed_zoh->value(-breaks(1)), zoh.value(breaks(1))));
  EXPECT_TRUE(CompareMatrices(reversed_zoh->value(-breaks(1)),
                              zoh.value(breaks(1) - 1e-14)));
  TestScaling(zoh, 2.3);

  const PiecewisePolynomial<double> foh =
      PiecewisePolynomial<double>::FirstOrderHold(breaks, samples);
  TestReverseTime(foh);
  TestScaling(foh, 1.2);
  TestScaling(foh, 3.6);

  const PiecewisePolynomial<double> spline =
      PiecewisePolynomial<double>::CubicWithContinuousSecondDerivatives(
          breaks, samples);
  TestReverseTime(spline);
  TestScaling(spline, 2.0);
  TestScaling(spline, 4.3);
}

GTEST_TEST(testPiecewisePolynomial, ReshapeAndBlockTest) {
  std::vector<double> breaks = {0, .5, 1.};
  std::vector<Eigen::MatrixXd> samples(3);
  samples[0].resize(2, 3);
  samples[0] << 1, 1, 2, 2, 0, 3;
  samples[1].resize(2, 3);
  samples[1] << 3, 4, 5, 6, 7, 8;
  samples[2].resize(2, 3);
  samples[2] << -.2, 33., 5.4, -2.1, 52, 12;

  PiecewisePolynomial<double> zoh =
      PiecewisePolynomial<double>::ZeroOrderHold(breaks, samples);
  EXPECT_EQ(zoh.rows(), 2);
  EXPECT_EQ(zoh.cols(), 3);

  zoh.Reshape(3, 2);
  EXPECT_EQ(zoh.rows(), 3);
  EXPECT_EQ(zoh.cols(), 2);

  samples[0].resize(3, 2);
  samples[1].resize(3, 2);
  EXPECT_TRUE(CompareMatrices(zoh.value(0.25), samples[0]));
  EXPECT_TRUE(CompareMatrices(zoh.value(0.75), samples[1]));

  PiecewisePolynomial<double> block = zoh.Block(1, 1, 2, 1);
  EXPECT_EQ(block.rows(), 2);
  EXPECT_EQ(block.cols(), 1);
  EXPECT_EQ(block.start_time(), zoh.start_time());
  EXPECT_EQ(block.end_time(), zoh.end_time());
  EXPECT_EQ(block.get_number_of_segments(), zoh.get_number_of_segments());

  EXPECT_EQ(zoh.Block(0, 0, 1, 1).value(0.25), samples[0].block(0, 0, 1, 1));
  EXPECT_EQ(zoh.Block(2, 1, 1, 1).value(0.75), samples[1].block(2, 1, 1, 1));
  EXPECT_EQ(zoh.Block(1, 1, 2, 1).value(0.75), samples[1].block(1, 1, 2, 1));
}

GTEST_TEST(testPiecewisePolynomial, IsApproxTest) {
  Eigen::VectorXd breaks(3);
  breaks << 0, .5, 1.;
  Eigen::MatrixXd samples(2, 3);
  samples << 1, 2, 3, -5, -4, -3;
  // Make the numbers bigger to exaggerate the tolerance test.
  samples *= 1000;

  const PiecewisePolynomial<double> pp1 =
      PiecewisePolynomial<double>::FirstOrderHold(breaks, samples);
  const PiecewisePolynomial<double> pp2 = pp1 + Eigen::Vector2d::Ones();
  EXPECT_FALSE(pp1.isApprox(pp2, 0.1, ToleranceType::kAbsolute));
  EXPECT_TRUE(pp1.isApprox(pp2, 0.1, ToleranceType::kRelative));
}

template <typename T>
void TestScalarType() {
  VectorX<T> breaks(3);
  breaks << 0, .5, 1.;
  MatrixX<T> samples(2, 3);
  samples << 1, 1, 2, 2, 0, 3;

  const PiecewisePolynomial<T> spline =
      PiecewisePolynomial<T>::CubicWithContinuousSecondDerivatives(
          breaks, samples);

  const MatrixX<T> value = spline.value(0.5);
  EXPECT_NEAR(ExtractDoubleOrThrow(value(0)),
              ExtractDoubleOrThrow(samples(0, 1)), 1e-14);
  EXPECT_NEAR(ExtractDoubleOrThrow(value(1)),
              ExtractDoubleOrThrow(samples(1, 1)), 1e-14);
}

GTEST_TEST(PiecewiseTrajectoryTest, ScalarTypes) {
  TestScalarType<double>();
  TestScalarType<AutoDiffXd>();
  TestScalarType<symbolic::Expression>();
}

// Confirm the expected behavior of PiecewisePolynomial<Expression>.
GTEST_TEST(PiecewiseTrajectoryTest, SymbolicValues) {
  using symbolic::Expression;
  using symbolic::Variable;

  const Vector3<Expression> breaks(0, .5, 1.);
  const RowVector3<Expression> samples(6, 5, 4);
  const PiecewisePolynomial<Expression> foh =
      PiecewisePolynomial<Expression>::FirstOrderHold(breaks, samples);

  // value() works if breaks and coefficients are Expressions holding double
  // values, evaluated at a double-valued time.
  EXPECT_NEAR(ExtractDoubleOrThrow(foh.value(0.25)(0)), 5.5, 1e-14);

  // Symbolic time throws (because GetSegmentIndex returns an int in the middle
  // of the evaluation stack, breaking the Expression pipeline),
  EXPECT_THROW(foh.value(Variable("t")), std::runtime_error);

  // Symbolic breaks causes the construction methods to throw.
  const Vector3<Expression> symbolic_breaks(Variable("t0"), Variable("t1"),
                                            Variable("t2"));
  EXPECT_THROW(
      PiecewisePolynomial<Expression>::FirstOrderHold(symbolic_breaks, samples),
      std::runtime_error);

  // For symbolic samples (and therefore coefficients), value() returns the
  // symbolic form at the specified time.
  const Variable x0("x0");
  const Variable x1("x1");
  const Variable x2("x2");
  const RowVector3<Expression> symbolic_samples(x0, x1, x2);
  const PiecewisePolynomial<Expression> foh_w_symbolic_coeffs =
      PiecewisePolynomial<Expression>::FirstOrderHold(breaks, symbolic_samples);
  EXPECT_TRUE(foh_w_symbolic_coeffs.value(0.25)(0).Expand().EqualTo(0.5 * x0 +
                                                                    0.5 * x1));
}

// Verifies that the derivatives obtained by evaluating a
// `PiecewisePolynomial<AutoDiffXd>` and extracting the gradient of the result
// match those obtained by taking the derivative of the whole trajectory and
// evaluating it at the same point.
GTEST_TEST(PiecewiseTrajectoryTest, AutoDiffDerivativesTest) {
  VectorX<AutoDiffXd> breaks(3);
  breaks << 0, .5, 1.;
  MatrixX<AutoDiffXd> samples(2, 3);
  samples << 1, 1, 2, 2, 0, 3;

  const PiecewisePolynomial<AutoDiffXd> trajectory =
      PiecewisePolynomial<AutoDiffXd>::CubicWithContinuousSecondDerivatives(
          breaks, samples);
  std::unique_ptr<Trajectory<AutoDiffXd>> derivative_trajectory =
      trajectory.MakeDerivative();
  const int num_times = 100;
  VectorX<double> t = VectorX<double>::LinSpaced(
      num_times, ExtractDoubleOrThrow(trajectory.start_time()),
      ExtractDoubleOrThrow(trajectory.end_time()));
  const double tolerance = 20 * std::numeric_limits<double>::epsilon();
  for (int k = 0; k < num_times; ++k) {
    AutoDiffXd t_k = math::initializeAutoDiff(Vector1d{t(k)})[0];
    MatrixX<double> derivative_value =
        autoDiffToGradientMatrix(trajectory.value(t_k));
    MatrixX<double> expected_derivative_value =
        DiscardGradient(derivative_trajectory->value(t(k)));
    EXPECT_TRUE(CompareMatrices(derivative_value, expected_derivative_value,
                                tolerance));
  }
}

}  // namespace
}  // namespace trajectories
}  // namespace drake


///////////////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::chi_squared::derivative_log_unnormalized_pdf.hpp //
//                                                                                //
//  (C) Copyright 2009 Erwann Rogard                                                     //
//  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_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_CHI_SQUARED_DERIVATIVE_LOG_UNNORMALIZED_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_CHI_SQUARED_DERIVATIVE_LOG_UNNORMALIZED_HPP_ER_2009
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/numeric/conversion/converter.hpp>
#include <boost/math/policies/policy.hpp>

namespace boost{
namespace math{

    template<typename T,typename P>
    T
    derivative_log_unnormalized_pdf(
        const boost::math::chi_squared_distribution<T,P>& dist,
        const T& x
    ){
        BOOST_MATH_STD_USING  // for ADL of std functions
        T degrees_of_freedom = dist.degrees_of_freedom();
        // Error check:
        T error_result;

        static const char* function =
            "log_unnormalized_pdf(const chi_squared_distribution<%1%>&, %1%)";

        if(false == boost::math::detail::check_df(
            function, degrees_of_freedom, &error_result, P()))
                return error_result;

        if((x < 0) || !(boost::math::isfinite)(x))
        {
            return boost::math::policies::raise_domain_error<T>(
            function, "Chi Square parameter was %1%, but must be > 0 !",
            x, P());
        }

        // Lumped case x == 0 in x<=0 above

        static T one = static_cast<T>(1);
        static T two = static_cast<T>(2);
        return (degrees_of_freedom/two-one) / x - one/two;
    }

}// math
}// boost

#endif


/*
 * filtered_statistics.hpp
 *
 *  Created on: Nov 16, 2018
 *      Author: Gregory Kramida
 *   Copyright: 2018 Gregory Kramida
 *
 *   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.
 */
//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

//local
#include "../math/typedefs.hpp"

#pragma once

namespace eig = Eigen;

/**
 * Routines for computing the statistics of fields operating on filtered field regions, e.g. gather statistics only
 * over the span of live & canonical TSDF union or intersection
 */

namespace math {

template<typename Scalar>
double ratio_of_vector_lengths_above_threshold_band_union(
		const Eigen::Matrix<math::Vector2<Scalar>, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& vector_field,
		Scalar threshold,
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& warped_live_field,
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& canonical_field);

template<typename Scalar>
double ratio_of_vector_lengths_above_threshold_band_union(
		const Eigen::Tensor<math::Vector3<Scalar>, 3, Eigen::ColMajor>& vector_field,
		Scalar threshold,
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& warped_live_field,
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& canonical_field);

template<typename Scalar>
void mean_and_std_vector_length_band_union(Scalar& mean, Scalar& standard_deviation,
		const Eigen::Matrix<math::Vector2<Scalar>, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& vector_field,
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& warped_live_field,
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& canonical_field);

template<typename Scalar>
void mean_and_std_vector_length_band_union(Scalar& mean, Scalar& standard_deviation,
		const Eigen::Tensor<math::Vector3<Scalar>, 3, Eigen::ColMajor>& vector_field,
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& warped_live_field,
		const  Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& canonical_field);

} //namespace math


#ifndef __aarch64__
#include <algorithm>
#include <boost/lexical_cast.hpp>

#include "json.hpp"
#include "jget.h"

using json = nlohmann::json;

#include "vnxvideoimpl.h"
#include "vnxvideologimpl.h"
#include "GrayAnalyticsBase.h"

#include <ipp.h>
#include <ippi.h>
#include <ippcv.h>

extern "C" {
#include <libswscale/swscale.h>
}

void vnxHistogramBasic_8u(const uint8_t* data, int stride, int width, int height, int* histogram) {
    memset(histogram, 0, 256*sizeof(int));
    for (int y = 0; y < height; ++y) {
        const uint8_t* ptr = data + stride*y;
        for (int x = 0; x < width; ++x)
            ++histogram[ptr[x]];
    }
}
void vnxMeanVariance_8u(const uint8_t* data, int stride, int width, int height, double& mean, double& variance) {
    uint64_t vari = 0;
    uint32_t meani = 0;
    for (int y = 0; y < height; ++y) {
        const uint8_t* ptr = data + stride*y;
        for (int x = 0; x < width; ++x) {
            int val = ptr[x];
            meani += val;
            vari += val*val;
        }
    }
    mean = double(meani)/double(width*height);
    variance = double(vari) / double(width*height);
    variance -= mean*mean;
}

const int motionCellsH = 8;
const int motionCellsV = 6;

struct SBasicAnalyticsStatus {
    bool alarmTooDark;
    bool alarmTooBright;
    bool alarmTooBlurry;
    uint64_t motionMask;
    bool alarmMotion;
    bool alarmGlobalChange;
    uint64_t timestamp;
    SBasicAnalyticsStatus() {
        memset(this, 0, sizeof *this);
    }
    int alertsMask() {
        return (alarmTooDark << 0) + (alarmTooBright << 1) + (alarmTooBlurry << 2) + (alarmMotion << 3) + (alarmGlobalChange << 4);
    }
};

class CBasicAnalytics : public CGrayAnalyticsBase {
public:
    CBasicAnalytics(const std::vector<float>& roi, float framerate, bool too_bright, bool too_dark, bool too_blurry, float motion, bool scene_change)
        : CGrayAnalyticsBase(roi)
        , detect_too_bright(too_bright)
        , detect_too_dark(too_dark)
        , detect_too_blurry(too_blurry)
        , detect_motion(motion)
        , detect_scene_change(scene_change)
        , skip_rate(framerate_to_skip_rate(framerate))
    {
        m_histogram.resize(256);
        m_histogramSum.resize(256);
        m_motionCells.resize(motionCellsH * motionCellsV);
    }
protected:
    virtual void reset(int width, int height) {
        m_ratio = std::max(1, std::min(width / 320, height/200));
        m_width = width / m_ratio;
        m_height = height / m_ratio;

        m_bufferLaplace.reset();
        m_bufferMorph.reset();
        m_morphSpec.reset();

        m_frameNumber = 0;
        m_stride = (m_width % 16) ? ((m_width / 16 + 1) * 16) : m_width;
        for (auto b : { &m_data, &m_buffer0, &m_buffer1, &m_motionBackground, &m_motionVariance, &m_motionLabel, &m_motionDelta }) {
            b->reset((uint8_t*)ippMalloc(m_stride*height), ippFree);
            memset(b->get(), 0, m_stride*height);
        }
        m_resizeCtx.reset(sws_getContext(width, height, AV_PIX_FMT_GRAY8,
            m_width, m_height, AV_PIX_FMT_GRAY8, SWS_POINT, nullptr, nullptr, nullptr), sws_freeContext);
    }
    virtual void process(uint8_t* data, int width, int stride, int height, uint64_t timestamp) {
        //auto b = ippGetCpuClocks();

        // here we assume timestamp is in 90kHz or 100 kHz units
        //VNXVIDEO_LOG(VNXLOG_DEBUG, "vnxvideo") << "timestamp diff: " << timestamp - m_timestamp;
        if (m_frameNumber != 0 && (skip_rate > 0) && timestamp - m_status.timestamp < 1000000 * (1 << skip_rate)) {
            // uncomment to show result (on each frame)
            //ippiCopy_8u_C1R(m_motionLabel.get(), m_stride, data + width / 2 + height*stride / 2, stride, { m_width, m_height });
            return;
        }
        uint8_t* dst = m_data.get();
        sws_scale(m_resizeCtx.get(), &data, &stride, 0, height, &dst, &m_stride);

        if (detect_too_bright || detect_too_dark)
            detectTooBrightDark(m_data.get(), m_width, m_stride, m_height);
        if (detect_too_blurry)
            detectTooBlurry(m_data.get(), m_width, m_stride, m_height);
        if (detect_motion)
            detectMotion(m_data.get(), m_width, m_stride, m_height);
        //auto e = ippGetCpuClocks();
        //VNXVIDEO_LOG(VNXLOG_DEBUG, "vnxvideo") << "Clocks elapsed: " << e-b;

        // uncomment this to show the resulting motion labels right on the image.
        //ippiCopy_8u_C1R(m_motionLabel.get(), m_stride, data + width / 2 + height*stride / 2, stride, { m_width, m_height });
        m_status.timestamp = timestamp;
        sendEvents();
    }
    void sendEvents() {
        json j(json::object());
        if (detect_motion && (m_status.alarmMotion || m_lastSentStatus.alarmMotion)) {
            j["motion"] = m_status.alarmMotion;
            j["motion_mask"] = m_status.motionMask;
        }
        if(detect_scene_change && (m_status.alarmGlobalChange || m_lastSentStatus.alarmGlobalChange))
            j["scene_changed"] = m_status.alarmGlobalChange;
        if (detect_too_bright && (m_status.alarmTooBright || m_lastSentStatus.alarmTooBright))
            j["too_bright"] = m_status.alarmTooBright;
        if(detect_too_dark && (m_status.alarmTooDark || m_lastSentStatus.alarmTooDark))
            j["too_dark"] = m_status.alarmTooDark;
        if(detect_too_blurry && (m_status.alarmTooBlurry || m_lastSentStatus.alarmTooBlurry))
            j["too_blurry"] = m_status.alarmTooBlurry;

        if ((m_status.alertsMask() != m_lastSentStatus.alertsMask()) ||
            ((m_status.alertsMask() != 0) && (m_status.timestamp - m_lastSentStatus.timestamp >= 10000000))) {
            sendJson(j, m_status.timestamp);
            m_lastSentStatus = m_status;
        }
    }
private:
    const bool detect_too_bright;
    const bool detect_too_dark;
    const bool detect_too_blurry;
    const float detect_motion;
    const bool detect_scene_change;
    const int skip_rate; // corresponds to 0 -> 25-30, 1 -> 10, 2 -> 5, 3 -> 2 or less FPS
                        // frames are skipped IF actual framerate exceeds that values. Otherwise, nothing is actually skipped.
                        // That is, if FPS is limited to 5 at source, and skip_rate=1, - each frame will be processed.

    std::vector<int> m_histogram;
    std::vector<int> m_histogramSum;
    int m_stride;
    int m_ratio;
    int m_width;
    int m_height;
    std::shared_ptr<SwsContext> m_resizeCtx;
    std::shared_ptr<uint8_t> m_data;
    std::shared_ptr<uint8_t> m_buffer0;
    std::shared_ptr<uint8_t> m_buffer1;
    std::shared_ptr<uint8_t> m_bufferLaplace;
    std::shared_ptr<uint8_t> m_bufferMorph;
    std::shared_ptr<IppiMorphState> m_morphSpec;

    std::shared_ptr<uint8_t> m_motionBackground;
    std::shared_ptr<uint8_t> m_motionDelta;
    std::shared_ptr<uint8_t> m_motionVariance;
    std::shared_ptr<uint8_t> m_motionLabel;

    std::vector<int> m_motionCells;

    SBasicAnalyticsStatus m_status;
    SBasicAnalyticsStatus m_lastSentStatus;

    int m_frameNumber;
private:
    void detectTooBrightDark(uint8_t* data, int width, int stride, int height) {
        vnxHistogramBasic_8u(data, stride, width, height, &m_histogram[0]);
        int sum = 0;
        for (int k = 0; k < 256; ++k) {
            sum += m_histogram[k];
            m_histogramSum[k] = sum;
        }
        const int histogramTotal = m_histogramSum[255];
        const int histogram10perc = histogramTotal * 1 / 10;
        const int histogram90perc = histogramTotal * 9 / 10;
        if (m_histogramSum[256 * 1 / 3] > histogram90perc) {
            //VNXVIDEO_LOG(VNXLOG_DEBUG, "vnxvideo") << "Image too dark";
            if(detect_too_dark)
                m_status.alarmTooDark = true;
            m_status.alarmTooBright = false;
        }
        else if (m_histogramSum[256 * 2 / 3] < histogram10perc) {
            //VNXVIDEO_LOG(VNXLOG_DEBUG, "vnxvideo") << "Image too bright";
            if(detect_too_bright)
                m_status.alarmTooBright = true;
            m_status.alarmTooDark = false;
        }
        else
            m_status.alarmTooBright = m_status.alarmTooDark = false;
    }
    void detectTooBlurry(uint8_t* data, int width, int stride, int height) {
        IppStatus s;
        if (m_bufferLaplace.get() == nullptr) {
            int sz;
            s=ippiFilterLaplaceBorderGetBufferSize({ width,height }, ippMskSize3x3, ipp8u, ipp8u, 1, &sz);
            if(s!=ippStsNoErr)
                throw std::runtime_error("ippiFilterLaplaceBorderGetBufferSize failed: "+boost::lexical_cast<std::string>(s));
            m_bufferLaplace.reset(reinterpret_cast<Ipp8u*>(ippMalloc(sz)), ippFree);
        }
        s = ippiFilterLaplaceBorder_8u_C1R(data, stride, m_buffer0.get(), m_stride, { width, height }, ippMskSize3x3,
            ippBorderConst, 0, m_bufferLaplace.get());
        if (s != ippStsNoErr)
            throw std::runtime_error("Could not perform ippiFilterLaplace_8uC1R");

        vnxHistogramBasic_8u(m_buffer0.get(), m_stride, width, height, &m_histogram[0]);
        if (s != ippStsNoErr)
            throw std::runtime_error("Could not perform ippiHistogramRange_8u_C1R");
        int sum = 0;
        for (int k = 0; k < 256; ++k) {
            sum += m_histogram[k];
            m_histogramSum[k] = sum;
        }
        int laplace90 = 0;
        int laplace95 = 0;
        for (int k = 0; k < 256; ++k) {
            if (!laplace90 && (m_histogramSum[k] >= (m_histogramSum[255] * 90) / 100))
                laplace90 = k;
            if (!laplace95 && (m_histogramSum[k] >= (m_histogramSum[255] * 95) / 100))
                laplace95 = k;
        }
        double d = double(laplace95 - laplace90) / double(laplace90 + 1);
        if (d<0.45) {
            m_status.alarmTooBlurry = true;
        }
        else
            m_status.alarmTooBlurry = false;
    }

    static void sigmaDeltaAdjust(uint8_t* data, int dstride, // data, reference to update to
        uint8_t* result, int rstride, // result - inout buffer to be updated/adjusted
        uint8_t* mask, int mstride, // mask - where to update. optional, may be 0
        uint8_t* buffer, int bstride, // temporary buffer
        IppiSize size, uint8_t learningRate)
    {
        ippiCompare_8u_C1R(data, dstride, result, rstride, buffer, bstride, size, ippCmpGreater);
        if (nullptr != mask)
            ippiAnd_8u_C1IR(mask, mstride, buffer, bstride, size);
        ippiAndC_8u_C1IR(learningRate, buffer, bstride, size);
        ippiAdd_8u_C1IRSfs(buffer, bstride, result, rstride, size, 0);
        ippiCompare_8u_C1R(data, dstride, result, rstride, buffer, bstride, size, ippCmpLess);
        if (nullptr != mask)
            ippiAnd_8u_C1IR(mask, mstride, buffer, bstride, size);
        ippiAndC_8u_C1IR(learningRate, buffer, bstride, size);
        ippiSub_8u_C1IRSfs(buffer, bstride, result, rstride, size, 0);
    }
    void detectMotion(uint8_t* data, int width, int stride, int height) {
        //"Zipfian estimation"
        //http://perso.ensta-paristech.fr/~manzaner/Publis/icip09.pdf
        //MOTION DETECTION: FAST AND ROBUST ALGORITHMS FOR EMBEDDED SYSTEMS
        //L. Lacassagne A.Manzanera
        const IppiSize size = { width, height };
        if(0 == m_frameNumber)
            ippiCopy_8u_C1R(data, stride, m_motionBackground.get(), m_stride, size);
        else {
            int sigma=1;
            int t = m_frameNumber % (64 >> skip_rate);
            while (t / (sigma * 2) > 0)
                sigma *= 2;
            // mask - where background should be updated
            IppStatus st;
            st=ippiThreshold_LTVal_8u_C1R(m_motionVariance.get(), m_stride,
                m_buffer1.get(), m_stride, size,
                sigma, 0);
            st = ippiThreshold_GTVal_8u_C1IR(m_buffer1.get(), m_stride, size,
                sigma-1, 255);
            sigmaDeltaAdjust(data, stride, m_motionBackground.get(), m_stride,
                m_buffer1.get(), m_stride, // mask
                m_buffer0.get(), m_stride, // temp buffer
                size, skip_rate + 1);
        }

        ippiAbsDiff_8u_C1R(m_motionBackground.get(), m_stride, data, stride, m_motionDelta.get(), m_stride, size);

        if (0 == (m_frameNumber % 4)) { // T_V
            ippiMulC_8u_C1RSfs(m_motionDelta.get(), m_stride, 4, m_buffer1.get(), m_stride, size, 0);

            if (0 == m_frameNumber)
                ippiCopy_8u_C1R(m_buffer1.get(), m_stride, m_motionVariance.get(), m_stride, size);
            else
                sigmaDeltaAdjust(m_buffer1.get(), m_stride, m_motionVariance.get(), m_stride,
                    0, 0, // no mask
                    m_buffer0.get(), m_stride, size, skip_rate + 1);
            ippiThreshold_LTVal_8u_C1IR(m_motionVariance.get(), m_stride, size, 2, 2);
            ippiThreshold_GTVal_8u_C1IR(m_motionVariance.get(), m_stride, size, 64, 64);
        }

        ippiCompare_8u_C1R(m_motionDelta.get(), m_stride, m_motionVariance.get(), m_stride, m_motionLabel.get(), m_stride, size, ippCmpGreater);

        // spatial postprocessing
        IppStatus s;
        if (m_morphSpec.get() == nullptr || m_bufferMorph.get() == nullptr) {
            int specSize, bufferSize;
            s = ippiMorphologyBorderGetSize_8u_C1R(size, { 3,3 }, &specSize, &bufferSize);
            if (ippStsNoErr != s)
                std::runtime_error("ippiMorphologyBorderGetSize_8u_C1R failed: "+boost::lexical_cast<std::string>(s));
            m_morphSpec.reset(reinterpret_cast<IppiMorphState*>(ippMalloc(specSize)), ippFree);
            m_bufferMorph.reset(reinterpret_cast<Ipp8u*>(ippMalloc(bufferSize)), ippFree);

            static uint8_t strel[9] = { 0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff, };
            ippiMorphologyBorderInit_8u_C1R(size, strel, {3,3}, m_morphSpec.get(), m_bufferMorph.get());

        }

        ippiErodeBorder_8u_C1R(m_motionLabel.get(), m_stride, m_buffer0.get(), m_stride, size,
            ippBorderConst, 0, m_morphSpec.get(), m_bufferMorph.get());
        ippiDilateBorder_8u_C1R(m_buffer0.get(), m_stride, m_motionLabel.get(), m_stride, size,
            ippBorderConst, 0, m_morphSpec.get(), m_bufferMorph.get());

        motionProcessFinal();

        ++m_frameNumber;
    }
    void motionProcessFinal() {
        int cellW = m_width / motionCellsH;
        int cellH = m_height / motionCellsV;
        int cellSize = cellW*cellH;
        m_status.motionMask = 0;
        int motionCellsActive = 0;
        for (int y = 0; y < motionCellsV; ++y) {
            for (int x = 0; x < motionCellsH; ++x) {
                int count = 0;
                ippiCountInRange_8u_C1R(m_motionLabel.get() + x*cellW + y*m_stride*cellH, m_stride, { cellW, cellH },
                    &count, 1, 255);
                m_motionCells[motionCellsH*y + x] = count;
                const int cellCountThreshold = std::min<int>(cellSize/2, std::max<int>(1, (int)ceil(cellSize) * (1.0 - detect_motion) * 0.2));
                if (count >= cellCountThreshold) {
                    m_status.motionMask |= (1UL << x) << (y*motionCellsH);
                    ++motionCellsActive;
                }
            }
        }
        if (m_status.motionMask > 0) {
            if (motionCellsActive < 2 * motionCellsH*motionCellsV / 3) {
                if(detect_motion)
                    m_status.alarmMotion = true;
                m_status.alarmGlobalChange = false;
                //VNXVIDEO_LOG(VNXLOG_DEBUG, "vnxvideo") << "Motion detector active, mask=" << m_motionMask;
            }
            else {
                if(detect_motion)
                    m_status.alarmMotion = true;
                if(detect_scene_change)
                    m_status.alarmGlobalChange = true;
                // todo: could it be just a lighting conditions change or camera exposure adjusted?
                //VNXVIDEO_LOG(VNXLOG_DEBUG, "vnxvideo") << "Global scene change detected";
            }
        }
        else {
            m_status.alarmMotion = false;
            m_status.alarmGlobalChange = false;
        }
    }
    static int framerate_to_skip_rate(float fps) {
        int skip_rate = 0; // corresponds to 0 -> 25-30, 1 -> 10, 2 -> 5, 3 -> 2 or less FPS
        if (fps > 0) {
            if (fps <= 2)
                skip_rate = 3;
            else if (fps <= 5)
                skip_rate = 2;
            else if (fps <= 10)
                skip_rate = 1;
        }
        return skip_rate;
    }
};

namespace VnxVideo {
    IAnalytics* CreateAnalytics_Basic(const std::vector<float>& roi, float framerate, bool too_bright, bool too_dark, bool too_blurry, float motion, bool scene_change) {
        return new CBasicAnalytics(roi, framerate, too_bright, too_dark, too_blurry, motion, scene_change);
    }

}
#endif // aarch64


#include <Eigen/Core>
#include <Eigen/Dense>

#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include "ceres-error-terms/parameterization/unit3-param.h"

TEST(Unit3, TestUnit3CeresParametrization) {
  // Initial state values.
  Eigen::Quaterniond q(Eigen::Quaterniond::Identity().slerp(
      0.10, Eigen::Quaterniond::FromTwoVectors(
                Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 1))));

  Eigen::Quaterniond p(Eigen::Quaterniond::Identity().slerp(
      0.20, Eigen::Quaterniond::FromTwoVectors(
                Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 1))));

  Eigen::Vector2d u(0.2, 0.3);

  // Check: (q boxplus u)
  Eigen::Quaterniond q_rot;
  Eigen::Quaterniond q_rot_ceres;
  Eigen::Vector2d u_new;
  ceres_error_terms::Unit3::Plus(q, u, &q_rot);

  ceres_error_terms::Unit3Parameterization ceres_unit3_parametrization;

  ceres_unit3_parametrization.Plus(
      q.coeffs().data(), u.data(), q_rot_ceres.coeffs().data());

  pose::Quaternion q_rot_(q_rot);
  pose::Quaternion q_rot_ceres_(q_rot_ceres);
  EXPECT_NEAR_KINDR_QUATERNION(q_rot_, q_rot_ceres_, 1e-5);

  // Check: p boxminus q
  Eigen::Vector2d theta;
  Eigen::Vector2d theta_ceres;
  Eigen::Quaterniond p_orig;
  ceres_error_terms::Unit3::Minus(p, q, &theta);
  ceres_unit3_parametrization.Minus(
      p.coeffs().data(), q.coeffs().data(), theta_ceres.data());

  EXPECT_NEAR_EIGEN(theta, theta_ceres, 1e-5);
}

TEST(Unit3, TestUnit3) {
  // Initial state values.
  Eigen::Quaterniond q(Eigen::Quaterniond::Identity().slerp(
      0.10, Eigen::Quaterniond::FromTwoVectors(
                Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 1))));

  Eigen::Quaterniond p(Eigen::Quaterniond::Identity().slerp(
      0.20, Eigen::Quaterniond::FromTwoVectors(
                Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 1))));

  Eigen::Vector2d u(0.2, 0.3);

  // Check: (q boxplus u) boxminus q = u
  Eigen::Quaterniond q_rot;
  Eigen::Vector2d u_new;
  ceres_error_terms::Unit3::Plus(q, u, &q_rot);
  ceres_error_terms::Unit3::Minus(q_rot, q, &u_new);

  EXPECT_NEAR_EIGEN(u, u_new, 1e-5);

  // Check: (q boxplus u) boxplus -u = q
  Eigen::Quaterniond q_orig;
  Eigen::Vector2d u_neg = -1.0 * u;
  ceres_error_terms::Unit3::Plus(q_rot, u_neg, &q_orig);

  pose::Quaternion q_(q);
  pose::Quaternion q_orig_(q_orig);
  EXPECT_NEAR_KINDR_QUATERNION(q_, q_orig_, 1e-5);

  // Check: q boxplus (p boxminus q) = p
  Eigen::Vector2d theta;
  Eigen::Quaterniond p_orig;
  ceres_error_terms::Unit3::Minus(p, q, &theta);
  ceres_error_terms::Unit3::Plus(q, theta, &p_orig);

  pose::Quaternion p_(p);
  pose::Quaternion p_orig_(p_orig);
  EXPECT_NEAR_KINDR_QUATERNION(p_, p_orig_, 1e-5);
}

MAPLAB_UNITTEST_ENTRYPOINT


/*===========================================================================*\
 *                                                                           *
 *                               CoMISo                                      *
 *      Copyright (C) 2008-2009 by Computer Graphics Group, RWTH Aachen      *
 *                           www.rwth-graphics.de                            *
 *                                                                           *
 *---------------------------------------------------------------------------*
 *  This file is part of CoMISo.                                             *
 *                                                                           *
 *  CoMISo is free software: you can redistribute it and/or modify           *
 *  it under the terms of the GNU General Public License as published by     *
 *  the Free Software Foundation, either version 3 of the License, or        *
 *  (at your option) any later version.                                      *
 *                                                                           *
 *  CoMISo is distributed in the hope that it will be useful,                *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of           *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the            *
 *  GNU General Public License for more details.                             *
 *                                                                           *
 *  You should have received a copy of the GNU General Public License        *
 *  along with CoMISo.  If not, see <http://www.gnu.org/licenses/>.          *
 *                                                                           *
\*===========================================================================*/

#include <iostream>

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if (COMISO_ARPACK_AVAILABLE && COMISO_SUITESPARSE_AVAILABLE && COMISO_EIGEN3_AVAILABLE)
//=============================================================================

#include <CoMISo/Utils/StopWatch.hh>
#include <vector>
#include <CoMISo/EigenSolver/ArpackSolver.hh>
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Sparse>
#include <Eigen/Dense>

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

// Example main
int main(void)
{
  // matrix types
#if EIGEN_VERSION_AT_LEAST(3,1,0)
  typedef Eigen::SparseMatrix<double,Eigen::ColMajor>           SMatrix;
#else
  typedef Eigen::DynamicSparseMatrix<double,Eigen::ColMajor>    SMatrix;
#endif
  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>  Matrix;

  std::cout << "---------- 1) Setting up matrix..." << std::endl;
  unsigned int n=5;
  SMatrix A(n,n);
  // 1D Laplacian
  for(unsigned int i=0; i<n; ++i)
  {
    int count = 0;
    if( i > 0)
    {
      A.coeffRef(i,i-1) = -1.0;
      ++count;
    }
    if(i<n-1)
    {
      A.coeffRef(i,i+1) = -1.0;
      ++count;
    }

    A.coeffRef(i,i) = count;
  }

  std::cout << "---------- 2) Solving for m smallest eigenvalues and eigenvectors..." << std::endl;
  unsigned int m=3;
  COMISO::ArpackSolver arsolv;
  std::vector<double> evals;
  Matrix evects;
  arsolv.solve(A, evals, evects, m);

  std::cout << "---------- 3) printing results..." << std::endl;
  std::cerr << "********* eigenvalues: ";
  for(unsigned int i=0; i<evals.size(); ++i)
    std::cerr << evals[i] << ", ";
  std::cerr << std::endl;

  std::cerr <<"********* eigenvectors:" << std::endl;
  std::cerr << evects << std::endl;

  return 0;
}

//=============================================================================
#else
//=============================================================================

// Example main
int main(void)
{
  std::cerr << "Info: required dependencies are missing, abort...\n";
  return 0;
}
//=============================================================================
#endif // COMISO_SUITESPARSE_AVAILABLE
//=============================================================================


/*!
 * \file hnf_impl_lll.hpp
 * \author Jun Yoshida
 * \copyright (c) 2020 Jun Yoshida.
 * The project is released under the 2-clause BSD License.
 * \date August, 2020: created
 */

#pragma once

#include <valarray>
#include <functional>
#include <tuple>
#include <cmath>
#include <Eigen/Dense>

#include "utils.hpp"
#include "matrixops.hpp"

namespace khover {

namespace _impl_LLL {

class Lambda_t {
public:
    using underlying_t = long double;
    static inline constexpr double delta = 0.75;

private:
    std::size_t m_size;

    //! Diagonal entries.
    std::valarray<underlying_t> m_diag;

    //! strictly lower trianglar matrix with row-major
    std::valarray<underlying_t> m_offDiag;

protected:
    static inline constexpr std::size_t rhead(std::size_t i) noexcept {
        return i*(i-1)/2;
    }

    inline void rev(std::size_t &k) noexcept {
        k = m_size - k - 1;
    }

public:
    Lambda_t(std::size_t n) noexcept
        : m_size(n),
          m_diag(static_cast<underlying_t>(1),n+1),
          m_offDiag(static_cast<underlying_t>(0),n*(n-1)/2)
    {}

    //! Element access (R/W)
    //! \pre n >= i >= j >= 0
    underlying_t& operator()(std::size_t i, std::size_t j) {
        rev(i);
        rev(j);
        return i==j ? m_diag[i+1] : m_offDiag[rhead(i)+j];
    }

    //! Element access (Read only)
    underlying_t const & operator()(std::size_t i, std::size_t j) const {
        return const_cast<Lambda_t*>(this)->operator()(i,j);
    }

    //! \pre isrc != itgt
    void axpy(underlying_t coeff, std::size_t isrc, std::size_t itgt) noexcept {
        rev(isrc);
        rev(itgt);
        auto imin = std::min(isrc,itgt);

        if (imin <= 0) return;

        std::slice_array<underlying_t> tgt
            =  m_offDiag[std::slice(rhead(itgt),imin,1)];

        std::valarray<underlying_t> qsrc(
            m_offDiag[std::slice(rhead(isrc),imin,1)]);

        tgt += std::valarray<underlying_t>(coeff,imin) * qsrc;
    }

    //! Update lambda in SWAP step in the HMM algorithm.
    //! \pre 0 < k < n
    void swap(std::size_t k) noexcept {
        rev(k);
        std::swap_ranges(
            std::begin(m_offDiag)+rhead(k),
            std::begin(m_offDiag)+rhead(k)+k-1,
            std::begin(m_offDiag)+rhead(k-1));

        for (size_t i = k+1; i < m_size; ++i) {
            underlying_t aux1 = m_offDiag[rhead(i)+k-1] / m_diag[k];
            underlying_t aux2 = m_offDiag[rhead(i)+k] / m_diag[k];
            m_offDiag[rhead(i)+k-1]
                = std::fma(aux1, m_offDiag[rhead(k)+k-1], aux2 * m_diag[k-1]);
            m_offDiag[rhead(i)+k]
                = std::fma(aux1, m_diag[k+1], (-aux2) * m_offDiag[rhead(k)+k-1]);
        }

        //! Be careful on overflows.
        m_diag[k] =
            std::fma(m_diag[k-1] / m_diag[k], m_diag[k+1],
                (m_offDiag[rhead(k)+k-1] / m_diag[k]) * m_offDiag[rhead(k)+k-1] );
    }

    //! Update lambda in MINUS step in the HMM algorithm.
    void negate(std::size_t k) noexcept{
        rev(k);
        if (k <= 1) return;

        // std::for_each(
        //     std::begin(m_offDiag)+rhead(k),
        //     std::begin(m_offDiag)+rhead(k)+k,
        //     std::negate<underlying_t>{});
        std::slice_array<underlying_t> sl = m_offDiag[std::slice(rhead(k),k,1)];
        sl *= std::valarray<underlying_t>(-1,k);

        for(size_t i = k+1; i < m_size; ++i)
            m_offDiag[rhead(i)+k] *= -1;
    }

    //! Check if SWAP should be performed.
    //! \pre k >= 1 && k < n
    int should_swap(size_t k) noexcept
    {
        rev(k);
        return m_diag[k-1] * m_diag[k+1] + sqpow(m_offDiag[rhead(k)+k-1])
            < delta * sqpow(m_diag[k]);
    }

};

/*!
 * "Swap" operation in the algorithm.
 * \pre 1 <= k < min(data->m.r, data->u.r)
 */
template<class Ops, class Derived, class...Us, class...Vs>
void swap(
    std::size_t k,
    Eigen::MatrixBase<Derived> &m,
    std::tuple<Us&...> &us, std::tuple<Vs&...> &vs,
    Lambda_t &lambda
    ) noexcept
{
    // Transform matrices
    Ops::swap(m, k, k+1);
    for_each_tuple(us, [k](auto& u){ Ops::dual_t::swap(u,k,k+1); });
    for_each_tuple(vs, [k](auto& v){ Ops::swap(v,k,k+1); });

    // Update lambda
    lambda.swap(k);
}

/*!
 * "Minus" operation in the algorithm.
 */
template <class Ops, class Derived, class...Us, class...Vs>
void negate(
    std::size_t k,
    Eigen::MatrixBase<Derived> &m,
    std::tuple<Us&...> &us, std::tuple<Vs&...> &vs,
    Lambda_t &lambda
    ) noexcept
{
    // Negate k-th rows
    Ops::scalar(m, k, -1);

    for_each_tuple(us, [k](auto& u){ Ops::dual_t::scalar(u, k, -1); });
    for_each_tuple(vs, [k](auto& v){ Ops::scalar(v, k, -1); });

    // Update lambda
    lambda.negate(k);
}

//! The type of the return value of "Reduce2" step.
enum HowSwap : uint_fast8_t {
    ShouldSwap  = 0b001,
    ZeroReducer = 0b010,
    ZeroReducee = 0b100,
    BothZero    = 0b110,
    // < 8bits
};

/*!
 * "Reduce2" operation in the algorithm.
 * \pre k < i
 * \tparam flag If true, swap check will be skipped.
 * \return Whether "Swap" should be performed or not.
 */
template <class Ops, bool flag, class Derived, class...Us, class...Vs>
auto reduce(
    std::size_t k, std::size_t i,
    Eigen::MatrixBase<Derived> &m,
    std::tuple<Us&...> &us, std::tuple<Vs&...> &vs,
    Lambda_t &lambda
    ) noexcept
    -> std::underlying_type_t<HowSwap>
{
    /* Find the first non-zero entry of the i-th and j-th vectors. */
    std::size_t l1 = Ops::find_nonzero_unsafe(
        m, i,
        [&](std::size_t, auto x) {
            if (std::signbit(x)) negate<Ops>(i, m, us, vs, lambda);
        } );
    std::size_t l2 = Ops::find_nonzero_unsafe(
        m, k,
        [&](std::size_t, auto x) {
            if (std::signbit(x)) negate<Ops>(k, m, us, vs, lambda);
        } );

    // Scalar factor of the reduction
    typename Eigen::MatrixBase<Derived>::Scalar q;

    // Compute the scalar factor
    if (l1 < Ops::size(m)) {
        q = - floor_div(Ops::at(m,k)[l1], Ops::at(m,i)[l1]);
    }
    else if (2.0*std::abs(lambda(k,i)) > lambda(i,i))
        q = - std::round( lambda(k,i)/ lambda(i,i) );
    else
        q = 0;

    // Reduce the k-th vector by the i-th one.
    if (q != 0) {
        Ops::axpy(m, q, i, k);
        for_each_tuple(us, [i,k,&q](auto& u){ Ops::dual_t::axpy(u,-q,k,i); });
        for_each_tuple(vs, [i,k,&q](auto& v){ Ops::axpy(v,q,i,k); });

        // Update lambda
        lambda.axpy(q, i, k);
    }

    if constexpr(!flag) {
        if (l1 < Ops::size(m)) {
            if (l2 >= Ops::size(m))
                return HowSwap::ShouldSwap | HowSwap::ZeroReducee;
            else
                return l1 <= l2 ? HowSwap::ShouldSwap : 0u;
        }
        else if (l2 < Ops::size(m))
            return HowSwap::ZeroReducer;
        else
            return HowSwap::BothZero
                | (lambda.should_swap(k) ? HowSwap::ShouldSwap : 0u);
    }
    else
        return 0;
}

} // end namespace _impl_LLL

} // end namespace khover


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

/// @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() } ;
}

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

}
}
}
}
}

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